第 10 章：Madgwick AHRS 姿态解算

10.1 知识要点

• 四元数表示姿态的优势（无万向锁，计算高效）
• Madgwick 6-DOF 算法原理（梯度下降优化）
• β 参数对收敛速度与噪声抑制的影响
• 四元数到欧拉角（Roll/Pitch/Yaw）的转换公式

10.2 课程内容

姿态航向参考系统（AHRS）将 IMU 的加速度计和陀螺仪数据融合，输出稳定的姿态估计。Madgwick 算法是一种计算效率高、适合嵌入式系统的互补滤波算法。OSRCORE 以 200 Hz（5 ms 周期）运行 AHRS，为平衡控制等应用提供实时姿态数据。

10.3 基础学习

四元数表示

四元数 q = [q0, q1, q2, q3]（标量 + 向量部分）可以无奇异点地表示三维旋转。初始值为单位四元数 [1, 0, 0, 0]（无旋转）。

Madgwick 算法原理

6-DOF 模式（无磁力计）：

1. 陀螺仪积分：用角速度更新四元数（预测步）
2. 加速度计修正：通过梯度下降最小化重力向量与测量值的误差（修正步）
3. β 参数控制修正步长：β 越大收敛越快但噪声越大，β=0.1 是典型平衡点

快速反平方根优化

Madgwick 算法需要频繁归一化四元数，使用 Quake III 的快速反平方根（invSqrt）可以显著减少计算量：

static float invSqrt(float x) {
    float halfx = 0.5f * x;
    float y = x;
    long i = *(long*)&y;
    i = 0x5f3759df - (i >> 1);
    y = *(float*)&i;
    y = y * (1.5f - (halfx * y * y));
    return y;
}

四元数到欧拉角

Roll  = atan2(2(q0q1 + q2q3), 1 - 2(q1² + q2²))
Pitch = asin(2(q0q2 - q3q1))
Yaw   = atan2(2(q0q3 + q1q2), 1 - 2(q2² + q3²))

10.4 程序学习

初始化与更新：

madgwick_t g_ahrs;
madgwick_init(&g_ahrs, 0.1f);  // beta = 0.1

// 每 5ms 调用一次（200 Hz）
qmi8658_data_t d;
if (qmi8658_read(&d)) {
    madgwick_update_imu(&g_ahrs,
        d.gx, d.gy, d.gz,   // 陀螺仪 deg/s
        d.ax, d.ay, d.az);  // 加速度计 g
}

读取欧拉角：

float roll, pitch, yaw;
madgwick_get_euler(&g_ahrs, &roll, &pitch, &yaw);
printf("R=%.1f P=%.1f Y=%.1f\n", roll, pitch, yaw);

IMU 任务（Core 1，P5，5ms）：

static void task_imu(void *arg)
{
    qmi8658_data_t d;
    int print_cnt = 0;
    while (1) {
        if (qmi8658_read(&d)) {
            madgwick_update_imu(&g_ahrs, d.gx, d.gy, d.gz, d.ax, d.ay, d.az);
            if (++print_cnt >= 20) {  // 每 100ms 打印
                print_cnt = 0;
                float roll, pitch, yaw;
                madgwick_get_euler(&g_ahrs, &roll, &pitch, &yaw);
                printf("q: %.4f %.4f %.4f %.4f  R=%.1f P=%.1f Y=%.1f\n",
                       g_ahrs.q0, g_ahrs.q1, g_ahrs.q2, g_ahrs.q3,
                       roll, pitch, yaw);
            }
        }
        vTaskDelay(pdMS_TO_TICKS(5));
    }
}

10.5 课程总结

本章掌握了 Madgwick AHRS 算法的原理和实现，学会了四元数姿态表示和欧拉角转换。200 Hz 的更新频率为动态运动提供了足够的时间分辨率，β=0.1 在静态精度和动态响应之间取得了良好平衡。

---