3. 惯性导航基础（下）：姿态表示与捷联惯导更新算法
各位同学，咱们接着聊惯性导航。上一节我们把陀螺和加速度计的原理讲透了，这一节重点说说姿态怎么表示、怎么算。说白了，惯导系统最核心的问题就两个：我朝哪看？我往哪走？前者是姿态，后者是位置。今天先啃姿态这块硬骨头。
3.1 姿态表示：三种主流方法
姿态表示，说白了就是描述一个坐标系相对于另一个坐标系的旋转关系。工程上常用的有三种：欧拉角、旋转矩阵、四元数。我当年刚入行时，觉得这三种方法随便选一个就行，后来踩了坑才明白——选错了表示方法，代码里全是坑。
3.1.1 欧拉角
欧拉角最直观。它用三个角度描述旋转：横滚角（Roll）、俯仰角（Pitch）、航向角（Yaw）。你想想看，飞机转弯时先转航向，再抬头，最后侧倾——这就是欧拉角的物理含义。
定义：
- 横滚角 φ：绕X轴旋转
- 俯仰角 θ：绕Y轴旋转
- 航向角 ψ：绕Z轴旋转
旋转顺序：Z→Y→X（工程中最常用）
欧拉角有个致命问题——万向锁。当俯仰角接近±90°时，横滚和航向会耦合，丢失一个自由度。我在做车载惯导时遇到过这问题：车辆在陡坡上，俯仰角接近90°，姿态解算直接炸了。嗯，从那以后我再也不敢在车载场景里只用欧拉角了。
3.1.2 旋转矩阵
旋转矩阵用3×3的正交矩阵表示旋转。它没有万向锁问题，但参数多（9个），计算量大。
// 旋转矩阵示例：绕Z轴旋转ψ角
R_z(ψ) = [cosψ -sinψ 0]
[sinψ cosψ 0]
[0 0 1]
// 完整旋转矩阵（Z→Y→X顺序）
R = R_x(φ) * R_y(θ) * R_z(ψ)旋转矩阵有个好处：可以链式相乘。你连续旋转两次，直接把两个矩阵乘起来就行。但要注意，矩阵乘法不满足交换律——先转航向再转俯仰，和先转俯仰再转航向，结果完全不同。
注意：旋转矩阵必须是正交矩阵，且行列式为+1。如果数值误差导致矩阵不正交，必须做正交化处理。我见过有人直接拿带误差的矩阵算，结果姿态越偏越远。
3.1.3 四元数
四元数是我个人最推荐的方法。它用四个参数表示旋转：一个实部加三个虚部。没有万向锁，计算量小，插值平滑。
// 四元数定义
q = [q0, q1, q2, q3]^T
其中 q0 = cos(θ/2)
q1 = e_x * sin(θ/2)
q2 = e_y * sin(θ/2)
q3 = e_z * sin(θ/2)
// e = [e_x, e_y, e_z]^T 是旋转轴单位向量
// θ 是旋转角度四元数更新公式也很简洁：
// 四元数微分方程
dq/dt = 0.5 * q ⊗ ω
// 其中 ω 是角速度，⊗ 表示四元数乘法
// 离散更新（一阶近似）
q_{k+1} = q_k + dq/dt * Δt我的习惯：车载项目里统一用四元数做姿态解算，只在输出给上层应用时转成欧拉角。这样既避免了万向锁，又方便调试人员理解。
3.2 三种方法的对比
| 特性 | 欧拉角 | 旋转矩阵 | 四元数 |
|---|---|---|---|
| 参数数量 | 3 | 9 | 4 |
| 万向锁 | 有 | 无 | 无 |
| 计算量 | 小 | 大 | 中 |
| 插值平滑性 | 差 | 中 | 好 |
| 工程推荐度 | 低（仅限小角度） | 中 | 高 |
3.3 捷联惯导更新算法
捷联惯导的核心思想：陀螺测角速度，加速度计测比力，然后积分得到姿态、速度、位置。说白了就是不断累加。
3.3.1 姿态更新
姿态更新是惯导里最关键的环节。我用四元数法给大家演示：
// 姿态更新伪代码
void updateAttitude(Quaternion &q, Vector3 gyro, float dt) {
// 1. 计算角增量
Vector3 delta_theta = gyro * dt;
// 2. 构造四元数增量
float theta = norm(delta_theta);
if (theta < 1e-10) return; // 无旋转
Vector3 axis = delta_theta / theta;
Quaternion dq;
dq.w = cos(theta / 2);
dq.x = axis.x * sin(theta / 2);
dq.y = axis.y * sin(theta / 2);
dq.z = axis.z * sin(theta / 2);
// 3. 更新四元数
q = q * dq;
// 4. 归一化（防止数值误差）
q = normalize(q);
}关键点：
- 每次更新后必须归一化四元数
- 角增量法比直接积分角速度更精确
- 高动态场景要用二阶或四阶龙格-库塔法
3.3.2 速度更新
速度更新需要扣除重力：
// 速度更新
Vector3 updateVelocity(Vector3 vel, Vector3 acc, Quaternion q, float dt) {
// 1. 将加速度从载体坐标系转到导航坐标系
Vector3 acc_nav = q.rotate(acc);
// 2. 扣除重力
acc_nav.z -= GRAVITY;
// 3. 积分
vel += acc_nav * dt;
return vel;
}这里有个坑：重力补偿必须在导航坐标系下做。我见过有人直接在载体坐标系里减重力，结果水平加速度全错了。
3.3.3 位置更新
位置更新最简单，直接对速度积分：
// 位置更新
Vector3 updatePosition(Vector3 pos, Vector3 vel, float dt) {
pos += vel * dt;
return pos;
}但要注意，如果车辆在转弯，速度方向变化很快，用一阶积分误差会很大。这时候建议用梯形法或龙格-库塔法。
3.4 知识体系总览
下面这张图总结了本章的核心逻辑：
3.5 工程实践中的避坑指南
最后分享几个我踩过的坑：
我曾经犯过的错：
- 四元数忘记归一化：跑了几分钟，姿态漂移了10度。后来加了归一化，问题解决。
- 欧拉角直接做插值：车辆转弯时，航向角从359°变到0°，插值结果跑了180°。改用四元数球面插值（Slerp）后正常。
- 重力补偿时机不对：在载体坐标系减重力，导致水平加速度有偏差。必须在导航坐标系下补偿。
我的建议：
- 新项目起步时，先用四元数做姿态解算
- 调试阶段，把四元数转成欧拉角打印出来看
- 高动态场景（急转弯、急加速），用二阶龙格-库塔法更新
- 定期做零速修正（ZUPT），抑制积分漂移
好了，姿态表示和捷联惯导更新算法就讲到这里。这些内容是惯导的基石，理解透了后面才能玩转组合导航。大家动手写代码时，建议先从四元数更新开始，跑通后再加加速度和位置更新。