CN106249756A - 一种基于碰撞概率的行星着陆障碍规避控制方法 - Google Patents

Abstract

本发明公开的一种基于碰撞概率的行星着陆障碍规避控制方法，涉及一种行星着陆障碍规避控制方法，属于深空探测领域。本发明的实现方法为，建立系统动力学方程；构建李雅普诺夫函数中关于目标点的势场函数，保证探测器趋近于目标着陆点，同时满足目标着陆速度；引入探测器位置的协方差矩阵，计算探测器与各障碍区域的碰撞概率；基于碰撞概率构建李雅普诺夫函数，利用李雅普诺夫稳定性原理求取控制加速度，利用求取的加速度控制探测器着陆轨迹，降低行星表面多扰动、不确知环境对探测器障碍规避控制的影响，对行星表面的复杂障碍进行有效规避，实现自主安全精确着陆。本发明还具有鲁棒性好和高实时性的优点。

Description

一种基于碰撞概率的行星着陆障碍规避控制方法

技术领域

本发明涉及一种行星着陆障碍规避控制方法，尤其涉及一种基于碰撞概率的行星着陆障碍规避控制方法，属于深空探测领域。

背景技术

小天体着陆障碍规避问题是小天体着陆探测的重要问题，关系小天体着陆任务的成败和小天体探测器的安全。由于小天体形状不规则且表面曲率较大，可供小天体着陆的区域一般面积较小。同时，小天体表面崎岖不平，且存在大量的陨石坑、岩石、斜坡、山丘等，对小天体探测器的安全构成直接威胁。由于小天体环境不确定性较大，且测绘精度有限，探测器在着陆过程中需对目标区域进行障碍检测，并对检测到的障碍进行有效规避，以实现自主安全着陆。

在先技术[1](参见Lopez,Ismael,McInnes,Colin R.Autonomous rendezvoususing artificial potential function guidance[J].Journal of Guidance,Control,and Dynamics,1995,18(2):237-241.)，针对自主交会对接问题，提出了一种人工势函数制导控制方法，在实现自主交会的同时，能够对多处障碍进行规避。在先技术[2](参见Zhu SY,Cui P Y,Hu H J,Hazard detection and avoidance for planetary landing basedon Lyapunov control method[C].Intelligent Control and Automation.Beijing:[s.n.],2012)，采用势函数控制方法研究了小天体着陆障碍规避问题，根据探测器当前的势能与障碍地形对探测器的威胁选取李雅普诺夫函数，并通过李雅普诺夫稳定性原理推导稳定控制律，因而能够保证探测器到达目标点的同时有效降低碰撞的概率。

适应行星表面环境的不确知特点，对行星表面的复杂障碍进行有效规避，提高障碍规避的鲁棒性和可靠性，上述技术问题是行星着陆障碍规避方法需要进一步改进的技术问题。

发明内容

本发明公开的一种基于碰撞概率的行星着陆障碍规避控制方法，要解决的技术问题是降低行星表面多扰动、不确知环境对探测器障碍规避控制的影响，对行星表面的复杂障碍进行有效规避，实现自主安全精确着陆。此外，本发明还具有鲁棒性好和高实时性的优点。

本发明的目的是通过以下方法实现的。

本发明公开的一种基于碰撞概率的行星着陆障碍规避控制方法，建立系统动力学方程。构建李雅普诺夫函数中关于目标点的势场函数，保证探测器趋近于目标着陆点，同时满足目标着陆速度。引入探测器位置的协方差矩阵，计算探测器与各障碍区域的碰撞概率。基于碰撞概率构建李雅普诺夫函数，利用李雅普诺夫稳定性原理求取控制加速度，利用求取的加速度控制探测器着陆轨迹，降低行星表面多扰动、不确知环境对探测器障碍规避控制的影响，对行星表面的复杂障碍进行有效规避，实现自主安全精确着陆。

本发明公开的一种基于碰撞概率的行星着陆障碍规避控制方法，包括如下步骤：

步骤一、建立系统动力学方程。

在着陆点固联坐标系下建立探测器的动力学方程：

$\left\{\begin{array}{c}\stackrel{\·}{r}=v\\ \stackrel{\·}{v}=g-2\mathrm{\ω}\×v-\mathrm{\ω}\×\mathrm{\ω}\×r+a\end{array}\right.---\left(1\right)$

其中r＝[x,y,z]^T为探测器在着陆点坐标系下的位置矢量，v＝[v_x,v_y,v_z]^T为探测器的速度矢量，ω为目标天体自转角速度矢量，g＝[g_x,g_y,g_z]^T为探测器受到的目标天体引力加速度，a为施加的控制加速度。

步骤二、构建步骤四李雅普诺夫函数中关于目标点的势场函数φ_q，保证探测器趋近于目标着陆点，同时满足目标着陆速度。

构建关于目标点的势场函数φ_q

φ_q＝(x-x_t)^TQ(x-x_t) (2)

其中x＝[x,y,z,v_x,v_y,v_z]^T为探测器状态变量，x_t为目标状态，Q为以q_i＞0,i＝1,...,6为对角线的对角矩阵。上式表示的势场，存在唯一的极小值点，为目标着陆状态x_t。只要保证探测器状态x沿势场降低的方向前进，探测器将趋近于目标着陆点，同时满足目标着陆速度。

步骤三、计算探测器与第i个障碍区域的碰撞概率p_i。

假设探测器的实际位置符合高斯分布，在着陆点坐标系下的水平位置名义值即均值为r_hr＝[x,y]^T，协方差矩阵为C。相比于名义位置，探测器处于障碍区域的概率密度比较均匀，则将障碍区内各点的概率密度近似视为相等。将障碍区内各点的概率密度近似为区域中心处的概率密度，则探测器实际位置处于第i个障碍区域，即与第i个障碍区域的碰撞概率为：

$p_i=S_i\frac{1}{2\mathrm{\π}\sqrt{\left|C\right|}}{e}^{-\frac{1}{2}{(r_{ci}-r_{hr})}^T{C}^{-1}(r_{ci}-r_{hr})},i=1,\mathrm{...},k---\left(3\right)$

其中r_ci为第i个障碍区域中心的位置，S_i为小天体表面第i个障碍区域的面积，k为障碍区的数量。

由于在碰撞概率p_i的计算中，通过引入探测器位置的协方差矩阵C考虑探测器位置的不确定性信息，因此能够提高探测器障碍规避的有效性，提高鲁棒性。

步骤四、构建李雅普诺夫函数。

根据步骤二中关于目标点的势场函数和步骤三计算的探测器与第i个障碍区域的碰撞概率p_i，构建如下形式的李雅普诺夫函数φ：

$\begin{array}{c}\mathrm{\φ}={\mathrm{\φ}}_q+{\mathrm{\φ}}_p\\ ={(x-x_t)}^{T}Q(x-x_t)+\underset{i=1}{\overset{k}{\Σ}}{k}_i{p}_i,i=1,\mathrm{...},k\end{array}---\left(4\right)$

其中k_i＞0为权重系数。

步骤五、求取控制加速度a，利用加速度a控制探测器着陆轨迹，降低行星表面多扰动、不确知环境对探测器障碍规避控制的影响，对行星表面的复杂障碍进行有效规避，实现自主安全精确着陆。

令

$a_q=\left[\begin{array}{c}-q_1(x-x_t)-{\mathrm{\ω}}^{2}x-2{\mathrm{\ωv}}_y-g_x-{\mathrm{\κv}}_x/q_4\\ -q_2(y-y_t)-{\mathrm{\ω}}^{2}y+2{\mathrm{\ωv}}_x-g_y-{\mathrm{\κv}}_y/q_5\\ -q_3(z-z_t)-g_z-{\mathrm{\κv}}_z/q_6\end{array}\right]---\left(5\right)$

$a_p=-\underset{i=1}{\overset{k}{\Σ}}\frac{k_i{S}_i}{8\mathrm{\π}\sqrt{\left|C\right|}}{e}^{-\frac{1}{2}{(r_{ci}-r_{hr})}^T{C}^{-1}(r_{ci}-r_{hr})}\left(C^{-1}\right(r_{ci}-r_{hr})+C^{-T}(r_{ci}-r_{hr}\left)\right){Q_v}^{-1}---\left(6\right)$

其中Q_v为以q_i,i＝4,5,6为对角线的对角矩阵，κ为正实数。

障碍规避的控制加速度a为：

a＝a_q+a_p (7)

利用加速度a控制探测器着陆轨迹，降低行星表面多扰动、不确知环境对探测器障碍规避控制的影响，对行星表面的复杂障碍进行有效规避，实现自主安全精确着陆。

步骤五求取的控制加速度a为解析形式，不含积分等复杂运算，满足在线反馈控制的实时性要求。

有益效果

1、本发明公开的一种基于碰撞概率的行星着陆障碍规避控制方法，通过构建李雅普诺夫函数，利用李雅普诺夫稳定性原理求取控制加速度，能够对行星表面的复杂障碍进行有效规避，实现自主安全精确着陆。

2、本发明公开的一种基于碰撞概率的行星着陆障碍规避控制方法，通过在碰撞概率的计算及控制加速度的求取过程中引入探测器位置的协方差矩阵，考虑探测器位置不确定性的影响，因此障碍规避效果明显优于传统障碍规避方法，此外，在探测器位置存在较大不确定性的情况下，具有更好的鲁棒性。

3、本发明公开的一种基于碰撞概率的行星着陆障碍规避控制方法，求取的控制加速度a为解析形式，不含积分等复杂运算，在保证精度的前提下能够降低碰撞概率计算量，满足在线反馈控制的实时性要求。

附图说明

图1为本发明一种基于碰撞概率的行星着陆障碍规避控制方法的流程图；

图2为本发明方法李雅普诺夫函数势场分布图；

图3为本发明方法李雅普诺夫函数势场水平分布图；

图4为存在较大位置误差情况下的障碍规避轨迹(水平方向，传统方法)；

图5为存在较大位置误差情况下的障碍规避轨迹(水平方向，本发明方法)；

图6为探测器的三轴速度时间历程(本发明方法)；

图7为探测器的三轴控制加速度时间历程(本发明方法)；

图8为蒙特卡洛仿真中距障碍中心的最短距离分布(传统方法)；

图9为蒙特卡洛仿真中距障碍中心的最短距离分布(本发明方法)；

具体实施方式

下面结合附图与实施例对本发明作进一步说明。

实施例1

本实施例公开的一种基于碰撞概率的行星着陆障碍规避控制方法，以小天体着陆障碍规避控制为例，实现本实施方式方法包括以下步骤，如图1所示：

步骤一、建立系统动力学方程。

在着陆点固联坐标系下建立探测器的动力学方程：

$\left\{\begin{array}{c}\stackrel{\·}{r}=v\\ \stackrel{\·}{v}=g-2\mathrm{\ω}\×v-\mathrm{\ω}\×\mathrm{\ω}\×r+a\end{array}\right.$

其中r＝[x,y,z]^T为探测器在着陆点坐标系下的位置矢量，v＝[v_x,v_y,v_z]^T为探测器的速度矢量，ω为目标天体自转角速度矢量，g＝[g_x,g_y,g_z]^T为探测器受到的目标天体引力加速度，a为施加的控制加速度。

步骤二、构建步骤四李雅普诺夫函数中关于目标点的势场函数φ_q，保证探测器趋近于目标着陆点，同时满足目标着陆速度。

构建关于目标点的势场函数φ_q

φ_q＝(x-x_t)^TQ(x-x_t)

其中x＝[x,y,z,v_x,v_y,v_z]^T为探测器状态变量，x_t为目标状态，Q为以q_i＞0,i＝1,...,6为对角线的对角矩阵。上式表示的势场，存在唯一的极小值点，为目标着陆状态x_t。只要保证探测器状态x沿势场降低的方向前进，探测器将趋近于目标着陆点，同时满足目标着陆速度。

步骤三、计算探测器与障碍区域的碰撞概率。

假设探测器的实际位置符合高斯分布，在着陆点坐标系下的水平位置名义值即均值为r_hr＝[x,y]^T，协方差矩阵为C，则探测器实际处于第i个障碍区域的概率，即与第i个障碍区域的碰撞概率为

$p_i=\∫\∫\frac{1}{2\mathrm{\π}\sqrt{\left|C\right|}}{e}^{-\frac{1}{2}{(r_i-r_{hr})}^T{C}^{-1}(r_i-r_{hr})}{\mathrm{dx}}_i{\mathrm{dy}}_i,i=1,\mathrm{...},k---\left(8\right)$

其中r_i＝[x_i,y_i]^T为第i个障碍区域各点的水平位置，积分区域即为小天体表面的第i个障碍区域，k为障碍区的数量。

相比于名义位置，探测器处于障碍区域的概率密度比较均匀，则将障碍区内各点的概率密度近似视为相等，以对上式进行简化，减小计算量，提高实时性。将障碍区内各点的概率密度近似为区域中心处的概率密度，则上式化简为

$p_i=S_i=\frac{1}{2\mathrm{\π}\sqrt{\left|C\right|}}{e}^{-\frac{1}{2}{(r_{ci}-r_{hr})}^T(r_{ci}-r_{hr})},i=1,\mathrm{...},k$

其中r_ci为第i个障碍区域中心的位置，S_i为小天体表面第i个障碍区域的面积。

由于在碰撞概率p_i的计算中，通过引入探测器位置的协方差矩阵C考虑探测器位置的不确定性信息，因此能够提高探测器障碍规避的有效性，提高鲁棒性。通过合理近似，对公式(8)进行简化，在保证精度的前提下降低碰撞概率p_i的计算量，提高算法实时性。

步骤四、构建李雅普诺夫函数。

根据步骤二中关于目标点的势场函数和步骤三计算的探测器与障碍区域的碰撞概率p_i，构建如下形式的李雅普诺夫函数：

$\begin{array}{c}\mathrm{\φ}={\mathrm{\φ}}_q+{\mathrm{\φ}}_p\\ ={(x-x_t)}^{T}Q(x-x_t)+\underset{i=1}{\overset{k}{\Σ}}{k}_i{p}_i,i=1,\mathrm{...},k\end{array}$

其中k_i＞0为权重系数。由于Q为正定矩阵，因而

$\left\{\begin{array}{cc}{\mathrm{\φ}}_q>0,& x-x_t\≠0\\ {\mathrm{\φ}}_q=0,& x-x_t=0\end{array}\right.---\left(9\right)$

且φ_p≥0，则对任意x-x_t≠0，有

φ＞0 (10)

且

φ→∞,当||x-x_t||→∞ (11)

满足稳定性条件。

步骤五、求取控制加速度a，利用加速度a控制探测器着陆轨迹，降低行星表面多扰动、不确知环境对探测器障碍规避控制的影响，对行星表面的复杂障碍进行有效规避，实现自主安全精确着陆。

根据李雅普诺夫稳定性理论，系统需满足

$\stackrel{\&CenterDot;}{\mathrm{\&phi;}}=\frac{\&part;\mathrm{\&phi;}}{\&part;x}\stackrel{\&CenterDot;}{x}<0,x-x_t\&NotEqual;0---\left(12\right)$

即

$(\frac{\&part;{\mathrm{\&phi;}}_q}{\&part;x}+\frac{\&part;{\mathrm{\&phi;}}_p}{\&part;x})\stackrel{\&CenterDot;}{x}<0,x-x_t\&NotEqual;0---\left(13\right)$

令

$a_q=\left[\begin{array}{c}-q_1(x-x_t)-{\mathrm{\&omega;}}^{2}x-2\mathrm{\&omega;}{v}_y-g_x-\mathrm{\&kappa;}{v}_x/q_4\\ -q_2(y-y_t)-{\mathrm{\&omega;}}^{2}y+2{\mathrm{\&omega;v}}_x-g_y-{\mathrm{\&kappa;v}}_y/q_5\\ -q_3(z-z_t)-g_z-{\mathrm{\&kappa;v}}_z/q_6\end{array}\right]$

$a_p=-\underset{i=1}{\overset{k}{\&Sigma;}}\frac{k_i{S}_i}{8\mathrm{\&pi;}\sqrt{\left|C\right|}}{e}^{-\frac{1}{2}{(r_{ci}-r_{hr})}^T{C}^{-1}(r_{ci}-r_{hr})}\left(C^{-1}\right(r_{ci}-r_{hr})+C^{-T}(r_{ci}-r_{hr}\left)\right){Q_v}^{-1}$

a＝a_q+a_q

其中Q_v为以q_i,i＝4,5,6为对角线的对角矩阵，κ为正实数。对探测器施加以上控制加速度时，满足

$\stackrel{\&CenterDot;}{\mathrm{\&phi;}}=-2\mathrm{\&kappa;}(v_x^2+v_y^2+v_z^2)<0,x-x_t\&NotEqual;0---\left(14\right)$

则系统为全局稳定的。

利用加速度a控制探测器着陆轨迹，降低行星表面多扰动、不确知环境对探测器障碍规避控制的影响，对行星表面的复杂障碍进行有效规避，实现自主安全精确着陆。

步骤五求取的控制加速度a为解析形式，不含积分等复杂运算，满足在线反馈控制的实时性要求。

本实施例以433Eros小行星为目标星进行仿真验证，仿真条件为：在小天体着陆点坐标系下，探测器的初始位置为[-1200,-1300,100]^T，目标状态为零点，小天体表面障碍中心位置为[-450,-600]^T，障碍区半径为100m，探测器三轴位置误差的标准差分别为50m，50m，10m。李雅普诺夫函数势场分布图(图2、图3)、传统方法与本发明方法在较大位置误差情况下的规避轨迹比较(图4、图5)、本方法的探测器三轴速度、加速度时间历程(图6、图7)、传统方法与本发明方法在500次蒙特卡洛仿真中距障碍中心的最短距离分布比较(图8、图9)。

附图的仿真结果显示：在存在较大位置不确定性，传统障碍规避方法无法有效规避障碍(图4)的情况下，本实施例提出的障碍规避控制方法由于引入了探测器位置协方差矩阵定量计算与障碍区域的碰撞概率，因此仍能有效完成障碍规避(图5)，且速度、加速度曲线平滑(图6、图7)，能够实现自主安全精确着陆。

蒙特卡洛仿真结果显示，在探测器位置存在较大误差的情况下，以探测器在着陆过程中距障碍中心的最短距离(水平方向)为性能评价指标，该距离小于100米(即在着陆过程中进入障碍区，任务失败)的比例为：传统障碍规避方法35.2％(图8)，本实施例提出的基于碰撞概率的行星着陆障碍规避控制方法15.8％(图9)。由于在控制加速度的求取过程中考虑了探测器位置的不确定性并通过碰撞概率定量描述，本实施例方法在不确定条件下的障碍规避成功率显著高于传统方法，具有更好的鲁棒性。

以上所述的具体描述，对发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施例，用于解释本发明，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (4)

1.一种基于碰撞概率的行星着陆障碍规避控制方法，其特征在于：包括如下步骤，

步骤一、建立系统动力学方程；

在着陆点固联坐标系下建立探测器的动力学方程：

$\left\{\begin{array}{c}\stackrel{\&CenterDot;}{r}=v\\ \stackrel{\&CenterDot;}{v}=g-2\mathrm{\&omega;}\&times;v-\mathrm{\&omega;}\&times;\mathrm{\&omega;}\&times;r+a\end{array}\right.---\left(1\right)$

其中r＝[x,y,z]^T为探测器在着陆点坐标系下的位置矢量，v＝[v_x,v_y,v_z]^T为探测器的速度矢量，ω为目标天体自转角速度矢量，g＝[g_x,g_y,g_z]^T为探测器受到的目标天体引力加速度，a为施加的控制加速度；

步骤二、构建步骤四李雅普诺夫函数中关于目标点的势场函数φ_q，保证探测器趋近于目标着陆点，同时满足目标着陆速度；

构建关于目标点的势场函数φ_q

φ_q＝(x-x_t)^TQ(x-x_t) (2)

其中x＝[x,y,z,v_x,v_y,v_z]^T为探测器状态变量，x_t为目标状态，Q为以q_i＞0,i＝1,...,6为对角线的对角矩阵；上式表示的势场，存在唯一的极小值点，为目标着陆状态x_t；只要保证探测器状态x沿势场降低的方向前进，探测器将趋近于目标着陆点，同时满足目标着陆速度；

步骤三、计算探测器与第i个障碍区域的碰撞概率p_i；

假设探测器的实际位置符合高斯分布，在着陆点坐标系下的水平位置名义值即均值为r_hr＝[x,y]^T，协方差矩阵为C；相比于名义位置，探测器处于障碍区域的概率密度比较均匀，则将障碍区内各点的概率密度近似视为相等；将障碍区内各点的概率密度近似为区域中心处的概率密度，则探测器实际位置处于第i个障碍区域，即与第i个障碍区域的碰撞概率为：

$p_i=S_i\frac{1}{2\mathrm{\&pi;}\sqrt{\left|C\right|}}{e}^{-\frac{1}{2}{(r_{ci}-r_{hr})}^T{C}^{-1}(r_{ci}-r_{hr})},i=1,...,k---\left(3\right)$

其中r_ci为第i个障碍区域中心的位置，S_i为小天体表面第i个障碍区域的面积，k为障碍区的数量；

步骤四、构建李雅普诺夫函数；

根据步骤二中关于目标点的势场函数和步骤三计算的探测器与第i个障碍区域的碰撞概率p_i，构建如下形式的李雅普诺夫函数φ：

$\begin{array}{c}\mathrm{\&phi;}={\mathrm{\&phi;}}_q+{\mathrm{\&phi;}}_p\\ ={(x-x_t)}^{T}Q(x-x_t)+\underset{i=1}{\overset{k}{\&Sigma;}}{k}_i{p}_i,i=1,\mathrm{...},k\end{array}---\left(4\right)$

其中k_i＞0为权重系数；

步骤五、求取控制加速度a，利用加速度a控制探测器着陆轨迹，降低行星表面多扰动、不确知环境对探测器障碍规避控制的影响，对行星表面的复杂障碍进行有效规避，实现自主安全精确着陆；

令

$a_q=\left[\begin{array}{c}-q_1(x-x_t)-{\mathrm{\&omega;}}^{2}x-2{\mathrm{\&omega;v}}_y-g_x-{\mathrm{\&kappa;v}}_x/q_4\\ -q_2(y-y_t)-{\mathrm{\&omega;}}^{2}y+2{\mathrm{\&omega;v}}_x-g_y-{\mathrm{\&kappa;v}}_y/q_5\\ -q_3(z-z_t)-g_z-{\mathrm{\&kappa;v}}_z/q_6\end{array}\right]---\left(5\right)$

$a_p=-\underset{i=1}{\overset{k}{\&Sigma;}}\frac{k_i{S}_i}{8\mathrm{\&pi;}\sqrt{\left|C\right|}}{e}^{-\frac{1}{2}{(r_{ci}-r_{hr})}^T{C}^{-1}(r_{ci}-r_{hr})}\left(C^{-1}\right(r_{ci}-r_{hr})+C^{-T}(r_{ci}-r_{hr}\left)\right){Q_v}^{-1}---\left(6\right)$

其中Q_v为以q_i,i＝4,5,6为对角线的对角矩阵，κ为正实数；

障碍规避的控制加速度a为：

a＝a_q+a_p (7)

利用加速度a控制探测器着陆轨迹，降低行星表面多扰动、不确知环境对探测器障碍规避控制的影响，对行星表面的复杂障碍进行有效规避，实现自主安全精确着陆。

2.如权利要求1所述的一种基于碰撞概率的行星着陆障碍规避控制方法，其特征在于：由于在碰撞概率p_i的计算及控制加速度a的求取过程中，通过引入探测器位置的协方差矩阵C考虑探测器位置的不确定性信息，因此能够提高探测器障碍规避的有效性，提高鲁棒性。

3.如权利要求1所述的一种基于碰撞概率的行星着陆障碍规避控制方法，其特征在于：步骤五求取的控制加速度a为解析形式，不含积分等复杂运算，满足在线反馈控制的实时性要求。

4.一种基于碰撞概率的行星着陆障碍规避控制方法，其特征在于：建立系统动力学方程；构建李雅普诺夫函数中关于目标点的势场函数，保证探测器趋近于目标着陆点，同时满足目标着陆速度；引入探测器位置的协方差矩阵，计算探测器与各障碍区域的碰撞概率；基于碰撞概率构建李雅普诺夫函数，利用李雅普诺夫稳定性原理求取控制加速度，利用求取的加速度控制探测器着陆轨迹，降低行星表面多扰动、不确知环境对探测器障碍规避控制的影响，对行星表面的复杂障碍进行有效规避，实现自主安全精确着陆。