[发明专利]一种空间组合机械臂的快速自碰撞检测方法

权利要求书

1.一种空间组合机械臂的快速自碰撞检测方法，其特征是：包括以下步骤：

步骤1：采用球体与胶囊体结合的包围盒方法对组合机械臂进行球体和胶囊体包围盒的模型包络，所述组合机械臂由核心舱机械臂和实验舱机械臂组合，并对包络模型进行数据采集；

步骤2：根据组合机械臂实际任务给定组合机械臂所需参数；

步骤3：根据各关节角度，进行正运动学变换，得到组合机械臂新的位姿，实现快速自身碰撞检测。

2.根据权利要求1所述的一种空间组合机械臂的快速自碰撞检测方法，其特征是：所述步骤1具体为：

采用球体与胶囊体结合的包围盒方法对组合机械臂进行球体和胶囊体包围盒的模型包络，并对包络模型进行数据采集并实时更新，所述组合机械臂包括末端执行器、关节延长件、各关节、臂杆、肘部相机、中央控制器和关节连接件，采用胶囊体包络臂杆，除臂杆外部件采用球体包络。

3.根据权利要求1所述的一种空间组合机械臂的快速自碰撞检测方法，其特征是：所述步骤2具体为：

根据组合机械臂实际任务给定组合机械臂所需参数，核心舱机械臂的末端负载为实验舱机械臂，给定核心舱机械臂的信息，为组合机械臂各杆件长度、各关节角度、基座适配器编号和肩部适配器编号信息；给定实验舱机械臂各杆件长度、各关节角度、末端负载状态，基座适配器编号和肩部适配器编号实现快速自身碰撞检测做准备。

4.根据权利要求1所述的一种空间组合机械臂的快速自碰撞检测方法，其特征是：所述步骤3具体为：

根据核心舱机械臂的各关节角度和实验舱机械臂各关节角度，进行正运动学变换，得到组合机械臂新的位姿，并以球体和胶囊体的模型数据更新，检测到碰撞后根据球体和胶囊体包络算法，计算组合机械臂自身部件之间的最短距离，以及距离最短的部件编号。

5.根据权利要求1所述的一种空间组合机械臂的快速自碰撞检测方法，其特征是：球体与球体的相交测试转化为求解两个球体的之间的距离，当球体间距离大于两个球体的半径和时，则不会发生碰撞，否则发生碰撞；

球体与胶囊体的相交测试转化为求解球心与胶囊体的轴线之间的最近距离，当轴线间距离大于球体和胶囊体半径和时，则不会发生碰撞，否则发生碰撞；

胶囊体与胶囊体的相交测试转化为求解两个胶囊体的轴线之间的最近距离，当胶囊体间距离大于两个胶囊体的半径和时，则不会发生碰撞，否则发生碰撞。