第3章传感器与数据融合

第3章：传感器与数据融合

各位同学，大家好。这一章我们聊聊四轴飞行器的“感官系统”。

说白了，飞控要稳定，首先得知道自己“在哪儿”、“什么姿态”。这全靠传感器。但每个传感器都有脾气，单独用哪个都不靠谱。所以，数据融合就成了核心中的核心。

加速度计测量的是三轴加速度。静止时，它唯一感受到的就是重力。所以，我们可以用它算出俯仰角和横滚角。

公式很简单：

roll = atan2(accel_y, accel_z)
pitch = atan2(-accel_x, sqrt(accel_y^2 + accel_z^2))

但注意，这玩意儿怕振动。我在项目中遇到过，电机一启动，加速度计数据直接炸了。你想想看，桨叶高速旋转带来的高频振动，全叠加到测量值里了。这时候算出来的角度，根本没法用。

警告： 加速度计对线性加速度敏感。飞行器加速或减速时，测得的“重力方向”是错的。千万别在剧烈机动时用它算姿态。

陀螺仪测量角速度。我们积分它，就能得到角度变化。它的优点是响应极快，延迟低。但缺点也很致命——积分会累积误差。

我刚开始做飞控时，就吃过这个亏。陀螺仪零偏哪怕只有0.1度/秒，积分10秒就偏了1度。飞着飞着，飞机就开始自己“点头”了。

所以，陀螺仪必须配合其他传感器来修正。单独用，就是慢性自杀。

磁力计就是电子罗盘，用来测航向（偏航角）。原理是测量地球磁场方向。

但问题来了——电机电流、机架上的铁磁物质，都会干扰它。我曾在调试时发现，磁力计数据在电机解锁前后差了20度。这要是直接用了，飞机直接原地转圈。

技巧： 上电后做一次“磁力计校准”，让飞控记住干扰的偏置。另外，安装位置尽量远离电机和电源线。

GPS提供经纬度和速度。但它的更新频率低（5-10Hz），而且高楼遮挡时信号会跳变。气压计用来测高度，原理是气压随高度变化。但它对温度、风都很敏感。

我记得有一次在室内飞，气压计高度突然跳了2米。后来发现是空调吹的风影响了气压。所以，室内飞行我一般不用气压计，改用超声波或激光测距。

好了，传感器各有优缺点。怎么把它们捏在一起？互补滤波是最简单实用的方法。

核心思想：

公式如下：

angle = 0.98 * (angle + gyro * dt) + 0.02 * accel_angle

这里的0.98和0.02就是权重系数。系数怎么调？我习惯先设成0.98，然后看响应。如果角度跟踪太慢，就减小陀螺仪权重；如果噪声太大，就增大陀螺仪权重。

  重点： 互补滤波的优点是计算量小，适合低算力MCU。缺点是对动态响应和噪声的平衡比较难调。

卡尔曼滤波是更高级的方法。它通过“预测-更新”两步，给出最优估计。

简单说：

卡尔曼滤波的公式有点复杂，但核心就五个方程：

// 预测
x = F * x + B * u
P = F * P * F^T + Q

// 更新
K = P * H^T * (H * P * H^T + R)^-1
x = x + K * (z - H * x)
P = (I - K * H) * P

嗯，这里要注意，Q和R矩阵的调参很关键。Q代表你对模型的信任度，R代表你对传感器的信任度。我刚开始调的时候，Q设太大，结果滤波结果跟着陀螺仪乱飘；R设太大，又跟不上动态。

经验： 对于四轴飞行器，我建议先用互补滤波快速验证，等系统稳定了再上卡尔曼滤波。卡尔曼滤波虽然精度高，但调参成本也高。

最后，分享一些我在项目中的避坑指南：

好了，这一章就到这里。传感器是飞控的“眼睛”，数据融合是“大脑”。下一章我们聊聊控制算法，怎么让飞机听话地飞起来。