第3章传感器数据采集与滤波

第三章传感器数据采集与滤波：IMU数据读取与处理

好，咱们进入第三章。这一章，说白了就是飞控系统的“眼睛”和“耳朵”——IMU的数据采集与滤波。

我个人习惯把IMU比作飞控的“前庭系统”。你想想看，没有它，飞控就是个瞎子。加速度计告诉你“我歪了”，陀螺仪告诉你“我在转”。但问题是，这些传感器天生带噪声，直接拿来用，飞控会抖得像筛糠。

3.1 IMU数据读取：加速度计与陀螺仪

先说说加速度计。它测量的是比力，也就是物体受到的惯性力与重力的合力。单位是g或者m/s²。静止时，它测到的就是重力加速度。

陀螺仪呢，测量的是角速度，单位是°/s或rad/s。它告诉你转得有多快，但不直接告诉你角度。

关键点：

加速度计对震动敏感，高频噪声大
陀螺仪有零偏，长时间积分会漂移
两者互补：加速度计低频准，陀螺仪高频准

避坑指南： 我曾经在一个项目中，直接用了加速度计的角度数据做姿态控制。结果飞机一震动，角度乱跳，直接炸机。后来才明白，加速度计必须滤波，而且不能单独用。

3.2 I2C/SPI通信协议实现

IMU和MCU怎么通信？主流就两种：I2C和SPI。

I2C： 两根线（SDA、SCL），半双工，速度慢（标准模式100kHz，快速模式400kHz）。优点是引脚少，缺点是速度慢，容易受干扰。

SPI： 四根线（MOSI、MISO、SCLK、CS），全双工，速度快（可达10MHz以上）。优点是速度快，可靠，缺点是引脚多。

我个人习惯，飞控系统里IMU一定要用SPI。为什么？因为I2C在长线传输时容易出问题，而且速度跟不上高刷新率。

来看一个典型的SPI读取代码（以MPU6050为例）：

// SPI初始化
void SPI_Init(void) {
    // 设置SCLK、MOSI、MISO、CS为GPIO模式
    // 配置SPI时钟极性、相位
    // 设置SPI速度（建议2-5MHz）
}

// 读取IMU寄存器
uint8_t SPI_ReadReg(uint8_t reg) {
    uint8_t data;
    CS_LOW();  // 片选拉低
    SPI_Transmit(reg | 0x80);  // 读操作，最高位置1
    data = SPI_Receive();      // 读取数据
    CS_HIGH(); // 片选拉高
    return data;
}

// 批量读取加速度计数据
void Read_Accel(int16_t* ax, int16_t* ay, int16_t* az) {
    uint8_t buf[6];
    CS_LOW();
    SPI_Transmit(0x3B | 0x80);  // ACCEL_XOUT_H寄存器地址
    for(int i=0; i<6; i++) {
        buf[i] = SPI_Receive();
    }
    CS_HIGH();
    *ax = (buf[0] << 8) | buf[1];
    *ay = (buf[2] << 8) | buf[3];
    *az = (buf[4] << 8) | buf[5];
}

经验之谈： SPI通信时，片选信号一定要干净。我遇到过因为片选毛刺导致数据错位的问题，后来加了10kΩ上拉电阻才解决。

3.3 低通滤波入门

传感器数据来了，但噪声很大。怎么办？滤波。

最简单的就是低通滤波。一阶低通滤波的公式：

y[n] = α * x[n] + (1-α) * y[n-1]

其中α是滤波系数，范围0~1。α越小，滤波越强，但响应越慢。

代码实现：

typedef struct {
    float alpha;  // 滤波系数
    float output; // 上一次输出
} LowPassFilter_t;

float LowPass_Update(LowPassFilter_t* filter, float input) {
    filter->output = filter->alpha * input + 
                     (1.0f - filter->alpha) * filter->output;
    return filter->output;
}

// 使用示例
LowPassFilter_t accel_x_filter = {.alpha = 0.2f, .output = 0.0f};
float filtered_ax = LowPass_Update(&accel_x_filter, raw_ax);

注意： α的选择很关键。太小了响应慢，太大了滤不掉噪声。我一般从0.1开始试，根据实际波形调整。

3.4 卡尔曼滤波入门

低通滤波太简单？那就上卡尔曼滤波。它是最优估计器，能融合加速度计和陀螺仪的数据。

卡尔曼滤波的核心思想：预测 + 更新。

对于姿态估计，我们通常用一维卡尔曼滤波：

typedef struct {
    float Q;  // 过程噪声协方差
    float R;  // 测量噪声协方差
    float P;  // 估计误差协方差
    float K;  // 卡尔曼增益
    float X;  // 状态估计值
} KalmanFilter_t;

void Kalman_Init(KalmanFilter_t* kf, float Q, float R) {
    kf->Q = Q;
    kf->R = R;
    kf->P = 1.0f;
    kf->X = 0.0f;
}

float Kalman_Update(KalmanFilter_t* 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.0f - kf->K) * kf->P;
    
    return kf->X;
}

// 融合加速度计和陀螺仪
// 加速度计提供角度测量，陀螺仪提供角速度积分
float angle = Kalman_Update(&kf, accel_angle);
// 再用陀螺仪数据做预测更新
angle += gyro_rate * dt;

核心要点： 卡尔曼滤波不是万能的。它需要调参（Q和R），而且计算量比低通滤波大。我建议新手先从互补滤波开始，再过渡到卡尔曼。

3.5 本章知识体系

下面这张图，是我自己总结的IMU数据处理流程。你照着这个思路走，基本不会错。

这张图把整个流程串起来了。从传感器原始数据，到通信协议，再到数据处理和滤波，最后输出可用的姿态信息。每一步都有坑，但每一步也都有解法。

3.6 实战建议

最后，给你几个实战建议：

先读原始数据：别急着滤波。先把加速度计和陀螺仪的原始数据读出来，用串口打印到电脑上，看看波形。
校准是第一步：陀螺仪有零偏，加速度计有刻度误差。不做校准，后面全是白费。
滤波不是越复杂越好：对于大多数四轴飞行器，互补滤波就够用了。卡尔曼滤波更适合高精度应用。
注意采样率：IMU的采样率一般1kHz左右，但你的控制环路可能只需要200Hz。别让滤波拖慢系统。

我的习惯： 调试时先用低通滤波把数据弄平滑，再逐步换成更复杂的算法。这样出了问题，你知道是滤波的问题还是其他问题。

嗯，这一章就到这里。IMU数据采集和滤波是飞控的基础，基础不牢，地动山摇。你把这些搞明白了，后面的姿态解算和控制就轻松多了。

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

📚 课程目录

第三章 传感器数据采集与滤波：IMU数据读取与处理

3.1 IMU数据读取：加速度计与陀螺仪

3.2 I2C/SPI通信协议实现

3.3 低通滤波入门

3.4 卡尔曼滤波入门

3.5 本章知识体系

3.6 实战建议

第三章传感器数据采集与滤波：IMU数据读取与处理