数学基础（二）：四元数——无人机姿态解算的“瑞士军刀”
好，我们接着聊。上一节我们把欧拉角、旋转矩阵这些老朋友都请出来了。说实话，在飞控里用欧拉角做姿态解算，我个人觉得就像开手动挡车——不是不行，但遇到急转弯（比如大角度机动）就容易熄火。万向锁问题，你想想看，多尴尬。
所以，这一节我们来聊聊四元数。这东西在飞控圈里，说白了就是“标准答案”。我刚开始做飞控那会儿，看到四元数公式就头疼，觉得这玩意儿太抽象了。后来在项目里被欧拉角坑过一次——飞机做大仰角爬升时姿态解算直接炸了——从那以后，我就老老实实拥抱四元数了。
1. 四元数的定义与性质
四元数是什么？你可以把它想象成一个“超级复数”。
普通复数有一个实部和一个虚部：a + bi。四元数呢，有一个实部和三个虚部：
q = w + xi + yj + zk这里 w 是实部，x, y, z 是虚部。而 i, j, k 是三个虚数单位，它们满足一个很关键的关系：
i² = j² = k² = ijk = -1嗯，这里要注意，它们之间不满足交换律，比如 i * j = k，但 j * i = -k。这一点和矩阵乘法很像，也是四元数能优雅表示旋转的原因之一。
我们通常把四元数写成向量形式：
q = [w, x, y, z]ᵀ或者拆成标量部分和向量部分：
q = [w, v] ，其中 v = [x, y, z]单位四元数是姿态解算中最常用的，它的模长为1：
||q|| = sqrt(w² + x² + y² + z²) = 1为什么一定要单位化？因为只有单位四元数才能表示纯旋转，不引入缩放。我在项目中就见过有人忘了归一化，结果姿态越解越飘，最后飞机自己画起了圈圈……
核心性质：单位四元数可以唯一表示三维空间中的旋转，且不会出现万向锁问题。
2. 四元数乘法
四元数乘法，也叫哈密顿积，是姿态解算里最频繁的操作。两个四元数相乘，结果还是一个四元数。
假设有两个四元数：
q₁ = [w₁, x₁, y₁, z₁]
q₂ = [w₂, x₂, y₂, z₂]它们的乘积 q = q₁ ⊗ q₂ 定义为：
w = w₁w₂ - x₁x₂ - y₁y₂ - z₁z₂
x = w₁x₂ + x₁w₂ + y₁z₂ - z₁y₂
y = w₁y₂ - x₁z₂ + y₁w₂ + z₁x₂
z = w₁z₂ + x₁y₂ - y₁x₂ + z₁w₂写成矩阵形式会更清晰，方便代码实现：
q = Q₁ · q₂其中 Q₁ 是 q₁ 的左乘矩阵：
Q₁ = [
[ w₁, -x₁, -y₁, -z₁],
[ x₁, w₁, -z₁, y₁],
[ y₁, z₁, w₁, -x₁],
[ z₁, -y₁, x₁, w₁]
]在代码里，我习惯这样写：
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]我的小技巧：四元数乘法不满足交换律，q₁ ⊗ q₂ ≠ q₂ ⊗ q₁。在代码里一定要搞清楚旋转顺序，是先转q₁再转q₂，还是反过来。我一般会在注释里写清楚“当前姿态 × 增量旋转”的顺序。
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²)]
]这个公式看起来有点吓人，但其实就是几个乘法和加减法。我在STM32上实现时，会提前算好 x², y², z², xy, xz, yz, wx, wy, wz 这些中间量，避免重复计算。
从旋转矩阵到四元数：
反过来，给定旋转矩阵 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)这里有个坑——如果 w 接近0，除法会出问题。我曾经在调试时遇到过这个情况，当时矩阵是从传感器数据直接算出来的，数值误差导致 w 几乎为0，结果四元数直接飞了。
避坑指南：当 w 接近0时，改用其他三个分量来恢复四元数。比如先算 x，再用 x 算其他分量。代码里一定要加这个判断，别问我怎么知道的……
4. 四元数更新方程
终于到了最核心的部分——四元数怎么随时间更新？
在飞控里，我们通过陀螺仪读到角速度 ω = [ωx, ωy, ωz]，然后要用它来更新当前姿态的四元数。
四元数的微分方程是：
dq/dt = 0.5 * q ⊗ ω_q其中 ω_q = [0, ωx, ωy, ωz] 是把角速度向量扩展成的纯四元数（实部为0）。
展开成标量形式：
dw/dt = -0.5 * (x*ωx + y*ωy + z*ωz)
dx/dt = 0.5 * (w*ωx + y*ωz - z*ωy)
dy/dt = 0.5 * (w*ωy + z*ωx - x*ωz)
dz/dt = 0.5 * (w*ωz + x*ωy - y*ωx)在实际代码里，我们用的是离散形式。最常用的是一阶龙格-库塔法，说白了就是：
q_new = q_old + dq/dt * Δt写成代码：
def quaternion_update(q, gyro, dt):
w, x, y, z = q
wx, wy, wz = gyro # 角速度，单位 rad/s
dw = -0.5 * (x*wx + y*wy + z*wz)
dx = 0.5 * (w*wx + y*wz - z*wy)
dy = 0.5 * (w*wy + z*wx - x*wz)
dz = 0.5 * (w*wz + x*wy - y*wx)
w_new = w + dw * dt
x_new = x + dx * dt
y_new = y + dy * dt
z_new = z + dz * dt
# 归一化，保证单位四元数
norm = sqrt(w_new**2 + x_new**2 + y_new**2 + z_new**2)
return [w_new/norm, x_new/norm, y_new/norm, z_new/norm]嗯，这里要注意，每次更新后一定要归一化。因为数值积分会引入误差，导致四元数模长偏离1。不归一化的话，姿态会慢慢漂移。
核心要点：四元数更新 = 微分方程 + 离散积分 + 归一化。三步缺一不可。
如果追求更高精度，可以用二阶或四阶龙格-库塔法。但说实话，在大多数飞控场景下，一阶法配合足够高的更新频率（比如1kHz），效果已经很好了。我做过对比测试，在400Hz的更新率下，一阶和四阶的误差差异不到0.01度，完全够用。
最后，我想说一句：四元数这东西，刚开始学觉得绕，但用顺手了就会发现它真的很优雅。没有万向锁，没有三角函数的计算开销，只有几个乘法和加法。在嵌入式平台上跑起来，那叫一个丝滑。
下一节，我们会把这些数学工具串起来，真正开始做姿态解算。到时候你会发现，今天学的这些，全都会用上。