机器人 / 系统课时

计算 TCP 偏置后的法兰目标

计算 TCP 偏置后的法兰目标

机器人执行抓取或加工任务时,期望到达目标位置的通常是工具中心点(TCP),而运动学模型的末端可能定义在机械臂法兰上。两者之间存在固定的安装平移和旋转。本篇通过刚体变换求出法兰目标位姿,并用完整 Python 示例验证计算结果。

学习目标

  • 区分 TCP 目标、法兰目标和工具安装变换的坐标含义。
  • 使用齐次变换求逆,将期望 TCP 位姿换算为法兰目标位姿。
  • 通过带有平移和旋转偏置的数值案例,验证正向回代误差并排查常见错误。

明确法兰和 TCP 的坐标关系

机器人法兰是机械臂末端用于安装工具的机械接口,TCP 是工具执行任务时选定的工作点。例如,夹爪的 TCP 可以位于两指之间,焊枪的 TCP 可以位于焊丝端点。TCP 不一定落在法兰轴线上,其坐标轴方向也可能与法兰不同。

定义三个坐标系:

  • 基座坐标系 B:机器人运动学计算使用的参考坐标系。
  • 法兰坐标系 F:机械臂末端法兰的坐标系。
  • 工具坐标系 TCP:原点位于工具工作点,坐标轴按照工具工作方向定义。

采用  ATB \,{}^{A}T_B\, 表示将 B 坐标系中的点坐标转换为 A 坐标系坐标的齐次变换。于是需要区分以下三个量:

变换 含义 来源
BTTCP{}^{B}T_{TCP} 期望 TCP 在基座系下的目标位姿 任务规划或视觉目标转换
FTTCP{}^{F}T_{TCP} TCP 相对法兰的固定安装位姿 工具标定或机械测量
BTF{}^{B}T_F 要求解的法兰目标位姿 计算后交给法兰逆运动学求解器

工具标定给出的 FTTCP{}^{F}T_{TCP} 包含平移向量 pFTp_{FT} 和旋转矩阵 RFTR_{FT}。其中平移分量表示 TCP 原点在法兰坐标系下的位置,而不是基座系下的位移。

例如,安装数据为 pFT=[0.10,0,0.15]Tp_{FT}=[0.10,0,0.15]^T 米,表示 TCP 沿法兰局部 X 轴偏移 100 mm,并沿局部 Z 轴偏移 150 mm。如果工具还有 90° 安装旋转,不能只使用这三个平移量。

从 TCP 目标反推法兰目标

沿着基座、法兰、TCP 的坐标链,正向变换关系为:

BTTCP=BTF FTTCP{}^{B}T_{TCP}={}^{B}T_F\,{}^{F}T_{TCP}

已知期望 TCP 位姿和工具安装位姿,需要消去右侧的工具变换,因此应在等式右侧乘以它的逆矩阵:

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

这是法兰目标求解的直接公式。变换矩阵的乘法不满足交换律,不能把工具变换的逆放在 TCP 目标左侧。

对于旋转矩阵 RR 和平移向量 pp 构成的合法刚体变换,其逆矩阵为:

T−1=[RT−RTp01]T^{-1}=\begin{bmatrix}R^{\mathsf T}&-R^{\mathsf T}p\\0&1\end{bmatrix}

将法兰目标的旋转和平移分别展开,可以得到:

RBF=RBTRFTTR_{BF}=R_{BT}R_{FT}^{\mathsf T}
pBF=pBT−RBFpFTp_{BF}=p_{BT}-R_{BF}p_{FT}

这里 RBTR_{BT} 和 pBTp_{BT} 对应目标 TCP,RFTR_{FT} 和 pFTp_{FT} 对应工具安装。特别注意,工具偏移必须先经过法兰目标旋转 RBFR_{BF} 变换到基座系,才能从 TCP 目标位置中扣除。

TCP 与法兰的固定安装偏置及逆变换关系

一个常见错误是直接计算 pBF=pBT−pFTp_{BF}=p_{BT}-p_{FT}。这种做法把法兰局部坐标中的偏移当成了基座坐标中的偏移,只有特定姿态下才可能碰巧正确。当工具姿态改变时,计算出的法兰位置会明显偏离正确目标。

用带旋转的工具偏置完成计算

考虑一个具体任务,机器人需要使 TCP 到达基座系下的目标位置 (0.60,0.20,0.40)(0.60,0.20,0.40) m,并绕基座正 Z 轴偏航 180°。工具相对法兰的固定安装参数如下:

参数 数值
TCP 目标位置 (0.60,0.20,0.40)(0.60,0.20,0.40) m
TCP 目标偏航 180°
工具局部平移 (0.10,0,0.15)(0.10,0,0.15) m
工具相对法兰偏航 90°

本例的旋转都绕 Z 轴进行,便于人工核对,但仍使用完整的 4×44\times4 齐次矩阵运算。由于 TCP 偏航为 180°、工具相对法兰偏航为 90°,法兰目标偏航应为 90°。

此时法兰局部 X 轴指向基座正 Y 方向,所以工具局部 X 方向的 0.10 m 偏移,在基座系下实际变为正 Y 方向的 0.10 m。局部 Z 方向的 0.15 m 偏移保持不变。

因此法兰目标位置应为 (0.60,0.10,0.25)(0.60,0.10,0.25) m,而不是简单逐坐标相减得到的 (0.50,0.20,0.25)(0.50,0.20,0.25) m。

下面使用 Python 3 标准库实现矩阵构造、求逆、连乘和回代验证,不需要安装第三方依赖。保存为 tcp_offset.py 后即可运行。

python
import math


def rotation_z(degrees):
    angle = math.radians(degrees)
    c, s = math.cos(angle), math.sin(angle)
    return [[c, -s, 0.0], [s, c, 0.0], [0.0, 0.0, 1.0]]


def pose(x, y, z, yaw):
    r = rotation_z(yaw)
    p = [x, y, z]
    return [[*r[i], p[i]] for i in range(3)] + [[0.0, 0.0, 0.0, 1.0]]


def multiply(a, b):
    return [[sum(a[i][k] * b[k][j] for k in range(4))
             for j in range(4)] for i in range(4)]


def inverse(t):
    # 刚体变换的旋转逆矩阵为转置,平移也必须随之变换
    rt = [[t[j][i] for j in range(3)] for i in range(3)]
    p = [t[i][3] for i in range(3)]
    q = [-sum(rt[i][k] * p[k] for k in range(3)) for i in range(3)]
    return [[*rt[i], q[i]] for i in range(3)] + [[0.0, 0.0, 0.0, 1.0]]


# 输入:基座下的TCP目标,以及法兰下的TCP安装位姿
base_tcp = pose(0.60, 0.20, 0.40, 180)
flange_tcp = pose(0.10, 0.00, 0.15, 90)

# 从TCP目标右乘工具安装变换的逆,得到法兰目标
base_flange = multiply(base_tcp, inverse(flange_tcp))
reached_tcp = multiply(base_flange, flange_tcp)

position = [base_flange[i][3] for i in range(3)]
yaw = math.degrees(math.atan2(base_flange[1][0], base_flange[0][0]))
error = max(abs(reached_tcp[i][j] - base_tcp[i][j])
            for i in range(4) for j in range(4))

print("法兰位置(m):", *[f"{v:.3f}" for v in position])
print("法兰偏航角(°):", f"{yaw:.1f}")
print("TCP回代通过:", error < 1e-9)

预期输出:

text
法兰位置(m): 0.600 0.100 0.250
法兰偏航角(°): 90.0
TCP回代通过: True

最后的回代操作是将求得的法兰位姿与已知工具安装变换重新相乘,检查是否恢复原始 TCP 目标。这里使用 10−910^{-9} 的矩阵元素最大绝对误差阈值进行数值验证,能够覆盖示例中的浮点计算误差。

这个程序的矩阵乘法和刚体变换求逆函数同样适用于包含绕 X、Y、Z 轴组合旋转的合法三维刚体变换。示例中的 pose() 只构造绕 Z 轴的姿态;处理任意三维工具姿态时,需要用经过验证的完整三维旋转矩阵替换它,而不能只修改偏航角。

将法兰目标接入机器人前的检查

数学上求出法兰目标,并不代表机器人可以直接安全执行。实际项目应依次检查坐标单位、工具定义、逆运动学求解结果和物理碰撞风险。

  1. 核对单位与坐标系。 确认输入位置全部使用米或按照求解器要求统一转换。若标定文件给出 100 mm,必须转换成 0.10 m;角度也要明确使用度还是弧度。检查工具偏移确实表达在法兰坐标系下。
  2. 检查安装旋转。 工具装配可能同时存在绕 X、Y、Z 轴的旋转。只校正 TCP 位置而忽略工具方向,会使焊枪、相机或夹爪的工作方向发生错误。
  3. 确认求解器的末端定义。 如果逆运动学模型的末端定义在法兰上,应传入 BTF{}^{B}T_F;如果控制器已经配置并启用了工具坐标系,且接口要求输入 TCP 目标,则应按接口约定直接传入 TCP 目标,不能再次扣除偏置。
  4. 先进行正向回代。 使用求解出的法兰目标乘以固定工具变换,检查恢复后的 TCP 位置和姿态是否满足任务容差。矩阵回代通过后,再检查关节限位、工作空间、奇异性与碰撞。

例如,精密抓取任务可先将位置误差阈值设置为 1 mm、姿态误差阈值设置为 0.5°,作为工程验证的初始标准;具体阈值必须根据机器人重复定位精度、工具标定误差和工艺需求确定。上述数值是验收示例,不代表所有机器人的制造商精度指标。

对于同一套固定安装工具,只需标定一次 FTTCP{}^{F}T_{TCP},之后每个新的 TCP 目标都可以使用相同的逆变换求出法兰目标。换装工具、调整夹具或重新定义 TCP 后,则必须更新安装变换,再执行法兰位姿计算。

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

目录与阅读设置

阅读设置

17

记录一个想法

计算 TCP 偏置后的法兰目标

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

登录哪都通