第三节：传感器原理与数据融合

各位同学好，我是你们的飞控算法讲师。今天咱们聊聊飞控系统里最核心的“感官”——传感器。

说实话，我入行那会儿，第一个项目就是调四旋翼。当时觉得算法最难，后来才发现，真正让飞机“摔跟头”的，往往是传感器数据没处理好。你想想看，算法再牛，喂进去的是垃圾数据，出来的只能是更垃圾的控制指令。

这一节，咱们就把加速度计、陀螺仪、磁力计这三个家伙彻底搞明白。然后，我会手把手带你走一遍互补滤波和Mahony滤波的实战流程。

3.1 三大核心传感器：它们各自在干嘛？

飞控里常用的传感器就这三种。它们各有各的脾气，也各有各的短板。

3.1.1 加速度计

原理：说白了，它测的是“比力”。内部有个质量块，当飞行器加速或倾斜时，质量块会挤压或拉伸周围的晶体结构，产生电荷变化。这个变化量，就对应了加速度值。

它能告诉我们什么？

• 静态姿态：当飞机悬停或缓慢移动时，加速度计可以感知重力方向。重力方向就是“向下”，由此可以算出俯仰角和横滚角。
• 动态加速度：飞机突然加速或减速时，它也能测出来。

3.1.2 陀螺仪

原理：陀螺仪测的是角速度。它利用的是科里奥利力效应——一个振动的质量块，当它旋转时，会受到一个垂直于振动方向的力。测出这个力，就能算出旋转速度。

它能告诉我们什么？

• 瞬时角速度：飞机绕X、Y、Z轴转得有多快。
• 通过积分得到角度：把角速度对时间积分，就能得到旋转的角度。

3.1.3 磁力计

原理：磁力计就是电子罗盘。它测量的是地球磁场的方向。通过感知磁场在三个轴上的分量，可以算出飞机相对于磁北的航向角（偏航角）。

它能告诉我们什么？

• 绝对航向：不像陀螺仪会漂移，磁力计给出的是绝对方向。
• 辅助修正：在GPS信号弱或室内环境下，磁力计是偏航角的主要来源。

3.2 为什么需要数据融合？

现在你知道了每个传感器的优缺点：

传感器	优点	缺点
加速度计	长期稳定，能测重力方向	易受振动干扰，动态响应慢
陀螺仪	动态响应快，短时精度高	有零偏漂移，长期积分会发散
磁力计	提供绝对航向	易受磁场干扰，需要校准

你看，没有一个传感器是完美的。但如果我们把它们组合起来，就能取长补短。这就是数据融合的核心思想：用加速度计和磁力计的低频信息，去修正陀螺仪的高频漂移。

3.3 互补滤波：最简单实用的融合方法

互补滤波的原理非常直观。它假设：

• 陀螺仪在高频段（快速运动）更可信。
• 加速度计/磁力计在低频段（静态或慢速运动）更可信。

所以，我们设计一个滤波器：让陀螺仪的高频通过，让加速度计的低频通过，然后加起来。

公式很简单：

角度 = α * (角度 + 陀螺仪角速度 * dt) + (1 - α) * 加速度计角度

这里的 α 是融合系数，通常取 0.98 左右。它决定了陀螺仪和加速度计的权重。

3.4 Mahony滤波：更优雅的解决方案

互补滤波虽然简单，但它有个问题：它假设角度变化是线性的，而且没有考虑四元数的优势。在实际工程中，我更推荐使用Mahony滤波。

Mahony滤波的核心思想是：用加速度计和磁力计的测量值，去修正陀螺仪的积分误差。它把这个问题转化成了一个“误差修正”问题。

具体流程如下：

1. 预测：用陀螺仪角速度更新四元数，得到当前姿态的预测值。
2. 观测：用加速度计和磁力计的测量值，计算出“期望”的重力方向和磁场方向。
3. 误差计算：比较预测的重力方向与实际测量的重力方向，得到误差向量。
4. 修正：用这个误差向量去修正陀螺仪的零偏，并调整四元数。

下面是一个简化的Mahony滤波核心代码（C语言风格）：

// Mahony 滤波单步更新
void MahonyAHRSupdate(float gx, float gy, float gz, 
                      float ax, float ay, float az,
                      float mx, float my, float mz) {
    float recipNorm;
    float q0q0, q0q1, q0q2, q0q3;
    float hx, hy, bx, bz;
    float vx, vy, vz;
    float wx, wy, wz;
    float ex, ey, ez;

    // 1. 归一化加速度计和磁力计数据
    recipNorm = invSqrt(ax*ax + ay*ay + az*az);
    ax *= recipNorm;
    ay *= recipNorm;
    az *= recipNorm;

    recipNorm = invSqrt(mx*mx + my*my + mz*mz);
    mx *= recipNorm;
    my *= recipNorm;
    mz *= recipNorm;

    // 2. 用当前四元数预测重力方向
    vx = 2*(q1q3 - q0q2);
    vy = 2*(q0q1 + q2q3);
    vz = q0q0 - q1q1 - q2q2 + q3q3;

    // 3. 计算误差（叉积）
    ex = (ay*vz - az*vy);
    ey = (az*vx - ax*vz);
    ez = (ax*vy - ay*vx);

    // 4. 用误差修正陀螺仪零偏
    integralFBx += Ki * ex * dt;
    integralFBy += Ki * ey * dt;
    integralFBz += Ki * ez * dt;

    gx += integralFBx + Kp * ex;
    gy += integralFBy + Kp * ey;
    gz += integralFBz + Kp * ez;

    // 5. 用修正后的角速度更新四元数
    q0 += (-q1*gx - q2*gy - q3*gz) * 0.5f * dt;
    q1 += ( q0*gx + q2*gz - q3*gy) * 0.5f * dt;
    q2 += ( q0*gy - q1*gz + q3*gx) * 0.5f * dt;
    q3 += ( q0*gz + q1*gy - q2*gx) * 0.5f * dt;

    // 6. 重新归一化四元数
    recipNorm = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
    q0 *= recipNorm;
    q1 *= recipNorm;
    q2 *= recipNorm;
    q3 *= recipNorm;
}

3.5 知识体系总览

为了让你更直观地理解整个数据融合的流程，我画了一张图：

这张图清晰地展示了数据从传感器采集，到预处理，再到融合算法，最后输出姿态的完整链路。注意那条红色的虚线反馈回路——这就是Mahony滤波的精髓所在：用观测误差去修正陀螺仪的零偏。

3.6 实战中的几个坑

最后，分享几个我踩过的坑，希望能帮你省点时间：

• 传感器时间戳要对齐：我曾经遇到过陀螺仪和加速度计的数据不是同一时刻采样的，导致融合出来的姿态一直在抖。解决办法是在中断里同时读取所有传感器数据。
• 初始化很重要：上电后，让飞机静止几秒钟，用这段时间采集加速度计和磁力计的初始值，用来做零偏校准。
• 不要迷信高精度传感器：有时候，一个便宜的MPU6050配合好的滤波算法，效果比一个昂贵的工业级传感器配合烂算法要好得多。算法才是灵魂。

---