4、速度解算基础：加速度计原理、比力方程、速度更新算法、重力补偿
4.1 加速度计原理与比力方程
在捷联惯导系统中，速度解算的核心输入来自加速度计。加速度计测量的是其敏感轴方向上的比力（Specific Force），即单位质量所受的惯性力（非引力外力）之和。在导航坐标系（通常为当地水平坐标系，如n系）中，比力方程描述了载体加速度、比力与重力之间的关系。
比力方程（导航系下）：
\[ \dot{\mathbf{v}}^n_{eb} = \mathbf{f}^n_{ib} - (2\boldsymbol{\omega}^n_{ie} + \boldsymbol{\omega}^n_{en}) \times \mathbf{v}^n_{eb} + \mathbf{g}^n \]其中：
- \(\dot{\mathbf{v}}^n_{eb}\)：载体相对于地球的速度在导航系下的导数（即加速度）。
- \(\mathbf{f}^n_{ib}\)：加速度计测量的比力，投影到导航系。
- \(2\boldsymbol{\omega}^n_{ie} \times \mathbf{v}^n_{eb}\)：地球自转引起的哥氏加速度。
- \(\boldsymbol{\omega}^n_{en} \times \mathbf{v}^n_{eb}\)：载体在地球表面运动引起的向心加速度（运输项）。
- \(\mathbf{g}^n\)：当地重力加速度矢量（包含引力与离心力）。
物理含义： 加速度计无法区分引力与运动加速度。比力方程的本质是从测量的比力中扣除有害加速度（哥氏项、运输项），并补偿重力，从而得到真实的运动加速度。
4.2 速度更新算法
速度更新是惯导解算的核心步骤之一，通常采用数值积分方法。在离散时间系统中，常用梯形积分法或龙格-库塔法。以下给出基于比力方程的标准速度更新流程。
算法步骤（离散化）：
- 姿态更新：首先利用陀螺仪输出更新当前时刻的姿态矩阵 \(C^n_b\)，用于将加速度计测量的比力从载体坐标系转换到导航坐标系。
- 比力投影：将加速度计输出 \(\mathbf{f}^b_{ib}\) 通过姿态矩阵投影到导航系： \[ \mathbf{f}^n_{ib} = C^n_b \cdot \mathbf{f}^b_{ib} \]
- 有害加速度补偿：计算哥氏加速度与运输项，并从比力中扣除： \[ \mathbf{a}_{coriolis} = (2\boldsymbol{\omega}^n_{ie} + \boldsymbol{\omega}^n_{en}) \times \mathbf{v}^n_{eb} \]
- 重力补偿：加上当地重力矢量。
- 积分更新：采用梯形法更新速度： \[ \mathbf{v}^n_{eb}(t_k) = \mathbf{v}^n_{eb}(t_{k-1}) + \frac{\Delta t}{2} \left[ \mathbf{a}(t_{k-1}) + \mathbf{a}(t_k) \right] \] 其中 \(\mathbf{a} = \mathbf{f}^n_{ib} - \mathbf{a}_{coriolis} + \mathbf{g}^n\)。
Python 实现示例（核心逻辑）：
import numpy as np
def velocity_update(v_prev, f_b_prev, f_b_curr, C_nb_prev, C_nb_curr,
omega_ie_n, omega_en_prev, omega_en_curr, g_n, dt):
"""
速度更新函数（梯形法）
参数:
v_prev: 上一时刻速度 (3,)
f_b_prev, f_b_curr: 前后时刻比力 (3,)
C_nb_prev, C_nb_curr: 前后时刻姿态矩阵 (3,3)
omega_ie_n: 地球自转角速度在导航系投影 (3,)
omega_en_prev, omega_en_curr: 前后时刻运输角速度 (3,)
g_n: 重力矢量 (3,)
dt: 时间间隔
返回:
v_curr: 当前时刻速度 (3,)
"""
# 比力投影到导航系
f_n_prev = C_nb_prev @ f_b_prev
f_n_curr = C_nb_curr @ f_b_curr
# 有害加速度
a_cor_prev = np.cross(2 * omega_ie_n + omega_en_prev, v_prev)
# 注意：当前时刻速度未知，需使用上一时刻速度近似（或迭代）
a_cor_curr = np.cross(2 * omega_ie_n + omega_en_curr, v_prev) # 简化处理
# 总加速度
a_prev = f_n_prev - a_cor_prev + g_n
a_curr = f_n_curr - a_cor_curr + g_n
# 梯形积分
v_curr = v_prev + 0.5 * dt * (a_prev + a_curr)
return v_curr
4.3 重力补偿
重力补偿的精度直接影响速度解算的长期稳定性。重力矢量 \(\mathbf{g}^n\) 通常采用正常重力模型计算，例如WGS-84椭球模型。
WGS-84 重力计算公式（导航系下，通常取北-东-地坐标系）：
\[ g(\phi, h) = g_0 \cdot \frac{1 + 0.00193185138639 \sin^2\phi}{\sqrt{1 - e^2 \sin^2\phi}} - 0.000003086 \cdot h \]其中：
- \(\phi\)：纬度
- \(h\)：高度（m）
- \(g_0 = 9.7803267714 \, \text{m/s}^2\)
- \(e^2 = 0.00669437999014\)（第一偏心率平方）
在导航系中的分量形式（NED系）：
\[ \mathbf{g}^n = \begin{bmatrix} 0 \\ 0 \\ g(\phi, h) \end{bmatrix} \]注意：在NED系中，重力方向指向地（向下），因此第三分量为正值（向下为正）。若使用ENU系，则第三分量为负值。
重力补偿的注意事项：
- 高度变化修正：高度变化较大时，需实时更新重力值。
- 异常重力场：高精度应用需引入重力异常模型或重力图匹配。
- 与哥氏项耦合：重力补偿与哥氏加速度计算相互独立，但均需准确的纬度和高度信息。
4.4 速度解算误差来源与补偿策略
| 误差来源 | 影响 | 补偿策略 |
|---|---|---|
| 加速度计零偏 | 速度随时间线性漂移 | 初始对准标定、零速修正（ZUPT） |
| 比力投影误差 | 姿态误差导致速度耦合 | 高精度姿态更新、圆锥补偿 |
| 重力模型误差 | 垂直通道发散 | 引入外部高度阻尼（如气压计） |
| 积分算法截断误差 | 高频振动引起速度噪声 | 采用高阶积分（如龙格-库塔4阶） |
总结： 速度解算的基础在于准确理解比力方程，并正确处理加速度计输出、有害加速度补偿以及重力模型。实际工程中，需结合姿态更新算法与误差补偿技术，才能实现长时间稳定的速度输出。