3D空间刚体运动基础：旋转矩阵、旋转向量、四元数、欧拉角、李群与李代数基础

各位同学，今天我们来聊聊3D空间里刚体运动的基础。说实话，这部分内容我当年学的时候也觉得有点绕，但后来做VIO项目时才发现——这些数学工具就是我们的“吃饭家伙”。你想想看，一个IMU在手里晃一下，怎么知道它转了多少？怎么把两帧之间的旋转平滑地表达出来？嗯，今天我们就把这些东西掰开揉碎了讲清楚。

旋转矩阵：最直观，但也最“笨重”

旋转矩阵，说白了就是一个3x3的正交矩阵，行列式为+1。它描述了一个坐标系相对于另一个坐标系的朝向。我个人习惯把旋转矩阵想象成一个“方向转换器”——你把一个向量喂给它，它就给你转到另一个坐标系里去。

举个例子，假设你在世界坐标系下有一个点 p，你想知道它在相机坐标系下的坐标 p'，那么：

p' = R * p

这里的R就是旋转矩阵。我在项目中遇到过一个问题：旋转矩阵虽然有9个元素，但自由度只有3个。这意味着什么？意味着你直接优化这9个参数，一定会引入冗余约束，导致数值不稳定。所以实际工程中，我们很少直接用旋转矩阵做优化变量。

旋转向量：用3个参数搞定旋转

既然旋转矩阵有冗余，那有没有更紧凑的表达？有，旋转向量。它用一个三维向量来表示旋转：向量的方向是旋转轴，向量的模长是旋转角度。说白了就是“绕某个轴转多少度”。

我记得有一次调试VIO系统，发现旋转矩阵的优化总是收敛很慢。后来换成旋转向量做参数化，迭代次数直接减半。为什么？因为旋转向量没有冗余约束，优化空间更“干净”。

旋转向量和旋转矩阵之间的转换，用罗德里格斯公式：

R = cosθ * I + (1 - cosθ) * n * nT + sinθ * n∧

其中θ是旋转角度，n是单位旋转轴，n^∧是n的反对称矩阵。

四元数：SLAM界的“瑞士军刀”

四元数，我愿称之为SLAM领域最优雅的旋转表达方式。它用4个参数（一个实部+三个虚部）来表示旋转，没有奇异性，插值平滑，计算效率高。

一个单位四元数 q = [w, x, y, z]^T，满足 w² + x² + y² + z² = 1。它对应的旋转矩阵是：

R = 2 * [ w² + x² - 0.5,  x*y - w*z,      x*z + w*y;
          x*y + w*z,      w² + y² - 0.5,  y*z - w*x;
          x*z - w*y,      y*z + w*x,      w² + z² - 0.5 ]

嗯，这个公式看着有点吓人，但你不用背，代码里直接调库就行。重要的是理解四元数的乘法：

q1 * q2 = [ w1*w2 - x1*x2 - y1*y2 - z1*z2,
            w1*x2 + x1*w2 + y1*z2 - z1*y2,
            w1*y2 - x1*z2 + y1*w2 + z1*x2,
            w1*z2 + x1*y2 - y1*x2 + z1*w2 ]T

这个乘法对应了旋转的复合。你想想看，先转q1再转q2，等价于一次转q2*q1（注意顺序！）。

欧拉角：直观但“坑”多

欧拉角用三个角度（比如roll、pitch、yaw）来描述旋转。直观是直观，但有两个致命问题：

• 万向锁：当pitch接近±90°时，roll和yaw会失去一个自由度。我在做无人机姿态估计时遇到过，飞机垂直俯冲时，yaw角突然跳变，吓得我赶紧切回了四元数。
• 顺序依赖：不同的旋转顺序（比如XYZ、ZYX）会得到不同的结果。团队协作时，一定要约定好顺序，否则代码合在一起就是灾难。

所以我的建议是：欧拉角只用来做可视化或人机交互，内部计算一律用四元数或旋转矩阵。

李群与李代数：优化问题的“利器”

这部分可能是大家觉得最抽象的。但说白了，李群就是连续的旋转群（SO(3)）或刚体运动群（SE(3)），而李代数就是它们在单位元处的切空间。

为什么要引入李代数？因为旋转矩阵的优化是一个带约束（正交且行列式为1）的问题，直接优化很麻烦。而李代数是一个无约束的向量空间，你可以在上面做高斯牛顿、LM等优化，然后通过指数映射回到李群。

SO(3)对应的李代数是so(3)，其实就是三维向量空间。指数映射把so(3)中的向量映射到SO(3)中的旋转矩阵：

exp(φ∧) = I + sinθ/θ * φ∧ + (1 - cosθ)/θ² * (φ∧)²

其中φ是旋转向量，θ是它的模长。这个公式和罗德里格斯公式其实是一回事。

我记得第一次在VIO系统中用李代数做优化时，代码量减少了一半，收敛速度也快了不少。说白了，这就是数学工具对工程实践的“降维打击”。

知识体系总览

下面这张图总结了本章的核心逻辑，我建议你把它存下来，以后做项目时对照着看：

从这张图你可以看到，四种旋转表达各有优劣。在实际的VIO系统中，我们通常用四元数做状态变量，用李代数做优化更新。说白了，就是“取各家之长”。

好了，这一章的内容就到这里。记住，数学工具是为工程服务的，别被公式吓到。多动手写代码，多跑实验，这些概念自然就刻在脑子里了。

---