四元数基础：定义、乘法、转换与更新

各位同学，今天我们来聊聊四元数。说实话，我刚入行那会儿，看到四元数这四个字就头疼。什么虚部实部、什么超复数，听着就玄乎。但后来做项目做多了，我慢慢发现——四元数这东西，其实就是个旋转的数学工具，没那么神秘。

为什么我们要用四元数？你想想看，欧拉角有万向锁问题，方向余弦矩阵又太笨重（9个参数啊）。四元数呢？4个参数，没有奇点，计算还快。我在做某型无人机飞控时，就是靠四元数才把姿态更新跑到了200Hz。嗯，今天我们就把它彻底搞明白。

1. 四元数的定义

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

q = q0 + q1*i + q2*j + q3*k

其中 i、j、k 满足：

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

嗯，这里要注意，四元数的乘法不满足交换律。也就是说 q1*q2 不等于 q2*q1。这个特性很重要，后面讲旋转合成时会用到。

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

q = [q0, q1, q2, q3]ᵀ

或者更直观地：

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

其中 n 是单位旋转轴，θ 是旋转角度。这个形式我特别喜欢，因为它把旋转的几何意义说得很清楚。

2. 四元数乘法

四元数乘法，也叫哈密顿积。两个四元数 p 和 q 相乘：

p = [p0, p1, p2, p3]
q = [q0, q1, q2, q3]

p ⊗ q = [p0*q0 - p1*q1 - p2*q2 - p3*q3,
         p0*q1 + p1*q0 + p2*q3 - p3*q2,
         p0*q2 - p1*q3 + p2*q0 + p3*q1,
         p0*q3 + p1*q2 - p2*q1 + p3*q0]

写成矩阵形式会更清晰：

p ⊗ q = [p0  -p1  -p2  -p3]   [q0]
         [p1   p0  -p3   p2] * [q1]
         [p2   p3   p0  -p1]   [q2]
         [p3  -p2   p1   p0]   [q3]

我个人习惯用这个矩阵形式来写代码，因为矩阵乘法在Python里用NumPy实现起来特别方便。我曾经在一个项目里手写四元数乘法，结果写错了符号，导致姿态解算跑飞了整整两天才找到bug。嗯，从那以后我就老老实实用矩阵形式了。

3. 四元数与旋转矩阵的转换

这个转换在惯导里太常用了。我们经常需要把四元数转成旋转矩阵，用来把加速度计数据从载体坐标系转到导航坐标系。

四元数 → 旋转矩阵：

给定单位四元数 q = [q0, q1, q2, q3]，对应的旋转矩阵为：

C = [q0²+q1²-q2²-q3²,  2(q1*q2 - q0*q3),    2(q1*q3 + q0*q2)  ]
    [2(q1*q2 + q0*q3),   q0²-q1²+q2²-q3²,   2(q2*q3 - q0*q1)  ]
    [2(q1*q3 - q0*q2),   2(q2*q3 + q0*q1),   q0²-q1²-q2²+q3²  ]

旋转矩阵 → 四元数：

这个稍微麻烦一点。给定旋转矩阵 C = [Cij]，我们可以这样求四元数：

q0 = 0.5 * sqrt(1 + C11 + C22 + C33)
q1 = 0.5 * sign(C32 - C23) * sqrt(1 + C11 - C22 - C33)
q2 = 0.5 * sign(C13 - C31) * sqrt(1 - C11 + C22 - C33)
q3 = 0.5 * sign(C21 - C12) * sqrt(1 - C11 - C22 + C33)

这里 sign() 是符号函数。为什么要加符号？因为四元数 (q) 和 (-q) 表示同一个旋转。我们需要通过符号来保证旋转方向的一致性。

4. 四元数更新

终于到了最核心的部分。在捷联惯导里，陀螺仪输出的是角速度，我们需要用角速度来更新四元数。

微分方程形式：

dq/dt = 0.5 * q ⊗ ω

其中 ω = [0, ωx, ωy, ωz] 是角速度的四元数形式。

离散化更新（一阶龙格-库塔法）：

Δθ = ω * Δt
q(t+Δt) = q(t) + 0.5 * q(t) ⊗ [0, Δθx, Δθy, Δθz]

写成代码就是：

def quaternion_update(q, gyro, dt):
    """
    q: 当前四元数 [q0, q1, q2, q3]
    gyro: 角速度 [wx, wy, wz] (rad/s)
    dt: 时间间隔 (s)
    """
    wx, wy, wz = gyro
    
    # 构造角增量四元数
    dq = np.array([0, wx*dt/2, wy*dt/2, wz*dt/2])
    
    # 四元数乘法更新
    q_new = q + quaternion_multiply(q, dq)
    
    # 归一化
    q_new = q_new / np.linalg.norm(q_new)
    
    return q_new

这里有个关键点：为什么更新完要归一化？因为数值积分会引入误差，导致四元数模长偏离1。如果不归一化，旋转矩阵就会失去正交性，姿态精度会逐渐下降。

更高精度的更新方法（二阶龙格-库塔法）：

def quaternion_update_rk2(q, gyro, dt):
    wx, wy, wz = gyro
    
    # 第一步：计算中间点
    k1 = 0.5 * quaternion_multiply(q, [0, wx, wy, wz])
    q_mid = q + k1 * dt/2
    q_mid = q_mid / np.linalg.norm(q_mid)
    
    # 第二步：用中间点计算最终更新
    k2 = 0.5 * quaternion_multiply(q_mid, [0, wx, wy, wz])
    q_new = q + k2 * dt
    q_new = q_new / np.linalg.norm(q_new)
    
    return q_new

嗯，这里要注意，二阶法虽然精度高，但计算量也翻倍了。如果你的处理器性能有限，一阶法配合高采样率也是可行的。我个人的经验是：采样率在200Hz以上时，一阶法就够用了；低于100Hz时，建议用二阶法。

总结一下

四元数这东西，说白了就是四个数搞定旋转。定义要记牢，乘法要小心顺序，转换要归一化，更新要选对方法。我在实际项目中踩过的坑，今天都给你们说了。希望你们少走弯路。

下一章我们会讲姿态解算的完整流程，到时候四元数会贯穿始终。嗯，今天就到这里，有问题随时问我。