CN102981507B - 一种软着陆自主障碍规避常推力器控制方法 - Google Patents

Abstract

本发明涉及一种软着陆自主障碍规避常推力器控制方法，属于深空探测技术领域。本发明首先利用探测器下降过程中的光学相机以及自带的障碍检测装置形成的地表障碍信息选择目标着陆点。然后利用光学相机以及高度计、速度计等导航设备获取当前探测器位置以及姿态信息，并建立含推力器的动力学方程；根据事先确定的障碍规避轨迹建立参考动力学方程。最后，通过稳定误差动力学方程，实时控制常推力器开关，完成跟踪标称轨迹，使探测器安全、平稳下降到目标着陆区域。具有算法简单、计算量小且控制精度高、燃料消耗少的特点。

Description

一种软着陆自主障碍规避常推力器控制方法

技术领域

本发明涉及一种软着陆自主障碍规避常推力器控制方法，属于深空探测技术领域。

背景技术

随着行星际探测任务的日益增多，探测器飞越、绕飞以及软着陆目标天体已经成为未来深空科学探测的重要任务和课题。为了能够对更具科学价值的区域进行研究和采样，希望探测器能够在具有多样地貌(岩石、弹坑和陡坡)的危险区域安全着陆。由于深空探测任务中目标天体与地球距离遥远，存在较长的通讯延迟，而障碍规避过程对探测器实时机动能力有较高要求，因此，探测器需具备自主障碍规避的能力。着陆过程中需要采用光学导航、高度计等多种传感器确定当前状态，而由于姿态的剧烈变化会严重影响测量精度，引发碰撞危险，因此自主障碍规避过程同时要稳定探测器姿态。由于目前探测器姿态和轨道控制过程均是采用常推力推力器实现，因此制导系统给出连续的推力和力矩要通过推力器的开关来实现。因此完成真正意义上的深空探测器软着陆障碍规避控制器要充分考虑实际情况的影响。目前已实施任务中仍没有进行过障碍规避控制，而软着陆障碍规避的控制技术是直接关系到深空探测任务危险地区着陆的关键技术，对任务的成败有重要作用，因此深空探测器软着陆自主障碍规避的控制方法是当前各国航天科研部门终点发展的研究方向之一。

在已发展的深空探测器软着陆自主障碍规避控制方法中，在先技术[1](参见朱圣英、崔平远、崔祜涛,基于LIDAR的月球软探测器障碍检测规避方法，中国宇航学会深空探测技术专业委员会第三届学术会议,中国陕西西安,2006年11月)，研究了基于激光扫描雷达进行障碍检测并进行障碍规避的控制算法。该算法根据LIDAR数据拟合出着陆区地形平面，并检测出障碍的类型、位置、大小等信息。将障碍对探测器的威胁表达成了危险势函数的形式，通过李雅普诺夫方法设计了控制律。由于控制过程考虑了障碍的威胁，因此这种控制方法能有效的避开障碍，但是在推力器实现的过程中，通过采用对控制律分段积分，然后将常推力等效逼近的方式实现，其中积分过程降低了该算法的执行效率。

在先技术[2](参见EdwardC.WongandGurkirpalSinghetal.,GuidanceandControlDesignforHazardAvoidanceandSafeLandingonMars[J].JournalofSpacecraftAndRockets,2006,43(2):378-384)，美国NASA下属JPL实验室与约翰逊空间中心联合开发的一套探测器着陆过程中的障碍规避控制算法。该套算法中所利用的障碍规避控制方法仍然是采用事先选择出着陆点，利用导航系统给出的探测器当前位置与速度信息规划规避轨迹，该轨迹也是通过多项式的形式完成两点边值问题。该控制方法是采用位置三次多项式的形式，每隔一段时间便利用导航信息进行轨迹规划，在规划间隔的时间内控制探测器沿上一条规划轨迹下降。该算法在保留了算法简单、计算时间少等特点外，还具有一定的鲁棒性，但是该算法采用的是变推力发动机，实际工程实现有较大的难度。

发明内容

本发明的目的是针对现有软着陆障碍规避控制方法存在的变推力器工程实现难，常规积分等效方法影响控制方法执行效率等问题，提供了一种软着陆自主障碍规避常推力器控制方法，易于工程实现、有较高控制精度。

本发明的技术方案为：首先利用探测器下降过程中的光学相机以及自带的障碍检测装置形成的地表障碍信息选择目标着陆点。然后利用光学相机以及高度计、速度计等导航设备获取当前探测器位置以及姿态信息，并建立含推力器的动力学方程；根据事先确定的障碍规避轨迹建立参考动力学方程。最后，通过稳定误差动力学方程，实时控制常推力器开关，完成跟踪标称轨迹，使探测器安全、平稳下降到目标着陆区域。

一种软着陆自主障碍规避常推力器控制方法，具体实现步骤如下：

步骤1，在探测器下降过程中，测得障碍的位置x_i,y_i和大小z_i。根据障碍分布，确定目标着陆点。

步骤2，在探测器本体系下三个主轴正反方向各安置一个常推力推力器，共布设六个推力器；分析打开推力器生成力的情况，建立包含推力器的系统动力学模型:

$\underset{\‾}{\stackrel{\·\·}{\mathrm{\ρ}}}=h(\underset{\‾}{\mathrm{\ρ}},\underset{\‾}{\stackrel{\·}{\mathrm{\ρ}}})+\frac{1}{m}{R_L}_S{\left(\underset{\‾}{\mathrm{\σ}}\right)}^S{F}_C---\left(1\right)$

其中，ρ为探测器当前位置向量，分别为探测器当前位置向量的一阶导数和二阶导数，g为目标天体产生的引力加速度；探测器本体坐标系到着陆点坐标系的转换矩阵着陆点坐标系到探测器本体坐标系的转换矩阵m为探测器的质量， $\[\underset{\‾}{\widetilde{\mathrm{\σ}}}\]=\left[\begin{array}{ccc}0& -{\mathrm{\σ}}_3& {\mathrm{\σ}}_2\\ {\mathrm{\σ}}_3& 0& -{\mathrm{\σ}}_1\\ -{\mathrm{\σ}}_2& {\mathrm{\σ}}_1& 0\end{array}\right],$ σ＝[σ₁σ₂σ₃]^T为修正罗德里格参数表达的当前姿态量。

探测器本体坐标系下生成推力^SF_C为：

^SF_C＝u_aHu(2)

其中u_a为常推力大小， $H=\left[\begin{array}{cccccc}-1& 0& 0& 1& 0& 0\\ 0& -1& 0& 0& 1& 0\\ 0& 0& -1& 0& 0& 1\end{array}\right],$ u＝[u₁,u₂,u₃,u₄,u₅,u₆]^T为推力器开关状态向量，其中第i个元素

所述探测器本体坐标系Σ_s:o_s-x_sy_sz_s，其原点o_s在探测器的质心处，o_sx_s轴、o_sy_s轴及o_sz_s轴分别与探测器的三个惯量主轴重合，该坐标系反映着陆过程中探测器的空间姿态。

所述着陆点坐标系Σ_l:o_l-x_ly_lz_l，其原点o_l位于预定的着陆点，o_lx_l沿经线的切线指向南极方向，o_lz_l和从行星质心指向着陆点的矢量o_ao_l方向一致，o_ly_l与o_lx_l、o_lz_l之间满足右手系法则。

步骤3，根据当前位置到着陆点的规划轨迹，形成参考动力学方程

${\underset{\‾}{\stackrel{\·\·}{\mathrm{\ρ}}}}_m+K_1{\underset{\‾}{\stackrel{\·}{\mathrm{\ρ}}}}_m+K_2{\underset{\‾}{\mathrm{\ρ}}}_m={\underset{\‾}{v}}_{\mathrm{\ρ}c}---\left(3\right)$

其中，v _ρc为参考动力学方程的输入量，ρ _m为输出的三维参考位置向量，分别为输出的三维参考速度向量和参考加速度向量，K₁，K₂是3×3的对称的正定矩阵。

步骤4，将动力学模型和参考动力学模型做差，定义状态误差变量ε＝ρ-ρ _m，求导得到令 $\underset{\‾}{e}=\left[\begin{array}{cc}\underset{\‾}{\mathrm{\ϵ}}& \underset{\‾}{\stackrel{\·}{\mathrm{\ϵ}}}\end{array}\right],$ 得到线性跟踪误差方程：

$\underset{\‾}{\stackrel{\·}{e}}=A_m\underset{\‾}{e}+B\left(\underset{\‾}{\mathrm{\σ}}\right)\left(H\underset{\‾}{u}-\underset{\‾}{\mathrm{\ω}}\right)---\left(4\right)$

其中 $A_m=\left[\begin{array}{cc}{0}_{3\×3}& I_{3\×3}\\ -K_1& -K_2\end{array}\right];B\left(\underset{\‾}{\mathrm{\σ}}\right)={\left[\begin{array}{c}{0}_{3\×3}\\ \frac{1}{m}{R_L}_s\left(\underset{\‾}{\mathrm{\σ}}\right)\end{array}\right]}^T;$ ω＝m^LR_s(-σ)(v _ρc-v _ρl)；

${\underset{\‾}{v}}_{\mathrm{\ρ}l}=h(\underset{\‾}{\mathrm{\ρ}},\underset{\‾}{\stackrel{\·}{\mathrm{\ρ}}})+K_1\underset{\‾}{\stackrel{\·}{\mathrm{\ρ}}}+K_2\underset{\‾}{\mathrm{\ρ}};$

计算 $\mathrm{\Δ}={\mathrm{\β}}_{\mathrm{\ρ}}^T\underset{\‾}{u}-{\mathrm{\δ}}_F---\left(5\right)$

其中 ${\mathrm{\β}}_{\mathrm{\ρ}}^T=u_a{\mathrm{\γ}}_{\mathrm{\ρ}}H,$ δ_F＝γ_ρ ω， ${\mathrm{\γ}}_{\mathrm{\ρ}}=\frac{1}{m}{\underset{\‾}{e}}^T{K}_{\mathrm{\ρ}}^T{R_L}_s\left(\underset{\‾}{\mathrm{\σ}}\right),$ 给定任意6×6正定矩阵Q，则根据 $A_m^{T}P+{\mathrm{PA}}_m=-Q,$ 求得正定对称矩阵 $P=\left[\begin{array}{cc}{P}_{\mathrm{\ρ}1}& P_{\mathrm{\ρ}2}\\ P_{\mathrm{\ρ}2}& P_{\mathrm{\ρ}3}\end{array}\right],$ P_ρ1、P_ρ2、P_ρ3和P_ρ4分别为3×3矩阵，取K_ρ＝[P_ρ2P_ρ3]为3×6矩阵；

令为如下向量：

则可表达为

当δ_F≥0时，Δ＜0，则无需打开推力器；

当δ_F＜0时，设计推力器控制量：

a)初始化新向量 其中，为1×6向量，和为3×1向量，和新变量n，令n＝1、 ${\widehat{\mathrm{\β}}}_{\mathrm{\ρ}}^T=\stackrel{\→}{0}.$

b)令A_j取向量中绝对值最大的元素，j为绝对值最大的量在向量中的位置，j的取值范围为1,2,3；然后令向量中第j个元素为A_j,令中第j个元素为0；

c)计算 分别为向量的3个元素；

d)n＝n+1,计算 $\underset{\&OverBar;}{\hat{u}}={\left[\begin{array}{cccc}S\left({\widehat{\mathrm{\&beta;}}}_{\mathrm{\&rho;}1}\right)& S\left({\widehat{\mathrm{\&beta;}}}_{\mathrm{\&rho;}2}\right)& \mathrm{...}& S\left({\widehat{\mathrm{\&beta;}}}_{\mathrm{\&rho;}6}\right)\end{array}\right]}^T,$ 其中 $S\left({\widehat{\mathrm{\&beta;}}}_{\mathrm{\&rho;}i}\right)=\left\{\begin{array}{cc}1& {\widehat{\mathrm{\&beta;}}}_{\mathrm{\&rho;}i}<0\\ 0& {\widehat{\mathrm{\&beta;}}}_{\mathrm{\&rho;}i}\&GreaterEqual;0\end{array}\right.$ 为切换函数；其中i＝1,…,6；分别为向量的6个元素；

e)如果Δ＜0或n＞3，则停止，输出当前的推力器控制向量 $\underset{\&OverBar;}{\hat{u}}={\left[\begin{array}{cccc}S\left({\widehat{\mathrm{\&beta;}}}_{\mathrm{\&rho;}1}\right)& S\left({\widehat{\mathrm{\&beta;}}}_{\mathrm{\&rho;}2}\right)& \mathrm{...}& S\left({\widehat{\mathrm{\&beta;}}}_{\mathrm{\&rho;}6}\right)\end{array}\right]}^T,$ 否则，返回b)；

步骤5，将步骤4得到的输入步骤2中包含推力器开关的系统动力学模型，实时控制探测器，完成基于常推力器开关的障碍规避轨迹跟踪。

有益效果

本发明给出的软着陆障碍规避常推力器控制方法具有实现跟踪规划轨迹的效果，且避免了复杂的推力器分配过程与变推力器工程上实现困难的难题，具有算法简单、计算量小且控制精度高的特点。

针对某障碍地形选择了目标着陆点，并根据规划的障碍规避轨迹，完成了该发明的仿真验证。仿真结果显示，本发明采用的常推力器控制方法能够满足实时控制的工程需求，能有效提高计算效率，而且跟踪轨迹的精度仍能够稳定在0.1m以内。

通过当前状态与参考动力学模型的误差信息，该控制方法能够实时的确定推力器的开关状态，且考虑到燃料问题，该方法能在每个制导周期打开最少的推力器数量，从而达到减少燃料的目的。

附图说明

图1为本发明方法的流程图；

图2为具体实施方式中着陆区障碍地形图；

图3为具体实施方式中规划的标称三维轨迹；

图4为具体实施方式中规划的标称三维速度曲线；

图5为具体实施方式中实际的三维位置曲线；

图6为具体实施方式中实际的三维速度曲线；

图7为具体实施方式中三维位置跟踪误差；

图8为具体实施方式中三维速度跟踪误差。

具体实施方式

为了更好的说明本发明的目的和优点，下面结合附图和实施例对本发明内容做进一步说明。

本发明提出了一种软着陆障碍规避常推力器控制方法，利用地形信息选择目标着陆点；利用探测器当前位置与速度信息以及规划的障碍规避轨迹，形成误差动力学模型；通过对误差动力学模型的稳定形成常推力器的控制指令，完成自主软着陆障碍规避过程。具体流程如图1所示，包括如下步骤：

1)通过探测器下降过程中的光学相机以及自带的障碍检测装置能够测得障碍的位置x_i,y_i，障碍的大小z_i信息。根据障碍分布信息，确定目标着陆点，选取的安全着陆点为着陆区域内与其最近障碍点距离最大的点，该点能够保证其附近的安全着陆区最大；

2)探测器本体系下三个主轴正反方向各安置一个常推力推力器，此时探测器本体坐标系下生成推力^SF_C为：

^SF_C＝u_aHu(7)

其中u_a为常推力大小， $H=\left[\begin{array}{cccccc}-1& 0& 0& 1& 0& 0\\ 0& -1& 0& 0& 1& 0\\ 0& 0& -1& 0& 0& 1\end{array}\right],$ u＝[u₁,u₂,u₃,u₄,u₅,u₆]^T为推力器开关状态向量，其中第i个元素

考虑最终的障碍规避时间较短，同时目标天体的自旋角速度也较小，在着陆点坐标系，探测器的动力学方程表示为：

$\underset{\&OverBar;}{\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&rho;}}}=h(\underset{\&OverBar;}{\mathrm{\&rho;}},\underset{\&OverBar;}{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}})+\frac{1}{m}{R_L}_S{\left(\underset{\&OverBar;}{\mathrm{\&sigma;}}\right)}^S{F}_C---\left(8\right)$

其中^LR_S(σ)为探测器本体坐标系到着陆点坐标系的转换矩阵，g为目标天体产生的引力加速度。

通过探测器的导航系统测得探测器当前位置向量ρ，分别为探测器当前位置向量的一阶导数与二阶导数。

3)根据当前位置形成的规划轨迹，形成参考动力学方程

${\underset{\&OverBar;}{\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&rho;}}}}_m+K_1{\underset{\&OverBar;}{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}}_m+K_2{\underset{\&OverBar;}{\mathrm{\&rho;}}}_m={\underset{\&OverBar;}{v}}_{\mathrm{\&rho;}c}---\left(9\right)$

其中ρ _m为参考动力学方程的输出量，K₁，K₂是3×3的对称的正定矩阵， 参考位置向量的一阶导数与二阶导数。v _ρc为参考动力学方程的输入量，通过下式获得v _ρc：

${\underset{\&OverBar;}{v}}_{\mathrm{\&rho;}c}={\underset{\&OverBar;}{\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&rho;}}}}_d+K_1{\underset{\&OverBar;}{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}}_d+K_2{\underset{\&OverBar;}{\mathrm{\&rho;}}}_d---\left(10\right)$

其中ρ _d、分别为规划的轨迹及其一阶导数和二阶导数。

步骤4，将动力学模型和参考动力学模型做差，定义状态误差变量ε＝ρ-ρ _m，求导得到令 $\underset{\&OverBar;}{e}=\left[\begin{array}{cc}\underset{\&OverBar;}{\mathrm{\&epsiv;}}& \underset{\&OverBar;}{\stackrel{\&CenterDot;}{\mathrm{\&epsiv;}}}\end{array}\right],$ 得到线性跟踪误差方程：

$\underset{\&OverBar;}{\stackrel{\&CenterDot;}{e}}=A_m\underset{\&OverBar;}{e}+B\left(\underset{\&OverBar;}{\mathrm{\&sigma;}}\right)\left(H\underset{\&OverBar;}{u}-\underset{\&OverBar;}{\mathrm{\&omega;}}\right)---\left(4\right)$

其中 $A_m=\left[\begin{array}{cc}{0}_{3\&times;3}& I_{3\&times;3}\\ -K_1& -K_2\end{array}\right];B\left(\underset{\&OverBar;}{\mathrm{\&sigma;}}\right)={\left[\begin{array}{c}{0}_{3\&times;3}\\ \frac{1}{m}{R_L}_s\left(\underset{\&OverBar;}{\mathrm{\&sigma;}}\right)\end{array}\right]}^T;$ ω＝m^LR_s(-σ)(v _ρc-v _ρl)；

${\underset{\&OverBar;}{v}}_{\mathrm{\&rho;}l}=h\left(\underset{\&OverBar;}{\mathrm{\&rho;}},\underset{\&OverBar;}{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}\right)+K_1\underset{\&OverBar;}{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}+K_2\underset{\&OverBar;}{\mathrm{\&rho;}};$

计算 $\mathrm{\&Delta;}={\mathrm{\&beta;}}_{\mathrm{\&rho;}}^T\underset{\&OverBar;}{u}-{\mathrm{\&delta;}}_F---\left(5\right)$

其中 ${\mathrm{\&beta;}}_{\mathrm{\&rho;}}^T=u_a{\mathrm{\&gamma;}}_{\mathrm{\&rho;}}H,$ δ_F＝γ_ρ ω， ${\mathrm{\&gamma;}}_{\mathrm{\&rho;}}=\frac{1}{m}{\underset{\&OverBar;}{e}}^T{K}_{\mathrm{\&rho;}}^T{R_L}_s\left(\underset{\&OverBar;}{\mathrm{\&sigma;}}\right),$ 给定任意6×6正定矩阵Q，则根据求得正定对称矩阵 $P=\left[\begin{array}{cc}{P}_{\mathrm{\&rho;}1}& P_{\mathrm{\&rho;}2}\\ P_{\mathrm{\&rho;}2}& P_{\mathrm{\&rho;}3}\end{array}\right],$ P_ρ1、P_ρ2、P_ρ3和P_ρ4分别为3×3矩阵，取K_ρ＝[P_ρ2P_ρ3]为3×6矩阵；

令为如下向量：

则可表达为

当δ_F≥0时，Δ＜0，则无需打开推力器；

当δ_F＜0时，设计推力器控制量：

a)初始化新向量 其中，为1×6向量，和为3×1向量，和新变量n，令n＝1、 ${\widehat{\mathrm{\&beta;}}}_{\mathrm{\&rho;}}^T=\stackrel{\&RightArrow;}{0}.$

b)令A_j取向量中绝对值最大的元素，j为绝对值最大的量在向量中的位置，j的取值范围为1,2,3；然后令向量中第j个元素为A_j,令中第j个元素为0；

c)计算 分别为向量的3个元素；

d)n＝n+1,计算 $\underset{\&OverBar;}{\hat{u}}={\left[\begin{array}{cccc}S\left({\widehat{\mathrm{\&beta;}}}_{\mathrm{\&rho;}1}\right)& S\left({\widehat{\mathrm{\&beta;}}}_{\mathrm{\&rho;}2}\right)& \mathrm{...}& S\left({\widehat{\mathrm{\&beta;}}}_{\mathrm{\&rho;}6}\right)\end{array}\right]}^T,$ 其中 $S\left({\widehat{\mathrm{\&beta;}}}_{\mathrm{\&rho;}i}\right)=\left\{\begin{array}{cc}1& {\widehat{\mathrm{\&beta;}}}_{\mathrm{\&rho;}i}<0\\ 0& {\widehat{\mathrm{\&beta;}}}_{\mathrm{\&rho;}i}\&GreaterEqual;0\end{array}\right.$ 为切换函数；其中i＝1,…,6；分别为向量的6个元素；

e)如果Δ＜0或n＞3，则停止，输出当前的推力器控制向量 $\underset{\&OverBar;}{\hat{u}}={\left[\begin{array}{cccc}S\left({\widehat{\mathrm{\&beta;}}}_{\mathrm{\&rho;}1}\right)& S\left({\widehat{\mathrm{\&beta;}}}_{\mathrm{\&rho;}2}\right)& \mathrm{...}& S\left({\widehat{\mathrm{\&beta;}}}_{\mathrm{\&rho;}6}\right)\end{array}\right]}^T,$ 否则，返回b)；

步骤5，将步骤4得到的输入步骤2中包含推力器开关的系统动力学模型，对探测器进行实时控制，完成基于常推力器开关的障碍规避轨迹跟踪。

为了验证上述方法和公式的可行性和可靠性，本实施例针对着陆小天体为例进行了数学仿真。在20m高处进行障碍规避。着陆区地形如图2所示，图中地形规格为20m×20m，具有的障碍为14个岩石与弹坑，这些障碍信息如表2中所示，选取的目标着陆点为r₀＝[-1.17,-3.03,0.15]，初始位置ρ₀＝[0,0,20]，初始速度小天体引力g＝[0,0,0.001]，推力大小u_a＝15，m＝200kg。

选取的正定对称矩阵：

$Q=\left[\begin{array}{cccccc}4\&times;{10}^6& 0& 0& 0& 0& 0\\ 0& 4\&times;{10}^6& 0& 0& 0& 0\\ 0& 0& 4\&times;{10}^6& 0& 0& 0\\ 0& 0& 0& 4\&times;{10}^6& 0& 0\\ 0& 0& 0& 0& 4\&times;{10}^6& 0\\ 0& 0& 0& 0& 0& 4\&times;{10}^6\end{array}\right]$

参考动力学模型的系数矩阵为：

$K_1=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 0.5\end{array}\right]K_2=\left[\begin{array}{ccc}4.5\&times;{10}^{-4}& 0& 0\\ 0& 4.5\&times;{10}^{-4}& 0\\ 0& 0& 6\&times;{10}^{-4}\end{array}\right]$

表2障碍位置及大小信息

图3为根据地形规划的三维位置轨迹，子图中分别为x轴、y轴、z轴的位置；图4为根据规划轨迹得到的速度轨迹，子图中分别为x轴、y轴、z轴的速度；图5为闭环仿真得到的实际位置曲线，三个子图对应的分别为实际着陆过程的x轴、y轴、z轴的位置，结果表明给出规划轨迹后，按照本方法能够实现很好跟踪；图6为闭环仿真得到的实际速度曲线，结果表明能够速度将为0m/s,实现障碍区的软着陆；从图7和图8可以看出，通过采用的常推力控制方法能够很好的跟踪规划轨迹，位置跟踪误差能够保证在0.1m之内,速度能够几乎完全跟踪。通过上述附图可以证明，本发明的软着陆自主障碍规避常推力控制方法能够很好跟踪规划轨迹，且避免了算法的复杂性。

Claims (4)

1.一种软着陆自主障碍规避常推力器控制方法，其特征在于：包括以下步骤：

步骤1，根据障碍分布，确定目标着陆点；

步骤2，在探测器本体系下三个主轴正反方向各安置一个常推力推力器；分析打开推力器生成力的情况，建立包含推力器的系统动力学模型:

$\underset{\&OverBar;}{\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&rho;}}}=h\left(\underset{\&OverBar;}{\mathrm{\&rho;}},\stackrel{\&CenterDot;}{\underset{\&OverBar;}{\mathrm{\&rho;}}}\right)+\frac{1}{m}{R_L}_S\left(\underset{\&OverBar;}{\mathrm{\&sigma;}}\right){F_S}_C---\left(1\right)$

其中，ρ为探测器当前位置向量，分别为探测器当前位置向量的一阶导数和二阶导数，g为目标天体产生的引力加速度；探测器本体坐标系到着陆点坐标系的转换矩阵着陆点坐标系到探测器本体坐标系的转换矩阵m为探测器的质量， $\&lsqb;\underset{\&OverBar;}{\widetilde{\mathrm{\&sigma;}}}\&rsqb;=\left[\begin{array}{ccc}0& -{\mathrm{\&sigma;}}_3& {\mathrm{\&sigma;}}_2\\ {\mathrm{\&sigma;}}_3& 0& -{\mathrm{\&sigma;}}_1\\ -{\mathrm{\&sigma;}}_2& {\mathrm{\&sigma;}}_1& 0\end{array}\right],$ σ＝[σ₁σ₂σ₃]^T为修正罗德里格参数表达的当前姿态量；

探测器本体坐标系下生成推力^SF_C为：

^SF_C＝u_aHu(2)

其中u_a为常推力大小， $H=\left[\begin{array}{cccccc}-1& 0& 0& 1& 0& 0\\ 0& -1& 0& 0& 1& 0\\ 0& 0& -1& 0& 0& 1\end{array}\right],$ u＝[u₁,u₂,u₃,u₄,u₅,u₆]^T为推力器开关状态向量，其中第i个元素

步骤3，根据当前位置到着陆点的规划轨迹，形成参考动力学方程

${\underset{\&OverBar;}{\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&rho;}}}}_m+K_1{\stackrel{\&CenterDot;}{\underset{\&OverBar;}{\mathrm{\&rho;}}}}_m+K_2{\underset{\&OverBar;}{\mathrm{\&rho;}}}_m={\underset{\&OverBar;}{v}}_{\mathrm{\&rho;}c}---\left(3\right)$

其中，v _ρc为参考动力学方程的输入量，ρ _m为输出的三维参考位置向量， 分别为输出的三维参考速度向量和参考加速度向量，K₁，K₂是3×3的对称的正定矩阵；

步骤4，将动力学模型和参考动力学模型做差，定义状态误差变量ε＝ρ-ρ _m，求导得到令得到线性跟踪误差方程：

$\underset{\&OverBar;}{\stackrel{\&CenterDot;}{e}}=A_m\underset{\&OverBar;}{e}+B\left(\underset{\&OverBar;}{\mathrm{\&sigma;}}\right)(H\underset{\&OverBar;}{u}-\underset{\&OverBar;}{\mathrm{\&omega;}})---\left(4\right)$

其中 $A_m=\left[\begin{array}{cc}{0}_{3\&times;3}& I_{3\&times;3}\\ -K_1& -K_2\end{array}\right];$ $B\left(\underset{\&OverBar;}{\mathrm{\&sigma;}}\right)={\left[\begin{array}{c}{0}_{3\&times;3}\\ \frac{1}{m}{R_L}_s\left(\underset{\&OverBar;}{\mathrm{\&sigma;}}\right)\end{array}\right]}^T;$ ω＝m^LR_s(-σ)(v _ρc-v _ρl)；

${\underset{\&OverBar;}{v}}_{\mathrm{\&rho;}l}=h(\underset{\&OverBar;}{\mathrm{\&rho;}},\stackrel{\&CenterDot;}{\underset{\&OverBar;}{\mathrm{\&rho;}}})+K_1\stackrel{\&CenterDot;}{\underset{\&OverBar;}{\mathrm{\&rho;}}}+K_2\underset{\&OverBar;}{\mathrm{\&rho;}};$

计算 $\mathrm{\&Delta;}={\mathrm{\&beta;}}_{\mathrm{\&rho;}}^T\underset{\&OverBar;}{u}-{\mathrm{\&delta;}}_F---\left(5\right)$

其中δ_F＝γ_ρ ω，给定任意6×6正定矩阵Q，则根据求得正定对称矩阵 $P=\left[\begin{array}{cc}{P}_{\mathrm{\&rho;}1}& P_{\mathrm{\&rho;}2}\\ P_{\mathrm{\&rho;}2}& P_{\mathrm{\&rho;}3}\end{array}\right],$ P_ρ1、P_ρ2、P_ρ3分别为3×3矩阵，取K_ρ＝[P_ρ2P_ρ3]为3×6矩阵；

令为如下向量：

则可表达为

当δ_F≥0时，Δ＜0，则无需打开推力器；

当δ_F＜0时，设计推力器控制量：

a)初始化新向量其中，为1×6向量，和为3×1向量，和新变量n，令n＝1、 ${\widehat{\mathrm{\&beta;}}}_{\mathrm{\&rho;}}^T=\stackrel{\&RightArrow;}{0};$

b)令A_j取向量中绝对值最大的元素，j为绝对值最大的量在向量中的位置，j的取值范围为1,2,3；然后令向量中第j个元素为A_j,令中第j个元素为0；

c)计算 分别为向量的3个元素；

d)n＝n+1,计算 $\underset{\&OverBar;}{\hat{u}}={\left[\begin{array}{cccc}S\left({\widehat{\mathrm{\&beta;}}}_{\mathrm{\&rho;}1}\right)& S\left({\widehat{\mathrm{\&beta;}}}_{\mathrm{\&rho;}2}\right)& ...& S\left({\widehat{\mathrm{\&beta;}}}_{\mathrm{\&rho;}6}\right)\end{array}\right]}^T,$ 其中 $S\left({\widehat{\mathrm{\&beta;}}}_{\mathrm{\&rho;}i}\right)=\left\{\begin{array}{cc}1& {\widehat{\mathrm{\&beta;}}}_{\mathrm{\&rho;}i}<0\\ 0& {\widehat{\mathrm{\&beta;}}}_{\mathrm{\&rho;}i}\&GreaterEqual;0\end{array}\right.$ 为切换函数；其中i＝1,…,6；分别为向量的6个元素；

e)如果Δ＜0或n＞3，则停止，输出当前的推力器控制向量 $\underset{\&OverBar;}{\hat{u}}={\left[\begin{array}{cccc}S\left({\widehat{\mathrm{\&beta;}}}_{\mathrm{\&rho;}1}\right)& S\left({\widehat{\mathrm{\&beta;}}}_{\mathrm{\&rho;}2}\right)& ...& S\left({\widehat{\mathrm{\&beta;}}}_{\mathrm{\&rho;}6}\right)\end{array}\right]}^T,$ 否则，返回b)；

步骤5，将步骤4得到的输入步骤2中包含推力器开关的系统动力学模型，对探测器进行实时控制，完成基于常推力器开关的障碍规避轨迹跟踪。

2.根据权利要求1所述的一种软着陆自主障碍规避常推力器控制方法，其特征在于：所述探测器本体坐标系Σ_s:o_s-x_sy_sz_s，其原点o_s在探测器的质心处，o_sx_s轴、o_sy_s轴及o_sz_s轴分别与探测器的三个惯量主轴重合，该坐标系反映着陆过程中探测器的空间姿态；

所述着陆点坐标系Σ_l:o_l-x_ly_lz_l，其原点o_l位于预定的着陆点，o_lx_l沿经线的切线指向南极方向，o_lz_l和从行星质心指向着陆点的矢量o_ao_l方向一致，o_ly_l与o_lx_l、o_lz_l之间满足右手系法则。

3.根据权利要求1所述的一种软着陆自主障碍规避常推力器控制方法，其特征在于：推力器控制量设计目标为误差动力学渐近稳定，且打开推力器数量最少。

4.根据权利要求1所述的一种软着陆自主障碍规避常推力器控制方法，其特征在于：步骤1所述着陆点为着陆区域内与其最近障碍点距离最大的点。