第3章 IMU数据预处理

第3章：IMU数据预处理——加速度计与陀螺仪的标定方法

说实话，IMU数据预处理这块，是我在飞控项目里踩坑最多的地方。很多新手拿到MPU6050或者BMI088，直接读原始数据就往姿态解算里送，结果飞机一起飞就漂得找不到北。我当年也干过这种事，后来才明白——不标定的IMU，就是一堆带毒的噪声。

这一章，咱们把IMU数据预处理这件事彻底讲透。从标定方法到滤波去噪，再到时间戳管理，每一步我都会结合实战经验来说。

3.1 加速度计与陀螺仪的标定方法

标定的目的很简单：把传感器输出的原始值，映射到真实的物理量上。加速度计输出的是m/s²，陀螺仪输出的是rad/s。但芯片出厂时，每个器件都有偏差，这就是所谓的“个体差异”。

3.1.1 六面法标定

六面法，说白了就是让IMU在六个标准姿态下采集数据。我习惯用这六个面：水平朝上、水平朝下、左侧立、右侧立、前倾、后倾。每个姿态保持静止5秒钟，采集加速度计的三轴数据。

为什么要六个面？因为加速度计在静止时，理论上只受重力作用。每个姿态下，重力在某一轴上的投影是已知的（±g），其他两轴为0。通过最小二乘法，我们可以解出三个轴的零偏和比例因子。

核心公式：

加速度计输出 = 比例因子 × 真实加速度 + 零偏

写成矩阵形式：

[Ax]   [Sx  0   0 ] [Gx]   [Bx]
[Ay] = [0   Sy  0 ] [Gy] + [By]
[Az]   [0   0   Sz] [Gz]   [Bz]

我在项目中遇到过一个问题：六面法标定后，加速度计在水平面旋转时，Z轴读数会轻微变化。后来发现是安装面的平面度不够，导致IMU没有真正水平。嗯，这里要注意——标定夹具的精度，直接影响标定结果。

实战技巧：

我个人习惯在六面法之后，再加一个“随机姿态”验证。拿着IMU随意转几圈，看看标定后的模值是否稳定在1g附近。如果偏差超过0.02g，说明标定参数有问题，需要重新来。

3.1.2 温度补偿

温度对IMU的影响，比你想象的大得多。我记得有一次做无人机高原测试，地面标定好好的，飞到3000米高空，陀螺仪零偏直接翻了一倍。后来一查，温度从25°C降到了-5°C，MEMS器件的温漂就是这么明显。

温度补偿的做法，一般分两步：

温漂数据采集：把IMU放进温箱，从-40°C到85°C，每5°C记录一次零偏值。
拟合补偿曲线：用多项式拟合温度与零偏的关系，通常是二次或三次多项式。

避坑指南：

我曾经犯过一个错误：只做了加速度计的温补，没做陀螺仪的。结果飞机在高温环境下，航向角越偏越离谱。记住，陀螺仪的温漂比加速度计更严重，因为它的零偏直接积分到角度上。

补偿公式很简单：

// 温度补偿示例代码
float temp_compensate_gyro(float raw_gyro, float temp) {
    // 二次多项式拟合
    float bias = 0.02 * temp * temp - 1.5 * temp + 35.0;
    return raw_gyro - bias;
}

3.2 零偏稳定性分析

零偏稳定性，是衡量IMU好坏的核心指标。它描述的是：在静止状态下，传感器输出的波动有多大。你想想看，如果陀螺仪静止时输出还在乱跳，那积分出来的角度肯定没法看。

我一般用Allan方差来分析零偏稳定性。这个方法不复杂，但很实用：

采集至少1小时的静态数据（采样率100Hz以上）
计算不同时间尺度下的方差
在log-log图上画出Allan曲线
曲线的最低点对应的值，就是零偏稳定性

实际案例：

我用BMI088做过测试，它的陀螺仪零偏稳定性大约在0.5°/h左右。而工业级的ADIS16470能做到0.1°/h。差距在哪？说白了就是MEMS工艺和温度补偿算法的差异。

对于飞控应用，我建议零偏稳定性至少优于5°/h。如果达不到，就需要用算法来补偿，比如在线估计零偏。

3.3 低通滤波与去噪

IMU的原始数据里，高频噪声是不可避免的。加速度计有振动噪声，陀螺仪有电子噪声。直接使用这些数据，姿态解算会抖得像筛子一样。

3.3.1 IIR滤波器

IIR滤波器，我用的最多的是巴特沃斯低通滤波器。它的特点是通带平坦，适合处理惯性数据。二阶巴特沃斯就够用了，阶数太高反而会引入相位延迟。

// 二阶IIR低通滤波器实现
typedef struct {
    float b0, b1, b2;
    float a1, a2;
    float z1, z2;
} IIRFilter;

float iir_filter(IIRFilter *f, float input) {
    float output = f->b0 * input + f->z1;
    f->z1 = f->b1 * input - f->a1 * output + f->z2;
    f->z2 = f->b2 * input - f->a2 * output;
    return output;
}

参数选择建议：

对于100Hz的IMU数据，我习惯把截止频率设在20-30Hz。太低会滤掉有用信号，太高又滤不干净噪声。你可以用示波器看看原始数据的频谱，找到噪声集中的频段，再决定截止频率。

3.3.2 FIR滤波器

FIR滤波器没有反馈，所以相位是线性的。这在某些场景下很重要，比如需要精确控制延迟的场合。但FIR的缺点是，要达到同样的滤波效果，阶数比IIR高得多。

我个人习惯：在飞控里用IIR，因为计算量小，实时性好。只有在做离线数据分析时，才会用FIR。

3.4 数据对齐与时间戳管理

这个问题，很多教程都不讲。但实际项目中，数据对齐出问题，比标定不准还致命。

为什么？因为IMU的数据是异步采集的。加速度计和陀螺仪虽然在同一颗芯片上，但它们的采样时刻可能差了几毫秒。如果你直接用这两个数据做姿态解算，相当于把不同时刻的物理量强行凑在一起，结果可想而知。

我曾经踩过的坑：

有一次做四旋翼，悬停时总是有高频抖动。查了三天，最后发现是加速度计和陀螺仪的时间戳差了2ms。2ms啊，对于100Hz的控制周期来说，就是20%的相位误差。后来加了时间戳对齐，问题立刻解决。

时间戳管理的做法：

硬件时间戳：用MCU的定时器捕获IMU的数据就绪中断，记录精确的采样时刻。
软件插值：如果两个传感器的采样时刻不同步，用线性插值把数据对齐到同一个时间点。
FIFO管理：用环形缓冲区存储带时间戳的数据，确保不会因为处理延迟而丢失数据。

// 时间戳对齐示例
typedef struct {
    uint32_t timestamp;  // 微秒级时间戳
    float accel[3];
    float gyro[3];
} IMUData;

// 环形缓冲区
IMUData imu_buffer[256];
uint8_t head, tail;

// 插入数据时，确保时间戳单调递增
void imu_push(IMUData *data) {
    imu_buffer[head] = *data;
    head = (head + 1) % 256;
}

// 读取对齐后的数据
IMUData imu_get_aligned(uint32_t target_time) {
    // 找到时间戳最接近target_time的数据
    // 用线性插值得到精确值
}

核心要点：

数据对齐的精度，决定了姿态解算的上限。我建议时间戳分辨率至少到微秒级，对齐误差控制在100微秒以内。对于100Hz的IMU，这就是1%的误差，基本可以忽略。

好了，IMU数据预处理这块，咱们就聊到这。标定、温补、滤波、对齐，每一步都是实战中必须啃下来的硬骨头。下一章，我会讲姿态解算的核心算法——四元数与互补滤波，到时候咱们再接着聊。

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

📚 课程目录