第4章姿态解算

姿态解算：互补滤波算法原理、Mahony滤波算法实现、四元数更新与归一化

各位同学，欢迎来到第四讲。前面我们搞定了传感器数据读取，也聊了聊坐标系。但传感器原始数据是“脏”的，有噪声、有漂移。怎么把这些乱七八糟的数据变成干净、可用的姿态？这就是姿态解算要干的事。

说白了，姿态解算就是回答一个问题：“我现在朝哪个方向？” 我个人的经验是，这一步做不好，后面PID调得再好，飞机也是“醉汉走路”。

互补滤波：为什么需要它？

先聊聊传感器各自的“性格”。

陀螺仪：动态响应快，短时间很准。但积分会漂移，时间一长就“跑偏”。
加速度计：静态很准，能告诉你重力方向。但一有震动或快速运动，数据就“炸毛”。
磁力计：能告诉你北在哪，但容易受环境干扰，像个“路痴”。

你看，没有一个传感器是完美的。那怎么办？互补滤波的思路很简单：取长补短。

陀螺仪负责“快变”的部分，加速度计和磁力计负责“慢变”的部分。两者通过一个权重系数融合起来。这个系数，我习惯叫它 alpha。

核心公式（一维角度为例）：

angle = alpha * (angle + gyro_data * dt) + (1 - alpha) * acc_angle

其中 alpha 通常取 0.98 左右。越接近1，越信任陀螺仪；越接近0，越信任加速度计。

嗯，这里要注意：alpha 不是拍脑袋定的。我在项目里调试时，一般先设0.98，然后看悬停时的角度波动。如果波动大，就稍微降一点。如果响应太慢，就升一点。说白了，这是个“手感活”。

Mahony滤波算法：从一维到三维

一维互补滤波好理解，但四轴飞行器是三维空间旋转。这时候就需要更高级的工具——Mahony滤波算法。

Mahony算法本质上还是互补滤波的思想，但它用四元数来表示姿态，并且引入了PI控制器来修正陀螺仪的漂移。我个人觉得，它是“互补滤波的工程化升级版”。

算法流程大致分三步：

预测：用陀螺仪数据更新四元数。
观测：用加速度计和磁力计数据计算“期望的姿态”。
修正：计算预测和观测之间的误差，用PI控制器修正陀螺仪偏差。

为什么会需要PI控制器？你想想看，陀螺仪的零偏是缓慢变化的。只用比例修正，永远会有静差。加上积分项，才能彻底消除漂移。我曾经在一个项目中，忘了加积分项，结果飞机悬停10秒后就开始慢慢自旋……嗯，那感觉，挺尴尬的。

四元数更新与归一化

四元数更新，说白了就是解微分方程。我们用的是一阶龙格-库塔法，也就是最简单的数值积分。

公式长这样：

// 四元数更新（一阶龙格-库塔）
float q0_dot = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
float q1_dot = 0.5f * ( q0 * gx + q2 * gz - q3 * gy);
float q2_dot = 0.5f * ( q0 * gy - q1 * gz + q3 * gx);
float q3_dot = 0.5f * ( q0 * gz + q1 * gy - q2 * gx);

q0 += q0_dot * dt;
q1 += q1_dot * dt;
q2 += q2_dot * dt;
q3 += q3_dot * dt;

这里 gx, gy, gz 是陀螺仪角速度（弧度/秒），dt 是采样周期。

归一化是必须的！

四元数在数学上要求模长为1。但数值积分会引入误差，导致模长偏离1。如果不归一化，姿态会“扭曲”。

我曾经见过一个新手，代码里忘了归一化，结果飞机在仿真里姿态越转越“胖”，最后直接崩溃。所以，每次更新完四元数，立刻做归一化，养成习惯。

归一化代码很简单：

float norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 /= norm;
q1 /= norm;
q2 /= norm;
q3 /= norm;

Mahony算法完整实现

下面是我常用的Mahony算法核心代码。注意，这里只展示了姿态解算部分，磁力计融合我简化了，实际项目中需要根据磁场方向做补偿。

void MahonyAHRSupdate(float gx, float gy, float gz, 
                      float ax, float ay, float az,
                      float mx, float my, float mz, float dt) {
    float recipNorm;
    float q0q0, q0q1, q0q2, q0q3;
    float q1q1, q1q2, q1q3;
    float q2q2, q2q3, q3q3;

    // 1. 归一化加速度计数据
    recipNorm = 1.0f / sqrt(ax*ax + ay*ay + az*az);
    ax *= recipNorm;
    ay *= recipNorm;
    az *= recipNorm;

    // 2. 归一化磁力计数据（略，实际需做硬铁/软铁校准）

    // 3. 计算误差（加速度计）
    float halfvx = q1q3 - q0q2;
    float halfvy = q0q1 + q2q3;
    float halfvz = q0q0 - 0.5f + q3q3;

    float halfex = ay * halfvz - az * halfvy;
    float halfey = az * halfvx - ax * halfvz;
    float halfez = ax * halfvy - ay * halfvx;

    // 4. PI控制器修正陀螺仪
    integralFBx += twoKi * halfex * dt;
    integralFBy += twoKi * halfey * dt;
    integralFBz += twoKi * halfez * dt;

    gx += twoKp * halfex + integralFBx;
    gy += twoKp * halfey + integralFBy;
    gz += twoKp * halfez + integralFBz;

    // 5. 四元数更新
    float qa = q0;
    float qb = q1;
    float qc = q2;
    q0 += (-qb*gx - qc*gy - q3*gz) * 0.5f * dt;
    q1 += ( qa*gx + qc*gz - q3*gy) * 0.5f * dt;
    q2 += ( qa*gy - qb*gz + q3*gx) * 0.5f * dt;
    q3 += ( qa*gz + qb*gy - qc*gx) * 0.5f * dt;

    // 6. 归一化
    recipNorm = 1.0f / sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
    q0 *= recipNorm;
    q1 *= recipNorm;
    q2 *= recipNorm;
    q3 *= recipNorm;
}

调参小技巧：

twoKp 是比例增益，控制收敛速度。我一般从0.5开始调。twoKi 是积分增益，控制漂移抑制，从0.05开始。如果发现姿态震荡，先降 twoKp。如果发现静态漂移，再升 twoKi。

知识体系总览

下面这张图，是我自己总结的姿态解算知识结构。你可以把它当作“地图”，随时回来看看自己走到哪了。

从这张图你可以看到，整个流程是线性的：传感器数据 → 互补滤波（一维理解） → Mahony滤波（三维实现） → 四元数更新 → 归一化 → 输出。每一步都环环相扣。

好了，这一讲的内容就到这里。姿态解算是飞控的“眼睛”，眼睛亮了，后面的控制才能稳。希望你能把代码跑起来，亲手调一调参数，感受一下“从乱数据到稳定姿态”的奇妙过程。

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