一种机器人夹具与机械臂干涉检测方法与流程

本发明涉及机器人干涉检测技术领域，更具体的，涉及一种机器人夹具与机械臂干涉检测方法。

背景技术：

在工业机器人工件抓取场景，由于需要抓取工件所以机械臂的末端都会设计相应的夹具；在抓放的过程中，工业机械臂自身轴臂之间的干涉检测在机器人的控制器中已经有预防机制，但是机械臂的夹具作为机械臂本体以外的配件，并未做任何的干涉预防机制；且机械臂末端的夹具一般包括夹持装置和气动或电动阀，结构较为复杂，给夹具与机械臂的干涉检测带来难度。

目前大部分的机器人抓取场景并未考虑夹具与机械臂的干涉问题，存在安全隐患；一旦夹具与机械臂的轴臂发生干涉，尤其在机械臂高速运动情况下，会损坏机械臂和夹具，导致安全事故。

现有技术中，如2015年5月20日公开的中国专利，航天器机械臂辅助装配安全工作空间建立及干涉预警方法，公开号为cn104626208a，通过在虚拟环境中导入机械臂、待装配产品及工作环境中其他相关物体的三维模型，实际测量机械臂与待装配产品的相对位置和相对姿态，并根据测量值对应设置虚拟环境中机械臂与待装配产品的相对位置和相对位姿，使其与实际测量结果一致；在虚拟环境中通过三维模型干涉检查判断机械臂及其附带物体是否达到安全工作空间边界，实现干涉检测；但没有对机械臂与其附带物体之间的干涉进行检测。

技术实现要素：

本发明为克服目前大部分的机器人抓取场景因未考虑夹具与机械臂的干涉情况而存在安全隐患的技术缺陷，提供一种机器人夹具与机械臂干涉检测方法。

为解决上述技术问题，本发明的技术方案如下：

一种机器人夹具与机械臂干涉检测方法，包括以下步骤：

s1：建立机器人的工具坐标系，并将工具坐标系作为参考坐标系，分别将夹具的各部件拟合得到夹具模型；

s2：建立机械臂的正运动学模型，实时获取机械臂各轴臂的坐标原点，并建立轴臂坐标系；

s3：根据各个轴臂坐标系分别将各轴臂拟合得到轴臂模型；

s4：分别将夹具模型投影到各个轴臂坐标系中进行表示；

s5：分别在各个轴臂坐标系中对夹具模型与轴臂模型进行干涉检测，得到检测结果。

优选的，在步骤s1中，夹具模型包括相机模型、夹爪模型和吸嘴模型；其中，相机模型通过将夹具的相机部件拟合成长方体得到；夹爪模型通过将夹具的夹爪部件拟合成长方体得到；吸嘴模型通过将夹具的吸嘴部件拟合成长方体得到。

优选的，在步骤s1中，还包括以下步骤：

以夹具模型的中心点为原点建立夹具坐标系；其中，夹具坐标系的x、y、z轴分别与夹具模型的长、宽、高三条棱边平行；

因此，夹具模型的各个顶点在夹具坐标系中的坐标为：

p1=（l/2，w/2，h/2）；

p2=（-l/2，w/2，h/2）；

p3=（l/2，-w/2，h/2）；

p4=（-l/2，-w/2，h/2）；

p5=（l/2，w/2，-h/2）；

p6=（-l/2，w/2，-h/2）；

p7=（l/2，-w/2，-h/2）；

p8=（-l/2，-w/2，-h/2）；

其中，l、w、h分别为夹具沿x、y、z轴方向的边长。

优选的，在步骤s1中，还包括以下步骤：

s1.1：通过实际测量得到p（x0，y0，z0）、、、、l、w、h；

其中，p（x0，y0，z0）为夹具坐标系的原点到参考坐标系的原点的平移向量；、、分别为夹具坐标系的x、y、z轴相对于参考坐标系的x、y、z轴的旋转角度；

s1.2：根据旋转角度计算夹具坐标系相对于参考坐标系的旋转矩阵r；

s1.3：将旋转矩阵与平移向量结合，得到夹具坐标系与参考坐标系的转化矩阵：

；

s1.4：通过转化矩阵将夹具坐标系中的夹具模型的各个顶点坐标转换到参考坐标系下表示。

优选的，在步骤s1.2中，旋转矩阵r为3*3矩阵，其第一行第一列的元素为：

r(1,1)=cos()*cos()；

第一行第二列的元素为：

r(1,2)=sin()*sin()*cos()-cos()*sin()；

第一行第三列的元素为：

r(1,3)=cos()*sin()*cos()+sin()*sin()；

第二行第一列的元素为：

r(2,1)=cos()*sin()；

第二行第二列的元素为：

r(2,2)=sin()*sin()*sin()+cos()*cos()；

第二行第三列的元素为：

r(2,3)=cos()*sin()*sin()-sin()*cos()；

第三行第一列的元素为：

r(3,1)=-sin()；

第三行第二列的元素为：

r(3,2)=cos()*sin()；

第三行第三列的元素为：

r(3,3)=cos()*cos()。

优选的，在步骤s2中，轴臂坐标系包括第一轴臂坐标系、第二轴臂坐标系、第三轴臂坐标系、第四轴臂坐标系、第五轴臂坐标系和第六轴臂坐标系。

优选的，在步骤s2中，还包括根据各个轴臂坐标系之间的平移向量和旋转矩阵建立轴臂坐标系转化矩阵；轴臂坐标系转化矩阵包括、、、和；

其中，为第一轴臂坐标系到第二轴臂坐标系的轴臂坐标系转化矩阵，为第二轴臂坐标系到第三轴臂坐标系的轴臂坐标系转化矩阵，为第三轴臂坐标系到第四轴臂坐标系的轴臂坐标系转化矩阵，为第四轴臂坐标系到第五轴臂坐标系的轴臂坐标系转化矩阵，为第五轴臂坐标系到第六轴臂坐标系的轴臂坐标系转化矩阵。

优选的，在步骤s4中，具体包括以下步骤：

s4.1：根据各个轴臂坐标系转化矩阵，计算由参考坐标系到各个轴臂坐标系的转化矩阵；

s4.2：根据步骤s4.1得到的转化矩阵，分别将参考坐标系下的各个夹具模型投影到各个轴臂坐标系中进行表示。

优选的，在步骤s5中，分别在各个臂轴坐标系中采用空间占据的方式对夹具模型与轴臂模型进行干涉检测；

若至少在其中一个臂轴坐标系中，夹具模型与轴臂模型发生干涉，则夹具与机械臂发生干涉；

若在六个轴臂坐标系中，夹具模型与轴臂模型均不发生干涉，则夹具与机械臂不发生干涉。

优选的，采用空间占据的方式进行干涉检测具体为：

将轴臂坐标系中的夹具模型和轴臂模型分别投影到所在轴臂坐标系的xy、yz和xz平面进行干涉检测；

若夹具模型和轴臂模型在xy、yz和xz三个平面上的投影都相交，则夹具模型与轴臂模型发生干涉；

否则，夹具模型与轴臂模型未发生干涉。

与现有技术相比，本发明技术方案的有益效果是：

本发明提供了一种机器人夹具与机械臂干涉检测方法，对机器人夹具进行三维拟合得到夹具模型，对机械臂各个轴臂进行实时三维拟合得到轴臂模型，然后对夹具模型和轴臂模型进行干涉检测，实现夹具与机械臂的干涉检测，预防安全事故的发生。

附图说明

图1为本发明的技术方案实施步骤流程图。

具体实施方式

附图仅用于示例性说明，不能理解为对本专利的限制；

为了更好说明本实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；

对于本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的。

下面结合附图和实施例对本发明的技术方案做进一步的说明。

实施例1

如图1所示，一种机器人夹具与机械臂干涉检测方法，包括以下步骤：

s1：建立机器人的工具坐标系，并将工具坐标系作为参考坐标系，分别将夹具的各部件拟合得到夹具模型；

s2：建立机械臂的正运动学模型，实时获取机械臂各轴臂的坐标原点，并建立轴臂坐标系；

s3：根据各个轴臂坐标系分别将各轴臂拟合得到轴臂模型；

s4：分别将夹具模型投影到各个轴臂坐标系中进行表示；

s5：分别在各个轴臂坐标系中对夹具模型与轴臂模型进行干涉检测，得到检测结果。

在具体实施过程中，机器人的工具坐标系通过机器人建立得到，工具坐标系的坐标原点为机器人夹具的吸嘴末端的中心点，z轴方向为平行于吸嘴杆件的长度方向，x轴方向为夹爪的开合方向，本实施例的夹爪绕铰接轴转动开合，因此，y轴方向平行于铰接轴，且夹爪位于y轴的正向。

更具体的，在步骤s1中，夹具模型包括相机模型、夹爪模型和吸嘴模型；其中，相机模型通过将夹具的相机部件拟合成长方体得到；夹爪模型通过将夹具的夹爪部件拟合成长方体得到；吸嘴模型通过将夹具的吸嘴部件拟合成长方体得到。

更具体的，在步骤s1中，还包括以下步骤：

以夹爪模型为例：

以夹爪模型的中心点为原点建立夹爪坐标系；其中，夹爪坐标系的x、y、z轴分别与夹爪模型的长、宽、高三条棱边平行；

因此，夹爪模型的各个顶点在夹爪坐标系中的坐标为：

p1=（l/2，w/2，h/2）；p2=（-l/2，w/2，h/2）；p3=（l/2，-w/2，h/2）；

p4=（-l/2，-w/2，h/2）；p5=（l/2，w/2，-h/2）；p6=（-l/2，w/2，-h/2）；

p7=（l/2，-w/2，-h/2）；p8=（-l/2，-w/2，-h/2）；

其中，l、w、h分别为夹爪沿x、y、z轴方向的边长。

在具体实施过程中，同理可以计算出相机模型和吸嘴模型的顶点坐标。

更具体的，在步骤s1中，还包括以下步骤：

以夹爪模型为例：

s1.1：通过实际测量得到p（x0，y0，z0）、、、、l、w、h；

其中，p（x0，y0，z0）为夹爪坐标系的原点到参考坐标系的原点的平移向量；、、分别为夹爪坐标系的x、y、z轴相对于参考坐标系的x、y、z轴的旋转角度；

s1.2：根据旋转角度计算夹爪坐标系相对于参考坐标系的旋转矩阵r；

s1.3：将旋转矩阵与平移向量结合，得到夹爪坐标系与参考坐标系的转化矩阵：

；

s1.4：通过转化矩阵将夹爪坐标系中的夹爪模型的各个顶点坐标转换到参考坐标系下表示。

在具体实施过程中，同理可以将相机模型和吸嘴模型的各个顶点坐标转换到参考坐标系下表示。

更具体的，在步骤s1.2中，旋转矩阵r为3*3矩阵，其第一行第一列的元素为：

r(1,1)=cos()*cos()；

第一行第二列的元素为：

r(1,2)=sin()*sin()*cos()-cos()*sin()；

第一行第三列的元素为：

r(1,3)=cos()*sin()*cos()+sin()*sin()；

第二行第一列的元素为：

r(2,1)=cos()*sin()；

第二行第二列的元素为：

r(2,2)=sin()*sin()*sin()+cos()*cos()；

第二行第三列的元素为：

r(2,3)=cos()*sin()*sin()-sin()*cos()；

第三行第一列的元素为：

r(3,1)=-sin()；

第三行第二列的元素为：

r(3,2)=cos()*sin()；

第三行第三列的元素为：

r(3,3)=cos()*cos()。

更具体的，在步骤s2中，轴臂坐标系包括第一轴臂坐标系、第二轴臂坐标系、第三轴臂坐标系、第四轴臂坐标系、第五轴臂坐标系和第六轴臂坐标系。

在具体实施过程中，实时获取机械臂第一轴臂的坐标原点并建立第一轴臂坐标系，实时获取机械臂第二轴臂的坐标原点并建立第二轴臂坐标系，实时获取机械臂第三轴臂的坐标原点并建立第三轴臂坐标系，实时获取机械臂第四轴臂的坐标原点并建立第四轴臂坐标系，实时获取机械臂第五轴臂的坐标原点并建立第五轴臂坐标系，实时获取机械臂第六轴臂的坐标原点并建立第六轴臂坐标系。

更具体的，在步骤s2中，还包括根据各个轴臂坐标系之间的平移向量和旋转矩阵建立轴臂坐标系转化矩阵；轴臂坐标系转化矩阵包括、、、和；

其中，为第一轴臂坐标系到第二轴臂坐标系的轴臂坐标系转化矩阵，为第二轴臂坐标系到第三轴臂坐标系的轴臂坐标系转化矩阵，为第三轴臂坐标系到第四轴臂坐标系的轴臂坐标系转化矩阵，为第四轴臂坐标系到第五轴臂坐标系的轴臂坐标系转化矩阵，为第五轴臂坐标系到第六轴臂坐标系的轴臂坐标系转化矩阵。

在具体实施过程中，机械臂各轴臂的转动角度通过tcp通讯实时从机械臂控制器获得。将轴臂坐标系转化矩阵逐一相乘就可以对应得到两个轴臂坐标系之间的轴臂坐标系转化矩阵，以得到第一轴臂坐标系到第五轴臂坐标系的轴臂坐标系转化矩阵为例，，进一步根据能够得到第五轴臂坐标系的原点的坐标。

在具体实施过程中，在步骤s3中，以第五轴臂模型为例，得到第五轴臂坐标系的原点的坐标后，通过查看机器人的工程图纸获得机械臂第五轴臂模型在第五轴臂坐标系的x、y和z轴的分量长度记为l5，w5和h5，根据分量计算机械臂第五轴臂模型的8个顶点坐标，公式如下：

p51=（l5/2，w5/2，0）；

p52=（-l5/2，w5/2，0）；

p53=（l5/2，-w5/2，0）；

p54=（-l5/2，-w5/2，0）；

p55=（l5/2，w5/2，h5）；

p56=（-l5/2，w5/2，h5）；

p57=（l5/2，-w5/2，h5）；

p58=（-l5/2，-w5/2，h5）；

同理可以计算出其他轴臂模型的顶点坐标。

更具体的，在步骤s4中，具体包括以下步骤：

s4.1：根据各个轴臂坐标系转化矩阵，计算由参考坐标系到各个轴臂坐标系的转化矩阵；

s4.2：根据步骤s4.1得到的转化矩阵，分别将参考坐标系下的各个夹具模型投影到各个轴臂坐标系中进行表示。

在具体实施过程中，以计算由参考坐标系到第五轴臂坐标系的转化矩阵为例，通过与机器人通信获得第一轴臂坐标系到参考坐标系的转化矩阵，则由参考坐标系到第五轴臂坐标系的转化矩阵，根据进一步得到转化矩阵，然后根据将参考坐标系下的夹爪模型投影到第五轴臂坐标系中进行表示。

更具体的，在步骤s5中，分别在各个臂轴坐标系中采用空间占据的方式对夹具模型与轴臂模型进行干涉检测；

若至少在其中一个臂轴坐标系中，夹具模型与轴臂模型发生干涉，则夹具与机械臂发生干涉；

若在六个轴臂坐标系中，夹具模型与轴臂模型均不发生干涉，则夹具与机械臂不发生干涉。

在具体实施过程中，在第一轴臂坐标系中对相机模型、夹爪模型、吸嘴模型和第一轴臂模型进行干涉检测，在第二轴臂坐标系中对相机模型、夹爪模型、吸嘴模型和第二轴臂模型进行干涉检测，在第三轴臂坐标系中对相机模型、夹爪模型、吸嘴模型和第三轴臂模型进行干涉检测，在第四轴臂坐标系中对相机模型、夹爪模型、吸嘴模型和第四轴臂模型进行干涉检测，在第五轴臂坐标系中对相机模型、夹爪模型、吸嘴模型和第五轴臂模型进行干涉检测，在第六轴臂坐标系中对相机模型、夹爪模型、吸嘴模型和第六轴臂模型进行干涉检测。

更具体的，采用空间占据的方式进行干涉检测具体为：

将轴臂坐标系中的夹具模型和轴臂模型分别投影到所在轴臂坐标系的xy、yz和xz平面进行干涉检测；

若夹具模型和轴臂模型在xy、yz和xz三个平面上的投影都相交，则夹具模型与轴臂模型发生干涉；

否则，夹具模型与轴臂模型未发生干涉。

在具体实施过程中，相机模型、夹爪模型、吸嘴模型和轴臂模型均由长方体拟合而成，因此在xy、yz和xz平面上的投影均为长方形，只需要检测相机模型、夹爪模型和吸嘴模型在平面上投影的长方形与轴臂模型在平面上投影的长方形是否相交即能判断出两者是否干涉。

显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。