高精度 IMU 姿态解算：基于 ICM-20602 的四元数与梯度下降滤波实现

1. 姿态解算概述#

姿态解算（Attitude Algorithm）是平衡车、无人机及穿戴式设备的核心。ICM-20602 是一款高性能的 6 轴运动跟踪器件，包含三轴陀螺仪和三轴加速度计。

单纯依靠陀螺仪积分会产生累积误差，而单纯依靠加速度计则在高频振动下极其不稳定。本程序采用四元数算法融合两者数据，并引入**梯度下降法（Steepest Descent）**进行预处理滤波，以获得平滑且精准的欧拉角（Pitch, Roll, Yaw）。

2. 算法核心步骤#

数据采集与去零偏：校准陀螺仪静态漂移。
梯度下降滤波：针对陀螺仪噪声进行最速下降法优化，寻找最优数据点。
四元数更新：利用角速度对四元数进行微分方程积分。
互补纠正：利用加速度计重力向量纠正陀螺仪的积分漂移。
欧拉角转换：将四元数转换为直观的角度值。

3. 核心代码实现#

3.1 零偏校正 (Gyro Offset)#

在系统启动时保持静止，采集 500 次数据求平均值，这是后续积分不漂移的基础。

1
#define OFFSET_COUNT 500
2
void IMU_offset(GYRO_VAR *gyro_var) {
3
    int64_t temp[6] = {0};
4
    for (uint32_t i = 0; i < OFFSET_COUNT; i++) {
5
        icm20602_get_data(gyro_var);
6
        systick_delay_ms(2);
7
        temp[0] += gyro_var->orig.acc.x; // ... 累加六轴数据
8
        // ... (省略重复累加代码)
9
    }
10
    gyro_var->fiter.gyro_offset.x = temp[3] / OFFSET_COUNT;
11
    gyro_var->fiter.offset_flag = 1; // 校准完成标志
12
}

3.2 快速平方根算法 (InvSqrt)#

姿态解算中需要大量的归一化计算，使用经典的 0x5f3759df 魔法数算法可极大节省单片机算力。

1
float my_sqrt(float number) {
2
    long i;
3
    float x, y;
4
    x = number * 0.5F;
5
    y = number;
6
    i = *(long*)&y;
7
    i = 0x5f3759df - (i >> 1); // 经典的雷神之锤快速平方根倒数算法
8
    y = *(float*)&i;
9
    y = y * (1.5F - (x * y * y)); // 牛顿迭代法
10
    return number * y;
11
}

3.3 四元数姿态融合 (Mahony 简化版)#

通过加速度计补偿陀螺仪的积分误差，确保静态工况下的绝对精度。

1
void Q_IMUupdata(GYRO_VAR *gyro_var) {
2
    static float q0 = 1, q1 = 0, q2 = 0, q3 = 0;
3
    float acc_length;
4
    _xyz_f_st acc_norm, vec_err;
5

6
    // 1. 加速度归一化
7
    acc_length = my_sqrt(pow(acc.x,2) + pow(acc.y,2) + pow(acc.z,2));
8
    acc_norm.x = acc.x / acc_length;
9
    // ... (处理 y, z)
10

11
    // 2. 提取四元数中的重力分量 (z轴向量)
12
    _xyz_f_st z_vec = { 2*(q1*q3 - q0*q2), 2*(q2*q3 + q0*q1), 1-2*(q1*q1 + q2*q2) };
13

14
    // 3. 向量外积计算误差（重力向量与加速度计向量的偏差）
15
    vec_err.x = acc_norm.y * z_vec.z - z_vec.y * acc_norm.z;
16
    // ... (处理 y, z)
17

18
    // 4. 利用 PI 控制器融合误差并更新四元数
19
    // d_angle = (Gyro + Kp * vec_err + Ki * vec_err_i) * dT / 2
20
    // ... (更新四元数 q0-q3)
21

22
    // 5. 欧拉角转换
23
    gyro_var->euler.pit  = asin(2 * q1 * q3 - 2 * q0 * q2) * 57.3f;
24
    gyro_var->euler.roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1) * 57.3f;
25
}

4. 调试经验总结#

采样频率：姿态解算的周期（dT）必须恒定。建议放在定时器中断中运行（如 200Hz 或 500Hz）。
加速度计限幅：在设备剧烈震动时，加速度计数据不可信，应通过 limit_filter 减小其权重，防止干扰姿态。
安装方向：注意传感器的坐标系（）与设备坐标系的一致性，否则 atan2 计算出的 Roll 和 Pitch 会反向。

5. 开源建议#

本项目代码已在 STM32F4/F1 系列及 K60 开发板上验证通过。如果您在调试过程中发现 Yaw 轴漂移严重，请检查是否引入了磁力计（HMC5883L 等）进行航向校正。