CN111486851A - 航天器近距离相对运动三维避障轨迹规划方法和装置 - Google Patents

Abstract

本申请涉及一种航天器近距离相对运动三维避障轨迹规划方法和装置。所述方法包括：获取相对运动坐标系下追踪航天器和参考航天器间的相对运动模型，获取三维障碍物椭球对应的非线性避障约束模型，构建平行于相对运动坐标系的笛卡尔坐标系，将追踪航天器的初始位置、目标位置以及三维障碍物椭球投影至笛卡尔坐标系的各个坐标平面内，在各个坐标平面内，采用二维旋转超平面方法将三维障碍物椭球投影所得的二维避障椭圆对应的非线性避障约束线性化，得到避障约束，根据各个坐标平面内的避障约束及相对运动模型，进行追踪航天器避障的轨迹规划，并从中选择性能最优的轨迹作为三维避障轨迹。采用本方法能够使快速进行三维空间避障的计算，从而大幅度提升轨迹规划的效率。

Description

航天器近距离相对运动三维避障轨迹规划方法和装置

技术领域

本申请涉及航天器轨迹规划技术领域，特别是涉及一种航天器近距离相对运动三维避障轨迹规划方法和装置。

背景技术

航天器近距离相对运动控制是实施近距离观测、推进剂加注、星上模块更换等在轨操控任务的重要支撑，相对运动指的是追踪航天器和参考航天器之间的相对运动。随着人类航天活动的频繁，空间碎片数目急剧增加，给航天器近距离相对运动任务的安全运营造成一定影响。一种有效解决方法是由传感器探测获得空间碎片、非合作航天器等障碍物信息，据此规划生成能够有效规避障碍物的任务轨迹，并控制航天器沿该任务轨迹运动从而实现避障。

为生成近距离相对运动避障轨迹，常见的处理方法是在最优控制框架下，将避障约束作为非线性椭球约束引入轨迹规划问题中，运用数值优化理论或方法进行求解。然而，由于非线性椭球形式的避障约束实质上是一种非凸约束，不利于提高数值求解效率。为此，Park等率先采用二维旋转超平面对上述避障约束进行凸化处理，其基本思想是在二维圆形障碍物表面设置一个与之相切的超平面，该超平面以预设的角速度沿着圆形表面旋转，只要使航天器始终位于该超平面的外侧即可实现避障。现有文献还提出了避障的双超平面方法，该方法从航天器的初始和目标位置分别做障碍物椭圆的切线，以此作为等效的避障约束，且整个任务过程中上述切线约束保持不变。Zagaris等进一步将双超平面方法扩展到了三维空间，并结合交会对接和轨迹跟踪两类任务场景与传统二维旋转超平面方法和直接线性化方法进行了性能对比。

旋转超平面方法最初主要用于解决参考轨道面内的近距离相对运动避障轨迹规划问题，即考虑的障碍物约束和相对运动轨迹均限定在参考轨道面这样的二维空间中。然而，实际相对运动轨迹与避障约束均处于三维空间内，因此需要将旋转超平面方法拓展到三维空间中。一种直观的思路是在三维避障椭球表面选取一系列交点，在各个交点处构建超平面以确保航天器始终在无碰撞风险的区域内运行。然而，在数值实现过程中，由避障椭球方程和航天器与障碍物中心的位置矢量计算获取上述交点和超平面的表达式同样需要占用较多计算资源，与引入旋转超平面技术处理避障约束的初衷相悖。

Zagaris等将Park提出的双超平面方法拓展至三维空间，具体是将优化问题投影到由障碍物中心、航天器初始和目标位置所构成的指定平面内，进而在该平面内分别从航天器的初始和目标位置构造与障碍物椭圆相切的超平面，以此代替非线性避障约束。该方法实质上是将避障轨迹限定在由障碍物中心、航天器初始和目标位置构成的指定平面内，因而能否生成避障轨迹、生成的避障轨迹是否性能最佳均无法保证。实际上，数值仿真表明该方法与传统二维旋转超平面方法和直接线性化方法相比，避障轨迹对应的速度增量最大，且求解失败的概率最高。

发明内容

基于此，有必要针对上述技术问题，提供一种能够解决避障轨迹规划求解计算量大以及规划成功率低问题的航天器近距离相对运动三维避障轨迹规划方法和装置。

一种航天器近距离相对运动三维避障轨迹规划方法，所述方法包括：

获取相对运动坐标系下追踪航天器和参考航天器间的相对运动模型；

获取三维障碍物椭球对应的非线性避障约束模型；

构建平行于所述相对运动坐标系的笛卡尔坐标系，将所述追踪航天器的初始位置、目标位置以及所述三维障碍物椭球投影至所述笛卡尔坐标系的各个坐标平面内；

在各个坐标平面内，用二维旋转超平面方式将三维障碍物椭球投影所得的二维避障椭圆对应的非线性避障约束线性化，得到避障约束；

根据各个坐标平面内的所述避障约束以及所述相对运动模型，进行追踪航天器避障的轨迹规划，并从中选择性能最优的轨迹作为三维避障轨迹。

在其中一个实施例中，还包括：获取追踪航天器与参考航天器以采样周期T进行离散得到相对运动坐标系中的离散形式相对运动方程；对所述动力学方程中的状态向量和控制输入向量进行叠加重组，得到相对运动模型为：

X＝Ψx(0)+ΩU

其中，X表示状态列向量，U表示控制输入列向量，Ψ和Ω表示状态转移矩阵。

在其中一个实施例中，还包括：根据障碍物测量信息对应的协方差矩阵，构建三维障碍物椭球禁飞区域；根据所述三维障碍物椭球禁飞区域、追踪航天器的位置矢量以及三维障碍物椭球中心的位置矢量确定障碍物的非线性避障约束模型。

在其中一个实施例中，还包括：在各个坐标平面内，分别判断所述初始位置或所述目标位置的投影点是否在所述三维障碍物椭球投影得到的二维避障椭圆内；若是，则表示追踪航天器的三维轨迹在该平面内的投影将违背避障约束，无需在所述二维避障椭圆的约束下规划轨迹；若否，则在各个坐标平面内，根据所述二维避障椭圆对应的非线性避障约束模型，采用二维旋转超平面方式，得到避障约束；所述二维避障椭圆对应的非线性避障约束模型是所述三维障碍物椭球对应的非线性避障约束模型投影得到的。

在其中一个实施例中，还包括：根据障碍物中心到投影所得的初始和目标位置的单位矢量r_s和r_g；根据所述单位矢量r_s和r_g，得到超平面沿第一方向旋转的总旋转角γ_tot＝arccos(r_s·r_g)；根据所述离散步数N，得到每一离散步旋转的旋转角度γ＝γ_tot/N；计算第k离散步的旋转角度为γ_k＝kγ；利用罗德里格旋转公式得到单位矢量r_s转动γ_k角度得到对应的单位矢量为：

r_k＝r_scos(γ_k)+(k×r_s)sin(γ_k)+k(k·r_s)(1-cos(γ_k))；

其中，k＝(r_s×r_g)/|r_s×r_g|表示转轴的单位向量；根据所述非线性避障约束模型，得到单位矢量r_k与障碍物表面的交点p_k以及在交点p_k处的椭球法向量n_k，根据交点p_k和向量n_k，得到避障约束为：

其中，e_k表示障碍物中心到交点p_k的矢量，表示障碍物中心到追踪航天器第k离散步时刻所处位置矢量。

在其中一个实施例中，还包括：根据所述单位矢量r_s和r_g，得到超平面沿与所述第一方向相反的第二方向的总旋转角γ′_tot＝2π-arccos(r_s·r_g)；根据所述离散步数N，得到每一离散步旋转的旋转角度γ′＝γ′_tot/N；计算第k离散步的旋转角度为γ′_k＝kγ′；

利用罗德里格旋转公式得到单位矢量r_s转动γ′_k角度得到对应的单位矢量r_k′为：

r′_k＝r_scos(γ′_k)+(k×r_s)sin(γ′_k)+k(k·r_s)(1-cos(γ′_k))

其中，k＝(r_s×r_g)/|r_s×r_g|表示旋转轴的单位向量；根据所述非线性避障约束模型，得到单位矢量r′_k与障碍物表面的交点p′_k以及在交点p′_k处的椭球法向量n′_k，根据交点p′_k和向量n′_k，得到避障约束为：

其中，e_k表示障碍物中心到交点p_k′的矢量，ρ_k表示障碍物中心到第k离散步时刻所处位置矢量。

在其中一个实施例中，还包括：获取最优规划目标，根据最优规划目标建立目标函数；根据所述避障约束、预先设置的控制输入饱和约束以及所述相对运动模型，确定约束条件；根据所述约束条件，分别求解所述目标函数，从中选择性能最优的轨迹作为最终的航天器避障轨迹。

一种航天器避障轨迹快速规划装置，所述装置包括：

运动模型构建模块，用于获取相对运动坐标系下追踪航天器和参考航天器间的相对运动模型；

碰撞模型构建模块，用于获取三维障碍物椭球对应的非线性避障约束模型；

坐标构建模块，用于构建平行于所述相对运动坐标系的笛卡尔坐标系，将所述追踪航天器的初始位置、目标位置以及所述三维障碍物椭球投影至所述笛卡尔坐标系的各个坐标平面内；

三维避障约束线性化模块，用于采用二维旋转超平面方法将三维障碍物椭球投影所得的二维避障椭圆对应的非线性避障约束线性化，得到避障约束；

轨迹规划模块，用于根据各个坐标平面内的所述避障约束及所述相对运动模型，进行追踪航天器避障的轨迹规划，并从中选择性能最优的轨迹作为三维避障轨迹。

一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，所述处理器执行所述计算机程序时实现以下步骤：

获取相对运动坐标系下追踪航天器和参考航天器间的相对运动模型；

获取三维障碍物椭球对应的非线性避障约束模型；

构建平行于所述相对运动坐标系的笛卡尔坐标系，将所述追踪航天器的初始位置、目标位置以及所述三维障碍物椭球投影至所述笛卡尔坐标系的各个坐标平面内；

在各个坐标平面内，采用二维旋转超平面方法将三维障碍物椭球投影所得的二维避障椭圆对应的非线性避障约束线性化，得到避障约束；

根据各个坐标平面内的所述避障约束及所述相对运动模型，进行追踪航天器避障的轨迹规划，并从中选择性能最优的轨迹作为三维避障轨迹。

一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现以下步骤：

获取相对运动坐标系下追踪航天器和参考航天器间的相对运动模型；

获取三维障碍物椭球对应的非线性避障约束模型；

构建平行于所述相对运动坐标系的笛卡尔坐标系，将所述追踪航天器的初始位置、目标位置以及所述三维障碍物椭球投影至所述笛卡尔坐标系的各个坐标平面内；

在各个坐标平面内，采用二维旋转超平面方法将三维障碍物椭球投影所得的二维避障椭圆对应的非线性避障约束线性化，得到避障约束；

根据各个坐标平面内的所述避障约束及所述相对运动模型，进行追踪航天器避障的轨迹规划，并从中选择性能最优的轨迹作为三维避障轨迹。

上述航天器近距离相对运动三维避障轨迹规划方法、装置、计算机设备和存储介质，通过建立笛卡尔坐标系，然后将三维空间中初始位置、目标位置以及三维障碍物椭球投影至二维平面中，然后采用二维旋转超平面方法处理该坐标平面内的避障约束，然后根据各个坐标平面内的避障约束以及相对运动模型，进行追踪航天器避障的轨迹规划，从而选择出最佳路径作为三维避障轨迹，本发明利用二维旋转超平面方法，可以快速进行三维空间避障的计算，并且保证了计算的准确性。

附图说明

图1为一个实施例中航天器近距离相对运动三维避障轨迹规划方法的流程示意图；

图2为一个实施例中椭球结构图；

图3为一个实施例中三维避障椭球投影的示意图；

图4为一个实施例中二维旋转超平面的构造示意图；

图5为一个实施例中算例1二维视角的避障轨迹示意图；

图6为一个实施例中算例1三维视角的避障轨迹示意图；

图7为一个实施例中算例2二维视角的避障轨迹示意图；

图8为一个实施例中算例2三维视角的避障轨迹示意图；

图9为一个实施例中算例3二维视角的避障轨迹示意图；

图10为一个实施例中算例3三维视角的避障轨迹示意图；

图11为一个实施例中航天器避障轨迹快速规划装置的结构框图；

图12为一个实施例中计算机设备的内部结构图。

具体实施方式

为了使本申请的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本申请进行进一步详细说明。应当理解，此处描述的具体实施例仅仅用以解释本申请，并不用于限定本申请。

在一个实施例中，如图1所示，提供了一种航天器避障轨迹快速规划方法，该方法可以应用于终端中，包括以下步骤：

步骤102，获取相对运动坐标系下追踪航天器和参考航天器间的相对运动模型。

在相对运动坐标系下，描述追踪航天器和参考航天器在空间的相对运动，为后续追踪航天器的轨迹规划做基础。

步骤104，获取三维障碍物椭球对应的非线性避障约束模型。

三维障碍物椭球对追踪航天器飞行存在潜在的威胁，因此，轨迹规划时，必须避开障碍物所在的范围，因此，基于障碍物的形状、位置可以构建非线性避障约束模型。

步骤106，构建平行于相对运动坐标系的笛卡尔坐标系，将追踪航天器的初始位置、目标位置以及三维障碍物椭球投影至笛卡尔坐标系的各个坐标平面内。

笛卡尔坐标系包括二维笛卡尔坐标系和三维笛卡尔坐标系，本步骤解决的是三维空间的问题，因此建立的是三维笛卡尔坐标系，三维笛卡尔坐标系中，包括了三个平面，分别是o′x′y′平面、o′y′z′平面以及o′x′z′平面，针对笛卡尔坐标系中的目标，可以投影至不同的平面中。

步骤108，在各个坐标平面内，采用二维旋转超平面方法将三维障碍物椭球投影所得的二维避障椭圆对应的非线性避障约束线性化，得到避障约束。

本步骤中，在每个坐标平面内，采用线性化的方式均可以计算得到避障约束。二维旋转超平面方式可以采用传统的二维旋转超平面的方式。

步骤110，根据各个坐标平面内的避障约束及相对运动模型，进行追踪航天器避障的轨迹规划，并从中选择性能最优的轨迹作为三维避障轨迹。

上述航天器近距离相对运动三维避障轨迹规划方法中，通过建立笛卡尔坐标系，然后将三维空间中初始位置、目标位置以及三维障碍物椭球投影至三个投影中，然后采用二维旋转超平面方法处理该坐标平面内的避障约束，然后根据各个坐标平面内的避障约束以及相对运动模型，进行追踪航天器避障的轨迹规划，本发明利用二维旋转超平面方法，可以快速进行三维空间避障的计算，并且保证了计算的准确性。

在其中一个实施例中，获取追踪航天器与参考航天器以采样周期T进行离散得到相对运动坐标系中的离散形式相对运动方程；对离散形式相对运动方程中的状态向量和控制输入向量进行叠加重组，得到相对运动模型为：

X＝Ψx(0)+ΩU

其中，X表示状态列向量，U表示控制输入列向量，Ψ和Ω表示状态转移矩阵。

具体的，X实际上表示所有离散时刻的相对状态x(k)叠加组成的列向量，其中k＝1,2,…,N。x(k)为6×1的列向量。

具体的，相对运动坐标系下，追踪航天器相对运动方程可表示为：

其中，

表示相对运动状态，a＝[a_x a_y a_z]^T表示三个方向的推力加速度，

表示追踪航天器的平均轨道角速度。

进一步的，给定采样周期T，可以得到离散形式的动力学方程为：

其中，k为当前步数，

可表示为：

脉冲推力模型对应的

为：

为了便于数值求解，将N步状态向量叠加为X＝[x(1)^T,x(2)^T,…,x(N)^T]^T∈R^6N×1，控制输入向量也可以重组为U＝[u(0)^T,u(1)^T,…,u(N-1)^T]^T∈R^3N×1，从而可以得到相对运动模型为：

X＝Ψx(0)+ΩU

其中，X表示状态列向量，U表示控制输入列向量，Ψ和Ω表示状态转移矩阵，式中，

在其中一个实施例中，根据障碍物测量信息对应的协方差矩阵，构建三维障碍物椭球禁飞区域，根据空间椭球禁飞区域、追踪航天器的位置矢量以及障碍物中心的位置矢量确定三维障碍物椭球的非线性避障约束模型。

具体的，对于追踪航天器避障问题，通常引入一个包含障碍物可能出现位置的三维障碍物椭球形空间禁飞区作为避障约束。如果追踪航天器运动轨迹与障碍物椭球相交，则认为违背了避障约束。椭球的结构如图2所示，由于障碍物尺寸小，地面观测设备对其状态估计的不确定性较大，因此，椭球的体积可能远大于障碍物的实际尺寸。

结合椭球方程，非线性避障约束模型可以表示为：

(r_pos-r_e)^TS(r_pos-r_e)≥1

其中，r_pos表示追踪航天器相对于相对运动坐标系原点的位置矢量，r_e表示障碍物中心的位置矢量，S为椭球的形状矩阵。

在实际应用中，状态估计的不确定性通常被量化为高斯型噪声和相应的协方差矩阵，而选择椭球形禁飞区域的主要原因是协方差矩阵可以直接用于定义不确定性椭球，椭球的尺寸大小与真实障碍物位于椭球中的概率有关。n_σ椭球的形状矩阵S可由n_σ的平方与协方差矩阵Σ乘积的逆来表示：

障碍物位于n_σ椭球内的概率可表示为：

对于三维问题，通过上式可以计算出，当n_σ＝3时，障碍物位于椭球内的概率为97.07％，当n_σ＝4时，该概率为99.74％。

在规划相对运动避障轨迹时，若直接处理上述非线性椭球形式的避障约束，通常容易造成求解时间较长，且计算结果对初始猜测值敏感。本发明基于空间几何投影的思想，以避障椭球中心为原点构建平行于相对运动坐标系的三维笛卡尔坐标系，该坐标系与三维避障椭球相交形成三个二维障碍物椭圆；将航天器在三维空间中的初始位置和目标位置分别投影到上述笛卡尔坐标系的各个坐标平面内，在每个坐标平面内分别运用传统二维旋转超平面技术对碰撞避约束进行线性化处理。

三维避障椭球投影方法的示意图如图3所示，其中o′x′y′z′是以三维避障椭球中心为原点构建的笛卡尔坐标系，其坐标轴与相对运动坐标系oxyz坐标轴分别平行；坐标系o′x′y′z′与三维避障椭球相交将形成三个相互正交的二维避障椭圆。以o′x′y′平面内的二维避障椭圆为例，由空间几何基本原理，若航天器相对运动轨迹在o′x′y′平面内的投影与该二维避障椭圆不相交，则空间中的航天器相对运动轨迹也将不可能与原始的三维避障椭球相交，即不可能违背避障约束；其余两个平面o′x′z′、o′y′z′内情形类似。据此，可将三维避障椭球约束分解为三个相互独立的二维避障椭圆约束进行凸化处理。

在其中一个实施例中，还需要考虑投影后初始位置以及目标位置是否在三维障碍物椭球投影得到的二维避障椭圆内，具体是：在各个坐标平面内，判断初始位置或目标位置是否在三维障碍物椭球投影得到的二维避障椭圆内；若是，则表示追踪航天器的三维轨迹在该平面内的投影将违背避障约束，无需在该二维避障椭圆的约束下规划轨迹；若否，则在各个坐标平面内，根据二维避障椭球的非线性避障约束模型，采用二维旋转超平面求解方式，得到避障约束。值得说明的是，在已知三维障碍物椭球的非线性避障约束模型时，对应可以得到其投影面的二维避障椭圆的非线性避障约束模型。

当初始和目标位置的投影点在所述三维障碍物椭球投影得到的二维避障椭圆外时，如图4所示，其中一个实施例中，以o′x′y′为例，根据障碍物的中心到初始位置以及目标位置的单位矢量r_s和r_g；根据单位矢量r_s和r_g，得到超平面沿第一方向的总旋转角γ_to_t＝arccos(r_s·r_g)；获取的离散步数N，根据离散步数N，得到每一离散步旋转的旋转角度γ＝γ_tot/N；计算第k离散步的旋转角度为γ_k＝kγ；利用罗德里格旋转公式得到单位矢量r_s转动γ_k角度得到对应的单位矢量r_k为：

r_k＝r_scos(γ_k)+(k×r_s)sin(γ_k)+k(k·r_s)(1-cos(_k))；

其中，k＝(r_s×r_g)/|r_s×r_g|表示旋转轴的单位向量。

根据非线性避障约束模型，得到单位矢量r_k与障碍物表面的交点p_k以及在交点p_k处的椭球法向量n_k，根据交点p_k和向量，得到避障约束为：

其中，e_k表示障碍物中心到交点p_k的矢量，ρ_k表示障碍物中心到追踪航天器当前位置的矢量。

值得说明的是，第一方向指的是顺时针方向或者是逆时针方向。

另外，上述是针对第一方向构建二维旋转超平面进行求解，还可以沿第二方向构建二维旋转超平面，值得说明的是，若第一方向为顺时针方向，则第二方向为逆时针方向，若第一方向是逆时针方向，则第二方向为顺时针方向，具体如下：

根据单位矢量r_s和r_g，得到超平面沿与第一方向相反的第二方向旋转的总旋转角γ′_tot＝360°-arccos(r_s·r_g)；

根据离散步数N，得到每一离散步旋转的旋转角度γ′＝γ′_tot/N；

计算第k离散步的旋转角度为γ′_k＝kγ′；

利用罗德里格旋转公式得到单位矢量r_s转动γ′_k角度得到对应的单位矢量r_k为：

r′_k＝r_scos(γ′_k)+(k×r_s)sin(γ′_k)+k(k·r_s)(1-cos(γ′_k))

其中，k＝(r_s×r_g)/|r_s×r_g|表示旋转轴的单位向量；

根据非线性避障约束模型，得到单位矢量r′_k与障碍物表面的交点p′_k以及在交点p′_k处的椭球法向量n′_k，根据交点p′_k和向量n′_k，得到避障约束为：

其中，e_k表示障碍物中心到交点p_k′的矢量，ρ_k表示障碍物中心到第k离散步时刻所处位置矢量。

上述实施例中，由于超平面约束围绕平面法向量旋转时，即可以顺时针旋转，也可以逆时针旋转，上述第一方向可以是顺时针方向也可以是逆时针方向，对应的，在第一方向为顺时针时，第二方向为逆时针方向，第一方向为逆时针方向时，第二方向为顺时针方向，由于预先无法确定哪个方向的性能更优，因此需要在一个平面内生成两组超平面约束。

进一步，将所有离散步长内的避障约束整理成矩阵形式：

-HΩU≤-Hε+HΨx₀

其中H和ε分别定义为：

同法，可以采用旋转超平面技术对o′x′z′和o′y′z′平面内的二维避障约束进行凸化处理。

在其中一个实施例中，进行轨迹规划时，获取最优规划目标，根据最优规划目标建立目标函数；根据避障约束、预先设置的控制输入饱和约束以及相对运动模型，确定约束条件；根据约束条件，分别求解目标函数，从中选择性能最优的轨迹作为最终的航天器避障轨迹。

具体的，选择能量最省为最优规划目标，表达式为：

然后确定约束为：

x(t₀)＝x₀ x(t_f)＝x_f

-u_max≤u_i≤u_max(i＝x,y,z)

如果是另一个方向旋转，则采用

通过观察可以发现，上述避障轨迹规划问题的性能指标为二次型形式，约束为线性约束，可通过现有的二次规划方法快速求解，以实现避障轨迹快速规划。本发明采用MATLAB优化工具箱中的quadprog求解器进行二次规划，其具体形式为：求解控制序列U，使得：

同时满足：

航天器避障轨迹规划中的不等式为约束主要包括避障约束和控制输入饱和约束，则矩阵A_ineq和b_ineq可进一步表示为：

等式约束矩阵A_eq和b_eq可表示为：

能量最省作为目标函数可以表示为二次型，则Π和f为：

需要注意的是，对于o′x′y′、o′x′z′和o′y′z′平面内的二维避障椭圆约束，其相应的旋转超平面既可以沿顺时针方向旋转，也可沿逆时针方向旋转，因此在实际求解中，需分别考虑不同的旋转超平面旋转方向，计算获得多条相对运动避障轨迹，选择性能最佳者为最终规划得到的航天器三维避障轨迹。

以下以具体的试验数据对本发明的有益效果进行说明。

假设参考航天器位于轨道高度为500km的近圆轨道上，追踪航天器的初始和目标相对状态分别设为：

x₀＝[0m 5000m 0m 0m/s 0m/s 0m/s]^T

x_f＝[0m 10m 0m 0m/s 0m/s 0m/s]^T

追踪航天器的最大脉冲推力为10N，质量为100kg，整个任务时长为1000s。为了验证三维拓展策略的性能，本节考虑了三个不同形状的障碍物，障碍物中心均位于[500,2500,0]^Tm，相应的协方差矩阵分别为：

算例1:Ξ＝diag([500,500,500])m²

算例2:Ξ＝diag([500,500,300])m²。

算例3:Ξ＝diag([500,500,100])m²

基于三维旋转超平面方法的二次规划问题采用MATLAB 2016b优化工具箱的quadprog函数进行求解，精度参数“TolX”、“TolFun”和“TolCon”均设置为1.0×10^-6。为进一步验证所提出方法的性能，在同样问题配置下利用非线性椭球法进行对比验证。优化工具箱的fmincon函数被用来求解非线性椭球法生成的非线性规划问题，其求解器精度参数与quadprog保持一致。由于非线性规划问题的计算效率受初值影响较大，此处采用不考虑障碍物约束的控制输入作为初始猜测值。仿真计算机配有2.93GHz的Intel Core i7处理器和8GRAM。数值仿真以能量消耗和计算所需时间为轨迹性能的评价指标，其中轨迹消耗的能量为：

两种方法生成的避障轨迹如图5-10所示，相应的性能指标对比如表1所示，其中符号“3D-IRHM”、“NEM”分别表示三维旋转超平面方法和非线性椭球法。

表1三维场景性能指标对比

由如图5-10可知，两种方法在三个场景均能成功生成可行的避障轨迹。在算例1中，两种方法生成的轨迹基本重合，能量消耗的相对误差仅为0.02％。在算例2和3中，随着三维避障椭球在z向的取值进一步减小，两种方法生成的避障轨迹略有不同，但其消耗能量的最大相对误差仍小于2.1％。对比两种方法所需计算时间可以发现，三维旋转超平面方法计算效率远高于非线性椭球法。

综上所述，三维旋转超平面方法求解速度快，计算精度高，适用于对时效性要求较高的在线避障轨迹规划。

应该理解的是，虽然图1的流程图中的各个步骤按照箭头的指示依次显示，但是这些步骤并不是必然按照箭头指示的顺序依次执行。除非本文中有明确的说明，这些步骤的执行并没有严格的顺序限制，这些步骤可以以其它的顺序执行。而且，图1中的至少一部分步骤可以包括多个子步骤或者多个阶段，这些子步骤或者阶段并不必然是在同一时刻执行完成，而是可以在不同的时刻执行，这些子步骤或者阶段的执行顺序也不必然是依次进行，而是可以与其它步骤或者其它步骤的子步骤或者阶段的至少一部分轮流或者交替地执行。

在一个实施例中，如图11所示，提供了一种航天器避障轨迹快速规划装置，包括：运动模型构建模块1102、碰撞模型构建模块1104、坐标构建模块1106、三维避障约束线性化模块1108和轨迹规划模块1110，其中：

运动模型构建模块1102，用于获取相对运动坐标系下追踪航天器和参考航天器间的相对运动模型；

碰撞模型构建模块1104，获取三维障碍物椭球对应的非线性避障约束模型；

坐标构建模块1106，构建平行于所述相对运动坐标系的笛卡尔坐标系，将所述追踪航天器的初始位置、目标位置以及所述三维障碍物椭球投影至所述笛卡尔坐标系的各个坐标平面内；

三维避障约束线性化模块1108，用于采用二维旋转超平面方法将三维障碍物椭球投影所得的二维避障椭圆对应的非线性避障约束线性化，得到避障约束；

轨迹规划模块1110，用于根据各个坐标平面内的所述避障约束及所述相对运动模型，进行追踪航天器避障的轨迹规划，并从中选择性能最优的轨迹作为三维避障轨迹。

在其中一个实施例中，运动模型构建模块1102还用于获取追踪航天器与参考航天器以采样周期T进行离散得到相对运动坐标系中的离散形式相对运动方程；对所述离散形式相对运动方程中的状态向量和控制输入向量进行叠加重组，得到相对运动模型为：

X＝Ψx(0)+ΩU

其中，X表示状态列向量，U表示控制输入列向量，Ψ和Ω表示状态转移矩阵。

在其中一个实施例中，碰撞模型构建模块1104还用于根据障碍物测量信息对应的协方差矩阵，构建三维障碍物椭球禁飞区域；根据所述三维障碍物椭球禁飞区域、追踪航天器的位置矢量以及三维障碍物椭球中心的位置矢量确定障碍物的非线性避障约束模型。

在其中一个实施例中，坐标构建模块1106还在各个坐标平面内，分别判断所述初始位置或所述目标位置的投影点是否在所述三维障碍物椭球投影得到的二维避障椭圆内；若是，则表示追踪航天器的三维轨迹在该平面内的投影将违背避障约束，无需在所述二维避障椭圆的约束下规划轨迹；若否，则在各个坐标平面内，根据所述二维避障椭圆对应的非线性避障约束模型，采用二维旋转超平面方式，得到避障约束。

在其中一个实施例中，三维避障约束线性化模块1108还用于根据障碍物中心到投影所得的初始和目标位置的单位矢量r_s和r_g；根据所述单位矢量r_s和r_g，得到超平面沿第一方向的总旋转角γ_to_t＝arccos(r_s·r_g)；根据所述离散步数N，得到每一离散步旋转的旋转角度γ＝γ_tot/N；计算第k离散步的旋转角度为γ_k＝kγ；利用罗德里格旋转公式得到单位矢量r_s转动γ_k角度得到对应的单位矢量为：

r_k＝r_scos(γ_k)+(k×r_s)sin(γ_k)+k(k·r_s)(1-cos(γ_k))；

其中，k＝(r_s×r_g)/|r_s×r_g|表示旋转轴的单位向量；根据所述非线性避障约束模型，得到单位矢量r_k与障碍物表面的交点p_k以及在交点p_k处的椭球法向量n_k，根据交点p_k和向量n_k，得到避障约束为：

其中，e_k表示障碍物中心到交点p_k的矢量，表示障碍物中心到追踪航天器第k离散步时刻所处位置矢量。

在其中一个实施例中，三维避障约束线性化模块1108还用于根据所述单位矢量r_s和r_g，得到超平面沿与第一方向相反的第二方向旋转的总旋转角γ′_tot＝360°-arccos(r_s·r_g)；

根据所述离散步数N，得到每一离散步旋转的旋转角度γ′＝γ′_tot/N；

计算第k离散步的旋转角度为γ′_k＝kγ′；

利用罗德里格旋转公式得到单位矢量r_s转动γ′_k角度得到对应的单位矢量r_k′为：

r′_k＝r_scos(γ′_k)+(k×r_s)sin(γ′_k)+k(k·r_s)(1-cos(γ′_k))

其中，k＝(r_s×r_g)/|r_s×r_g|表示旋转轴的单位向量；

根据所述非线性避障约束模型，得到单位矢量r′_k与障碍物表面的交点p′_k以及在交点p′_k处的椭球法向量n′_k，根据交点p′_k和向量n′_k，得到避障约束为：

其中，e_k表示障碍物中心到交点p_k′的矢量，ρ_k表示障碍物中心到第k离散步时刻所处位置矢量。

在其中一个实施例中，轨迹规划模块1110还用于获取最优规划目标，根据最优规划目标建立目标函数；根据所述避障约束、控制输入饱和约束以及所述相对运动模型，确定约束条件；根据所述约束条件，分别求解所述目标函数，从中选择性能最优的轨迹作为最终的航天器避障轨迹。

关于航天器避障轨迹快速规划装置的具体限定可以参见上文中对于航天器避障轨迹快速规划方法的限定，在此不再赘述。上述航天器避障轨迹快速规划装置中的各个模块可全部或部分通过软件、硬件及其组合来实现。上述各模块可以硬件形式内嵌于或独立于计算机设备中的处理器中，也可以以软件形式存储于计算机设备中的存储器中，以便于处理器调用执行以上各个模块对应的操作。

在一个实施例中，提供了一种计算机设备，该计算机设备可以是终端，其内部结构图可以如图12所示。该计算机设备包括通过系统总线连接的处理器、存储器、网络接口、显示屏和输入装置。其中，该计算机设备的处理器用于提供计算和控制能力。该计算机设备的存储器包括非易失性存储介质、内存储器。该非易失性存储介质存储有操作系统和计算机程序。该内存储器为非易失性存储介质中的操作系统和计算机程序的运行提供环境。该计算机设备的网络接口用于与外部的终端通过网络连接通信。该计算机程序被处理器执行时以实现一种航天器近距离相对运动三维避障轨迹规划方法。该计算机设备的显示屏可以是液晶显示屏或者电子墨水显示屏，该计算机设备的输入装置可以是显示屏上覆盖的触摸层，也可以是计算机设备外壳上设置的按键、轨迹球或触控板，还可以是外接的键盘、触控板或鼠标等。

本领域技术人员可以理解，图12中示出的结构，仅仅是与本申请方案相关的部分结构的框图，并不构成对本申请方案所应用于其上的计算机设备的限定，具体的计算机设备可以包括比图中所示更多或更少的部件，或者组合某些部件，或者具有不同的部件布置。

在一个实施例中，提供了一种计算机设备，包括存储器和处理器，该存储器存储有计算机程序，该处理器执行计算机程序时实现上述实施例中方法的步骤。

在一个实施例中，提供了一种计算机可读存储介质，其上存储有计算机程序，计算机程序被处理器执行时实现上述实施例中方法的步骤。

本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一非易失性计算机可读取存储介质中，该计算机程序在执行时，可包括如上述各方法的实施例的流程。其中，本申请所提供的各实施例中所使用的对存储器、存储、数据库或其它介质的任何引用，均可包括非易失性和/或易失性存储器。非易失性存储器可包括只读存储器(ROM)、可编程ROM(PROM)、电可编程ROM(EPROM)、电可擦除可编程ROM(EEPROM)或闪存。易失性存储器可包括随机存取存储器(RAM)或者外部高速缓冲存储器。作为说明而非局限，RAM以多种形式可得，诸如静态RAM(SRAM)、动态RAM(DRAM)、同步DRAM(SDRAM)、双数据率SDRAM(DDRSDRAM)、增强型SDRAM(ESDRAM)、同步链路(Synchlink)DRAM(SLDRAM)、存储器总线(Rambus)直接RAM(RDRAM)、直接存储器总线动态RAM(DRDRAM)、以及存储器总线动态RAM(RDRAM)等。

以上实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。

以上所述实施例仅表达了本申请的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本申请构思的前提下，还可以做出若干变形和改进，这些都属于本申请的保护范围。因此，本申请专利的保护范围应以所附权利要求为准。

Claims (10)

1.一种航天器近距离相对运动三维避障轨迹规划方法，所述方法包括：

获取相对运动坐标系下追踪航天器和参考航天器间的相对运动模型；

获取三维障碍物椭球对应的非线性避障约束模型；

构建平行于所述相对运动坐标系的笛卡尔坐标系，将所述追踪航天器的初始位置、目标位置以及所述三维障碍物椭球投影至所述笛卡尔坐标系的各个坐标平面内；

在各个坐标平面内，采用二维旋转超平面方式将三维障碍物椭球投影所得的二维避障椭圆对应的非线性避障约束线性化，得到避障约束；

根据各个坐标平面内的所述避障约束及所述相对运动模型，进行追踪航天器避障的轨迹规划，并从中选择性能最优的轨迹作为三维避障轨迹。

2.根据权利要求1所述的方法，其特征在于，所述获取相对运动坐标系下追踪航天器和参考航天器间的相对运动模型，包括：

获取追踪航天器与参考航天器以采样周期T进行离散得到相对运动坐标系中的离散形式相对运动方程；

对所述离散形式相对运动方程中的状态向量和控制输入向量进行叠加重组，得到相对运动模型为：

X＝Ψx(0)+ΩU

其中，X表示状态列向量，U表示控制输入列向量，Ψ和Ω表示状态转移矩阵。

3.根据权利要求1所述的方法，其特征在于，所述获取三维障碍物椭球对应的非线性避障约束模型，包括：

根据障碍物测量信息对应的协方差矩阵，构建三维障碍物椭球禁飞区域；

根据所述三维障碍物椭球禁飞区域、追踪航天器的位置矢量以及三维障碍物椭球中心的位置矢量确定障碍物的非线性避障约束模型。

4.根据权利要求1所述的方法，其特征在于，构建平行于所述相对运动坐标系的笛卡尔坐标系，在将所述追踪航天器的初始位置、目标位置以及所述三维障碍物椭球投影至所述笛卡尔坐标系的各个坐标平面内之后，所述方法还包括：

在各个坐标平面内，分别判断所述初始位置或所述目标位置的投影点是否在所述三维障碍物椭球投影得到的二维避障椭圆内；

若是，则表示追踪航天器的三维轨迹在该平面内的投影将违背避障约束，无需在所述二维避障椭圆的约束下规划轨迹；

若否，则在各个坐标平面内，根据所述二维避障椭圆对应的非线性避障约束模型，采用二维旋转超平面方式，得到避障约束；所述二维避障椭圆对应的非线性避障约束模型是所述三维障碍物椭球对应的非线性避障约束模型投影得到的。

5.根据权利要求1所述的方法，其特征在于，在各个坐标平面内，采用二维旋转超平面方法将三维障碍物椭球投影所得的二维避障椭圆对应的非线性避障约束线性化，得到避障约束，包括：

根据障碍物中心到投影所得的初始位置和目标位置的单位矢量r_s和r_g；

根据所述单位矢量r_s和r_g，得到超平面沿第一方向旋转的总旋转角γ_tot＝arccos(r_s·r_g)；

根据预先设置的离散步数N，得到每一离散步旋转的旋转角度γ＝γ_tot/N；

计算第k离散步的旋转角度为γ_k＝kγ；

利用罗德里格旋转公式得到单位矢量r_s转动γ_k角度得到对应的单位矢量为：

r_k＝r_scos(γ_k)+(k×r_s)sin(γ_k)+k(k·r_s)(1-cos(γ_k))；

其中，k＝(r_s×r_g)/|r_s×r_g|表示旋转轴的单位向量；

根据所述非线性避障约束模型，得到单位矢量r_k与障碍物表面的交点p_k以及在交点p_k处的椭球法向量n_k，根据交点p_k和向量n_k，得到避障约束为：

其中，e_k表示障碍物中心到交点p_k的矢量，ρ_k表示障碍物中心到追踪航天器第k离散步时刻所处位置矢量。

6.根据权利要求5所述的方法，其特征在于，所述方法还包括：

根据所述单位矢量r_s和r_g，得到超平面沿与所述第一方向相反的第二方向旋转的总旋转角γ′_tot＝360°-arccos(r_s·r_g)；

根据所述离散步数N，得到每一离散步旋转的旋转角度γ′＝γ′_tot/N；

计算第k离散步的旋转角度为γ′_k＝kγ′；

利用罗德里格旋转公式得到单位矢量r_s转动γ′_k角度得到对应的单位矢量r_k′为：

r′_k＝r_scos(γ′_k)+(k×r_s)sin(γ′_k)+k(k·r_s)(1-cos(γ′_k))

其中，k＝(r_s×r_g)/|r_s×r_g|表示旋转轴的单位向量；

根据所述非线性避障约束模型，得到单位矢量r′_k与障碍物表面的交点p′_k以及在交点p′_k处的椭球法向量n′_k，根据交点p′_k和向量n′_k，得到避障约束为：

其中，e_k表示障碍物中心到交点p_k′的矢量，ρ_k表示障碍物中心到第k离散步时刻所处位置矢量。

7.根据权利要求5或6所述的方法，其特征在于，根据各个坐标平面内的所述避障约束以及所述相对运动模型，进行追踪航天器避障的轨迹规划，并从中选择性能最优的轨迹作为三维避障轨迹，包括：

获取最优规划目标，根据最优规划目标建立目标函数；

根据所述避障约束、预先设置的控制输入饱和约束以及所述相对运动模型，确定约束条件；

根据所述约束条件，分别求解所述目标函数，从中选择性能最优的轨迹作为最终的航天器避障轨迹。

8.一种航天器近距离相对运动三维避障轨迹规划装置，其特征在于，所述装置包括：

运动模型构建模块，用于获取相对运动坐标系下追踪航天器和参考航天器间的相对运动模型；

碰撞模型构建模块，用于获取三维障碍物椭球对应的非线性避障约束模型；

坐标构建模块，用于构建平行于所述相对运动坐标系的笛卡尔坐标系，将所述追踪航天器的初始位置、目标位置以及所述三维障碍物椭球投影至所述笛卡尔坐标系的各个坐标平面内；

三维避障约束线性化模块，用于采用二维旋转超平面方法将三维障碍物椭球投影所得的二维避障椭圆对应的非线性避障约束线性化，得到避障约束；

轨迹规划模块，用于根据各个坐标平面内的所述避障约束及所述相对运动模型，进行追踪航天器避障的轨迹规划，并从中选择性能最优的轨迹作为三维避障轨迹。

9.一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，其特征在于，所述处理器执行所述计算机程序时实现权利要求1至7中任一项所述方法的步骤。

10.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现权利要求1至7中任一项所述的方法的步骤。

Images