4、捷联惯导解算基础：姿态更新算法（四元数更新、等效旋转矢量）、速度与位置更新方程、圆锥误差与划船误差补偿

捷联惯导系统（SINS）的核心任务是在载体动态运动过程中，利用陀螺仪和加速度计的测量输出，实时、高精度地解算出载体的姿态、速度和位置。与平台式惯导不同，捷联系统将惯性传感器直接固联于载体，通过数学平台替代物理平台，因此其解算算法的精度与鲁棒性直接决定了系统的整体性能。本章聚焦于捷联惯导解算的三大基础模块：姿态更新算法（重点对比四元数与等效旋转矢量）、速度与位置更新方程，以及动态环境下不可忽略的圆锥误差与划船误差补偿。理解这些基础，是后续学习初始对准、组合导航及动基座技术的先决条件。

上图清晰地展示了捷联惯导解算的完整链路：传感器输出原始角增量和速度增量，分别进入姿态更新和速度/位置更新通道，而圆锥误差与划船误差补偿则贯穿于两个通道之中，最终输出高精度的导航参数。下面我们将逐一深入剖析各模块的数学原理与工程实现要点。

4.1 姿态更新算法：四元数与等效旋转矢量

4.1.1 核心原理：从方向余弦到四元数的必然选择

姿态描述的本质是确定载体坐标系（b系）相对于导航坐标系（n系）的角位置关系。方向余弦矩阵（DCM）虽然直观，但存在9个参数、6个约束，计算冗余且存在正交化漂移问题。四元数以其4个参数、1个约束的紧凑形式，避免了三角函数运算，成为捷联惯导姿态更新的主流方法。

四元数定义为 q = [q0, q1, q2, q3]ᵀ，其中 q0 为标量部分，表示旋转角的一半余弦；[q1, q2, q3] 为矢量部分，表示旋转轴方向。姿态更新方程可表示为：

q_b^n(k+1) = q_b^n(k) ⊗ q_Δ(k)

其中 ⊗ 表示四元数乘法，q_Δ(k) 为从 k 时刻到 k+1 时刻的姿态变化四元数。若已知陀螺输出的角增量 Δθ，则 q_Δ 可近似为：

q_Δ ≈ [cos(|Δθ|/2), (Δθ/|Δθ|)·sin(|Δθ|/2)]

然而，这种直接基于角增量的四元数更新在动态环境下存在严重的原理性误差——旋转不可交换性误差。当载体同时绕多个轴转动时，角增量矢量合成的旋转顺序与真实旋转过程不一致，导致姿态误差累积。

4.1.2 等效旋转矢量：解决不可交换性的钥匙

Bortz于1971年提出的等效旋转矢量理论，从根本上解决了这一问题。旋转矢量 Φ 定义为：从初始姿态到最终姿态的等效单次旋转，其方向为旋转轴，大小为旋转角。其微分方程为：

Φ̇ = ω + 0.5·(Φ × ω) + (1/|Φ|²)·(1 - |Φ|·sin|Φ|/(2·(1-cos|Φ|)))·(Φ × (Φ × ω))

在实际工程中，通常采用简化形式，并利用陀螺的角增量输出进行多子样算法求解。以经典的二子样算法为例，在一个更新周期 T 内，将陀螺输出分为两个半周期角增量 Δθ₁ 和 Δθ₂，则旋转矢量更新为：

Φ = Δθ₁ + Δθ₂ + (2/3)·(Δθ₁ × Δθ₂)

这里的叉乘项 (2/3)·(Δθ₁ × Δθ₂) 正是对圆锥误差的补偿。三子样、四子样算法在此基础上进一步增加子样数，提高对高动态环境的适应能力。

4.1.3 案例拆解：高动态环境下四元数与旋转矢量对比

某型无人机在飞行试验中，经历持续 5 秒的快速机动（滚转 ±60°，俯仰 ±30°，偏航 ±90° 同时进行）。采用纯四元数更新（基于角增量直接积分）与等效旋转矢量二子样算法进行对比。

• 初始条件：陀螺零偏稳定性 0.5°/h，角度随机游走 0.05°/√h，采样率 200Hz，更新周期 5ms。
• 试验结果：机动结束后，纯四元数方法姿态误差达到 0.8°（主要来源于圆锥误差累积）；等效旋转矢量方法姿态误差仅为 0.05°。
• 分析：在复合角运动下，角增量矢量合成的旋转顺序误差被放大，而旋转矢量通过叉乘补偿项有效抑制了该误差。

这一案例充分说明，在动基座或高动态场景下，必须采用等效旋转矢量进行姿态更新，否则姿态误差将迅速发散。

4.1.4 常见误区与总结

总结：姿态更新算法的核心在于：采用四元数作为姿态表示，采用等效旋转矢量作为更新中间量，通过多子样算法补偿圆锥误差。三者缺一不可，共同构成高精度姿态解算的基础。

4.2 速度与位置更新方程

4.2.1 核心原理：比力方程与导航坐标系投影

速度更新的理论基础是比力方程，它描述了载体加速度与比力、重力、哥氏加速度之间的关系。在导航坐标系（通常为当地地理坐标系，即东北天坐标系）中，速度微分方程为：

V̇^n = C_b^n · f^b - (2ω_ie^n + ω_en^n) × V^n + g^n

其中：

• V̇^n：导航系下的速度变化率
• C_b^n · f^b：加速度计测量的比力投影到导航系
• (2ω_ie^n + ω_en^n) × V^n：哥氏加速度和载体运动引起的向心加速度
• g^n：重力加速度矢量

位置更新则基于速度积分，对于经纬度表示的位置，其微分方程为：

L̇ = V_N / (R_M + h)
λ̇ = V_E / ((R_N + h)·cosL)
ḣ = V_U

其中 R_M 为子午圈曲率半径，R_N 为卯酉圈曲率半径，L 为纬度，λ 为经度，h 为高度。

4.2.2 速度更新的离散化实现

在实际解算中，速度更新采用增量形式。设加速度计在一个更新周期 T 内输出的速度增量为 Δv，则速度更新方程为：

V^n(k+1) = V^n(k) + C_b^n(k) · Δv + [g^n - (2ω_ie^n + ω_en^n) × V^n(k)] · T

这里的关键在于 C_b^n(k) 应使用周期内的平均姿态矩阵，而非起始时刻的姿态矩阵。若直接使用起始时刻姿态矩阵，当载体存在角运动时，会引入划船误差。

4.2.3 划船误差的产生与补偿

划船误差与圆锥误差类似，源于线运动和角运动的耦合。当载体同时存在线振动和角振动时，加速度计测量的比力方向在导航系中不断变化，若仅用周期起始时刻的姿态矩阵投影，会产生明显的速度误差。划船误差的补偿项通常与角增量和速度增量的叉乘相关。

以二子样划船补偿为例，设一个周期内两个半周期的速度增量为 Δv₁ 和 Δv₂，角增量为 Δθ₁ 和 Δθ₂，则划船补偿项为：

Δv_scull = 0.5·(Δθ₁ × Δv₂ + Δv₁ × Δθ₂)

完整的二子样速度更新为：

Δv_comp = Δv₁ + Δv₂ + 0.5·(Δθ₁ × Δv₂ + Δv₁ × Δθ₂)

4.2.4 实战场景：车载导航中的速度位置解算

某车载定位系统采用捷联惯导，车辆行驶在颠簸路面，同时频繁加减速。加速度计输出包含强烈的振动噪声，陀螺输出包含角振动。若不进行划船补偿，速度误差在 10 分钟内累积可达 2 m/s，位置误差超过 600 米。加入二子样划船补偿后，速度误差降至 0.1 m/s 以内，位置误差控制在 30 米以内。

具体实施步骤：

1. 在每个解算周期（如 5ms）内，采集陀螺和加速度计输出，分别计算角增量 Δθ₁、Δθ₂ 和速度增量 Δv₁、Δv₂。
2. 计算圆锥补偿项和划船补偿项。
3. 更新旋转矢量 Φ，并转换为四元数 q。
4. 利用补偿后的速度增量更新速度 V^n。
5. 利用速度更新位置 L、λ、h。

4.2.5 常见误区与总结

总结：速度与位置更新是捷联惯导解算的最终输出环节，其精度依赖于姿态矩阵的准确性、比力投影的正确性以及动态误差补偿的完备性。划船补偿与圆锥补偿同等重要，是保证高动态环境下导航精度的关键。

4.3 圆锥误差与划船误差的工程补偿策略

4.3.1 误差机理的深入理解

圆锥误差的物理本质是：当载体绕两个正交轴同时做同频、相位差 90° 的角振动时，即使角振动幅值很小，也会产生一个沿第三轴的常值漂移角速度。这一现象在数学上表现为角增量矢量的叉乘项不为零。划船误差的物理本质类似，是线振动与角振动耦合产生的常值加速度误差。

从频谱角度看，圆锥误差和划船误差的能量集中在振动频率附近，且与振动频率的平方成正比。因此，在高频振动环境下，误差会急剧放大。

4.3.2 多子样算法的优化设计

多子样算法的核心思想是在一个更新周期内，利用多个角增量/速度增量样本，通过优化组合系数来逼近理想积分。以三子样圆锥补偿为例，其补偿项为：

Φ_comp = k₁·(Δθ₁ × Δθ₃) + k₂·(Δθ₂ × (Δθ₃ - Δθ₁))

其中系数 k₁、k₂ 通过最优化方法确定，使得在典型振动频率下误差最小。工程中常用的优化准则包括：最小化特定频率下的误差、最小化频带内的均方误差等。

4.3.3 案例拆解：高动态飞行试验中的误差补偿

某型战术导弹在飞行中经历 20Hz 的角振动和 15Hz 的线振动。采用三子样圆锥补偿和划船补偿后，姿态误差从 0.5° 降至 0.03°，速度误差从 5 m/s 降至 0.2 m/s。具体实施中，将陀螺和加速度计采样率设为 400Hz，解算周期 2.5ms，每个周期内使用 3 个子样。

关键参数设置：

采样率: 400 Hz
解算周期: 2.5 ms
子样数: 3
圆锥补偿系数: k1=0.45, k2=0.675
划船补偿系数: 与圆锥系数对偶

试验结果表明，补偿后的导航精度满足制导控制要求，证明了多子样算法在高动态环境下的有效性。

4.3.4 常见误区与总结

总结：圆锥误差与划船误差补偿是捷联惯导高精度解算的核心技术。工程实现中，需要根据载体的动态特性、传感器性能和解算周期，综合选择子样数和优化系数，并在实际试验中验证和调整。只有将理论算法与工程实践紧密结合，才能充分发挥捷联惯导系统的性能潜力。