2. 坐标系与刚体运动基础
做多传感器融合，说白了就是跟坐标系打交道。我刚开始接触这个领域时，觉得坐标系不就是个参考系嘛，有什么好讲的？结果第一次做激光雷达和相机的联合标定，就被坐标系变换搞得晕头转向。嗯，今天咱们就把这块硬骨头啃下来。
2.1 为什么坐标系这么重要？
你想想看，一个机器人身上可能同时装着激光雷达、IMU、相机、GPS。每个传感器都有自己的"小世界"——自己的坐标系。激光雷达说"前方3米有障碍物"，相机说"图像中心偏右100像素有特征点"，IMU说"当前加速度是0.5m/s²"。这些数据如果不统一到一个坐标系下，根本没法用。
我在项目中遇到过最典型的例子：一个AGV小车，激光雷达检测到前方1米有障碍物，但实际障碍物在车体左前方。为什么？因为激光雷达装在车头右侧，它的"前方"和车体的"前方"差了30度。这就是坐标系没对齐的后果。
2.2 常用坐标系
做SLAM，你至少得搞明白下面这几个坐标系：
| 坐标系 | 符号 | 说明 |
|---|---|---|
| 世界坐标系 | W | 固定在地面上的全局参考系，通常以机器人起始位置为原点 |
| 机器人本体坐标系 | B | 固定在机器人质心，x轴朝前，y轴朝左，z轴朝上 |
| 传感器坐标系 | S | 每个传感器自己的坐标系，比如相机坐标系、激光雷达坐标系 |
我个人习惯把世界坐标系叫做"地图坐标系"，因为最终建图的结果就是在这个坐标系下表达的。机器人本体坐标系嘛，说白了就是"我当前在哪儿、朝哪个方向看"。
2.3 刚体运动与变换矩阵
刚体运动，就是物体在空间中移动和旋转，但形状大小不变。一个刚体在三维空间中的位姿，可以用6个自由度来描述：3个平移 + 3个旋转。
怎么表示这个变换呢？最常用的就是齐次变换矩阵：
// 齐次变换矩阵 T 的形式
// T = [R t]
// [0 1]
// 其中 R 是 3x3 旋转矩阵
// t 是 3x1 平移向量
// 举个例子：将点 p 从传感器坐标系变换到机器人坐标系
p_robot = T_sensor_to_robot * p_sensor这里有个坑，我曾经踩过：旋转矩阵必须是正交矩阵，行列式为+1。如果你标定出来的旋转矩阵不满足这个条件，那肯定是算错了。嗯，别问我怎么知道的。
2.4 旋转的多种表示方式
旋转矩阵、欧拉角、四元数、轴角……初学者看到这些肯定头大。我建议你这样理解：
- 旋转矩阵：最直观，但9个参数有6个约束，不适合优化
- 欧拉角：3个参数，但有万向锁问题
- 四元数：4个参数，无奇异性，适合插值和优化
- 轴角：3个参数，但表达不唯一
做SLAM，我个人强烈推荐用四元数。为什么？因为它在优化时不会遇到奇异性，而且插值平滑。我在做视觉惯性里程计(VIO)时，所有旋转都用四元数表示，省了不少麻烦。
// 四元数表示旋转
// q = [w, x, y, z]，其中 w 是实部
// 用四元数旋转一个三维向量 v
// 先把 v 写成纯四元数 [0, vx, vy, vz]
// 然后计算 q * v * q_conjugate
Eigen::Quaterniond q(0.707, 0.707, 0, 0); // 绕z轴旋转90度
Eigen::Vector3d v(1, 0, 0);
Eigen::Vector3d v_rotated = q * v; // 结果是 (0, 1, 0)2.5 坐标系变换链
实际系统中，坐标系变换往往是链式的。比如：
// 从激光雷达点变换到世界坐标系
p_world = T_world_to_robot * T_robot_to_lidar * p_lidar
// 或者写成
p_world = T_world_to_robot * (T_robot_to_lidar * p_lidar)这里要注意变换的顺序。矩阵乘法不满足交换律，先旋转后平移和先平移后旋转结果完全不同。我见过有人把这两个搞反了，结果点云全部飞到天上去。
2.6 知识体系总览
下面这张图总结了本章的核心内容，我建议你保存下来，做项目时对照着看：
2.7 实践中的坐标系管理
在ROS中，坐标系管理有现成的工具——tf库。你不需要手动维护变换矩阵，tf会帮你搞定一切。但理解背后的原理仍然很重要，因为：
- 标定传感器外参时，你需要自己推导变换关系
- 调试时，你需要能看懂tf树，快速定位问题
- 优化算法时，你可能需要自己实现变换，而不是依赖tf
我记得有一次，一个学生在做激光雷达和IMU融合时，发现定位结果总是飘。查了两天，最后发现是IMU的坐标系定义和激光雷达差了90度。这种问题，如果你不懂坐标系变换，根本无从下手。
公众号:蓝海资料掘金营，微信deep3321