3、姿态解算算法：从互补滤波到梯度下降

姿态解算，说白了就是回答一个问题：“我现在朝哪个方向？”

IMU 里陀螺仪积分快但会飘，加速度计和磁力计准但怕振动。怎么把这两类数据揉在一起？这就是姿态解算算法要干的事。

我个人习惯把算法分成四类：互补滤波、Mahony、EKF、梯度下降。它们各有脾气，选对了事半功倍。

核心观点：没有最好的算法，只有最合适的场景。资源受限选互补滤波，精度优先上 EKF，平衡之选看 Mahony 或梯度下降。

3.1 互补滤波原理与实现

互补滤波的思路很直白：陀螺仪负责高频姿态，加速度计/磁力计负责低频修正。两者一互补，就得到了全频段都靠谱的姿态。

数学上就一行公式：

angle = α × (angle + gyro×dt) + (1-α) × acc_angle

α 是权重系数，一般取 0.98 左右。α 越大，越信任陀螺仪；α 越小，越信任加速度计。

经验值：α = 0.98 是个好起点。如果发现姿态跟随慢，可以降到 0.95；如果噪声大，升到 0.995。

我在项目里用过一次纯互补滤波——一个低成本玩具四轴。当时 MCU 是 STM32F103，算力有限，互补滤波跑起来只占不到 5% 的 CPU。效果嘛，悬停没问题，但做大机动时会明显滞后。

实现步骤：

读取陀螺仪角速度，积分得到角度增量
读取加速度计，通过 atan2 计算俯仰/横滚
按权重融合：新角度 = α × (旧角度 + 陀螺增量) + (1-α) × 加速度计角度
重复 1-3，每秒 100-1000 次

注意：互补滤波不估计陀螺零偏。如果陀螺有温漂，姿态会慢慢飘走。我曾经在零下 10°C 的户外试飞，互补滤波的悬停高度直接偏了 5 度——后来换了 Mahony 才解决。

3.2 Mahony 滤波算法详解

Mahony 滤波是互补滤波的“升级版”。它引入了 PI 反馈环，用加速度计和磁力计的误差来修正陀螺仪偏差。

你想想看，互补滤波只是简单加权平均，而 Mahony 会主动计算“当前姿态与观测值的误差”，然后用这个误差去调整陀螺仪的积分。这就好比开车时不仅看路，还不断修正方向盘。

核心流程：

用当前四元数计算理论重力向量和磁场向量
与加速度计/磁力计实测值做叉积，得到误差
误差经过 PI 控制器，补偿到陀螺仪角速度上
用修正后的角速度更新四元数

代码实现大概长这样：

// Mahony 滤波单步更新
void MahonyUpdate(float gx, float gy, float gz, 
                  float ax, float ay, float az,
                  float mx, float my, float mz, float dt) {
    float q0 = q[0], q1 = q[1], q2 = q[2], q3 = q[3];
    
    // 计算理论重力方向
    float norm = invSqrt(ax*ax + ay*ay + az*az);
    ax *= norm; ay *= norm; az *= norm;
    
    float hx = 2.0f*(mx*(0.5f - q2*q2 - q3*q3) + my*(q1*q2 - q0*q3) + mz*(q1*q3 + q0*q2));
    float hy = 2.0f*(mx*(q1*q2 + q0*q3) + my*(0.5f - q1*q1 - q3*q3) + mz*(q2*q3 - q0*q1));
    float bx = sqrt(hx*hx + hy*hy);
    float bz = 2.0f*(mx*(q1*q3 - q0*q2) + my*(q2*q3 + q0*q1) + mz*(0.5f - q1*q1 - q2*q2));
    
    // 误差计算（叉积）
    float ex = (ay*vz - az*vy) + (my*bz - mz*by);
    float ey = (az*vx - ax*vz) + (mz*bx - mx*bz);
    float ez = (ax*vy - ay*vx) + (mx*by - my*bx);
    
    // PI 补偿
    exInt += ex * Ki * dt;
    eyInt += ey * Ki * dt;
    ezInt += ez * Ki * dt;
    
    gx += Kp*ex + exInt;
    gy += Kp*ey + eyInt;
    gz += Kp*ez + ezInt;
    
    // 四元数更新
    q0 += (-q1*gx - q2*gy - q3*gz) * dt * 0.5f;
    q1 += ( q0*gx + q2*gz - q3*gy) * dt * 0.5f;
    q2 += ( q0*gy - q1*gz + q3*gx) * dt * 0.5f;
    q3 += ( q0*gz + q1*gy - q2*gx) * dt * 0.5f;
    
    // 归一化
    norm = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
    q[0] = q0*norm; q[1] = q1*norm; q[2] = q2*norm; q[3] = q3*norm;
}

调参口诀：Kp 决定收敛速度，Ki 决定稳态误差。Kp 太大容易震荡，Ki 太大容易超调。我一般先调 Kp 到不震荡，再加一点点 Ki 消除静差。

我记得有一次在四旋翼上调试 Mahony，Kp 设到 2.0 时姿态响应很快，但悬停时高频抖动。后来降到 0.8，再加了 0.1 的 Ki，稳如老狗。嗯，调参这事，急不得。

3.3 EKF 姿态估计

扩展卡尔曼滤波（EKF）是姿态解算里的“重武器”。它把姿态估计建模成一个非线性系统，用预测-更新两步走的方式，给出最优估计。

EKF 的状态量通常包括：

状态变量	含义	维度
q0, q1, q2, q3	姿态四元数	4
bg_x, bg_y, bg_z	陀螺仪零偏	3
ba_x, ba_y, ba_z	加速度计零偏（可选）	3

预测步：用陀螺仪角速度更新四元数，同时预测协方差矩阵。

更新步：用加速度计和磁力计作为观测，计算卡尔曼增益，修正状态。

EKF 的坑：协方差矩阵调参非常痛苦。Q 和 R 矩阵设不好，要么发散要么收敛极慢。我曾经花了一周时间，用 Allan 方差分析陀螺噪声，才把 Q 矩阵定下来。

EKF 的优势在于精度高，而且能同时估计陀螺零偏。工业级无人机、机器人导航基本都用它。但代价是算力——在 STM32F4 上跑一次 EKF 更新大约需要 0.5ms，比 Mahony 慢了 10 倍。

什么时候用 EKF？当你的 MCU 算力充裕（比如 Cortex-M7 以上），且对姿态精度要求极高（误差 < 0.5°）时，EKF 是不二之选。

3.4 基于梯度下降的姿态解算

梯度下降法，是 Madgwick 在 2010 年提出的。它的思路很巧妙：把姿态解算看作一个优化问题——找到一组四元数，让理论观测值与实际观测值的误差最小。

说白了，就是沿着误差函数的负梯度方向，一步步逼近最优姿态。

核心公式：

q_new = q_old - μ × ∇f(q_old, acc, mag)

其中 μ 是步长，∇f 是误差函数的梯度。步长越大，收敛越快，但容易震荡。

Madgwick 的论文里给出了一个自适应步长策略：μ 与陀螺仪角速度的模长成正比。这样在快速运动时步长大，静态时步长小，兼顾了动态响应和稳态精度。

梯度下降 vs Mahony：

对比项	梯度下降	Mahony
核心思想	优化误差函数	PI 反馈修正
收敛速度	快（步长可调）	中等（依赖 Kp）
陀螺偏置估计	不支持	支持（通过积分项）
算力需求	中等	低
调参难度	低（仅步长）	中等（Kp, Ki）

我在一个手持云台项目里用过梯度下降。当时需要快速响应手部运动，Mahony 的 PI 环有点跟不上，换成梯度下降后，步长设到 0.5，收敛速度明显提升。不过静态时会有微小抖动——后来加了死区才解决。

我的建议：如果你刚入门，从 Mahony 开始。它足够好，而且社区资源多。等你对姿态解算有了感觉，再尝试 EKF 或梯度下降。别一上来就上 EKF，容易劝退。

好了，这四种算法各有千秋。选哪个，取决于你的硬件、算力和精度要求。记住一句话：没有银弹，只有权衡。

📚 课程目录

3、姿态解算算法：从互补滤波到梯度下降

3.1 互补滤波原理与实现

3.2 Mahony 滤波算法详解

3.3 EKF 姿态估计

3.4 基于梯度下降的姿态解算