4. 姿态更新算法（一）：四元数微分方程、毕卡逼近法、龙格-库塔法（RK4）实现
姿态更新，说白了就是实时算出来「载体现在朝哪个方向」。在捷联惯导里，陀螺仪输出的是角速度，我们要靠它推算姿态。这中间的核心，就是解四元数微分方程。
我个人习惯把姿态更新比作「积分」——陀螺仪给的是角速度，积分一次得到角度。但这里有个坑：四元数不是线性系统，不能简单累加。你想想看，刚体转动本身是非线性的，四元数微分方程里还带着乘法，这就逼着我们用数值方法去逼近。
4.1 四元数微分方程
先看基本形式。四元数微分方程长这样：
dq/dt = 0.5 * q ⊗ ω其中 q 是姿态四元数，ω 是角速度向量（通常写成 [0, ωx, ωy, ωz] 的形式）。⊗ 表示四元数乘法。
展开成矩阵形式：
| dq0/dt | | 0 -ωx -ωy -ωz | | q0 |
| dq1/dt | = | ωx 0 ωz -ωy | | q1 |
| dq2/dt | | ωy -ωz 0 ωx | | q2 |
| dq3/dt | | ωz ωy -ωx 0 | | q3 |嗯，这里要注意：角速度是载体坐标系下的，也就是陀螺仪直接测量的值。我在项目中遇到过有人直接用导航系下的角速度去算，结果姿态飞了——这个坑踩过一次就记住了。
核心要点：四元数微分方程的解，本质上是求一个旋转矩阵的指数映射。角速度变化越快，对数值方法的要求越高。
4.2 毕卡逼近法
毕卡逼近法，说白了就是「泰勒展开截断」。把四元数微分方程的解写成指数形式：
q(t+Δt) = exp(0.5 * Ω * Δt) * q(t)其中 Ω 是角速度的反对称矩阵。展开指数项：
exp(0.5 * Ω * Δt) = I + (0.5 * Ω * Δt) + (0.5 * Ω * Δt)^2 / 2! + ...实际工程中，我们一般取到二阶或三阶。一阶毕卡逼近：
q(t+Δt) ≈ [I + 0.5 * Ω * Δt] * q(t)二阶毕卡逼近：
q(t+Δt) ≈ [I + 0.5 * Ω * Δt + (0.5 * Ω * Δt)^2 / 2] * q(t)我曾经在低端MCU上用过一阶毕卡，采样率500Hz，角速度变化不大时精度还行。但一旦遇到剧烈转动，误差就明显了。说白了，毕卡逼近法适合角速度变化平缓的场景，或者采样率足够高的时候。
我的经验：毕卡逼近法计算量小，适合资源受限的平台。但要注意归一化——每次更新后必须对四元数做归一化，否则误差会累积。
4.3 龙格-库塔法（RK4）
RK4是工程上最常用的数值积分方法之一。它通过四个中间步的加权平均，把截断误差压到 O(h^5)。对于四元数微分方程，RK4的实现如下：
// 四元数微分方程函数
void quaternion_derivative(const float q[4], const float w[3], float dq[4]) {
dq[0] = 0.5f * (-q[1]*w[0] - q[2]*w[1] - q[3]*w[2]);
dq[1] = 0.5f * ( q[0]*w[0] + q[2]*w[2] - q[3]*w[1]);
dq[2] = 0.5f * ( q[0]*w[1] - q[1]*w[2] + q[3]*w[0]);
dq[3] = 0.5f * ( q[0]*w[2] + q[1]*w[1] - q[2]*w[0]);
}
// RK4 姿态更新
void rk4_update(float q[4], const float w[3], float dt) {
float k1[4], k2[4], k3[4], k4[4];
float q_temp[4];
float w_mid[3];
// k1
quaternion_derivative(q, w, k1);
// k2: 使用中点角速度
w_mid[0] = w[0]; // 实际应用中角速度可能变化
w_mid[1] = w[1]; // 这里假设采样间隔内角速度恒定
w_mid[2] = w[2];
q_temp[0] = q[0] + 0.5f * dt * k1[0];
q_temp[1] = q[1] + 0.5f * dt * k1[1];
q_temp[2] = q[2] + 0.5f * dt * k1[2];
q_temp[3] = q[3] + 0.5f * dt * k1[3];
quaternion_derivative(q_temp, w_mid, k2);
// k3
q_temp[0] = q[0] + 0.5f * dt * k2[0];
q_temp[1] = q[1] + 0.5f * dt * k2[1];
q_temp[2] = q[2] + 0.5f * dt * k2[2];
q_temp[3] = q[3] + 0.5f * dt * k2[3];
quaternion_derivative(q_temp, w_mid, k3);
// k4
q_temp[0] = q[0] + dt * k3[0];
q_temp[1] = q[1] + dt * k3[1];
q_temp[2] = q[2] + dt * k3[2];
q_temp[3] = q[3] + dt * k3[3];
quaternion_derivative(q_temp, w, k4);
// 加权平均
q[0] += (dt / 6.0f) * (k1[0] + 2.0f*k2[0] + 2.0f*k3[0] + k4[0]);
q[1] += (dt / 6.0f) * (k1[1] + 2.0f*k2[1] + 2.0f*k3[1] + k4[1]);
q[2] += (dt / 6.0f) * (k1[2] + 2.0f*k2[2] + 2.0f*k3[2] + k4[2]);
q[3] += (dt / 6.0f) * (k1[3] + 2.0f*k2[3] + 2.0f*k3[3] + k4[3]);
// 归一化
float norm = sqrtf(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
q[0] /= norm;
q[1] /= norm;
q[2] /= norm;
q[3] /= norm;
}注意：RK4的计算量是毕卡逼近法的4倍左右。在嵌入式平台上，如果采样率是1000Hz，每个姿态更新周期只有1ms，RK4可能吃紧。我建议先评估CPU负载，再决定用哪种方法。
4.4 三种方法对比
| 方法 | 精度 | 计算量 | 适用场景 |
|---|---|---|---|
| 一阶毕卡 | O(Δt^2) | 低 | 高采样率、低动态 |
| 二阶毕卡 | O(Δt^3) | 中 | 中等动态、资源受限 |
| RK4 | O(Δt^5) | 高 | 高动态、精度优先 |
我个人的选择原则：如果MCU主频在100MHz以上，采样率500Hz以内，我直接上RK4。如果资源紧张，比如用STM32F1跑1000Hz，我会用二阶毕卡，配合高精度定时器。
4.5 知识体系结构图
这张图把姿态更新的核心脉络理清楚了。从四元数微分方程出发，三个分支对应不同的数值解法。毕卡逼近法适合资源受限场景，RK4适合高精度需求。底部那四个关键考量，是我在实际项目中反复踩坑后总结出来的。
避坑指南：我曾经在无人机项目里用RK4，采样率400Hz，一切正常。后来换到200Hz，姿态开始漂移。查了半天发现是角速度变化太快，RK4的中间步假设角速度恒定已经不成立了。解决办法是提高采样率，或者在陀螺仪数据之间做插值。
好了，这一章就到这里。姿态更新是惯导算法的基石，四元数微分方程的各种解法各有优劣。毕卡逼近法简单高效，RK4精度更高。实际选型时，一定要结合你的硬件平台和动态场景来权衡。