第四节：四元数入门
四元数这东西，我刚入行时觉得它特别玄乎。说实话，第一次看到那四个分量，我脑子里全是问号——这玩意儿怎么就能表示旋转了？后来在项目里被欧拉角狠狠坑过一次，才老老实实把四元数啃了下来。
今天咱们就把四元数掰开揉碎。你不用担心数学基础，我会用最直白的方式讲清楚。
4.1 四元数的定义
四元数本质上是一个超复数。普通复数有一个实部和一个虚部，四元数有一个实部和三个虚部。
数学上写成这样：
q = w + xi + yj + zk其中 w 是实部，x、y、z 是虚部。i、j、k 是三个虚数单位，它们满足：
- i² = j² = k² = -1
- ij = k, jk = i, ki = j
- ji = -k, kj = -i, ik = -j
你看，这跟复数很像，但多了两个维度。我个人习惯把四元数看作一个标量加上一个三维向量：
q = [w, v] 其中 v = (x, y, z)为什么要这么看？因为后面做旋转时，这个视角特别直观。
单位四元数：用于表示旋转的四元数，其模长必须为1。
||q|| = sqrt(w² + x² + y² + z²) = 1
嗯，这里要注意：不是所有四元数都能表示旋转。只有单位四元数才行。我在项目中遇到过有人直接把传感器输出的四元数拿来用，结果发现旋转矩阵的行列式不是1——那就是没归一化。
4.2 四元数与旋转矩阵的转换
这部分是联调时的硬功夫。你肯定要在代码里来回转换，我建议你直接记住公式，别每次都推导。
4.2.1 四元数 → 旋转矩阵
给定单位四元数 q = (w, x, y, z)，对应的旋转矩阵 R 为：
R = | 1-2y²-2z² 2xy-2wz 2xz+2wy |
| 2xy+2wz 1-2x²-2z² 2yz-2wx |
| 2xz-2wy 2yz+2wx 1-2x²-2y² |这个公式看着长，但你别怕。写代码时直接套用就行。我曾经手写过一个版本，结果把符号搞反了，调试了整整一个下午才发现是 2xy-2wz 写成了 2xy+2wz。你想想看，一个符号错了，整个旋转方向就反了。
4.2.2 旋转矩阵 → 四元数
反过来转换稍微麻烦一点。从旋转矩阵 R 提取四元数：
w = sqrt(1 + R[0][0] + R[1][1] + R[2][2]) / 2
x = (R[2][1] - R[1][2]) / (4w)
y = (R[0][2] - R[2][0]) / (4w)
z = (R[1][0] - R[0][1]) / (4w)这里有个坑：如果 w 接近0，除法会出问题。我建议你判断一下 w 的绝对值，如果太小就换一种方式计算。
数值稳定性问题：当 w 接近0时，用 x、y、z 中最大的那个做分母。具体做法是：先找出 R 对角线上最大的元素，然后根据它计算对应的四元数分量。
4.3 四元数的插值应用
这部分在实际工程中太常用了。比如你要让相机从姿态A平滑过渡到姿态B，或者做路径规划中的姿态插值。
4.3.1 线性插值（LERP）
最简单的做法：
q(t) = (1-t) * q1 + t * q2但这样插出来的四元数不是单位四元数，需要归一化：
q(t) = normalize((1-t) * q1 + t * q2)说白了就是先线性混合，再归一化。这个方法计算量小，但角速度不均匀。如果你对精度要求不高，用这个就够了。
4.3.2 球面线性插值（SLERP）
这才是正经做法。SLERP 保证插值过程中角速度恒定，旋转路径是测地线。
q(t) = (sin((1-t)θ) / sinθ) * q1 + (sin(tθ) / sinθ) * q2其中 θ 是 q1 和 q2 之间的夹角：
cosθ = q1 · q2 = w1*w2 + x1*x2 + y1*y2 + z1*z2我的经验：SLERP 实现时要注意两点：
- 如果 cosθ 接近1，说明两个四元数几乎相同，直接用 LERP 就行
- 如果 cosθ 是负数，取反一个四元数，走短路径
我曾经在自动驾驶项目中用 SLERP 做轨迹插值。当时车辆要从当前姿态转到目标姿态，如果用欧拉角插值，会出现万向锁，车会突然翻一下。换成四元数 SLERP 后，旋转就顺滑了。
4.4 知识体系总览
下面这张图帮你理清四元数相关的核心逻辑：
4.5 代码示例
最后给你一个 Python 实现，这是我项目里常用的版本：
import numpy as np
def quaternion_to_rotation_matrix(q):
"""四元数转旋转矩阵"""
w, x, y, z = q
R = np.array([
[1-2*y*y-2*z*z, 2*x*y-2*w*z, 2*x*z+2*w*y],
[2*x*y+2*w*z, 1-2*x*x-2*z*z, 2*y*z-2*w*x],
[2*x*z-2*w*y, 2*y*z+2*w*x, 1-2*x*x-2*y*y]
])
return R
def slerp(q1, q2, t):
"""球面线性插值"""
cos_theta = np.dot(q1, q2)
# 如果cos_theta为负，取反走短路径
if cos_theta < 0:
q2 = -q2
cos_theta = -cos_theta
# 如果夹角太小，用LERP
if cos_theta > 0.9995:
result = (1-t) * q1 + t * q2
return result / np.linalg.norm(result)
theta = np.arccos(cos_theta)
sin_theta = np.sin(theta)
q = (np.sin((1-t)*theta) / sin_theta) * q1 + \
(np.sin(t*theta) / sin_theta) * q2
return q避坑指南：我曾经在 SLERP 里忘记处理 cos_theta 为负的情况，结果插值路径绕了大半个球面。后来加了这个判断，路径就对了。你写代码时一定别忘了这一步。
好了，四元数入门就讲到这里。你只要记住三点：定义是基础，转换是工具，插值是应用。把这三点吃透，后面做多传感器融合时，姿态处理这块就不会卡壳了。
可可精品资料库