CN111558940A - 一种机器人料框抓取规划与碰撞检测方法 - Google Patents

一种机器人料框抓取规划与碰撞检测方法 Download PDF

Info

Publication number
CN111558940A
CN111558940A CN202010463181.7A CN202010463181A CN111558940A CN 111558940 A CN111558940 A CN 111558940A CN 202010463181 A CN202010463181 A CN 202010463181A CN 111558940 A CN111558940 A CN 111558940A
Authority
CN
China
Prior art keywords
coordinate system
material frame
grabbing
robot
workpiece
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.)
Granted
Application number
CN202010463181.7A
Other languages
English (en)
Other versions
CN111558940B (zh
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 CN202010463181.7A priority Critical patent/CN111558940B/zh
Publication of CN111558940A publication Critical patent/CN111558940A/zh
Application granted granted Critical
Publication of CN111558940B publication Critical patent/CN111558940B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1674—Programme controls characterised by safety, monitoring, diagnostic
    • B25J9/1676—Avoiding collision or forbidden zones
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1679—Programme controls characterised by the tasks executed

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Manipulator (AREA)

Abstract

本发明提供一种机器人料框抓取规划与碰撞检测方法，包括以下步骤：S1：将实际模型简化，输入模型参数建立简化模型；S2：根据简化模型建立坐标系，并进行坐标系变换；S3：获取目标工件相对于机器人的位姿；S4：将位姿进行变换后得到抓取姿态，并带入到坐标系进行碰撞检测；S5：在碰撞检测的基础上做抓取规划。本发明提供一种机器人料框抓取规划与碰撞检测方法，通过对复杂的环境进行抽象及简化，建立简化模型，经过坐标系变换进行碰撞检测和抓取规划，实现减少繁琐的模型制作与导入，快速地给出抓取结果。

Description

一种机器人料框抓取规划与碰撞检测方法
技术领域
本发明涉及机器人技术领域，更具体的，涉及一种机器人料框抓取规划与碰撞检测方法。
背景技术
随着劳动力成本的不断增加和自动化技术的提高，以及“中国制造2025”战略的提出，使用自动化设备代替人工生产的比率越来越高。工件上料是工厂非常常见的一道工序，其工作繁重乏味，越来越少人愿意从事这一工作。料框抓取是目前解决上料问题的关键方法之一，其中就涉及到机器人抓取工件时对料框碰撞的检测和避障，同时规划出较好的抓取姿态提高抓取的成功率。现有的料框抓取方法繁琐，需要导入机器人模型、夹具模型和料框模型，并且给出结果的时间较长。
发明内容
本发明为克服现有的料框抓取方法繁琐的技术缺陷，提供一种机器人料框抓取规划与碰撞检测方法。
为解决上述技术问题，本发明的技术方案如下：
一种机器人料框抓取规划与碰撞检测方法，包括以下步骤：
S1：将实际模型简化，输入模型参数建立简化模型；
S2：根据简化模型建立坐标系，并进行坐标系变换；
S3：获取目标工件相对于机器人的位姿；
S4：将位姿进行变换后得到抓取姿态，并带入到坐标系进行碰撞检测；
S5：在碰撞检测的基础上做抓取规划。
上述方案中，通过对复杂的环境进行抽象及简化，建立简化模型，经过坐标系变换进行碰撞检测和抓取规划，实现减少繁琐的模型制作与导入，快速地给出抓取结果。
优选的，在步骤S1中，将实际模型简化包括：将机器人的抓取夹爪的两片夹爪简化为一个长方体，将料框简化为五个平面。
上述方案中，抓取单元主要包括：一台工业串联机器人、一个末端抓取夹爪、一个料框以及被抓取物品。一般工业抓取夹爪为二指，两指平行且对齐，每片夹爪在机器人末端法向方向上尺寸较大，而厚度较小；料框为长方体结构，包含五个面，四个侧面和一个底面。因为实际情况为夹爪在机器人末端法向方向上尺寸会大于料框的高度，即机器人本体一般不会和料框发生碰撞，而抓取过程中也一般不考虑与料框底部的碰撞，因此在抓取碰撞检测问题中，只需考虑夹爪与料框侧面的碰撞情况。
优选的，在步骤S2中，建立的坐标系包括机器人坐标系O1，夹爪坐标系O2和料框坐标系O3。
上述方案中，机器人坐标系由机器人本体决定；料框坐标系以三条棱边建立坐标系；夹爪坐标系的圆心为夹爪向外最远处中心点，以机器人末端向外为z轴，x轴为夹爪闭合张开方向，y轴由右手定则决定。
优选的，在步骤S2中，将夹爪坐标系O2变换到料框坐标系O3下考虑夹爪与料框侧面的碰撞情况。
上述方案中，料框坐标系和夹爪坐标系都可以在机器人坐标系下进行表示，但由于只考虑侧料框和夹爪的碰撞检测，故需要进行坐标变换，将夹爪坐标系变换到料框坐标系下进行考虑。
优选的，坐标系变换的算法如下：
已知夹爪坐标系O2在机器人坐标系O1下的齐次变换矩阵为
Figure 427386DEST_PATH_IMAGE001
，
料框坐标系O3在机器人坐标系O1下的齐次变换矩阵为
Figure 180578DEST_PATH_IMAGE002
，
则夹爪坐标系O2在料框坐标系O3下的齐次变换矩阵为
Figure 514476DEST_PATH_IMAGE003
；
将位置和姿态分开考虑，即：
Figure 73633DEST_PATH_IMAGE004
；
Figure 350156DEST_PATH_IMAGE005
；
其中，
Figure 602146DEST_PATH_IMAGE006
为
Figure 564286DEST_PATH_IMAGE007
的旋转矩阵，
Figure 599238DEST_PATH_IMAGE008
为
Figure 697644DEST_PATH_IMAGE009
的旋转矩阵，
Figure 164877DEST_PATH_IMAGE010
为
Figure 83154DEST_PATH_IMAGE011
的旋转矩阵，
Figure 780852DEST_PATH_IMAGE012
为
Figure 733765DEST_PATH_IMAGE007
的平移向量，
Figure 625759DEST_PATH_IMAGE013
为
Figure 562491DEST_PATH_IMAGE009
的平移向量，
Figure 673667DEST_PATH_IMAGE014
为
Figure 605720DEST_PATH_IMAGE011
的平移向量；
在实际生产过程中，可以控制料框的摆放位置和自定义料框坐标系，当建立的料框坐标系与机器人坐标系的xyz方向一致时，
Figure 871878DEST_PATH_IMAGE008
为单位矩阵，则
Figure 436852DEST_PATH_IMAGE015
；
Figure 476352DEST_PATH_IMAGE016
。
上述方案中，通过坐标系变换的算法将夹爪坐标系变换到料框坐标系下，实现碰撞检测问题的简化。
优选的，在步骤S3中，通过3D相机与标定好的手眼变换矩阵得到目标工件相对于机器人的位姿
Figure 935015DEST_PATH_IMAGE017
；其中，
Figure 277135DEST_PATH_IMAGE018
为工件姿态，
Figure 188459DEST_PATH_IMAGE019
为目标工件相对于机器人的平移向量。
上述方案中，3D相机安装在机器人上，手眼变换矩阵由工作人员根据实际情况进行标定。
优选的，在步骤S4中，具体包括以下步骤：
S4.1：设置工件姿态变换区间Rx[x1，x2]、Ry[y1，y2]、Rz[z1，z2]以及步长Step；
S4.2：对变换区间进行遍历，将工件姿态
Figure 798694DEST_PATH_IMAGE018
分别绕Rx、Ry、Rz旋转，得到变换姿态
Figure 721651DEST_PATH_IMAGE020
；
S4.3：根据变换姿态
Figure 952781DEST_PATH_IMAGE020
和平移向量
Figure 492347DEST_PATH_IMAGE019
得到变换后的位姿
Figure 873649DEST_PATH_IMAGE021
，将变换后的位姿
Figure 182271DEST_PATH_IMAGE022
作为抓取姿态带入到坐标系进行碰撞检测。
上述方案中，在无干涉的情况下机器人可以直接走TOB位姿去抓取工件，但由于存在料框的干涉，有些TOB位姿机器人并不能到达；而机器人能够抓取工件的姿态
Figure 492292DEST_PATH_IMAGE018
并不是唯一的，因此可以通过变换工件的姿态改变抓取姿态，并对抓取姿态进行碰撞检测，从而得到机器人的实际抓取姿态。
优选的，在步骤S4.2中，还包括通过对工件姿态
Figure 378208DEST_PATH_IMAGE018
进行约束，得到约束后的工件姿态
Figure 438568DEST_PATH_IMAGE023
，并通过计算得到变换姿态
Figure 460751DEST_PATH_IMAGE020
；约束方法如下：
针对绕X轴旋转对称的工件，首先将工件姿态
Figure 971367DEST_PATH_IMAGE018
转换为ZYX顺序的欧拉角，接着从欧拉角中提取绕X轴旋转的角度值θ，将θ乘以-1后再转换为旋转矩阵RXr，然后通过计算得到约束后的工件姿态
Figure 954366DEST_PATH_IMAGE023
：
Figure 444515DEST_PATH_IMAGE024
；
针对绕Y轴旋转对称的工件，首先将工件姿态
Figure 586784DEST_PATH_IMAGE018
转换为ZXY顺序的欧拉角，接着从欧拉角中提取绕Y轴旋转的角度值θ，将θ乘以-1后再转换为旋转矩阵RYr，然后通过计算得到约束后的工件姿态
Figure 612509DEST_PATH_IMAGE023
：
Figure 473017DEST_PATH_IMAGE025
；
再将
Figure 671917DEST_PATH_IMAGE023
代入
Figure 668692DEST_PATH_IMAGE026
，计算得到变换姿态
Figure 130898DEST_PATH_IMAGE020
。
上述方案中，针对绕X轴或绕Y轴旋转对称的工件，工件绕X轴或绕Y轴旋转实际不影响机器人的抓取，但是由于识别发生旋转导致抓取姿态也发生旋转，因此可以对识别后的姿态进行约束，将X或Y的旋转方向约束为0度。
优选的，在步骤S4.3中，碰撞检测的具体方法为：
设在抓取姿态下，夹爪的简化模型的底部端面的四个顶点中任意一个顶点在料框坐标系下的坐标为：
Figure 714588DEST_PATH_IMAGE027
，从该顶点发出的射线方向为：（nx，ny，nz）；其中，nx，ny，nz分别表示射线在料框坐标系的x，y，z轴下模长为1的方向分量；
由旋转矩阵的物理意义可知，射线方向为
Figure 451600DEST_PATH_IMAGE028
的最后一列的反方向，则该射线的参数方程为：
Figure 443827DEST_PATH_IMAGE029
，其中，t为射线的长度参数；
（a）当射线至少与料框侧面有一个交点时，则一条射线最多和四个侧面有两个交点，有两个侧面与射线无交点；
设料框的尺寸为L1*L2*h，L1为x方向，L2为y方向，则料框的四个侧面的平面方程为：
Figure 201567DEST_PATH_IMAGE030
；
计算射线与四个侧面的交点的z坐标：
Figure 36668DEST_PATH_IMAGE031
；
由于射线的参数方程中t>0，因此，
Figure 311791DEST_PATH_IMAGE032
，
Figure 158525DEST_PATH_IMAGE033
，
Figure 821587DEST_PATH_IMAGE034
和
Figure 645449DEST_PATH_IMAGE035
只有两个有效，取
Figure 927526DEST_PATH_IMAGE036
，
Figure 425503DEST_PATH_IMAGE037
，
Figure 790625DEST_PATH_IMAGE038
和
Figure 69160DEST_PATH_IMAGE039
中大于0的项所对应的z值与料框高度h进行比较；
若所有取得的z值均大于h，则说明夹爪与料框侧面无碰撞，该抓取姿态为无碰撞的抓取姿态；否则，说明夹爪与料框侧面有碰撞，该抓取姿态为有碰撞的抓取姿态；
（b）当射线与料框侧面没有交点时，即nx，ny均为零时，只需根据四个顶点的x坐标和y坐标进行判断：
当四个顶点的x坐标均在0到L1之间，且四个顶点的y坐标均在0到L2之间，此时夹爪与料框侧面无碰撞，该抓取姿态为无碰撞的抓取姿态；否则，夹爪与料框侧面有碰撞，该抓取姿态为有碰撞的抓取姿态。
上述方案中，通过齐次变换矩阵
Figure 420507DEST_PATH_IMAGE040
实现将夹爪的简化模型的底部端面的四个顶点的坐标从夹爪坐标系转换到料框坐标系下表示。以夹爪矩阵的最下端端面矩形的四个顶点向夹爪坐标系z轴的反方向作射线（假设四个顶点都在料框内部），比较四条射线与料框侧面的交点的z坐标（料框坐标系）与料框高度h之间的大小，若所有的交点z坐标均大于料框高度h，则无碰撞。
优选的，在步骤S5中，具体包括以下步骤：
S5.1：通过碰撞检测选取出所有无碰撞的抓取姿态；
S5.2：设定筛选条件，对所有无碰撞的抓取姿态进行筛选，得到最佳的抓取姿态，完成抓取规划。
上述方案中，姿态变换区间在一定程度上会约束机器人可能取到的抓取姿态，但有些情况下得到的抓取姿态虽然没有与料框碰撞，但是姿态过大，导致抓取姿态过于别扭，则需要通过工作人员根据具体应用情况设定筛选条件，从而剔除这类抓取姿态。一般情况下，机器人夹具向下抓取时，抓取姿态越接近垂直状态，机器人的抓取姿态变化越小；同时，机器人应优先抓取上层的和靠近料框中间的工件。
与现有技术相比，本发明技术方案的有益效果是：
本发明提供了一种机器人料框抓取规划与碰撞检测方法，通过对复杂的环境进行抽象及简化，建立简化模型，经过坐标系变换进行碰撞检测和抓取规划，实现减少繁琐的模型制作与导入，快速地给出抓取结果。
附图说明
图1为本发明的技术方案实施步骤流程图；
图2为本发明中步骤S2的机器人坐标系O1，夹爪坐标系O2和料框坐标系O3的示意图。
具体实施方式
附图仅用于示例性说明，不能理解为对本专利的限制；
为了更好说明本实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；
对于本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的。
下面结合附图和实施例对本发明的技术方案做进一步的说明。
实施例1
如图1所示，一种机器人料框抓取规划与碰撞检测方法，包括以下步骤：
S1：将实际模型简化，输入模型参数建立简化模型；
S2：根据简化模型建立坐标系，并进行坐标系变换；
S3：获取目标工件相对于机器人的位姿；
S4：将位姿进行变换后得到抓取姿态，并带入到坐标系进行碰撞检测；
S5：在碰撞检测的基础上做抓取规划。
在实施过程中，通过对复杂的环境进行抽象及简化，建立简化模型，经过坐标系变换进行碰撞检测和抓取规划，实现减少繁琐的模型制作与导入，快速地给出抓取结果。
更具体的，在步骤S1中，将实际模型简化包括：将机器人的抓取夹爪的两片夹爪简化为一个长方体，将料框简化为五个平面。
在实施过程中，抓取单元主要包括：一台工业串联机器人、一个末端抓取夹爪、一个料框以及被抓取物品。一般工业抓取夹爪为二指，两指平行且对齐，每片夹爪在机器人末端法向方向上尺寸较大，而厚度较小；料框为长方体结构，包含五个面，四个侧面和一个底面。因为实际情况为夹爪在机器人末端法向方向上尺寸会大于料框的高度，即机器人本体一般不会和料框发生碰撞，而抓取过程中也一般不考虑与料框底部的碰撞，因此在抓取碰撞检测问题中，只需考虑夹爪与料框侧面的碰撞情况。
更具体的，如图2所示，在步骤S2中，建立的坐标系包括机器人坐标系O1，夹爪坐标系O2和料框坐标系O3。
在实施过程中，机器人坐标系由机器人本体决定；料框坐标系以三条棱边建立坐标系；夹爪坐标系的圆心为夹爪向外最远处中心点，以机器人末端向外为z轴，x轴为夹爪闭合张开方向，y轴由右手定则决定。
更具体的，在步骤S2中，将夹爪坐标系O2变换到料框坐标系O3下考虑夹爪与料框侧面的碰撞情况。
在实施过程中，料框坐标系和夹爪坐标系都可以在机器人坐标系下进行表示，但由于只考虑侧料框和夹爪的碰撞检测，故需要进行坐标变换，将夹爪坐标系变换到料框坐标系下进行考虑。
更具体的，坐标系变换的算法如下：
已知夹爪坐标系O2在机器人坐标系O1下的齐次变换矩阵为
Figure 772991DEST_PATH_IMAGE001
，
料框坐标系O3在机器人坐标系O1下的齐次变换矩阵为
Figure 43435DEST_PATH_IMAGE002
，
则夹爪坐标系O2在料框坐标系O3下的齐次变换矩阵为
Figure 950211DEST_PATH_IMAGE003
；
将位置和姿态分开考虑，即：
Figure 188471DEST_PATH_IMAGE004
；
Figure 661040DEST_PATH_IMAGE005
；
其中，
Figure 102386DEST_PATH_IMAGE006
为
Figure 496458DEST_PATH_IMAGE007
的旋转矩阵，
Figure 48662DEST_PATH_IMAGE008
为
Figure 703634DEST_PATH_IMAGE009
的旋转矩阵，
Figure 987985DEST_PATH_IMAGE010
为
Figure 603774DEST_PATH_IMAGE011
的旋转矩阵，
Figure 726713DEST_PATH_IMAGE012
为
Figure 236192DEST_PATH_IMAGE007
的平移向量，
Figure 425865DEST_PATH_IMAGE013
为
Figure 528950DEST_PATH_IMAGE009
的平移向量，
Figure 688536DEST_PATH_IMAGE014
为
Figure 318101DEST_PATH_IMAGE011
的平移向量；
在实际生产过程中，可以控制料框的摆放位置和自定义料框坐标系，当建立的料框坐标系与机器人坐标系的xyz方向一致时，
Figure 678675DEST_PATH_IMAGE008
为单位矩阵，则
Figure 534635DEST_PATH_IMAGE015
；
Figure 733798DEST_PATH_IMAGE016
。
在实施过程中，通过坐标系变换的算法将夹爪坐标系变换到料框坐标系下，实现碰撞检测问题的简化。
更具体的，在步骤S3中，通过3D相机与标定好的手眼变换矩阵得到目标工件相对于机器人的位姿
Figure 93235DEST_PATH_IMAGE017
；其中，
Figure 890290DEST_PATH_IMAGE018
为工件姿态，
Figure 827022DEST_PATH_IMAGE019
为目标工件相对于机器人的平移向量。
在实施过程中，3D相机安装在机器人上，手眼变换矩阵由工作人员根据实际情况进行标定。
更具体的，在步骤S4中，具体包括以下步骤：
S4.1：设置工件姿态变换区间Rx[x1，x2]、Ry[y1，y2]、Rz[z1，z2]以及步长Step；
S4.2：对变换区间进行遍历，将工件姿态
Figure 593989DEST_PATH_IMAGE018
分别绕Rx、Ry、Rz旋转，得到变换姿态
Figure 807933DEST_PATH_IMAGE020
；
S4.3：根据变换姿态
Figure 775889DEST_PATH_IMAGE020
和平移向量
Figure 199917DEST_PATH_IMAGE019
得到变换后的位姿
Figure 6461DEST_PATH_IMAGE021
，将变换后的位姿
Figure 74912DEST_PATH_IMAGE022
作为抓取姿态带入到坐标系进行碰撞检测。
在实施过程中，在无干涉的情况下机器人可以直接走TOB位姿去抓取工件，但由于存在料框的干涉，有些TOB位姿机器人并不能到达；而机器人能够抓取工件的姿态
Figure 807244DEST_PATH_IMAGE018
并不是唯一的，因此可以通过变换工件的姿态改变抓取姿态，并对抓取姿态进行碰撞检测，从而得到机器人的实际抓取姿态。
更具体的，在步骤S4.2中，还包括通过对工件姿态
Figure 718568DEST_PATH_IMAGE018
进行约束，得到约束后的工件姿态
Figure 437126DEST_PATH_IMAGE023
，并通过计算得到变换姿态
Figure 750295DEST_PATH_IMAGE020
；约束方法如下：
针对绕X轴旋转对称的工件，首先将工件姿态
Figure 60054DEST_PATH_IMAGE018
转换为ZYX顺序的欧拉角，接着从欧拉角中提取绕X轴旋转的角度值θ，将θ乘以-1后再转换为旋转矩阵RXr，然后通过计算得到约束后的工件姿态
Figure 599620DEST_PATH_IMAGE023
：
Figure 482387DEST_PATH_IMAGE024
；
针对绕Y轴旋转对称的工件，首先将工件姿态
Figure 650063DEST_PATH_IMAGE018
转换为ZXY顺序的欧拉角，接着从欧拉角中提取绕Y轴旋转的角度值θ，将θ乘以-1后再转换为旋转矩阵RYr，然后通过计算得到约束后的工件姿态
Figure 599565DEST_PATH_IMAGE023
：
Figure 219902DEST_PATH_IMAGE025
；
再将
Figure 77000DEST_PATH_IMAGE023
代入
Figure 364762DEST_PATH_IMAGE026
，计算得到变换姿态
Figure 219585DEST_PATH_IMAGE020
。
在实施过程中，针对绕X轴或绕Y轴旋转对称的工件，工件绕X轴或绕Y轴旋转实际不影响机器人的抓取，但是由于识别发生旋转导致抓取姿态也发生旋转，因此可以对识别后的姿态进行约束，将X或Y的旋转方向约束为0度。
更具体的，在步骤S4.3中，碰撞检测的具体方法为：
设在抓取姿态下，夹爪的简化模型的底部端面的四个顶点中任意一个顶点在料框坐标系下的坐标为：
Figure 359842DEST_PATH_IMAGE027
，从该顶点发出的射线方向为：（nx，ny，nz）；其中，nx，ny，nz分别表示射线在料框坐标系的x，y，z轴下模长为1的方向分量；
由旋转矩阵的物理意义可知，射线方向为
Figure 755051DEST_PATH_IMAGE028
的最后一列的反方向，则该射线的参数方程为：
Figure 631740DEST_PATH_IMAGE029
，其中，t为射线的长度参数；
（a）当射线至少与料框侧面有一个交点时，则一条射线最多和四个侧面有两个交点，有两个侧面与射线无交点；
设料框的尺寸为L1*L2*h，L1为x方向，L2为y方向，则料框的四个侧面的平面方程为：
Figure 923044DEST_PATH_IMAGE030
；
计算射线与四个侧面的交点的z坐标：
Figure 783553DEST_PATH_IMAGE031
；
由于射线的参数方程中t>0，因此，
Figure 982453DEST_PATH_IMAGE032
，
Figure 713648DEST_PATH_IMAGE033
，
Figure 175854DEST_PATH_IMAGE034
和
Figure 290703DEST_PATH_IMAGE035
只有两个有效，取
Figure 496556DEST_PATH_IMAGE036
，
Figure 347837DEST_PATH_IMAGE037
，
Figure 246523DEST_PATH_IMAGE038
和
Figure 550466DEST_PATH_IMAGE039
中大于0的项所对应的z值与料框高度h进行比较；
若所有取得的z值均大于h，则说明夹爪与料框侧面无碰撞，该抓取姿态为无碰撞的抓取姿态；否则，说明夹爪与料框侧面有碰撞，该抓取姿态为有碰撞的抓取姿态；
（b）当射线与料框侧面没有交点时，即nx，ny均为零时，只需根据四个顶点的x坐标和y坐标进行判断：
当四个顶点的x坐标均在0到L1之间，且四个顶点的y坐标均在0到L2之间，此时夹爪与料框侧面无碰撞，该抓取姿态为无碰撞的抓取姿态；否则，夹爪与料框侧面有碰撞，该抓取姿态为有碰撞的抓取姿态。
在实施过程中，通过齐次变换矩阵
Figure 419065DEST_PATH_IMAGE040
实现将夹爪的简化模型的底部端面的四个顶点的坐标从夹爪坐标系转换到料框坐标系下表示。以夹爪矩阵的最下端端面矩形的四个顶点向夹爪坐标系z轴的反方向作射线（假设四个顶点都在料框内部），比较四条射线与料框侧面的交点的z坐标（料框坐标系）与料框高度h之间的大小，若所有的交点z坐标均大于料框高度h，则无碰撞。
更具体的，在步骤S5中，具体包括以下步骤：
S5.1：通过碰撞检测选取出所有无碰撞的抓取姿态；
S5.2：设定筛选条件，对所有无碰撞的抓取姿态进行筛选，得到最佳的抓取姿态，完成抓取规划。
上述方案中，姿态变换区间在一定程度上会约束机器人可能取到的抓取姿态，但有些情况下得到的抓取姿态虽然没有与料框碰撞，但是姿态过大，导致抓取姿态过于别扭，则需要通过工作人员根据具体应用情况设定筛选条件，从而剔除这类抓取姿态。一般情况下，机器人夹具向下抓取时，抓取姿态越接近垂直状态，机器人的抓取姿态变化越小；同时，机器人应优先抓取上层的和靠近料框中间的工件。
显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。

Claims (10)

1.一种机器人料框抓取规划与碰撞检测方法，其特征在于，包括以下步骤：
S1：将实际模型简化，输入模型参数建立简化模型；
S2：根据简化模型建立坐标系，并进行坐标系变换；
S3：获取目标工件相对于机器人的位姿；
S4：将位姿进行变换后得到抓取姿态，并带入到坐标系进行碰撞检测；
S5：在碰撞检测的基础上做抓取规划。
2.根据权利要求1所述的一种机器人料框抓取规划与碰撞检测方法，其特征在于，在步骤S1中，将实际模型简化包括：将机器人的抓取夹爪的两片夹爪简化为一个长方体，将料框简化为五个平面。
3.根据权利要求2所述的一种机器人料框抓取规划与碰撞检测方法，其特征在于，在步骤S2中，建立的坐标系包括机器人坐标系O1，夹爪坐标系O2和料框坐标系O3。
4.根据权利要求3所述的一种机器人料框抓取规划与碰撞检测方法，其特征在于，在步骤S2中，将夹爪坐标系O2变换到料框坐标系O3下考虑夹爪与料框侧面的碰撞情况。
5.根据权利要求4所述的一种机器人料框抓取规划与碰撞检测方法，其特征在于，坐标系变换的算法如下：
已知夹爪坐标系O2在机器人坐标系O1下的齐次变换矩阵为
Figure 966003DEST_PATH_IMAGE001
，
料框坐标系O3在机器人坐标系O1下的齐次变换矩阵为
Figure 96770DEST_PATH_IMAGE002
，
则夹爪坐标系O2在料框坐标系O3下的齐次变换矩阵为
Figure 156999DEST_PATH_IMAGE003
；
将位置和姿态分开考虑，即：
Figure 6006DEST_PATH_IMAGE004
；
Figure 521301DEST_PATH_IMAGE005
；
其中，
Figure 178678DEST_PATH_IMAGE006
为
Figure 144229DEST_PATH_IMAGE007
的旋转矩阵，
Figure 949374DEST_PATH_IMAGE008
为
Figure 799518DEST_PATH_IMAGE009
的旋转矩阵，
Figure 311402DEST_PATH_IMAGE010
为
Figure 792062DEST_PATH_IMAGE011
的旋转矩阵，
Figure 5875DEST_PATH_IMAGE012
为
Figure 394131DEST_PATH_IMAGE007
的平移向量，
Figure 353996DEST_PATH_IMAGE013
为
Figure 412082DEST_PATH_IMAGE009
的平移向量，
Figure 116121DEST_PATH_IMAGE014
为
Figure 511330DEST_PATH_IMAGE011
的平移向量；
在实际生产过程中，可以控制料框的摆放位置和自定义料框坐标系，当建立的料框坐标系与机器人坐标系的xyz方向一致时，
Figure 856861DEST_PATH_IMAGE008
为单位矩阵，则
Figure 351427DEST_PATH_IMAGE015
；
Figure 618460DEST_PATH_IMAGE016
。
6.根据权利要求5所述的一种机器人料框抓取规划与碰撞检测方法，其特征在于，在步骤S3中，通过3D相机与标定好的手眼变换矩阵得到目标工件相对于机器人的位姿
Figure 348519DEST_PATH_IMAGE017
；其中，
Figure 17398DEST_PATH_IMAGE018
为工件姿态，
Figure 932133DEST_PATH_IMAGE019
为目标工件相对于机器人的平移向量。
7.根据权利要求6所述的一种机器人料框抓取规划与碰撞检测方法，其特征在于，在步骤S4中，具体包括以下步骤：
S4.1：设置工件姿态变换区间Rx[x1，x2]、Ry[y1，y2]、Rz[z1，z2]以及步长Step；
S4.2：对变换区间进行遍历，将工件姿态
Figure 358566DEST_PATH_IMAGE018
分别绕Rx、Ry、Rz旋转，得到变换姿态
Figure 361157DEST_PATH_IMAGE020
；
S4.3：根据变换姿态
Figure 415701DEST_PATH_IMAGE020
和平移向量
Figure 501337DEST_PATH_IMAGE019
得到变换后的位姿
Figure 742963DEST_PATH_IMAGE021
，将变换后的位姿
Figure 690190DEST_PATH_IMAGE022
作为抓取姿态带入到坐标系进行碰撞检测。
8.根据权利要求7所述的一种机器人料框抓取规划与碰撞检测方法，其特征在于，在步骤S4.2中，还包括通过对工件姿态
Figure 864820DEST_PATH_IMAGE018
进行约束，得到约束后的工件姿态
Figure 465565DEST_PATH_IMAGE023
，并通过计算得到变换姿态
Figure 584700DEST_PATH_IMAGE020
；约束方法如下：
针对绕X轴旋转对称的工件，首先将工件姿态
Figure 601197DEST_PATH_IMAGE018
转换为ZYX顺序的欧拉角，接着从欧拉角中提取绕X轴旋转的角度值θ，将θ乘以-1后再转换为旋转矩阵RXr，然后通过计算得到约束后的工件姿态
Figure 364754DEST_PATH_IMAGE023
：
Figure 136401DEST_PATH_IMAGE024
；
针对绕Y轴旋转对称的工件，首先将工件姿态
Figure 28919DEST_PATH_IMAGE018
转换为ZXY顺序的欧拉角，接着从欧拉角中提取绕Y轴旋转的角度值θ，将θ乘以-1后再转换为旋转矩阵RYr，然后通过计算得到约束后的工件姿态
Figure 583528DEST_PATH_IMAGE023
：
Figure 936012DEST_PATH_IMAGE025
；
再将
Figure 409719DEST_PATH_IMAGE023
代入
Figure 847653DEST_PATH_IMAGE026
，计算得到变换姿态
Figure 455221DEST_PATH_IMAGE020
。
9.根据权利要求7所述的一种机器人料框抓取规划与碰撞检测方法，其特征在于，在步骤S4.3中，碰撞检测的具体方法为：
设在抓取姿态下，夹爪的简化模型的底部端面的四个顶点中任意一个顶点在料框坐标系下的坐标为：
Figure 193370DEST_PATH_IMAGE027
，从该顶点发出的射线方向为：（nx，ny，nz）；其中，nx，ny，nz分别表示射线在料框坐标系的x，y，z轴下模长为1的方向分量；
由旋转矩阵的物理意义可知，射线方向为
Figure 306819DEST_PATH_IMAGE028
的最后一列的反方向，则该射线的参数方程为：
Figure 904154DEST_PATH_IMAGE029
，其中，t为射线的长度参数；
（a）当射线至少与料框侧面有一个交点时，则一条射线最多和四个侧面有两个交点，有两个侧面与射线无交点；
设料框的尺寸为L1*L2*h，L1为x方向，L2为y方向，则料框的四个侧面的平面方程为：
Figure 784254DEST_PATH_IMAGE030
；
计算射线与四个侧面的交点的z坐标：
Figure 111330DEST_PATH_IMAGE031
；
由于射线的参数方程中t>0，因此，
Figure 926839DEST_PATH_IMAGE032
，
Figure 11470DEST_PATH_IMAGE033
，
Figure 695261DEST_PATH_IMAGE034
和
Figure 673582DEST_PATH_IMAGE035
只有两个有效，取
Figure 535358DEST_PATH_IMAGE036
，
Figure 169602DEST_PATH_IMAGE037
，
Figure 660014DEST_PATH_IMAGE038
和
Figure 492840DEST_PATH_IMAGE039
中大于0的项所对应的z值与料框高度h进行比较；
若所有取得的z值均大于h，则说明夹爪与料框侧面无碰撞，该抓取姿态为无碰撞的抓取姿态；否则，说明夹爪与料框侧面有碰撞，该抓取姿态为有碰撞的抓取姿态；
（b）当射线与料框侧面没有交点时，即nx，ny均为零时，只需根据四个顶点的x坐标和y坐标进行判断：
当四个顶点的x坐标均在0到L1之间，且四个顶点的y坐标均在0到L2之间，此时夹爪与料框侧面无碰撞，该抓取姿态为无碰撞的抓取姿态；否则，夹爪与料框侧面有碰撞，该抓取姿态为有碰撞的抓取姿态。
10.根据权利要求9所述的一种机器人料框抓取规划与碰撞检测方法，其特征在于，在步骤S5中，具体包括以下步骤：
S5.1：通过碰撞检测选取出所有无碰撞的抓取姿态；
S5.2：设定筛选条件，对所有无碰撞的抓取姿态进行筛选，得到最佳的抓取姿态，完成抓取规划。
CN202010463181.7A 2020-05-27 2020-05-27 一种机器人料框抓取规划与碰撞检测方法 Active CN111558940B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010463181.7A CN111558940B (zh) 2020-05-27 2020-05-27 一种机器人料框抓取规划与碰撞检测方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010463181.7A CN111558940B (zh) 2020-05-27 2020-05-27 一种机器人料框抓取规划与碰撞检测方法

Publications (2)

Publication Number Publication Date
CN111558940A true CN111558940A (zh) 2020-08-21
CN111558940B CN111558940B (zh) 2020-10-23

Family

ID=72073986

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010463181.7A Active CN111558940B (zh) 2020-05-27 2020-05-27 一种机器人料框抓取规划与碰撞检测方法

Country Status (1)

Country Link
CN (1) CN111558940B (zh)

Cited By (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112060087A (zh) * 2020-08-28 2020-12-11 佛山隆深机器人有限公司 一种用于机器人抓取场景的点云碰撞检测方法
CN112464410A (zh) * 2020-12-02 2021-03-09 熵智科技(深圳)有限公司 一种工件抓取顺序的确定方法、装置、计算机设备及介质
CN112936257A (zh) * 2021-01-22 2021-06-11 熵智科技(深圳)有限公司 一种工件抓取方法、装置、计算机设备及存储介质
CN113483664A (zh) * 2021-07-20 2021-10-08 科派股份有限公司 一种基于线结构光视觉的屏风板自动上料系统及方法
CN113538582A (zh) * 2021-07-20 2021-10-22 熵智科技(深圳)有限公司 一种工件抓取顺序的确定方法、装置、计算机设备及介质
CN113524187A (zh) * 2021-07-20 2021-10-22 熵智科技(深圳)有限公司 一种工件抓取顺序的确定方法、装置、计算机设备及介质
CN113538459A (zh) * 2021-07-07 2021-10-22 重庆大学 一种基于落点区域检测的多模式抓取避障检测优化方法
CN113800270A (zh) * 2021-09-17 2021-12-17 知行高科（北京）科技有限公司 一种用于物流拆垛的机器人控制方法及系统
CN113910235A (zh) * 2021-10-29 2022-01-11 珠海格力智能装备有限公司 机器人抓取物料的碰撞检测方法、装置、设备和存储介质
CN114619447A (zh) * 2022-03-16 2022-06-14 梅卡曼德（北京）机器人科技有限公司 抓取方法、抓取装置及机器人
CN114700940A (zh) * 2022-03-24 2022-07-05 梅卡曼德（北京）机器人科技有限公司 抓取方法、装置及计算设备
CN115056215A (zh) * 2022-05-20 2022-09-16 梅卡曼德（北京）机器人科技有限公司 碰撞检测方法、控制方法、抓取系统及计算机存储介质
CN113538582B (zh) * 2021-07-20 2024-06-07 熵智科技(深圳)有限公司 一种工件抓取顺序的确定方法、装置、计算机设备及介质

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107116540A (zh) * 2016-02-24 2017-09-01 中国科学院沈阳计算技术研究所有限公司 一种基于scs包围结构的机器人碰撞检测方法
WO2017182567A1 (de) * 2016-04-22 2017-10-26 Kuka Roboter Gmbh Endeffektor-vorrichtung
CN107953334A (zh) * 2017-12-25 2018-04-24 深圳禾思众成科技有限公司 一种基于a星算法的工业机械臂无碰撞路径规划方法
CN110370268A (zh) * 2018-09-11 2019-10-25 天津京东深拓机器人科技有限公司 箱内拣选的方法、装置和系统
CN110605711A (zh) * 2018-06-14 2019-12-24 中瑞福宁机器人（沈阳）有限公司 一种控制协作机器人抓取物体的方法、装置及系统
CN111099343A (zh) * 2020-01-21 2020-05-05 苏州优得精智装备有限公司 一种能有效排除料框干涉的视觉上料机构

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107116540A (zh) * 2016-02-24 2017-09-01 中国科学院沈阳计算技术研究所有限公司 一种基于scs包围结构的机器人碰撞检测方法
WO2017182567A1 (de) * 2016-04-22 2017-10-26 Kuka Roboter Gmbh Endeffektor-vorrichtung
CN107953334A (zh) * 2017-12-25 2018-04-24 深圳禾思众成科技有限公司 一种基于a星算法的工业机械臂无碰撞路径规划方法
CN110605711A (zh) * 2018-06-14 2019-12-24 中瑞福宁机器人（沈阳）有限公司 一种控制协作机器人抓取物体的方法、装置及系统
CN110370268A (zh) * 2018-09-11 2019-10-25 天津京东深拓机器人科技有限公司 箱内拣选的方法、装置和系统
CN111099343A (zh) * 2020-01-21 2020-05-05 苏州优得精智装备有限公司 一种能有效排除料框干涉的视觉上料机构

Cited By (19)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112060087A (zh) * 2020-08-28 2020-12-11 佛山隆深机器人有限公司 一种用于机器人抓取场景的点云碰撞检测方法
CN112464410A (zh) * 2020-12-02 2021-03-09 熵智科技(深圳)有限公司 一种工件抓取顺序的确定方法、装置、计算机设备及介质
CN112464410B (zh) * 2020-12-02 2021-11-16 熵智科技(深圳)有限公司 一种工件抓取顺序的确定方法、装置、计算机设备及介质
CN112936257A (zh) * 2021-01-22 2021-06-11 熵智科技(深圳)有限公司 一种工件抓取方法、装置、计算机设备及存储介质
CN113538459A (zh) * 2021-07-07 2021-10-22 重庆大学 一种基于落点区域检测的多模式抓取避障检测优化方法
CN113538459B (zh) * 2021-07-07 2023-08-11 重庆大学 一种基于落点区域检测的多模式抓取避障检测优化方法
CN113483664B (zh) * 2021-07-20 2022-10-21 科派股份有限公司 一种基于线结构光视觉的屏风板自动上料系统及方法
CN113483664A (zh) * 2021-07-20 2021-10-08 科派股份有限公司 一种基于线结构光视觉的屏风板自动上料系统及方法
CN113538582A (zh) * 2021-07-20 2021-10-22 熵智科技(深圳)有限公司 一种工件抓取顺序的确定方法、装置、计算机设备及介质
CN113524187A (zh) * 2021-07-20 2021-10-22 熵智科技(深圳)有限公司 一种工件抓取顺序的确定方法、装置、计算机设备及介质
CN113538582B (zh) * 2021-07-20 2024-06-07 熵智科技(深圳)有限公司 一种工件抓取顺序的确定方法、装置、计算机设备及介质
CN113524187B (zh) * 2021-07-20 2022-12-13 熵智科技(深圳)有限公司 一种工件抓取顺序的确定方法、装置、计算机设备及介质
CN113800270A (zh) * 2021-09-17 2021-12-17 知行高科（北京）科技有限公司 一种用于物流拆垛的机器人控制方法及系统
CN113910235A (zh) * 2021-10-29 2022-01-11 珠海格力智能装备有限公司 机器人抓取物料的碰撞检测方法、装置、设备和存储介质
CN114619447A (zh) * 2022-03-16 2022-06-14 梅卡曼德（北京）机器人科技有限公司 抓取方法、抓取装置及机器人
CN114619447B (zh) * 2022-03-16 2023-12-22 梅卡曼德（北京）机器人科技有限公司 抓取方法、抓取装置及机器人
CN114700940A (zh) * 2022-03-24 2022-07-05 梅卡曼德（北京）机器人科技有限公司 抓取方法、装置及计算设备
CN114700940B (zh) * 2022-03-24 2023-08-25 梅卡曼德（北京）机器人科技有限公司 抓取方法、装置及计算设备
CN115056215A (zh) * 2022-05-20 2022-09-16 梅卡曼德（北京）机器人科技有限公司 碰撞检测方法、控制方法、抓取系统及计算机存储介质

Also Published As

Publication number Publication date
CN111558940B (zh) 2020-10-23

Similar Documents

Publication Publication Date Title
CN111558940B (zh) 一种机器人料框抓取规划与碰撞检测方法
CN112060087B (zh) 一种用于机器人抓取场景的点云碰撞检测方法
Chang et al. Collision avoidance of two general robot manipulators by minimum delay time
US9089971B2 (en) Information processing apparatus, control method thereof and storage medium
CN107571260B (zh) 控制机器人抓取物体的方法和装置
WO2018137431A1 (zh) 一种机器人自动寻找折弯位置的方法
CN110378325B (zh) 一种机器人抓取过程中的目标位姿识别方法
CN111496849B (zh) 一种料框与夹具快速碰撞检测的方法
CN106997201A (zh) 多机器人协作的路径规划方法
CN113232021A (zh) 一种机械臂抓取路径碰撞检测方法
CN113379849A (zh) 基于深度相机的机器人自主识别智能抓取方法及系统
CN113269723A (zh) 三维视觉定位与机械手协同工作的零部件无序抓取系统
CN112192577A (zh) 一种应用于机器人抓取场景的一拍多抓方法
Krishnan et al. Image space trajectory tracking of 6-DOF robot manipulator in assisting visual servoing
CN113483664B (zh) 一种基于线结构光视觉的屏风板自动上料系统及方法
Ranjan et al. Identification and control of NAO humanoid robot to grasp an object using monocular vision
Tongloy et al. An image-based visual servo control system based on an eye-in-hand monocular camera for autonomous robotic grasping
Funakubo et al. Recognition and handling of clothes with different pattern by dual hand-eyes robotic system
Vogt et al. Automatic end tool alignment through plane detection with a RANSAC-algorithm for robotic grasping
Cheon et al. Object manipulation using robot arm-hand system
CN115556102B (zh) 一种基于视觉识别的机器人分拣规划方法及规划设备
Xie Research on the industrial robot grasping method based on multisensor data fusion and binocular vision
CN116197888B (zh) 物品位置确定方法、装置、电子设备和存储介质
Erdős et al. Visual servo guided cyber-physical robotic assembly cell
Tao et al. An improved RRT algorithm for the motion planning of robot manipulator picking up scattered piston

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
GR01 Patent grant
GR01 Patent grant