[发明专利]一种基于SCS包围结构的机器人碰撞检测方法

权利要求书

1.一种基于SCS包围结构的机器人碰撞检测方法，其特征在于，包括以下步骤：

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

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

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

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

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

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

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

2.根据权利要求1所述的一种基于SCS包围结构的机器人碰撞检测方法，其特征在于，所述采用D-H方法建立的机器人各杆坐标系，具体为：

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

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

3.根据权利要求1所述的一种基于SCS包围结构的机器人碰撞检测方法，其特征在于，所述采用SCS包围结构，对机器人连杆和关节分别进行包围，使连杆或关节的两端分别在两个球体内部，连杆或者关节的中间部分在圆柱体内部。

4.根据权利要求1所述的一种基于SCS包围结构的机器人碰撞检测方法，其特征在于，所述求机器人各个SCS包围结构的两个球心坐标，具体为：

其中，P_i^i-1、W_i^i-1分别为坐标系{i}相对坐标系{i-1}的姿态矩阵和位置矩阵，1≤i≤n，P、W分别为坐标系{n}相对于世界坐标系的姿态矩阵和位置矩阵，球心在世界坐标系中的坐标即为(P_x，P_y，P_z)。

5.根据权利要求1所述的一种基于SCS包围结构的机器人碰撞检测方法，其特征在于，所述将滚轴工件模型以及各个SCS包围结构模型向世界坐标系中的XY、YZ、XZ三个平面投影，判断滚轴工件模型与各个SCS包围结构的投影在三个平面上是否均相交，包括以下步骤：

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

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