3、惯性导航基础（下）：姿态表示方法（欧拉角、四元数、旋转矩阵）、捷联惯导更新算法

好，我们接着聊。上一节我们把惯性导航的传感器原理和误差模型梳理了一遍。这一节，咱们要啃一块硬骨头——姿态表示。说白了，就是怎么用数学语言描述无人机“脑袋”朝哪边转。

你想想看，无人机在天上飞，机头朝东还是朝西？是平飞还是侧倾？这些信息必须精确描述，导航计算机才能算得准。我刚开始做组合导航那会儿，觉得姿态不就是三个角度嘛，有啥难的？结果第一次跑飞控，数据直接发散，差点把飞机摔了。后来才发现，是姿态表示方法没选对，算法里出现了“万向锁”。

嗯，咱们今天就把它彻底讲透。

3.1 欧拉角：最直观，但有个大坑

欧拉角，就是大家熟悉的俯仰角（Pitch）、横滚角（Roll）、偏航角（Yaw）。分别对应绕X轴、Y轴、Z轴的旋转。直观吧？非常直观。你一看数据就知道飞机是不是在低头或者侧身。

但这里有个大坑——万向锁（Gimbal Lock）。当俯仰角接近±90°时，横滚和偏航的旋转轴会重合，导致丢失一个自由度。说白了，就是飞机垂直向上或向下时，你分不清它是在滚转还是在转弯。

欧拉角的另一个问题是三角函数计算量大。每次更新都要算sin、cos，对低成本MCU来说负担不小。

3.2 旋转矩阵：数学上完美，但太“重”

旋转矩阵，也叫方向余弦矩阵（DCM）。它是一个3×3的正交矩阵，用来描述从一个坐标系到另一个坐标系的旋转关系。

数学上，它非常完美。没有奇点，可以表示任意姿态。但问题在于——它太“重”了。9个元素，每次更新都要做矩阵乘法，还要不断正交化修正，防止矩阵漂移。我早期在STM32F4上跑过纯DCM算法，CPU占用率直接飙到70%以上，留给控制环的时间就很少了。

所以，旋转矩阵现在更多用在理论推导和坐标变换中，实际嵌入式导航算法里，大家更爱用四元数。

3.3 四元数：导航算法的主力

四元数，说白了就是一个四维复数：q = w + xi + yj + zk。其中w是实部，x、y、z是虚部。它用4个参数表示三维旋转，没有奇点，计算量小，还容易做插值。

我个人习惯，所有姿态解算都用四元数。为什么？

• 无万向锁：随便你怎么转，都不会丢自由度。
• 计算量小：只有4个参数，更新时只用加减乘除，没有三角函数。
• 容易归一化：每次更新后做一次归一化，就能保证四元数的模为1，防止漂移。

我建议，你在做低成本INS/GPS组合导航时，内部姿态统一用四元数，只在输出给上层控制或显示时，才转成欧拉角。

q_new = q ⊗ [cos(|Δθ|/2), (Δθ/|Δθ|) * sin(|Δθ|/2)]

3.4 捷联惯导更新算法：从陀螺仪到姿态

好，有了姿态表示方法，我们来看看怎么用陀螺仪数据实时更新姿态。这就是捷联惯导更新算法。

核心思路很简单：陀螺仪输出的是角速度，积分得到角度增量，再用这个增量更新姿态。

具体步骤我总结为三步：

1. 读取陀螺仪数据：得到三轴角速度 ω_x, ω_y, ω_z（单位：rad/s）。
2. 计算角度增量：Δθ = ω × Δt，其中Δt是采样间隔。
3. 更新四元数：用上面提到的四元数乘法公式更新。

这里有个细节——陀螺仪有零偏。如果不补偿，积分几分钟就会漂移好几度。我建议在每次上电后，先静止采集几秒数据，取平均作为零偏，然后在算法中实时减去。

// 二阶龙格-库塔法更新四元数
void quaternion_update(quat_t *q, float gx, float gy, float gz, float dt) {
    float half_dt = 0.5f * dt;
    float q0 = q->w, q1 = q->x, q2 = q->y, q3 = q->z;
    
    // 一阶估计
    float dq0 = 0.5f * (-q1*gx - q2*gy - q3*gz);
    float dq1 = 0.5f * ( q0*gx + q2*gz - q3*gy);
    float dq2 = 0.5f * ( q0*gy - q1*gz + q3*gx);
    float dq3 = 0.5f * ( q0*gz + q1*gy - q2*gx);
    
    // 二阶修正
    q->w = q0 + half_dt * dq0;
    q->x = q1 + half_dt * dq1;
    q->y = q2 + half_dt * dq2;
    q->z = q3 + half_dt * dq3;
    
    // 归一化
    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;
}

3.5 三种姿态表示方法的对比

为了让你一目了然，我整理了一个表格：

看到没？四元数在各方面都占优。所以，我建议你把四元数作为姿态解算的“内功心法”，欧拉角只作为“外功招式”用于输出。

3.6 小结与避坑

这一节内容不少，我帮你捋一下重点：

• 欧拉角：直观但有万向锁，别在大机动飞行时用它做解算。
• 旋转矩阵：数学完美但计算量大，适合理论分析。
• 四元数：导航算法的主力，无奇点、计算快、易归一化。
• 捷联惯导更新：核心是角速度积分+四元数更新，别忘了补偿陀螺零偏。

下一节，我们会把加速度计和磁力计的数据融合进来，用卡尔曼滤波修正陀螺仪的漂移。嗯，那才是组合导航真正精彩的地方。