📚 课程目录第4章速度微分方程

4、速度微分方程：比力方程推导、哥氏加速度与向心加速度补偿、重力模型在速度积分中的处理

捷联惯导系统（SINS）的速度解算本质上是比力方程（Specific Force Equation）的数值积分过程。与平台式惯导不同，捷联系统通过数学平台替代物理平台，将加速度计测量的比力从载体坐标系（b系）转换到导航坐标系（n系），再扣除有害加速度（哥氏加速度、向心加速度）和重力加速度，最终得到载体在导航系中的速度增量。本章将深入剖析比力方程的物理含义、哥氏项与向心项的补偿机理，以及重力模型在速度积分中的具体处理方法，帮助学习者建立从传感器原始输出到导航速度的完整数学链路。

上图展示了捷联惯导速度解算的完整信号流。加速度计输出的比力 f_b 经过姿态矩阵 C_b^n 变换到导航系，得到 f_n；同时，陀螺仪输出用于更新姿态矩阵，并参与计算哥氏加速度与向心加速度补偿项；最后，将补偿后的比力与重力模型输出相加，进行积分得到速度。整个过程是一个闭环反馈系统，位置和速度结果又反过来影响重力模型和有害加速度的计算。

4.1 比力方程的物理内涵与数学推导

比力方程是捷联惯导速度解算的核心，它描述了载体在导航坐标系中的加速度与加速度计测量值之间的关系。加速度计本质上测量的是比力（Specific Force），即单位质量上作用的非引力外力，而非直接的加速度。在惯性坐标系（i系）中，牛顿第二定律可表示为：

m · a_i = F_gravity + F_external

其中 F_gravity 为万有引力，F_external 为其他外力（推力、气动力等）。加速度计感受到的是支撑力（即非引力外力），其输出比力 f = F_external / m。因此，在惯性系中，载体的加速度为：

a_i = g_i + f_i

其中 g_i 为引力加速度。然而，捷联惯导通常在地球表面附近工作，导航坐标系（n系，通常为当地水平坐标系）相对于惯性系存在旋转和运动。因此，需要将上述关系转换到导航系中。

根据哥氏定理（Coriolis Theorem），任意矢量在旋转坐标系中的导数与在惯性系中的导数满足：

(d/dt)_i = (d/dt)_n + ω_in ×

将载体的位置矢量 r 应用两次哥氏定理，可以得到导航系中的加速度表达式。经过推导，最终得到捷联惯导中广泛使用的比力方程：

v̇^n = f^n - (2ω_ie^n + ω_en^n) × v^n + g^n

式中各符号含义如下：

v̇^n：载体在导航系中的加速度矢量（对地速度的导数）；
f^n：加速度计测量的比力经坐标变换后在导航系中的投影；
ω_ie^n：地球自转角速度在导航系中的投影（约15.04°/h）；
ω_en^n：导航系相对地球系的旋转角速度（由载体运动引起，包含纬度和东向速度分量）；
v^n：载体相对地球的速度（即对地速度）；
g^n：重力加速度在导航系中的投影（包含引力与离心力）。

该方程中，-(2ω_ie^n + ω_en^n) × v^n 即为有害加速度项，其中 2ω_ie^n × v^n 是哥氏加速度（Coriolis Acceleration），ω_en^n × v^n 是向心加速度（Centripetal Acceleration）的组成部分。这些项并非真实作用在载体上的力，而是由于坐标系旋转产生的虚拟力，必须在速度积分前予以扣除。

  核心理解：比力方程的本质是“测量值 = 真实加速度 + 有害加速度 - 重力”。加速度计无法区分引力与惯性力，因此必须通过数学手段将坐标系旋转引起的附加项和重力项从测量值中分离，才能得到载体相对地球的真实运动加速度。

4.2 哥氏加速度与向心加速度的补偿机理

哥氏加速度和向心加速度是比力方程中两个关键的补偿项，它们的物理来源和计算方法各有不同，但在工程实现中通常合并处理。

哥氏加速度（Coriolis Acceleration）：当载体在地球表面运动时，由于地球自转，载体的速度方向在惯性空间中不断改变，从而产生哥氏加速度。其表达式为 a_cor = 2ω_ie × v。在导航系中，地球自转角速度 ω_ie^n 的分量为：

ω_ie^n = [0, ω_ie · cos(L), ω_ie · sin(L)]^T

其中 L 为当地纬度。哥氏加速度的典型量级：当载体以 100 m/s 的速度向东飞行时，在中纬度地区（L=45°），哥氏加速度约为 2 × 7.29e-5 × 100 × sin(45°) ≈ 0.0103 m/s²。虽然数值不大，但在高精度导航中不可忽略，尤其是长时间积分后会产生显著的位置误差。

向心加速度（Centripetal Acceleration）：由载体相对地球运动引起导航系旋转而产生。导航系相对地球系的旋转角速度 ω_en^n 为：

ω_en^n = [-v_N / (R_M + h), v_E / (R_N + h), v_E · tan(L) / (R_N + h)]^T

其中 v_N、v_E 分别为北向和东向速度，R_M、R_N 分别为子午圈和卯酉圈曲率半径，h 为高度。向心加速度项 ω_en^n × v^n 的典型量级在高速运动时可达 10⁻³ ~ 10⁻² m/s²。

在工程实现中，这两项通常合并计算：

a_harmful = (2ω_ie^n + ω_en^n) × v^n

具体计算流程如下：

根据当前纬度和高度，计算 ω_ie^n 的三个分量；
根据当前北向、东向速度和曲率半径，计算 ω_en^n 的三个分量；
将两者相加得到 Ω = 2ω_ie^n + ω_en^n；
计算叉积 Ω × v^n，得到有害加速度矢量；
在速度更新时，从比力中减去该矢量。

工程陷阱：在低精度或简化模型中，部分实现会忽略 ω_en^n 项，仅补偿哥氏加速度。这在低速、短航时场景下误差可接受，但在高动态飞行器（如战斗机、导弹）或长航时导航中，忽略向心加速度会导致速度误差累积，最终造成数百米的定位偏差。

4.3 重力模型在速度积分中的处理

重力模型是比力方程中另一个关键组成部分。严格来说，g^n 是重力加速度（引力与地球自转离心力的合力），而非纯引力。在捷联惯导中，重力模型的精度直接影响速度积分和位置解算的准确性。

正常重力模型：工程中最常用的是 WGS-84 椭球模型下的正常重力公式。该公式将地球视为旋转椭球体，给出海平面上的重力随纬度变化的解析表达式：

g_0(L) = 9.7803253359 × (1 + 0.00193185265241 × sin²(L)) / sqrt(1 - 0.00669437999014 × sin²(L))

对于非零高度，还需进行高度修正：

g(h) = g_0(L) / (1 + h / R_E)²

其中 R_E 为地球平均半径（约6371 km）。在导航系中，重力矢量通常表示为：

g^n = [0, 0, -g]^T

即北向和东向分量为零，天向分量为负值（指向地心）。

重力扰动：正常重力模型忽略了地球内部质量分布不均匀引起的重力异常。在高精度导航（如潜艇长航时、精密测量）中，需要考虑重力扰动矢量 δg^n，其量级可达 10⁻⁴ ~ 10⁻³ m/s²。常用的重力扰动模型包括 EGM2008、EGM96 等球谐展开模型，或使用重力图数据库进行查表插值。

在速度积分中的处理策略：

应用场景	重力模型选择	更新频率	精度需求
消费级无人机/车辆	常数重力（9.81 m/s²）	初始化时设定	低（米级定位）
中精度航空导航	WGS-84 正常重力 + 高度修正	每次速度更新	中（亚米级）
高精度航海/导弹	正常重力 + 重力扰动模型	每次速度更新	高（厘米级）

在离散化实现中，重力模型通常在每个速度更新周期内计算一次，并与有害加速度补偿项一起从比力中扣除。由于重力随位置变化缓慢，在高速运动时（如导弹再入段），需要根据实时位置（纬度和高度）更新重力值。

实现建议：在嵌入式系统中，为避免重复计算复杂的三角函数，可预先计算当前纬度的 sin(L)、cos(L) 值，并缓存曲率半径 R_M、R_N。当位置变化较小时（如步长小于10米），可降低重力更新频率，以节省计算资源。

4.4 案例拆解：某型无人机速度解算中的补偿实现

下面通过一个实际工程案例，展示比力方程在无人机导航计算机中的具体实现过程。

背景与约束：某型固定翼无人机搭载战术级 MEMS 惯导系统，飞行速度约 50 m/s，飞行高度 3000 m，纬度 30°N。导航计算机主频 200 MHz，速度更新周期 5 ms（200 Hz）。要求速度解算精度优于 0.05 m/s（1σ）。

初始现象与目标：在初步实现中，工程师仅补偿了哥氏加速度，忽略了向心加速度项。试飞数据显示，在持续 30 分钟的飞行中，东向速度误差达到 1.2 m/s，位置误差超过 2 km。目标是将速度误差控制在 0.05 m/s 以内。

关键输入数据：

纬度 L = 30°，sin(L) = 0.5，cos(L) = 0.866；
地球自转角速度 ω_ie = 7.2921159e-5 rad/s；
飞行速度 v_E = 50 m/s，v_N = 0 m/s；
曲率半径 R_N ≈ 6388 km，R_M ≈ 6352 km（含高度修正）；
重力加速度 g ≈ 9.797 m/s²（WGS-84 模型计算）。

分析判断过程：

忽略向心加速度时：
a_cor = 2ω_ie^n × v^n = 2 × 7.29e-5 × [0, 0.866, 0.5] × [50, 0, 0]
      = 2 × 7.29e-5 × [0, 0.5×50, -0.866×50]
      = [0, 3.65e-3, -6.31e-3] m/s²

考虑向心加速度时：
ω_en^n = [0, 50/6388000, 50×tan(30°)/6388000]
       = [0, 7.83e-6, 4.52e-6] rad/s
a_cent = ω_en^n × v^n = [0, 7.83e-6, 4.52e-6] × [50, 0, 0]
       = [0, 4.52e-6×50, -7.83e-6×50]
       = [0, 2.26e-4, -3.92e-4] m/s²

总有害加速度 = a_cor + a_cent ≈ [0, 3.88e-3, -6.70e-3] m/s²

忽略向心项导致东向加速度误差约 2.26e-4 m/s²，30 分钟（1800 s）积分后速度误差为 2.26e-4 × 1800 ≈ 0.41 m/s，加上其他误差源，最终达到 1.2 m/s 的偏差。

实际操作步骤：

在速度更新函数中增加 ω_en^n 的计算模块，使用当前速度和位置参数；
将 ω_en^n 与 2ω_ie^n 相加，再与速度矢量做叉积；
在重力模型中引入 WGS-84 正常重力公式，并根据高度进行修正；
将补偿后的比力与重力相加，进行数值积分（采用二阶龙格-库塔法）；
在每次位置更新后，刷新纬度和高度值，用于下一周期的补偿计算。

结果对比：修改后，同样 30 分钟飞行，东向速度误差降至 0.03 m/s，位置误差缩小至 150 m 以内，满足指标要求。

风险与代价：增加 ω_en^n 计算使单次速度更新的 CPU 开销增加约 12%，但仍在预算内。需要额外存储曲率半径参数，内存占用增加 32 字节。

复盘经验：在高速、高纬度飞行场景下，向心加速度项不可忽略。设计初期应充分评估各项误差源的量级，避免后期返工。同时，重力模型的选择应与系统精度指标匹配，过度简化或过度复杂都会带来不必要的风险。

4.5 常见误区与总结复盘

在比力方程的实现过程中，工程师常犯以下几类错误，需要特别警惕：

坐标系混淆：将 ω_ie^n 误用为 ω_ie^b（载体坐标系中的地球自转角速度），导致补偿方向错误。必须确保所有矢量都在同一坐标系下运算。
叉积方向错误：比力方程中的叉积顺序为 (2ω_ie^n + ω_en^n) × v^n，不能交换顺序。叉积不满足交换律，顺序颠倒会导致补偿项符号错误。
重力符号处理：在导航系（NED 或 ENU）中，重力矢量方向指向地心。若采用 NED 坐标系，g^n = [0, 0, g]（向下为正）；若采用 ENU 坐标系，g^n = [0, 0, -g]。符号错误会导致垂直通道发散。
忽略高度对重力的影响：在高层建筑或山区飞行时，高度变化引起的重力变化可达 3e-6 m/s²/m。在 3000 m 高度，重力减少约 0.009 m/s²，若忽略将产生持续加速度误差。
积分方法选择不当：简单的欧拉积分在动态环境下误差较大，建议至少使用二阶龙格-库塔法或更高级的积分方法。

  总结：比力方程是捷联惯导速度解算的基石。正确理解哥氏加速度与向心加速度的物理来源，合理选择重力模型，并在离散化实现中注意坐标系一致性、叉积顺序和符号处理，是保证速度解算精度的关键。建议在系统联调前，使用仿真数据对比不同补偿策略的误差特性，形成标准化的测试用例库。