2、三维空间刚体运动（上）：坐标系定义与欧拉角、旋转矩阵的性质与推导、旋转向量与罗德里格斯公式
2.1 坐标系定义——先把这个说清楚
做SLAM这么多年，我见过太多同学在坐标系上栽跟头。说白了，坐标系没定好，后面全是白搭。
三维空间里，我们通常用右手坐标系。怎么判断？伸出右手，拇指指向X轴，食指指向Y轴，中指指向Z轴——这就是右手系。我习惯把Z轴朝上，这样符合直觉，比如无人机的高度就是Z值。
在SLAM里，常见的坐标系有这么几个：
- 世界坐标系：固定不动，通常选在起始位置。我一般把Z轴朝上，X轴指向正北。
- 相机坐标系：跟着相机走。Z轴指向镜头前方，X轴向右，Y轴向下——嗯，这是图像坐标系的习惯。
- IMU坐标系：跟着IMU走。不同IMU定义不同，用之前一定要看数据手册。
核心要点：坐标系之间的转换，本质就是刚体运动。刚体运动包含旋转和平移，旋转用旋转矩阵或四元数表示，平移用向量表示。
我曾经在项目里遇到过一个坑：IMU和相机的坐标系定义不一致，结果融合出来的轨迹歪了45度。后来花了整整两天才排查出来——就是坐标系定义没对齐。
2.2 欧拉角——直观但容易踩坑
欧拉角是最直观的旋转表示方式。你想想看，一个物体怎么转？绕X轴转一下，再绕Y轴转一下，最后绕Z轴转一下。三个角度搞定。
但这里有个大问题：万向锁。
什么意思？当绕Y轴旋转±90度时，X轴和Z轴就重合了。这时候你丢失了一个自由度。我刚开始做姿态估计时，就因为这个吃了大亏——无人机在俯仰90度时，航向角突然乱跳。
避坑指南：在SLAM里，尽量不要用欧拉角做内部计算。它只适合给人看，或者做简单的姿态初始化。我曾经在IMU预积分里用欧拉角，结果姿态发散得一塌糊涂。
欧拉角的定义顺序也很重要。常见的顺序有：
| 顺序 | 说明 | 常见应用 |
|---|---|---|
| ZYX | 先绕Z轴转（偏航），再绕Y轴转（俯仰），最后绕X轴转（横滚） | 无人机、机器人 |
| XYZ | 先绕X轴转，再绕Y轴转，最后绕Z轴转 | 某些机械臂 |
| ZYZ | 先绕Z轴转，再绕Y轴转，最后再绕Z轴转 | 某些数学推导 |
我个人习惯用ZYX顺序，因为偏航-俯仰-横滚的顺序最符合直觉。但记住：不同顺序得到的角度值完全不同，千万别混用。
2.3 旋转矩阵——最基础也最可靠
旋转矩阵是3x3的正交矩阵，行列式为+1。它描述了一个坐标系相对于另一个坐标系的旋转关系。
旋转矩阵的性质，我总结了几条：
- 正交性：RTR = I，R-1 = RT
- 行列式为1：det(R) = 1
- 封闭性：两个旋转矩阵相乘，结果还是旋转矩阵
- 不满足交换律：R1R2 ≠ R2R1
小技巧：验证一个矩阵是不是旋转矩阵，就检查RTR是否等于单位阵，以及行列式是否为1。我在代码里经常加这个检查，能避免很多bug。
旋转矩阵的推导，其实很简单。假设一个点P在世界坐标系下的坐标为Pw，在相机坐标系下的坐标为Pc，那么：
P_c = R * P_w + t其中R是旋转矩阵，t是平移向量。这个公式就是SLAM里最基础的坐标变换公式。
旋转矩阵的逆就是它的转置，这个性质非常有用。比如你要把相机坐标系下的点转回世界坐标系：
P_w = R^T * (P_c - t)嗯，这里要注意：平移向量t是在世界坐标系下表示的相机原点位置。
2.4 旋转向量与罗德里格斯公式
旋转矩阵有9个参数，但自由度只有3个。说白了，它太冗余了。旋转向量就是更紧凑的表示方式——用一个三维向量表示旋转，方向是旋转轴，长度是旋转角度。
旋转向量和旋转矩阵之间的转换，靠的就是罗德里格斯公式：
R = I + sin(θ) * [n]× + (1 - cos(θ)) * [n]ײ其中n是单位旋转轴，θ是旋转角度，[n]×是n的反对称矩阵。
这个公式怎么来的？我简单说下思路：
- 把向量分解成平行于旋转轴和垂直于旋转轴两部分
- 平行部分不变
- 垂直部分绕轴旋转θ角度
- 合并得到旋转后的向量
反过来，从旋转矩阵求旋转向量：
θ = arccos((tr(R) - 1) / 2)
n = (R - R^T) / (2 * sin(θ))这里有个特殊情况：当θ接近0时，sin(θ)也接近0，数值不稳定。我一般会加个阈值判断，如果θ很小，直接用泰勒展开近似。
实战经验：在IMU预积分里，旋转向量比旋转矩阵更常用。因为旋转向量可以方便地进行插值和求导。我做的VINS-Mono系统里，IMU预积分用的就是旋转向量。
2.5 本章知识体系
为了让你更直观地理解本章的知识结构，我画了张图：
这张图把本章的核心内容串起来了。从刚体运动出发，引出三种表示方式：坐标系定义、欧拉角、旋转矩阵。最后旋转向量和罗德里格斯公式作为旋转矩阵的紧凑表示，是连接上下两章的桥梁。
我的建议：刚开始学的时候，先把旋转矩阵吃透。它虽然参数多，但性质好、推导简单。等熟练了，再慢慢过渡到旋转向量和四元数。别一上来就搞四元数，容易晕。
好了，这一章就到这里。旋转矩阵和欧拉角是基础中的基础，下一章我们会继续聊旋转的另一种表示——四元数，以及它们之间的互相转换。
可可精品资料库