四元数：旋转的优雅数学工具

说到四元数，我得先坦白一件事。我刚开始做机器人时，对四元数其实是有点抗拒的。毕竟欧拉角多直观啊，俯仰、偏航、翻滚，一看就懂。直到有一次，我在调试一个机械臂的轨迹规划时，遇到了传说中的「万向锁」——手臂突然卡在一个奇怪的角度，怎么都转不过去。嗯，从那天起，我就老老实实学四元数了。

说白了，四元数就是用来描述旋转的。它比旋转矩阵更紧凑，比欧拉角更优雅。你想想看，一个三维空间的旋转，用四个数就能搞定，而且没有奇点问题，多划算。

四元数可以看作复数的推广。一个四元数 q 写成：

q = w + xi + yj + zk

其中 w 是实部，x、y、z 是虚部。i、j、k 满足一个很特别的乘法规则：

i² = j² = k² = ijk = -1
ij = k, ji = -k
jk = i, kj = -i
ki = j, ik = -j

这个规则看起来有点绕，但记住一点就行：四元数乘法不满足交换律。也就是说，q₁ × q₂ 不等于 q₂ × q₁。这在后面做旋转合成时会特别重要。

单位四元数：描述旋转的四元数，模长必须为1。即：

w² + x² + y² + z² = 1

我个人习惯把四元数理解成「轴角表示法的优雅版本」。一个旋转，绕单位轴 u 转 θ 角度，对应的四元数是：

q = cos(θ/2) + sin(θ/2) · (u_x i + u_y j + u_z k)

注意这里用的是半角。为什么是半角？因为四元数做两次乘法才能完成一次旋转。这个细节我当年也困惑了很久。

实际工程中，我们经常需要在四元数和旋转矩阵之间来回切换。比如传感器输出的是四元数，但可视化工具需要旋转矩阵。

从四元数到旋转矩阵：

给定单位四元数 q = (w, x, y, z)，对应的旋转矩阵 R 为：

R = [
    [1-2(y²+z²),   2(xy-wz),     2(xz+wy)],
    [2(xy+wz),     1-2(x²+z²),   2(yz-wx)],
    [2(xz-wy),     2(yz+wx),     1-2(x²+y²)]
]

这个公式看起来有点吓人，但别硬背。我一般在代码里直接调库，比如 Eigen 或 SciPy。不过有一次在嵌入式平台上，库不支持，我只能手写。那次我对着公式检查了三遍才敢用。

从旋转矩阵到四元数：

反过来稍微麻烦一点。给定旋转矩阵 R，四元数可以通过以下方式提取：

w = 0.5 * sqrt(1 + R[0][0] + R[1][1] + R[2][2])
x = (R[2][1] - R[1][2]) / (4 * w)
y = (R[0][2] - R[2][0]) / (4 * w)
z = (R[1][0] - R[0][1]) / (4 * w)

注意数值稳定性：当 w 接近 0 时，分母会很小，导致数值不稳定。我曾经在无人机姿态估计中遇到过这个问题，飞控数据突然跳变。后来改用其他方法，比如先找矩阵中最大的对角元素，再计算对应的分量。

这是四元数最实用的地方之一。在轨迹规划中，我们需要让机器人从姿态 A 平滑过渡到姿态 B。用欧拉角插值？那画面太美我不敢看——会出现奇怪的抖动。用旋转矩阵插值？矩阵的线性插值结果往往不是正交矩阵。

四元数插值就优雅多了。最常用的是 Slerp（球面线性插值）：

q(t) = (sin((1-t)θ) / sinθ) · q₁ + (sin(tθ) / sinθ) · q₂

其中 t 从 0 到 1，θ 是 q₁ 和 q₂ 之间的夹角：

θ = arccos(q₁ · q₂)

这里 q₁ · q₂ 是四元数的点积，即 w₁w₂ + x₁x₂ + y₁y₂ + z₁z₂。

一个小技巧：如果 q₁ · q₂ 是负数，说明两个四元数在球面上相隔较远。这时可以把其中一个取负，让插值路径更短。因为 q 和 -q 表示同一个旋转。

我曾经在机械臂的轨迹规划中用过 Slerp。效果确实好，末端执行器的运动非常平滑。但要注意，Slerp 的计算量比线性插值大不少，在实时性要求高的场合要考虑性能。

还有一种更快的近似方法叫 Lerp（线性插值后归一化）：

q(t) = normalize((1-t) · q₁ + t · q₂)

Lerp 的优点是快，缺点是角速度不恒定。如果插值角度很小（比如小于 10 度），Lerp 和 Slerp 的差别几乎看不出来。我一般在小角度插值时用 Lerp，大角度时用 Slerp。

做四元数相关开发，有几个坑我踩过，分享给你：

忘记归一化：四元数经过多次乘法后，数值误差会累积，模长不再为1。我曾经在 SLAM 系统中因为这个原因，地图越跑越歪。解决方案很简单：每次用之前检查模长，必要时重新归一化。
搞混左右乘：四元数乘法不满足交换律。左乘和右乘表示不同的旋转顺序。我建议统一用「左乘表示世界坐标系下的旋转，右乘表示局部坐标系下的旋转」这个约定。
忽略双覆盖：q 和 -q 表示同一个旋转。这在做插值时特别容易出问题。我一般会在插值前检查 q₁ · q₂ 的符号，如果为负就把 q₂ 取反。

嗯，四元数这东西，初学时觉得抽象，用多了反而觉得顺手。它就像一把瑞士军刀，在旋转相关的场景中几乎都能用上。我个人觉得，理解四元数的关键是把它和轴角表示法联系起来，而不是死记公式。

最后送你一句话：四元数不是魔法，只是把旋转这件事用更聪明的方式表达出来。多写几段代码，多调几次参数，你自然就熟悉了。