CN105929844B - 一种地外天体软着陆多障碍约束环境下避障方法 - Google Patents

Abstract

一种地外天体软着陆多障碍约束环境下避障方法，属于导航、制导与控制技术领域。本发明将在前者的基础上，针对在多障碍约束环境下的最优制导问题，提出一种基于凸优化的改进最优软着陆制导方法。首先，建立不加障碍约束的燃料最优二阶锥规划模型，并将其标准化；其次，针对火星表面凸起障碍进行分析建模，并进行线性转换，将非凸约束转化为凸约束，融入到二阶锥规划问题之中，并建立完整的考虑障碍约束的最优二阶锥规划模型；最后通过三种不同类型障碍约束的仿真分析来验证算法的正确性。将其应用到月球软着陆研究中，仿真结果表明新算法实现了三维空间障碍的有效规避，合理利用了障碍周围和上方的可飞行空间，同时满足燃料最优。

Description

一种地外天体软着陆多障碍约束环境下避障方法

技术领域

本发明属于导航、制导与控制技术领域，涉及一种多障碍约束环境下避障最优制导方法。

背景技术

目前，针对地外天体的软着陆过程最优制导问题已有多种方法运用其中，但在多障碍约束环境下的避障问题仍未有较好的解决方案提出。

下面给出的是针对地外天体的软着陆过程最优制导问题的一般解决方法，其过程为：

在整个着陆过程的接近段，着陆器已经离月表很近，此时可以忽略月球自传的影响，而将整个动力学模型建立在一个以着陆点为原点的月表固定坐标系中。月表固定坐标系的定义如图1示所示，ox轴垂直月表，ox,oy,oz三轴构成右手坐标系。

由于月表没有空气，且不考虑复杂的控制问题，只考虑制导问题，可得接近段的动力学方程：

其中，

r——为着陆器在oxyz系下的位置矢量，r＝[r_x r_y r_z]^T；

v——为着陆器在oxyz系下的速度矢量，v＝[v_x v_y v_z]^T；

a——为着陆器在oxyz系下的加速度矢量，a＝[a_x a_y a_z]^T；

g_m——为火星重力加速度；

T_c——为着陆器发动机的净推力矢量(指向着陆器中心轴线)，T_c＝||T_c||，为其大小；

m——为着陆器每时刻的质量；

α——为着陆器发动机燃耗率，大于0，(g_e为地球重力加速度，φ发动机安装角，即喷气方向与着陆器中心轴线的夹角，I_sp为发动机比冲)。

考虑边界条件、控制力大小约束、着陆器姿态角约束以及地表高度约束得到的轨迹燃料最优问题为：

目标函数：

满足式(1)的约束方程以及以下约束：

0<T₁≤T_c≤T₂ (3)

r_x≥0 (4)

其中T₁、T₂为控制力上下限；θ_alt如图1所示，称之为着陆器的倾斜角，为其给定的上限，该约束的作用是保证着陆器以一定的着陆角度着陆，而不在着陆之前碰上地面的小凸起。从以上可看出，现有的地外天体的软着陆最优制导过程针对多障碍约束环境下的避障问题给出有效地解决方案。

发明内容

本发明的目的在于提供了一种地外天体软着陆多障碍约束环境下避障方法，以解决地外天体的软着陆最优制导过程中多障碍约束环境下的避障问题。

本发明为解决上述技术问题采取的技术方案是：

凸起障碍模型选择

首先，要为月表上的凸起障碍选择一个空间几何模型，以便用数学的形式描述出来，使障碍成为整个轨迹优化问题中的一个状态约束。对于模型的选择，必须在总体上符合约束的外廓，且具有能够刻画绝大多数凸起障碍的能力。在这里，要使用凸优化的方法来进行障碍规避，还需考虑所选择的三维几何模型，在经过数学描述以及一定的数学变换后，能够并且能较为方便地转换为凸约束。综合上面的考虑，将月表凸起障碍描述成圆锥型约束，如图2所示。

图2所示(a)、(b)、(c)分别表示三种不通过的障碍类型，(a)为大顶角障碍，其半顶角较大，一般30°以上；(b)为小顶角障碍，一般30°以下；(c)则是(b)的一种特例，当圆锥的高度取得很大，远高于着陆器可能经过的空间点的高度，且其半顶角取得很小，这时，圆锥型的约束又可以近似的看作是圆柱型约束，用来描述那些近似为圆柱的凸起障碍。通过调整圆锥的高度和半顶角大小，来近似描述月表上的绝大多数凸起障碍，可以看出，将凸起障碍描述成圆锥型约束是具有通用性的。

凸起障碍模型数学描述

通过着陆器扫描处理之后，可以获得这些凸起障碍的位置和大致的高度信息，其中高度信息并不需要准确知道，一般取一个较为保守的估计值；圆锥的半顶角也可根据需要的安全裕度取一个估计值。总之，所取的圆锥体积比实际障碍体积越大，安全裕度愈高，越能规避障碍，但也会减小了着陆器的可飞行空间大小。

设圆锥高度为H_h，半顶角为α，在月表固定着陆坐标系中，圆锥顶点坐标为H＝(H_h H_y H_z)^T，任意时刻，着陆器在月表固定坐标系中的坐标为P＝(x y z)^T，则着陆器与通过圆锥高的单位向量的夹角的余弦值为：

其中n为圆锥高度方向上的单位矢量，即n＝(1 0 0)^T；β为P-H矢量与n矢量的夹角。

要使着陆器避开这些凸起障碍，着陆器的飞行轨迹不能经过这些圆锥，如图3和4所示，着陆器首先不能低于水平面飞行，其次是其与障碍顶点的连线必须在圆锥的母线之外，则需满足即

对(6)式进行变形得

由于着陆器最终的着陆点肯定低于障碍的高度，因此从而式(7)就变为

注意到式(8)所描述的约束是一个非凸性约束，这一约束的加入将使得整个轨迹优化及避障问题无法用凸优化方法求解，因此，有必要将其进行凸化转换。

障碍约束凸化转化

对上面得到的非凸性障碍约束进行凸化转换，以使得整个问题化为一个凸优化问题，更准确的是化为一个二阶锥规划问题。

将式(8)这一非凸约束化为一个凸性约束的总体思路是对含有的范数约束取一阶泰勒展 开项，使整个约束变成一个线性约束，由于线性约束是凸约束的一种，因此也就将障碍约束转换成了凸约束。

为方便求解，先将矢量写成分量形式：

P-H＝(x-H_h y-H_y z-H_z)

设f(P)＝norm(P-H)，

可得f(P)对P中各分量一阶导数以及对P的一阶导数f′(P)，

继续求导，得其二阶导数为

f″(P)为一个黑塞矩阵，其中

可以得到f(P)的一阶泰勒展开式

上式中，0<ξ<1，P_ξ＝P₀+ξ(P-P₀)。对(14)取其一阶线性项得

f(P)＝f(P₀)+f′(P₀)(P-P₀) (15)

这时，式(8)约束变成了一个线性约束

在忽略二阶余项的情况下，得到(16)式所描述的线性约束，式中的P₀可以取没有加入避障约束时得到的优化轨迹各时间节点处的着陆器位置，这样取P₀可以尽可能地减小余项，使转换后的线性约束更加贴近原约束。

加入障碍约束后的接近段着陆燃料最优问题

定义共有v_c＝n＝[1 0 0]^T，则约束可写成

其中，

将该约束加入到原问题当中，得到既有燃料最优，又能够实现三维空间避障的轨迹优化问题：

指标函数：

满足：

本发明提出的一种地外天体软着陆多障碍约束环境下避障制导方法考虑到未来月球探测任务的可靠性和安全性，精确定点软着陆以及有效的障碍规避将起到决定性的作用。之前已有相关方法就精确软着陆问题提出了基于凸规划的最优数值求解算法，本发明将在前者的基础上，针对在多障碍约束环境下的最优制导问题，提出一种基于凸优化的改进最优软着陆制导算法。

本发明的主要优点体现在：给出了月表凸起障碍的圆锥几何数学模型，给出了该圆锥障碍模型的数学描述，继而通过非凸向线性转化，使得圆锥障碍约束融入到二阶锥规划问题中，做到同时考虑燃料最优以及障碍规避，可以实现三维空间的障碍规避，有效利用了障碍周围和上面的可飞行空间，不再是以平面约束的形式实现障碍规避，浪费了障碍约束的上部空间。

本发明创新点在于：针对地天表面的凸起障碍进行分析建模，并完成线性转化，将非凸约束转化为凸约束，融入到二阶锥规划问题之中，并建立完整的考虑障碍约束的最优二阶锥规划模型，通过三种不同类型障碍约束的仿真分析验证了方法的正确性，有实际应用价值。

本发明，首先，建立不加障碍约束的燃料最优二阶锥规划模型，并将其标准化；其次， 针对火星表面凸起障碍进行分析建模，并进行线性转换，将非凸约束转化为凸约束，融入到二阶锥规划问题之中，并建立完整的考虑障碍约束的最优二阶锥规划模型；最后通过三种不同类型障碍约束的仿真分析来验证算法的正确性。将其应用到月球软着陆研究中，仿真结果表明新算法实现了三维空间障碍的有效规避，合理利用了障碍周围和上方的可飞行空间，同时满足燃料最优。

本发明对地外天体典型的凸起障碍进行了数学建模；对凸起障碍进行了非线性到线性的转化，非凸向凸的转化；建立了加入障碍约束后的接近段着陆燃料最优问题，对传统的软着陆最优制导问题提出了新的思路；通过对三种凸起障碍约束的选择，验证了所提方法的正确性及在使用中的可行性。

附图说明

图1是月表固定坐标系，图2是约束模型示意图，图2中(a)、(b)、(c)分别表示三种不通过的障碍类型，(a)为大顶角障碍，(b)为小顶角障碍，(c)则是(b)的一种特例。图3约束模型示意图；图4近似处理与处理前可飞行区域对比；图5是速度变化曲线图；图6是推理阀门控制曲线图；图7是无避障轨迹俯视图；图8是有避障轨迹俯视图；图9是障碍1避障等高散点图；图10是障碍1无避障等高散点图；图11是障碍2避障等高散点图；图12是障碍2无避障等高散点图。

具体实施方式

具体实施方式一：如图2至4所示，本实施方式所述的一种地外天体软着陆多障碍约束环境下避障方法的实现过程为：

步骤一、针对地外天体表面的凸起障碍进行分析并构建凸起障碍数学模型；

步骤二、将所述凸起障碍数学模型进行线性转化，将非凸约束转化为凸约束；

步骤三、将线性转化后的凸起障碍数学模型融入到二阶锥规划问题之中，并建立完整的考虑障碍约束的最优二阶锥规划模型；

步骤四、利用完整的考虑障碍约束的最优二阶锥规划模型实现地外天体软着陆多障碍约束环境下的最优避障。

具体实施方式二：本实施方式中，步骤一中所述的针对地外天体表面的凸起障碍进行分析并构建凸起障碍数学模型，具体过程为：

步骤一一、凸起障碍模型选择

选择的凸起障碍模型在总体上符合约束的外廓，且具有能够刻画绝大多数凸起障碍的能 力；

使用凸优化的方法来进行障碍规避，所选择的三维几何模型能够转换为凸约束，将月球表面凸起障碍描述成圆锥型约束；

通过调整圆锥的高度和半顶角大小，来近似描述月表上的绝大多数凸起障碍，将凸起障碍描述成圆锥型约束是具有通用性；

步骤一二、凸起障碍模型数学描述

通过着陆器扫描处理之后，获得凸起障碍的位置和高度信息；

圆锥的半顶角根据需要的安全裕度取一个估计值；设圆锥高度为H_h，半顶角为α，在月表固定着陆坐标系中，圆锥顶点坐标为H＝(H_h H_y H_z)^T，任意时刻，着陆器在月表固定坐标系中的坐标为P＝(x y z)^T，则着陆器与通过圆锥高的单位向量的夹角的余弦值为：

其中n为圆锥高度方向上的单位矢量，即n＝(1 0 0)^T；β为P-H矢量与n矢量的夹角；

要使着陆器避开所述凸起障碍，着陆器的飞行轨迹不能经过这些圆锥，着陆器首先不能低于水平面飞行，其次是其与障碍顶点的连线必须在圆锥的母线之外，则需满足即

对(6)式进行变形得

由于着陆器最终的着陆点肯定低于障碍的高度，因此从而式(7)就变为

式(8)所描述的约束是一个非凸性约束，非凸性约束的加入将使得整个轨迹优化及避障问题无法用凸优化方法求解，因此，有必要将其进行凸化转换。

其他步骤与具体实施方式一相同。

具体实施方式三：如图2至4所示，本实施方式在步骤二中，所述的将所述凸起障碍数学模型进行线性转化，将非凸约束转化为凸约束(障碍约束凸化转化)，具体过程为：

对非凸性障碍约束进行凸化转换，以使得整个问题化为一个凸优化问题，更准确的是化为一个二阶锥规划问题；

将式(8)所示的非凸约束化为一个凸性约束的总体思路是对含有的范数约束取一阶泰勒展开项，使整个约束变成一个线性约束，由于线性约束是凸约束的一种，因此也就将障碍约束转换成了凸约束；

为方便求解，先将矢量写成分量形式：

P-H＝(x-H_h y-H_y z-H_z)

设f(P)＝norm(P-H)，

可得f(P)对P中各分量一阶导数以及对P的一阶导数f′(P)，

继续求导，得其二阶导数为

f″(P)为一个黑塞矩阵，其中

可以得到f(P)的一阶泰勒展开式

上式中，0<ξ<1，P_ξ＝P₀+ξ(P-P₀)；对(14)取其一阶线性项得

f(P)＝f(P₀)+f′(P₀)(P-P₀) (15)

这时，式(8)约束变成了一个线性约束

在忽略二阶余项的情况下，得到(16)式所描述的线性约束，式中的P₀取没有加入避障约束时得到的优化轨迹各时间节点处的着陆器位置，这样取P₀可以尽可能地减小余项，使转换后的线性约束更加贴近原约束。

f(P)是纯数学的泰勒展开，解释没有什么意义。这里的P就是代表着陆器在月表固定坐标系下的三维坐标。f″(P_ξ)就是2阶导数；ξ(P-P₀)为在P₀的某一邻域内。

其他步骤与具体实施方式一或二相同。

具体实施方式四：本实施方式在步骤三中，所述的将线性转化后的凸起障碍数学模型融入到二阶锥规划问题之中，并建立完整的考虑障碍约束的最优二阶锥规划模型，具体过程为：

定义共有v_c＝n＝[1 0 0]^T，则约束写成

式(17)就是对上面约束的变形推导，Q是中间变量，值为下边所述；

其中，

将该约束加入到原问题当中，得到燃料最优，并实现三维空间避障的轨迹优化问题的指标函数：

指标函数：

满足：

其中：

E_u＝[I₃ 0_3×1],E_x＝[I₆ 0_6×1],γ_k＝[0_4×4k I₆ 0_4×4(n-k)]_4×4(n+1),(k＝0,1,…,n)

A∈R^7×7,B∈R^7×4

其他步骤与具体实施方式一、二或三相同。

下面结合附图对本发明及其产生的技术效果作进一步详细地说明：

仿真参数设计

利用MATLAB yalmip优化工具箱进行仿真。仿真参数如表1。

表1初始参数设置

Table1 Initial parameters:

这里取两个障碍作为多个障碍仿真实例。如下：

障碍1：顶点坐标[500 40-83]，障碍半顶角10°，松弛因子为1。

障碍2：顶点坐标[1500 50-350]，障碍半顶角10°，松弛因子为1。

仿真结果分析

图5、6给出了着陆器的速度变化以及推力阀门控制曲线。最终的悬停坐标为[30_.8091 -0_.477208 -0_.266704]m，速度为[-0_.49179 0_.028592 0_.081788]m/s。满足要求范围，证明了约束的正确性。

图7、8给出了没有加障碍约束和加了障碍约束在三维空间中的优化轨迹及其平面投影。 从两个俯视图可以很明显的观察到，在加避障之前，原优化轨迹后程绝大部分在同一平面内，穿过两个大小障碍，当然没有穿过障碍的中心；加上避障之后，避障优化轨迹在后程并不在同一平面内，而是在两个大小障碍中间作规避飞行，并且大致可以看出是以绕飞的形式来规避障碍。

图9～12所示的四个等高散点图给出了加避障和未加避障飞行轨迹分别相对两个障碍的位置变化。没加避障之前，x＝961.5261到x＝673.482之间的点在障碍2内部，x＝366.541到x＝186.1168之间的点在障碍1内部，也即原优化轨迹有很大一部分处于两个障碍内部；加上避障之后，所有点都在相对应高度的圆之外，且按各节点的走势方向，相邻两点的连线都没有跟内圆(连线一侧半径更小的圆)有交点，也即不会有两点连线穿过障碍的情况，说明着陆器成功避开障碍，而且飞行轨迹与障碍比较贴合，充分利用了可飞行空间。

综上可得，通过加入圆锥型的障碍约束，利用二阶锥规划方法，实现了燃料最优和障碍规避两个目的。仿真给出了小顶角、大顶角、多障碍三种不同类型障碍的仿真结果，从上面的仿真结果看出，这种方法是比较理想的。

此外，一些高度很低的障碍也可以用以上模型作为障碍约束加进到整个优化问题中，但是一般约束作用很小，在飞行过程的绝大一部分起不到什么作用，只有在着陆器触地前的一段轨迹才会起约束作用，而这时基本可以把这些约束当成平面约束处理。

Claims (1)

1.一种地外天体软着陆多障碍约束环境下避障方法，其特征在于，所述方法的实现过程为：

步骤一、针对地外天体表面的凸起障碍进行分析并构建凸起障碍数学模型；

步骤二、将所述凸起障碍数学模型进行线性转化，将非凸约束转化为凸约束；

步骤三、将线性转化后的凸起障碍数学模型融入到二阶锥规划问题之中，并建立完整的考虑障碍约束的最优二阶锥规划模型；

步骤四、利用完整的考虑障碍约束的最优二阶锥规划模型实现地外天体软着陆多障碍约束环境下的最优避障；

步骤一中所述的针对地外天体表面的凸起障碍进行分析并构建凸起障碍数学模型，具体过程为：

步骤一一、凸起障碍模型选择

选择的凸起障碍模型在总体上符合约束的外廓，且具有能够刻画绝大多数凸起障碍的能力；

使用凸优化的方法来进行障碍规避，所选择的三维几何模型能够转换为凸约束，将月球表面凸起障碍描述成圆锥型约束；

通过调整圆锥的高度和半顶角大小，来近似描述月表上的绝大多数凸起障碍，将凸起障碍描述成圆锥型约束是具有通用性；

步骤一二、凸起障碍模型数学描述

通过着陆器扫描处理之后，获得凸起障碍的位置和高度信息；

圆锥的半顶角根据需要的安全裕度取一个估计值；设圆锥高度为H_h，半顶角为α，在月表固定着陆坐标系中，圆锥顶点坐标为H＝(H_h H_y H_z)^T，任意时刻，着陆器在月表固定坐标系中的坐标为P＝(x y z)^T，则着陆器与通过圆锥高的单位向量的夹角的余弦值为：

其中n为圆锥高度方向上的单位矢量，即n＝(1 0 0)^T；β为P-H矢量与n矢量的夹角；

要使着陆器避开所述凸起障碍，着陆器的飞行轨迹不能经过这些圆锥，着陆器首先不能低于水平面飞行，其次是其与障碍顶点的连线必须在圆锥的母线之外，则需满足即

对(6)式进行变形得

由于着陆器最终的着陆点肯定低于障碍的高度，因此从而式(7)就变为

式(8)所描述的约束是一个非凸性约束，非凸性约束的加入将使得整个轨迹优化及避障问题无法用凸优化方法求解，因此，有必要将其进行凸化转换；

步骤二中，所述的将所述凸起障碍数学模型进行线性转化，将非凸约束转化为凸约束，具体过程为：

对非凸性障碍约束进行凸化转换，以使得整个问题化为一个凸优化问题，更准确的是化为一个二阶锥规划问题；

将式(8)所示的非凸约束化为一个凸性约束的总体思路是对含有的范数约束取一阶泰勒展开项，使整个约束变成一个线性约束，由于线性约束是凸约束的一种，因此也就将障碍约束转换成了凸约束；

为方便求解，先将矢量写成分量形式：

P-H＝(x-H_h y-H_y z-H_z)

设f(P)＝norm(P-H)，

可得f(P)对P中各分量一阶导数以及对P的一阶导数f′(P)，

继续求导，得其二阶导数为

f″(P)为一个黑塞矩阵，其中

可以得到f(P)的一阶泰勒展开式

上式中，0<ξ<1，P_ξ＝P₀+ξ(P-P₀)；对(14)取其一阶线性项得

f(P)＝f(P₀)+f′(P₀)(P-P₀) (15)

这时，式(8)约束变成了一个线性约束

在忽略二阶余项的情况下，得到(16)式所描述的线性约束，式中的P₀取没有加入避障约束时得到的优化轨迹各时间节点处的着陆器位置，这样取P₀可以尽可能地减小余项，使转换后的线性约束更加贴近原约束；

步骤三中，所述的将线性转化后的凸起障碍数学模型融入到二阶锥规划问题之中，并建立完整的考虑障碍约束的最优二阶锥规划模型，具体过程为：

定义共有v_c＝n＝[1 0 0]^T，则约束写成

其中，

将该约束加入到原问题当中，得到燃料最优，并实现三维空间避障的轨迹优化问题的指标函数：

指标函数：

满足：

||SΨ_kp+S(Φ_ky₀+Λ_kg₄)||≤-c^TΨ_kp-c^T(Φ_ky₀+Λ_kg₄)

其中：

c＝[-tan(θ_alt) 0 0 0 0 0 0]^T

e_σ＝[0_1×31]^T,e_z＝[0_1×6 1]^T,e_h＝[1 0_1×6]^T,E_u＝[I₃ 0_3×1],E_x＝[I₆ 0_6×1],Υ_k＝[0_4×4k I₆ 0_4×4(n-k)]_4×4(n+1),(k＝0,1,…,n)A∈R^7×7,B∈R^7×4。