工具坐标系标定 (TCP)

工具坐标系 (Tool Coordinate System, TCS) 又称 TCP (Tool Center Point), 是以机器人末端工具的工作点为原点、以工具自身方向为 X/Y/Z 的坐标系。TCP 标定是机器人应用上线前的第一道硬门槛 — 没有正确的 TCP, 任何 MoveL / MoveC / 相对偏移都会错位, 焊枪会烧偏, 涂胶会跑线, 夹爪对不上孔。

本页给出 4 点法 / 6 点法 / 姿态对齐 三种标定算法, 以及示教器操作步骤、数学原理、精度验证方法和典型排错。

概述

什么是 TCP

法兰 (Flange) 坐标系: 机器人第 6 轴末端法兰面中心, 由运动学自然给出
工具 0 (Tool0): 默认等于法兰坐标, 用户未配置工具时使用
工具 N (Tool1 / Tool2 /...): 用户自定义, 记录工具尖端相对于法兰的平移 + 旋转

TCP 就是要求出 Tool N 到 Flange 的齐次变换矩阵 ^{Flange}T_{Tool} (6 个自由度: X/Y/Z + Rx/Ry/Rz)。

适用场景

工具类型	关键要求
焊枪 / 切割枪	尖端位置精度 ±0.3 mm, 姿态对齐 Z 轴沿焊丝方向
涂胶嘴 / 喷涂	尖端精度 ±0.5 mm, Z 轴沿胶嘴
夹爪 (气动 / 电动)	夹持中心 ±0.5 mm, Z 轴沿开合方向
吸盘	吸盘中心 ±1.0 mm, Z 轴沿法向
打磨 / 抛光头	接触点 ±0.3 mm, Z 轴沿磨头轴向
视觉相机	通常与手眼标定合并, 见视觉标定

前置条件

工具已机械安装到机器人末端法兰, 螺丝拧紧无松动
工作区放置一个尖锥参考点 (或用工件上一个已知的尖点), 固定不可移动
机器人已使能, 可正常点动
机器人已标定关节零位, 否则 TCP 会带入零位误差
工具不能太重超过额定负载, 否则标定姿态下会下垂变形

4 点法 — 仅求位置 (X/Y/Z)

原理

让工具尖端从 4 个差异尽量大的姿态去重合同一空间点 P。设法兰在各姿态下的齐次变换为 ^{World}T_{Flange,i}, TCP 偏移为 p = (px, py, pz)。由于每次尖端都在同一点 P, 有:

^{World}T_{Flange,i} · [p; 1] = P   (i = 1..4)

展开得到 3 个未知数 (px, py, pz) 的超定线性方程组, 用最小二乘法求解:

(R_i - R_j) · p = t_j - t_i     (i != j)

其中 R_i / t_i 是第 i 次姿态的旋转矩阵与平移向量。方程个数 = C(4,2) × 3 = 18, 未知数 = 3, 可解。

操作步骤

在工作区固定一个尖锥, 建议锥角 < 30°, 高度 50~100 mm
项目浏览器 → 机器人 → 右键 → 坐标系标定 → 工具坐标系 (TCP) → 选 4 点法
切换示教器到 关节模式 (奇异点附近稳定)
用慢速将工具尖对准尖锥顶点, 肉眼 (或放大镜) 确认重合, 点 记录点 1
保持工具尖端不动 (可先抬起再下来), 换一个明显不同的姿态 (建议绕工具 Z 轴转 90°, 或绕 X 轴俯仰 30°), 再对准同一尖锥顶点, 点 记录点 2
重复换姿态 2 次, 得到点 3 和点 4
点计算, 系统输出 TCP 偏移 (px, py, pz) 和残差 (RMS 误差, 单位 mm)
残差 < 0.5 mm → 接受; 残差 > 0.5 mm → 重做, 通常是姿态差异不够大

姿态差异建议

4 个点	姿态建议
点 1	工具竖直向下 (Z 轴朝 -Z world)
点 2	绕工具 Z 轴旋转 90°
点 3	绕工具 X 轴俯仰 +30°
点 4	绕工具 Y 轴翻滚 -30°

姿态差异太小 = 方程病态 = 解不准确。四个姿态应当在"姿态球"上尽量分散。

6 点法 — 求位置 + 姿态 (X/Y/Z + Rx/Ry/Rz)

在 4 点法基础上, 再加 2 个点确定 TCP 的姿态:

点 5 (定 Z 轴方向): 从点 4 位置出发, 沿工具自身 Z 轴正方向直线抬起 30~50 mm, 点 记录点 5
点 6 (定 X 轴方向): 从点 4 位置出发, 沿工具自身 X 轴正方向平移 30~50 mm, 点 记录点 6

系统由点 4 → 点 5 的向量计算 Z 轴单位向量 z_t, 由点 4 → 点 6 的向量计算 X 轴单位向量 x_t, 再正交化:

z_t = normalize(P5 - P4)
x_t = normalize((P6 - P4) - ((P6 - P4) · z_t) · z_t)
y_t = z_t × x_t
R_tool = [x_t, y_t, z_t]

得到旋转矩阵 R_tool 后转成 RPY (Rx, Ry, Rz) 或四元数, 即完整的 6 DoF TCP。

姿态对齐法 — 仅改姿态

如果工具仅更换了夹持角度 (如快换夹爪转了 45°), 位置不变, 只需重做姿态对齐:

切到 工具姿态对齐 向导
让工具目视竖直, 点 记录参考姿态 (此时 TCP 的 Z 轴对齐 World -Z)
系统保留原 TCP 位置, 仅重写 Rx/Ry/Rz

耗时不到 1 分钟, 适合产线频繁换夹具的场合。

示教器 SCL 示例 (MC Part 4)

VAR
    TcpCalibrate : MC_SetKinTransform;   // PLCopen MC Part 4
    TcpResult    : LREAL;
END_VAR

TcpCalibrate(
    Robot         := Robot1,
    Execute       := bStart,
    ToolId        := 1,
    Method        := TCP_METHOD_4POINT,   // 或 TCP_METHOD_6POINT
    Point1        := pCal1,
    Point2        := pCal2,
    Point3        := pCal3,
    Point4        := pCal4
);

IF TcpCalibrate.Done THEN
    TcpResult := TcpCalibrate.Residual;   // RMS 残差 mm
    IF TcpResult < 0.5 THEN
        bAccept := TRUE;                  // 接受标定结果
    ELSE
        bReject := TRUE;                  // 重做
    END_IF;
END_IF;

参数表

参数名	类型	单位	范围	默认值	说明
ToolId	UINT	-	1..15	1	工具编号 (Tool0 保留给法兰)
Method	ENUM	-	4P/6P/POSE	4P	标定算法
PosX / PosY / PosZ	LREAL	mm	±500	0	TCP 位置偏移
RotX / RotY / RotZ	LREAL	°	±180	0	TCP 姿态欧拉角 (RPY XYZ)
Residual	LREAL	mm	0..10	-	标定 RMS 残差
Mass	LREAL	kg	0..100	0	工具质量 (用于重力补偿)
Cog_X / Y / Z	LREAL	mm	±500	0	工具质心位置

精度验证

标定完必须验证, 否则发现错了已经撞过工件:

工具系点动测试: 示教器切到 工具坐标系, 按 +Z, 工具应沿自身方向平移, 不应产生明显姿态变化
绕 TCP 姿态旋转测试: 保持 TCP 不动, 按 +Rx / +Ry / +Rz, 工具尖端应在原地转, 如有晃动 → TCP 位置不准
对点复现测试: 把工具尖对准尖锥, 记录机器人位姿; 换任意姿态再对准, 对比两次关节角, 差异应在误差范围内
3D 视口验证: 打开 3D 视口, 切 显示坐标系, 工具 Z 轴应从法兰指向尖端, 长度正确

排错清单

现象	可能原因	解决方法
残差 > 2 mm	4 个姿态差异太小	扩大姿态差异, 建议相邻姿态至少差 60°
残差 > 5 mm	尖锥松动, 对点时移动了	固定尖锥, 重做
点动 +Z 但工具斜着走	6 点法的 Z 轴向量取偏了	重做点 5, 从点 4 沿工具轴严格直线抬起
工具系 +X 方向反了	点 6 取在负方向	重做点 6, 取正方向
负载重导致 TCP 漂移	工具过重手臂形变	标定时用与实际接近的重载姿态
多次标定结果不一致	尖锥顶点对不准 (肉眼误差)	换成十字瞄准镜或光学对准仪
TCP 正确但焊道偏	工件坐标系未标	去用户坐标系标工件

最佳实践

换工具即标定: 哪怕是同型号的备用工具, 制造公差也会有 0.1~0.5 mm 差异
尖锥越尖越好: 锥角越小, 对准精度越高, 推荐 20~30° 硬质合金尖锥
用快换法兰: 快换法兰的重复精度通常 ±0.02 mm, 换下再装回几乎不用重标
标定姿态靠近工作姿态: 不要在工作区外标完再到工作区用, 臂形变会造成偏差
记录所有 TCP: 不要覆盖, 给每个工具独立 ToolId, 程序里 SetTool(1) 切换
周期性复核: 每周用对点复现测试抽查 TCP, 误差变化 > 0.3 mm 就重标
焊接工具用 6 点法: 位置 + 姿态都必须准, 否则焊丝方向错
吸盘工具允许 4 点法: 姿态不敏感, 吸盘旋转对结果无影响

数学补充: 解的唯一性与奇异性

4 点法的线性最小二乘解 p = (A^T A)^{-1} A^T b 要求 A^T A 可逆, 等价于 4 个姿态的旋转差异线性无关。直观判断:

4 个姿态的 X 轴向量不共面 → 可解
所有姿态的 X 轴都在同一平面内 → 奇异 → 无解

系统内部计算 A^T A 的条件数 (condition number), 超过阈值 (默认 1e6) 会提示姿态差异不足并拒绝结果。

概述​

什么是 TCP​

适用场景​

前置条件​

4 点法 — 仅求位置 (X/Y/Z)​

原理​

操作步骤​

姿态差异建议​

6 点法 — 求位置 + 姿态 (X/Y/Z + Rx/Ry/Rz)​

姿态对齐法 — 仅改姿态​

示教器 SCL 示例 (MC Part 4)​

参数表​

精度验证​

排错清单​

最佳实践​

数学补充: 解的唯一性与奇异性​

相关文档​

概述