CN109514549B

CN109514549B - 一种可实现六自由度的tcp在线快速标定方法及装置

Info

Publication number: CN109514549B
Application number: CN201811207015.XA
Authority: CN
Inventors: 乔贵方; 王东霞; 温秀兰; 张颖; 万其; 崔俊宇; 芮平
Original assignee: Nanjing Institute of Technology
Current assignee: Nanjing Institute of Technology
Priority date: 2018-10-17
Filing date: 2018-10-17
Publication date: 2021-10-26
Anticipated expiration: 2038-10-17
Also published as: CN109514549A

Abstract

本发明公开了一种可实现六自由度的TCP在线快速标定方法和装置，包括控制柜、TCP标定装置、机器人、末端工具，控制柜分别连接TCP标定装置和机器人，末端工具安装在机器人上；TCP标定装置包括TCP检测装置、标定控制器和安装底座，TCP检测装置通过安装底座固定安装在机器人的一侧，测量平面与机器人基坐标系的XOY平面平行。根据对射式光电传感器的通断信号，记录法兰位姿数据，计算并补偿末端工具在X/Y/Z轴方向上的角度与位置偏差，通过多次重复操作，降低TCP标定误差，有效的提高机器人的作业精度，减少机器人停机维护时间，提高了工业生产线的自动化程度。

Description

一种可实现六自由度的TCP在线快速标定方法及装置

技术领域

本发明涉及一种在线快速标定方法及装置，具体涉及一种可实现六自由度的TCP在线快速标定方法及装置。

背景技术

随着工业机器人技术的快速发展，其在制造业得到了越来越广泛的应用。 3C产业的产品更新速度快，传统人工或刚性自动化系统逐渐无法满足3C产业的发展需求。工业机器人逐渐进入到3C产业(指以电脑、通讯和消费性电子为主的科技产业)，利用机器人承担分拣、装配等重复、繁琐的操作。根据不同的作业任务，工业机器人的末端需安装不同的工具。而工业机器人末端工具中心点 (Tool Center Point，TCP)的位置定位精度直接影响生产线上产品的质量，精度较差甚至会导致无法完成相应作业任务。

通常工业机器人的末端安装二指气爪实现零件的装配等，经过长时间工作的机器人会产生一定的位置偏差，从而导致机器人无法完成预设操作等问题。目前，针对机器人末端工具TCP的标定主要采用离线人工示教方法，该方法的标定精度受人为因素的影响较大，同时标定过程耗时较长，影响生产线的生产效率。专利ZL201610351194.9提出了一种应用于工业机器人的TCP在线快速标定方法及装置，但该方法仅能够实现机器人末端工具的5自由度标定，其中对于Z轴的角度误差无法实现标定。对于某些微型接插零件的装配，Z轴的角度误差直接关系到该零件能否正确装配。

因此，亟待提出一种面向3C装配的工业机器人二指夹爪的六自由度在线标定方法及装置，能够实现周期性的在线标定，保证工业机器人的作业精度。

发明内容

为解决现有技术的不足，本发明的目的在于提供一种可面向3C装配的工业机器人二指气爪的在线标定方法及装置，该标定方法及装置能够实现机器人二指气爪的六自由度精确标定。

为了实现上述目标，本发明采用如下的技术方案：

一种可实现六自由度的TCP在线快速标定方法，包括以下步骤：

S1、确定坐标关系：保持末端工具垂直于基坐标系的XOY平面，通过人工示教方法或测量装置实现末端工具与TCP检测装置的空间位置数据转换；

S2、末端工具以作业姿态进入并在TCP检测装置中做轨迹标定运动；

S3、X轴与Y轴上的角度差：末端工具做正方形轨迹的标定运动；运动过程中，微控制器单元监测TCP检测装置内X轴与Y轴上的对射式光电传感器通断状态，根据该通断信号的时间点，读取并存储在该时间点的空间位置数据；利用存储的空间位置数据计算末端工具在X轴与Y轴方向上的角度偏差，补偿TCP 的误差；

S4、Z轴上的角度差：末端工具绕Z轴分别按顺时针方向和逆时针方向旋转一个已知角，并保持位姿沿Y轴方向直线运动，获取在Y轴方向上的空间位置数据，计算并补偿Z轴方向上的角度偏差；

S5、X轴与Y轴上的位置差：末端工具沿正方形轨迹做标定运动，根据对射式光电传感器通断信号的时间点，读取并存储该时间点的末端空间位置数据；计算末端工具在X轴与Y轴方向上的位置偏差，并补偿TCP的误差；

S6、Z轴上的位置差：末端工具在Z轴方向上做直线标定运动，以确定机器人TCP在Z轴方向上位置偏差，计算并补偿TCP的误差。

上述步骤S3中末端工具在X轴与Y轴方向上的角度偏差的计算方法为：

A1、读取末端工具运动的空间位置数据：

第一次通过第1组对射式光电传感器时的空间位置数据为P₁₁₁₁，

第一次通过第2组对射式光电传感器时的空间位置数据为P₂₁₁₁，

第二次通过第1组对射式光电传感器时的空间位置数据为P₁₁₂₁，

第二次通过第2组对射式光电传感器时的空间位置数据为P₂₁₂₁；

A2、末端工具沿着Z方向往下运动距离d，重复上述正方形轨迹运动，并读取空间位置数据：

第一次通过第1组对射式光电传感器时的空间位置数据为P₁₁₁₂，

第一次通过第2组对射式光电传感器时的空间位置数据为P₂₁₁₂，

第二次通过第1组对射式光电传感器时的空间位置数据为P₁₁₂₂，

第二次通过第2组对射式光电传感器时的空间位置数据为P₂₁₂₂；

A3、末端工具在X轴方向的角度偏差，依式计算：

在Y轴方向的角度偏差，依式计算：

上述步骤S4中的末端工具在Z轴方向上的角度偏差的计算方法为：

B1、控制末端工具绕Z轴顺时针转动已知角β后，沿Y轴方向做直线运动，读取其在Y轴方向上的空间位置数据，分别记为P₁，P₂，可得末端工具在Y方向的距离L₁；

B2、控制末端工具绕Z轴逆时针转动已知角2β后，沿Y轴方向做直线运动，读取其在Y轴方向上的空间位置数据，分别记为P₃，P₄，可得末端工具在Y 方向的距离L₂；

B3、末端工具在Z轴上的角度偏差，依式计算：

上述步骤S5中的末端工具在X轴方向和Y轴方向上的位置偏差的计算方法为：

C1、读取末端工具运动的空间位置数据，并计算坐标：

第一次通过第1组对射式光电传感器时的空间位置数据分别为P₁₁₁₁、P₁₂₁₁，则末端工具在X轴的坐标为：

第一次通过第2组对射式光电传感器时的空间位置数据分别为P₂₁₁₁、P₂₂₁₁，则，在Y轴的坐标为：

第二次通过第1组对射式光电传感器时的空间位置数据分别为P₁₁₂₁、P₁₂₂₁，则，在X轴的坐标为：

第二次通过第2组对射式光电传感器时的空间位置数据分别为P₂₁₂₁、P₂₂₂₁，则，在Y轴的坐标为：

C2、计算末端工具在X轴方向上的位置偏差为：

C3、计算末端工具在Y轴方向上的位置偏差为：

上述步骤S6中的末端工具在Z轴方向上位置偏差的计算方法为：

控制末端工具运动到对射式光电传感器交点的正上方，沿Z轴方向匀速直线向下运动，当阻断任意一组对射式光电传感器时，获取末端工具的空间位置数据，与通过步骤S1得到的初始值P_TCP0之间的差值，即为末端工具的TCP在Z 轴方向上的偏差：

TCP_Δz＝P_TCP.Z-P_TCP0.Z (10)。

上述步骤S3、S4、S5、S6分别进行重复操作，通过标定结果的迭代，降低 TCP标定误差。

上述步骤S2中末端工具周期性的以作业姿态进入TCP检测装置中。

一种TCP检测装置，包括装置上盖、装置本体、两组对射式光电传感器；所述装置本体为纵向镂空正方形的长方体，两组对射式光电传感器分别设置于装置本体内侧面的纵向中线上，对射式光电传感器的激光射线相互垂直且在同一水平面。

一种可实现六自由度的TCP在线快速标定方法的装置，包括控制柜、TCP 标定装置、机器人、末端工具；

所述控制柜分别连接TCP标定装置和机器人，末端工具安装在机器人上；所述TCP标定装置包括TCP检测装置、标定控制器和安装底座，TCP检测装置通过安装底座固定安装在机器人的一侧，测量平面与机器人基坐标系的XOY平面平行；

所述TCP检测装置包括装置上盖、装置本体、精度检验开关以及两组对射式光电传感器，所述设置于装置本体上表面，所述装置本体为纵向镂空正方形的长方体，两组对射式光电传感器分别设置于装置本体内侧面的纵向中线上，对射式光电传感器的激光射线相互垂直且在同一水平面。

上述标定控制器包括装置外壳、微控制器单元、显示单元、按键单元、状态指示灯和通信接口；

所述标定控制器获取的输出信号以及两组对射式光电传感器的通断信号，通过通信接口与控制柜进行数据通信，并通过显示单元、状态指示灯反馈工作信息。

上述末端工具包括二指气爪。

本发明的有益之处在于：

本发明的一种可实现六自由度的TCP在线快速标定装置及方法，一、能够实现机器人二指气爪的六自由度位置误差标定，提高机器人的装配操作精度；二、能够实现机器人的在线精度维护，包括X/Y/Z三轴方向上的角度和位置的六自由度，三、减少停机维护时间，提高工业生产线的效率和产能，有效地提高工业生产线的自动化程度，节省人力、物力，降低成本。

附图说明

图1是本发明的可实现六自由度的TCP在线快速标定装置的结构示意图。

图2是本发明的TCP标定装置的结构示意图。

图3是本发明的TCP检测装置的结构结构图。

图4是本发明的标定控制器的结构示意图。

图5是本发明的末端二指气爪运动轨迹示意图。

图6是本发明的TCP检测装置测量X/Y轴方向上误差示意图。

图7是本发明的计算X/Y轴方向上角度误差示意图。

图8是本发明的TCP检测装置测量Z轴方向上角度误差示意图。

图9是本发明的TCP检测装置测量X/Y轴方向上位移误差示意图。

图10是本发明的TCP检测装置测量Z轴方向上位移误差示意图。

图11是本发明的TCP在线标定方法流程图。

附图中标记的含义如下：

1、控制柜，2、TCP标定装置，3、机器人，4、末端工具，5、控制总线；

201、TCP检测装置，202、安装底座，203、标定控制器；

301、上盖，302、对射式光电传感器1，303、装置本体，304、对射式光电传感器3，305、对射式光电传感器2，306、对射式光电传感器4；

401、装置外壳，402、显示单元，403、按键单元，404、状态指示灯，405、通信接口。

具体实施方式

以下结合附图和具体实施例对本发明作具体的介绍。

如图1所示，一种可实现六自由度的TCP在线快速标定装置，由控制柜、 TCP标定装置、机器人、末端工具和控制总线组成；控制柜通过控制总线分别连接TCP标定装置和机器人，末端工具安装在机器人上。

本实施例的末端工具选用二指气爪。

如图2所示，TCP标定装置包括TCP检测装置、标定控制器和安装底座， TCP检测装置通过安装底座固定安装在机器人的一侧，测量平面与机器人基坐标系的XOY平面平行；

对射式光电传感器302(305)的激光射线与机器人基坐标系的X轴平行，对射式光电传感器304(306)的激光射线与机器人基坐标系的Y轴平行。

如图3所示，TCP检测装置包括装置上盖、装置本体、以及两组对射式光电传感器302(305)、304(306)，设置于装置本体上表面，装置本体为纵向镂空正方形的长方体，两组对射式光电传感器分别设置于装置本体内侧面的纵向中线上，对射式光电传感器的激光射线相互垂直且在同一水平面。

如图4所示，标定控制器包括装置外壳、微控制器单元、显示单元、按键单元、状态指示灯和通信接口；标定控制器获取的输出信号以及两组对射式光电传感器的通断信号，通过连接通信接口的控制总线与控制柜进行数据通信，并通过显示单元、状态指示灯反馈工作信息。

如图10所示，一种可实现六自由度的TCP在线快速标定方法，包括以下步骤：

S1、保持机器人末端的二指气爪垂直于基坐标系的XOY平面，通过人工示教方法或测量装置实现二指气爪与TCP检测装置的校准，记录机器人法兰的空间位置数据；

测量装置可选择激光跟踪仪；

S2、连续工作一段时间后的机器人，周期性的控制二指气爪进入TCP标定装置中进行六自由度的标定校正；

S3、如图5所示，二指气爪在TCP检测装置201内做正方形轨迹的低速匀速运动，在运动过程中前半路径内的二指气爪保持姿态不变，在后半路径内的二指气爪绕法兰坐标系的Z轴旋转180°。

如图6与图7所示，装置控制器203通过机器人控制柜1控制机器人3的二指气爪以作业姿态在TCP检测装置201中做正方形轨迹的标定运动。

在标定运动过程中，二指气爪首先通过对射式光电传感器302(305)，每经过一组对射式光电传感器，装置控制器203读取机器人3末端的空间位置数据，记为P_ijnm(i表示第i组对射式光电传感器，其中1为对射式光电传感器302(305)、 2为对射式光电传感器304(306)；j表示对射式光电传感器的通断，其中1表示对射式光电传感器信号接通、2表示对射式光电传感器信号断开；n表示单次循环运动中，第n次通过第i组对射式光电传感器，用1，2表示)。m表示第m 次循环运动，用1，2表示。

机器人3的二指气爪第一次通过第1组对射式光电传感器302(305)时，装置控制器203读取机器人3末端的空间位置数据为P₁₁₁₁，机器人3的二指气爪第一次通过第2组对射式光电传感器304(306)时，装置控制器203读取机器人3末端的空间位置数据为P₂₁₁₁，机器人3的二指气爪第二次通过第1组对射式光电传感器302(305)时，装置控制器203读取机器人3末端的空间位置数据为P₁₁₂₁，机器人3的二指气爪第二次通过第2组对射式光电传感器304(306) 时，装置控制器203读取机器人3末端的空间位置数据为P₂₁₂₂。

控制机器人3的二指气爪沿着Z方向往下运动距离d，重复上述运动。

机器人3的二指气爪第一次通过第1组对射式光电传感器302(305)时，装置控制器203读取机器人3末端的空间位置数据为P₁₁₁₂，机器人3的二指气爪第一次通过第2组对射式光电传感器304(306)时，装置控制器203读取机器人3末端的空间位置数据为P₂₁₁₂，机器人3的二指气爪第二次通过第1组对射式光电传感器302(305)时，装置控制器203读取机器人3末端的空间位置数据为P₁₁₂₂，机器人3的二指气爪第二次通过第2组对射式光电传感器304(306) 时，装置控制器203读取机器人3末端的空间位置数据为P₂₁₂₁。

基于以上数据，已知二指气爪在竖直方向上的距离差值为d，可以计算机器人3的二指气爪的X方向的角度偏差如下式所示：

基于以上数据，已知二指气爪在竖直方向上的距离差值为d，可以计算机器人3的二指气爪的Y方向的角度偏差如下式所示：

装置控制器203将误差值补偿到相应的参数中。

多次重复以上操作，通过标定结果的迭代，提高TCP的标定精度，在本实施例中取2次。

S4、如图8所示，装置控制器203通过机器人控制柜1控制机器人3的二指气爪绕Z轴顺时针方向转动一个已知角β，并控制二指气爪在TCP标定装置中做低速匀速直线运动，装置控制器203根据传感器302(305)的通断分别读取机器人3末端的空间位置数据，分别记为P₁，P₂，可以计算出二指气爪在Y方向的距离L₁。

再将二指气爪绕Z轴逆时针方向转动一个已知角2β，并控制二指气爪在 TCP标定装置中做低速匀速直线运动，装置控制器203根据传感器302(305) 的通断分别读取机器人3末端的空间位置数据，分别记为P₃，P₄，可以计算出此时二指气爪在Y方向的距离L₂。

基于以上数据，可以计算出Z轴上的角度误差为：

装置控制器203将误差值补偿到相应的参数中。

S5、如图9所示，装置控制器203通过机器人控制柜1控制机器人3的二指气爪以作业姿态在TCP检测装置201中做正方形轨迹的标定运动。在标定运动过程中，二指气爪首先通过对射式光电传感器302(305)，每经过一组对射式光电传感器，装置控制器203读取机器人3末端的空间位置数据。

机器人3的二指气爪第一次通过第1组对射式光电传感器302(305)时，装置控制器203读取机器人3末端的空间位置数据分别为P₁₁₁₁，P₁₂₁₁。计算出二指气爪的中线在X轴的坐标为：

机器人3的二指气爪第一次通过第2组对射式光电传感器304(306)时，装置控制器203读取机器人3末端的空间位置数据分别为P₂₁₁₁，P₂₂₁₁。计算出二指气爪的中线在Y轴的坐标为：

机器人3的二指气爪第二次通过第1组对射式光电传感器302(305)时，装置控制器203读取机器人3末端的空间位置数据分别为P₁₁₂₁，P₁₂₂₁。计算出二指气爪的中线在X轴的坐标为：

机器人3的二指气爪第二次通过第2组对射式光电传感器304(306)时，装置控制器203读取机器人3末端的空间位置数据分别为P₂₁₂₁，P₂₂₂₁。计算出二指气爪的中线在Y轴的坐标为：

基于以上数据，可以计算得到机器人1的二指气爪在X轴方向上的位置偏差为：

同理，计算得到机器人1的二指气爪在Y轴方向上的位置偏差为：

装置控制器203将误差值补偿到相应的参数中。多次重复以上操作，通过标定结果的迭代，提高TCP的标定精度，在本实施例中取2次。

S6、如图10所示，装置控制器203通过机器人控制柜1控制机器人3的二指气爪运动到对射式光电传感器302(305)、304(306)交点的正上方，沿着Z 轴方向低速匀速直线向下运动，当阻断任意一组对射式光电传感器302(305)、 304(306)时，获取二指气爪的位置数据。与通过人工示教得到的初始值P_TCP0之间的差值就是二指气爪的TCP在Z轴方向上的偏差。

TCP_Δz＝P_TCP.Z-P_TCP0.Z(10)

如图11 所示，机器人的TCP在线快速标定方法的工作流程图。

机器人3达到一定工作周期自动进入TCP标定程序，首先控制机器人3二指气爪在TCP检测装置内做正方形轨迹的运动，根据装置控制器203获取的机器人3二指气爪的位置数据，计算并补偿机器人3的TCP在X轴和Y轴方向上的角度误差。其次，控制二指气爪顺、逆时针分别转动一个已知角β，在TCP 标定装置中做低速匀速直线运动，获取二指气爪在Y方向上的距离，计算并补偿Z轴方向上的角度偏差。然后控制业机器人3二指气爪在TCP检测装置内做正方形轨迹的运动，根据装置控制器203获取的机器人3二指气爪的位置数据，并补偿机器人3的TCP在X轴和Y轴方向上的补偿位置误差。最后，控制机器人3二指气爪沿Z轴方向做直线运动，并根据装置控制器203获取的机器人3 二指气爪的位置数据，计算并补偿机器人3的TCP在Z轴方向上的位置偏差。

以上显示和描述了本发明的基本原理、主要特征和优点。本行业的技术人员应该了解，上述实施例不以任何形式限制本发明，凡采用等同替换或等效变换的方式所获得的技术方案，均落在本发明的保护范围内。

Claims

1.一种可实现六自由度的TCP在线快速标定方法，其特征在于，包括以下步骤：

S3、X轴与Y轴上的角度差：末端工具做正方形轨迹的标定运动；运动过程中，微控制器单元监测TCP检测装置内X轴与Y轴上的对射式光电传感器通断状态，根据该通断信号的时间点，读取并存储在该时间点的空间位置数据；利用存储的空间位置数据计算末端工具在X轴与Y轴方向上的角度偏差，补偿TCP的误差；

S4、Z轴上的角度差：定义末端工具的长边与TCP检测装置的X轴平行，末端工具绕Z轴分别按顺时针方向和逆时针方向旋转一个已知角，并保持位姿沿Y轴方向直线运动，获取在Y轴方向上的空间位置数据，计算并补偿Z轴方向上的角度偏差；

S6、Z轴上的位置差：末端工具在Z轴方向上做直线标定运动，以确定机器人TCP在Z轴方向上位置偏差，计算并补偿TCP的误差；

所述步骤S4中的末端工具在Z轴方向上的角度偏差的计算方法为：

B2、控制末端工具绕Z轴逆时针转动已知角2β后，沿Y轴方向做直线运动，读取其在Y轴方向上的空间位置数据，分别记为P₃，P₄，可得末端工具在Y方向的距离L₂；

B3、末端工具在Z轴上的角度偏差，依式计算：

2.根据权利要求1所述的一种可实现六自由度的TCP在线快速标定方法，其特征在于，所述步骤S3中末端工具在X轴与Y轴方向上的角度偏差的计算方法为：

A1、读取末端工具运动的空间位置数据：

A2、末端工具沿着Z方向往下运动距离d，重复上述运动，并读取空间位置数据：

A3、末端工具在X轴方向的角度偏差，依式计算：

在Y轴方向的角度偏差，依式计算：

3.根据权利要求1所述的一种可实现六自由度的TCP在线快速标定方法，其特征在于，所述步骤S5中的末端工具在X轴方向和Y轴方向上的位置偏差的计算方法为：

C1、读取末端工具运动的空间位置数据，并计算坐标：

C2、计算末端工具在X轴方向上的位置偏差为：

C3、计算末端工具在Y轴方向上的位置偏差为：

4.根据权利要求1所述的一种可实现六自由度的TCP在线快速标定方法，其特征在于，所述步骤S6中的末端工具在Z轴方向上位置偏差的计算方法为：

控制末端工具运动到对射式光电传感器交点的正上方，沿Z轴方向匀速直线向下运动，当阻断任意一组对射式光电传感器时，获取末端工具的空间位置数据，与通过步骤S1得到的初始值P_TCP0之间的差值，即为末端工具的TCP在Z轴方向上的偏差：

TCP_Δz＝P_TCP.Z-P_TCP0.Z。

5.根据权利要求1所述的一种可实现六自由度的TCP在线快速标定方法，其特征在于，所述步骤S3、S4、S5、S6分别进行重复操作，通过标定结果的迭代，降低TCP标定误差。

6.根据权利要求1所述的一种可实现六自由度的TCP在线快速标定方法，其特征在于，所述步骤S2中末端工具周期性的以作业姿态进入TCP检测装置中。