4. 坐标系统与空间变换：世界坐标系、传感器坐标系、刚体变换、四元数与欧拉角
做多传感器融合，绕不开一个核心问题：每个传感器都在用自己的语言描述世界。
激光雷达说「前方3米有个障碍物」，摄像头说「像素坐标(320,240)处有个目标」，IMU说「我感受到的加速度是(0.1, 9.8, 0.2)」。这些数据如果不统一到同一个坐标系下，融合就是空谈。
我个人习惯把坐标系变换比作「翻译官」——你得让不同传感器说同一种语言。今天我们就来聊聊这个翻译过程。
4.1 世界坐标系与传感器坐标系
先搞清楚两个基本概念。
世界坐标系，也叫全局坐标系。它是我们定义整个场景的参考框架。比如自动驾驶中，通常以车辆启动位置为原点，东为X轴，北为Y轴，天为Z轴。这个坐标系一旦定下来，整个融合系统就有了统一的「地图」。
传感器坐标系，是每个传感器自己的小天地。激光雷达的坐标系原点在它的旋转中心，摄像头的光心就是它的坐标系原点。每个传感器测量到的数据，都是相对于它自己的坐标系。
我在项目中遇到过一个问题：两个激光雷达装在车头两侧，它们各自检测到的障碍物位置差了十几厘米。原因很简单——没有把数据从各自的传感器坐标系转换到同一个车体坐标系。嗯，这个坑我踩过。
核心原则：所有传感器数据，最终都要统一到世界坐标系下进行融合。中间可以经过车体坐标系、IMU坐标系等过渡。
4.2 刚体变换：旋转+平移
坐标系之间的变换，说白了就是刚体变换。刚体变换包含两部分：旋转和平移。物体不会变形，只是位置和朝向变了。
数学上，一个三维点从坐标系A变换到坐标系B，可以写成：
P_B = R * P_A + T其中R是3x3的旋转矩阵，T是3x1的平移向量。
你想想看，这个公式其实很直观。先把点绕着原点转一下（R），再挪个位置（T），就完成了坐标系变换。
在实际工程中，我们通常用齐次坐标把旋转和平移合并成一个4x4的变换矩阵：
| P_B | | R T | | P_A |
| 1 | = | 0 1 | * | 1 |这样做的好处是，多个变换可以连乘，形成一个变换链。比如从激光雷达到世界坐标系：
T_world_lidar = T_world_vehicle * T_vehicle_imu * T_imu_lidar我曾经在调试一个多传感器标定系统时，发现变换链中少乘了一个矩阵，结果所有融合数据都偏了30度。找了两天才找到问题。所以，变换链的维护一定要用版本管理，每次修改都要记录。
4.3 欧拉角：直观但危险
旋转矩阵有9个参数，但自由度只有3个。所以工程师们更喜欢用欧拉角——用三个角度来描述旋转：偏航角(Yaw)、俯仰角(Pitch)、横滚角(Roll)。
直观吧？飞机怎么转的，传感器就怎么转的。
但欧拉角有个致命问题：万向锁。当俯仰角达到±90度时，偏航和横滚会失去一个自由度。说白了就是，你没法区分是偏航变了还是横滚变了。
避坑指南：我曾经在无人机项目中用欧拉角做姿态融合，结果飞机做筋斗动作时，姿态解算直接炸了。后来全部改用四元数，问题解决。
所以我的建议是：人机交互用欧拉角（直观），内部计算用四元数（稳定）。
4.4 四元数：稳定但抽象
四元数是个数学工具，形式是 q = w + xi + yj + zk，其中w是实部，(x,y,z)是虚部。它用4个参数表示3个自由度的旋转，避免了万向锁。
四元数的好处：
- 无万向锁：任何姿态都能平滑表示
- 插值平滑：两个姿态之间可以球面线性插值(SLERP)
- 计算高效：比旋转矩阵少乘几次
四元数乘法对应旋转的复合。比如先旋转q1再旋转q2，等效于 q = q2 * q1。注意顺序，四元数乘法不满足交换律。
在实际代码中，我习惯用Eigen或ROS的tf库来处理四元数。手写四元数乘法容易出错，我建议直接用成熟的库。
// C++示例：使用Eigen进行四元数变换
Eigen::Quaterniond q1(0.707, 0.707, 0, 0); // 绕Z轴旋转90度
Eigen::Quaterniond q2(1, 0, 0, 0); // 单位四元数
Eigen::Quaterniond q_combined = q2 * q1; // 复合旋转
// 用四元数旋转一个点
Eigen::Vector3d point(1, 0, 0);
Eigen::Vector3d rotated = q1 * point; // 结果应该是(0, 1, 0)小技巧：四元数一定要归一化！非单位四元数会导致旋转后点的模长发生变化。我见过有人忘了归一化，结果点云越转越远。
4.5 知识体系总览
下面这张图总结了本章的核心逻辑。从传感器原始数据到融合结果，坐标系变换是必经之路。
4.6 实际工程中的选择建议
| 场景 | 推荐表示方式 | 原因 |
|---|---|---|
| 人机交互界面 | 欧拉角 | 直观，容易理解 |
| 姿态融合滤波 | 四元数 | 无万向锁，插值平滑 |
| 坐标变换链 | 齐次变换矩阵 | 便于连乘和求逆 |
| 标定优化 | 旋转向量/轴角 | 参数少，优化稳定 |
最后说一句：坐标系变换是融合系统的地基。地基不稳，上层算法再牛也白搭。我建议你在项目初期就把变换关系理清楚，画个变换树，标定好每个外参。别像我当年那样，等到数据对不上了才回头查变换链。
嗯，今天就聊到这里。坐标系变换这块，多动手算几次就熟了。
公众号:蓝海资料掘金营，微信deep3321