3、四元数基础：定义、运算与旋转矩阵的关系
四元数这东西，我第一次接触时也觉得挺抽象的。说白了，它就是用来描述旋转的一种数学工具。在 ArduPilot 里，姿态估计和控制的核心就是四元数。你想想看，欧拉角有万向锁问题，旋转矩阵又太占内存，四元数刚好是个折中方案——紧凑、无奇点、适合插值。
我个人习惯把四元数看作“带标量的三维向量”。它由四个实数组成：一个实部加三个虚部。嗯，这里要注意，四元数的乘法不满足交换律，这点和矩阵乘法一样。
3.1 四元数的定义
一个四元数可以写成：
q = w + xi + yj + zk其中 w 是实部，x、y、z 是虚部。i、j、k 是虚数单位，满足：
- i² = j² = k² = -1
- ij = k, ji = -k
- jk = i, kj = -i
- ki = j, ik = -j
在代码里，我们通常用四维向量表示：
q = [w, x, y, z]单位四元数的模为1，即 w² + x² + y² + z² = 1。所有用于表示旋转的四元数都是单位四元数。
核心概念：单位四元数 q 和 -q 表示同一个旋转。这在插值时要注意，我曾经踩过这个坑——两个四元数如果符号相反，直接插值会走远路。
3.2 四元数的基本运算
四元数的运算规则其实不复杂，记住几条就够了。
3.2.1 加法与减法
对应分量相加减：
q1 + q2 = [w1+w2, x1+x2, y1+y2, z1+z2]
q1 - q2 = [w1-w2, x1-x2, y1-y2, z1-z2]3.2.2 乘法
四元数乘法不交换，公式如下：
q1 * q2 = [w1*w2 - x1*x2 - y1*y2 - z1*z2,
w1*x2 + x1*w2 + y1*z2 - z1*y2,
w1*y2 - x1*z2 + y1*w2 + z1*x2,
w1*z2 + x1*y2 - y1*x2 + z1*w2]这个公式看着长，其实有规律。我记的时候只记第一项是点积的负数，后面三项是叉积加标量乘。在 ArduPilot 源码里，四元数乘法是这样实现的：
// ArduPilot 四元数乘法实现（简化版）
Quaternion operator*(const Quaternion &q) const {
Quaternion ret;
ret.w = w*q.w - x*q.x - y*q.y - z*q.z;
ret.x = w*q.x + x*q.w + y*q.z - z*q.y;
ret.y = w*q.y - x*q.z + y*q.w + z*q.x;
ret.z = w*q.z + x*q.y - y*q.x + z*q.w;
return ret;
}3.2.3 共轭与模
四元数的共轭就是把虚部取反：
q* = [w, -x, -y, -z]模的计算：
|q| = sqrt(w² + x² + y² + z²)单位四元数的模为1，共轭等于逆：
q⁻¹ = q*实用技巧：在 ArduPilot 里，如果你要对一个向量 v 做旋转，用 q * v * q⁻¹ 就行。注意 v 要写成纯四元数 [0, vx, vy, vz]。
3.3 四元数与旋转矩阵的关系
四元数和旋转矩阵可以互相转换。这在飞控里很常见——传感器输出可能是旋转矩阵，但姿态估计用四元数更稳定。
3.3.1 四元数转旋转矩阵
给定单位四元数 q = [w, x, y, z]，对应的旋转矩阵 R 为：
| 分量 | 公式 |
|---|---|
| R₁₁ | 1 - 2(y² + z²) |
| R₁₂ | 2(xy - wz) |
| R₁₃ | 2(xz + wy) |
| R₂₁ | 2(xy + wz) |
| R₂₂ | 1 - 2(x² + z²) |
| R₂₃ | 2(yz - wx) |
| R₃₁ | 2(xz - wy) |
| R₃₂ | 2(yz + wx) |
| R₃₃ | 1 - 2(x² + y²) |
在 ArduPilot 里，这个转换用在姿态初始化阶段。我记得有一次调试，发现四元数转矩阵后行列式不为1，排查半天发现是四元数没归一化。
3.3.2 旋转矩阵转四元数
反过来，从旋转矩阵 R 提取四元数：
w = 0.5 * sqrt(1 + R₁₁ + R₂₂ + R₃₃)
x = (R₃₂ - R₂₃) / (4 * w)
y = (R₁₃ - R₃₁) / (4 * w)
z = (R₂₁ - R₁₂) / (4 * w)当 w 接近0时，需要用其他方法。我建议用数值稳定的方法，比如取最大对角元做分母。
避坑指南：我曾经在旋转矩阵转四元数时，直接用上面公式没做边界处理。结果在 w≈0 时，x、y、z 的分母接近0，数值爆炸。后来改用基于最大对角元的分支方法才稳定。
3.4 四元数在 ArduPilot 中的应用
在 ArduPilot 里，四元数主要用于三个方面：
- 姿态估计：EKF 用四元数作为状态量，避免欧拉角的奇点问题
- 姿态控制：控制器输出四元数误差，再转成角速度指令
- 姿态插值：在航点之间平滑旋转，用球面线性插值（SLERP）
你看，四元数贯穿了整个飞控的姿态链路。我个人觉得，理解四元数最好的方式就是动手写代码——从定义到乘法，再到转矩阵，每一步都自己实现一遍。
3.5 知识体系图
下面这张图总结了四元数的核心知识结构：
3.6 小结
四元数其实没那么可怕。记住三点：
- 它是单位四元数才能表示旋转
- 乘法不交换，顺序很重要
- 和旋转矩阵可以互相转换，但要注意数值稳定性
在 ArduPilot 里，四元数无处不在。你理解了它，就等于拿到了姿态控制的第一把钥匙。嗯，接下来你可以打开源码看看 AP_Math/quaternion.cpp，里面全是这些运算的实现。