第三节：四元数——旋转的“瑞士军刀”

说实话，我刚入行那会儿，看到四元数就头疼。

四个数？还带虚部？旋转矩阵不香吗？

直到我在项目里遇到了万向锁——嗯，就是那个让机械臂突然“抽筋”的问题。从那以后，我老老实实把四元数啃了下来。今天咱们就聊聊这个感知领域绕不开的工具。

3.1 四元数的数学基础

四元数说白了就是一个扩展版的复数。复数有实部和虚部，四元数有1个实部和3个虚部。

数学上写成：

q = w + xi + yj + zk

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

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

我习惯把四元数看作一个向量形式：

q = [w, x, y, z]ᵀ

这里 w 是实部，[x, y, z] 是虚部向量。单位四元数满足 w² + x² + y² + z² = 1，这个条件在旋转表示里特别重要。

q = [cos(θ/2), n·sin(θ/2)]

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

实际工程中，我们经常需要在四元数和旋转矩阵之间来回切换。为什么？因为四元数适合插值和拼接，旋转矩阵适合变换点云——各有所长。

3.2.1 四元数 → 旋转矩阵

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

这个公式看着复杂，但写代码时直接套用就行。我一般封装成一个函数：

def quat_to_rotmat(q):
    """四元数转旋转矩阵"""
    w, x, y, z = q
    R = np.array([
        [1-2*(y**2+z**2), 2*(x*y-w*z), 2*(x*z+w*y)],
        [2*(x*y+w*z), 1-2*(x**2+z**2), 2*(y*z-w*x)],
        [2*(x*z-w*y), 2*(y*z+w*x), 1-2*(x**2+y**2)]
    ])
    return R

3.2.2 旋转矩阵 → 四元数

反过来转换稍微麻烦一点。给定旋转矩阵 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)

我建议用现成库函数，比如 scipy 的 Rotation.from_matrix()，但理解原理能帮你排查bug。

3.3 四元数的插值

这是四元数最实用的地方。你想想看，两个姿态之间怎么平滑过渡？用欧拉角插值会出问题，用旋转矩阵插值得到的矩阵可能不是正交的。四元数插值就优雅多了。

3.3.1 线性插值（LERP）

最简单的做法：

q(t) = (1-t) * q₁ + t * q₂

但这样插出来的四元数不是单位四元数，需要归一化：

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

LERP 的问题是角速度不均匀。当两个四元数夹角较大时，中间会“忽快忽慢”。

3.3.2 球面线性插值（SLERP）

这才是正经做法。SLERP 在单位球面上沿大圆弧匀速运动：

q(t) = [sin((1-t)θ) / sin(θ)] * q₁ + [sin(tθ) / sin(θ)] * q₂

其中 θ 是 q₁ 和 q₂ 之间的夹角：

cos(θ) = q₁ · q₂

代码实现：

def slerp(q1, q2, t):
    """四元数球面线性插值"""
    dot = np.dot(q1, q2)
    
    # 处理负点积——取最短路径
    if dot < 0:
        q2 = -q2
        dot = -dot
    
    # 防止数值问题
    if dot > 0.9995:
        return normalize((1-t) * q1 + t * q2)
    
    theta = np.arccos(dot)
    sin_theta = np.sin(theta)
    
    q = (np.sin((1-t)*theta) / sin_theta) * q1 + \
        (np.sin(t*theta) / sin_theta) * q2
    return q

3.4 知识体系总览

下面这张图概括了四元数在感知中的核心逻辑：

3.5 实战中的避坑指南

最后分享几个我踩过的坑：

• 四元数顺序问题：不同库的存储顺序不一样。Eigen 是 [x, y, z, w]，ROS 是 [x, y, z, w]，scipy 是 [w, x, y, z]。我曾经混用过，结果旋转方向全反了。
• 单位化检查：每次从传感器读取四元数后，先检查模长是否接近1。如果偏差超过1e-6，手动归一化。数值误差会累积。
• SLERP 的负点积：如果 q₁ · q₂ 为负，说明走的是长弧路径。取反后插值，路径更短。
• 不要用欧拉角做插值：除非你明确知道自己在做什么。欧拉角的万向锁问题在插值时尤其明显。