3. 刚体变换与齐次矩阵：旋转矩阵、平移向量、齐次坐标、4x4齐次变换矩阵、变换矩阵的乘法与逆运算
在3D视觉定位与手眼标定中，机器人末端执行器与相机之间的空间关系描述，本质上是一个刚体变换问题。刚体变换是指在三维空间中，物体只发生旋转和平移，而不发生形变、缩放或剪切。本章将系统建立描述刚体变换的数学工具：从旋转矩阵、平移向量，到齐次坐标与4×4齐次变换矩阵，并深入讲解变换矩阵的乘法与逆运算。
3.1 旋转矩阵：描述姿态的核心
旋转矩阵是一个3×3的正交矩阵，用于描述两个坐标系之间的纯旋转关系。设世界坐标系为{A}，物体坐标系为{B}，则旋转矩阵 ABR 的每一列表示{B}的基向量在{A}中的投影。
| 属性 | 数学表达 | 物理含义 |
|---|---|---|
| 正交性 | RTR = I | 列向量两两正交且为单位向量 |
| 行列式 | det(R) = +1 | 保持右手系，无镜像 |
| 逆矩阵 | R-1 = RT | 旋转矩阵的逆等于其转置 |
| 自由度 | 3个独立参数 | 绕X、Y、Z轴的三个旋转角 |
3.2 平移向量：描述位置
平移向量是一个3×1的列向量，表示两个坐标系原点之间的位移。设坐标系{B}的原点在坐标系{A}中的位置为 AtB = [x, y, z]T，则空间中一点P在{A}和{B}下的坐标满足：
^A P = ^A_B R · ^B P + ^A t_B其中 AP 和 BP 分别是点P在{A}和{B}下的坐标向量。
3.3 齐次坐标：统一旋转与平移
为了将旋转和平移统一为线性变换，引入齐次坐标。齐次坐标将三维点 [x, y, z]T 表示为四维向量 [x, y, z, 1]T，将方向向量（如法向量）表示为 [x, y, z, 0]T。
| 类型 | 三维坐标 | 齐次坐标 | w分量含义 |
|---|---|---|---|
| 空间点 | (x, y, z) | (x, y, z, 1) | w=1 表示位置 |
| 方向向量 | (x, y, z) | (x, y, z, 0) | w=0 表示方向 |
| 无穷远点 | 无 | (x, y, z, 0) | w=0 表示无穷远 |
3.4 4×4齐次变换矩阵
齐次变换矩阵 T 是一个4×4矩阵，将旋转矩阵和平移向量整合为一个整体：
┌ ┐
│ R t │
T = │ 3×3 3×1 │
│ 0 1 │
└ 1×3 ┘
其中左上角3×3子块为旋转矩阵 R，右上角3×1子块为平移向量 t，左下角为1×3零向量，右下角为标量1。
3.5 变换矩阵的乘法：链式法则
当存在多个坐标系串联时，齐次变换矩阵的乘法实现了坐标的链式传递。设坐标系{A}到{B}的变换为 ABT，{B}到{C}的变换为 BCT，则{A}到{C}的复合变换为：
^A_C T = ^A_B T · ^B_C T矩阵乘法顺序遵循右乘规则：先发生的变换在右侧，后发生的变换在左侧。对于点P的坐标变换：
^A P_hom = ^A_B T · ^B_C T · ^C P_hom3.6 变换矩阵的逆运算
已知坐标系{B}相对于{A}的变换 ABT，求{A}相对于{B}的变换 BAT，即求逆矩阵。利用旋转矩阵的正交性，逆变换可高效计算：
^B_A T = (^A_B T)^(-1) = ┌ ┐
│ R^T -R^T · t │
│ │
│ 0 1 │
└ ┘其中 RT 是旋转矩阵的转置（即逆旋转），-RT·t 是平移向量在{B}下的表示。
| 运算 | 公式 | 计算量 | 应用场景 |
|---|---|---|---|
| 正变换 | AP = R·BP + t | 9次乘法+9次加法 | 从工具坐标系到基坐标系 |
| 逆变换 | BP = RT·(AP - t) | 9次乘法+9次加法 | 从基坐标系到工具坐标系 |
| 复合变换 | ACT = ABT · BCT | 64次乘法+48次加法 | 机器人运动学链 |
3.7 刚体变换的性质总结
- 保距性：刚体变换保持任意两点间的欧氏距离不变。
- 保角性：刚体变换保持向量间的夹角不变。
- 保定向性：右手系变换后仍为右手系（det=+1）。
- 可逆性：每个刚体变换都有唯一的逆变换，且逆变换也是刚体变换。
- 封闭性：两个刚体变换的复合仍然是刚体变换。