CN112264991A - 一种适用于空间机械臂的分层次快速在轨碰撞检测方法 - Google Patents

Abstract

本发明涉及一种适用于空间机械臂的分层次快速在轨碰撞检测方法，属于空间机器人运动规划与碰撞检测领域；步骤一、建立空间机械臂模型；步骤二、对星本体和对接环、2个机械臂、2个末端执行机构分别建立包围盒；步骤三、建立2个执行机构包围盒之间碰撞判断解析式，并判断2个执行机构包围盒是否发生碰撞；建立其中1个机械臂对应末端执行机构的包围盒与另一个机械臂包围盒的碰撞判断解析式，并判断该末端执行机构与该机械臂包围盒是否发生碰撞；建立2个机械臂包围盒之间的碰撞判断解析式，并判断2个机械臂包围盒是否发生碰撞；本发明提高了对星上碰撞情况的检测效率、数据存储效率、动态更新效率以及检测精确度。

Description

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

技术领域

本发明属于空间机器人运动规划与碰撞检测领域，涉及一种适用于空间机械臂的分层次快速在轨碰撞检测方法。

背景技术

在多个空间机械臂在轨协同操作的过程中，可能出现臂杆与臂杆碰撞、臂杆与末端执行机构碰撞、末端执行机构与末端执行机构碰撞、臂杆与星本体碰撞及末端执行机构与星本体碰撞等5种异常情况。为高效、精确地监测多臂协同操作的安全性，对星上碰撞情况做出实时判断和适时预警，需发展合理有效的在轨碰撞检测方法。

目前的分层次碰撞检测方法多是通过遍历分析平表面包围盒层次树的投影是否重叠，来获取潜在的碰撞对象。然而平表面包围盒虽然构型简单，但对星上的圆滑表面包围不够紧密，并且遍历会降低星上的检测效率、数据存储效率及动态更新效率。基于圆柱体包围盒的碰撞检测方法可提高包围盒紧密性，但目前多是通过可视化方法判断其是否相交或重叠，然而在在轨卫星平台上不具备可视化条件，还需通过解析式计算进行判断。另外，基于均匀网格分割做点集相交检测也可以精确地检测碰撞情况，但相对费时。

发明内容

本发明解决的技术问题是：克服现有技术的不足，提出一种适用于空间机械臂的分层次快速在轨碰撞检测方法，提高了对星上碰撞情况的检测效率、数据存储效率、动态更新效率以及检测精确度，满足对星上动态复杂机构的异常工作状态实时监测和快速规避的应用需求。

本发明解决技术的方案是：

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

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

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

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

在上述的一种适用于空间机械臂的分层次快速在轨碰撞检测方法，所述步骤一中，星本体为水平放置的长方体结构；对接环为同轴放置的2个柱状结构；对接环固定设置在星本体的上表面；2个机械臂固定设置在星本体的顶部，且2个机械臂对称放置在对接环的两侧；每个末端执行机构对应固定安装在1个机械臂的顶端；每个机械臂包括四个轴端依次连接的柱状机械臂关节。

在上述的一种适用于空间机械臂的分层次快速在轨碰撞检测方法，所述步骤二中，建立包围盒的具体方法为：

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

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

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

在上述的一种适用于空间机械臂的分层次快速在轨碰撞检测方法，所述步骤三中，2个执行机构包围盒之间碰撞判断解析式的建立方法为：

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

|O₁-O₂|>a₁+a₂

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

在上述的一种适用于空间机械臂的分层次快速在轨碰撞检测方法，所述步骤三中，其中1个机械臂对应末端执行机构的执行机构包围盒与另一个机械臂包围盒的碰撞判断解析式的建立方法为：

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

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

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

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

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

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

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

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

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

在上述的一种适用于空间机械臂的分层次快速在轨碰撞检测方法，所述步骤三中，2个机械臂包围盒之间的碰撞判断解析式的建立方法为：

S1、设定其中1个机械臂包围盒中的任意柱状盒体为i，半径为r_i；由该柱状盒体与相邻的2个柱状盒体的连接点确定该柱状盒体的轴线l_i；设定另1个机械臂包围盒中的任意柱状盒体为j，半径为rj；由该柱状盒体与相邻的2个柱状盒体的连接点确定该柱状盒体的轴线l_j；

S2、确定轴线l_i与轴线l_j所在直线的公垂线长度L_ij,min(u,v)，其中，u为公垂线垂足相对于轴线l_i的位置；v为公垂线垂足相对于轴线l_j的位置；当0≤u≤1时，垂足在轴线l_i上，当u＜0或u>1时，垂足在轴线l_j的延长线上；当0≤v≤1时，垂足在轴线l_j上，当v＜0或v>1时，垂足在轴线l_j的延长线上；

S3、建立2个柱状盒体的碰撞判断解析式L_ij,min(u,v)>r_i+r_j；当碰撞判断解析式成立时，判断2个柱状盒体不发生碰撞；当碰撞判断解析式不成立时，进行进一步判断，进入S4；

S4、建立2个柱状盒体侧面碰撞判断解析式；判断2个柱状盒体否发生侧面碰撞；建立其中1个柱状盒体侧面与另1个柱状盒体端面碰撞判断解析式；判断是否1个柱状盒体侧面与另1个柱状盒体端面发生碰撞；建立2个柱状盒体端面碰撞判断解析式；判断2个柱状盒体否发生端面碰撞。

在上述的一种适用于空间机械臂的分层次快速在轨碰撞检测方法，所述S4中，2个柱状盒体侧面碰撞判断解析式的建立方法为：

当0≤u≤1，0≤v≤1时，建立2个柱状盒体侧面碰撞判断解析式：L_min(u,v)≤r_i+r_j，当解析式成立时，则判断2个柱状盒体不发生碰撞；否则判断2个柱状盒体发生侧面碰撞。

在上述的一种适用于空间机械臂的分层次快速在轨碰撞检测方法，所述S4中，其中1个柱状盒体侧面与另1个柱状盒体端面碰撞判断解析式的建立方法为：

当0≤u≤1且v＜0、或0≤u≤1且v>1、或u＜0且0≤v≤1、或u>1且0≤v≤1时，将其中1个柱状盒体的端面圆心与另1个柱状盒体的轴线中点之间连线作为分离轴，通过计算两个柱状盒体在该分离轴上的投影长度之和是否大于最小安全距离，建立其中1个柱状盒体侧面与另1个柱状盒体端面碰撞判断解析式：

设定柱状盒体i与相邻2个柱状盒体的连接点为P₁₁和P₁₂，柱状盒体i的半径为r_i；另1个柱状盒体j与相邻2个柱状盒体的连接点为P₂₁和P₂₂，该柱状盒体j的半径为r_j；则解析式为：

当解析式成立时，则判断其中1个柱状盒体侧面与另1个柱状盒体端面碰撞；否则判断2个柱状盒体不发生碰撞。

在上述的一种适用于空间机械臂的分层次快速在轨碰撞检测方法，所述S4中，2个柱状盒体端面碰撞判断解析式的建立方法为：

当u＜0且v>1、或u>1且v＜0时，计算连接点P₁₁到柱状盒体j下端面的距离d₁₂；计算连接点P₂₂到柱状盒体i上端面的距离d₂₁：

则解析式为：

|P₂₂-P₁₁|²≤r_i ²+r_j ²

当解析式成立时，则判断2个柱状盒体端面碰撞；否则判断2个柱状盒体不发生碰撞。

本发明与现有技术相比的有益效果是：

(1)本发明的基于分层次碰撞检测方式，首先利用包围盒碰撞检测对星上碰撞情况进行初步排查，能够提高检测速度；进一步针对碰撞包围盒对应的部分做点集相交检测，能够提高检测精确度；

(2)本发明充分考虑星上机构的几何特征，利用球体或圆柱体等最小包围盒简化星上检测对象，提高包围盒紧密性，减少冗余；

(3)本发明仅需获知活动关节的实时位置，就可以以少量的输入参数，通过解析式判据快速判断包围盒是否相交，从而降低数据存储需求、减少计算量，提高在轨运算效率和动态更新效率；

(4)本发明在快速排除不可能发生碰撞的部位后，利用机构上的真实点云数据进行分层次相交检测，逐步缩小检测范围，既加快了碰撞检测速度，又提高了碰撞检测信息的精确度；

(5)本发明的在轨碰撞检测方法不仅可以应用于空间机械臂，还适用于其他星上形状复杂的活动机构，从而快速且精确地对星上碰撞情况做出实时判断和适时预警，检测和确保在轨操作的安全性。

附图说明

图1为本发明空间机械臂模型示意图；

图2为本发明2个执行机构包围盒碰撞示意图；

图3为本发明执行机构包围盒与机械臂包围盒碰撞示意图；

图4为本发明轴线l_i、轴线l_j、公垂线示意图；

图5为本发明2个柱状盒体碰撞示意图；

图6为本发明基于八叉树的基元分割及点集求交示意图。

具体实施方式

下面结合实施例对本发明作进一步阐述。

本发明设计了一种适用于空间机械臂的分层次快速在轨碰撞检测方法，涉及空间机器人运动规划与碰撞检测领域；该方法基于分层次碰撞检测方式，首先利用包围盒碰撞检测对星上碰撞情况进行初步排查，并提出一套仅需获知活动关节实时位置即可快速做出碰撞判断的解析式判据，以提高检测速度；为防止误判，进一步针对碰撞包围盒对应的部分机构做点集相交检测，以提高检测精确度。本发明可提高对星上碰撞情况的检测效率、数据存储效率、动态更新效率以及检测精确度，满足对星上动态复杂机构的异常工作状态实时监测和快速规避的应用需求。

适用于空间机械臂的分层次快速在轨碰撞检测方法，主要包括如下步骤：

步骤一、由于星上机械臂及末端执行机构形状较为复杂，为提高星上碰撞检测效率和精确度，建立空间机械臂模型，如图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个执行机构包围盒是否发生碰撞，如图2所示，2个执行机构包围盒之间碰撞判断解析式的建立方法为：

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

|O₁-O₂|>a₁+a₂

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

建立其中1个机械臂3对应末端执行机构4的包围盒与另一个机械臂包围盒的碰撞判断解析式，并判断该末端执行机构4与该机械臂包围盒是否发生碰撞；如图3所示，其中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；当该柱状盒体j位于机械臂包围盒的最底端时，测量该柱状盒体j与本体包围盒的连接点P_j-1、测量该柱状盒体j与相邻柱状盒体的连接点P_j+1；当该柱状盒体j位于机械臂包围盒的最顶端时，测量该柱状盒体j与相连的柱状盒体的连接点P_j-1，测量该柱状盒体j与执行机构包围盒的连接点P_j+1。

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

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

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

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

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

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

建立2个机械臂包围盒之间的碰撞判断解析式，并判断2个机械臂包围盒是否发生碰撞。如图5所示，2个机械臂包围盒之间的碰撞判断解析式的建立方法为：

S1、设定其中1个机械臂包围盒中的任意柱状盒体为i，半径为r_i；由该柱状盒体与相邻的2个柱状盒体的连接点确定该柱状盒体的轴线l_i；设定另1个机械臂包围盒中的任意柱状盒体为j，半径为rj；由该柱状盒体与相邻的2个柱状盒体的连接点确定该柱状盒体的轴线l_j；

S2、如图4所示，确定轴线l_i与轴线l_j所在直线的公垂线长度L_ij,min(u,v)，其中，u为公垂线垂足相对于轴线l_i的位置；v为公垂线垂足相对于轴线l_j的位置；当0≤u≤1时，垂足在轴线l_i上，当u＜0或u>1时，垂足在轴线l_j的延长线上；当0≤v≤1时，垂足在轴线l_j上，当v＜0或v>1时，垂足在轴线l_j的延长线上；

S3、建立2个柱状盒体的碰撞判断解析式L_ij,min(u,v)>r_i+r_j；当碰撞判断解析式成立时，判断2个柱状盒体不发生碰撞；当碰撞判断解析式不成立时，进行进一步判断，进入S4；

S4、建立2个柱状盒体侧面碰撞判断解析式；判断2个柱状盒体否发生侧面碰撞；2个柱状盒体侧面碰撞判断解析式的建立方法为：

当0≤u≤1，0≤v≤1时，建立2个柱状盒体侧面碰撞判断解析式：L_min(u,v)≤r_i+r_j，当解析式成立时，则判断2个柱状盒体不发生碰撞；否则判断2个柱状盒体发生侧面碰撞。

建立其中1个柱状盒体侧面与另1个柱状盒体端面碰撞判断解析式；判断是否1个柱状盒体侧面与另1个柱状盒体端面发生碰撞；其中1个柱状盒体侧面与另1个柱状盒体端面碰撞判断解析式的建立方法为：

当0≤u≤1且v＜0、或0≤u≤1且v>1、或u＜0且0≤v≤1、或u>1且0≤v≤1时，将其中1个柱状盒体的端面圆心与另1个柱状盒体的轴线中点之间连线作为分离轴，通过计算两个柱状盒体在该分离轴上的投影长度之和是否大于最小安全距离，建立其中1个柱状盒体侧面与另1个柱状盒体端面碰撞判断解析式：

设定柱状盒体i与相邻2个柱状盒体的连接点为P₁₁和P₁₂，柱状盒体i的半径为r_i；另1个柱状盒体j与相邻2个柱状盒体的连接点为P₂₁和P₂₂，该柱状盒体j的半径为r_j；则解析式为：

当解析式成立时，则判断其中1个柱状盒体侧面与另1个柱状盒体端面碰撞；否则判断2个柱状盒体不发生碰撞。

建立2个柱状盒体端面碰撞判断解析式；判断2个柱状盒体否发生端面碰撞。2个柱状盒体端面碰撞判断解析式的建立方法为：

当u＜0且v>1、或u>1且v＜0时，计算连接点P₁₁到柱状盒体j下端面的距离d₁₂；计算连接点P₂₂到柱状盒体i上端面的距离d₂₁：

则解析式为：

|P₂₂-P₁₁|²≤r_i ²+r_j ²

当解析式成立时，则判断2个柱状盒体端面碰撞；否则判断2个柱状盒体不发生碰撞。

在确定了出现碰撞的基础上，基于八叉树的基元分割及点集求交示意图，由图6可知，将可能发生碰撞的两部分机构离散为三维点云模型，判断两个点集是否存在交集。若不存在交集，则证明无碰撞；若存在交集，则证明有碰撞。

点集相交检测方法是采用八叉树思想对点集进行分层次地分割和检测。首先将两部分机构的点云坐标统一到惯性坐标系中，然后利用较大的基元立方体(如100mm×100mm×100mm)分别对两部分机构的三维点云模型进行分割，并对两部分机构的基元立方体中心点集进行求交运算。若交集为空集，则无碰撞；若交集为非空集，则有碰撞。假若某基元立方体相交，则将两部分机构的相交部分进一步分割成较小的基元立方体(如10mm×10mm×10mm)，并对两部分机构的基元立方体中心点集进行求交运算。若交集仍为非空集，则继续分割，直至最小基元(可根据机械臂上某些凸出部位的最小尺寸或避障冗余量确定最小基元尺寸)。按照上述分割式点集相交检测方法，可逐步缩小检测范围，比直接利用最小基元进行分割和检测要节省运算成本，从而既加快星上碰撞检测速度，又能满足精确性要求。

本发明中基于分层次碰撞检测方式，首先利用包围盒碰撞检测对星上碰撞情况进行初步排查，能够提高检测速度；进一步针对碰撞包围盒对应的部分做点集相交检测，能够提高检测精确度。本发明充分考虑星上机构的几何特征，利用球体或圆柱体等最小包围盒简化星上检测对象，提高包围盒紧密性，减少冗余。本发明仅需获知活动关节的实时位置，就可以以少量的输入参数，通过解析式判据快速判断包围盒是否相交，从而降低数据存储需求、减少计算量，提高在轨运算效率和动态更新效率。本发明在快速排除不可能发生碰撞的部位后，利用机构上的真实点云数据进行分层次相交检测，逐步缩小检测范围，既加快了碰撞检测速度，又提高了碰撞检测信息的精确度。本发明的在轨碰撞检测方法不仅可以应用于空间机械臂，还适用于其他星上形状复杂的活动机构，从而快速且精确地对星上碰撞情况做出实时判断和适时预警，检测和确保在轨操作的安全性。

本发明虽然已以较佳实施例公开如上，但其并不是用来限定本发明，任何本领域技术人员在不脱离本发明的精神和范围内，都可以利用上述揭示的方法和技术内容对本发明技术方案做出可能的变动和修改，因此，凡是未脱离本发明技术方案的内容，依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化及修饰，均属于本发明技术方案的保护范围。

Claims (10)

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

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

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

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

2.根据权利要求1所述的一种适用于空间机械臂的分层次快速在轨碰撞检测方法，其特征在于：所述步骤一中，星本体(1)为水平放置的长方体结构；对接环(2)为同轴放置的2个柱状结构；对接环(2)固定设置在星本体(1)的上表面；2个机械臂(3)固定设置在星本体(1)的顶部，且2个机械臂(3)对称放置在对接环(2)的两侧；每个末端执行机构(4)对应固定安装在1个机械臂(3)的顶端；每个机械臂(3)包括四个轴端依次连接的柱状机械臂关节。

3.根据权利要求2所述的一种适用于空间机械臂的分层次快速在轨碰撞检测方法，其特征在于：所述步骤二中，建立包围盒的具体方法为：

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

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

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

4.根据权利要求3所述的一种适用于空间机械臂的分层次快速在轨碰撞检测方法，其特征在于：所述步骤三中，2个执行机构包围盒之间碰撞判断解析式的建立方法为：

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

|O₁-O₂|>a₁+a₂

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

5.根据权利要求4所述的一种适用于空间机械臂的分层次快速在轨碰撞检测方法，其特征在于：所述步骤三中，其中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、建立侧壁碰撞判断解析式：

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

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

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

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

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

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

7.根据权利要求6所述的一种适用于空间机械臂的分层次快速在轨碰撞检测方法，其特征在于：所述步骤三中，2个机械臂包围盒之间的碰撞判断解析式的建立方法为：

S1、设定其中1个机械臂包围盒中的任意柱状盒体为i，半径为r_i；由该柱状盒体与相邻的2个柱状盒体的连接点确定该柱状盒体的轴线l_i；设定另1个机械臂包围盒中的任意柱状盒体为j，半径为r_j；由该柱状盒体与相邻的2个柱状盒体的连接点确定该柱状盒体的轴线l_j；

S2、确定轴线l_i与轴线l_j所在直线的公垂线长度L_ij,min(u,v)，其中，u为公垂线垂足相对于轴线l_i的位置；v为公垂线垂足相对于轴线l_j的位置；当0≤u≤1时，垂足在轴线l_i上，当u＜0或u>1时，垂足在轴线l_j的延长线上；当0≤v≤1时，垂足在轴线l_j上，当v＜0或v>1时，垂足在轴线l_j的延长线上；

S3、建立2个柱状盒体的碰撞判断解析式L_ij,min(u,v)>r_i+r_j；当碰撞判断解析式成立时，判断2个柱状盒体不发生碰撞；当碰撞判断解析式不成立时，进行进一步判断，进入S4；

S4、建立2个柱状盒体侧面碰撞判断解析式；判断2个柱状盒体否发生侧面碰撞；建立其中1个柱状盒体侧面与另1个柱状盒体端面碰撞判断解析式；判断是否1个柱状盒体侧面与另1个柱状盒体端面发生碰撞；建立2个柱状盒体端面碰撞判断解析式；判断2个柱状盒体否发生端面碰撞。

8.根据权利要求7所述的一种适用于空间机械臂的分层次快速在轨碰撞检测方法，其特征在于：所述S4中，2个柱状盒体侧面碰撞判断解析式的建立方法为：

当0≤u≤1，0≤v≤1时，建立2个柱状盒体侧面碰撞判断解析式：L_min(u,v)≤r_i+r_j，当解析式成立时，则判断2个柱状盒体不发生碰撞；否则判断2个柱状盒体发生侧面碰撞。

9.根据权利要求8所述的一种适用于空间机械臂的分层次快速在轨碰撞检测方法，其特征在于：所述S4中，其中1个柱状盒体侧面与另1个柱状盒体端面碰撞判断解析式的建立方法为：

当0≤u≤1且v＜0、或0≤u≤1且v>1、或u＜0且0≤v≤1、或u>1且0≤v≤1时，将其中1个柱状盒体的端面圆心与另1个柱状盒体的轴线中点之间连线作为分离轴，通过计算两个柱状盒体在该分离轴上的投影长度之和是否大于最小安全距离，建立其中1个柱状盒体侧面与另1个柱状盒体端面碰撞判断解析式：

设定柱状盒体i与相邻2个柱状盒体的连接点为P₁₁和P₁₂，柱状盒体i的半径为r_i；另1个柱状盒体j与相邻2个柱状盒体的连接点为P₂₁和P₂₂，该柱状盒体j的半径为r_j；则解析式为：

当解析式成立时，则判断其中1个柱状盒体侧面与另1个柱状盒体端面碰撞；否则判断2个柱状盒体不发生碰撞。

10.根据权利要求9所述的一种适用于空间机械臂的分层次快速在轨碰撞检测方法，其特征在于：所述S4中，2个柱状盒体端面碰撞判断解析式的建立方法为：

当u＜0且v>1、或u>1且v＜0时，计算连接点P₁₁到柱状盒体j下端面的距离d₁₂；计算连接点P₂₂到柱状盒体i上端面的距离d₂₁：

则解析式为：

|P₂₂-P₁₁|²≤r_i ²+r_j ²

当解析式成立时，则判断2个柱状盒体端面碰撞；否则判断2个柱状盒体不发生碰撞。

Images