第3章传感器基础与数据融合

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

各位同学，欢迎来到传感器与数据融合这一章。

说实话，飞控系统里最核心的硬件就是传感器。你算法写得再漂亮，传感器数据不准，飞控就是空中楼阁。我当年刚入行时，就吃过这个亏——代码调了两个月，结果发现是加速度计噪声太大，白白浪费了时间。

今天咱们把三个关键传感器讲透：加速度计、陀螺仪、磁力计。然后重点聊聊数据融合，也就是互补滤波和Mahony滤波。

3.1 加速度计：感知重力方向

加速度计测量的是物体的加速度。注意，它测的是比力，不是单纯的运动加速度。

什么意思？静止时，加速度计感受到的是重力加速度g。所以它能告诉我们：重力方向在哪。

我习惯把加速度计想象成一个「摆锤」。你倾斜它，摆锤的方向就变了。通过测量三个轴上的重力分量，就能算出俯仰角和横滚角。

核心公式（简化版）：

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

但加速度计有个大问题：噪声大。尤其是飞行器震动时，数据跳得厉害。你想想看，电机一转，整个机身都在抖，加速度计读数能准吗？

避坑指南：我曾经在测试时发现，油门推到50%以上，加速度计的俯仰角直接漂了5度。后来加了低通滤波才解决。所以，加速度计数据一定要做滤波处理。

3.2 陀螺仪：测量角速度

陀螺仪测量的是角速度，单位是°/s。它告诉你：我转得有多快。

通过积分角速度，就能得到角度。比如，你以10°/s转了1秒，角度就增加了10度。

陀螺仪的优点是：响应快，高频特性好。震动对它影响小。但缺点也很明显：有零偏漂移。积分时间一长，误差累积，角度就飞了。

我记得有一次做长航时测试，飞了10分钟，陀螺仪积分出来的偏航角已经偏了30度。这就是典型的「积分漂移」。

个人经验：陀螺仪上电后，前几秒的数据不要用。因为刚上电时，零偏还不稳定。我习惯等5秒，取前100个数据做平均，作为零偏补偿值。

3.3 磁力计：寻找北向

磁力计测量的是地球磁场。它能告诉你：北在哪。

有了磁力计，我们就能得到偏航角（yaw）。否则，只用加速度计和陀螺仪，偏航角是没法确定的。

但磁力计非常娇气。周围有铁磁物质、电机、大电流导线，都会干扰它。我曾在测试场地发现，靠近金属桌子时，磁力计读数直接偏了20度。

磁力计校准方法：

水平旋转一圈，记录x、y轴最大值和最小值
计算偏移量：offset = (max + min) / 2
计算缩放因子：scale = (max - min) / 2
校准后数据 = (原始数据 - offset) / scale

说白了，磁力计只能用来辅助修正偏航角，不能完全依赖它。

3.4 为什么需要数据融合？

现在你看到了：

加速度计：低频准，高频噪
陀螺仪：高频准，低频漂
磁力计：容易受干扰

单独用哪个都不行。所以我们需要数据融合，把它们的优点结合起来。

下面这张图展示了数据融合的核心逻辑：

3.5 互补滤波：简单有效

互补滤波是最简单的融合方法。它的思想是：

最终角度 = α × (陀螺仪积分角度) + (1-α) × (加速度计角度)

其中α是一个系数，通常在0.98左右。α越大，越信任陀螺仪；α越小，越信任加速度计。

为什么叫「互补」？因为陀螺仪擅长高频，加速度计擅长低频。互补滤波就是让它们各司其职。

互补滤波代码示例（C语言）：

float complementary_filter(float gyro_rate, float accel_angle, float dt) {
    static float angle = 0.0f;
    float alpha = 0.98f;
    
    // 陀螺仪积分
    angle += gyro_rate * dt;
    
    // 互补融合
    angle = alpha * angle + (1.0f - alpha) * accel_angle;
    
    return angle;
}

嗯，这里要注意：α的取值很关键。我习惯在调试时先设0.98，然后看响应速度。如果角度跟踪太慢，就减小α；如果噪声太大，就增大α。

3.6 Mahony滤波：更优雅的方案

互补滤波虽然简单，但只适用于一维角度。对于三维姿态，我们需要更高级的方法。

Mahony滤波就是其中之一。它基于四元数，用PI控制器来修正陀螺仪的漂移。

核心思想是：

用加速度计和磁力计计算期望姿态
用陀螺仪积分得到当前姿态
计算两者之间的误差
用PI控制器修正陀螺仪的零偏

说白了，Mahony滤波就是让陀螺仪「跟着」加速度计和磁力计走，但又不会完全被它们带偏。

个人经验：Mahony滤波的PI参数怎么调？我一般先设P=0.5，I=0。然后看收敛速度。如果收敛太慢，增大P；如果超调严重，减小P。最后再慢慢加I，用来消除稳态误差。

Mahony滤波核心代码（简化版）：

void mahony_update(float gx, float gy, float gz, 
                   float ax, float ay, float az,
                   float mx, float my, float mz, float dt) {
    float recipNorm;
    float q0q0, q0q1, q0q2, q0q3;
    // ... 四元数运算省略 ...
    
    // 计算误差（加速度计）
    float ex = (ay * vz - az * vy);
    float ey = (az * vx - ax * vz);
    float ez = (ax * vy - ay * vx);
    
    // PI控制器修正陀螺仪
    integralFBx += Ki * ex * dt;
    integralFBy += Ki * ey * dt;
    integralFBz += Ki * ez * dt;
    
    gx += Kp * ex + integralFBx;
    gy += Kp * ey + integralFBy;
    gz += Kp * ez + integralFBz;
    
    // 四元数积分
    // ... 更新四元数 ...
}

3.7 三种传感器对比总结

传感器	测量量	优点	缺点	适用场景
加速度计	比力（含重力）	低频稳定，无漂移	高频噪声大，受震动影响	俯仰/横滚角参考
陀螺仪	角速度	高频响应快，抗震动	有零偏漂移，积分误差累积	姿态角短期预测
磁力计	地磁场强度	提供绝对偏航角	易受铁磁干扰，精度有限	偏航角辅助修正

3.8 实际项目中的选择建议

我做了这么多年飞控，给你几个实在的建议：

入门级飞控：用互补滤波就够了。代码简单，调试方便。
中级飞控：用Mahony滤波。精度更高，抗干扰能力更强。
高级飞控：可以考虑卡尔曼滤波。但说实话，对于四旋翼，Mahony滤波已经足够好。卡尔曼滤波计算量大，收益有限。

曾经踩过的坑：有一次我为了追求精度，在STM32F103上跑了卡尔曼滤波。结果CPU占用率飙到80%，控制周期都乱了。后来换回Mahony滤波，CPU占用率降到20%，效果也没差多少。所以，不要盲目追求高级算法，适合的才是最好的。

好了，传感器和数据融合就讲到这里。下一章咱们开始写真正的飞控代码。

公众号:蓝海资料掘金营，微信deep3321

📚 课程目录