3. 刚体变换与位姿表示：旋转矩阵、平移向量、齐次坐标、欧拉角、四元数
各位同学，欢迎来到第三讲。
上一章我们聊了相机模型和成像原理，说白了就是搞清楚了「像素坐标」和「物理坐标」之间怎么换算。但有个问题一直悬着：相机坐标系和世界坐标系之间，到底怎么对齐？
嗯，这就是我们今天要啃的硬骨头——刚体变换与位姿表示。
我个人习惯把这一章叫做「标定的数学灵魂」。你想想看，机器人手眼标定、自动驾驶的车辆定位、AR眼镜的虚实叠加……哪一样离得开这玩意儿？我当年刚入行时，被欧拉角的万向锁坑过好几次，后来老老实实换成了四元数，才算是彻底解脱。
核心观点：刚体变换 = 旋转 + 平移。旋转用矩阵、欧拉角、四元数三种方式表示，各有优劣。齐次坐标把旋转和平移统一成一次矩阵乘法，是工程实现的标准姿势。
3.1 刚体变换：不改变形状的移动
什么叫刚体？说白了就是「硬邦邦的物体」，你推它、转它，它不会变形。在三维空间里，一个刚体从位置A到位置B，只可能发生两件事：旋转和平移。
数学上，刚体变换可以写成：
P' = R * P + t其中：
- P 是原始点坐标（3×1向量）
- R 是旋转矩阵（3×3正交矩阵）
- t 是平移向量（3×1向量）
- P' 是变换后的点坐标
我在做机器人标定时，经常需要把机械臂末端坐标系下的点，变换到相机坐标系下。说白了，就是解一个 R 和 t 的问题。但这里有个坑——R 必须是正交矩阵，也就是 R^T * R = I，且 det(R) = 1。如果你随便填几个数进去，大概率会得到一个「扭曲」的变换，标定结果直接崩掉。
避坑指南：我曾经在项目中直接用数值优化出来的矩阵当旋转矩阵用，结果发现物体越转越「缩水」。后来才意识到，必须对矩阵做正交化处理（比如SVD分解后强制正交）。记住：旋转矩阵不是随便一个3×3矩阵，它必须满足正交性和行列式为+1。
3.2 旋转矩阵：最直观，但最啰嗦
旋转矩阵 R 是一个 3×3 的矩阵，它的每一列代表旋转后的坐标轴在原坐标系下的方向向量。举个例子，绕Z轴旋转θ角：
R_z(θ) = [
[cosθ, -sinθ, 0],
[sinθ, cosθ, 0],
[0, 0, 1]
]绕X轴和Y轴类似，只是把 sin/cos 放到对应位置。
优点：直观，容易理解，组合旋转就是矩阵乘法。
缺点：9个参数，但只有3个自由度（冗余）。而且连续旋转时，矩阵乘法会累积误差，我见过有人迭代几十次后矩阵彻底「漂移」了。
3.3 欧拉角：直观但有毒
欧拉角用三个角度表示旋转：比如绕Z轴转γ（偏航）、绕Y轴转β（俯仰）、绕X轴转α（滚转）。听起来很直观对吧？
但问题来了：旋转顺序不同，结果完全不同。ZYX、XYZ、ZYZ……每种顺序对应不同的旋转矩阵。你想想看，如果两个人用不同的顺序标定同一个机器人，结果能一样吗？
更坑的是万向锁（Gimbal Lock）。当俯仰角β = ±90°时，偏航和滚转的旋转轴会重合，导致丢失一个自由度。我当年做无人机姿态估计时，就因为这个原因，飞机在俯冲时姿态突然「卡住」了，差点炸机。
我的建议：除非你只是做简单的可视化展示，否则别用欧拉角做内部计算。用它做用户输入可以，但一旦进入数学运算，立刻转成四元数或旋转矩阵。
3.4 四元数：低调的王者
四元数 q = w + xi + yj + zk，其中 w 是实部，x、y、z 是虚部，且满足 i² = j² = k² = ijk = -1。
用四元数表示旋转，公式很简单：
P' = q * P * q⁻¹其中 P 是纯四元数（实部为0），q 是单位四元数（模长为1）。
为什么我偏爱四元数？
- 无万向锁：永远不会丢失自由度
- 插值平滑：用球面线性插值（SLERP）可以生成平滑的旋转过渡
- 紧凑：4个参数，无冗余
- 计算稳定：乘法比矩阵乘法快，且容易归一化
我在做机械臂轨迹规划时，所有内部旋转计算都用四元数。只有在最后输出给用户看时，才转成欧拉角。
3.5 齐次坐标：把旋转和平移统一起来
还记得刚体变换公式 P' = R*P + t 吗？这个公式有个麻烦——它不是线性变换。如果你想把多个变换串联起来，比如先旋转再平移再旋转，公式会变得非常啰嗦。
齐次坐标就是来解决这个问题的。我们把三维点 P = (x, y, z) 扩展成四维向量 (x, y, z, 1)，然后变换矩阵变成 4×4：
T = [
[R(3×3), t(3×1)],
[0 0 0, 1 ]
]这样，刚体变换就变成了一个矩阵乘法：
P'_hom = T * P_hom多个变换串联，就是矩阵连乘：T_total = T_n * ... * T_2 * T_1。这在计算机图形学和机器人学里是标准操作。
实战经验：我在做相机-激光雷达联合标定时，会把所有传感器坐标系都统一到齐次坐标下。这样，从激光雷达到相机、从相机到车体、从车体到世界，就是一连串的4×4矩阵乘法。代码写起来非常清爽，调试也方便。
3.6 四种表示方法的对比
| 表示方法 | 参数数量 | 优点 | 缺点 | 推荐场景 |
|---|---|---|---|---|
| 旋转矩阵 | 9 | 直观，易组合 | 冗余，需正交化 | 理论推导、可视化 |
| 欧拉角 | 3 | 最直观 | 万向锁，顺序依赖 | 用户输入、简单展示 |
| 四元数 | 4 | 无奇点，插值平滑 | 不够直观 | 内部计算、滤波、优化 |
| 齐次坐标 | 16 | 统一旋转+平移 | 参数多，但计算快 | 工程实现、多变换串联 |
3.7 知识体系总览
下面这张图，是我自己梳理的刚体变换知识结构。你可以把它当作这一章的「地图」：
3.8 实战中的选择建议
说了这么多，到底该用哪种？我个人的经验是：
- 做理论推导和写论文：用旋转矩阵，因为公式推导方便
- 做工程实现（C++/Python）：用四元数 + 齐次坐标。Eigen库、Sophus库都支持得很好
- 做可视化调试：用欧拉角，方便人眼理解
- 做优化求解（比如BA）：用四元数或李代数（下一章会讲），避免冗余参数
一个小技巧：如果你在代码里看到旋转矩阵，可以用 cv::Rodrigues() 把它转成旋转向量（3参数），再转成四元数。OpenCV 和 Eigen 都提供了这些转换函数，别自己手写，容易出错。
好了，这一章的内容就到这里。刚体变换是标定的地基，地基不稳，上面盖的房子迟早要塌。我建议你把旋转矩阵、四元数、齐次坐标之间的转换关系亲手推导一遍，写几行代码验证一下。相信我，这个功夫花得值。
可可精品资料库