CN102168981A - 一种深空探测器火星捕获段自主天文导航方法 - Google Patents

Abstract

本发明涉及一种深空探测器火星捕获段自主天文导航方法。根据圆形限制性四体轨道动力学模型建立深空探测器的状态模型；利用敏感器获得火星、火星卫星以及背景恒星的像元像线信息，把所获得的像元像线信息转换为火星、火星卫星以及背景恒星的角度信息，建立火星、火卫一和火卫二的角度量测模型；并使用q-method方法估计探测器姿态信息，结合Unscented卡尔曼滤波估计深空探测器的位置和速度。本发明估计精度高，非常适用于火星捕获的自主导航。本发明属于航天导航技术领域，不仅可以为深空探测器被火星捕获提供高精度导航参数，而且可为其自主导航系统设计提供参考。

Description

一种深空探测器火星捕获段自主天文导航方法

技术领域

本发明涉及在深空探测器处于火星捕获段时，基于火星、火卫一和火卫二角度信息的自主天文导航方法，是一种非常适用于火星捕获段的自主导航方法。

背景技术

在深空探测器的飞行过程中，探测器火星捕获段的导航精度直接影响了下一阶段的探测任务，如火星捕获着陆等的精度，借力飞行导航精度等，如果这一阶段导航误差太大，就无法及时通过轨道控制推进系统调整探测器的轨道,错过轨道控制的时机后，就无法再调整回预定轨道，不能被火星捕获或由火星借力飞行后轨道严重偏离预定轨道，最终导致深空探测任务的失败。因此深空探测器对火星捕获段的导航品质的要求很高。但处于火星捕获这一阶段的深空探测器距离地球较远，飞行速度较快，飞行距离远，时间长，环境未知性较强，传统上依靠地面测控的深空探测器导航与控制方法在实时性、成本和资源上受到种种限制，存在很多不足，很难满足深空探测一些特殊任务对高精度导航与控制的需要，因而对自主导航的要求更为迫切。

由于深空探测器火星捕获段距离太阳和其他各行星的距离都较远，用于近地逃逸轨道或行星着陆等阶段的自主导航方法，如基于IMU、测距测速敏感器、地面图像等的自主导航方法都无法应用，天文导航是唯一有效的自主导航手段。

目前可以用于深空探测器火星捕获段的自主导航方式主要有：基于小行星的自主天文导航方法、基于脉冲星的自主天文导航方法和基于太阳和大行星的自主天文导航方法。基于小行星的自主天文导航方法是利用地火轨道之间的小行星位置信息来确定探测器的位置，但由于火星捕获阶段可以观测的小行星有限，且对探测器的轨道设计要求较高，因此这种方法无法提供高精度的导航信息且受到小行星交会时间的限制。基于脉冲星的自主天文导航方法是利用脉冲接收机接收宇宙空间中X射线脉冲星的脉冲，测量脉冲到达时间，但由于目前脉冲到达时间测量精度较低，且可用于导航的X射线脉冲星数目较少，因此这种方法尚未实际应用。基于太阳和大行星的自主天文导航方法是借助太阳和大行星的位置信息来确定探测器的位置，由于在捕获火星这一阶段探测器距离火星和火星的卫星较近，适于观测且可以提供精确的图像信息，但是传统上利用图像信息都是基于像元像线这一量测信息，这种方法在敏感器的成像过程受探测器姿态约束，而导航滤波中需要探测器估计姿态确定量测预测值，由于姿态估计信息与真实姿态之间存在误差，因此探测器的姿态估计误差会造成较大的量测预测误差，最终影响探测器的定位精度。

发明内容

本发明要解决的技术问题是：克服基于小行星和脉冲星自主导航方法中导航小行星或导航脉冲星数量少、精度低、对探测器轨道设计要求高等缺点，弥补传统上观测火星、火卫一和火卫二的方法中像元像线信息受探测器姿态约束这一不足，为深空探测器在捕获火星过程中提供一种高精度的自主天文导航方法。

本发明解决其技术问题所采用的技术方案为：建立高精度的深空探测器的状态模型，通过敏感器获得火星、火星卫星及其相应图像中的背景恒星的像元像线信息，之后把像元像线信息转换为方向矢量，建立火星、火卫一和火卫二之间夹角的量测模型，其中使用Unscented卡尔曼滤波估计深空探测器的导航参数。

具体包括以下步骤：

1.建立基于轨道动力学的深空探测器火星捕获段导航系统状态模型；

采用圆形限制性四体模型，考虑太阳中心引力、火星中心引力和地球中心引力对探测器的作用，选取日心黄道惯性坐标系，可得深空探测器的状态模型为

$\left\{\begin{array}{c}\stackrel{\·}{x}=v_x\\ \stackrel{\·}{y}=v_y\\ \stackrel{\·}{z}=v_z\\ {\stackrel{\·}{v}}_x=-{\mathrm{\μ}}_s\frac{x}{r_{\mathrm{ps}}^3}-{\mathrm{\μ}}_m[\frac{x-x_1}{r_{\mathrm{pm}}^3}+\frac{x_1}{r_{\mathrm{sm}}^3}]-{\mathrm{\μ}}_e[\frac{x-x_2}{r_{\mathrm{pe}}^3}+\frac{x_2}{r_{\mathrm{se}}^3}]+w_x\\ {\stackrel{\·}{v}}_y=-{\mathrm{\μ}}_s\frac{y}{r_{\mathrm{ps}}^3}-{\mathrm{\μ}}_m[\frac{y-y_1}{r_{\mathrm{pm}}^3}+\frac{y_1}{r_{\mathrm{sm}}^3}]-{\mathrm{\μ}}_e[\frac{y-y_2}{r_{\mathrm{pe}}^3}+\frac{y_2}{r_{\mathrm{se}}^3}]+w_y\\ {\stackrel{\·}{v}}_z=-{\mathrm{\μ}}_s\frac{z}{r_{\mathrm{ps}}^3}-{\mathrm{\μ}}_m[\frac{z-z_1}{r_{\mathrm{pm}}^3}+\frac{z_1}{r_{\mathrm{sm}}^3}]-{\mathrm{\μ}}_e[\frac{z-z_2}{r_{\mathrm{pe}}^3}+\frac{z_2}{r_{\mathrm{se}}^3}]+w_z\end{array}\right.---\left(1\right)$

式中，

探测器三轴位置的微分，v_x，v_y，v_z为探测器三轴的速度坐标，

为探测器三轴速度的微分，μ_s、μ_m和μ_e分别为太阳、火星和地球引力常数，r_ps为日心到探测器的距离，r_pm为火星到探测器的距离，r_sm为火心到日心的距离，r_pe为地球到探测器的距离，r_se为地心到日心的距离，(x₁，y₁，z₁)，(x₂，y₂，z₂)和(x，y，z)分别为火星、地球和探测器在日心黄道惯性坐标系中的位置，其中火星和地球的位置可由行星星历表获得，w_x，w_y，w_z分别为探测器三轴的模型误差；

式(1)中的各变量都是与时间t有关的函数，可简写为

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

状态变量为X＝[x，y，z，v_x，v_y，v_z]^T，f(X(t)，t)为系统非线性连续状态转移函数，状态噪声为w＝[w_x，w_y，w_x]^T。

2.建立火星、火卫一和火卫二与其背景恒星的像元像线模型

建立火星、火卫一和火卫二与其背景恒星的像元像线模型为

$\left\{\begin{array}{c}\left[\begin{array}{c}{p}_m\\ l_m\end{array}\right]=K_m\left[\begin{array}{c}{x}_m^{2d}\\ y_m^{2d}\end{array}\right]+\left[\begin{array}{c}{p}_{0m}\\ l_{0m}\end{array}\right]=K_m\frac{f_m}{z_m^c}\left[\begin{array}{c}{x}_m^c\\ y_m^c\end{array}\right]+\left[\begin{array}{c}{p}_{0m}\\ l_{0m}\end{array}\right]+\left[\begin{array}{c}{v}_{p_m}\\ v_{l_m}\end{array}\right]\\ \left[\begin{array}{c}{p}_{\mathrm{msi}}\\ l_{\mathrm{msi}}\end{array}\right]=K_m\left[\begin{array}{c}{x}_{\mathrm{msi}}^{2d}\\ y_{\mathrm{msi}}^{2d}\end{array}\right]+\left[\begin{array}{c}{p}_{0m}\\ l_{0m}\end{array}\right]=K_m\frac{f_m}{z_{\mathrm{msi}}^c}\left[\begin{array}{c}{x}_{\mathrm{msi}}^c\\ y_{\mathrm{msi}}^c\end{array}\right]+\left[\begin{array}{c}{p}_{0m}\\ l_{0m}\end{array}\right]+\left[\begin{array}{c}{v}_{p_{\mathrm{msi}}}\\ v_{l_{\mathrm{msi}}}\end{array}\right]\end{array}\right.---\left(3\right)$

$\left\{\begin{array}{c}\left[\begin{array}{c}{p}_p\\ l_p\end{array}\right]=K_p\left[\begin{array}{c}{x}_p^{2d}\\ y_p^{2d}\end{array}\right]+\left[\begin{array}{c}{p}_{0p}\\ l_{0p}\end{array}\right]=K_p\frac{f_p}{z_p^c}\left[\begin{array}{c}{x}_p^c\\ y_p^c\end{array}\right]+\left[\begin{array}{c}{p}_{0p}\\ l_{0p}\end{array}\right]+\left[\begin{array}{c}{v}_{p_p}\\ v_{l_p}\end{array}\right]\\ \left[\begin{array}{c}{p}_{\mathrm{psi}}\\ l_{\mathrm{psi}}\end{array}\right]=K_p\left[\begin{array}{c}{x}_{\mathrm{psi}}^{2d}\\ y_{\mathrm{psi}}^{2d}\end{array}\right]+\left[\begin{array}{c}{p}_{0p}\\ l_{0p}\end{array}\right]=K_p\frac{f_p}{z_{\mathrm{psi}}^c}\left[\begin{array}{c}{x}_{\mathrm{psi}}^c\\ y_{\mathrm{psi}}^c\end{array}\right]+\left[\begin{array}{c}{p}_{0p}\\ l_{0p}\end{array}\right]+\left[\begin{array}{c}{v}_{p_{\mathrm{psi}}}\\ v_{l_{\mathrm{psi}}}\end{array}\right]\end{array}\right.---\left(4\right)$

$\left\{\begin{array}{c}\left[\begin{array}{c}{p}_d\\ l_d\end{array}\right]=K_d\left[\begin{array}{c}{x}_d^{2d}\\ y_d^{2d}\end{array}\right]+\left[\begin{array}{c}{p}_{0d}\\ l_{0d}\end{array}\right]=K_d\frac{f_d}{z_d^c}\left[\begin{array}{c}{x}_d^c\\ y_d^c\end{array}\right]+\left[\begin{array}{c}{p}_{0d}\\ l_{0d}\end{array}\right]+\left[\begin{array}{c}{v}_{p_d}\\ v_{l_d}\end{array}\right]\\ \left[\begin{array}{c}{p}_{\mathrm{dsi}}\\ l_{\mathrm{dsi}}\end{array}\right]=K_d\left[\begin{array}{c}{x}_{\mathrm{dsi}}^{2d}\\ y_{\mathrm{dsi}}^{2d}\end{array}\right]+\left[\begin{array}{c}{p}_{0d}\\ l_{0d}\end{array}\right]=K_d\frac{f_d}{z_{\mathrm{dsi}}^c}\left[\begin{array}{c}{x}_{\mathrm{dsi}}^c\\ y_{\mathrm{dsi}}^c\end{array}\right]+\left[\begin{array}{c}{p}_{0d}\\ l_{0d}\end{array}\right]+\left[\begin{array}{c}{v}_{p_{\mathrm{dsi}}}\\ v_{l_{\mathrm{dsi}}}\end{array}\right]\end{array}\right.---\left(5\right)$

式中，(p_m，l_m)、(p_p，l_p)和(p_d，l_d)分别为火星、火卫一和火卫二成像在各敏感器二维成像平面的像元和像线，(p_msi，l_msi)、(p_psi，l_psi)和(p_dsi，l_dsi)分别为火星敏感器、火卫一敏感器和火卫二敏感器中的第i颗背景恒星的像元和像线，i＝1，2，3，(p_0m，l_0m)，(p_0p，l_0p)和(p_0d，l_0d)分别为火星、火卫一和火卫二在敏感器二维成像平面中心的像元和像线；K_m、K_p和K_d分别为火星、火卫一和火卫二敏感器由毫米转为像素的敏感器转换矩阵；f_m、f_p和f_d分别为火星、火卫一和火卫二敏感器的焦距，

和分别为火星、火卫一和火卫二在敏感器二维像平面的坐标，

和

分别为火星敏感器、火卫一敏感器和火卫二敏感器中的第i颗背景恒星的二维像平面坐标，

和

分别为敏感器坐标系中的火星、火卫一和火卫二的坐标，这些坐标可以由惯性系中的坐标经惯性系到敏感器坐标系的转换矩阵A_ic得出，A_ic＝A_ib·A_bc，其中A_ib可以由q-method方法结合星敏感器获取的恒星矢量方向获得；

分别为火星、火卫一、火卫二在图像中的像元像线观测误差，

分别为火星、火卫一、火卫二图像中的第i颗背景恒星的像元像线观测误差；

3.建立火星、火卫一和火卫二角度信息的量测模型

火星与第i颗背景恒星之间角度信息的表达式为

${\mathrm{\θ}}_{\mathrm{mi}}=\arccos (-{\stackrel{\→}{l}}_{\mathrm{pm}}^c\·{\stackrel{\→}{s}}_{1i})---\left(6\right)$

式中，

为在火星敏感器坐标系中第i颗导航背景恒星星光的单位矢量，i＝1，2，3，

为火星到探测器的单位矢量，可表示为

${\stackrel{\→}{l}}_{\mathrm{pm}}^c=\frac{1}{\sqrt{{\left(x_m^{2d}\right)}^2+{\left(y_m^{2d}\right)}^2+{f_m}^2}}\left[\begin{array}{c}{x}_m^{2d}\\ y_m^{2d}\\ -f_m\end{array}\right]---\left(7\right)$

可以由像元像线转换得出，具体表达式为

$\left[\begin{array}{c}{x}_m^{2d}\\ y_m^{2d}\end{array}\right]=K_m^{-1}(\left[\begin{array}{c}{p}_m\\ l_m\end{array}\right]-\left[\begin{array}{c}{p}_{0m}\\ l_{0m}\end{array}\right])---\left(8\right)$

火卫一与第i颗背景恒星之间角度信息的表达式为

${\mathrm{\θ}}_{\mathrm{pi}}=\arccos (-{\stackrel{\→}{l}}_{\mathrm{pp}}^c\·{\stackrel{\→}{s}}_{2i})---\left(9\right)$

式中，

为在火卫一敏感器坐标系中第i颗导航背景恒星星光的单位矢量，i＝1，2，3，

为火卫一到探测器的单位矢量，可表示为

${\stackrel{\→}{l}}_{\mathrm{pp}}^c=\frac{1}{\sqrt{{\left(x_p^{2d}\right)}^2+{\left(y_p^{2d}\right)}^2+{f_p}^2}}\left[\begin{array}{c}{x}_p^{2d}\\ y_p^{2d}\\ -f_p\end{array}\right]---\left(10\right)$

可以由像元像线转换得出，具体表达式为

$\left[\begin{array}{c}{x}_p^{2d}\\ y_p^{2d}\end{array}\right]=K_p^{-1}(\left[\begin{array}{c}{p}_p\\ l_p\end{array}\right]-\left[\begin{array}{c}{p}_{0p}\\ l_{0p}\end{array}\right])---\left(11\right)$

火卫二与第i颗背景恒星之间角度信息的表达式为

${\mathrm{\θ}}_{\mathrm{di}}=\arccos (-{\stackrel{\→}{l}}_{\mathrm{pd}}^c\·{\stackrel{\→}{s}}_{3i})---\left(12\right)$

式中，

为在火卫二敏感器坐标系中第i颗导航背景恒星星光的单位矢量，i＝1，2，3，

为火卫二到探测器的单位矢量，可表示为

${\stackrel{\→}{l}}_{\mathrm{pd}}^c=\frac{1}{\sqrt{{\left(x_d^{2d}\right)}^2+{\left(y_d^{2d}\right)}^2+{f_d}^2}}\left[\begin{array}{c}{x}_d^{2d}\\ y_d^{2d}\\ -f_d\end{array}\right]---\left(13\right)$

可以由像元像线转换得出，具体表达式为

$\left[\begin{array}{c}{x}_d^{2d}\\ y_d^{2d}\end{array}\right]=K_d^{-1}(\left[\begin{array}{c}{p}_d\\ l_d\end{array}\right]-\left[\begin{array}{c}{p}_{0d}\\ l_{0d}\end{array}\right])---\left(14\right)$

因此火星、火卫一和火卫二以及三颗背景恒星的角度信息θ_mi、θ_pi和θ_di表达式为

$\left\{\begin{array}{c}{\mathrm{\θ}}_{\mathrm{mi}}=\arccos (-{\stackrel{\→}{l}}_{\mathrm{pm}}^c\·{\stackrel{\→}{s}}_{1i})\\ {\mathrm{\θ}}_{\mathrm{pi}}=\arccos (-{\stackrel{\→}{l}}_{\mathrm{pp}}^c\·{\stackrel{\→}{s}}_{2i})\\ {\mathrm{\θ}}_{\mathrm{di}}=\arccos (-{\stackrel{\→}{l}}_{\mathrm{pd}}^c\·{\stackrel{\→}{s}}_{3i})\end{array}\right.---\left(15\right)$

设系统量测量Z＝[θ_m1，θ_m2，θ_m3，θ_p1，θ_p2，θ_p3，θ_d1，θ_d2，θ_d3]^T，量测噪声

分别为测量θ_m1，θ_m2，θ_m3，θ_p1，θ_p2，θ_p3，θ_d1，θ_d2，θ_d3的观测误差，由于各变量都是与时间t有关的变量，则可建立火星、火卫一和火卫二角度信息量测模型的表达式为

Z(t)＝h[X(t)，t]+v(t)           (16)

4.对步骤1式(2)所示状态模型及步骤3式(16)所示的量测模型进行离散化

X(k+1)＝F(X(k)，k)+w(k)         (17)

Z(k)＝H(X(k)，k)+v(k)           (18)

式中，k＝1，2，…，F(X(k)，k)为f(X(t)，t)离散后的非线性状态转移函数，H(X(k)，k)为h(X(t)，t)离散后的非线性量测函数，w(k)、v(k)互不相关。

5.轨道Unscented卡尔曼滤波。

利用Unscented卡尔曼滤波算法，结合所述的状态模型和量测模型进行滤波，利用敏感器获取测量得到的火星、火卫一、火卫二和背景恒星的像元像线信息，并将此测量信息转换为量测量火星、火卫一、火卫二和背景恒星的角度信息，通过量测量与量测模型相减得到系统量测残差，用系统这一残差校正量测模型的模型误差；利用Unscented采样13个采样点，利用系统状态模型，对采样点进行一步预测，并得出与上一步迭代状态值之间协方差阵，以消除状态模型模型误差的影响，最终输出导航信息，具体步骤为：

①初始化状态量

和状态误差方差阵P₀

${\hat{x}}_0=E\left[x_0\right],$ $P_0=E\left[(x_0-{\hat{x}}_0){(x_0-{\hat{x}}_0)}^T\right]---\left(19\right)$

②计算状态量采样点

在

附近选取一系列样本点，这些样本点的均值和协方差分别为

和P_k-1|k-1，这里

且P_k-1|k-1＝P_k-1。设状态变量为6×1维，那么13个样本点χ_0，k，χ_1，k，...，χ_12，k及其权重W₀，W₁…，W₁₂分别如下

${\mathrm{\χ}}_{0,k}={\hat{x}}_k,$ W₀＝-1

${\mathrm{\χ}}_{i,k}={\hat{x}}_k+\sqrt{3}{\left(\sqrt{P\left(k\right|k)}\right)}_j,$ W_j＝1/6          (20)

${\mathrm{\χ}}_{i+6,k}={\hat{x}}_k-\sqrt{3}{\left(\sqrt{P\left(k\right|k)}\right)}_j,$ W_j+6＝1/6

式中，当P(k|k)＝A^TA时，

取A的第j行，当P(k|k)＝AA^T时，

取A的第j列，则所有采样点的表达式为

j＝1，2，....，6    (21)

③时间更新

状态量的一步预测χ_k+1|k为

χ_k|k-1＝f(χ_k-1，k-1)         (22)

所有采样点状态量的一步预测加权后结果

为

${\hat{x}}_k^{-}=\underset{j=0}{\overset{12}{\mathrm{\Σ}}}{W}_j{\mathrm{\χ}}_{j,k|k-1}---\left(23\right)$

式中，W_j为第i个采样点的权值；

状态量的估计方差一步预测为

$P_k^{-}=\underset{j=0}{\overset{12}{\mathrm{\Σ}}}{W}_j[{\mathrm{\χ}}_{j,k|k-1}-{\hat{x}}_k^{-}]{[{\mathrm{\χ}}_{j,k|k-1}-{\hat{x}}_k^{-}]}^T+Q_k---\left(24\right)$

式中，Q_k为k时刻状态模型噪声协方差阵；

采样点对应的量测估计值X_k|k-1

Z_k|k-1＝h(χ_k|k-1，k)        (25)

采样点量测估计加权值

${\hat{z}}_k^{-}=\underset{j=0}{\overset{12}{\mathrm{\Σ}}}{W}_j{Z}_{j,k|k-1}---\left(26\right)$

④量测更新

量测方差阵

为

$P_{{\hat{z}}_k{\hat{z}}_k}=\underset{j=0}{\overset{12}{\mathrm{\Σ}}}{W}_j[Z_{j,k|k-1}-{\hat{z}}_k^{-}]{[Z_{j,k|k-1}-{\hat{z}}_k^{-}]}^T+R_k---\left(27\right)$

式中，R_k为量测噪声协方差；

状态变量量测量方差阵

$P_{{\hat{x}}_k{\hat{z}}_k}=\underset{j=0}{\overset{12}{\mathrm{\Σ}}}{W}_j[{\mathrm{\χ}}_{j,k|k-1}-{\hat{x}}_k^{-}]{[Z_{j,k|k-1}-{\hat{z}}_k^{-}]}^T---\left(28\right)$

滤波增益K_k为

$K_k=P_{{\hat{x}}_k{\hat{z}}_k}{P}_{{\hat{z}}_k{\hat{z}}_k}^{-1}---\left(29\right)$

状态量的估计值

和估计方差P_k为

${\hat{x}}_k={\hat{x}}_k^{-}+K_k(Z_k-{\hat{z}}_k^{-})---\left(30\right)$

$P_k=P_k^{-}-K_k{P}_{{\hat{z}}_k{\hat{z}}_k}{K}_k^T---\left(31\right)$

式中，Q_k和R_k分别为系统和量测噪声协方差。

将获得的状态量的估计值

和估计方差P_k返回滤波器，用于k+1时刻，k＝1，2，...，最终将状态估计值

和状态估计方差P_k输出，其中状态估计值包括探测器的速度、位置信息，输出的状态估计方差P_k表示了滤波估计的性能。

本发明的原理是：首先建立深空探测器轨道动力学模型，利用圆形限制性四体模型建立其状态模型，然后根据敏感器像元像线与矢量方向之间的变换关系，建立火星、火卫一和火卫二之间角度信息的量测模型。由于除了受到测量仪器精度的制约之外，系统状态模型和量测模型都存在模型误差，模型误差的非线性问题是限制深空探测器捕获前导航精度的主要因素，因此在轨道确定滤波中使用Unscented卡尔曼滤波方法，利用成像敏感器的实际测量得到的像元像线，经过像元像线到惯性坐标系的坐标转换，从而间接地通过像元像线信息获得火星、火卫一和火卫二之间的角度信息这一量测量，用量测量与模型之间的残差校正模型误差，并解决系统的非线性问题，实现对位置、速度等导航参数进行估计。

本发明与现有技术相比的优点在于：(1)充分利用火星敏感器、火卫一敏感器、火卫二敏感器等光学成像敏感器获得的图像信息，将目标天体(火星、火卫一和火卫二)的像元像线信息和图像中的背景恒星信息相结合，为探测器被火星捕获提供高精度的导航性能；(2)利用火星、火卫一和火卫二与火星、火卫一和火卫二敏感器图像中的背景恒星之间的角度信息，克服了姿态估计误差对火星、火卫一和火卫二像元像线量测模型精度的影响，进一步提高了深空探测器的导航精度。

附图说明

图1为本发明深空探测器火星捕获段天文导航方法的流程图。

图2为本发明中像元像线模型的示意图。

图3为本发明中的敏感器安装方位角与高度角示意图。

图4为本发明中火星、火卫一和火卫二角度信息量测模型的示意图。

具体实施方式

如图1所示，本发明的具体实施方法如下：

1.建立基于轨道动力学的深空探测器火星捕获段导航系统状态模型

首先初始化探测器位置、速度，设状态量X＝[x y z v_x v_y v_z]^T，x，y，z，v_x，v_y，v_z分别为探测器在日心惯性坐标系中三轴的位置和速度，根据探测器的轨道设计，选取探测器的位置和速度初值为

X＝[-1.572×10¹¹m-1.574×10¹¹m-6.797×10¹⁰m

1.381×10⁴m/s-1.488×10⁴m/s-6.478×10³m/s]^T

之后建立基于圆形限制性四体模型的深空探测器导航系统状态模型：圆形限制性四体模型考虑了太阳中心引力、火星中心引力和地球中心引力对探测器的作用，选取日心黄道惯性坐标系，可得深空探测器的状态模型为

$\left\{\begin{array}{c}\stackrel{\·}{x}=v_x\\ \stackrel{\·}{y}=v_y\\ \stackrel{\·}{z}=v_z\\ {\stackrel{\·}{v}}_x=-{\mathrm{\μ}}_s\frac{x}{r_{\mathrm{ps}}^3}-{\mathrm{\μ}}_m[\frac{x-x_1}{r_{\mathrm{pm}}^3}+\frac{x_1}{r_{\mathrm{sm}}^3}]-{\mathrm{\μ}}_e[\frac{x-x_2}{r_{\mathrm{pe}}^3}+\frac{x_2}{r_{\mathrm{se}}^3}]+w_x\\ {\stackrel{\·}{v}}_y=-{\mathrm{\μ}}_s\frac{y}{r_{\mathrm{ps}}^3}-{\mathrm{\μ}}_m[\frac{y-y_1}{r_{\mathrm{pm}}^3}+\frac{y_1}{r_{\mathrm{sm}}^3}]-{\mathrm{\μ}}_e[\frac{y-y_2}{r_{\mathrm{pe}}^3}+\frac{y_2}{r_{\mathrm{se}}^3}]+w_y\\ {\stackrel{\·}{v}}_z=-{\mathrm{\μ}}_s\frac{z}{r_{\mathrm{ps}}^3}-{\mathrm{\μ}}_m[\frac{z-z_1}{r_{\mathrm{pm}}^3}+\frac{z_1}{r_{\mathrm{sm}}^3}]-{\mathrm{\μ}}_e[\frac{z-z_2}{r_{\mathrm{pe}}^3}+\frac{z_2}{r_{\mathrm{se}}^3}]+w_z\end{array}\right.---\left(32\right)$

式中，

探测器三轴位置的微分，v_x，v_y，v_z为探测器三轴的速度，

为探测器三轴速度的微分，μ_s、μ_m和μ_e分别为太阳、火星和地球引力常数，r_ps为日心到探测器的距离，r_pm为火星到探测器的距离，r_sm为火心到日心的距离，r_pe为地球到探测器的距离，r_se为地心到日心的距离，(x₁，y₁，z₁)，(x₂，y₂，z₂)和(x，y，z)分别为火星、地球和探测器的位置，其中火星和地球的位置可由行星星历表获得，w_x，w_y，w_z分别为探测器三轴的模型误差；

式(32)中各变量都是与时间t有关的函数，可简写为

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

状态变量为X＝[x，y，z，v_x，v_y，v_z]^T，f(X(t)，t)为系统非线性连续状态转移函数，状态噪声为w＝[w_x，w_y，w_z]^T。

2.建立火星、火卫一、火卫二和背景恒星的的像元像线模型

图2以火星为例描述了火星敏感器的成像过程，其他敏感器成像过程与之类似。火星敏感器主要由光学透镜和二维成像面阵组成，在火星敏感器测量坐标系OX_cY_cZ_c中火星反射太阳光线沿火星到探测器的方向矢量射向火星敏感器，此时，火星在火星敏感器测量坐标系中的坐标为(x_c，y_c，z_c)；火星敏感器的光学透镜以焦距f将火星的光线折射后成像在二维成像面阵上，二维成像面阵将照在每个成像单元上的图像亮度信号储存；由于火星在二维成像面阵上的图像并不是一个点，而是一个圆，通过质心识别等图像处理技术确定火星图像在二维成像平面坐标系PX_2dY_2d的质心(x_2d，y_2d)，这一中心可以用像元像线坐标系O_p1X_p1Y_p1中的像元像线(p，l)。

因此图2中火星敏感器像元像线坐标系OX_p1Y_p1中的火星的像元像线坐标可以表示为

$\left[\begin{array}{c}p\\ l\end{array}\right]=K\left[\begin{array}{c}{x}_{2d}\\ y_{2d}\end{array}\right]+\left[\begin{array}{c}{p}_0\\ l_0\end{array}\right]---\left(34\right)$

式中，p和l分别为火星在火星敏感器二维成像平面的像元和像线，

为由毫米转为像素的敏感器转换矩阵，(x_2d，y_2d)为火星在敏感器二维成像平面OX_2dY_2d中的坐标，p₀和l₀分别为火星敏感器中心在像元像线坐标系OX_p1Y_p1中的像元和像线。其中火星在火星敏感器二维成像平面坐标系OX_2dY_2d中的坐标(x_2d，y_2d)可由下式得出

$\left[\begin{array}{c}{x}_{2d}\\ y_{2d}\end{array}\right]=\frac{f}{z_c}\left[\begin{array}{c}-x_c\\ -y_c\end{array}\right]---\left(35\right)$

式中，(x_c，y_c，z_x)为火星在火星敏感器测量坐标系OX_cY_cZ_c中的坐标，f为火星敏感器的焦距。其中火星在成像坐标系中的坐标(x_c，y_c，z_c)可由式(36)得出

$\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\end{array}\right]=A_{\mathrm{cb}}\left[\begin{array}{c}{x}_b\\ y_b\\ z_b\end{array}\right]=A_{\mathrm{cb}}{A}_{\mathrm{bi}}\left[\begin{array}{c}{x}_i\\ y_i\\ z_i\end{array}\right]---\left(36\right)$

式中，(x_i，y_i，z_i)为火星在惯性坐标系中的坐标，由星历表获取；(x_b，y_b，z_b)为火星在探测器本体坐标系中的坐标；A_bi为惯性坐标系到探测器本体系的坐标转换阵，由姿态控制系统输出。A_cb为探测器本体系到探测器测量坐标系的坐标转换阵，由火星敏感器安装在探测器上的位置确定可以由火星敏感器安装方位角Az和高度角El表示，如图3所示，火星敏感器安装方位角Az和高度角El描述了火星敏感器测量坐标系OX_cY_cZ_c与火星探测器本体系OX_bY_bZ_b之间的关系，其中安装方位角Az描述了火星敏感器测量坐标系中的X_c轴与Y_c轴在火星探测器本体系OX_bY_b平面内的投影X′与Y′与本体系X_b轴和Y_b之间的角度，高度角El描述了火星敏感器测量坐标系中的Z_c轴到火星探测器本体系OX_bY_b平面的夹角。可以借助安装方位角Az和高度角El描述火星探测器本体系到火星敏感器测量坐标系的转换，即火星探测器本体系OX_bY_bZ_b绕Z_b旋转Az角，则火星探测器本体系Y_b轴与火星敏感器测量坐标系Y_c轴重合，火星探测器本体系X_b轴旋转至X′轴位置；之后经上述旋转后的坐标系OX′Y′Z′绕Y′旋转90°-El角，则旋转后的坐标系OX″Y″Z″与火星探测器测量坐标系OX_cY_cZ_c重合，即火星探测器本体系可以经过上述轴角转换转换至火星敏感器测量坐标系，则A_cb可以表示为

$A_{\mathrm{cb}}=R_y(90-\mathrm{El})R_z\left(\mathrm{Az}\right)$

$=\left[\begin{array}{ccc}\sin \left(\mathrm{El}\right)\cos \left(\mathrm{Az}\right)& \sin \left(\mathrm{El}\right)\sin \left(\mathrm{Az}\right)& -\cos \left(\mathrm{Az}\right)\\ -\sin \left(\mathrm{Az}\right)& \cos \left(\mathrm{Az}\right)& 0\\ \cos \left(\mathrm{El}\right)\cos \left(\mathrm{Az}\right)& \cos \left(\mathrm{El}\right)\sin \left(\mathrm{Az}\right)& \sin \left(\mathrm{El}\right)\end{array}\right]---\left(37\right)$

式(37)表示火星坐标从日心惯性坐标系转换至探测器本体系，再由探测器本体系转换到到火星敏感器测量坐标系。

则由式(34)～式(37)可以推出火星及其背景恒星的像元像线表达式为

$\left\{\begin{array}{c}\left[\begin{array}{c}{p}_m\\ l_m\end{array}\right]=K_m\left[\begin{array}{c}{x}_m^{2d}\\ y_m^{2d}\end{array}\right]+\left[\begin{array}{c}{p}_{0m}\\ l_{0m}\end{array}\right]=K_m\frac{f_m}{z_m^c}\left[\begin{array}{c}{x}_m^c\\ y_m^c\end{array}\right]+\left[\begin{array}{c}{p}_{0m}\\ l_{0m}\end{array}\right]+\left[\begin{array}{c}{v}_{p_m}\\ v_{l_m}\end{array}\right]\\ \left[\begin{array}{c}{p}_{\mathrm{ms}1}\\ l_{\mathrm{ms}1}\end{array}\right]=K_m\left[\begin{array}{c}{x}_{\mathrm{ms}1}^{2d}\\ y_{\mathrm{ms}1}^{2d}\end{array}\right]+\left[\begin{array}{c}{p}_{0m}\\ l_{0m}\end{array}\right]=K_m\frac{f_m}{z_{\mathrm{ms}1}^c}\left[\begin{array}{c}{x}_{\mathrm{ms}1}^c\\ y_{\mathrm{ms}1}^c\end{array}\right]+\left[\begin{array}{c}{p}_{0m}\\ l_{0m}\end{array}\right]+\left[\begin{array}{c}{v}_{p_{\mathrm{ms}1}}\\ v_{l_{\mathrm{ms}1}}\end{array}\right]\\ \left[\begin{array}{c}{p}_{\mathrm{ms}2}\\ l_{\mathrm{ms}2}\end{array}\right]=K_m\left[\begin{array}{c}{x}_{\mathrm{ms}2}^{2d}\\ y_{\mathrm{ms}2}^{2d}\end{array}\right]+\left[\begin{array}{c}{p}_{0m}\\ l_{0m}\end{array}\right]=K_m\frac{f_m}{z_{\mathrm{ms}2}^c}\left[\begin{array}{c}{x}_{\mathrm{ms}2}^c\\ y_{\mathrm{ms}2}^c\end{array}\right]+\left[\begin{array}{c}{p}_{0m}\\ l_{0m}\end{array}\right]+\left[\begin{array}{c}{v}_{p_{\mathrm{ms}2}}\\ v_{l_{\mathrm{ms}2}}\end{array}\right]\\ \left[\begin{array}{c}{p}_{\mathrm{ms}3}\\ l_{\mathrm{ms}3}\end{array}\right]=K_m\left[\begin{array}{c}{x}_{\mathrm{ms}3}^{2d}\\ y_{\mathrm{ms}3}^{2d}\end{array}\right]+\left[\begin{array}{c}{p}_{0m}\\ l_{0m}\end{array}\right]=K_m\frac{f_m}{z_{\mathrm{ms}3}^c}\left[\begin{array}{c}{x}_{\mathrm{ms}3}^c\\ y_{\mathrm{ms}3}^c\end{array}\right]+\left[\begin{array}{c}{p}_{0m}\\ l_{0m}\end{array}\right]+\left[\begin{array}{c}{v}_{p_{\mathrm{ms}3}}\\ v_{l_{\mathrm{ms}3}}\end{array}\right]\end{array}\right.---\left(38\right)$

式中，(p_m，l_m)为火星在火星敏感器二维成像平面的像元和像线，(p_ms1，l_ms1)、(p_ms2，l_ms2)和(p_ms3，l_ms3)为火星敏感器中三颗背景恒星的像元和像线，(p_0m，l_0m)为火星敏感器二维成像平面中心的像元和像线；

分别为火星及其背景恒星的像元像线观测误差，K_m为火星敏感器由毫米转为像素的敏感器转换矩阵；f_m为火星敏感器的焦距，

分别为火星在火星敏感器二维像平面的坐标，

和为火星敏感器中的三颗背景恒星的二维像平面坐标，

为敏感器坐标系中的火星的坐标，可以由惯性系中的火星坐标经惯性系到敏感器坐标系的转换矩阵A_ic得出，A_ic＝A_ib·A_bc，其中由星敏感器获取的背景恒星图像信息可以恢复为背景恒星的矢量方向，利用q-method方法输出的姿态信息确定A_ib。

同理可得火卫一、火卫二及其背景恒星的像元像线表达式为

$\left\{\begin{array}{c}\left[\begin{array}{c}{p}_p\\ l_p\end{array}\right]=K_p\left[\begin{array}{c}{x}_p^{2d}\\ y_p^{2d}\end{array}\right]+\left[\begin{array}{c}{p}_{0p}\\ l_{0p}\end{array}\right]=K_p\frac{f_p}{z_p^c}\left[\begin{array}{c}{x}_p^c\\ y_p^c\end{array}\right]+\left[\begin{array}{c}{p}_{0p}\\ l_{0p}\end{array}\right]+\left[\begin{array}{c}{v}_{p_p}\\ v_{l_p}\end{array}\right]\\ \left[\begin{array}{c}{p}_{\mathrm{ps}1}\\ l_{\mathrm{ps}1}\end{array}\right]=K_p\left[\begin{array}{c}{x}_{\mathrm{ps}1}^{2d}\\ y_{\mathrm{ps}1}^{2d}\end{array}\right]+\left[\begin{array}{c}{p}_{0p}\\ l_{0p}\end{array}\right]=K_p\frac{f_p}{z_{\mathrm{ps}1}^c}\left[\begin{array}{c}{x}_{\mathrm{ps}1}^c\\ y_{\mathrm{ps}1}^c\end{array}\right]+\left[\begin{array}{c}{p}_{0p}\\ l_{0p}\end{array}\right]+\left[\begin{array}{c}{v}_{p_{\mathrm{ps}1}}\\ v_{l_{\mathrm{ps}1}}\end{array}\right]\\ \left[\begin{array}{c}{p}_{\mathrm{ps}2}\\ l_{\mathrm{ps}2}\end{array}\right]=K_p\left[\begin{array}{c}{x}_{\mathrm{ps}2}^{2d}\\ y_{\mathrm{ps}2}^{2d}\end{array}\right]+\left[\begin{array}{c}{p}_{0p}\\ l_{0p}\end{array}\right]=K_p\frac{f_p}{z_{\mathrm{ps}2}^c}\left[\begin{array}{c}{x}_{\mathrm{ps}2}^c\\ y_{\mathrm{ps}2}^c\end{array}\right]+\left[\begin{array}{c}{p}_{0p}\\ l_{0p}\end{array}\right]+\left[\begin{array}{c}{v}_{p_{\mathrm{ps}2}}\\ v_{l_{\mathrm{ps}2}}\end{array}\right]\\ \left[\begin{array}{c}{p}_{\mathrm{ps}3}\\ l_{\mathrm{ps}3}\end{array}\right]=K_p\left[\begin{array}{c}{x}_{\mathrm{ps}3}^{2d}\\ y_{\mathrm{ps}3}^{2d}\end{array}\right]+\left[\begin{array}{c}{p}_{0p}\\ l_{0p}\end{array}\right]=K_p\frac{f_p}{z_{\mathrm{ps}3}^c}\left[\begin{array}{c}{x}_{\mathrm{ps}3}^c\\ y_{\mathrm{ps}3}^c\end{array}\right]+\left[\begin{array}{c}{p}_{0p}\\ l_{0p}\end{array}\right]+\left[\begin{array}{c}{v}_{p_{\mathrm{ps}3}}\\ v_{l_{\mathrm{ps}3}}\end{array}\right]\end{array}\right.---\left(39\right)$

$\left\{\begin{array}{c}\left[\begin{array}{c}{p}_d\\ l_d\end{array}\right]=K_d\left[\begin{array}{c}{x}_d^{2d}\\ y_d^{2d}\end{array}\right]+\left[\begin{array}{c}{p}_{0d}\\ l_{0d}\end{array}\right]=K_d\frac{f_d}{z_d^c}\left[\begin{array}{c}{x}_d^c\\ y_d^c\end{array}\right]+\left[\begin{array}{c}{p}_{0d}\\ l_{0d}\end{array}\right]+\left[\begin{array}{c}{v}_{p_d}\\ v_{l_d}\end{array}\right]\\ \left[\begin{array}{c}{p}_{\mathrm{ds}1}\\ l_{\mathrm{ds}1}\end{array}\right]=K_d\left[\begin{array}{c}{x}_{\mathrm{ds}1}^{2d}\\ y_{\mathrm{ds}1}^{2d}\end{array}\right]+\left[\begin{array}{c}{p}_{0d}\\ l_{0d}\end{array}\right]=K_d\frac{f_d}{z_{\mathrm{ds}1}^c}\left[\begin{array}{c}{x}_{\mathrm{ds}1}^c\\ y_{\mathrm{ds}1}^c\end{array}\right]+\left[\begin{array}{c}{p}_{0d}\\ l_{0d}\end{array}\right]+\left[\begin{array}{c}{v}_{p_{\mathrm{ds}1}}\\ v_{l_{\mathrm{ds}1}}\end{array}\right]\\ \left[\begin{array}{c}{p}_{\mathrm{ds}2}\\ l_{\mathrm{ds}2}\end{array}\right]=K_d\left[\begin{array}{c}{x}_{\mathrm{ds}2}^{2d}\\ y_{\mathrm{ds}2}^{2d}\end{array}\right]+\left[\begin{array}{c}{p}_{0d}\\ l_{0d}\end{array}\right]=K_d\frac{f_d}{z_{\mathrm{ds}2}^c}\left[\begin{array}{c}{x}_{\mathrm{ds}2}^c\\ y_{\mathrm{ds}2}^c\end{array}\right]+\left[\begin{array}{c}{p}_{0d}\\ l_{0d}\end{array}\right]+\left[\begin{array}{c}{v}_{p_{\mathrm{ds}2}}\\ v_{l_{\mathrm{ds}2}}\end{array}\right]\\ \left[\begin{array}{c}{p}_{\mathrm{ds}3}\\ l_{\mathrm{ds}3}\end{array}\right]=K_d\left[\begin{array}{c}{x}_{\mathrm{ds}3}^{2d}\\ y_{\mathrm{ds}3}^{2d}\end{array}\right]+\left[\begin{array}{c}{p}_{0d}\\ l_{0d}\end{array}\right]=K_d\frac{f_d}{z_{\mathrm{ds}3}^c}\left[\begin{array}{c}{x}_{\mathrm{ds}3}^c\\ y_{\mathrm{ds}3}^c\end{array}\right]+\left[\begin{array}{c}{p}_{0d}\\ l_{0d}\end{array}\right]+\left[\begin{array}{c}{v}_{p_{\mathrm{ds}3}}\\ v_{l_{\mathrm{ds}3}}\end{array}\right]\end{array}\right.---\left(40\right)$

式中，(p_p，l_p)和(p_d，l_d)分别为火卫一和火卫二在各敏感器二维成像平面的像元和像线，(p_ps1，l_ps1)，(p_ps2，l_ps2)，(p_ps3，l_ps3)和(p_ds1，l_ds1)，(p_ds2，l_ds2)，(p_ds3，l_ds3)分别为火卫一敏感器和火卫二敏感器中的三颗背景恒星的像元和像线，(p_0p，l_0p)和(p_0d，l_0d)分别为火卫一和火卫二在敏感器二维成像平面中心的像元和像线；K_p和K_d分别为火卫一和火卫二敏感器由毫米转为像素的敏感器转换矩阵；f_p和f_d分别为火卫一和火卫二敏感器的焦距，

分别为火卫一及其背景恒星的像元像线观测误差，

分别为火卫二及其背景恒星的像元像线观测误差，

和

分别为火卫一和火卫二在敏感器二维像平面的坐标，

和

分别为火卫一敏感器和火卫二敏感器中的三颗背景恒星的二维像平面坐标；

分别为敏感器坐标系中的火卫一和火卫二的位置，

和

分别为火卫一敏感器坐标系中三颗背景恒星的位置，

和

分别为火卫二敏感器坐标系中三颗背景恒星的位置，这些坐标可以由惯性系中的坐标经惯性系到敏感器坐标系的转换矩阵A_ic得出，A_ic＝A_ib·A_bc，其中由星敏感器获取的背景恒星图像信息可以恢复为背景恒星的矢量方向，利用q-method方法输出的姿态信息确定A_ib。

q-method方法是根据星敏感器提供的星光矢量方向直接进行姿态确定，具体步骤为：

①获取由星敏感器输出的在探测器本体坐标系上三颗背景恒星星光矢量方向

建立在探测器本体坐标系中的星光观测矩阵

②由星敏感器星图识别，识别这三颗背景恒星的背景恒星编号，由原始星图，确定背景恒星在惯性坐标系中的星光矢量方向

建立在惯性坐标系中的星光观测矩阵

③设

S＝B+B^T，

σ＝tr(B)；

④求解出K的最大特征值，则这个特征值对应的特征向量就是满足姿态矩阵的四元数q＝[q₁ q₂ q₃ q₄]^T；

⑤求解姿态矩阵

$A_{\mathrm{ib}}=A_1^B=\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];$

3.建立火星、火卫一和火卫二与其背景恒星之间角度信息量测模型

图4描述了火星、火卫一、火卫二与背景恒星之间角度信息，图中火星矢量方向

与其第一颗背景恒星的星光矢量方向

之间所形成的夹角为θ_mi，图中火卫一矢量方向

与其第一颗背景恒星的星光矢量方向

之间所形成的夹角为θ_pi，图中火星矢量方向

与其第一颗背景恒星的星光矢量方向

之间所形成的夹角为θ_di，为了方便描述图中只表示了每个近天体(火星、火卫一、火卫二)和一颗背景恒星的角度信息，实际应用时各敏感器成像平面的三颗背景恒星都需要建模。则在火星敏感器中火星与其三颗背景恒星之间角度信息的表达式为

$\left\{\begin{array}{c}{\mathrm{\θ}}_{m1}=\arccos (-{\stackrel{\→}{l}}_{\mathrm{pm}}^c\·{\stackrel{\→}{s}}_{11})\\ {\mathrm{\θ}}_{m2}=\arccos (-{\stackrel{\→}{l}}_{\mathrm{pm}}^c\·{\stackrel{\→}{s}}_{12})\\ {\mathrm{\θ}}_{m3}=\arccos (-{\stackrel{\→}{l}}_{\mathrm{pm}}^c\·{\stackrel{\→}{s}}_{13})\end{array}\right.---\left(41\right)$

式中，

为在火星敏感器坐标系中三颗导航背景恒星星光的单位矢量，

为火星到探测器的单位矢量，可表示为

${\stackrel{\→}{l}}_{\mathrm{pm}}^c=\frac{1}{\sqrt{{\left(x_m^{2d}\right)}^2+{\left(y_m^{2d}\right)}^2+{f_m}^2}}\left[\begin{array}{c}{x}_m^{2d}\\ y_m^{2d}\\ -f_m\end{array}\right]---\left(42\right)$

可以由像元像线转换得出，具体表达式为

$\left[\begin{array}{c}{x}_m^{2d}\\ y_m^{2d}\end{array}\right]=K_m^{-1}(\left[\begin{array}{c}{p}_m\\ l_m\end{array}\right]-\left[\begin{array}{c}{p}_{0m}\\ l_{0m}\end{array}\right])---\left(43\right)$

在火卫一敏感器中火卫一与三颗背景恒星之间角度信息的表达式为

$\left\{\begin{array}{c}{\mathrm{\θ}}_{p1}=\arccos (-{\stackrel{\→}{l}}_{\mathrm{pp}}^c\·{\stackrel{\→}{s}}_{21})\\ {\mathrm{\θ}}_{p2}=\arccos (-{\stackrel{\→}{l}}_{\mathrm{pp}}^c\·{\stackrel{\→}{s}}_{22})\\ {\mathrm{\θ}}_{p3}=\arccos (-{\stackrel{\→}{l}}_{\mathrm{pp}}^c\·{\stackrel{\→}{s}}_{23})\end{array}\right.---\left(44\right)$

式中，

为在火卫一敏感器坐标系中三颗导航背景恒星星光的单位矢量，

为火卫一到探测器的单位矢量，可表示为

${\stackrel{\→}{l}}_{\mathrm{pp}}^c=\frac{1}{\sqrt{{\left(x_p^{2d}\right)}^2+{\left(y_p^{2d}\right)}^2+{f_p}^2}}\left[\begin{array}{c}{x}_p^{2d}\\ y_p^{2d}\\ -f_p\end{array}\right]---\left(45\right)$

可以由像元像线转换得出，具体表达式为

$\left[\begin{array}{c}{x}_p^{2d}\\ y_p^{2d}\end{array}\right]=K_p^{-1}(\left[\begin{array}{c}{p}_p\\ l_p\end{array}\right]-\left[\begin{array}{c}{p}_{0p}\\ l_{0p}\end{array}\right])---\left(46\right)$

在火卫二敏感器中火卫二与三颗背景恒星之间角度信息的表达式为

$\left\{\begin{array}{c}{\mathrm{\θ}}_{d1}=\arccos (-{\stackrel{\→}{l}}_{\mathrm{pd}}^c\·{\stackrel{\→}{s}}_{31})\\ {\mathrm{\θ}}_{d2}=\arccos (-{\stackrel{\→}{l}}_{\mathrm{pd}}^c\·{\stackrel{\→}{s}}_{32})\\ {\mathrm{\θ}}_{d3}=\arccos (-{\stackrel{\→}{l}}_{\mathrm{pd}}^c\·{\stackrel{\→}{s}}_{33})\end{array}\right.---\left(47\right)$

式中，

为在火卫二敏感器坐标系中三颗导航背景恒星星光的单位矢量，

为火卫二到探测器的单位矢量，可表示为

${\stackrel{\→}{l}}_{\mathrm{pd}}^c=\frac{1}{\sqrt{{\left(x_d^{2d}\right)}^2+{\left(y_d^{2d}\right)}^2+{f_d}^2}}\left[\begin{array}{c}{x}_d^{2d}\\ y_d^{2d}\\ -f_d\end{array}\right]---\left(48\right)$

可以由像元像线转换得出，具体表达式为

$\left[\begin{array}{c}{x}_d^{2d}\\ y_d^{2d}\end{array}\right]=K_d^{-1}(\left[\begin{array}{c}{p}_d\\ l_d\end{array}\right]-\left[\begin{array}{c}{p}_{0d}\\ l_{0d}\end{array}\right])---\left(49\right)$

因此火星、火卫一和火卫二及三颗背景恒星的角度信息表达式为

$\left\{\begin{array}{c}{\mathrm{\θ}}_{m1}=\arccos (-{\stackrel{\→}{l}}_{\mathrm{pm}}^c\·{\stackrel{\→}{s}}_{11})\\ {\mathrm{\θ}}_{m2}=\arccos (-{\stackrel{\→}{l}}_{\mathrm{pm}}^c\·{\stackrel{\→}{s}}_{12})\\ {\mathrm{\θ}}_{m3}=\arccos (-{\stackrel{\→}{l}}_{\mathrm{pm}}^c\·{\stackrel{\→}{s}}_{13})\\ {\mathrm{\θ}}_{p1}=\arccos (-{\stackrel{\→}{l}}_{\mathrm{pp}}^c\·{\stackrel{\→}{s}}_{11})\\ {\mathrm{\θ}}_{p2}=\arccos (-{\stackrel{\→}{l}}_{\mathrm{pp}}^c\·{\stackrel{\→}{s}}_{12})\\ {\mathrm{\θ}}_{p3}=\arccos (-{\stackrel{\→}{l}}_{\mathrm{pp}}^c\·{\stackrel{\→}{s}}_{13})\\ {\mathrm{\θ}}_{d1}=\arccos (-{\stackrel{\→}{l}}_{\mathrm{pd}}^c\·{\stackrel{\→}{s}}_{31})\\ {\mathrm{\θ}}_{d2}=\arccos (-{\stackrel{\→}{l}}_{\mathrm{pd}}^c\·{\stackrel{\→}{s}}_{32})\\ {\mathrm{\θ}}_{d3}=\arccos (-{\stackrel{\→}{l}}_{\mathrm{pd}}^c\·{\stackrel{\→}{s}}_{33})\end{array}\right.---\left(50\right)$

设系统量测量Z＝[θ_m1，θ_m2，θ_m3，θ_p1，θ_p2，θ_p3，θ_d1，θ_d2，θ_d3]^T，量测噪声

分别为测量θ_m1，θ_m2，θ_m3，θ_p1，θ_p2，θ_p3，θ_d1，θ_d2，θ_d3的观测误差，由于各变量都是与时间t有关的变量，则可建立火星、火卫一和火卫二角度信息量测模型的表达式为

Z(t)＝h[X(t)，t]+v(t)                (51)

由量测模型可以看出，方程中不含姿态矩阵，因此与图像坐标信息和矢量方向信息相比，以角度信息作为观测量的导航方法不受姿态确定精度的影响，可以为探测器位置速度的确定提供较高的导航精度。

4.对步骤1中的状态模型及步骤3中的量测模型进行离散化

X(k+1)＝F(X(k)，k)+w(k)         (52)

Z(k)＝H(X(k)，k)+v(k)           (53)

式中，k＝1，2，…，F(X(k)，k)为f(X(t)，t)离散后的非线性状态转移函数，H(X(k)，k)为h(X(t)，t)离散后的非线性量测函数，w(k)、v(k)互不相关。

5.轨道Unscented卡尔曼滤波

利用Unscented卡尔曼滤波算法，结合所述的状态模型和量测模型进行滤波，利用敏感器获取测量得到的火星、火卫一、火卫二和背景恒星的像元像线信息，并将此测量信息转换为量测量火星、火卫一、火卫二和背景恒星的角度信息，通过量测量与量测模型相减得到系统量测残差，用系统这一残差校正量测模型的模型误差；利用Unscented采样13个采样点，利用系统状态模型，对采样点进行一步预测，并得出与上一步迭代状态值之间协方差阵，以消除状态模型模型误差的影响，最终输出导航信息，具体步骤为：

①初始化状态量

和状态误差方差阵P₀

${\hat{x}}_0=E\left[x_0\right],$ $P_0=E\left[(x_0-{\hat{x}}_0){(x_0-{\hat{x}}_0)}^T\right]---\left(54\right)$

②计算状态量采样点

在附近选取一系列样本点，这些样本点的均值和协方差分别为

和

这里

且P_k-1|k-1＝P_k-1。设状态变量为6×1维，那么13个样本点χ_0，k，χ_1，k，...，χ_12，k及其权重W₀，W₁…，W₁₂分别如下

${\mathrm{\χ}}_{0,k}={\hat{x}}_k,$ W₀＝-1

${\mathrm{\χ}}_{i,k}={\hat{x}}_k+\sqrt{3}{\left(\sqrt{P\left(k\right|k)}\right)}_j,$ W_j＝1/6             (55)

${\mathrm{\χ}}_{i+6,k}={\hat{x}}_k-\sqrt{3}{\left(\sqrt{P\left(k\right|k)}\right)}_j,$ W_j+6＝1/6

式中，当P(k|k)＝A^TA时，

取A的第j行，当P(k|k)＝AA^T时，

取A的第j列，则所有采样点的表达式为

j＝1，2，....，6       (56)

③时间更新

状态量的一步预测χ_k+1|k为

χ_k|k-1＝f(χ_k-1，k-1)    (57)

所有采样点状态量的一步预测加权后结果

为

${\hat{x}}_k^{-}=\underset{j=0}{\overset{12}{\mathrm{\Σ}}}{W}_j{\mathrm{\χ}}_{j,k|k-1}---\left(58\right)$

式中，W_j为第j个采样点的权值；

状态量的估计方差一步预测

为

$P_k^{-}=\underset{j=0}{\overset{12}{\mathrm{\Σ}}}{W}_j[{\mathrm{\χ}}_{j,k|k-1}-{\hat{x}}_k^{-}]{[{\mathrm{\χ}}_{j,k|k-1}-{\hat{x}}_k^{-}]}^T+Q_k---\left(59\right)$

式中，Q_k为k时刻状态模型噪声协方差阵；

采样点对应的量测估计值Z_k|k-1

Z_k|k-1＝h(χ_k|k-1，k)            (60)

采样点量测估计加权值

${\hat{z}}_k^{-}=\underset{j=0}{\overset{12}{\mathrm{\Σ}}}{W}_j{Z}_{j,k|k-1}---\left(61\right)$

④量测更新

量测方差阵

$P_{{\hat{z}}_k{\hat{z}}_k}=\underset{j=0}{\overset{12}{\mathrm{\Σ}}}{W}_j[Z_{j,k|k-1}-{\hat{z}}_k^{-}]{[Z_{j,k|k-1}-{\hat{z}}_k^{-}]}^T+R_k---\left(62\right)$

式中，R_k为量测噪声协方差；

状态变量量测量方差阵

$P_{{\hat{x}}_k{\hat{z}}_k}=\underset{j=0}{\overset{12}{\mathrm{\Σ}}}{W}_j[{\mathrm{\χ}}_{j,k|k-1}-{\hat{x}}_k^{-}]{[Z_{j,k|k-1}-{\hat{z}}_k^{-}]}^T---\left(63\right)$

滤波增益K_k为

$K_k=P_{{\hat{x}}_k{\hat{z}}_k}{P}_{{\hat{z}}_k{\hat{z}}_k}^{-1}---\left(64\right)$

状态量的估计值

和估计方差P_k为

${\hat{x}}_k={\hat{x}}_k^{-}+K_k(Z_k-{\hat{z}}_k^{-})---\left(65\right)$

$P_k=P_k^{-}-K_k{P}_{{\hat{z}}_k{\hat{z}}_k}{K}_k^T---\left(66\right)$

式中，Q_k和R_k分别为系统和量测噪声协方差。

将获得的状态量的估计值

和估计方差P_k返回滤波器，用于k+1时刻，k＝1，2，...，最终将状态估计值

和状态估计方差P_k输出，其中状态估计值包括探测器的位置、速度导航信息，输出的状态估计方差P_k表示了滤波估计的性能。

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

Claims (1)

1.一种深空探测器火星捕获段的自主天文导航方法，其特征在于：首先根据圆形限制性四体模型建立深空探测器的状态模型，再利用火星敏感器、火卫一敏感器和火卫二敏感器分别获得火星及其背景恒星、火卫一及其背景恒星和火卫二及其背景恒星的像元像线信息，之后把像元像线转换为角度信息，建立火星、火卫一和火卫二与各自背景恒星之间角度信息的量测模型，其中使用q-method姿态估计方法为量测模型提供深空探测器的姿态，使用Unscented卡尔曼滤波确定深空探测器的位置和速度；具体包括以下步骤：

①建立基于轨道动力学的深空探测器火星捕获段导航系统状态模型；

考虑太阳中心引力、火星中心引力和地球中心引力对探测器的作用，选取日心黄道惯性坐标系，可得深空探测器的状态模型：

$\left\{\begin{array}{c}\stackrel{\·}{x}=v_x\\ \stackrel{\·}{y}=v_y\\ \stackrel{\·}{z}=v_z\\ {\stackrel{\·}{v}}_x=-{\mathrm{\μ}}_s\frac{x}{r_{\mathrm{ps}}^3}-{\mathrm{\μ}}_m[\frac{x-x_1}{r_{\mathrm{pm}}^3}+\frac{x_1}{r_{\mathrm{sm}}^3}]-{\mathrm{\μ}}_e[\frac{x-x_2}{r_{\mathrm{pe}}^3}+\frac{x_2}{r_{\mathrm{se}}^3}]+w_x\\ {\stackrel{\·}{v}}_y=-{\mathrm{\μ}}_s\frac{y}{r_{\mathrm{ps}}^3}-{\mathrm{\μ}}_m[\frac{y-y_1}{r_{\mathrm{pm}}^3}+\frac{y_1}{r_{\mathrm{sm}}^3}]-{\mathrm{\μ}}_e[\frac{y-y_2}{r_{\mathrm{pe}}^3}+\frac{y_2}{r_{\mathrm{se}}^3}]+w_y\\ {\stackrel{\·}{v}}_z=-{\mathrm{\μ}}_s\frac{z}{r_{\mathrm{ps}}^3}-{\mathrm{\μ}}_m[\frac{z-z_1}{r_{\mathrm{pm}}^3}+\frac{z_1}{r_{\mathrm{sm}}^3}]-{\mathrm{\μ}}_e[\frac{z-z_2}{r_{\mathrm{pe}}^3}+\frac{z_2}{r_{\mathrm{se}}^3}]+w_z\end{array}\right.---\left(1\right)$

式中，

探测器三轴位置的微分，v_x，v_y，v_z为探测器三轴的速度，

为探测器三轴速度的微分，μ_s、μ_m和μ_e分别为太阳、火星和地球引力常数，r_ps为日心到探测器的距离，r_pm为火星到探测器的距离，r_sm为火心到日心的距离，r_pe为地球到探测器的距离，r_se为地心到日心的距离，(x₁，y₁，z₁)，(x₂，y₂，z₂)和(x，y，z)分别为火星、地球和深空探测器在日心惯性坐标系中的位置，其中火星和地球的位置可由行星星历表获得，w_x，w_y，w_z分别为探测器三轴的状态模型误差；

式(1)中变量都是与t有关的函数，可简写为

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

状态变量为X＝[x，y，z，v_x，v_y，v_z]^T，f(X(t)，t)为系统非线性连续状态转移函数，状态噪声为w＝[w_x，w_y，w_z]^T；

②建立火星、火卫一和火卫二与其背景恒星的像元像线模型；

火星、火卫一和火卫二与其背景恒星的像元像线可以表示为

$\left\{\begin{array}{c}\left[\begin{array}{c}{p}_m\\ l_m\end{array}\right]=K_m\left[\begin{array}{c}{x}_m^{2d}\\ y_m^{2d}\end{array}\right]+\left[\begin{array}{c}{p}_{0m}\\ l_{0m}\end{array}\right]=K_m\frac{f_m}{z_m^c}\left[\begin{array}{c}{x}_m^c\\ y_m^c\end{array}\right]+\left[\begin{array}{c}{p}_{0m}\\ l_{0m}\end{array}\right]+\left[\begin{array}{c}{v}_{p_m}\\ v_{l_m}\end{array}\right]\\ \left[\begin{array}{c}{p}_{\mathrm{msi}}\\ l_{\mathrm{msi}}\end{array}\right]=K_m\left[\begin{array}{c}{x}_{\mathrm{msi}}^{2d}\\ y_{\mathrm{msi}}^{2d}\end{array}\right]+\left[\begin{array}{c}{p}_{0m}\\ l_{0m}\end{array}\right]=K_m\frac{f_m}{z_{\mathrm{msi}}^c}\left[\begin{array}{c}{x}_{\mathrm{msi}}^c\\ y_{\mathrm{msi}}^c\end{array}\right]+\left[\begin{array}{c}{p}_{0m}\\ l_{0m}\end{array}\right]+\left[\begin{array}{c}{v}_{p_{\mathrm{msi}}}\\ v_{l_{\mathrm{msi}}}\end{array}\right]\end{array}\right.---\left(3\right)$

$\left\{\begin{array}{c}\left[\begin{array}{c}{p}_p\\ l_p\end{array}\right]=K_p\left[\begin{array}{c}{x}_p^{2d}\\ y_p^{2d}\end{array}\right]+\left[\begin{array}{c}{p}_{0p}\\ l_{0p}\end{array}\right]=K_p\frac{f_p}{z_p^c}\left[\begin{array}{c}{x}_p^c\\ y_p^c\end{array}\right]+\left[\begin{array}{c}{p}_{0p}\\ l_{0p}\end{array}\right]+\left[\begin{array}{c}{v}_{p_p}\\ v_{l_p}\end{array}\right]\\ \left[\begin{array}{c}{p}_{\mathrm{psi}}\\ l_{\mathrm{psi}}\end{array}\right]=K_p\left[\begin{array}{c}{x}_{\mathrm{psi}}^{2d}\\ y_{\mathrm{psi}}^{2d}\end{array}\right]+\left[\begin{array}{c}{p}_{0p}\\ l_{0p}\end{array}\right]=K_p\frac{f_p}{z_{\mathrm{psi}}^c}\left[\begin{array}{c}{x}_{\mathrm{psi}}^c\\ y_{\mathrm{psi}}^c\end{array}\right]+\left[\begin{array}{c}{p}_{0p}\\ l_{0p}\end{array}\right]+\left[\begin{array}{c}{v}_{p_{\mathrm{psi}}}\\ v_{l_{\mathrm{psi}}}\end{array}\right]\end{array}\right.---\left(4\right)$

$\left\{\begin{array}{c}\left[\begin{array}{c}{p}_d\\ l_d\end{array}\right]=K_d\left[\begin{array}{c}{x}_d^{2d}\\ y_d^{2d}\end{array}\right]+\left[\begin{array}{c}{p}_{0d}\\ l_{0d}\end{array}\right]=K_d\frac{f_d}{z_d^c}\left[\begin{array}{c}{x}_d^c\\ y_d^c\end{array}\right]+\left[\begin{array}{c}{p}_{0d}\\ l_{0d}\end{array}\right]+\left[\begin{array}{c}{v}_{p_d}\\ v_{l_d}\end{array}\right]\\ \left[\begin{array}{c}{p}_{\mathrm{dsi}}\\ l_{\mathrm{dsi}}\end{array}\right]=K_d\left[\begin{array}{c}{x}_{\mathrm{dsi}}^{2d}\\ y_{\mathrm{dsi}}^{2d}\end{array}\right]+\left[\begin{array}{c}{p}_{0d}\\ l_{0d}\end{array}\right]=K_d\frac{f_d}{z_{\mathrm{dsi}}^c}\left[\begin{array}{c}{x}_{\mathrm{dsi}}^c\\ y_{\mathrm{dsi}}^c\end{array}\right]+\left[\begin{array}{c}{p}_{0d}\\ l_{0d}\end{array}\right]+\left[\begin{array}{c}{v}_{p_{\mathrm{dsi}}}\\ v_{l_{\mathrm{dsi}}}\end{array}\right]\end{array}\right.---\left(5\right)$

式中，(p_m，l_m)、(p_p，l_p)和(p_d，l_d)分别为火星、火卫一和火卫二成像在各敏感器二维成像平面的像元和像线，(p_msi，l_msi)、(p_psi，l_psi)和(p_dsi，l_dsi)分别为火星敏感器、火卫一敏感器和火卫二敏感器中的第i颗背景恒星的像元和像线，i＝1，2，3，(p_0m，l_0m)，(p_0p，l_0p)和(p_0d，l_0d)分别为火星敏感器、火卫一敏感器和火卫二敏感器二维成像平面中心的像元和像线；K_m、K_p和K_d分别为火星敏感器、火卫一敏感器和火卫二敏感器由毫米转为像素的敏感器转换矩阵；f_m、f_p和f_d分别为火星敏感器、火卫一敏感器和火卫二敏感器的焦距，

和

分别为火星敏感器、火卫一敏感器和火卫二敏感器的二维像平面坐标，

和

分别为火星敏感器、火卫一敏感器和火卫二敏感器中的第i颗背景恒星的二维像平面坐标，

和

分别为敏感器坐标系中的火星、火卫一和火卫二的坐标，

和分别为火星敏感器、火卫一敏感器和火卫二敏感器中的第i颗背景恒星的坐标，同时由星敏感器获取背景恒星矢量方向，并利用q-method方法输出姿态信息，转换为姿态矩阵，为像元像线模型提供坐标转换矩阵；

分别为火星、火卫一、火卫二在图像中的像元像线观测误差，

分别为火星、火卫一、火卫二图像中的第i颗背景恒星的像元像线观测误差；

③建立火星、火卫一和火卫二与其背景恒星之间角度信息的量测模型；

将步骤②中的火星、火卫一和火卫二以及所对应的背景恒星的像元像线信息转换为火星、火卫一和火卫二以及所对应的背景恒星的矢量方向，确定它们之间的夹角θ_mi、θ_pi和θ_di表达式为

$\left\{\begin{array}{c}{\mathrm{\θ}}_{\mathrm{mi}}=\arccos (-{\stackrel{\→}{l}}_{\mathrm{pm}}^c\·{\stackrel{\→}{s}}_{1i})\\ {\mathrm{\θ}}_{\mathrm{pi}}=\arccos (-{\stackrel{\→}{l}}_{\mathrm{pp}}^c\·{\stackrel{\→}{s}}_{2i})\\ {\mathrm{\θ}}_{\mathrm{di}}=\arccos (-{\stackrel{\→}{l}}_{\mathrm{pd}}^c\·{\stackrel{\→}{s}}_{3i})\end{array}\right.---\left(6\right)$

式中，

分别为在火星敏感器坐标系、火卫一敏感器坐标系和火卫二敏感器坐标系中火星、火卫一、火卫二到探测器的矢量方向，

为在火星敏感器坐标系、火卫一敏感器坐标系和火卫二敏感器坐标系中第i颗导航背景恒星星光的矢量方向；

设系统量测量Z＝[θ_m1，θ_m2，θ_m3，θ_p1，θ_p2，θ_p3，θ_d1，θ_d2，θ_d3]^T，量测噪声

分别为测量θ_m1，θ_m2，θ_m3，θ_p1，θ_p2，θ_p3，θ_d1，θ_d2，θ_d3的观测误差，由于各变量都是与时间t有关的变量，则可建立火星、火卫一和火卫二角度信息量测模型的表达式为：

Z(t)＝h[X(t)，t]+v(t)                    (7)

④对步骤①中式(2)所示的状态模型及步骤③式(7)所示的量测模型进行离散化，

X(k+1)＝F(X(k)，k)+w(k)                  (8)

Z(k)＝H(X(k)，k)+v(k)                    (9)

式中，k＝1，2，…，F(X(k)，k)为f(X(t)，t)离散后的非线性状态转移函数，H(X(k)，k)为h(X(t)，t)离散后的非线性量测函数，w(k)、v(k)互不相关；

⑤利用Unscented卡尔曼滤波算法，结合步骤①和步骤②所述的状态模型和量测模型进行滤波，利用敏感器获取测量得到的火星、火卫一、火卫二和背景恒星的像元像线信息，并将此测量信息转换为量测量火星、火卫一、火卫二和背景恒星的角度信息，通过量测量与量测模型相减得到系统量测残差，用系统这一残差校正量测模型的模型误差；利用Unscented采样13个采样点，利用系统状态模型，对采样点进行一步预测，并得出与上一步迭代状态值之间协方差阵，以消除状态模型模型误差的影响；

⑥输出导航信息

将步骤⑤中获得的状态量的估计值和状态估计方差返回滤波器，用于k+1时刻，并输出状态估计值和状态估计方差。

Images