坐标系与变换基础：世界坐标系、传感器坐标系、刚体变换与SE(3)、四元数与旋转矩阵

做多传感器融合，说白了就是跟坐标系打交道。我刚开始接触这个领域时，觉得坐标系不就是个参考系嘛，有啥好讲的？结果第一次做激光雷达和IMU的联合标定，数据怎么都对不上，折腾了两天才发现是坐标系定义搞反了。嗯，从那以后，我再也不敢小看坐标系了。

世界坐标系与传感器坐标系

先说说世界坐标系。你可以把它想象成一个「绝对参考系」。比如你在房间里放一个机器人，房间的墙角就是世界坐标系的原点。X轴指向东，Y轴指向北，Z轴指向天——这是最常见的定义方式。

传感器坐标系呢？每个传感器都有自己的「小世界」。比如相机，它的坐标系原点在光心，Z轴指向镜头前方。激光雷达的坐标系原点在旋转中心，X轴指向车头方向。IMU的坐标系就更复杂了，不同厂家的定义还不一样。

我遇到过最坑的一次，是某个IMU的Z轴朝下，而我的算法默认Z轴朝上。结果融合出来的姿态，俯仰角直接反了180度。所以拿到新传感器，第一件事就是看它的坐标系定义文档。

刚体变换与SE(3)

刚体变换，说白了就是「旋转+平移」。一个刚体在三维空间里运动，不会变形，不会扭曲，只会转一转、挪一挪。数学上我们用SE(3)来表示这个变换。

SE(3)是Special Euclidean Group的缩写，翻译过来就是「特殊欧几里得群」。别被名字吓到，它本质上就是一个4x4的矩阵：

SE(3) = [R  t]
        [0  1]

其中R是3x3的旋转矩阵，t是3x1的平移向量。最后一行是[0 0 0 1]，固定不变。

为什么用4x4矩阵？因为这样可以把旋转和平移写在一起，方便做连续变换。比如你要把激光雷达的点云从传感器坐标系变换到世界坐标系：

P_world = T * P_sensor

这里的T就是SE(3)变换矩阵。我习惯把这种变换叫做「外参」，因为它是传感器相对于世界坐标系的位姿。

旋转矩阵

旋转矩阵R是一个3x3的正交矩阵，行列式为+1。它描述了坐标系之间的旋转关系。比如绕Z轴旋转θ角度：

R_z(θ) = [cosθ  -sinθ  0]
         [sinθ   cosθ  0]
         [0      0     1]

绕X轴和Y轴的类似，我就不写了。你想想看，三个轴的旋转组合起来，就能表示任意姿态。

旋转矩阵有个好处：它很直观，而且可以连续乘。但坏处也很明显——9个参数，只有3个自由度，冗余太多。而且数值误差会让矩阵不再是正交的，需要定期做正交化处理。

四元数

四元数是我个人最喜欢的旋转表示方式。它只有4个参数，没有奇异性，插值也方便。一个四元数q可以写成：

q = w + xi + yj + zk

其中w是实部，x、y、z是虚部。单位四元数（模长为1）可以用来表示旋转。

为什么用四元数？说白了，欧拉角有万向锁问题，旋转矩阵有冗余。四元数刚好解决了这两个痛点。我在做无人机姿态估计时，全程用四元数，从来没出过问题。

四元数和旋转矩阵可以互相转换。比如从四元数到旋转矩阵：

R = [1-2(y²+z²)  2(xy-wz)    2(xz+wy)]
    [2(xy+wz)    1-2(x²+z²)  2(yz-wx)]
    [2(xz-wy)    2(yz+wx)    1-2(x²+y²)]

反过来，从旋转矩阵到四元数，需要解一个方程组。我一般直接用现成的库函数，比如Eigen或Sophus，手写容易出错。

知识体系结构图

下面这张图展示了本章的核心逻辑：

总结

坐标系和变换是SLAM的基石。世界坐标系是绝对参考，传感器坐标系是局部参考。刚体变换用SE(3)表示，旋转部分可以用旋转矩阵或四元数。我个人更推荐四元数，因为它没有奇异性，数值稳定性好。

最后提醒一句：做多传感器融合时，一定要先确认所有传感器的坐标系定义。我曾经因为IMU的Z轴方向搞反，浪费了整整一周的时间。嗯，这种坑踩过一次就够了。