CN103063217A - 一种基于星历修正的深空探测器天文/无线电组合导航方法 - Google Patents

Abstract

本发明涉及一种基于目标天体星历修正的深空探测器捕获段天文/无线电组合导航方法，首先建立火星探测器状态模型、天文导航和无线电导航子系统量测模型，然后获取天文和无线电导航子系统的量测量，滤波估计得到探测器在日心和以目标天体为中心惯性坐标系中的位置和速度；在此基础上，建立目标天体星历误差的状态模型和量测模型，并由天文和无线电两个导航子系统的估计状态向量获得目标天体星历误差量测量，利用卡尔曼滤波方法估计目标天体星历误差，并反馈至导航系统模型中，再进行信息融合。本发明属于航天导航技术领域，可在线估计天体星历误差，修正导航系统的模型误差，适用于探测器捕获段。

Description

一种基于星历修正的深空探测器天文/无线电组合导航方法

技术领域

本发明涉及在深空探测器处于捕获段时，基于目标天体图像和无线电信号时间延迟和频移信息的组合导航方法，是一种非常适用于深空探测器捕获段的组合导航方法。

背景技术

深空探测技术作为一个国家综合国力和科学技术发展水平的重要特征与标志，已引起世界各国的极大关注。新一轮深空探测的争夺战已经拉开了序幕。21世纪初，各航天大国纷纷将目光聚焦至距离地球38万公里以外的深空宇宙。美国、欧空局、俄罗斯、日本以及印度在内的世界主要航天集团都提出了未来的深空探测计划，要对各大行星及其卫星进行载人或基于机器人的无人探测。随着我国运载火箭和其他深空探测技术的发展和经济实力的提高，我国已具备探测火星甚至更远太阳系行星的能力。

深空探测器飞行过程主要包括地球逃逸、日心转移、目标捕获、环绕、着陆、巡视探测等过程。其中捕获段是指从深空探测器进入目标影响球开始到点火制动的全过程，处于该阶段的深空探测器飞行速度快，飞行弧段短，控制精度要求高，且机会唯一。深空探测器减速制动的入轨点距离目标行星表面非常近(近地点)，被捕获是整个深空探测任务的一个关键时间节点，捕获阶段相对目标天体的相对导航精度和相对于太阳或者地球的绝对导航精度对后续探测任务有直接影响。然而深空探测器在捕获段航行速度快、空间电离环境未知、大气环境复杂，这些因素都对深空探测器的入轨精度有很大的影响，也制约着深空探测器捕获后绕飞、着陆等阶段的导航精度，当入轨精度不能达到指标要求时，甚至无法完成科学探测任务，导致整个任务的失败。

目标天体星历数据是影响深空探测器捕获段导航性能的主要因素之一。目标天体星历数据是描述目标天体位置、速度等特征的一类天体数据库，由长期天文观测拟合而成的。若一段时间没有天文观测信息进行修正，目标天体星历数据误差会随时间的递推而增加。太阳系内行星(除地球外)目前的星历误差约为200m～100km。

现有的深空探测导航方式主要包括地面无线电导航和天文导航两种导航方式，其中无线电导航的主要观测量为探测器相对于地面站的距离与距离变化率等，这种导航方式可以测量相对于太阳的导航信息；而天文导航是测量目标天体及其背景恒星图像信息，从而从图像中获得相对于目标天体(如火星)的导航信息。由于目标天体星历误差大、天文敏感器测量精度有限等因素的影响，这两种方法仍存在以下不足：无线电导航方法仅能获取相对太阳的导航信息，因此由该方法获得的相对目标天体导航信息受到目标天体星历误差的影响大，相对目标天体导航精度低；天文导航方法由于受到天文导航敏感器精度有限这一因素的影响，其获得的相对目标天体导航信息精度有限，且受到目标天体星历误差影响，由该方法获得的相对太阳导航精度低。因此如何有效利用无线电导航和天文导航所提供的两种测量信息、减小星历误差对导航性能的影响，是捕获段高精度导航的一个关键问题。

传统的天文/无线电组合导航方法有两种：一种方法为将无线电导航的结果转换至目标天体为中心的惯性坐标系中后，将天文导航子系统和无线电子系统的导航结果进行信息融合。这种方法计算简单，易于实现，但由于在信息融合过程中没有考虑星历误差，因此无法克服目标天体星历误差对状态模型精度的影响，限制了导航精度的提高，导航精度较低。另一种方法为将无线电导航信息与天文导航信息组合进行几何解析，求解目标天体星历及星历误差，此后将无线电导航导航结果转换至目标天体为中心的惯性坐标系中，获得较高精度的导航信息。但是这种方法由于没有实时修正状态模型的模型误差，仍然无法消除目标天体星历误差对探测器状态模型精度的影响，导航精度仍较低。

发明内容

本发明要解决的技术问题是：克服星历误差对无线电导航信息相对目标天体精度的影响，弥补现有方法难以消除目标天体星历误差对探测器状态模型的影响这一不足，合理有效利用无线电导航和天文导航所提供的导航信息，为深空探测器捕获段提供一种高精度的组合导航方法。

本发明解决其技术问题所采用的技术方案为：建立目标天体为中心惯性坐标系中基于太阳和八大行星引力的深空探测器状态模型，通过光学导航敏感器获得目标天体及其卫星与恒星之间角度信息量测量，建立天文导航系统的量测模型，并使用Unscented卡尔曼滤波方法获得深空探测器相对目标天体的位置和速度参数；建立日心惯性坐标系中基于太阳和八大行星引力的深空探测器状态模型，通过无线电接收机获得探测器相对太阳的位置、速度量测量，建立无线电导航系统的量测模型，并使用Unscented卡尔曼滤波方法获得深空探测器相对日心的位置和速度参数；根据目标天体星历误差特性，建立目标天体星历误差状态模型和量测模型，利用无线电子系统和天文导航子系统的导航信息，获得目标天体星历误差量测量，并使用卡尔曼滤波方法获得目标天体星历误差的估计状态向量与估计均方误差阵，并将其反馈回无线电导航系统及天文导航系统的状态模型中，修正状态模型误差；利用星历修正系统获得的星历误差，将无线电导航系统的导航信息转换至以目标天体为中心的惯性坐标系中，与天文导航系统的导航信息进行融合，最终为处于捕获段的深空探测器提供位置、速度等导航参数。

具体包括以下步骤：

1.建立基于太阳和八大行星引力轨道动力学的深空探测器导航系统状态模型

A.在目标天体为中心的惯性坐标系中建立深空探测器基于太阳和八大行星引力轨道动力学的第一状态模型，即天文导航子系统的状态模型；

${\stackrel{\·}{X}}^{\′}\left(t\right)=f_1(X^{\′}\left(t\right),t)+w^{\′}\left(t\right)---\left(1\right)$

式中，X′(t)＝[x′,y′,z′,v′_x,v′_y,v′_z]T为状态向量，f₁(X′(t),t)为系统非线性连续状态转移函数，w′(t)＝[w′_x,w′_y,w′_z]^T为状态模型误差。

B.在日心惯性坐标系中建立深空探测器基于太阳和八大行星引力轨道动力学的第二状态模型，即无线电导航子系统的状态模型；

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

式中，X(t)＝[x,y,z,v_x,v_y,v_z]^T为状态向量，f₂(X(t),t)为系统非线性连续状态转移函数，w(t)＝[w_x,w_y,w_z]^T为状态模型误差。

2.分别建立天文导航子系统和无线电导航子系统量测模型

(1)天文导航子系统量测模型

目标天体和两个卫星与三颗背景恒星的角度信息θ_1i、θ_2i和θ_3i(i＝1,2,3)表达式为：

$\left\{\begin{array}{c}{\mathrm{\θ}}_{1i}=\arccos (-{\stackrel{\→}{l}}_{\mathrm{pl}}^I\·{\stackrel{\→}{s}}_{1i}^I)\\ {\mathrm{\θ}}_{2i}=\arccos (-{\stackrel{\→}{l}}_{p2}^I\·{\stackrel{\→}{s}}_{2i}^I)\\ {\mathrm{\θ}}_{3i}=\arccos (-{\stackrel{\→}{l}}_{p3}^I\·{\stackrel{\→}{s}}_{3i}^I)\end{array}---\left(3\right)\right.$

式中，

为在惯性坐标系中目标天体到探测器的单位矢量，

为在惯性坐标系中目标天体图像中第i个恒星的单位矢量；

为在惯性坐标系中目标天体的第一个卫星到探测器的单位矢量，为在惯性坐标系中目标天体第一个卫星图像中i个恒星的单位矢量；

为在惯性坐标系中第二个目标天体的卫星到探测器的单位矢量，

为在惯性坐标系中目标天体第二个卫星图像中i个恒星的单位矢量。

设天文导航子系统量测量Z₁＝[θ₁₁,θ₁₂,θ₁₃,θ₂₁,θ₂₂,θ₂₃,θ₃₁,θ₃₂,θ₃₃]^T，天文导航子系统量测噪声 $v_1={[v_{{\mathrm{\θ}}_{11}},v_{{\mathrm{\θ}}_{12}},v_{{\mathrm{\θ}}_{13}},v_{{\mathrm{\θ}}_{21}},v_{{\mathrm{\θ}}_{22}},v_{{\mathrm{\θ}}_{23}},v_{{\mathrm{\θ}}_{31}},v_{{\mathrm{\θ}}_{32}},v_{{\mathrm{\θ}}_{33}}]}^T,$ 分别为测量θ₁₁,θ₁₂,θ₁₃,θ₂₁,θ₂₂,θ₂₃,θ₃₁,θ₃₂,θ₃₃的观测误差，由于各变量都是与时间t有关的变量，则可根据式(3)建立天文导航子系统量测模型的表达式为：

Z₁(t)＝h₁[X(t),t]+v₁(t)        (4)

式中，h₁[X′(t),t]为天文导航子系统非线性连续量测函数。

(2)无线电导航子系统量测模型

无线电测控站可根据无线电信号的时延与频移测量探测器到测控站的距离与距离变化率，一般主测控站附近还配有多个副站，根据多个测控站测量的距离与距离变化率可以获得探测器相对于地心的位置与速度，无线电导航所获得的距离与距离变化率表达式为：

$R_i=\sqrt{{(x-x_f)}^2+{(y-y_f)}^2+{(z-z_f)}^2}---\left(5\right)$

式中，R_i为第i个测控站的测距量测信息(探测器与测控站之间的距离)，

为第i个测控站的距离变化率量测信息(探测器与测控站之间的距离变化率)，(x_f,y_f,z_f)为地面测控站在地心惯性坐标系中的位置坐标。

设无线电导航子系统量测量

无线电导航子系统量测噪声 $v_2={[v_{R_1},v_{{\stackrel{\·}{R}}_1},v_{R_2},v_{{\stackrel{\·}{R}}_2},v_{R_3},v_{{\stackrel{\·}{R}}_3}]}^T,$

分别为三个测控站所测量得到的探测器与测控站之间的距离和距离变化率，

分别为测量

的观测误差，由于各变量都是与时间t有关的变量，则可根据式(5)和式(6)建立无线电导航子系统量测模型的表达式为：

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

式中，h₂[X(t),t]为无线电导航子系统非线性连续量测函数。

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

X′(k)＝F₁(X′(k-1),k-1)+W′(k-1)      (8)

X(k)＝F₂(X(k-1),k-1)+W(k-1)     (9)

Z′(k)＝H₁(X′(k),k)+V₁(k)      (10)

Z(k)＝H₂(X(k),k)+V₂(k)      (11)

式中，k＝1,2,…，F₁(X′(k-1),k-1)和F₂(X(k-1),k-1)分别为f₁(X′(t),t)和f₂(X(t),t)离散后从第k-1时刻到第k时刻的非线性状态转移函数，H₁(X′(k),k)和H₂(X(k),k)分别为h₁(X′(t),t)和h₂(X(t),t)离散后第k时刻的非线性量测函数，W′(k)，W(k)，V₁(k)，V₂(k)分别为w′(t)，w(t)，v₁(t)和v₂(t)离散后第k时刻的等效噪声，且W′(k)和V₁(k)、W(k)和V₂(k)互不相关。

4.天文导航和无线电导航量测量的获取及处理

(1)天文导航子系统量测量的获取及处理

由天文导航敏感器获得目标天体的图像，利用图像处理技术，确定天体质心的像元像线坐标；经过从像元像线坐标系到二维成像平面坐标系、从二维成像平面坐标系到敏感器测量坐标系的三次转换，确定天体及其背景恒星在敏感器坐标系中的单位矢量；最后计算天体单位矢量与背景恒星单位矢量间的星光角距。

(2)无线电导航子系统量测量的获取及处理

探测器与测控站之间的距离变化率是通过测量探测器接收的载波信号频率f′_rec与地面站发射的载波信号频率f₀相比的频率偏移来获得的；探测器与测控站之间的距离是通过测量无线电信号由地面测控站发射至探测器再返回的时间延迟而获得的

5.对天文导航子系统进行Unscented卡尔曼滤波

根据第一状态模型、天文导航子系统量测模型、天文导航敏感器获得的量测量，进行天文导航子系统Unscented卡尔曼滤波，获得在目标天体惯性坐标系中表示深空探测器位置和速度的估计状态向量

和估计均方误差阵P′_k。

6.对无线电导航子系统进行Unscented卡尔曼滤波

根据第二状态模型、无线电导航子系统量测模型、由无线电接收装置获得的量测量，进行无线电导航子系统Unscented卡尔曼滤波，获得在日心惯性坐标系中表示深空探测器位置和速度的估计状态向量

和估计均方误差阵P_k。

7.判定是否需要进行目标天体星历校正

当天文导航子系统U nscented卡尔曼滤波的估计均方误差阵P′_k大于已有目标天体星历误差均方误差阵时，即P′_k＞P_eph，则不进行星历校正，直接进行第9步信息融合；当P′_k＜P_eph时，则进行星历误差校正，执行第8步；

8.对目标天体星历误差进行建模、估计并反馈校正

A.建立目标天体星历误差状态模型

将捕获段内其误差特性考虑为常值误差，在日心惯性坐标系中建立目标天体星历误差状态模型为：

式中，

为日心惯性坐标系中目标天体星历的三轴位置误差的微分，可离散化后简写为：

X_err(k)＝F_err(X_err(k-1),k-1)+W_err(k-1)      (13)

式中，状态转移函数F_err(X_err(k-1),k-1)=Φ_{err，k，k-1}X_err，k-1，Φ_{err，k，k-1}为第k-1时刻到第k时刻的状态转移矩阵，X_err(k)为第k时刻目标天体星历误差状态向量，且X_err(k)＝X_err，k，W_err(k-1)为第k-1时刻目标天体星历误差状态模型误差。

B.建立目标天体星历误差量测模型

目标天体星历误差的量测模型可以表示为：

Z_err＝H₃(X_err(k),k)+V₃       (14)

式中，H₃(X_err(k),k)为k时刻的量测函数，V₃为目标天体星历误差量测噪声。

C.获取目标天体星历误差量测量

目标天体星历误差量测量Z_err可以表示为：

$Z_{\mathrm{err}}=X_{\mathrm{radio}}^s-X_{\mathrm{opt}}^{t\arg \mathrm{et}}-X_{t\arg \mathrm{et}}^m---\left(15\right)$

式中，为无线电导航子系统获得的相对于太阳的位置和速度，

为天文导航子系统获得的相对于目标天体的位置和速度，为目标天体相对于太阳的位置和速度，从天体星历数据库中获得。

D.对目标天体星历误差进行卡尔曼滤波估计

根据步骤A和步骤B建立的目标天体星历误差状态模型和量测模型，以及步骤C所获取的目标天体星历误差量测量，利用卡尔曼滤波方法，对目标天体星历误差进行估计，获得目标天体星历误差估计状态向量与估计均方误差阵，具体如下：

目标天体星历误差状态向量的一步预测

$X_{\mathrm{err},k/k-1}={\mathrm{\Φ}}_{\mathrm{err},k,k-1}{\hat{X}}_{\mathrm{err},k-1}---\left(16\right)$

式中，

为k-1时刻火星星历误差一步预测状态向量。

目标天体星历误差状态向量的估计均方误差阵一步预测

P_err,k/k-1＝Φ_err,k，k-1P_err,k-1Φ_err,k，k-1 ^T+Q_err，k    (17)

式中，P_err,k-1为k-1时刻目标天体星历误差状态向量的估计均方误差阵，Q_err，k为k时刻目标天体星历误差状态模型误差协方差阵。

卡尔曼滤波增益

K_err，k＝P_err,k/k-1H_err，k ^T(H_err，kP_err,k/k-1H_err，k ^T+R_err,k)^-1   (18)

式中，H_err,k为k时刻目标天体星历误差量测矩阵，H_err，kX_err，k＝H₃(X_err,k)，R_err，k为k时刻目标天体星历误差量测模型误差协方差阵。

目标天体星历误差估计状态向量

${\hat{X}}_{\mathrm{err},k}=X_{\mathrm{err},k/k-1}+K_{\mathrm{err},k}(Z_{\mathrm{err},k}-H_{\mathrm{err},k}{X}_{\mathrm{err},k/k-1})---\left(19\right)$

式中，z_err,k为k时刻目标天体星历误差量测量。

目标天体星历误差估计均方误差阵

P_err,k＝(I-K_err,kH_err,k)P_err,k/k-1      (20)

式中，I为单位阵。

E.对目标天体星历误差进行反馈校正

将步骤D中获得的目标天体星历误差

和目标天体星历误差估计均方误差阵P_err,k反馈回深空探测器的第一状态模型和第二状态模型中，并重新确定第一状态模型和第二状态模型的模型误差协方差阵，最后将校正后的模型误差协方差阵输入至天文导航子系统Unscented卡尔曼滤波和无线电导航子系统Unscented卡尔曼滤波中，修正下一时刻的导航结果。

9.信息融合

利用星历修正系统获得的星历误差，将无线电导航系统的导航信息转换至以目标天体为中心的惯性坐标系中，与天文导航系统的导航信息进行融合。当探测器不在无线电测控范围内时，无线电导航子系统没有输入的无线电导航量测量，此时对天文导航子系统进行Unscented卡尔曼滤波，无线电导航子系统只利用第二状态模型进行时间更新；当探测器处于无线电测控范围内时，无线电导航子系统有输入的无线电导航量测量，对两个导航子系统同时进行Unscented卡尔曼滤波；

最终输出第k时刻在目标天体为中心的惯性坐标系中表示探测器位置和速度的估计状态向量和估计均方误差阵，并根据修正后的目标行星星历，将该结果转换至日心惯性坐标系中，输出在日心惯性坐标系中表示探测器位置和速度的估计状态向量和估计均方误差阵，将这些导航信息分别返回天文导航子系统和无线电导航子系统中，用于k+1时刻的位置、速度导航信息的估计，k＝1,2,...；

本发明的原理是：首先选择基于太阳和八大行星引力的轨道动力学模型作为系统状态模型，根据天文导航和无线电导航测量的不同特点，分别建立在目标天体为中心的惯性坐标系和日心惯性坐标系中的两个状态模型；之后根据天文导航敏感器和无线电导航接收装置的工作原理，获取并处理的天文导航敏感器和无线电导航接收装置所测量的量测量；然后，建立天文导航子系统和无线电导航子系统的量测模型；此后，由于探测器的状态模型和量测模型都呈现非线性特性，且系统噪声为非高斯噪声，因此采用Unscented卡尔曼滤波方法，对两个子系统的探测器导航信息进行估计；其次，由于天文导航可以测量高精度的相对目标天体导航信息，而无线电导航可以测量高精度的相对于太阳导航信息；结合目标天体、探测器、太阳的几何关系，可以确定目标天体的星历，与原有的目标天体星历相比较，即可得到目标天体星历误差量测量；为了获得更为准确的目标天体星历误差，结合捕获段持续时间短、目标天体星历误差在捕获段变化小的特点，建立捕获段目标天体星历误差的状态模型和量测模型，并根据目标天体星历误差状态模型和量测模型都为线性模型的特点，采用卡尔曼滤波方法，估计目标天体星历误差，并将所估计的目标天体星历误差反馈至第一状态模型和第二状态模型中，修正目标天体星历数据，提高状态模型的模型精度；最后通过信息融合方法，有效利用天文导航子系统和无线电子系统的测量信息，提高对相对于目标天体和相对于日心的探测器导航信息的估计精度。

本发明与现有技术相比的优点在于：利用星历校正过程所估计的目标天体星历误差，一方面实现了对目标天体星历误差的高精度估计，同时获得了相对目标天体和相对日心的探测器高精度导航信息，另一方面修正了导航系统的状态模型，减小了目标天体星历误差对状态模型精度的影响，进一步提高了深空探测器的导航精度。

附图说明

图1为本发明基于星历修正的天文无线电组合导航流程图。

图2为本发明天文导航子系统所用天文导航敏感器的成像原理图。

具体实施方式

如图1所示，前述技术方案中所涉及的目标天体可以为火星、金星、木星、土星等太阳系内的天体，以下以火星作为实施例，说明本发明的具体实施过程：

1.建立深空探测器基于太阳和八大行星引力轨道动力学的状态模型

首先初始化位置、速度，设X＝[x y z v_x v_y v_z]^T为在日心惯性坐标系中的状态向量，x,y,z,v_x,v_y，v_z分别为探测器在日心惯性坐标系中三轴的位置和速度，X′＝[x′ y′ z′ v_x′ v_y′ v_z′]^T为在火心惯性坐标系中的状态向量，x′,y′,z′,v′_x,v′_y,v′_z分别为探测器在火心惯性坐标系中三轴的位置和速度，上述变量都是与t有关的函数，根据探测器的轨道设计，选取探测器的位置和速度初值为X(0)和X′(0)；其次初始化火星星历误差初值为X_err(0)=[x_err y_err z_err v_xerr v_yerr v_zerr]^T，x_err,y_err,z_err,v_xerr,v_yerr,v_zerr分别为日心坐标系中火星星历三轴的位置误差和速度误差。

A.在火心惯性坐标系中建立深空探测器基于太阳和八大行星引力轨道动力学的第一状态模型，即天文导航子系统的状态模型；

考虑太阳和火星、地球等太阳和八大行星对探测器的引力作用，选取火心惯性坐标系，可得深空探测器在火心惯性坐标系中第一状态模型，即天文导航子系统的状态模型为：

$\left\{\begin{array}{c}{\stackrel{\·}{x}}^{\′}=v_x^{\′}\\ {\stackrel{\·}{y}}^{\′}=v_y^{\′}\\ {\stackrel{\·}{z}}^{\′}=v_z^{\′}\\ {\stackrel{\·}{v}}_x^{\′}=-{\mathrm{\μ}}_m\frac{x^{\′}}{r_{\mathrm{pm}}^{\′3}}-{\mathrm{\μ}}_s[\frac{x^{\′}-x_s^{\′}}{r_{\mathrm{ps}}^{\′3}}+\frac{x_s^{\′}}{r_{\mathrm{ms}}^{\′3}}]-\underset{i_c}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\μ}}_{i_c}[\frac{x^{\′}-x_{i_c}^{\′}}{r_{{\mathrm{pi}}_c}^{\′3}}+\frac{x_{i_c}^{\′}}{r_{{\mathrm{mi}}_c}^3}]+w_x^{\′}\\ {\stackrel{\·}{v}}_y^{\′}=-{\mathrm{\μ}}_m\frac{y^{\′}}{r_{\mathrm{pm}}^{\′3}}-{\mathrm{\μ}}_s[\frac{y^{\′}-y_s^{\′}}{r_{\mathrm{ps}}^{\′3}}+\frac{y_s^{\′}}{r_{\mathrm{ms}}^{\′3}}]-\underset{i_c}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\μ}}_{i_c}[\frac{y^{\′}-y_{i_c}^{\′}}{r_{{\mathrm{pi}}_c}^{\′3}}+\frac{y_{i_c}^{\′}}{r_{{\mathrm{mi}}_c}^3}]+w_y^{\′}\\ {\stackrel{\·}{v}}_z^{\′}=-{\mathrm{\μ}}_m\frac{z^{\′}}{r_{\mathrm{pm}}^{\′3}}-{\mathrm{\μ}}_s[\frac{z^{\′}-z_s^{\′}}{r_{\mathrm{ps}}^{\′3}}+\frac{z_s^{\′}}{r_{\mathrm{ms}}^{\′3}}]-\underset{i_c}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\μ}}_{i_c}[\frac{z^{\′}-z_{i_c}^{\′}}{r_{{\mathrm{pi}}_c}^{\′3}}+\frac{z_{i_c}^{\′}}{r_{{\mathrm{mi}}_c}^3}]+w_z^{\′}\end{array}\right.---\left(1\right)$

式中，x′,y′,z′为探测器在火心惯性坐标系中三轴位置，v′_x,v′_y,v′_z为探测器在火心惯性坐标系中三轴速度，

为探测器在火心惯性坐标系中三轴位置的微分，

为探测器在火心惯性坐标系中三轴速度的微分，μ_s、μ_m和

分别为太阳、火星和第i_c颗行星的引力常数；r′_ps为日心到探测器的距离；r′_pm为火星到探测器的距离；r′_ms为日心到火心的距离；为第i_c颗行星到探测器的距离；r′_mi为第i_c颗行星质心到火心的距离；(x′_s,y′_s,z′_s)，

分别为太阳、第i_c颗行星在火心惯性坐标系中的三轴位置坐标，可根据时间由行星星历表获得，w_x′,w_y′,w_z′分别为第一状态模型中探测器三轴的状态模型误差；i_c表示太阳和八大行星中从内至外的第i_c颗行星，i_c＝1,2,3...,N(i_c≠4)，N=8，由于i_c＝4表示第4颗行星(火星)，因此不包含在求和表达式中。

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

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

式中，X′(t)＝[x′,y′,z′,v′_x,v′_y,v′_z]^T为第一状态模型的状态向量，为X′(t)的微分，f₁(X′(t),t)为第一状态模型的系统非线性连续状态转移函数，w′(t)＝[w′_x,w′_y,w′_z]^T为第一状态模型的状态模型误差。

B.在日心惯性坐标系中建立深空探测器基于太阳和八大行星引力轨道动力学的第二状态模型，即无线电导航子系统的状态模型；

考虑太阳和火星、地球等太阳和八大行星对探测器的引力作用，选取日心惯性坐标系，可得深空探测器在日心惯性坐标系中展开为分量形式的第二状态模型，即无线电导航子系统的状态模型为：

$\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}-\underset{i_c}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\μ}}_{i_c}[\frac{x-x_{i_c}}{r_{{\mathrm{pi}}_c}^3}+\frac{x_{i_c}}{r_{{\mathrm{si}}_c}^3}]+w_x\\ {\stackrel{\·}{v}}_y=-{\mathrm{\μ}}_s\frac{y}{r_{\mathrm{ps}}^3}-\underset{i_c}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\μ}}_{i_c}[\frac{x-x_{i_c}}{r_{{\mathrm{pi}}_c}^3}+\frac{x_{i_c}}{r_{{\mathrm{si}}_c}^3}]+w_y\\ {\stackrel{\·}{v}}_z=-{\mathrm{\μ}}_s\frac{z}{r_{\mathrm{ps}}^3}-\underset{i_c}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\μ}}_{i_c}[\frac{x-x_{i_c}}{r_{{\mathrm{pi}}_c}^3}+\frac{x_{i_c}}{r_{{\mathrm{si}}_c}^3}]+w_z\end{array}\right.---\left(3\right)$

式中，x,y,z为探测器在日心惯性坐标系中三轴位置，v_x,v_y,v_z为探测器在日心惯性坐标系中三轴速度，

为探测器在日心惯性坐标系中三轴位置的微分，

为探测器在日心惯性坐标系中三轴速度的微分，μ_s为太阳引力常数，

为第i_c个行星的引力常数；r_ps为日心到探测器的距离；

为第i_c个行星到探测器的距离；

为第i_c个行星质心到日心的距离；

分别为第i_c个行星在日心惯性坐标系中的坐标，可根据时间由行星星历表获得，w_x,w_y,w_z分别为第二状态模型中探测器三轴的状态模型误差；

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

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

式中，X(t)＝[x,y,z,v_x,v_y,v_z]^T为第二状态模型的状态向量，

为X(t)的微分，f₂(X(t),t)为第二状态模型系统非线性连续状态转移函数，w(t)＝[w_x,w_y,w_z]^T为第二状态模型的状态模型误差。

2.分别建立天文导航子系统和无线电导航子系统量测模型；

(1)天文导航子系统量测模型

火星与第i颗背景恒星之间角度信息θ_mi的大小在不同坐标系中是相同的，因此其表达式为：

${\mathrm{\θ}}_{\mathrm{mi}}=\arccos (-{\stackrel{\→}{l}}_{\mathrm{pm}}^S\·{\stackrel{\→}{s}}_{\mathrm{li}}^S)=\arccos (-{\stackrel{\→}{l}}_{\mathrm{pm}}^I\·{\stackrel{\→}{s}}_{\mathrm{li}}^I)---\left(5\right)$

式中，

为在火星敏感器测量坐标系中从火星至探测器的单位矢量，

为在惯性坐标系中火星到探测器的单位矢量，可表示为：

${\stackrel{\→}{l}}_{\mathrm{pm}}^I=\frac{1}{\sqrt{x^{\′2}+y^{\′2}+z^{\′2}}}\left[\begin{array}{c}{x}^{\′}\\ y^{\′}\\ z^{\′}\end{array}\right]---\left(6\right)$

式中，(x′,y′,z′)为探测器在火心惯性坐标系中三轴位置坐标，

为火星图像中第i颗背景恒星在火星敏感器测量坐标系中的单位矢量，

为在惯性坐标系中第i颗背景恒星星光的单位矢量，i＝1,2,3，可由恒星星历数据库直接得到，

同理可得火卫一和火卫二与其第i颗背景恒星之间角度信息θ_pi和θ_di的表达式为：

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

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

设天文导航子系统量测量Z₁＝[θ_m1,θ_m2,θ_m3,θ_p1,θ_p2,θ_p3,θ_d1,θ_d2,θ_d3]^T，天文导航子系统量测噪声 $v_1={[v_{{\mathrm{\θ}}_{m1}},v_{{\mathrm{\θ}}_{m2}},v_{{\mathrm{\θ}}_{m3}},v_{{\mathrm{\θ}}_{p1}},v_{{\mathrm{\θ}}_{p2}},v_{{\mathrm{\θ}}_{p3}},v_{{\mathrm{\θ}}_{d1}},v_{{\mathrm{\θ}}_{d2}},v_{{\mathrm{\θ}}_{d3}}]}^T,$

分别为测量θ_m1,θ_m2,θ_m3,θ_p1,θ_p2,θ_p3,θ_d1,θ_d2,θ_d3的观测误差，由于各变量都是与时间t有关的变量，则可根据式(6)～式(8)建立天文导航子系统量测模型的表达式为

Z₁(t)＝h₁[X′(t),t]+v₁(t)      (9)

式中，h₁[X′(t),t]为天文导航子系统非线性连续量测函数。

(2)无线电导航子系统量测模型

无线电测控站可根据无线电信号的时延与多普勒频移测量探测器到测控站的距离与距离变化率，一般主测控站附近还配有多个副站，根据多个测控站测量的距离与距离变化率可以获得探测器相对于地心的位置与速度，无线电测控导航所获得的距离与距离变化率表达式为：

$R_i=\sqrt{{(x-x_f)}^2+{(y-y_f)}^2+{(z-z_f)}^2}---\left(10\right)$

式中，R_i为第i个测控站的测距量测信息(探测器与测控站之间的距离)，

为第i个测控站的距离变化率量测信息(探测器与测控站之间的距离变化率)，i＝1,2,3，(x_f,y_f,z_f)为地面测控站在地心惯性坐标系中的位置坐标。

设无线电导航子系统量测量无线电导航子系统量测噪声 $v_2={[v_{R_1},v_{{\stackrel{\·}{R}}_1},v_{R_2},v_{{\stackrel{\·}{R}}_2},v_{R_3},v_{{\stackrel{\·}{R}}_3}]}^T,$ 分别为三个测控站所测量得到的探测器与测控站之间的距离和距离变化率，

分别为测量

的观测误差，由于各变量都是与时间t有关的变量，则可根据式(10)和式(11)建立无线电导航子系统量测模型的表达式为：

Z₂(t)＝h₂[X(t),t]+v₂(t)       (12)

式中，h₂[X(t),t]为无线电导航子系统非线性连续量测函数。

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

X′(k)＝F₁(X′(k-1),k-1)+W′(k-1)     (13)

X(k)＝F₂(X(k-1),k-1)+W(k-1)      (14)

Z′(k)＝H₁(X′(k),k)+V₁(k)        (15)

Z(k)＝H₂(X(k),k)+V₂(k)       (16)

式中，k＝1,2,…，F₁(X′(k-1),k-1)和F₂(X(k-1),k-1)分别为f₁(X′(t),t)和f₂(X(t),t)离散后从第k-1时刻到第k时刻的非线性状态转移函数，H₁(X′(k),k)和H₂(X(k),k)分别为h₁(X′(t),t)和h₂(X(t),t)离散后第k时刻的非线性量测函数，W′(k)，W(k)，V₁(k)，V₂(k)分别为w′(t)，w(t)，v₁(t)和v₂(t)离散后第k时刻的等效噪声，且W′(k)和V₁(k)、W(k)和V₂(k)互不相关。

4.天文导航和无线电导航量测量的获取及处理

(1)天文导航子系统量测量的获取及处理

图2给出了天文导航子系统所用火星敏感器的成像原理图，火卫一敏感器、火卫二敏感器成像过程与之类似。火星敏感器主要由光学透镜和二维成像面阵组成，在敏感器测量坐标系O′X_cY_cZ_c中沿火星到探测器的矢量方向

火星反射太阳光线射向火星敏感器，此时，火星在火星敏感器测量坐标系中的坐标为(x_c,y_c,z_c)；火星敏感器的光学透镜以焦距f将火星的光线折射后成像在二维成像面阵上，二维成像面阵将照在每个成像单元上的图像亮度信号储存。根据敏感器的成像原理，天文导航子系统量测量的处理过程如下所示：

A.天体图像的处理

由于火星在二维成像面阵上的图像并不是一个点，而是一个圆，通过质心识别等图像处理技术确定火星图像在二维成像平面坐标系OX_2dY_2d的质心(x_2d,y_2d)，这一中心可以用像元像线坐标系O_plX_plY_pl中的像元像线(p,l)表示。

B.质心坐标从像元像线坐标系转换至二维成像平面坐标系的转换

根据像元像线坐标系与二维成像平面坐标系之间的关系，将火星质心坐标从像元像线坐标系转换至二维成像平面坐标系：

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

式中，(x_2d,y_2d)为火星在二维成像平面OX_2dY_2d中的坐标，p和l分别为火星在火星敏感器二维成像平面的像元和像线， $K=\left[\begin{array}{cc}{K}_x& K_{\mathrm{yx}}\\ K_{\mathrm{xy}}& K_y\end{array}\right]$ 为由毫米转为像素的敏感器转换矩阵，p₀和l₀分别为火星敏感器中心在像元像线坐标系OX_plY_pl中的像元和像线。

C.质心坐标从二维成像平面坐标系转换至敏感器测量坐标系的转换

根据透镜成像原理，将火星质心坐标从二维成像平面坐标系转换至敏感器测量坐标系：

${\stackrel{\→}{l}}_{\mathrm{pm}}^S=\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\end{array}\right]=\frac{1}{\sqrt{x_{2d}^2+y_{2d}^2+f^2}}\left[\begin{array}{c}{x}_{2d}\\ y_{2d}\\ -f\end{array}\right]---\left(18\right)$

式中，f为火星敏感器的焦距，

为在火星敏感器测量坐标系中从火星至探测器的单位矢量。

同理可获得火星图像中第i颗背景恒星在火星敏感器测量坐标系中的单位矢量i＝1,2,3。

D.计算星光角距

根据在火星敏感器测量坐标系中火星至探测器的单位矢量

与火星图像中第i颗背景恒星的单位矢量

计算两个矢量之间的星光角距θ_mi

${\mathrm{\θ}}_{\mathrm{mi}}=\arccos [\left({-\stackrel{\→}{l}}_{\mathrm{pm}}^S\right)\·{\stackrel{\→}{s}}_{\mathrm{li}}^S]---\left(19\right)$

同理可获得火卫一与其背景恒星、火卫二与其背景恒星之间的星光角距θ_pi,θ_di。

(2)无线电导航子系统量测量的获取及处理

选取探测器与测控站之间的距离变化率

和探测器与测控站之间的距离R作为无线电导航子系统的量测量。

探测器与测控站之间的距离变化率

是通过测量探测器接收的载波信号频率f′_rec与地面站发射的载波信号频率f₀相比的频率偏移来获得的，具体可表示为：

$\stackrel{\·}{R}=c(1-\frac{f_{\mathrm{rec}}^{\′}-{\mathrm{\δf}}_{\mathrm{atm}}-{\mathrm{\δf}}_0}{f_0})---\left(20\right)$

式中，

为探测器与地面站间的相对距离变化率，c为光在真空中的传播速度，f₀为地面站发射的无线电信号的固有频率，f′_rec为探测器上的接收机接收到的无线电信号的频率，δf_atm为大气层对信号的时延，δf₀为由信号源本振频率的漂移引起的误差，由于目前地面站多采用USO(Ultra Stable Oscillators)，该误差的量级很小。

探测器与测控站之间的距离R是通过测量无线电信号由地面测控站发射至探测器再返回的时间延迟而获得的，具体可表示为：

R＝cτ/2+cδt_R-cδ_T+δρ_atm       (21)

式中，c为光在真空中的传播速度，τ为无线电信号由地面测控站发射至探测器再返回的时间延迟，δt_R为接收装置时钟同步误差，δt_T为发射装置时钟同步误差，δρ_atm为由原子钟差引起的距离测量误差。

5.对天文导航子系统进行Unscented卡尔曼滤波

根据第一状态模型式(13)、天文导航子系统量测模型式(15)、由天文导航敏感器获得的量测量式(19)，进行天文导航子系统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(22\right)$

式中，

为第0时刻(初始时刻)在火心惯性坐标系中探测器的三轴位置和速度估计值，x′₀为第0时刻(初始时刻)在火心惯性坐标系中探测器的三轴位置和速度真实值，P′₀为状态向量的初始均方误差阵。

B.计算采样点

在天文导航子系统第k-1时刻状态向量附近选取一系列样本点，这些样本点的均值和均方误差阵分别为

和P′_k-1。设状态向量为6×1维，那么13个天文导航子系统的样本点χ′_0，k，...,χ′_12,k及其权重W′₀…W′₁₂分别如下：

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

${\mathrm{\χ}}_{j,k-1}^{\′}={\hat{x}}_{k-1}^{\′}+\sqrt{3}{\left(\sqrt{P_{k-1}^{\′}}\right)}_j,$ W′_j＝1/6    (23)

${\mathrm{\χ}}_{j+6,k-1}^{\′}={\hat{x}}_{k-1}^{\′}-\sqrt{3}{\left(\sqrt{P_{k-1}^{\′}}\right)}_j,$ W′_j+6＝1/6

式中，当P′_k-1＝A′^TA′时，

取A′的第j行，当P′_k-1＝A′A′^T时，

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

j＝1,2,....,6(24)

C.时间更新

天文导航子系统状态向量的一步预测χ′_k|k-1为：

χ′_k|k-1＝F₁(χ′_k-1,k-1)     (25)

天文导航子系统所有采样点状态向量的一步预测加权后结果

为：

${\hat{x}}_k^{\′-}=\underset{j=0}{\overset{12}{\mathrm{\Σ}}}{W}_j^{\′}{{\mathrm{\χ}}^{\′}}_{j,k|k-1}---\left(26\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(27\right)$

式中，Q′_k为k时刻天文导航子系统的状态模型误差协方差阵；

天文导航子系统采样点对应的量测估计向量Z′_k|k-1

Z′_k|k-1＝H₁(χ′_k|k-1,k)    (28)

天文导航子系统所有采样点量测估计加权向量

${\hat{z}}_k^{\′-}=\underset{j=0}{\overset{12}{\mathrm{\Σ}}}{W}_j^{\′}{Z^{\′}}_{j,k|k-1}---\left(29\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(30\right)$

式中，R′_k为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(31\right)$

天文导航子系统滤波增益K′_k为：

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

天文导航子系统的估计状态向量

和估计均方误差阵P′_k为

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

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

6.对无线电导航子系统进行Unscented卡尔曼滤波

根据第二状态模型式(14)、无线电导航子系统量测模型式(16)、由无线电接收装置获得的量测量式(20)和式(21)，进行无线电导航子系统Unscented卡尔曼滤波。具体步骤如下：

A.初始化

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

式中，为第0时刻(初始时刻)无线电导航子系统在日心惯性坐标系中探测器的三轴位置和速度估计值，x₀为第0时刻(初始时刻)无线电导航子系统在日心惯性坐标系中探测器的三轴位置和速度真实值，P₀为无线电导航子系统状态向量的初始均方误差阵。

B.计算采样点

在无线电导航子系统第k-1时刻状态向量

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

和P_k-1。设状态向量为6×1维，那么13个无线电导航子系统的样本点χ_0，k，...,χ_12,k及其权重W₀…W₁₂分别如下：

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

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

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

式中，当P_k-1＝A^TA时，

取A的第j行，当P_k-1＝AA^T时，

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

j＝1,2,....,6   (37)

C.时间更新

无线电导航子系统状态向量的一步预测χ_k|k-1为：

χ_k|k-1＝F₂(χ_k-1，k-1)    (38)

无线电导航子系统所有采样点状态向量的一步预测加权后结果

为：

${\hat{x}}_k^{-}=\underset{j=0}{\overset{12}{\mathrm{\Σ}}}{W}_j{\mathrm{\χ}}_{j,k|k-1}---\left(39\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(40\right)$

式中，Q_k为k时刻无线电导航子系统的状态模型误差协方差阵；

无线电导航子系统采样点对应的量测估计向量Z_k|k-1

Z_k|k-1＝H₂(χ_k|k-1,k)     (41)

无线电导航子系统所有采样点量测估计加权向量

${\hat{z}}_k^{-}=\underset{j=0}{\overset{12}{\mathrm{\Σ}}}{W}_j{Z}_{j,k|k-1}---\left(42\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(43\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(44\right)$

无线电导航子系统滤波增益K_k为：

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

无线电导航子系统估计状态向量

和估计均方误差阵P_k为：

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

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

7.判定是否需要进行火星星历校正

当天文导航子系统Unscented卡尔曼滤波的估计均方误差阵P′_k大于已有火星星历误差均方误差阵时，即P′_k＞P_eph，则不进行星历校正，直接进行第9步信息融合；当P′_k<P_eph时，则进行星历误差校正，执行第8步；

8.对火星星历误差进行建模、估计并反馈校正

无线电测控导航主要的观测量为探测器相对于地面站的距离与距离变化率等，这种导航方式可以直接测量相对于太阳的导航信息，天文导航可以直接测量相对于目标天体(如火星)的导航信息。由于目标天体的星历存在误差(200m～100km)，而无线电导航方法仅能获取相对太阳的高精度导航信息，因此由该方法获得的相对目标天体导航信息精度低；虽然天文导航方法可以获得较为准确的相对目标天体导航信息，因此由该方法无法获得相对太阳高精度的导航信息。因此需要对目标天体星历误差进行估计，并反馈校正由目标天体星历误差引起的导航误差。

天文导航子系统获得的相对于火星的位置和速度为

天文导航子系统获得的相对于太阳的位置和速度为为火星相对于太阳的位置和速度，可从天体星历数据库获得)；无线电导航子系统获得的相对于太阳的位置和速度为

无线电导航子系统获得的相对于火星的位置和速度为

由此可以看出，单一导航系统会受到火星星历误差的影响，无法同时满足探测器相对太阳和火星高精度导航的需求。因此可利用天文导航子系统Unscented卡尔曼滤波的结果和无线电导航子系统Unscented卡尔曼滤波的结果对火星星历误差进行估计，具体步骤如下：

A.建立火星星历误差状态模型

考虑到捕获段持续时间短(约40天)这一特点，火星星历误差变化较小，将捕获段内火星的星历误差特性考虑为常值误差，在日心惯性坐标系中建立火星星历误差状态模型为：

式中，

为日心惯性坐标系中火星星历的三轴位置误差的微分，离散化后简写为：

X_err(k)＝F_err(X_err(k-1)，k-1)+W_err(k-1)     (49)

式中，状态转移函数F_err(X_err(k-1),k-1)=Φ_{err，k，k-1}X_err，k-1，Φ_err,k，k-1为第k-1时刻到第k时刻的状态转移矩阵，X_err(k)为第k时刻火星星历误差状态向量，且X_err(k)＝X_err，k，W_err(k-1)为第k-1时刻火星误差状态模型误差。

B.建立火星星历误差量测模型

因此火星星历误差的量测模型可以表示为：

Z_err＝H₃(X_err(k),k)+V₃     (50)

式中，H₃(X_err(k),k)为k时刻的量测函数，V₃为火星星历误差量测噪声。

C.获取火星星历误差量测量

火星星历误差量测量z_err可以表示为：

$Z_{\mathrm{err}}=X_{\mathrm{radio}}^s-X_{\mathrm{opt}}^{t\arg \mathrm{et}}-X_{t\arg \mathrm{et}}^m---\left(51\right)$

式中，

为无线电导航子系统获得的相对于太阳的位置和速度，为天文导航子系统获得的相对于火星的位置和速度，

为火星相对于太阳的位置和速度，从天体星历数据库中获得。

D.对火星星历误差进行卡尔曼滤波估计

根据步骤A和步骤B建立的火星星历误差状态模型式(49)和量测模型式(50)，以及步骤C所获取的火星星历误差量测量式(51)，利用卡尔曼滤波方法，对火星星历误差进行估计，获得火星星历误差估计状态向量与估计均方误差阵，具体如下：

火星星历误差状态向量的一步预测

$X_{\mathrm{err},k/k-1}={\mathrm{\&Phi;}}_{\mathrm{err},k,k-1}{\hat{X}}_{\mathrm{err},k-1}---\left(52\right)$

式中，

为k-1时刻火星星历误差一步预测状态向量。

火星星历误差状态向量的估计均方误差阵一步预测

P_err,k/k-1＝Φ_{err，k，k-1}P_err,k-1Φ_err,k，k-1 ^T+Q_err，k     (53)

式中，P_err，k-1为k-1时刻火星星历误差状态向量的估计均方误差阵，Q_err，k为k时刻火星星历误差状态模型误差均方误差阵。

卡尔曼滤波增益

K_err，k＝P_err,k/k-1H_err，k ^T(H_err，kP_err,k/k-1H_err，k ^T+R_err,k)^-1     (54)

式中，H_err,k为k时刻火星星历误差量测矩阵，H_err，kX_err，k＝H₃(X_err,k)，R_err，k为k时刻火星星历误差量测模型误差协方差阵。

火星星历误差估计状态向量

${\hat{X}}_{\mathrm{err},k}=X_{\mathrm{err},k/k-1}+K_{\mathrm{err},k}(Z_{\mathrm{err},k}-H_{\mathrm{err},k}{X}_{\mathrm{err},k/k-1})---\left(55\right)$

式中，z_err，k为k时刻火星星历误差量测量。

火星星历误差估计均方误差阵

P_err,k＝(I-K_err，kH_err，k)P_err,k/k-1     (56)

式中，I为单位阵。

E.对火星星历误差进行反馈校正

将步骤D中获得的火星星历误差和火星星历估计均方误差阵P_err，k反馈回深空探测器的第一状态模型和第二状态模型中，并重新确定第一状态模型和第二状态模型的模型误差协方差阵Q′_k和Q_k，最后将校正后的模型误差协方差阵Q′_k和Q_k输入至天文导航子系统Unscented卡尔曼滤波和无线电导航子系统Unscented卡尔曼滤波中，修正下一时刻的导航结果。

9.信息融合

当探测器不在无线电测控范围内时，无线电导航子系统没有输入的无线电导航量测量，此时对天文导航子系统进行Unscented卡尔曼滤波，包括时间更新和量测更新(第5步的步骤C和步骤D)等步骤，无线电导航子系统只进行时间更新(第6步的步骤C)；当探测器处于无线电测控范围内时，无线电导航子系统有输入的无线电导航量测量，对两个子系统同时进行Unscented卡尔曼滤波，都进行时间更新和量测更新。

在滤波过程中得到的两个局部估计状态向量 $X_{i_w}\left(k\right)(i_w=\mathrm{1,2})(X_1\left(k\right)={\hat{x}}_k^{\&prime;},$ $X_2\left(k\right)={\hat{x}}_k-X_{\mathrm{mars},k}^m-{\hat{X}}_{\mathrm{err},k}),$ 两个估计均方误差阵 $P_{i_w}\left(k\right)(P_1\left(k\right)=P_k^{\&prime;},P_2\left(k\right)=P_k),$ 按下式进行融合，得到全局的估计状态向量和全局估计均方误差阵分别为：

${\hat{X}}_g\left(k\right)P_g\left(k\right)[P_1^{-1}\left(k\right)X_1\left(k\right)+P_2^{-1}\left(k\right)X_2\left(k\right)]---\left(57\right)$

$P_g\left(k\right)={[P_1^{-1}\left(k\right)+P_2^{-1}\left(k\right)]}^{-1}---\left(58\right)$

将全局估计结果反馈给两个导航子系统，作为k时刻两个导航子系统的估计结果：

${\hat{X}}_1\left(k\right)={\hat{X}}_g\left(k\right)---\left(59\right)$

${\hat{X}}_2^s\left(k\right)={\hat{X}}_g\left(k\right)+X_{\mathrm{mars},k}^m+{\hat{X}}_{\mathrm{err},k}---\left(60\right)$

$Q_{i_w}^{-1}\left(k\right)={\mathrm{\&beta;}}_{i_w}{Q}_g^{-1}\left(k\right)---\left(61\right)$

$P_{i_w}^{-1}\left(k\right)={\mathrm{\&beta;}}_{i_w}{P}_g^{-1}\left(k\right)---\left(62\right)$

${\mathrm{\&beta;}}_1+{\mathrm{\&beta;}}_2=1(0\&le;{\mathrm{\&beta;}}_{i_w}\&le;1,i_w=\mathrm{1,2})---\left(63\right)$

式中，

为信息分配因子。

信息分配因子选择的基本原则是在满足信息守恒公式的前提下与局部导航滤波精度成正比，为了使导航系统具有更强的自适应能力和容错能力，使用基于估计均方误差阵范数的动态分配信息因子的算法。

令

${\mathrm{\&beta;}}_{i_w}\left(k\right)=\frac{{\left({\left|\right|P_{i_w}(k-1)\left|\right|}_F\right)}^{-1}}{\underset{i_w=1}{\overset{2}{\mathrm{\&Sigma;}}}{\left({\left|\right|P_{i_w}(k-1)\left|\right|}_F\right)}^{-1}}---\left(64\right)$

式中，||·||_F为Frobenius范数，即对于任意矩阵A，

最终将式(59)和式(60)获得的第k时刻在火心惯性坐标系中和在日心惯性坐标系中的估计状态向量

和估计均方误差阵P₁(k),P₂(k)输出，估计状态向量

分别表示在火心惯性坐标系中和日心惯性坐标系中探测器的位置、速度信息，输出的估计均方误差阵P₁(k),P₂(k)表示了滤波估计的性能，并将这些导航信息分别返回天文导航子系统和无线电导航子系统中，用于k+1时刻的位置、速度导航信息，k＝1,2,...。

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

Claims (1)

1.一种基于星历修正的深空探测器天文/无线电组合导航方法，其特征在于：首先建立深空探测器的状态模型和量测模型，利用无线电导航子系统和天文导航子系统分别获取相对太阳和相对目标天体的量测量，通过Unscented滤波估计得到探测器在日心和目标天体为中心惯性坐标系中的位置和速度；在此基础上，建立目标天体星历误差的状态模型和量测模型，并由天文和无线电两个子系统的导航信息获得目标天体星历误差量测量，利用卡尔曼滤波方法对目标天体星历误差进行估计，并将目标天体星历误差反馈至导航系统模型中，对系统模型进行修正，此后将两个子系统的结果进行信息融合，获得校正星历误差后的相对于目标天体和相对于日心的探测器位置和速度；具体包括以下步骤：

①建立深空探测器基于太阳和八大行星引力轨道动力学状态模型；

A.在目标天体为中心的惯性坐标系中，建立深空探测器基于太阳和八大行星引力轨道动力学的第一状态模型，即天文导航子系统的状态模型；

B.在日心惯性坐标系中，建立深空探测器基于太阳和八大行星引力轨道动力学的第二状态模型，即无线电导航子系统的状态模型；

②分别建立天文导航子系统和无线电导航子系统量测模型；

③对步骤①和步骤②中的状态模型和量测模型进行离散化；

④天文导航和无线电导航量测量的获取及处理；

⑤对天文导航子系统进行Unscented卡尔曼滤波；

根据第一状态模型、天文导航子系统量测模型、天文导航敏感器获得的量测量，进行天文导航子系统Unscented卡尔曼滤波，获得在目标天体惯性坐标系中深空探测器位置和速度的估计状态向量和估计均方误差阵P′_k；

⑥对无线电导航子系统进行Unscented卡尔曼滤波；

根据第二状态模型、无线电导航子系统量测模型、由无线电接收装置获得的量测量，进行无线电导航子系统Unscented卡尔曼滤波，获得在日心惯性坐标系中深空探测器位置和速度的估计状态向量与估计均方误差阵P_k；

⑦判定是否需要进行目标天体星历校正；

当天文导航子系统Unscented卡尔曼滤波的估计均方误差阵P′_k大于已有目标天体星历误差均方差阵P_eph时，即P′_k＞P_eph，则不进行星历校正，直接进行第⑨步信息融合；当P′_k<P_eph时，进行目标天体星历误差校正，执行步骤⑧；

⑧对目标天体星历误差进行建模、估计并反馈校正；

A.建立目标天体星历误差状态模型

在日心惯性坐标系中建立目标天体星历误差状态模型为：

式中，

为日心惯性坐标系中目标天体星历的三轴位置误差的微分，离散化后为：

X_err(k)＝F_err(X_err(k-1),k-1)+W_err(k-1)

式中，状态转移函数F_err(X_err(k-1),k-1)=Φ_{err，k，k-1}X_err，k-1，其中Φ_{err，k，k-1}为第k-1时刻到第k时刻的状态转移矩阵，X_err(k)为第k时刻目标天体星历误差状态向量，且X_err(k)＝X_err，k，W_err(k-1)为第k-1时刻目标天体星历误差状态模型误差，k＝1,2,...；

B.建立目标天体星历误差量测模型

建立目标天体星历误差的量测模型为：

Z_err＝H₃(X_err(k),k)+V₃

式中，H₃(X_err(k),k)为k时刻的量测函数，V₃为目标天体星历误差量测噪声；

C.获取目标天体星历误差量测量

目标天体星历误差量测量Z_err表示为：

$Z_{\mathrm{err}}=X_{\mathrm{radio}}^s-X_{\mathrm{opt}}^{t\arg \mathrm{et}}-X_{t\arg \mathrm{et}}^m$

式中，

为无线电导航子系统获得的相对于太阳的位置和速度，

为天文导航子系统获得的相对于目标天体的位置和速度，

为目标天体相对于太阳的位置和速度，从天体星历数据库中获得；

D.对目标天体星历误差进行卡尔曼滤波估计

根据目标天体星历误差的状态模型、量测模型和获取的目标天体星历误差量测量利用卡尔曼滤波方法，对目标天体星历误差进行估计，获得目标天体星历误差估计状态向量与估计均方误差阵；

E.对目标天体星历误差进行反馈校正

将步骤D中获得的目标天体星历误差估计状态向量及估计均方误差阵反馈回深空探测器的第一状态模型和第二状态模型中，并重新确定第一状态模型和第二状态模型的模型误差协方差阵，最后将校正目标天体星历后的状态模型和模型误差协方差阵输入至天文导航子系统Unscented卡尔曼滤波和无线电导航子系统Unscented卡尔曼滤波中，修正下一时刻的导航结果；

⑨信息融合；

利用星历修正系统获得的星历误差，将无线电导航系统的导航信息转换至以目标天体为中心的惯性坐标系中，与天文导航系统的导航信息进行融合。当探测器不在无线电测控范围内时，无线电导航子系统没有输入的无线电导航量测量，此时对天文导航子系统进行Unscented卡尔曼滤波，无线电导航子系统只利用第二状态模型进行时间更新；当探测器处于无线电测控范围内时，无线电导航子系统有输入的无线电导航量测量，对两个导航子系统同时进行Unscented卡尔曼滤波；

最终输出第k时刻在目标天体为中心的惯性坐标系中表示探测器位置和速度的估计状态向量和估计均方误差阵，并根据修正后的目标行星星历，将该结果转换至日心惯性坐标系中，输出在日心惯性坐标系中表示探测器位置和速度的估计状态向量和估计均方误差阵，将这些导航信息分别返回天文导航子系统和无线电导航子系统中，用于k+1时刻的位置、速度导航信息的估计，k＝1,2,...。

Images