3、惯性导航原理（下）：捷联惯导系统（SINS）基本算法、姿态更新（四元数法）、速度与位置更新

好，咱们接着聊。上一节我们把惯性导航的“骨架”——比力方程、哥氏加速度这些概念理清了。这一节，咱们要动真格的了，直接上手捷联惯导系统（SINS）的核心算法。

说白了，捷联惯导就是把IMU“焊死”在载体上，然后靠计算机去“虚拟”出一个数学平台。这个平台稳不稳，全看算法硬不硬。我个人习惯把SINS算法拆成三块：姿态更新、速度更新、位置更新。其中姿态更新是灵魂，也是最容易出bug的地方。

3.1 姿态更新：为什么我偏爱四元数法？

姿态描述，说白了就是搞清楚“载体坐标系”相对于“导航坐标系”到底转了多少。方法有好几种：欧拉角、方向余弦矩阵、四元数。

欧拉角直观，但有“万向节死锁”这个硬伤，在飞行器大角度机动时直接完蛋。方向余弦矩阵（DCM）没有奇点，但9个参数，计算量太大，在嵌入式里跑起来有点吃力。

所以，工程上最常用的就是四元数法。它只有4个参数，没有奇点，计算量适中。嗯，这里要注意：四元数虽然数学上很优雅，但物理意义不太直观。你想想看，一个四维的超复数在描述三维旋转，刚开始接触确实有点绕。

我在项目中遇到过一位同事，直接用欧拉角做姿态解算，结果飞机在做横滚机动时，俯仰角直接跳变，差点炸机。从那以后，我们团队所有SINS项目，姿态更新一律用四元数。

3.1.1 四元数的基本概念

一个四元数可以写成：

q = q0 + q1*i + q2*j + q3*k

其中，q0是标量部分，q1,q2,q3是矢量部分。它满足一个约束：q0² + q1² + q2² + q3² = 1。这个叫归一化约束，非常重要。如果计算过程中四元数模长偏离1，姿态就会慢慢漂移。

四元数描述旋转的核心思想是：任何三维旋转都可以表示为绕某个单位轴旋转某个角度。这个轴和角度，就编码在四元数的四个分量里。

q = cos(θ/2) + (ux*sin(θ/2))*i + (uy*sin(θ/2))*j + (uz*sin(θ/2))*k

3.1.2 姿态更新算法（四元数法）

姿态更新的本质，就是求解四元数的微分方程。陀螺仪输出的是角速度 ω，我们需要用这个角速度去更新四元数。

四元数的微分方程长这样：

dq/dt = 0.5 * q ⊗ ω

这里的 ⊗ 是四元数乘法，ω 是角速度构成的纯四元数 (0, ωx, ωy, ωz)。

在实际代码里，我们用的是离散化的形式。最常用的是毕卡（Picard）逼近法，也叫一阶龙格-库塔法。但说实话，如果陀螺仪采样率够高（比如200Hz以上），直接用一阶近似就够了。

我曾经在一个低成本的MEMS项目里，为了省算力，用了最简单的算法，结果姿态发散得厉害。后来发现是采样率太低，角速度变化太快，一阶近似跟不上。所以，算法精度要和硬件性能匹配。

下面是一个典型的姿态更新代码片段（C语言风格）：

// 输入：上一时刻四元数 q，角速度增量 dtheta (三个轴)
// 输出：更新后的四元数 q_new
void attitude_update(float q[4], float dtheta[3]) {
    float dtheta2 = dtheta[0]*dtheta[0] + dtheta[1]*dtheta[1] + dtheta[2]*dtheta[2];
    float s, c;
    
    if (dtheta2 < 1.0e-8) {
        // 小角度近似
        s = 0.5;
        c = 1.0;
    } else {
        float theta = sqrtf(dtheta2);
        float half_theta = 0.5 * theta;
        s = sinf(half_theta) / theta;
        c = cosf(half_theta);
    }
    
    // 构造旋转四元数 dq
    float dq[4];
    dq[0] = c;
    dq[1] = s * dtheta[0];
    dq[2] = s * dtheta[1];
    dq[3] = s * dtheta[2];
    
    // 四元数乘法：q_new = q ⊗ dq
    float q_new[4];
    q_new[0] = q[0]*dq[0] - q[1]*dq[1] - q[2]*dq[2] - q[3]*dq[3];
    q_new[1] = q[0]*dq[1] + q[1]*dq[0] + q[2]*dq[3] - q[3]*dq[2];
    q_new[2] = q[0]*dq[2] - q[1]*dq[3] + q[2]*dq[0] + q[3]*dq[1];
    q_new[3] = q[0]*dq[3] + q[1]*dq[2] - q[2]*dq[1] + q[3]*dq[0];
    
    // 归一化
    float norm = sqrtf(q_new[0]*q_new[0] + q_new[1]*q_new[1] + 
                       q_new[2]*q_new[2] + q_new[3]*q_new[3]);
    q[0] = q_new[0] / norm;
    q[1] = q_new[1] / norm;
    q[2] = q_new[2] / norm;
    q[3] = q_new[3] / norm;
}

3.2 速度更新：比力方程怎么落地？

速度更新，说白了就是对比力方程做积分。加速度计测量的是比力 f，我们需要从中扣除重力 g 和哥氏项，才能得到真正的运动加速度。

速度更新的离散形式：

v_new = v_old + [C(q) * f - (2*ω_ie + ω_en) × v_old + g] * dt

这里 C(q) 是从载体坐标系到导航坐标系的旋转矩阵，由四元数 q 转换而来。f 是加速度计输出的比力。

在实际工程中，我建议把速度更新和姿态更新分开做。为什么？因为姿态更新频率通常比速度更新高。陀螺仪数据来得快，姿态需要高频更新；而速度更新可以稍微慢一点，和加速度计数据同步即可。

我见过有人把姿态和速度放在一个循环里更新，结果姿态更新频率被速度更新拖慢，导致姿态精度下降。嗯，这里要注意：解耦更新频率是工程优化的关键。

3.2.1 圆锥误差与划船误差

这里要提两个坑：圆锥误差和划船误差。

• 圆锥误差：当载体做圆锥运动时，如果只用角速度的简单积分，姿态会发散。这是由角速度的不可交换性引起的。
• 划船误差：类似地，当载体做划船运动时，速度积分也会产生误差。

我曾经在一个高动态的无人机项目里，忽略了圆锥误差补偿，结果悬停时姿态一直在漂。后来加了多子样算法，问题才解决。

解决方法是使用多子样算法，比如双子样、三子样。简单说，就是在一次更新周期内，对多个陀螺仪采样做更精细的积分，补偿掉不可交换性误差。

3.3 位置更新：从速度到经纬高

位置更新相对简单，就是对速度做积分。但要注意，地球是椭球体，经纬度的计算要考虑曲率半径。

位置更新的离散形式：

lat_new = lat_old + v_N * dt / (R_M + h)
lon_new = lon_old + v_E * dt / ((R_N + h) * cos(lat))
h_new = h_old + v_D * dt

其中：

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

这里有个细节：高度更新用的是地向速度 v_D，方向是向下为正。所以高度增加时，v_D 是负的。这个符号问题，我刚开始做的时候搞反过，结果导航轨迹直接钻到地底下去了。

3.4 完整的SINS解算流程

把上面三块串起来，就是一个完整的SINS解算周期：

1. 读取IMU数据：陀螺仪角速度 ω，加速度计比力 f
2. 姿态更新：用 ω 更新四元数 q
3. 速度更新：用 q 和 f 更新速度 v
4. 位置更新：用 v 更新经纬高
5. 输出：当前姿态、速度、位置

这个流程在每个IMU数据到来时执行一次。如果IMU输出频率是100Hz，那这个流程每秒跑100次。

好了，这一节的内容就到这。说白了，SINS算法就是把物理方程翻译成计算机能跑的代码。姿态更新是核心，速度和位置更新是顺理成章的事。下一节，我们会把SINS和GPS结合起来，看看组合导航是怎么工作的。