四元数运动学方程：从微分到工程实现
好，我们进入第四章。这一章，说白了就是回答一个问题：陀螺仪测出了角速度，怎么用它来更新四元数？
我在做第一个飞控项目时，就卡在这里。角速度数据哗哗地来，但姿态就是飘得离谱。后来才发现，是四元数更新的算法没选对。嗯，今天我们就把它彻底讲透。
4.1 四元数微分方程的推导
先看数学基础。四元数描述的是旋转，而旋转是随时间变化的。我们要找的就是这个变化率——四元数对时间的导数。
假设一个刚体在旋转，它的姿态用四元数 q 表示。旋转角速度是 ω。那么，四元数随时间的变化满足：
dq/dt = 0.5 * q ⊗ ω这里的 ⊗ 是四元数乘法。ω 要写成纯四元数形式：ω = [0, ωx, ωy, ωz]。
为什么会这样？我简单推导一下。
四元数旋转公式是：q(t) = q(0) ⊗ exp(0.5 * ω * t)。对时间求导，自然就得到上面的微分方程。你想想看，这和复数的指数旋转其实是一个道理，只是从二维扩展到了四维。
核心公式：四元数运动学方程
dq/dt = 0.5 * Ω(ω) * q
其中 Ω(ω) 是角速度的矩阵形式：
Ω(ω) = [ 0 -ωx -ωy -ωz
ωx 0 ωz -ωy
ωy -ωz 0 ωx
ωz ωy -ωx 0 ]4.2 角速度与四元数更新的关系
有了微分方程，下一步就是离散化。陀螺仪输出的是离散的角速度采样，我们要用这些采样来更新四元数。
这里有个关键点：角速度是瞬时量，而四元数更新是积分过程。说白了，就是用一小段时间内的角速度，去推算姿态的变化量。
我习惯把这个问题分成两步看：
- 角速度积分：把角速度乘上时间步长 Δt，得到角度增量 Δθ
- 四元数更新：用角度增量去旋转当前的四元数
数学上，离散更新公式是：
q_{k+1} = q_k ⊗ Δq(Δθ)其中 Δq(Δθ) 是角度增量对应的四元数：
Δq = [cos(|Δθ|/2), (Δθ/|Δθ|) * sin(|Δθ|/2)]我的经验：在实际项目中，Δt 通常取陀螺仪的采样周期。比如 100Hz 的陀螺仪，Δt = 0.01s。但要注意，如果角速度变化剧烈，这个 Δt 可能不够小，需要做插值或使用更高阶的积分方法。
4.3 毕卡逼近法求解
直接解四元数微分方程，其实有解析解。但工程上我们更常用毕卡逼近法（Picard iteration）。
毕卡法的思路很简单：把微分方程写成积分形式，然后迭代逼近。对于四元数微分方程：
q(t) = q(0) + ∫₀ᵗ 0.5 * Ω(ω) * q(τ) dτ第一次近似：q₁(t) = q(0) + 0.5 * Ω(ω) * q(0) * t
第二次近似：q₂(t) = q(0) + 0.5 * Ω(ω) * q₁(t) * t
... 以此类推
在实际工程中，我们通常只取前两项或前三项。为什么？因为高阶项对精度提升有限，但计算量翻倍。我做过测试，对于大多数无人机应用，二阶毕卡逼近已经足够。
二阶毕卡逼近公式：
q_{k+1} = [I * (1 - |Δθ|²/8) + 0.5 * Ω(Δθ)] * q_k其中 Δθ = ω * Δt，I 是 4x4 单位矩阵。
4.4 工程实现中的坑与对策
讲完理论，说说我踩过的坑。
我曾经犯过的错：
- 忘记归一化：四元数更新后，由于数值误差，模长会偏离1。如果不做归一化，姿态会慢慢漂移。我有个项目因此飞了30秒就翻了。
- 角速度单位搞错：陀螺仪输出通常是 deg/s，但公式里要用 rad/s。这个坑我至少见过三次。
- 时间步长不匹配：如果陀螺仪采样不稳定，直接用固定 Δt 会引入误差。建议用硬件定时器或时间戳。
下面是我常用的 C 代码实现，你可以参考：
// 四元数更新函数（二阶毕卡法）
void quaternion_update(quat_t *q, float gx, float gy, float gz, float dt) {
// 角速度转弧度，计算角度增量
float dtheta_x = gx * DEG2RAD * dt;
float dtheta_y = gy * DEG2RAD * dt;
float dtheta_z = gz * DEG2RAD * dt;
float dtheta_sq = dtheta_x*dtheta_x + dtheta_y*dtheta_y + dtheta_z*dtheta_z;
// 二阶毕卡逼近
float q0 = q->w;
float q1 = q->x;
float q2 = q->y;
float q3 = q->z;
float half_dt = 0.5f * dt;
float one_minus_theta_sq_8 = 1.0f - dtheta_sq / 8.0f;
q->w = one_minus_theta_sq_8 * q0 + (-dtheta_x*q1 - dtheta_y*q2 - dtheta_z*q3) * half_dt;
q->x = one_minus_theta_sq_8 * q1 + ( dtheta_x*q0 - dtheta_y*q3 + dtheta_z*q2) * half_dt;
q->y = one_minus_theta_sq_8 * q2 + ( dtheta_y*q0 + dtheta_x*q3 - dtheta_z*q1) * half_dt;
q->z = one_minus_theta_sq_8 * q3 + ( dtheta_z*q0 - dtheta_x*q2 + dtheta_y*q1) * half_dt;
// 归一化
float norm = sqrt(q->w*q->w + q->x*q->x + q->y*q->y + q->z*q->z);
q->w /= norm;
q->x /= norm;
q->y /= norm;
q->z /= norm;
}我个人的建议：在嵌入式平台上，如果计算资源紧张，可以用一阶毕卡法（只取第一项）。虽然精度稍差，但胜在速度快。对于大多数消费级产品，一阶已经够用。
4.5 本章知识体系
下面这张图，是我梳理的本章核心逻辑。你看一遍，应该就能把整个流程串起来。
嗯，这一章的内容就到这里。四元数微分方程是姿态解算的发动机，毕卡逼近法就是那个点火开关。你把这个逻辑理清了，后面的卡尔曼滤波、互补滤波学起来会轻松很多。
公众号:蓝海资料掘金营，微信deep3321