第2章 四元数入门：复数推广、四元数乘法、旋转表示与插值

好，咱们开始聊四元数。

说实话，我第一次接触四元数的时候，心里是有点抵触的。你想想看，搞无人机嘛，欧拉角多直观——俯仰、横滚、偏航，一看就懂。但后来我在实际项目中吃了大亏：有一次做高速机动测试，飞机刚翻了个筋斗，欧拉角直接炸了，万向锁搞得姿态全乱套。嗯，从那以后，我就老老实实学四元数了。

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

咱们先回忆一下复数。复数长这样：a + bi，其中 i² = -1。它用来表示二维平面上的旋转，很方便。

那三维旋转怎么办？

有人想：再加一个虚部不就行了？于是就有了四元数：q = w + xi + yj + zk。

这里的关键规则是：

• i² = j² = k² = -1
• ij = k，jk = i，ki = j
• 注意乘法不满足交换律：ij = -ji

我个人习惯把四元数想象成一个“带方向的旋转”。它比欧拉角更优雅，没有万向锁，而且计算量小。

2.2 四元数乘法：核心运算

四元数乘法看起来复杂，其实有规律。假设有两个四元数：

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

它们的乘积是：

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

看着有点吓人？别怕。我在代码里一般写成矩阵形式，或者直接用现成的库函数。但理解原理很重要——它本质上就是两个旋转的叠加。

2.3 用四元数表示旋转

怎么用四元数旋转一个向量？步骤很简单：

1. 把向量 v 写成纯四元数：p = (0, v)
2. 用旋转四元数 q 做变换：p' = q * p * q⁻¹
3. 取 p' 的虚部，就是旋转后的向量

这里 q⁻¹ 是四元数的逆。对于单位四元数（模长为1），逆就是共轭：q⁻¹ = (w, -x, -y, -z)。

我在项目中遇到过一个问题：一开始我忘了归一化四元数，结果旋转后的向量长度变了，飞机姿态越算越偏。嗯，这是个坑，大家记住——旋转四元数必须是单位四元数。

2.4 四元数插值：SLERP

做姿态解算时，经常需要在两个姿态之间平滑过渡。比如云台从A点转到B点，不能直接跳过去，得插值。

四元数的线性插值（LERP）简单，但会导致角速度不均匀。我推荐用球面线性插值（SLERP）。

SLERP的公式：

q(t) = (sin((1-t)θ) * q1 + sin(tθ) * q2) / sin(θ)

其中 θ = arccos(q1 · q2)，t 从0到1。

代码实现也不复杂：

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.9995:
        result = q1 + t * (q2 - q1)
        return normalize(result)
    
    # 确保取最短路径
    if dot < 0:
        q2 = -q2
        dot = -dot
    
    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

2.5 四元数与欧拉角的转换

虽然咱们用四元数做解算，但调试时还是想看欧拉角。转换公式如下：

转换方向	公式
四元数 → 欧拉角	roll = atan2(2(wx + yz), 1 - 2(x² + y²)) pitch = asin(2(wy - zx)) yaw = atan2(2(wz + xy), 1 - 2(y² + z²))
欧拉角 → 四元数	q = q_yaw * q_pitch * q_roll 每个轴单独构造四元数再相乘

注意：从四元数转欧拉角时，pitch接近±90°会有数值问题。这就是万向锁的体现。所以我在飞控里，内部全用四元数，只在日志和地面站显示时转成欧拉角。

2.6 本章小结

四元数这东西，刚开始觉得抽象，用多了就发现它真香。总结几个要点：

• 四元数是复数的三维推广，用4个数表示旋转
• 乘法对应旋转叠加，注意顺序
• 旋转向量用 q * p * q⁻¹
• 插值用SLERP，平滑又均匀
• 调试时转欧拉角，但内部运算别用

下一章咱们就进入实战了——怎么用四元数做姿态解算。到时候我会拿IMU的数据，一步步带大家算。准备好了吗？