CN103148856B - 一种基于自适应尺度变化的借力飞行探测器自主天文导航方法 - Google Patents

Abstract

本发明涉及一种基于自适应尺度变化的借力飞行探测器自主天文导航方法，首先建立深空探测器状态模型、自主天文导航***量测模型，然后获取自主天文导航***的量测量，采用自适应尺度变化Unscented卡尔曼滤波方法，一方面获得适合当前模型的时间尺度，另一方面获得探测器在目标天体为中心惯性坐标系中的位置和速度；将所得的时间尺度、位置、速度用于下一时刻的导航。本发明属于航天导航技术领域，可随探测器受到的引力加速度随时间的变化自适应变化时间尺度，减小由固定时间尺度时间更新引起的导航***模型误差，适用于借力飞行深空探测器。

Description

一种基于自适应尺度变化的借力飞行探测器自主天文导航方法

技术领域

本发明涉及在深空探测器借力飞行时基于轨道模型自适应尺度变化的自主导航方法，是一种非常适用于深空探测器借力飞行阶段的自主导航方法。

背景技术

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

随着探测范围的扩大和探测距离的增加，深空探测器所需的发射能量也大幅增加，这是深空探测任务面临的最大困境。在寻求解决此问题的过程中，借力飞行技术由于能够有效降低发射能量和探测任务所需的速度增量，而成为深空探测任务中应用最为广泛的技术之一。借力飞行是指利用第二引力体来改变探测器相对中心引力体的能量，从而改变探测器速度的大小或方向，以节省发射能量，用较小的初始速度，在没有任何动力消耗的情况下实现对探测器的加速，完成探测任务，甚至实现仅靠目前的运载火箭所无法完成的探测任务。

虽然借力飞行轨道对运载火箭发射能量要求较低，但由于在探测器飞行过程中需要对天体借力，并且借力时对探测器所处位置和运行速度都有严格的指标要求，因此要求探测器导航***必须可以提供实时精确的导航信息，如果导航信息不及时或不够准确，探测器将错过向导航天体借力的最佳时机，无法进行正确的速度修正，甚至偏离预定轨道，最终导致任务失败。因此借力飞行阶段的导航精度是借力飞行任务成功实施的重要保证。

在深空探测器借力飞行阶段的导航精度受轨道动力学模型精度的影响。在自主导航过程中，需要对引力加速度进行积分得到速度和位置。因此影响轨道动力学模型精度的其中一个重要因素就是轨道动力学的积分步长。

现有的自主导航***都采用固定积分步长进行轨道动力学模型积分，这种方法存在以下不足：现有自主导航***采用的固定积分步长方法将天体引力加速度在一个积分区间中考虑 为常值。但是在实际运行的借力飞行深空探测任务中，由于随着探测器与火星逐渐接近，探测器受到火星引力逐渐增大，且火星的引力加速度并不是常值，随时间变化较大。现有固定步长积分方法将随时间变化较大的火星引力加速度在一个积分区间内考虑为常值的，因此将引入轨道动力学模型误差，该误差随积分步长的增大而增大。当自主导航***设定较小的积分步长时，在借力飞行前期由于探测器运动随时间变化缓慢，这种较小的积分步长计算量大，浪费了探测器星上计算资源；当自主导航***设定较大的积分步长时，在借力飞行时导航精度低，无法跟踪中心天体的引力变化。

发明内容

本发明要解决的技术问题是：克服现有固定步长积分方法在借力飞行前期计算量大、在借力飞行阶段导航精度低的问题，为借力飞行深空探测器提供一种高效高精度的自主导航方法。

本发明解决其技术问题所采用的技术方案为：建立目标天体为中心惯性坐标系中基于太阳和八大行星引力的深空探测器状态模型，通过光学导航敏感器获得目标天体及其卫星与恒星之间角度信息量测量，建立自主导航***的量测模型，并在现有时间尺度下，使用自适应尺度变化Unscented卡尔曼滤波方法，一方面获得深空探测器相对目标天体的位置和速度参数；另一方面根据轨道递推的结果，返回合适的时间尺度，在时间更新过程中采用所得的时间尺度进行更新。实现在轨道模型随时间变化小时时间尺度大，而在轨道模型随时间变化快时时间尺度小，最终为借力飞行深空探测器高效地提供高精度位置、速度等导航参数。

具体包括以下步骤：

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

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

X′(t)＝f₁(X′(t),t)+w′(t)    (5) 

式中，X′(t)＝[x′,y′,z′,v′_x,v′_y,v′_z]^T为t时刻的状态向量，x′,y′,z′,v′_x,v′_y,v′_z分别为探测器在目标天体质心惯性坐标系中三轴的位置和速度，f₁(X′(t),t)为***非线性连续状态转移函数，w′(t)＝[0,0,0,w′_x,w′_y，w′_z]^T为t时刻的状态模型误差，w′_x,w′_y,w′_z为三轴速度微分的模型误差。

2.建立基于星光角矩的自主天文导航***量测模型

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

$\left\{\begin{array}{c}{\mathrm{\θ}}_{1i}=\arccos (-{\stackrel{\→}{l}}_{p1}^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}\right.---\left(6\right)$

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

可根据式(6)建立自主天文导航***量测模型的表达式为：

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

式中，h₁[X′(t),t]为自主天文导航***非线性连续量测函数，Z′(t)＝[θ_11,θ₁₂,θ₁₃,θ₂₁,θ₂₂,θ₂₃,θ₃₁,θ₃₂,θ₃₃]^T为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{\θ}}_{31}},v_{{\mathrm{\θ}}_{32}},v_{{\mathrm{\θ}}_{33}}]}^T$ 为自主天文导航***量测噪声， 分别为测量θ₁₁,θ₁₂,θ₁₃,θ₂₁,θ₂₂,θ₂₃,θ₃₁,θ₃₂,θ₃₃的观测误差。

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

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

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

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

4.通过自主天文导航敏感器获得量测量

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

5.对自主天文导航***进行自适应尺度变化Unscented卡尔曼滤波

根据由步骤1～步骤3获得的自主天文导航***离散形式的状态模型和量测模型、以及由步骤4获得由天文导航敏感器获得的量测量，进行自主天文导航***自适应尺度变化 Unscented卡尔曼滤波。具体步骤如下：

A.初始化第n+1次滤波自主天文导航***的状态向量X′(n)和第n+1次滤波自主天文导航***的时间尺度h_n，第n次滤波所对应的时刻n＝0,1,2,......，其中X′(n)＝；

当n＝0时，

${\hat{x}}_0^{\′}=E\left[x_0^{\′}\right],P_0^{\′}=E\left[(x_0^{\′}-{\hat{x}}_0^{\′}){(x_0^{\′}-{\hat{x}}_0^{\′})}^T\right],h_n=h_0---\left(10\right)$ 式中，为初始时刻在目标天体质心惯性坐标系中探测器的三轴位置和速度初始值，x′₀为初始时刻在目标天体质心惯性坐标系中探测器的三轴位置和速度的估计值，P′₀为状态向量的初始均方误差阵，h_n为第n+1次滤波所使用的时间尺度，h₀为初始时刻滤波的时间尺度。当n≠0时，

${\hat{x}}_n^{\′}=E\left[x_n^{\′}\right],P_n^{\′}=E\left[(x_n^{\′}-{\hat{x}}_n^{\′}){(x_n^{\′}-{\hat{x}}_n^{\′})}^T\right],h_n=h_{n-1}---\left(11\right)$

式中，为第n+1次滤波在目标天体质心惯性坐标系中探测器的三轴位置和速度的初始值，x′_n为第n次滤波在目标天体质心惯性坐标系中探测器的三轴位置和速度估计值，P′_n为第n+1次滤波时状态向量的初始均方误差阵，h_n为第n+1次滤波所使用的时间尺度，h_n-1为第n次滤波所使用的时间尺度，第n次滤波所对应的时刻

B.利用步骤A初始化后的状态向量和时间尺度进行时间尺度自适应调整

a）计算时间尺度自适应调整过程中的12个中间时间点右函数K_stepi

K_step1＝F₁(X′(n),n)    (12) 

$K_{\mathrm{stepi}}=F_1(X^{\′}\left({n+a}_{\mathrm{stepi}}h\right)+h\underset{\mathrm{stepj}=0}{\overset{\mathrm{stepi}-1}{\mathrm{\Σ}}}{b}_{\mathrm{stepi},\mathrm{stepj}}{K}_{\mathrm{stepj}},{n+a}_{\mathrm{stpi}}h,),---\left(13\right)$

式中，X′(n)为第n+1次滤波所对应时刻的初始状态向量，F₁(X′(n),n)为从第n次滤波到第n+1次滤波的非线性状态转移离散函数，stepi=2,3...,12，其中b_stepi,stepj的非零值为：b_1,0＝2/27,b_2,0＝1/36,b_2,1＝1/12,b_3,0＝1/24,b_3,2＝1/8,b_4,0＝5/12,b_4,2＝-25/16,b_4,3＝25/16,b_5,0＝1/20,b_5,3＝1/4,b_5,4＝1/5,b_6,0＝-25/108,b_6,3＝125/108,b_6,4＝-65/27,b_6,5＝125/54,b_7,0＝31/300,b_7,4＝61/225,b_7,5＝-2/9,b_7,6＝13/900,b_8,0＝2b_8,3＝-53/6,b_8,4＝704/45,b_8,5＝-107/9,b_8,6＝67/90,b_9,0＝-91/108,b_9,3＝23/108,b_9,4＝-976/135,b_9,5＝311/54,b_9,6＝-19/60,b_9,7＝17/6,b_9,8＝-1/12,b_10,0＝2383/4100,b_10,3＝-341/164,b_10,4＝4496/1024,b_10,5＝-301/82,b_10,6＝2133/4100,b_10,7＝45/82,b_10,8＝45/162,b_10,9＝18/41,b_11,0＝3/205,b_11,5＝-6/41,b_11,6＝-3/205,b_11,7＝-3/41,b_11,8＝3/41,b_11,9＝6/41, b_12,0＝-1777/4100,b_12,5＝-289/82,b_12,6＝2193/4100,b_12,7＝51/82,b_12,8＝33/164,b_12,9＝12/41,b_12,11＝1，a_step0=0,a_step1=2/27,a_step2=1/9,a_step3=1/6,a_step4=5/12,a_step5=1/2,a_step6=5/6,a_step7=1/6,a_step8=2/3,a_step9=1/3,a_step10=1,a_step11=0,a_step12=1，X′(n)＝，h=h_n；

b）计算时间尺度自适应调整过程中的7阶和8阶预测状态向量

$X_{n+1}^{\′}=X_n^{\′}+h\underset{\mathrm{stepi}=0}{\overset{10}{\mathrm{\Σ}}}{c}_{\mathrm{stepi}}{K}_{\mathrm{stepi}}---\left(14\right)$

${\tilde{X}}_{n+1}^{\′}=X_n^{\′}+h\underset{\mathrm{stepi}=0}{\overset{12}{\mathrm{\Σ}}}{\tilde{c}}_{\mathrm{stepi}}{K}_{\mathrm{stepi}}---\left(15\right)$

式中，X′_n+1为时间尺度自适应调整过程中的7阶预测状态向量，为时间尺度自适应调整过程中的8阶预测状态向量，c_step0=41/840,c_step1=0,c_step2=0,c_step3=0,c_step4=0,c_step5=34/105,c_step6=9/35,c_step7=9/35,c_step8=9/280,c_step9＝9/280，c_step10＝41/840， ${\tilde{c}}_{\mathrm{step}0}=0,{\tilde{c}}_{\mathrm{step}1}=0,{\tilde{c}}_{\mathrm{step}2}=0,{\tilde{c}}_{\mathrm{step}3}=0,{\tilde{c}}_{\mathrm{step}4}=0,{\tilde{c}}_{\mathrm{step}5}=034/105,{\tilde{c}}_{\mathrm{step}6}=9/35,$ ${\tilde{c}}_{\mathrm{step}7}=9/35,{\tilde{c}}_{\mathrm{step}8}=9/280,{\tilde{c}}_{\mathrm{step}9}=9/280,{\tilde{c}}_{\mathrm{step}10}=0,{\tilde{c}}_{\mathrm{step}11}=41/840,{\tilde{c}}_{\mathrm{step}12}=41/840;$

c）计算状态向量的局部截断误差Δ

${\mathrm{\Δ}~X}_{n+1}-{\tilde{X}}_{n+1}~\frac{41}{810}(K_0+K_{10}-K_{11}{K}_{12})h---\left(16\right)$

d）判断自适应尺度变化Unscented卡尔曼滤波的步长是否需要调整

如果Δ>1e-16，则调整步长h＝h/2，返回步骤a）；否则进行步骤C；

C.计算自适应尺度变化Unscented卡尔曼滤波的采样点

在自主天文导航***第n+1次滤波所对应时刻的状态向量附近选取一系列样本点，这些样本点的均值和均方误差阵分别为和。设状态向量为6×1维，那么13个自主天文导航***的样本点χ′_0,n,...,χ′_12,n及其权重W₀′…W′₁₂分别如下：

${\mathrm{\χ}}_{0,n}^{\′}={\hat{x}}_n^{\′},W_0^{\′}=-1$

${\mathrm{\χ}}_{j,n}^{\′}={\hat{x}}_n^{\′}+\sqrt{3}{\left(\sqrt{P_n^{\′}}\right)}_j,W_j^{\′}=1/6---\left(17\right)$

${\mathrm{\χ}}_{j+6,n}^{\′}={\hat{x}}_n^{\′}-\sqrt{3}{\left(\sqrt{P_n^{\′}}\right)}_j,W_{j+6}^{\′}=1/6$

式中,当P′_n＝A′^TA′时，取A′的第j行，当P′_n＝A′A′^T时，取A′的第j列，得第n+1次滤波时采样点χ′_n的统一表达式为：

D.时间更新

根据步骤B所得的步长h，对步骤C所得的所有采样点χ′_n进行时间更新，获得自主天文导航***状态向量的一步预测估计均方误差阵一步预测量测估计向量

K_step1＝F₁(χ′_n,n)    (19) 

$K_{\mathrm{stepi}}=F_1({\mathrm{\χ}}_{n+a_{\mathrm{stepi}}h}^{\′}+h\underset{\mathrm{stepj}=0}{\overset{\mathrm{stepi}-1}{\mathrm{\Σ}}}{b}_{\mathrm{stepi},\mathrm{stepj}}{K}_{\mathrm{stepj}},n+a_{\mathrm{stepi}}h,)---\left(20\right)$

${\mathrm{\χ}}_{n+1|n}^{\′}={\mathrm{\χ}}_n^{\′}+h\underset{\mathrm{stepi}=0}{\overset{10}{\mathrm{\Σ}}}{c}_{\mathrm{stepi}}{K}_{\mathrm{stepi}}---\left(21\right)$

χ′_n+1|n＝F₁(χ′_n,n)    (22) 

自主天文导航***所有采样点状态向量的一步预测加权后结果为：

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

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

自主天文导航***状态向量的估计均方误差阵一步预测为：

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

式中，Q′_n+1为第n+1次滤波时自主天文导航***的状态模型误差协方差阵；

自主天文导航***采样点对应的量测估计向量Z′_n+1|n

Z′_n+1|n＝H₁(χ′_n+1|n,n+1)    (25) 

自主天文导航***所有采样点量测估计加权向量

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

E.量测更新

根据步骤D所得量测估计加权向量状态向量的一步预测加权后结果对量测均方误差阵、状态向量量测量均方误差阵、滤波增益、估计状态向量、估计均方误差阵进行更新：

自主天文导航***量测均方误差阵为：

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

式中，R′_n+1为第n+1次滤波时自主天文导航***的量测噪声协方差阵；

自主天文导航***状态向量量测量均方误差阵：

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

自主天文导航***滤波增益K′_n+1为：

$K_{n+1}^{\′}=P_{{\hat{x}}_{n+1}{\hat{z}}_{n+1}}^{\′}{P}_{{\hat{z}}_{n+1}{\hat{z}}_{n+1}}^{\′-1}---\left(29\right)$

自主天文异航***的估计状态向量和估计均方误差阵P′_n+1：

${\hat{x}}_{n+1}^{\′}={\hat{x}}_{n+1}^{\′-}+K_{n+1}^{\′}(Z_{n+1}^{\′}-{\hat{z}}_{n+1}^{\′-})---\left(30\right)$

$P_{n+1}^{\′}=P_{n+1}^{\′-}-K_{n+1}^{\′}{P}_{{\hat{z}}_{n+1}{\hat{z}}_{n+1}}^{\′}{K}_{n+1}^{\′T}---\left(31\right)$

最终将式(30)和式(31)获得的第n+1次滤波时在目标天体质心惯性坐标系的估计状态向量和估计均方误差阵P′_n+1输出，估计状态向量表示在目标天体质心惯性坐标系中探测器的位置、速度信息，输出的估计均方误差阵P′_n+1表示了滤波估计的性能，并将这些导航信息分别返回自主天文导航***中，用于第n+2次滤波时的位置、速度导航信息。

本发明的原理是：首先根据太阳和八大行星引力的轨道动力学，在目标天体为中心的惯性坐标系中建立***状态模型；然后，建立自主天文导航***的量测模型，之后根据天文导航敏感器的工作原理，获取并处理的天文导航敏感器所测量的量测量；此后，由于探测器的状态模型和量测模型都呈现非线性特性，且***噪声为非高斯噪声，因此采用Unscented卡尔曼滤波方法，对两个子***的探测器导航信息进行估计；其次，由于***状态模型在探测器借力飞行阶段变化快。若采用较大固定时间尺度求解状态方程误差大，难以实现高精度借力飞行，若采用较小固定时间尺度求解，借力飞行前期计算成本太大，浪费了星上的资源。因此采用自适应时间尺度变化的Unscented卡尔曼滤波方法，一方面获得深空探测器相对目标天体的高精度位置和速度参数；另一方面根据轨道递推的结果，返回合适的时间尺度，在时间更新过程中采用所得的时间尺度进行更新。实现在轨道模型随时间变化小时时间尺度大，而在轨道模型随时间变化快时时间尺度小，最终为借力飞行深空探测器高效地提供高精度位置、速度等导航参数。

本发明与现有技术相比的优点在于：采用自适应尺度变化的Unscented卡尔曼滤波方法，在时间更新过程中，根据轨道递推的结果，返回合适的时间尺度。实现在轨道模型随时间变化小时时间尺度大，而在轨道模型随时间变化快时时间尺度小，最终为借力飞行深空探测器高效地提供高精度位置、速度等导航参数

附图说明

图1为本发明基于自适应尺度变化Unscented卡尔曼滤波的自主天文导航流程图；

图2为本发明自主天文导航***所用天文导航敏感器的成像原理图；

图3为本发明中时间尺度自适应调整的流程图。

具体实施方式

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

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

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

在火心惯性坐标系中建立深空探测器基于太阳和八大行星引力轨道动力学的状态模型，考虑太阳和火星、地球等太阳和八大行星对探测器的引力作用，选取火心惯性坐标系，可得深空探测器在火心惯性坐标系中的状态模型为：

$\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颗行星到探测器的距离；为第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为t时刻的自主天文导航***状态模型的状态向量，(t)为X′(t)的微分，f₁(X′(t),t)为状态模型的***非线性连续状态转移函数，w′(t)＝[0,0,0,w′_x,w′_y,w′_z]^T为t时刻的状态模型误差，w′_x，w′_y,w′_z为三轴速度微分的模型误差。

2.建立基于星光角距的自主天文导航***量测模型；

火星与第i颗背景恒星之间角度信息θ_1i的大小在火星敏感器测量坐标系和火心惯性坐标系中的表达式一致，可以表示为：

${\mathrm{\θ}}_{1i}=\arccos (-{\stackrel{\→}{l}}_{\mathrm{pm}}^S\·{\stackrel{\→}{s}}_{1i}^S)=\arccos (-{\stackrel{\→}{l}}_{\mathrm{pm}}^I\·{\stackrel{\→}{s}}_{1i}^I)---\left(3\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(4\right)$

式中，(x′,y′,z′)为探测器在火心惯性坐标系中三轴位置坐标，为火星图像中第i颗背景恒星在火星敏感器测量坐标系中的单位矢量，为在惯性坐标系中第i颗背景恒星星光的单位矢量，i＝1,2,3，可由恒星星历数据库直接得到，

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

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

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

则可根据式(4)～式(6)建立自主天文导航量测模型的表达式为

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

式中，Z′(t)＝[θ₁₁,θ₁₂,θ₁₃,θ₂₁,θ₂₂,θ₂₃,θ₃₁,θ₃₂,θ₃₃]^T为t时刻的自主天文导航***量测量，θ₁₁,θ₁₂,θ₁₃,θ₂₁,θ₂₂,θ₂₃,θ₃₁,θ₃₂,θ₃₃分别为目标天体和两个卫星与三颗背景恒星的角度信息，h₁[X′(t),t]为自主天文导航非线性连续量测函数，为自主天文导航***量测噪声，分别为测量θ₁₁,θ₁₂,θ₁₃,θ₂₁,θ₂₂,θ₂₃,θ₃₁,θ₃₂,θ₃₃的观测误差。

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

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

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

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

4.通过自主天文导航敏感器获得量测量

图2给出了自主天文导航***所用火星敏感器的成像原理图，火卫一敏感器、火卫二敏感器成像过程与之类似。火星敏感器主要由光学透镜和二维成像面阵组成，在敏感器测量坐标系O′X_cY_cZ_c中沿火星到探测器的矢量方向火星反射太阳光线射向火星敏感器，此时，火星在火星敏感器测量坐标系中的坐标为(x_c,y_c,z_c)；火星敏感器的光学透镜以焦距f将火星的光线折射后成像在二维成像面阵上，二维成像面阵将照在每个成像单元上的图像亮度信号储存。根据敏感器的成像原理，自主天文导航***量测量的处理过程如下所示：

A.天体图像的处理

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

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

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

$\left[\begin{array}{c}{x}_{2d}\\ y_{2d}\end{array}\right]=K^{-1}(\left[\begin{array}{c}p\\ l\end{array}\right]-\left[\begin{array}{c}{p}_0\\ l_0\end{array}\right])---\left(10\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=\begin{array}{c}\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\end{array}\right]\end{array}=\frac{1}{\sqrt{x_{2d}^2+y_{2d}^2+f^2}}\left[\begin{array}{c}{x}_{2d}\\ y_{2d}\\ -f\end{array}\right]---(11)$

式中，(x_c,y_c,z_c)为火星质心在火星敏感器测量坐标系O′X_cY_cZ_c中的坐标，f为火星敏感器的焦距，为在火星敏感器测量坐标系中从火星至探测器的单位矢量。

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

D.计算星光角距 

根据在火星敏感器测量坐标系中火星至探测器的单位矢量与火星图像中第i颗背景恒星的单位矢量计算两个矢量之间的星光角距θ_1i

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

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

5.对自主天文导航***进行自适应尺度变化Unscented卡尔曼滤波

根据状态模型式(8)、自主天文导航***量测模型式(9)、由天文导航敏感器获得的量测量式(12)，进行自主天文导航***自适应尺度变化Unscented卡尔曼滤波。具体步骤如下：

A.初始化第n+1次滤波自主天文导航***的状态向量X′(n)和第n+1次滤波自主天文导航***的时间尺度h_n，第n次滤波所对应的时刻其中

当n＝0时，

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

式中，为初始时刻在火心惯性坐标系中探测器的三轴位置和速度初始值，x′₀为初始时刻在火心惯性坐标系中探测器的三轴位置和速度真实值，P₀′为状态向量的初始均方误差阵，h_n为滤波所使用的时间尺度，h₀为初始时刻滤波的时间尺度。

当n≠0时，

${\hat{x}}_n^{\′}=E\left[x_n^{\′}\right],P_n^{\′}=E\left[(x_n^{\′}-{\hat{x}}_n^{\′}){(x_n^{\′}-{\hat{x}}_n^{\′})}^T\right],h_n=h_{n-1}---\left(14\right)$

式中，为第n+1次滤波在火心惯性坐标系中探测器的三轴位置和速度的初始值，x′_n为第n次滤波在火心惯性坐标系中探测器的三轴位置和速度估计值，P′_n为第n+1次滤波时状态向量的初始均方误差阵，h_n为第n+1次滤波所使用的时间尺度，h_n-1为第n次滤波所使用的时间 尺度，第n次滤波所对应的时刻

B.利用步骤A初始化后的状态向量和时间尺度进行时间尺度自适应调整，流程图如图3所示；

a）计算时间尺度自适应调整过程中的12个中间步点右函数K_stepi

K_step1＝F₁(X′(n),n)    (15) 

$K_{\mathrm{stepi}}=F_1(X^{\′}(n+a_{\mathrm{stepi}}h)+h\underset{\mathrm{stepj}=1}{\overset{\mathrm{stepi}-1}{\mathrm{\Σ}}}{b}_{\mathrm{stepi},\mathrm{stepj}}{K}_{\mathrm{stepj}},n+a_{\mathrm{stepi}}h,)---\left(16\right)$

式中，X′(n)为第n+1次滤波所对应时刻的初始状态向量，F₁(X′(n),n)为从第n次滤波到第n+1次滤波的非线性状态转移离散函数，stepi=2,3...,12，其中b_stepi,stepj的非零值为：b_1,0＝2/27,b_2,0＝1/36,b_2,1＝1/12,b_3,0＝1/24,b_3,2＝1/8,b_4,0＝5/12,b_4,2＝-25/16,b_4,3＝25/16,b_5,0＝1/20,b_5,3＝1/4,b_5,4＝1/5,b_6,0＝-25/108,b_6,3＝125/108,b_6,4＝-65/27,b_6,5＝125/54,b_7,0＝31/300,b_7,4＝61/225,b_7,5＝-2/9,b_7,6＝13/900,b_8,0＝2b_8,3＝-53/6,b_8,4＝704/45,b_8,5＝-107/9,b_8,6＝67/90,b_9,0＝-91/108,b_9,3＝23/108,b_9,4＝-976/135,b_9,5＝311/54,b_9,6＝-19/60,b_9,7＝17/6,b_9,8＝-1/12,b_10,0＝2383/4100,b_10,3＝-341/164,b_10,4＝4496/1024,b_10,5＝-301/82,b_10,6＝2133/4100,b_10,7＝45/82,b_10,8＝45/162,b_10,9＝18/41,b_11,0＝3/205,b_11,5＝-6/41,b_11,6＝-3/205,b_11,7＝-3/41,b_11,8＝3/41,b_11,9＝6/41,b_12,0＝-1777/4100,b_12,5＝-289/82,b_12,6＝2193/4100,b_12,7＝51/82,b_12,8＝33/164,b_12,9＝12/41,b_12,11＝1a_step0=0,a_step1=2/27,a_step2=1/9,a_step3=1/6,a_step4=5/12,a_step5=1/2,a_step6=5/6,a_step7=1/6,a_step8=2/3,a_step9=1/3,a_step10=1,a_step11=0,a_step12=1，h=h_n；

b）计算时间尺度自适应调整过程中的7阶和8阶预测状态向量

$X_{n+1}^{\′}=X_n^{\′}+h\underset{\mathrm{stepi}=0}{\overset{10}{\mathrm{\Σ}}}{c}_{\mathrm{stepi}}{K}_{\mathrm{stepi}}---\left(17\right)$

${\tilde{X}}_{n+1}^{\′}=X_n^{\′}+h\underset{\mathrm{stepi}=0}{\overset{12}{\mathrm{\Σ}}}{\tilde{c}}_{\mathrm{stepi}}{K}_{\mathrm{stepi}}---\left(18\right)$

式中，X′_n+1为时间尺度自适应调整过程中的7阶预测状态向量，为时间尺度自适应调整过程中的8阶预测状态向量，c_step0=41/840,c_step1=0,c_step2=0,c_step3=0,c_step4=0,c_step5=34/105,c_step6=9/35,c_step7=9/35,c_step8=9/280,c_step9=9/280,c_step10=41/840， ${\tilde{c}}_{\mathrm{step}0}=0,{\tilde{c}}_{\mathrm{step}1}=0,{\tilde{c}}_{\mathrm{step}2}=0,{\tilde{c}}_{\mathrm{step}3}=0,{\tilde{c}}_{\mathrm{step}4}=0,{\tilde{c}}_{\mathrm{step}5}=34/105,{\tilde{c}}_{\mathrm{step}6}=9/35,$ ${\tilde{c}}_{\mathrm{step}7}=9/35,{\tilde{c}}_{\mathrm{step}8}=9/280,{\tilde{c}}_{\mathrm{step}9}=9/280,{\tilde{c}}_{\mathrm{step}10}=0,{\tilde{c}}_{\mathrm{step}11}=41/840,{\tilde{c}}_{\mathrm{step}12}=41/840;$

c）计算状态向量的局部截断误差Δ

$\mathrm{\Δ}~X_{n+1}-{\tilde{X}}_{n+1}~\frac{41}{810}(K_0+K_{10}-K_{11}-K_{12})h---\left(19\right)$

d）判断自适应尺度变化Unscented卡尔曼滤波的步长是否需要调整

如果Δ>1e-16，则调整步长h＝h/2，返回步骤a）；否则进行步骤C

C.计算自适应尺度变化Unscented卡尔曼滤波的采样点

在自主天文导航***第n+1次滤波所对应时刻的状态向量附近选取一系列样本点，这些样本点的均值和均方误差阵分别为和P′_n。设状态向量为6×1维，那么13个自主天文导航***的样本点χ′_0,n,...,χ′_12,n及其权重W′₀…W′₁₂分别如下：

${\mathrm{\χ}}_{0,n}^{\′}={\hat{x}}_n^{\′},W_0^{\′}=-1$

${\mathrm{\χ}}_{j,n}^{\′}={\hat{x}}_n^{\′}+\sqrt{3}{\left(\sqrt{P_n^{\′}}\right)}_j,W_j^{\′}=1/6---\left(20\right)$

${\mathrm{\χ}}_{j+6,n}^{\′}={\hat{x}}_n^{\′}-\sqrt{3}{\left(\sqrt{P_n^{\′}}\right)}_j,W_{j+6}^{\′}=1/6$

式中,当P′_n＝A′^TA′时，取A′的第j行，当P′_n＝A′A′^T时，取A′的第j列，得第n+1次滤波时采样点χ′_n的统一表达式为：

D.时间更新

根据步骤B所得的步长h，对步骤C所得的所有采样点χ′_n进行时间更新，获得自主天文导航***状态向量的一步预测估计均方误差阵一步预测量测估计向量

$K_{\mathrm{step}1}=F_1({\mathrm{\χ}}_n^{\′},n)---\left(22\right)$

$K_{\mathrm{stepi}}=F_1({\mathrm{\χ}}_{n+a_{\mathrm{stepi}}h}^{\′}+h\underset{\mathrm{stepj}=0}{\overset{\mathrm{stepi}-1}{\mathrm{\Σ}}}{b}_{\mathrm{stepi},\mathrm{stepj}}{K}_{\mathrm{stepj}},n+a_{\mathrm{stepi}}h,),---\left(23\right)$

${\mathrm{\χ}}_{n+1|n}^{\′}={\mathrm{\χ}}_n^{\′}+h\underset{\mathrm{stepi}=0}{\overset{10}{\mathrm{\Σ}}}{c}_{\mathrm{stepi}}{K}_{\mathrm{stepi}}---\left(24\right)$

χ′_n+1|n＝F₁(χ′_n,n)    (25) 

自主天文导航***所有采样点状态向量的一步预测加权后结果为：

${\hat{x}}_{n+1}^{\′-}=\underset{j=0}{\overset{12}{\mathrm{\Σ}}}{W}_j^{\′}{\mathrm{\χ}}_{j,n+1|n}^{\′}---\left(26\right)$

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

自主天文导航***状态向量的估计均方误差阵一步预测为：

$P_{n+1}^{\′-}=\underset{j=0}{\overset{12}{\mathrm{\Σ}}}{W}_j^{\′}[{\mathrm{\χ}}_{j,n+1|n}^{\′}-{\widehat{\mathrm{\χ}}}_{n+1}^{\′-}]{[{\mathrm{\χ}}_{j,n+1|n}^{\′}-{\hat{x}}_{n+1}^{\′-}]}^T+Q_{n+1}^{\′}---\left(27\right)$

式中，Q′_n+1为第n+1次滤波时自主天文导航***的状态模型误差协方差阵；

自主天文导航***采样点对应的量测估计向量Z′_n+1|n

Z′_n+1|n＝H₁(χ′_n+1|n,n+1)    (28) 

自主天文导航***所有采样点量测估计加权向量

${\hat{z}}_{n+1}^{\′-}=\underset{j=0}{\overset{12}{\mathrm{\Σ}}}{W}_j^{\′}{Z}_{j,n+1|n}^{\′}---\left(29\right)$

E.量测更新

根据步骤D所得量测估计加权向量、状态向量的一步预测加权后结果对量测均方误差阵、状态向量量测量均方误差阵、滤波增益、估计状态向量、估计均方误差阵进行更新：

自主天文导航***量测均方误差阵为：

$P_{{\hat{z}}_{n+1}{\hat{z}}_{n+1}}^{\′}=\underset{j=0}{\overset{12}{\mathrm{\Σ}}}{W}_j^{\′}[Z_{j,n+1|n}^{\′}-{\hat{z}}_{n+1}^{\′-}]{[Z_{j,n+1|n}^{\′}-{\hat{z}}_{n+1}^{\′-}]}^T+R_{n+1}^{\′}---\left(30\right)$

式中，R′_n+1为第n+1次波时自主天文导航***的量测噪声协方差阵；

自主天文导航***状态向量量测量均方误差阵

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

自主天文导航***滤波增益K′_n+1为：

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

自主天文导航***的估计状态向量和估计均方误差阵P′_n+1为

${\hat{x}}_{n+1}^{\′}={\hat{x}}_{n+1}^{\′-}+K_{n+1}^{\′}(Z_{n+1}^{\′}-{\hat{z}}_{n+1}^{\′-})---\left(33\right)$

$P_{n+1}^{\′}=P_{n+1}^{\′-}-K_{n+1}^{\′}{P}_{{\hat{z}}_{n+1}{\hat{z}}_{n+1}}^{\′}{K}_{n+1}^{\′T}---\left(34\right)$

最终将式(33)和式(34)获得的第n+1次滤波时在火心惯性坐标系中的估计状态向量和估计均方误差阵P′_n+1输出，估计状态向量表示在火心惯性坐标系中探测器的位置、速度信息，输出的估计均方误差阵P′_n+1表示了滤波估计的性能，并将这些导航信息分别返回自主天文导航***中，用于第n+2次滤波时的位置、速度导航信息。

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

Claims (1)

1.一种基于自适应尺度变化的借力飞行探测器自主天文导航方法，其特征在于：首先建立深空探测器的状态模型和量测模型，利用自主天文导航***获取相对目标天体的量测量，通过自适应尺度变化Unscented滤波估计得到探测器在目标天体为中心惯性坐标系中的位置和速度，具体包括以下步骤：

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

深空探测器在目标天体质心惯性坐标系中的状态模型为：

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

式中，X′(t)＝[x′,y′,z′,v′_x,v′_y,v′_z]^T为t时刻的自主天文导航***状态向量，为X′(t)的微分，x′,y′,z′,v′_x,v′_y,v′_z分别为探测器在在目标天体质心惯性坐标系中三轴的位置和速度，f₁(X′(t),t)为***非线性连续状态转移函数，w′(t)＝[0,0,0,w′_x,w′_y,w′_z]^T为t时刻的状态模型误差，w′_x,w′_y,w′_z为三轴速度微分的模型误差；

②建立基于星光角距的自主天文导航***量测模型；

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

式中，Z′(t)＝[θ₁₁,θ₁₂,θ₁₃,θ₂₁,θ₂₂,θ₂₃,θ₃₁,θ₃₂,θ₃₃]^T为t时刻的自主天文导航***量测量，θ₁₁,θ₁₂,θ₁₃,θ₂₁,θ₂₂,θ₂₃,θ₃₁,θ₃₂,θ₃₃分别为目标天体和两个卫星与三颗背景恒星的角度信息，h₁[X′(t),t]为自主天文导航非线性连续量测函数，为自主天文导航***量测噪声，分别为测量θ₁₁,θ₁₂,θ₁₃,θ₂₁,θ₂₂,θ₂₃,θ₃₁,θ₃₂,θ₃₃的观测误差；

③对步骤①和步骤②中的状态模型和量测模型进行离散化，获得自主天文导航***离散形式的状态模型和量测模型；

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

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

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

④通过自主天文导航敏感器获得量测量；

⑤根据由步骤③获得的自主天文导航***离散形式的状态模型和量测模型、以及由步骤④获得由天文导航敏感器获得的量测量，进行自主天文导航***自适应尺度变化Unscented卡尔曼滤波，具体步骤如下：

A.初始化第n+1次滤波自主天文导航***的状态向量和第n+1次滤波的时间尺度h_n，第n次滤波所对应的时刻n＝0,1,2,......，其中

B.利用步骤A初始化后的状态向量和时间尺度进行时间尺度自适应调整；

a)计算时间尺度自适应调整过程中的12个中间时间点右函数K_stepi

K_step1＝F₁(X′(n),n)

$K_{\mathrm{stepi}}=F_i(X^{\′}(n+a_{\mathrm{stepi}}h)+h\underset{\mathrm{stepj}=0}{\overset{\mathrm{stepi}-1}{\mathrm{\Σ}}}{b}_{\mathrm{stepi},\mathrm{stepj}}{K}_{\mathrm{stepj}},n+a_{\mathrm{stepi}}h,)$

式中，X′(n)为第n+1次滤波所对应时刻的初始状态向量，F₁(X′(n),n)为从第n次滤波到第n+1次滤波的非线性状态转移离散函数，stepi＝2,3...,12，其中b_stepi,stepj的非零值为：

b_1,0＝2/27,b_2,0＝1/36,b_2,1＝1/12,b_3,0＝1/24,b_3,2＝1/8,b_4,0＝5/12,

b_4,2＝-25/16,b_4,3＝25/16,b_5,0＝1/20,b_5,3＝1/4,b_5,4＝1/5,b_6,0＝-25/108,b_6,3＝125/108,

b_6,4＝-65/27,b_6,5＝125/54,b_7,0＝31/300,b_7,4＝61/225,b_7,5＝-2/9,b_7,6＝13/900,b_8,0＝2

b_8,3＝-53/6,b_8,4＝704/45,b_8,5＝-107/9,b_8,6＝67/90,b_9,0＝-91/108,b_9,3＝23/108,b_9,4＝-976/135,

b_9,5＝311/54,b_9,6＝-19/60,b_9,7＝17/6,b_9,8＝-1/12,b_10,0＝2383/4100,b_10,3＝-341/164,

b_10,4＝4496/1024,b_10,5＝-301/82,b_10,6＝2133/4100,b_10,7＝45/82,b_10,8＝45/162,

b_10,9＝18/41,b_11,0＝3/205,b_11,5＝-6/41,b_11,6＝-3/205,b_11,7＝-3/41,b_11,8＝3/41,b_11,9＝6/41,

b_12,0＝-1777/4100,b_12,5＝-289/82,b_12,6＝2193/4100,b_12,7＝51/82,b_12,8＝33/164,b_12,9＝12/41,

b_12,11＝1a_step0＝0,a_step1＝2/27,a_step2＝1/9,a_step3＝1/6,a_step4＝5/12,a_step5＝1/2,a_step6＝5/6,a_step7＝1/6,a_step8＝2/3,a_step9＝1/3,a_step10＝1,a_step11＝0,a_step12＝1，h＝h_n；

b)计算时间尺度自适应调整过程中的7阶和8阶预测状态向量

$X_{n+1}^{\′}=X_n^{\′}+h\underset{\mathrm{stepi}=0}{\overset{10}{\mathrm{\Σ}}}{c}_{\mathrm{stepi}}{K}_{\mathrm{stepi}}---\left(2\right)$

${\tilde{X}}_{n+1}^{\′}=X_n^{\′}+h\underset{\mathrm{stepi}=0}{\overset{12}{\mathrm{\Σ}}}{\tilde{c}}_{\mathrm{stepi}}{K}_{\mathrm{stepi}}---\left(3\right)$

式中，为时间尺度自适应调整过程中的7阶预测状态向量，为时间尺度自适应调整过程中的8阶预测状态向量c_step0＝41/840,c_step1＝0,c_step2＝0,c_step3＝0,c_step4＝0,c_step5＝34/105,c_step6＝9/35,c_step7＝9/35,c_step8＝9/280,c_step9＝9/280,c_step10＝41/840， ${\tilde{c}}_{\mathrm{step}0}=0,{\tilde{c}}_{\mathrm{step}1}=0,{\tilde{c}}_{\mathrm{step}2}=0,{\tilde{c}}_{\mathrm{step}3}=0,{\tilde{c}}_{\mathrm{step}4}=0,{\tilde{c}}_{\mathrm{step}5}=34/105,{\tilde{c}}_{\mathrm{step}6}=9/35,$ ${\tilde{c}}_{\mathrm{step}7}=9/35,{\tilde{c}}_{\mathrm{step}8}=9*280,{\tilde{c}}_{\mathrm{step}9}=9/280,{\tilde{c}}_{\mathrm{step}10}=0,{\tilde{c}}_{\mathrm{step}11}=41/840,{\tilde{c}}_{\mathrm{step}12}=41/840;$

c)计算状态向量的局部截断误差Δ

$\mathrm{\Δ}~X_{n+1}^{\′}-{\tilde{X}}_{n+1}^{\′}~\frac{41}{810}(K_0+K_{10}-K_{11}-K_{12})h---\left(4\right)$

d)判断自适应尺度变化Unscented卡尔曼滤波的步长是否需要调整

如果Δ>1e-16，则调整步长h＝h/2，返回步骤a)；否则进行步骤C；

C.计算自适应尺度变化Unscented卡尔曼滤波的采样点；

在自主天文导航***第n+1次滤波所对应时刻的状态向量附近选取一系列样本点，这些样本点的均值和均方误差阵分别为和P′_n，设状态向量为6×1维，那么13个自主天文导航***的样本点χ′₀,n,...,χ′₁₂,n及其权重W′₀…W′₁₂分别如下：

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

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

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

式中,当P′_n＝A^′TA′时，取A′的第j行，当P′_n＝A′A^′T时，取A′的第j列，得第n+1次滤波时采样点χ′_n的统一表达式为：

D.时间更新；

根据步骤B所得的步长h，对步骤C所得的所有采样点χ′_n进行时间更新，获得自主天文导航***状态向量的一步预测估计均方误差阵一步预测量测估计向量

K_step1＝F₁(χ′_n,n)

$K_{\mathrm{stepi}}=F_1({\mathrm{\χ}}_{n+a_{\mathrm{stepi}}h}^{\′}+h\underset{\mathrm{stepj}=0}{\overset{\mathrm{stepi}-1}{\mathrm{\Σ}}}{b}_{\mathrm{stepi},\mathrm{stepj}}{K}_{\mathrm{stepj}},n+a_{\mathrm{stepi}}h,)$

${\mathrm{\χ}}_{n+1|n}^{\′}={\mathrm{\χ}}_n^{\′}+h\underset{\mathrm{stepi}=0}{\overset{10}{\mathrm{\Σ}}}{c}_{\mathrm{stepi}}{K}_{\mathrm{stepi}}$

χ′_n+1|n＝F₁(χ′_n,n)

自主天文导航***所有采样点状态向量的一步预测加权后结果为：

${\hat{x}}_{n+1}^{\′-}=\underset{j=0}{\overset{12}{\mathrm{\Σ}}}{W}_j^{\′}{{\mathrm{\χ}}^{\′}}_{j,n+1|n}$

式中，W′_j为第j个采样点的权值；

自主天文导航***状态向量的估计均方误差阵一步预测为：

$P_{n+1}^{\′-}=\underset{j=0}{\overset{12}{\mathrm{\Σ}}}{W}_j^{\′}[{\mathrm{\χ}}_{j,n+1|n}^{\′}-{\hat{x}}_{n+1}^{\′-}]{[{\mathrm{\χ}}_{j,n+1|n}^{\′}-{\hat{x}}_{n+1}^{\′-}]}^T+Q_{n+1}^{\′}$

式中，Q′_n+1为第n+1次滤波时自主天文导航***的状态模型误差协方差阵；

自主天文导航***采样点对应的量测估计向量Z′_n+1|n

Z′_n+1|n＝H₁(χ′_n+1|n,n+1)

自主天文导航***所有采样点量测估计加权向量

${\hat{z}}_{n+1}^{\′-}=\underset{j=0}{\overset{12}{\mathrm{\Σ}}}{W}_J^{\′}{Z}_{j,n+1|n}^{\′}$

E.量测更新；

根据步骤D所得量测估计加权向量状态向量的一步预测加权后结果对量测均方误差阵、状态向量量测量均方误差阵、滤波增益、估计状态向量、估计均方误差阵进行更新；

自主天文导航***量测均方误差阵为：

$P_{{\hat{z}}_{n+1}{\hat{z}}_{n+1}}^{\′}=\underset{j=0}{\overset{12}{\mathrm{\Σ}}}{W}_j^{\′}[Z_{j,n+1|n}^{\′}-{\hat{z}}_{n+1}^{\′-}]{[Z_{j,n+1|n}^{\′}-{\hat{z}}_{n+1}^{\′-}]}^T+R_{n+1}^{\′}$

式中，R′_n+1为第n+1次滤波时自主天文导航***的量测噪声协方差阵；

自主天文导航***状态向量量测量均方误差阵

$P_{{\hat{x}}_{n+1}{\hat{z}}_{n+1}}^{\′}=\underset{j=0}{\overset{12}{\mathrm{\Σ}}}{W}_j^{\′}[{\mathrm{\χ}}_{j,n+1|n}^{\′}-{\hat{x}}_{n+1}^{\′-}]{[Z_{j,n+1|n}^{\′}-{\hat{z}}_{n+1}^{\′-}]}^T$

自主天文导航***滤波增益K′_n+1为：

$K_{n+1}^{\′}=P_{{\hat{x}}_{n+1}{\hat{z}}_{n+1}}^{\′}{P}_{{\hat{z}}_{n+1}{\hat{z}}_{n+1}}^{\′-1}$

自主天文导航***的估计状态向量和估计均方误差阵P′_n+1为

${\hat{x}}_{n+1}^{\′}={\hat{x}}_{n+1}^{\′-}+K_{n+1}^{\′}(Z_{n+1}^{\′}-{\hat{z}}_{n+1}^{\′-})$

$P_{n+1}^{\′}=P_{n+1}^{\′-}-K_{n+1}^{\′}{P}_{{\hat{z}}_{n+1}{\hat{z}}_{n+1}}^{\′}{K}_{n+1}^{\′T}$

最终获得的第n+1次滤波时在火心惯性坐标系中的估计状态向量和估计均方误差阵P′_n+1输出，估计状态向量表示在火心惯性坐标系中探测器的位置、速度信息，输出的估计均方误差阵P′_n+1表示了滤波估计的性能，并将这些导航信息分别返回自主天文导航***中，用于第n+2次滤波时的位置、速度导航信息。