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

Abstract

本发明是一种空间机械臂的快速自碰撞检测方法。所述方法为采用球体包围盒方法对空间机械臂进行球体包围盒的模型包络，并对包络模型进行数据采集；根据空间机械臂实际任务给定空间机械臂所需参数；根据各关节角度，进行正运动学变换，得到空间机械臂新的位姿，采用两层碰撞检测方案实现快速自身碰撞检测。本发明能够保证在有限资源下针对空间机器人嵌入式系统能够实时有效检测自身碰撞。实际在对空间机械臂操作时，采用两层碰撞检测方案，保证快速自碰撞检测方法有效。

Description

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

技术领域

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

背景技术

空间站机械臂系统是完成空间站组装、维护维修、辅助运营管理、载荷照料和航天员在轨作业支持等操作的重要工具，它有助于延长空间站及载荷的使用寿命以及减少航天员出舱风险，从而获得更多的科研和经济回报，是空间站的重要组成部分。

为了保证空间站以及空间机械臂工作安全，根据中央控制器的碰撞检测模块，实现快速检测空间机械臂运动过程中与自身发生碰撞的目标。碰撞检测得到的信息可以直接在空间机械臂路径规划中使用，因此快速碰撞检测对空间机械臂路径规划是至关重要的。

包围盒碰撞检测方法有球体、圆柱体、K-DOP、OBB以及AABB等方法。对于AABB包围盒算法来讲，它被定义为包含该对象，且边平行于坐标轴的最小六面体。AABB构造比较简单，存储空间小，但紧密性差，尤其对不规则几何形体，冗余空间很大，当对象旋转时，无法对其进行相应的旋转。对于OBB包围盒来讲，它被称作有向包围盒或定向包围盒，会随着物体的移动、缩放、旋转。具有方向的任意性，这使得它可以根据被包围对象的形状特点尽可能紧密的包围对象，但同时也使得它的相交测试变得复杂。需要解决对象变形后OBB树的更新问题，而重新计算每个结点的OBB的代价又太大。对于K-DOP来讲，它是指定对象的凸包，它的面由一系列半空间决定的。各个面上的法向量在k个固定方向中选取，需要进行额外计算以确定如何选取包夹平面。对于圆柱体包围盒来讲，按端面圆是否参与相交划分二者的相对位置关系，它仅需要旋转柱体中心和顶面圆心这两个点，计算量较小，拥有AABB和OBB的特点，但是碰撞时存在尖角，并不太理想。对于球体来讲，它是比较简单的包围盒，而且当对象发生旋转运动时，包围球不需做任何更新，当几何对象进行频繁的旋转运动时，使用包围球体会得到很好的结果。

发明内容

本发明为保证空间机械臂工作安全，本发明提供了一种空间机械臂的快速自碰撞检测方法，本发明提供了以下技术方案：

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

步骤1：采用球体包围盒方法对空间机械臂进行球体包围盒的模型包络，并对包络模型进行数据采集；

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

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

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

采用球体包围盒方法对空间机械臂进行球体包围盒的模型包络，并对包络模型进行数据采集和实时更新，球体包络共分为两层，第一层为粗略球体包络，第二层为精细球体包络，其中，针对空间机械臂，粗略球体包络为最大可达空间的圆球；精细球体包络为数个球体对粗略球体包络中的每个元素进行包络。

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

根据空间机械臂实际任务的需要，给定空间机械臂各杆件长度、各关节角度、末端负载状态、基座适配器编号和肩部适配器编号信息，为空间机械臂实现快速自碰撞检测做准备。

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

利用粗略球体包络的第一层碰撞检测，判断在当前时刻空间机械臂自身是否与各部分发生碰撞，当空间机械臂自身是否与各部分发生碰撞是，则读取发生碰撞的部件编号，利用精细球体包络进行第二层碰撞检测，检测到碰撞后根据球体包络算法，确定空间机械臂自身部件之间的最短距离，以及距离最短的部件编号。

优选地，忽略关节的柔性效应，将机械臂视为刚体，采用D-H方法进行建模，根据空间机械臂的各杆件参数确定齐次变换矩阵，通过下式表示齐次变换矩阵：

其中，

为齐次变换矩阵，a_i-1为各关节连杆长度，d_i为连杆偏距，θ_i为关节角，i为空间机械臂关节数，空间机械臂关节数范围为1至7。

确定空间机械臂末端坐标系相对于基座坐标系的齐次变换矩阵，通过下式表示机械臂末端坐标系相对于基座坐标系的齐次变换矩阵为：

其中，

为机械臂末端坐标系相对于基座坐标系的齐次变换矩阵，

为机械臂关节1坐标系相对于基座坐标系的齐次变换矩阵，

为机械臂关节2坐标系相对于关节1坐标系的齐次变换矩阵，

为机械臂关节3坐标系相对于关节2坐标系的齐次变换矩阵，

为机械臂关节4坐标系相对于关节3坐标系的齐次变换矩阵，

为机械臂关节5坐标系相对于关节4坐标系的齐次变换矩阵，

为机械臂关节6坐标系相对于关节5坐标系的齐次变换矩阵，

为机械臂关节7坐标系相对于关节6坐标系的齐次变换矩阵，R_3×3为机械臂末端坐标系相对于基座坐标系的旋转变换，P_3×1为机械臂末端坐标系相对于基座坐标系的移动变换；

根据机械臂末端坐标系相对于基座坐标系的齐次变换矩阵，在给定各个关节角度值的条件下，确定机械臂末端的位置和姿态。

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

本发明能够保证在有限资源下针对空间机器人嵌入式系统能够实时有效检测自身碰撞。实际在对空间机械臂操作时，采用两层碰撞检测方案，保证快速自碰撞检测方法有效。

附图说明

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

图2为空间机械臂本体球包络模型图；

图3为球体与球体最近距离示意图；

图4为空间机械臂D-H坐标系示意图。

具体实施方式

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

具体实施例一：

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

步骤1：采用球体包围盒方法对空间机械臂进行球体包围盒的模型包络，并对包络模型进行数据采集；

采用球体包围盒方法对空间机械臂进行球体包围盒的模型包络，并对包络模型进行数据采集和实时更新，球体包络共分为两层，第一层为粗略球体包络，第二层为精细球体包络，其中，针对空间机械臂，粗略球体包络为最大可达空间的圆球；精细球体包络为数个球体对粗略球体包络中的每个元素进行包络。

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

根据空间机械臂实际任务的需要，给定空间机械臂各杆件长度、各关节角度、末端负载状态、基座适配器编号和肩部适配器编号信息，为空间机械臂实现快速自碰撞检测做准备。

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

所述步骤3具体为：

利用粗略球体包络的第一层碰撞检测，判断在当前时刻空间机械臂自身是否与各部分发生碰撞，当空间机械臂自身是否与各部分发生碰撞是，则读取发生碰撞的部件编号，利用精细球体包络进行第二层碰撞检测，检测到碰撞后根据球体包络算法，确定空间机械臂自身部件之间的最短距离，以及距离最短的部件编号。

忽略关节的柔性效应，将机械臂视为刚体，采用D-H方法进行建模，根据空间机械臂的各杆件参数确定齐次变换矩阵，通过下式表示齐次变换矩阵：

其中，

为齐次变换矩阵，a_i-1为各关节连杆长度，d_i为连杆偏距，θ_i为关节角，i为空间机械臂关节数，从1～7。

确定空间机械臂末端坐标系相对于基座坐标系的齐次变换矩阵，通过下式表示机械臂末端坐标系相对于基座坐标系的齐次变换矩阵为：

其中，

为机械臂末端坐标系相对于基座坐标系的齐次变换矩阵，

为机械臂关节1坐标系相对于基座坐标系的齐次变换矩阵，

为机械臂关节2坐标系相对于关节1坐标系的齐次变换矩阵，

为机械臂关节3坐标系相对于关节2坐标系的齐次变换矩阵，

为机械臂关节4坐标系相对于关节3坐标系的齐次变换矩阵，

为机械臂关节5坐标系相对于关节4坐标系的齐次变换矩阵，

为机械臂关节6坐标系相对于关节5坐标系的齐次变换矩阵，

为机械臂关节7(也是末端坐标系)坐标系相对于关节6坐标系的齐次变换矩阵，R_3×3为机械臂末端坐标系相对于基座坐标系的旋转变换，P_3×1为机械臂末端坐标系相对于基座坐标系的移动变换；

根据机械臂末端坐标系相对于基座坐标系的齐次变换矩阵，在给定各个关节角度值的条件下，确定机械臂末端的位置和姿态。

具体实施例2：

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

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

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

struct Sphere

{

Point c；//球心

float r；//半径

}

该结构只包含4个分量，因而包围球是一类节省内存的包围体。通常情况下，物体对象的中心或原点与球心基本相符，因此只需存储上述结构中的半径分量。

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

球体包络共分为两层：第一层为粗略球体包络，第二层为精细球体包络。其中，针对空间机械臂，粗略球体包络为其最大可达空间的圆球；

精细球体包络为用数个球体对粗略球体包络中的每个元素进行包络，包络更加精细，更能反应每个元素的真实情况。

步骤2、根据空间机械臂实际任务给定空间机械臂所需各相关参数。

根据空间机械臂实际任务的需要，给出空间机械臂各杆件长度、各关节角度、末端负载状态、基座适配器编号、肩部适配器编号等信息，为空间机械臂实现快速自碰撞检测做准备。

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

该步骤为利用粗略球体包络第一层碰撞检测，以判断在当前时刻，空间机械臂自身是否有与各部分发生碰撞的可能，若有，则读取可能发生碰撞的部件编号；然后，利用精细球体包络进行第二层碰撞检测，该碰撞检测只检测第一步中获得的可能与空间机械臂发生碰撞的部件，检测到碰撞后根据球体包络算法，计算空间机械臂自身部件之间的最短距离，以及距离最短的部件编号。

根据图4所示，本发明中的空间机械臂为实验舱机械臂(小臂)，是7自由度冗余机械臂。它由七个关节，两个末端执行器和两个臂杆组成。机械臂配置为R⊥R⊥R∥R∥R⊥R⊥R，其中R表示旋转关节，⊥表示两个相邻关节的轴相互垂直，∥表示相邻两个关节的轴相互平行。机械臂的两个末端作用器一个作为基座连接空间站，另一个用于执行抓捕操作，两者功能可以复用，即可使机械臂具有“爬行”的能力，增加其操作空间。另外，机械臂的冗余构型不仅能够提高灵活性，也提高了其应对突发故障的能力，例如在某关节发生故障无法运动时，其余关节依然能够确保完成三维空间中的任务。

忽略关节的柔性效应，将机械臂视为刚体，采用改进的D-H方法进行建模。对应的坐标系如图4所示。根据机械臂的各杆件参数得到齐次变换矩阵为：

对于空间机械臂，在不同的构型下，其各关节连杆长度a_i-1、连杆转角α_i-1和连杆偏距d_i均为定值，只有关节角θ_i为变量。这样就可得到机械臂的各关节坐标系相对于前一坐标系的齐次变换矩阵。

则机械臂末端坐标系相对于基座坐标系的齐次变换矩阵为：

上式即为机械臂的正运动学模型，在给定各个关节角度值的条件下，可以唯一确定机械臂末端的位置和姿态。

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

Claims (5)

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

步骤1：采用球体包围盒方法对空间机械臂进行球体包围盒的模型包络，并对包络模型进行数据采集；

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

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

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

采用球体包围盒方法对空间机械臂进行球体包围盒的模型包络，并对包络模型进行数据采集和实时更新，球体包络共分为两层，第一层为粗略球体包络，第二层为精细球体包络，其中，针对空间机械臂，粗略球体包络为最大可达空间的圆球；精细球体包络为数个球体对粗略球体包络中的每个元素进行包络。

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

根据空间机械臂实际任务的需要，给定空间机械臂各杆件长度、各关节角度、末端负载状态、基座适配器编号和肩部适配器编号信息，为空间机械臂实现快速自碰撞检测做准备。

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

利用粗略球体包络的第一层碰撞检测，判断在当前时刻空间机械臂自身是否与各部分发生碰撞，当空间机械臂自身是否与各部分发生碰撞是，则读取发生碰撞的部件编号，利用精细球体包络进行第二层碰撞检测，检测到碰撞后根据球体包络算法，确定空间机械臂自身部件之间的最短距离，以及距离最短的部件编号。

5.根据权利要求1所述的一种空间机械臂的快速自碰撞检测方法，其特征是：忽略关节的柔性效应，将机械臂视为刚体，采用D-H方法进行建模，根据空间机械臂的各杆件参数确定齐次变换矩阵，通过下式表示齐次变换矩阵：

其中，

为齐次变换矩阵，a_i-1为各关节连杆长度，d_i为连杆偏距，θ_i为关节角，i为空间机械臂关节数，空间机械臂关节数范围为1至7；

确定空间机械臂末端坐标系相对于基座坐标系的齐次变换矩阵，通过下式表示机械臂末端坐标系相对于基座坐标系的齐次变换矩阵为：

其中，

为机械臂末端坐标系相对于基座坐标系的齐次变换矩阵，

为机械臂关节1坐标系相对于基座坐标系的齐次变换矩阵，

为机械臂关节2坐标系相对于关节1坐标系的齐次变换矩阵，

为机械臂关节3坐标系相对于关节2坐标系的齐次变换矩阵，

为机械臂关节4坐标系相对于关节3坐标系的齐次变换矩阵，

为机械臂关节5坐标系相对于关节4坐标系的齐次变换矩阵，

为机械臂关节6坐标系相对于关节5坐标系的齐次变换矩阵，

为机械臂关节7坐标系相对于关节6坐标系的齐次变换矩阵，R_3×3为机械臂末端坐标系相对于基座坐标系的旋转变换，P_3×1为机械臂末端坐标系相对于基座坐标系的移动变换；

根据机械臂末端坐标系相对于基座坐标系的齐次变换矩阵，在给定各个关节角度值的条件下，确定机械臂末端的位置和姿态。

Images