机器人 / 系统课时
按坐标变换链换算抓取目标点
按坐标变换链换算抓取目标点
视觉系统给出的抓取点通常位于工件或世界坐标系,机器人却需要基座系中的运动目标。本课使用一组可复算的标定数据,将工件特征点转换成基座系 TCP 位姿,再反推出法兰目标,并验证矩阵乘法顺序、工具偏置和计算结果。
学习目标
- 理解齐次变换矩阵的方向约定,正确组合工件、世界与机器人基座坐标系。
- 根据工件特征点和工具安装偏置,计算 TCP、法兰及预抓取位置。
- 使用数值算例和 Python 验证变换顺序,识别容易造成抓取偏差的错误。
统一坐标约定与已知数据
计算前必须确定每个变换矩阵的起点和终点。定义 为将 B 坐标系中的点转换到 A 坐标系的齐次变换矩阵。使用列向量,矩阵左乘点坐标,平移单位统一为米,旋转采用右手坐标系。
本例建立四个坐标系:世界坐标系 W、机器人基座坐标系 B、工件坐标系 P、机器人法兰坐标系 F,以及工具中心点坐标系 TCP。已知的标定关系如下。
| 变换 | 坐标关系 | 本例参数 |
|---|---|---|
T_W_B |
基座 B 到世界 W | 平移 (0.50, 0.20, 0.00) m,绕 Z 轴旋转 +90° |
T_W_P |
工件 P 到世界 W | 平移 (0.80, 0.45, 0.20) m,绕 Z 轴旋转 +90° |
T_P_TCP |
目标 TCP 到工件 P | 平移 (0.10, 0.02, 0.00) m,绕局部 X 轴旋转 180° |
T_F_TCP |
TCP 到法兰 F | 沿法兰局部 +Z 方向平移 0.12 m,无相对旋转 |
其中,T_P_TCP 的平移部分表示抓取特征点在工件坐标系中的位置;旋转部分规定抓取时工具局部 +Z 轴朝下。T_F_TCP 则描述真实工具中心点相对法兰的安装位置。
沿正确方向组合变换链
要将工件坐标系中的抓取目标换算到机器人基座坐标系,先通过 T_W_P 转换到世界坐标系,再通过 T_B_W 转换到机器人基座坐标系。
本例已知的是基座到世界的 T_W_B,因此需要先求逆得到 T_B_W。TCP 的完整目标位姿为:
矩阵乘法从右向左作用。最右侧确定目标 TCP 在工件中的位姿,中间转换到世界坐标系,最左侧转换到机器人基座坐标系。相邻变换的坐标系必须衔接,不能因为矩阵都是 4×4 就任意交换顺序。
机器人控制器通常利用已标定的工具数据将 TCP 目标换算为法兰运动目标,其对应的数学关系为:
这里必须对法兰到 TCP 的固定变换求逆。直接将 TCP 的位置当作法兰位置,会忽略工具长度;只从基座坐标中减去一个固定的 Z 偏置,也无法正确处理工具旋转后的方向。

以工件特征点 (0.10, 0.02, 0.00) m 为例,工件绕世界 Z 轴旋转 +90° 后,特征点在世界系中的位置为 (0.78, 0.55, 0.20) m。再减去基座在世界中的平移,并按照基座的逆向旋转转换,可得基座系坐标 (0.35, -0.28, 0.20) m。
由于本例工件与基座相对世界的 Z 轴旋转相同,TCP 在基座系中的旋转只剩绕 X 轴的 180° 翻转,其局部 +Z 方向为基座 -Z。TCP 位于 z=0.20 m 时,沿工具反方向回退 0.12 m,法兰目标高度应为 z=0.32 m。
使用 Python 计算并核对目标位姿
在安装了 NumPy 2.x 的 Python 3.12 环境中运行下面的完整示例。pose 函数构造齐次变换矩阵,分别设置工件、基座、目标 TCP 与工具安装偏置,再依照上述顺序进行矩阵运算。
import numpy as np
def pose(t, yaw_deg=0, tool_down=False):
a = np.deg2rad(yaw_deg)
c, s = np.cos(a), np.sin(a)
rz = np.array([[c, -s, 0], [s, c, 0], [0, 0, 1]])
flip = np.diag([1, -1, -1]) if tool_down else np.eye(3)
result = np.eye(4)
result[:3, :3] = rz @ flip
result[:3, 3] = t
return result
# 所有平移使用米;工具朝下表示局部 X 轴旋转 180 度
T_W_B = pose([0.50, 0.20, 0.00], 90)
T_W_P = pose([0.80, 0.45, 0.20], 90)
T_P_TCP = pose([0.10, 0.02, 0.00], tool_down=True)
T_F_TCP = pose([0.00, 0.00, 0.12])
# 按坐标系衔接顺序得到 TCP 与法兰目标
T_B_W = np.linalg.inv(T_W_B)
T_B_TCP = T_B_W @ T_W_P @ T_P_TCP
T_B_F = T_B_TCP @ np.linalg.inv(T_F_TCP)
# 独立转换工件特征点,用于核对 TCP 目标位置
p_P = np.array([0.10, 0.02, 0.00, 1.00])
p_W = T_W_P @ p_P
p_B = T_B_W @ p_W
# 预抓取点沿基座 +Z 方向提高 0.10 m
T_B_TCP_pre = T_B_TCP.copy()
T_B_TCP_pre[2, 3] += 0.10
def xyz(v):
return [round(float(x), 2) for x in v[:3]]
print("feature_W_m:", xyz(p_W))
print("feature_B_m:", xyz(p_B))
print("TCP_B_m:", xyz(T_B_TCP[:3, 3]))
print("flange_B_m:", xyz(T_B_F[:3, 3]))
print("pregrasp_B_m:", xyz(T_B_TCP_pre[:3, 3]))
print("tool_down_ok:", np.allclose(T_B_TCP[:3, 2], [0, 0, -1]))
print("offset_closure_ok:", np.allclose(T_B_F @ T_F_TCP, T_B_TCP))预期输出:
feature_W_m: [0.78, 0.55, 0.2]
feature_B_m: [0.35, -0.28, 0.2]
TCP_B_m: [0.35, -0.28, 0.2]
flange_B_m: [0.35, -0.28, 0.32]
pregrasp_B_m: [0.35, -0.28, 0.3]
tool_down_ok: True
offset_closure_ok: Truefeature_B_m 与 TCP_B_m 一致,说明目标 TCP 的平移位置正确落在工件特征点上。flange_B_m 比 TCP 高 0.12 m,符合当前工具朝下、沿工具 +Z 伸出的安装方式。两项布尔校验同时为 True,表明工具方向和变换重组与输入约定一致。
预抓取点位于 (0.35, -0.28, 0.30) m,比接触点高 0.10 m。其姿态与抓取目标保持一致,便于在仿真中检查沿基座 -Z 接近工件的直线运动。实际接近方向仍应根据夹爪结构、工件形状和周围障碍重新确认。
排查变换顺序与工具偏置错误
当抓取目标出现偏移时,先按坐标链的方向检查输入,而不是立即调整机器人末端位置。以下三个错误可以通过本例的具体数值快速识别。
| 错误做法 | 可能表现 | 正确检查 |
|---|---|---|
直接使用 T_W_B 代替 T_B_W |
世界点没有正确转换到基座系 | 先计算 inv(T_W_B),再左乘世界目标 |
将 T_W_P 与 T_B_W 交换顺序 |
结果受旋转和平移次序影响,位置偏离 | 按 T_B_W @ T_W_P 拼接并核对中间坐标 |
| 从 TCP 的基座 Z 坐标直接减去 0.12 m | 本例法兰被算到 z=0.08 m,方向错误 | 用 T_B_TCP @ inv(T_F_TCP),正确高度为 0.32 m |
还要区分位置误差和姿态误差。一个目标点即使落在正确的 (0.35, -0.28, 0.20) m,夹爪也可能因为工具轴方向相反而从工件下方接近。因此位置验证之后,必须检查旋转矩阵的轴向以及控制器采用的姿态表达约定。
在仿真或离线计算中,可采用以下验收条件:
- 工件特征点经工件、世界、基座两段变换后,与直接矩阵连乘的结果一致,数值差小于 0.000001 m。
- 将法兰目标与工具固定变换重新相乘,能够还原 TCP 目标;旋转矩阵满足正交性,行列式接近 +1。
- 工件、基座与工具的平移单位一致,角度单位在计算中明确转换;示例使用米和角度输入,NumPy 三角函数使用弧度。
- 实际工位另行检查标定残差、关节限位、可达性、奇异位形、夹爪朝向及整个接近路径的碰撞风险。
如果结果不符合预期,应优先核实每个矩阵代表的坐标方向以及工具 TCP 的实际标定值。只有独立测量结果、矩阵重组结果和仿真中的抓取姿态一致,才具备继续执行工位联调的条件。