21、多传感器协同：同时使用陀螺仪、加速度计、磁力计
前面几章我们分别聊了陀螺仪、加速度计、磁力计。每个传感器单独拿出来，都能做不少事。但说实话，单个传感器都有它的短板。陀螺仪有漂移，加速度计怕震动，磁力计容易受干扰。
那怎么办？把它们三个凑在一起用。这就是多传感器协同，也叫传感器融合。
我个人习惯把这种组合叫做「铁三角」。三个传感器互相补位，才能得到稳定可靠的姿态数据。今天我们就来拆解这个铁三角是怎么工作的。
为什么需要三个传感器？
先说说各自的毛病。
- 陀螺仪：短时间精度高，但长时间会漂移。你放在桌上不动，它可能慢慢读出几度每秒的角速度。积分后姿态就歪了。
- 加速度计：能测重力方向，算出俯仰和横滚。但它怕加速度干扰。你走路、跑步、坐车，它分不清哪个是重力，哪个是运动加速度。
- 磁力计：能测地磁场方向，算出航向角。但它容易受周围铁磁物质干扰。手机旁边放个音箱，磁力计读数就飘了。
你看，每个传感器都有「靠谱」和「不靠谱」的场景。多传感器协同的核心思路就是：在各自擅长的场景取信任，在各自不擅长的场景做补偿。
核心原则：陀螺仪负责短期动态响应，加速度计和磁力计负责长期静态校准。三者互补，才能得到稳定、低延迟、无漂移的姿态数据。
传感器融合的基本框架
我画了一张图，帮你理解三个传感器在融合中的角色。
三个传感器把原始数据送进融合算法。算法输出三个姿态角：俯仰、横滚、航向。同时，输出的姿态还会反馈给陀螺仪，用来修正漂移。这就是一个闭环。
三种常见的融合算法
我在项目中用过几种不同的融合算法。各有各的脾气。
| 算法 | 原理 | 计算量 | 精度 | 适用场景 |
|---|---|---|---|---|
| 互补滤波 | 陀螺仪高频可信，加速度计/磁力计低频可信。用高通+低通滤波组合。 | 低 | 中等 | 入门项目、Arduino、低功耗设备 |
| 卡尔曼滤波 | 建立状态方程和观测方程，用预测+更新迭代估计最优姿态。 | 高 | 高 | 无人机、机器人、高精度导航 |
| Madgwick算法 | 基于梯度下降的姿态估计，融合四元数。 | 中 | 高 | IMU模块、VR/AR设备 |
我的建议：如果你是第一次做传感器融合，先从互补滤波入手。代码简单，效果也够用。等理解了原理，再上卡尔曼滤波。我曾经在一个平衡车项目里直接上卡尔曼，结果调参调了三天没调好，换成互补滤波半天就搞定了。
互补滤波的代码实现
互补滤波的原理很简单。陀螺仪算出的姿态变化，加上加速度计和磁力计算出的静态姿态，按比例混合。
公式长这样：
姿态 = α × (陀螺仪积分姿态) + (1 - α) × (加速度计/磁力计姿态)α 通常取 0.98 左右。意思是：98% 信任陀螺仪的短期变化，2% 信任加速度计的长期校准。
下面是一个 Android 上的互补滤波实现片段：
// 互补滤波更新姿态
public void updateComplementary(float dt,
float gx, float gy, float gz,
float ax, float ay, float az,
float mx, float my, float mz) {
// 1. 陀螺仪积分（四元数更新）
float halfDt = dt * 0.5f;
float qDot1 = halfDt * (-q1 * gx - q2 * gy - q3 * gz);
float qDot2 = halfDt * ( q0 * gx + q2 * gz - q3 * gy);
float qDot3 = halfDt * ( q0 * gy - q1 * gz + q3 * gx);
float qDot4 = halfDt * ( q0 * gz + q1 * gy - q2 * gx);
q0 += qDot1;
q1 += qDot2;
q2 += qDot3;
q3 += qDot4;
// 归一化
float norm = (float) Math.sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 /= norm;
q1 /= norm;
q2 /= norm;
q3 /= norm;
// 2. 从加速度计计算俯仰和横滚
float pitchAcc = (float) Math.atan2(-ax, Math.sqrt(ay*ay + az*az));
float rollAcc = (float) Math.atan2(ay, az);
// 3. 从磁力计计算航向
// 先转换到水平坐标系
float hx = mx * (float) Math.cos(pitchAcc) +
my * (float) Math.sin(rollAcc) * (float) Math.sin(pitchAcc) -
mz * (float) Math.cos(rollAcc) * (float) Math.sin(pitchAcc);
float hy = my * (float) Math.cos(rollAcc) +
mz * (float) Math.sin(rollAcc);
float yawMag = (float) Math.atan2(hy, hx);
// 4. 互补融合
float alpha = 0.98f;
pitch = alpha * pitch + (1 - alpha) * pitchAcc;
roll = alpha * roll + (1 - alpha) * rollAcc;
yaw = alpha * yaw + (1 - alpha) * yawMag;
}注意：互补滤波的 α 值不是固定的。传感器采样率越高，α 可以越接近 1。我一般会在 0.96 到 0.995 之间调试。调的时候拿手机在桌上转几圈，看回零后姿态有没有漂移。
传感器融合的避坑指南
我曾经在一个 AR 导航项目里吃过亏。三个传感器单独测试都没问题，一融合就出乱子。后来排查了三天，发现是这几个坑：
- 时间戳不同步：三个传感器的数据到达时间不一样。陀螺仪可能 100Hz，加速度计 50Hz，磁力计 20Hz。如果不做时间对齐，融合出来的姿态会抖动。解决办法是用系统时间戳做插值。
- 坐标系不统一：每个传感器的坐标系可能不同。Android 里陀螺仪和加速度计是同一个坐标系，但磁力计可能不同。融合前一定要转换到同一坐标系。
- 磁力计没校准：硬铁和软铁干扰会导致磁力计读数偏好几度。融合前必须做磁力计校准。我一般会在 app 里画一个圆，让用户拿着手机画 8 字，自动采集校准数据。
- 加速度计没低通滤波：运动中的高频震动会污染加速度计数据。建议先做低通滤波，再送进融合算法。
经验之谈：多传感器融合，70% 的工作量在数据预处理上。校准、滤波、时间对齐、坐标系转换，这些做好了，融合算法本身反而简单。别一上来就调卡尔曼的协方差矩阵，先把数据质量搞上去。
Android 上的 SensorManager 配置
在 Android 里同时注册三个传感器，需要注意采样率的匹配。我习惯这样配置：
SensorManager sensorManager = (SensorManager) getSystemService(SENSOR_SERVICE);
Sensor gyro = sensorManager.getDefaultSensor(Sensor.TYPE_GYROSCOPE);
Sensor accel = sensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);
Sensor magnet = sensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD);
// 注册监听器，统一使用 SENSOR_DELAY_GAME 延迟
sensorManager.registerListener(this, gyro, SensorManager.SENSOR_DELAY_GAME);
sensorManager.registerListener(this, accel, SensorManager.SENSOR_DELAY_GAME);
sensorManager.registerListener(this, magnet, SensorManager.SENSOR_DELAY_GAME);为什么用 SENSOR_DELAY_GAME？因为它大约是 20ms 一次，也就是 50Hz。这个频率对大多数融合算法来说够用了。太快了反而增加 CPU 负担，而且磁力计本身也跟不上。
小技巧：在 onSensorChanged 回调里，用 System.nanoTime() 记录每个传感器数据的时间戳。然后在融合时，用这个时间戳做插值对齐。别用 event.timestamp，那个是硬件时间，不同传感器之间可能有偏移。
融合后的效果
三个传感器融合后，你能得到什么？
- 零漂移的姿态角：手机放桌上，俯仰、横滚、航向都不会慢慢飘走。
- 低延迟的响应：快速转动手机，姿态能跟上，不会卡顿。
- 抗干扰能力强：走路、坐车时，姿态不会因为震动而乱跳。
说白了，融合后的姿态数据，才是真正可用的数据。单独用陀螺仪或者加速度计，都做不到这个效果。
嗯，多传感器协同就聊到这里。下一章我们聊聊怎么把这些数据用在实际项目里。到时候我会拿一个 AR 导航的例子，把今天讲的融合算法串起来。
公众号:蓝海资料掘金营，微信deep3321