坐标系与变换：世界坐标系、机器人坐标系、齐次变换矩阵、李群与李代数基础

各位同学，大家好。今天我们来聊聊SLAM里最基础、也最绕不开的一个话题——坐标系与变换。

说实话，我刚开始做SLAM那会儿，最头疼的就是坐标系。明明看着一个点，怎么换个坐标系就完全不一样了？后来踩的坑多了，才慢慢摸清楚这里面的门道。今天我就把这些经验掰开了揉碎了讲给你听。

先问个问题：你站在房间里，怎么告诉别人你手机掉哪儿了？

你可能会说“在桌子下面，靠墙的位置”。这个“桌子”和“墙”就是你的参考系。在SLAM里，我们管这个叫世界坐标系。它是个固定不动的坐标系，通常以机器人启动时的位置为原点，或者以某个已知的地标为原点。

而机器人坐标系，是跟着机器人一起动的。它的原点在机器人中心，x轴朝前，y轴朝左，z轴朝上（这是ROS里的惯例，但不同平台可能有差异）。

我有个习惯：每次拿到新传感器，第一件事就是确认它的坐标系定义。因为不同厂商的IMU、激光雷达，坐标系可能完全不一样。有一次我在项目里用了两个不同品牌的IMU，结果融合出来的姿态直接飞了——后来发现，一个定义z轴向上，另一个定义z轴向下。嗯，这种低级错误，犯一次就够了。

  核心要点：
  世界坐标系：固定不动，全局参考
机器人坐标系：随机器人运动，局部参考
两者之间通过旋转和平移进行转换

好了，现在有两个坐标系了。怎么把一个点从机器人坐标系转换到世界坐标系？

你可能会想：先旋转，再平移。没错，但这里有个坑——如果你先平移再旋转，结果就错了。因为旋转是绕着原点转的，平移之后原点位置变了，旋转效果就不对了。

所以正确的顺序是：先旋转，再平移。

用数学表达就是：

P_world = R * P_robot + t

其中R是旋转矩阵，t是平移向量。

但这样写起来有点麻烦，尤其是你要连续做多次变换的时候。于是就有了齐次变换矩阵。它把旋转和平并到一个4x4的矩阵里：

T = [ R  t ]
    [ 0  1 ]

这样，一个点就可以写成齐次坐标形式 [x, y, z, 1]^T，然后直接乘T就行了：

P_world_homo = T * P_robot_homo

我个人特别喜欢这种写法，因为它把复杂的变换变成了简单的矩阵乘法。而且多个变换连起来，就是矩阵连乘，非常直观。

小技巧： 在代码里，我习惯用Eigen库的Isometry3d来表示齐次变换矩阵。它比手动拼4x4矩阵要安全得多，不容易出错。

说到这，你可能觉得：不就是旋转平移嘛，有什么难的？

嗯，确实。但当你开始做优化的时候，问题就来了——旋转矩阵R是正交矩阵，有9个元素，但自由度只有3。你直接优化R的9个元素，结果很可能得到一个非正交的矩阵，那就不是旋转了。

这时候，李群与李代数就派上用场了。

说白了，李群就是旋转矩阵的集合（SO(3)）和变换矩阵的集合（SE(3)）。它们构成一个群，满足封闭性、结合律、单位元和逆元。

而李代数，是李群在单位元处的切空间。你可以把它理解为“微小扰动”的集合。对于SO(3)，它的李代数是so(3)，对应三维向量（其实就是旋转向量）。

为什么用李代数？因为它在优化里特别好用。你可以在李代数上做加法（微扰），然后通过指数映射回到李群。这样就能保证优化过程中始终得到合法的旋转矩阵。

我记得第一次用g2o做图优化时，看到代码里一堆的SE3Quat、SO3，完全不知道在干嘛。后来才明白，原来这些就是李群和李代数的具体实现。

避坑指南： 我曾经在优化时直接对旋转矩阵的9个元素做梯度下降，结果优化出来的矩阵行列式变成了0.8，完全不是一个合法的旋转。后来改用李代数表示，问题就解决了。所以，千万不要直接优化旋转矩阵的元素。

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

从这张图你可以看到，整个知识体系是层层递进的：从坐标系定义，到齐次变换矩阵，再到李群与李代数。每一步都是为了解决前一步的问题。

最后，分享几个我在实际项目中总结的经验：

  一句话总结： 坐标系是SLAM的基石，齐次变换矩阵是工具，李群与李代数是优化利器。这三者缺一不可。

好了，今天就聊到这。希望这些内容能帮你少走一些弯路。毕竟，坐标系搞错了，后面所有的定位和建图都是白搭。