3. 惯性导航基础（二）：捷联惯导系统（SINS）的数学平台、姿态更新算法（欧拉角法、方向余弦法、四元数法）、速度与位置更新算法
好，我们接着聊。上一节我们把惯性导航的基本概念和物理平台过了一遍。这一节，咱们得进入真正的核心——捷联惯导系统（SINS）。
说白了，物理平台惯导是把陀螺和加速度计装在一个实体的稳定平台上，让它模拟出一个“数学水平面”。而捷联惯导呢？它把传感器直接“绑”在载体上，平台没了，全靠计算机算出一个虚拟的“数学平台”。
我个人习惯把捷联惯导的数学平台想象成一个“软件定义的惯性空间”。你想想看，载体在动，传感器跟着动，但我们得知道它相对于导航坐标系（比如东北天）的姿态、速度和位置。这个转换过程，就是捷联惯导的核心。
核心思想： 捷联惯导用“数学平台”替代了物理平台。这个数学平台本质上是一组实时更新的方向余弦矩阵或四元数，它负责把载体坐标系（b系）的测量值转换到导航坐标系（n系）。
3.1 数学平台的本质
我在项目中遇到过不少新手，一上来就纠结“数学平台到底是个什么东西”。其实没那么玄乎。
数学平台，就是一组描述载体姿态的参数。它实时跟踪着载体坐标系相对于导航坐标系的旋转关系。这个关系可以用三种数学工具来表达：欧拉角、方向余弦矩阵、四元数。
嗯，这里要注意：数学平台不是静止的。载体在动，姿态在变，数学平台就得跟着更新。更新的过程，就是我们常说的姿态更新算法。
为什么需要更新？因为陀螺仪测的是角速度，我们得对角速度积分才能得到角度。但这个积分不是简单的累加，因为旋转不满足交换律。说白了，旋转的顺序很重要。
3.2 姿态更新算法
姿态更新算法，我把它分成三类来讲。这三类各有优劣，咱们在实际工程中得根据需求来选。
3.2.1 欧拉角法
欧拉角法是最直观的。它用三个角度（横滚角φ、俯仰角θ、航向角ψ）来描述姿态。更新公式长这样：
φ_dot = ωx + ωy * sin(φ) * tan(θ) + ωz * cos(φ) * tan(θ)
θ_dot = ωy * cos(φ) - ωz * sin(φ)
ψ_dot = (ωy * sin(φ) + ωz * cos(φ)) / cos(θ)看到没？当俯仰角θ接近±90°时，分母cos(θ)趋近于0，航向角ψ的更新会爆炸。这就是著名的“万向锁”问题。
避坑指南： 我曾经在一个无人机项目中用过欧拉角法，结果飞机做大机动俯冲时，姿态解算直接发散。后来我换成了四元数法，问题才解决。所以，如果你的载体可能出现大角度机动（比如无人机、导弹），千万别用欧拉角法。
欧拉角法的优点：直观，三个角度一看就懂。缺点：有万向锁，计算涉及大量三角函数，实时性差。
3.2.2 方向余弦法
方向余弦法用一个3×3的矩阵C来表示姿态。这个矩阵的每一列，就是载体坐标系各轴在导航坐标系中的方向余弦。
更新公式是：
C_dot = C * [ω×]其中[ω×]是角速度的反对称矩阵。这个公式看着简单，但实际计算时，矩阵有9个元素，每个都得更新。而且，每次更新后还得做正交化处理，否则矩阵会慢慢偏离正交性，导致姿态误差累积。
我建议，除非你对精度有变态的要求，否则别用方向余弦法。9个参数，计算量太大，而且正交化处理也很麻烦。
3.2.3 四元数法
四元数法是目前工程中最常用的方法。它用四个参数（q0, q1, q2, q3）来描述姿态。更新公式是：
q_dot = 0.5 * q ⊗ ω其中⊗表示四元数乘法。写成矩阵形式：
[q0_dot] [ 0 -ωx -ωy -ωz] [q0]
[q1_dot] = [ ωx 0 ωz -ωy] [q1]
[q2_dot] [ ωy -ωz 0 ωx] [q2]
[q3_dot] [ ωz ωy -ωx 0 ] [q3]四元数法没有万向锁问题，计算量小（只有4个参数），而且更新后只需要做归一化处理，比正交化简单得多。
个人经验： 我在做车载导航时，一直用四元数法。更新频率100Hz，归一化处理每50步做一次就够了。你想想看，100Hz的更新频率，每步只做4个参数的乘加运算，CPU负担很小。
3.3 速度与位置更新算法
姿态更新完了，接下来就是速度和位置的更新。这部分相对简单，但细节不少。
速度更新的核心是比力方程：
V_dot = C * f - (2ωie + ωen) × V + g其中：
- C * f：把加速度计测的比力从b系转换到n系
- (2ωie + ωen) × V：哥里奥利力和向心力的修正项
- g：重力加速度
位置更新就更直接了：
Lat_dot = Vn / (Rm + h)
Lon_dot = Ve / ((Rn + h) * cos(Lat))
h_dot = -Vd这里Rm和Rn分别是子午圈曲率半径和卯酉圈曲率半径。说白了，地球不是完美的球体，得考虑椭球模型。
关键点： 速度和位置的更新，依赖于姿态的准确性。姿态错了，比力转换就错了，速度和位置自然跟着错。这就是为什么姿态更新是捷联惯导的“心脏”。
3.4 知识体系总览
为了让你更直观地理解这一章的内容，我画了一张流程图。它展示了捷联惯导系统的核心逻辑：从陀螺和加速度计的原始测量，到姿态、速度、位置的最终输出。
这张图把整个流程串起来了。你看，陀螺仪的输出直接驱动姿态更新，姿态更新得到数学平台（姿态矩阵），然后数学平台把加速度计的比力转换到导航坐标系，再经过比力方程得到速度，最后积分得到位置。
嗯，这里有个细节要注意：速度更新中的ωen项（地球自转和载体运动引起的角速度）需要用到当前的速度和位置信息。所以，速度和位置会反过来影响姿态更新。这就是为什么捷联惯导是一个闭环系统。
工程建议： 在实际代码实现时，我建议把姿态更新、速度更新、位置更新放在同一个循环里。先更新姿态，再用新姿态更新速度，最后用新速度更新位置。这样能保证时间一致性。
好了，这一节的内容就到这儿。捷联惯导的数学平台和更新算法是后续所有融合算法的基础。你把这些搞明白了，后面讲松耦合、紧耦合的时候，就会轻松很多。
公众号:蓝海资料掘金营，微信deep3321