15、传感器融合基础：卡尔曼滤波原理、互补滤波算法、多传感器数据融合、姿态解算优化

各位同学，欢迎来到传感器融合这一章。说实话，这是整个传感器开发中最有挑战性、也最有意思的部分。你想想看，单个传感器总有它的短板——加速度计怕振动，陀螺仪会漂移，磁力计容易受干扰。但把它们组合起来，就能取长补短。这就是传感器融合的核心思想。

我在做无人机飞控项目时，深刻体会过单传感器的无力感。当时只用陀螺仪做姿态估计，飞了不到两分钟，姿态角就漂了十几度。后来加入了加速度计和磁力计做融合，才真正稳住了。嗯，今天我们就来聊聊这背后的原理。

15.1 为什么需要传感器融合？

先看一个简单场景：你要测量手机当前的俯仰角。

• 加速度计：能测重力方向，静态时很准。但手机一运动，加速度和重力混在一起，角度就乱了。
• 陀螺仪：测角速度，积分可得角度。动态响应快，但积分会累积误差，时间一长就漂移。

你看，单独用哪个都不完美。融合的目的，就是利用加速度计的低频准确性来修正陀螺仪的高频漂移。说白了，就是让两个传感器互相“补台”。

15.2 卡尔曼滤波原理

卡尔曼滤波，可以说是传感器融合的“黄金标准”。我第一次接触它时觉得公式很吓人，但实际用起来，核心思想其实很朴素。

它做两件事：预测和更新。

• 预测：根据上一时刻的状态，推测当前时刻的状态。比如用陀螺仪积分预测当前角度。
• 更新：用新的观测值（比如加速度计测的角度）来修正预测值。

卡尔曼滤波会计算一个“卡尔曼增益”，它决定了你更相信预测还是更相信观测。增益大，就多听观测的；增益小，就多信预测的。

下面是一个简化的一维卡尔曼滤波代码，用于融合加速度计和陀螺仪的角度估计：

public class KalmanFilter {
    private float angle;      // 估计的角度
    private float bias;       // 陀螺仪偏差
    private float[] P = new float[4]; // 误差协方差矩阵

    public KalmanFilter() {
        // 初始化
        angle = 0;
        bias = 0;
        P[0] = 0.1f; P[1] = 0;
        P[2] = 0;    P[3] = 0.1f;
    }

    public float update(float gyroRate, float accAngle, float dt) {
        // 预测阶段
        angle += dt * (gyroRate - bias);
        P[0] += dt * (dt * P[3] - P[1] - P[2] + 0.001f);
        P[1] -= dt * P[3];
        P[2] -= dt * P[3];
        P[3] += 0.003f * dt;

        // 更新阶段
        float y = accAngle - angle;  // 残差
        float S = P[0] + 0.03f;      // 测量噪声
        float K0 = P[0] / S;         // 卡尔曼增益
        float K1 = P[2] / S;

        angle += K0 * y;
        bias  += K1 * y;

        P[0] -= K0 * P[0];
        P[1] -= K0 * P[1];
        P[2] -= K1 * P[0];
        P[3] -= K1 * P[1];

        return angle;
    }
}

15.3 互补滤波算法

卡尔曼滤波虽然强大，但计算量稍大。在一些资源受限的嵌入式设备上，互补滤波是更轻量的选择。

互补滤波的思路很简单：陀螺仪的高频信号可信，加速度计的低频信号可信。所以用高通滤波器处理陀螺仪，用低通滤波器处理加速度计，然后加起来。

公式长这样：

angle = 0.98 * (angle + gyroRate * dt) + 0.02 * accAngle

这里的 0.98 和 0.02 就是权重系数。系数越大，越信任陀螺仪；系数越小，越信任加速度计。

完整实现：

public class ComplementaryFilter {
    private float angle;
    private float alpha = 0.98f; // 权重系数

    public ComplementaryFilter() {
        angle = 0;
    }

    public float update(float gyroRate, float accAngle, float dt) {
        // 陀螺仪积分
        float gyroAngle = angle + gyroRate * dt;
        // 融合
        angle = alpha * gyroAngle + (1 - alpha) * accAngle;
        return angle;
    }

    public void setAlpha(float alpha) {
        this.alpha = alpha;
    }
}

15.4 多传感器数据融合

在实际项目中，我们往往不止两个传感器。比如一个完整的姿态参考系统（AHRS）会用到：

• 3轴加速度计：提供俯仰和横滚的参考
• 3轴陀螺仪：提供角速度
• 3轴磁力计：提供航向参考

融合这三个传感器，就能得到完整的俯仰、横滚、偏航角（即欧拉角）。

我常用的融合策略是：

1. 用加速度计计算俯仰和横滚的初始值
2. 用磁力计计算偏航的初始值
3. 用陀螺仪做高频积分更新
4. 用卡尔曼或互补滤波做低频修正

下面是一个多传感器融合的流程示意图：

15.5 姿态解算优化

有了融合算法，是不是就万事大吉了？当然不是。实际工程中，还有很多坑要填。

优化点一：时间戳对齐

不同传感器的采样时间可能不同。我遇到过陀螺仪100Hz、加速度计50Hz的情况。如果不做时间对齐，融合出来的角度会抖动。解决办法是用系统时钟给每个传感器数据打时间戳，在融合时做插值。

优化点二：动态阈值判断

当设备剧烈运动时，加速度计的数据基本是废的。这时候应该降低加速度计的权重。我一般会计算加速度计的模长，如果偏离1g太多，就减少它的信任度。

float accMagnitude = sqrt(ax*ax + ay*ay + az*az);
float trustFactor = 1.0f - Math.abs(accMagnitude - 9.8f) / 4.9f;
trustFactor = Math.max(0.1f, Math.min(1.0f, trustFactor));
// 用 trustFactor 动态调整融合权重

优化点三：磁力计干扰处理

磁力计在室内很容易受钢筋、电器干扰。我曾经在一个商场里测试，磁力计的航向直接偏了30度。解决方案是：检测磁力计模长是否异常，如果异常就只用陀螺仪和加速度计做融合，放弃磁力计。

15.6 实战建议

最后，给你几个我这些年总结的实战建议：

• 从互补滤波开始：如果你的设备算力有限，或者刚入门，先用互补滤波。它简单、稳定、容易调参。
• 卡尔曼滤波做进阶：当互补滤波满足不了精度要求时，再上卡尔曼。但要做好心理准备，调参可能需要一两天。
• 一定要做传感器校准：陀螺仪的零偏、加速度计的尺度因子、磁力计的硬磁干扰，这些不校准，融合算法再牛也白搭。
• 测试要覆盖各种场景：静止、匀速运动、剧烈晃动、快速旋转，每种场景下融合效果都可能不同。

好了，传感器融合的基础就讲到这里。记住一句话：没有完美的传感器，只有巧妙的融合。下一章我们会深入四元数，看看如何用更优雅的方式表示姿态旋转。

---

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