3. 基础数学知识回顾（二）：四元数基础、四元数与旋转、四元数更新算法、等效旋转矢量
各位同学，欢迎来到数学基础的第二部分。上一节我们聊了聊方向余弦矩阵，今天的主角是四元数。说实话，我刚入行时觉得四元数这东西神神叨叨的，四个数就能描述三维旋转？后来在项目里被方向余弦矩阵的“万向锁”折磨过一次，才真正体会到四元数的好。
这一节，我会把四元数掰开了讲。从定义到旋转，再到更新算法，最后引出等效旋转矢量。嗯，这些都是惯导算法里的硬通货。
3.1 四元数基础：一个超复数的故事
四元数，说白了就是一个超复数。普通复数有一个实部和一个虚部，四元数有一个实部和三个虚部。我习惯把它写成这样：
q = q0 + q1*i + q2*j + q3*k其中 i、j、k 是三个虚数单位。它们有个很特别的乘法规则：
- i² = j² = k² = -1
- i*j = k, j*i = -k
- j*k = i, k*j = -i
- k*i = j, i*k = -j
你想想看，这跟向量叉积是不是很像？没错，四元数的虚部天然就带有旋转的基因。
我们通常把四元数写成标量+向量的形式：
q = [q0, q1, q2, q3] = [s, v]其中 s 是标量部分，v 是向量部分。这个表示法在后面的旋转推导中会非常方便。
3.2 四元数与旋转：为什么它能避免万向锁？
为什么四元数能描述旋转？其实原理很简单。一个单位四元数（模长为1）可以表示三维空间中的任意旋转。具体来说：
绕单位轴 u = [ux, uy, uz] 旋转角度 θ，对应的四元数是：
q = [cos(θ/2), ux*sin(θ/2), uy*sin(θ/2), uz*sin(θ/2)]注意这里用的是半角。为什么是半角？我当年也困惑过。其实是因为四元数乘法对应旋转的复合，半角能让两次旋转的叠加正好对应角度相加。
用四元数旋转一个向量 v，步骤是：
- 把 v 写成纯四元数：p = [0, vx, vy, vz]
- 计算：p' = q * p * q⁻¹
- p' 的虚部就是旋转后的向量
这里 q⁻¹ 是 q 的逆。对于单位四元数，逆就是共轭：q⁻¹ = [q0, -q1, -q2, -q3]。
3.3 四元数更新算法：时间在走，姿态在变
实际应用中，陀螺仪输出的是角速度，我们需要用角速度来更新四元数。这就是四元数更新算法要干的事。
四元数的微分方程是：
dq/dt = 0.5 * q * ω其中 ω 是角速度四元数，形式为 [0, ωx, ωy, ωz]。
离散化之后，常用的更新公式是：
q(t+Δt) = q(t) * [cos(|ω|Δt/2), (ω/|ω|)*sin(|ω|Δt/2)]这个公式看着眼熟吗？其实就是把角速度看成旋转轴，Δt 时间内转过的角度是 |ω|Δt。
实际工程中，我一般用一阶近似来简化计算：
q(t+Δt) ≈ q(t) * [1, ωx*Δt/2, ωy*Δt/2, ωz*Δt/2]但要注意，这个近似只在 Δt 很小的时候才准。如果陀螺仪采样率低，或者角速度很大，误差会累积。
3.4 等效旋转矢量：解决不可交换误差
说到四元数更新，就不得不提等效旋转矢量。为什么需要它？因为角速度在积分时有个坑：旋转不可交换。
你想想看，先绕 X 轴转 90 度，再绕 Y 轴转 90 度，跟先绕 Y 再绕 X，结果一样吗？不一样！这就是旋转的不可交换性。
等效旋转矢量 Φ 就是为了解决这个问题而生的。它定义为：
Φ = [Φx, Φy, Φz]其中 Φ 的方向是旋转轴，模长是旋转角度。用等效旋转矢量更新四元数的公式是：
q(t+Δt) = q(t) * [cos(|Φ|/2), (Φ/|Φ|)*sin(|Φ|/2)]关键来了：Φ 怎么算？不能简单地把角速度积分。常用的方法是：
Φ = ∫ω dt + 0.5 * ∫(ω × ∫ω dt) dt第二项就是补偿不可交换误差的。实际工程中，我们常用二子样或三子样算法来近似这个积分。
3.5 知识体系总览
下面这张图总结了四元数相关的核心知识点和它们之间的关系。你可以把它当作本章的思维导图：
从这张图可以看出，四元数定义是基础，旋转和更新是两大应用方向。而等效旋转矢量，正是为了解决更新过程中的不可交换误差而引入的。
好了，这一节的内容就到这里。四元数这东西，刚开始可能觉得绕，但用多了就会发现它真的很优雅。下一节我们会聊卡尔曼滤波，那是惯导融合的另一个核心工具。
公众号:蓝海资料掘金营，微信deep3321