三维空间刚体运动（下）：四元数与李群李代数

好，咱们接着聊。上一节我们把旋转矩阵和欧拉角掰扯清楚了，这一节要啃两块硬骨头——四元数和李群李代数。

说实话，我刚入行那会儿，看到四元数这四个字就头大。什么 i、j、k，还满足 i² = j² = k² = ijk = -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

你看，ijk 的乘法规则其实跟三维向量的叉积很像。我个人习惯把四元数记作 q = [w, v]，其中 v = (x, y, z) 是向量部分。

1. 四元数的加减法

跟向量一样，对应分量相加减：

q1 ± q2 = (w1 ± w2) + (x1 ± x2)i + (y1 ± y2)j + (z1 ± z2)k

2. 四元数的乘法

这个稍微复杂点。两个四元数相乘，结果还是四元数：

q1 * q2 = (w1*w2 - v1·v2) + (w1*v2 + w2*v1 + v1×v2)

注意，这里出现了点积和叉积。所以四元数乘法不满足交换律——q1*q2 ≠ q2*q1。我在项目里调试IMU姿态解算时，就因为这个顺序搞反过，结果姿态飞了半小时才反应过来。

3. 共轭与模长

四元数的共轭就是把虚部取反：

q* = w - xi - yj - zk

模长就是各分量平方和开根号：

||q|| = sqrt(w² + x² + y² + z²)

单位四元数的模长为1，旋转四元数必须是单位四元数。这个坑我踩过——归一化忘做了，结果旋转完点云全飘到天上去。

二、四元数与旋转矩阵的相互转换

为什么我们要用四元数？因为它在插值和避免万向锁方面比欧拉角强太多。但实际工程中，我们经常需要在四元数和旋转矩阵之间来回切换。

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² |

这个公式看着吓人，其实你写代码时直接套公式就行。我一般会封装成一个函数，每次调用前检查一下四元数模长是否接近1。

2. 旋转矩阵 → 四元数

反过来稍微麻烦点。从旋转矩阵 R 的迹（trace）出发：

tr = R[0][0] + R[1][1] + R[2][2]
w = sqrt(tr + 1) / 2
x = (R[2][1] - R[1][2]) / (4*w)
y = (R[0][2] - R[2][0]) / (4*w)
z = (R[1][0] - R[0][1]) / (4*w)

注意，当 tr 接近 -1 时，w 会接近0，这时候除法会出问题。我建议用 Shepperd 方法做数值稳定处理，网上有现成代码。

三、李群 SO(3) 与李代数 so(3) 简介

好，接下来是重头戏——李群和李代数。别被名字吓到，这东西在SLAM里天天用。

1. 什么是李群 SO(3)？

SO(3) 就是所有旋转矩阵的集合。它满足：

• R 是 3×3 矩阵
• RᵀR = I（正交性）
• det(R) = 1（右手系）

SO(3) 是一个群，因为旋转矩阵乘法封闭、有逆元、有单位元。但它不是向量空间——你不能把两个旋转矩阵加起来再期望它还是个旋转矩阵。

2. 李代数 so(3) 是什么？

so(3) 是 SO(3) 在单位元处的切空间。说白了，就是旋转矩阵的「导数」或「微小变化量」。so(3) 中的元素是 3×3 的反对称矩阵：

ω^ = | 0   -ωz  ωy |
     | ωz   0   -ωx |
     | -ωy  ωx   0  |

其中 ω = (ωx, ωy, ωz) 就是三维向量，对应旋转轴方向，模长对应旋转速度。

3. 指数映射与对数映射

SO(3) 和 so(3) 之间通过指数映射和对数映射相互转换：

• 指数映射：从 so(3) → SO(3)，即从角速度到旋转矩阵
• 对数映射：从 SO(3) → so(3)，即从旋转矩阵到角速度

指数映射的公式就是罗德里格斯公式：

R = exp(θ·n^) = I + sinθ·n^ + (1-cosθ)·(n^)²

其中 θ 是旋转角度，n 是单位旋转轴。

4. 为什么SLAM里要用李群李代数？

你想想看，如果我们直接用旋转矩阵做优化，每次迭代都要保证 RᵀR = I，这约束太强了。而用李代数，我们只需要优化一个三维向量，没有任何约束，优化完再映射回旋转矩阵就行。

我在做VIO（视觉惯性里程计）时，后端优化用的就是 so(3) 参数化。每次迭代更新 δω，然后通过指数映射更新旋转矩阵。这样既保证了旋转矩阵的正交性，又让优化变得简单。

四、知识体系总览

为了让你更直观地理解这一章的知识结构，我画了一张图：

这张图把三个核心概念的关系理清楚了。四元数和旋转矩阵可以互相转换，而李群李代数为我们提供了在优化中处理旋转的数学工具。

我个人觉得，学这部分内容最好的方式就是动手写代码。把四元数乘法、旋转矩阵转换、指数映射这些函数自己实现一遍，比看十遍书都管用。我在做第一个SLAM项目时，就是一边写代码一边理解这些概念的。

嗯，这一节的内容就到这儿。记住：四元数管表示，李代数管优化，两者配合才能打好SLAM的基础。