一阶龙格-库塔积分：角速度驱动的四元数更新

各位同学，今天我们来聊聊陀螺仪积分里最基础、也最常用的一招——一阶龙格-库塔积分。说白了，就是怎么把陀螺仪测到的角速度，变成我们想要的姿态四元数。

我在项目里见过不少新手，上来就写四元数更新代码，结果飞着飞着姿态就飘了。嗯，这里要注意，问题往往就出在积分方法上。

为什么需要离散积分？

陀螺仪输出的是角速度，单位是 °/s 或者 rad/s。但我们要的是姿态，是随时间累积的结果。你想想看，MCU 是数字系统，只能每隔 Δt 读一次数据。所以我们必须把连续微分方程，变成离散的差分方程。

四元数的微分方程长这样：

dq/dt = 0.5 * q ⊗ ω

其中 ω 是角速度四元数，形式为 [0, ωx, ωy, ωz]。这个方程描述的是：四元数随时间的变化率，等于当前四元数乘以角速度的一半。

那怎么离散化呢？最简单的办法就是——一阶龙格-库塔法，也就是欧拉法的升级版。

一阶龙格-库塔法的核心思想

一阶 RK 法的思路很直白：

1. 在当前时刻，用微分方程算出斜率 k1
2. 用这个斜率乘以步长 Δt，得到增量
3. 把增量加到当前值上

用公式表达就是：

q(t+Δt) = q(t) + Δt * f(q(t), ω(t))

其中 f(q, ω) = 0.5 * q ⊗ ω。

我个人习惯把这个过程叫做「向前推一步」。你想想看，这不就是高中物理里的匀加速运动公式吗？只不过这里推的是四元数，不是位移。

四元数更新的具体实现

好了，理论讲完，咱们直接上代码。这是我个人习惯的写法，你可以在自己的项目里直接套用：

// 四元数结构体
typedef struct {
    float w;
    float x;
    float y;
    float z;
} Quaternion;

// 一阶龙格-库塔法更新四元数
void quaternion_update_rk1(Quaternion *q, float gx, float gy, float gz, float dt) {
    // 角速度四元数
    float omega_w = 0.0f;
    float omega_x = gx;
    float omega_y = gy;
    float omega_z = gz;

    // 计算 q ⊗ ω
    float qw = q->w;
    float qx = q->x;
    float qy = q->y;
    float qz = q->z;

    float dqw = 0.5f * (-qx * omega_x - qy * omega_y - qz * omega_z);
    float dqx = 0.5f * ( qw * omega_x + qy * omega_z - qz * omega_y);
    float dqy = 0.5f * ( qw * omega_y + qz * omega_x - qx * omega_z);
    float dqz = 0.5f * ( qw * omega_z + qx * omega_y - qy * omega_x);

    // 一阶积分：q_new = q_old + dt * dq
    q->w += dqw * dt;
    q->x += dqx * dt;
    q->y += dqy * dt;
    q->z += dqz * dt;

    // 归一化，防止累积误差
    float norm = sqrt(q->w*q->w + q->x*q->x + q->y*q->y + q->z*q->z);
    if (norm > 0.0f) {
        q->w /= norm;
        q->x /= norm;
        q->y /= norm;
        q->z /= norm;
    }
}

一阶 RK 法的局限性

说实话，一阶 RK 法虽然简单，但精度有限。为什么？因为它假设在 Δt 时间内，角速度是恒定的。但实际运动中，角速度是连续变化的。

我举个例子：

• 如果陀螺仪采样率是 100Hz，Δt=10ms
• 在快速旋转时（比如 300°/s），10ms 内角度变化了 3°
• 一阶 RK 法用起始时刻的角速度来近似整个 10ms 的变化
• 这就会引入截断误差

那什么时候用一阶 RK 法比较合适呢？

场景	推荐步长	说明
慢速运动（< 50°/s）	10ms	误差可忽略
中等运动（50~200°/s）	5ms	需要适当补偿
快速运动（> 200°/s）	≤ 2ms	建议用二阶或四阶 RK

核心逻辑流程图

下面我用一张 SVG 图，把一阶 RK 法更新四元数的流程串起来。你一看就明白了：

总结一下

一阶龙格-库塔积分，说白了就是「用当前斜率外推一步」。它简单、高效，适合大多数嵌入式场景。但你要记住它的局限：精度有限，不适合快速旋转。

我在实际项目中，一般先用一阶 RK 法快速验证算法，等系统稳定了，再根据需求升级到二阶或四阶 RK 法。嗯，这个我们后面章节会详细讲。

最后送你一句话：四元数更新，归一化是底线，步长稳定是关键。记住了，你的姿态就不会飘。