机器人 / 系统课时
计算 TCP 偏置后的法兰目标
计算 TCP 偏置后的法兰目标
机器人执行抓取或加工任务时,期望到达目标位置的通常是工具中心点(TCP),而运动学模型的末端可能定义在机械臂法兰上。两者之间存在固定的安装平移和旋转。本篇通过刚体变换求出法兰目标位姿,并用完整 Python 示例验证计算结果。
学习目标
- 区分 TCP 目标、法兰目标和工具安装变换的坐标含义。
- 使用齐次变换求逆,将期望 TCP 位姿换算为法兰目标位姿。
- 通过带有平移和旋转偏置的数值案例,验证正向回代误差并排查常见错误。
明确法兰和 TCP 的坐标关系
机器人法兰是机械臂末端用于安装工具的机械接口,TCP 是工具执行任务时选定的工作点。例如,夹爪的 TCP 可以位于两指之间,焊枪的 TCP 可以位于焊丝端点。TCP 不一定落在法兰轴线上,其坐标轴方向也可能与法兰不同。
定义三个坐标系:
- 基座坐标系 B:机器人运动学计算使用的参考坐标系。
- 法兰坐标系 F:机械臂末端法兰的坐标系。
- 工具坐标系 TCP:原点位于工具工作点,坐标轴按照工具工作方向定义。
采用 表示将 B 坐标系中的点坐标转换为 A 坐标系坐标的齐次变换。于是需要区分以下三个量:
| 变换 | 含义 | 来源 |
|---|---|---|
| 期望 TCP 在基座系下的目标位姿 | 任务规划或视觉目标转换 | |
| TCP 相对法兰的固定安装位姿 | 工具标定或机械测量 | |
| 要求解的法兰目标位姿 | 计算后交给法兰逆运动学求解器 |
工具标定给出的 包含平移向量 和旋转矩阵 。其中平移分量表示 TCP 原点在法兰坐标系下的位置,而不是基座系下的位移。
例如,安装数据为 米,表示 TCP 沿法兰局部 X 轴偏移 100 mm,并沿局部 Z 轴偏移 150 mm。如果工具还有 90° 安装旋转,不能只使用这三个平移量。
从 TCP 目标反推法兰目标
沿着基座、法兰、TCP 的坐标链,正向变换关系为:
已知期望 TCP 位姿和工具安装位姿,需要消去右侧的工具变换,因此应在等式右侧乘以它的逆矩阵:
这是法兰目标求解的直接公式。变换矩阵的乘法不满足交换律,不能把工具变换的逆放在 TCP 目标左侧。
对于旋转矩阵 和平移向量 构成的合法刚体变换,其逆矩阵为:
将法兰目标的旋转和平移分别展开,可以得到:
这里 和 对应目标 TCP, 和 对应工具安装。特别注意,工具偏移必须先经过法兰目标旋转 变换到基座系,才能从 TCP 目标位置中扣除。

一个常见错误是直接计算 。这种做法把法兰局部坐标中的偏移当成了基座坐标中的偏移,只有特定姿态下才可能碰巧正确。当工具姿态改变时,计算出的法兰位置会明显偏离正确目标。
用带旋转的工具偏置完成计算
考虑一个具体任务,机器人需要使 TCP 到达基座系下的目标位置 m,并绕基座正 Z 轴偏航 180°。工具相对法兰的固定安装参数如下:
| 参数 | 数值 |
|---|---|
| TCP 目标位置 | m |
| TCP 目标偏航 | 180° |
| 工具局部平移 | m |
| 工具相对法兰偏航 | 90° |
本例的旋转都绕 Z 轴进行,便于人工核对,但仍使用完整的 齐次矩阵运算。由于 TCP 偏航为 180°、工具相对法兰偏航为 90°,法兰目标偏航应为 90°。
此时法兰局部 X 轴指向基座正 Y 方向,所以工具局部 X 方向的 0.10 m 偏移,在基座系下实际变为正 Y 方向的 0.10 m。局部 Z 方向的 0.15 m 偏移保持不变。
因此法兰目标位置应为 m,而不是简单逐坐标相减得到的 m。
下面使用 Python 3 标准库实现矩阵构造、求逆、连乘和回代验证,不需要安装第三方依赖。保存为 tcp_offset.py 后即可运行。
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)预期输出:
法兰位置(m): 0.600 0.100 0.250
法兰偏航角(°): 90.0
TCP回代通过: True最后的回代操作是将求得的法兰位姿与已知工具安装变换重新相乘,检查是否恢复原始 TCP 目标。这里使用 的矩阵元素最大绝对误差阈值进行数值验证,能够覆盖示例中的浮点计算误差。
这个程序的矩阵乘法和刚体变换求逆函数同样适用于包含绕 X、Y、Z 轴组合旋转的合法三维刚体变换。示例中的 pose() 只构造绕 Z 轴的姿态;处理任意三维工具姿态时,需要用经过验证的完整三维旋转矩阵替换它,而不能只修改偏航角。
将法兰目标接入机器人前的检查
数学上求出法兰目标,并不代表机器人可以直接安全执行。实际项目应依次检查坐标单位、工具定义、逆运动学求解结果和物理碰撞风险。
- 核对单位与坐标系。 确认输入位置全部使用米或按照求解器要求统一转换。若标定文件给出 100 mm,必须转换成 0.10 m;角度也要明确使用度还是弧度。检查工具偏移确实表达在法兰坐标系下。
- 检查安装旋转。 工具装配可能同时存在绕 X、Y、Z 轴的旋转。只校正 TCP 位置而忽略工具方向,会使焊枪、相机或夹爪的工作方向发生错误。
- 确认求解器的末端定义。 如果逆运动学模型的末端定义在法兰上,应传入 ;如果控制器已经配置并启用了工具坐标系,且接口要求输入 TCP 目标,则应按接口约定直接传入 TCP 目标,不能再次扣除偏置。
- 先进行正向回代。 使用求解出的法兰目标乘以固定工具变换,检查恢复后的 TCP 位置和姿态是否满足任务容差。矩阵回代通过后,再检查关节限位、工作空间、奇异性与碰撞。
例如,精密抓取任务可先将位置误差阈值设置为 1 mm、姿态误差阈值设置为 0.5°,作为工程验证的初始标准;具体阈值必须根据机器人重复定位精度、工具标定误差和工艺需求确定。上述数值是验收示例,不代表所有机器人的制造商精度指标。
对于同一套固定安装工具,只需标定一次 ,之后每个新的 TCP 目标都可以使用相同的逆变换求出法兰目标。换装工具、调整夹具或重新定义 TCP 后,则必须更新安装变换,再执行法兰位姿计算。