4. 姿态解算入门：加速度计与陀螺仪的工作原理、互补滤波原理、Mahony姿态解算算法

各位同学，欢迎来到姿态解算的第一课。说实话，很多新手调参调得头大，根源就在于没搞懂飞控里那个“姿态”到底是怎么算出来的。你想想看，飞控又没有眼睛，它怎么知道自己朝哪边歪了？

答案就藏在两个小传感器里：加速度计和陀螺仪。今天我们就从它们的工作原理讲起，再到经典的互补滤波，最后落地到工程上最常用的 Mahony 算法。这一章啃下来，后面调 PID 你心里就有底了。

4.1 加速度计：它到底在测什么？

加速度计测的不是“速度变化率”那么简单。在飞控里，它真正告诉我们的是：重力向量在机体坐标系下的投影。

我刚开始做飞控时，一直有个误区——以为加速度计能直接读出角度。其实它只能给出一个“感觉”：

• 当飞控水平静止时，Z 轴感受到 1g 的重力，X、Y 轴为 0。
• 当飞控倾斜时，重力被分解到各个轴上。

通过反三角函数，我们可以算出横滚角和俯仰角。但这里有个大坑——加速度计对水平方向的运动非常敏感。你想想看，如果飞机在加速前飞，加速度计会把这个水平加速度误判为倾斜，导致姿态发散。

4.2 陀螺仪：角速度的“积分陷阱”

陀螺仪直接输出的是角速度（°/s），要得到角度，必须做积分。这听起来很简单，对吧？

但实际工程里，陀螺仪有三个致命问题：

1. 零偏漂移：静止时输出不为 0，积分后角度会一直跑。
2. 温度漂移：温度变化，零偏也跟着变。
3. 积分累积误差：时间越长，误差越大。

我见过一个新手，直接用陀螺仪积分做姿态，飞机上天 10 秒后就开始乱翻。为什么？因为积分误差已经累积到 30 度以上了。

4.3 互补滤波：取长补短的智慧

现在问题来了：

• 加速度计：低频准，高频噪声大。
• 陀螺仪：高频准，低频漂移大。

这不正好互补吗？互补滤波的核心思想就是：用加速度计的低频信息去修正陀螺仪的高频积分。

数学上很简单：

角度 = α × (角度 + 陀螺仪角速度 × dt) + (1 - α) × 加速度计角度

其中 α 通常取 0.98 左右。α 越大，越信任陀螺仪；α 越小，越信任加速度计。

4.4 Mahony 姿态解算算法

互补滤波只能解算横滚和俯仰，偏航角怎么办？而且它没有考虑四元数的数学优雅性。这时候，Mahony 算法就登场了。

Mahony 算法本质上是一个基于 PI 控制器的互补滤波。它的核心流程是：

1. 用陀螺仪积分更新四元数。
2. 用加速度计和磁力计的测量值，计算当前姿态的“误差”。
3. 用 PI 控制器把这个误差反馈到陀螺仪角速度上，进行修正。

说白了，就是让加速度计“拉着”陀螺仪，不让它跑偏。

下面是我在实际项目中使用的 Mahony 算法核心代码（C 语言）：

void MahonyAHRSupdate(float gx, float gy, float gz, 
                      float ax, float ay, float az,
                      float mx, float my, float mz) {
    float recipNorm;
    float q0q0, q0q1, q0q2, q0q3;
    float q1q1, q1q2, q1q3;
    float q2q2, q2q3, q3q3;
    
    // 1. 归一化加速度计数据
    recipNorm = invSqrt(ax*ax + ay*ay + az*az);
    ax *= recipNorm;
    ay *= recipNorm;
    az *= recipNorm;
    
    // 2. 计算重力在机体坐标系下的估计值
    q0q0 = q0 * q0;
    q0q1 = q0 * q1;
    q0q2 = q0 * q2;
    q0q3 = q0 * q3;
    q1q1 = q1 * q1;
    q1q2 = q1 * q2;
    q1q3 = q1 * q3;
    q2q2 = q2 * q2;
    q2q3 = q2 * q3;
    q3q3 = q3 * q3;
    
    float vx = 2.0f * (q1q3 - q0q2);
    float vy = 2.0f * (q0q1 + q2q3);
    float vz = q0q0 - q1q1 - q2q2 + q3q3;
    
    // 3. 计算误差（叉积）
    float ex = ay * vz - az * vy;
    float ey = az * vx - ax * vz;
    float ez = ax * vy - ay * vx;
    
    // 4. PI 控制器修正陀螺仪
    integralFBx += Ki * ex * dt;
    integralFBy += Ki * ey * dt;
    integralFBz += Ki * ez * dt;
    
    gx += Kp * ex + integralFBx;
    gy += Kp * ey + integralFBy;
    gz += Kp * ez + integralFBz;
    
    // 5. 四元数更新（一阶龙格-库塔）
    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;
    
    // 6. 归一化四元数
    recipNorm = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
    q0 *= recipNorm;
    q1 *= recipNorm;
    q2 *= recipNorm;
    q3 *= recipNorm;
}

4.5 三种方法的对比

为了让你更直观地理解，我整理了一个对比表：

4.6 知识体系总览

下面这张图总结了本章的核心逻辑，你可以把它当作一个“思维导图”来用：