四元数基础：从懵圈到通透

说实话，我刚接触四元数那会儿，也是一头雾水。

什么 w、x、y、z，四个数就能表示旋转？旋转矩阵 3x3 九个参数它不香吗？

后来在项目里被万向锁坑过几次，才老老实实回来啃四元数。今天我就把这块硬骨头，给你掰开了揉碎了讲清楚。

1. 四元数的数学定义

四元数本质上是一个超复数。你想想看，普通复数有一个实部一个虚部，四元数则是一个实部加三个虚部。

数学上写成这样：

q = w + xi + yj + zk

其中 i、j、k 满足一个很关键的关系：

i² = j² = k² = ijk = -1

嗯，这里要注意，i、j、k 之间是不满足交换律的。也就是说：

i × j = k
j × i = -k

这个性质，说白了就是四元数能表示三维旋转的根本原因。

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

我个人习惯把四元数看作「绕着某个轴转某个角度」的紧凑表达。轴是单位向量 (x, y, z)，角度是 θ，那么：

q = [cos(θ/2), sin(θ/2)*x, sin(θ/2)*y, sin(θ/2)*z]

为什么是半角？这个问题我当时也想了很久。简单说，因为两次旋转叠加时，半角能让四元数乘法完美对应旋转的复合。

3. 与旋转矩阵的转换

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

四元数 → 旋转矩阵

给定单位四元数 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²)]
]

这个公式看着吓人，其实有规律。我记的时候只看对角线：每个元素都是 1 减去两倍的另外两个虚部平方和。

旋转矩阵 → 四元数

反过来稍微麻烦一点。我记得有一次调试机械臂，矩阵转四元数时符号搞反了，结果轨迹直接飞出去。这里给出稳定版本：

w = sqrt(1 + R[0][0] + R[1][1] + R[2][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)

4. 四元数插值（Slerp）

做轨迹规划时，我们经常需要让机器人从姿态 A 平滑过渡到姿态 B。直接线性插值四元数的四个分量？不行，那样出来的路径不是最短的，而且中间点的模长会变。

这时候就要用 Slerp（Spherical Linear Interpolation）。

它的核心思想：在四维单位球面上，沿着大圆弧走最短路径。

Slerp(q1, q2, t) = (sin((1-t)Ω) / sinΩ) * q1 + (sin(tΩ) / sinΩ) * q2

其中 Ω 是两个四元数之间的夹角：

cosΩ = q1 · q2 = w1*w2 + x1*x2 + y1*y2 + z1*z2

代码实现

def slerp(q1, q2, t):
    # 计算点积
    dot = q1.w * q2.w + q1.x * q2.x + q1.y * q2.y + q1.z * q2.z
    
    # 如果点积为负，取反走短路径
    if dot < 0:
        q2 = -q2
        dot = -dot
    
    # 防止除零
    if dot > 0.9995:
        return linear_interp(q1, q2, t)
    
    theta = acos(dot)
    sin_theta = sin(theta)
    
    scale1 = sin((1-t) * theta) / sin_theta
    scale2 = sin(t * theta) / sin_theta
    
    return scale1 * q1 + scale2 * q2

你想想看，这个插值保证了角速度恒定，轨迹平滑。我在做协作机器人示教轨迹复现时，就是用 Slerp 把离散的示教点连成连续轨迹，效果非常好。

5. 知识体系总览

下面这张图，是我梳理的四元数知识脉络。你可以把它当作学习地图：

6. 避坑指南

最后，分享几个我实战中遇到的坑：

• 四元数顺序：不同库的存储顺序不一样。Eigen 是 (w, x, y, z)，ROS 是 (x, y, z, w)。我曾经混用过，调试了整整两天。
• 万向锁：虽然四元数本身没有万向锁问题，但如果你转成欧拉角再用，该锁还是会锁。我建议全程用四元数运算。
• 数值漂移：长时间累积旋转后，四元数会偏离单位球面。记得定期归一化。