3、数学基础（上）：三维空间刚体运动、旋转矩阵、四元数、欧拉角、李群与李代数
各位同学，欢迎来到数学基础部分。说实话，很多做SLAM的朋友一听到数学就头疼。我当年刚入行时也一样，觉得搞机器人不就是写写代码嘛，干嘛要跟矩阵、四元数过不去？直到我在一个实际项目中，因为对旋转表示理解不到位，导致整个建图系统出现严重的漂移……嗯，从那以后我老老实实把这块啃了下来。
今天这节内容，我们只讲一件事：如何描述一个刚体在三维空间中的运动。说白了，就是回答两个问题——它转了多少？它挪到了哪？
3.1 旋转矩阵：最直观的旋转表示
先从一个简单场景说起。假设你面前有一个相机，它朝正前方拍了一张图。现在你把相机绕Z轴转了30度，再拍一张。这两张图之间的像素对应关系，就需要用旋转来描述。
旋转矩阵R是一个3x3的正交矩阵，满足R^T R = I，且det(R)=1。它把三维空间中的一个向量，从一个坐标系变换到另一个坐标系。
核心公式：p' = R · p
其中p是原始坐标系下的坐标，p'是旋转后的坐标。
我在项目中遇到过一个问题：直接用旋转矩阵做连续旋转时，由于浮点数误差累积，矩阵会慢慢偏离正交性。这时候就需要做正交化处理。我建议每次更新旋转矩阵后，都做一次SVD分解来修正。
// 旋转矩阵正交化示例（C++）
Eigen::Matrix3d R;
// ... 经过多次乘法后，R不再正交
Eigen::JacobiSVD<Eigen::Matrix3d> svd(R, Eigen::ComputeFullU | Eigen::ComputeFullV);
R = svd.matrixU() * svd.matrixV().transpose(); // 强制正交化注意：旋转矩阵有9个参数，但自由度只有3。这意味着它存在冗余。冗余带来的好处是计算稳定，坏处是占用内存大、效率低。在实际SLAM系统中，我们很少直接用旋转矩阵做优化。
3.2 欧拉角：人类最友好的旋转描述
欧拉角用三个角度来描述旋转：绕X轴转（roll）、绕Y轴转（pitch）、绕Z轴转（yaw）。你想想看，这多符合直觉啊！飞机飞行员就用这套东西。
但欧拉角有个致命问题——万向锁。当pitch接近±90度时，roll和yaw会变得无法区分。我曾经在一个无人机项目中，用欧拉角做姿态控制，结果飞机在做大角度俯冲时直接失控……后来我查了日志，发现就是万向锁导致的。
| 旋转顺序 | 常见应用场景 | 万向锁风险 |
|---|---|---|
| ZYX（偏航-俯仰-滚转） | 无人机、车辆姿态 | pitch=±90°时 |
| XYZ（滚转-俯仰-偏航） | 机械臂末端 | pitch=±90°时 |
| ZYZ | 理论分析 | 第二个Z=0或π时 |
我的建议：欧拉角只适合做人机交互（比如给用户看姿态数值），或者做小角度近似（比如IMU的初始对准）。在SLAM后端优化中，千万别用欧拉角做状态变量。
3.3 四元数：SLAM中的主力军
四元数是什么？说白了就是一个实部加三个虚部：q = w + xi + yj + zk。它用4个参数表示旋转，没有奇异性，而且插值方便。
我个人习惯用单位四元数来表示旋转。单位四元数满足w² + x² + y² + z² = 1。它和旋转矩阵可以互相转换，转换公式如下：
// 四元数转旋转矩阵
// 假设四元数 q = [w, x, y, z]
R = | 1-2y²-2z² 2xy-2wz 2xz+2wy |
| 2xy+2wz 1-2x²-2z² 2yz-2wx |
| 2xz-2wy 2yz+2wx 1-2x²-2y² |四元数乘法对应旋转的复合。注意：q1 * q2 表示先做q2旋转，再做q1旋转。这个顺序我经常搞混，后来我记住一个口诀——从左到右，先右后左。
避坑指南：我曾经在代码里直接用Eigen的Quaterniond做乘法，结果发现它默认的乘法顺序和数学定义不一样。Eigen中 q1 * q2 对应的是先q1后q2。所以一定要看文档！
3.4 李群与李代数：SLAM优化的核心工具
好了，前面讲的都是怎么表示旋转。但SLAM的核心是优化——我们要不断调整位姿，让误差最小化。这就涉及到一个问题：怎么对旋转求导？
旋转矩阵R是正交的，R + ΔR 就不再是旋转矩阵了。所以你不能直接在旋转矩阵上做加法。这时候李群和李代数就派上用场了。
李群SO(3)是所有旋转矩阵的集合。李代数so(3)是它的切空间，说白了就是三维向量φ = [φ₁, φ₂, φ₃]^T。从李代数到李群的映射叫指数映射：
R = exp(φ^) // φ^ 是 φ 的反对称矩阵反过来，从李群到李代数叫对数映射：
φ = log(R)为什么要搞这么复杂？因为李代数是一个向量空间，你可以直接在它上面做加法、做优化。优化完成后，再通过指数映射回到旋转矩阵。
实际用法：在g2o、Ceres等优化库中，位姿节点通常用李代数参数化。比如在Ceres中，你可以用Sophus::SE3作为参数块，自动处理求导。我建议初学者直接用这些现成的库，不要自己手写李代数求导——太容易出错了。
3.5 总结与对比
好了，我们来梳理一下这几种旋转表示法的特点：
| 表示方法 | 参数数量 | 奇异性 | 计算效率 | 适用场景 |
|---|---|---|---|---|
| 旋转矩阵 | 9 | 无 | 中等 | 坐标变换、可视化 |
| 欧拉角 | 3 | 有（万向锁） | 高 | 人机交互、小角度 |
| 四元数 | 4 | 无 | 高 | SLAM前端、插值 |
| 李代数 | 3 | 无 | 中等 | SLAM后端优化 |
我个人在实际项目中的选择策略是这样的：
- 前端里程计：用四元数，因为插值方便，而且没有奇异性
- 后端图优化：用李代数，因为优化时需要求导
- 可视化与调试：用欧拉角，方便人看
- 坐标变换计算：用旋转矩阵，因为乘法直观
最后说一句：数学基础这东西，光看是学不会的。我建议你打开Eigen或Sophus的文档，把今天讲的这些函数一个个跑一遍。遇到报错不要怕，那都是学习的过程。下一节我们会讲平移、变换矩阵和SE(3)，到时候这些旋转表示法会组合起来用，你会更清楚它们的价值。
课后练习：写一个程序，用四元数生成一个随机旋转，然后分别转换成旋转矩阵和欧拉角，验证它们的一致性。再试试用李代数做一个小优化——比如给定两组对应点，用李代数求解最优旋转。