机器人 / 系统课时

按坐标变换链换算抓取目标点

按坐标变换链换算抓取目标点

视觉系统给出的抓取点通常位于工件或世界坐标系,机器人却需要基座系中的运动目标。本课使用一组可复算的标定数据,将工件特征点转换成基座系 TCP 位姿,再反推出法兰目标,并验证矩阵乘法顺序、工具偏置和计算结果。

学习目标

  • 理解齐次变换矩阵的方向约定,正确组合工件、世界与机器人基座坐标系。
  • 根据工件特征点和工具安装偏置,计算 TCP、法兰及预抓取位置。
  • 使用数值算例和 Python 验证变换顺序,识别容易造成抓取偏差的错误。

统一坐标约定与已知数据

计算前必须确定每个变换矩阵的起点和终点。定义 ATB^{A}T_B 为将 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 的完整目标位姿为:

BTTCP=(WTB)−1 WTP PTTCP{}^{B}T_{TCP}=({}^{W}T_B)^{-1}\,{}^{W}T_P\,{}^{P}T_{TCP}

矩阵乘法从右向左作用。最右侧确定目标 TCP 在工件中的位姿,中间转换到世界坐标系,最左侧转换到机器人基座坐标系。相邻变换的坐标系必须衔接,不能因为矩阵都是 4×4 就任意交换顺序。

机器人控制器通常利用已标定的工具数据将 TCP 目标换算为法兰运动目标,其对应的数学关系为:

BTF=BTTCP (FTTCP)−1{}^{B}T_F={}^{B}T_{TCP}\,({}^{F}T_{TCP})^{-1}

这里必须对法兰到 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 与工具安装偏置,再依照上述顺序进行矩阵运算。

python
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))

预期输出:

text
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: True

feature_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,夹爪也可能因为工具轴方向相反而从工件下方接近。因此位置验证之后,必须检查旋转矩阵的轴向以及控制器采用的姿态表达约定。

在仿真或离线计算中,可采用以下验收条件:

  1. 工件特征点经工件、世界、基座两段变换后,与直接矩阵连乘的结果一致,数值差小于 0.000001 m。
  2. 将法兰目标与工具固定变换重新相乘,能够还原 TCP 目标;旋转矩阵满足正交性,行列式接近 +1。
  3. 工件、基座与工具的平移单位一致,角度单位在计算中明确转换;示例使用米和角度输入,NumPy 三角函数使用弧度。
  4. 实际工位另行检查标定残差、关节限位、可达性、奇异位形、夹爪朝向及整个接近路径的碰撞风险。

如果结果不符合预期,应优先核实每个矩阵代表的坐标方向以及工具 TCP 的实际标定值。只有独立测量结果、矩阵重组结果和仿真中的抓取姿态一致,才具备继续执行工位联调的条件。

按自己的节奏,确认真正理解。

目录与阅读设置

阅读设置

17

记录一个想法

按坐标变换链换算抓取目标点

0 / 10000 · 笔记保存到账号。未提交的修改会在关闭、离开或刷新后丢失。

登录哪都通