CN101672651A - 一种基于改进mmupf滤波的火星探测器自主天文导航方法 - Google Patents

Abstract

一种基于改进MMUPF滤波方法的火星探测器自主天文导航方法，涉及一种火星探测器的自主导航方法。该方法先对处于转移轨道上的深空探测器进行精确建模，然后以星光角距作为量测量，最后采用改进MMUPF滤波方法进行导航参数的最优估计，解决当距离不同时，行星引力大小和轨道动力学模型变化连续变化的问题，显著提高导航精度。可用于火星探测器或转移轨道上的行星探测器的导航参数的确定。

Description

一种基于改进MMUPF滤波的火星探测器自主天文导航方法

技术领域

本发明涉及航天导航技术领域，可应用火星探测器或其他绕行星飞行探测器导航参数的精确确定，特别涉及一种基于改进MMUPF滤波方法(改进多模型UPF滤波方法)的火星探测器自主天文导航方法，适用于转移轨道上的深空探测器的导航定位。

背景技术

行星探测器在其转移轨道运行中，所受的太阳及各行星引力的大小会随其位置的变化而变化，当距离某颗行星较远时，其引力可忽略不计，但当距离较近时，则行星的引力就会增大，并对探测器的轨道运动产生较大的影响，因此在行星探测器的轨道运动中，其轨道动力学模型不是一成不变的，而是连续变化的。

由于行星探测器在其转移轨道运行中动力学模型建立不准确，测量的量测量也存在的噪声，所以要采用滤波的方法，减小模型误差和量测噪声。

以往的滤波方法，如IMM和MMPF方法，由于行星探测器在其转移轨道运行中，轨道动力学模型连续变化，为了获得好的滤波效果，预先设置大量的子模型，且在滤波过程中保持不变，从而估计出导航参数，但大量的子模型滤波增加了计算量，直接影响了导航的实时性。

综上，由于行星探测器在转移轨道运行中的轨道动力学模型是连续变化，而目前所采用的滤波方法，要设置大量子模型，对应动力学模型连续变化的问题，但是由于是子模型之间的切换，并不是实时连续变化，也因此导航精度较低；由于设置了大量的子模型，导致计算量大，影响了导航的实时性。所以目前在转移轨道运行的深空探测器采用目前的滤波方法导航精度不高、实时性差。

发明内容

本发明要解决的技术问题是：克服现有滤波方法的精度低、计算量大的不足，提供一种精度高、自主性强的转移轨道上火星探测器自主天文导航方法。

本发明解决其技术问题所采用的技术方案为：一种基于改进MMUPF滤波方法的火星探测器自主天文导航方法，首先对处于转移轨道上的深空探测器进行精确建模，其中系统模型采用四体Singer模型，然后以星光角距作为量测量；最后采用一种新的滤波方法改进MMUPF滤波方法进行导航参数的最优估计，解决当距离不同时，行星引力大小和轨道动力学模型变化连续变化的问题，确定火星探测器或转移轨道上的行星探测器的导航参数，显著提高导航精度；

具体包括以下步骤：

1.建立基于轨道动力学的转移轨道上火星探测器导航系统状态方程：

${\stackrel{\·\·}{r}}_{\mathrm{ps}}=-{\mathrm{\μ}}_s\frac{r_{\mathrm{ps}}}{r_{\mathrm{ps}}^3}-{\mathrm{\μ}}_m[\frac{r_{\mathrm{pm}}}{r_{\mathrm{pm}}^3}-\frac{r_{\mathrm{sm}}}{r_{\mathrm{sm}}^3}]-{\mathrm{\μ}}_e[\frac{r_{\mathrm{pe}}}{r_{\mathrm{pe}}^3}-\frac{r_{\mathrm{se}}}{r_{\mathrm{se}}^3}]+a---\left(1\right)$

式中，μ_s、μ_m和μ_e分别为太阳、火星和地球的引力常数；r_se和r_sm分别为太阳相对地球和火星的位置矢量，r_ps，r_pe和r_pm分别为太阳、地球和火星的质心到探测器的位置矢量；a为可用一阶Singer模型表示的摄动力；

具体的状态方程为：

$\left\{\begin{array}{c}\mathrm{dx}/\mathrm{dt}=v_x+w_x\\ \mathrm{dy}/\mathrm{dt}=v_y+w_y\\ \mathrm{dz}/\mathrm{dt}=v_z+w_z\\ {\mathrm{dv}}_x/\mathrm{dt}=-{\mathrm{\μ}}_{s}x/r_{\mathrm{ps}}^3-{\mathrm{\μ}}_m[(x-x_1)/r_{\mathrm{pm}}^3-x_1/r_{\mathrm{sm}}^3]-{\mathrm{\μ}}_e[(x-x_2)/r_{\mathrm{pe}}^3-x_2/r_{\mathrm{se}}^3]+a_x+w_{v_x}\\ {\mathrm{dv}}_y/\mathrm{dt}=-{\mathrm{\μ}}_{s}y/r_{\mathrm{ps}}^3-{\mathrm{\μ}}_m[(y-y_1)/r_{\mathrm{pm}}^3-y_1/r_{\mathrm{sm}}^3]-{\mathrm{\μ}}_e[(y-y_2)/r_{\mathrm{pe}}^3-y_2/r_{\mathrm{se}}^3]+a_y+w_{v_y}\\ {\mathrm{dv}}_z/\mathrm{dt}=-{\mathrm{\μ}}_{s}z/r_{\mathrm{ps}}^3-{\mathrm{\μ}}_m[(z-z_1)/r_{\mathrm{pm}}^3-z_1/r_{\mathrm{sm}}^3]-{\mathrm{\μ}}_e[(z-z_2)/r_{\mathrm{pe}}^3-z_2/r_{\mathrm{se}}^3]+a_z+w_{v_z}\\ {\mathrm{da}}_x/\mathrm{dt}=-{\mathrm{\β}}_x{a}_x+w_{a_x}\\ {\mathrm{da}}_y/\mathrm{dt}=-{\mathrm{\β}}_y{a}_y+w_{a_y}\\ {\mathrm{da}}_z/\mathrm{dt}=-{\mathrm{\β}}_z{a}_z+w_{a_z}\end{array}\right.---\left(2\right)$

式中(x₁，y₁，z₁)，(x₂，y₂，z₂)和(x，y，z)分别为火星、地球和探测器的位置坐标，a_x，a_y，a_z为可用一阶Singer模型表示的三个方向的摄动力，β_x，β_y，β_z为时间相关常数的倒数；

可简写为

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

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

2.建立以星光角距为量测量的量测方程；

如图2所示，从火星探测器上观测到得导航恒星星光的矢量方向与太阳、地球和行星球心的矢量方向之间的夹角，表达式如下：

$\left\{\begin{array}{c}{\mathrm{\α}}_s=\arccos (-\frac{r_{\mathrm{ps}}\·s}{r_{\mathrm{ps}}})+v_{a_s}\\ {\mathrm{\α}}_e=\arccos (-\frac{r_{\mathrm{pe}}\·s}{r_{\mathrm{pe}}})+v_{a_e}\\ {\mathrm{\α}}_m=\arccos (-\frac{r_{\mathrm{pm}}\·s}{r_{\mathrm{pm}}})+v_{a_m}\end{array}\right.---\left(4\right)$

可简写为：

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

式中，Z＝[α_s，α_e，α_m]^T，测量噪声 $v={[v_{{\mathrm{\α}}_s},v_{{\mathrm{\α}}_e},v_{{\mathrm{\α}}_m}]}^T,$

是太阳、地球、火星与导航星之间星光角距的量测误差，α_s，α_e，α_m分别为太阳、地球、火星与导航星之间的星光角距，s为导航星在日心惯性坐标系中的位置矢量，r_ps，r_pe和r_pm分别为太阳、地球和火星的质心到探测器的位置矢量，h(X(t)，t)为非线性连续量测函数，α_s，α_e，α_m，

s，r_ps，r_pe，r_pm均为时间t的变量。

3.对以上状态方程及量测方程进行离散化；

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

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

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

4.采用改进MMUPF滤波算法，并输出导航信息。

用模型参数替代模型编号作为增补的状态量增补到状态变量中，首先对每个粒子进行模型更新，然后按照粒子滤波方法(UPF)对该粒子的状态(包含增补状态)进行状态更新，而后进行重采样，最后输出经MMUPF滤波后的导航参数。其中步骤4所用的改进MMUPF滤波算法，其实现步骤为：

①T＝0时，初始化X＝x₀，生成N个服从先验分布p(x₀)的粒子x₀ ⁱ，i＝1，2，…N，设置每个粒子的初始UPF权值w₀ ⁱ均设为1/N，i＝1，2，…N；其中N为大于10的整数，需满足导航精度和实时性的要求，通常为10~150时。

②T＝k时滤波过程

A.模型更新

根据先验概率密度p(x_s(k)|γ(k)，γ(k+1)，Z(k))采样，该先验概率密度可近似表示为扩维的粒子集{x_s(k)，x_M(k)，x_M(k+1|k)}；其中x_M(k+1|k)可由x_M(k)经过马尔科夫移动产生：根据模型编号为j的x_M(k+1|k)，计算满足条件 $\underset{i=1}{\overset{l-1}{\mathrm{\&Sigma;}}}{p}_{\mathrm{ij}}<s_l\&le;\underset{i=1}{\overset{l}{\mathrm{\&Sigma;}}}{p}_{\mathrm{ij}}$ 的模型编号l，使得模型编号为j的x_M(k+1|k)更新为模型编号是l的x_M(k+1|k)，其中j，l分别表示第j个模型和第l个模型；其中s_l为(0，1]间的服从均匀分布的随机数，代表了马尔科夫链的概率分布函数P(γ(k-1)≤γ_i|γ(k)＝γ_j)；根据上述准则对每个粒子进行模型更新，将x_M，k-1 ⁱ更新为x_M，k ⁱ；

B.状态更新，用UKF方法进行状态更新，包括时间更新和量测更新；

C.计算权值，计算归一化的UPF权值w_k ⁱ和有效粒子尺寸 $N_{\mathrm{eff}}=1/\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}{\left(w_k^i\right)}^2$

D.重采样

根据归一后的权重对粒子集重采样，使得重采样后的样本集

的近似分布为后验概率密度p(x_k|z_k)。并将权值w_k ⁱ重新置为1/N。

E.结果输出

式中，x_k+1为k+1时刻的状态估计值，包含导航所需的位置、速度、摄动加速度，P_k+1为k+1时刻的估计方差矩阵，

为第i个粒子经过模型更新、状态更新以及重采样后的状态估计，P_k+1 ⁱ为第i个粒子经过模型更新和状态更新后的状态估计方差，w_k+1 ⁱ为第i个粒子的UPF归一化权值。

本发明的原理是：由于行星探测器在其转移轨道运行中，所受的太阳及各行星引力的大小会随其位置的变化而变化，当距离某颗行星较远时，其引力可忽略不计，但当距离较近时，则行星的引力就会增大，并对探测器的轨道运动产生较大的影响，因此在行星探测器的轨道运动中，其轨道动力学模型不是一成不变的，而是连续变化的。改进MMUPF的基本思想是用一组带有不同参数值的滤波器的估计值通过加权得到对系统状态的最优估计，从而达到对于未知或不确定性系统参数的系统进行自适应的目的，设置多个子模型，用模型参数替代模型编号，建立利用模型参数作为状态变量的多模型状态方程，准确描述行星探测器在不同位置时的运动状态；由于利用模型参数替代模型编号，不必对子模型整体进行更新，只更新与状态变量有关的模型参数，节省了计算量，提高了导航精度，并且加入了基于星光角距的量测信息，如图2所示，s为导航星在日心惯性坐标系中的位置矢量，r_ps，r_pe和r_pm分别为太阳、地球和火星的质心到探测器的位置矢量，α_s，α_e，α_m分别为利用星敏感器获得的太阳、地球、火星与导航恒星之间的星光角距，利用UPF滤波方法对利用粒子滤波对该原状态变量和含有模型参数的增补状态进行更新，对状态进行最优估计，对系统进行自适应最优估计。

本发明与现有技术相比的优点在于：本发明所采用的改进MMUPF滤波方法与UPF、IMMUKF和传统的MMUPF滤波方法相比，不必对子模型整体进行更新，只更新与状态变量有关的模型参数，在计算量没有增加的基础上提高导航精度，在相同导航精度的前提下，实时性也有提高，实现转移轨道上火星探测器的实时准确定位。

附图说明

图1为本发明的流程图。

图2为本发明中的量测信息——星光角距示意图。

图3为本发明中的滤波方法——改进MMUPF原理图。

具体实施方式

如图1所示，本发明以火星探测器为例，具体实施方法如下：

建立基于轨道动力学的转移轨道上火星探测器导航系统状态方程。

根据天文导航系统的需要，采用四体Singer模型，该模型是在四体模型的基础上利用一阶Singer模型对其他摄动项进行建模。该模型(系统状态方程)在日心惯性坐标系(J2000.0)下，可以表示为：

${\stackrel{\&CenterDot;\&CenterDot;}{r}}_{\mathrm{ps}}=-{\mathrm{\&mu;}}_s\frac{r_{\mathrm{ps}}}{r_{\mathrm{ps}}^3}-{\mathrm{\&mu;}}_m[\frac{r_{\mathrm{pm}}}{r_{\mathrm{pm}}^3}-\frac{r_{\mathrm{sm}}}{r_{\mathrm{sm}}^3}]-{\mathrm{\&mu;}}_e[\frac{r_{\mathrm{pe}}}{r_{\mathrm{pe}}^3}-\frac{r_{\mathrm{se}}}{r_{\mathrm{se}}^3}]+a---\left(8\right)$

具体可以写为：

$\left\{\begin{array}{c}\mathrm{dx}/\mathrm{dt}=v_x+w_x\\ \mathrm{dy}/\mathrm{dt}=v_y+w_y\\ \mathrm{dz}/\mathrm{dt}=v_z+w_z\\ {\mathrm{dv}}_x/\mathrm{dt}=-{\mathrm{\&mu;}}_{s}x/r_{\mathrm{ps}}^3-{\mathrm{\&mu;}}_m[(x-x_1)/r_{\mathrm{pm}}^3-x_1/r_{\mathrm{sm}}^3]-{\mathrm{\&mu;}}_e[(x-x_2)/r_{\mathrm{pe}}^3-x_2/r_{\mathrm{se}}^3]+a_x+w_{v_x}\\ {\mathrm{dv}}_y/\mathrm{dt}=-{\mathrm{\&mu;}}_{s}y/r_{\mathrm{ps}}^3-{\mathrm{\&mu;}}_m[(y-y_1)/r_{\mathrm{pm}}^3-y_1/r_{\mathrm{sm}}^3]-{\mathrm{\&mu;}}_e[(y-y_2)/r_{\mathrm{pe}}^3-y_2/r_{\mathrm{se}}^3]+a_y+w_{v_y}\\ {\mathrm{dv}}_z/\mathrm{dt}=-{\mathrm{\&mu;}}_{s}z/r_{\mathrm{ps}}^3-{\mathrm{\&mu;}}_m[(z-z_1)/r_{\mathrm{pm}}^3-z_1/r_{\mathrm{sm}}^3]-{\mathrm{\&mu;}}_e[(z-z_2)/r_{\mathrm{pe}}^3-z_2/r_{\mathrm{se}}^3]+a_z+w_{v_z}\\ {\mathrm{da}}_x/\mathrm{dt}=-{\mathrm{\&beta;}}_x{a}_x+w_{a_x}\\ {\mathrm{da}}_y/\mathrm{dt}=-{\mathrm{\&beta;}}_y{a}_y+w_{a_y}\\ {\mathrm{da}}_z/\mathrm{dt}=-{\mathrm{\&beta;}}_z{a}_z+w_{a_z}\end{array}\right.---\left(9\right)$

式中，μ_s、μ_m和μ_e分别为太阳、火星和地球的引力常数；r_se和r_sm分别为地球和火星的位置矢量，r_ps，r_pe和r_pm分别为太阳、地球和火星的质心到探测器的位置矢量；(x₁，y₁，z₁)，(x₂，y₂，z₂)和(x，y，z)分别为火星、地球和探测器的位置。(v_x，v_y，v_z)是探测器的速度，a_x，a_y，a_z分别为可用一阶Singer模型表示的部分摄动项之和。β_x，β_y，β_z分别为时间相关常数的倒数，(dx/dt，dy/dt，dz/dt)为探测器位置的微分，即探测器的速度；(dv_x/dt，dv_y/dt，dv_z/dt)为探测器速度的微分，即探测器的加速度；(da_x/dt，da_y/dt，da_z/dt)为摄动项的微分，即摄动项对时间的变化率  a为可用一阶Singer模型表示的摄动力；

可简写为

$\stackrel{\&CenterDot;}{X}\left(t\right)=f(X\left(t\right),t)+w\left(t\right)---\left(10\right)$

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

2、建立以星光角距为量测量的量测方程。

从探测器上观测到得导航恒星星光的矢量方向与太阳、地球和行星球心的矢量方向之间的夹角，表达式如下：

$\left\{\begin{array}{c}{\mathrm{\&alpha;}}_s=\arccos (-\frac{r_{\mathrm{ps}}\&CenterDot;s}{r_{\mathrm{ps}}})+v_{a_s}\\ {\mathrm{\&alpha;}}_e=\arccos (-\frac{r_{\mathrm{pe}}\&CenterDot;s}{r_{\mathrm{pe}}})+v_{a_e}\\ {\mathrm{\&alpha;}}_m=\arccos (-\frac{r_{\mathrm{pm}}\&CenterDot;s}{r_{\mathrm{pm}}})+v_{a_m}\end{array}\right.---\left(11\right)$

式中，α_s，α_e，α_m分别为太阳、地球、火星与导航星之间的星光角距，s为导航星在日心惯性坐标系中的位置矢量，r_ps，r_pe和r_pm分别为太阳、地球和火星的质心到探测器的位置矢量，α_s，α_e，α_m，

-s，r_ps，r_pe，r_pm均为时间t的变量。

令Z＝[α_s，α_e，α_m]^T，测量噪声 $v={[v_{{\mathrm{\&alpha;}}_s},v_{{\mathrm{\&alpha;}}_e},v_{{\mathrm{\&alpha;}}_m}]}^T,$ 其中

是太阳、地球、火星与导航星之间星光角距的量测误差，则量测方程可表示为

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

式中，h(X(t)，t)为非线性连续量测函数。

3、对以上状态方程及两个量测方程进行离散化

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

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

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

4、采用改进MMUPF滤波算法，估计并输出导航信息。

本发明所采用的MMUPF方法用模型参数作为增补的状态量，即令X＝[x_s，x_M]，其中x_S为原始状态量，而x_M＝[γ₁，γ₂，…，γ_M]为模型参数，包括模型的模型误差和量测误差，这样k时刻的后验概率密度p(x_s(k)|γ(k)，Z(k))就可由一组粒子{x_s(k)，x_M(k)}表示，其中γ(k)代表模型参数。第j个模型的条件概率密度p(X_s(k)|γ(k)＝j，Z(k))就可近似为一个子粒子集{x_s(k)，x_M(k)＝j}中的粒子数。并对该增补状态通过粒子滤波的方法进行状态的更新，具体算法步骤如图3所示：

(1)T＝0时，初始化X＝x₀：

生成N个服从先验分布p(x₀)分布的粒子x₀ ⁱ，t＝1，…，N，其中N为大于10的整数，需满足导航精度和实时性的要求，通常为10~150时，粒子数N越大，导航精度越高，但是实时性受到限制，本实施例中，N＝20满足导航精度中的位置精度实现22km、速度精度实现0.8m/s。所选粒子均值x₀ ⁱ和方差P₀ ⁱ满足

$\left\{\begin{array}{c}{\stackrel{\&OverBar;}{x}}_0^i=E\left[x_0^i\right]\\ P_0^i=E\left[(x_0^i-{\stackrel{\&OverBar;}{x}}_0^i){(x_0^i-{\stackrel{\&OverBar;}{x}}_0^i)}^T\right]\end{array}\right.---\left(15\right)$

设置每个样本的初始UPF权值w₀ ⁱ均设为1/N，i＝1，2，…N。在初始先验分布p(x₀)未知的情况下，通常取为初始状态量x₀为中心的均匀分布或高斯分布，通常初始分布的选取对结果的影响较小。

(2)T＝k时滤波过程

①模型更新

根据先验概率密度p(x_s(k)|γ(k)，γ(k+1)，Z(k))采样，该先验概率密度可近似表示为扩维的粒子集{x_s(k)，x_M(k)，x_M(k+1|k)}；其中x_M(k+1|k)可由x_M(k)经过马尔科夫移动产生：如果x_M(k+1|k)的模型编号j，则当l满足 $\underset{i=1}{\overset{l-1}{\mathrm{\&Sigma;}}}{p}_{\mathrm{ij}}<s_l\&le;\underset{i=1}{\overset{l}{\mathrm{\&Sigma;}}}{p}_{\mathrm{ij}}$ 时，令x_M(k+1|k)为模型编号是l的x_M，其中j，l分别表示第j个模型和第l个模型；其中s_l为(0，1]间的服从均匀分布的随机数，

代表了马尔科夫链的概率分布函数P(γ(k-1)≤γ_l|γ(k)＝γ_j)；根据上述准则对每个粒子进行模型更新，将x_M，k ⁱ更新为x_M，k+1 ⁱ；

②状态更新

用UKF方法对第i个粒子 ${\hat{x}}_k^i=\{x_{S,k}^i,x_{M,k}^i\}$ 进行更新，具体算法如下：

a)采样

在第k时刻的第i个粒子

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

和P(k|k)。设状态变量为n×1维，那么第i个粒子对应的2n+1个样本点χ_0，k…χ_η+n，k及其UKF加权值W₀…W_η+n分别如下

${\mathrm{\&chi;}}_{0,k}^i={\hat{x}}_k^i,$ W₀＝τ/(n+τ)

${\mathrm{\&chi;}}_{\mathrm{\&eta;},k}^i={\hat{x}}_k^i+\sqrt{n+\mathrm{\&tau;}}{\left(\sqrt{P\left(k\right|k)}\right)}_{\mathrm{\&eta;}},$ W_η＝1/[2(n+τ)]    (16)

${\mathrm{\&chi;}}_{\mathrm{\&eta;}+n,k}^i={\hat{x}}_k^i-\sqrt{n+\mathrm{\&tau;}}{\left(\sqrt{P\left(k\right|k)}\right)}_{\mathrm{\&eta;}},$ W_η+n＝1/[2(n+τ)]

式中，η＝1，2，...，τ∈R；当P(k|k)＝A^TA时，

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

取A的第η列。

b)时间更新

第i个粒子的第Γ个采样点的状态量一步预测χ_k+1|k为

${\mathrm{\&chi;}}_{\mathrm{\&Gamma;},k+1|k}^i=F({\mathrm{\&chi;}}_{\mathrm{\&Gamma;},k}^i,k)---\left(17\right)$

由式(17)得第i个粒子的所有采样点状态量的一步预测加权后结果

为

${\hat{x}}_{k+1}^{i-}=\underset{\mathrm{\&Gamma;}=0}{\overset{2n}{\mathrm{\&Sigma;}}}{W}_{\mathrm{\&Gamma;}}^i{\mathrm{\&chi;}}_{\mathrm{\&Gamma;},k+1|k}^i---\left(18\right)$

式中，W_Γ ⁱ为第i个粒子的第Γ个采样点的权值；

由式(17)和式(18)得状态量的估计方差一步预测P_k+1 ^-为

$P_{k+1}^{i-}=\underset{\mathrm{\&Gamma;}=0}{\overset{2n}{\mathrm{\&Sigma;}}}{W}_{\mathrm{\&Gamma;}}^i[{\mathrm{\&chi;}}_{\mathrm{\&Gamma;},k+1|k}^i-{\hat{x}}_{k+1}^{i-}]{[{\mathrm{\&chi;}}_{\mathrm{\&Gamma;},k+1|k}^i-{\hat{x}}_{k+1}^{i-}]}^T+Q_{k+1}---\left(19\right)$

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

由式(17)可以得出第i个粒子的第Γ个采样点对应得量测估计值Z_Γ，k+1|k ⁱ

$Z_{\mathrm{\&Gamma;},k+1|k}^i=H({\mathrm{\&chi;}}_{\mathrm{\&Gamma;},k+1|k}^i,k)---\left(20\right)$

由式(20)可以得出第i个粒子的所有采样点量测估计加权值

${\hat{z}}_{k+1}^{i-}=\underset{\mathrm{\&Gamma;}=0}{\overset{2n}{\mathrm{\&Sigma;}}}{W}_{\mathrm{\&Gamma;}}^i{Z}_{\mathrm{\&Gamma;},k+1|k}^i---\left(21\right)$

c)量测更新

由式(20)、式(21)可得第i个粒子的量测方差阵

为：

$P_{{\hat{z}}_{k+1}{\hat{z}}_{k+1}}^i=\underset{\mathrm{\&Gamma;}=0}{\overset{2n}{\mathrm{\&Sigma;}}}{W}_{\mathrm{\&Gamma;}}^i[Z_{\mathrm{\&Gamma;},k+1|k}^i-{\hat{z}}_{k+1}^{i-}]{[Z_{\mathrm{\&Gamma;},k+1|k}^i-{\hat{z}}_{k+1}^{i-}]}^T+R_{k+1}---\left(22\right)$

式中，R_k+1分别为量测噪声协方差；

由式(17)、式(18)、式(20)、式(21)可得第i个粒子的状态变量量测量方差阵

$P_{{\hat{x}}_{k+1}{\hat{z}}_{k+1}}^i=\underset{\mathrm{\&Gamma;}=0}{\overset{2n}{\mathrm{\&Sigma;}}}{W}_{\mathrm{\&Gamma;}}^i[{\mathrm{\&chi;}}_{\mathrm{\&Gamma;},k+1|k}^i-{\hat{x}}_{k+1}^{i-}]{[Z_{\mathrm{\&Gamma;},k+1|k}^i-{\hat{z}}_{k+1}^{i-}]}^T---\left(23\right)$

由式(22)、式(23)第i个粒子的滤波增益K_k+1为：

$K_{k+1}^i=P_{{\hat{x}}_{k+1}{\hat{z}}_{k+1}}^i{\left(P_{{\hat{z}}_{k+1}{\hat{z}}_{k+1}}^i\right)}^{-1}---\left(24\right)$

第i个粒子状态量的估计值

和估计方差P_k+1为：

${\hat{x}}_{k+1}^i={\hat{x}}_{k+1}^{i-}+K_{k+1}^i(Z_{k+1}-{\hat{z}}_{k+1}^{i-})---\left(25\right)$

$P_{k+1}^i=P_{k+1}^{i-}-K_{k+1}^i{P}_{{\hat{z}}_{k+1}{\hat{z}}_{k+1}}^i{\left(K_{K+1}^i\right)}^T---\left(26\right)$

式中，Q_k+1和R_k+1分别为系统和量测噪声协方差。当x(k)假定为高斯分布时，通常选取n+τ＝3。

③计算权值

并计算归一化的UPF权值w_k ⁱ

$w_k^i={\tilde{w}}_k^i/\underset{j=1}{\overset{N}{\mathrm{\&Sigma;}}}{\tilde{w}}_k^i---\left(28\right)$

计算有效粒子尺寸N_eff

$N_{\mathrm{eff}}=1/\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}{\left(w_k^i\right)}^2---\left(29\right)$

如果N_eff小于既定门限，一般门限值取为2N/3，则执行步骤D，否则直接执行步骤E。

④重采样

根据归一后的权重对粒子集重采样，使得重采样后的样本集

的近似分布为后验概率密度p(x_k|z_k)。并将权值w_k ⁱ重新置为1/N。

⑤结果输出

式中，x_k+1为k+1时刻的状态估计值，包含导航所需的位置、速度、摄动加速度，P_k+1为k+1时刻的估计方差矩阵，

为第i个粒子经过模型更新、状态更新以及重采样后的状态估计，P_k+1 ⁱ为第i个粒子经过模型更新和状态更新后的状态估计方差，w_k+1 ⁱ为第i个粒子的UPF归一化权值。

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

Claims (2)

1、一种基于改进MMUPF滤波方法的火星探测器自主天文导航方法，其特征在于：对处于转移轨道上的深空探测器进行建模，其中系统模型采用四体Singer模型，然后以星光角距作为量测量；采用改进MMUPF滤波方法进行导航参数的最优估计，输出导航信息，具体包括以下步骤：

(1)建立基于轨道动力学的转移轨道上火星探测器导航系统状态方程；火星探测器在转移轨道上的状态方程采用四体Singer模型，该模型在J2000.0日心惯性坐标系下表示为

${\stackrel{\&CenterDot;\&CenterDot;}{r}}_{\mathrm{ps}}=-{\mathrm{\&mu;}}_s\frac{r_{\mathrm{ps}}}{r_{\mathrm{ps}}^3}-{\mathrm{\&mu;}}_m[\frac{r_{\mathrm{pm}}}{r_{\mathrm{pm}}^3}-\frac{r_{\mathrm{sm}}}{r_{\mathrm{sm}}^3}]-{\mathrm{\&mu;}}_e[\frac{r_{\mathrm{pe}}}{r_{\mathrm{pe}}^3}-\frac{r_{\mathrm{se}}}{r_{\mathrm{se}}^3}]+a$

式中，μ_s、μ_m和μ_e分别为太阳、火星和地球的引力常数；r_se和r_sm分别为地球和火星的位置矢量，r_ps，r_pe和r_pm分别为太阳、地球和火星的质心到探测器的位置矢量；a为可用一阶Singer模型表示的摄动力；

状态方程为：

$\left\{\begin{array}{c}\mathrm{dx}/\mathrm{dt}=v_x+w_x\\ \mathrm{dy}/\mathrm{dt}=v_y+w_y\\ \mathrm{dz}/\mathrm{dt}=v_z+w_z\\ {\mathrm{dv}}_x/\mathrm{dt}=-{\mathrm{\&mu;}}_{s}x/r_{\mathrm{ps}}^3-{\mathrm{\&mu;}}_m[(x-x_1)/r_{\mathrm{pm}}^3-x_1/r_{\mathrm{sm}}^3]-{\mathrm{\&mu;}}_e[(x-x_2)/r_{\mathrm{pe}}^3-x_2/r_{\mathrm{se}}^3]+a_x+w_{v_x}\\ {\mathrm{dv}}_y/\mathrm{dt}=-{\mathrm{\&mu;}}_{s}y/r_{\mathrm{ps}}^3-{\mathrm{\&mu;}}_m[(y-y_1)/r_{\mathrm{pm}}^3-y_1/r_{\mathrm{sm}}^3]-{\mathrm{\&mu;}}_e[(y-y_2)/r_{\mathrm{pe}}^3-y_2/r_{\mathrm{se}}^3]+a_y+w_{v_y}\\ {\mathrm{dv}}_z/\mathrm{dt}=-{\mathrm{\&mu;}}_{s}z/r_{\mathrm{ps}}^3-{\mathrm{\&mu;}}_m[(z-z_1)/r_{\mathrm{pm}}^3-z_1/r_{\mathrm{sm}}^3]-{\mathrm{\&mu;}}_e[(z-z_2)/r_{\mathrm{pe}}^3-z_2/r_{\mathrm{se}}^3]+a_z+w_{v_z}\\ {\mathrm{da}}_x/\mathrm{dt}=-{\mathrm{\&beta;}}_x{a}_x+w_{a_x}\\ {\mathrm{da}}_y/\mathrm{dt}=-{\mathrm{\&beta;}}_y{a}_y+w_{a_y}\\ {\mathrm{da}}_z/\mathrm{dt}=-{\mathrm{\&beta;}}_z{a}_z+w_{a_z}\end{array}\right.$

式中(x₁，y₁，z₁)，(x₂，y₂，z₂)和(x，y，z)分别为火星、地球和探测器的位置坐标，a_x，a_y，a_z为可用一阶Singer模型表示的三个方向的摄动力，β_x，β_y，β_z为时间相关常数的倒数；

可简写为

$\stackrel{\&CenterDot;}{X}\left(t\right)=f(X\left(t\right),t)+w\left(t\right)$

令状态变量X(t)＝[x，y，z，v_x，v_y，v_z，a_x，a_y，a_z]^T，其中x，y，z为探测器三轴的位置坐标，v_x，v_y，v_z为探测器三轴的速度坐标，a_x，a_y，a_z为探测器三轴所受摄动加速度，f(X(t)，t)为系统非线性连续状态转移函数，状态噪声为w(t)＝[w_x，w_y，w_z，w_vx，w_vy，w_vz，w_ax，w_ay，w_az]^T。

(2)建立以星光角距为量测量的量测方程；

以星光角距为量测量建立系统的量测方程为从火星探测器上观测到得导航恒星星光的矢量方向与太阳、地球和行星球心的矢量方向之间的夹角，表达式如下：

$\left\{\begin{array}{c}{\mathrm{\&alpha;}}_s=\arccos (-\frac{r_{\mathrm{ps}}\&CenterDot;s}{r_{\mathrm{ps}}})+v_{a_s}\\ {\mathrm{\&alpha;}}_e=\arccos (-\frac{r_{\mathrm{pe}}\&CenterDot;s}{r_{\mathrm{pe}}})+v_{a_e}\\ {\mathrm{\&alpha;}}_m=\arccos (-\frac{r_{\mathrm{pm}}\&CenterDot;s}{r_{\mathrm{pm}}})+v_{a_m}\end{array}\right.$

可简写为：

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

式中，Z(t)＝[α_s，α_e，α_m]^T，测量噪声

是太阳、地球、火星与导航星之间星光角距的量测误差，α_s，α_e，α_m分别为太阳、地球、火星与导航星之间的星光角距，s为导航星在日心惯性坐标系中的位置矢量，由星敏感器识别，h(X(t)，t)为非线性连续量测函数，α_s，α_e，α_m，s，r_ps，r_pe，r_pm均为时间t的变量。

(3)对以上状态方程及量测方程进行离散化；

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

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

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

(4)采用改进MMUPF滤波算法，估计并输出导航信息，用模型参数替代模型编号作为增补的状态量增补到状态变量中，其中模型参数包括模型的模型误差和量测误差，k时刻的后验概率密度可由一组粒子表示，对每个粒子进行模型更新，然后按照标准粒子滤波方法对该粒子的状态进行状态更新，而后进行重采样，最后输出经MMUPF滤波后的导航参数。

2、根据权利要求1所述的一种基于改进MMUPF滤波方法的火星探测器自主天文导航方法，其特征在于：所述步骤(4)所用的改进MMUPF滤波算法，其实现步骤为：

(1)T＝0时，初始化X＝x₀，生成N个服从先验分布p(x₀)的粒子x₀ ⁱ，i＝1，2，…N，设置每个粒子的初始UPF权值w₀ ⁱ均设为1/N，i＝1，2，…N；其中N为大于10的整数，需满足导航精度和实时性的要求，通常为10~150时。

(2)T＝k时滤波过程

A.模型更新

根据先验概率密度p(x_s(k)|γ(k)，γ(k+1)，Z(k))采样，该先验概率密度可近似表示为扩维的粒子集{x_s(k)，x_M(k)，x_M(k+1|k)}；其中x_M(k+1|k)可由x_M(k)经过马尔科夫移动产生：根据模型编号为j的x_M(k+1|k)，计算满足条件的模型编号l，使得模型编号为j的x_M(k+1|k)更新为模型编号是l的x_M(k+1|k)，其中j，l分别表示第j个模型和第l个模型；其中s_l为(0，1]间的服从均匀分布的随机数，

代表了马尔科夫链的概率分布函数P(γ(k-1)≤γ_l|γ(k)＝γ_j)；根据上述准则对每个粒子进行模型更新，将x_M，k-1 ⁱ更新为x_M，k ⁱ；

B.状态更新，用UKF方法进行状态更新，包括时间更新和量测更新；

C.计算权值，计算归一化的UPF权值w_k ⁱ和有效粒子尺寸 $N_{\mathrm{eff}}=1/\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}{\left(w_k^i\right)}^2$

D.重采样

根据归一后的权重对粒子集重采样，使得重采样后的样本集的近似分布为后验概率密度p(x_k|z_k)。并将权值w_k ⁱ重新置为1/N。

E.结果输出

式中，x_k+1为k+1时刻的状态估计值，包含导航所需的位置、速度、摄动加速度，P_k+1为k+1时刻的估计方差矩阵，为第i个粒子经过模型更新、状态更新以及重采样后的状态估计，P_k+1 ⁱ为第i个粒子经过模型更新和状态更新后的状态估计方差，w_k+1 ⁱ为第i个粒子的UPF归一化权值。

Images