4. 四元数基础：定义、运算与旋转矩阵转换
聊到机器人连杆坐标系的姿态描述，绕不开四元数。说实话，我刚入行时也觉得这东西有点玄乎——四个数就能表示三维旋转？后来在写一个机械臂的实时插补程序时，遇到了万向锁问题，才真正体会到四元数的好。今天咱们就把这个工具彻底搞明白。
4.1 四元数的定义
四元数可以看作复数的推广。一个复数有实部和虚部，四元数则有一个实部和三个虚部。我习惯把它写成这样：
q = w + xi + yj + zk其中 w 是实部，x、y、z 是虚部系数。i、j、k 是三个虚数单位，它们满足一个很关键的关系：
i² = j² = k² = ijk = -1这个关系决定了四元数的所有运算规则。你也可以把四元数看作一个标量和一个三维向量的组合：
q = [w, v] 其中 v = (x, y, z)在机器人学里，我们最常用的是单位四元数——它的模长为1。为什么？因为单位四元数正好对应三维空间中的旋转，没有缩放。
关键点：单位四元数满足 w² + x² + y² + z² = 1。任何旋转都可以用单位四元数唯一表示（除了正负号等价）。
4.2 四元数的基本运算
四元数的运算其实不复杂，记住几条规则就行。我在做运动学解算时天天跟它们打交道。
4.2.1 加法与数乘
加法和数乘都是逐分量进行的，跟向量一样：
q₁ + q₂ = (w₁+w₂) + (x₁+x₂)i + (y₁+y₂)j + (z₁+z₂)k
λq = (λw) + (λx)i + (λy)j + (λz)k4.2.2 乘法
四元数乘法就不一样了。它不满足交换律，也就是说 q₁q₂ ≠ q₂q₁。这一点跟矩阵乘法很像。乘法规则来自 i、j、k 的运算关系：
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₂)i
+ (w₁y₂ - x₁z₂ + y₁w₂ + z₁x₂)j
+ (w₁z₂ + x₁y₂ - y₁x₂ + z₁w₂)k我的经验：写代码时别手算这个展开式，直接用向量形式实现更不容易出错。我曾经在嵌入式平台上手写四元数乘法，结果把符号搞反了，机械臂转了个奇怪的角度……后来老老实实对照公式逐行检查。
4.2.3 共轭与模长
四元数的共轭就是把虚部取反：
q* = w - xi - yj - zk模长就是各分量平方和开根号：
|q| = √(w² + x² + y² + z²)单位四元数的模长就是1，它的逆等于共轭：q⁻¹ = q*。这个性质在旋转插值中特别有用。
4.3 四元数与旋转
好了，现在说重点——四元数怎么表示旋转？
假设我们要绕单位向量 n = (nx, ny, nz) 旋转 θ 角度，对应的四元数是：
q = cos(θ/2) + sin(θ/2)(nx i + ny j + nz k)注意角度是半角。为什么是半角？嗯，这跟四元数旋转的运算方式有关。对一个三维向量 p 进行旋转，得到 p'：
p' = q p q*这里 p 被看作实部为0的四元数 (0, p)。这个公式就是四元数旋转的核心。你想想看，一次乘法不够，要前后各乘一次，所以角度要折半。
容易踩的坑：我曾经在写姿态控制代码时，直接用 θ 而不是 θ/2 构造四元数，结果旋转角度翻了一倍。机械臂差点撞到旁边的设备。从那以后，我每次写四元数构造都会加一行注释提醒自己。
4.4 四元数与旋转矩阵的转换
在实际工程中，我们经常需要在四元数和旋转矩阵之间来回切换。比如从 IMU 读出来的是四元数，但运动学计算用的是旋转矩阵。
4.4.1 四元数 → 旋转矩阵
给定单位四元数 q = (w, x, y, z)，对应的旋转矩阵 R 为：
R = | 1-2y²-2z² 2xy-2zw 2xz+2yw |
| 2xy+2zw 1-2x²-2z² 2yz-2xw |
| 2xz-2yw 2yz+2xw 1-2x²-2y² |这个公式看起来有点长，但写代码时直接套用就行。我建议把它封装成一个函数，别每次都重新推导。
4.4.2 旋转矩阵 → 四元数
反过来转换稍微麻烦一点。从旋转矩阵 R 的元素提取四元数：
w = √(1 + R₁₁ + R₂₂ + R₃₃) / 2
x = (R₃₂ - R₂₃) / (4w)
y = (R₁₃ - R₃₁) / (4w)
z = (R₂₁ - R₁₂) / (4w)这里有个问题——如果 w 接近0，除法会放大数值误差。我一般会先判断哪个分量最大，用那个分量做分母：
if w 最大:
用 w 公式
elif x 最大:
x = √(1 + R₁₁ - R₂₂ - R₃₃) / 2
w = (R₃₂ - R₂₃) / (4x)
y = (R₁₂ + R₂₁) / (4x)
z = (R₁₃ + R₃₁) / (4x)
elif y 最大:
...类似处理
elif z 最大:
...类似处理避坑指南：我曾经在嵌入式平台上用单精度浮点做这个转换，结果在 w 接近0时出现了明显的数值抖动。后来改用双精度，并加了分支判断，问题就解决了。如果你的平台性能允许，尽量用 double。
4.5 四元数的优势与局限
我整理了一个表格，方便你对比四元数和旋转矩阵：
| 特性 | 四元数 | 旋转矩阵 |
|---|---|---|
| 存储空间 | 4个浮点数 | 9个浮点数 |
| 插值平滑性 | 球面线性插值(SLERP)，平滑 | 线性插值会导致非正交 |
| 万向锁 | 无 | 欧拉角有 |
| 组合旋转 | 乘法，计算量小 | 矩阵乘法，计算量大 |
| 直观性 | 不直观 | 较直观 |
| 数值稳定性 | 容易归一化保持单位 | 累积误差需正交化 |
说白了，四元数在计算效率和数值稳定性上占优，但不够直观。我个人的习惯是：内部计算全用四元数，只在调试和显示时转成旋转矩阵或欧拉角。
4.6 知识结构总览
下面这张图把四元数的核心脉络梳理了一遍。你可以把它当作快速参考：
四元数这东西，刚开始用会觉得别扭，但用顺手了就离不开。尤其是在做姿态插值和传感器融合时，它的优势太明显了。记住：内部计算用四元数，对外接口用旋转矩阵，这样既高效又方便调试。