4、姿态解算基础：加速度计、陀螺仪、磁力计原理与特性，互补滤波初步
姿态解算是飞控系统的核心环节，其任务是将传感器原始测量值转换为飞行器的横滚角、俯仰角和偏航角。几乎所有多旋翼和固定翼飞控都依赖加速度计、陀螺仪和磁力计这三种MEMS传感器。理解它们的物理原理、误差特性以及如何融合是进行后续控制律设计的前提。本章将深入剖析这三种传感器的测量模型，并引入经典的互补滤波算法，帮助读者建立从原始数据到可用姿态角的完整工程链路。
上图展示了三种传感器在姿态解算中的角色分工：加速度计提供重力方向的参考，陀螺仪提供短时高精度的角速度积分，磁力计则弥补偏航角无参考的缺陷。三者特性互补，正是互补滤波命名的由来。下面我们逐一分析其原理与工程特性。
4.1 加速度计：重力矢量与线性加速度的纠缠
加速度计测量的是比力（Specific Force），即物体受到的惯性力与重力之差。在静止或匀速运动时，加速度计输出恰好等于重力加速度的反向矢量。利用这一特性，我们可以通过三轴加速度分量直接解算出横滚角（Roll）和俯仰角（Pitch）。
核心公式（静态解算）：
Roll = atan2(accel_y, accel_z)
Pitch = atan2(-accel_x, sqrt(accel_y^2 + accel_z^2))其中 accel_x/y/z 为三轴加速度读数（单位 g）。atan2 函数可避免象限歧义。
然而，当飞行器加速、减速或做圆周运动时，加速度计会混入运动加速度分量，导致解算出的姿态角出现严重偏差。例如，多旋翼急加速前飞时，加速度计会误判为“抬头”。因此，加速度计适合作为低频参考，必须通过滤波抑制高频噪声和运动加速度干扰。
4.2 陀螺仪：角速度积分与漂移的博弈
陀螺仪直接输出绕机体三轴的角速度（单位：°/s 或 rad/s）。通过对角速度进行时间积分，可以获得姿态角增量。陀螺仪的优点是动态响应快、不受运动加速度影响，在短时间（数秒至数十秒）内精度极高。
但积分过程会累积零偏误差（Bias）和随机游走噪声。即使静止放置，陀螺仪输出也非严格为零，而是存在一个缓慢变化的偏置。积分后，这个偏置会线性增长为姿态漂移。例如，一个 0.1°/s 的零偏，积分 100 秒后就会产生 10° 的姿态误差。因此，陀螺仪必须依赖加速度计和磁力计进行周期性校正。
4.3 磁力计：航向角的唯一参考
磁力计测量地球磁场在机体坐标系下的分量。当飞行器水平时，磁力计水平分量指向磁北，从而可以计算偏航角（Yaw）。但磁力计有两个显著弱点：一是需要倾斜补偿，即必须结合当前的横滚和俯仰角将磁场矢量投影到水平面；二是极易受环境磁场干扰，包括电机电流产生的硬磁干扰和周围铁磁材料产生的软磁干扰。
| 传感器 | 测量量 | 优点 | 缺点 | 适用频段 |
|---|---|---|---|---|
| 加速度计 | 比力 (g) | 无漂移，参考 | 高频噪声大，受运动加速度干扰 | 低频（<1Hz） |
| 陀螺仪 | 角速度 (°/s) | 动态响应快，短时精确 | 积分漂移，零偏温漂 | 高频（>0.1Hz） |
| 磁力计 | 磁场强度 (μT) | 提供航向 | 易受干扰，需校准 | 极低频（<0.5Hz） |
4.4 互补滤波原理：频域上的最优融合
互补滤波的核心思想是利用传感器在频域上的互补特性：陀螺仪在动态时准确，加速度计和磁力计在静态时准确。通过一个低通滤波器处理矢量参考（加速度计/磁力计），一个高通滤波器处理陀螺仪积分，再将两者相加，即可在全频段获得可靠的姿态估计。
以一阶互补滤波为例，其离散形式如下：
angle = k * (angle_acc) + (1 - k) * (angle_gyro + gyro_rate * dt)其中 k 为滤波增益（0~1），dt 为采样周期。当 k 较大时，更信任加速度计，滤波结果平滑但动态延迟大；当 k 较小时，更信任陀螺仪，动态响应快但可能引入漂移。工程上通常取 k = 0.02 ~ 0.1，对应截止频率约 0.5~2Hz。
工程经验：互补滤波的截止频率应高于飞行器的姿态控制带宽（通常 5~10Hz），但低于运动加速度的基频。对于大多数多旋翼，k = 0.05 是一个不错的起点。若飞行器机动剧烈，可适当减小 k 以降低运动加速度的影响。
4.5 案例拆解：四旋翼悬停姿态解算的调参与排障
某四旋翼在悬停测试中出现姿态晃动，飞控日志显示 Roll 角在 ±3° 范围内以约 2Hz 频率振荡。初步排查发现，加速度计数据噪声峰峰值达 0.15g，而陀螺仪零偏稳定在 0.08°/s。
分析过程：首先检查滤波器增益。原代码中 k = 0.1，意味着加速度计权重较高，而悬停时旋翼气流和电机振动会引入高频加速度噪声，导致姿态估计抖动。其次，陀螺仪零偏虽小，但积分 10 秒后会产生 0.8° 漂移，若不校正会缓慢累积。
操作步骤：
- 将互补滤波增益从
k = 0.1调整为k = 0.03，降低加速度计高频噪声权重。 - 在陀螺仪积分前增加一阶高通滤波（截止频率 0.05Hz），抑制零偏累积。
- 对加速度计数据增加 20Hz 低通滤波，进一步抑制螺旋桨振动。
- 重新进行静态校准，记录 1000 组静止数据求平均，消除固定零偏。
结果对比：调整后，悬停 Roll 角振荡幅度从 ±3° 降至 ±0.8°，姿态曲线明显平滑。但动态响应略有延迟，在快速横滚指令下超调量增加 0.5°，通过将 k 微调至 0.04 取得平衡。
风险提示：过度降低 k 会导致姿态估计长期漂移，尤其在长时间飞行时。务必在飞控代码中加入陀螺仪零偏在线估计（如通过加速度计长期平均值校正），否则 5 分钟后姿态误差可能超过 10°。
4.6 常见误区与总结复盘
误区一：认为加速度计可以直接用于动态姿态测量。实际上，任何线性加速度都会污染横滚/俯仰解算，必须通过滤波或融合算法抑制。
误区二：忽视磁力计校准。未校准的磁力计在室内飞行时可能产生 20°~30° 的航向误差，且该误差无法通过互补滤波消除。
误区三：将互补滤波增益视为固定常数。不同机型、不同振动环境下，最优 k 值差异显著，应通过飞行日志分析进行整定。
总结：互补滤波是工程中最实用的姿态解算方案，它不需要复杂的矩阵运算和协方差调参，计算量小，适合在低成本 MCU 上实时运行。理解三种传感器的物理特性是正确使用互补滤波的前提：加速度计提供长期稳定的横滚/俯仰参考，陀螺仪提供短期精确的角速度积分，磁力计提供航向参考。三者通过频域互补，即可获得满足控制需求的姿态估计。下一阶段，读者可在此基础上学习卡尔曼滤波或 Mahony 算法，以应对更严苛的动态场景。