📚 课程目录第4章 IMU数据预处理

4、IMU数据预处理：滤波去噪（低通、卡尔曼、互补滤波）、单位换算、坐标系对齐

IMU（惯性测量单元）是手势控制机器人的“感知核心”，通常包含三轴加速度计和三轴陀螺仪。然而，原始IMU数据并不能直接用于手势识别——加速度计对振动和运动加速度极其敏感，陀螺仪则存在零偏漂移和积分累积误差。如果跳过预处理环节，分类模型将接收到大量噪声和失真特征，导致手势识别准确率大幅下降。本章围绕滤波去噪、单位换算和坐标系对齐三大核心任务，系统讲解从原始寄存器数据到可用姿态特征的全链路处理方法。

4.1 滤波去噪：从时域噪声到可用信号

IMU原始信号中的噪声来源复杂：传感器自身的热噪声、机械振动引起的高频抖动、运动加速度与重力加速度的混叠、以及ADC量化误差。手势识别关注的是低频姿态变化（通常0.5Hz~10Hz），而振动噪声往往集中在50Hz以上。因此，滤波的核心目标是在保留手势有效频段的同时，最大限度抑制高频干扰。

4.1.1 低通滤波：最基础的时域平滑

一阶低通滤波器（LPF）是最简单有效的去噪手段，其差分方程如下：

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

其中，α = Δt / (RC + Δt)，Δt为采样周期，RC为滤波器时间常数。α越小，平滑效果越强，但滞后也越大。对于手势识别场景，采样率通常为50~100Hz，截止频率设为5~10Hz较为合适。实际调试时，可通过以下步骤确定α：

采集静止状态下的IMU数据，观察噪声幅值；
采集快速挥动手臂的数据，观察信号峰值；
在MATLAB或Python中模拟不同α值，对比滤波前后的信噪比；
选择既能有效抑制静止噪声、又不明显削减手势峰值的α。

经验值参考：当采样率100Hz、截止频率5Hz时，α≈0.24；若手势动作较慢（如画圈），可适当降低截止频率至3Hz，α≈0.16。注意α过小会导致手势起始段延迟明显，影响实时性。

4.1.2 卡尔曼滤波：最优估计的工程实践

卡尔曼滤波通过建立系统状态方程和观测方程，在噪声统计特性已知的前提下，给出最小均方误差意义下的最优估计。对于IMU姿态估计，常将姿态角作为状态量，陀螺仪角速度作为控制输入，加速度计解算的姿态角作为观测值。其核心迭代公式包括预测和更新两个阶段：

预测：x̂⁻ = A·x̂ + B·u，P⁻ = A·P·Aᵀ + Q
更新：K = P⁻·Hᵀ·(H·P⁻·Hᵀ + R)⁻¹
      x̂ = x̂⁻ + K·(z - H·x̂⁻)
      P = (I - K·H)·P⁻

在实际嵌入式系统中，直接使用完整卡尔曼滤波计算量较大。工程上常采用简化方案：将加速度计和陀螺仪融合为二维状态（角度和角速度偏置），通过调节过程噪声协方差矩阵Q和观测噪声协方差矩阵R来平衡响应速度与平滑度。Q越大，滤波器越信任观测值（响应快但噪声大）；R越大，滤波器越信任预测值（平滑但滞后）。

4.1.3 互补滤波：轻量级融合方案

互补滤波利用加速度计低频准确、陀螺仪高频准确的频域互补特性，通过一个高通滤波器和一个低通滤波器实现数据融合。其离散形式为：

angle = α · (angle + gyro·Δt) + (1 - α) · accel_angle

其中α通常取0.95~0.98。互补滤波计算量远小于卡尔曼滤波，且无需精确的噪声统计模型，非常适合STM32等资源受限的嵌入式平台。下表对比了三种滤波方法的适用场景：

滤波方法	计算量	延迟	适用场景	典型应用
一阶低通	极低	中等	静态或慢速运动	静止姿态测量
卡尔曼	高	低	动态剧烈、精度要求高	无人机姿态解算
互补滤波	低	低	动态适中、实时性要求高	手势识别、平衡车

4.2 单位换算：从寄存器原始值到物理量

IMU传感器（如MPU6050、ICM20602）的寄存器输出为16位有符号整数（LSB），必须根据量程配置换算为物理单位。加速度计量程可选±2g、±4g、±8g、±16g，陀螺仪量程可选±250°/s、±500°/s、±1000°/s、±2000°/s。换算公式为：

物理值 = 原始值 / 灵敏度系数

灵敏度系数（LSB/g或LSB/(°/s)）由量程决定。以MPU6050为例：

量程配置	加速度计灵敏度	陀螺仪灵敏度
±2g / ±250°/s	16384 LSB/g	131 LSB/(°/s)
±4g / ±500°/s	8192 LSB/g	65.5 LSB/(°/s)
±8g / ±1000°/s	4096 LSB/g	32.8 LSB/(°/s)
±16g / ±2000°/s	2048 LSB/g	16.4 LSB/(°/s)

常见错误：部分开发者直接使用默认量程（±2g/±250°/s）而不检查配置寄存器，导致换算系数错误。务必在初始化代码中明确写入量程配置，并读取实际生效值进行换算。

4.3 坐标系对齐：统一参考框架

IMU芯片的坐标系定义与机器人机体坐标系往往不一致。例如，MPU6050的X轴指向芯片正面右侧，Y轴指向上方，Z轴垂直纸面向外；而机器人可能定义X轴为前进方向，Z轴为竖直向上。若不做对齐，手势特征将出现方向错乱，分类模型无法学习到正确的运动模式。

4.3.1 轴序映射与方向校正

坐标系对齐分为两步：轴序映射（将传感器轴重新排列为机体轴顺序）和方向校正（对反向轴取负号）。假设传感器原始三轴为(x_s, y_s, z_s)，机体坐标系为(x_b, y_b, z_b)，则映射关系可表示为：

[x_b]   [m11 m12 m13] [x_s]
[y_b] = [m21 m22 m23] [y_s]
[z_b]   [m31 m32 m33] [z_s]

其中m矩阵为置换矩阵（每行每列仅有一个1或-1）。实际工程中，可通过以下流程确定映射矩阵：

将机器人平放，记录加速度计三轴读数，确定重力方向对应的轴；
将机器人绕各轴旋转90°，观察读数变化，确定正方向；
根据机器人定义的前/右/上方向，建立映射表；
编写代码实现映射，并用静态测试验证。

4.3.2 案例：手势控制小车的坐标系标定

某手势控制小车项目使用MPU6050，传感器安装时Y轴朝前，Z轴朝上。但小车机体坐标系定义为X轴朝前、Z轴朝上。初始测试时，手势“前推”被识别为“左转”。分析发现：传感器Y轴对应机体X轴，且传感器Y轴正方向与机体X轴正方向一致，因此映射关系为：

x_b = y_s
y_b = -x_s   // 传感器X轴朝右，机体Y轴朝左
z_b = z_s

修改代码后，重新采集数据验证：前推手势的加速度计X轴分量显著增大，左挥手势的Y轴分量变化明显，识别准确率从68%提升至92%。此案例说明，坐标系对齐错误会导致特征方向错乱，且难以通过增加训练数据弥补。

4.4 预处理流程整合与常见误区

完整的IMU预处理流程应遵循“先换算、再滤波、后对齐”的顺序。先进行单位换算，确保后续滤波和特征计算使用统一的物理单位；滤波去除高频噪声；最后进行坐标系对齐，使特征向量与机器人运动方向一致。以下为推荐的代码结构：

void imu_preprocess(float accel_raw[3], float gyro_raw[3]) {
    // 1. 单位换算
    accel_g[0] = accel_raw[0] / ACCEL_SENSITIVITY;
    gyro_dps[0] = gyro_raw[0] / GYRO_SENSITIVITY;
    // 2. 低通滤波（以X轴为例）
    accel_filtered[0] = alpha * accel_g[0] + (1 - alpha) * accel_filtered[0];
    // 3. 坐标系对齐
    accel_body[0] = accel_filtered[1];  // 轴序映射
    accel_body[1] = -accel_filtered[0]; // 方向校正
    accel_body[2] = accel_filtered[2];
}

常见误区总结：
滤波过度：α过小导致手势特征被严重平滑，峰值和过零率失真，分类模型无法区分相似手势。
忽略零偏校准：陀螺仪静止时输出不为零，长时间积分会产生累积漂移。应在系统启动时采集100~200个样本求平均，作为零偏补偿值。
坐标系对齐不完整：只做轴序映射而忽略方向校正，或未考虑传感器安装倾斜角（如非水平安装），导致特征向量存在固定偏差。
滤波顺序错误：先对齐再滤波会导致噪声在坐标变换中耦合，增加滤波难度。

在实际项目调试中，建议使用串口或蓝牙将预处理后的数据实时上传至上位机，通过波形对比滤波前后效果，并验证坐标系对齐的正确性。只有确保预处理环节可靠，后续的窗口特征提取和分类模型训练才有意义。预处理质量直接决定了手势识别系统的上限，值得投入充分的调试时间。