第四章 捷联惯导系统力学编排：比力方程、姿态更新算法（四元数法）、速度更新算法、位置更新算法

各位工程师朋友，今天我们聊聊捷联惯导系统的力学编排。说实话，这部分内容是整个惯导系统的核心算法，也是我当年刚入行时啃得最久的一块硬骨头。你想想看，一个陀螺仪输出角速度，一个加速度计输出比力，怎么就能算出位置、速度和姿态呢？这背后的数学逻辑，就是力学编排要解决的问题。

我个人习惯把力学编排分成四个模块：比力方程、姿态更新、速度更新、位置更新。它们环环相扣，缺一不可。下面我们一个一个来拆解。

4.1 比力方程：从加速度计到导航加速度

加速度计测量的是什么？是比力，不是真正的加速度。这个区别很关键。我在项目中遇到过一位刚毕业的同事，直接把加速度计读数当加速度积分，结果位置误差飞到了天上去。嗯，这里要注意：比力方程的作用，就是把加速度计测得的比力，转换成导航坐标系下的加速度。

比力方程的标准形式是这样的：

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

其中：

• f^n：导航坐标系下的比力（加速度计输出转换后）
• v̇^n：导航坐标系下的加速度（我们要算的）
• 2ω_ie^n × v^n：地球自转引起的哥氏加速度
• ω_en^n × v^n：载体运动引起的向心加速度
• g^n：重力加速度

说白了，加速度计测到的力，包含了运动加速度、哥氏加速度、向心加速度和重力。我们要把后三项去掉，才能得到真正的运动加速度。我建议你在做算法验证时，先单独测试比力方程这部分——给一个静态的比力输入，看看能不能正确扣除重力。

4.2 姿态更新算法：四元数法

姿态更新是捷联惯导的「灵魂」。为什么？因为所有的比力转换、速度积分，都依赖于当前姿态。姿态算错了，后面全完蛋。

四元数法是目前工程中最常用的姿态更新方法。为什么不用欧拉角？因为欧拉角有万向锁问题，而且计算量大。四元数没有奇点，计算效率高，非常适合嵌入式实时系统。

四元数姿态更新的核心公式：

q̇ = 0.5 × q ⊗ ω

其中 q 是姿态四元数，ω 是角速度（陀螺仪输出），⊗ 表示四元数乘法。

实际工程中，我们用的是离散化形式。我记得有一次调试一个高动态飞行器，采样率只有100Hz，但角速度变化很快。如果用简单的欧拉法更新四元数，姿态误差会迅速累积。后来我换成了二阶龙格-库塔法，效果好了很多。

这里给出一个常用的四元数更新代码片段（C语言风格）：

// 四元数更新（二阶龙格-库塔法）
void quaternion_update(quat_t *q, const vec3_t *omega, float dt) {
    float wx = omega->x * 0.5f;
    float wy = omega->y * 0.5f;
    float wz = omega->z * 0.5f;
    
    float q0 = q->w, q1 = q->x, q2 = q->y, q3 = q->z;
    
    // 计算导数
    float dq0 = -q1*wx - q2*wy - q3*wz;
    float dq1 =  q0*wx + q2*wz - q3*wy;
    float dq2 =  q0*wy - q1*wz + q3*wx;
    float dq3 =  q0*wz + q1*wy - q2*wx;
    
    // 更新四元数
    q->w += dq0 * dt;
    q->x += dq1 * dt;
    q->y += dq2 * dt;
    q->z += dq3 * dt;
    
    // 归一化（防止累积误差）
    float norm = sqrt(q->w*q->w + q->x*q->x + q->y*q->y + q->z*q->z);
    q->w /= norm;
    q->x /= norm;
    q->y /= norm;
    q->z /= norm;
}

4.3 速度更新算法

速度更新相对直观一些。有了导航坐标系下的加速度，直接积分就能得到速度。但实际工程中，有几个坑要注意。

速度更新的基本公式：

v^n(t+Δt) = v^n(t) + ∫[f^n(t) + g^n - (2ω_ie^n + ω_en^n) × v^n(t)] dt

你可能会问：为什么不能简单用 v = v0 + a*Δt？因为加速度本身是时变的，而且哥氏项和向心项也依赖于当前速度。说白了，这是一个耦合的微分方程。

我建议采用梯形积分法，比矩形法精度高一个阶次：

// 速度更新（梯形法）
void velocity_update(nav_state_t *nav, const vec3_t *f_n, float dt) {
    vec3_t a_prev = nav->accel;  // 上一时刻加速度
    vec3_t a_curr = *f_n;        // 当前时刻加速度
    
    // 梯形积分
    nav->velocity.x += (a_prev.x + a_curr.x) * 0.5f * dt;
    nav->velocity.y += (a_prev.y + a_curr.y) * 0.5f * dt;
    nav->velocity.z += (a_prev.z + a_curr.z) * 0.5f * dt;
    
    // 更新加速度缓存
    nav->accel = a_curr;
}

4.4 位置更新算法

位置更新是力学编排的最后一步。有了速度，积分得到位置。但这里有一个细节：位置是用经纬高表示的，不是直角坐标。

位置更新的核心公式：

φ̇ = v_N / (R_M + h)
λ̇ = v_E / ((R_N + h) * cos(φ))
ḣ = v_D

其中：

• φ, λ, h：纬度、经度、高度
• v_N, v_E, v_D：北向、东向、地向速度
• R_M, R_N：子午圈曲率半径、卯酉圈曲率半径

这里有个容易忽略的点：R_M 和 R_N 是随纬度变化的。我见过有人直接用地球平均半径6371km，在低纬度地区误差不大，但到了高纬度地区，位置误差会显著增大。

实际工程中，位置更新通常用一阶欧拉法就够了，因为位置变化相对较慢。但如果你做的是长时间导航（比如潜艇水下航行几天），建议用二阶方法。

// 位置更新（经纬高）
void position_update(nav_state_t *nav, float dt) {
    float lat = nav->latitude;
    float h   = nav->altitude;
    
    // 计算曲率半径
    float sin_lat = sin(lat);
    float R_M = RE * (1 - E2) / pow(1 - E2*sin_lat*sin_lat, 1.5);
    float R_N = RE / sqrt(1 - E2*sin_lat*sin_lat);
    
    // 更新位置
    nav->latitude  += nav->v_north / (R_M + h) * dt;
    nav->longitude += nav->v_east  / ((R_N + h) * cos(lat)) * dt;
    nav->altitude  -= nav->v_down  * dt;  // 注意：向下为正
}

知识体系总览

下面我用一张SVG图来总结本章的核心逻辑。你一看就明白了：

从这张图可以清楚看到：陀螺仪输出给姿态更新，姿态更新算出姿态矩阵后，把加速度计的比力转换到导航系。然后比力方程扣除哥氏项和重力，得到导航加速度。接着积分得到速度，再积分得到位置。同时，速度还会反馈回比力方程，用于计算哥氏项。这是一个闭环系统。

好了，以上就是捷联惯导力学编排的四个核心算法。我个人觉得，理解这些公式背后的物理意义，比死记硬背公式更重要。你想想看，当你真正理解了「比力方程是在扣除什么」、「四元数为什么没有奇点」、「速度积分为什么不能用矩形法」，你就能在工程中灵活应对各种问题了。

---

公众号:蓝海资料掘金营，微信deep3321