三维空间刚体运动基础（下）：旋转向量与罗德里格斯公式、四元数基础、欧拉角与万向锁问题
上一节我们把旋转矩阵聊透了。但说实话，旋转矩阵有9个参数，却只有3个自由度，这太冗余了。而且你想想看，每次做插值或者优化的时候，还得保证它是正交阵，行列式为+1——这约束条件够烦人的。
所以工程上，我们很少直接用旋转矩阵去“表达”一个旋转过程。我更常用的是下面这三种：旋转向量、四元数、欧拉角。它们各有各的脾气，也各有各的坑。今天我就带你一个一个过一遍。
1. 旋转向量与罗德里格斯公式
旋转矩阵是9个数，但旋转本身只有3个自由度。那能不能用3个数表达旋转？当然可以——旋转向量（也叫轴角）就是干这个的。
核心思想：任何一个旋转，都可以用一个单位轴 n 和一个角度 θ 来描述。把 θ 乘上 n，就得到一个三维向量，这就是旋转向量。
旋转向量 vs 旋转矩阵
- 旋转向量：3个参数，无约束，直观
- 旋转矩阵：9个参数，带正交约束，冗余
那怎么从旋转向量变回旋转矩阵？罗德里格斯公式登场：
R = cosθ * I + (1 - cosθ) * n * nᵀ + sinθ * n^其中 n^ 是 n 的反对称矩阵。这个公式我手推过好几遍，每次推导都能加深理解。我个人习惯把它拆成三部分看：
- cosθ * I：投影到旋转轴的分量
- (1 - cosθ) * n * nᵀ：垂直于轴的分量在旋转后的补偿
- sinθ * n^：叉乘带来的切向分量
反过来，从旋转矩阵提取旋转向量也不难：
θ = arccos( (tr(R) - 1) / 2 )
n = (R - Rᵀ)∨ / (2 sinθ)注意数值稳定性：当 θ 接近 0 或 π 时，上面的公式会出问题。θ≈0 时 sinθ≈0，除法会爆炸；θ≈π 时旋转矩阵对称，无法从反对称部分提取轴。我在做 VIO 初始化时就被这个坑过——小角度下直接提取轴角，结果全是 NaN。
我的建议是：小角度时直接用泰勒展开近似；大角度时用奇异值分解（SVD）求轴。
2. 四元数基础
四元数，说实话，刚接触时我觉得这东西很反直觉。一个标量加三个虚部，怎么就表达旋转了？但用久了你会发现，它真香。
四元数的定义：
q = w + xi + yj + zk
其中 i² = j² = k² = ijk = -1单位四元数（模长为1）可以用来表示旋转。给定旋转轴 n 和角度 θ：
q = [cos(θ/2), n * sin(θ/2)]注意这里用的是半角。为什么是半角？因为四元数乘法对应旋转的复合，半角能保证两次旋转后角度正确叠加。这个设计很巧妙，我第一次推导时还愣了半天。
我的习惯：在代码里，我通常用 Hamilton 约定（左乘），并且把四元数存成 [w, x, y, z] 的顺序。Eigen 和 ROS 都是这个约定，保持一致能少很多 bug。
四元数的运算：
- 乘法：对应旋转的复合，注意不满足交换律
- 共轭：对应逆旋转
- 旋转点：p' = q * p * q⁻¹（p 是纯虚四元数）
四元数最大的优势是：无奇点、可插值、适合优化。SLAM 后端优化里，我几乎只用四元数做位姿图优化，因为它的雅可比矩阵形式简单，而且不会像欧拉角那样突然“卡住”。
3. 欧拉角与万向锁问题
欧拉角可能是最直观的旋转表达方式了——绕 X 轴转多少，绕 Y 轴转多少，绕 Z 轴转多少。但直观归直观，坑也最大。
常见的欧拉角约定：
| 约定 | 旋转顺序 | 常见应用 |
|---|---|---|
| ZYX（偏航-俯仰-滚转） | 先绕 Z，再绕 Y，最后绕 X | 无人机、机器人 |
| ZYZ | 先绕 Z，再绕 Y，再绕 Z | 机械臂运动学 |
| XYZ | 先绕 X，再绕 Y，再绕 Z | 某些视觉系统 |
万向锁（Gimbal Lock）：当第二个旋转角为 ±90° 时，第一个和第三个旋转轴会重合，丢失一个自由度。这时候你无论怎么调第一个和第三个角度，都只能在一个平面内旋转。
我曾经踩过的坑：在做无人机姿态估计时，用了欧拉角做 EKF 的状态量。结果飞机做大俯仰机动时，协方差矩阵突然奇异，滤波器直接发散。查了两天才发现是万向锁导致的——俯仰角接近 90° 时，偏航和滚转不可区分，雅可比矩阵秩亏。
从那以后，我在任何需要做姿态估计或优化的系统里，都只用四元数或旋转矩阵。欧拉角我只在可视化或人机交互时用——毕竟给人看，还是偏航俯仰滚转最直观。
4. 三种表达方式的对比
| 特性 | 旋转向量 | 四元数 | 欧拉角 |
|---|---|---|---|
| 参数数量 | 3 | 4（单位约束） | 3 |
| 奇异性 | θ=0, π 时奇异 | 无奇点 | 万向锁 |
| 插值 | 线性插值不自然 | Slerp 平滑 | 插值结果依赖顺序 |
| 优化友好度 | 中等（需处理小角度） | 高（流形优化） | 低（奇点附近雅可比病态） |
| 直观性 | 中等 | 低 | 高 |
我的选择策略很简单：
- 做优化：用四元数（加流形约束）
- 做初始化：用旋转向量（无冗余参数）
- 给人看：转成欧拉角
5. 知识结构图
下面这张图梳理了三种旋转表达方式的核心关系，以及它们之间的转换路径：
6. 实战中的选择建议
嗯，说了这么多理论，最后给点实在的。你在实际项目中到底该用哪个？
我的经验法则：
- SLAM 后端优化：四元数 + 李代数扰动模型。这是主流方案，g2o、Ceres、GTSAM 都支持。
- IMU 预积分：旋转向量。因为 IMU 测量的是角速度，积分后自然得到旋转向量，再转成四元数。
- 手眼标定：旋转向量。求解 AX=XB 时，旋转向量形式能直接线性化。
- 可视化调试：欧拉角。但记得加保护，检测俯仰角是否接近 ±90°。
我曾经在一个项目里，因为偷懒直接用欧拉角做 EKF 状态量，结果在无人机做大机动时滤波器崩溃。后来改成四元数 + 误差状态卡尔曼滤波（ESKF），问题就解决了。所以我的建议是：别在核心算法里用欧拉角，它只适合给人看。
好了，这一节的内容就到这里。三种旋转表达方式各有千秋，但记住一点：没有银弹。理解它们的数学本质和工程特性，才能在合适的场景选合适的工具。