为什么要融合多个传感器

做姿态估算（无人机、平衡车、机械臂末端姿态）最经典的问题：单一传感器都不准。加速度计能测重力方向，知道设备"大概朝哪"，但一加运动加速度就飘。陀螺仪测转动角速度，短期很准，但积分时间一长就漂移，角度越算越偏。

两个传感器单独用都不行，合起来互补，就是所谓传感器融合。
传感器融合配图：陀螺仪芯片辐射三轴波形与滤波波纹
把加速度计的长期稳定和陀螺仪的短期精准合在一起

这篇讲的是工业界最常用的融合方法：互补滤波，以及它的升级版卡尔曼滤波。

MPU6050：加速度计 + 陀螺仪

MPU6050 是一颗 6 轴 IMU：三轴加速度计（测 X/Y/Z 方向的线加速度）+ 三轴陀螺仪（测 X/Y/Z 轴的角速度）。一颗芯片十几块钱，I2C 接口，几乎是所有姿态项目的入门选择。

// 从 MPU6050 读原始数据（简化）
struct imu_raw { int16_t ax, ay, az; int16_t gx, gy, gz; };
void imu_read(struct imu_raw * r) {
    i2c_read(0x68, 0x3B, r, sizeof(*r));
    // 换算成物理量：
    // accel: ±2g 量程，1g = 16384 LSB
    // gyro:  ±250°/s 量程，1°/s = 131 LSB
    r->ax /= 16384.0f;
    r->gx /= 131.0f;
}

读数只是第一步，真正的工作是把这些原始数据合成一个稳定的姿态角。

加速度计算姿态的局限

静止时，加速度计测到的就是重力。把 X/Y/Z 三个方向的重力分量做反正切，就能算出俯仰角和横滚角。

// 由加速度计算俯仰角 pitch
pitch_acc = atan2f(ay, az) * 57.3f;
roll_acc  = atan2f(-ax, sqrtf(ay*ay + az*az)) * 57.3f;

问题是：设备一旦动起来，测到的是重力加运动加速度，算出来的角度就飘。所以加速度计只能当"长期参考"，不能当实时角度用。

互补滤波：最实用的融合算法

思路很简单：陀螺仪短期积分得到当前角度，加速度计算出的角度当参考，用一个系数把两者加权平均。系数一般取 0.98，也就是相信陀螺仪 98%、相信加速度计 2%。

float pitch = 0, dt = 0.01f;
void complementary_filter(float gx, float pitch_acc) {
    // 陀螺仪积分：当前角 = 上一帧角 + 角速度 * 时间
    float gyro_pitch = pitch + gx * dt;
    // 互补融合：98% 信陀螺，2% 信加速度
    pitch = 0.98f * gyro_pitch + 0.02f * pitch_acc;
}
互补滤波为什么有效
  • 陀螺仪高频准、低频飘。
  • 加速度计低频准、高频噪。
  • 融合后高频走陀螺、低频走加速度，刚好互补。