第三章 坐标系与空间变换：世界坐标系、车身坐标系、传感器坐标系，以及欧拉角、四元数、旋转矩阵的转换
做多传感器融合，说白了就是跟坐标系打交道。我刚开始入行那会儿，觉得这玩意儿不就是个坐标嘛，有啥好学的？结果第一次做激光雷达和摄像头联合标定，数据怎么都对不上，折腾了两天，最后发现是坐标系定义反了。嗯，从那以后我再也不敢小看坐标系了。
这一章，咱们就把坐标系和空间变换彻底讲透。你想想看，一个激光雷达点云，一个摄像头图像，一个IMU的加速度数据，它们各自在自己的小世界里都好好的。但要让它们协同工作，就必须把数据统一到同一个坐标系下。这就是坐标系变换的核心意义。
3.1 三种核心坐标系
多传感器融合系统里，我们主要跟三种坐标系打交道。我个人习惯把它们分成三个层次来理解：
- 世界坐标系：也叫全局坐标系。它是固定不动的，通常以地球上的某个点为原点。比如GPS用的WGS-84坐标系，或者我们做室内定位时自己定义的一个墙角。所有传感器的数据最终都要转换到这个坐标系下，才能进行全局的路径规划和地图构建。
- 车身坐标系：也叫载体坐标系。原点在车辆或机器人的质心（或者后轴中心），x轴朝前，y轴朝左，z轴朝上。IMU和轮速计的数据天然就在这个坐标系下。我在做自动驾驶项目时，车身坐标系是用的最多的，因为大部分控制算法都在这个坐标系里跑。
- 传感器坐标系：每个传感器都有自己的小世界。激光雷达的原点在它的旋转中心，摄像头的光心就是它的原点。这些传感器的数据必须先转换到车身坐标系，才能进一步融合。
核心原则：所有传感器数据，最终都要统一到同一个坐标系下。要么都转到世界坐标系，要么都转到车身坐标系。千万别混着用，否则你会哭的。
3.2 欧拉角：最直观但最坑爹
欧拉角是描述旋转最直观的方式。三个角度：偏航角（Yaw）、俯仰角（Pitch）、横滚角（Roll）。你想象一下飞机在空中转，偏航就是左右转头，俯仰就是抬头低头，横滚就是侧身翻滚。
但是！欧拉角有个大坑——万向锁。我曾经在一个无人机项目里，用欧拉角做姿态控制，结果飞机在俯仰90度的时候突然失控了。为什么？因为当俯仰角接近±90度时，偏航和横滚的旋转轴重合了，系统丢失了一个自由度。这就是万向锁。
避坑指南：我曾经在IMU数据融合时直接用欧拉角做插值，结果姿态跳得跟抽风一样。后来才明白，欧拉角不适合做插值和迭代计算。如果你要做连续旋转或者姿态平滑，请用四元数。
3.3 旋转矩阵：数学上最严谨
旋转矩阵是一个3x3的正交矩阵，行列式为1。它描述了一个坐标系相对于另一个坐标系的旋转关系。比如，从传感器坐标系到车身坐标系的旋转矩阵R_sb，满足：
p_b = R_sb * p_s其中p_s是传感器坐标系下的点，p_b是车身坐标系下的点。
旋转矩阵的好处是：
- 没有奇异性，不会出现万向锁
- 可以连续相乘，组合多个旋转
- 数学性质好，求逆就是转置
但缺点也很明显：9个参数，冗余了。而且你想想看，每次更新姿态都要维护一个3x3矩阵，计算量不小。我在嵌入式平台上做实时融合时，一般不会直接用旋转矩阵做状态更新，而是用四元数。
3.4 四元数：工程上的最优解
四元数是个神奇的东西。它用四个参数（一个实部，三个虚部）来表示旋转。形式上像这样：
q = w + xi + yj + zk其中w是实部，x、y、z是虚部，且满足w² + x² + y² + z² = 1。
我个人最喜欢四元数，原因有三：
- 无奇异性：不会出现万向锁
- 插值平滑：可以用球面线性插值（SLERP）做平滑旋转
- 计算高效：比旋转矩阵少5个参数，乘法也更快
我记得在做IMU和视觉融合时，状态向量里就包含四元数。每次预测和更新，四元数都能保持单位长度，不会像欧拉角那样漂移。
实用技巧：如果你在代码里用四元数，记得每次更新后做归一化。否则累积误差会让四元数偏离单位球面，导致旋转失真。我一般会在每个融合周期末尾加一行：q = q / norm(q)。
3.5 三种表示方式的转换
实际工程中，这三种表示方式经常需要互相转换。下面这张表总结了转换关系：
| 转换方向 | 公式/方法 | 注意事项 |
|---|---|---|
| 欧拉角 → 旋转矩阵 | 按ZYX顺序连乘三个基本旋转矩阵 | 注意旋转顺序，不同领域习惯不同 |
| 旋转矩阵 → 欧拉角 | 从矩阵元素反解角度 | 注意万向锁时的特殊处理 |
| 欧拉角 → 四元数 | 用半角公式组合三个轴的旋转 | 同样注意旋转顺序 |
| 四元数 → 旋转矩阵 | 用四元数元素构造矩阵 | 确保四元数已归一化 |
| 旋转矩阵 → 四元数 | 从矩阵迹和元素求解 | 注意数值稳定性，避免除零 |
这里我给出一个常用的C++代码片段，展示四元数转旋转矩阵的实现：
// 四元数转旋转矩阵
Eigen::Matrix3d quatToRotMat(const Eigen::Quaterniond& q) {
Eigen::Matrix3d R;
double w = q.w(), x = q.x(), y = q.y(), z = q.z();
R(0,0) = 1 - 2*y*y - 2*z*z;
R(0,1) = 2*x*y - 2*w*z;
R(0,2) = 2*x*z + 2*w*y;
R(1,0) = 2*x*y + 2*w*z;
R(1,1) = 1 - 2*x*x - 2*z*z;
R(1,2) = 2*y*z - 2*w*x;
R(2,0) = 2*x*z - 2*w*y;
R(2,1) = 2*y*z + 2*w*x;
R(2,2) = 1 - 2*x*x - 2*y*y;
return R;
}3.6 坐标系变换的完整流程
在实际的多传感器融合系统中，坐标系变换通常分三步走：
- 传感器内参标定：确定传感器自身的内部参数，比如相机的焦距、畸变系数，激光雷达的扫描角度范围等。这一步通常在出厂时或实验室里完成。
- 外参标定：确定传感器坐标系相对于车身坐标系的旋转和平移。这就是我们常说的「标定」。我做过最头疼的一次标定，是在一辆卡车上装6个激光雷达和4个摄像头，光对齐就花了一周。
- 数据同步与变换：在运行时，每个传感器数据到达后，先根据时间戳做同步，然后用外参矩阵把数据从传感器坐标系变换到车身坐标系，最后再根据车辆位姿变换到世界坐标系。
关键公式：从传感器坐标系到世界坐标系的完整变换：
p_world = R_wb * (R_bs * p_sensor + t_bs) + t_wb其中R_wb和t_wb是车身在世界坐标系下的位姿，R_bs和t_bs是传感器在车身坐标系下的外参。
3.7 工程中的常见坑
做坐标系变换，有几个坑我几乎每次都会遇到：
- 旋转顺序搞反：不同领域对欧拉角的旋转顺序定义不同。航空航天用ZYX，机器人学用XYZ。我建议你在代码里明确注释旋转顺序，或者干脆只用四元数。
- 左手系右手系混用：OpenCV是右手系，ROS是右手系但z轴朝上，有些游戏引擎是左手系。转换前一定要确认。
- 单位问题：平移向量的单位是米还是毫米？角度是弧度还是度？我曾经因为忘记把角度转弧度，导致融合结果差了57倍。
- 时间戳对齐：不同传感器的数据到达时间不同。如果你用上一个时刻的位姿去变换当前时刻的传感器数据，结果会差很多。我一般会用IMU做时间戳插值。
好了，这一章的内容就到这里。坐标系变换是融合系统的地基，地基不稳，上层算法再牛也白搭。下一章我们会讲传感器的时间同步，这也是个让人头大的问题。到时候见。