4、坐标系与变换：世界坐标系、车体坐标系、传感器坐标系，欧拉角与四元数

做传感器融合，说白了就是跟坐标系打交道。我见过不少新手，算法推得头头是道，结果一跑起来数据全飞了——最后发现是坐标系没对齐。嗯，这种坑我踩过不止一次。

这一章我们就把坐标系和旋转变换彻底讲透。你搞懂了这些，后面标定、融合、时序同步才能落地。

4.1 三大坐标系：谁在哪儿，从哪儿看

一个自动驾驶系统里，至少有三个坐标系在同时工作。我习惯把它们分成三层：

这三个坐标系之间靠刚体变换连接。说白了就是旋转加平移。你想想看，一个点在世界坐标系里是 (X, Y, Z)，要变成车体坐标系下的 (x, y, z)，中间就是一个4x4的变换矩阵。

P_body = T_world_to_body * P_world

4.2 欧拉角：直观但危险

欧拉角是我最早接触的旋转表示法。三个角度：横滚（Roll）、俯仰（Pitch）、偏航（Yaw），非常直观。但我要说一句——千万别在融合算法内部用欧拉角。

为什么？两个原因：

1. 万向锁（Gimbal Lock）：当俯仰角接近±90°时，横滚和偏航会失去一个自由度。我曾在无人机项目里遇到过，飞控一做大角度机动，姿态直接崩了。
2. 插值困难：两个欧拉角之间做线性插值，路径不是最短旋转。你想想看，从 (0°, 0°, 0°) 到 (180°, 0°, 0°) 线性插值，中间会绕一个大圈，而不是直接翻转。

欧拉角适合做什么？人机交互和可视化。比如你在调试界面里看车辆姿态，用欧拉角一目了然。但内部计算，请用四元数。

4.3 四元数：优雅但反直觉

四元数这东西，我第一次看的时候也觉得头大。一个标量加三个虚部，怎么就表示旋转了？

其实你不需要深究它的数学推导，记住几个关键点就够了：

• 单位四元数：q = [w, x, y, z]，满足 w² + x² + y² + z² = 1
• 旋转表示：绕单位轴 u 旋转 θ 角，q = [cos(θ/2), u*sin(θ/2)]
• 无万向锁：四元数是4维单位球面上的点，永远不会有奇点
• 插值平滑：用球面线性插值（SLERP），路径是最短旋转

4.4 坐标系变换实战：从传感器到世界

我们来看一个实际场景。车上装了LiDAR和IMU，要把LiDAR点云投影到世界坐标系下。流程是这样的：

1. LiDAR点云在传感器坐标系下：P_lidar
2. 通过外参变换到车体坐标系：P_body = T_lidar_to_body * P_lidar
3. 通过IMU姿态（四元数）变换到世界坐标系：P_world = T_body_to_world * P_body

这里的关键是链式变换。我习惯把所有变换矩阵都写成齐次形式，然后直接乘起来：

// 伪代码示例
// 定义变换链
Matrix4 T_lidar_to_body = getExtrinsic();   // 外参标定结果
Quaternion q_body_to_world = imu.getOrientation(); // IMU四元数
Vector3 t_body_to_world = imu.getPosition();       // IMU位置

// 构建齐次变换
Matrix4 T_body_to_world = quatToMatrix(q_body_to_world);
T_body_to_world.setTranslation(t_body_to_world);

// 最终变换
Matrix4 T_lidar_to_world = T_body_to_world * T_lidar_to_body;

// 应用变换
Point3D p_world = T_lidar_to_world * p_lidar;

4.5 知识结构总览

下面这张图把本章的核心逻辑串起来了。你可以把它当作坐标系变换的“地图”：

4.6 常见问题与避坑