3. 坐标系与变换：机器人坐标系、世界坐标系、欧拉角与四元数
好，咱们进入第三章。坐标系与变换，说白了就是解决一个核心问题：“我在哪？周围的东西在哪？”
做SLAM这么多年，我见过太多新手一上来就对着坐标变换公式发懵。其实没那么玄乎。你想想看，我们每天走路、拿杯子，大脑都在做坐标变换——只是你自己没意识到罢了。
这一章，我会把机器人坐标系、世界坐标系、欧拉角、四元数这几个东西串起来讲。保证你听完就能上手用。
3.1 机器人坐标系 vs 世界坐标系
先搞清楚两个基本概念。
世界坐标系，就是全局的、固定不动的参考系。你可以把它想象成房间的墙角——原点定在那，X轴朝东，Y轴朝北，Z轴朝天。不管机器人怎么动，这个坐标系纹丝不动。
机器人坐标系，也叫本体坐标系，是跟着机器人走的。原点在机器人中心，X轴朝前，Y轴朝左，Z轴朝上。机器人转个身，这个坐标系也跟着转。
我在项目里遇到过一个问题：有个同学把激光雷达的数据直接当成世界坐标来用，结果地图全歪了。为什么？因为激光雷达的数据是在机器人坐标系下采集的，你得先把它变换到世界坐标系才行。
核心原则：所有传感器数据，最终都要统一到同一个坐标系下处理。要么都转到世界系，要么都转到机器人系。千万别混着用。
举个具体例子。激光雷达扫到一个点，距离是2米，角度是30度。在机器人坐标系下，这个点的坐标是：
x_robot = 2 * cos(30°) ≈ 1.732 米
y_robot = 2 * sin(30°) = 1.0 米如果机器人当前在世界坐标系下的位置是 (1, 1)，朝向是0度，那这个点在世界坐标系下就是：
x_world = 1 + 1.732 = 2.732 米
y_world = 1 + 1.0 = 2.0 米嗯，就是这么简单。但实际项目中，机器人还会旋转，那就得用到旋转矩阵了。
3.2 旋转的三种表示方式
旋转怎么表示？有三种主流方式：旋转矩阵、欧拉角、四元数。我一个个说。
3.2.1 旋转矩阵
旋转矩阵是个3x3的矩阵，用来描述一个坐标系相对于另一个坐标系的旋转关系。它的好处是直观，可以直接乘向量。但缺点也很明显——参数太多，9个数，还有正交约束，优化起来麻烦。
我个人习惯，在代码里很少直接用旋转矩阵做运算，除非是底层库已经封装好了。
3.2.2 欧拉角
欧拉角用三个角度来描述旋转：绕X轴转的叫横滚角（roll），绕Y轴转的叫俯仰角（pitch），绕Z轴转的叫偏航角（yaw）。
在SLAM里，我们最常用的是偏航角yaw，因为机器人基本在平面上运动，横滚和俯仰变化不大。
注意：欧拉角有个致命问题——万向锁。当俯仰角接近±90度时，横滚和偏航会失去一个自由度。我曾经在调试无人机时被这个坑过，姿态解算突然就炸了。从那以后，我只要涉及三维旋转，一律用四元数。
3.2.3 四元数
四元数是个超复数，形式是 q = w + xi + yj + zk。它用四个数表示旋转，没有万向锁问题，插值也平滑。
你可能会问：四个数比三个数多一个，为什么反而更好？因为欧拉角的三个角度不是独立的，它们之间有耦合。四元数的四个数虽然多了一个，但约束条件简单——模长为1就行。
看个例子，用四元数表示绕Z轴转90度：
q = [cos(45°), 0, 0, sin(45°)]
= [0.707, 0, 0, 0.707]在代码里，我建议直接用Eigen或Sophus库，别自己手写四元数乘法。容易出错。
3.3 坐标变换的数学表达
一个完整的坐标变换，包括旋转和平移。用公式表示就是：
P_world = R * P_robot + t其中R是旋转矩阵，t是平移向量。反过来，从世界系到机器人系：
P_robot = R^T * (P_world - t)这里R^T是R的转置，因为旋转矩阵是正交矩阵，逆等于转置。
为了计算方便，我们通常用齐次坐标，把旋转和平并到一个4x4的变换矩阵里：
T = [R t]
[0 1]这样，一次矩阵乘法就能完成变换：
[P_world] = T * [P_robot]
[ 1 ] [ 1 ]小技巧：在写代码时，尽量用齐次坐标。虽然多了一维，但代码更简洁，不容易出错。我习惯把所有点都存成4维向量，最后一维是1。
3.4 坐标系变换在SLAM中的应用
在激光SLAM前端，坐标系变换无处不在。我挑两个典型场景说说。
场景一：激光点云畸变矫正
激光雷达扫描一圈需要时间，如果机器人在这段时间内运动了，点云就会发生畸变。怎么矫正？把每个激光点从它被采集时的机器人坐标系，变换到扫描起始时刻的机器人坐标系。
这需要知道每个点对应的机器人位姿。一般通过IMU或轮式里程计插值得到。
场景二：多传感器融合
激光雷达、IMU、相机，每个传感器都有自己的坐标系。你得先做外参标定，得到它们之间的变换关系。然后，把所有数据统一到机器人坐标系下。
我记得有一次做多传感器融合，激光雷达和IMU的外参标定差了2厘米，结果建图精度一直上不去。折腾了两天才找到原因。所以，外参标定一定要仔细，差之毫厘，谬以千里。
3.5 本章知识体系
下面这张图，把本章的核心逻辑串起来了。你可以看到，从传感器数据到最终的地图，坐标系变换贯穿始终。
3.6 避坑指南
最后，我把自己踩过的坑总结一下，你遇到了能少走弯路。
- 坐标系方向搞反：我曾经把机器人坐标系下的点直接当成世界坐标用，结果地图全反了。记住：先旋转，再平移。顺序不能错。
- 欧拉角顺序混淆：不同库的欧拉角定义不一样。Eigen是ZYX顺序，ROS是ZYX但绕固定轴。用之前一定要确认。
- 四元数归一化：四元数运算后模长会变，记得归一化。我见过有人忘了这步，结果旋转越来越离谱。
- 齐次坐标最后一维：平移时最后一维是1，旋转时最后一维是0。搞混了会得到奇怪的结果。
嗯，这一章就到这里。坐标系变换是SLAM的基石，看似简单，但细节很多。你动手写几行代码，跑几个例子，很快就熟了。
公众号:蓝海资料掘金营，微信deep3321