[发明专利]一种适用于空间机械臂的分层次快速在轨碰撞检测方法

权利要求书

1.一种适用于空间机械臂的分层次快速在轨碰撞检测方法，其特征在于：

步骤一、建立空间机械臂模型，包括星本体(1)、对接环(2)、2个机械臂(3)和2个末端执行机构(4)；

星本体(1)为水平放置的长方体结构；对接环(2)为同轴放置的2个柱状结构；对接环(2)固定设置在星本体(1)的上表面；2个机械臂(3)固定设置在星本体(1)的顶部，且2个机械臂(3)对称放置在对接环(2)的两侧；每个末端执行机构(4)对应固定安装在1个机械臂(3)的顶端；每个机械臂(3)包括四个轴端依次连接的柱状机械臂关节；

步骤二、对星本体(1)和对接环(2)、2个机械臂(3)、2个末端执行机构(4)分别建立包围盒；获得本体包围盒、2个机械臂包围盒和2个执行机构包围盒；

建立包围盒的具体方法为：

对星本体(1)和对接环(2)建立本体包围盒，本体包围盒形状为星本体(1)和对接环(2)组成的最小外形面；

对2个机械臂(3)分别建立机械臂包围盒，机械臂包围盒包括4个柱状盒体；每个柱状盒体的形状为对应机械臂关节的最小外形面；

对2个末端执行机构(4)分别建立执行机构包围盒；执行机构包围盒为球形结构；执行机构包围盒为包络末端执行机构(4)外形的最小球体；

步骤三、建立2个执行机构包围盒之间碰撞判断解析式，并判断2个执行机构包围盒是否发生碰撞；建立其中1个机械臂(3)对应末端执行机构(4)的包围盒与另一个机械臂包围盒的碰撞判断解析式，并判断该末端执行机构(4)与该机械臂包围盒是否发生碰撞；建立2个机械臂包围盒之间的碰撞判断解析式，并判断2个机械臂包围盒是否发生碰撞；

2个执行机构包围盒之间碰撞判断解析式的建立方法为：

设定其中1个执行机构包围盒的球心为O₁，半径为a₁；另一个执行机构包围盒的球心为O₂，半径为a₂；则碰撞判断解析式为：

|O₁-O₂|＞a₁+a₂

当解析式成立时，则判断2个执行机构包围盒不发生碰撞；否则判断2个执行机构包围盒发生碰撞；

其中1个机械臂(3)对应末端执行机构(4)的执行机构包围盒与另一个机械臂包围盒的碰撞判断解析式的建立方法为：

S1、设定该执行机构包围盒的球心为O₁，半径为a₁；设定相对的机械臂包围盒其中1个柱状盒体为j；j为柱状盒体序号，j＝1,2,3,4；柱状盒体j的半径为r_j；

S2、测量该柱状盒体j与相邻2个该柱状盒体的连接点分别为P_j和P_j+1；

S3、建立侧壁碰撞判断解析式：

当解析式(1)成立时，则判断该执行机构包围盒与相对的柱状盒体j不发生碰撞；否则判断该执行机构包围盒与相对的柱状盒体j的侧壁发生碰撞；

S4、建立边界碰撞判断解析式：

当解析式(2)成立时，则判断该执行机构包围盒与相对的柱状盒体j不发生碰撞；否则判断该执行机构包围盒与相对的柱状盒体j的边界发生碰撞；

S5、建立端面碰撞判断解析式：

当解析式(3)成立时，则判断该执行机构包围盒与相对的柱状盒体j不发生碰撞；否则判断该执行机构包围盒与相对的柱状盒体j的端面发生碰撞。

2.根据权利要求1所述的一种适用于空间机械臂的分层次快速在轨碰撞检测方法，其特征在于：所述步骤三的S2中，当该柱状盒体j位于机械臂包围盒的最底端时，测量该柱状盒体j与本体包围盒的连接点P_j-1、测量该柱状盒体j与相邻柱状盒体的连接点P_j+1；当该柱状盒体j位于机械臂包围盒的最顶端时，测量该柱状盒体j与相连的柱状盒体的连接点P_j-1，测量该柱状盒体j与执行机构包围盒的连接点P_j+1。