3、坐标系统与变换：世界坐标系、车身坐标系、传感器坐标系、欧拉角与四元数、坐标变换矩阵

做多传感器融合，说白了就是让不同传感器「说同一种语言」。你想想看，激光雷达看到的是自己坐标系下的点云，IMU输出的是车身姿态，摄像头感知的是像素平面——这些数据如果不统一到一个坐标系下，融合就是空谈。

我个人习惯，拿到一个融合项目，第一件事不是调算法，而是先把坐标系理清楚。坐标系乱了，后面全是坑。

3.1 三大坐标系：你总得有个「家」

多传感器融合里，我们通常跟三个坐标系打交道：

• 世界坐标系（World Frame）：绝对参考系，通常用W表示。比如GPS的经纬高、UTM坐标，或者你自定义的一个固定原点。它是所有数据的最终归宿。
• 车身坐标系（Body Frame）：固定在车辆/机器人上的坐标系，用B表示。原点一般在质心或后轴中心，x轴朝前，y轴朝左（或右），z轴朝上。IMU、轮速计的数据天然就在这个系里。
• 传感器坐标系（Sensor Frame）：每个传感器自己的小天地，用S表示。比如激光雷达的坐标系原点在旋转中心，摄像头在光心。

我记得有一次做无人车项目，团队里有人把激光雷达的安装位置算反了，结果点云地图里的障碍物全偏了半米。排查了两天才发现——坐标系标定错了。嗯，从那以后我要求每个传感器上车前，必须做一次「坐标系一致性检查」。

3.2 欧拉角：直观但危险

欧拉角用三个角度描述旋转：横滚（Roll）、俯仰（Pitch）、偏航（Yaw）。

为什么说它直观？因为你可以直接读出来：车头偏了多少度、车身侧倾了多少度。但为什么说它危险？

• 万向锁（Gimbal Lock）：当俯仰角接近±90°时，横滚和偏航会耦合，丢失一个自由度。我在做无人机飞控时遇到过这个问题，飞机垂直爬升时姿态解算突然炸了——就是万向锁搞的鬼。
• 多值性：同一个旋转可以用多组欧拉角表示，比如(30°, 20°, 10°)和(30°+360°, 20°, 10°)是同一个姿态。
• 插值困难：两个欧拉角之间做线性插值，得到的路径往往不是最短旋转路径。

3.3 四元数：低调的实力派

四元数用四个数表示旋转：q = w + xi + yj + zk，其中w是实部，(x,y,z)是虚部。它没有万向锁问题，插值平滑，计算效率高。

你可能会问：「四个数怎么表示三维旋转？」

其实很简单：一个四元数代表绕某个轴旋转某个角度。轴是单位向量(x,y,z)，角度是θ，那么四元数就是：

q = [cos(θ/2), x*sin(θ/2), y*sin(θ/2), z*sin(θ/2)]

注意，这里w = cos(θ/2)，不是θ本身。我刚开始学的时候老搞混，后来记住一个口诀：「实部是半角余弦，虚部是半角正弦乘轴」。

四元数有几个重要性质：

• 单位四元数：w² + x² + y² + z² = 1，表示纯旋转（无缩放）。
• 共轭：q* = [w, -x, -y, -z]，表示反向旋转。
• 乘法：q1 * q2 表示先做q2旋转，再做q1旋转（注意顺序！）。

3.4 坐标变换矩阵：把数据搬个家

坐标变换矩阵是一个4×4的齐次矩阵，包含旋转和平移：

T = [ R   t ]
    [ 0   1 ]

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

举个例子，把激光雷达的点云从传感器坐标系变换到车身坐标系：

P_body = T_sensor_to_body * P_sensor

这里的T_sensor_to_body就是传感器相对于车身的安装位置和姿态。安装位置用尺子量，安装姿态用标定板算。

我建议你记住几个常用变换：

• 欧拉角→旋转矩阵：按ZYX顺序（偏航→俯仰→横滚）乘起来。
• 四元数→旋转矩阵：有标准公式，网上随便查。
• 旋转矩阵→四元数：从矩阵的迹和元素反推。

3.5 知识体系总览

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

3.6 实战中的坐标系管理

我建议你在代码里这样组织坐标系：

1. 定义枚举：用枚举列出所有坐标系，比如WORLD、BODY、LIDAR_FRONT、CAMERA_LEFT等。
2. 维护变换树：用一个字典或列表存储所有坐标系之间的变换关系。每次有新传感器加入，就更新这个树。
3. 统一接口：写一个TransformPoint函数，输入源坐标系、目标坐标系、点坐标，输出变换后的点。

好了，坐标系这块就聊到这儿。记住一句话：坐标系不乱，融合就成功了一半。

可可精品资料库