第三章：坐标系统与变换
各位同学，今天我们来聊聊传感器融合里最基础、也最容易踩坑的一个话题——坐标系统与变换。
说实话，我刚开始做融合算法时，觉得坐标变换不就是矩阵乘来乘去嘛，有什么难的？结果第一次实车测试，数据全乱套了。后来排查了一整天，发现是某个传感器的坐标系定义反了。嗯，从那以后，我再也不敢小看这个环节了。
3.1 为什么需要多个坐标系？
你想想看，一辆车上装了激光雷达、摄像头、IMU、GPS……每个传感器都有自己的“视角”。摄像头看到的是像素坐标，激光雷达返回的是极坐标点云，IMU测量的是加速度和角速度，GPS给的是经纬度。
这些数据要融合在一起，就必须统一到一个“共同语言”里。这个共同语言，就是坐标系。
我个人习惯把坐标系分成三个层次：
- 世界坐标系：全局的、固定的参考系。比如WGS-84经纬度坐标系，或者UTM投影坐标系。
- 车身坐标系：以车辆中心为原点，通常X轴朝前，Y轴朝左，Z轴朝上。这是车辆自身的“身体感知”。
- 传感器坐标系：每个传感器自己的小天地。比如激光雷达装在车顶，它的原点就在车顶那个位置。
我在项目中遇到过最典型的问题：有人直接把激光雷达的点云当成车身坐标系的数据用，结果障碍物的位置全偏了。为什么？因为激光雷达装在车顶右前方，它的原点跟车辆中心差了1.5米啊！
3.2 欧拉角：直观但危险
欧拉角是最直观的旋转表示方式。说白了，就是用三个角度来描述一个物体的朝向：绕X轴转多少（横滚角Roll）、绕Y轴转多少（俯仰角Pitch）、绕Z轴转多少（偏航角Yaw）。
听起来很简单对吧？但这里有个大坑——万向锁。
欧拉角的另一个问题是：旋转顺序很重要。同样的三个角度，按XYZ顺序旋转和按ZYX顺序旋转，结果完全不同。所以你在看算法文档时，一定要搞清楚它用的是哪种顺序。
| 旋转顺序 | 常见应用场景 | 注意事项 |
|---|---|---|
| ZYX（偏航→俯仰→横滚） | 车辆、无人机姿态 | 最常用，符合直觉 |
| XYZ（横滚→俯仰→偏航） | 机械臂、机器人 | 容易产生万向锁 |
| ZXY | 某些IMU厂商默认 | 需要查阅数据手册 |
3.3 四元数：优雅的解决方案
四元数是什么？你可以把它想象成一个“带方向的旋转轴加上一个旋转角度”。它用四个数来表示旋转：一个实部和三个虚部。
为什么四元数比欧拉角好？三个理由：
- 无万向锁：怎么转都不会出现奇异点
- 便于插值：做平滑旋转时非常自然
- 计算稳定：数值误差不会累积
我建议所有做传感器融合的同学，都养成用四元数的习惯。虽然它不如欧拉角直观，但写代码时你会感谢自己的。
来看一个简单的四元数转欧拉角的代码示例：
// 四元数转欧拉角（ZYX顺序）
void quaternionToEuler(float qw, float qx, float qy, float qz,
float& roll, float& pitch, float& yaw) {
// 计算俯仰角，注意asin的范围限制
float sinr_cosp = 2.0f * (qw * qx + qy * qz);
float cosr_cosp = 1.0f - 2.0f * (qx * qx + qy * qy);
roll = atan2f(sinr_cosp, cosr_cosp);
// 俯仰角，这里要防止数值越界
float sinp = 2.0f * (qw * qy - qz * qx);
if (fabsf(sinp) >= 1.0f)
pitch = copysignf(M_PI / 2.0f, sinp); // 处理万向锁
else
pitch = asinf(sinp);
// 偏航角
float siny_cosp = 2.0f * (qw * qz + qx * qy);
float cosy_cosp = 1.0f - 2.0f * (qy * qy + qz * qz);
yaw = atan2f(siny_cosp, cosy_cosp);
}3.4 坐标变换矩阵
坐标变换矩阵，说白了就是一个4x4的矩阵，把旋转和平移打包在一起。它的结构是这样的：
| R11 R12 R13 Tx |
| R21 R22 R23 Ty |
| R31 R32 R33 Tz |
| 0 0 0 1 |左上角3x3是旋转矩阵，右上角3x1是平移向量。最后一行永远是[0,0,0,1]，这是齐次坐标的固定格式。
为什么要用4x4矩阵而不是分开算？因为矩阵乘法可以一次搞定旋转+平移，而且可以链式变换。比如你要把激光雷达的点云转到世界坐标系：
P_world = T_world_to_vehicle * T_vehicle_to_lidar * P_lidar这里T_world_to_vehicle是车身到世界的变换，T_vehicle_to_lidar是激光雷达到车身的变换。两个矩阵一乘，就得到了从激光雷达直接到世界的变换矩阵。
核心要点：坐标变换的本质就是“换个角度看世界”。你只需要记住：
- 每个传感器都有自己的坐标系
- 用变换矩阵把它们统一到车身坐标系
- 再用车身到世界的变换，把数据放到全局地图里
3.5 实战中的坐标系标定
理论说完了，咱们聊聊实际干活时要注意什么。
我记得有一次做多传感器融合，激光雷达和摄像头的数据总是对不上。明明标定过了，但投影到图像上就是有偏差。后来发现，是标定板的摆放位置太近了，导致外参矩阵的平移分量误差很大。
这里给大家几个建议：
- 标定距离要覆盖实际工作范围：比如你的激光雷达最远探测100米，那标定板至少要放到50米外
- 多角度采集数据：不要只在一个位置标定，前后左右都来几组
- 验证标定结果：标定完一定要用独立的数据集验证，看看投影误差是否在可接受范围内
另外，坐标系的方向定义一定要统一。我见过有的团队用“X前Y左Z上”，有的用“X右Y前Z上”，结果两个模块的数据一合并，车辆位置直接飞了。所以，项目一开始就要定好坐标系规范，写在文档里，贴在墙上。
好了，这一章的内容就到这里。坐标变换看起来简单，但它是传感器融合的基石。你想想看，如果连数据在哪个坐标系下都搞不清楚，后面的融合算法再漂亮也是白搭。
下一章我们会讲传感器的时间同步问题，那也是另一个容易踩坑的地方。到时候见。