4. 刚体变换
在机械臂视觉定位中，我们最终要回答的问题往往不是“像素在哪里”，而是“目标在机器人基坐标系下的位姿是什么”。刚体变换正是连接这两个世界的数学桥梁。它描述一个物体在三维空间中既不拉伸也不扭曲的运动——只有位置改变（平移）和姿态改变（旋转）。理解刚体变换的构建、求逆与链式相乘规则，是后续手眼标定、坐标链推导和抓取位姿计算的基础。
一、核心原理：平移与旋转的分离表达
刚体变换由两部分组成：平移向量 t = [tx, ty, tz]ᵀ 描述坐标系原点的位移，旋转矩阵 R（3×3 正交矩阵，满足 RᵀR = I 且 det(R)=1）描述坐标轴的朝向变化。若已知点 P 在坐标系 B 中的坐标 p_B，要转换到坐标系 A，则：
p_A = R_AB · p_B + t_AB这种“先旋转再平移”的写法直观，但存在两个工程痛点：一是复合变换时需要反复展开矩阵乘法与加法，二是求逆需要单独推导 Rᵀ 与 -Rᵀt，容易出错。齐次变换矩阵正是为解决这两个问题而生。
二、齐次变换矩阵的构建
齐次变换矩阵把旋转和平移统一到一个 4×4 矩阵中：
T_AB = [ R_AB t_AB ]
[ 0 1 ]
其中 R_AB 为 3×3 旋转矩阵，t_AB 为 3×1 平移向量，
最后一行为 [0 0 0 1]，保证齐次坐标的仿射性质。点的齐次坐标写作 [x, y, z, 1]ᵀ，于是变换变为一次矩阵乘法：
p_A_h = T_AB · p_B_h构建 T 时，旋转部分可由欧拉角、轴角或四元数转换而来。以绕 Z 轴旋转 θ 为例：
Rz(θ) = [ cosθ -sinθ 0 ]
[ sinθ cosθ 0 ]
[ 0 0 1 ]在机械臂场景中，常见的构建顺序是：先确定末端工具坐标系相对于法兰的变换 T_tool_flange，再确定法兰相对于基座的变换 T_flange_base，最后通过链式相乘得到工具在基座下的位姿。
三、复合变换与链式相乘规则
当存在多个坐标系串联时，复合变换遵循“下标相消”的链式规则：
T_AC = T_AB · T_BC即从 A 到 C 的变换，等于从 A 到 B 的变换乘以从 B 到 C 的变换。这个规则可以无限延伸：T_AE = T_AB · T_BC · T_CD · T_DE。在机械臂视觉抓取中，典型链路为：
T_base_target = T_base_flange · T_flange_tool · T_tool_camera · T_camera_target需要特别注意的是乘法顺序不可交换。矩阵乘法不满足交换律，T_AB · T_BC ≠ T_BC · T_AB。物理含义是：先平移再旋转，与先旋转再平移，结果完全不同。工程中建议始终按“从基座到末端”或“从末端到基座”的统一方向书写，避免顺序混乱。
四、求逆：从目标位姿反推关节指令
已知 T_AB，求 T_BA 时，直接对 4×4 矩阵求逆计算量大且易引入数值误差。利用刚体变换的结构特性，可以快速求逆：
T_BA = T_AB⁻¹ = [ R_ABᵀ -R_ABᵀ · t_AB ]
[ 0 1 ]其原理是：旋转矩阵是正交矩阵，逆等于转置；平移部分则需先旋转回原方向再取负。这一公式在实时控制中非常重要，因为机械臂需要根据目标在基座下的位姿，反推末端需要到达的位置，再通过逆运动学求解关节角。
五、工程案例：视觉抓取中的坐标链
假设相机安装在机械臂末端（eye-in-hand），要抓取传送带上的工件。已知：
T_base_flange：由关节编码器通过正运动学计算得到；T_flange_camera：手眼标定结果，固定不变；T_camera_target：视觉算法输出的目标在相机下的位姿。
则目标在基座下的位姿为：
T_base_target = T_base_flange · T_flange_camera · T_camera_target若需要让工具坐标系对准目标，还需左乘工具与法兰的逆：
T_flange_desired = T_base_target · T_tool_target⁻¹ · T_flange_tool⁻¹实际调试中，建议先用一个已知位置的标定板验证整条链路的数值一致性，再投入抓取测试。
六、常见误区与复盘清单
| 误区 | 后果 | 正确做法 |
|---|---|---|
| 复合变换顺序写反 | 目标位姿偏差巨大，抓取失败 | 统一按“基座→末端”方向书写，下标相消验证 |
| 求逆时直接对 4×4 矩阵数值求逆 | 数值不稳定，实时性差 | 使用 Rᵀ 与 -Rᵀt 解析公式 |
| 忽略旋转矩阵正交性校验 | 累积误差导致姿态漂移 | 定期检查 RᵀR≈I，必要时重新正交化 |
| 平移向量单位不统一 | 毫米与米混用，结果差千倍 | 全链路统一使用米或毫米，并在接口处显式转换 |
Transform 类，提供 compose()、inverse()、apply() 三个方法，并在每次链式相乘后校验旋转矩阵正交性。这样可以把坐标变换的错误限制在局部，而不是等到抓取失败才排查。
[0, 0, 0, 1]，这是刚体变换与一般仿射变换（如含缩放、剪切）的关键区别。若在标定或优化过程中破坏了这一行，变换将不再保持刚体性质，必须重新约束。
掌握刚体变换的构建、求逆与链式相乘规则后，后续的手眼标定、逆运动学求解和抓取位姿生成都将建立在一个清晰、可验证的数学框架之上。建议在每一段坐标链的末端都保留一次“回代验证”：用已知点分别走正向和逆向变换，确认误差在允许范围内。