CN103900576B - 一种深空探测自主导航的信息融合方法 - Google Patents

Abstract

一种深空探测自主导航的信息融合方法，步骤为：获取测距测速时刻附近的两组惯性导航结果，利用惯性导航结果计算测距时刻着陆器相对天体表面的高度，同时利用测距信息计算着陆器相对天体表面的高度，通过计算高度残差进行导航位置的高度修正。利用惯性导航结果计算测速时刻着陆器相对天体表面的速度，同时确定测速波束方向的速度，通过计算测速波束方向的速度残差并进行处理，计算得到正交速度修正量，利用速度修正量对惯性导航的速度输出进行修正。本发明方法提高了导航算法对测量信息时间不同步、过程中出现波束失效等问题的适应性和鲁棒性，同时也提高了导航精度。

Description

一种深空探测自主导航的信息融合方法

技术领域

本发明属于航天器制导、导航与控制技术领域，涉及一种深空探测器的自主导航方法，适用于深空探测器对月球、火星、小行星等深空天体的探测任务。

背景技术

对于地外天体软着陆探测器的制导、导航与控制***来说，获取准确的导航数据是保证着陆器安全软着陆的前提。由于软着陆过程时间短、自主性高，因此已实施的着陆任务以及已计划进行的着陆任务大都采用了基于惯性导航配以测距测速修正的自主导航方式。惯性导航技术依靠陀螺直接测量角速度并积分获得探测器的姿态角，或者用陀螺建立稳定平台并通过框架角获得姿态；依靠加速度计测量比力，并积分获得速度和位置。由于惯性导航是一种外推算法，其导航误差取决于导航初值以及惯性器件的误差，且呈现出随时间不断增长的特点。因此，需要配备有微波或者激光测距或测速敏感器，以对惯性导航进行修正。

为了保证速度信息的高可靠性，一般采用3个及3个以上的测速波束信息，来保证测速信息的冗余。但由于波束测量信息的时间不同步，而且某时刻可能有效波束仅有1～2个，因此已有的利用3个波束测量直接转换为3维速度信息进行修正的导航方法不再可用。而其它采用多个有效测量波束中的每个波束依次修正的导航方式，由于多个波束测量间包含了相互耦合的信息量，就会引起测量信息的重复修正，导致惯性导航信息过修正，引起导航误差增大。

发明内容

本发明的技术解决问题是：克服现有技术的不足，提供了一种利用正交分解信息的自主导航信息融合方法，通过改进深空着陆探测器自主导航信息的融合结构，提高了自主导航对测量信息时间不同步、过程中出现波束失效等问题的适应性和鲁棒性，从而提高了导航精度，满足地外天体软着陆任务的需要。

本发明的技术解决方案是：一种深空探测自主导航的信息融合方法，包括如下步骤：

(1)在时刻t_u获取着陆器上安装的测距敏感器的测量值ρ_u，同时获取惯性导航***与t_u最接近的两个时刻t_n-2和t_n-1对应的着陆惯性系下的位置计算值和速度计算值和

(2)利用t_n-2和t_n-1以及对应的和计算得到t_u时刻着陆惯性系下着陆器的位置计算值由此得到根据惯性导航***输出信息的着陆器相对着陆天体表面的高度其中r_M为对应的着陆天体表面的参考高度；

(3)利用ρ_u直接计算得到根据测距敏感器测量信息的着陆器相对着陆天体表面的高度 $h_{u,radar}={\mathrm{\ρ}}_u\frac{u_R^I\·(-r_u^I)}{\left|\right|r_u^I\left|\right|},$ 其中 $u_R^I=C_{ib}{u}_R^b,$ 为着陆惯性系下测距敏感器的测量波束指向，为着陆器本体坐标系下测距敏感器的测量波束指向，C_bi为着陆器本体坐标系相对于着陆惯性坐标系的姿态矩阵，符号·表示两个矢量的点乘；

(4)利用步骤(2)和步骤(3)的结果计算得到高度误差δh_u＝h_u,radar-h_u,ins，并采用公式对惯性导航***输出的当前周期的位置r^I进行修正，其中为修正后的位置，高度修正系数 的取值在0～1之间，取值大于首次高度修正时对应的高度值，高度修正系数w_h随高度下降逐渐增大；

(5)利用t_n-2和t_n-1以及对应的和计算得到t_u时刻惯性导航***输出的着陆惯性系下的着陆器速度计算值由此转换得到根据惯性导航***输出信息的着陆器相对着陆天体表面的速度 $v_{g,ins}^I=v_u^I-{\mathrm{\ω}}_t^I\×r_u^I$ 以及在测速敏感器波束方向上的分量 $v_{um,ins}=v_{g,ins}^I\·u_{vm}^I,$ 其中为着陆天体的自转角速度矢量，为测速敏感器的波束在着陆惯性系下的指向，m＝0,1,2,…,n，n为测速敏感器的波束数量；

(6)在t_u时刻获取着陆器上安装的测速敏感器的测量值v_um,radar，根据步骤(5)的计算结果得到速度误差δv_um＝v_um,radar-v_um,ins，并由此计算得到测速敏感器波束方向的修正量 ${\mathrm{dv}}_p^m=w_{vm}\·{\mathrm{\δv}}_{um},$ 其中 $w_{vm}=w_{vm}^{*}(1-|\left|v_u^I\right||/v_{vm}^{*}),$ 的取值在0～1之间，取值大于首次速度修正时对应的速度值，速度修正系数w_vm随速度降低逐渐增大；

(7)根据测速敏感器的实际有效测速波束，确定不同情况下着陆惯性坐标系下的速度修正量dv_p，

(a)当有效测速波束为0时，dv_p＝[0 0 0]^T；

(b)当有效测速波束为1时，

(c)当有效测速波束为2时， ${\mathrm{dv}}_p={\mathrm{\δv}}_{xm}\·x_m^b+{\mathrm{\δv}}_{ym}\·y_m^b,$ 其中 ${\mathrm{\δv}}_{xm}={\mathrm{dv}}_p^i,$ ${\mathrm{\δv}}_{ym}=-\cos \left({\mathrm{\θ}}_{ij}\right)\·{\mathrm{dv}}_p^i/\sin \left({\mathrm{\θ}}_{ij}\right)+{\mathrm{dv}}_p^j/\sin \left({\mathrm{\θ}}_{ij}\right),{\mathrm{\θ}}_{ij}=\arccos \left(u_{vi}^b,u_{vj}^b\right),x_m^b=u_{vi}^b,y_m^b=z_m^b\×x_m^b,$ 角标i和j用于区分两个不同的有效测速波束，和分别是两个波束在着陆器本体坐标系下的指向；

(d)当有效测速波束为3时， ${\mathrm{dv}}_p={\left[\begin{array}{ccc}{u}_{vi}^I& u_{vj}^I& u_{vk}^I\end{array}\right]}^{-T}\left[\begin{array}{c}{\mathrm{dv}}_p^i\\ {\mathrm{dv}}_p^j\\ {\mathrm{dv}}_p^k\end{array}\right],$ 角标i、j和k用于区分三个不同的有效测速波束，矩阵计算A^-T表示(A^T)^-1；

(e)当有效测速波束大于3时，采用最小二乘拟合出三个正交轴上的合成速度修正量dv_p；

(8)利用步骤(7)得到的结果，采用公式对惯性导航***输出的当前周期的速度v^I进行修正，为修正后的速度。本发明与现有技术相比的优点在于：本发明方法利用多波束测量信息构建了正交新息，形成了基于正交新息的导航修正结构；在导航过程中，对于任意一个或多个波束有效情况，本发明方法都能够在一定方向上对惯性导航结果进行修正，且避免了多个波束依次修正导致的导航信息过修正，提高了自主导航对测量信息时间不同步、过程中出现波束失效等问题的适应性和鲁棒性，从而提高了导航精度，能够满足地外天体软着陆任务的需要。

附图说明

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

图2为本发明方法中各修正量之间的几何关系示意图。图中u_vi、u_vj、u_vk、和u_vl分别表示测速敏感器的四个波束指向，和分别表示利用波束u_vi和波束u_vj形成的三个正交轴的指向。

具体实施方式

如图1所示，为本发明方法的流程框图，主要步骤如下：

第一步、进行惯性导航解算

(1)参考坐标系的建立和导航***赋初值(初始化)

着陆器本体坐标系(O_B-X_BY_BZ_B)固联在着陆器，原点O_B与着陆器质心重合，O_BX_B轴沿着陆器某一惯性主轴方向，O_BY_B轴和O_BZ_B轴也沿着陆器另外两个惯性主轴方向，且O_BX_B、O_BY_B和O_BZ_B构成右手坐标系。导航***选用J2000惯性坐标系(O_m-X_IY_IZ_I)作为参考坐标系，原点为天体中心O_m，O_mX_I轴指向J2000的平春分点，O_mZ_I轴垂直于平地心赤道面，且O_mX_I、O_mY_I和O_mZ_I构成右手坐标系。

导航***的初值包括着陆器的位置、速度和姿态。其中着陆器的位置、速度由地面测控给出，而姿态由星敏感器和陀螺测量得到。设位置矢量为r^I、速度矢量为v^I、着陆器本体相对着陆惯性系的姿态矩阵为C_bi(对应的姿态四元数为q)。

(2)姿态确定算法

姿态确定算法的目的是更新姿态矩阵C_bi，它可以根据陀螺测量递推获得。设陀螺测量的角速度为ω^b，则姿态运动方程可以用四元数描述为：

$\stackrel{\·}{q}=\frac{1}{2}Eq\left(q\right){\mathrm{\ω}}^b---\left(1\right)$

其中

$Eq\left(q\right)=\left[\begin{array}{ccc}{q}_4& -q_3& q_2\\ q_3& q_4& -q_1\\ -q_2& q_1& q_4\\ -q_1& -q_2& -q_3\end{array}\right]---\left(2\right)$

对(1)式进行积分可以完成四元数的更新，进一步可以解算出姿态矩阵：

$C_{bi}=\left[\begin{array}{ccc}{q}_1^2-q_2^2-q_3^2+q_4^2& 2\left(q_1{q}_2+q_3{q}_4\right)& 2\left(q_1{q}_3-q_2{q}_4\right)\\ 2\left(q_1{q}_2-q_3{q}_4\right)& -q_1^2+q_2^2-q_3^2+q_4^2& 2\left(q_2{q}_3+q_1{q}_4\right)\\ 2\left(q_1{q}_3+q_2{q}_4\right)& 2\left(q_2{q}_3-q_1{q}_4\right)& -q_1^2-q_2^2+q_3^2+q_4^2\end{array}\right]---\left(3\right)$

(3)位置速度递推，即利用加速度计测量得到的速度增量累加计算当前时刻着陆器的速度，并积分获得着陆器在惯性坐标系下的位置。

着陆惯性坐标系下描述的惯导基本方程为

${\stackrel{\·\·}{r}}^I=g^I+f^I---\left(4\right)$

其中，g^I为天体引力加速度，忽略天体非球引力摄动，有

$g^I={\mathrm{\μ}}_t\frac{r^I}{r^3}---\left(5\right)$

其中，μ_t为天体引力常数，r＝||r^I||为着陆器相对天体中心的距离，f^I是惯性系下表示的非引力加速度(比力)，它可以根据加速度计测量得到。但是由于加速度计测量是表示在本体系下的，即f^b，因此需要根据姿态矩阵进行转换。

$f^I=C_{ib}{f}^b=C_{bi}^T{f}^b---\left(6\right)$

由于因此对(4)式进行两重积分就可以分别获得着陆器的速度矢量v^I和位置矢量r^I。

在获得位置后，还可以计算出着陆器相对天体表面的高度

h＝||r^I||-r_t (7)

r_t为r^I对应的参考天体表面高度。

第二步、进行测距修正

利用测距信息可以修正惯性导航的位置。测距敏感器提供的是视线距离，因此利用着陆器的姿态信息和测距敏感器波束方向(视线方向)，可以把视线距离转化为着陆器相对天体表面的高度。将测距敏感器获得的信息与惯导解算的位置信息相比较获得的新信息，就可以通过卡尔曼滤波对惯导的位置误差进行修正。

根据测量时刻的着陆器相对天体参考面的高度和测距敏感器数据转换得到的高度，计算测量残差；根据优化修正系数进行位置修正，并利用修正后的位置计算惯导方程中的天体引力加速度。具体算法为：

(1)导航数据外推

由于测距信息的获取时间与惯导解算的时间往往不同步，因此，需要利用导航数据外推来同步时间点。

设测距信息对应的时间为t_u，与之最为接近的前两步惯导计算的时间为t_n-2和t_n-1，对应的位置计算值为和则利用惯导信息预测的t_u时刻着陆器的位置为

$r_u^I=r_{n-1}^I+\frac{r_{n-1}^I-r_{n-2}^I}{t_{n-1}-t_{n-2}}(t_u-t_{n-1})---\left(8\right)$

(2)误差计算

由惯导计算的位置信息可以获得着陆器相对天体表面的高度

$h_{u,ins}=\left|\right|r_u^I\left|\right|-r_M---\left(9\right)$

r_M为对应的参考天体表面高度。

设测距敏感器波束在本体系的安装指向为则利用姿态信息可以将它转到着陆惯性坐标系中

$u_R^I=C_{ib}{u}_R^b---\left(10\right)$

设测距敏感器的测量值为ρ_u，则可以解算出高度为

$h_{u,radar}={\mathrm{\ρ}}_u\frac{u_R^I\·(-r_u^I)}{\left|\right|r_u^I\left|\right|}---\left(11\right)$

其中，符号·表示两个矢量的点乘。

由此，可以得到高度计算的误差为

δh_u＝h_u,radar-h_u,ins (12)

(3)单波束滤波修正

取高度修正系数w_h随高度下降逐渐增大，即

$w_h=w_h^{*}(1-h_{u,ins}/h_h^{*})---\left(13\right)$

其中，的取值在0～1之间，取值大于首次高度修正时对应的高度值，高度修正系数w_h随高度下降逐渐增大。

对惯性导航输出的当前周期的位置r^I进行修正，修正后的惯性系位置为

${\hat{r}}^I=r^I+w_h\·{\mathrm{\δh}}_u\·{r_u}^I/\left|\right|{r_u}^I\left|\right|---\left(14\right)$

第三步、进行测速修正

测速敏感器获得的是着陆器相对月面的速度在波束方向的分量，因此利用着陆器的姿态信息和测速敏感器的波束方向，并根据着陆器位置补偿上天体自转引起的速度，就可以获得着陆器的惯性速度。将测速敏感器获得的信息与惯导解算的速度信息相比较获得新的信息，就可以通过卡尔曼滤波对惯导的速度误差进行修正。

(1)导航数据外推

根据t_n-2和t_n-1时刻惯导的数据外推得到测速敏感器测量时刻t_u时惯导计算的着陆器速度。设t_n-2和t_n-1时刻速度计算值为和则t_u时刻的着陆器速度为

$v_u^I=v_{n-1}^I+\frac{v_{n-1}^I-v_{n-2}^I}{t_{n-1}-t_{n-2}}(t_u-t_{n-1})---\left(15\right)$

(2)速度差计算

惯导计算出的速度是相对惯性系的速度，需要将其转换为相对天体表面的速度。

$v_{g,ins}^I=v_u^I-{\mathrm{\ω}}_t^I\×r_u^I---\left(16\right)$

为着陆天体的自转角速度矢量。

先按照(16)式将惯导解算的速度转换为相对天体表面的速度，然后将其投影到波束方向，具体算法如下。

设测速敏感器某波束在着陆器本体系的指向为其中，m＝1,2,…,n，n≥3，则利用姿态信息可以将它转到着陆惯性坐标系中

$u_{vm}^I=C_{ib}{u}_{vm}^b---\left(17\right)$

则惯导解算的速度在测速波束方向的分量为

$v_{um,ins}=<v_{g,ins}^I\&CenterDot;u_{vm}^I>---\left(18\right)$

测速敏感器实际获得的速度测量为v_um,radar，因此，两者之差为

δv_um＝v_um,radar-v_um,ins (19)

(3)计算波束方向修正量

取速度修正系数w_vm随速度降低逐渐增大，即

$w_{vm}=w_{vm}^{*}(1-|\left|v_u^I\right||/v_{vm}^{*})---\left(20\right)$

其中，的取值在0～1之间，取值大于首次速度修正时对应的速度值，速度修正系数w_vm随速度降低逐渐增大。

计算波束方向修正量

${\mathrm{dv}}_p^m=w_{vm}\&CenterDot;{\mathrm{\&delta;v}}_{um}---\left(21\right)$

(4)计算惯性系下的速度修正量

如果有效测速波束为0，则置惯性系下的速度修正量

dv_p＝[0 0 0]^T (22)

如果只有1个有效测速波束，则惯性系下的速度修正量为

${\mathrm{dv}}_p={\mathrm{dv}}_p^m\&CenterDot;u_{vm}^I---\left(23\right)$

如果只有2个有效测速波束，则：

1)设两个有效测速波束(角标分别为i和j)的夹角为

${\mathrm{\&theta;}}_{ij}=arccos(u_{vi}^b,u_{vj}^b)---\left(24\right)$

2)设则与波束1共面且正交方向的速度修正量为

${\mathrm{\&delta;v}}_{ym}=-cos\left({\mathrm{\&theta;}}_{ij}\right)\&CenterDot;{\mathrm{dv}}_p^i/sin\left({\mathrm{\&theta;}}_{ij}\right)+{\mathrm{dv}}_p^j/sin\left({\mathrm{\&theta;}}_{ij}\right)---\left(25\right)$

3)设则两波束组成平面的法线方向为

$z_m^b=u_{vi}^b\&times;u_{vj}^b---\left(26\right)$

4)与波束1共面且正交的方向为

$y_m^b=z_m^b\&times;x_m^b---\left(27\right)$

5)计算惯性系下的速度修正量为

${\mathrm{dv}}_p={\mathrm{\&delta;v}}_{xm}\&CenterDot;x_m^b+{\mathrm{\&delta;v}}_{ym}\&CenterDot;y_m^b---\left(28\right)$

如果只有3个有效测速波束，则惯性系下的速度修正量为：

${\mathrm{dv}}_p={\left[\begin{array}{ccc}{u}_{vi}^I& u_{vj}^I& u_{vk}^I\end{array}\right]}^{-T}\left[\begin{array}{c}{\mathrm{dv}}_p^i\\ {\mathrm{dv}}_p^j\\ {\mathrm{dv}}_p^k\end{array}\right]---\left(29\right)$

式中，矩阵计算A^-T表示(A^T)^-1。

如果有效测速波束数大于3，则采用最小二乘拟合出三个正交轴上的合成速度修正量。如图2所示，例如有效测速波束数为4，计算惯性系下的速度修正量如下

${\mathrm{dv}}_p={\left\{{\left[\begin{array}{cccc}{u}_{vi}^I& u_{vj}^I& u_{vk}^I& u_{vl}^I\end{array}\right]}^T\right\}}^{-p}\left[\begin{array}{c}{\mathrm{dv}}_p^i\\ {\mathrm{dv}}_p^j\\ {\mathrm{dv}}_p^k\\ {\mathrm{dv}}_p^j\end{array}\right]---\left(30\right)$

式中，矩阵计算A^-P表示(A^TA)^-1A^T。

(5)最终速度修正

${\hat{v}}^I=v^I+{\mathrm{dv}}_p---\left(31\right)$

式中，v^I为惯性导航***输出的当前周期速度，为修正后的惯性系速度。

第四步、对每一个控制周期重复(1)～(3)的步骤，完成导航解算和测量修正。

本发明说明书中未作详细描述的内容属本领域技术人员的公知技术。

Claims (1)

1.一种深空探测自主导航的信息融合方法，其特征在于包括如下步骤：

(1)在时刻t_u获取着陆器上安装的测距敏感器的测量值ρ_u，同时获取惯性导航***与t_u最接近的两个时刻t_n-2和t_n-1对应的着陆惯性系下的位置计算值和速度计算值和

(2)利用t_n-2和t_n-1以及对应的和计算得到t_u时刻着陆惯性系下着陆器的位置计算值由此得到根据惯性导航***输出信息的着陆器相对着陆天体表面的高度其中r_M为对应的着陆天体表面的参考高度；

(3)利用ρ_u直接计算得到根据测距敏感器测量信息的着陆器相对着陆天体表面的高度其中 为着陆惯性系下测距敏感器的测量波束指向，为着陆器本体坐标系下测距敏感器的测量波束指向，C_bi为着陆器本体坐标系相对于着陆惯性坐标系的姿态矩阵，符号·表示两个矢量的点乘；

(4)利用步骤(2)和步骤(3)的结果计算得到高度误差δh_u＝h_u,radar-h_u,ins，并采用公式对惯性导航***输出的当前周期的位置r^I进行修正，其中为修正后的位置，高度修正系数 的取值在0～1之间，取值大于首次高度修正时对应的高度值，高度修正系数w_h随高度下降逐渐增大；

(5)利用t_n-2和t_n-1以及对应的和计算得到t_u时刻惯性导航***输出的着陆惯性系下的着陆器速度计算值由此转换得到根据惯性导航***输出信息的着陆器相对着陆天体表面的速度 $v_{g,ins}^I=v_u^I-{\mathrm{\&omega;}}_t^I\&times;r_u^I$ 以及在测速敏感器波束方向上的分量 $v_{um,ins}=v_{g,ins}^I\&CenterDot;u_{vm}^I,$ 其中为着陆天体的自转角速度矢量，为测速敏感器的波束在着陆惯性系下的指向，m＝0,1,2,…,n，n为测速敏感器的波束数量；

(6)在t_u时刻获取着陆器上安装的测速敏感器的测量值v_um,radar，根据步骤(5)的计算结果得到速度误差δv_um＝v_um,radar-v_um,ins，并由此计算得到测速敏感器波束方向的修正量 ${\mathrm{dv}}_p^m=w_{vm}\&CenterDot;{\mathrm{\&delta;v}}_{um},$ 其中 $w_{vm}=w_{vm}^{*}(1-|\left|v_u^I\right||/v_{vm}^{*}),$ 的取值在0～1之间，取值大于首次速度修正时对应的速度值，速度修正系数w_vm随速度降低逐渐增大；

(7)根据测速敏感器的实际有效测速波束，确定不同情况下着陆惯性坐标系下的速度修正量dv_p，

(a)当有效测速波束为0时，dv_p＝[0 0 0]^T；

(b)当有效测速波束为1时，

(c)当有效测速波束为2时，其中 ${\mathrm{\&delta;v}}_{ym}=-\cos \left({\mathrm{\&theta;}}_{ij}\right)\&CenterDot;{\mathrm{dv}}_p^i/\sin \left({\mathrm{\&theta;}}_{ij}\right)+{\mathrm{dv}}_p^j/\sin \left({\mathrm{\&theta;}}_{ij}\right),{\mathrm{\&theta;}}_{ij}=\arccos (u_{vi}^b,u_{vj}^b),x_m^b=u_{vi}^b,y_m^b=z_m^b\&times;x_m^b,$ 角标i和j用于区分两个不同的有效测速波束，和分别是两个波束在着陆器本体坐标系下的指向；

(d)当有效测速波束为3时， ${\mathrm{dv}}_p={\left[\begin{array}{ccc}{u}_{vi}^I& u_{vj}^I& u_{vk}^I\end{array}\right]}^{-T}\left[\begin{array}{c}d{v}_p^i\\ {\mathrm{dv}}_p^j\\ {\mathrm{dv}}_p^k\end{array}\right],$ 角标i、j和k用于区分三个不同的有效测速波束，矩阵计算A^-T表示(A^T)^-1；

(e)当有效测速波束大于3时，采用最小二乘拟合出三个正交轴上的合成速度修正量dv_p；

(8)利用步骤(7)得到的结果，采用公式对惯性导航***输出的当前周期的速度v^I进行修正，为修正后的速度。