CN102175241A - 一种火星探测器巡航段自主天文导航方法 - Google Patents

Abstract

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

Description

一种火星探测器巡航段自主天文导航方法

技术领域

本发明涉及在火星探测器处于巡航段时，基于太阳、地球及火星角度信息的自主天文导航方法，是一种非常适用于火星探测器巡航段的自主导航方法。

背景技术

火星是距离地球最近的行星，对火星进行探测有利于人类进一步认识地球、火星和太阳系的形成和演化过程。随着我国深空探测技术的不断发展，我国的深空探测能力不断提升，我国已经具备探测月球以远天体的深空探测能力。对于火星探测任务，传统的基于地面无线电测控的导航方式存在如下缺点：由于探测器距离地球遥远，导致通讯延迟大；地面测控设施复杂庞大，运行费用高；不适于同时对多个探测器进行导航，因此，实现探测器的自主导航是火星探测的必然趋势。

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

目前可以用于火星探测器巡航段的自主导航方式主要有：基于小行星的自主天文导航方法、基于脉冲星的自主天文导航方法和基于太阳和大行星的自主天文导航方法。基于小行星的自主天文导航方法是利用地火轨道之间的小行星位置信息来确定探测器的位置，但由于火星探测巡航阶段可以观测的小行星有限，且对探测器的轨道设计要求较高，因此这种方法无法保证全转移轨道的高精度导航。基于脉冲星的自主天文导航方法是利用脉冲接收机接收宇宙空间中X射线脉冲星的脉冲到达时间，但由于目前脉冲到达时间测量精度较低，且可用于导航的X射线脉冲星数目较少，因此这种方法尚未实际应用。基于太阳和大行星的自主天文导航方法是借助太阳和大行星的位置信息来确定探测器的位置，火星探测器在巡航段运行时，太阳、地球和火星是整个转移过程中最易于观测到的天体，可以提供精确的图像信息，已有的巡航段自主天文导航方法是以太阳的视线矢量和径向速度为量测量，由于现有方法是利用了太阳视线方向，所以系统可观测度不高，无法在短时间内得到可靠、准确的导航信息，而且由于使用的太阳敏感器在成像过程受探测器姿态约束，而导航滤波中需要探测器估计姿态确定量测预测值，姿态估计信息与真实姿态之间存在误差，因此探测器的姿态估计误差会造成较大的量测预测误差，最终影响探测器的定位精度。

发明内容

本发明要解决的技术问题是：克服基于小行星和脉冲星自主导航方法中导航小行星或导航脉冲星数量少、精度低、对探测器轨道设计要求高等缺点，弥补现有方法观测太阳视线方向和径向速度受探测器姿态约束这一不足，为火星探测器巡航段提供一种高精度的自主天文导航方法。

本发明解决其技术问题所采用的技术方案为：建立高精度的火星探测器的状态模型，通过太阳敏感器、地球敏感器、火星敏感器和星敏感器获得太阳、地球、火星及恒星的像元像线信息，之后把像元像线信息转换为方向矢量，建立太阳、地球和火星与恒星之间角度信息的量测模型，并使用Unscented递推最小方差估计方法获得火星探测器的位置和速度参数，使用q-method方法获得火星探测器的姿态参数，最终为处于巡航段的火星探测器提供位置、速度和姿态导航信息。

具体包括以下步骤：

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}]-{\text{\μ}}_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为状态模型噪声。

2.建立太阳、地球、火星和恒星的像元像线模型；

太阳、地球、火星和第i颗恒星的像元像线模型可以表示为：

$\left[\begin{array}{c}{p}_s\\ l_s\end{array}\right]=K_s\left[\begin{array}{c}{x}_s^{2d}\\ y_s^{2d}\end{array}\right]+\left[\begin{array}{c}{p}_{0s}\\ l_{0s}\end{array}\right]=K_s\frac{f_s}{z_s^c}\left[\begin{array}{c}{x}_s^c\\ y_s^c\end{array}\right]+\left[\begin{array}{c}{p}_{0s}\\ l_{0s}\end{array}\right]+\left[\begin{array}{c}{v}_{p_s}\\ v_{l_s}\end{array}\right]---\left(3\right)$

$\left[\begin{array}{c}{p}_e\\ l_e\end{array}\right]=K_e\left[\begin{array}{c}{x}_e^{2d}\\ y_e^{2d}\end{array}\right]+\left[\begin{array}{c}{p}_{0e}\\ l_{0e}\end{array}\right]=K_e\frac{f_e}{z_e^c}\left[\begin{array}{c}{x}_e^c\\ y_e^c\end{array}\right]+\left[\begin{array}{c}{p}_{0e}\\ l_{0e}\end{array}\right]+\left[\begin{array}{c}{v}_{p_e}\\ v_{l_e}\end{array}\right]---\left(4\right)$

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

$\left[\begin{array}{c}{p}_{\mathrm{si}}\\ l_{\mathrm{si}}\end{array}\right]=K_{\mathrm{star}}\left[\begin{array}{c}{x}_{\mathrm{si}}^{2d}\\ y_{\mathrm{si}}^{2d}\end{array}\right]+\left[\begin{array}{c}{p}_{0s}\\ l_{0s}\end{array}\right]=K_{\mathrm{star}}\frac{f_{\mathrm{star}}}{z_{\mathrm{si}}^c}\left[\begin{array}{c}{x}_{\mathrm{si}}^c\\ y_{\mathrm{si}}^c\end{array}\right]+\left[\begin{array}{c}{p}_{0\mathrm{star}}\\ l_{0\mathrm{star}}\end{array}\right]+\left[\begin{array}{c}{v}_{p_{\mathrm{si}}}\\ v_{l_{\mathrm{si}}}\end{array}\right]---\left(6\right)$

式中，(p_s，l_s)、(p_e，l_e)、(p_m，l_m)和(p_si，l_si)分别为太阳、地球、火星和恒星在太阳敏感器、地球敏感器、火星敏感器和星敏感器二维成像平面的像元和像线，i＝1，2，3，(p_0s，l_0s)、(p_0e，l_0e)、(p_0m，l_0m)和(p_0star，l_0star)分别为太阳敏感器、地球敏感器、火星敏感器和星敏感器二维成像平面中心的像元和像线；K_s、K_e、K_m和K_star分别为太阳敏感器、地球敏感器、火星敏感器和星敏感器由毫米转为像素的转换矩阵；f_s、f_e、f_m和f_star分别为太阳敏感器、地球敏感器、火星敏感器和星敏感器的焦距；

和

分别为太阳、地球、火星和第i颗恒星在太阳敏感器、地球敏感器、火星敏感器和星敏感器二维成像平面的坐标，

和

分别为太阳敏感器、地球敏感器、火星敏感器和星敏感器坐标系中的太阳、地球、火星和第i颗恒星的坐标，可以由惯性系中的坐标经惯性系到敏感器坐标系的转换矩阵得出，同时由星敏感器获取恒星矢量方向，并利用q-method方法输出姿态信息，转换为姿态矩阵，为像元像线模型提供坐标转换矩阵，

分别为太阳、地球、火星和恒星的像元像线模型误差。

3.建立太阳、地球、火星与恒星之间角度信息的量测模型；

从像元像线模型经过矢量转换可得太阳、地球、火星与三颗恒星之间的9个角度，则太阳、地球、火星与第i颗恒星之间角度θ_si、θ_ei和θ_mi的表达式为

$\left\{\begin{array}{c}{\mathrm{\θ}}_{\mathrm{si}}=\arccos (-{\stackrel{\→}{l}}_{\mathrm{ps}}\·{\stackrel{\→}{s}}_i)\\ {\mathrm{\θ}}_{\mathrm{ei}}=\arccos (-{\stackrel{\→}{l}}_{\mathrm{pe}}\·{\stackrel{\→}{s}}_i)\\ {\mathrm{\θ}}_{\mathrm{mi}}=\arccos (-{\stackrel{\→}{l}}_{\mathrm{pm}}\·{\stackrel{\→}{s}}_i)\end{array}\right.---\left(7\right)$

式中，

为太阳、地球和火星的方向，为在各敏感器坐标系中由星敏感器识别的第i颗恒星星光的矢量方向。

设Z为系统观测量，X为系统状态量，则由式(7)可建立太阳、地球和火星之间角度信息量测方程的表达式为

$Z=\left[\begin{array}{c}{\mathrm{\θ}}_{\mathrm{si}}\\ {\mathrm{\θ}}_{\mathrm{ei}}\\ {\mathrm{\θ}}_{\mathrm{mi}}\end{array}\right]+\left[\begin{array}{c}{v}_{{\mathrm{\θ}}_s}\\ v_{{\mathrm{\θ}}_e}\\ v_{{\mathrm{\θ}}_m}\end{array}\right]=\left[\begin{array}{c}\arccos (-l_{\mathrm{pe}}\·s_i)\\ \arccos (-l_{\mathrm{pe}}\·s_i)\\ \arccos (-l_{\mathrm{pm}}\·s_i)\end{array}\right]+\left[\begin{array}{c}{v}_{{\mathrm{\θ}}_{\mathrm{si}}}\\ v_{{\mathrm{\θ}}_{\mathrm{ei}}}\\ v_{{\mathrm{\θ}}_{\mathrm{mi}}}\end{array}\right]=h\left(X\right)+v---\left(8\right)$

式中，h(·)为观测量Z的量测函数，

为θ_si，θ_ei，θ_mi的观测误差。式中各变量都是与时间有关的函数，则式(8)可以简写为

Z(t)＝h(X(t)，t)+v(t)                (9)

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

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

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

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

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

5.成像敏感器获取量测量；

①太阳敏感器、地球敏感器、火星敏感器、星敏感器等成像敏感器获取日地火和恒星的图像信息；

②对步骤①中图像信息进行质心提取后，获得太阳在太阳敏感器中图像的像元像线(p_ms，l_ms)、地球在地球敏感器中图像的像元像线(p_me，l_me)、火星在火星敏感器中图像的像元像线(p_mm，l_mm)和第i颗恒星在星敏感器中图像的像元像线(p_mi，l_mi)；

③利用太阳、地球、火星和恒星的二维像元像线转换为三维矢量方向；

④将太阳、地球和火星和恒星的矢量方向信息转换为太阳、地球和火星和恒星之间的角度信息。

6.q-method定姿；

由星敏感器获得的恒星矢量方向信息可以借助q-method方法进行姿态确定，具体步骤为：

①获取由星敏感器输出的三颗恒星星光矢量方向

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

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

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

③设

S＝B+B^T，

σ＝tr(B)；

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

⑤求解姿态矩阵

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

⑥输出步骤④所得得姿态信息，并反馈姿态矩阵A_ib至步骤2。

7.巡航段轨道Unscented递推最小方差估计；

由于状态模型和量测模型存在非线性误差，而且这些误差是除了测量仪器精度的之外，限制火星探测器导航精度的主要因素，因此在轨道确定滤波中使用Unscented递推最小方差估计方法，通过量测量与量测方程相减得到系统量测残差，用系统这一残差校正量测方程的模型误差；利用Unscented采样13个采样点，利用系统状态方程，对采样点进行一步预测，并得出与上一步迭代状态值之间协方差阵，以消除状态方程模型误差的影响，最终输出导航信息。

本发明的原理是：以太阳、地球和火星与恒星之间的角度信息为观测量，火星探测器巡航段的天文导航方法，并用Unscented递推最小方差估计对位置、速度等导航参数进行估计。首先选择计算简单而且精度损失较小火星探测器四体轨道动力学模型作为系统状态模型；然后根据太阳敏感器、地球敏感器、火星敏感器和星敏感器等成像敏感器的成像原理，建立太阳、地球、火星和恒星像元像线模型，并通过像元像线坐标系到敏感器测量坐标系的坐标转换，建立太阳、地球、火星和恒星角度信息的量测模型；然后利用太阳敏感器、地球敏感器、火星敏感器和星敏感器等成像敏感器直接获取太阳、地球、火星和恒星的图像信息，由图像信息对步骤A中图像信息进行质心提取后获得像元像线信息，之后间接地转换为太阳、地球和火星与恒星之间角度信息这一量测量。由于状态模型和量测模型存在非线性误差，而且这些误差是除了测量仪器精度的之外，限制火星探测器导航精度的主要因素，因此在轨道确定滤波中使用Unscented递推最小方差估计方法，通过量测量与量测方程相减得到系统量测残差，用系统这一残差校正量测方程的模型误差，利用Unscented采样13个采样点，利用系统状态方程，对采样点进行一步预测，并得出与上一步迭代状态值之间协方差阵，以消除状态方程模型误差的影响，实现对位置、速度等导航参数进行估计，此外，利用星敏感器输出的恒星矢量信息利用q-method定姿方法实现对探测器姿态的估计。最后通过本发明所采用的方法，输出探测器的位置、速度和姿态导航信息。

本发明与现有技术相比的优点在于：(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(0)＝[-2.024×10¹¹m-5.336×10¹⁰m-2.276×10¹⁰m-1.384×10³m/s-2.194×10⁴m/s-9.520×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}]-{\text{\μ}}_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为状态模型噪声。

2.建立太阳、地球和火星的像元像线模型；

图2以火星为例描述了火星敏感器的成像过程，其他敏感器成像过程与之类似。火星敏感器主要由光学透镜和二维成像面阵组成，在火星敏感器测量坐标系OX_cY_cZ_c中火星反射太阳光线沿火星到探测器的方向矢量

射向火星敏感器，此时，火星在火星敏感器测量坐标系中的坐标为(x_c，y_c，z_c)；火星敏感器的光学透镜以焦距f将火星的光线折射后成像在二维成像面阵上，二维成像面阵将照在每个成像单元上的图像亮度信号储存；由于火星在二维成像面阵上的图像并不是一个点，而是一个圆，通过质心识别等图像处理技术确定火星图像在二维成像平面坐标系OX_2dY_2d的质心(x_2d，y_2d)，这一中心可以用像元像线坐标系O_p1X_p1Y_p1中的像元像线(p，l)。

因此，在图2中火星敏感器像元像线坐标系O_p1X_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(3\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(4\right)$

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

$\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(5\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(6\right)$

因此由式(3)～式(6)可以得出太阳、地球、火星和恒星的像元像线可以表示为

$\left[\begin{array}{c}{p}_s\\ l_s\end{array}\right]=K_s\left[\begin{array}{c}{x}_s^{2d}\\ y_s^{2d}\end{array}\right]+\left[\begin{array}{c}{p}_{0s}\\ l_{0s}\end{array}\right]=K_s\frac{f_s}{z_s^c}\left[\begin{array}{c}{x}_s^c\\ y_s^c\end{array}\right]+\left[\begin{array}{c}{p}_{0s}\\ l_{0s}\end{array}\right]+\left[\begin{array}{c}{v}_{p_s}\\ v_{l_s}\end{array}\right]---\left(7\right)$

$\left[\begin{array}{c}{p}_e\\ l_e\end{array}\right]=K_e\left[\begin{array}{c}{x}_e^{2d}\\ y_e^{2d}\end{array}\right]+\left[\begin{array}{c}{p}_{0e}\\ l_{0e}\end{array}\right]=K_e\frac{f_e}{z_e^c}\left[\begin{array}{c}{x}_e^c\\ y_e^c\end{array}\right]+\left[\begin{array}{c}{p}_{0e}\\ l_{0e}\end{array}\right]+\left[\begin{array}{c}{v}_{p_e}\\ v_{l_e}\end{array}\right]---\left(8\right)$

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

$\left\{\begin{array}{c}\left[\begin{array}{c}{p}_{s1}\\ l_{s1}\end{array}\right]=K_{\mathrm{star}}\left[\begin{array}{c}{x}_{s1}^{2d}\\ y_{s1}^{2d}\end{array}\right]+\left[\begin{array}{c}{p}_{0\mathrm{star}}\\ l_{\mathrm{ostar}}\end{array}\right]=K_{\mathrm{star}}\frac{f_{\mathrm{star}}}{z_{s1}^c}\left[\begin{array}{c}{x}_{s1}^c\\ y_{s1}^c\end{array}\right]+\left[\begin{array}{c}{p}_{0\mathrm{star}}\\ l_{0\mathrm{star}}\end{array}\right]+\left[\begin{array}{c}{v}_{p_{s1}}\\ v_{l_{s1}}\end{array}\right]\\ \left[\begin{array}{c}{p}_{s2}\\ l_{s2}\end{array}\right]=K_{\mathrm{star}}\left[\begin{array}{c}{x}_{s2}^{2d}\\ y_{s3}^{2d}\end{array}\right]+\left[\begin{array}{c}{p}_{0\mathrm{star}}\\ l_{0\mathrm{star}}\end{array}\right]=K_{\mathrm{star}}\frac{f_{\mathrm{star}}}{z_{s2}^c}\left[\begin{array}{c}{x}_{s2}^c\\ y_{s2}^c\end{array}\right]+\left[\begin{array}{c}{p}_{0\mathrm{star}}\\ l_{0\mathrm{star}}\end{array}\right]+\left[\begin{array}{c}{v}_{p_{s2}}\\ v_{l_{s2}}\end{array}\right]\\ \left[\begin{array}{c}{p}_{s3}\\ l_{s3}\end{array}\right]=K_{\mathrm{star}}\left[\begin{array}{c}{x}_{s2}^{2d}\\ y_{s3}^{2d}\end{array}\right]+\left[\begin{array}{c}{p}_{0\mathrm{star}}\\ l_{0\mathrm{star}}\end{array}\right]=K_{\mathrm{star}}\frac{f_{\mathrm{star}}}{z_{s3}^c}\left[\begin{array}{c}{x}_{s3}^c\\ y_{s3}^c\end{array}\right]+\left[\begin{array}{c}{p}_{0\mathrm{star}}\\ l_{0\mathrm{star}}\end{array}\right]+\left[\begin{array}{c}{v}_{p_{s3}}\\ v_{l_{s3}}\end{array}\right]\end{array}\right.---\left(10\right)$

式中，(p_s，l_s)、(p_e，l_e)、(p_m，l_m)和(p_si，l_si)分别为太阳、地球、火星和恒星在太阳敏感器、地球敏感器、火星敏感器和星敏感器二维成像平面的像元和像线，i＝1，2，3，(p_0s，l_0s)、(p_0e，l_0e)、(p_0m，l_0m)和(p_0star，l_0star)分别为太阳敏感器、地球敏感器、火星敏感器和星敏感器二维成像平面中心的像元和像线；K_s、K_e、K_m和K_star分别为太阳敏感器、地球敏感器、火星敏感器和星敏感器由毫米转为像素的转换矩阵；f_s、f_e、f_m和f_star分别为太阳敏感器、地球敏感器、火星敏感器和星敏感器的焦距；

和分别为太阳、地球和火星在太阳敏感器、地球敏感器、火星敏感器二维成像平面的坐标，

和

为三颗恒星在星敏感器二维成像平面的坐标，

和

分别为太阳敏感器、地球敏感器和火星敏感器的太阳、地球和火星坐标，

和

为星敏感器坐标系中三颗恒星的坐标，这些坐标可以由惯性系中的坐标经惯性系到敏感器坐标系的转换矩阵得出，

分别为太阳、地球、火星和恒星的像元像线模型误差。

3.建立太阳、地球、火星与恒星之间角度信息的量测模型；

图4描述了太阳、地球、火星与恒星之间角度信息，图中太阳矢量方向与第一颗恒星的星光矢量方向

之间所形成的夹角为θ_s1，图中地球矢量方向

与第二颗恒星的星光矢量方向

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

与第三颗恒星的星光矢量方向

之间所形成的夹角为θ_s3，图中只表示了每个近天体(太阳、地球、火星)和一颗恒星的角度信息，实际应用时每个近天体(太阳、地球、火星)和图中所示三颗恒星之间角度信息都需要建模。因此太阳、地球、火星以及三颗恒星之间的9个角度表达式为

$\left\{\begin{array}{c}{\mathrm{\θ}}_{s1}=\arccos (-{\stackrel{\→}{l}}_{\mathrm{ps}}\·{\stackrel{\→}{s}}_1)\\ {\mathrm{\θ}}_{s2}=\arccos (-{\stackrel{\→}{l}}_{\mathrm{ps}}\·{\stackrel{\→}{s}}_2)\\ {\mathrm{\θ}}_{s3}=\arccos (-{\stackrel{\→}{l}}_{\mathrm{ps}}\·{\stackrel{\→}{s}}_3)\\ {\mathrm{\θ}}_{e1}=\arccos (-{\stackrel{\→}{l}}_{\mathrm{pe}}\·{\stackrel{\→}{s}}_1)\\ {\mathrm{\θ}}_{e2}=\arccos (-{\stackrel{\→}{l}}_{\mathrm{pe}}\·{\stackrel{\→}{s}}_2)\\ {\mathrm{\θ}}_{e3}=\arccos (-{\stackrel{\→}{l}}_{\mathrm{pe}}\·{\stackrel{\→}{s}}_3)\\ {\mathrm{\θ}}_{m1}=\arccos (-{\stackrel{\→}{l}}_{\mathrm{pm}}\·{\stackrel{\→}{s}}_1)\\ {\mathrm{\θ}}_{m2}=\arccos (-{\stackrel{\→}{l}}_{\mathrm{pm}}\·{\stackrel{\→}{s}}_2)\\ {\mathrm{\θ}}_{m3}=\arccos (-{\stackrel{\→}{l}}_{\mathrm{pm}}\·{\stackrel{\→}{s}}_3)\end{array}\right.---\left(11\right)$

式中，为太阳、地球和火星的方向，

为由星敏感器识别的第一颗恒星、第二颗恒星和第三颗恒星星光的矢量方向。

设Z＝[θ_s1，θ_s2，θ_s3，θ_e1，θ_e2，θ_e3，θ_m1，θ_m2，θ_m3]^T为系统观测量，X为系统状态量，式中各变量都是与时间有关的函数，则由式(11)可建立太阳、地球和火星之间角度信息量测方程的表达式为

$Z=\left[\begin{array}{c}{\mathrm{\θ}}_{s1}\\ {\mathrm{\θ}}_{s2}\\ {\mathrm{\θ}}_{s3}\\ {\mathrm{\θ}}_{e1}\\ {\mathrm{\θ}}_{e2}\\ {\mathrm{\θ}}_{e3}\\ {\mathrm{\θ}}_{m1}\\ {\mathrm{\θ}}_{m2}\\ {\mathrm{\θ}}_{m3}\end{array}\right]+\left[\begin{array}{c}{v}_{{\mathrm{\θ}}_{s1}}\\ v_{{\mathrm{\θ}}_{s2}}\\ v_{{\mathrm{\θ}}_{s3}}\\ v_{{\mathrm{\θ}}_{e1}}\\ v_{{\mathrm{\θ}}_{e2}}\\ v_{{\mathrm{\θ}}_{e3}}\\ v_{{\mathrm{\θ}}_{m1}}\\ v_{{\mathrm{\θ}}_{m2}}\\ v_{{\mathrm{\θ}}_{m3}}\end{array}\right]=\left[\begin{array}{c}\arccos (-{\stackrel{\→}{l}}_{\mathrm{ps}}\·{\stackrel{\→}{s}}_1)\\ \arccos (-{\stackrel{\→}{l}}_{\mathrm{ps}}\·{\stackrel{\→}{s}}_2)\\ \arccos (-{\stackrel{\→}{l}}_{\mathrm{ps}}\·{\stackrel{\→}{s}}_3)\\ \arccos (-{\stackrel{\→}{l}}_{\mathrm{pe}}\·{\stackrel{\→}{s}}_1)\\ \arccos (-{\stackrel{\→}{l}}_{\mathrm{pe}}\·{\stackrel{\→}{s}}_2)\\ \arccos (-{\stackrel{\→}{l}}_{\mathrm{pe}}\·{\stackrel{\→}{s}}_3)\\ \arccos (-{\stackrel{\→}{l}}_{\mathrm{pm}}\·{\stackrel{\→}{s}}_1)\\ \arccos (-{\stackrel{\→}{l}}_{\mathrm{pm}}\·{\stackrel{\→}{s}}_2)\\ \arccos (-{\stackrel{\→}{l}}_{\mathrm{pm}}\·{\stackrel{\→}{s}}_3)\end{array}\right]+\left[\begin{array}{c}{v}_{{\mathrm{\θ}}_{s1}}\\ v_{{\mathrm{\θ}}_{s2}}\\ v_{{\mathrm{\θ}}_{s3}}\\ v_{{\mathrm{\θ}}_{e1}}\\ v_{{\mathrm{\θ}}_{e2}}\\ v_{{\mathrm{\θ}}_{e3}}\\ v_{{\mathrm{\θ}}_{m1}}\\ v_{{\mathrm{\θ}}_{m2}}\\ v_{{\mathrm{\θ}}_{m3}}\end{array}\right]=h\left(X\right)+v---\left(12\right)$

式中，h(·)为观测量Z的量测函数，

为θ_si，θ_si，θ_si，θ_e1，θ_e2，θ_e3，θ_m1，θ_m2，θ_m3的观测误差。

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

Z(t)＝h(X(t)，t)+v(t)                   (13)

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

4.对步骤1和步骤3中的状态方程和量测方程进行离散化；

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

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

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

5.成像敏感器获取量测量；

①太阳敏感器、地球敏感器、火星敏感器、星敏感器等成像敏感器获取日地火和恒星的图像信息；

②对步骤①中图像信息进行质心提取后，获得太阳的像元像线(p_ms，l_ms)、地球的像元像线(p_me，l_me)和火星的像元像线(p_mm，l_mm)及三颗恒星的像元像线(p_m1，l_m1)，(p_m2，l_m2)，(p_m3，l_m3)；

③利用太阳、地球、火星和恒星的二维像元像线转换为三维矢量方向；

A.首先将太阳、地球、火星和恒星的像元像线信息转换为敏感器二维成像平面坐标系中的坐标

$\left[\begin{array}{c}{x}_{\mathrm{ms}}^{2d}\\ y_{\mathrm{ms}}^{2d}\end{array}\right]=K_s^{-1}(\left[\begin{array}{c}{p}_{\mathrm{ms}}\\ l_{\mathrm{ms}}\end{array}\right]-\left[\begin{array}{c}{p}_{0s}\\ l_{0s}\end{array}\right])---\left(16\right)$

$\left[\begin{array}{c}{x}_{\mathrm{me}}^{2d}\\ y_{\mathrm{me}}^{2d}\end{array}\right]=K_e^{-1}(\left[\begin{array}{c}{p}_{\mathrm{me}}\\ l_{\mathrm{me}}\end{array}\right]-\left[\begin{array}{c}{p}_{0e}\\ l_{0e}\end{array}\right])---\left(17\right)$

$\left[\begin{array}{c}{x}_{\mathrm{mm}}^{2d}\\ y_{\mathrm{mm}}^{2d}\end{array}\right]=K_m^{-1}(\left[\begin{array}{c}{p}_{\mathrm{mm}}\\ l_{\mathrm{mm}}\end{array}\right]-\left[\begin{array}{c}{p}_{0m}\\ l_{0m}\end{array}\right])---\left(18\right)$

$\left\{\begin{array}{c}\left[\begin{array}{c}{x}_{\mathrm{ms}1}^{2d}\\ y_{\mathrm{ms}2}^{2d}\end{array}\right]=K_{\mathrm{star}}^{-1}(\left[\begin{array}{c}{p}_{m1}\\ l_{m1}\end{array}\right]-\left[\begin{array}{c}{p}_{0\mathrm{star}}\\ l_{0\mathrm{star}}\end{array}\right])\\ \left[\begin{array}{c}{x}_{\mathrm{ms}2}^{2d}\\ y_{\mathrm{ms}2}^{2d}\end{array}\right]=K_{\mathrm{star}}^{-1}(\left[\begin{array}{c}{p}_{m2}\\ l_{m2}\end{array}\right]-\left[\begin{array}{c}{p}_{0\mathrm{star}}\\ l_{0\mathrm{star}}\end{array}\right])\\ \left[\begin{array}{c}{x}_{\mathrm{ms}3}^{2d}\\ y_{\mathrm{ms}3}^{2d}\end{array}\right]=K_{\mathrm{star}}^{-1}(\left[\begin{array}{c}{p}_{m3}\\ l_{m3}\end{array}\right]-\left[\begin{array}{c}{p}_{0\mathrm{star}}\\ l_{0\mathrm{star}}\end{array}\right])\end{array}\right.---\left(19\right)$

式中，

和

为太阳敏感器、地球敏感器和火星敏感器测量得到的太阳、地球和火星在对应敏感器二维成像平面坐标系中的坐标，

为星敏感器测量得到的三颗恒星在星敏感器二维成像平面坐标系中的坐标，K_s、K_e、K_m和K_star分别为太阳敏感器、地球敏感器、火星敏感器和星敏感器由毫米转为像素的转换矩阵，(p_0s，l_0s)、(p_0e，l_0e)、(p_0m，l_0m)和(p_0star，l_0star)分别为太阳敏感器、地球敏感器、火星敏感器和星敏感器二维成像平面中心的像元和像线。

B.将太阳、地球和火星及恒星在太阳敏感器、地球敏感器、火星敏感器和星敏感器二维成像平面坐标系中的坐标转换为三维矢量信息坐标

${\stackrel{\→}{l}}_{\mathrm{ms}}^c=\frac{1}{\sqrt{{\left(x_{\mathrm{ms}}^{2d}\right)}^2+{\left(y_{\mathrm{ms}}^{2d}\right)}^2+{f_s}^2}}\left[\begin{array}{c}{x}_{\mathrm{ms}}^{2d}\\ y_{\mathrm{ms}}^{2d}\\ -f_s\end{array}\right]---\left(20\right)$

${\stackrel{\→}{l}}_{\mathrm{me}}^c=\frac{1}{\sqrt{{\left(x_{\mathrm{me}}^{2d}\right)}^2+{\left(y_{\mathrm{me}}^{2d}\right)}^2+{f_e}^2}}\left[\begin{array}{c}{x}_{\mathrm{me}}^{2d}\\ y_{\mathrm{me}}^{2d}\\ -f_e\end{array}\right]---\left(21\right)$

${\stackrel{\→}{l}}_{\mathrm{mm}}^c=\frac{1}{\sqrt{{\left(x_{\mathrm{mm}}^{2d}\right)}^2+{\left(y_{\mathrm{mm}}^{2d}\right)}^2+{f_m}^2}}\left[\begin{array}{c}{x}_{\mathrm{mm}}^{2d}\\ y_{\mathrm{mm}}^{2d}\\ -f_m\end{array}\right]---\left(22\right)$

$\left\{\begin{array}{c}{\stackrel{\→}{l}}_{\mathrm{ms}1}^c=\frac{1}{\sqrt{{\left(x_{\mathrm{ms}1}^{2d}\right)}^2+{\left(y_{\mathrm{ms}1}^{2d}\right)}^2+{f_{\mathrm{star}}}^2}}{\left[\begin{array}{ccc}{x}_{\mathrm{ms}1}^{2d}& y_{\mathrm{ms}1}^{2d}& -f_{\mathrm{star}}\end{array}\right]}^T\\ {\stackrel{\→}{l}}_{\mathrm{ms}2}^c=\frac{1}{\sqrt{{\left(x_{\mathrm{ms}2}^{2d}\right)}^2+{\left(y_{\mathrm{ms}2}^{2d}\right)}^2{f_{\mathrm{star}}}^2}}{\left[\begin{array}{ccc}{x}_{\mathrm{ms}2}^{2d}& y_{\mathrm{ms}2}^{2d}& {-f}_{\mathrm{star}}\end{array}\right]}^T\\ {\stackrel{\→}{l}}_{\mathrm{ms}3}^c=\frac{1}{\sqrt{{\left(x_{\mathrm{ms}3}^{2d}\right)}^2+{\left(y_{\mathrm{ms}3}^{2d}\right)}^2+{f_{\mathrm{star}}}^2}}{\left[\begin{array}{ccc}{x}_{\mathrm{ms}3}^{2d}& y_{\mathrm{ms}3}^{2d}& {-f}_{\mathrm{star}}\end{array}\right]}^T\end{array}\right.---\left(23\right)$

式中，

和

分别为量测得到的太阳、地球和火星相对探测器的单位矢量，

分别为量测得到的三颗恒星的单位矢量。

④将太阳、地球和火星和恒星的矢量方向信息转换为太阳、地球和火星和恒星之间的角度信息。

量测得到的太阳和三颗恒星角度信息为

$\left\{\begin{array}{c}{\mathrm{\θ}}_{\mathrm{ms}1}=\arccos (-{\stackrel{\→}{l}}_{\mathrm{ms}}\·{\stackrel{\→}{s}}_{m1})\\ {\mathrm{\θ}}_{\mathrm{ms}1}=\arccos (-{\stackrel{\→}{l}}_{\mathrm{ms}}\·{\stackrel{\→}{s}}_{m1})\\ {\mathrm{\θ}}_{\mathrm{ms}1}=\arccos (-{\stackrel{\→}{l}}_{\mathrm{ms}}\·{\stackrel{\→}{s}}_{m1})\end{array}\right.---\left(24\right)$

量测得到的地球和三颗恒星角度信息为

$\left\{\begin{array}{c}{\mathrm{\θ}}_{\mathrm{me}1}=\arccos (-{\stackrel{\→}{l}}_{\mathrm{me}}\·{\stackrel{\→}{s}}_{m1})\\ {\mathrm{\θ}}_{\mathrm{me}2}=\arccos (-{\stackrel{\→}{l}}_{\mathrm{me}}\·{\stackrel{\→}{s}}_{m2})\\ {\mathrm{\θ}}_{\mathrm{me}3}=\arccos (-{\stackrel{\→}{l}}_{\mathrm{me}}\·{\stackrel{\→}{s}}_{m3})\end{array}\right.---\left(25\right)$

量测得到的火星和第i颗恒星角度信息为

$\left\{\begin{array}{c}{\mathrm{\θ}}_{\mathrm{mm}1}=\arccos (-{\stackrel{\→}{l}}_{\mathrm{mm}}\·{\stackrel{\→}{s}}_{m1})\\ {\mathrm{\θ}}_{\mathrm{mm}2}=\arccos (-{\stackrel{\→}{l}}_{\mathrm{mm}}\·{\stackrel{\→}{s}}_{m2})\\ {\mathrm{\θ}}_{\mathrm{mm}3}=\arccos (-{\stackrel{\→}{l}}_{\mathrm{mm}}\·{\stackrel{\→}{s}}_{m3})\end{array}\right.---\left(26\right)$

太阳、地球和火星的角度量测量为：

Z_k＝[θ_ms1(k)，θ_ms2(k)，θ_ms3(k)，θ_me1(k)，θ_me2(k)，θ_me3(k)，θ_mm1(k)，θ_mm2(k)，θ_mm3(k)]^T    (27)

式中，θ_ms1(k)，θ_ms2(k)，θ_ms3(k)，θ_me1(k)，θ_me2(k)，θ_me3(k)，θ_mm1(k)，θ_mm2(k)，θ_mm3(k)分别表示第k时刻量测得到的太阳、地球和火星与第一颗恒星、第二颗恒星、第三颗恒星之间的角度信息。Z_k表示第k时刻的系统量测量，

6.q-method定姿；

由式(23)星敏感器获得的三颗恒星矢量方向信息利用q-method方法进行姿态确定，具体步骤为：

①获取由星敏感器输出的三颗恒星星光矢量方向建立在探测器本体坐标系中的星光观测矩阵

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

③设

S＝B+B^T， σ＝tr(B)；

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

⑤求解姿态矩阵

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

输出步骤④所得的姿态信息，并反馈姿态矩阵A_ib至步骤2。

7.巡航段轨道Unscented递推最小方差估计；开始Unscented递推最小方差估计

A.初始化

${\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(28\right)$

B.计算采样点

在第k-1时刻状态量附近选取一系列样本点，这些样本点的均值和协方差分别为

和P(k-1|k-1)，这里

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

${\mathrm{\χ}}_{0,k-1}={\hat{x}}_{k-1},W_0=-1$

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

${\mathrm{\χ}}_{j+6,k-1}={\hat{x}}_{k-1}-\sqrt{3}{\left(\sqrt{P(k-1|k-1)}\right)}_i,W_{j+6}=1/6$

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

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

取A的第j列，得第k-1时刻采样点χ_k-1的统一表达式为

C.时间更新

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

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

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

为

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

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

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

为

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

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

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

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

所有采样点量测估计加权值

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

D.量测更新

量测方差阵

为

$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(36\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(37\right)$

滤波增益K_k为

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

状态量的估计值

和估计方差P_k为

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

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

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

最终将式(39)和式(40)获得的第k时刻的状态量的估计值

和估计方差P_k输出，状态估计值

包括探测器的速度、位置信息，输出的估计方差P_k表示了滤波估计的性能，并将这些导航信息返回滤波器，用于k+1时刻的位置、速度导航信息，k＝1，2，...；并将步骤6中q-method方法估计的四元数姿态信息输出，提供姿态导航信息。

如巡航段没有结束，返回滤波中的步骤B，进行下一时刻的位置速度估计。如果巡航段结束，则计算完毕

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

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}]-{\text{\μ}}_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为状态模型噪声；

②建立太阳、地球、火星以及恒星的像元像线模型；

太阳、地球、火星以及第i颗恒星的像元像线模型可以表示为：

$\left[\begin{array}{c}{p}_s\\ l_s\end{array}\right]=K_s\left[\begin{array}{c}{x}_s^{2d}\\ y_s^{2d}\end{array}\right]+\left[\begin{array}{c}{p}_{0s}\\ l_{0s}\end{array}\right]=K_s\frac{f_s}{z_s^c}\left[\begin{array}{c}{x}_s^c\\ y_s^c\end{array}\right]+\left[\begin{array}{c}{p}_{0s}\\ l_{0s}\end{array}\right]+\left[\begin{array}{c}{v}_{p_s}\\ v_{l_s}\end{array}\right]---\left(3\right)$

$\left[\begin{array}{c}{p}_e\\ l_e\end{array}\right]=K_e\left[\begin{array}{c}{x}_e^{2d}\\ y_e^{2d}\end{array}\right]+\left[\begin{array}{c}{p}_{0e}\\ l_{0e}\end{array}\right]=K_e\frac{f_e}{z_e^c}\left[\begin{array}{c}{x}_e^c\\ y_e^c\end{array}\right]+\left[\begin{array}{c}{p}_{0e}\\ l_{0e}\end{array}\right]+\left[\begin{array}{c}{v}_{p_e}\\ v_{l_e}\end{array}\right]---\left(4\right)$

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

$\left[\begin{array}{c}{p}_{\mathrm{si}}\\ l_{\mathrm{si}}\end{array}\right]=K_m\left[\begin{array}{c}{x}_{\mathrm{si}}^{2d}\\ y_{\mathrm{si}}^{2d}\end{array}\right]+\left[\begin{array}{c}{p}_{0s}\\ l_{0s}\end{array}\right]=K_{\mathrm{star}}\frac{f_{\mathrm{star}}}{z_{\mathrm{si}}^c}\left[\begin{array}{c}{x}_{\mathrm{si}}^c\\ y_{\mathrm{si}}^c\end{array}\right]+\left[\begin{array}{c}{p}_{0\mathrm{star}}\\ l_{0\mathrm{star}}\end{array}\right]+\left[\begin{array}{c}{v}_{p_{\mathrm{si}}}\\ v_{l_{\mathrm{si}}}\end{array}\right]---\left(6\right)$

式中，(p_s，l_s)、(p_e，l_e)、(p_m，l_m)和(p_si，l_si)分别为太阳、地球、火星和第i颗恒星在太阳敏感器、地球敏感器、火星敏感器和星敏感器二维成像平面的像元和像线，i＝1，2，3，(p_0s，l_0s)、(p_0e，l_0e)、(p_0m，l_0m)和(p_0star，l_0star)分别为太阳敏感器、地球敏感器、火星敏感器和星敏感器二维成像平面中心的像元和像线；K_s、K_e、K_m和K_star分别为太阳敏感器、地球敏感器、火星敏感器和星敏感器由毫米转为像素的转换矩阵；f_s、f_e、f_m和f_star分别为太阳敏感器、地球敏感器、火星敏感器和星敏感器的焦距；

和

分别为太阳、地球、火星和第i颗恒星在太阳敏感器、地球敏感器、火星敏感器和星敏感器二维成像平面的坐标，

和

分别为太阳敏感器、地球敏感器、火星敏感器和星敏感器坐标系中的太阳、地球、火星和第i颗恒星的坐标，可以由惯性系中的坐标经惯性系到敏感器坐标系的转换矩阵得出，同时由星敏感器获取恒星矢量方向，并利用q-method方法输出姿态信息，转换为姿态矩阵，为像元像线模型提供坐标转换矩阵，

分别为太阳、地球、火星和恒星的像元像线模型误差；

③建立太阳、地球、火星与恒星之间角度信息的量测模型；

太阳、地球、火星分别与第i颗恒星的夹角，共9个角度θ_si、θ_ei和θ_mi表达式为：

$\left\{\begin{array}{c}{\mathrm{\θ}}_{\mathrm{si}}=\arccos (-{\stackrel{\→}{l}}_{\mathrm{ps}}\·{\stackrel{\→}{s}}_i)\\ {\mathrm{\θ}}_{\mathrm{ei}}=\arccos (-{\stackrel{\→}{l}}_{\mathrm{pe}}\·{\stackrel{\→}{s}}_i)\\ {\mathrm{\θ}}_{\mathrm{mi}}=\arccos (-{\stackrel{\→}{l}}_{\mathrm{pm}}\·{\stackrel{\→}{s}}_i)\end{array}\right.---\left(7\right)$

式中，

为太阳、地球和火星的方向，由步骤②中的像元像线转换得出，

为在各敏感器坐标系中由星敏感器识别的第i颗恒星星光的矢量方向；

设Z为系统观测量，X为系统状态量，则由式(7)可建立太阳、地球和火星之间角度信息量测方程的表达式为：

$Z=\left[\begin{array}{c}{\mathrm{\θ}}_{\mathrm{si}}\\ {\mathrm{\θ}}_{\mathrm{ei}}\\ {\mathrm{\θ}}_{\mathrm{mi}}\end{array}\right]+\left[\begin{array}{c}{v}_{{\mathrm{\θ}}_{\mathrm{si}}}\\ v_{{\mathrm{\θ}}_{\mathrm{ei}}}\\ v_{{\mathrm{\θ}}_{\mathrm{mi}}}\end{array}\right]=\left[\begin{array}{c}\arccos (-{\stackrel{\→}{l}}_{\mathrm{ps}}\·{\stackrel{\→}{s}}_i)\\ \arccos (-{\stackrel{\→}{l}}_{\mathrm{pe}}\·{\stackrel{\→}{s}}_i)\\ \arccos (-{\stackrel{\→}{l}}_{\mathrm{pm}}\·{\stackrel{\→}{s}}_i)\end{array}\right]+\left[\begin{array}{c}{v}_{{\mathrm{\θ}}_{\mathrm{si}}}\\ v_{{\mathrm{\θ}}_{\mathrm{ei}}}\\ v_{{\mathrm{\θ}}_{\mathrm{mi}}}\end{array}\right]=h\left(X\right)+v---\left(8\right)$

式中，h(·)为观测量Z的量测函数，为θ_si，θ_ei，θ_mi的观测误差；式中各变量都是与时间有关的函数，则式(8)可以简写为

Z(t)＝h(X(t)，t)+v(t)                    (9)

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

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

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

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

⑤成像敏感器获取量测量；

A.太阳敏感器、地球敏感器、火星敏感器和星敏感器获取日地火和恒星的图像信息；

B.对步骤A中图像信息进行质心提取后，获得太阳在太阳敏感器中图像的像元像线(p_ms，l_ms)、地球在地球敏感器中图像的像元像线(p_me，l_me)和火星在火星敏感器中图像的像元像线(p_mm，l_mm)及第i颗恒星在星敏感器中图像的像元像线(p_mi，l_mi)；

C.利用太阳、地球和火星的二维像元像线转换为三维矢量方向；

D.将太阳、地球和火星和恒星的矢量方向信息转换为太阳、地球和火星和恒星之间的角度信息；

⑥q-method定姿；

利用星敏感器输出的恒星矢量方向信息，确定探测器的姿态信息，并反馈姿态矩阵至步骤②；

⑦巡航段轨道Unscented递推最小方差估计；

利用Unscented递推最小方差估计算法，结合所述步骤①中的状态方程和步骤③中的量测方程进行滤波，利用太阳敏感器、火星敏感器、地球敏感器和星敏感器分别获取测量得到的太阳、火星、地球和恒星的像元像线信息，并将此测量信息转换为量测量太阳、火星、地球和恒星的角度信息，通过量测量与量测方程相减得到系统量测残差，用系统这一残差校正量测方程的模型误差；利用Unscented采样13个采样点，利用系统状态方程，对采样点进行一步预测，并得出与上一步迭代状态值之间协方差阵，以消除状态方程模型误差的影响，输出导航信息。

Images