2、坐标系与位姿表示：世界坐标系、机器人坐标系、旋转矩阵、四元数、欧拉角

做SLAM这些年，我见过太多人在坐标系上栽跟头了。

说实话，坐标系和位姿表示是整个定位系统的地基。地基没打牢，后面什么卡尔曼滤波、图优化都是白搭。我记得刚入行那会儿，就因为搞混了旋转矩阵的左右乘顺序，让一台AGV在仓库里画了个完美的八字形——嗯，差点撞到货架。

今天咱们就把这块硬骨头啃下来。你想想看，搞清楚了坐标系，后面做轨迹评估才能有的放矢。

2.1 世界坐标系与机器人坐标系

先聊最基础的。世界坐标系，说白了就是那个「绝对参考系」。你把它想象成房间里的固定墙角，不管机器人怎么动，这个墙角的位置永远不变。在SLAM里，我们通常把第一帧相机的位置设为世界坐标系原点。

机器人坐标系呢？它是跟着机器人走的。我习惯叫它「本体坐标系」。一般以机器人的质心为原点，前进方向为X轴，左侧为Y轴，头顶为Z轴（右手系）。

我在项目中遇到过一个问题：有人把IMU的加速度数据直接当成世界坐标系下的量来用。结果呢？机器人一倾斜，积分出来的位置就飞了。这就是没搞清楚「测量值在哪个坐标系下」的典型错误。

2.2 旋转矩阵：最直观的旋转表示

旋转矩阵，R矩阵，3x3的方阵。它描述了一个坐标系相对于另一个坐标系的姿态。

数学上长这样：

R = [r11 r12 r13
     r21 r22 r23
     r31 r32 r33]

性质很简单：正交矩阵，行列式为+1。这意味着它的逆等于它的转置。这个性质在实际代码里特别有用——求逆的时候直接转置就行，省了多少计算量啊。

我个人习惯把旋转矩阵理解成「基向量的投影」。R的每一列，就是机器人坐标系的基向量在世界坐标系下的坐标。你想想看，是不是这个理？

2.3 欧拉角：直观但危险

欧拉角，三个角度：roll（横滚）、pitch（俯仰）、yaw（偏航）。

为什么说它直观？因为人脑能理解「先转yaw再转pitch最后转roll」这种操作。我在调试时经常用欧拉角来可视化姿态变化，一眼就能看出机器人是不是在翻跟头。

为什么说它危险？万向锁。当pitch接近±90°时，roll和yaw会失去一个自由度。你想想看，无人机做筋斗动作时，如果姿态解算用欧拉角，直接就炸了。

常用的旋转顺序是ZYX（先绕Z轴转yaw，再绕Y轴转pitch，最后绕X轴转roll）。对应的转换公式：

R = Rz(yaw) * Ry(pitch) * Rx(roll)

注意乘法的顺序！从右往左读。我刚开始总搞反，后来记住一句话：「先转的放右边，后转的放左边」。

2.4 四元数：SLAM的标配

四元数，q = w + xi + yj + zk。四个数，没有奇点，插值平滑。

说实话，我刚学四元数时也觉得抽象。什么超复数、什么纯虚数，头都大了。后来我发现，你根本不需要理解它的几何意义——只需要记住三个操作：

1. 乘法：两个四元数相乘，表示旋转的复合
2. 共轭：表示逆旋转
3. 旋转向量：用四元数旋转一个三维向量 v' = q * v * q^-1

在代码里，我几乎只用四元数做内部运算。Eigen库的 Eigen::Quaterniond 用起来很顺手：

Eigen::Quaterniond q1(1.0, 0.0, 0.0, 0.0);  // 单位四元数
Eigen::Quaterniond q2 = Eigen::AngleAxisd(0.5, Eigen::Vector3d::UnitZ());
Eigen::Vector3d v(1, 0, 0);
Eigen::Vector3d v_rot = q2 * v;  // 旋转向量

注意：Eigen里四元数乘以向量，会自动调用旋转操作。这个设计很贴心，但你要知道背后发生了什么——其实就是上面那个公式。

2.5 三种表示方式的对比

表示方式	优点	缺点	我的使用场景
旋转矩阵	直观，容易理解坐标变换	9个参数，冗余，不紧凑	坐标变换计算、可视化调试
欧拉角	三个数，人脑友好	万向锁，插值困难	仅用于显示和人工读取
四元数	无奇点，插值平滑，紧凑	不够直观，需要适应	内部运算、优化、滤波

2.6 坐标系变换的实战套路

在轨迹评估里，我们经常要做一件事：把机器人坐标系下的观测点，变换到世界坐标系下。

公式很简单：

P_world = R * P_robot + t

其中R是旋转矩阵，t是平移向量。这个操作叫「刚体变换」，也叫SE(3)。

我习惯用齐次坐标来统一表示：

T = [R  t
     0  1]   // 4x4矩阵

P_world_homo = T * P_robot_homo

为什么用齐次？因为你可以把多次变换连乘起来：T_total = T1 * T2 * T3。这样代码写起来特别干净，而且不容易出错。

2.7 本章知识体系

下面这张图，是我自己总结的坐标系与位姿表示的核心逻辑。你把它存脑子里，后面做轨迹评估就不会迷路。

这张图把整个逻辑串起来了：世界坐标系和机器人坐标系之间通过刚体变换联系，而旋转部分可以用旋转矩阵、欧拉角或四元数来表示。最终，所有轨迹评估指标都要在世界坐标系下计算。

好了，坐标系和位姿表示就聊到这儿。记住我一句话：写代码前先问自己三个问题——数据在哪个坐标系？要转到哪个坐标系？用什么方式表示旋转？ 这三个问题想清楚了，代码基本不会跑偏。

可可精品资料库