一種改進的粒子濾波算法在SINS初始對準中的應(yīng)用

徐曉蘇，劉心雨

（1. 微慣性儀表與先進導(dǎo)航技術(shù)教育部重點實驗室，南京 210096；2. 東南大學(xué) 儀器科學(xué)與工程學(xué)院，南京 210096）

一種改進的粒子濾波算法在SINS初始對準中的應(yīng)用

徐曉蘇1,2，劉心雨1,2

（1. 微慣性儀表與先進導(dǎo)航技術(shù)教育部重點實驗室，南京 210096；2. 東南大學(xué) 儀器科學(xué)與工程學(xué)院，南京 210096）

在實際工程環(huán)境中，針對捷聯(lián)慣導(dǎo)系統(tǒng)（SINS）大失準角初始對準中噪聲統(tǒng)計特性未知的問題，設(shè)計了一種基于H∞濾波算法的魯棒無跡粒子濾波算法（RUPF）。通過將無跡卡爾曼濾波算法（UKF）和魯棒環(huán)節(jié)引入到粒子濾波(PF)的重要性密度函數(shù)中，得到了RUPF算法，提高了算法的魯棒性。通過半物理實驗，將RUPF算法與無跡粒子濾波算法（UPF）在SINS靜基座大失準角對準中的性能進行了比較，在不同實驗條件下，航向失準角精度至少提高了40%，對準精度優(yōu)于0.05°，對準時間減少了約 50 s。實驗結(jié)果表明，RUPF算法可以以較高的精度和較快的速度完成大失準角初始對準，且對準精度和對準速度均優(yōu)于UPF算法。

魯棒無跡粒子濾波；捷聯(lián)慣性導(dǎo)航系統(tǒng)；初始對準；大失準角

在捷聯(lián)慣性導(dǎo)航（SINS）中，初始對準是一項關(guān)鍵技術(shù)，其精度直接影響到慣性導(dǎo)航的精度。當初始失準角較小時，SINS 的誤差模型可以簡化成線性模型，卡爾曼( Kalman) 濾波器是有效的濾波工具; 當初始失準角較大時，需建立 SINS非線性誤差模型，需利用非線性濾波器才能準確估計出失準角。

目前使用較為廣泛的非線性濾波器有三大類：擴展卡爾曼濾波（EKF）[1]、無跡卡爾曼濾波（UKF）[2]和粒子濾波（PF）[3]。EKF算法是將非線性函數(shù)進行泰勒級數(shù)展開并保留線性項以獲取線性模型，對于弱非線性系統(tǒng)可以取得較好的濾波效果，但對于強非線性系統(tǒng)濾波效果較差，EKF算法會不可避免地引入截斷誤差且雅可比矩陣計算較為復(fù)雜。UKF是利用UT變換得到一組 sigma采樣點，經(jīng)過非線性函數(shù)映射，得到隨機變量的均值與方差，這種算法避免了線性化誤差，具有較高的估計精度，但是隨著系統(tǒng)維數(shù)的增大，UKF算法估計精度急劇下降，甚至可能會發(fā)散。PF算法是一種基于隨機采樣的濾波方法，它是一個完全的非線性器，該方法直接根據(jù)概率密度計算條件均值，理論上不受模型線性和高斯假設(shè)的限制，可用于非線性非高斯的隨機系統(tǒng)。

PF算法的核心是合理選擇重要性密度函數(shù)，重要性密度函數(shù)與真實的密度越接近，濾波效果就越好，反之則越差，甚至有可能發(fā)散。文獻[4]和文獻[5]通過將粒子濾波與EKF或UKF組合起來的方法，改善粒子濾波器性能。在這些方法中，重要性密度函數(shù)由EKF或UKF來確定，這樣既可以解決粒子退化的問題，又能使粒子更新時獲得量測量的最新驗后信息，有利于粒子移向似然比高的區(qū)域[6]。

然而以上濾波方法都需要知道噪聲的一些統(tǒng)計信息。但慣性系統(tǒng)的固有特性以及系統(tǒng)初始對準時的實際工作環(huán)境具有隨機不確定性，因此其噪聲統(tǒng)計特性的數(shù)學(xué)描述具有明顯的不確定性，那么在噪聲統(tǒng)計信息未知情況下，濾波精度極有可能出現(xiàn)急劇下降甚至發(fā)散。針對模型的非線性以及未知模型噪聲統(tǒng)計特性的問題，本文將魯棒性較強的H∞濾波算法與無跡粒子濾波算法（UPF）相結(jié)合得到魯棒無跡粒子濾波算法（RUPF），用來提高初始對準的估計精度與魯棒性。

1 簡化UPF算法

由UKF產(chǎn)生PF的重要性密度函數(shù)稱為無跡粒子濾波器(UPF)，由UKF產(chǎn)生的重要性密度函數(shù)與真實狀態(tài)概率密度函數(shù)的支集重疊部分更大，估計精度較高。在這個算法中，每個量測量時刻采用UKF對粒子進行迭代，并利用這些量測值對粒子進行重采樣。這就好比同時運用N個卡爾曼濾波器（每一個粒子對應(yīng)一個濾波器），在每次量測后再進行一步重采樣。

設(shè)系統(tǒng)方程和量測方程分別為

式中：狀態(tài)方程為非線性方程，量測方程為線性方程；Xk是系統(tǒng)狀態(tài)向量； f(·)是非線性函數(shù)；Hk為量測陣；Vk為量測噪聲序列，Wk為系統(tǒng)激勵噪聲序列，其中Wk和 Vk為互不相關(guān)的高斯分布白噪聲，均值為零，方差陣分別為Qk和Rk。

文獻[7]說明了簡化UKF算法，使用簡化UKF算法產(chǎn)生PF的重要性密度函數(shù)，由此可以推出簡化UPF算法。該算法步驟如下：

步驟 1：初值確定（k= 0）。根據(jù)先驗概率密度p(X0)采樣生成粒子初始值X，i= 1,2,…,N 。

步驟2：簡化UKF濾波（k=1,2…）。根據(jù)簡化UKF算法對每個粒子進行時間更新和量測更新，計算簡化UKF濾波后的系統(tǒng)狀態(tài)估計 以及協(xié)方差Pki。

步驟3：重要性采樣（k=1,2…）。依據(jù)簡化UKF濾波結(jié)果構(gòu)造建議分布函數(shù)：

步驟4：二次采樣。采用SIR法或殘差二次采樣法對原始粒子集進行二次采樣，重新計算權(quán)重系數(shù)Wkj=，從而得到新的支撐粒子集：

步驟5：根據(jù)二次采樣粒子得到濾波值。

2 H∞濾波與RUPF算法

2.1 H∞濾波算法

H∞濾波算法是從H∞線性控制理論的基礎(chǔ)上發(fā)展起來的，它不需要知道環(huán)境噪聲的統(tǒng)計特性等先驗信息。其本質(zhì)就是建立一個從干擾輸入到誤差輸出的H∞范數(shù)最小的濾波器，用于解決濾波系統(tǒng)和外界干擾存在不確定性的問題，因此該算法對系統(tǒng)模型誤差和外界干擾具有很強的魯棒性[8-9]?？紤]如下隨機線性離散時間系統(tǒng)：

Xk是系統(tǒng)狀態(tài)向量；Φk,k-1為狀態(tài)轉(zhuǎn)移矩陣；Hk為量測陣； Vk為量測噪聲序列；Wk為系統(tǒng)激勵噪聲序列； Lk是自定義矩陣（假設(shè)它滿秩）。根據(jù)文獻[10]，H∞濾波算法狀態(tài)方程如下：

步驟1：初值確定：

步驟2：狀態(tài)估計：

步驟3：估計狀態(tài)線性組合：

步驟4：濾波增益：

步驟5：計算R,ek：

步驟6：計算Riccati方程：

為了將UPF濾波算法應(yīng)用于H∞濾波器，首先對線性H∞濾波方法中實現(xiàn)狀態(tài)估計協(xié)方差陣遞推的Riccati方程進行轉(zhuǎn)換，狀態(tài)協(xié)方差陣Pk的H∞魯棒更新形式可以寫成[11]：

下標k|k-1表示由k-1時刻求得k時刻的一步預(yù)測。

矩陣這樣變換后，H∞濾波便與卡爾曼濾波具有相同的形式，不同之處是在卡爾曼濾波算法基礎(chǔ)上引入了γ來調(diào)節(jié)系統(tǒng)的魯棒性，只有當γ滿足如下條件時，非線性的H∞濾波算法才能夠正常工作。

γ是調(diào)節(jié)算法魯棒性與穩(wěn)定性的“調(diào)節(jié)因子”。如果γ過大，則算法的魯棒性將會降低；如果γ取的過小，在濾波過程中，有可能無法滿足上式中的限制條件，造成濾波器無法正常工作。

對于非線性系統(tǒng)模型，利用UPF算法求解H∞濾波器中隨機變量經(jīng)非線性變換之后的均值和方差，就可得到RUPF非線性濾波算法。

2.2 RUPF算法

設(shè)系統(tǒng)方程和量測方程如式(1)所示，不同的是Wk和Vk分別為統(tǒng)計特性未知的系統(tǒng)噪聲和觀測噪聲。假設(shè)Wk和Vk滿足：

其中：δij為δ函數(shù)；Qk為系統(tǒng)噪聲的方差陣，Rk為量測噪聲的方差陣，實際上這兩者統(tǒng)計特性未知，這只是估計值，使用H∞濾波算法可以提高算法的魯棒性，使估計值接近真實值，以適應(yīng)不同的環(huán)境噪聲統(tǒng)計特性。

RUPF濾波算法流程如下：

步驟1：初始化

假設(shè)初始狀態(tài)的概率密度函數(shù)p(X0) 已知，方差陣為P0?；趐(X0)生成粒子初值 χ，并選取ω=p(χ0(i))，i=1,2,…,N。選擇N時需綜合考慮計算量與估計精度。為了簡化計算，假設(shè)每個粒子都服從正太分布N(,P0i)，其中，0(i)=χ0(i)，=P0。對于i=1,2,…,N ，k=1,2,3,…執(zhí)行。

步驟2：對于采樣時刻 k=1,2,3,…進行RUKF濾波計算：

1）計算σ樣本點：

2）時間更新

正態(tài)分布β=2.

3）量測更新

步驟3：選取重要性函數(shù)

步驟4：計算如下權(quán)重系數(shù)：

步驟6：根據(jù)二次采樣粒子計算濾波值：

RUPF算法通過調(diào)整γ值來調(diào)整Pk，以犧牲一定的精度為代價來換取濾波算法的魯棒性。參數(shù)γ控制狀態(tài)估計在最不利條件下的估計誤差，約束水平γ越小，則系統(tǒng)的魯棒性越強。

3 SINS大失準角非線性誤差模型

本文將RUPF算法應(yīng)用于捷聯(lián)慣導(dǎo)系統(tǒng)中。本節(jié)將依據(jù)參考文獻[12]建立以 SINS誤差方程為基礎(chǔ)的組合導(dǎo)航系統(tǒng)非線性誤差方程和線性量測方程。

選取“東北天”地理坐標系為導(dǎo)航坐標系n系；選取載體“右前上”坐標系為載體坐標系b系。n系先后經(jīng)過3次歐拉角轉(zhuǎn)動至b系，三個歐拉角記為航向角，縱搖角，橫搖角；n系與b系之間的姿態(tài)矩陣記為 C；真實姿態(tài)角記為；真實速度記為；真實地理坐標系為P=[L λ H]T，其中，L是緯度，λ是經(jīng)度，H是高度；SINS解算出的姿態(tài)角記為，速度記為，地理坐標系為；SINS解算出的數(shù)學(xué)平臺記為n′系，n′系與 b系之間的姿態(tài)矩陣記為 C；記姿態(tài)角誤差為；速度誤差為，位置誤差為。則非線性誤差模型如下：

cφi和sφi分別代表cosφi和sinφi，i=n,u,e。

非線性誤差方程建立過程如下：

以采樣周期T作為濾波周期，可以使用四階龍格-庫塔積分方法，以T為步長將其離散化，記離散后狀態(tài)濾波方程為

線性量測方程建立過程如下：

對SINS的水平速度輸出作如下分解：

式中：VE和VN為載體的理想速度。若載體有線運動時，該速度可由GPS等設(shè)備提供。在靜基座下載體無線運動，因此

同理對SINS的緯度輸出和經(jīng)度輸出做如下分解：

式中，L和λ為載體的理想緯度和經(jīng)度，該位置信息可由 GPS等設(shè)備提供。在靜基座下，載體無位置移動且載體所在的地理位置精確已知，即L和λ已知。所以量測方程為

式中，V為噪聲陣，量測矩陣為

同樣以T作為濾波周期，并以T作為步長進行簡單離散化，得離散化后的量測方程為

綜上，由狀態(tài)方程和量測方程組成如下非線性濾波方程為

4 半物理實驗結(jié)果與分析

SINS解算模塊采集到慣性測量單元（IMU）模塊輸出的陀螺輸出值和加速度計輸出值進行捷聯(lián)解算，得到姿態(tài)角、姿態(tài)矩陣、速度、位置等信息；靜基座下載體真實速度為0，且真實位置信息已知。將SINS輸出的信息輸入到RUPF濾波器中，進行信息的濾波處理，系統(tǒng)方案如圖1所示。

為了進一步驗證RUPF算法的有效性，利用實驗室的光纖陀螺捷聯(lián)慣性系統(tǒng)設(shè)備（FOSN）在三軸高精度轉(zhuǎn)臺上進行實際環(huán)境下的工程驗證。三軸轉(zhuǎn)臺所在實驗室的地理位置為北緯32.057 305°N，東經(jīng)118.786 389 °E。分別采用UPF算法和RUPF算法進行SINS靜基座大失準角初始對準的半物理實驗，觀察實際情況下RUPF算法的效果。實驗時，將 FOSN固聯(lián)在三軸轉(zhuǎn)臺上，標定出FOSN與轉(zhuǎn)臺之間的安裝誤差角，采集FOSN捷聯(lián)慣性儀表敏感的角速度信息和加速度信息，利用所設(shè)計的算法完成導(dǎo)航運算。有以下四種情況：

圖1 系統(tǒng)方案圖Fig.1 System schematic diagram

當真實的初始姿態(tài)角為θ=0°、γ=0°、Ψ=0°時，初始姿態(tài)角為θ=? 5°、γ=? 5°、Ψ=? 20°和θ=? 15°、γ=? 15°、Ψ=? 30°。

當真實的初始姿態(tài)角為θ=0°、γ=0°、Ψ=90°時，初始姿態(tài)角為θ=? 5°、γ=? 5°、Ψ=? 115°和θ=? 10°、γ=? 10°、Ψ=? 130°。

利用UPF、RUPF算法進行SINS靜基座大失準角初始對準，對準時間為600 s。兩種算法在上述兩種大失準角情況下的失準角估計誤差統(tǒng)計表如表1和表2所示，其中均值與標準差是用在對準結(jié)束后100 s內(nèi)，即600 s至700 s時間段內(nèi)數(shù)據(jù)計算出的。四種情況下失準角估計誤差曲線分別如圖2(a)、2(b) 和圖3(a)、3(b)所示。圖 2和表 1真實的初始姿態(tài)角為θ=0°、γ=0°、Ψ=0°；圖 3和表 2真實的初始姿態(tài)角為θ=0°、γ=0°、Ψ=90°。

由圖2可知，在靜基座大失準角情況下，當真實的初始姿態(tài)角為θ=0°、γ=0°、Ψ=0°，設(shè)置初始姿態(tài)角為θ=?5°、γ=?5°、Ψ=? 20°時，這兩種方法水平失準角誤差曲線較為接近，對準精度高，時間短，水平對準精度在 0.012°以內(nèi)，水平失準角大約在 200 s后基本收斂。但在航向失準角方面RUPF算法精度明顯優(yōu)于UPF算法，RUPF算法誤差在0.027°而 UPF算法誤差在0.055°，精度提高了約50%。RUPF在對準時間約為350 s時航向失準角基本收斂，而UPF在400 s時收斂；當設(shè)置初始失準角為θ=?15°、γ=?15°、Ψ=? 30°時，這兩種方法水平失準角誤差曲線也較為接近，但航向失準角RUPF算法精度明顯優(yōu)于UPF算法，

RUPF算法誤差在0.042°而 UPF算法誤差在0.083°，精度提高了約50%。RUPF在對準時間約為400 s時航向失準角基本收斂，而UPF在450 s時收斂。

表1 失準角估計誤差統(tǒng)計Tab.1 Statistics on misalignment estimation errors of UPF and RUPF

表2 失準角估計誤差統(tǒng)計Tab.2 Statistics on misalignment estimation errors of UPF and RUPF

圖2(a) 失準角估計誤差Fig.2(a) Estimation errors of misalignment angle

圖3(a) 失準角估計誤差Fig.3(a) Estimation errors of misalignment angle

圖2(b) 失準角估計誤差Fig.2(b) Estimation errors of misalignment angle

圖3(b) 失準角估計誤差Fig.3(b) Estimation errors of misalignment angle

與上述結(jié)論一樣，由圖3可知，當真實的初始姿態(tài)角為θ=0°、γ=0°、Ψ=90°，設(shè)置初始姿態(tài)角為θ=?5°、γ=?5°、Ψ=? 115°時，這兩種方法水平失準角誤差曲線較為接近，但在航向失準角方面RUPF算法精度明顯優(yōu)于UPF算法，RUPF算法誤差在0.0328°，而 UPF算法誤差在0.0571°，精度提高了約42.5%。RUPF在對準時間約為400 s時，航向失準角基本收斂，而UPF在450 s時收斂；當設(shè)置初始姿態(tài)角為θ=?5°、γ=?5°、Ψ=? 115°時，這兩種方法水平失準角誤差曲線較為接近，但在航向失準角方面RUPF算法精度明顯優(yōu)于UPF算法，RUPF算法誤差在0.0499°，而 UPF算法誤差在0.0909°，精度提高了約40%。RUPF在對準時間約為450 s時航向失準角基本收斂，而UPF在500 s時收斂。當失準角增大后，兩種算法的對準收斂速度都在變慢，精度也在下降，但RUPF算法下降較小。

通過以上分析可以發(fā)現(xiàn)，在實際工程環(huán)境中，當失準角較大的情況下，RUPF算法可以以較高的精度和較快的速度完成初始對準，且對準精度特別是航向角對準精度高于UPF算法，對準速度也更快。

5 結(jié) 論

本文采用魯棒無跡粒子濾波(RUPF)算法對SINS靜基座大失準角初始對準進行了研究。魯棒環(huán)節(jié)的引入使得RUPF算法具有很強的魯棒性。在半物理實驗中將RUPF算法與UPF算法進行了對比，實驗結(jié)果表明，在實際工程環(huán)境中，當系統(tǒng)噪聲陣和觀測噪聲陣無法準確獲知并且失準角較大時，RUPF算法的初始對準精度較高，對準速度較快，并且該算法對準誤差，特別是航向角對準誤差，遠小于UPF算法。因此該算法具有很強的工程應(yīng)用價值。

（Reference）：

[1] Sebesta K D, Boizot N. A real-time adaptive high-gain EKF, applied to a quadcopter inertial navigation system [J]. IEEE Transactions on Industrial Electronics, 2014, 61(1): 495-503.

[2] Li W, Wang J, Lu L, et al. A novel scheme for DVL-aided SINS in-motion alignment using UKF techniques[J]. Sensors, 2013, 13(1): 1046-1063.

[3] Chen Z, Qu Y, Zhang T, et al. Hybrid adaptive particle swarm optimized particle filter for integrated navigation system[J]. Computer Modeling in Engineering & Sciences, 2015, 106(6): 379-393.

[4] Chen X, Shen C, Zhao Y. Study on GPS/INS system using novel filtering methods for vessel attitude determination [J]. Mathematical Problems in Engineering, 2013(1): 289-325.

[5] Jwo D J, Yang C F, Chuang C H, et al. A novel design for the ultra-tightly coupled GPS/INS navigation system[J]. Journal of Navigation, 2012, 65(4): 717-747.

[6] 秦永元, 張洪鉞, 汪叔華. 卡爾曼濾波與組合導(dǎo)航原理[M]. 西安: 西北工業(yè)大學(xué)出版社, 2011. Qin Yong-yuan, Zhang Hong-yue,Wang Shu-hua. Kalman filter and the principle of integrated navigation[M]. Xi’an, China: Northwestern Polytechnical University Press, 2011.

[7] 嚴恭敏, 嚴衛(wèi)生, 徐德民. 簡化UKF濾波在SINS大失準角初始對準中的應(yīng)用[J]. 中國慣性技術(shù)學(xué)報, 2008, 16(3): 253-264. Yan Gong-min, Yan Wei-sheng, Xu De-min. Application of simplified UKF in SINS initial alignment for large misalignment angles[J]. Journal of Chinese Inertial Technology, 2008, 16(3): 253-264.

[8] Wan-xin S. Application of H∞ filtering algorithm in SINS/GPS integrated navigation system[C]//2014 2nd International Conference on Information Technology and Electronic Commerce. IEEE, 2014: 72-76.

[9] Yu F, Lv C, Dong Q. A novel robust H∞ filter based on Krein space theory in the SINS/CNS attitude reference system[J]. Sensors, 2016, 16(3): 396.

[10] Liu X, Xu X, Wang L, et al. H∞filter for flexure deformation and lever arm effect compensation in M/S INS integration[J]. International Journal of Naval Architecture and Ocean Engineering, 2014, 6(3): 626-637.

[11] Einicke G A, White L B. Robust extended Kalman filtering[J]. IEEE Transactions on Signal Processing, 1999, 47(9): 2596-2599.

[12] 孫進, 徐曉蘇, 劉義亭, 等. 基于自適應(yīng)無跡粒子濾波的SINS大方位失準角初始對準[J]. 中國慣性技術(shù)學(xué)報, 2016, 24(2): 154-159. Sun Jin, Xu Xiao-su, Liu Yi-ting, et al. Initial alignment of large azimuth misalignment in SINS based on adaptive unscented particle filter[J]. Journal of Northwestern Polytechnical University, 2016, 24(2): 154-159.

Improved particle filter algorithm in SINS initial alignment

XU Xiao-su1,2, LIU Xin-yu1,2
(1. Key Laboratory of Micro-inertial Instrument and Advanced Navigation Technology, Ministry of Education, Southeast University, Nanjing 210096, China; 2. School of Instrument Science and Engineering, Southeast University, Nanjing 210096, China)

In real engineering environments, the noise statistical characteristics are unknown in the initial alignment of SINS with large misalignment angle. To solve this problem, a RUPF algorithm is designed based on H∞filtering algorithm. By combining UKF algorithm and robust link into importance density function in PF, the RUPF algorithm is obtained to improve the robustness of this algorithm. By means of emi-physical experiment, the filter performance of RUPF and UPF in SINS initial alignment on a static base is compared with that of large misalignment angles under various experimental conditions, which show that the accuracy of heading misalignment is increased by at least 40%, the alignment accuracy is better than 0.05°, and the alignment time is reduced about 50 s. These results show that the RUPF can realize the initial alignment of SINS with large misalignment angles, whose alignment accuracy and alignment speed are higher than those of UPF.

robust unscented particle filter; SINS; initial alignment; large misalignment angle

U666.1

：A

2016-03-30；

：2016-04-12

國家自然科學(xué)基金項目（51175082，61473085）資助

徐曉蘇（1961—），男，博士生導(dǎo)師，從事測控技術(shù)與導(dǎo)航定位領(lǐng)域的研究。E-mail: xxs@seu.edu.cn

1005-6734(2016)03-0299-07

10.13695/j.cnki.12-1222/o3.2016.03.005