3、坐标系与变换：世界坐标系、机器人坐标系、传感器坐标系、欧拉角与四元数、齐次变换矩阵

说实话，做SLAM这几年，我见过太多新手一上来就猛怼算法，结果连坐标系都没搞明白。最后跑出来的地图歪七扭八，还以为是代码写错了。其实啊，坐标系和变换才是整个地图构建的基石。你想想看，机器人怎么知道自己在哪？传感器测得的数据怎么对齐？全靠这些坐标系之间的变换关系。

3.1 世界坐标系：一切测量的基准

世界坐标系，说白了就是整个地图的"大地原点"。我习惯把它记作 {W}。它固定在地球上的某个位置，比如房间的角落、建筑物的入口。所有机器人的位置、地图中的特征点，最终都要用世界坐标来表达。

我记得有一次做室外大场景建图，团队里有人把世界坐标系原点设在了移动的车辆上。结果跑完一圈回来，地图全散了。嗯，这就是典型的坐标系选择失误。

3.2 机器人坐标系：跟着机器人走的"身体"

机器人坐标系 {R} 是固定在机器人本体上的。通常原点在机器人的几何中心，x轴朝前，y轴朝左，z轴朝上。这个坐标系会随着机器人移动而移动。

为什么要搞这么个坐标系？因为机器人只能感知自己身上的传感器数据。激光雷达扫到的点，最初都是在雷达坐标系下的。你得先把它转到机器人坐标系，再转到世界坐标系，才能知道这个点在地图上的真实位置。

3.3 传感器坐标系：每个传感器都有自己的"小世界"

激光雷达、IMU、相机……每个传感器都有自己的坐标系。比如激光雷达坐标系 {L}，原点在雷达旋转中心；相机坐标系 {C}，原点在光心。

这些传感器坐标系和机器人坐标系之间，存在一个固定的变换关系。这个关系叫做"外参"。你想想看，如果外参标定不准，那传感器数据融合起来就会打架。

3.4 欧拉角与四元数：旋转的两种表达

旋转怎么描述？最直观的是欧拉角。绕x轴转叫roll，绕y轴转叫pitch，绕z轴转叫yaw。听起来很简单对吧？

但欧拉角有个致命问题——万向锁。当pitch接近90度时，roll和yaw会变得无法区分。我在做无人机SLAM时就踩过这个坑，飞机悬停时姿态解算突然跳变，差点炸机。

所以，实际工程中我更推荐用四元数。它没有万向锁，而且插值平滑。四元数长这样：

q = w + x*i + y*j + z*k
其中 w^2 + x^2 + y^2 + z^2 = 1

别看它四个数有点抽象，但用起来真香。我建议你直接把它当成一个"旋转的紧凑表达"，别纠结几何意义。

表达方式	优点	缺点
欧拉角	直观，容易理解	万向锁，插值不连续
四元数	无奇异性，插值平滑	不够直观
旋转矩阵	计算方便	9个参数，冗余

3.5 齐次变换矩阵：把旋转和平移打包

旋转用四元数或矩阵，平移用向量。但每次变换都要分开算，太麻烦。齐次变换矩阵就是来解决这个问题的。

它长这样：

T = [ R  t ]
    [ 0  1 ]

其中 R 是 3x3 旋转矩阵
      t 是 3x1 平移向量

用这个矩阵，你可以把坐标系A下的点，一步变换到坐标系B下。比如从传感器坐标系到机器人坐标系：

p_R = T_R_L * p_L

其中 T_R_L 就是传感器到机器人的齐次变换矩阵。我习惯把它叫做"外参矩阵"。

3.6 知识体系总览

为了让你更直观地理解这些坐标系之间的关系，我画了一张图：

从这张图你可以看到，整个坐标系体系就像一条链条。传感器数据先通过外参 T_R_L 转到机器人坐标系，再通过位姿 T_W_R 转到世界坐标系。每一步都离不开齐次变换矩阵。

好了，坐标系与变换这部分就聊到这儿。记住一句话：坐标系是地图的骨架，变换是骨架上的关节。骨架歪了，地图就散了。