第二章 传感器原理与数据融合
做飞控这些年，我最大的体会就是：姿态解算就像在嘈杂的酒吧里听清朋友说话。传感器就是你的耳朵，但每个耳朵都有点“耳背”——陀螺仪漂移、加速度计抖动、磁力计受干扰。怎么把这些不完美的信号融合成可靠的姿态？这就是本章要聊的核心。
本章知识体系
2.1 陀螺仪：角速度的忠实记录者
陀螺仪测量的是角速度，单位是 °/s 或 rad/s。说白了，就是告诉你飞机转得有多快。我最早接触的是 MPU6050 里的 MEMS 陀螺仪，那玩意儿便宜但噪声大。
工作原理：MEMS 陀螺仪利用科里奥利力效应。内部有一个高速振动的质量块，当传感器旋转时，质量块会受到一个垂直于振动方向的力，这个力的大小与角速度成正比。通过检测这个力，就能算出角速度。
我的经验：陀螺仪最怕的是零偏漂移。刚上电时数据还行，飞了十分钟就开始慢慢偏。我有个项目，就是因为没做温补，悬停时飞机慢慢自旋。后来加了温度补偿，问题才解决。
误差模型：
- 零偏（Bias）：静止时输出不为零，随时间缓慢变化
- 比例因子误差：实际角速度与测量值之间的比例偏差
- 交叉轴耦合：一个轴的旋转会影响到其他轴的输出
- 随机游走：积分后产生的随机误差累积
校准方法：
// 陀螺仪零偏校准 - 静止采样取平均
void calibrateGyro() {
float sum[3] = {0, 0, 0};
int samples = 200;
for (int i = 0; i < samples; i++) {
readGyro(gx, gy, gz);
sum[0] += gx;
sum[1] += gy;
sum[2] += gz;
delay(5);
}
gyroBias[0] = sum[0] / samples;
gyroBias[1] = sum[1] / samples;
gyroBias[2] = sum[2] / samples;
}注意：校准时要确保无人机完全静止。我曾经在桌面上校准，结果桌子在抖，校准出来的零偏全是错的。后来我都是把飞机放地上，等三分钟再校准。
2.2 加速度计：重力方向的感知者
加速度计测量的是比力，也就是物体受到的加速度（包括重力）。静止时，它测到的就是重力加速度 g。利用这个特性，我们可以算出俯仰角和横滚角。
工作原理：MEMS 加速度计内部有一个微小的质量块，通过弹簧结构悬挂。当有加速度时，质量块会移动，改变电容值。测量电容变化就能算出加速度。
为什么加速度计不能单独用？因为它分不清重力加速度和运动加速度。你想想看，飞机往前加速时，加速度计会以为重力方向变了，导致姿态计算错误。这就是为什么需要陀螺仪来帮忙。
误差模型：
- 零偏：静止时输出不为 0g 或 1g
- 比例因子：不同轴灵敏度不一致
- 非线性：大加速度下输出失真
- 噪声：高频振动噪声，尤其是电机转动时
六面校准法：
| 位置 | X轴 | Y轴 | Z轴 |
|---|---|---|---|
| 水平朝上 | 0g | 0g | +1g |
| 水平朝下 | 0g | 0g | -1g |
| 左侧朝上 | 0g | +1g | 0g |
| 右侧朝上 | 0g | -1g | 0g |
| 前倾朝上 | +1g | 0g | 0g |
| 后倾朝上 | -1g | 0g | 0g |
避坑指南：我曾经用塑料支架做六面校准，结果支架本身有弹性，放上去后不是完全水平。后来我改用金属直角块，精度才上来。校准这事，工装很重要。
2.3 磁力计：航向的指南针
磁力计测量的是地磁场。没有它，无人机就不知道自己的航向（偏航角）。但磁力计也是最娇气的传感器——周围有铁磁物质就会干扰。
工作原理：常见的是各向异性磁阻（AMR）传感器。内部有磁阻材料，外部磁场会改变其电阻值。通过测量电阻变化，就能算出磁场强度。
误差模型：
- 硬铁干扰：永磁体或带磁物体产生的固定偏移
- 软铁干扰：铁磁材料对磁场的扭曲，导致椭圆变形
- 比例因子误差：各轴灵敏度不一致
- 非正交误差：三个轴不是严格垂直
椭圆拟合校准：
// 磁力计校准 - 采集数据后做椭圆拟合
void calibrateMag() {
// 旋转无人机采集所有方向的数据
// 理想情况下，数据点应分布在球面上
// 实际是椭球，需要求偏移和缩放矩阵
// 最小二乘法拟合椭球
// 得到偏移量 offset[3] 和缩放矩阵 scale[3][3]
// 校准后的值 = scale * (raw - offset)
}警告：磁力计校准必须在远离铁磁物质的地方进行。我见过有人直接在金属桌子上校准，结果飞出去后航向偏了 30 度。还有，电机和电调也会产生磁场干扰，最好在装机后做最终校准。
2.4 互补滤波：简单有效的融合方法
互补滤波的原理很直观：陀螺仪动态响应快但会漂移，加速度计和磁力计没有漂移但噪声大。那就让陀螺仪负责高频部分，加速度计和磁力计负责低频部分。
说白了就是：
姿态 = α × (陀螺仪积分) + (1-α) × (加速度计/磁力计计算)其中 α 通常取 0.98 左右。α 越大，越信任陀螺仪；α 越小，越信任加速度计。
我的习惯：α 值我一般根据运动剧烈程度动态调整。悬停时 α 小一点，让加速度计多参与；快速机动时 α 大一点，避免加速度计被运动加速度干扰。
2.5 Mahony 与 Madgwick：更优雅的姿态解算
这两个算法都是基于四元数的，比互补滤波更精确。我个人更常用 Madgwick，因为它收敛速度快，而且计算量适中。
Mahony 算法：
- 基于 PI 控制器的思想
- 用加速度计和磁力计的测量值修正陀螺仪的积分误差
- 计算量小，适合低性能 MCU
Madgwick 算法：
- 基于梯度下降法
- 每一步都向误差减小的方向调整四元数
- 收敛速度快，精度高
- 需要调整步长参数 β
// Madgwick 算法核心 - 简化版
void MadgwickUpdate(float gx, float gy, float gz,
float ax, float ay, float az,
float mx, float my, float mz, float dt) {
// 1. 计算陀螺仪的四元数导数
// 2. 用加速度计计算误差梯度
// 3. 用磁力计计算误差梯度
// 4. 梯度下降法修正四元数
// 5. 归一化四元数
// 具体实现可参考 Sebastian Madgwick 的论文
}选型建议：
- STM32F103 级别：用互补滤波或 Mahony
- STM32F4 及以上：用 Madgwick，精度更好
- 带 FPU 的芯片：可以跑更复杂的 EKF（扩展卡尔曼滤波）
嗯，这里要注意一点：Madgwick 的 β 参数很关键。β 太大，算法会过度信任加速度计，导致动态响应变差；β 太小，又收敛太慢。我一般从 0.1 开始调，看实际效果再微调。
做飞控这些年，我越来越觉得传感器融合就像和人打交道——每个传感器都有自己的脾气，你要了解它、信任它该信任的部分、忽略它该忽略的部分。互补滤波、Mahony、Madgwick，说到底都是帮我们做这件事的工具。
最后分享一个经验：不管用什么算法，一定要做传感器数据的时间戳对齐。我踩过这个坑——陀螺仪和加速度计采样时间差了 2ms，结果姿态解算出来一直在抖。后来加了时间戳同步，问题就解决了。
公众号:蓝海资料掘金营，微信deep3321