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

权利要求书

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

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

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

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

式中，X′(t)＝[x′,y′,z′,v′_x,v′_y,v′_z]^T为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,......，其中X′(n)＝

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

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

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

Kstepi=F1(X′(n+astepih)+hΣstepj=0stepi-1bstepi,stepjKstepj,n+astepih,)]]>

式中，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，X′(n)＝′，h=h_n；

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

Xn+1′=Xn′+hΣstepi=010cstepiKstepi---(2)]]>

X~n+1′=Xn′+hΣstepi=012c~stepiKstepi---(3)]]>

式中，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，c~step0=0,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=0,c~step11=41/840,c~step12=41/840;]]>

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

Δ~Xn+1-X~n+1~41810(K0+K10-K11-K12)h---(4)]]>

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

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

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

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

χ0,n′=x^n′,W0′=-1]]>

χj,n′=x^n′+3(Pn′)j,Wj′=1/6]]>

χj+6,n′=x^n′+3(Pn′)j,Wj+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)

Kstepi=F1(χn+astepih′+hΣstepj=0stepi-1bstepi,stepjKstepj,n+astepih,)]]>

χn+1|n′=χn′+hΣstepi=010cstepiKstepi]]>

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

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

x^n+1′-=Σj=012Wj′χj,n+1|n′]]>

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

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

Pn+1′-=Σj=012Wj′[χj,n+1|n′-x^n+1′-][χj,n+1|n′-x^n+1′-]T+Qn+1′]]>

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

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

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

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

z^n+1′-=Σj=012Wj′Zj,n+1|n′]]>

E.量测更新；

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

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

Pz^n+1z^n+1′=Σj=012Wj′[Zj,n+1|n′-z^n+1′-][Zj,n+1|n′-z^n+1′-]TRn+1′]]>

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

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

Px^n+1z^n+1′=Σj=012Wj′[χj,n+1|n′-x^n+1′-][Zj,n+1|n′-z^n+1′-]T]]>

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

Kn+1′=Px^n+1z^n+1′Pz^n+1z^n+1′-1]]>

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

x^n+1′=x^n+1′-+Kn+1′(Zn+1′-z^n+1′-)]]>

Pn+1′=Pn+1′--Kn+1′Pz^n+1z^n+1′Kn+1′T]]>

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