3、捷联惯导更新算法概览：姿态更新、速度更新、位置更新的流程与相互关系
捷联惯导系统（SINS）的核心任务，是在载体运动过程中，利用陀螺仪和加速度计的测量输出，实时解算出载体的姿态、速度和位置信息。这一过程并非三个独立模块的简单串联，而是一个存在严格时序依赖和反馈耦合的递推更新体系。本章从系统级视角，梳理姿态更新、速度更新、位置更新的算法流程，重点剖析三者之间的数据依赖关系，并明确“速度积分”在位置解算中的承上启下作用。理解这一概览，是后续深入推导各子算法（如圆锥补偿、划船补偿、涡卷补偿）的前提。
3.1 三大更新的核心原理与递推逻辑
捷联惯导的更新算法本质上是一个时间离散化的递推过程。设导航更新周期为 \( T \)（通常为 1ms~10ms），每个周期内完成一次姿态、速度和位置的刷新。三者并非并行独立计算，而是存在严格的前后依赖关系：姿态更新是速度更新的前提，速度更新是位置更新的前提。这种依赖关系源于惯性导航的物理本质——加速度计测量的是载体坐标系下的比力，必须通过姿态矩阵将其投影到导航坐标系才能进行积分；而位置变化率（即速度）本身又是位置递推的直接输入。
从算法实现角度看，三个更新模块各自承担不同的补偿任务。姿态更新需要处理载体角运动引起的圆锥误差，通常采用旋转矢量多子样算法；速度更新需要处理线振动与角振动耦合产生的划船误差，并扣除有害加速度（哥氏加速度和重力加速度）；位置更新则需要对速度进行积分，并考虑地球曲率半径变化引起的涡卷误差。三者之间的数据流形成了典型的“串行-反馈”结构：姿态矩阵输出给速度更新，速度输出给位置更新，而位置和速度又反过来影响姿态更新中的重力模型和地球自转参数。
3.2 更新流程的时序关系与数据依赖
在一个典型的导航更新周期内，算法执行顺序如下：
- 采集IMU数据：读取当前周期的陀螺增量 \(\Delta\theta\) 和加速度计增量 \(\Delta v\)。
- 姿态更新：利用上一周期的姿态四元数 \(q(t_{k-1})\) 和当前周期的角增量，计算旋转矢量并完成四元数更新，得到 \(q(t_k)\)，进而提取姿态矩阵 \(C_b^n(t_k)\)。
- 速度更新：将加速度计输出的比力增量通过 \(C_b^n(t_k)\) 投影到导航系，扣除有害加速度项，加入划船补偿，得到速度增量 \(\Delta v^n\)，累加得到 \(V^n(t_k)\)。
- 位置更新：利用 \(V^n(t_k)\) 和上一周期位置 \(P(t_{k-1})\)，结合曲率半径和涡卷补偿，计算位置增量 \(\Delta P\)，更新得到 \(P(t_k)\)。
- 参数刷新：根据新位置更新重力模型、地球自转分量、曲率半径等参数，供下一周期使用。
值得注意的是，姿态更新中使用的旋转矢量算法需要陀螺的角增量输入，而速度更新中的划船补偿需要角增量和速度增量的叉乘项。这意味着在工程实现上，IMU 的采样时序必须严格同步，否则会引入时间不同步误差。下表总结了三大更新的核心输入、输出及关键补偿项：
| 更新模块 | 核心输入 | 核心输出 | 关键补偿/修正项 |
|---|---|---|---|
| 姿态更新 | 陀螺角增量 \(\Delta\theta\)、上一周期姿态 \(q(t_{k-1})\) | 姿态四元数 \(q(t_k)\)、姿态矩阵 \(C_b^n(t_k)\) | 圆锥补偿（多子样旋转矢量）、不可交换误差修正 |
| 速度更新 | 加速度计速度增量 \(\Delta v\)、姿态矩阵 \(C_b^n(t_k)\)、上一周期速度 \(V^n(t_{k-1})\) | 导航系速度 \(V^n(t_k)\) | 划船补偿、有害加速度扣除（哥氏项+重力项）、比力坐标变换 |
| 位置更新 | 导航系速度 \(V^n(t_k)\)、上一周期位置 \(P(t_{k-1})\) | 经纬高 \(L, \lambda, h\) | 涡卷补偿、曲率半径修正（\(R_M, R_N\)）、速度积分 |
3.3 速度积分在位置解算中的核心地位
速度积分是连接速度更新与位置更新的桥梁。从物理意义上看，位置是速度的时间积分，但在捷联惯导中，这一积分并非简单的 \(\int V dt\)，而是需要考虑地球曲率和导航坐标系旋转的复杂过程。具体而言，位置更新方程可写为：
dL/dt = V_N / (R_M + h)
dλ/dt = V_E / [(R_N + h) * cos(L)]
dh/dt = V_U其中 \(V_N, V_E, V_U\) 分别为北向、东向、天向速度，\(R_M, R_N\) 分别为子午圈和卯酉圈曲率半径。从该方程可以看出，速度积分的精度直接决定了位置解算的精度。如果速度存在常值偏差 \(\delta V\)，则位置误差将随时间线性增长——这正是惯导系统位置误差随时间发散的根本原因。
在工程实现中，速度积分通常采用梯形积分或更高阶的数值积分方法。由于导航更新周期很短（毫秒级），梯形积分已能保证足够的精度。但需要注意，当载体做高动态机动时，速度方向变化剧烈，简单的梯形积分会引入积分误差。此时需要引入涡卷补偿项，其物理本质是：当载体同时做线运动和角运动时，速度方向的变化会导致位置积分产生附加误差，类似于划船效应在速度积分中的影响。
3.4 三者的耦合关系与常见误区
三大更新模块并非孤立的“流水线”，而是存在双向反馈的耦合系统。姿态更新需要位置信息来计算重力矢量和地球自转分量；速度更新需要姿态矩阵和位置信息来计算有害加速度；位置更新又依赖速度输出。这种耦合关系在长时间导航中尤为显著——位置误差会导致重力模型偏差，进而影响姿态和速度解算，形成误差传播链。
在实际工程中，常见的误区有以下几类：
- 误区一：认为姿态更新可以独立完成。实际上，姿态更新中的陀螺漂移补偿需要依赖速度或位置信息（如通过外部参考进行修正），纯惯导模式下姿态误差会随时间累积。
- 误区二：忽略速度积分中的曲率修正。在中低精度应用中，部分工程师直接采用平面近似 \(dP = V \cdot T\)，这在短时间、小范围内可行，但在长航时或高纬度地区会引入显著误差。例如，在北纬 60° 处，忽略曲率修正的东向位置误差可达实际值的 2 倍。
- 误区三：将划船补偿和涡卷补偿混为一谈。划船补偿作用于速度更新，补偿的是比力积分中的线振动-角振动耦合误差；涡卷补偿作用于位置更新，补偿的是速度积分中的方向变化误差。两者物理来源不同，算法形式也不同，不能互相替代。
- 误区四：忽视更新周期的一致性。姿态、速度、位置更新必须使用相同的周期，且 IMU 采样时刻必须与更新时刻对齐。否则会引入时间不同步误差，该误差在高动态环境下会被显著放大。
3.5 案例拆解：高动态旋翼无人机的位置解算误差分析
某型小型旋翼无人机搭载消费级 MEMS 惯导，在强风环境下进行悬停和快速机动测试。初始对准后，前 30 秒位置误差小于 2 米，但 60 秒后位置误差迅速增大至 15 米，且呈振荡发散趋势。初步排查发现，姿态更新和速度更新均正常，问题集中在位置更新环节。
进一步分析飞行数据发现，无人机在强风作用下存在明显的水平振动（频率约 5Hz，幅度 0.3g）和角运动（滚转振荡 ±5°）。在位置更新中，原始算法仅采用简单的梯形积分，未加入涡卷补偿项。由于水平振动导致速度方向快速变化，梯形积分在每个周期内产生微小误差，该误差在振动频率处被放大，形成振荡累积。
针对该问题，工程师在位置更新中加入了涡卷补偿项，具体实现如下：
// 伪代码：位置更新中的涡卷补偿
// Δθ: 陀螺角增量, Δv: 速度增量, T: 更新周期
Vector3d delta_v_scull = 0.5 * (Δθ × Δv); // 涡卷补偿项
Vector3d v_avg = (V_prev + V_curr) * 0.5;
Vector3d delta_P = (v_avg + delta_v_scull) * T;
P_curr = P_prev + delta_P;修改后重新进行飞行测试，在相同风况和机动条件下，60 秒位置误差从 15 米降至 4 米以内，振荡现象明显抑制。该案例说明，在振动环境中，速度积分不能简单视为“速度×时间”，必须考虑角运动与线运动的耦合效应。同时，该案例也验证了三大更新模块的耦合性——位置误差的根源虽然出现在位置更新环节，但其诱因是姿态和速度的高频变化，只有理解了三者关系才能准确定位问题。
3.6 总结与工程复盘
本章从系统层面梳理了捷联惯导三大更新算法的流程与关系。核心要点可归纳为：
- 串行依赖：姿态→速度→位置，前级输出是后级输入，任何一级的误差都会向后续传递。
- 反馈耦合：位置和速度又通过重力、地球自转等参数反作用于姿态更新，形成闭环。
- 速度积分的关键地位：位置更新本质是速度积分，但必须考虑曲率修正和涡卷补偿，不能简化为平面积分。
- 工程验证顺序：先姿态、后速度、再位置，逐级验证，便于误差溯源。