4. 旋转表示方法（下）：四元数基础、四元数运算、四元数与旋转矩阵的转换、各种表示方法的优缺点对比

好，我们接着聊旋转表示。上一节我们把欧拉角和旋转矩阵讲透了，这一节轮到四元数了。

说实话，我刚入行时看到四元数就头疼。什么 i、j、k 的平方等于 -1，这不是复数吗？怎么跟旋转扯上关系了？后来在项目里被欧拉角万向锁坑过几次，才老老实实把四元数啃下来。嗯，今天我就把这块硬骨头给你拆碎了讲。

4.1 四元数基础

四元数，说白了就是一个超复数。它由一个实部和三个虚部组成：

q = w + xi + yj + zk

其中 i、j、k 满足：

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

你想想看，这跟三维空间中的叉积是不是很像？没错，四元数天生就是为三维旋转准备的。

我们通常把四元数写成标量-向量形式：

q = [w, v]  其中 v = (x, y, z)

w 是实部，v 是虚部向量。单位四元数（模长为1）可以用来表示旋转。

为什么会是半角？我记得第一次推导这个公式时也懵了。简单说，因为四元数旋转是通过 q·p·q⁻¹ 这种双乘操作实现的，半角刚好让两次乘法凑出完整角度。

4.2 四元数运算

四元数的运算其实不复杂，就是记住几条规则。我在项目中经常用到的有这些：

4.2.1 加减法

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

q₁ ± q₂ = (w₁ ± w₂) + (x₁ ± x₂)i + (y₁ ± y₂)j + (z₁ ± z₂)k

4.2.2 乘法

这个要小心，四元数乘法不满足交换律。我刚开始写代码时经常搞反顺序，结果旋转方向全错了。

q₁ · q₂ = (w₁w₂ - v₁·v₂) + (w₁v₂ + w₂v₁ + v₁×v₂)

写成代码就是：

def quaternion_multiply(q1, q2):
    w1, x1, y1, z1 = q1
    w2, x2, y2, z2 = q2
    w = w1*w2 - x1*x2 - y1*y2 - z1*z2
    x = w1*x2 + x1*w2 + y1*z2 - z1*y2
    y = w1*y2 - x1*z2 + y1*w2 + z1*x2
    z = w1*z2 + x1*y2 - y1*x2 + z1*w2
    return [w, x, y, z]

4.2.3 共轭与模长

共轭：q* = w - xi - yj - zk
模长：||q|| = sqrt(w² + x² + y² + z²)

单位四元数的模长为1，它的逆等于共轭：q⁻¹ = q*

4.3 四元数与旋转矩阵的转换

这部分是标定中的高频操作。传感器输出的姿态可能是四元数，也可能是旋转矩阵，你得能在两者间自由切换。

4.3.1 四元数 → 旋转矩阵

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

R = | 1-2y²-2z²   2xy-2wz     2xz+2wy   |
    | 2xy+2wz      1-2x²-2z²   2yz-2wx   |
    | 2xz-2wy      2yz+2wx     1-2x²-2y² |

这个公式看着长，其实有规律。我一般记成：对角线是 1 减去两倍的另外两个虚部平方和，非对角线是 2×(对应项乘积) 再加减 2w×另一个分量。

4.3.2 旋转矩阵 → 四元数

反过来稍微麻烦点。从旋转矩阵 R 的元素反推四元数：

w = sqrt(1 + R₁₁ + R₂₂ + R₃₃) / 2
x = (R₃₂ - R₂₃) / (4w)
y = (R₁₃ - R₃₁) / (4w)
z = (R₂₁ - R₁₂) / (4w)

4.4 各种表示方法的优缺点对比

做了这么多年多传感器融合，我总结了一张对比表，你直接拿去用：

表示方法	优点	缺点	适用场景
旋转矩阵	直观、容易组合、无奇异	9个参数冗余、计算量大	底层图形渲染、刚体变换
欧拉角	最直观、3个参数	万向锁、顺序依赖、插值困难	人机交互、简单姿态显示
四元数	无奇异、4个参数、易插值	不够直观、半角概念难理解	SLAM、姿态估计、传感器融合
轴角	最物理、3个参数	0°时奇异、组合复杂	旋转表示、李群李代数

我个人在标定工作中，内部计算一律用四元数，只在最后给用户展示时转成欧拉角。为什么？因为四元数做插值和滤波太方便了。比如你要平滑两个姿态之间的过渡，用四元数球面线性插值（Slerp）就行，旋转矩阵和欧拉角都做不到这么优雅。

4.5 本章知识体系

下面这张图帮你理清四元数在整个旋转表示体系中的位置：

嗯，四元数这块内容确实需要多花点时间消化。不过一旦你理解了它的几何意义，就会发现它比旋转矩阵和欧拉角好用得多。下一节我们开始讲多传感器标定中最重要的工具——李群与李代数，到时候你会更深刻地体会到四元数的价值。