第2章三维空间刚体运动

2. 三维空间刚体运动：旋转矩阵、旋转向量、欧拉角、四元数、Eigen库基础使用

各位同学，欢迎来到第二章。这一章我们聊点硬核的——三维空间里的刚体运动。

说实话，我刚入行做机器人时，觉得旋转不就是转个角度嘛，有什么好学的？直到第一次在项目里把四元数搞反了顺序，导致机械臂直接撞上了旁边的设备……嗯，从那以后我再也不敢小看这些数学工具了。

这一章，我会带你彻底搞懂旋转矩阵、旋转向量、欧拉角、四元数这四种表达方式，以及怎么用Eigen库在代码里操作它们。你想想看，搞懂了这些，SLAM里的位姿估计、VIO里的IMU预积分，你都能轻松拿捏。

2.1 旋转矩阵：最直观，但最啰嗦

旋转矩阵，说白了就是一个3x3的正交矩阵，行列式为+1。它描述了一个坐标系相对于另一个坐标系的朝向。

假设我们有一个点 p，在世界坐标系下的坐标为 p_w，在相机坐标系下的坐标为 p_c。那么：

p_c = R * p_w + t

这里的 R 就是旋转矩阵，t 是平移向量。

我个人习惯把旋转矩阵理解为「坐标系的映射器」。你给它一个世界坐标，它告诉你这个点在相机眼里长什么样。

  核心性质：
  正交矩阵：R^T * R = I
行列式为+1（纯旋转，不含反射）
9个元素，但只有3个自由度

为什么说它啰嗦？因为9个参数描述3个自由度，太冗余了。而且你每次更新它，还得保证它仍然是正交矩阵——这可不是件容易事。

避坑指南： 我曾经在EKF里直接对旋转矩阵做加法更新，结果矩阵慢慢变得不正交了，导致整个滤波器发散。后来才意识到，旋转矩阵应该在流形上更新，而不是欧式空间。

2.2 旋转向量：紧凑但奇异

旋转向量，也叫轴角（Axis-Angle）。它用一个三维向量表示旋转：方向代表旋转轴，模长代表旋转角度。

公式很简单：

θ = ||v||   // 旋转角度
n = v / θ   // 单位旋转轴

从旋转向量到旋转矩阵，用罗德里格斯公式：

R = cosθ * I + (1 - cosθ) * n * n^T + sinθ * n^

其中 n^ 是 n 的反对称矩阵。

你想想看，旋转向量只有3个参数，比旋转矩阵的9个少多了。但它有个问题：当θ接近0或π时，数值不稳定。θ=0时，旋转轴无法定义；θ=π时，旋转方向有歧义。

我的经验： 在VIO初始化阶段，我经常用旋转向量来表示小角度旋转，因为这时候θ很小，数值问题不明显。但一旦角度大了，我建议赶紧转成四元数。

2.3 欧拉角：直观但万向锁

欧拉角大家应该不陌生，就是绕三个轴依次旋转：偏航（Yaw）、俯仰（Pitch）、滚转（Roll）。

常见的顺序是ZYX：先绕Z轴转Yaw，再绕Y轴转Pitch，最后绕X轴转Roll。

角度	轴	范围
Yaw（偏航）	Z	[-π, π]
Pitch（俯仰）	Y	[-π/2, π/2]
Roll（滚转）	X	[-π, π]

欧拉角最大的问题是什么？万向锁（Gimbal Lock）。当Pitch接近±90°时，Yaw和Roll的旋转轴会重合，丢失一个自由度。

我曾经踩过的坑： 在无人机飞控里用欧拉角做姿态控制，结果飞机做大机动时Pitch过了90°，Yaw和Roll直接乱套了。从那以后，我所有涉及大角度运动的项目，一律用四元数。

所以我的建议是：欧拉角只适合给人看（比如显示姿态），不适合做计算。

2.4 四元数：SLAM和VIO的标配

四元数，说白了就是一个4维的复数扩展。形式是：

q = w + x*i + y*j + z*k

其中 w 是实部，(x, y, z) 是虚部。单位四元数（模长为1）可以用来表示旋转。

为什么SLAM和VIO都用四元数？三个原因：

无奇异性： 没有万向锁问题
紧凑： 4个参数，比旋转矩阵的9个少
易插值： 球面线性插值（SLERP）非常平滑

四元数的乘法不满足交换律，这个要注意。两个四元数 q1 和 q2 的乘积：

q = q1 * q2

表示先应用 q2 的旋转，再应用 q1 的旋转。顺序很重要！

  四元数 vs 旋转矩阵：
  四元数乘法：12次浮点运算
旋转矩阵乘法：27次浮点运算
四元数效率更高，尤其在链式旋转时

我个人习惯在优化器里用四元数，因为它的更新只需要在切空间上做加法，然后归一化就行，比旋转矩阵的更新简单多了。

2.5 Eigen库基础使用

Eigen是C++里最常用的线性代数库，没有之一。SLAM和VIO的代码里，你到处都能看到它的身影。

先看怎么定义旋转矩阵：

#include <Eigen/Dense>

// 定义3x3旋转矩阵
Eigen::Matrix3d R;
R = Eigen::AngleAxisd(M_PI/4, Eigen::Vector3d::UnitZ()) *
    Eigen::AngleAxisd(M_PI/6, Eigen::Vector3d::UnitY());

// 或者用四元数构造
Eigen::Quaterniond q(1.0, 0.0, 0.0, 0.0);  // w, x, y, z
q.normalize();
Eigen::Matrix3d R_from_q = q.toRotationMatrix();

旋转向量和四元数的转换：

// 旋转向量 -> 四元数
Eigen::AngleAxisd angle_axis(M_PI/3, Eigen::Vector3d::UnitX());
Eigen::Quaterniond q_from_av(angle_axis);

// 四元数 -> 旋转向量
Eigen::AngleAxisd av_from_q(q_from_av);

欧拉角在Eigen里没有直接支持，但你可以自己写：

// 从旋转矩阵提取欧拉角 (ZYX顺序)
Eigen::Vector3d euler = R.eulerAngles(2, 1, 0);  // Z, Y, X
double yaw = euler[0];
double pitch = euler[1];
double roll = euler[2];

我的习惯： 在代码里，我统一用四元数做内部存储和计算，只在需要显示或保存时转成欧拉角。这样既避免了万向锁，又方便调试。

最后，别忘了Eigen的矩阵运算非常高效，因为它用了表达式模板（Expression Templates）技术。你写 R * p + t 这种表达式，它不会生成临时变量，而是直接计算最终结果。

2.6 知识体系总览

下面这张图，是我自己总结的四种旋转表达方式的关系图。你把它存下来，以后写代码时对照着看，能少踩很多坑。

这张图里，我特意把欧拉角和四元数的连线标成了「慎用」。因为在实际项目中，我见过太多人在欧拉角和四元数之间来回转换，结果角度符号搞反了，或者顺序搞错了。

  总结一下我的建议：
  做理论推导时，用旋转矩阵（直观）
做优化时，用四元数（无奇异、易更新）
做可视化时，用欧拉角（给人看）
做初始化时，用旋转向量（参数少）

好了，这一章的内容就到这里。记住，旋转是SLAM和VIO的基石，搞不懂它，后面的位姿图优化、IMU预积分你都会很痛苦。多写代码，多画图，慢慢就熟了。

📚 课程目录

2. 三维空间刚体运动：旋转矩阵、旋转向量、欧拉角、四元数、Eigen库基础使用

2.1 旋转矩阵：最直观，但最啰嗦

2.2 旋转向量：紧凑但奇异

2.3 欧拉角：直观但万向锁

2.4 四元数：SLAM和VIO的标配

2.5 Eigen库基础使用

2.6 知识体系总览