3、传感器与状态估计：IMU原理与数据融合
做飞控这么多年，我始终觉得传感器是飞控的「眼睛」。眼睛不好使，再厉害的算法也白搭。今天咱们就聊聊IMU——这个飞控里最核心的传感器。
3.1 IMU是什么？
IMU，全称Inertial Measurement Unit，惯性测量单元。说白了，就是陀螺仪加加速度计的组合体。有的还带磁力计，但咱们今天先聊前两个。
陀螺仪测什么？角速度。就是飞机绕X轴、Y轴、Z轴转得有多快。加速度计测什么？比力。就是除了重力以外的加速度。
你可能会问：「那为啥不直接用陀螺仪算角度？」嗯，这里有个坑——陀螺仪有零偏，积分久了就飘。我刚开始做飞控时，就吃过这个亏。
核心概念：IMU提供的是原始数据，不是直接可用的姿态角。我们需要通过数据融合，才能得到稳定可靠的姿态估计。
3.2 陀螺仪特性分析
陀螺仪这东西，我习惯把它分成三类：
- MEMS陀螺仪：消费级，便宜，但噪声大、零偏大。比如MPU6050、ICM-20602。
- 光纤陀螺仪：工业级，精度高，但贵。一般用在无人机上有点奢侈。
- 激光陀螺仪：军用级，精度极高，但体积大、价格离谱。
咱们做多旋翼，99%的情况用的是MEMS陀螺仪。它的主要问题有三个：
- 零偏不稳定性：每次上电，零偏都不一样。温度变了，零偏也变。
- 噪声：高频噪声明显，需要滤波。
- 刻度因子误差：输入角速度和输出值不是完美的线性关系。
我记得有一次，客户反馈飞机悬停时慢慢往一个方向漂。查了半天，发现是陀螺仪零偏没校准好。后来我加了个上电自动校准的流程，问题就解决了。
避坑指南：我曾经在项目里直接用陀螺仪积分算角度，结果10秒后角度就偏了5度。记住，陀螺仪只能短时使用，必须配合加速度计做融合。
3.3 加速度计特性分析
加速度计测的是比力，不是纯加速度。什么意思？静止时，它测到的是重力加速度g。水平放置时，Z轴输出1g，X轴和Y轴输出0g。
加速度计的优势和劣势都很明显：
| 特性 | 优势 | 劣势 |
|---|---|---|
| 低频响应 | 长期稳定，不漂移 | 对高频振动敏感 |
| 动态响应 | 能测瞬时加速度 | 运动加速度会干扰姿态解算 |
| 噪声特性 | 低频噪声小 | 高频噪声大 |
你想想看，飞机在悬停时，加速度计基本只测到重力。但一旦开始加速飞行，运动加速度就会混进来。这时候直接用加速度计算俯仰和横滚角，结果就不准了。
3.4 互补滤波：简单又实用的融合方法
互补滤波，说白了就是「取长补短」。陀螺仪高频好、低频差；加速度计低频好、高频差。把它们互补一下，不就完美了吗？
核心公式其实很简单：
// 互补滤波核心代码
angle = 0.98 * (angle + gyro * dt) + 0.02 * acc_angle;这里的0.98和0.02就是权重系数。系数怎么选？我一般这样调：
- 如果飞机振动大，适当减小加速度计的权重（比如0.99 + 0.01）
- 如果飞机动作快，适当增大加速度计的权重（比如0.95 + 0.05）
- 实际调试时，我会先设成0.98 + 0.02，然后看波形调
实际经验：互补滤波的截止频率一般设在0.5Hz到2Hz之间。低于这个频率，用加速度计；高于这个频率，用陀螺仪。这个参数直接影响飞控的响应速度。
3.5 卡尔曼滤波入门
卡尔曼滤波，听起来高大上，其实核心思想就五个字——「预测+修正」。
我刚开始学卡尔曼滤波时，被那一堆矩阵公式吓到了。后来发现，对于姿态估计这种简单场景，完全可以用标量卡尔曼滤波。
卡尔曼滤波的五个步骤：
- 预测状态：用陀螺仪数据预测当前角度
- 预测协方差：估计预测的不确定性
- 计算卡尔曼增益：决定相信预测还是相信测量
- 更新状态：用加速度计测量值修正预测值
- 更新协方差：更新不确定性估计
代码实现其实不长：
// 一维卡尔曼滤波示例
float kalman_filter(float z_measure, float dt) {
// 预测
x_pred = x_est + gyro * dt;
p_pred = p_est + q * dt;
// 更新
k = p_pred / (p_pred + r);
x_est = x_pred + k * (z_measure - x_pred);
p_est = (1 - k) * p_pred;
return x_est;
}这里的q是过程噪声协方差，r是测量噪声协方差。q越大，越相信加速度计；r越大，越相信陀螺仪。调参时，我一般先固定r，然后调q，直到波形平滑且响应够快。
注意事项：卡尔曼滤波不是万能的。如果模型不准，或者噪声特性变化太大，卡尔曼滤波反而会发散。我见过有人把卡尔曼滤波调了三天都没调好，最后换回互补滤波，半小时搞定。
3.6 互补滤波 vs 卡尔曼滤波
很多新手会问：「到底用哪个好？」我的回答是：看场景。
| 对比项 | 互补滤波 | 卡尔曼滤波 |
|---|---|---|
| 计算量 | 极低 | 中等 |
| 调参难度 | 简单（1个参数） | 较复杂（至少2个参数） |
| 精度 | 够用 | 理论上更高 |
| 适用场景 | 大多数飞控 | 高精度需求 |
我个人习惯是：原型阶段用互补滤波，快速验证。如果发现精度不够，再上卡尔曼滤波。别一上来就搞卡尔曼，容易把自己绕进去。
3.7 数据融合的完整流程
说了这么多，咱们把整个流程串起来。下面这张图是我自己画的，展示了从传感器原始数据到最终姿态角的完整链路：
整个流程其实就三步：
- 采集原始数据：从陀螺仪和加速度计读原始值
- 预处理：校准零偏、滤除高频噪声
- 数据融合：用互补滤波或卡尔曼滤波，输出稳定姿态角
嗯，这里要注意一点：预处理这一步很多人会忽略。我见过有人直接把原始数据扔进融合算法，结果姿态角抖得跟筛子似的。加个简单的低通滤波，效果立竿见影。
3.8 实际调试建议
最后，分享几个我在实际项目中总结的经验：
- 先看原始数据：别急着调融合算法。先看看陀螺仪和加速度计的原始波形，确认传感器工作正常。
- 零偏校准要到位：静止时采集1000个陀螺仪数据取平均，作为零偏值。这个步骤不能省。
- 融合频率要匹配：陀螺仪和加速度计的采样频率最好一致，或者陀螺仪频率是加速度计的整数倍。
- 调试时用串口打印：把原始角度、融合后的角度都打出来，画成波形图，一眼就能看出问题。
小技巧：调试互补滤波时，可以先用一个很大的权重（比如0.999 + 0.001），看看陀螺仪积分效果。再用很小的权重（比如0.5 + 0.5），看看加速度计效果。然后慢慢调到一个平衡点。
传感器和状态估计这块，说白了就是「信谁多一点」的问题。陀螺仪短期准，加速度计长期准。把它们结合起来，就能得到既稳定又灵敏的姿态估计。这个思路，贯穿了整个飞控系统的设计。