CN111745692A - 一种机器人夹具与机械臂干涉检测方法 - Google Patents

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

Info

Publication number
CN111745692A
CN111745692A CN202010891909.6A CN202010891909A CN111745692A CN 111745692 A CN111745692 A CN 111745692A CN 202010891909 A CN202010891909 A CN 202010891909A CN 111745692 A CN111745692 A CN 111745692A
Authority
CN
China
Prior art keywords
coordinate system
arm
model
axis
clamp
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202010891909.6A
Other languages
English (en)
Inventor
候金良
盛国强
汪良红
王辉
王雪峰
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Foshan Longshen Robot Co Ltd
Original Assignee
Foshan Longshen Robot Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Foshan Longshen Robot Co Ltd filed Critical Foshan Longshen Robot Co Ltd
Priority to CN202010891909.6A priority Critical patent/CN111745692A/zh
Publication of CN111745692A publication Critical patent/CN111745692A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J19/00—Accessories fitted to manipulators, e.g. for monitoring, for viewing; Safety devices combined with or specially adapted for use in connection with manipulators
    • B25J19/0095—Means or methods for testing manipulators
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01B—MEASURING LENGTH, THICKNESS OR SIMILAR LINEAR DIMENSIONS; MEASURING ANGLES; MEASURING AREAS; MEASURING IRREGULARITIES OF SURFACES OR CONTOURS
    • G01B21/00—Measuring arrangements or details thereof, where the measuring technique is not covered by the other groups of this subclass, unspecified or not relevant
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F30/00—Computer-aided design [CAD]
    • G06F30/20—Design optimisation, verification or simulation

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Computer Hardware Design (AREA)
  • Evolutionary Computation (AREA)
  • Geometry (AREA)
  • General Engineering & Computer Science (AREA)
  • Numerical Control (AREA)

Abstract

本发明提供一种机器人夹具与机械臂干涉检测方法，包括以下步骤：S1：建立机器人的工具坐标系，并将工具坐标系作为参考坐标系，分别将夹具的各部件拟合得到夹具模型；S2：建立机械臂的正运动学模型，实时获取机械臂各轴臂的坐标原点，并建立轴臂坐标系；S3：分别将各轴臂拟合得到轴臂模型；S4：分别将夹具模型投影到各个轴臂坐标系中进行表示；S5：分别在各个轴臂坐标系中对夹具模型与轴臂模型进行干涉检测，得到检测结果。本发明提供一种机器人夹具与机械臂干涉检测方法，通过对夹具模型和轴臂模型进行干涉检测，实现夹具与机械臂的干涉检测，解决了目前大部分的机器人抓取场景因未考虑夹具与机械臂的干涉情况而存在安全隐患的问题。

Description

一种机器人夹具与机械臂干涉检测方法
技术领域
本发明涉及机器人干涉检测技术领域，更具体的，涉及一种机器人夹具与机械臂干涉检测方法。
背景技术
在工业机器人工件抓取场景，由于需要抓取工件所以机械臂的末端都会设计相应的夹具；在抓放的过程中，工业机械臂自身轴臂之间的干涉检测在机器人的控制器中已经有预防机制，但是机械臂的夹具作为机械臂本体以外的配件，并未做任何的干涉预防机制；且机械臂末端的夹具一般包括夹持装置和气动或电动阀，结构较为复杂，给夹具与机械臂的干涉检测带来难度。
目前大部分的机器人抓取场景并未考虑夹具与机械臂的干涉问题，存在安全隐患；一旦夹具与机械臂的轴臂发生干涉，尤其在机械臂高速运动情况下，会损坏机械臂和夹具，导致安全事故。
现有技术中，如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）、
Figure 575197DEST_PATH_IMAGE001
、
Figure 190986DEST_PATH_IMAGE002
、
Figure 422247DEST_PATH_IMAGE003
、L、W、H；
其中，P（x0，y0，z0）为夹具坐标系的原点到参考坐标系的原点的平移向量；
Figure 72671DEST_PATH_IMAGE001
、
Figure 731186DEST_PATH_IMAGE002
、
Figure 99850DEST_PATH_IMAGE003
分别为夹具坐标系的x、y、z轴相对于参考坐标系的x、y、z轴的旋转角度；
S1.2：根据旋转角度计算夹具坐标系相对于参考坐标系的旋转矩阵R；
S1.3：将旋转矩阵与平移向量结合，得到夹具坐标系与参考坐标系的转化矩阵
Figure 869223DEST_PATH_IMAGE004
：
Figure 905312DEST_PATH_IMAGE005
；
S1.4：通过转化矩阵
Figure 734728DEST_PATH_IMAGE004
将夹具坐标系中的夹具模型的各个顶点坐标转换到参考坐标系下表示。
优选的，在步骤S1.2中，旋转矩阵R为3*3矩阵，其第一行第一列的元素为：
R(1,1)=cos(
Figure 325109DEST_PATH_IMAGE002
)*cos(
Figure 898173DEST_PATH_IMAGE003
)；
第一行第二列的元素为：
R(1,2)=sin(
Figure 520260DEST_PATH_IMAGE001
)*sin(
Figure 317315DEST_PATH_IMAGE002
)*cos(
Figure 129413DEST_PATH_IMAGE003
)-cos(
Figure 506167DEST_PATH_IMAGE001
)*sin(
Figure 188953DEST_PATH_IMAGE003
)；
第一行第三列的元素为：
R(1,3)=cos(
Figure 360171DEST_PATH_IMAGE001
)*sin(
Figure 659565DEST_PATH_IMAGE002
)*cos(
Figure 371169DEST_PATH_IMAGE003
)+sin(
Figure 970778DEST_PATH_IMAGE001
)*sin(
Figure 312898DEST_PATH_IMAGE003
)；
第二行第一列的元素为：
R(2, 1)=cos(
Figure 630746DEST_PATH_IMAGE002
)*sin(
Figure 614883DEST_PATH_IMAGE003
)；
第二行第二列的元素为：
R(2, 2)= sin(
Figure 803419DEST_PATH_IMAGE001
)*sin(
Figure 582019DEST_PATH_IMAGE002
)* sin(
Figure 590426DEST_PATH_IMAGE003
) + cos(
Figure 844166DEST_PATH_IMAGE001
) * cos(
Figure 152787DEST_PATH_IMAGE003
)；
第二行第三列的元素为：
R(2, 3)=cos(
Figure 836709DEST_PATH_IMAGE001
)*sin(
Figure 863571DEST_PATH_IMAGE002
)* sin(
Figure 455090DEST_PATH_IMAGE003
) - sin(
Figure 618218DEST_PATH_IMAGE001
)* cos(
Figure 738620DEST_PATH_IMAGE003
)；
第三行第一列的元素为：
R(3, 1) = -sin(
Figure 721620DEST_PATH_IMAGE002
)；
第三行第二列的元素为：
R(3, 2) = cos(
Figure 585671DEST_PATH_IMAGE002
) * sin(
Figure 400043DEST_PATH_IMAGE001
)；
第三行第三列的元素为：
R(3, 3) = cos(
Figure 629030DEST_PATH_IMAGE001
)*cos(
Figure 364905DEST_PATH_IMAGE002
)。
优选的，在步骤S2中，轴臂坐标系包括第一轴臂坐标系、第二轴臂坐标系、第三轴臂坐标系、第四轴臂坐标系、第五轴臂坐标系和第六轴臂坐标系。
优选的，在步骤S2中，还包括根据各个轴臂坐标系之间的平移向量和旋转矩阵建立轴臂坐标系转化矩阵；轴臂坐标系转化矩阵包括
Figure 767067DEST_PATH_IMAGE006
、
Figure 901858DEST_PATH_IMAGE007
、
Figure 98484DEST_PATH_IMAGE008
、
Figure 321655DEST_PATH_IMAGE009
和
Figure 324246DEST_PATH_IMAGE010
；
其中，
Figure 50894DEST_PATH_IMAGE006
为第一轴臂坐标系到第二轴臂坐标系的轴臂坐标系转化矩阵，
Figure 215159DEST_PATH_IMAGE007
为第二轴臂坐标系到第三轴臂坐标系的轴臂坐标系转化矩阵，
Figure 925626DEST_PATH_IMAGE008
为第三轴臂坐标系到第四轴臂坐标系的轴臂坐标系转化矩阵，
Figure 669591DEST_PATH_IMAGE009
为第四轴臂坐标系到第五轴臂坐标系的轴臂坐标系转化矩阵，
Figure 391690DEST_PATH_IMAGE010
为第五轴臂坐标系到第六轴臂坐标系的轴臂坐标系转化矩阵。
优选的，在步骤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）、
Figure 195698DEST_PATH_IMAGE001
、
Figure 127882DEST_PATH_IMAGE002
、
Figure 675538DEST_PATH_IMAGE003
、L、W、H；
其中，P（x0，y0，z0）为夹爪坐标系的原点到参考坐标系的原点的平移向量；
Figure 108269DEST_PATH_IMAGE001
、
Figure 348758DEST_PATH_IMAGE002
、
Figure 502658DEST_PATH_IMAGE003
分别为夹爪坐标系的x、y、z轴相对于参考坐标系的x、y、z轴的旋转角度；
S1.2：根据旋转角度计算夹爪坐标系相对于参考坐标系的旋转矩阵R；
S1.3：将旋转矩阵与平移向量结合，得到夹爪坐标系与参考坐标系的转化矩阵
Figure 854005DEST_PATH_IMAGE004
：
Figure 409752DEST_PATH_IMAGE005
；
S1.4：通过转化矩阵
Figure 821141DEST_PATH_IMAGE004
将夹爪坐标系中的夹爪模型的各个顶点坐标转换到参考坐标系下表示。
在具体实施过程中，同理可以将相机模型和吸嘴模型的各个顶点坐标转换到参考坐标系下表示。
更具体的，在步骤S1.2中，旋转矩阵R为3*3矩阵，其第一行第一列的元素为：
R(1,1)=cos(
Figure 462338DEST_PATH_IMAGE002
)*cos(
Figure 617376DEST_PATH_IMAGE003
)；
第一行第二列的元素为：
R(1,2)=sin(
Figure 27629DEST_PATH_IMAGE001
)*sin(
Figure 344341DEST_PATH_IMAGE002
)*cos(
Figure 738413DEST_PATH_IMAGE003
)-cos(
Figure 962721DEST_PATH_IMAGE001
)*sin(
Figure 493059DEST_PATH_IMAGE003
)；
第一行第三列的元素为：
R(1,3)=cos(
Figure 977743DEST_PATH_IMAGE001
)*sin(
Figure 593532DEST_PATH_IMAGE002
)*cos(
Figure 90372DEST_PATH_IMAGE003
)+sin(
Figure 271955DEST_PATH_IMAGE001
)*sin(
Figure 930469DEST_PATH_IMAGE003
)；
第二行第一列的元素为：
R(2, 1)=cos(
Figure 33555DEST_PATH_IMAGE002
)*sin(
Figure 802927DEST_PATH_IMAGE003
)；
第二行第二列的元素为：
R(2, 2)= sin(
Figure 307858DEST_PATH_IMAGE001
)*sin(
Figure 402853DEST_PATH_IMAGE002
)* sin(
Figure 524393DEST_PATH_IMAGE003
) + cos(
Figure 97457DEST_PATH_IMAGE001
) * cos(
Figure 722473DEST_PATH_IMAGE003
)；
第二行第三列的元素为：
R(2, 3)=cos(
Figure 722790DEST_PATH_IMAGE001
)*sin(
Figure 800468DEST_PATH_IMAGE002
)* sin(
Figure 708381DEST_PATH_IMAGE003
) - sin(
Figure 922324DEST_PATH_IMAGE001
)* cos(
Figure 90613DEST_PATH_IMAGE003
)；
第三行第一列的元素为：
R(3, 1) = -sin(
Figure 186745DEST_PATH_IMAGE002
)；
第三行第二列的元素为：
R(3, 2) = cos(
Figure 367191DEST_PATH_IMAGE002
) * sin(
Figure 232378DEST_PATH_IMAGE001
)；
第三行第三列的元素为：
R(3, 3) = cos(
Figure 574498DEST_PATH_IMAGE001
)*cos(
Figure 157926DEST_PATH_IMAGE002
)。
更具体的，在步骤S2中，轴臂坐标系包括第一轴臂坐标系、第二轴臂坐标系、第三轴臂坐标系、第四轴臂坐标系、第五轴臂坐标系和第六轴臂坐标系。
在具体实施过程中，实时获取机械臂第一轴臂的坐标原点并建立第一轴臂坐标系，实时获取机械臂第二轴臂的坐标原点并建立第二轴臂坐标系，实时获取机械臂第三轴臂的坐标原点并建立第三轴臂坐标系，实时获取机械臂第四轴臂的坐标原点并建立第四轴臂坐标系，实时获取机械臂第五轴臂的坐标原点并建立第五轴臂坐标系，实时获取机械臂第六轴臂的坐标原点并建立第六轴臂坐标系。
更具体的，在步骤S2中，还包括根据各个轴臂坐标系之间的平移向量和旋转矩阵建立轴臂坐标系转化矩阵；轴臂坐标系转化矩阵包括
Figure 142063DEST_PATH_IMAGE006
、
Figure 65019DEST_PATH_IMAGE007
、
Figure 374778DEST_PATH_IMAGE008
、
Figure 914344DEST_PATH_IMAGE009
和
Figure 967750DEST_PATH_IMAGE010
；
其中，
Figure 10793DEST_PATH_IMAGE006
为第一轴臂坐标系到第二轴臂坐标系的轴臂坐标系转化矩阵，
Figure 960294DEST_PATH_IMAGE007
为第二轴臂坐标系到第三轴臂坐标系的轴臂坐标系转化矩阵，
Figure 518314DEST_PATH_IMAGE008
为第三轴臂坐标系到第四轴臂坐标系的轴臂坐标系转化矩阵，
Figure 313095DEST_PATH_IMAGE009
为第四轴臂坐标系到第五轴臂坐标系的轴臂坐标系转化矩阵，
Figure 7382DEST_PATH_IMAGE010
为第五轴臂坐标系到第六轴臂坐标系的轴臂坐标系转化矩阵。
在具体实施过程中，机械臂各轴臂的转动角度通过tcp通讯实时从机械臂控制器获得。将轴臂坐标系转化矩阵逐一相乘就可以对应得到两个轴臂坐标系之间的轴臂坐标系转化矩阵，以得到第一轴臂坐标系到第五轴臂坐标系的轴臂坐标系转化矩阵
Figure 127784DEST_PATH_IMAGE011
为例，
Figure 376363DEST_PATH_IMAGE012
，进一步根据
Figure 771572DEST_PATH_IMAGE011
能够得到第五轴臂坐标系的原点的坐标。
在具体实施过程中，在步骤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得到的转化矩阵，分别将参考坐标系下的各个夹具模型投影到各个轴臂坐标系中进行表示。
在具体实施过程中，以计算由参考坐标系到第五轴臂坐标系的转化矩阵为例，通过与机器人通信获得第一轴臂坐标系到参考坐标系的转化矩阵
Figure 789207DEST_PATH_IMAGE013
，则由参考坐标系到第五轴臂坐标系的转化矩阵
Figure 101019DEST_PATH_IMAGE014
，根据
Figure 368052DEST_PATH_IMAGE015
进一步得到转化矩阵
Figure 770215DEST_PATH_IMAGE016
，然后根据
Figure 439093DEST_PATH_IMAGE017
将参考坐标系下的夹爪模型投影到第五轴臂坐标系中进行表示。
更具体的，在步骤S5中，分别在各个臂轴坐标系中采用空间占据的方式对夹具模型与轴臂模型进行干涉检测；
若至少在其中一个臂轴坐标系中，夹具模型与轴臂模型发生干涉，则夹具与机械臂发生干涉；
若在六个轴臂坐标系中，夹具模型与轴臂模型均不发生干涉，则夹具与机械臂不发生干涉。
在具体实施过程中，在第一轴臂坐标系中对相机模型、夹爪模型、吸嘴模型和第一轴臂模型进行干涉检测，在第二轴臂坐标系中对相机模型、夹爪模型、吸嘴模型和第二轴臂模型进行干涉检测，在第三轴臂坐标系中对相机模型、夹爪模型、吸嘴模型和第三轴臂模型进行干涉检测，在第四轴臂坐标系中对相机模型、夹爪模型、吸嘴模型和第四轴臂模型进行干涉检测，在第五轴臂坐标系中对相机模型、夹爪模型、吸嘴模型和第五轴臂模型进行干涉检测，在第六轴臂坐标系中对相机模型、夹爪模型、吸嘴模型和第六轴臂模型进行干涉检测。
更具体的，采用空间占据的方式进行干涉检测具体为：
将轴臂坐标系中的夹具模型和轴臂模型分别投影到所在轴臂坐标系的xy、yz和xz平面进行干涉检测；
若夹具模型和轴臂模型在xy、yz和xz三个平面上的投影都相交，则夹具模型与轴臂模型发生干涉；
否则，夹具模型与轴臂模型未发生干涉。
在具体实施过程中，相机模型、夹爪模型、吸嘴模型和轴臂模型均由长方体拟合而成，因此在xy、yz和xz平面上的投影均为长方形，只需要检测相机模型、夹爪模型和吸嘴模型在平面上投影的长方形与轴臂模型在平面上投影的长方形是否相交即能判断出两者是否干涉。
显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。

Claims (10)

1.一种机器人夹具与机械臂干涉检测方法，其特征在于，包括以下步骤：
S1：建立机器人的工具坐标系，并将工具坐标系作为参考坐标系，分别将夹具的各部件拟合得到夹具模型；
S2：建立机械臂的正运动学模型，实时获取机械臂各轴臂的坐标原点，并建立轴臂坐标系；
S3：根据各个轴臂坐标系分别将各轴臂拟合得到轴臂模型；
S4：分别将夹具模型投影到各个轴臂坐标系中进行表示；
S5：分别在各个轴臂坐标系中对夹具模型与轴臂模型进行干涉检测，得到检测结果。
2.根据权利要求1所述的一种机器人夹具与机械臂干涉检测方法，其特征在于，在步骤S1中，夹具模型包括相机模型、夹爪模型和吸嘴模型；其中，相机模型通过将夹具的相机部件拟合成长方体得到；夹爪模型通过将夹具的夹爪部件拟合成长方体得到；吸嘴模型通过将夹具的吸嘴部件拟合成长方体得到。
3.根据权利要求2所述的一种机器人夹具与机械臂干涉检测方法，其特征在于，在步骤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轴方向的边长。
4.根据权利要求3所述的一种机器人夹具与机械臂干涉检测方法，其特征在于，在步骤S1中，还包括以下步骤：
S1.1：通过实际测量得到P（x0，y0，z0）、
Figure 297218DEST_PATH_IMAGE001
、
Figure 520389DEST_PATH_IMAGE002
、
Figure 257401DEST_PATH_IMAGE003
、L、W、H；
其中，P（x0，y0，z0）为夹具坐标系的原点到参考坐标系的原点的平移向量；
Figure 984049DEST_PATH_IMAGE001
、
Figure 882735DEST_PATH_IMAGE002
、
Figure 124360DEST_PATH_IMAGE003
分别为夹具坐标系的x、y、z轴相对于参考坐标系的x、y、z轴的旋转角度；
S1.2：根据旋转角度计算夹具坐标系相对于参考坐标系的旋转矩阵R；
S1.3：将旋转矩阵与平移向量结合，得到夹具坐标系与参考坐标系的转化矩阵
Figure 71588DEST_PATH_IMAGE004
：
Figure 715059DEST_PATH_IMAGE005
；
S1.4：通过转化矩阵
Figure 253487DEST_PATH_IMAGE004
将夹具坐标系中的夹具模型的各个顶点坐标转换到参考坐标系下表示。
5.根据权利要求4所述的一种机器人夹具与机械臂干涉检测方法，其特征在于，在步骤S1.2中，旋转矩阵R为3*3矩阵，其第一行第一列的元素为：
R(1,1)=cos(
Figure 982409DEST_PATH_IMAGE002
)*cos(
Figure 530065DEST_PATH_IMAGE003
)；
第一行第二列的元素为：
R(1,2)=sin(
Figure 228375DEST_PATH_IMAGE001
)*sin(
Figure 22DEST_PATH_IMAGE002
)*cos(
Figure 153923DEST_PATH_IMAGE003
)-cos(
Figure 36428DEST_PATH_IMAGE001
)*sin(
Figure 857753DEST_PATH_IMAGE003
)；
第一行第三列的元素为：
R(1,3)=cos(
Figure 3564DEST_PATH_IMAGE001
)*sin(
Figure 441499DEST_PATH_IMAGE002
)*cos(
Figure 330957DEST_PATH_IMAGE003
)+sin(
Figure 6789DEST_PATH_IMAGE001
)*sin(
Figure 120239DEST_PATH_IMAGE003
)；
第二行第一列的元素为：
R(2, 1)=cos(
Figure 514311DEST_PATH_IMAGE002
)*sin(
Figure 738619DEST_PATH_IMAGE003
)；
第二行第二列的元素为：
R(2, 2)= sin(
Figure 3378DEST_PATH_IMAGE001
)*sin(
Figure 287729DEST_PATH_IMAGE002
)* sin(
Figure 903518DEST_PATH_IMAGE003
) + cos(
Figure 400358DEST_PATH_IMAGE001
) * cos(
Figure 581941DEST_PATH_IMAGE003
)；
第二行第三列的元素为：
R(2, 3)=cos(
Figure 974876DEST_PATH_IMAGE001
)*sin(
Figure 874699DEST_PATH_IMAGE002
)* sin(
Figure 909651DEST_PATH_IMAGE003
) - sin(
Figure 411652DEST_PATH_IMAGE001
)* cos(
Figure 772226DEST_PATH_IMAGE003
)；
第三行第一列的元素为：
R(3, 1) = -sin(
Figure 628187DEST_PATH_IMAGE002
)；
第三行第二列的元素为：
R(3, 2) = cos(
Figure 997988DEST_PATH_IMAGE002
) * sin(
Figure 91846DEST_PATH_IMAGE001
)；
第三行第三列的元素为：
R(3, 3) = cos(
Figure 357743DEST_PATH_IMAGE001
)*cos(
Figure 966579DEST_PATH_IMAGE002
)。
6.根据权利要求4所述的一种机器人夹具与机械臂干涉检测方法，其特征在于，在步骤S2中，轴臂坐标系包括第一轴臂坐标系、第二轴臂坐标系、第三轴臂坐标系、第四轴臂坐标系、第五轴臂坐标系和第六轴臂坐标系。
7.根据权利要求6所述的一种机器人夹具与机械臂干涉检测方法，其特征在于，在步骤S2中，还包括根据各个轴臂坐标系之间的平移向量和旋转矩阵建立轴臂坐标系转化矩阵；轴臂坐标系转化矩阵包括
Figure 343333DEST_PATH_IMAGE006
、
Figure 354015DEST_PATH_IMAGE007
、
Figure 525233DEST_PATH_IMAGE008
、
Figure 621365DEST_PATH_IMAGE009
和
Figure 801811DEST_PATH_IMAGE010
；
其中，
Figure 870261DEST_PATH_IMAGE006
为第一轴臂坐标系到第二轴臂坐标系的轴臂坐标系转化矩阵，
Figure 9118DEST_PATH_IMAGE007
为第二轴臂坐标系到第三轴臂坐标系的轴臂坐标系转化矩阵，
Figure 61388DEST_PATH_IMAGE008
为第三轴臂坐标系到第四轴臂坐标系的轴臂坐标系转化矩阵，
Figure 311103DEST_PATH_IMAGE009
为第四轴臂坐标系到第五轴臂坐标系的轴臂坐标系转化矩阵，
Figure 499639DEST_PATH_IMAGE010
为第五轴臂坐标系到第六轴臂坐标系的轴臂坐标系转化矩阵。
8.根据权利要求7所述的一种机器人夹具与机械臂干涉检测方法，其特征在于，在步骤S4中，具体包括以下步骤：
S4.1：根据各个轴臂坐标系转化矩阵，计算由参考坐标系到各个轴臂坐标系的转化矩阵；
S4.2：根据步骤S4.1得到的转化矩阵，分别将参考坐标系下的各个夹具模型投影到各个轴臂坐标系中进行表示。
9.根据权利要求8所述的一种机器人夹具与机械臂干涉检测方法，其特征在于，在步骤S5中，分别在各个臂轴坐标系中采用空间占据的方式对夹具模型与轴臂模型进行干涉检测；
若至少在其中一个臂轴坐标系中，夹具模型与轴臂模型发生干涉，则夹具与机械臂发生干涉；
若在六个轴臂坐标系中，夹具模型与轴臂模型均不发生干涉，则夹具与机械臂不发生干涉。
10.根据权利要求9所述的一种机器人夹具与机械臂干涉检测方法，其特征在于，采用空间占据的方式进行干涉检测具体为：
将轴臂坐标系中的夹具模型和轴臂模型分别投影到所在轴臂坐标系的xy、yz和xz平面进行干涉检测；
若夹具模型和轴臂模型在xy、yz和xz三个平面上的投影都相交，则夹具模型与轴臂模型发生干涉；
否则，夹具模型与轴臂模型未发生干涉。
CN202010891909.6A 2020-08-31 2020-08-31 一种机器人夹具与机械臂干涉检测方法 Pending CN111745692A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010891909.6A CN111745692A (zh) 2020-08-31 2020-08-31 一种机器人夹具与机械臂干涉检测方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010891909.6A CN111745692A (zh) 2020-08-31 2020-08-31 一种机器人夹具与机械臂干涉检测方法

Publications (1)

Publication Number Publication Date
CN111745692A true CN111745692A (zh) 2020-10-09

Family

ID=72713436

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010891909.6A Pending CN111745692A (zh) 2020-08-31 2020-08-31 一种机器人夹具与机械臂干涉检测方法

Country Status (1)

Country Link
CN (1) CN111745692A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113664874A (zh) * 2021-08-30 2021-11-19 湖南工业大学 一种移动机器人多臂的干涉检测方法

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103236079A (zh) * 2013-04-19 2013-08-07 浙江理工大学 一种基于三维模型体素化的内部球改进构造方法
CN104850699A (zh) * 2015-05-19 2015-08-19 天津市天锻压力机有限公司 冲压线搬运机器人防碰撞控制方法
DE102017129959A1 (de) * 2017-12-14 2019-04-25 Schaeffler Technologies AG & Co. KG Verfahren zum Betrieb eines Systems mit mehreren Maschinen
CN110139552A (zh) * 2016-11-08 2019-08-16 道格图斯科技有限公司 机器人水果采摘系统
CN111360824A (zh) * 2020-02-27 2020-07-03 中科新松有限公司 一种双臂自碰撞检测方法和计算机可读存储介质
CN111515944A (zh) * 2020-03-30 2020-08-11 季华实验室 非固定路径机器人自动标定方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103236079A (zh) * 2013-04-19 2013-08-07 浙江理工大学 一种基于三维模型体素化的内部球改进构造方法
CN104850699A (zh) * 2015-05-19 2015-08-19 天津市天锻压力机有限公司 冲压线搬运机器人防碰撞控制方法
CN110139552A (zh) * 2016-11-08 2019-08-16 道格图斯科技有限公司 机器人水果采摘系统
DE102017129959A1 (de) * 2017-12-14 2019-04-25 Schaeffler Technologies AG & Co. KG Verfahren zum Betrieb eines Systems mit mehreren Maschinen
CN111360824A (zh) * 2020-02-27 2020-07-03 中科新松有限公司 一种双臂自碰撞检测方法和计算机可读存储介质
CN111515944A (zh) * 2020-03-30 2020-08-11 季华实验室 非固定路径机器人自动标定方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
单春艳: "面向井下无人机自主导航的位姿估计与障碍物检测技术研究", 《中国硕士学位论文全文数据库（电子期刊）工程科技Ⅰ辑》 *
王岩: "基于虚拟现实的空间站机械臂任务仿真研究", 《中国硕士学位论文全文数据库（电子期刊）科技信息辑》 *

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113664874A (zh) * 2021-08-30 2021-11-19 湖南工业大学 一种移动机器人多臂的干涉检测方法

Similar Documents

Publication Publication Date Title
JP7003463B2 (ja) ロボットの制御装置、ロボットシステム、並びに、カメラの校正方法
Zhuang et al. Simultaneous robot/world and tool/flange calibration by solving homogeneous transformation equations of the form AX= YB
US11267142B2 (en) Imaging device including vision sensor capturing image of workpiece
JP7035657B2 (ja) ロボットの制御装置、ロボット、ロボットシステム、並びに、カメラの校正方法
US9937620B2 (en) Robot system having function to calculate position and orientation of sensor
US20150321354A1 (en) Picking apparatus and picking method
US20180345493A1 (en) Teaching position correction device and teaching position correction method
US9248572B2 (en) Axis angle determination method for six-axis robot and control apparatus for six-axis robot
Siradjuddin et al. A position based visual tracking system for a 7 DOF robot manipulator using a Kinect camera
CN111496849B (zh) 一种料框与夹具快速碰撞检测的方法
JP5250858B2 (ja) 複数ロボットの衝突検知方法及びロボット装置
CN109623849B (zh) 限制机器人结构部件的速度的控制装置
Popov et al. Real-time external contact force estimation and localization for collaborative robot
JP2000263481A (ja) ビンピッキング装置
CN111745692A (zh) 一种机器人夹具与机械臂干涉检测方法
CN112720458A (zh) 一种在线实时校正机器人工具坐标系的系统及方法
JP7358747B2 (ja) ロボットシステム
CN114074331A (zh) 一种基于视觉的无序抓取方法及机器人
CN108972547B (zh) 一种八自由度焊接轨迹在线生成系统协调运动控制方法
JP4233620B2 (ja) 把持位置姿勢教示装置
JP2000039911A (ja) ロボット制御装置
Li et al. An efficient method for inverse dynamics of kinematically defective parallel platforms
Tang et al. Dual manipulator system of the field hot-line working robot in 110-kV substations
KR20200078885A (ko) 다축기계의 최대직각프리즘 작업공간 산출방법
TW201945144A (zh) 握持控制裝置

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
RJ01 Rejection of invention patent application after publication

Application publication date: 20201009

RJ01 Rejection of invention patent application after publication