CN111546376A - 一种空间组合机械臂的快速自碰撞检测方法 - Google Patents

Abstract

本发明是一种空间组合机械臂的快速自碰撞检测方法。所述方法为采用球体与胶囊体结合的包围盒方法对组合机械臂进行球体和胶囊体包围盒的模型包络，所述组合机械臂由核心舱机械臂和实验舱机械臂组合，并对包络模型进行数据采集；根据组合机械臂实际任务给定组合机械臂所需参数；根据各关节角度，进行正运动学变换，得到组合机械臂新的位姿，实现快速自身碰撞检。本发明能够保证在有限资源下针对组合机械臂嵌入式系统能够实时有效检测自身碰撞。实际在对组合空间机械臂操作时，改进的碰撞算法能极大地缩短检测时间，提高了快速自身碰撞检测方法的实时性。

Description

一种空间组合机械臂的快速自碰撞检测方法

技术领域

本发明涉及空间组合机械臂遥操作技术领域，是一种空间组合机械臂的快速自碰撞检测方法。

背景技术

随着空间技术的飞速发展，特别是空间站、航天飞机、空间机器人等的诞生及成功应用，空间机械臂作为在轨支持、服务的一项关键性技术已经进入太空,并越来越受到人们的关注。我国空间站机械臂系统由核心舱机械臂和实验舱机械臂构成，核心舱机械臂和实验舱机械臂可以双臂组合，从而可以扩大工作空间实现更大范围作业。在空间组合机械臂状态下，实验舱机械臂在核心舱机械臂的支持下精确定位、操作，跨舱段转移自身基座位置的能力。

目前，碰撞检测领域的主要问题是实时性与准确性之间的矛盾。影响精确性的主要因素是场景中模型的精确程度，模型越精细，仿真出来的效果越好，但相应地数据量也会增大，导致检测时间增加、实时性变差。本发明主要研究组合机械臂的快速自碰撞检测方法，在保证准确性的前提下提高实时性。

由于现有的碰撞检测方法存在实时性与准确性之间的矛盾，所以本发明将针对组合机械臂自身碰撞，研究最适合的碰撞检测方法。最终决定采用包围盒技术实现组合机械臂的快速自身碰撞检测。为了保证组合机械臂的工作安全，根据中央控制器的碰撞检测模块，实现防止组合机械臂运动过程中与自身发生碰撞的目标。

目前针对空间机械臂的快速碰撞方法有球体、圆柱体、K-DOP、OBB以及AABB等方法。无论圆柱体、K-DOP、OBB还是AABB包围盒方法都各自有缺点，并且与空间机械臂碰撞检测要求不相符。球体包围盒相对简单且旋转时不需更新。但针对空间机器人嵌入式系统的在轨有限资源约束与实时性要求，单纯采用球体包围盒对组合机械臂进行碰撞检测时包围球太多，计算量太大，不能满足实时性要求，因此在此基础上发明了基于球体与胶囊体结合的组合机械臂快速碰撞检测方法。

发明内容

本发明为解决现有的碰撞检测方法存在实时性与准确性之间的矛盾，本发明提供了一种空间组合机械臂的快速自碰撞检测方法，本发明提供了以下技术方案：

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

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

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

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

优选地，所述步骤1具体为：

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

优选地，所述步骤2具体为：

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

优选地，所述步骤3具体为：

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

优选地，球体与球体的相交测试转化为求解两个球体的之间的距离，当球体间距离大于两个球体的半径和时，则不会发生碰撞，否则发生碰撞；

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

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

本发明具有以下有益效果：

本发明能够保证在有限资源下针对组合机械臂嵌入式系统能够实时有效检测自身碰撞。实际在对组合空间机械臂操作时，改进的碰撞算法能极大地缩短检测时间，提高了快速自身碰撞检测方法的实时性。

附图说明

图1为空间组合机械臂的快速自碰撞检测方法流程图；

图2为组合机械臂模型图；

图3为实验舱机械臂的球与胶囊包络图；

图4为球-球、球-胶囊和胶囊-胶囊最近距离示意图。

具体实施方式

以下结合具体实施例，对本发明进行了详细说明。

具体实施例一：

根据图1所示，本发明提供一种空间组合机械臂的快速自碰撞检测方法，包括以下步骤：

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

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

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

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

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

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

优选地，球体与球体的相交测试转化为求解两个球体的之间的距离，当球体间距离大于两个球体的半径和时，则不会发生碰撞，否则发生碰撞；

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

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

两线段之间最近点可以通过两直线之间的最近点来求取，一种常见的错误是将外部最近点截取为距其最近的线段端点。大体分为以下三种情况分析：

(1)两直线之间的最近点恰好位于对应两线段内部，则可直接利用两直线最近点算法求解两线段之间最近点。

(2)两直线之间某一最近点位于相关线段的外部延长线上，该点是可以截取至相应线段的最近端点处的。利用点到线段的最近点算法求解两线段之间最近点。

(3)两直线间最近点皆位于各自线段的外部延长线上，则上述截取操作需要重复计算2 次。分别考虑各线段上的最近点情况，利用点到线段的最近点算法求解两线段之间最近点，二者进行比较，得出最近距离。

具体实施例2：

本实施方法所述的是一种针对空间机械臂嵌入式系统在有限资源下的面向组合机械臂的快速自碰撞检测方法。该方法能够保证组合机械臂实现快速自身碰撞检测的要求。包括如下步骤：

步骤1、采用球体与胶囊体结合的包围盒技术对组合机械臂进行球体和胶囊体包围盒的模型包络，并对包络模型进行数据采集和实时更新，如图2所示。

球体是一类比较常用的包围体。球体具备快速相交测试的这一特征，同时，球体基本不受旋转变换的影响，只需简单地平移至一个新位置。球体按照球心和半径的方式加以定义：

struct Sphere

{

Point c；//球心

float r；//半径

}

胶囊体是球体的一种变化形式，可看成半径为R的球体沿AB线段扫掠，形成最终包围体 -胶囊体。胶囊体同样按照球心和半径的方式加以定义：

结合图3简要说明包络情况。本图为实验舱机械臂球与胶囊包络图，其中配置为R⊥R⊥ R∥R∥R⊥R⊥R，其中R表示旋转关节，⊥表示两个相邻关节的轴相互垂直，∥表示相邻两个关节的轴相互平行，因此为对称构型。本图中标号代表球体包络模型和胶囊体包络模型，标号7和12为胶囊体包络模型，其余所有标号为球体包络模型。包括末端执行器、关节延长件、各关节、臂杆、肘部相机、中央控制器和关节连接件等。臂杆细长，采用胶囊体包络，其他部件采用球体包络。核心舱机械臂的包络同理。

步骤2、根据组合机械臂实际任务给定组合机械臂所需各相关参数。下述为所需参数的具体说明

组合机械臂是空间站机械臂系统工作十分重要的一环，主要指实验舱机械臂俗称小臂) 和核心舱机械臂(俗称大臂)协同工作完成任务。根据组合机械臂实际任务的需要，这要求核心舱机械臂的末端负载为实验舱机械臂，给出核心舱机械臂(俗称大臂)各杆件长度、各关节角度、基座适配器编号、肩部适配器编号等信息，给出实验舱机械臂(俗称小臂)各杆件长度、各关节角度、末端负载状态，基座适配器编号、肩部适配器编号等信息，为组合机械臂实现快速自身碰撞检测做出准备。

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

该步骤在进行了正运动学变换后，得到组合机械臂新的位姿，并以球体和胶囊体的模型数据更新，检测到碰撞后根据球体和胶囊体包络算法，计算组合机械臂自身部件之间的最短距离，以及距离最短的部件编号。

结合图4简单说明球-球算法检测过程。球体与球体的相交测试可转化为求球心A与球心 B的之间的距离。若其距离大于二者半径和，则不会发生碰撞，否则发生碰撞。可以采用距离的平方值进行相关计算，从而避免计算成本较高的平方根操作。

结合图4简单说明球-胶囊算法检测过程。球体与胶囊体的相交测试可转化为求球心C与胶囊体的轴线AB之间的最近距离。若其距离大于二者半径和，则不会发生碰撞，否则发生碰撞。可以采用距离的平方值进行相关计算，从而避免计算成本较高的平方根操作。一点C与线段AB之间的(平方)距离可以直接获取，无须显示计算线段AB上距离C的最近点。

我们将点C投影到AB上，如果投影点位于线段内，则此点到C点的距离即为最近距离；如果投影点位于线段AB的延长线上，则线段上的近C端点到C点的距离即为最近距离。

结合图4简单说明胶囊-胶囊算法检测过程。胶囊体与胶囊体的相交测试可转化为求胶囊体AB与胶囊体CD的两轴线之间的最近距离。若其距离大于二者半径和，则不会发生碰撞，否则发生碰撞。可以采用距离的平方值进行相关计算，从而避免计算成本较高的平方根操作。

两线段之间最近点可以通过两直线之间的最近点来求取，一种常见的错误是将外部最近点截取为距其最近的线段端点。大体分为以下三种情况分析：

(1)两直线之间的最近点恰好位于对应两线段内部，则可直接利用两直线最近点算法求解两线段之间最近点。

(2)两直线之间某一最近点位于相关线段的外部延长线上，该点是可以截取至相应线段的最近端点处的。利用点到线段的最近点算法求解两线段之间最近点。

(3)两直线间最近点皆位于各自线段的外部延长线上，则上述截取操作需要重复计算2 次。分别考虑各线段上的最近点情况，利用点到线段的最近点算法求解两线段之间最近点，二者进行比较，得出最近距离。

以上所述仅是一种空间组合机械臂的快速自碰撞检测方法的优选实施方式，一种空间组合机械臂的快速自碰撞检测方法的保护范围并不仅局限于上述实施例，凡属于该思路下的技术方案均属于本发明的保护范围。应当指出，对于本领域的技术人员来说，在不脱离本发明原理前提下的若干改进和变化，这些改进和变化也应视为本发明的保护范围。

Claims (5)

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

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

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

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

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

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

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

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

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

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

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

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

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

Images