CN111360824B - 一种双臂自碰撞检测方法和计算机可读存储介质 - Google Patents

一种双臂自碰撞检测方法和计算机可读存储介质 Download PDF

Info

Publication number
CN111360824B
CN111360824B CN202010123225.1A CN202010123225A CN111360824B CN 111360824 B CN111360824 B CN 111360824B CN 202010123225 A CN202010123225 A CN 202010123225A CN 111360824 B CN111360824 B CN 111360824B
Authority
CN
China
Prior art keywords
arm
collision detection
collision
joint
left arm
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.)
Active
Application number
CN202010123225.1A
Other languages
English (en)
Other versions
CN111360824A (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.)
Siasun Co Ltd
Original Assignee
Siasun 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 Siasun Co Ltd filed Critical Siasun Co Ltd
Priority to CN202010123225.1A priority Critical patent/CN111360824B/zh
Publication of CN111360824A publication Critical patent/CN111360824A/zh
Application granted granted Critical
Publication of CN111360824B publication Critical patent/CN111360824B/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
    • 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
    • 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/1679—Programme controls characterised by the tasks executed

Landscapes

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

Abstract

本发明提供了一种双臂自碰撞检测方法和计算机可读存储介质。本发明利用包围盒做粗碰撞检测再利用网格做精碰撞检测,对双臂协作机器人，给定任务级运动规划，实时检测双臂在运动规划过程中左臂与左臂、右臂、本体或者右臂与右臂、本体之间碰撞情况，机器人控制器根据碰撞反馈信号及时做出响应。本发明具有碰撞检测更加优化更加准确的有益技术效果。

Description

一种双臂自碰撞检测方法和计算机可读存储介质
技术领域
本发明涉及机器人领域，具体涉及一种双臂自碰撞检测方法和计算机可读存储介质。
背景技术
当前对于自碰撞检测的方式主要有两种：一是基于力矩传感器检测。二是基于几何模型的碰撞检测，用基本几何体包围实际模型的碰撞检测。然而，现有的自碰撞检测的方式存在以下不足：1)力矩传感器的检测方法，成本过高，且在机器人设计阶段就需要考虑力矩传感器的安装问题，也就是说，该方法并不一定适用于所有现有的机器人；再者，单纯的力矩检测在机器人调试过程中，会出现很多误判的情况；其三，基于力矩传感器的检测，检出时碰撞已经发生，无法起到预防作用。2)几何模型碰撞检测方法通过基本几何体包络实际模型，不足之处在于其碰撞精度取决于包络后的模型与实际模型的紧密程度。本方法是几何模型碰撞检测方法的一种扩展，更通用的方式。在机器人领域，特别是双臂协作机器人，需要保证双臂在给定的任务规划过程中左臂与左臂、右臂、本体或者右臂与右臂、本体之间不发生碰撞的约束情况，此时就需要一种可以在线且实时的碰撞检测算法来确保机器人运动过程中的安全。一般来说，双臂运动规划过程中的碰撞检测问题，都是基于包围盒思想，即将机械臂(左臂、右臂及基座)用基本几何体(如长方体、圆柱等)进行包络，构建碰撞模型，这样机械臂自碰撞问题(左臂-右臂、左臂-基座、右臂-基座)就转化为基本几何体之间的碰撞，最后通过空间解析几何知识进行求解。当执行双臂任务规划时，考虑不同应用要求，基于包围盒的方法，会因扩大实际模型的体积而缩小左、右臂协同区域的工作空间，就可能触发碰撞检测而无法完成任务。例如双臂协同完成某个工件轴孔装配或组装等，此时就需要确定当前包络模型发生碰撞前提下，实际模型是否触发了碰撞检测，如果是，则减速或者停止，并重新规划任务；否则，继续执行任务并继续对实际模型进行碰撞检测。
发明内容
本发明的目的在于提供一种碰撞检测更加优化更加准确的双臂自碰撞检测方法和计算机可读存储介质。为实现本发明的目的，本发明的技术方案如下。
一种双臂自碰撞检测方法,包括：
构建碰撞模型；
更新空间位置点信息；
执行碰撞检测；
其中，所述碰撞模型包括：外层包围盒和内层实际模型；所述碰撞检测包括外层包围盒碰撞检测和内层实际模型碰撞检测；
优选的，外层包围盒碰撞检测优先于内层实际模型碰撞检测，并且仅在外层包围盒碰撞时才执行内层实际模型碰撞检测。
优选的，所述构建外层包围盒包括：
设左臂的关节数目记为m，右臂的关节数目记为n，上标表示左臂，上标r表示右臂，
Figure BDA0002393638270000031
表示左臂第j关节中心点；
设左臂、右臂的基坐标系分别为：
Figure BDA0002393638270000032
设左臂、右臂各关节初始关节角分别为：
Figure BDA0002393638270000033
已知双臂模型尺寸数据：左臂关节连杆
Figure BDA0002393638270000034
三维尺寸
Figure BDA0002393638270000035
半径
Figure BDA0002393638270000036
三角面网格数据
Figure BDA0002393638270000037
右臂关节连杆
Figure BDA0002393638270000038
三维尺寸
Figure BDA0002393638270000039
半径
Figure BDA00023936382700000310
三角面网格数据
Figure BDA00023936382700000311
本体：三维尺寸So、半径Ro、三角面网格数据Do；
设包围盒八个顶点的坐标分别为：(x1,y1,z1)，(x2,y2,z2)，…，(x8,y8,z8)，则根据空间向量法获得膨胀后的包围盒；其中，根据空间向量法获得膨胀后的包围盒包括：设V0为膨胀前的顶点，V0沿x方向单位向量nx为：
Figure BDA00023936382700000312
其中，Kx＝[x1 y1 z1]-[x2 y2 z2]；
V0沿y方向单位向量ny为：
Figure BDA00023936382700000313
Ky＝[x1 y1 z1]-[x5 y5 z5]；
V0顶点沿z方向单位向量nz为：
Figure BDA0002393638270000041
Kz＝[x1 y1 z1]-[x4 y4 z4]；
膨胀后的顶点：
Figure BDA0002393638270000042
Figure BDA0002393638270000043
4.根据权利要求3所述的双臂自碰撞检测方法，其特征在于，更新模型空间位置点信息包括：
采用下式计算臂本体坐标系与双臂基坐标系位置关系的齐次矩阵：
Figure BDA0002393638270000044
其中，Pos2Mat()为将笛卡尔空间位姿转化为对应齐次矩阵的函数；
Figure BDA0002393638270000045
为左臂本体坐标系到双臂基坐标系的位姿，
Figure BDA0002393638270000046
为经过Pos2Mat()函数计算后得到的左臂本体坐标系与双臂基坐标系的齐次变换矩阵；
Figure BDA0002393638270000047
为右臂本体坐标系到双臂基坐标系的位姿，
Figure BDA0002393638270000048
为经过Pos2Mat()函数计算后得到的右臂本体坐标系与双臂基坐标系的齐次变换矩阵；
采用下式计算各关节在臂坐标系下位置关系的齐次矩阵：
Figure BDA0002393638270000049
其中，fk()为由初始关节角求得各关节处的齐次坐标变换矩阵的函数，k为关节号，k∈[1,m+1]，k’∈[1,n+1]；Ql、Qr分别为左臂、右臂各关节初始关节角；
Figure BDA00023936382700000410
分别为臂、右臂各关节中心点相对于臂本体的位姿关系；
采用下式，利用已求得的臂与双臂基坐标系齐次矩阵、关节与臂坐标系齐次矩阵，计算关节与双臂基坐标系齐次矩阵：
Figure BDA0002393638270000051
其中，
Figure BDA0002393638270000052
分别为左臂、右臂各关节中心点的相对于双臂基坐标系的齐次坐标变换矩阵；
采用下式实时更新左臂、右臂包围盒空间位置；
Figure BDA0002393638270000053
Figure BDA0002393638270000054
其中，
Figure BDA0002393638270000055
分别表示左臂第j个、右臂第i个关节包围盒顶点集；
Figure BDA0002393638270000056
分别表示左臂第j个、右臂第i个关节包围盒更新后的顶点集。
优选的，执行碰撞检测包括：
生成碰撞检测矩阵；
遍历碰撞检测矩阵；
判断是否发生碰撞。
优选的，碰撞检测矩阵为
Figure BDA0002393638270000057
其中，
Figure BDA0002393638270000058
表示右臂节点
Figure BDA0002393638270000059
与右臂节点
Figure BDA00023936382700000510
之间所连接的杆件，
Figure BDA00023936382700000511
表示左臂节点
Figure BDA00023936382700000512
与左臂节点
Figure BDA00023936382700000513
之间所连接的杆件，
Figure BDA00023936382700000514
的值为0或1，0代表不可能发生碰撞，1代表可能发生碰撞。
优选的，遍历碰撞检测矩阵包括：
采用AABB层次包围盒结构，在碰撞检测过程中，首先进行父节点的碰撞检测，
如果父节点不相交，即不发生碰撞，此时无需对子节点做检测，以减小计算资源消耗；
如果父节点相交，即发生碰撞，此时需切换为对子节点做检测，即实际模型的精碰撞检测，此时不再对父节点做碰撞检测；
若检测到发生碰撞，则触发碰撞信号，双臂减速或停止运行；
若检测到未发生碰撞，则忽略，双臂继续工作，但限制允许的最大速度，子节点继续做检测；
若子节点检测中发现实际模型最小距离值呈放大趋势且大于父节点碰撞阈值，则切换为对父节点检测，并且不再限制最大速度。
优选的，父节点的碰撞检测包括：
设两对称几何体A,B，其中心点分别为CA,CB,投影区间的半径分别为rA，rB,
几何体A的中心与几何体B的中心之间的方向向量
Figure BDA0002393638270000061
为：
Figure BDA0002393638270000062
将方向向量
Figure BDA0002393638270000063
与分离轴L的向量相乘得到两个中心点CACB连线在分离轴L上的投影长度d：
Figure BDA0002393638270000064
其中，L为常数；
如果rA+rB<d,则A与B分离，即不发生碰撞；反之A与B相交，即碰撞。
优选的，子节点碰撞检测包括：
构建左臂、右臂碰撞几何模型：
左臂：
Figure BDA0002393638270000071
其中右臂：
Figure BDA0002393638270000072
更新左臂、右臂碰撞几何模型：
左臂：
Figure BDA0002393638270000073
右臂：
Figure BDA0002393638270000074
进行碰撞检测：
Figure BDA0002393638270000075
若result为true，则左臂第j个杆件、右臂第i个杆件发生碰撞；反之，则左臂第j个杆件、右臂第i个杆件不发生碰撞；
其中,addSubModel()、CollisionObjectf()、collide()为开源碰撞检测库的接口函数，left_vertices、right_vertices为目为标杆件顶点，left_triangles，right_vertices为三角面的空间坐标。
一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现上述方法。
相对于现有技术，本发明的有益技术效果在于：本发明利用包围盒做粗碰撞检测再利用网格做精碰撞检测；对某应用场景下的双臂协作机器人，给定任务级运动规划，需实时检测双臂在运动规划过程中，左臂与右臂、左臂或者右臂与本体之间碰撞情况，机器人控制器根据碰撞反馈信号及时做出响应，如重新规划或者减速、停止运行等措施，确保机器人安全完成任务。本发明具有碰撞检测更加优化更加准确的有益技术效果。
为使本申请的上述目的、特征和优点能更明显易懂，下文特举较佳实施例，并配合所附附图，作详细说明如下。
附图说明
为了更清楚地说明本申请实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本申请的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。
图1为碰撞检测流程示意图。
图2为双臂碰撞模型示意图；
图3为碰撞判断示意图；
具体实施方式
为使本申请实施例的目的、技术方案和优点更加清楚，下面将结合本申请实施例中附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。通常在此处附图中描述和示出的本申请实施例的组件可以以各种不同的配置来布置和设计。因此，以下对在附图中提供的本申请的实施例的详细描述并非旨在限制要求保护的本申请的范围，而是仅仅表示本申请的选定实施例。基于本申请的实施例，本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本申请保护的范围。
实施例一
如图1所示，本实施例提供了一种双臂自碰撞检测方法，该方法利用包围盒做粗碰撞检测再利用网格做精碰撞检测；对某应用场景下的双臂协作机器人，给定任务级运动规划，需实时检测双臂在运动规划过程中，左臂与右臂、左臂或者右臂与本体之间碰撞情况，机器人控制器根据碰撞反馈信号及时做出响应，如重新规划或者减速、停止运行等措施，确保机器人安全完成任务。
本实例的碰撞模型主要包括两层：外层是基于包围盒的思想，用基本几何模型包络实际模型，内层是实际模型，由CAD模型文件通过优化处理后，由三角形网格构成。
本实例的双臂连杆模型如图2所示，其中，左、右臂均为串联链式结构，分别安装于本体左、右两侧；左臂的关节数目记为m,右臂的关节数目记为n，上标l表示左臂，上标r表示右臂，
Figure BDA0002393638270000091
表示左臂第j关节中心点。
碰撞模型构建过程如下：
已知条件：
1)由CAD模型，易知双臂模型尺寸数据,包括：
左臂关节-连杆
Figure BDA0002393638270000101
三维尺寸
Figure BDA0002393638270000102
(长、宽、高)、半径
Figure BDA0002393638270000103
三角面网格数据
Figure BDA0002393638270000104
右臂关节-连杆
Figure BDA0002393638270000105
三维尺寸
Figure BDA0002393638270000106
(长、宽、高)、半径
Figure BDA0002393638270000107
三角面网格数据
Figure BDA0002393638270000108
本体：三维尺寸So(长、宽、高)、半径Ro、三角面网格数据Do；
2)左臂、右臂在本体安装处的基坐标系分别设为：
Figure BDA0002393638270000109
3)左、右臂各关节初始关节角：
Figure BDA00023936382700001010
构建过程：
由双臂模型尺寸，用基本几何体包络实际模型，本发明左、右臂采用的碰撞模型是方向包围盒(OBB)，本体一般采用轴向包围盒(AABB)包络。
左、右臂构建过程一样，具体地，以左臂包围盒构建为例说明：
从基座，杆件
Figure BDA00023936382700001011
依次构建,给定中心点坐标，默认(0,0,0)以及x,y,z方向轴向半径rx、ry和rz(左臂三维尺寸
Figure BDA00023936382700001012
的一半)，膨胀半径
Figure BDA00023936382700001013
结合几何知识很容易得到左臂各关节-连杆及本体包围盒及其各个顶点坐标。
左臂某关节-连杆任意膨胀半径
Figure BDA00023936382700001014
(碰撞精度)参数的OBB包围盒构建，已知OBB包围盒八个顶点的坐标，不妨设为(x1,y1,z1),(x2,y2,z2),…(x8,y8,z8)，则根据空间向量法获得膨胀后的包围盒；以下是某一个顶点Vi(i＝0,…7)计算方法，V0沿x方向单位向量nx为
Kx＝[x1 y1 z1]-[x2 y2 z2]，
Figure BDA0002393638270000111
V0沿y方向单位向量ny为
Ky＝[x1 y1 z1]-[x5 y5 z5]，
Figure BDA0002393638270000112
V0顶点沿z方向单位向量nz为
Kz＝[x1 y1 z1]-[x4 y4 z4]，
Figure BDA0002393638270000113
膨胀后的顶点：
Figure BDA0002393638270000114
Figure BDA0002393638270000115
其它顶点计算，同理可求。
最后构建的碰撞模型层次结构，
外层：基于几何模型构建的包围盒；
内层：由三角网格数据构成的实际模型；
1)参数计算。由左、右臂各关节当前关节角，结合机械臂正运动学求解，实时获得左臂、右臂各关节中心点的相对于双臂基坐标系的齐次坐标变换矩阵
Figure BDA0002393638270000116
Figure BDA0002393638270000121
Figure BDA0002393638270000122
Figure BDA0002393638270000123
其中，Pos2Mat()功能是将笛卡尔空间位姿转化为对应齐次矩阵，fk()是机械臂正运动学，由初始关节角求得各关节处的齐次坐标变换矩阵，
k∈[1,m+1],k’∈[1,n+1]；
包围盒更新。双臂运动过程中，其左、右臂各包围盒空间位置随左、右臂各关节运动实时更新，为了减小计算消耗及碰撞检测实时性，需实时更新外层包围盒空间位置，只有当碰撞模型的外层相互间发生碰撞，此时才更新实际模型。
左臂，右臂更新过程一样，本体无需更新：
Figure BDA0002393638270000124
其中，
Figure BDA0002393638270000125
分别表示左臂第j个、右臂第i个关节-连杆包围盒顶点集(或三角形网格数据)；
Figure BDA0002393638270000126
分别表示左臂第j个、右臂第i个关节-连杆包围盒更新后的顶点集(或三角形网格数据)；
1)左-右臂间的碰撞检测采用的是基于“分离轴定理”算法，给定分离轴L，假设两对称几何体A,B，给定某一分离轴L，其中心点分别为CA,CB,投影区间的半径分别为rA，rB,两中心点CACB连线在L上的投影长度d；
Figure BDA0002393638270000127
Figure BDA0002393638270000131
如果rA+rB<d,,则A与B分离，即不发生碰撞；反之A与B相交，即碰撞。
如图3所示，针对某一分离轴L,两OBB的投影半径和小于投影中心间的距离，则两者分离
遍历机制
针对碰撞模型，自动或手动生成碰撞检测矩阵，即剔除双臂运动过程中无需检测的连杆对，双臂模型的碰撞检测矩阵如下。
Figure BDA0002393638270000132
其中，
Figure BDA0002393638270000133
表示右臂节点
Figure BDA0002393638270000134
与右臂节点
Figure BDA0002393638270000135
之间所连接的杆件，
Figure BDA0002393638270000136
表示左臂节点
Figure BDA0002393638270000137
与左臂节点
Figure BDA0002393638270000138
之间所连接的杆件，
Figure BDA0002393638270000139
的值为0或1，0代表不可能发生碰撞，1代表可能发生碰撞。
示例性的，双臂碰撞检测矩阵可如表1所示，
表1
Figure BDA00023936382700001310
其中：0表示此对模型无需检测，1表示此对模型需要检测。
设计平衡二叉树的层次包围盒树形结构，存储碰撞模型数据，降低检测的时间复杂度。
采用此结构，在碰撞检测过程中，首先进行粗碰撞检测，即父节点的碰撞检测，对应于碰撞模型的外层包围盒的碰撞检测，
1)如果父节点不相交，即不发生碰撞，此时无需对子节点做检测，以减小计算资源消耗；
2)如果父节点相交，即发生碰撞，此时需切换为对子节点做检测，即实际模型的精碰撞检测，此时不再对父节点做碰撞检测，以减小计算资源消耗。
i)若检测到发生碰撞，则触发碰撞信号，双臂减速或停止运行；
ii)若检测到未发生碰撞，则忽略，双臂继续工作，但限制允许的最大速度，子节点继续做检测。
iii)若子节点检测中发现实际模型最小距离值呈放大趋势且大于父节点碰撞阈值，则切换为对父节点检测，并且不再限制最大速度。
子节点碰撞检测包括利用开源碰撞检测库接口函数对实际模型做精碰撞检测，具体如下：
精碰撞几何模型构建：
根据双臂实际模型尺寸，这里只需三角面网格数据(左臂：
Figure BDA0002393638270000141
右臂：
Figure BDA0002393638270000142
)，便于描述构建过程，这里以左臂第j个杆件、右臂第i个杆件为例进行说明。
首先，从左臂：
Figure BDA0002393638270000143
和右臂：
Figure BDA0002393638270000144
的实际模型网格存储数据中分别提取目标杆件所有顶点left_vertices,right_vertices、三角面left_triangles,right_vertices(三个顶点构成)的空间坐标值；
其次,利用开源碰撞检测库(fcl)的接口函数，构建碰撞几何模型；
左臂：
Figure BDA0002393638270000151
右臂：
Figure BDA0002393638270000152
模型更新：
左臂：
Figure BDA0002393638270000153
右臂：
Figure BDA0002393638270000154
碰撞检测：
Figure BDA0002393638270000155
若result为true,则左臂第j个杆件、右臂第i个杆件发生碰撞；反之，则左臂第j个杆件、右臂第i个杆件不发生碰撞；
其中addSubModel()、CollisionObjectf()、collide()为fcl库接口函数。
实施例二
本实施例提供了一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现实施例一所述方法。
本申请实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，又例如，多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些通信接口，装置或单元的间接耦合或通信连接，可以是电性，机械或其它的形式。
所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。
另外，在本申请提供的实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。
所述功能如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本申请各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-OnlyMemory)、随机存取存储器(RAM，RandomAccessMemory)、磁碟或者光盘等各种可以存储程序代码的介质。
应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步定义和解释，此外，术语“第一”、“第二”、“第三”等仅用于区分描述，而不能理解为指示或暗示相对重要性。
最后应说明的是：以上所述实施例，仅为本申请的具体实施方式，用以说明本申请的技术方案，而非对其限制，本申请的保护范围并不局限于此，尽管参照前述实施例对本申请进行了详细的说明，本领域的普通技术人员应当理解：任何熟悉本技术领域的技术人员在本申请揭露的技术范围内，其依然可以对前述实施例所记载的技术方案进行修改或可轻易想到变化，或者对其中部分技术特征进行等同替换；而这些修改、变化或者替换，并不使相应技术方案的本质脱离本申请实施例技术方案的精神和范围。都应涵盖在本申请的保护范围之内。因此，本申请的保护范围应以所述权利要求的保护范围为准。

Claims (8)

1.一种双臂自碰撞检测方法，其特征在于，包括：
构建碰撞模型；
更新空间位置点信息；
执行碰撞检测；
其中，所述碰撞模型包括：外层包围盒和内层实际模型；所述碰撞检测包括外层包围盒碰撞检测和内层实际模型碰撞检测；
外层包围盒碰撞检测优先于内层实际模型碰撞检测，并且仅在外层包围盒碰撞时才执行内层实际模型碰撞检测；
外层包围盒碰撞检测包括：
设左臂的关节数目记为m，右臂的关节数目记为n，上标
Figure 695173DEST_PATH_IMAGE001
表示左臂，上标r表示右臂，
Figure 581089DEST_PATH_IMAGE002
表示左臂第j关节中心点，
Figure 500504DEST_PATH_IMAGE003
表示右臂第i关节中心点；
设左臂、右臂的基坐标分别为：
Figure 663632DEST_PATH_IMAGE004
设左臂、右臂各关节初始关节角分别为：
Figure 908668DEST_PATH_IMAGE005
；
已知双臂模型尺寸数据：左臂关节连杆
Figure 157247DEST_PATH_IMAGE006
：三维尺寸
Figure 880352DEST_PATH_IMAGE007
、半径
Figure 22621DEST_PATH_IMAGE008
、三角面网格数据
Figure 313925DEST_PATH_IMAGE009
；右臂关节连杆
Figure 908854DEST_PATH_IMAGE010
：三维尺寸
Figure 701230DEST_PATH_IMAGE011
、半径
Figure 573371DEST_PATH_IMAGE012
、三角面网格数据
Figure 163139DEST_PATH_IMAGE013
；
本体：三维尺寸
Figure 245365DEST_PATH_IMAGE014
、半径
Figure 451218DEST_PATH_IMAGE015
、三角面网格数据
Figure 302500DEST_PATH_IMAGE016
；
设包围盒八个顶点的坐标分别为：（
Figure 201186DEST_PATH_IMAGE017
，
Figure 770707DEST_PATH_IMAGE018
，
Figure 639306DEST_PATH_IMAGE019
），（
Figure 486039DEST_PATH_IMAGE020
，
Figure 414681DEST_PATH_IMAGE021
，
Figure 612444DEST_PATH_IMAGE022
），…，(
Figure 284734DEST_PATH_IMAGE023
)，则根据空间向量法获得膨胀后的包围盒；其中，根据空间向量法获得膨胀后的包围盒包括：设V0为膨胀前的顶点，V0沿x方向单位向量
Figure 110608DEST_PATH_IMAGE024
为：
Figure 351096DEST_PATH_IMAGE025
其中，
Figure 650138DEST_PATH_IMAGE026
；
V0沿y方向单位向量
Figure 1485DEST_PATH_IMAGE027
为：
Figure 681865DEST_PATH_IMAGE028
Figure 952310DEST_PATH_IMAGE029
V0顶点沿z方向单位向量
Figure 718140DEST_PATH_IMAGE030
为：
Figure 607599DEST_PATH_IMAGE031
Figure 673644DEST_PATH_IMAGE032
膨胀后的顶点：
Figure 990356DEST_PATH_IMAGE033
Figure 243483DEST_PATH_IMAGE034
。
2.根据权利要求1所述的双臂自碰撞检测方法，其特征在于，更新模型空间位置点信息包括：
采用下式计算臂本体坐标系与双臂基坐标系位置关系的齐次矩阵：
Figure 61266DEST_PATH_IMAGE035
，
其中，Pos2Mat()为将笛卡尔空间位姿转化为对应齐次矩阵的函数；
Figure 326025DEST_PATH_IMAGE036
为左臂本体坐标系到双臂基坐标系的位姿，
Figure 938272DEST_PATH_IMAGE037
为经过Pos2Mat()函数计算后得到的左臂本体坐标系与双臂基坐标系的齐次变换矩阵；
Figure 681625DEST_PATH_IMAGE038
为右臂本体坐标系到双臂基坐标系的位姿，
Figure 178465DEST_PATH_IMAGE039
为经过Pos2Mat()函数计算后得到的右臂本体坐标系与双臂基坐标系的齐次变换矩阵；
采用下式计算各关节在臂坐标系下位置关系的齐次矩阵：
Figure DEST_PATH_FDA0002929038670000031
其中，fk()为由初始关节角求得各关节处的齐次坐标变换矩阵的函数，k为关节号，k∈[1,m+1]，k’∈[1,n+1]；
Figure 471092DEST_PATH_IMAGE041
、分别为左臂、右臂各关节初始关节角；
Figure 574177DEST_PATH_IMAGE042
分别为左臂、右臂各关节中心点相对于臂本体的位姿关系；
采用下式，利用已求得的臂与双臂基坐标系齐次矩阵、关节与臂坐标系齐次矩阵，计算关节与双臂基坐标系齐次矩阵：
Figure 468184DEST_PATH_IMAGE043
；
其中，
Figure 363328DEST_PATH_IMAGE044
分别为左臂、右臂各关节中心点的相对于双臂基坐标系的齐次坐标变换矩阵；
采用下式实时更新左臂、右臂包围盒空间位置；
Figure 192743DEST_PATH_IMAGE045
Figure 173338DEST_PATH_IMAGE046
其中，
Figure 746402DEST_PATH_IMAGE047
、
Figure 230473DEST_PATH_IMAGE048
分别表示左臂第j个、右臂第i个关节包围盒顶点集；
Figure 496369DEST_PATH_IMAGE049
、
Figure 698680DEST_PATH_IMAGE050
分别表示左臂第j个、右臂第i个关节包围盒更新后的顶点集。
3.根据权利要求1或2所述的双臂自碰撞检测方法，其特征在于，执行碰撞检测包括：
生成碰撞检测矩阵；
遍历碰撞检测矩阵；
判断是否发生碰撞。
4.根据权利要求3所述的双臂自碰撞检测方法，其特征在于，碰撞检测矩阵为
Figure 341014DEST_PATH_IMAGE051
其中，
Figure 942241DEST_PATH_IMAGE052
表示右臂节点
Figure 238093DEST_PATH_IMAGE053
与右臂节点
Figure 803067DEST_PATH_IMAGE054
之间所连接的杆件，
Figure 108146DEST_PATH_IMAGE055
表示左臂节点
Figure 442175DEST_PATH_IMAGE056
与左臂节点
Figure 174508DEST_PATH_IMAGE057
之间所连接的杆件，
Figure 226778DEST_PATH_IMAGE058
的值为0或1，0代表不可能发生碰撞，1代表可能发生碰撞。
5.根据权利要求4所述的双臂自碰撞检测方法，其特征在于，遍历碰撞检测矩阵包括：
采用AABB层次包围盒结构，在碰撞检测过程中，首先进行父节点的碰撞检测，
如果父节点不相交，即不发生碰撞，此时无需对子节点做检测，以减小计算资源消耗；
如果父节点相交，即发生碰撞，此时需切换为对子节点做检测，即实际模型的精碰撞检测，此时不再对父节点做碰撞检测；
若检测到发生碰撞，则触发碰撞信号，双臂减速或停止运行；
若检测到未发生碰撞，则忽略，双臂继续工作，但限制允许的最大速度，子节点继续做检测；
若子节点检测中发现实际模型最小距离值呈放大趋势且大于父节点碰撞阈值，则切换为对父节点检测，并且不再限制最大速度。
6.根据权利要求5所述的双臂自碰撞检测方法，其特征在于，父节点的碰撞检测包括：
设两对称几何体A,B，其中心点分别为CA,CB,投影区间的半径分别为rA，rB,
几何体A的中心与几何体B的中心之间的方向向量
Figure 69969DEST_PATH_IMAGE059
为：
Figure 524084DEST_PATH_IMAGE060
；
将方向向量
Figure 427318DEST_PATH_IMAGE061
与分离轴L的向量相乘得到两个中心点CACB连线在分离轴L上的投影长度d：
Figure 966883DEST_PATH_IMAGE062
；其中，L为常数；
如果rA+rB<d,则A与B分离，即不发生碰撞；反之A与B相交，即碰撞。
7.根据权利要求6所述的双臂自碰撞检测方法，其特征在于，子节点碰撞检测包括：
构建左臂、右臂碰撞几何模型：
左臂：
Figure 613765DEST_PATH_IMAGE063
= addSubModel(left_vertices,left_triangles)，其中
右臂：
Figure 656808DEST_PATH_IMAGE064
= addSubModel(right_vertices,right_triangles)
更新左臂、右臂碰撞几何模型：
左臂：
Figure 996522DEST_PATH_IMAGE065
= CollisionObjectf(
Figure 492226DEST_PATH_IMAGE066
，
Figure 945728DEST_PATH_IMAGE067
）
右臂：
Figure 108856DEST_PATH_IMAGE068
= CollisionObjectf(
Figure 353893DEST_PATH_IMAGE069
，
Figure 868051DEST_PATH_IMAGE070
）
进行碰撞检测：
Figure 122315DEST_PATH_IMAGE071
若result 为true，则左臂第j个杆件、右臂第i个杆件发生碰撞；反之，则左臂第j个杆件、右臂第i个杆件不发生碰撞；
其中,addSubModel()、CollisionObjectf()、collide()为开源碰撞检测库的接口函数，left_vertices、right_vertices为目为标杆件的顶点坐标，left_triangles，right_triangles为三角面的空间坐标。
8.一种计算机可读存储介质，其特征在于，其上存储有计算机程序，所述计算机程序被处理器执行时实现权利要求1-7任一所述方法。
CN202010123225.1A 2020-02-27 2020-02-27 一种双臂自碰撞检测方法和计算机可读存储介质 Active CN111360824B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010123225.1A CN111360824B (zh) 2020-02-27 2020-02-27 一种双臂自碰撞检测方法和计算机可读存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010123225.1A CN111360824B (zh) 2020-02-27 2020-02-27 一种双臂自碰撞检测方法和计算机可读存储介质

Publications (2)

Publication Number Publication Date
CN111360824A CN111360824A (zh) 2020-07-03
CN111360824B true CN111360824B (zh) 2021-04-30

Family

ID=71201408

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010123225.1A Active CN111360824B (zh) 2020-02-27 2020-02-27 一种双臂自碰撞检测方法和计算机可读存储介质

Country Status (1)

Country Link
CN (1) CN111360824B (zh)

Families Citing this family (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112060087B (zh) * 2020-08-28 2021-08-03 佛山隆深机器人有限公司 一种用于机器人抓取场景的点云碰撞检测方法
CN111745692A (zh) * 2020-08-31 2020-10-09 佛山隆深机器人有限公司 一种机器人夹具与机械臂干涉检测方法
CN112264991B (zh) * 2020-09-09 2022-06-03 北京控制工程研究所 一种适用于空间机械臂的分层次快速在轨碰撞检测方法
CN112549025B (zh) * 2020-11-27 2022-06-21 北京工业大学 一种基于融合人体运动学约束的协调图双臂协作控制方法
CN112549037B (zh) * 2021-02-24 2021-06-01 佛山隆深机器人有限公司 一种双臂机器人的无碰运动控制方法及计算机存储介质
CN113510746B (zh) * 2021-04-27 2022-03-29 中铁工程装备集团有限公司 一种多机械臂协同的快速自碰撞检测方法
CN113244622B (zh) * 2021-06-22 2022-11-08 腾讯科技（深圳）有限公司 碰撞数据处理方法和装置、存储介质及电子设备
CN113664832A (zh) * 2021-09-01 2021-11-19 上海节卡机器人科技有限公司 一种机器人碰撞预测方法、计算机存储介质及电子设备
CN113799142B (zh) * 2021-10-29 2023-02-21 遨博（北京）智能科技有限公司 对机械臂的碰撞防护方法、控制柜以及机械臂系统
CN114211498B (zh) * 2021-12-30 2023-09-08 中国煤炭科工集团太原研究院有限公司 基于方向包围盒的锚杆支护机器人碰撞检测方法及系统
CN114536342B (zh) * 2022-03-23 2024-07-09 宁波睿达医疗器械有限公司 一种多臂系统及其臂间防撞控制方法
CN114872043B (zh) * 2022-05-09 2023-11-17 苏州艾利特机器人有限公司 一种机器人碰撞检测方法、存储介质及电子设备
CN115716265B (zh) * 2022-10-31 2023-11-10 中国电器科学研究院股份有限公司 一种机器人双臂碰撞神经反射控制方法

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102368280A (zh) * 2011-10-21 2012-03-07 北京航空航天大学 一种面向虚拟装配的基于aabb-obb混合包围盒的碰撞检测方法
CN108052703A (zh) * 2017-11-22 2018-05-18 南京航空航天大学 基于混合层次包围盒的快速碰撞检测方法
WO2018190936A1 (en) * 2017-04-13 2018-10-18 Battelle Memorial Institute Teach mode collision avoidance system and method for industrial robotic manipulators
CN109773785A (zh) * 2018-12-29 2019-05-21 南京埃斯顿机器人工程有限公司 一种工业机器人防碰撞方法

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102368280A (zh) * 2011-10-21 2012-03-07 北京航空航天大学 一种面向虚拟装配的基于aabb-obb混合包围盒的碰撞检测方法
WO2018190936A1 (en) * 2017-04-13 2018-10-18 Battelle Memorial Institute Teach mode collision avoidance system and method for industrial robotic manipulators
CN108052703A (zh) * 2017-11-22 2018-05-18 南京航空航天大学 基于混合层次包围盒的快速碰撞检测方法
CN109773785A (zh) * 2018-12-29 2019-05-21 南京埃斯顿机器人工程有限公司 一种工业机器人防碰撞方法

Also Published As

Publication number Publication date
CN111360824A (zh) 2020-07-03

Similar Documents

Publication Publication Date Title
CN111360824B (zh) 一种双臂自碰撞检测方法和计算机可读存储介质
JP6716178B2 (ja) ロボットの障害物回避制御システム、方法、ロボット及びプログラム
CN108908331B (zh) 超冗余柔性机器人的避障方法及系统、计算机存储介质
US5056031A (en) Apparatus for detecting the collision of moving objects
US9827675B2 (en) Collision avoidance method, control device, and program
CN103236079B (zh) 一种基于三维模型体素化的内部球改进构造方法
CN110553600B (zh) 一种用于工件检测的结构光传感器仿真激光线的生成方法
CN113211495B (zh) 一种机械臂碰撞检测方法、系统、存储介质和机械臂
CN112669434A (zh) 一种基于网格与包围盒的碰撞检测方法
CN113203420B (zh) 一种基于变密度搜索空间的工业机器人动态路径规划方法
CN108213757B (zh) 一种用于焊接机器人的碰撞检测方法
CN112264991B (zh) 一种适用于空间机械臂的分层次快速在轨碰撞检测方法
CN113618742A (zh) 一种机器人避障方法、装置和机器人
CN114012726B (zh) 一种航天机械臂碰撞检测方法
Wang et al. Path planning for the gantry welding robot system based on improved RRT
GB2227106A (en) Detecting collision
CN109807933B (zh) 一种能力图点云更新方法、装置、设备及存储介质
CN111251335B (zh) 基于包围盒算法的高精度机械臂碰撞检测方法
Fan et al. Fast global collision detection method based on feature-point-set for robotic machining of large complex components
CN115008475B (zh) 一种基于混合几何表征的双机械臂协同避障运动规划优化方法
JP6848761B2 (ja) 物体間距離評価方法及び相対的に移動する物体間の干渉評価方法
JP2017131990A (ja) 干渉回避方法
JPH085028B2 (ja) 移動体の衝突判定方法並びに衝突判定装置
Qu et al. Dynamic Viewpoint-Based Obstacle Avoidance of Robotic Arms in Complex Environments
JP2019086876A (ja) 画像処理装置、画像処理方法およびプログラム

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