第一章 坐标系与刚体运动：世界坐标系、车体坐标系、传感器坐标系、欧拉角、四元数、旋转矩阵
做多传感器融合，第一道坎就是坐标系。
我记得刚入行那会儿，被各种坐标系绕得晕头转向。明明传感器数据都对，一融合就炸。后来才发现，是坐标系没对齐。说白了，坐标系就是给空间里的点一个「身份证」，让不同传感器能说同一种语言。
1.1 世界坐标系：一切测量的基准
世界坐标系，也叫全局坐标系。它是我们描述整个场景的「绝对」参考系。通常用 W 表示，比如 W系。
在自动驾驶里，世界坐标系的原点一般选在车辆启动的位置，或者某个固定的地理点。X轴指向东，Y轴指向北，Z轴指向天——也就是我们常说的 ENU 坐标系。
为什么需要它？因为所有传感器最终都要把数据转换到世界坐标系下，才能做全局的定位和建图。你想想看，激光雷达扫到的点，如果不在同一个世界坐标系下，那地图就是散的。
1.2 车体坐标系：车辆自身的「视角」
车体坐标系，也叫载体坐标系，通常用 B 表示。它固定在车辆上，随车辆一起运动。
一般定义：
- X轴：指向车辆前方
- Y轴：指向车辆左侧（或右侧，看具体约定）
- Z轴：指向车辆上方
我在项目中遇到过一个问题：IMU 的数据是在车体坐标系下的，但 GPS 的数据是在世界坐标系下的。如果不做转换，直接拿来用，定位结果会飞掉。嗯，这里要注意，车体坐标系是「动」的，世界坐标系是「静」的。
1.3 传感器坐标系：每个传感器都有自己的「小世界」
每个传感器都有自己的坐标系。比如：
- 相机坐标系：光心为原点，Z轴指向镜头前方
- 激光雷达坐标系：通常以激光发射点为原点，X轴指向车头方向
- IMU坐标系：以IMU芯片中心为原点，三轴方向由芯片封装决定
这些传感器坐标系之间，以及它们与车体坐标系之间，存在固定的旋转和平移关系。这个关系叫做「外参」。
1.4 旋转矩阵：最直观的旋转表示
旋转矩阵是一个 3x3 的正交矩阵，行列式为 +1。它描述了一个坐标系相对于另一个坐标系的旋转关系。
比如，从车体坐标系到世界坐标系的旋转，可以写成 R_WB。一个点 P 在车体坐标系下的坐标是 P_B，那么它在世界坐标系下的坐标就是：
P_W = R_WB * P_B旋转矩阵的好处是直观，可以直接用于向量旋转。但它有 9 个参数，存在冗余（实际只有 3 个自由度）。而且，多次旋转后，数值误差会导致矩阵不再正交——这就是我常说的「矩阵漂移」问题。
1.5 欧拉角：直观但容易踩坑
欧拉角用三个角度表示旋转：横滚角（roll）、俯仰角（pitch）、偏航角（yaw）。
听起来很直观对吧？但这里有个大坑——万向锁。当俯仰角接近 ±90° 时，横滚和偏航会变得无法区分，自由度丢失。
我在做无人机姿态估计时，就遇到过这个问题。飞机垂直爬升时，俯仰角接近 90°，欧拉角直接失效。从那以后，我内部计算一律用四元数，只在可视化或人机交互时才转成欧拉角。
| 表示方式 | 优点 | 缺点 |
|---|---|---|
| 旋转矩阵 | 直观，可直接用于向量旋转 | 冗余参数，需正交化 |
| 欧拉角 | 三个参数，物理意义明确 | 万向锁，插值困难 |
| 四元数 | 无奇异性，适合插值 | 不够直观 |
1.6 四元数：SLAM 里的「真香」选择
四元数是一个超复数，形式为 q = w + xi + yj + zk，其中 w 是实部，x、y、z 是虚部。它用 4 个参数表示旋转，没有奇异性，而且插值平滑。
在 SLAM 里，我几乎只用四元数做姿态优化。为什么？因为四元数乘法可以方便地组合旋转，而且求导也简单。
举个例子，两个旋转 q1 和 q2 的复合，就是四元数乘法：
q = q1 * q2注意顺序！四元数乘法不满足交换律，q1 * q2 和 q2 * q1 是不同的。
.toRotationMatrix() 方法。千万别手写公式，容易出错。我刚开始就吃过这个亏。
1.7 知识体系总览
下面这张图，是我自己总结的坐标系与刚体运动知识结构。你可以把它当作本章的「地图」：
1.8 实践建议
说了这么多理论，来点实际的。我个人建议，在工程中遵循以下原则：
- 统一坐标系约定：整个团队使用相同的坐标系定义，比如 ENU 世界系、前左上车体系。避免「你的 X 轴是我的 Y 轴」这种低级错误。
- 内部计算用四元数：优化、滤波、插值，全部用四元数。只在最后输出或显示时转成欧拉角。
- 做好坐标变换树：用 TF 树（ROS）或类似工具，管理所有坐标系之间的关系。每次新增传感器，先标定外参，再注册到变换树中。
- 写单元测试：我每次写完坐标变换代码，都会写一个测试用例：给一个已知点，手动算一遍，再和代码结果对比。确保万无一失。
公众号:蓝海资料掘金营，微信deep3321