CN103017773A - 一种基于天体表面特征和天然卫星路标的环绕段导航方法 - Google Patents

Abstract

一种基于天体表面特征和天然卫星路标的环绕段导航方法，在进行导航过程中，首先由陀螺和星敏感器进行惯性姿态估计；根据加速度计测量，结合惯性姿态估计值，利用动力学方程进行探测器位置、速度外推；接下来，以一定间隔，利用导航相机按照序列对若干路标进行拍摄，识别出路标点的图像坐标，路标点的可选范围包括深空天体表面的已知地貌和天体的天然卫星，观测序列根据不同轨道高度和光照条件制定；以路标点的图像坐标作为测量，以外推的探测器位置、速度为基础，使用扩展卡尔曼滤波估计并修正探测器位置、速度；重复上述步骤，完成导航解算。本发明扩大了环绕段光学导航路标点选择的范围，提高了导航***的灵活性和精度。

Description

一种基于天体表面特征和天然卫星路标的环绕段导航方法

技术领域

本发明涉及一种基于天体表面特征和天然卫星路标的环绕段导航方法，属于深空探测自主导航领域。

背景技术

深空天体环绕探测是对目标天体长期、抵近和全面观测的重要手段。受到携带推进剂总量的限制，深空天体探测器经过行星际飞行抵达目标天体后，并不能直接制动和进入环绕目标天体的圆形轨道，而是运行在大椭圆轨道上。由于大椭圆轨道受天体摄动的影响较为复杂，因此探测器的轨道确定(导航)成为深空环绕探测的一项重要技术。

目前为止，国外深空大天体环绕探测器的轨道确定还主要依靠地面测控网或深空网进行。这种方法的成本和代价很大，对测控站的全球布局、测量精度等提出了很高的要求，实时性也不易保证。而自主导航则是弥补这些缺陷的一种良好的技术手段。但是深空探测的自主导航目前还只在转移段或者着陆段进行了验证。

但在方法层面上，国内外研究人员还是对深空环绕段的自主导航技术进行了大量的研究。包括依靠目标天体视半径和视线方向测量的方法，以及以天体表面特征地形为路标的方法等。但这些方法都存在各种缺陷，难以工程应用。例如，基于目标天体视半径和视线方向测量的方法依赖于大视场的光学敏感器，它只能应用在轨道高度较高的圆形环绕轨道上，而且容易受到目标天体光照条件的影响。而基于天体表面路标点的方法适用于距离目标天体很近的环绕轨道弧段，但它依赖于大量、详细的目标天体局部地形数据或影像资料。而这恰恰是深空目标抵近探测前所缺乏的。除此以外，由于目标天体相对太阳运动，再加上探测器飞行轨道的限制和目标天体天气条件的影响，可供选择的天体表面路标点会进一步减少，这大大增加了该方法实现的困难。

发明内容

本发明的技术解决问题：克服现有技术的不足，针对深空环绕探测器往往运行于大椭圆轨道的特点，提供了一种将目标天体的天然卫星和它表面的特征地形作为路标配合使用的新方法。在轨道高度较高时以天然卫星和目标天体表面大型和明显的地貌作为路标，在轨道高度较低时以天体局部地形作为路标。扩大了路标的选择范围，提高了信息量，增加了可观测弧段，提高了自主导航的适应能力和精度。

本发明所采用的技术方案为：一种基于天体表面特征和天然卫星路标的深空探测环绕段自主导航方法，探测器运行于环绕目标深空天体的大椭圆轨道，探测器上装备有IMU(包含陀螺和加速度计)、星敏感器和光学导航相机。实现步骤如下：

(1)陀螺和星敏的惯性姿态估计

利用前一个采样周期内陀螺测量的角速度对当前时刻的探测器惯性姿态进行外推；然后利用星敏获得的光轴惯性指向测量，使用卡尔曼滤波对陀螺外推惯性姿态的误差进行估计；最后用误差的估计值修正陀螺预估的惯性姿态，获得最终的惯性姿态估计；

(2)利用加速度计测量的位置、速度外推

在陀螺测量的同时，使用加速度计获得探测器非引力加速度的测量值，利用已获得的惯性姿态估计值将它转化到惯性坐标系中；然后根据轨道动力学模型，对探测器的位置和速度进行外推；

(3)路标点信息的获取

使用探测器上安装的光学导航相机按照一定拍摄序列对可能的路标点进行拍照，路标点的选择包含了天体表面的已知位置的特征地貌和天体的天然卫星；路标点根据轨道高度、光照条件事先选择，并列入观测序列，一般来说轨道较高时选择天然卫星和目标天体大型特征地貌，轨道较低时选择天体局部地形；最后，从导航图像上提取出路标点的像素坐标，传递给导航滤波器；

(4)基于路标的探测器位置、速度的估计

基于步骤(2)中外推的探测器位置、速度对路标点的图像坐标进行预报；将实际获得的路标点像素坐标与预报值进行比对，构成测量新息；然后使用扩展卡尔曼滤波对预测的探测器位置、速度进行修正，得到最终的探测器位置、速度估计值。

(5)对每一个控制周期重复(1)～(4)的步骤，完成导航解算。

所述方法同时将天体表面特征和天然卫星均作为导航路标，在轨道高度高于天然卫星轨道时以天然卫星和天体表面大型地貌为路标，在轨道低于天然卫星轨道时以天体局部地形为路标，导航滤波解算过程中路标点的数量可变。

本发明与现有技术相比的优点在于：本发明将深空天体的天然卫星也扩展为自主导航的路标点，从而增加了可供选择的路标点的种类，降低了对目标天体表面局部特征地形先验知识的需求，减少了地面选择和确定路标点的困难。而且针对深空环绕段大椭圆轨道上探测器距目标天体高度变化很大的特点，在环绕轨道的近点附近主要使用天体特征地形为路标，在环绕段的远点附近将天然卫星与天体表面明显的地貌结合使用，提高了导航方法对深空大椭圆环绕轨道的适应能力。

附图说明

图1是深空环绕段基于路标的自主导航***框图；

图2是以火星为代表的深空环绕段以天体表面特征和天然卫星作为路标的导航方法原理；

图3是深空环绕段不同时间点所选择的路标类型；

图4是深空环绕段导航位置误差变化曲线；

图5是深空环绕段导航速度误差变化曲线。

具体实施方式

探测器运行于环绕目标深空天体的大椭圆轨道，探测器上装备有IMU(包含陀螺和加速度计)、星敏感器和光学导航相机。

如图1所示，本发明具体计算过程如下：

第一步，进行陀螺+星敏的探测器姿态估计

(1)陀螺测量

设当前时刻为t_k，上一次采样时刻为t_k-1，那么陀螺可以获得在[t_k-1，t_k]时间段内的探测器角度增量Δg^b，上标b表示探测器的本体坐标系(O_b-X_bY_bZ_b)，该坐标系固联在探测器上，原点与探测器质心重合，三个轴平行于探测器惯量主轴。

角度增量除以采样时间间隔就可以获得该段时间内的探测器平均角速度ω^b的测量值。

${\widetilde{\mathrm{\ω}}}_k^b=\frac{{\mathrm{\Δg}}^b}{t_k-t_{k-1}}---\left(1\right)$

上标“～”表示测量值。

(2)姿态运动学外推

用四元数q＝[q₁，q₂，q₃，q₄]^T表示探测器相对惯性系的姿态，上标表示转置，惯性系(O_I-X_IY_IZ_I)的原点在目标天体质心，三个坐标轴在惯性空间中指向特定的方向(当描述姿态运动时，将该坐标系原点从目标天体质心平移到探测器质心)。那么姿态运动学方程为

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

其中，

$E\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(3\right)$

由于陀螺测量中含有常值漂移b^b，假设常值漂移的估计值为且用

表示导航***给出的姿态四元数估计值，则利用式(2)可以进行姿态运动学的外推

${\hat{q}}_{k-1}\≈{\hat{q}}_{k-1}+E\left({\hat{q}}_{k-1}\right)({\widetilde{\mathrm{\ω}}}_k^b-{\hat{b}}_k^b)(t_k-t_{k-1})---\left(4\right)$

(3)星敏感器测量

在利用陀螺数据外推的同时，星敏感器也按照一定间隔进行测量。星敏感器的输出为光轴在惯性空间的指向和

(上标i表示该矢量描述在惯性系下，下标star表示星敏感器)，利用步骤(2)预估的姿态四元数

将它们转换到探测器的本体系中，

$\left\{\begin{array}{c}{\hat{X}}_{\mathrm{star}}^i=A\left({\hat{q}}_k\right){\tilde{X}}_{\mathrm{star}}^i\\ {\hat{Y}}_{\mathrm{star}}^i=A\left({\hat{q}}_k\right){\tilde{Y}}_{\mathrm{star}}^i\\ {\hat{Z}}_{\mathrm{star}}^i=A\left({\hat{q}}_k\right){\tilde{Z}}_{\mathrm{star}}^i\end{array}\right.---\left(5\right)$

其中A(q)表示将姿态四元数转换为姿态矩阵，有

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

利用

和

和由星敏感器的安装位置决定的星敏感器三轴指向

和

获取姿态预估的误差四元数

${\mathrm{\Δ}\tilde{q}}_k=\left[\begin{array}{c}\mathrm{\δ}\tilde{q}\\ 1\end{array}\right]=\left[\begin{array}{c}\frac{1}{2}(X_{\mathrm{star}}^b\×{\hat{X}}_{\mathrm{star}}^b+Y_{\mathrm{star}}^b\×{\hat{Y}}_{\mathrm{star}}^b+Z_{\mathrm{star}}^b\×{\hat{Z}}_{\mathrm{star}}^b)\\ 1\end{array}\right]---\left(7\right)$

(4)卡尔曼滤波修正

由于姿态预估误差四元数为

对其两边求导并将式(2)代入其中，则可以推导出姿态四元数外推过程中标量部分的误差传播方程为

$\mathrm{\δ}\stackrel{\·}{q}=-{\widehat{\mathrm{\ω}}}^b\×\mathrm{\δq}-\frac{1}{2}{\mathrm{\Δb}}^b-\frac{1}{2}{\mathrm{\η}}_1^b---\left(8\right)$

式中

是陀螺测量的随机白噪声。而陀螺的常值漂移估计误差可以建模为

${\mathrm{\Δ}\stackrel{\·}{b}}^b={\mathrm{\η}}_2^b---\left(9\right)$

也是随机白噪声。联立式(8)、(9)可以构造滤波器的状态方程为

$\left[\begin{array}{c}\mathrm{\δ}\stackrel{\·}{q}\\ {\mathrm{\Δ}\stackrel{\·}{b}}^b\end{array}\right]=\left[\begin{array}{cc}-[{\widehat{\mathrm{\ω}}}^b\×]& -\frac{1}{2}{I}_{3\×3}\\ 0_{3\×3}& 0_{3\×3}\end{array}\right]\left[\begin{array}{c}\mathrm{\δq}\\ {\mathrm{\Δb}}^b\end{array}\right]+\left[\begin{array}{cc}\frac{1}{2}{I}_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& I_{3\×3}\end{array}\right]\left[\begin{array}{c}{\mathrm{\η}}_1^b\\ {\mathrm{\η}}_2^b\end{array}\right]---\left(10\right)$

其中表示

的斜对称阵，即

$\left[{\widehat{\mathrm{\ω}}}^b\right]=\left[\begin{array}{ccc}0& -{\widehat{\mathrm{\ω}}}_z^b& {\widehat{\mathrm{\ω}}}_y^b\\ {\widehat{\mathrm{\ω}}}_z^b& 0& -{\widehat{\mathrm{\ω}}}_x^b\\ -{\widehat{\mathrm{\ω}}}_y^b& {\widehat{\mathrm{\ω}}}_x^b& 0\end{array}\right]$ (下标xyz分别表示在本体系三个坐标轴的分量)

滤波器的测量方程为

$\mathrm{\δ}\tilde{q}=\left[\begin{array}{cc}{I}_{3\×3}& 0_{3\×3}\end{array}\right]\left[\begin{array}{c}\mathrm{\δq}\\ {\mathrm{\Δb}}^b\end{array}\right]+v_{\mathrm{star}}---\left(11\right)$

其中，V_star表示星敏感器的测量噪声，I_3×3是3×3的单位矩阵，0_3×3是3×3的零矩阵。对于由式(10)和(11)构成的***，可以使用卡尔曼滤波估计出姿态误差和陀螺常值漂移误差。

令***状态

$x=\left[\begin{array}{c}\mathrm{\δq}\\ {\mathrm{\Δb}}^b\end{array}\right]---\left(12\right)$

根据式(10)状态转移矩阵为

${\mathrm{\Φ}}_{k|k-1}\≈I+\mathrm{\Δt}\left[\begin{array}{cc}-[{\widehat{\mathrm{\ω}}}_{k-1}^b\×]& -\frac{1}{2}{I}_{3\×3}\\ 0_{3\×3}& 0_{3\×3}\end{array}\right]---\left(13\right)$

根据式(11)观测矩阵为

H＝[I_3×3  0_3×3]        (14)

***噪声

$w=\left[\begin{array}{c}{\mathrm{\η}}_1^b\\ {\mathrm{\η}}_2^b\end{array}\right]---\left(15\right)$

其方差为Q。并且令

$G=\left[\begin{array}{cc}\frac{1}{2}{I}_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& I_{3\×3}\end{array}\right]---\left(16\right)$

令测量量为

$z=\mathrm{\δ}\tilde{q}---\left(17\right)$

测量噪声为

v＝v_star                    (18)

它的方差为R，则可以使用卡尔曼滤波进行状态估计，方法如下

x_k|k-1＝Φ_k|k-1x_k-1         (19)

$P_{k|k-1}={\mathrm{\Φ}}_{k|k-1}{P}_{k-1}{\mathrm{\Φ}}_{k|k-1}^T+G_k{Q}_k{G}_k^T---\left(20\right)$

$K_k=\left(P_{k|k-1}{H}_k^T\right){(H_k{P}_{k|k-1}{H}_k^T+R_k)}^{-1}---\left(21\right)$

x_k＝x_k|k-1+K_k(z_k-H_kx_k|k-1)  (22)

$P_k=(I-K_k{H}_k)P_{k|k-1}{(I-K_k{H}_k)}^T+K_k{R}_k{K}_k^T---\left(23\right)$

在获得状态估计

后，根据式(12)的定义，就可以得到修正量的估计值和

利用这两个估计值就可以修正姿态四元数和陀螺漂移估计

${\hat{q}}_k\←{\hat{q}}_k+E\left({\hat{q}}_k\right)\mathrm{\δ}{\hat{q}}_k---\left(1\right)$

${\hat{b}}_k^b\←{\hat{b}}_k^b+{\mathrm{\Δ}\hat{b}}_k^b---\left(25\right)$

并且，可以根据修正后的姿态四元数更新姿态矩阵

$C_i^b=A\left({\hat{q}}_k\right)---\left(26\right)$

第二步，进行基于路标点的位置、速度估计

(1)加速度计测量

加速度计可以获得在[t_k-1，t_k]时间段内的探测器的速度增量ΔV^b。速度增量除以采样时间间隔就可以获得该段时间内的探测器平均比力f^b的测量值。

${\tilde{f}}_k^b=\frac{{\mathrm{\Δv}}^b}{t_k-t_{k-1}}---\left(27\right)$

(2)轨道动力学外推

利用同时间段内的加速度计测量和姿态估计值，进行位置、速度的预估。用r表示探测器位置、v表示探测器速度，则惯性系(O_I-X_IY_IZ_I)下探测器动力学方程为

${\stackrel{\·}{r}}^i=v^i$

${\stackrel{\·}{v}}^i=-\frac{{\mathrm{\μ}}_m}{{\left|\right|r^i\left|\right|}^3}{r}^i+a_{\mathrm{RCS}}^i+g_{\mathrm{dis}}^i---\left(28\right)$

其中，a_RCS为推进***产生的加速度，g_dis为引力摄动。a_RCS可由加速度计的测量，并通过姿态信息计算，即

${\tilde{a}}_{\mathrm{RCS}}^i={\left(C_i^b\right)}^T{\tilde{f}}^b---\left(29\right)$

取状态量为

$X=\left[\begin{array}{c}{r}^i\\ v^i\end{array}\right]---\left(30\right)$

并且令

那么(28)式可以写为

$\stackrel{\·}{X}=f(X,u)+w---\left(31\right)$

其中

$f(X,u)=\left[\begin{array}{c}{v}^i\\ -\frac{{\mathrm{\μ}}_m}{{\left|\right|r^i\left|\right|}^3}{r}^i+a_{\mathrm{RCS}}^i\end{array}\right]---\left(32\right)$

这里

并将它简单建模为均值为

，方差为

白噪声。

那么可以利用式(31)由t_k-1时刻的状态估计

预估t_k时刻的状态

${\hat{X}}_k=F({\hat{X}}_{k-1},u_{k-1})$

$\≈{\hat{X}}_{k-1}+(t_k-t_{k-1})\·f({\hat{X}}_{k-1},u_{k-1})+\frac{{(t_k-t_{k-1})}^2}{2!}\left[\frac{\∂f}{\∂X}f\right]{|}_{X={\hat{X}}_{k-1}}---\left(33\right)$

同时可进行状态估计方差的更新

$P_k={\mathrm{\Φ}}_{k|k-1}{P}_{k-1}{\mathrm{\Φ}}_{k|k-1}^T+Q_{k-1}---\left(34\right)$

其中

${\mathrm{\Φ}}_{k|k-1}=\frac{\∂F}{\∂X}{|}_{X={\hat{X}}_{k-1}}\≈I+\frac{\∂f}{\∂X}{|}_{X={\hat{X}}_{k-1}}\·(t_k-t_{k-1})---\left(35\right)$

(3)预估路标点位置

假定在t_k时刻，根据观测序列可以在导航图像中观测到m(m≥0)个路标。以其中的第j个路标为例，它在惯性系下的位置可以根据天体或者卫星的历书计算出来。将t_k时刻第j个路标的位置记为

(这里省略代表时间的下标k)，则它相对探测器的视线方向可用式(36)预报

${\hat{p}}_{m,j}^i=\frac{r_{m,j}^i-{\hat{r}}^i}{\left|\right|r_{m,j}^i-{\hat{r}}^i\left|\right|}---\left(36\right)$

写成分量形式，令rⁱ＝[xyz]^T， $r_{m,j}^i={\left[\begin{array}{ccc}{x}_{m,j}& y_{m,j}& z_{m,j}\end{array}\right]}^T,$ 则有

${\hat{p}}_{m,j}^i=\frac{1}{\sqrt{{(x_{m,j}-\hat{x})}^2+{(y_{m,j}-\hat{y})}^2+{(z_{m,j}-\hat{z})}^2}}\left[\begin{array}{c}{x}_{m,j}-\hat{x}\\ y_{m,j}-\hat{y}\\ z_{m,j}-\hat{z}\end{array}\right]---\left(37\right)$

光学导航敏感器测量坐标系(O_c-X_cY_cZ_c，其中原点在光学导航敏感器镜头中心，X_c和Y_c平行于焦平面，Z_c沿光轴方向)相对探测器本体系的转换关系由光学导航敏感器安装决定，记为

探测器相对惯性系的姿态矩阵为(由式(26)给出)，则根据坐标变换可将路标的预测视线方向转换到光学导航敏感器测量坐标系下

${\hat{p}}_{m,j}^c=C_b^c\·C_i^b\·{\hat{p}}_{m,j}^i---\left(38\right)$

设相机的焦距为f，路标点成像的像点坐标为x_cm，j和y_cm，j，则根据成像原理，可以用式(39)和(40)预测出路标点像点的坐标为

${\hat{x}}_{\mathrm{cm},j}=-f\frac{C_{11}(x_{m,j}-\hat{x})+C_{12}(y_{m,j}-\hat{y})+C_{13}(z_{m,j}-\hat{z})}{C_{31}(x_{m,j}-\hat{x})+C_{32}(y_{m,j}-\hat{y})+C_{33}(z_{m,j}-\hat{z})}---\left(39\right)$

${\hat{y}}_{\mathrm{cm},j}=-f\frac{C_{21}(x_{m,j}-\hat{x})+C_{22}(y_{m,j}-\hat{y})+C_{23}(z_{m,j}-\hat{z})}{C_{31}(x_{m,j}-\hat{x})+C_{32}(y_{m,j}-\hat{y})+C_{33}(z_{m,j}-\hat{z})}---\left(40\right)$

其中，

$C_i^c=C_b^c\·C_i^b$

$=\left[\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right]$

光学导航敏感器成像，经图像处理后实际给出的是路标点的像素坐标(p，l)。忽略图像畸变，则根据式(41)和(42)可以进一步预测第j个路标的像素坐标

${\hat{p}}_{m,j}=-K_{x}f\frac{C_{11}(x_{m,j}-\hat{x})+C_{12}(y_{m,j}-\hat{y})+C_{13}(z_{m,j}-\hat{z})}{C_{31}(x_{m,j}-\hat{x})+C_{32}(y_{m,j}-\hat{y})+C_{33}(z_{m,j}-\hat{z})}+p_0---\left(41\right)$

${\hat{l}}_{m,j}=-K_{y}f\frac{C_{21}(x_{m,j}-\hat{x})+C_{22}(y_{m,j}-\hat{y})+C_{23}(z_{m,j}-\hat{z})}{C_{31}(x_{m,j}-\hat{x})+C_{32}(y_{m,j}-\hat{y})+C_{33}(z_{m,j}-\hat{z})}+l_0---\left(42\right)$

其中，K_x和K_y是相机的成像参数，它反映了像点位置与像素坐标之间的比例关系。

(4)通过导航相机获取路标点位置

在t_k时刻，使用光学导航敏感器成像，通过图像处理技术从实际拍摄的导航图像中提取路标点的像素坐标。设当前导航图像中包含了m个路标，其中第j个路标的实际像素坐标为(p_m，j，l_m，j)，它与真实的探测器位置之间满足成像关系

$p_{m,j}=-K_{x}f\frac{C_{11}(x_{m,j}-x)+C_{12}(y_{m,j}-y)+C_{13}(z_{m,j}-z)}{C_{31}(x_{m,j}-x)+C_{32}(y_{m,j}-y)+C_{33}(z_{m,j}-z)}+p_0+v_{p,j}---\left(43\right)$

$l_{m,j}=-K_{x}f\frac{C_{21}(x_{m,j}-x)+C_{22}(y_{m,j}-y)+C_{23}(z_{m,j}-z)}{C_{31}(x_{m,j}-x)+C_{32}(y_{m,j}-y)+C_{33}(z_{m,j}-z)}+l_0+v_{l,j}---\left(44\right)$

其中，v_p，j和v_l，j是成像噪声。

(5)扩展卡尔曼滤波

令Z_j＝[p_m，j，l_m，j]^T，则式(43)和(44)可以构成测量方程

Z_j＝h_j(X)+v_j                (45)

其中，

$h_j\left(X\right)=\left[\begin{array}{c}-K_{x}f\frac{C_{11}(x_{m,j}-x)+C_{12}(y_{m,j}-y)+C_{13}(z_{m,j}-z)}{C_{31}(x_{m,j}-x)+C_{32}(y_{m,j}-y)+C_{33}(z_{m,j}-z)}+p_0\\ -K_{y}f\frac{C_{21}(x_{m,j}-x)+C_{22}(y_{m,j}-y)+C_{23}(z_{m,j}-z)}{C_{31}(x_{m,j}-x)+C_{32}(y_{m,j}-y)+C_{33}(z_{m,j}-z)}+l_0\end{array}\right]---\left(46\right)$

$v_j=\left[\begin{array}{c}{v}_{p,j}\\ v_{l,j}\end{array}\right]---\left(47\right)$

那么由当前时刻同时获得的m个路标构成的完成的滤波测量方程为

Z＝h(X)+v                (48)

其中，

$Z=\left[\begin{array}{c}{Z}_1\\ .\\ .\\ .\\ Z_m\end{array}\right]---\left(49\right)$

$h\left(X\right)=\left[\begin{array}{c}{h}_1\left(X\right)\\ .\\ .\\ .\\ h_m\left(X\right)\end{array}\right]---\left(50\right)$

$v\left[\begin{array}{c}{v}_1\\ .\\ .\\ .\\ v_m\end{array}\right]---\left(51\right)$

并设v的方差为R。

如此，就可以使用扩展卡尔曼滤波进行状态估计。

首先，根据式(34)预测的状态方差阵计算滤波增益阵

$K_k=P_k{H}_k^T{(H_k{P}_k{H}_k^T+R_k)}^{-1}---\left(52\right)$

且有

$H_k=\frac{\∂h\left(X\right)}{\∂\left(X\right)}{|}_{X={\hat{X}}_{k-1}}---\left(53\right)$

然后利用实际的测量对预估的状态进行修正

${\hat{X}}_k={\hat{X}}_k+K_k[Z_k-h\left({\hat{X}}_k\right)]---\left(54\right)$

最后对预测的状态方差阵进行修正

$P_k=(I-K_k{H}_k)P_k{(I-K_k{H}_k)}^T+K_k{R}_k{K}_k^T---\left(55\right)$

随着时间的递推，重复上述步骤，就完成了基于路标的环绕段导航解算。

仿真分析。以环绕火星飞行的探测器为例对本发明提出的导航方法进行验证。设探测器轨道的近火点高度为353km，远火点高度56600km。探测器在环绕飞行过程中根据光照条件、飞行高度和目标的可见性分段选择火卫一、火卫二或者火星表面的特征地形为路标进行自主导航，如图2所示。使用的导航相机视场为2°，焦距为0.1m，像素分辨率为1024×1024。路标点像素提取精度为0.1像素(1σ)，导航修正的周期为1分钟。

从近火点开始一个轨道周期内的导航路标选择如图3所示。其优先级按照可见性和光照条件，以先火卫一、然后火卫二、最后火星表面特征地形为顺序。相应的导航位置误差和速度误差的变化曲线如图4和图5所示。从图可见，全程导航位置误差小于2km，速度误差小于3m/s。

本发明未详细阐述部分属于本领域公知技术。

Claims (2)

1.一种基于天体表面特征和天然卫星路标的环绕段导航方法，其特征在于实现步骤如下：

(1)陀螺和星敏感器的惯性姿态估计

利用前一个采样周期内陀螺测量的角速度对当前时刻的探测器惯性姿态进行外推；然后利用星敏感器获得的光轴惯性指向测量，使用卡尔曼滤波对陀螺外推惯性姿态的误差进行估计；最后用误差的估计值修正陀螺预估的惯性姿态，获得最终的惯性姿态估计；

(2)利用加速度计测量的位置、速度外推

在陀螺测量的同时，使用加速度计获得探测器非引力加速度的测量值，利用已获得的惯性姿态估计值将它转化到惯性坐标系中；然后根据轨道动力学模型，对探测器的位置和速度进行外推；

(3)路标点信息的获取

使用探测器上安装的光学导航相机按照一定拍摄序列对可能的路标点进行拍照，路标点的选择包含了天体表面的已知位置的特征地貌和天体的天然卫星；路标点根据轨道高度、光照条件事先选择，并列入观测序列；最后，从导航图像上提取出路标点的像素坐标，传递给导航滤波器；

(4)基于路标的探测器位置、速度的估计

基于步骤(2)中外推的探测器位置、速度对路标点的图像坐标进行预报；将实际获得的路标点像素坐标与预报值进行比对，构成测量新息；然后使用扩展卡尔曼滤波对预测的探测器位置、速度进行修正，得到最终的探测器位置、速度估计值；

(5)对每一个控制周期重复(1)～(4)的步骤，完成导航解算。

2.根据权利要求1所述的基于天体表面特征和天然卫星路标的环绕段导航方法，其特征在于：所述方法同时将天体表面特征和天然卫星均作为导航路标，在轨道高度高于天然卫星轨道时以天然卫星和天体表面大型地貌为路标，在轨道低于天然卫星轨道时以天体局部地形为路标，导航滤波解算过程中路标点的数量可变。

Images