4、姿态解算算法（二）：四元数基础

各位同学，欢迎回来。上一讲我们聊了欧拉角，大家应该还记得那个让人头疼的「万向锁」问题。说实话，我第一次在无人机飞控里遇到万向锁时，飞机直接翻了跟头，吓得我一身冷汗。从那以后，我就彻底倒向了四元数。

今天这一讲，我们来啃四元数这块硬骨头。别怕，我会用最接地气的方式讲明白。

4.1 从复数到四元数：一个自然的推广

先问大家一个问题：二维平面上的旋转，用什么表示最方便？

对，复数。

一个复数 \( z = a + bi \)，乘以 \( e^{i\theta} \) 就相当于旋转了 \( \theta \) 角度。这个操作，我们在信号处理、电路分析里都用烂了。

那三维空间里的旋转呢？

嗯，问题来了。三维旋转有三个自由度，复数只有两个分量，不够用。数学家哈密顿当年也被这个问题折磨了很久。据说1843年的一天，他在都柏林的布鲁姆桥上散步时，突然灵光一闪——为什么不试试四个分量？

于是，四元数诞生了。

四元数的形式很简单：

q = w + xi + yj + zk

其中 \( w \) 是实部，\( x, y, z \) 是虚部。\( i, j, k \) 是三个虚数单位，它们的关系是：

• \( i^2 = j^2 = k^2 = -1 \)
• \( ij = k, ji = -k \)
• \( jk = i, kj = -i \)
• \( ki = j, ik = -j \)

你看，这个乘法规则是不是很眼熟？其实就是叉积的代数版本。我在做IMU标定时，经常用这个性质来快速验证四元数乘法的代码有没有写对。

4.2 四元数的运算法则

既然要拿四元数干活，基本的运算法则得烂熟于心。我建议你把下面这几条记在笔记本上，面试时也经常考。

4.2.1 加法与减法

跟向量一样，对应分量相加减：

q1 + q2 = (w1+w2) + (x1+x2)i + (y1+y2)j + (z1+z2)k

4.2.2 乘法（最关键！）

四元数乘法不满足交换律，也就是说 \( q_1 q_2 \neq q_2 q_1 \)。这一点跟矩阵乘法一样，千万别搞混了。

两个四元数相乘的结果是：

q1 * q2 = (w1w2 - x1x2 - y1y2 - z1z2) +
          (w1x2 + x1w2 + y1z2 - z1y2)i +
          (w1y2 - x1z2 + y1w2 + z1x2)j +
          (w1z2 + x1y2 - y1x2 + z1w2)k

看着复杂是吧？我一般用向量形式来记：

q1 = [w1, v1], q2 = [w2, v2]
q1 * q2 = [w1w2 - v1·v2, w1v2 + w2v1 + v1×v2]

这样就好记多了。点积和叉积，都是老朋友了。

4.2.3 共轭与模长

四元数的共轭很简单，虚部取反：

q* = w - xi - yj - zk

模长就是各分量的平方和开根号：

||q|| = sqrt(w² + x² + y² + z²)

单位四元数的模长为1，这是我们最常用的。

4.3 四元数表示旋转

好了，重头戏来了。怎么用四元数表示三维旋转？

假设我们要绕单位向量 \( \mathbf{n} = (n_x, n_y, n_z) \) 旋转 \( \theta \) 角度，对应的四元数是：

q = cos(θ/2) + sin(θ/2)(n_x i + n_y j + n_z k)

注意，这里用的是半角！我第一次写代码时忘了除以2，结果旋转角度翻倍，找了半天bug。嗯，这种坑我踩过，你们就别再踩了。

旋转操作本身也很优雅：

p' = q * p * q*

其中 \( p \) 是待旋转的向量（写成纯虚四元数 \( 0 + xi + yj + zk \)），\( q* \) 是 \( q \) 的共轭。结果 \( p' \) 就是旋转后的向量。

4.4 四元数微分方程与更新

在实际系统中，陀螺仪输出的是角速度，我们需要用角速度来更新姿态。这就用到了四元数微分方程。

四元数随时间的变化率与角速度的关系是：

dq/dt = 0.5 * q * ω

其中 \( \omega = 0 + \omega_x i + \omega_y j + \omega_z k \) 是角速度四元数。

写成矩阵形式更便于编程：

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

有了微分方程，我们就可以做数值积分了。最常用的是一阶龙格-库塔法（RK1）：

q(t+Δt) = q(t) + dq/dt * Δt

不过，我建议你用更高阶的方法，比如RK4。我在一个水下机器人项目里用过RK1，结果姿态发散得厉害，换成RK4后稳如老狗。

4.5 四元数转欧拉角

虽然四元数做内部运算很爽，但最终输出给用户或者上层控制算法时，往往需要转成欧拉角。毕竟，俯仰、横滚、偏航这三个角度，人类更容易理解。

转换公式如下（ZYX顺序）：

roll  = atan2(2(wx + yz), 1 - 2(x² + y²))
pitch = asin(2(wy - zx))
yaw   = atan2(2(wz + xy), 1 - 2(y² + z²))

这里要注意几点：

• 俯仰角用 asin，当角度接近 ±90° 时，数值不稳定。这就是万向锁的另一种表现形式。
• atan2 比 atan 好，能自动判断象限。
• 不同旋转顺序（ZYX、XYZ等）的转换公式不一样，别搞混了。

4.6 四元数归一化

最后，我们来聊聊归一化。这步看似简单，但极其重要。

为什么需要归一化？因为数值计算中的舍入误差、积分步长选择不当，都会让四元数的模长偏离1。一旦模长偏离，旋转矩阵就不再是正交矩阵，姿态信息就会失真。

归一化的方法很简单：

norm = sqrt(w² + x² + y² + z²)
w /= norm
x /= norm
y /= norm
z /= norm

就这么几行代码。但我建议你：

• 每次更新后都做归一化，不要偷懒
• 如果计算资源紧张，可以用快速归一化算法（比如用牛顿迭代法近似求倒数平方根）
• 检查归一化后的模长，如果偏离1超过某个阈值（比如1e-6），说明系统可能有问题

小结

这一讲我们覆盖了四元数的核心内容：

• 从复数到四元数的推广思路
• 四元数的加减乘除、共轭、模长
• 用四元数表示三维旋转（半角是关键）
• 四元数微分方程与数值更新
• 四元数与欧拉角的互转
• 归一化的必要性和方法

下一讲，我们会把这些知识串起来，实现一个完整的姿态解算算法。到时候，你会看到四元数、陀螺仪、加速度计、磁力计是怎么协同工作的。

好，今天就到这里。有问题随时找我。

---

公众号:蓝海资料掘金营，微信deep3321