第三章 捷联惯导系统力学编排：姿态更新算法
各位好，我是老张。今天咱们聊聊捷联惯导系统里最核心的一块——力学编排。说白了，就是怎么用陀螺和加速度计的数据，算出载体的姿态、速度和位置。我做了十几年惯导，每次带新人，第一件事就是让他们把这块吃透。因为后面所有的误差补偿、滤波算法，都建立在这个基础上。
这一章我们重点讲姿态更新。为什么姿态最重要？你想想看，速度更新要用到姿态矩阵，位置更新也要用到姿态矩阵。姿态算偏了，后面全完蛋。我在项目里见过太多这样的案例了。
3.1 姿态更新算法概述
姿态更新，就是根据陀螺输出的角速度，实时计算载体坐标系相对于导航坐标系的旋转关系。常用的数学工具有三种：欧拉角、方向余弦矩阵、四元数。我个人最推荐四元数，原因很简单——没有奇点，计算量小，适合嵌入式实时计算。
但四元数也有坑。嗯，这里要注意：四元数更新存在非交换误差。说白了，就是旋转的顺序会影响结果。你想想看，先绕X轴转90度再绕Y轴转90度，和反过来转，结果是不一样的。陀螺采样是离散的，这个误差就会累积。
核心要点：姿态更新算法的选择，本质是在精度和计算量之间做权衡。四元数适合大多数场景，等效旋转矢量适合高动态环境。
3.2 四元数姿态更新算法
四元数是个好东西。一个四元数 q = [q0, q1, q2, q3]^T，就能完整描述三维空间的旋转。更新公式也很简洁：
q(k+1) = q(k) ⊗ Δq其中 Δq 是姿态变化对应的四元数，由陀螺角增量计算得到。我习惯用毕卡逼近法，精度够用，计算也简单。
具体实现时，先算角增量模值：
Δθ = sqrt(Δθx² + Δθy² + Δθz²)然后构造更新四元数：
Δq0 = cos(Δθ/2)
Δq1 = (Δθx/Δθ) * sin(Δθ/2)
Δq2 = (Δθy/Δθ) * sin(Δθ/2)
Δq3 = (Δθz/Δθ) * sin(Δθ/2)最后做四元数乘法更新。这里有个细节：更新完一定要归一化。我曾经见过一个同事，忘了归一化，跑了十分钟姿态就飘到天上去了。
避坑指南：四元数更新后必须归一化！否则模长会逐渐偏离1，导致姿态矩阵不正交。我建议每个更新周期都做一次归一化，不要偷懒。
3.3 等效旋转矢量算法
四元数算法在高动态环境下精度不够，因为忽略了非交换误差。这时候就要用等效旋转矢量了。这个算法是Bortz在1971年提出的，核心思想是用一个旋转矢量 Φ 来描述姿态变化，然后通过求解Bortz方程来补偿非交换误差。
等效旋转矢量的更新方程：
Φ = Δθ + (1/12) * Δθ × Δθ_prev其中 Δθ 是当前周期的角增量，Δθ_prev 是上一周期的角增量。这个叉乘项就是非交换误差补偿项。我在做高动态飞行器导航时，用这个算法比纯四元数精度提高了至少一个数量级。
得到等效旋转矢量后，再转换成四元数：
Δq0 = cos(|Φ|/2)
Δq = (Φ/|Φ|) * sin(|Φ|/2)然后和四元数算法一样做更新和归一化。
个人经验：如果载体角速度变化不大，用四元数就够了。但如果是战斗机、导弹这类高机动载体，我建议直接用等效旋转矢量。别问我怎么知道的——有一次项目测试，用四元数算法，飞机做个筋斗姿态就偏了3度，换成等效旋转矢量后误差降到0.1度以内。
3.4 速度更新算法
速度更新相对简单一些。比力方程是核心：
v̇ = C_b^n * f^b - (2ω_ie^n + ω_en^n) × v^n + g^n离散化后：
v(k+1) = v(k) + Δv_f + Δv_cor + Δv_g其中 Δv_f 是比力积分项，Δv_cor 是哥氏加速度和向心加速度补偿，Δv_g 是重力补偿。
比力积分项的计算要注意：加速度计输出的是比力，不是真正的加速度。我刚开始做惯导时，经常把这两个搞混，结果速度越算越离谱。
Δv_f = C_b^n(k) * (Δv_acc + 0.5 * Δθ × Δv_acc)这个叉乘项是速度的旋转补偿，和姿态更新里的非交换误差补偿类似。高动态环境下不能忽略。
3.5 位置更新算法
位置更新用经纬高表示。纬度、经度、高度的更新公式：
L(k+1) = L(k) + v_N * dt / (R_M + h)
λ(k+1) = λ(k) + v_E * dt / ((R_N + h) * cos(L))
h(k+1) = h(k) + v_U * dt其中 R_M 是子午圈曲率半径，R_N 是卯酉圈曲率半径。这两个参数随纬度变化，不能取常数。我见过有人图省事用固定值，结果在极区附近位置误差大得吓人。
关键提醒：位置更新虽然公式简单，但精度受限于姿态和速度的精度。姿态偏1角分，位置误差每小时可能累积到几公里。所以位置更新的根本，还是要把姿态和速度算准。
3.6 知识体系总览
下面这张图是我自己整理的，把姿态、速度、位置更新的关系画清楚了。你一看就明白：
从这张图可以清楚看到：陀螺输出驱动姿态更新，姿态矩阵用于把加速度计比力从载体坐标系转换到导航坐标系，然后做速度更新，最后用速度积分得到位置。位置反过来又影响重力模型的计算。这是一个闭环系统，每个环节的误差都会传递和放大。
3.7 算法选择建议
说了这么多，到底该用哪种算法？我根据多年经验，给个参考：
| 应用场景 | 推荐算法 | 更新频率 | 注意事项 |
|---|---|---|---|
| 低动态（车辆、船舶） | 四元数 + 毕卡逼近 | 100Hz | 注意归一化 |
| 中动态（无人机、导弹） | 四元数 + 二阶龙格库塔 | 200Hz | 考虑圆锥补偿 |
| 高动态（战斗机、火箭） | 等效旋转矢量 | 400Hz以上 | 必须做非交换误差补偿 |
我个人建议，如果硬件资源允许，直接上等效旋转矢量。虽然代码量多几行，但心里踏实。毕竟惯导这东西，精度就是生命。
小技巧：实际工程中，陀螺和加速度计的采样频率可以不一样。我习惯把陀螺频率设高一些（比如400Hz），加速度计用200Hz，然后做时间对齐。这样既能保证姿态精度，又不会让CPU太累。
好了，这一章的内容就到这里。姿态更新是惯导的根基，一定要理解透。下一章我们讲误差传播特性，到时候你会看到，姿态误差是怎么一步步影响速度和位置的。
公众号:蓝海资料掘金营，微信deep3321