第1章：惯性导航基础（一）——坐标系与姿态表示
各位同学好，我是你们的老朋友。今天咱们正式进入惯性导航的世界。说实话，我刚开始接触惯导那会儿，最头疼的就是坐标系——天上飞的、地上跑的、传感器里算的，全是不同的坐标系。搞不清楚这个，后面什么融合定位都是空中楼阁。
这一章，咱们就把地基打牢。我会从三个核心坐标系讲起，再带你吃透三种姿态表示方法。嗯，都是干货，慢慢来。
1.1 三大坐标系：定位的“世界观”
做定位，首先得知道“我在哪”是相对于谁说的。你想想看，你说“我在北京”，这是相对于地球说的。但IMU（惯性测量单元）可不知道地球，它只知道自己的加速度和角速度。所以，我们必须定义清楚几个坐标系。
1.1.1 地心地固系（ECEF）
这个坐标系，说白了就是“地球的坐标系”。原点在地球质心，Z轴指向北极，X轴指向本初子午线与赤道的交点，Y轴按右手定则补齐。它跟着地球一起转，所以叫“地固”。
特点：
- 原点：地球质心
- Z轴：指向北极（CIO，国际习用原点）
- X轴：指向格林尼治子午线与赤道交点
- Y轴：右手定则，在赤道平面内
重要：ECEF是GPS和很多地图服务的基准坐标系。高精地图里的经纬度，最终都要转换到ECEF下才能和IMU数据融合。
我在项目中遇到过一个问题：用RTK（实时动态定位）得到的经纬度，直接和IMU的加速度积分结果做差，结果发现误差越来越大。后来才意识到，IMU的加速度是在载体系下测量的，而经纬度是ECEF下的。坐标系没对齐，融合个寂寞。
1.1.2 导航坐标系（n系）
导航系，也叫“东北天”系。原点在载体所在位置，X轴指向东，Y轴指向北，Z轴指向天顶。这个坐标系最符合人类的直觉——你开车时，导航说“前方500米右转”，这个“前方”就是相对于导航系的。
为什么用导航系？
- 直观：东、北、天，一目了然
- 方便：重力方向就是Z轴负方向，加速度计读数可以直接补偿
- 常用：大部分组合导航算法都在n系下做
我的习惯：在融合定位中，我通常把状态量定义在导航系下。这样位置、速度的物理意义清晰，调试起来也方便。但要注意，导航系的原点会随着载体移动而移动，所以它是一个“动坐标系”。
1.1.3 载体坐标系（b系）
载体系，就是“IMU自己的坐标系”。原点在IMU的测量中心，X轴指向载体前方，Y轴指向右侧，Z轴指向下方（右手系）。IMU测量的加速度和角速度，都是在这个坐标系下的原始值。
关键点：
- IMU输出：加速度（b系）、角速度（b系）
- 我们需要：将b系下的测量值转换到n系下，才能做导航解算
- 转换工具：姿态矩阵（后面会讲）
避坑指南：我曾经因为IMU安装方向搞反了，导致整个导航结果颠倒了。后来我养成了一个习惯：拿到IMU数据后，先做静态测试。把IMU水平放置，看Z轴加速度是不是接近9.8 m/s²，X、Y轴是不是接近0。这一步能帮你快速确认坐标系定义是否正确。
1.2 姿态表示方法：三种武器
坐标系定义好了，接下来就是“怎么从一个系转到另一个系”。这就是姿态表示要解决的问题。常用的有三种：欧拉角、旋转矩阵、四元数。各有优劣，咱们一个一个说。
1.2.1 欧拉角
欧拉角是最直观的表示方法。它用三个角度来描述旋转：航向角（yaw）、俯仰角（pitch）、横滚角（roll）。你可以想象成：先绕Z轴转yaw，再绕Y轴转pitch，最后绕X轴转roll。
优点：
- 直观：三个角度，一看就懂
- 物理意义明确：航向就是车头方向，俯仰就是抬头低头
缺点：
- 万向锁问题：当pitch接近±90°时，yaw和roll的旋转轴重合，会丢失一个自由度
- 插值困难：两个欧拉角之间做线性插值，结果往往不是最短路径
我的经验：在车载场景下，pitch和roll通常变化不大（地面车辆嘛），所以欧拉角勉强能用。但在无人机或机器人领域，我强烈建议用四元数。我曾经在一个无人机项目里用欧拉角做姿态控制，结果飞机翻了个跟头后，姿态直接炸了——万向锁！从那以后，我再也不敢在需要全姿态运动的场景里用欧拉角了。
1.2.2 旋转矩阵
旋转矩阵是一个3×3的正交矩阵，它描述了从一个坐标系到另一个坐标系的线性变换。比如，从b系到n系的旋转矩阵记为 \( C_b^n \)。
数学形式：
// 从b系到n系的旋转矩阵（欧拉角形式）
C_b^n = R_z(yaw) * R_y(pitch) * R_x(roll)
其中：
R_z(yaw) = [[cos(yaw), -sin(yaw), 0],
[sin(yaw), cos(yaw), 0],
[0, 0, 1]]
R_y(pitch) = [[cos(pitch), 0, sin(pitch)],
[0, 1, 0],
[-sin(pitch), 0, cos(pitch)]]
R_x(roll) = [[1, 0, 0],
[0, cos(roll), -sin(roll)],
[0, sin(roll), cos(roll)]]优点：
- 无奇异性：不会出现万向锁
- 可组合：多个旋转矩阵相乘，可以表示任意旋转
- 可逆：转置就是逆旋转
缺点：
- 冗余：9个参数，但只有3个自由度
- 计算量大：矩阵乘法比四元数乘法慢
- 需要正交化：数值误差会导致矩阵不再正交，需要定期修正
实用技巧：在代码实现中，我通常用旋转矩阵做坐标变换（因为直接），但用四元数做姿态更新（因为高效）。两者之间可以互相转换，后面会给出转换公式。
1.2.3 四元数
四元数是我个人最喜欢的姿态表示方法。它是一个四维向量 \( q = [q_w, q_x, q_y, q_z]^T \)，其中 \( q_w \) 是实部，\( q_x, q_y, q_z \) 是虚部。它用一个旋转轴和一个旋转角来描述旋转。
数学形式：
// 四元数表示旋转
q = [cos(θ/2), sin(θ/2) * u_x, sin(θ/2) * u_y, sin(θ/2) * u_z]
其中：
θ 是旋转角度
u = [u_x, u_y, u_z]^T 是单位旋转轴
// 四元数乘法（用于组合旋转）
q_result = q1 * q2
// 用四元数旋转向量
v' = q * v * q_conjugate优点：
- 无奇异性：可以表示任意旋转
- 紧凑：4个参数，无冗余
- 高效：乘法比矩阵乘法快
- 易插值：球面线性插值（SLERP）可以得到平滑的旋转路径
缺点：
- 不直观：四个数，很难直接看出姿态
- 需要归一化：数值误差会导致四元数不再单位化，需要定期归一化
核心建议：在融合定位算法中，我强烈推荐用四元数作为姿态状态量。原因有三：一是无奇异性，二是更新效率高，三是和卡尔曼滤波配合得很好（四元数误差状态是3维的，可以避免过参数化）。
1.3 三种表示方法的转换
实际工程中，这三种表示方法经常需要互相转换。下面给出核心公式：
| 转换方向 | 公式 |
|---|---|
| 欧拉角 → 旋转矩阵 | \( C = R_z(yaw) R_y(pitch) R_x(roll) \) |
| 旋转矩阵 → 欧拉角 | \( yaw = atan2(C_{21}, C_{11}) \), \( pitch = -asin(C_{31}) \), \( roll = atan2(C_{32}, C_{33}) \) |
| 欧拉角 → 四元数 | \( q = q_z(yaw) * q_y(pitch) * q_x(roll) \) |
| 四元数 → 旋转矩阵 | \( C = \begin{bmatrix} 1-2(q_y^2+q_z^2) & 2(q_x q_y - q_w q_z) & 2(q_x q_z + q_w q_y) \\ 2(q_x q_y + q_w q_z) & 1-2(q_x^2+q_z^2) & 2(q_y q_z - q_w q_x) \\ 2(q_x q_z - q_w q_y) & 2(q_y q_z + q_w q_x) & 1-2(q_x^2+q_y^2) \end{bmatrix} \) |
注意：欧拉角转四元数时，旋转顺序很重要。我习惯用ZYX顺序（先yaw，再pitch，最后roll），这也是航空领域最常用的顺序。如果你用不同的顺序，结果会完全不同。
1.4 本章知识体系
下面我用一张图来总结本章的核心内容。这张图展示了坐标系、姿态表示方法以及它们之间的关系。
1.5 本章小结
好了，这一章的内容就到这里。咱们把坐标系和姿态表示方法都过了一遍。说实话，这些概念看起来简单，但真正用起来，坑还是挺多的。我建议你：
- 坐标系：先搞清楚你的IMU是b系，你的地图是ECEF系，你的导航解算是n系。三者之间的转换关系，一定要烂熟于心。
- 姿态表示：日常调试用欧拉角（直观），算法实现用四元数（高效），坐标变换用旋转矩阵（直接）。
- 转换公式：最好手写一遍，加深记忆。我当年就是靠手写推导，才彻底搞懂了这些公式。
下一章，咱们会深入IMU的测量模型和误差特性。嗯，那才是真正考验算法功底的地方。做好准备吧。