4、姿态解算（一）：陀螺仪积分与姿态漂移、加速度计与磁力计辅助修正、互补滤波原理与实现
各位同学，欢迎来到姿态解算的第一讲。说实话，姿态解算是飞控算法里最核心、也最容易让人头疼的部分。我当年刚入行时，第一版飞控上天后，飞机像喝醉了酒一样乱晃——那就是姿态解算没做好。今天咱们就来啃这块硬骨头。
4.1 陀螺仪积分：姿态的“直觉”从哪来？
陀螺仪测量的是角速度，单位是 °/s 或 rad/s。要得到姿态角，最直接的方法就是积分。
说白了，就是每一小段时间内，把角速度乘以时间，累加起来。比如你转了个身，陀螺仪告诉你每秒转 90°，转了 0.5 秒，那角度变化就是 45°。
数学上很简单：
// 一阶龙格-库塔法（最常用）
angle += gyro * dt;
// 或者用四元数更新（更精确）
q += 0.5 * q * omega * dt;嗯，这里要注意。我见过很多新手直接用 float 累加，结果飞了 10 分钟，角度漂了 30 度。为什么？因为陀螺仪有零偏误差，积分会把误差无限放大。
4.2 姿态漂移：为什么积分会“跑偏”？
姿态漂移主要有三个来源：
- 零偏误差：陀螺仪静止时输出不为 0，积分后产生累积误差。
- 噪声：高频噪声被积分后变成低频漂移。
- 温度漂移：温度变化导致零偏缓慢变化。
你想想看，如果陀螺仪每秒有 0.01° 的零偏，一小时就是 36°。这还只是理想情况。实际项目中，零偏可能达到 0.5°/s 甚至更高。
核心结论：陀螺仪短期精度高（高频特性好），但长期会漂移。加速度计和磁力计正好相反——长期稳定，但短期噪声大。
4.3 加速度计辅助修正：用重力“拉”回姿态
加速度计测量的是比力（重力 + 运动加速度）。当飞行器处于静止或匀速运动时，加速度计的输出就是重力向量。
我们可以利用这个特性来修正俯仰角和横滚角：
// 从加速度计计算俯仰角和横滚角
roll_acc = atan2(accel_y, accel_z);
pitch_acc = atan2(-accel_x, sqrt(accel_y*accel_y + accel_z*accel_z));但这里有个大坑——加速度计对运动加速度非常敏感。我做过一个测试：飞机急加速时，加速度计给出的“重力方向”偏差了 20° 以上。所以不能完全相信它。
💡 个人经验：我习惯在代码里加一个运动检测。如果加速度计的总模值偏离 1g 太多（比如超过 0.2g），就降低加速度计的修正权重。这样急转弯时不会把姿态带偏。
4.4 磁力计辅助修正：给航向“指北”
加速度计只能修正俯仰和横滚，航向角（偏航）它管不了。这时候就需要磁力计。
磁力计测量的是地球磁场方向。结合加速度计得到的姿态，可以解算出航向：
// 将磁力计数据转换到水平坐标系
mag_x_horiz = mag_x * cos(pitch) + mag_y * sin(roll) * sin(pitch) + mag_z * cos(roll) * sin(pitch);
mag_y_horiz = mag_y * cos(roll) - mag_z * sin(roll);
// 计算航向角
yaw_mag = atan2(-mag_y_horiz, mag_x_horiz);不过磁力计也有自己的问题。我在一个工业现场调试时，发现航向一直在跳。后来发现是旁边有个大电机，磁场干扰严重。所以磁力计需要做硬铁和软铁校准。
4.5 互补滤波原理：取长补短的艺术
好了，现在我们有三个传感器：
| 传感器 | 优点 | 缺点 |
|---|---|---|
| 陀螺仪 | 短期精度高，动态响应快 | 长期漂移 |
| 加速度计 | 长期稳定，无漂移 | 短期噪声大，受运动加速度影响 |
| 磁力计 | 提供绝对航向 | 易受干扰，需要校准 |
互补滤波的核心思想很简单：高频相信陀螺仪，低频相信加速度计/磁力计。
数学表达式：
angle = (1 - alpha) * (angle + gyro * dt) + alpha * accel_angle;其中 alpha 是滤波系数，通常在 0.01~0.1 之间。alpha 越大，加速度计权重越大，响应越快但噪声也大；alpha 越小，陀螺仪权重越大，更平滑但漂移也大。
关键点：互补滤波本质上是一个一阶低通滤波器 + 一个一阶高通滤波器的组合。陀螺仪积分通过高通滤波器（保留短期变化），加速度计通过低通滤波器（保留长期趋势），两者相加得到最终姿态。
4.6 互补滤波的代码实现
下面是一个经典的互补滤波实现，我用了很多年：
// 互补滤波参数
#define TAU 0.5f // 时间常数（秒）
#define DT 0.002f // 采样周期（秒）
float alpha = TAU / (TAU + DT);
// 姿态更新
void complementary_filter(float gyro_rate, float accel_angle, float *angle) {
// 陀螺仪积分
float gyro_angle = *angle + gyro_rate * DT;
// 互补融合
*angle = (1 - alpha) * gyro_angle + alpha * accel_angle;
}这段代码看着简单，但实际用起来有很多细节。我建议你注意两点：
- 时间常数 TAU 的选择：TAU 越大，滤波越平滑，但响应越慢。我一般从 0.5 秒开始调，根据实际效果微调。
- 异常值处理：如果加速度计角度突然跳变（比如飞机被撞了一下），要加限幅。我习惯限制每次修正不超过 5°。
💡 调试技巧：把陀螺仪积分角度、加速度计角度、融合后的角度同时打印出来，用串口绘图工具观察。你会发现陀螺仪角度平滑但会漂移，加速度计角度噪声大但均值正确，融合后的角度既平滑又准确。
4.7 三维姿态的互补滤波
上面讲的是单轴互补滤波。实际飞控需要处理三维姿态，通常用四元数或旋转矩阵来实现。
这里给出一个基于四元数的互补滤波框架：
// 四元数互补滤波（简化版）
void quaternion_complementary_filter(quaternion_t *q,
vec3_t gyro,
vec3_t accel,
vec3_t mag) {
// 1. 陀螺仪积分更新四元数
quaternion_derivative(q, gyro, dt);
// 2. 用加速度计和磁力计计算误差
vec3_t error = compute_error(q, accel, mag);
// 3. 用误差修正陀螺仪
gyro += Kp * error;
// 4. 重新归一化四元数
quaternion_normalize(q);
}这个框架其实就是 Mahony 滤波器的核心思想。我当年第一次实现时，花了两周才调通。后来发现，最关键的是误差计算和增益 Kp 的选取。
4.8 总结与建议
今天的内容就到这里。咱们回顾一下：
- 陀螺仪积分是姿态解算的基础，但会漂移
- 加速度计和磁力计提供绝对参考，但噪声大
- 互补滤波把两者结合起来，取长补短
我个人建议，初学者先从单轴互补滤波开始，理解原理后再扩展到四元数。不要一上来就搞复杂的卡尔曼滤波——互补滤波在大多数飞控场景下已经足够用了。
下一讲，我们会深入 Mahony 滤波器和 Madgwick 滤波器的数学推导。到时候你会发现，理解了互补滤波，后面的内容就水到渠成了。
课后思考：如果飞机在做剧烈机动（比如筋斗），加速度计的输出完全不可信，这时候互补滤波会出什么问题？该怎么处理？
好，今天就到这里。有问题欢迎在课程群里讨论，咱们下节课见。