一种用于SINS行进间对准的模糊抗野值滤波算法

邵海俊，缪玲娟，郭岩冰

(北京理工大学自动化学院，北京 100081)

0 引 言

对于全球定位系统(Global positioning system, GPS)辅助的捷联惯性导航系统(Strapdown inertial navigation system, SINS)，可以通过GPS所提供的位置、速度信息来辅助SINS进行行进间对准。相比于静基座对准，行进间对准是一种兼顾高精度与高灵活性的对准方案[1]。

传统GPS辅助的SINS行进间对准方法包括粗对准和精对准两个部分，但这种方案耗时较长，为了满足快速对准的要求，滤波器通常需要在初始姿态角误差较大时就能进行滤波工作[2]。但较大的姿态误差会使滤波模型呈非线性，于是扩展卡尔曼滤波器(Extended Kalman filter, EKF)[3]、无迹卡尔曼滤波器(Unscented Kalman filter, UKF)[4]、容积卡尔曼滤波器(Cubature Kalman filter, CKF)[5]、集合卡尔曼滤波器(Ensemble Kalman filter, EnKF)[6-8]等改进的非线性卡尔曼滤波器被相继提出，并应用到GPS辅助的SINS行进间对准中。虽然这些改进的非线性卡尔曼滤波器在一定程度上解决了卡尔曼滤波器(Kalman filter, KF)在非线性系统中的运用问题，但是它们与KF一样，在推导卡尔曼增益时要求系统状态满足高斯分布，而系统的非线性会使得状态高斯分布的假设不能成立，因此当系统非线性较强时，这些改进的卡尔曼滤波器的滤波精度也会不同程度下降。粒子滤波器(Particle filter, PF)是一种基于蒙特卡洛采样原理的滤波方案，它对系统的非线性程度以及系统状态的分布特性没有任何限制，所以PF也被广泛运用于非线性滤波中。但目前常用的PF存在粒子退化和粒子贫化的问题[9-10]。文献[11]提出了集合粒子滤波器(Ensemble particle filter, EnPF)，该方案通过EnKF来为PF生成状态建议分布，这虽然可以一定程度缓解粒子退化问题，但当系统非线性较强时，EnKF所生成的状态建议分布与真实状态后验分布相差较大。采用类似思路的无迹粒子滤波器(Unscented particle filter, UPF)[12]和容积粒子滤波器(Cubature particle filter, CPF)[13]也同样存在这个问题。此外，GPS在信号受影响时所带来的观测野值问题，也会使得GPS辅助的SINS行进间对准结果恶化。常用的抗野值滤波器多采用新息卡方分布判据方案[14]，但这些抗野值方案均通过设定门限将量测信息分为可用观测和野值两类，而在门限附近的量测值常常会使得抗野值滤波器出现虚警或漏检。

为了避免传统EnPF算法所存在的上述问题，本文提出了一种基于模糊理论的抗野值EnPF算法(REnPF)。在REnPF中多高斯和近似算法(Gaussian sum approximation, GSA)[15]和马尔科夫链蒙特卡洛算法(Markov chain Monte Carlo, MCMC)[16]被用于从状态后验分布抽取粒子，而EnKF的使用则能提高MCMC算法中粒子移动的效率。此外，基于模糊理论所设计的模糊抗野值方案，使得REnPF算法具有了抗野值功能。

1 基本算法介绍

1.1 粒子滤波基本原理

对于含有加性噪声的非线性系统，可将其抽象为式(1)和式(2)所表达的状态空间模型：

xk=fk(xk-1,uk)+wk

(1)

yk=hk(xk)+vk

(2)

式中：xk和yk分别为系统状态和观测，uk表示系统输入，fk是系统状态转移函数，hk为系统观测函数，wk和vk分别表示系统噪声和观测噪声，并假设其分别是协方差矩阵为Q和R的零均值白噪声。

针对上述非线性系统，PF希望通过蒙特卡洛随机撒点的方式来模拟状态后验分布：

(3)

式中：p(xk|Yk)表示当观测Yk={y1,y2,…,yk}已知时，状态xk的条件概率密度函数，即状态后验概率密度函数；n是所用粒子数；δ(·)为Kronecker函数；xj,k表示k时刻的第j个粒子点。

于是函数f(xk)的估计就可以表示为：

(4)

但通常情况下真实的状态后验分布难以获取，所以在PF的实际使用中，需要寻找一个与后验分布相近并且易于采样的建议分布，然后从中抽取粒子，并为每个粒子赋予权值以模拟状态后验分布。

1.2 GSA算法与MCMC算法的互逆性

GSA算法[15]可以通过粒子集来拟合此粒子集所对应的概率密度函数p(x)。其原理为：

(5)

式中：μi是粒子点xi，同时也是高斯核函数的期望；Σi是带宽参数，同时也是高斯核函数的协方差矩阵；ωi表示高斯核函数所对应的权值。

与GSA算法相反，MCMC算法[16]可以帮助计算机从某一概率密度函数中抽取粒子。目前常用的MCMC算法的实现方法有Gibbs采样法和Metropolis Hasting法，由于后者实现简单、易于编程，所以本文采用了后者。

(6)

1.3 EnKF

EnKF用数值统计的方法代替KF中预测状态、预测协方差矩阵等的解析计算，从而将KF推广到了非线性系统中。其实现步骤为：

(7)

(8)

(9)

(10)

(11)

(12)

(13)

(14)

(15)

(16)

虽然EnKF算法将KF推广到了非线性系统，但其在计算卡尔曼增益时仍须假设系统状态满足高斯分布，而系统非线性的存在使得这样的假设不能成立。因此，系统较强的非线性会降低EnKF的滤波精度，甚至使其滤波发散。传统的EnPF使用EnKF的估计粒子作为从状态建议分布中抽取的粒子。但与EnKF所存在的问题一样，具有较强非线性的系统依然会使得EnPF中的建议分布偏离真实状态后验分布，最终导致粒子退化。

1.4 模糊抗野值方法

模糊理论是一种模仿人类推理与决策过程的理论[17]。它首先按照人类语言形式对输入变量的数域进行划分，如：{小，中，大}，从而完成对系统输入的模糊化；然后根据已有知识构建模糊规则，并对输入进行模糊推理得到模糊输出；最后对系统的模糊输出进行解模糊，从而得到系统的精确输出[18]。

由于在滤波过程中，新息的实际值与理论值之间的差距能一定程度反映出观测量的可信程度，所以本文使用归一化新息ρ作为检验统计量：

(17)

式中：xk为预测状态,yk为真实观测,Pin为新息的理论协方差。

由于传统抗野值方法的门限为固定值，这使得在门限附近的统计量会导致虚警或漏检。而模糊抗野值法能对统计量进行多级划分，并在经过模糊推理后为观测量赋予权值，从而决定对观测量的利用率。所以模糊抗野值方案将有助于改善虚警、漏检问题。

2 REnPF

鉴于传统EnPF算法在非线性滤波中所存在的问题，以及传统抗野值方案所存在的缺点，本文提出了一种模糊抗野值集合粒子滤波器，命名为REnPF。

REnPF采用第1.2节所述的方案，综合运用GSA算法和MCMC算法得到近似状态后验分布并从中采样。此外，虽然MCMC算法对粒子初始位置没有要求，但是如果让粒子初始位置与所需稳态分布相近，将有利于减少MCMC算法移动粒子所需步数。因此，在REnPF中，EnKF将用于生成MCMC算法中所需的初始粒子，从而加快粒子收敛速度。

(18)

式中：λ(·)表示输出隶属度函数，γ表示输出数域。

于是将EnKF中的式(15)改为：

(19)

若γf接近1说明yk的可信度高，可利用其对预测状态进行修正；若γf接近0说明yk可能是野值，于是估计状态直接取为预测状态值。这样EnKF便具备了抗野值能力。

(20)

综上所述，REnPF的算法流程如图1所示，而其具体执行过程可总结为：

1) 初始化滤波器，设定所需预置参数。

2) 按照式(7)～式(17)进行EnKF的时间更新过程。

4) 按照式(14)～式(16)进行EnKF量测更新过程，生成MCMC算法所需的初始粒子。其中式(15)应替换为式(19)。

6) 将第4步所得粒子的均值代入式(17)求取ρm，再将第4步所得各单个粒子代入式(17)分别求取其所对应的ρs，并以ρm和ρs为第二级模糊系统的输入，经模糊推理后得到各粒子所对应的γs。

8) 将MCMC算法移动后的粒子代入式(4)获得k时刻的估计结果。

9) 返回第2步进行循环，直至时间历元结束。

由于REnPF算法是PF的一种实现方法，所以REnPF的滤波收敛性、稳定性取决于PF本身。而根据文献[19]对于PF收敛性、稳定性的描述可知：对于∀k≥0，存在独立于n的常数c，对任意有界可测函数f有：

(21)

图1 REnPF程序流程图Fig.1 Flow chart of the REnPF

这个条件表明PF的收敛率为1/n。而在一些条件下，随着时间k增加，如果n以k2增加，则逼近误差保持稳定，而对于固定的n，误差是否稳定，尚无一般性结论[19]。

3 仿真校验

3.1 滤波模型

对于GPS辅助的SINS行进间对准问题而言，其滤波模型即为SINS的误差模型，而GPS提供位置、速度信息构建观测量。文献[20]详细推导了SINS的误差模型，其中姿态误差方程为：

(22)

速度误差方程为：

δVn+δgn

(23)

位置误差方程为：

(24)

而陀螺仪零偏和加速度计偏值的误差方程为：

(25)

仿真中，考虑到文献[21]所述载体机动方式与滤波模型可观性的关系，让载体进行加速、减速，并进行小幅“s”型机动。载体的运动轨迹与速度如图2所示，行驶过程的总时长为600 s，最大加速度2 m/s2，最大速度30 m/s。惯性测量单元(Inertial measurement unit, IMU)和GPS的性能指标如表1所示。由于姿态解算采用文献[22]所提的优化单子样算法，所以导航解算周期与IMU输出周期相同，均为0.01 s，而滤波周期与GPS输出周期一致，均为1 s。两级模糊滤波器的输入均采用图3所示的隶属度函数进行模糊化，而输出的隶属度函数则如图4所示。第一级模糊系统和第二级模糊系统的模糊规则分别如表2和表3所示。

图2 仿真运动轨迹与速度Fig.2 Simulation of trajectory and speed

表1 IMU与GPS性能指标
Table 1 Sensor precision of IMU and GPS

设备参数名参数值陀螺仪零偏0.02 (°)/h加速度计偏值10-4gIMU陀螺仪噪声(1σ)1 (°)/h加速度计噪声(1σ)10-4g采样频率100 Hz水平位置精度3 m高程位置精度8 mGPS水平测速精度0.05 m/s高程测速精度0.1 m/s采样频率1 Hz

图3 模糊系统的输入隶属度函数Fig.3 Fuzzy membership function of inputs

图4 模糊系统输出隶属度函数Fig.4 Fuzzy membership function of outputs

仿真所用硬件设备为一台计算机，其CPU为Intel Core I3-M370，主频达2.40 GHz，内存为2.99 GB，操作系统为Windows 10，仿真软件为Matlab 2013B。

表2 第一级模糊系统的模糊规则Table 2 Fuzzy rules of the first fuzzy system

表3 第二级模糊系统的模糊规则Table 3 Fuzzy rules of the second fuzzy system

3.2 仿真一：GPS无野值

对于GPS辅助的SINS行进间对准而言，当GPS信号良好时，主要考虑初始姿态角误差所导致的系统非线性对滤波精度的影响。所以在仿真一中，设置正常的GPS输出，SINS初始姿态误差(俯仰、横滚、方位)设置为[10°,-10°,30°]，以使滤波系统呈非线性特性，三个方向的初始速度误差均设置为0.1 m/s，位置误差设置为[1 m, 1 m, 3 m]。与此同时，本文选择EnKF、传统EnPF(记为：EnPF_old)作为对比算法，以验证REnPF算法在系统非线性较强且不存在野值时，也能具有较高的滤波精度。所有算法的粒子数均取为20。导航系统采用了闭环反馈修正方式，即将滤波器输出的估计状态用于修正导航解算结果，并将此结果用于下一次的航位推算。考虑到粒子滤波算法中粒子的随机性，所以在验证算法的非线性滤波性能时进行了30次蒙特卡洛仿真，而30次仿真所得各姿态误差角的均值变化曲线如图5所示，各算法在第600 s对准结束时的姿态误差均值，以及单历元平均运算耗时则分别列于表4和表5中。

表4 无野值时30次蒙特卡洛实验各滤波算法 第600 s的姿态角误差均值Table 4 Average attitude errors obtained by the different filters at 600 s in 30 Monte Carlo experiments without outliers

表5 各滤波算法的单历元平均运算耗时 Tabel 5 Average time consumption in 1 epoch

从图5可以看出，在无野值的情况下，EnKF、EnPF_old、REnPF都能使俯仰角、横滚角、方位角的误差平均值快速收敛。在滤波精度方面，EnKF在三个方向上的精度都最低，这主要是由于EnKF使用的卡尔曼增益在理论推导时需满足状态高斯分布的假设，但系统的非线性会破坏这一假设，从而使得滤波精度较低；EnPF_old的滤波精度优于EnKF，这说明EnPF_old虽然采用EnKF获得估计粒子点，但其后续PF步骤的赋权操作将有助于提高整个滤波器的滤波精度；而REnPF取得的滤波精度高于其他两种算法。因为REnPF算法综合使用GSA算法、贝叶斯公式、MCMC算法来获得从后验分布中抽取的粒子，而其中的EnKF步骤仅用于加快MCMC算法中的粒子移动速度，这使得REnPF能很好地避免粒子退化问题，因此REnPF能在仿真中获得最高的滤波精度。表4所列的对准结束时的姿态误差平均值也进一步证明了REnPF在非线性滤波方面的有效性。但从表5所列各算法的单历元平均运算耗时情况可以看出，REnPF的计算负担远大于其他两种算法，这主要是因为REnPF中GSA算法的累加求和过程以及MCMC算法的搜索过程会消耗大量的计算资源。

图5 无野值时30次蒙特卡洛实验中各滤波算法 所得姿态误差均值曲线Fig.5 Attitude error mean curves of 30 Monte Carlo experiments without outliers

3.3 仿真二：GPS存在野值

当GPS信号受到遮挡时，GPS输出的位置、速度中会存在野值。所以在仿真二中，仿真条件与仿真一的基本保持一致，仅在300 s后，每隔100 s给GPS输出的位置叠加15 m的误差作为位置观测野值，给GPS输出的速度叠加0.5 m/s的误差作为速度观测野值。对比算法选为不具有抗野值功能的传统EnPF(记为EnPF_old)，以及采用了文献[14]所提出的抗野值方法的传统EnPF(记为：REnPF_old)，该抗野值方法是根据新息的卡方分布设定门限，取显著性水平为25%时，其门限为6.626。仿真进行了多次，其对准结果大致相同，现选取其中一次的归一化新息的变化曲线和姿态误差曲线分别展示于图6和图7，而各算法在第600 s对准结束时的姿态误差结果列于表6中。

从图6可以看出，在0 s～100 s滤波尚未收敛时，归一化新息的幅值波动较大。尤其可从图6(b)看到REnPF_old算法的归一化新息中有幅值超过其所设门限(黑色虚线)，从而造成虚警现象。但由于滤波频率较快，且虚警现象在该仿真中没有频繁出现，所以对滤波精度影响较小。之后滤波逐渐收敛，直到野值出现，归一化新息明显地反应着野值的存在。结合图7可知，不具有抗野值功能的EnPF_old算法的滤波过程受野值影响较大，每次野值出现时，姿态误差曲线都因为错误修正而产生波动；REnPF_old在第400 s时也受到了野值的影响，结合图6(b)可知，这是因为第400 s时REnPF_old算法的归一化新息幅值小于了门限从而造成漏检；REnPF算法的姿态误差曲线较为平滑，这说明REnPF算法依靠模糊抗野值系统很好地检测出了野值并屏蔽了其影响，从而保证了滤波精度。表6所列第600 s的对准结果也从滤波精度方面说明了REnPF算法的优势。

图6 归一化新息的变化曲线Fig.6 Variation curves of the normalized innovation obtained by the different filters

图7 有野值时各滤波算法对准结果Fig.7 Alignment results of the different filters with outliers

表6 有野值时各滤波算法第600 s的姿态误差
Table 6 Attitude errors of the different filters at 600 s with outliers

滤波算法俯仰角/(°)横滚角/(°)方位角/(°)EnPF_old0.1673-0.19211.0151REnPF_old-0.08380.06290.5363REnPF-0.06080.02210.3097

4 结 论

针对GPS辅助的SINS行进间对准所存在的非线性和观测野值问题，本文提出了一种模糊抗野值集合粒子滤波算法(REnPF)。REnPF主要利用GSA算法拟合状态的先验概率密度函数，然后根据贝叶斯公式求取状态的后验概率密度函数，并利用MCMC算法从中抽取粒子，而EnKF算法被用于加快MCMC算法中粒子的移动速度，最后根据模糊理论为REnPF设计了模糊抗野值功能。在非线性滤波方面，REnPF因其能从后验分布中抽取粒子，所以避免了粒子退化和粒子贫化问题。而在抗野值方面，REnPF使用模糊权因子很好地避免了虚警、漏检问题。GPS辅助的SINS行进间对准仿真实验证明了REnPF能够胜任对观测存在野值的非线性系统的滤波工作。

但由于模糊系统中，模糊隶属度函数和模糊规则的设计依赖于设计者的经验，这会限制REnPF广泛适用性，不仅如此，由于REnPF是一种数值滤波方案，其中的GSA算法和MCMC算法存在严重的计算负担。因此设计出能对大多数系统都广泛适用的抗野值非线性滤波器，并降低其计算复杂度将是未来的工作之一。