2. 坐标系与姿态表示：地理坐标系与载体坐标系、欧拉角、四元数基础、旋转矩阵与坐标变换
各位同学，欢迎来到实战案例的第一节硬核内容。
做车载定位，说白了就是回答三个问题：我在哪？我朝哪？我动得怎么样？ 而回答这些问题，首先得把「坐标系」这玩意儿搞明白。我见过不少新手，算法写得飞起，结果坐标系搞反了，整个系统直接崩掉。嗯，今天我们就来把这几个基础概念彻底理清。
2.1 两个核心坐标系：地理坐标系与载体坐标系
先说说最常用的两个坐标系。
地理坐标系（n系，Navigation Frame），也叫导航坐标系。我习惯用 ENU（东-北-天） 坐标系：
- X轴：指向东（East）
- Y轴：指向北（North）
- Z轴：指向天顶（Up）
为什么用ENU？因为直观。你开车往东走，X轴数据就变大；往北走，Y轴数据就变大。我在做高精度地图匹配时，ENU坐标系下的计算最顺手。
载体坐标系（b系，Body Frame），是固定在车上的坐标系：
- X轴：车头方向（前进方向）
- Y轴：车身右侧
- Z轴：垂直车身向下（符合右手定则）
这里有个坑——Z轴方向。有些IMU厂家定义Z轴向上，有些向下。我曾经在项目里没仔细看手册，直接把IMU数据拿来用，结果航向角差了180度。嗯，后来我养成了一个习惯：拿到IMU第一件事，就是手动旋转一下，看三个轴的输出变化，确认坐标系定义。
核心要点： 所有传感器数据最终都要统一到同一个坐标系下。我建议在代码里用枚举类型明确标注每个数据的坐标系，避免混淆。
2.2 欧拉角：横滚、俯仰、航向
欧拉角是描述姿态最直观的方式。说白了，就是告诉你车「歪了多少」。
| 角度 | 英文 | 含义 | 范围 |
|---|---|---|---|
| 横滚角 (Roll) | φ | 绕X轴旋转，车身左右倾斜 | -180° ~ 180° |
| 俯仰角 (Pitch) | θ | 绕Y轴旋转，车头抬头/低头 | -90° ~ 90° |
| 航向角 (Yaw) | ψ | 绕Z轴旋转，车头朝向 | 0° ~ 360° |
你想想看，开车上坡时，俯仰角就变了；过弯时，航向角在变化；路面有侧倾，横滚角就出来了。这三个角度组合起来，就能完整描述车身的姿态。
⚠️ 万向锁问题： 当俯仰角接近 ±90° 时，横滚和航向会失去一个自由度。这在车载场景中很少见（除非你翻车了），但在无人机或机器人领域要特别小心。我曾经在仿真测试中遇到过，俯仰角到85度时，欧拉角插值直接跳变，导致控制指令异常。
2.3 四元数基础
欧拉角直观，但不好用。为什么？因为计算复杂，还有万向锁。所以实际工程中，我们几乎都用四元数。
四元数可以理解为「带约束的复数扩展」。一个四元数 q 包含四个分量：
q = [w, x, y, z]其中 w 是实部，x、y、z 是虚部。约束条件：w² + x² + y² + z² = 1
我个人觉得，理解四元数不用死磕数学推导。你只需要记住三点：
- 四元数表示旋转：绕某个轴旋转某个角度，都能用一个四元数表示
- 乘法对应旋转叠加：
q1 * q2表示先做 q2 旋转，再做 q1 旋转 - 共轭对应逆旋转：
q * conj(q) = [1, 0, 0, 0]
💡 实用技巧： 在代码里，我习惯把四元数定义为结构体，并实现乘法、共轭、归一化三个基础函数。其他所有操作都基于这三个函数构建，代码会非常干净。
2.4 旋转矩阵与坐标变换
旋转矩阵是连接两个坐标系的桥梁。从载体坐标系到地理坐标系的旋转矩阵 C_b^n 可以这样计算：
// 从四元数到旋转矩阵
C_b_n[0][0] = 1 - 2*(y*y + z*z);
C_b_n[0][1] = 2*(x*y - w*z);
C_b_n[0][2] = 2*(x*z + w*y);
C_b_n[1][0] = 2*(x*y + w*z);
C_b_n[1][1] = 1 - 2*(x*x + z*z);
C_b_n[1][2] = 2*(y*z - w*x);
C_b_n[2][0] = 2*(x*z - w*y);
C_b_n[2][1] = 2*(y*z + w*x);
C_b_n[2][2] = 1 - 2*(x*x + y*y);有了这个矩阵，坐标变换就简单了：
// 将载体坐标系下的向量 v_b 转换到地理坐标系
v_n = C_b_n * v_b;举个例子：IMU测得的加速度是载体坐标系下的，要用于导航计算，必须先转换到地理坐标系。这一步做错了，后面全白搭。
实战经验： 我在做惯性导航时，会先验证旋转矩阵的正确性。方法很简单：把地理坐标系下的重力向量 [0, 0, g] 转换到载体坐标系，看是否与IMU静止时的输出一致。如果一致，说明旋转矩阵是对的。
2.5 知识体系总览
下面这张图，是我自己总结的坐标系与姿态表示的知识结构。你可以把它当作本章的「地图」：
这张图把本章的核心内容串起来了。你从坐标系出发，选择一种姿态表示方法，然后通过核心操作完成坐标变换，最后应用到工程实践中。每一步都有坑，但每一步也都有对应的解决思路。
💡 我的建议： 刚开始学的时候，先别急着搞四元数。先用欧拉角理解姿态的物理意义，再用旋转矩阵做坐标变换。等这些都熟练了，再深入四元数的数学细节。循序渐进，反而学得更扎实。
好了，坐标系与姿态表示的基础就讲到这里。下一节我们会把这些知识用到实际的IMU数据融合中，到时候你就知道今天这些内容有多重要了。