一种基于SCS包围结构的机器人碰撞检测方法与流程

本发明属于机器人技术领域，具体的说是一种基于scs(sphere-cylinder-sphere，球体-圆柱体-球体)包围结构的机器人碰撞检测方法。

背景技术：

机器人技术是实现先进制造技术的重要基础之一，正不断向高精度、高效率、高协调性方向发展。机器人技术中的多自由度机器人是工业机器人的一种。为了提高机器人运动过程中的安全性，需要设计相应的碰撞检测，避免机器人与工件之间发生碰撞，从而使机器人在各种作业情况下都能安全快速准确地到达给定的位置。

早期的碰撞检测大多是基于格子或者bsp树的。基于格子的碰撞检测实现简单但精度不够，不属于严格意义的3d碰撞检测。基于bsp树的碰撞检测需要很长的处理时间，占用较多的资源容量。

当前多采用基于包围体的碰撞检测算法。包围体技术是在1976年由clark提出的，基本思想是用一个简单的几何形体将动画场景中的几何形体围住，通过构造树状层次结构来逼近真实的包围体。基于层次包围体的碰撞检测算法根据包围体类型的不同可以区分为：包围球体、aabb轴对齐包围体、obb有向包围体、k-dop包围体等。obb和k-dop包围体方法虽然紧密性较好，但复杂度比较高，旋转更新慢，不适合多关节机器人在运动过程中需持续旋转这一特点。包围球的方法不需要旋转更新，aabb的旋转更新快，适合工业机器人的碰撞检测，但二者包装的紧密性较差，降低了碰撞检测的精度。由于机器人的关节以及连杆多为柱形，并且机器人在运动过程中各关节和连杆需要不断旋转，持续求解包围体位置比较复杂，因而直接用于碰撞检测的包围球和aabb轴对齐包围体方法有一定的局限性。

技术实现要素：

为了克服机器人现有包围体碰撞检测算法的计算速度慢，检测精度低等缺点，本发明提供一种基于scs包围结构的机器人碰撞检测方法，根据机器人的结构特点，可以大幅度提高碰撞检测效率，同时在一定程度上提高机器人工作过程中的安全性和碰撞检测精度。

本发明为实现上述目的所采用的技术方案是：一种基于scs包围结构的机器人碰撞检测方法，包括以下步骤：

步骤1：采用d-h方法建立的机器人各杆坐标系；

步骤2：采用包括两个球体和一个圆柱体的scs包围结构，对机器人连杆和关节分别进行包围；

步骤3：求机器人各个scs包围结构的两个球心坐标；

步骤4：将滚轴工件模型以及各个scs包围结构向世界坐标系中的xy、yz、xz三个平面投影，判断滚轴工件模型与各个scs包围结构的投影在三个平面上是否均相交；

步骤5：如果是，则检测到碰撞发生，重新规划机器人路径，返回步骤3；否则，判断是否到达路径终点；

步骤6：如果已到达路径终点，则表明未检测到碰撞发生；如果未到达路径终点，则更新机器人状态，返回步骤3。

所述采用d-h方法建立的机器人各杆坐标系，具体为：

坐标系{i-1}到坐标系{i}的变化矩阵为：

其中，分别为坐标系{i}相对坐标系{i-1}的姿态矩阵和位置矩阵；θi是关节i的旋转角；βi是关节i和i+1间的扭转角；αi为关节i+1的长度；di是关节i的偏距。

所述scs包围结构具体为：两个球体的球心分别与圆柱体底面的两个圆心重合，scs包围结构的球体半径与圆柱体底面半径比值大于或者等于1。

所述采用scs包围结构，对机器人连杆和关节分别进行包围，使连杆或关节的两端分别在两个球体内部，连杆或者关节的中间部分在圆柱体内部。

所述求机器人各个scs包围结构的两个球心坐标，具体为：

其中，分别为坐标系{i}相对坐标系{i-1}的姿态矩阵和位置矩阵，1≤i≤n，p、w分别为坐标系{n}相对于世界坐标系的姿态矩阵和位置矩阵，球心在世界坐标系中的坐标即为(px，py，pz)。

所述将滚轴工件模型以及各个scs包围结构模型向世界坐标系中的xy、yz、xz三个平面投影，判断滚轴工件模型与各个scs包围结构的投影在三个平面上是否均相交，包括以下步骤：

将scs包围结构在任一平面上的投影转化为相互连接的三个部分，即第一部分为圆形：以第一球体的球心在平面上的投影点为圆心，以第一球体的半径为半径的圆；第二部分为矩形：连接scs包围结构在平面上的两球心投影点，形成线段，过两个投影点分别做所述线段的垂线，两投影点分别为两垂线上线段的中点，其中所述两垂线上线段的长度均等于scs包围结构中圆柱体底面直径的长度，所述两垂线上的线段在平面上平行且相等，为矩形的一组对边；第三部分为圆形：以第二球体的球心在平面上的投影点为圆心，以第二球体的半径为半径的圆；

判断滚轴工件模型与各个scs包围结构的投影在三个平面上是否相交，简化为判断在三个平面上，滚轴工件模型的投影与scs包围结构投影转化后的三个部分是否相交，若在任一平面上两模型的投影不相交，则被检测的结构没有发生碰撞，当且仅当三个平面上两模型的投影均相交时，被检测的结构认定为发生了碰撞。

本发明具有以下优点及有益效果：

一是碰撞检测速度快。scs包围结构普遍适用于在三维空间中连续不定向旋转的柱形结构，机器人的结构特点和运动特点符合这一需求。在碰撞检测阶段，只需定位球心的坐标就可得出碰撞检测结果。

二是检测精度较高。采用scs层次包围结构与大部分工业机器人的结构相吻合，可以比较紧密将机器人包围，提高检测精度。

附图说明

图1为scs包围结构截面示意图；

图2采用d-h方法建立的motoman-mh6机器人运动学坐标系；

图3为scs包围结构在平面上的投影转化图；

图4基于scs包围结构的机器人碰撞检测流程图。

具体实施方式

下面结合附图及实施例对本发明做进一步的详细说明，基于scs包围结构的机器人碰撞检测方法流程如图4所示。

1.在初始阶段，以motoman-mh6机器人为对象，如图2所示，采用d-h方法建立的机器人各杆坐标系，其中，坐标系{0}三个坐标轴方向与世界坐标系中相应的x、y、z坐标轴方向一致，坐标系{i-1}到坐标系{i}的变化矩阵为：

其中，分别为坐标系{i}相对坐标系{i-1}的姿态矩阵和位置矩阵；θi是关节i的旋转角；βi是关节i和i+1间的扭转角；αi为关节i+1的长度；di是关节i的偏距。

2.采用sphere-cylinder-sphere(球体-圆柱体-球体)包围结构，对机器人连杆和关节分别进行包围。如图1所示，其中：scs包围结构由球体s1，圆柱体c，以及球体s2组成；两个球体的球心分别与圆柱体底面的两个圆心重合；scs包围结构的球体半径与圆柱体底面半径比值大于或者等于1；采用scs包围空间对机器人各个连杆和关节进行包围，并且保证机器人各个连杆和关节在scs包围结构内部。包围特点为：连杆或者关节的两端分别在两个球体内部，连杆或者关节的中间主体部分在圆柱体内部。

3.在碰撞检测阶段一，利用代数解析法求机器人各个scs包围结构的两个球心坐标。

求各个scs包围结构的两个球心坐标，求解过程为：

其中，分别为坐标系{i}相对坐标系{i-1}的姿态矩阵和位置矩阵，p、w分别为坐标系{n}相对于世界坐标系的姿态矩阵和位置矩阵，球心在世界坐标系中的坐标即为(px，py，pz)。

4.在碰撞检测阶段二，将滚轴工件模型以及各个scs包围结构向世界坐标系中的xy、yz、xz三个平面投影。

采用常见的包围结构对滚轴工件模型进行包围，之后向世界坐标系中的xy、yz、xz三个平面投影。

scs包围结构在任一平面上的投影可以转化为相互连接的三个部分。如图示3所示，第一部分为圆形：以s1的球心在平面上的投影点为圆心，以s1的半径为半径的圆。第二部分为矩形：连接scs包围结构在平面上的两球心投影点，形成线段o1o2，过两个投影点分别做o1o2的垂线l1，l2，其中o1为l1上线段ab的中点，o2为l2上线段cd的中点，其中ab、cd的长度均等于圆柱体底面直径的长度，矩形abdc即为所求。第三部分为圆形:以s2的球心在平面上的投影点为圆心，以s2的半径为半径的圆。

判断滚轴工件模型与各个scs包围结构的投影在三个平面上是否相交，可以简化为判断在三个平面上，滚轴工件模型的投影与scs包围结构模型投影转化后的三个部分是否相交，若在任一平面上两模型的投影不相交，则被检测的结构没有发生碰撞，当且仅当三个平面上两模型的投影均相交时，被检测的结构认定为发生了碰撞。

5.在碰撞检测阶段三：如果检测到碰撞发生，重新规划机器人路径，进行碰撞检测；如果未检测到碰撞发生，判断机器人是否到达路径规划终点，若未达到终点，更新机器人状态，进行碰撞检测。

在结束阶段：如果在机器人运动路径中始终未检测到碰撞发生，则认定规划路径安全，碰撞检测结束。