专利名称：一种基于改进mmupf滤波的火星探测器自主天文导航方法
技术领域：
本发明涉及航天导航技术领域，可应用火星探测器或其他绕行星飞行探测器导航参数的精确确定，特别涉及一种基于改进MMUPF滤波方法(改进多模型UPF滤波方法)的火星探测器自主天文导航方法，适用于转移轨道上的深空探测器的导航定位。
背景技术：
行星探测器在其转移轨道运行中，所受的太阳及各行星引力的大小会随其位置的变化而变化，当距离某颗行星较远时，其引力可忽略不计，但当距离较近时，则行星的引力就会增大，并对探测器的轨道运动产生较大的影响，因此在行星探测器的轨道运动中，其轨道动力学模型不是一成不变的，而是连续变化的。
由于行星探测器在其转移轨道运行中动力学模型建立不准确，测量的量测量也存在的噪声，所以要采用滤波的方法，减小模型误差和量测噪声。
以往的滤波方法，如IMM和MMPF方法，由于行星探测器在其转移轨道运行中，轨道动力学模型连续变化，为了获得好的滤波效果，预先设置大量的子模型，且在滤波过程中保持不变，从而估计出导航参数，但大量的子模型滤波增加了计算量，直接影响了导航的实时性。
综上，由于行星探测器在转移轨道运行中的轨道动力学模型是连续变化，而目前所采用的滤波方法，要设置大量子模型，对应动力学模型连续变化的问题，但是由于是子模型之间的切换，并不是实时连续变化，也因此导航精度较低；由于设置了大量的子模型，导致计算量大，影响了导航的实时性。所以目前在转移轨道运行的深空探测器采用目前的滤波方法导航精度不高、实时性差。
发明内容
本发明要解决的技术问题是克服现有滤波方法的精度低、计算量大的不足，提供一种精度高、自主性强的转移轨道上火星探测器自主天文导航方法。
本发明解决其技术问题所采用的技术方案为一种基于改进MMUPF滤波方法的火星探测器自主天文导航方法，首先对处于转移轨道上的深空探测器进行精确建模，其中系统模型采用四体Singer模型，然后以星光角距作为量测量；最后采用一种新的滤波方法改进MMUPF滤波方法进行导航参数的最优估计，解决当距离不同时，行星引力大小和轨道动力学模型变化连续变化的问题，确定火星探测器或转移轨道上的行星探测器的导航参数，显著提高导航精度； 具体包括以下步骤 1.建立基于轨道动力学的转移轨道上火星探测器导航系统状态方程 式中，μs、μm和μe分别为太阳、火星和地球的引力常数；rse和rsm分别为太阳相对地球和火星的位置矢量，rps，rpe和rpm分别为太阳、地球和火星的质心到探测器的位置矢量；a为可用一阶Singer模型表示的摄动力； 具体的状态方程为 式中(x1，y1，z1)，(x2，y2，z2)和(x，y，z)分别为火星、地球和探测器的位置坐标，ax，ay，az为可用一阶Singer模型表示的三个方向的摄动力，βx，βy，βz为时间相关常数的倒数； 可简写为 状态变量为X＝[x y，z，vx，vy，vz，ax，ay，az]T，f(X(t)，t)为系统非线性连续状态转移函数，状态噪声为w＝[wx，wy，wz，wvx，wvy，wvz，wax，way，waz]T。
2.建立以星光角距为量测量的量测方程； 如图2所示，从火星探测器上观测到得导航恒星星光的矢量方向与太阳、地球和行星球心的矢量方向之间的夹角，表达式如下 可简写为 Z(t)＝h(X(t)，t)+v(t)(5) 式中，Z＝[αs，αe，αm]T，测量噪声
是太阳、地球、火星与导航星之间星光角距的量测误差，αs，αe，αm分别为太阳、地球、火星与导航星之间的星光角距，s为导航星在日心惯性坐标系中的位置矢量，rps，rpe和rpm分别为太阳、地球和火星的质心到探测器的位置矢量，h(X(t)，t)为非线性连续量测函数，αs，αe，αm，
s，rps，rpe，rpm均为时间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＝x0，生成N个服从先验分布p(x0)的粒子x0i，i＝1，2，…N，设置每个粒子的初始UPF权值w0i均设为1/N，i＝1，2，…N；其中N为大于10的整数，需满足导航精度和实时性的要求，通常为10~150时。
②T＝k时滤波过程 A.模型更新 根据先验概率密度p(xs(k)|γ(k)，γ(k+1)，Z(k))采样，该先验概率密度可近似表示为扩维的粒子集{xs(k)，xM(k)，xM(k+1|k)}；其中xM(k+1|k)可由xM(k)经过马尔科夫移动产生根据模型编号为j的xM(k+1|k)，计算满足条件的模型编号l，使得模型编号为j的xM(k+1|k)更新为模型编号是l的xM(k+1|k)，其中j，l分别表示第j个模型和第l个模型；其中sl为(0，1]间的服从均匀分布的随机数，
代表了马尔科夫链的概率分布函数P(γ(k-1)≤γi|γ(k)＝γj)；根据上述准则对每个粒子进行模型更新，将xM，k-1i更新为xM，ki； B.状态更新，用UKF方法进行状态更新，包括时间更新和量测更新； C.计算权值，计算归一化的UPF权值wki和有效粒子尺寸 D.重采样 根据归一后的权重对粒子集重采样，使得重采样后的样本集
的近似分布为后验概率密度p(xk|zk)。并将权值wki重新置为1/N。
E.结果输出
式中，xk+1为k+1时刻的状态估计值，包含导航所需的位置、速度、摄动加速度，Pk+1为k+1时刻的估计方差矩阵，
为第i个粒子经过模型更新、状态更新以及重采样后的状态估计，Pk+1i为第i个粒子经过模型更新和状态更新后的状态估计方差，wk+1i为第i个粒子的UPF归一化权值。
本发明的原理是由于行星探测器在其转移轨道运行中，所受的太阳及各行星引力的大小会随其位置的变化而变化，当距离某颗行星较远时，其引力可忽略不计，但当距离较近时，则行星的引力就会增大，并对探测器的轨道运动产生较大的影响，因此在行星探测器的轨道运动中，其轨道动力学模型不是一成不变的，而是连续变化的。改进MMUPF的基本思想是用一组带有不同参数值的滤波器的估计值通过加权得到对系统状态的最优估计，从而达到对于未知或不确定性系统参数的系统进行自适应的目的，设置多个子模型，用模型参数替代模型编号，建立利用模型参数作为状态变量的多模型状态方程，准确描述行星探测器在不同位置时的运动状态；由于利用模型参数替代模型编号，不必对子模型整体进行更新，只更新与状态变量有关的模型参数，节省了计算量，提高了导航精度，并且加入了基于星光角距的量测信息，如图2所示，s为导航星在日心惯性坐标系中的位置矢量，rps，rpe和rpm分别为太阳、地球和火星的质心到探测器的位置矢量，αs，αe，αm分别为利用星敏感器获得的太阳、地球、火星与导航恒星之间的星光角距，利用UPF滤波方法对利用粒子滤波对该原状态变量和含有模型参数的增补状态进行更新，对状态进行最优估计，对系统进行自适应最优估计。
本发明与现有技术相比的优点在于本发明所采用的改进MMUPF滤波方法与UPF、IMMUKF和传统的MMUPF滤波方法相比，不必对子模型整体进行更新，只更新与状态变量有关的模型参数，在计算量没有增加的基础上提高导航精度，在相同导航精度的前提下，实时性也有提高，实现转移轨道上火星探测器的实时准确定位。
图1为本发明的流程图。
图2为本发明中的量测信息——星光角距示意图。
图3为本发明中的滤波方法——改进MMUPF原理图。
具体实施例方式 如图1所示，本发明以火星探测器为例，具体实施方法如下 建立基于轨道动力学的转移轨道上火星探测器导航系统状态方程。
根据天文导航系统的需要，采用四体Singer模型，该模型是在四体模型的基础上利用一阶Singer模型对其他摄动项进行建模。该模型(系统状态方程)在日心惯性坐标系(J2000.0)下，可以表示为 具体可以写为 式中，μs、μm和μe分别为太阳、火星和地球的引力常数；rse和rsm分别为地球和火星的位置矢量，rps，rpe和rpm分别为太阳、地球和火星的质心到探测器的位置矢量；(x1，y1，z1)，(x2，y2，z2)和(x，y，z)分别为火星、地球和探测器的位置。(vx，vy，vz)是探测器的速度，ax，ay，az分别为可用一阶Singer模型表示的部分摄动项之和。βx，βy，βz分别为时间相关常数的倒数，(dx/dt，dy/dt，dz/dt)为探测器位置的微分，即探测器的速度；(dvx/dt，dvy/dt，dvz/dt)为探测器速度的微分，即探测器的加速度；(dax/dt，day/dt，daz/dt)为摄动项的微分，即摄动项对时间的变化率 a为可用一阶Singer模型表示的摄动力； 可简写为 状态变量为X＝[x，y，z，vx，vy，vz，ax，ay，az]T，f(X(t)，t)为系统非线性连续状态转移函数，状态噪声为w＝[wx，wy，wz，wvx，wvy，wvz，wax，way，waz]T。
2、建立以星光角距为量测量的量测方程。
从探测器上观测到得导航恒星星光的矢量方向与太阳、地球和行星球心的矢量方向之间的夹角，表达式如下 式中，αs，αe，αm分别为太阳、地球、火星与导航星之间的星光角距，s为导航星在日心惯性坐标系中的位置矢量，rps，rpe和rpm分别为太阳、地球和火星的质心到探测器的位置矢量，αs，αe，αm，
-s，rps，rpe，rpm均为时间t的变量。
令Z＝[αs，αe，α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＝[xs，xM]，其中xS为原始状态量，而xM＝[γ1，γ2，…，γM]为模型参数，包括模型的模型误差和量测误差，这样k时刻的后验概率密度p(xs(k)|γ(k)，Z(k))就可由一组粒子{xs(k)，xM(k)}表示，其中γ(k)代表模型参数。第j个模型的条件概率密度p(Xs(k)|γ(k)＝j，Z(k))就可近似为一个子粒子集{xs(k)，xM(k)＝j}中的粒子数。并对该增补状态通过粒子滤波的方法进行状态的更新，具体算法步骤如图3所示 (1)T＝0时，初始化X＝x0 生成N个服从先验分布p(x0)分布的粒子x0i，t＝1，…，N，其中N为大于10的整数，需满足导航精度和实时性的要求，通常为10~150时，粒子数N越大，导航精度越高，但是实时性受到限制，本实施例中，N＝20满足导航精度中的位置精度实现22km、速度精度实现0.8m/s。所选粒子均值x0i和方差P0i满足 设置每个样本的初始UPF权值w0i均设为1/N，i＝1，2，…N。在初始先验分布p(x0)未知的情况下，通常取为初始状态量x0为中心的均匀分布或高斯分布，通常初始分布的选取对结果的影响较小。
(2)T＝k时滤波过程 ①模型更新 根据先验概率密度p(xs(k)|γ(k)，γ(k+1)，Z(k))采样，该先验概率密度可近似表示为扩维的粒子集{xs(k)，xM(k)，xM(k+1|k)}；其中xM(k+1|k)可由xM(k)经过马尔科夫移动产生如果xM(k+1|k)的模型编号j，则当l满足时，令xM(k+1|k)为模型编号是l的xM，其中j，l分别表示第j个模型和第l个模型；其中sl为(0，1]间的服从均匀分布的随机数，
代表了马尔科夫链的概率分布函数P(γ(k-1)≤γl|γ(k)＝γj)；根据上述准则对每个粒子进行模型更新，将xM，ki更新为xM，k+1i； ②状态更新 用UKF方法对第i个粒子进行更新，具体算法如下 a)采样 在第k时刻的第i个粒子
附近选取一系列样本点，这些样本点的均值和协方差分别为
和P(k|k)。设状态变量为n×1维，那么第i个粒子对应的2n+1个样本点χ0，k…χη+n，k及其UKF加权值W0…Wη+n分别如下 W0＝τ/(n+τ) Wη＝1/[2(n+τ)](16) Wη+n＝1/[2(n+τ)] 式中，η＝1，2，...，τ∈R；当P(k|k)＝ATA时，
取A的第η行，当P(k|k)＝AAT时，
取A的第η列。
b)时间更新 第i个粒子的第Γ个采样点的状态量一步预测χk+1|k为 由式(17)得第i个粒子的所有采样点状态量的一步预测加权后结果
为 式中，WΓi为第i个粒子的第Γ个采样点的权值； 由式(17)和式(18)得状态量的估计方差一步预测Pk+1-为 式中，Qk+1为k+1时刻状态模型噪声协方差阵； 由式(17)可以得出第i个粒子的第Γ个采样点对应得量测估计值ZΓ，k+1|ki 由式(20)可以得出第i个粒子的所有采样点量测估计加权值
c)量测更新 由式(20)、式(21)可得第i个粒子的量测方差阵
为 式中，Rk+1分别为量测噪声协方差； 由式(17)、式(18)、式(20)、式(21)可得第i个粒子的状态变量量测量方差阵
由式(22)、式(23)第i个粒子的滤波增益Kk+1为 第i个粒子状态量的估计值
和估计方差Pk+1为 式中，Qk+1和Rk+1分别为系统和量测噪声协方差。当x(k)假定为高斯分布时，通常选取n+τ＝3。
③计算权值 并计算归一化的UPF权值wki
计算有效粒子尺寸Neff 如果Neff小于既定门限，一般门限值取为2N/3，则执行步骤D，否则直接执行步骤E。
④重采样 根据归一后的权重对粒子集重采样，使得重采样后的样本集
的近似分布为后验概率密度p(xk|zk)。并将权值wki重新置为1/N。
⑤结果输出
式中，xk+1为k+1时刻的状态估计值，包含导航所需的位置、速度、摄动加速度，Pk+1为k+1时刻的估计方差矩阵，
为第i个粒子经过模型更新、状态更新以及重采样后的状态估计，Pk+1i为第i个粒子经过模型更新和状态更新后的状态估计方差，wk+1i为第i个粒子的UPF归一化权值。
本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。
权利要求
1、一种基于改进MMUPF滤波方法的火星探测器自主天文导航方法，其特征在于对处于转移轨道上的深空探测器进行建模，其中系统模型采用四体Singer模型，然后以星光角距作为量测量；采用改进MMUPF滤波方法进行导航参数的最优估计，输出导航信息，具体包括以下步骤
(1)建立基于轨道动力学的转移轨道上火星探测器导航系统状态方程；火星探测器在转移轨道上的状态方程采用四体Singer模型，该模型在J2000.0日心惯性坐标系下表示为
式中，μs、μm和μe分别为太阳、火星和地球的引力常数；rse和rsm分别为地球和火星的位置矢量，rps，rpe和rpm分别为太阳、地球和火星的质心到探测器的位置矢量；a为可用一阶Singer模型表示的摄动力；
状态方程为
式中(x1，y1，z1)，(x2，y2，z2)和(x，y，z)分别为火星、地球和探测器的位置坐标，ax，ay，az为可用一阶Singer模型表示的三个方向的摄动力，βx，βy，βz为时间相关常数的倒数；
可简写为
令状态变量X(t)＝[x，y，z，vx，vy，vz，ax，ay，az]T，其中x，y，z为探测器三轴的位置坐标，vx，vy，vz为探测器三轴的速度坐标，ax，ay，az为探测器三轴所受摄动加速度，f(X(t)，t)为系统非线性连续状态转移函数，状态噪声为w(t)＝[wx，wy，wz，wvx，wvy，wvz，wax，way，waz]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，rps，rpe，rpm均为时间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＝x0，生成N个服从先验分布p(x0)的粒子x0i，i＝1，2，…N，设置每个粒子的初始UPF权值w0i均设为1/N，i＝1，2，…N；其中N为大于10的整数，需满足导航精度和实时性的要求，通常为10~150时。
(2)T＝k时滤波过程
A.模型更新
根据先验概率密度p(xs(k)|γ(k)，γ(k+1)，Z(k))采样，该先验概率密度可近似表示为扩维的粒子集{xs(k)，xM(k)，xM(k+1|k)}；其中xM(k+1|k)可由xM(k)经过马尔科夫移动产生根据模型编号为j的xM(k+1|k)，计算满足条件
的模型编号l，使得模型编号为j的xM(k+1|k)更新为模型编号是l的xM(k+1|k)，其中j，l分别表示第j个模型和第l个模型；其中sl为(0，1]间的服从均匀分布的随机数，
代表了马尔科夫链的概率分布函数P(γ(k-1)≤γl|γ(k)＝γj)；根据上述准则对每个粒子进行模型更新，将xM，k-1i更新为xM，ki；
B.状态更新，用UKF方法进行状态更新，包括时间更新和量测更新；
C.计算权值，计算归一化的UPF权值wki和有效粒子尺寸
D.重采样
根据归一后的权重对粒子集重采样，使得重采样后的样本集
的近似分布为后验概率密度p(xk|zk)。并将权值wki重新置为1/N。
E.结果输出
式中，xk+1为k+1时刻的状态估计值，包含导航所需的位置、速度、摄动加速度，Pk+1为k+1时刻的估计方差矩阵，
为第i个粒子经过模型更新、状态更新以及重采样后的状态估计，Pk+1i为第i个粒子经过模型更新和状态更新后的状态估计方差，wk+1i为第i个粒子的UPF归一化权值。
全文摘要
一种基于改进MMUPF滤波方法的火星探测器自主天文导航方法，涉及一种火星探测器的自主导航方法。该方法先对处于转移轨道上的深空探测器进行精确建模，然后以星光角距作为量测量，最后采用改进MMUPF滤波方法进行导航参数的最优估计，解决当距离不同时，行星引力大小和轨道动力学模型变化连续变化的问题，显著提高导航精度。可用于火星探测器或转移轨道上的行星探测器的导航参数的确定。
文档编号G01C21/20GK101672651SQ200910093148
公开日2010年3月17日 申请日期2009年9月25日 优先权日2009年9月25日
发明者宁晓琳, 吴伟仁, 辛 马 申请人:北京航空航天大学