CN107953333B - 一种机械手末端工具标定的控制方法及系统 - Google Patents

Abstract

本发明公开了一种机械手末端工具标定的控制方法，其包括以下步骤：系统控制机械手运动并确保标志点位于拍摄视野内，系统记录下所述机械手法兰盘的位姿P₁及所述标志点所在的空间位置；系统控制所述机械手绕第一空间轴旋转θ1角，并移动所述机械手，使得所述标志点的中心返回至所述标志点原来所在的空间位置P₀，系统记录此时机械手法兰盘的位姿P₂；系统控制所述机械手绕第二空间轴旋转θ2角，记录所述标志点在拍摄视野内的像素坐标(u1，v1)；并移动所述机械手，使得所述标志点的中心返回至所述标志点原来所在的空间位置P₀；以及一种机械手末端工具标定的控制系统。本方案标定结果稳定，无人为误差，广泛应用于机械手控制领域。

Description

一种机械手末端工具标定的控制方法及系统

技术领域

本发明涉及机械手控制领域，具体为机械手末端工具标定的控制方法及系统。

背景技术

随着工业化的不断深入，工业机器人在世界范围内得到快速发展与推广应用。工业机器人在使用过程中还面临一些亟待解决的问题，其中一个比较关键的问题是工具标定问题。工具是固定安装在工业机器人机械臂末端法兰盘上的特殊部件，例如吸盘、焊枪、夹具等装置，为方便工具的使用以及路径规划需要在工具上建立工具坐标系，工具坐标系的原点被称为工具中心点TCP(Tool Center Point)，例如吸盘中心、焊枪焊口。工具标定是为了获得工具中心点与机械手末端法兰盘之间的偏移关系。传统工具标定需要操作员按照机器人指示做出几个特殊的姿态，但是此方法不仅操作复杂还引入了人工误差。工具标定引入的误差会直接导致机器人在制造过程中的良品率降低，甚至引发安全事故。

现有的常用方法有三种，第一种是利用空间坐标仪、自动经纬仪、坐标测量机等高精度空间测量设备直接获得工具中心点与机械手法兰盘之间的相对位置关系。第二种是利用机械手工具末端保持同一位置在空间中做出特定的一组姿势，从而获得工具中心点到机械手法兰面之间的相对位置关系。第三种是利用一些测量传感器比如激光、位置传感器配合机械手做一些姿态求解算出工具参数。第一种方法虽然精度高但是所需要的设备昂贵且操作复杂。第二种虽然无需外部设备也可比较精确的获得工具的参数，但对机械手操作人员要求苛刻，效率低下，且会引入人为的误差。第三种方式虽然在一定程度上简化了标定过程，操作相对复杂而且存在对安装方式的精度要求较高、需要的外部设备通用性不高等缺点；且在第二种第三种方案中均存在求解工具z方向结果时操作复杂，精度低的问题。

综上，该技术有必要进行改进。

发明内容

为了解决上述技术问题，本发明的目的是提供一种机械手末端工具标定的控制方法及系统。

本发明所采用的技术方案是：

本发明提供一种机械手末端工具标定的控制方法，其包括以下步骤：

系统控制机械手运动并确保标志点位于拍摄视野内，系统记录下所述机械手法兰盘的位姿P₁及所述标志点所在的(u0，v0)空间位置P₀；

系统控制所述机械手绕第一空间轴旋转θ1角，并移动所述机械手，使得所述标志点的中心返回至所述标志点原来所在的空间位置P₀，系统记录此时机械手法兰盘的位姿P₂；

系统控制所述机械手绕第二空间轴旋转θ2角，记录所述标志点在拍摄视野内的像素坐标(u1，v1)；系统记录此时机械手法兰盘的位姿P₃；

系统控制所述机械手再次绕所述第一空间轴旋转θ3角，并移动所述机械手，使得所述标志点在拍摄视野内的像素坐标为(u1，v1)，系统记录此时机械手法兰盘的位姿P₄；

系统根据上述数据，确定出所述机械手末端工具的参数。

作为该技术方案的改进，所述方法还包括系统根据所述机械手法兰盘的位姿P₁及所述标志点所在的空间位置P₀、机械手法兰盘的位姿P₂确定出所述机械手末端工具在空间平面上的第一偏移量。

作为该技术方案的改进，所述方法还包括系统根据所述机械手法兰盘的位姿P₃和位姿P₄确定出所述机械手末端工具在空间平面上的第二偏移量。

作为该技术方案的改进，所述第一空间轴和第二空间轴不同。

进一步地，所述标志点的形状为圆形或正方形。

进一步地，所述第一空间轴为z轴，所述第二空间轴为y轴。

进一步地，所述标志点位于所述机械手末端工具上。

另一方面，本发明还提供一种机械手末端工具标定的控制系统，其包括：

初始状态控制模块，用于执行步骤系统控制机械手运动并确保标志点位于拍摄视野内，系统记录下所述机械手法兰盘的位姿P₁及所述标志点所在的(u0，v0)空间位置P₀；

第一状态控制模块，用于执行步骤系统控制所述机械手绕第一空间轴旋转θ1角，并移动所述机械手，使得所述标志点的中心返回至所述标志点原来所在的空间位置P₀，系统记录此时机械手法兰盘的位姿P₂；

第二状态控制模块，用于执行步骤系统控制所述机械手绕第二空间轴旋转θ2角，记录所述标志点在拍摄视野内的像素坐标(u1，v1)；系统记录此时机械手法兰盘的位姿P₃；

第三状态控制模块，用于执行步骤系统控制所述机械手再次绕所述第一空间轴旋转θ3角，并移动所述机械手，使得所述标志点在拍摄视野内的像素坐标为(u1，v1)，系统记录此时机械手法兰盘的位姿P₄；

系统根据上述数据，确定出所述机械手末端工具的参数。

本发明的有益效果是：本发明针对机械手工具标定过程操作复杂、耗时长且精度低等问题，提出一种工具标定的方法，该方法可配合视觉或者人工手动操作，可实现工具的快速精确标定。

本方案通过两步控制进行机械手末端工具标定，第一步中通过绕其中一轴旋转确保标志点中心处于同一位置，计算出其中一方向偏移量；第二步中通过绕另一轴旋转一个任意角度，然后再绕第一轴旋转并保证标志点位于同一位置，计算另一方向偏移量；其对机械手控制要求很低，按照本发明的方法在保证标定结果准确的情况下，可使得标定过程大大简化；且无需特殊的外部设备，计算结果精确，尤其是在配合视觉时可以实现标定过程无人工干预，标定结果稳定，无人为误差。

附图说明

下面结合附图对本发明的具体实施方式作进一步说明：

图1是本发明第一实施例的示意图；

图2是本发明第二实施例的示意图；

图3是本发明第三实施例的示意图；

图4是本发明第一实施例在相机视野内的示意图；

图5是本发明第一实施例的平面标定点位关系示意图；

图6是本发明第一实施例的标定z方向时各点关系立体图；

图7是本发明第一实施例的基于工具的点位关系仰视图；

图8是本发明第一实施例的基于工具的一般点位关系图；

图9是本发明第一实施例的旋转180度时点位关系图。

具体实施方式

需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。

参照图1至图9，本发明提供一种机械手末端工具标定的控制方法，其包括以下步骤：

系统控制机械手运动并确保标志点位于拍摄视野内，如图4所示，为本发明第一实施例在相机视野内的示意图，系统记录下所述机械手法兰盘的位姿P₁及所述标志点所在的(u0，v0)空间位置P₀；

系统控制所述机械手绕第一空间轴旋转θ1角，并移动所述机械手，使得所述标志点的中心返回至所述标志点原来所在的空间位置P₀，系统记录此时机械手法兰盘的位姿P₂；

系统控制所述机械手绕第二空间轴旋转θ2角，记录所述标志点在拍摄视野内的像素坐标(u1，v1)；系统记录此时机械手法兰盘的位姿P₃；

系统控制所述机械手再次绕所述第一空间轴旋转θ3角，并移动所述机械手，使得所述标志点在拍摄视野内的像素坐标为(u1，v1)，系统记录此时机械手法兰盘的位姿P₄；

系统根据上述数据，确定出所述机械手末端工具的参数。

作为该技术方案的改进，所述方法还包括系统根据所述机械手法兰盘的位姿P₁及所述标志点所在的空间位置P₀、机械手法兰盘的位姿P₂确定出所述机械手末端工具在空间平面上的第一偏移量。

作为该技术方案的改进，所述方法还包括系统根据所述机械手法兰盘的位姿P₃和位姿P₄确定出所述机械手末端工具在空间平面上的第二偏移量。

作为该技术方案的改进，所述第一空间轴和第二空间轴不同。

进一步地，所述标志点的形状为圆形或正方形。

参照图1至图3，作为该技术方案的改进，所述标志点的形状为圆形或正方形等。

进一步地，所述第一空间轴为z轴，所述第二空间轴为y轴。

其中所述标志点位于所述机械手末端工具上。

作为一实施例，将图1中的标志点贴在要标定的工具上，并确保标志点位于拍摄视野内，如相机视野内，此时相机视角可以看到贴在工具末端的标志点，如图4相机视角图像所示。

本方案的标定过程分两步，作为一实施例，第一步用于标定工具在xy平面上的偏移，第二步用于标定工具在z深度方向的偏移。坐标系的描述方式有24种，优选的，本实施例固定xyz坐标系进行描述(其它坐标系相同)，三个基轴分别是x、y、z，绕z轴旋转定义为u，绕y轴旋转定义为v，绕x轴旋转定义为w。

第一步：控制机械手使得圆形标志点出现在相机视野范围内，记录此时机械手法兰盘的位姿(x1，y1，z1，u1，v1，w1)及此时圆形标志点所在的空间位置为P₀，P₀点未知，但此时标志点P₀在图像上坐标为(U0，V0)。令机械手绕任意一个轴旋转θ1度，为方便描述这里选择绕z轴(角度u)旋转θ1，然后移动机械手使得圆形标志点的圆心回到(U0，V0)处，记录此时机械手法兰盘的位姿(x2,y2,z2,u2,v2,w2)。从相机视角P₀、P₁、P₂三点之间关系如图5所示，其中P_m是一个中间量。

所述末端工具包括三个参数：tool.x，tool.y，tool.z；

综上，得到了工具中心在机械手法兰盘平面的投影与机械手法兰盘之间的相对位置关系。

tool.x的绝对值是P₁P_m，tool.y的绝对值是P₀P_m；

将此计算值乘以P₁时刻机械手法兰盘的姿态就可以得到真实的tool.x以及tool.y；

这里令P₁点(x1，y1，z1，u1，v1，w1)姿态为(u1，v1，w1)

代入以下公式可得三个旋转矩阵：

将上述三个旋转矩阵代入以下公式可以计算出工具在以旋转轴为法向量的平面内的真实值：

其中toolz＝0。

特别的，这里令(u,v,w)为(0，0，180)，令绕z轴旋转了θ，所以存在：

v₁＝v₂

w₁＝w₂

u₁+θ＝u₂

为了简便计算，令θ＝180。此时P_m点与P₀点重合，可以方便的算出工具Too1的tool.x以及tool._y；

tool.x＝(x2-x1)/2

tool.y＝(y2-y1)/2；

代入w＝180，v＝0，u＝0可得：

计算可得：

第二步：选择一个与第一步不同的旋转轴，优选的，选择绕y轴旋转β，即v(Ry)增加β度，为方便理解这里令β＝45；此时控制机械手移动使得标志点清晰的出现在相机视野范围内，图像内标志点像素坐标为(u1，v1),记标志点的空间坐标为P₀₀(该点未知)，记录当前机械位置P₃(x3,y3,z3,u3,v3,w3)，此时将机械手绕第一步中的旋转轴，即这里的z轴旋转α，然后保持当前机械手姿势移动使得标志点依旧出现在(u1，v1)。记录当前机械手法兰盘位置P₄(x4,y4,z4,u4,v4,w4)，如图6所示。

此时利用第一步中的结果(工具tool含有两个参数tool.x、tool.y,tool.z＝0),以及当前角度可以算出P_t3以及P_t4坐标，其计算方法为：

其中u、v、w分别是P₃当前姿态的旋转角，算出Rx、Ry、Rz后代入下式，可求出P_t3坐标：

同理可得P_t4的坐标值：

此时得到P_t3坐标、P_t4以及O点的关系在相机视角下如图7所示。

按照第一步中的方法可以求出O点的坐标以及P_t3o的长度；

此时已知P_t3o的长度已知，旋转角度β已知，由此可得P_t3P₀₀的长度，如图8所示，P_t3P₀₀＝P_t3O/(sinβ)；

线段P_t3P₀₀长度即工具在z方向的偏移，这样工具的标定就完成了。

Tool的三个参数分别是：

特别的，为便于计算，令α＝180，此时O点与O1点重合，此时所有点都处在XOZ平面上。这些点之间的关系如图9所示：

这时O点坐标可以由以下公式算出：

此时，α＝45。

可以算出

本方案的运算中初始角度不一定是0，0，180，设置成其他角度，亦可实现，本方案角度描述方式有欧拉角12种坐标系定义，固定坐标系同样有12种坐标系定义，本方案按照xyz固定坐标系定义描述，其他坐标系描述亦可实现本发明内容。且本方案不使用视觉，使用其它装置或者方法保证一个固定点始终处于同一位置也可实现。

另一方面，本发明还提供一种机械手末端工具标定的控制系统，其包括：

初始状态控制模块，用于执行步骤系统控制机械手运动并确保标志点位于拍摄视野内，系统记录下所述机械手法兰盘的位姿P₁及所述标志点所在的(u0，v0)空间位置P₀；

第一状态控制模块，用于执行步骤系统控制所述机械手绕第一空间轴旋转θ1角，并移动所述机械手，使得所述标志点的中心返回至所述标志点原来所在的空间位置P₀，系统记录此时机械手法兰盘的位姿P₂；

第二状态控制模块，用于执行步骤系统控制所述机械手绕第二空间轴旋转θ2角，记录所述标志点在拍摄视野内的像素坐标(u1，v1)；系统记录此时机械手法兰盘的位姿P₃；其中，此时只需要移动机械手保证标志点在相机视野内清晰成像即可，无其他约束条件。

第三状态控制模块，用于执行步骤系统控制所述机械手再次绕所述第一空间轴旋转θ3角，并移动所述机械手，使得所述标志点在拍摄视野内的像素坐标为(u1，v1)，系统记录此时机械手法兰盘的位姿P₄；

系统根据上述数据，确定出所述机械手末端工具的参数。

本发明针对机械手工具标定过程操作复杂、耗时长且精度低等问题，提出一种工具标定的方法，该方法可配合视觉或者人工手动操作，可实现工具的快速精确标定。

本方案通过两步控制进行机械手末端工具标定，第一步中通过绕其中一轴旋转确保标志点中心处于同一位置，计算出其中一方向偏移量；第二步中通过绕另一轴旋转一个任意角度，然后再绕第一轴旋转并保证标志点位于同一位置，计算另一方向偏移量；其对机械手控制要求很低，按照本发明的方法在保证标定结果准确的情况下，可使得标定过程大大简化；且无需特殊的外部设备，计算结果精确，尤其是在配合视觉时可以实现标定过程无人工干预，标定结果稳定，无人为误差。

以上是对本发明的较佳实施进行了具体说明，但本发明创造并不限于所述实施例，熟悉本领域的技术人员在不违背本发明精神的前提下还可做出种种的等同变形或替换，这些等同的变形或替换均包含在本申请权利要求所限定的范围内。

Claims (8)

1.一种机械手末端工具标定的控制方法，其特征在于，其包括以下步骤：

系统控制机械手运动并确保标志点位于拍摄视野内，系统记录下所述机械手法兰盘的位姿P₁及所述标志点所在的空间位置P₀；

系统控制所述机械手绕第一空间轴旋转θ1角，并移动所述机械手，使得所述标志点的中心返回至所述标志点原来所在的空间位置P₀，系统记录此时机械手法兰盘的位姿P₂；

系统控制所述机械手绕第二空间轴旋转θ2角，记录所述标志点在拍摄视野内的像素坐标(u1，v1)；系统记录此时机械手法兰盘的位姿P₃；

系统控制所述机械手再次绕所述第一空间轴旋转θ3角，并移动所述机械手，使得所述标志点在拍摄视野内的像素坐标为(u1，v1)，系统记录此时机械手法兰盘的位姿P₄；

系统根据上述数据，确定出所述机械手末端工具的参数。

2.根据权利要求1所述的机械手末端工具标定的控制方法，其特征在于：所述方法还包括系统根据所述机械手法兰盘的位姿P₁及所述标志点所在的空间位置P₀、机械手法兰盘的位姿P₂确定出所述机械手末端工具在空间平面上的第一偏移量。

3.根据权利要求2所述的机械手末端工具标定的控制方法，其特征在于：所述方法还包括系统根据所述机械手法兰盘的位姿P₃和位姿P₄确定出所述机械手末端工具在空间平面上的第二偏移量。

4.根据权利要求1至3任一项所述的机械手末端工具标定的控制方法，其特征在于：所述第一空间轴和第二空间轴不同。

5.根据权利要求4所述的机械手末端工具标定的控制方法，其特征在于：所述标志点的形状为圆形或正方形。

6.根据权利要求5所述的机械手末端工具标定的控制方法，其特征在于：所述第一空间轴为z轴，所述第二空间轴为y轴。

7.根据权利要求6所述的机械手末端工具标定的控制方法，其特征在于：所述标志点位于所述机械手末端工具上。

8.一种机械手末端工具标定的控制系统，其特征在于，其包括：

初始状态控制模块，用于执行步骤系统控制机械手运动并确保标志点位于拍摄视野内，系统记录下所述机械手法兰盘的位姿P₁及所述标志点所在的空间位置P₀；

第一状态控制模块，用于执行步骤系统控制所述机械手绕第一空间轴旋转θ1角，并移动所述机械手，使得所述标志点的中心返回至所述标志点原来所在的空间位置P₀，系统记录此时机械手法兰盘的位姿P₂；

第二状态控制模块，用于执行步骤系统控制所述机械手绕第二空间轴旋转θ2角，记录所述标志点在拍摄视野内的像素坐标(u1，v1)；系统记录此时机械手法兰盘的位姿P₃；

第三状态控制模块，用于执行步骤系统控制所述机械手再次绕所述第一空间轴旋转θ3角，并移动所述机械手，使得所述标志点在拍摄视野内的像素坐标为(u1，v1)，系统记录此时机械手法兰盘的位姿P₄；

系统根据上述数据，确定出所述机械手末端工具的参数。

Images