第4章陀螺仪数据读取与滤波

第4章：陀螺仪数据读取与滤波

陀螺仪是光学防抖系统的「眼睛」。它负责感知镜头的角速度变化。但说实话，这颗「眼睛」看到的画面并不干净。原始数据里掺杂着噪声、温漂、还有各种高频抖动。如果不做处理，防抖系统会乱抖，甚至比不防抖还糟糕。

这一章，我们就来聊聊怎么把陀螺仪的原始数据「洗干净」。我会从最基础的读取开始，一步步带你走进滤波的世界。

4.1 原始数据读取：第一步，别搞错方向

我刚开始做防抖时，犯过一个低级错误。陀螺仪数据读回来了，但镜头纹丝不动。查了半天，发现是I2C的时序不对，数据全是0。嗯，这种坑踩过一次就记住了。

陀螺仪通常通过SPI或I2C接口与主控通信。以常见的MPU6050为例，读取流程大致如下：

// 伪代码：读取陀螺仪原始数据
uint8_t reg_addr = 0x43;  // 角速度数据起始寄存器
uint8_t buf[6];

i2c_read(dev_addr, reg_addr, buf, 6);

int16_t gyro_x = (buf[0] << 8) | buf[1];
int16_t gyro_y = (buf[2] << 8) | buf[3];
int16_t gyro_z = (buf[4] << 8) | buf[5];

// 转换为角速度（度/秒）
float sensitivity = 131.0;  // ±250°/s 量程
float gx = gyro_x / sensitivity;
float gy = gyro_y / sensitivity;
float gz = gyro_z / sensitivity;

这里有个关键点：量程选择。手机防抖一般用±250°/s就够了。但如果是运动相机或无人机，建议选±500°/s或更高。量程越小，分辨率越高，但容易溢出。我见过有人用±250°/s做无人机航拍，结果一个急转弯数据直接饱和，画面瞬间飞掉。

注意：陀螺仪数据有「零偏」现象。静止时输出不为0。这个值每个芯片都不一样，甚至同芯片每次上电都不同。建议在系统初始化时做一次静态校准，减去这个偏置。

4.2 低通滤波：IIR与FIR的选择

原始数据里最烦人的是高频噪声。比如电机PWM的耦合噪声、机械振动的谐波。低通滤波就是把这些高频成分干掉。

常用的有两种：IIR和FIR。我个人的习惯是：资源紧张用IIR，追求线性相位用FIR。

4.2.1 IIR滤波：简单高效，但有坑

IIR滤波器的核心是递归。说白了，就是用过去的输出和输入来算现在的输出。代码量极小，适合嵌入式。

// 一阶IIR低通滤波
float alpha = 0.1;  // 滤波系数，越小越平滑
float filtered = 0;

float iir_filter(float raw) {
    filtered = alpha * raw + (1 - alpha) * filtered;
    return filtered;
}

alpha的取值很关键。alpha越小，截止频率越低，平滑效果越好，但延迟也越大。我在项目中一般取0.05~0.2之间。具体值要看你的采样率和目标带宽。

经验值：采样率1kHz，alpha=0.1时，截止频率大约16Hz。这个值对光学防抖来说偏低了。我通常用alpha=0.2~0.3，截止频率30~50Hz，既能滤掉高频噪声，又不至于让响应太慢。

IIR有个致命问题：相位非线性。不同频率的信号延迟不一样。这在防抖系统里会引起「相位滞后」，导致补偿动作跟不上实际抖动。嗯，这里要注意，IIR用多了，系统会感觉「肉肉的」。

4.2.2 FIR滤波：线性相位，但吃内存

FIR滤波器没有反馈，只靠当前和过去的输入值来计算。它的最大优点是线性相位——所有频率的延迟都一样。这对防抖系统来说非常友好。

// 简单的3阶FIR低通滤波
#define TAP_NUM 3
float coeff[TAP_NUM] = {0.25, 0.5, 0.25};  // 系数
float buffer[TAP_NUM] = {0};
int idx = 0;

float fir_filter(float raw) {
    buffer[idx] = raw;
    idx = (idx + 1) % TAP_NUM;
    
    float out = 0;
    for (int i = 0; i < TAP_NUM; i++) {
        out += coeff[i] * buffer[(idx + i) % TAP_NUM];
    }
    return out;
}

FIR的阶数越高，滤波效果越好，但需要的RAM也越多。在资源受限的MCU上，我一般用8~16阶。再高就扛不住了。

  我的建议：如果MCU的RAM够用（比如有32KB以上），优先用FIR。如果RAM紧张，用IIR但要注意补偿相位滞后。我在一个低端MCU项目上用过IIR+相位补偿，效果也还行。

4.3 卡尔曼滤波入门：别被名字吓到

卡尔曼滤波听起来高大上，其实核心思想很简单：用预测+测量，不断修正。你想想看，陀螺仪有噪声，但我们可以用模型预测下一秒的角度，然后用实际测量值来修正预测值。一来二去，结果就越来越准。

对于光学防抖，我们通常用一维卡尔曼滤波来估计角速度。状态量就是角速度本身。

// 一维卡尔曼滤波（简化版）
typedef struct {
    float Q;  // 过程噪声协方差
    float R;  // 测量噪声协方差
    float P;  // 估计误差协方差
    float K;  // 卡尔曼增益
    float X;  // 状态估计值
} Kalman1D;

float kalman_filter(Kalman1D *kf, float measurement) {
    // 预测
    kf->P = kf->P + kf->Q;
    
    // 更新
    kf->K = kf->P / (kf->P + kf->R);
    kf->X = kf->X + kf->K * (measurement - kf->X);
    kf->P = (1 - kf->K) * kf->P;
    
    return kf->X;
}

调参是关键。Q和R怎么设？我有个笨办法：先让系统静止，记录噪声的方差作为R。Q则根据你对模型的信任程度来设。Q越大，滤波器越相信测量值；Q越小，越相信预测值。

避坑指南：我曾经在一个项目里把Q设得太小，结果滤波器几乎不响应实际抖动，防抖完全失效。后来把Q调大了一个数量级，效果立竿见影。记住，卡尔曼滤波不是万能的，参数调不好还不如简单的IIR。

4.4 互补滤波：简单实用的折中方案

如果你觉得卡尔曼滤波太复杂，IIR又不够好，那互补滤波是个不错的选择。它把陀螺仪的高频信息和加速度计的低频信息融合起来。说白了，就是「取长补短」。

// 互补滤波（角度估计）
float angle = 0;
float dt = 0.001;  // 采样周期1ms
float tau = 0.5;   // 时间常数

float complementary_filter(float gyro_rate, float acc_angle) {
    float alpha = tau / (tau + dt);
    angle = alpha * (angle + gyro_rate * dt) + (1 - alpha) * acc_angle;
    return angle;
}

tau值决定了滤波器的截止频率。tau越大，越信任陀螺仪；tau越小，越信任加速度计。我一般从0.5开始调，根据实际响应微调。

小技巧：互补滤波的代码量只有卡尔曼滤波的1/3，但效果能达到80%。如果你的MCU性能有限，或者你不想花太多时间调参，互补滤波是首选。我在一个量产项目上就用它，跑了两年没出过问题。

4.5 本章知识体系

下面这张图总结了陀螺仪数据处理的完整流程。从原始数据到最终可用的角速度，每一步都有讲究。

从这张图可以看出，滤波不是孤立的一步。它和前面的预处理、后面的控制算法紧密相关。我个人习惯在选型阶段就把滤波方法定下来，因为不同的滤波方法对MCU算力和RAM的要求差别很大。

好了，这一章的内容就到这里。陀螺仪数据的读取和滤波是光学防抖的基础，也是决定系统性能的关键。下一章我们会聊聊怎么把这些处理好的数据送给电机驱动，让镜头真正动起来。

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

📚 课程目录