2、坐标系与刚体变换：世界坐标系、机器人坐标系、齐次坐标与SE(3)变换

做SLAM也好，搞自动驾驶也罢，你绕不开的第一个坎儿就是坐标系。我刚开始入行那会儿，总觉得坐标系这东西太简单了，不就是三个轴嘛。结果第一次做多传感器融合，激光雷达和相机对不上，折腾了两天才发现是坐标系定义搞反了。嗯，从那以后我再也不敢小看坐标系了。

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

说白了，世界坐标系就是一个固定的参考系。你想象一下，你在一个房间里，房间的墙角就是世界坐标系的原点。不管机器人怎么动，这个坐标系纹丝不动。我们通常用 W 或 world 来表示它。

机器人坐标系就不一样了。它跟着机器人走，原点在机器人的中心，一般叫 body 或 base_link。我习惯把机器人坐标系叫做 B 系。你想想看，机器人往前走，在B系里看，前方永远是x轴正方向。但在世界系里看，它的方向一直在变。

为什么会这样？因为每个传感器都有自己的坐标系。激光雷达有 lidar 系，IMU有 imu 系，相机有 camera 系。你如果不把它们对齐，数据就是乱的。

2.2 齐次坐标：为什么需要它？

你可能会问，三维空间里的点用 (x, y, z) 不就够了？为什么还要搞个齐次坐标？

我举个例子。假设你要把点 p 先旋转再平移。用普通坐标，你得写成：

p' = R * p + t

这看起来还行。但如果你连续做多次变换呢？

p'' = R2 * (R1 * p + t1) + t2

这就开始乱了。更麻烦的是，你没法把旋转和平移写成一个矩阵乘法。齐次坐标就是来解决这个问题的。

齐次坐标给三维点加了一个维度，变成 (x, y, z, 1)。这样旋转和平移就能合并成一个 4x4 的变换矩阵：

| R  t |   | x |   | x' |
| 0  1 | * | y | = | y' |
            | z |   | z' |
            | 1 |   | 1  |

你看，一次矩阵乘法搞定所有事。我在项目中经常用这个技巧，尤其是在做多传感器标定时，能把代码写得特别干净。

2.3 SE(3) 变换：刚体运动的数学表达

SE(3) 这个名字听起来挺唬人。其实它就是 Special Euclidean Group 的缩写，翻译过来就是「特殊欧几里得群」。说白了，它描述的就是刚体在三维空间里的运动——旋转加平移。

一个 SE(3) 变换矩阵长这样：

T = | R  t |
    | 0  1 |

其中 R 是 3x3 的旋转矩阵，t 是 3x1 的平移向量。这个矩阵有几个重要性质：

• 可逆性：T^{-1} 存在，表示逆变换
• 封闭性：两个 SE(3) 矩阵相乘，结果还是 SE(3)
• 结合律：(T1 * T2) * T3 = T1 * (T2 * T3)

我记得有一次做轨迹评估，需要把不同时刻的位姿对齐。当时我直接用欧拉角做插值，结果轨迹出现了奇怪的抖动。后来换成 SE(3) 的插值方法，问题就解决了。你想想看，旋转矩阵本身是正交的，直接线性插值会破坏这个性质，导致结果不合法。

2.4 坐标系变换的链式法则

在实际系统中，坐标系变换往往是链式的。比如你要把激光雷达的点云转换到世界坐标系：

T_world_lidar = T_world_body * T_body_lidar

这里的 T_world_body 是机器人的位姿，T_body_lidar 是激光雷达相对于机器人的外参。我习惯用下标来表示变换的起点和终点，这样不容易搞混。

链式法则的核心就是：下标要对齐。你看上面的式子，body 在中间，正好消掉了。如果写反了，结果就是错的。

我给大家画个图，把整个坐标系变换的逻辑理清楚：

2.5 实践中的坐标系管理

在实际项目中，我一般会这样做：

1. 统一命名规范：所有坐标系都用 frame_xxx 命名，比如 frame_world、frame_base、frame_lidar
2. 维护变换树：用 ROS 的 tf 树或者自己写一个变换管理器，保证每个时刻都能查到任意两个坐标系之间的变换
3. 写单元测试：每次标定完外参，我都会写一个测试用例，验证变换的正确性。比如把一个点从传感器系转到世界系，再转回来，看是不是同一个点

好了，坐标系和刚体变换就讲到这里。记住一句话：坐标系是SLAM的基石，SE(3)是刚体运动的通用语言。搞懂了这些，后面的轨迹评估才能站得住脚。