3、姿态解算入门：欧拉角与四元数、互补滤波算法原理、代码实现与调试

姿态解算，说白了就是让飞控知道「我现在是头朝上还是脚朝下」。你想想看，无人机在空中飘着，如果没有一个准确的姿态信息，它连自己歪没歪都不知道，更别提稳定飞行了。

我个人习惯把姿态解算比作「给飞控装一个内耳前庭」。我们人类靠内耳感知倾斜，飞控则靠陀螺仪和加速度计。但这两样东西各有毛病——陀螺仪长期漂移，加速度计短期抖动。怎么把它们捏合到一起？这就是本章要聊的核心。

3.1 欧拉角：最直观的姿态表示

欧拉角就是三个角度：横滚（Roll）、俯仰（Pitch）、偏航（Yaw）。

我在项目中遇到过一个问题：用欧拉角做姿态控制时，到了90度俯仰突然就失控了。后来才意识到，这就是著名的「万向锁」问题。说白了，当俯仰角接近±90°时，横滚和偏航会耦合在一起，丢失一个自由度。

3.2 四元数：没有死角的姿态神器

四元数是什么？你可以把它理解成一个「带方向的旋转轴 + 旋转角度」。它用四个数表示姿态：q = [w, x, y, z]，其中 w 是标量部分，[x, y, z] 是矢量部分。

为什么飞控圈都爱用四元数？因为它没有万向锁，而且计算效率高。我刚开始学的时候也觉得四元数很抽象，但后来发现，你根本不需要理解它的几何意义——你只需要知道怎么用它做运算就够了。

嗯，这里要注意：四元数必须归一化。也就是 w² + x² + y² + z² = 1。如果不归一化，姿态会越算越歪。

3.3 互补滤波：把两个传感器的优点结合起来

陀螺仪动态响应快，但会漂移。加速度计静态准确，但怕振动。互补滤波的思路很简单：

• 陀螺仪：负责短期姿态变化，高通滤波
• 加速度计：负责长期姿态校准，低通滤波

说白了就是：陀螺仪算出来的姿态，用加速度计慢慢「拉回来」。这个拉回来的力度，由一个参数 Kp 控制。

我曾经在调试一款四轴时，发现悬停时姿态总在缓慢漂移。查了半天，原来是互补滤波的 Kp 设得太小了，加速度计拉不回来。调大 Kp 后，问题立刻解决。

3.4 代码实现：STM32上的互补滤波

下面是我在项目中实际用过的互补滤波代码。注意，这里用的是 Mahony 算法的一个简化版本，非常适合 STM32F4 系列跑。

// 互补滤波姿态解算
// gx, gy, gz: 陀螺仪角速度 (rad/s)
// ax, ay, az: 加速度计测量值 (归一化后)
// dt: 采样时间 (s)

void complementary_filter(float gx, float gy, float gz,
                         float ax, float ay, float az, float dt)
{
    float recipNorm;
    float halfvx, halfvy, halfvz;
    float halfex, halfey, halfez;

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

    // 2. 用当前四元数估计重力方向
    halfvx = q1*q3 - q0*q2;
    halfvy = q0*q1 + q2*q3;
    halfvz = q0*q0 - 0.5f + q3*q3;

    // 3. 计算误差（加速度计测量值与估计值的叉积）
    halfex = ay*halfvz - az*halfvy;
    halfey = az*halfvx - ax*halfvz;
    halfez = ax*halfvy - ay*halfvx;

    // 4. 误差积分（消除稳态误差）
    integralFBx += halfex * Ki * dt;
    integralFBy += halfey * Ki * dt;
    integralFBz += halfez * Ki * dt;

    // 5. 补偿陀螺仪
    gx += halfex * Kp + integralFBx;
    gy += halfey * Kp + integralFBy;
    gz += halfez * Kp + integralFBz;

    // 6. 四元数更新（一阶龙格-库塔）
    q0 += (-q1*gx - q2*gy - q3*gz) * 0.5f * dt;
    q1 += ( q0*gx + q2*gz - q3*gy) * 0.5f * dt;
    q2 += ( q0*gy - q1*gz + q3*gx) * 0.5f * dt;
    q3 += ( q0*gz + q1*gy - q2*gx) * 0.5f * dt;

    // 7. 归一化四元数
    recipNorm = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
    q0 *= recipNorm;
    q1 *= recipNorm;
    q2 *= recipNorm;
    q3 *= recipNorm;
}

3.5 调试实战：从串口看姿态

代码写完了，怎么知道它算得对不对？我建议你先把四元数转成欧拉角，通过串口打印出来，然后手动转动飞控板观察数值变化。

// 四元数转欧拉角
float roll  = atan2f(2.0f*(q0*q1 + q2*q3), 1.0f - 2.0f*(q1*q1 + q2*q2));
float pitch = asinf(2.0f*(q0*q2 - q3*q1));
float yaw   = atan2f(2.0f*(q0*q3 + q1*q2), 1.0f - 2.0f*(q2*q2 + q3*q3));

// 弧度转角度
roll  = roll  * 180.0f / M_PI;
pitch = pitch * 180.0f / M_PI;
yaw   = yaw   * 180.0f / M_PI;

printf("Roll:%.1f Pitch:%.1f Yaw:%.1f\r\n", roll, pitch, yaw);

调试时有个坑：如果你把飞控板平放，Roll 和 Pitch 应该是 0° 左右。如果发现偏了好几度，先检查加速度计有没有校准。我曾经因为加速度计零偏没去掉，折腾了一下午才发现问题。

3.6 小结

姿态解算是飞控的基石。欧拉角直观但有限制，四元数强大但抽象。互补滤波则是把两者结合起来的实用方案。我个人建议：

• 做显示和日志记录时，用欧拉角
• 做控制解算时，全程用四元数
• 互补滤波的 Kp 和 Ki 一定要根据实际硬件调，别照搬别人的参数

下一章我们会聊到 PID 控制器的实现。到时候你会发现，姿态解算的精度直接决定了 PID 能不能调好。所以，这一章的内容一定要亲手跑一遍代码，别只看不练。