CN107609222A - 一种空间机器人末端接触碰撞力计算方法 - Google Patents

Abstract

本发明公开了一种空间机器人末端接触碰撞力计算方法，该方法包括如下步骤：将捕获装置及被捕获装置的机械设计三维CAD模型进行简化；对建立的捕获装置及被捕获装置的三角片元化模型进行特征几何要素的提取；根据捕获装置及被捕获装置的几何特征模型得到捕获装置及被捕获装置的碰撞点位置及嵌入深度量；建立捕获装置和被捕获装置的碰撞模型；通过碰撞嵌入量和碰撞嵌入速度得到碰撞力；通过碰撞点嵌入速度的切向速度、碰撞力和摩擦系数得到接触摩擦力；结合碰撞力和接触摩擦力得到碰撞力模型。本发明针对不规则的目标几何构型，提供了简化几何特征的高效碰撞检测方法，为机械臂关节承载以及捕获控制方案仿真验证工作提供支撑。

Description

一种空间机器人末端接触碰撞力计算方法

技术领域

本发明属于空间机器人在轨维修维护领域，涉及一种空间机器人末端接触碰撞力计算方法，适用于对空间故障卫星、大型碎片等空间非合作目标的抓捕操作。

背景技术

在空间机器人末端夹持工具对目标星箭对接环捕获过程中，夹持工具与星箭对接环将不可避免的发生碰撞冲击，而且两体在碰撞过程中呈现出滑动摩擦，刚性碰撞及分离等多种运动形式，使得捕获过程中的动力学行为较为复杂，同时也影响到捕获任务的成败。另一方面，直接的碰撞会导致机械臂或目标损坏，为时限高品质的操作控制，必须将接触力的信息引入控制闭环，使得接触力控制在允许范围之内，以完成柔顺捕获。现有利用地面试验获得两者的碰撞信息，一是试验的复杂程度大，二是试验的重复可操作性差。

发明内容

本发明所解决的问题是：克服现有技术不足，提供了一种空间机器人末端接触碰撞力计算方法，针对不规则的目标几何构型，提供了简化几何特征的高效碰撞检测方法，根据碰撞两体的材料特性及几何特征，并建立工程适用的碰撞力模型，进行碰撞力计算，为机械臂关节承载以及捕获控制方案仿真验证工作提供支撑。

本发明的技术解决方案是：一种空间机器人末端接触碰撞力计算方法，所述方法包括如下步骤：

步骤1：将捕获装置及被捕获装置的机械设计三维CAD模型进行简化，基于简化模型对捕获装置及被捕获装置的表面建立三角片元化模型；

步骤2：对建立的捕获装置及被捕获装置的三角片元化模型进行特征几何要素的提取，对捕获装置及被捕获装置发生碰撞区域的点、线、面特征标定及提取，建立能够用于快速碰撞检测的捕获装置及被捕获装置的几何特征模型；

步骤3：根据捕获装置及被捕获装置的几何特征模型，采用沿坐标轴的包围盒AABB碰撞检测算法对捕获装置和被捕获装置进行碰撞检测，从而得到捕获装置及被捕获装置的碰撞点位置及嵌入深度量；

步骤4：由捕获装置及被捕获装置的碰撞点位置及嵌入深度量，建立捕获装置和被捕获装置的碰撞模型；

步骤5：根据捕获装置和被捕获装置的碰撞模型，通过碰撞嵌入量和碰撞嵌入速度得到碰撞力；

步骤6：根据捕获装置及被捕获装置的碰撞模型，通过碰撞点嵌入速度的切向速度、碰撞力和摩擦系数得到接触摩擦力；

步骤7：结合步骤5中的碰撞力和步骤6中的接触摩擦力得到碰撞力模型。

上述空间机器人末端接触碰撞力计算方法中，在所述步骤1中，三角片元为根据一个表面上的相邻三个节点的坐标定义一个三角片。

上述空间机器人末端接触碰撞力计算方法中，在所述步骤2中，特征几何要素包括点、线和面。

上述空间机器人末端接触碰撞力计算方法中，在所述步骤3中，包围盒AABB碰撞检测算法包括：首先判断2棵树的根节点的包围盒是否相交，如果不相交，那么这2个物体肯定不发生碰撞；否则递归遍历2棵树，对相应的子节点进行求交判断；如果参与求交运算的2个子节点中至少有一个为非叶节点，则检测2节点的包围盒是否相交，如果不相交则不必再判断子节点；若2节点均为叶节点，且检测出它们的包围盒相交，则还要进一步判断这2个包围盒内所包含的多边形是否发生碰撞，如果的相交检测都判断完，没有发现相交的情况，则2个物体不碰撞；若检测到2个物体发生碰撞，则记录两个复杂机械结构的碰撞点位置及嵌入深度量。

上述空间机器人末端接触碰撞力计算方法中，在步骤5中，碰撞力的公式如下：

其中，F_c为碰撞力，K_c为接触碰撞刚度系数，n为接触点公法线方向，δ为沿接触点公法线方向n的碰撞嵌入量，C_c为接触碰撞阻尼系数，为接触点的碰撞嵌入速度。

上述空间机器人末端接触碰撞力计算方法中，在所述步骤6中，接触摩擦力的公式如下：

f_t＝μ(|v_t|)F_c，

其中，v_t为碰撞点嵌入速度的切向分量，f_t为接触摩擦力，μ为摩擦系数，F_c为碰撞力。

上述空间机器人末端接触碰撞力计算方法中，在所述步骤7中，得到碰撞力模型包括：

将碰撞力与接触摩擦力进行求和得到碰撞力的数学模型，即得到捕获装置与被捕获装置的碰撞力模型。

本发明与现有技术相比的优点在于：

(1)本发明的三角片元化模型有助于提高碰撞检测的效率；

(2)本发明将包围盒AABB碰撞检测算法应用于捕获装置和被捕获装置碰撞检测中，具有较好的紧密性，且计算简单；

(3)本发明建立碰撞力的数学模型，有助于对捕获装置和被捕获装置的碰撞行为进行重复性验证分析，同时也能够辅助验证空间机器人柔顺捕获控制算法。

附图说明

图1为本发明的空间机器人夹持过程示意图；

图2(a)为本发明的空间机器人末端夹持工具的示意图；

图2(b)为本发明的对接环的示意图；

图3为本发明的实体单元三角片元化定义的示意图；

图4为本发明的给定对象的轴向包围盒AABB的示意图；

图5为本发明的碰撞检测算法实现流程图；

图6为本发明的空载接触阶段夹持工具滑块与对接环简化碰撞模型示意图；

图7为本发明的匀速夹持阶段夹持工具上下夹手与对接环简化碰撞模型示意图；

图8为本发明的碰撞力计算示意图；

图9为采用本发明方法获得的滑块与对接环接触碰撞力曲线示意图；

图10为采用本发明方法获得的滑块与对接环位置误差曲线示意图；

图11为采用本发明方法获得的上下夹手与对接环接触碰撞力曲线示意图；

图12为采用本发明方法获得的上下夹手与对接环位置误差曲线示意图。

具体实施方式

下面结合附图对本发明作进一步详细说明：

夹持工具通过一个六自由度的空间机械臂与服务航天器的基体相连，通过空间机械臂的在轨运动实现定位与定姿，保证其能够与目标星的对接环接触并将对接环夹紧，夹持捕获时夹持工具与对接环的相对位姿如图1所示，图2(a)和图2(b)所示为夹持工具和对接环的CAD模型。本实施例中，捕获装置选择为夹持工具，被捕获装置选择为对接环。针对夹持对接环过程，设置材料特性、几何参数以及控制参数进行仿真。每个刚体质心在其几何中心，主要特性参数如下：

材料特性：碰撞刚度K_c＝5000N/m，碰撞阻尼C_c＝10Ns/m，摩擦系数μ＝0.2；

几何参数：上夹手楔角β₁＝27^°，上下夹手距离0.1m；

控制参数：引导速度系数阻尼速度系数

初始参数：基体位置及速度均为0，各关节速度均为0，下夹手夹合速度0.01m/s。

本实施例的具体步骤如下：

(1)将捕获装置及被捕获装置的机械设计三维CAD模型进行简化，基于简化模型对捕获装置及被捕获装置的表面建立三角片元化模型。

具体的，为了提高碰撞检测的效率，将捕获装置即夹持工具及被捕获装置即对接环的机械设计三维CAD模型进行简化，基于简化模型对零件的表面进行三角片元化，片元化方法如图3所示，用三角片元可以逼近实体的复杂几何体外形。

(2)对建立的捕获装置及被捕获装置的三角片元化模型进行特征几何要素的提取，对捕获装置及被捕获装置发生碰撞区域的点、线、面特征标定及提取，建立能够用于快速碰撞检测的捕获装置及被捕获装置的几何特征模型。

具体的，为了完成高效的碰撞检测，对三维结构模型进行特征几何要素的提取，对可能发生碰撞的区域的点、线、面等特征的精确标定及提取，建立能够用于快速碰撞检测的解析的几何特征模型，模型几何特征。特征几何要素包括点、线和面。

(3)根据捕获装置及被捕获装置的几何特征模型，采用沿坐标轴的包围盒AABB碰撞检测算法对捕获装置和被捕获装置进行碰撞检测，从而得到捕获装置及被捕获装置的碰撞点位置及嵌入深度量。

具体的，采用沿坐标轴的包围盒AABB算法对捕获机构和被捕获机构进行碰撞检测，AABB包围盒的示例如图4所示，从而得到两个复杂机械结构的碰撞点位置及嵌入深度量。碰撞检测算法步骤如图5所示，为：首先判断2棵树的根节点的包围盒是否相交，如果不相交，那么这2个物体肯定不发生碰撞；否则递归遍历2棵树，对相应的子节点进行求交判断。如果参与求交运算的2个子节点中至少有一个为非叶节点，则检测2节点的包围盒是否相交，如果不相交则不必再判断子节点；若2节点均为叶节点，且检测出它们的包围盒相交，则还要进一步判断这2个包围盒内所包含的多边形(一般为三角形)是否发生碰撞。如果的相交检测都判断完，没有发现相交的情况，则2个物体不碰撞。

(4)由捕获装置及被捕获装置的碰撞点位置及嵌入深度量，建立捕获装置和被捕获装置的碰撞模型。

具体的，建立简化的夹持工具和对接环的碰撞模型：

夹持工具和对接环的碰撞过程主要分二部分，空载接触阶段和匀速夹持阶段：

空载接触阶段：夹持工具在机械臂的控制下贴近对接环，机械臂保持，此时，检测夹持工具滑块与对接环的碰撞过程。

匀速夹持阶段：滑块贴近对接环后，下夹手向对接环运动，与对接环下部发生碰撞，然后对接环上部与上夹手碰撞，卡牢对接环，完成夹持过程。

a.空载接触阶段

此阶段为夹持工具滑块与对接环碰撞检测分析。夹持机构在机械臂末端带动下靠向对接环，接触对接环后停止，为了分析方便，将滑块和对接环碰撞部分简化为一个点，建立空载阶段碰撞模型，如图6所示。计算点对接环的侵入量，碰撞后即停止机械臂运动。

点点碰撞比较简单，滑块在对接环坐标系下的矢量为当的Z值小于0时，发生碰撞，反之则没有发生碰撞，嵌入量为Z的绝对值。

b.匀速夹持阶段

下夹手与对接环碰撞检测阶段与空载接触阶段的碰撞检测原理一致，区别是坐标系的变化。对接环与上卡槽碰撞检测阶段与锥-杆碰撞分析原理一致，区别是坐标系的变化，匀速夹持阶段的碰撞模型如图7所示。为在对接环坐标系下对接环到下夹手的矢量，为在上夹手坐标系下上夹手对接结束点到对接环的矢量。

(5)根据捕获装置和被捕获装置的碰撞模型，通过碰撞嵌入量和碰撞嵌入速度得到碰撞力。

(6)根据捕获装置及被捕获装置的碰撞模型，通过碰撞点嵌入速度的切向速度、碰撞力和摩擦系数得到接触摩擦力；

(7)结合步骤(5)中的碰撞力和步骤(6)中的接触摩擦力得到碰撞力模型。

具体的，碰撞力建模与计算包括如下：

a.法向碰撞力模型

对两个几何体进行碰撞检测，若发现几何体之间存在侵入，则返回侵入点个数以及每个碰撞点的坐标和法线方向，如图8所示，然后根据这些检测信息施加一个和侵入量成函数关系的抵抗力作用在发生侵入的点上，同时根据作用力和反作用力原理，也在另外一侧的面上施加一个大小相等方向相反的力，相当于在所有检测几何体之间放置法向弹簧。如果面是解析刚体表面的话，就将此力等效为力和弯矩施加到刚体质心上。图8中，δ为侵入深度，n为碰撞法向方向，t为碰撞切向方向，v_n为法向速度，v_t为切向速度，v_r为相对速度。

当进行抓取操作时，由于接触力作用，接触点处将发生局部变形，变形量δ为沿接触点公法线方向n的相互侵入量，采用建立在弹性理论基础上的赫兹接触力模型，将接触过程中存在的能量消耗视为材料阻尼引起，则碰撞力为：

其中，F_c为碰撞力，K_c为接触碰撞刚度系数，n为接触点公法线方向，δ为沿接触点公法线方向n的碰撞嵌入量，C_c为接触碰撞阻尼系数，δ为接触点的碰撞嵌入速度。

b.切向摩擦力模型

根据库仑摩擦，接触摩擦力f_t为：

f_t＝μ(|v_t|)F_c，

其中，v_t为碰撞点嵌入速度的切向分量，f_t为接触摩擦力，μ为摩擦系数，F_c为碰撞力。

v_t其定义为：

v_t＝v_r-(v_r·n)n，

v_r为对应在两个检测体上的碰撞点之间的相对速度。

摩擦系数μ由切向相对速度v_t决定：

其中v_s和v_d分别为静、动摩擦的临界速度，μ_s和μ_d分别为静、动摩擦系数，这些系数一般都由实验来测得。

上式中函数step(v_t,-v_s,-μ_s,v_s,μ_s)的定义为：

其中，a＝h₁-h₀，Δ＝(v_t-v_s)/(μ_s-v_s)。h₀为常实数，h₁为常实数，a为常实数，Δ为常实数。

(6)实例仿真分析

a.空载接触阶段

滑块底面离散化为9个点：(0.0030.0060.006),(0.00300.006),(0.003-0.0060.006),(00.0060.006),(00.00.006),(0-0.0060.006),(-0.0030.0060.006),(-0.0030.00.006),(-0.003-0.0060.006)，均在滑块坐标系下描述。

空载接触阶段碰撞力作用于机械臂末端和基体上，基体姿态采用PD控制，机械臂采用计算力矩控制。所得仿真结果如图9和图10所示。图9所示为空载接触阶段碰撞力大小仿真曲线，由图可知在仿真末阶段，碰撞力发生突变，表明夹持工具滑块与对接环发生了接触碰撞。图10所示为夹持工具滑块与对接环的相对位置误差曲线，由仿真结果知，Z方向位置误差成功降至0，在误差位置小于0后，产生碰撞力，随即机械臂保持现有状态不动，成功贴合到对接环上。

b.匀速夹持阶段

下夹手底面离散化为9个点：(-0.005 -0.005 -0.005),(-0.005 -0.005 0.00),(-0.005 -0.005 0.005),(-0.005 0.0 -0.005),(-0.005 0.0 0),(-0.005 0.0 0.005),(-0.005 0.005 -0.005),(-0.005 0.005 0.0),(-0.005 0.005 0.005)，均在下夹手坐标系下描述。

匀速夹持阶段碰撞力作用于机械臂末端和基体上，基体姿态采用PD控制，机械臂采用主动阻尼控制。所得仿真结果如图11和图12所示，图11所示为匀速夹持阶段碰撞力大小仿真曲线，由图可知在夹持过程中，碰撞力发生两次变化，表明在夹持过程中上下夹手分别与对接环发生了接触，最终完成对对接环的夹持。图12所示为夹持工具上下夹手与对接环的相对位置误差曲线，由仿真结果知，上下夹手方向矢量的X方向位置误差成功降至0，可以看出在2秒左右处，下夹手与对接环发生碰撞，在3.5秒左右处对接环与上夹手发生碰撞，各个曲线趋势一一对应，说明对接环夹持成功。

以上所述的实施例只是本发明较优选的具体实施方式，本领域的技术人员在本发明技术方案范围内进行的通常变化和替换都应包含在本发明的保护范围内。

Claims (8)

1.一种空间机器人末端接触碰撞力计算方法，其特征在于，所述方法包括如下步骤：

步骤1：将捕获装置及被捕获装置的机械设计三维CAD模型进行简化，基于简化模型分别对捕获装置及被捕获装置的表面建立三角片元化模型；

步骤2：对三角片元化模型进行特征几何要素的提取以及对捕获装置及被捕获装置发生碰撞区域的点、线、面特征标定及提取得到能够用于快速碰撞检测的捕获装置及被捕获装置的几何特征模型；

步骤3：根据捕获装置及被捕获装置的几何特征模型，采用碰撞检测算法对捕获装置和被捕获装置进行碰撞检测，得到捕获装置及被捕获装置的碰撞点位置及嵌入深度量；

步骤4：由捕获装置及被捕获装置的碰撞点位置及嵌入深度量，建立捕获装置和被捕获装置的碰撞模型；

步骤5：根据捕获装置和被捕获装置的碰撞模型，通过碰撞嵌入量和碰撞嵌入速度得到碰撞力；

步骤6：根据捕获装置及被捕获装置的碰撞模型，通过碰撞点嵌入速度的切向速度、碰撞力和摩擦系数得到接触摩擦力；

步骤7：结合步骤5中的碰撞力和步骤6中的接触摩擦力得到碰撞力模型。

2.根据权利要求1所述的空间机器人末端接触碰撞力计算方法，其特征在于：在所述步骤1中，三角片元为根据一个表面上的相邻三个节点的坐标定义一个三角片。

3.根据权利要求1所述的空间机器人末端接触碰撞力计算方法，其特征在于：在所述步骤2中，对三角片元化模型进行特征几何要素的提取包括对捕获装置的三角片元化模型进行点、线和面的提取和对被捕获装置的三角片元化模型进行点、线和面的提取。

4.根据权利要求1所述的空间机器人末端接触碰撞力计算方法，其特征在于：在所述步骤3中，碰撞检测算法为采用沿坐标轴的包围盒AABB碰撞检测算法。

5.根据权利要求1所述的空间机器人末端接触碰撞力计算方法，其特征在于：包围盒AABB碰撞检测算法包括：

首先判断2棵树的根节点的包围盒是否相交，如果不相交，那么这2个物体肯定不发生碰撞；否则递归遍历2棵树，对相应的子节点进行求交判断；

如果参与求交运算的2个子节点中至少有一个为非叶节点，则检测2节点的包围盒是否相交，如果不相交则不必再判断子节点；

若2节点均为叶节点，且检测出它们的包围盒相交，则还要进一步判断这2个包围盒内所包含的多边形是否发生碰撞，如果的相交检测都判断完，没有发现相交的情况，则2个物体不碰撞；

若检测到2个物体发生碰撞，则记录两个复杂机械结构的碰撞点位置及嵌入深度量。

6.根据权利要求1所述的空间机器人末端接触碰撞力计算方法，其特征在于：在步骤5中，碰撞力的公式如下：

<mrow> <msub> <mi>F</mi> <mi>c</mi> </msub> <mo>=</mo> <msub> <mi>K</mi> <mi>c</mi> </msub> <mi>&amp;delta;</mi> <mi>n</mi> <mo>+</mo> <msub> <mi>C</mi> <mi>c</mi> </msub> <mover> <mi>&amp;delta;</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>n</mi> <mo>,</mo> </mrow>

其中，F_c为碰撞力，K_c为接触碰撞刚度系数，n为接触点公法线方向，δ为沿接触点公法线方向n的碰撞嵌入量，C_c为接触碰撞阻尼系数，为接触点的碰撞嵌入速度。

7.根据权利要求1所述的空间机器人末端接触碰撞力计算方法，其特征在于：在所述步骤6中，接触摩擦力的公式如下：

f_t＝μ(|v_t|)F_c，

其中，v_t为碰撞点嵌入速度的切向分量，f_t为接触摩擦力，μ为摩擦系数，F_c为碰撞力。

8.根据权利要求1所述的空间机器人末端接触碰撞力计算方法，其特征在于：在所述步骤7中，得到碰撞力模型包括：

将碰撞力与接触摩擦力进行求和得到碰撞力的数学模型，即得到捕获装置与被捕获装置的碰撞力模型。