3. 四元数：原理、运算与插值
说到四元数，很多刚接触SLAM的朋友第一反应就是「这玩意儿好抽象」。说实话，我当年学的时候也绕了好一阵子。但等你真正用起来，会发现它比欧拉角和旋转矩阵都顺手得多。尤其是在做姿态插值和连续旋转时，四元数几乎是唯一的选择。
3.1 为什么需要四元数？
先说说我们之前用的旋转矩阵和欧拉角有什么毛病。
- 旋转矩阵：9个参数描述3个自由度，冗余。而且矩阵乘法累积误差大，归一化也麻烦。
- 欧拉角：直观是直观，但有个致命问题——万向锁。当俯仰角接近±90°时，滚转和偏航会耦合，丢失一个自由度。
我在做无人机姿态估计时，就踩过万向锁的坑。当时用欧拉角做卡尔曼滤波，飞机一抬头，角度直接飞了。后来换成四元数，问题迎刃而解。
四元数的核心优势：
- 无奇异性，不会出现万向锁
- 只需要4个参数，比旋转矩阵少5个
- 插值平滑，适合做相机轨迹的平滑过渡
- 组合旋转效率高，只需一次乘法
3.2 四元数的数学定义
四元数可以看作复数的推广。一个复数有实部和虚部，四元数则有一个实部和三个虚部。
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 = [x, y, z]ᵀw 是实部，v 是虚部向量。
3.3 四元数的基本运算
这些运算在代码里天天用，我建议你直接背下来。
3.3.1 加法
q₁ + q₂ = [w₁+w₂, v₁+v₂]就是对应分量相加，没什么特别的。
3.3.2 乘法
四元数乘法不满足交换律，这点要特别注意。两个四元数相乘：
q₁ ⊗ q₂ = [w₁w₂ - v₁·v₂, w₁v₂ + w₂v₁ + v₁×v₂]你看，这里出现了点积和叉积。我在写代码时习惯把乘法封装成一个函数，避免手算出错。
// C++ 示例：四元数乘法
Quaternion operator*(const Quaternion& q1, const Quaternion& q2) {
return Quaternion(
q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z,
q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y,
q1.w * q2.y - q1.x * q2.z + q1.y * q2.w + q1.z * q2.x,
q1.w * q2.z + q1.x * q2.y - q1.y * q2.x + q1.z * q2.w
);
}3.3.3 共轭与模
四元数的共轭就是把虚部取反：
q* = [w, -v]模长计算和向量一样：
||q|| = sqrt(w² + x² + y² + z²)单位四元数的模为1，我们做旋转时用的就是单位四元数。
3.3.4 逆
q⁻¹ = q* / ||q||²对于单位四元数，逆就等于共轭。这个性质非常有用。
我的小技巧：在代码里，如果确定四元数是单位化的，直接用共轭代替求逆，能省一次除法运算。我在嵌入式平台上就是这么优化的。
3.4 用四元数表示旋转
这才是重头戏。一个三维空间中的点 p = (x, y, z)，用四元数表示为：
p_quat = [0, x, y, z]绕单位轴 u 旋转 θ 角度，对应的四元数为：
q = [cos(θ/2), u·sin(θ/2)]旋转后的点：
p' = q ⊗ p_quat ⊗ q⁻¹注意顺序：先乘 q，再乘 q⁻¹。这个公式你闭着眼睛都要能写出来。
我曾经犯过的错：在写代码时把乘法顺序搞反了，结果旋转方向完全不对。调试了一下午才发现是 q 和 q⁻¹ 的顺序问题。记住，是 q * p * q⁻¹，不是 q⁻¹ * p * q。
3.5 四元数与旋转矩阵的转换
实际工程中，我们经常需要在四元数和旋转矩阵之间来回切换。比如从IMU读到的数据是四元数，但做视觉SLAM时又需要旋转矩阵。
从四元数到旋转矩阵：
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²)]]从旋转矩阵到四元数稍微麻烦一点，需要根据迹的大小分情况讨论。这里我直接给代码：
// 旋转矩阵转四元数
Quaternion RotationMatrixToQuaternion(const Matrix3x3& R) {
Quaternion q;
float tr = R(0,0) + R(1,1) + R(2,2);
if (tr > 0) {
float S = sqrt(tr + 1.0) * 2;
q.w = 0.25 * S;
q.x = (R(2,1) - R(1,2)) / S;
q.y = (R(0,2) - R(2,0)) / S;
q.z = (R(1,0) - R(0,1)) / S;
} else if (R(0,0) > R(1,1) && R(0,0) > R(2,2)) {
// ... 其他分支情况
}
return q;
}嗯，这里要注意数值稳定性。当 tr 接近 -1 时，上面的公式会除以一个很小的数，导致误差放大。我一般会加一个阈值判断。
3.6 四元数插值
插值是四元数最实用的功能之一。比如你要让相机从姿态 A 平滑过渡到姿态 B，直接线性插值旋转矩阵会出问题，但四元数插值就非常自然。
3.6.1 线性插值（Lerp）
q(t) = (1-t)q₁ + t·q₂然后归一化。这个方法简单，但有个问题：插值出来的角速度不是恒定的。当夹角较大时，中间会「忽快忽慢」。
3.6.2 球面线性插值（Slerp）
这才是正统做法。Slerp 保证在单位球面上沿大圆弧运动，角速度恒定。
q(t) = [sin((1-t)θ) / sinθ] · q₁ + [sin(tθ) / sinθ] · q₂其中 θ 是 q₁ 和 q₂ 之间的夹角：
cosθ = q₁·q₂实际工程中的注意事项：
- 如果 q₁·q₂ < 0，说明它们方向相反，需要取反一个再插值，否则会绕远路
- 当 θ 接近 0 时，sinθ 接近 0，公式会数值不稳定。这时直接退化为 Lerp
- 我一般设一个阈值，当 cosθ > 0.999 时用 Lerp，否则用 Slerp
// Slerp 实现
Quaternion Slerp(const Quaternion& q1, const Quaternion& q2, float t) {
float cosTheta = q1.w*q2.w + q1.x*q2.x + q1.y*q2.y + q1.z*q2.z;
// 如果方向相反，取反
if (cosTheta < 0) {
q2 = -q2;
cosTheta = -cosTheta;
}
// 夹角很小时用线性插值
if (cosTheta > 0.999) {
return Lerp(q1, q2, t);
}
float theta = acos(cosTheta);
float sinTheta = sin(theta);
float scale1 = sin((1-t)*theta) / sinTheta;
float scale2 = sin(t*theta) / sinTheta;
return scale1 * q1 + scale2 * q2;
}3.7 知识体系总览
下面这张图是我自己整理的，把四元数的核心知识点串起来了。你照着这个结构学，不会乱。
3.8 避坑指南
最后分享几个我实际项目中踩过的坑：
- 忘记归一化：四元数经过多次乘法后，模长会偏离1。如果不重新归一化，旋转会逐渐变形。我习惯每次乘法后都调一下 normalize()。
- 搞混左右乘：四元数乘法不交换，左乘和右乘结果不同。在组合旋转时，一定要搞清楚坐标系变换的顺序。
- 插值方向反了：Slerp 时如果 q₁·q₂ 为负，插值会走「远路」。记得先判断符号，必要时取反。
- 万向锁的错觉：虽然四元数本身没有万向锁，但如果你从四元数转成欧拉角再转回来，还是会有问题。尽量全程用四元数。
我的习惯：在代码里统一用四元数做内部表示，只在可视化或人机交互时才转成欧拉角。这样既避免了万向锁，又方便调试。
好了，四元数这部分就讲到这里。你把它理解成「三维旋转的优雅表达」就行。多写几遍代码，自然就熟了。
可可精品资料库