一種用于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算法存在嚴重的計算負擔。因此設計出能對大多數系統都廣泛適用的抗野值非線性濾波器，并降低其計算復雜度將是未來的工作之一。