23、陀螺仪漂移补偿：利用加速度计和磁力计修正漂移

陀螺仪漂移，这玩意儿我太熟了。刚入行那会儿，我拿着手机做AR导航，明明站在原地，屏幕里的箭头却自己慢慢转圈。嗯，这就是典型的陀螺仪漂移。

说白了，陀螺仪测的是角速度，不是绝对角度。它通过积分算出姿态，但积分嘛，误差会一点点累积。时间一长，数据就飘了。你想想看，一个静止的手机，陀螺仪输出却显示它在旋转，这谁能忍？

漂移是怎么来的？

我总结下来，漂移主要有三个来源：

• 零偏不稳定：陀螺仪静止时输出不为零，有个小偏置。这个偏置还会随温度、时间变化。
• 积分误差累积：角速度积分一次变角度，误差也跟着积分。时间越长，误差越大。
• 噪声放大：高频噪声在积分过程中会被放大，导致角度抖动。

补偿思路：多传感器融合

加速度计和磁力计就是我们的「锚点」。加速度计感知重力方向，告诉你「哪边是下」；磁力计感知地磁场，告诉你「哪边是北」。两者结合，就能给出绝对的姿态参考。

我个人习惯用互补滤波来做融合。它简单、计算量小，在嵌入式设备上跑得飞快。当然，卡尔曼滤波更精确，但调参麻烦。初学者先从互补滤波入手，准没错。

互补滤波原理

互补滤波的核心思想很简单：

• 陀螺仪：短期准，长期漂。给它一个高通滤波器，滤掉低频漂移。
• 加速度计+磁力计：长期准，短期噪声大。给它一个低通滤波器，滤掉高频抖动。
• 两者相加，就得到了一个既稳定又准确的姿态。

公式长这样：

角度 = α × (角度 + 陀螺仪角速度 × dt) + (1 - α) × 加速度计/磁力计计算的角度

这里的 α 是权重系数，通常在 0.9~0.99 之间。α 越大，越信任陀螺仪；α 越小，越信任加速度计。我在项目中一般取 0.98，效果不错。

实战：Android 代码实现

下面是我写的一个简单互补滤波类。你直接拿去用就行。

public class ComplementaryFilter {
    private static final float ALPHA = 0.98f;
    private static final float DT = 0.01f; // 采样周期，10ms

    private float pitch = 0f;
    private float roll = 0f;

    public void update(float gyroX, float gyroY, float gyroZ,
                       float accelX, float accelY, float accelZ) {
        // 1. 陀螺仪积分
        pitch += gyroX * DT;
        roll += gyroY * DT;

        // 2. 加速度计计算角度
        float accelPitch = (float) Math.atan2(-accelX, 
                Math.sqrt(accelY * accelY + accelZ * accelZ));
        float accelRoll = (float) Math.atan2(accelY, accelZ);

        // 3. 互补融合
        pitch = ALPHA * pitch + (1 - ALPHA) * accelPitch;
        roll = ALPHA * roll + (1 - ALPHA) * accelRoll;
    }

    public float getPitch() { return pitch; }
    public float getRoll() { return roll; }
}

磁力计加入：解决偏航角漂移

加速度计只能修正俯仰和横滚，偏航角（Yaw）它管不了。因为偏航是绕重力轴旋转，加速度计感知不到。这时候就需要磁力计上场了。

磁力计给出地磁场方向，结合加速度计的姿态，就能算出绝对的偏航角。我曾经在一个无人机项目里试过，只用陀螺仪积分偏航，10分钟漂了30度。加上磁力计补偿后，漂移控制在2度以内。

不过要注意，磁力计很容易受干扰。手机扬声器、金属外壳、甚至旁边的电脑都会影响它。我建议在使用前做一下硬铁和软铁校准。

避坑指南

知识结构图

下面这张图总结了陀螺仪漂移补偿的核心逻辑：

总结一下

陀螺仪漂移补偿，说白了就是「用长期准的传感器去校准短期准的传感器」。加速度计管俯仰和横滚，磁力管偏航。互补滤波是最简单的实现方式，适合入门。

我在实际项目中，还会加入动态阈值判断——当检测到手机在剧烈运动时，暂时降低加速度计的权重，避免运动加速度干扰姿态计算。这个技巧很实用，你可以试试。