3. 四元数
说到姿态解算，很多人第一反应是欧拉角。嗯，我刚开始做导航时也是这么想的。直到有一次在无人机项目里，遇到了传说中的“万向锁”——飞机在俯仰90度时突然失控翻滚。那一刻我才意识到，欧拉角这玩意儿，说白了就是个有缺陷的工具。
后来我接触到了四元数。说实话，第一次看到四个分量的数学结构，我也觉得头大。但用久了你会发现，它真的很优雅。没有奇点，计算高效，插值平滑。今天我就带你彻底搞懂它。
3.1 四元数的定义与基本运算
四元数是什么？简单说，它是复数在三维空间的推广。一个复数有实部和虚部，四元数则有一个实部和三个虚部：
q = w + xi + yj + zk其中 i、j、k 满足一个关键规则：
i² = j² = k² = ijk = -1
ij = k, ji = -k
jk = i, kj = -i
ki = j, ik = -j这个规则我建议你死记硬背。我在项目里吃过亏，有一次手算四元数乘法，把符号搞反了，结果姿态解算出来的角度完全不对，排查了整整两天。
四元数通常写成向量形式：
q = [w, x, y, z]ᵀ或者：
q = w + v (其中 v = xi + yj + zk)3.1.1 基本运算
加法：对应分量相加，没什么特别的。
q₁ + q₂ = [w₁+w₂, x₁+x₂, y₁+y₂, z₁+z₂]乘法：这个要小心。四元数乘法不满足交换律，也就是说 q₁q₂ ≠ q₂q₁。公式如下：
q₁q₂ = [w₁w₂ - v₁·v₂, w₁v₂ + w₂v₁ + v₁×v₂]写成标量形式：
q₁q₂ = [
w₁w₂ - x₁x₂ - y₁y₂ - z₁z₂,
w₁x₂ + x₁w₂ + y₁z₂ - z₁y₂,
w₁y₂ - x₁z₂ + y₁w₂ + z₁x₂,
w₁z₂ + x₁y₂ - y₁x₂ + z₁w₂
]共轭：虚部取反。
q* = [w, -x, -y, -z]模长：
||q|| = sqrt(w² + x² + y² + z²)归一化：姿态解算中用的四元数必须是单位四元数，即模长为1。
q_normalized = q / ||q||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²)]
]这个公式我建议你直接写成函数，别每次都手算。我在一个嵌入式项目里，因为手写这个矩阵时把符号写反了，导致云台控制反向旋转，差点把相机摔了。
3.2.2 旋转矩阵 → 四元数
反过来，从旋转矩阵 R 提取四元数：
w = 0.5 * sqrt(1 + R₁₁ + R₂₂ + R₃₃)
x = (R₃₂ - R₂₃) / (4w)
y = (R₁₃ - R₃₁) / (4w)
z = (R₂₁ - R₁₂) / (4w)注意，当 w 接近0时，这个公式会数值不稳定。我建议用另一种方法：
t = trace(R)
if t > 0:
s = 0.5 / sqrt(t)
w = 0.25 / s
x = (R₃₂ - R₂₃) * s
y = (R₁₃ - R₃₁) * s
z = (R₂₁ - R₁₂) * s
else:
# 处理对角线最大元素的情况
...3.3 四元数插值（SLERP）
为什么需要插值？你想想看，无人机从姿态A飞到姿态B，中间不能瞬间跳过去吧？需要平滑过渡。这就是 SLERP（球面线性插值）的用武之地。
SLERP 的公式：
q(t) = (sin((1-t)θ) / sinθ) * q₁ + (sin(tθ) / sinθ) * q₂其中 θ 是两个四元数之间的夹角：
cosθ = q₁ · q₂ = w₁w₂ + x₁x₂ + y₁y₂ + z₁z₂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:
q2 = -q2
dot = -dot
# 防止数值问题
if dot > 0.9995:
return lerp(q1, q2, t) # 线性插值近似
theta = acos(dot)
sin_theta = sin(theta)
s1 = sin((1-t) * theta) / sin_theta
s2 = sin(t * theta) / sin_theta
return q1 * s1 + q2 * s23.4 四元数在姿态解算中的应用
好了，理论讲完了。咱们来看看实际怎么用。在惯性导航中，我们用陀螺仪测角速度，然后更新四元数。
核心公式是四元数微分方程：
dq/dt = 0.5 * q ⊗ ω其中 ω 是角速度四元数 [0, ωx, ωy, ωz]，⊗ 表示四元数乘法。
离散化后：
q_new = q_old + 0.5 * q_old ⊗ ω * dt或者用更精确的一阶龙格-库塔：
q_new = q_old ⊗ [cos(||ω||*dt/2), (ω/||ω||)*sin(||ω||*dt/2)]我个人习惯用第二种方法，精度更高，而且天然保持单位四元数性质。
下面是一个完整的姿态更新示例：
class AttitudeEstimator:
def __init__(self):
self.q = Quaternion(1, 0, 0, 0) # 初始姿态
def update(self, gyro, dt):
# gyro: [wx, wy, wz] in rad/s
wx, wy, wz = gyro
omega_norm = sqrt(wx*wx + wy*wy + wz*wz)
if omega_norm < 1e-6:
return # 没有旋转
half_theta = omega_norm * dt / 2.0
sin_half = sin(half_theta) / omega_norm
dq = Quaternion(
cos(half_theta),
wx * sin_half,
wy * sin_half,
wz * sin_half
)
self.q = self.q * dq
self.q.normalize() # 别忘了归一化嗯，四元数这部分就讲到这里。你可能会问，为什么不用欧拉角？说白了，欧拉角在工程中就是个坑。我建议你从现在开始，所有姿态相关的计算都用四元数，只在最后可视化时转成欧拉角给人看。
可可精品资料库