第 10 章：Madgwick AHRS 姿态解算

📂 示例代码：10_ahrs

10.1 知识要点

• 四元数表示姿态的优势（无万向锁，计算高效）
• Madgwick 6-DOF 算法原理（梯度下降优化）
• β 参数对收敛速度与噪声抑制的影响
• 四元数到欧拉角（Roll/Pitch/Yaw）的转换公式
• IMU 加热电阻（GPIO46）：通过恒温控制消除温度漂移对陀螺仪偏置的影响
• 硬件前置条件：加热功能需要 V_IN（9–26V）供电，仅 USB 供电时功率不足

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²))

IMU 加热器工作原理

QMI8658 的陀螺仪零偏随温度变化显著。OSRCORE 板载一颗加热电阻，由 GPIO46 的 MOS 管控制，将 IMU 管芯温度稳定在约 56°C，从根本上消除温漂。

供电要求：加热电阻通过板载 TPS54540 降压后的 5V 供电，电源来自 V_IN（9–26V）。仅使用 USB 供电时，5V 轨电流不足，无法达到满功率加热，温控效果不可保证。

GPIO46 特殊处理：GPIO46 是 ESP32-S3 的 boot strapping 引脚，gpio_config() 会静默跳过输出驱动使能。必须直接写 GPIO 寄存器：

#include "soc/gpio_reg.h"
#include "soc/io_mux_reg.h"
#define HEATER_PIN_BIT (1UL << (46 - 32))

// 初始化：配置为输出
PIN_FUNC_SELECT(IO_MUX_GPIO46_REG, FUNC_GPIO46_GPIO46);
REG_WRITE(GPIO_ENABLE1_W1TS_REG, HEATER_PIN_BIT);

// 开启加热
REG_WRITE(GPIO_OUT1_W1TS_REG, HEATER_PIN_BIT);

// 关闭加热
REG_WRITE(GPIO_OUT1_W1TC_REG, HEATER_PIN_BIT);

两阶段控制：

1. 预热阶段：目标温度 56°C，当温度低于 (56 - 8) = 48°C 时全功率加热
2. 调节阶段：bang-bang 控制，±1°C 滞回，维持在 56°C 附近

里程碑：

• 38°C warm：陀螺偏置开始收敛，可以启动 AHRS
• 54°C stable：热稳态，偏置最优，建议此时重启一次陀螺校准
• 56°C target：bang-bang 调节中心

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));
    }
}

启动顺序

加热器需要 IMU 任务持续喂入温度数据，因此必须先启动 IMU 任务，再等待 warm 里程碑：

imu_heater_init(56.0f);

// 先启动 IMU 任务，让它持续喂温度给加热器
xTaskCreatePinnedToCore(task_imu, "imu", 4096, NULL, 5, NULL, 1);

// 等待 38°C warm 里程碑
printf("Waiting for IMU heater warm (38C)...\n");
while (!imu_heater_warm()) {
    vTaskDelay(pdMS_TO_TICKS(500));
}
printf("Heater warm, starting AHRS\n");

10.5 课程总结

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

---