第1章：姿态解算算法——从传感器数据到飞行姿态

各位同学，欢迎来到《惯性导航与飞控系统集成实战》的第一章。今天咱们聊聊姿态解算算法。说白了，就是怎么把陀螺仪、加速度计、磁力计这些传感器读出来的原始数据，变成飞行器当前是“抬头”还是“低头”、“左倾”还是“右倾”这些我们能理解的姿态信息。

我刚开始做飞控那会儿，觉得这步很简单——不就是积分嘛。结果第一次试飞，飞机在天上翻了几个跟头，差点炸机。嗯，从那以后我才明白，姿态解算的水深着呢。

核心观点：姿态解算是飞控系统的“感知层”，没有准确的姿态，控制就是盲人摸象。

1.1 互补滤波原理与实现

先说说互补滤波。这个名字听起来挺唬人，其实原理很简单。

陀螺仪动态响应快，但积分会漂移。加速度计静态准，但受振动干扰大。互补滤波就是把两者的优点结合起来——低频段相信加速度计，高频段相信陀螺仪。你想想看，这不就是“取长补短”嘛。

我在项目中遇到过一个问题：用纯陀螺仪积分，10秒后姿态就偏了5度。加上互补滤波后，漂移被加速度计拉回来了，误差控制在0.5度以内。

互补滤波的公式其实就一行：

// 互补滤波核心代码
angle = 0.98 * (angle + gyro * dt) + 0.02 * acc_angle;

这里的0.98和0.02就是权重系数。系数怎么选？我个人的习惯是：振动大的场景（比如四轴飞行器），加速度计权重可以再小一点，比如0.005；静态场景（比如地面机器人），可以放大到0.05。

小技巧：互补滤波的截止频率可以用公式 f = (1 - α) / (2π * dt * α) 来估算。α是加速度计权重，dt是采样周期。

1.2 Mahony滤波算法详解

Mahony滤波是互补滤波的进阶版。它不再直接融合角度，而是用PI控制器来修正陀螺仪的偏差。

我最早接触Mahony滤波是在2015年，当时做一款手持稳定器。用互补滤波效果总是不太理想，换了Mahony之后，稳得像块石头。

Mahony的核心思想是：用加速度计和磁力计的测量值，构建一个“期望的”姿态方向，然后跟当前姿态做叉积，得到误差。这个误差经过PI控制器后，去修正陀螺仪的角速度。

// Mahony滤波核心步骤（简化版）
// 1. 计算当前姿态下的重力方向
g = quat_rotate(gravity_ref, q);

// 2. 计算误差（叉积）
error = cross(acc_meas, g);

// 3. PI控制器修正陀螺仪
gyro_corrected = gyro + Kp * error + Ki * integral(error);

// 4. 用修正后的角速度更新四元数
q = quat_update(q, gyro_corrected, dt);

这里有个坑，我曾经踩过：Ki参数不能太大，否则积分项会饱和，导致姿态发散。我一般先调Kp，等动态响应满意了，再加一点点Ki消除稳态误差。

注意：Mahony滤波对磁力计比较敏感。如果磁力计受到硬铁干扰，建议先做校准，否则姿态会跟着磁场一起“跳舞”。

1.3 Madgwick滤波算法详解

Madgwick滤波是另一个经典算法。它跟Mahony的区别在于：Madgwick用梯度下降法来寻找最优姿态，而不是用PI控制器。

我记得第一次看Madgwick的论文时，被里面的数学公式吓了一跳。但实际用起来，发现代码量比Mahony还少。Madgwick算法的核心就两步：

  1. 陀螺仪预测：用陀螺仪角速度更新四元数，得到预测姿态
  2. 梯度下降修正：用加速度计/磁力计数据，沿着误差函数的负梯度方向修正姿态
// Madgwick滤波核心代码（简化版）
// 1. 陀螺仪预测
q_dot = 0.5 * quat_multiply(q, [0, gyro_x, gyro_y, gyro_z]);
q_pred = q + q_dot * dt;

// 2. 梯度下降修正
gradient = compute_gradient(q_pred, acc, mag);
q = q_pred - beta * gradient / norm(gradient);

beta参数控制修正速度。beta越大，收敛越快，但噪声也越大。我一般从0.1开始调，看实际效果再微调。

Madgwick的一个优点是：它不需要磁力计也能工作（只用加速度计和陀螺仪）。但加上磁力计后，航向角会更稳定。

对比总结：

算法 优点 缺点 适用场景
互补滤波 简单、计算量小 精度一般 低成本MCU、简单应用
Mahony滤波 抗干扰能力强 需要调PI参数 四轴飞行器、手持稳定器
Madgwick滤波 收敛快、代码简洁 beta参数敏感 无人机、VR/AR设备

1.4 四元数更新与归一化

不管用哪种滤波算法，最后都要更新四元数。四元数更新说白了就是解微分方程：

// 四元数更新（一阶龙格-库塔法）
void quaternion_update(quat_t *q, float gx, float gy, float gz, float dt) {
    float q0 = q->w, q1 = q->x, q2 = q->y, q3 = q->z;
    
    float q0_dot = 0.5 * (-q1*gx - q2*gy - q3*gz);
    float q1_dot = 0.5 * ( q0*gx + q2*gz - q3*gy);
    float q2_dot = 0.5 * ( q0*gy - q1*gz + q3*gx);
    float q3_dot = 0.5 * ( q0*gz + q1*gy - q2*gx);
    
    q->w += q0_dot * dt;
    q->x += q1_dot * dt;
    q->y += q2_dot * dt;
    q->z += q3_dot * dt;
    
    // 归一化
    quaternion_normalize(q);
}

归一化这一步千万不能省！我见过有人偷懒不归一化，结果姿态越算越偏，最后直接炸机。四元数不归一化，旋转矩阵就不正交，姿态就会慢慢“变形”。

归一化公式： q = q / sqrt(q0² + q1² + q2² + q3²)

建议每次更新后都做归一化，或者至少每10次更新做一次。

1.5 姿态角解算（从四元数到欧拉角）

最后一步，把四元数转成欧拉角。欧拉角就是我们常说的俯仰角（Pitch）、横滚角（Roll）、航向角（Yaw）。

转换公式如下：

// 四元数转欧拉角
void quaternion_to_euler(quat_t q, float *roll, float *pitch, float *yaw) {
    float q0 = q.w, q1 = q.x, q2 = q.y, q3 = q.z;
    
    *roll  = atan2(2*(q0*q1 + q2*q3), 1 - 2*(q1*q1 + q2*q2));
    *pitch = asin(2*(q0*q2 - q3*q1));
    *yaw   = atan2(2*(q0*q3 + q1*q2), 1 - 2*(q2*q2 + q3*q3));
}

这里有个坑：当俯仰角接近±90度时，会出现万向锁问题。航向角和横滚角会变得不稳定。我在做固定翼飞控时遇到过这个问题，后来改用四元数做控制，绕开了欧拉角的奇异性。

警告：欧拉角在俯仰角±90度附近会“跳变”。如果做全姿态控制，建议直接用四元数，不要转欧拉角。

本章知识体系

下面这张图是我自己画的，把本章的知识结构梳理了一下。你可以把它当作一个“导航图”，方便回顾。

姿态解算算法知识体系 传感器原始数据 互补滤波 Mahony滤波 Madgwick滤波 四元数更新 + 归一化 姿态角解算（四元数→欧拉角） Roll / Pitch / Yaw

从这张图可以看得很清楚：传感器数据进来后，经过三种滤波算法之一处理，得到修正后的角速度，然后更新四元数并归一化，最后转成欧拉角输出。每一步都有坑，每一步也都有技巧。

好了，第一章的内容就到这里。姿态解算是飞控的“眼睛”，把这一步搞扎实了，后面的控制算法才能发挥威力。下一章我们聊聊传感器校准——别小看这一步，它决定了你整个系统的精度上限。

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

← 上一章 📖 返回目录 下一章 →