[发明专利]一种基于改进MMUPF滤波的火星探测器自主天文导航方法

权利要求书

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

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

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

状态方程为：

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

可简写为

令状态变量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为探测器三轴的速度坐标，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)建立以星光角距为量测量的量测方程；

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

可简写为：

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₀)的粒子 i＝1，2，…N，设置每个粒子的初始UPF权值 均设为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)；根据上述准则对每个粒子进行模型更新，将 更新为 

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

C.计算权值，计算归一化的UPF权值 和有效粒子尺寸 

D.重采样

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

E.结果输出

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