CN101059349A - 微型组合导航系统及自适应滤波方法 - Google Patents

Abstract

一种微型组合导航系统及自适应滤波方法，属惯性导航系统。该组合导航系统包括余度配置惯性测量单元、微型GPS接收机、导航计算机，导航计算机包括数据采集、导航解算、自适应平淡粒子滤波器。该滤波方法的具体步骤是：首先设计系统状态方程和量测方程；根据系统状态和自适应原则生成符合先验分布的状态样本粒子；根据UKF滤波门限判断标准决定粒子是否进入UKF滤波，其他不进入UKF滤波的粒子只进行时间更新；更新样本权值；判断是否需要重采样；最后粒子进行状态融合得到状态的估计值。以了解惯导性能并方便导航解算中的补偿。本发明克服了平淡粒子滤波滤波计算量大，提高了导航系统的精度，降低导航系统误差，应用于航空、航天等工业领域。

Description

微型组合导航系统及自适应滤波方法

技术领域

本发明是一种应用于微型组合导航系统及自适应滤波方法，属于惯性导航领域。

背景技术

惯性技术发展的一个重要标志是惯性传感器的发展，主要体现在测量原理、器件精度和加工工艺方面。新型的低成本、微型IMU中，有代表性和发展前途的是微电子机械系统(Micro-Electro-Mechanical System，MEMS)IMU，它是当代微电子技术发展的产物，也是下一代IMU的典型代表。MEMS惯性传感器是惯性器件的一个大种类，以其制造采用集成电路的加工工艺而明确地区别与其它惯性器件。随着MEMS惯性传感器性能提高，MEMS INS以及组合导航系统以其体积小、重量轻、功耗低、成本低的特点，大大拓展了惯性导航系统的应用范围。军事应用和民用领域对导航的需求共同构成了促进MEMS-INS不断发展的动力。微电子技术、微机械加工技术的发展将从技术上不断推动MEMS-INS的向高性能、微型化发展。

用MEMS-IMU构成的惯性导航系统与常规的惯性导航系统相比，它们具有显著的体积小、重量轻、成本低、可靠性高及动态范围宽、响应快等优点，特别适合于构成余度配置式捷联惯性导航系统。利用新型的MIMU构成的惯性导航系统，不仅具有传统的惯性导航系统的各种性能，而且可以达到小巧、可靠性高的战术导航、制导性能，有着广泛的应用前景，可应用于各种战术战略武器的精确制导，微型飞行器的导航、制导系统等领域，也是实现武器系统微小型化、信息化的一项关键技术。

余度MEMS-IMU与微型GPS接收机相结合，构成高可靠性、低成本、轻巧型的微型惯性组合导航系统，可应用在微型飞行器导航、小型机器人定位跟踪等多种场合。组合导航方案中的滤波算法通常采用卡尔曼滤波技术。但是组合系统方程本身就是非线性的，在线性化卡尔曼滤波的过程中必然损失一定精度。而且微型惯性组合导航系统重要的用途是在微飞行器上，因为微飞行器的小巧，比一般的大型飞行器更容易做大幅度机动，GPS接收信号更容易受到遮挡，该状态下系统模型和观测信息就会出现较大的突变，采用卡尔曼滤波方法就会带来很大的误差。

粒子滤波是近年来的研究热点，它基于递推算法，是对物理模型的近似最优解，系统的非线性越强，其优势越明显。它是滤波领域的一大技术突破，在导航领域得到了广泛应用，包括大方位失准角下的初始对准、地形辅助导航、目标跟踪等等。但是粒子滤波存在退化现象，抑制退化的重要手段是选择合适的重要性概率密度函数。平淡粒子滤波是一种新的粒子滤波算法，使用UT变换产生重要性概率函数密度，更接近系统的真实分布。

发明内容

本发明的目的是针对平淡粒子滤波计算量大的缺点，提供一种微型组合导航系统及自适应滤波方法，该方法可以为微型余度配置导航系统提供精度高于卡尔曼滤波，而计算量远小于普通粒子滤波的滤波技术。

为了达到上述的发明目的，本发明是

1.一种微型组合导航系统，其特征在于，包括余度配置惯性测量单元(MEMS-IMU)、微型GPS接收机、导航计算机，该导航计算机包括数据采集、导航解算、自适应平淡粒子滤波器。，其中余度配置惯性测量单元采用六个单自由度陀螺和两个双轴加速度计组合，该余度配置惯性测量单元输出数据经过数据采集，输入到导航解算，该导航解算输出的惯导信息输入自适应平淡粒子滤波器，微型GPS接收机的输出经自适应平淡粒子滤波器后的输出对惯导导航解算的输出校正，校正后的输出与导航解算的输出一起进入飞控系统。

2.一种如权利要求1所述的微型组合导航系统的自适应滤波方法，包括如下步骤：设计系统状态方程、设计系统量测方程、UKF滤波，权值归一化、状态估计；

所述设计系统状态方程是以惯性导航系统的误差量加上GPS时钟信号测距等效误差δt_u作为状态，并结合余度配置MEMS-INS中MEMS传感器的噪声状况，建立系统模型，其状态方程为：

式中：状态量X＝[φ_E φ_N φ_U δv_E δv_N δv_N δL δλ δh δt_u]^T，由东、北、天向的平台误差角φ_E、φ_N、φ_U，东、北、天向的速度误差δv_E、δv_N、δv_N，纬度、经度和高度的误差δL、δλ、δh，以及时间测距等效误差δt_u构成；W为噪声向量，F，G是状态方程的状态转移矩阵和噪声系数矩阵。

所述设计系统量测方程，第i颗GPS卫星测得的实际伪距为：

${\mathrm{\ρ}}_{\mathrm{Gi}}=\sqrt{[{(x_{\mathrm{si}}-x_I)}^2+{(y_{\mathrm{si}}-y_I)}^2+{(z_{\mathrm{si}}-z_I)}^2]}+\mathrm{\δ}{t}_u+v_{\mathrm{\ρi}}$ i＝1，2，3，4

式中：x_si，y_si，z_si为该卫星的位置，x_I，y_I，z_I为惯性导航位置，v_ρi为伪距测量误差，δt_u为时间测距等效误差，(x，y，z)坐标系为地球固定坐标系(ECEF)，而导航计算中是采用经纬高信息表示位置的，因此要进行坐标转换，下式中的R_N为卯酉圈曲率半径，L_I，λ_I，h_I表示惯性导航的经、纬、高位置信息，f为地球椭圆偏心率，其他符号意义同上所述，

$\left\{\begin{array}{c}{x}_I=(R_N+h_I-\mathrm{\δh})\cos (L_I-\mathrm{\δL})\cos ({\mathrm{\λ}}_I-\mathrm{\δ\λ})\\ y_I=(R_N+h_I-\mathrm{\δh})\cos (L_I-\mathrm{\δL})\sin ({\mathrm{\λ}}_I-\mathrm{\δ\λ})\\ z_I=[R_N(1-f^2)+h_I-\mathrm{\δh}]\sin (L_I-\mathrm{\δL})\end{array}\right.$

得到：

${\mathrm{\ρ}}_{\mathrm{Gi}}=[{(x_{\mathrm{si}}-(R_N+h_I-\mathrm{\δh})\cos (L_I-\mathrm{\δL})\cos ({\mathrm{\λ}}_I-\mathrm{\δ\λ}))}^2$

$+{(y_{\mathrm{si}}-(R_N+h_I-\mathrm{\δh})\cos (L_I-\mathrm{\δL})\sin ({\mathrm{\λ}}_I-\mathrm{\δ\λ}))}^2$

$+(z_{\mathrm{si}}-[R_N(1-f^2)+h_I-\mathrm{\δh}]\sin {\left(L_I-\mathrm{\δL})\right)}^2{]}^{\frac{1}{2}}$

$+\mathrm{\δ}{t}_u+v_{\mathrm{\ρi}};$

所述UKF滤波，包括初始化、UT变换计算采样点、时间更新、量测更新；所述权值归一化步骤，是指更新样本权值 ${\stackrel{\%}{w}}_k={\stackrel{\%}{w}}_{k-1}\frac{p\left(z_k\right|x_k^i)p\left(x_k^i\right|x_{k-1}^i)}{q\left(x_k^i\right|x_{0:k-1}^i,z_{1:k})},$ $w_k^i=\frac{{\stackrel{\%}{w}}_k}{\mathrm{\Σ}{\stackrel{\%}{w}}_k},$ p()表示条件概率密度，q(x_k ⁱ|x_0：k-1 ⁱ，z_1：k)为重要性函数；所述状态估计步骤，采用下式 $x(k/k)=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{x}_k^i{\mathrm{\ω}}_k^i$ 进行状态估计；其特征在于，在系统测量方程与UKF滤波之间还包括粒子生成和门限判断两个步骤，在权值归一化步骤与状态估计步骤之间还包括重采样判断步骤，

所述粒子生成步骤，为了满足系统性能要求，需要保持粒子数目不低于预定门限M，因此提出粒子数目的自适应决策方法：

根据上式就可以在线调节粒子数目。INT表示取整数，当丢失信号5秒以内，进行小幅度调整粒子数目，当GPS信号丢失超过5秒，则加大幅度调整粒子数目；

所述门限判断步骤，设定权值控制参数T，在UKF滤波之前进行判断，如果该粒子权值不小于T，则进行UKF滤波，计算其对应的估计值和状态方差阵，如果该粒子权值小于T，则不进行UKF滤波，只利用状态方程进行时间更新X(k+1)＝F(k)X(k)+G(k)W(k)，其中k为当前时刻，k+1为更新后的时刻。从而构成了自适应平淡粒子滤波器，所述权值控制参数 $T=\frac{1}{N},$ N为粒子总数，从几千个到数万个不等，但是对每一次滤波而言，N是固定的。

所述重采样判断步骤，为了均衡权值在控制参数T左右的粒子，避免太多的粒子权值大于或者小于T，在重采样环节的判断处，提出了基于参数平衡原则的重采样判断标准：判断重采样阈值S_T是否小于 ${(\frac{N_2}{N}\underset{i=1}{\overset{N_1}{\mathrm{\Σ}}}{w}_i^2+\frac{N_1}{N}\underset{i=1}{\overset{N_2}{\mathrm{\Σ}}}{w}_j^2)}^{-1},$ 当N1＞N2，也就是权值大于T的粒子数目较多，则在计算采样阈值时候，增加权值小于T的粒子的比重；反之，当N1＜N2，也就是权值小于T的粒子数目较多，则在计算采样阈值时候，增加权值大于T的粒子的比重，其中N1为权值大于T的粒子总数，N2为权值小于T的粒子总数。

本发明的最大优点是自适应平淡粒子滤波器计算量小，精度高，可靠性好。

附图说明，

图1是微型组合导航系统组成框图

图2是微型组合导航系统的自适应滤波方法流程示意图

图3是纬度误差对比曲线示意图

图4是精度误差对比曲线示意图

图5是高度误差对比曲线示意图

具体实施方式

多余度微惯导系统是采用MEMS-IMU余度配置技术的微型捷联惯导系统。余度配置方案随着器件数目的不同有多种，采用的六个单自由度陀螺和两个双轴加速度计的配置方案即陀螺敏感轴沿正十二面体六个面的法线方向。MEMS加速度计采用两个双轴加速度计正交配置的方案。

该多余度惯性系统比正交配置的惯性系统具有更高的精度和可靠性。在微惯性器件发生故障的情况下，只要剩余器件(陀螺/加速度计)数目均不少于三个，系统即可正常工作，容错性较高。

余度MEMS-IMU/GPS组合导航系统结构如图1所示，硬件包括余度配置惯性测量单元、微型GPS接收机、导航计算机，该导航计算机包括数据采集、导航解算、自适应平淡粒子滤波器。。该余度MEMS-IMU/GPS组合导航系统采用紧密组合中的伪距组合模式，用微型GPS接收机给出的相应于惯导位置的伪距ρ_G作为量测值，然后通过星历数据和余度微型惯导系统给出的位置、速度计算出伪距ρ_I，与ρ_G相比较。通过改进平淡粒子滤波器估计惯性导航系统的导航参数误差和GPS的时钟误差，然后对组合导航系统进行测量校正。

本发明滤波方法的具体步骤是：首先设计系统状态方程和量测方程；然后根据系统状态和自适应原则生成符合先验分布的状态样本粒子；然后根据UKF滤波门限判断标准决定粒子是否进入UKF滤波，其他不进入UKF滤波的粒子只进行时间更新；更新样本权值；判断是否需要重采样；最后粒子进行状态融合得到状态的估计值。以了解惯导性能并方便导航解算中的补偿。

1)系统状态方程

以惯性导航系统的误差量加上GPS时钟信号测距等效误差δt_u作为状态，并结合余度配置MEMS-INS中MEMS传感器的噪声状况，建立系统模型。目前系统使用的MEMS加速度计和陀螺仪经过测试建模分析，误差考虑为温度误差、常值偏差、安装误差和随机噪声。经过标定温度补偿和非随机误差补偿以后，认为MEMS传感器的误差为随机有色噪声，不在系统状态方程当中进行扩充建模。因此，状态方程为

式中：状态量X＝[φ_E φ_N φ_U δv_E δv_N δv_N δL δλ δh δt_u]^T，由东、北、天向的平台误差角φ_E、φ_N、φ_U，东、北、天向的速度误差δv_E、δv_N、δv_N，纬度、经度和高度的误差δL、δλ、δh，以及时间测距等效误差δt_u构成；W为噪声向量，F，G是状态方程的状态转移矩阵和噪声系数矩阵。

2)系统量测方程

第i颗GPS卫星测得的实际伪距为：

${\mathrm{\ρ}}_{\mathrm{Gi}}=\sqrt{[{(x_{\mathrm{si}}-x_I)}^2+{(y_{\mathrm{si}}-y_I)}^2+{(z_{\mathrm{st}}-z_I)}^2]}+\mathrm{\δ}{t}_u+v_{\mathrm{\ρi}}$ i＝1，2，3，4

式中：x_si，y_si，z_si为该卫星的位置，x_I，y_I，z_I为惯性导航位置，v_ρi为伪距测量误差，δt_u为时间测距等效误差，(x，y，z)坐标系为地球固定坐标系(ECEF)，而导航计算中是采用经纬高信息表示位置的，因此要进行坐标转换，下式中的R_N为卯酉圈曲率半径，L_I，λ_I，h_I表示惯性导航的经、纬、高位置信息，f为地球椭圆偏心率，其他符号意义同上所述。

$\left\{\begin{array}{c}{x}_I=(R_N+h_I-\mathrm{\δh})\cos (L_I-\mathrm{\δL})\cos ({\mathrm{\λ}}_I-\mathrm{\δ\λ})\\ y_I=(R_N+h_I-\mathrm{\δh})\cos (L_I-\mathrm{\δL})\sin ({\mathrm{\λ}}_I-\mathrm{\δ\λ})\\ z_I=[R_N(1-f^2)+h_I-\mathrm{\δh}]\sin (L_I-\mathrm{\δL})\end{array}\right.$

得到：

${\mathrm{\ρ}}_{\mathrm{Gi}}=[{(x_{\mathrm{si}}-(R_N+h_I-\mathrm{\δh})\cos (L_I-\mathrm{\δL})\cos ({\mathrm{\λ}}_I-\mathrm{\δ\λ}))}^2$

$+{(y_{\mathrm{si}}+(R_N+h_1-\mathrm{\δh})\cos (L_I-\mathrm{\δL})\sin ({\mathrm{\λ}}_I-\mathrm{\δ\λ}))}^2$

$+(z_{\mathrm{si}}-[R_N(1-f^2)+h_I-\mathrm{\δh})\sin {(L_I-\mathrm{\δL}))}^2{]}^{\frac{1}{2}}$

$+\mathrm{\δ}{t}_u+v_{\mathrm{\ρi}}$

量测方程是强非线性方程，卡尔曼滤波中是将其用泰勒级数展开，进行线性化处理，增加惯性导航的伪距，然后取GPS和惯性导航两者的伪距差作为量测量。但是这样无疑会损失计算精度，这在系统误差量级比较小的情况下可能并不明显，但是一旦增大系统误差(比如GPS信号收到遮挡，惯性导航缺乏校正量)，系统就会迅速发散。

余度MEMS-IMU/GPS组合导航系统中使用自适应平淡粒子滤波时，不必再对系统线性化处理，伪距组合模式观测量只取微GPS接收机输出的伪距信号即可，伪距信号组合Z＝[ρ_G1 ρ_G2 ρ_G3 ρ_G4]^T。这是针对可见星不少于4颗的情况，该观测量随着GPS性能的变化可以在系统中对其调整，当可见星只有1颗的时候，Z＝[ρ_G1]。

3)粒子生成

粒子滤波是非线性条件下对贝叶斯估计的一种近似方法，用一个加权样本集合来估计后验概率密度，当样本数无穷大的时候，其估计值等于真实后验概率密度。初始化：生成符合先验分布的样本，令{x_0：k ⁱ}(i＝1，...，N)，权值 $w_k^i=\frac{1}{N},$ w_k ⁱ是和为1的归一化权系数，上标表示第i个粒子，下标表示第k个时刻，如系统导航进行第五次滤波时，k＝5；

就单次滤波更新而言，如果采用较少的粒子，当导航系统丢失目GPS信号时，容易造成大部分粒子的权值接近于0，即使采用重采样步骤，由于缺乏有效的粒子而使得粒子复制难以进行。此时增加粒子数目可以提高系统性能。而当有GPS信号阶段，可以降低粒子数目，从而节约系统计算和存储资源。

为了满足系统性能要求，需要保持粒子数目不低于预定门限M，因此提出粒子数目的自适应决策方法：

根据上式就可以在线调节粒子数目。INT表示取整数，当丢失信号5秒以内，进行小幅度调整粒子数目，当GPS信号丢失超过5秒，则加大幅度调整粒子数目。假设初始粒子数目是500，当t_p＝4秒，根据公式，粒子数目N＝600；当t_p＝20秒，粒子数目N＝800；当t_p＝300秒，粒子数目N＝2500。

4)门限判断

平淡粒子滤波算法将每个粒子都进行一次平淡卡尔曼滤波，计算量很大。而实际上粒子的权值并不相同，权值大的粒子对系统影响大，权值很小的粒子对系统影响较小，虽然使用UT变换产生重要性概率函数密度，能够接近粒子的真实分布。但是，对于小权值粒子而言，这种改进对系统的影响比较微弱。因此，系统可以加入平淡卡尔曼滤波必要性判断功能。设定权值控制参数T，在UKF滤波之前进行判断，如果该粒子权值不小于T，则进行UKF滤波，计算其对应的估计值和状态方差阵，如果该粒子权值小于T，则不进行UKF滤波，只利用状态方程进行时间更新X(k+1)＝F(k)X(k)+G(k)W(k)，其中k为当前时刻，k+1为更新后的时刻。从而构成了自适应平淡粒子滤波器。

由于有效样本数目小于门限值的时候滤波系统会进行重采样，因此，在权值控制参数T选择适当的情况下，该判断功能的加入不会导致系统只有几个大权值粒子进入平淡卡尔曼滤波。算法流程图参见图2所示。粒子初始化时刻，粒子权值的初始值为 $w_k^i=\frac{1}{N},$ 也就是粒子的初始权值是平均分布的。如果选择 $T=\frac{1}{N},$ 即N个粒子权值的平均值，则在初始时刻所有粒子都进行一次完整的平淡粒子滤波。下一时刻以后，粒子权值分布不再相等，则控制算法选择权值 $w_k^i\≥\frac{1}{N}$ 的进行平淡粒子滤波，而其他权值较小的粒子不再做平淡滤波，经过时间更新以后，直接进入粒子滤波。

5)UKF滤波

平淡卡尔曼滤波(Unscented Kalman Filter，UKF)的基础是UT变换，该变换是计算随机变量的非线性变换的一种方法，变换后采样点的分布以3阶精度近似于真实均值和方差。从数学模型上来说，一步最优预测值是求上一步状态变量与噪声通过系统传递后的数学期望。将每一个粒子进行UKF滤波，实现过程包括如下几个部分：

(一)初始化

将过程噪声和量测噪声增广为状态向量，增广后的状态向量为X^a，相应的采样点向量为ξ^a，P₀为原状态向量协方差初始估计值，P_v为过程噪声方差，P_η为量测噪声方差。则有

x^a＝[x^T v^T η^T]^T，ξ^a＝[(ξ^x)^T(ξ^v)^T(ξη)^T]^T

${\hat{x}}_0^a=E\left[x^a\right],P_0=E\left[(x_0-{\hat{x}}_0){(x_0-{\hat{x}}_0)}^T\right]$

$P_0^a=E\left[(x_0^a-{\hat{x}}_0^a){(x_0^a-{\hat{x}}_0^a)}^T\right]=\left[\begin{array}{ccc}{P}_0& 0& 0\\ 0& P_v& 0\\ 0& 0& P_{\mathrm{\η}}\end{array}\right]$

(二)UT变换计算采样点

构造一个n行(n为增广状态向量的维数)，2n+1列的矩阵，各列形式如下：

${\mathrm{\ξ}}_{0,k-1}^a={\hat{x}}_{k-1}^a,{\mathrm{\ξ}}_{i,k-1}^a={\hat{x}}_{k-1}^a+{\left(\sqrt{(n+\mathrm{\λ})P_{k-1}^a}\right)}_i,$ i＝1，2，...，n

${\mathrm{\ξ}}_{i,k-1}^a={\hat{x}}_{k-1}^a-{\left(\sqrt{(n+\mathrm{\λ})P_{k-1}^a}\right)}_{i-n},$ i＝n+1，n+2，...，2n

式中：λ＝α²(n+τ)-n，α决定采样点距均值的远近程度，通常被赋一个较小的正值；τ的取值需保证矩阵(n+λ)P_k-1 ^a的半正定性，n＝24。

(三)时间更新

${\mathrm{\ξ}}_{i,k/k-1}^x=f\left({\mathrm{\ξ}}_{i,k/k-1,}^x{\mathrm{\ξ}}_{i,k-1}^v\right)$ i＝0，1，L，2n， ${\hat{x}}_{k/k-1}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{W}_i^{\left(m\right)}{\mathrm{\ξ}}_{i,k/k-1}^x$

$P_{k/k-1}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{W}_i^{\left(c\right)}[{\mathrm{\ξ}}_{i,k/k-1}^x-{\hat{x}}_{k/k--1}]\×{[{\mathrm{\ξ}}_{i,k/k-1}^x-{\hat{x}}_{k/k-1}]}^T,Z_{k/k-1}=h({\mathrm{\ξ}}_{k/k-1}^x,{\mathrm{\ξ}}_{k-1}^{\mathrm{\η}})$

式中，Z_k/k-1为由采样点序列预测的量测值序列。Z_k/k-1的第i列表示为Z_i，k/k-1，则

${\hat{z}}_{k/k-1}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{W}_i^{\left(m\right)}{Z}_{i,k/k-1}$

式中，权值 $W_0^{\left(m\right)}=\frac{\mathrm{\λ}}{n+\mathrm{\λ}},$ $W_0^{\left(c\right)}=\frac{\mathrm{\λ}}{n+\mathrm{\λ}}+(1-{\mathrm{\α}}^2+\mathrm{\β}),$ $W_i^{\left(m\right)}=W_i^{\left(c\right)}=\frac{1}{2(n+\mathrm{\λ})}.$

(四)量测更新

$P_{{\hat{z}}_k{\hat{z}}_k}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{W}_i^{\left(c\right)}[Z_{i,k/k-1}-{\hat{z}}_{k/k-1}]\×{[Z_{i,k/k-1}-{\hat{z}}_{k/k-1}]}^T$

$P_{x_k{z}_k}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{W}_i^{\left(c\right)}[{\mathrm{\ξ}}_{i,k/k-1}^x-{\hat{x}}_{k/k-1}]\×{[Z_{i,k/k-1}-{\hat{z}}_{k/k-1}]}^T$

$\mathrm{\κ}=P_{x_k{z}_k}{P}_{{\hat{z}}_k{\hat{z}}_k}^{-1}$

${\hat{x}}_k={\hat{x}}_{k/k-1}+\mathrm{\κ}(z_k-{\hat{z}}_{k/k-1})$

$P_k=P_{k/k-1}-\mathrm{\κ}{P}_{{\hat{z}}_k{\hat{z}}_k}{\mathrm{\κ}}^T$

6)权值归一化

更新样本权值 ${\stackrel{\%}{w}}_k={\stackrel{\%}{w}}_{k-1}\frac{p\left(z_k\right|x_k^i)p\left(x_k^i\right|x_{k-1}^i)}{q\left(x_k^i\right|x_{0:k-1}^i,z_{1:k})},$ $w_k^i=\frac{{\stackrel{\%}{w}}_k}{\mathrm{\Σ}{\stackrel{\%}{w}}_k}.$ q(x_k ⁱ|x_0：k-1 ⁱ，z_1：k)为

重要性函数。

7)重采样判断

为了均衡权值在控制参数T左右的粒子，避免太多的粒子权值大于或者小于T，在重采样环节的判断处，提出了基于参数平衡原则的重采样判断标准：判断重采样阈值S_T是否小于 ${(\frac{N_2}{N}\underset{i=1}{\overset{N_1}{\mathrm{\Σ}}}{w}_i^2+\frac{N_1}{N}\underset{j=1}{\overset{N_2}{\mathrm{\Σ}}}{w}_j^2)}^{-1},$ 当N1＞N2，也就是权值大于T的粒子数目较多，则在计算采样阈值时候，增加权值小于T的粒子的比重；反之，当N1＜N2，也就是权值小于T的粒子数目较多，则在计算采样阈值时候，增加权值大于T的粒子的比重，其中N1为权值大于T的粒子总数，N2为权值小于T的粒子总数。

8)状态估计

$x(k/k)=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{x}_k^i{\mathrm{\ω}}_k^i$

有益效果：

根据对目前用于余度配置的MEMS陀螺和加速度计性能指标的实际特性分析，取MEMS陀螺和加速度计的随机误差取为有色噪声，均值为随机小量，方差分别为1.0°/s和1.5×10^-3g，微GPS经度、纬度和高度的定位误差均方差为20m、20m、50m。仿真的时间长度为3600秒，MEMS-INS计算周期20ms，滤波周期1s。改进UPF的权值控制参数 $T=\frac{1}{N}.$ 将采用自适应平淡粒子滤波的伪距组合导航系统和目前应用较多的采用卡尔曼滤波线性化后的伪距组合导航系统进行对比分析。平淡粒子滤波粒子数目为1500，自适应平淡粒子滤波初始数目为500。

仿真航迹包括起飞、爬升、转弯等多个动作。假设在600s～900s之间GPS信号完全受到遮挡，信号不可用。图3～图5分别为不同滤波模式下的纬度、经度、高度误差对比曲线图中KF表示普通卡尔曼滤波，改进UPF表示改进后的自适应平淡粒子滤波。从图中可以看到，自适应平淡粒子滤波性能高于卡尔曼滤波，其收敛较快，在稳态时候，位置误差效果好于卡尔曼滤波。

GPS信号丢失期间，余度微惯性系统工作在纯惯性导航模式下，而由于惯性导航误差的随时间累加性，系统误差会迅速发散。虽然滤波在纯惯性导航模式下依旧对系统进行状态预测，但是不同的滤波系统性能差别很大。改进平淡粒子滤波通过概率密度估计粒子的分布，更接近系统的真实分布，得到了比卡尔曼滤波更好的效果。在微GPS接收机信号不可用时段，卡尔曼滤波方式下系统误差迅速扩大，发散趋势明显，而改进平淡粒子滤波依然保持较好的稳定性。抑制发散效果明显。因此，改进后的自适应平淡粒子滤波更适合应用于余度MEMS-IMU/GPS组合导航系统。

不同滤波导航性能结果对比参见表1所示，可以看到：自适应平淡粒子滤波伪距组合导航系统的性能远好于基于卡尔曼滤波的伪距差组合导航系统。另外，多次仿真的统计结果表明，自适应平淡粒子滤波与平淡粒子滤波性能相当，但是计算量大大减少，在粒子初始数值M＝500，重采样阈值S_T＝500情况下，滤波计算量减少了至少50％，如表2所示。

表1  不同滤波方法导航性能对比

导航方式	稳态位置误差值(m)			GPS失效期间高度误差峰值(m)
					经度误差	纬度误差	高度误差
	卡尔曼滤波伪距差组合导航	2.99	2.25					14.79	155.32
平淡粒子滤波伪距组合导航				2.76	2.04	7.82	18.68
	自适应平淡粒子滤波伪距组合导航	2.78	2.04					7.83	18.68

表2  粒子滤波方法时间性能对比

粒子滤波方式	时间
		平淡粒子滤波	t
自适应平淡粒子滤波	0.418t

Claims (2)

1.一种微型组合导航系统，其特征在于，包括余度配置惯性测量单元、微型GPS接收机、导航计算机，该导航计算机包括数据采集、导航解算、自适应平淡粒子滤波器，其中余度配置惯性测量单元采用六个单自由度陀螺和两个双轴加速度计组合，该余度配置惯性测量单元输出数据经过数据采集，输入到导航解算，该导航解算输出的惯导信息输入自适应平淡粒子滤波器，微型GPS接收机的输出经自适应平淡粒子滤波器后的输出对惯导导航解算的输出校正，校正后的输出与导航解算的输出一起进入飞控系统。

2.一种如权利要求1所述的微型组合导航系统的自适应滤波方法，包括如下步骤：设计系统状态方程、设计系统量测方程、UKF滤波，权值归一化、状态估计；

所述设计系统状态方程是以惯性导航系统的误差量加上GPS时钟信号测距等效误差δt_u作为状态，并结合余度配置MEMS-INS中MEMS传感器的噪声状况，建立系统模型，其状态方程为：

                      X^&＝F(t)X(t)+G(t)W(t)

式中：状态量X＝[φ_E φ_N φ_U δv_E δv_N δv_N δL δλ δh δt_u]^T，由东、北、天向的平台误差角φ_E、φ_N、φ_U，东、北、天向的速度误差δv_E、δv_N、δv_N，纬度、经度和高度的误差δL、δλ、δh，以及时间测距等效误差δt_u构成；W为噪声向量，F，G是状态方程的状态转移矩阵和噪声系数矩阵。

所述设计系统量测方程，第i颗GPS卫星测得的实际伪距为：

${\mathrm{\ρ}}_{\mathrm{Gi}}=\sqrt{[{(x_{\mathrm{si}}-x_I)}^2+{(y_{\mathrm{si}}-y_I)}^2+{(z_{\mathrm{si}}-z_I)}^2]}+\mathrm{\δ}{t}_u+v_{\mathrm{\ρi}},i=\mathrm{1,2,3,4}$

式中：x_si，y_si，z_si为该卫星的位置，x_I，y_I，z_I为惯性导航位置，v_ρi为伪距测量误差，δt_u为时间测距等效误差，(x，y，z)坐标系为地球固定坐标系(ECEF)，而导航计算中是采用经纬高信息表示位置的，因此要进行坐标转换，下式中的R_N为卯酉圈曲率半径，L_I，λ_I，h_I表示惯性导航的经、纬、高位置信息，f为地球椭圆偏心率，其他符号意义同上所述，

$\left\{\begin{array}{c}{x}_I=(R_N+h_I-\mathrm{\δh})\cos (L_I-\mathrm{\δL})\cos ({\mathrm{\λ}}_I-\mathrm{\δ\λ})\\ y_I=(R_N+h_I-\mathrm{\δh})\cos (L_I-\mathrm{\δL})\sin ({\mathrm{\λ}}_I-\mathrm{\δ\λ})\\ z_I=[R_N(1-f^2){+h}_I-\mathrm{\δh}]\sin (L_I-\mathrm{\δL})\end{array}\right.$

得到：

${\mathrm{\ρ}}_{\mathrm{Gi}}=[{(x_{\mathrm{si}}-(R_N+h_I-\mathrm{\δh})\cos (L_I-\mathrm{\δL})\cos ({\mathrm{\λ}}_I-\mathrm{\δ\λ}))}^2$

$+{(y_{\mathrm{si}}-(R_N+h_I-\mathrm{\δh})\cos (L_I-\mathrm{\δL})\sin ({\mathrm{\λ}}_I-\mathrm{\δ\λ}))}^2$

$+{(z_{\mathrm{si}}-[R_N(1-f^2)+h_I-\mathrm{\δh}\left]\sin (L_I-\mathrm{\δL})\right)}^2{]}^{\frac{1}{2}}$

$+{\mathrm{\δt}}_u+v_{\mathrm{\ρi}};$

所述UKF滤波，包括初始化、UT变换计算采样点、时间更新、量测更新；

所述权值归一化步骤，是指更新样本权值 ${\stackrel{\%}{w}}_k={\stackrel{\%}{w}}_{k-1}\frac{p\left(z_k\right|x_k^i)p\left(x_k^i\right|x_{k-1}^i)}{q\left(x_k^i\right|x_{0:k-1}^i,z_{1:k})},$

$w_k^i=\frac{{\stackrel{\%}{w}}_k}{\mathrm{\Σ}{\stackrel{\%}{w}}_k},$ p()表示条件概率密度，q(x_k ⁱ|x_0:k-1 ⁱ，z_1:k)为重要性函数：

所述状态估计步骤，采用下式 $x(k/k)=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{x}_k^i{\mathrm{\ω}}_k^i$ 进行状态估计；

其特征在于，在系统测量方程与UKF滤波之间还包括粒子生成和门限判断两个步骤，在权值归一化步骤与状态估计步骤之间还包括重采样判断步骤；

所述粒子生成步骤，为了满足系统性能要求，需要保持粒子数目不低于预定门限M，因此提出粒子数目的自适应决策方法：

根据上式就可以在线调节粒子数目。INT表示取整数，当丢失信号5秒以内，进行小幅度调整粒子数目，当GPS信号丢失超过5秒，则加大幅度调整粒子数目；

所述门限判断步骤，设定权值控制参数T，在UKF滤波之前进行判断，如果该粒子权值不小于T，则进行UKF滤波，计算其对应的估计值和状态方差阵，如果该粒子权值小于T，则不进行UKF滤波，只利用状态方程进行时间更新X(k+1)＝F(k)X(k)+G(k)W(k)，其中k为当前时刻，k+1为更新后的时刻。从而构成了自适应平淡粒子滤波器，所述权值控制参数 $T=\frac{1}{N},$ N为粒子总数，从几千个到数万个不等，但是对每一次滤波而言，N是固定的；

所述重采样判断步骤，为了均衡权值在控制参数T左右的粒子，避免太多的粒子权值大于或者小于T，在重采样环节的判断处，提出了基于参数平衡原则的重采样判断标准：判断重采样阈值S_T是否小于 ${(\frac{N_2}{N}\underset{i=1}{\overset{N_1}{\mathrm{\Σ}}}{w_i}^2+\frac{N_1}{N}\underset{j=1}{\overset{N_2}{\mathrm{\Σ}}}{w_j}^2)}^{-1},$ 当N1＞N2，也就是权值大于T的粒子数目较多，则在计算采样阈值时候，增加权值小于T的粒子的比重；反之，当N1＜N2，也就是权值小于T的粒子数目较多，则在计算采样阈值时候，增加权值大于T的粒子的比重，其中N1为权值大于T的粒子总数，N2为权值小于T的粒子总数。

Images