一种基于多指机构的三维空间翻滚目标包络抓捕方法与流程

本发明属于航空航天领域，涉及用于三维空间翻滚目标的抓捕方案，具体涉及一种基于多指机构的三维空间翻滚目标包络抓捕方法。

背景技术：

伴随着人类对空间资源的不断探索与利用，由故障失效卫星、各种爆炸以及碎片等各种原因造成的大量空间垃圾，不仅占据着宝贵的轨道资源，还对正常在轨运行的航天器的安全造成了严重威胁。空间碎片大多是非合作目标，由于目标已失去姿态调整能力，且长期处于失控状态运行，受太阳光压、重力梯度等摄动力矩及失效前自身残余角动量的影响，往往会出现复杂的旋转运动，乃至最终趋向于自由翻滚运动，使得其抓捕极具挑战性。此外，通常这类目标还不具有专门用于抓捕连接的借口和合作性的测量装置等，这无疑进一步增加了空间翻滚目标的难度。

技术实现要素：

本发明的目的在于针对上述现有技术中的问题，提供一种基于多指机构的三维空间翻滚目标包络抓捕方法。本方法在对空间翻滚目标进行抓捕时，具有不需要抓捕点、不需要精确的目标信息、对目标几何外形具有更好的兼容性以及仅通过简单的位置控制就能够实现对目标运动约束的优点。

为了实现上述目的，本发明提供了如下的技术方案。

一种基于多指机构的三维空间翻滚目标包络抓捕方法，包括以下步骤：

s100，根据多指机构的各个手指的基关节之间的相对位置关系以及各个手指自身的构型描述，建立多指机构的结构描述模型，

s200，根据多指机构能够约束住三维空间翻滚目标运动时，多指机构和三维空间翻滚目标之间应当满足的条件，推导多指机构的有效包络条件；

s300，根据多指机构包络算法，寻找出满足包络条件的有效多指机构构型；

所述多指机构包络算法具体包括：

s301，将多指机构的手指划分为一根主手指和剩余从手指两类；

s302，控制主手指基关节追踪包络点实现运动同步；

s303，通过调整主手指的关节角取值，使得主手指构型与包络边匹配；

s304，利用s100中的多指机构的结构描述模型，确定从手指构型；

s305，利用s200中的多指机构的有效包络条件，确定有效包络构型。

作为本发明的进一步改进，s100中，所述多指机构通过机械臂和服务卫星连接；多指机构包括手掌和n个手指，其中每个手指由m个连杆和m个单自由度的旋转关节组成。

作为本发明的进一步改进，第i个手指的基关节手指位置pb_i和第j个手指的基关节手指位置pb_j之间满足如下关系：

pb_j＝pb_i+δij(j≠i,i≤n,j≤n)(2)

其中，δij是由多指机构自身结构决定的第i个手指的基关节手指位置pb_i和第j个手指的基关节手指位置pb_j之间的相对矢量关系。

作为本发明的进一步改进，s100中，根据给定任意一个手指基关节的空间位置，结合基关节间的相对位置关系，确定其它手指基关节的位置；再根据各个手指自身的关节角范围，确定各个手指自身可能的构型，进而确定整个多指机构的可能构型。

作为本发明的进一步改进，s200中，多指机构的有效包络条件表示为：

g(θ):＝max(di,i→i+1,di,k→k+1,{di,j})-lcl<0

其中，i,j＝1,2,…,n；i≠j；k＝1,2,…,m-1；其中，di→i+1,k表示线段ji,kji,k+1和线段ji+1,kji+1,k+1之间的距离；di,k→k+1表示线段ji,kji+1,k和线段ji,k+1ji+1,k+1之间的距离。

作为本发明的进一步改进，s300中包络点选择为三维空间翻滚目标附近某一距离范围内且相对于空间翻滚目标静止的点；包络边选择为三维空间翻滚目标某一面上的一条线。

作为本发明的进一步改进，s300中主手指基关节追踪包络点是通过模型预测控制实现的。

作为本发明的进一步改进，s300中主手指构型与包络边匹配是通过快速搜索随机树算法寻找使得主手指构型与包络边间单向距离最小的主手指关节角。

与现有技术相比，本发明具有以下有益效果：

本方法利用包络抓捕自身的特性—仅通过位置控制实现对目标的运动约束以及不需要考虑诸如接触等力学特性。一方面，“主—从”式的多指机构包络算法具有处理三维空间翻滚目标动态性的能力，而且还简化了系统自由度进而提升了计算效率；另一方面，具有不需要抓捕点、不需要精确的目标信息、对目标几何外形具有更好的兼容性以及仅通过简单的位置控制就能够实现对目标运动约束的优点。在对空间翻滚目标进行抓捕时，具有不需要抓捕点、不需要精确的目标信息、对目标几何外形具有更好的兼容性以及仅通过简单的位置控制就能够实现对目标运动约束的优点。

附图说明

在此描述的附图仅用于解释目的，而不意图以任何方式来限制本发明公开的范围。另外，图中的各部件的形状和比例尺寸等仅为示意性的，用于帮助对本发明的理解，并不是具体限定本发明各部件的形状和比例尺寸。本领域的技术人员在本发明的教导下，可以根据具体情况选择各种可能的形状和比例尺寸来实施本发明。在附图中：

图1是本发明多指机构示意图；

图2是多指机构包络三维空间翻滚目标的可能情形示意图；

图3是多指手相邻手指以及指尖距离计算说明图；

图4是主手指可能构型说明图；

图5是包络边匹配算法；

图6是多指机构基关节位置关系图；

图7是主手指基关节追踪包络点结果图；

图8是主手指与包络边匹配结果图；

图9多指机构有效构型图。

其中，1、服务卫星(基座)；2、机械臂；3、手掌；4、手指；5、关节；6、连杆；7、多指关节。

具体实施方式

为了使本技术领域的人员更好地理解本发明中的技术方案，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都应当属于本发明保护的范围。

除非另有定义，本文所使用的所有的技术和科学术语与属于本发明的技术领域的技术人员通常理解的含义相同。本文中在本发明的说明书中所使用的术语只是为了描述具体的实施例的目的，不是旨在于限制本发明。本文所使用的术语“和/或”包括一个或多个相关的所列项目的任意的和所有的组合。

本发明一种基于多指机构的三维空间翻滚目标包络抓捕方法，包括以下步骤：

s100，建立多指机构的结构描述模型；

其中，一旦给定任意一个手指基关节的空间位置，根据基关节间的相对位置关系，就可以确定其它手指基关节的位置；然后，再根据各个手指自身的关节角范围，就可以确定各个手指自身可能的构型，进而可以确定整个多指机构的可能构型。

s200，推导多指机构的有效包络条件；

其中，多指机构的有效包络条件是指多指机构能够约束住三维空间翻滚目标运动时，多指机构和三维空间翻滚目标之间应当满足的条件。

s300，设计“主—从”式的多指机构包络算法，寻找出满足包络条件的有效多指机构构型。

其中，多指机构的结构描述模型给定了多指机构的各个手指的基关节之间的相对位置关系以及各个手指自身的构型描述。

“主—从”式的多指机构包络算法的基本思想是：考虑到多指机构的高自由度问题以及多指机构和三维空间翻滚目标之间存在相对运动，首先，将多指机构的手指划分为一根主手指和剩余从手指两类；其次，控制主手指基关节追踪包络点实现运动同步；接着，通过调整主手指的关节角取值，使得主手指构型与包络边匹配；然后，利用s100中的多指机构的结构描述模型，确定从手指构型；最后，利用s200中的多指机构的有效包络条件，确定有效包络构型。

进一步地，s300中主手指的选择是随机的，包络点选择为三维空间翻滚目标附近某一距离范围内且相对于空间翻滚目标静止的点。主手指基关节追踪包络点是通过模型预测控制实现的。包络边选择为三维空间翻滚目标某一面上的一条线。主手指构型与包络边匹配是通过快速搜索随机树算法寻找使得主手指构型与包络边间单向距离最小的主手指关节角。

下面结合附图以及具体实施例对本发明进行详细说明。

实施例

一种基于多指机构的三维空间翻滚目标包络抓捕方法，其具体步骤包括：

s100，建立多指机构的结构描述模型。

首先，确定多指机构的各个手指4的基关节之间的相对位置。如图1所示，多指机构通过机械臂2和服务卫星连接。多指机构由手掌3和n个手指4组成，其中每个手指4还由m个连杆6和m个单自由度的旋转关节5组成。受多指机构的机构约束，第i个手指4的基关节手指位置pb_i和第j个手指4的基关节手指位置pb_j之间满足如下关系：

pb_j＝pb_i+δij(j≠i,i≤n,j≤n)(3)

其中，δij是由多指机构自身结构决定的第i个手指4的基关节手指位置pb_i和第j个手指4的基关节手指位置pb_j之间的相对矢量关系。

然后，确定各个手指4自身的可能构型。对于第i(i＝1,2,…,n)个手指4的构型si，它是由手指的关节角θi＝[θi,1,θi,2,…,θi,m]^t决定的：

si＝f(θi)＝f(θi,1,θi,2,…,θi,m)(i＝1,2,…,n)(4)

其中，f(θi)表示si和θi之间的函数关系。

所以，一旦给定任意一个手指4基关节的空间位置，根据基关节间的相对位置关系，就可以确定其它手指基关节的位置；然后，再根据各个手指4自身的关节角范围，就可以确定各个手指4自身可能的构型，进而可以确定整个多指机构的可能构型。

进一步地，手指的关节角转动范围是受限制的，假设这里和分别第i个手指4的第k个关节可以转动范围的上下界。

s200，推导多指机构的有效包络条件。多指机构有效包络住三维空间翻滚目标意味着三维空间翻滚目标不能从多指机构的约束中逃离。具体来说，就是不能从多指机构相邻手指的间隙以及各个手指4的指尖间隙中逃离。

进一步地，三维空间翻滚目标处于自由漂浮状态，这意味着多指机构可以从任意方向来包络三维空间翻滚目标，如图2所示。当采取不同的包络方向时，三维空间翻滚目标和多指机构间的相对姿态存在很大的差异，这会造成有效包络条件推导的复杂性和高难度。不是一般性，我们假设在包络过程中，多指机构的手掌3始终平行于三维空间翻滚目标的一个平面(图(c))，这可以通过调整多指机构的姿态实现。

首先，计算第i个手指4和第(i+1)个手指4连杆6间的间隙，如图3中(a)所示，由第i个手指4和第(i+1)个手指4的第k个连杆6组成的图形为空间四边形ji,kji,k+1ji+1,k+1ji+1,k(i＝1,2,…,n；n+1＝1；k＝1,2,…,m；m+1＝e,e表示指尖)。如果ji,kji,k+1ji+1,k+1ji+1,k对边间的距离小于三维空间翻滚目标相应的特征长度lcl，那么三维空间翻滚目标则不能从第i个手指4和第(i+1)个手指4的间隙中逃离：

其中，di→i+1,k表示线段ji,kji,k+1和线段ji+1,kji+1,k+1之间的距离；di,k→k+1表示线段ji,kji+1,k和线段ji,k+1ji+1,k+1之间的距离。

接着，计算各个手指4的指尖之间的距离。如图3(b)所示，多指机构的各个手指4的指尖形成了一个空间n边形，第i个手指4的指尖ei和第j个手指4的指尖ej(i,j＝1,2,…,n；i≠j)之间的距离di,j为：

其中，d(·,·)表示空间中任意两点间距离计算的函数，本专利中采用欧几里得距离，此时p＝2。为了保证三维空间翻滚目标不从指尖的间隙中逃离，应当满足如下关系：

进一步地，结合方程(5)和(7)，多指机构的有效包络条件可以表示为：

其中，公式(8)给出了多指机构能够有效包络住三维空间翻滚目标时，多指机构手指的各个关节角应当满足的条件。

s300，设计“主—从”式的多指机构包络算法，寻找出满足包络条件的有效多指机构构型。考虑到多指机构的高自由度问题以及多指机构和三维空间翻滚目标之间存在相对运动，将多指机构的手指划分为一根主手指和剩余从手指这两种类型，提出了“主—从”式的多指机构包络算法。其中，主手指的选择是随机的。

首先，基于模型预测控制，实现主手指基关节和包络点的运动同步。其中，包络点选择为三维空间翻滚目标附近某一距离范围内且相对于空间翻滚目标静止的点。服务航天器上任意一点和三维空间翻滚目标上任意一点间以采样周期为t的离散形式的相对运动方程可以表示为：

x(k+1)＝atx(k)+btu(k)+ctγ(k)(9)

其中，x(k)和u(k)分别表示采样时间k∈z⁺时的状态和控制矢量；γ(k)是扰动变量。

进一步地，输出变量可以表示为：

令和分别表示状态变量集、输出变量集、控制输入集和常矢量集，它们的表达式如下：

其中，np和nc分别表示预测时域和控制时域；x(k+j|k)是基于x(k)预测的时刻k+j的状态量。

进一步地，基于方程(9)和(10)，有

其中，as、bs、cs和ds的定义如下：

进一步地，在推导离散形式的相对运动方程时，采用了控制力矩和外界扰动力矩均为0的假设。此外，假设服务航天器具有实现与三维空间翻滚目标姿态运动同步的能力，即服务航天器和三维空间翻滚目标间的相对角速度ω＝[ωx,ωy,ωz]^t→0和角加速度

进一步地，为了保证主手指基关节追踪包络点的安全性，对碰撞避免约束、控制输入饱和约束和速度约束进行建模表示：

gsus(k)≤gs(14)

其中，gs和gs的表达式如下：

其中，表示包络点的坐标；rpl表示安全半径；uupper表示控制输入的上界；vupper表示速度的边界值矢量；i表示单位矩阵。

进一步地，主手指基关节追踪包络点问题可以表示为如下的二次规划问题：

其中，π(k)是方程(16)所示的目标函数；ρij_d是主手指基关节和包络点之间的期望相对位置矢量；和分别表示正定状态权重矩阵和正定控制权重矩阵；

进一步地，当主手指基关节位置和包络点位置pl＝pt^j时，可以实现主手指基关节和包络点的运动同步。

接着，通过调整主手指的关节角取值，使得主手指构型与包络边匹配。其中，包络边选择为三维空间翻滚目标某一面上的一条线(图4中红色线)。理论上，主手指可能具有很多可能的构型，为了使主手指ll＝{jl,1,jl,2,...,jl,m,el}与包络边el尽可能地匹配，引入单向距离dowd(ll,el)用来衡量两者形状的相似度：

其中，||ll||表示主手指连杆6的总长度；lsl＝{ll,1～2,ll,2～3,…,ll,m-1～m,ll,m～m+1}(ll,m～m+1＝(jl,m,el))代表了主手指ll中所有的连杆6(每个连杆6都简化为一条线段)；dowd(ll,i～i+1,el)表示连杆6ll,i～i+1到包络边el的单向距离，可以通过方程(18)计算。

其中，dpoint(p,el)表示连杆6ll,i～i+1上的任意一点p到包络边el的最短距离：

进一步地，欧几里得距离deuclid(p,q)＝||p-q||2。

进一步地，通过快速搜索随机树算法寻找使得主手指构型与包络边间单向距离最小的主手指关节角取值θopt，具体的算法如图5所示。其中，θnew是否有效是通过检测主手指与三维空间翻滚目标是否发生碰撞来进行判断的。

然后，利用s100中的多指机构的结构描述模型，确定从手指构型。首先，根据任意一个从手指li(i＝1,2,…,n；i≠l)基关节和主手指ll基关节之间的位置矢量δil，确定li的基关节位置pb_i：

pb_i＝pb_l+δil(i≠l,1≤i≤n,1≤l≤n)(22)

然后，根据li的允许关节角转动范围确定li的可能构型：

其中，和分别表示li的允许关节角转动范围的下界和上界；表示li不能与三维空间翻滚目标ot发生碰撞的约束要求。

最后，利用s200中的多指机构的有效包络条件，确定有效包络构型。首先，可以确定多指机构的可能构型的集合cmf_possible：

cmf_possible＝{ll，{li}}(i≠l,1≤i≤n,1≤l≤n)(24)

然后，利用有效包络条件，从cmf_possible中随机选择一个有效包络构型。

仿真实例

本发明以图1所示的采用4指3关节多指机构包络图4中的目标为具体实施例，对本发明进行进一步地说明：

设每个连杆6长均为dl＝0.25m，关节角取值范围均为θi∈[-90°,90°]；多指机构所有关节角的初始值均为θi＝0°；主手指的编号为1；多指机构的基关节间的位置关系如图6所示。三维空间翻滚目标的长、宽和高分别为1.2m、0.6m和0.4m；轨道半径为7100km。初始时刻t0＝0s时，包络点的坐标为[0,-0.225,-0.31]^tm；主手指基关节的坐标为pb_l＝[0.5,-0.5,0.5]^tm；相对位置矢量为ρij(t0)＝[-9,16,18]^tm；进一步地，设t＝0.1s；nc＝20；np＝20；uupper＝1(m/s²)；rpl＝1m。在给定的期望时间tc＝40s内完成了对包络的跟踪，如图7所示。

图8展示了主手指与包络边的匹配结果，其中dowd(ll,el)收敛到0.02974，θopt＝[-90,-2.35,-5.16]^t°。

图9展示了一个有效的多指机构构型：pb_1＝[0m,-0.225m,-0.31m]^t，θ1＝[-90°,-2.35°,-5.16°]^t；pb_2＝[-0.7m,0.025m,-0.31m]^t，θ2＝[90°,10°,8°]^t；pb_3＝[0m,0.275m,-0.31m]^t，θ3＝[90°,8°,6°]^t；pb_4＝[0.7m,0.025m,-0.31m]^t，θ4＝[-90°,-10°,-8°]^t。此外，手指1和手指3的指尖间的距离是0.3619m，小于此方向上的特征长度—0.4m；手指2和手指4的指尖间的距离是1.1587m，小于此方向上的特征长度—0.6m。

应该理解，以上描述是为了进行图示说明而不是为了进行限制。通过阅读上述描述，在所提供的示例之外的许多实施例和许多应用对本领域技术人员来说都将是显而易见的。因此，本教导的范围不应该参照上述描述来确定，而是应该参照前述权利要求以及这些权利要求所拥有的等价物的全部范围来确定。出于全面之目的，所有文章和参考包括专利申请和公告的公开都通过参考结合在本文中。在前述权利要求中省略这里公开的主题的任何方面并不是为了放弃该主体内容，也不应该认为申请人没有将该主题考虑为所公开的发明主题的一部分。