CN101082494A - 一种基于预测滤波和upf航天器自标定方法 - Google Patents

Abstract

一种基于预测滤波和UPF航天器自标定方法，涉及航天导航技术领域，可应用航天器陀螺自标定，特别涉及一种基于预测滤波和UPF(UnscentedParticle Filter)信息融合的惯性/星光组合航天器自标定方法，进而适用于航天器的导航定姿。首先建立航天器自标定状态方程，然后以星敏感器观测得到的姿态信息作为量测量建立系统的量测方程，最后采用基于预测滤波和UPF航天器自标定算法估计并修正陀螺的漂移误差，得到高精确的航天器姿态。

Description

一种基于预测滤波和UPF航天器自标定方法

技术领域

本发明涉及航天导航技术领域，可应用航天器陀螺自标定，特别涉及一种基于预测滤波和UPF(Unscented Particle Filter，给出中文名称)信息融合的惯性/星光组合航天器自标定方法，进而适用于航天器的导航定姿。

背景技术

随着国民经济的发展，急需利用宽幅大范围卫星对海洋、环境、农林等资源进行高分辨率的对地观测与监控，目前国内外还都是利用幅宽窄、成本高的航天器来实现。为有效实现此任务必须突破高精度的航天器定姿技术。

航天器长期在空间飞行，其自主姿态确定非常重要，所以航天器上除装有各种姿态敏感器外，常装有自主定向的陀螺仪，由于航天器在空间长期飞行，陀螺仪的工作时间很长，由于存在陀螺的漂移误差，必须对其进行的标定和校准，以便补偿陀螺仪的漂移误差，进而提高空间机动平台的自主定姿精度。

由于航天器在空间飞行，处于失重状态，并且能源有限不能做大机动，不能采用地面的传统多位置标定方法进行航天器陀螺仪标定。目前现有的航天器陀螺仪标定主要有两种方式：一种是根据航天器姿态测量信息计算角速度，然后与陀螺仪输出比较，获得陀螺仪误差系数的估计；另一种是根据陀螺仪输出的角速度信息积分得到姿态信息，然后与姿态测量比较，由此获取陀螺仪误差系数的估计。两种方法没有建立精确的陀螺模型，算法简单精度较低，因此现有技术无法满足高精度航天器自标定的要求，也就不能提高航天器的定姿精度。

发明内容

本发明的技术解决问题是：克服现有技术的不足，提供一种基于预测滤波和UPF航天器自标定方法，可实现利用星敏感器观测信息进行航天器自标定，进而提高航天器姿态确定精度。

本发明的技术解决方案是：一种基于预测滤波和UPF航天器自标定方法，首先建立基于精确陀螺模型和姿态运动学的航天器自标定状态方程，然后以星敏感器观测得到的姿态信息作为量测量建立系统的量测方程，最后采用基于预测滤波和UPF航天器自标定算法估计并修正陀螺的漂移误差，进而得到航天器姿态。具体步骤为：

(1)建立基于精确陀螺模型和姿态运动学的航天器自标定状态方程；

(2)建立以星敏感器观测得到的姿态信息作为量测量建立系统的量测方程；

(3)采用预测滤波进行模型误差预测；

(4)采用UPF算法进行状态估计，完成航天器自标定，并输出导航信息。

本发明的原理是：惯性测量系统能够自主、连续、实时地精确测量航天器的位置、速度和姿态，但是由于陀螺漂移误差的存在，导致惯性测量系统的姿态误差随时间积累，无法满足航天器长时间在轨运行的要求。星敏感器以天空中的天体作为信息源，能提供精度高且不随时间漂移的姿态信息。将惯性测量系统与天体敏感器组合，利用天体敏感器提供的姿态信息，不仅可以修正惯性测量系统的姿态误差，还可以在线标定出陀螺的漂移误差，提供长期连续输出高精度姿态信息。利用高精度的航天器自标定模型，结合星敏感器观测确定的航天器的三轴姿态信息。并采用预测滤波和UPF滤波算法，实现航天器自标定。

本发明与现有技术相比的优点在于：

(1)克服了现有航天器自标定模型采用的陀螺模型不准确的不足，建立具有精确陀螺模型的航天器自标定模型；

(2)采用UPF滤波算法，克服了EKF和UKF只能应用于系统噪声为高斯白噪声的不足，适合于非线性、噪声非告斯的系统模型，并采用预测滤波实时修正系统模型，能够取得比EKF和UKF更快的滤波收敛性和更高的滤波精度。

附图说明

图1为本发明的一种基于预测滤波和UPF航天器自标定方法的流程图；

具体实施方式

如图1所示，本发明具体实现步骤如下：

1、建立基于姿态运动学的航天器自标定状态方程：

(1)陀螺量测模型

考虑陀螺误差源对陀螺的漂移误差影响，建立精确的陀螺模型，将陀螺误差源(如：刻度因子误差，不对准误差，陀螺常值漂移，陀螺输出噪声)建入陀螺模型得：

ω_g＝ω+b+g_sf+g_ma+n_a             (1)

式中，ω_g为陀螺测量值，ω为陀螺真值，b为陀螺的常值漂移，g_sf为陀螺刻度因数误差，g_ma陀螺不对准误差，n_a为陀螺输出噪声。

g_sf＝diag([g_sfx g_sfy g_sfz])·ω    (2)

$g_{\mathrm{ma}}=\left[\begin{array}{c}{g}_{\max }\\ g_{\mathrm{may}}\\ g_{\mathrm{maz}}\end{array}\right]=\left[\begin{array}{ccc}0& g_{\mathrm{xy}}& g_{\mathrm{xz}}\\ g_{\mathrm{yx}}& 0& g_{\mathrm{yz}}\\ g_{\mathrm{zx}}& g_{\mathrm{zy}}& 0\end{array}\right]\·\begin{array}{c}\left[\begin{array}{c}{\mathrm{\ω}}_x\\ {\mathrm{\ω}}_y\\ {\mathrm{\ω}}_z\end{array}\right]\end{array}---\left(3\right)$

其中：ω_x，ω_y，ω_z为ω的分量形式。g_sfx，g_sfy，g_sfz为g_sf的对应分量，g_max，g_may，g_maz，g_xy，g_xz，g_yz，g_yx，g_zx，g_zy为g_ma的对应分量。

将式(2)和式(3)代入式(1)得：

$\left\{\begin{array}{c}{\mathrm{\ω}}_{\mathrm{gx}}={\mathrm{\ω}}_x+b_x+g_{\mathrm{sfx}}\·{\mathrm{\ω}}_x+g_{\mathrm{xy}}\·{\mathrm{\ω}}_y+g_{\mathrm{xz}}\·{\mathrm{\ω}}_z+n_{\mathrm{ax}}\\ {\mathrm{\ω}}_{\mathrm{gy}}={\mathrm{\ω}}_y+b_y+g_{\mathrm{sfy}}\·{\mathrm{\ω}}_y+g_{\mathrm{yx}}\·{\mathrm{\ω}}_x+g_{\mathrm{yz}}\·{\mathrm{\ω}}_z+n_{\mathrm{ay}}\\ {\mathrm{\ω}}_{\mathrm{gz}}={\mathrm{\ω}}_z+b_z+g_{\mathrm{sfz}}\·{\mathrm{\ω}}_z+g_{\mathrm{zx}}\·{\mathrm{\ω}}_x+g_{\mathrm{zy}}\·{\mathrm{\ω}}_y+n_{\mathrm{az}}\end{array}\right.---\left(4\right)$

其中：ω_gx，ω_gy，_gz，为ω_g的分量形式。

陀螺常值漂移b用一个随机游走过程表示： $\stackrel{\·}{b}=n_b,$ n_b是陀螺随机游走噪声。由于航天器飞行时间长，环境变化复杂，陀螺刻度因数误差g_sf和不正交误差g_ma采用指数相关随机过程表示，设为一阶马尔可夫过程过程， ${\stackrel{\·}{g}}_{\mathrm{sf}}=-\frac{1}{{\mathrm{\τ}}_{\mathrm{sf}}}{g}_{\mathrm{sf}}+n_{\mathrm{sf}},$ ${\stackrel{\·}{g}}_{\mathrm{ma}}=-\frac{1}{{\mathrm{\τ}}_{\mathrm{ma}}}{g}_{\mathrm{ma}}+n_{\mathrm{ma}},$ τ_sf和τ_ma为时间相关常数，n_a、n_b、n_sf和n_ma为系统模型噪声且互不相关。

(2)航天器姿态运动学方程

航天器的姿态用本体坐标系相对于惯性坐标系的四元数q表示，定义为：

q=[q，e]^T    (5)

式中，e=[e₁，e₂，e₃]为四元数矢量部分，g为四元数标量部分。由四元数所表示的星体运动学方程为：

$\stackrel{\·}{q}=\frac{1}{2}\mathrm{\Ω}\left(\mathrm{\ω}\right)$ $q=\frac{1}{2}q\⊗\mathrm{\ω}---\left(6\right)$

式中，ω＝[ω_x，ω_y，ω_z]^T是星体坐标系相对于惯性坐标系的转动角速度， $\mathrm{\Ω}\left(\mathrm{\ω}\right)=\left[\begin{array}{cc}-\left[\mathrm{\ωx}\right]& \mathrm{\ω}\\ {-\mathrm{\ω}}^T& 0\end{array}\right],$ $\left[\mathrm{\ωx}\right]=\left[\begin{array}{ccc}0& -{\mathrm{\ω}}_z& {\mathrm{\ω}}_y\\ {\mathrm{\ω}}_z& 0& -{\mathrm{\ω}}_x\\ -{\mathrm{\ω}}_y& {\mathrm{\ω}}_x& 0\end{array}\right].$

(3)系统状态方程

四元数存在范数约束，如果选择四元数的四个分量作为状态变量，则方差阵是奇异的，这种奇异性随着方差递推的进行将无法得到保证。为了避开方差阵奇异的问题，采用乘性四元数定义真实四元数q与四元数计算值

之间的误差四元数为状态变量：

$\mathrm{\δq}={\hat{q}}^{-1}\⊗q={[\mathrm{\δq},\mathrm{\δe}]}^T---\left(7\right)$

取误差四元数的矢量部分δe和陀螺漂移估计误差Δb、Δg_sf、Δg_ma为误差状态变量：ΔX＝[δe，Δb，Δg_sf，Δg_ma]^T，Δb、Δg_sf、Δg_ma为真实陀螺的漂移误差b，g_sf，g_ma与陀螺的漂移误差计算值

之间的误差。由四元数运动学方程有：

$\stackrel{\·}{q}=\frac{1}{2}q\⊗\mathrm{\ω}---\left(8\right)$

$\stackrel{\stackrel{\·}{^}}{q}=\frac{1}{2}\hat{q}\⊗\widehat{\mathrm{\ω}}---\left(9\right)$

对式(8)求导，并将式(9)和式(10)代入得：

$\mathrm{\δ}\stackrel{\·}{q}=\frac{1}{2}[\mathrm{\δq}\⊗\mathrm{\ω}-\widehat{\mathrm{\ω}}\⊗\mathrm{\δq}]---\left(10\right)$

其中δω定义为：

$\mathrm{\δ\ω}=[\mathrm{\ω}-\stackrel{\‾}{\mathrm{\ω}}]=[(b-\hat{b})+(g_{\mathrm{sf}}-{\hat{g}}_{\mathrm{sf}})+(g_{\mathrm{ma}}-{\hat{g}}_{\mathrm{ma}})+n_a]---\left(11\right)$

将式(11)代入(10)得：

$\mathrm{\δ}\stackrel{\·}{q}=\frac{1}{2}[\mathrm{\δq}\⊗\widehat{\mathrm{\ω}}-\widehat{\mathrm{\ω}}\⊗\mathrm{\δq}]+\frac{1}{2}\⊗\mathrm{\δ\ω}---\left(12\right)$

将式(12)线性化得：

$\mathrm{\δ}\stackrel{\·}{q}=-\widehat{\mathrm{\ω}}\×\mathrm{\δe}-\frac{1}{2}(\mathrm{\Δb}+\mathrm{\Δ}{g}_{\mathrm{sf}}+\mathrm{\Δ}{g}_{\mathrm{ma}}+n_a)---\left(13\right)$

则误差变量状态方程为：

$\mathrm{\Δ}\stackrel{\·}{X}\left(t\right)=F\left(t\right)\mathrm{\ΔX}+G\left(t\right)\mathrm{\ϵ}\left(t\right)---\left(14\right)$

式中： $F\left(t\right)=\left[\begin{array}{cccc}-[\mathrm{\ω}\left(t\right)\×]& -\frac{1}{2}{I}_{3\×3}& \frac{1}{2}{F}_{\mathrm{sf}}& \frac{1}{2}{F}_{\mathrm{ma}}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×6}\\ 0_{3\×3}& 0_{3\×3}& -\frac{1}{{\mathrm{\τ}}_{\mathrm{sf}}}{I}_{3\×3}& 0_{3\×6}\\ 0_{6\×3}& 0_{6\×3}& 0_{6\×3}& -\frac{1}{{\mathrm{\τ}}_{\mathrm{ma}}}{I}_{6\×6}\end{array}\right],$

$G\left(t\right)=\left[\begin{array}{ccccc}-\frac{1}{2}{I}_{3\×3}& o_{3\×3}& o_{3\×3}& o_{3\×3}& o_{3\×3}\\ o_{3\×3}& I_{3\×3}& o_{3\×3}& o_{3\×3}& o_{3\×3}\\ o_{3\×3}& o_{3\×3}& I_{3\×3}& o_{3\×3}& o_{3\×3}\\ o_{3\×3}& o_{3\×3}& o_{3\×3}& I_{3\×3}& o_{3\×3}\\ o_{3\×3}& o_{3\×3}& o_{3\×3}& o_{3\×3}& I_{3\×3}\end{array}\right],$ $\mathrm{\ϵ}\left(t\right)=\left[\begin{array}{c}{n}_a\\ n_b\\ n_{\mathrm{sf}}\\ n_{\mathrm{ma}}\end{array}\right],$

$F_{\mathrm{sf}}=\left[\begin{array}{ccc}-{\mathrm{\ω}}_x& 0& 0\\ 0& -{\mathrm{\ω}}_y& 0\\ 0& 0& -{\mathrm{\ω}}_z\end{array}\right],$ $F_{\mathrm{ma}}=\left[\begin{array}{cccccc}{-\mathrm{\ω}}_y& -{\mathrm{\ω}}_z& 0& 0& 0& 0\\ 0& 0& {-\mathrm{\ω}}_x& -{\mathrm{\ω}}_z& 0& 0\\ 0& 0& 0& 0& {-\mathrm{\ω}}_x& -{\mathrm{\ω}}_y\end{array}\right]$

2、建立以星敏感器观测得到的姿态信息作为量测量的量测方程：

星敏感器根据观测的星光矢量信息确定卫星本体坐标系相对于惯性坐标系的姿态四元数q，定义星敏感器输出的四元数q与计算四元数

之间的误差四元数为Δq，则Δq表达为：

$\mathrm{\Δq}=[\mathrm{\Δq},\mathrm{\Δe}]={\hat{q}}^{-1}\⊗q---\left(15\right)$

取Δq的矢量部分Δe作为观测量，将式(15)线性化，得到线性化的误差量测方程：

Δe＝δe+v＝HΔX+v＝[I_3×3 O_3×12]^TΔX+v    (16)

式中：H为量测矩阵，v为量测噪声，ΔX为状态变量。

3、采用预测滤波进行模型误差预测：

非线性预测滤波的基本思想是通过比较量测输出与预测输出来估计对应的模型误差，从而修正滤波器状态，实现对真实状态量的估计。

将前面得到的系统状态方程和量测方程改写为如下形式：

$\stackrel{\·}{x}\left(t\right)=f(x\left(t\right),t)+G(x\left(t\right),t)D\left(t\right)---\left(17\right)$

z(t)＝h(x(t)，t)+v(t)                (18)

其中，f∈Rⁿ是连续可微的非线性函数，x(t)∈Rⁿ是状态变量，D(t)∈R^q是模型误差向量，G∈R^n×q是模型误差分布矩阵。z(t)∈R^m是量测向量，v(t)是量测噪声向量，并且假定其为零均值的高斯白噪声，协方差为E{v(t)v^T(t)}＝R。

状态估计和预测输出估计的关系如下：

$\stackrel{\stackrel{\·}{^}}{x}\left(t\right)=f(\hat{x}\left(t\right),t)+G(\hat{x}\left(t\right),t)D\left(t\right)---\left(19\right)$

$\hat{z}\left(t\right)=h(\hat{x}\left(t\right),t)---\left(20\right)$

记 的第i个分量为h_i，i＝1，...m。对式(20)进行连续微分，并将式

的表达式带入等式右端，记D(t)的任何分量第一次出现在h_i的微分中的最低阶数为p_i。取小的时间间隔Δt，将式(20)的第i个分量近似展开成p_i阶泰勒级数，并忽略高阶项，得到：

${\hat{z}}_i(t+\mathrm{\Δt})\≈{\hat{z}}_i\left(t\right)+\mathrm{\Δt}\frac{\∂{\hat{z}}_i\left(t\right)}{\∂t}+\frac{\mathrm{\Δt}}{2!}\frac{{\∂}^2{\hat{z}}_i\left(t\right)}{{\∂t}^2}+\·\·\·+\frac{{\mathrm{\Δt}}^{p_i}}{p_i!}\frac{{\∂}^{p_i}{\hat{z}}_i\left(t\right)}{{\∂t}^{p_i}}$

$={\hat{z}}_i\left(t\right)+\mathrm{\Δt}\frac{{\∂h}_i}{\∂\hat{x}}[f(\hat{x}\left(t\right),t)+G(\hat{x}\left(t\right),t)D\left(t\right)]+\frac{\mathrm{\Δt}}{2!}\frac{\∂}{\∂t}\left(\frac{{\∂h}_i}{\∂t}\right)+\·\·\·+\frac{\mathrm{\Δ}{t}^{p_i}}{p_i!}\frac{\∂}{\∂t}\left(\frac{{\∂}^{p_i-1}{h}_i}{{\∂t}^{p_i-1}}\right)---\left(21\right)$

根据李导数定义k阶李导数L_f ^k(h_i)：

$L_f^0\left(h_i\right)=h_i$

$L_f^k\left(h_i\right)=\frac{\∂L_f^{k-1}\left(h_i\right)}{\∂\hat{x}}f(\hat{x}\left(t\right),t),k\≥1---\left(22\right)$

由于D(t)的任何分量第一次在出现在h_i的微分中的最低阶数为p_i，因此当微分阶数低于p_i时， $\frac{\∂h_i}{\∂\hat{x}}G(\hat{x}\left(t\right),t)D\left(t\right)=0,$ 从而根据李导数定义，式(21)可以写为：

${\hat{z}}_i(t+\mathrm{\Δt})\≈{\hat{z}}_i\left(t\right)+\mathrm{\Δt}{L}_f^i\left(h_i\right)+\frac{\mathrm{\Δt}}{2!}{L}_f^2\left(h_i\right)+\·\·\·+\frac{\mathrm{\Δ}{t}^{p_i}}{p_i!}{L}_f^{p_i}\left(h_i\right)+\frac{\mathrm{\Δ}{t}^{p_i}}{p_i!}\frac{\∂L_f^{p_i-1}}{\∂\hat{x}}G(\hat{x}\left(t\right),t)D\left(t\right)---\left(23\right)$

分别求出i＝1，...m时

的形如式(23)的泰勒展开式，并将其合写成矩阵形式，可以得到

$\hat{z}(t+\mathrm{\Δt})\≈\hat{z}\left(t\right)+S(\hat{x}\left(t\right),\mathrm{\Δt})+\mathrm{\Λ}\left(\mathrm{\Δt}\right)U\left(\hat{x}\left(t\right)\right)D\left(t\right)---\left(24\right)$

其中，Λ(Δt)∈R^m×m为对角阵，其对角元素为：

${\mathrm{\λ}}_{\mathrm{ii}}=\frac{\mathrm{\Δ}{t}^{p_i}}{p_i!},i=1,...,m---\left(25\right)$

标量函数L_f ^k(h_i)关于向量场

的一阶李导数记为L_gjL^k _f(h_i)：

$L_{g_i}{L}_f^k\left(h_i\right)=\frac{\∂L_f^k\left(h_i\right)}{\∂\hat{x}}{g}_j(\hat{x}\left(t\right),t)---\left(26\right)$

$U\left(\hat{x}\left(t\right)\right)\∈R^{m\×q}$ 是灵敏度矩阵，由式(26)

可以表示为：

$U\left(\hat{x}\left(t\right)\right)=\left[\begin{array}{ccc}{L}_{g_1}{L}_f^{p_1-1}\left(h_1\right)& \·\·\·& L_{g_q}{L}_f^{p_1-1}\left(h_1\right)\\ L_{g_1}{L}_f^{p_2-1}\left(h_2\right)& \·\·\·& L_{g_q}{L}_f^{p_2-1}\left(h_2\right)\\ \·\·\·& \·\·\·& \·\·\·\\ L_{g_1}{L}_f^{p_m-1}\left(h_m\right)& \·\·\·& L_{g_q}{L}_f^{p_m-1}\left(h_m\right)\end{array}\right]---\left(27\right)$

g_j为G的第j列，j＝1，...q。

为q维列向量，其各分量为：

$S_i(\hat{x}\left(t\right),\mathrm{\Δt})=\underset{k=1}{\overset{p_i}{\mathrm{\Σ}}}\frac{{\mathrm{\Δt}}^k}{k!}{L}_f^k\left(h_i\right),i=1,...q---\left(28\right)$

预测滤波器的工作原理是用滤波器的预测输出跟踪量测输出，从而估计出模型误差。因此定义性能指标函数：

$J\left(D\left(t\right)\right)=\frac{1}{2}{[z(t+\mathrm{\Δt})-\hat{z}(t+\mathrm{\Δt})]}^T{R}^{-1}[z(t+\mathrm{\Δt})-\hat{z}(t+\mathrm{\Δt})]+\frac{1}{2}{D}^T\left(t\right)\mathrm{WD}\left(t\right)---\left(29\right)$

它是由量测输出与预测输出间残差的加权平方和以及模型修正项的加权平方和组成。式中，W∈R^q×q是模型误差加权矩阵。W的取值对估计效果的影响很大，当模型误差较大时，W的值不应过大；当模型误差较小时，W的值可以大一些。

假定小的时间间隔为常值，则z(t)＝z_k，z(t+Δt)＝z_k+1为了使J最小，需要满足条件J/D_k＝0，由此可以得到[t_k，t_k+1]时间区间内的模型误差估计：

${\hat{D}}_k=-{\left\{{\left[\mathrm{\Λ}\left(\mathrm{\Δt}\right)U\left({\hat{x}}_k\right)\right]}^T{R}^{-1}\right[\mathrm{\Λ}\left(\mathrm{\Δt}\right)U\left({\hat{x}}_k\right)]+W\}}^{-1}---\left(30\right)$

$\·{\left[\mathrm{\Λ}\left(\mathrm{\Δt}\right)U\left({\hat{x}}_k\right)\right]}^T{R}^{-1}[S({\hat{x}}_k,\mathrm{\Δt})-z_{k+1}+{\hat{z}}_k]$

令 $M_k={\left\{{\left[\mathrm{\Λ}\left(\mathrm{\Δt}\right)U\left({\hat{x}}_k\right)\right]}^T{R}^{-1}\right[\mathrm{\Λ}\left(\mathrm{\Δt}\right)U\left({\hat{x}}_k\right)+W\}}^{-1}\·{\left[\mathrm{\Λ}\left(\mathrm{\Δt}\right)U\left({\hat{x}}_k\right)\right]}^T{R}^{-1},$ 则上式可写成：

${\hat{D}}_k=-M_k[S({\hat{x}}_k,\mathrm{\Δt})-z_{k+1}+{\hat{z}}_k]---\left(31\right)$

4、UPF算法进行状态估计完成航天器自标定：

粒子滤波(PF)是递推贝叶斯滤波器的另一种实现形式，其基本思想是用随机样本来描述概率分布，这些样本被称为“粒子”，然后在测量的基础上，通过调节各粒子权值的大小和样本的位置，来近似实际概率分布，并以样本的均值作为系统的估计值，原则上可用于任意非线性非高斯随机系统的状态估计。但该方法的缺点是存在退化现象，消除退化可依赖于适当选取重要密度函数。

UPF算法就是利用UKF来得到粒子重要密度函数的一种粒子滤波方法，由于该重要密度函数中包含了最新量测信息，因此具有更好的性能。深空探测器导航系统实际上是非线性、噪声非高斯分别的系统，因此UPF方法是比较适用的一种滤波方法。首先根据前一时刻粒子及其方差确定一组西格马点，此点集的位置和权值是由粒子的期望和方差唯一确定的，能较准确地抓住粒子概率分布的特征。然后将这些点分别代入状态方程，得到一新点集，用这些点集的加权和作为期望，用其方差的加权和作为方差，然后再用测量方程对已经求得的期望和方差进行修正，并用修正后的值作为高斯分布的期望和方差，产生一个当前时刻的粒子。因为其充分考虑当前量测对后验概率分布的影响，提高了粒子的利用效率。具体步骤如下：

系统的状态方程和量测方程可由式(17)和式(18)得到，且x_k＝f(x_k-1，w_k-1)p(x_k，x_k-1)，z_k＝f(x_k-1，v_k)p(z_k，x_k)

①初始化

T＝0时，对于i＝1，......，N，生成服从先验分布q(x₀)的N个采样粒子{x₀ ¹，x₀ ²，…，x₀ ⁿ}，并将每个样本的初始权值ω₀ ⁱ均设为1/N。

${\stackrel{\‾}{x}}_0^i=E\left[x_0^i\right]$

$P_0^i=E\left[(x_0^i-{\stackrel{\‾}{x}}_0^i){(x_0^i-{\stackrel{\‾}{x}}_0^i)}^T\right]$

②对于T＝1，2，......，T＝k时的滤波过程如下：

(a)重要采样

对于N个粒子分别用UKF方法更新，由{x_k-1 ⁱ，P_k-1 ⁱ}得到

并采集一个新的采样点 使得 ${\hat{x}}_k^i~q\left(x_k^i\right|x_{k-1}^i,Z_k)=N({\stackrel{\‾}{x}}_k^i,{\stackrel{\‾}{P}}_k^i).$

根据样本的相似程度更新权值，使得 ${\stackrel{\‾}{\mathrm{\ω}}}_k^i={\mathrm{\ω}}_{k-1}^i\frac{p\left(Z_k\right|{\hat{x}}_k^i)p\left({\hat{x}}_k^i\right|x_{k-1}^i)}{q\left({\hat{x}}_k^i\right|x_{k-1}^i,Z_{k-1})}.$

然后对每个粒子的权值进行归一化，即令 ${\mathrm{\ω}}_k^i=\frac{1}{{\mathrm{\Σ}}_{j=1}^N{\stackrel{\‾}{\mathrm{\ω}}}_k^i}.$

最后计算有效粒子的尺寸N_eff， $N_{\mathrm{eff}}=\frac{1}{{\mathrm{\Σ}}_{i=1}^N{\left({\mathrm{\ω}}_k^i\right)}^2}.$ 如果N_eff小于门限值N_th，说明粒子多样性降低，需转入步骤(b)进行重采样，否则转入步骤(c)输出该次的滤波结果。

(b)重采样

重采样的目的是消除权值较小的粒子，增加权值较大的粒子，使重采样后的样本集的分布符合后验密度p(x_k|Z_k)。重采样时每个样本的权值ω_k ⁱ被重新设置为1/N。

(c)结果输出

用权重ω_k ⁱ分别乘以粒子(x_k ⁱ，P_k ⁱ)得到N个随机粒子序列。

状态估计值为 $x_k=\underset{i}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\ω}}_k^i{x}_k^i.$

状态估计误差阵为 $P_k=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\ω}}_k^i{P}_k^i=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\ω}}_k^i(x_k^i-{\hat{x}}_k^i){(x_k^i-{\hat{x}}_k^i)}^{\′}.$

由此得到航天器陀螺仪误差系数的估计，完成航天器自标定，进而提高航天器自主定姿精度。

UPF算法利用UKF得到粒子滤波的重要性采样密度函数，从而克服了EKF和UKF只能应用于噪声为高斯分布的不足。该方法可以取得比标准粒子滤波、EKF和UKF更快的滤波收敛性和更高的滤波精度。

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

Claims (3)

1、一种基于预测滤波和UPF航天器自标定方法，其特征在于：

(1)建立基于精确陀螺模型和姿态运动学的航天器自标定状态方程；

(2)建立以星敏感器观测得到的姿态信息作为量测量建立系统的量测方程；

(3)采用预测滤波进行模型误差预测；

(4)采用UPF算法进行状态估计，完成航天器自标定，并输出导航信息。

2、根据权利要求1所述的基于预测滤波和UPF航天器自标定方法，其特征在于：所述步骤(1)中所述的精确陀螺模型中陀螺误差源包括：刻因子误差、不对准误差、陀螺常值漂移和陀螺输出噪声，得到陀螺模型得：

ω_g＝ω+b+g_sf+g_ma+n_a

式中，ω_g为陀螺测量值，ω为陀螺真值，b为陀螺的常值漂移，g_sf为陀螺刻度因数误差，g_ma陀螺不对准误差，n_a为陀螺输出噪声。

3、根据权利要求1所述的一种基于预测滤波和UPF航天器自标定方法，其特征在于：所述步骤(4)中采用UPF算法进行状态估计的方法如下：

①初始化

T＝0时，对于i＝1，……，N，生成服从先验分布q(x₀)的N个采样粒子{x₀ ¹，x₀ ²，…，x₀ ⁿ}，并将每个样本的初始权值ω₀ ⁱ均设为1/N，

${\stackrel{\‾}{x}}_0^i=E\left[x_0^i\right]$

$P_0^i=E\left[(x_0^i-{\stackrel{\‾}{x}}_0^i){(x_0^i-{\stackrel{\‾}{x}}_0^i)}^T\right]$

②对于T＝1，2，……，T＝k时的滤波过程如下：

(a)重要采样

对于N个粒子分别用UKF方法更新，由{x_k-1 ⁱ，P_k-1 ⁱ}得到

并采集一个新的采样点

使得 ${\hat{x}}_k^i~q\left(x_k^i\right|x_{k-1}^i,Z_k)=N({\stackrel{\‾}{x}}_k^i,{\stackrel{\‾}{P}}_k^i),$

根据样本的相似程度更新权值，使得 ${\stackrel{\‾}{\mathrm{\ω}}}_k^i={\mathrm{\ω}}_{k-1}^i\frac{p\left(Z_k\right|{\hat{x}}_k^i)p\left({\hat{x}}_k^i\right|x_{k-1}^i)}{q\left({\hat{x}}_k^i\right|x_{k-1}^i,Z_{k-1})}$ 然后对每个粒子的权值进行归一化，即令 ${\mathrm{\ω}}_k^i=\frac{1}{{\mathrm{\Σ}}_{j=1}^N{\stackrel{\‾}{\mathrm{\ω}}}_k^i}$ 最后计算有效粒子的尺寸N_eff， $N_{\mathrm{eff}}=\frac{1}{{\mathrm{\Σ}}_{i=1}^N{\left({\mathrm{\ω}}_k^i\right)}^2},$ 如果N_eff小于门限值N_th，说明粒子多样性降低，需转入步骤(b)进行重采样，否则转入步骤(c)输出该次的滤波结果；

(b)重采样

重采样的目的是消除权值较小的粒子，增加权值较大的粒子，使重采样后的样本集的分布符合后验密度p(x_k|Z_k)，重采样时每个样本的权值ω_k ⁱ被重新设置为1/N；

(c)结果输出

用权重ω_k ⁱ分别乘以粒子(x_k ⁱ，P_k ⁱ)得到N个随机粒子序列状态估计值为 $x_k=\underset{i}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\ω}}_k^i{x}_k^i$

状态估计误差阵为 $P_k=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\ω}}_k^i{P}_k^i=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\ω}}_k^i(x_k^i-{\hat{x}}_k^i){(x_k^i-{\hat{x}}_k^i)}^{\′},$

由此得到航天器陀螺仪误差系数的估计，完成航天器自标定，进而提高航天器自主定姿精度。

Images