CN111730597B

CN111730597B - 一种搬运机械手的旋转标定方法

Info

Publication number: CN111730597B
Application number: CN202010638350.6A
Authority: CN
Inventors: 陈新; 刘思文; 寇慧; 王辉; 汪良红
Original assignee: Foshan Longshen Robot Co Ltd
Current assignee: Foshan Longshen Robot Co Ltd
Priority date: 2020-07-06
Filing date: 2020-07-06
Publication date: 2021-01-12
Anticipated expiration: 2040-07-06
Also published as: CN111730597A

Abstract

本发明公开一种搬运机械手的旋转标定方法，包括以下步骤：建立平面坐标系，确定旋转关节f在坐标系中的TCP，相机设置在该旋转关节f的末端；在坐标系上设置用于与相机进行标定的标定图，旋转关节f进行转动，完成相机标定；记录旋转关节f在标定位置的特征中心的坐标值以及角度；计算旋转关节f的旋转中心；旋转关节f从标定位置进行旋转，得到转动后的旋转关节f'在当前位置的特征中心的坐标值和角度以及旋转后相机拍照得到物料的特征中心的坐标值和角度，再结合旋转关节f的旋转中心，求得旋转后的旋转关节f'的坐标；将旋转关节f'的坐标与旋转后相机拍照得到物料的坐标作差，算出偏移量。该标定方法能够用于相机自身进行旋转移动的工作场合中。

Description

一种搬运机械手的旋转标定方法

技术领域

本发明涉及搬运机械手技术领域，具体涉及一种搬运机械手的旋转标定方法。

背景技术

搬运机器人是近代高科技自动化的工业产物，能够根据设定的指令进行自动化的搬运工作；其中，机械手是搬运机器人的主要代表，可通过编程来完成各种预期的作业任务，在构造和性能上兼有人手和机器各自的优点，基于作业的准确性和通用性，从而在国民经济各领域有着广阔的发展前景，逐渐进入工业全自动化时代，实现制造业升级，推进制造2025的进程。

在现有的自动化生产领域中，物料的位置关系以及摆放姿态主要靠机械手上的各种传感器感应或者图像识别来确定，例如在机械手搬运物料的过程中，需要对物料进行标定，从而确保机械手能精确地将物料放置在预定的位置上。

具体地，在相机往下进行标定的场合中，相机一般固定设置在标定工位的上方，在工作中，机械手将物料搬运至相应工位后，用于对标的标定物也移动至标定工位中，继而相机往下进行标定识别，从而保证物料当前的位置。上述标定方法中，相机处于静止不动的状态进行标定识别，标定的方法已经广泛应用在各种自动化生产中，

但是，当相机设置在机械手的运动端时，例如安装在SCARA机器人的第一旋转关节，在相机完成向下的标定后，相机进行旋转移动，此时的相机不仅发生移位，而且姿态也发生相应的变化，所以上述固定不动的标定方法不适用于相机自身进行旋转移动的工作场合中，从而不能完成该工作场合中的标定工作。

发明内容

本发明的目的在于克服上述存在的问题，提供一种搬运机械手的旋转标定方法，该旋转标定方法能够用于相机自身进行旋转移动的工作场合中。

本发明的目的通过以下技术方案实现：

一种搬运机械手的旋转标定方法，包括以下步骤：

建立物料搬运的平面坐标系，确定搬运机械手的旋转关节f在平面坐标系中的TCP，相机设置在该旋转关节f的末端；在平面坐标系上设置用于与相机进行标定的标定图，旋转关节f进行转动，使得相机移动至的标定图上方，完成相机标定；记录旋转关节f在标定位置的特征中心的坐标值以及角度，作为用于计算旋转关节f经过旋转移动后的偏差量的参考位置；计算旋转关节f的旋转中心；

搬运机械手的旋转关节f从标定位置进行旋转，旋转后的旋转关节为f'，得到转动后的旋转关节f'在当前位置的特征中心的坐标值和角度以及旋转后相机拍照得到物料的特征中心的坐标值和角度，再结合旋转关节f的旋转中心，求得旋转后的旋转关节f'的坐标；将旋转关节f'的坐标与旋转后相机拍照得到物料的坐标作差，算出搬运机械手的旋转关节需要移动的偏移量。

本发明的一个优选方案，其中，计算搬运机械手的旋转关节f的旋转中心包括以下步骤：

假设旋转关节f的旋转中心为已知的坐标点O_已知（O_X已知，O_Y已知）；在平面坐标系中，将旋转关节f绕着假设的旋转中心在同一个方向连续转动三次，每次转动的角度相同，旋转关节f的末端在三次转动后所处的位置分别用A、B、C代表；根据转动的角度以及坐标规律，求得A、B、C的坐标；连接点A和点B，并先后求得直线段AB、OA、OB的长度，再求得线段AB和OA在本平面坐标系中的方位角；基于A点坐标、线段OA的长度 L_OA以及线段OA的方位角α_OA求得旋转关节f的旋转中心，并进行有效验证。

优选地，得到A（A_X，A_Y）和B（ B_X，B_Y）的坐标后，通过以下公式得到线段AB的长度L_AB：

L_AB= SQRT((A_X-B_X)^2+(A_Y-B_Y)^2)。

优选地，将OA的转动角度设为∠AOB，通过以下公式得到OA的长度L_OA以及OB的长度L_OB：

L_OA=(L_AB /2)/SIN((∠AOB /2)*PI()/180)；

由于OAB为以AB为底边的等腰三角形，因此OB的长度L_OB= L_OA。

优选地，通过以下公式得到AB在本平面坐标系中的方位角α_AB：

α_AB=DEGREES(PI()*(1-SIGN(B_Y - A_Y)/2)-ATAN((B_X - A_X)/( B_Y - A_Y)))；

其中，若求得的α_AB小于0 ，则α_AB加上360。

优选地，结合AB的方位角α_AB，通过以下公式得到OA在本平面坐标系中的方位角α_OA：

α_OA1=α_AB -（90-∠AOB /2）；

α_OA2=α_AB +（90-∠AO B/2）。

在上述公式中，由于OA具有对称性，通常有两个解，在往后的步骤中将会对两个方位角α_OA求得旋转中心进行有效验证。

优选地，结合A（A_X，A_Y）、L_OA以及α_OA，通过以下公式得到旋转关节f的旋转中心Pcenter（P_X，P_Y）：

P_X =( A_X +L_OA*COS(α_OA*PI()/180))；

P_Y =( A_Y +L_OA*COS(α_OA*PI()/180))；

其中，上述得到旋转关节f的旋转中心有两个：P₁（P_X1，P_Y1）和P₂（P_X2，P_Y2）。

优选地，求得旋转中心后，通过向量夹角公式分别得到BC与O₁A和O₂A的夹角，公式如下：

cos<BC, OA>=BC* OA /| BC || OA |；

当BC与OA的的夹角小于90°时，即为有效的旋转中心。

本发明的一个优选方案，其中，将旋转关节f在标定位置的特征中心的坐标值以及角度设为refPos(x,y,u)，将转动后的旋转关节f'在当前位置的特征中心的坐标值以及角度设为Cur_pos(x,y,u)，将旋转后相机拍照得到物料的特征中心的坐标值和角度设为Obj_pos(x,y,u)；

结合旋转关节f的旋转中心，通过绕点旋转公式求得旋转后的旋转关节f'的坐标F'（X₀,Y₀)：

X₀= (x - rx₀)* cos(a)-(y - ry₀)*sin(a)+rx₀；

Y₀= (x - rx₀)* sin(a)+(y - ry₀)*cos(a)+ry₀；

其中，x = Obj_pos_X ；y=Obj_pos_Y；rx₀=Pcenter_X；ry₀= Pcenter_Y；a为已知的旋转角度。当然，也可以通过Halcon算子或者数学库方法或者获得F'（X₀,Y₀)。

本发明的一个优选方案，其中，将F'（X₀,Y₀)与Obj_pos(x,y,u)作差：

(X₀-Obj_pos_x，Y₀-Obj_pos_y)；

即算出旋转关节需要移动的偏移量。

本发明与现有技术相比具有以下有益效果：

本发明中的旋转标定方法在相机发生旋转后，将机械手的旋转关节在发生旋转后的坐标还原回来，解决了“相机在发生旋转后转换出来的数据仍然是标定位置的数据”的问题，而且通过计算旋转差，可得拍照后任意位置的准确坐标，计算出偏移量，使得机械手可以准确地将物料搬运至指定位置上。

附图说明

图1为本发明中的搬运机械手的旋转标定方法的相机标定坐标系，其中，f代表相机标定位置的旋转关节；B、C、D和E围成的矩形区域表示标定图的位姿；f ’代表旋转后的旋转关节，B ’代表B点经过旋转的位置。

图2为本发明中的搬运机械手在平面坐标系中进行旋转的示意图，其中，f代表相机标定位置的旋转关节；B、C、D和E围成的矩形区域表示标定图的位姿；F点表示标定位置的旋转关节f的特征中心坐标；f ’代表旋转后的旋转关节，B ’代表B点在旋转后的位置；F ’表示旋转后的旋转关节f的特征中心坐标。

图3为本发明中的搬运机械手的旋转标定方法求解旋转关节f的旋转中心的演示图。

具体实施方式

为了使本领域的技术人员很好地理解本发明的技术方案，下面结合实施例和附图对本发明作进一步描述，但本发明的实施方式不仅限于此。

参见图1-3，本实施例中的搬运机械手的旋转标定方法，包括以下步骤：

步骤一、

首先建立平面坐标系，确定旋转关节f在平面坐标系中的TCP，其中，TCP全称是tool centre position，亦即工具中心点：进一步，为了确定该工具的位置，在工具上绑定一个工具坐标系TCS （Tool Coordinate System），TCS的原点就是TCP（Tool CenterPoint，工具中心点）。相机设置在该旋转关节f的末端，在平面坐标系上设置用于与相机进行标定的标定图（具体地，本实施例中的标定图中设有矩阵式排列的9个标定点），旋转关节f进行转动，使得相机移动至的标定图上方，完成相机标定。记录旋转关节f在标定位置的特征中心的坐标值以及角度refPos(x,y,u)，作为用于计算旋转关节f经过旋转移动后的偏差量的参考位置。

接着，计算旋转关节f的旋转中心：

参见图3，假设旋转关节f的旋转中心为已知的坐标点O_已知（O_X已知，O_Y已知），例如为原点O₀（0，0）。在平面坐标系中，旋转关节f绕着旋转中心在逆时针方向连续转动三次（在转动之前，旋转关节f与X轴共线），每次转动的角度相同，旋转关节f的末端在三次转动后所处的位置分别用A、B、C代表（这三个位置最好是9点标定转换出来的机器人坐标系下的坐标，如果是像素坐标，应当根据像素单量进行转换），其中转动的角度为∠AOB取15°，当然也可以取其他数值。根据转动的角度以及坐标规律，求得A、B、C的坐标：A（3，1）、B (2.64，1.74)、 C(2.1，237)。

接着，连接点A和点B，通过以下公式得到线段AB的长度L_AB：

L_AB= SQRT((A_X-B_X)^2+(A_Y-B_Y)^2)=0.8229。

通过上述公式得到L_AB，再通过以下公式得到OA的长度L_OA以及OB的长度L_OB：

L_OA=(L_AB /2)/SIN((∠AOB /2)*PI()/180)=3.1523。

其中，由于OAB为以AB为底边的等腰三角形，所以OB的长度L_OB= L_OA=3.1523。

接着，通过以下公式得到线段AB在本平面坐标系中的方位角α_AB：

α_AB=DEGREES(PI()*(1-SIGN(1.74 - 1)/2)-ATAN((2.64 - 3)/( 1.74 - 1)))

= 115.9422°。

上述公式得到方位角α_AB，再通过以下公式得到OA在本平面坐标系中的方位角α_OA：

α_OA1=α_AB -（90-∠AOB /2）=33.44；

α_OA2=α_AB +（90-∠AOB/2）=198.44。

结合以上公式得到的A（3，1）、L_OA以及α_OA，通过以下公式得到旋转关节f的旋转中心P（P_X，P_Y）：

其中，P₁为：

P_X1 =( A_X +L_OA*COS(α_OA1*PI()/180))=5；

P_Y1 =( A_Y +L_OA*COS(α_OA1*PI()/180))=2.73。

P₂为：

P_X2 =( A_X +L_OA*COS(α_OA2*PI()/180))=0.0096；

P_Y2 =( A_Y +L_OA*COS(α_OA2*PI()/180))=0.0028。

求得旋转中心后，通过向量夹角公式分别得到BC与O₁A和O₂A的夹角，公式如下：

cos<BC, OA>=BC* OA /| BC || OA |；

通过上述公式可得，BC与O₁A夹角为97.16°，BC与O₂A夹角为67.84°。由于BC与O₂A夹角小于90°时，所以O₂（0.0096，0.0028）为有效的旋转中心，与开始假设的旋转中心（0，0）接近，验证完成。

步骤二、

旋转关节f从标定位置进行旋转，旋转角度为a；旋转后的旋转关节为f'，得到转动后的旋转关节f'在当前位置的特征中心的坐标值和角度Cur_pos(x,y,u)以及旋转后相机拍照得到物料的特征中心的坐标值和角度Obj_pos(x,y,u)，再结合旋转关节f的旋转中心，求得旋转后的旋转关节f'的坐标F'（X₀,Y₀)：

X₀= (x - rx₀)* cos(a)-(y - ry₀)*sin(a)+rx₀；

Y₀= (x - rx₀)* sin(a)+(y - ry₀)*cos(a)+ry₀；

其中，x = Obj_pos_X ；y=Obj_pos_Y；rx₀=Pcenter_X；ry₀= Pcenter_Y。

当然，也可以通过Halcon算子获得：可用vector_angle_to_rigid算子建立旋转关系矩阵，得到HomMat2D，再使用转换算子affine_trans_point_2d代入旋转点以及旋转角度即可得到坐标F'（X₀,Y₀) 。

或者通过数学库方法获得：把旋转中心想象成一个空间坐标系，把向量OF求出来，接下来把Z轴旋转矩阵与向量OF作乘法运算，即可得到旋转后的坐标F'（X₀,Y₀)。

再将F'（X₀,Y₀)与Obj_pos(x,y,u)作差：

(X₀-Obj_pos_x，Y₀-Obj_pos_y)；

即可算出旋转关节需要移动的偏移量。

上述为本发明较佳的实施方式，但本发明的实施方式并不受上述内容的限制，其他的任何未背离本发明的精神实质与原理下所做的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。

Claims

1.一种搬运机械手的旋转标定方法，其特征在于，包括以下步骤：

建立物料搬运的平面坐标系，确定搬运机械手的旋转关节f在平面坐标系中的TCP，相机设置在搬运机械手的旋转关节f的末端；在平面坐标系上设置用于与相机进行标定的标定图，旋转关节f进行转动，使得相机移动至标定图上方，完成相机标定；记录旋转关节f在标定位置的特征中心的坐标值以及角度，作为用于计算旋转关节f经过旋转移动后的偏差量的参考位置；计算旋转关节f的旋转中心；

2.根据权利要求1所述的搬运机械手的旋转标定方法，其特征在于，计算搬运机械手的旋转关节f的旋转中心包括以下步骤：

3.根据权利要求2所述的搬运机械手的旋转标定方法，其特征在于，得到A（A_X，A_Y）和B（B_X，B_Y）的坐标后，通过以下公式得到线段AB的长度L_AB：

L_AB= SQRT((A_X-B_X)^2+(A_Y-B_Y)^2)。

4.根据权利要求3所述的搬运机械手的旋转标定方法，其特征在于，将OA的转动角度设为∠AOB，通过以下公式得到OA的长度L_OA以及OB的长度L_OB：

L_OA=(L_AB /2)/SIN((∠AOB /2)*PI()/180)；

L_OB= L_OA。

5.根据权利要求2所述的搬运机械手的旋转标定方法，其特征在于，通过以下公式得到AB在本平面坐标系中的方位角α_AB：

α_AB=DEGREES(PI()*(1-SIGN(B_Y -A_Y)/2)-ATAN((B_X -A_X)/(B_Y -A_Y)))；

其中，若求得的α_AB小于0 ，则α_AB加上360。

6.根据权利要求5所述的搬运机械手的旋转标定方法，其特征在于，结合AB的方位角α_AB，通过以下公式得到OA在本平面坐标系中的方位角α_OA：

α_OA1=α_AB -（90-∠AOB /2）；

α_OA2=α_AB +（90-∠AOB/2）。

7.根据权利要求6所述的搬运机械手的旋转标定方法，其特征在于，结合A（A_X，A_Y）、L_OA以及α_OA，通过以下公式得到旋转关节f的旋转中心Pcenter（P_X，P_Y）：

P_X =(A_X +L_OA*COS(α_OA*PI()/180))；

P_Y =(A_Y +L_OA*COS(α_OA*PI()/180))；

8.根据权利要求6所述的搬运机械手的旋转标定方法，其特征在于，求得旋转关节f的旋转中心后，通过向量夹角公式分别得到BC与OA₁和OA₂的夹角，公式如下：

cos<BC, OA>=BC* OA/| BC || OA |；

当BC与OA的夹角小于90°时，即为有效的旋转中心。

9.根据权利要求7所述的搬运机械手的旋转标定方法，其特征在于，将旋转关节f在标定位置的特征中心的坐标值以及角度设为refPos(x,y,u)，将转动后的旋转关节f'在当前位置的特征中心的坐标值以及角度设为Cur_pos(x,y,u)，将旋转后相机拍照得到物料的特征中心的坐标值和角度设为Obj_pos(x,y,u)；

X₀= (x - rx₀)*cos(a)-(y - ry₀)*sin(a)+rx₀；

Y₀= (x - rx₀)* sin(a)+(y - ry₀)*cos(a)+ry₀；

其中，x = Obj_pos_x；y=Obj_pos_y；rx₀=Pcenter_P_x；ry₀= Pcenter_P_y；a为已知的旋转角度。

10.根据权利要求9所述的搬运机械手的旋转标定方法，其特征在于，将F'（X₀,Y₀)与Obj_pos(x,y,u)作差：

(X₀-Obj_pos_x，Y₀-Obj_pos_y)；

计算出旋转关节需要移动的偏移量。