改進的粒子濾波算法在船用組合導航中的應用

江健， 李偉峰， 姚健， 史國友

(大連海事大學 a. 航海學院； b. 航海安全保障重點實驗室， 遼寧 大連 116026)

0 引 言

慣性導航系統(inertial navigation system, INS)能夠在不依賴其他輔助設備的條件下進行自主導航，獲得較為準確的導航信息，具有數據更新頻率高、動態性能好等優點。然而基于慣性傳感器的INS由于其傳感器存在漂移誤差，其導航位置誤差會隨著時間積累從而造成跟蹤精度大幅降低，因此其無法進行長時間獨立導航。[1]全球定位系統(global positioning system, GPS)具有在長時間工作時定位精度高、不隨時間累積誤差等優點，可以對INS的信息進行補充與修正，極大地提高慣性導航的精度。[2]利用INS和GPS的優點所構建的INS/GPS船用組合導航系統能夠極大地增強導航系統的精度和整體性能[3]。

INS/GPS組合導航系統常用的信息處理方法大多是利用卡爾曼濾波思想進行基本的濾波[4],但對于非線性的INS/GPS組合導航系統，利用卡爾曼濾波并不能取得理想的濾波效果。解決此問題的通常做法是采用擴展卡爾曼濾波(extended Kalman filter, EKF)算法對非線性的系統進行濾波處理，但EKF算法采用線性誤差模型，是泰勒級數一階或二階近似的線性化結果，濾波結果存在較大的誤差，屬于一種非線性次優算法[5]。相比于卡爾曼濾波，粒子濾波(particle filter, PF)更適用于非線性/非高斯的動態系統[6-7]。PF算法在基于蒙特卡洛方法和遞推貝葉斯估計框架下通過一組帶有權值的隨機粒子來表示后驗密度函數，進一步通過樣本的均值計算出實際狀態的估計值。然而，PF算法存在缺陷，如重要性分布函數難以選取、重要性函數與似然函數不能很好地匹配、粒子退化嚴重等問題，從而導致濾波精度不高。DINI等[8]和JOHANSEN等[9]提出的unscented PF(UPF)算法通過無損變換(unscented transform, UT)在減少采樣粒子數的條件下近似得到粒子均值和方差，從而獲得狀態變量的后驗分布，這雖然保證了濾波的精度但也存在著粒子退化問題。PITT等[10]提出的輔助粒子濾波(auxiliary PF， APF)通過構建輔助變量使與觀測值更為接近的粒子被采樣的概率變大，從而使高似然區域的粒子數增加，但當觀測噪聲較為顯著時，基于最新的觀測值所優化的重要性函數(重要性采樣概率密度函數)并不能顯著地提高濾波精度。TORMA等[11]將重要性函數作為先驗概率密度函數，提出的自適應調整的PF算法在一定程度上降低了濾波對異常的敏感性，但其忽略了最新的觀測值對系統的影響，在傳感器存在較大的漂移誤差時，似然函數與重要性函數并不能很好地匹配[11]。雖然上述算法大多集中在利用最新的觀測值來優化重要性函數[8-11]，都是假設粒子在前一時刻獲得的估計值是準確的，但需要注意的是只有在觀測值相對準確時，上述算法才能獲得較高的精度[12]。由于INS的傳感器存在漂移誤差，位置誤差隨時間累積使其精度無法保證，所以以上UPF、APF等基于最新觀測值的優化算法并不能很好地適用于INS/GPS組合導航系統。本文在采用最新觀測值優化重要性函數的基礎上考慮最新觀測值偏移誤差的影響，通過計算獲得最新觀測值與預測觀測值之間的偏移誤差，進一步對狀態變量進行誤差修正，從而很好地解決INS隨時間累積誤差的問題，且在傳感器存在漂移誤差時呈現出很好的魯棒性。

1 INS/GPS組合導航系統數學模型

INS 的基本方程由速度方程、位置方程和姿態方程所構成，其在當地水平坐標系下的表達式[1,13-15]為

(1)

(2)

(3)

GPS/INS組合導航系統的非線性形式為

(4)

(5)

在INS/GPS組合導航系統中，GPS的觀測方程為

z(k)=H(k)x(k)+v(k)

(6)

式中：z(k)為GPS的觀測向量；H(k)為觀測矩陣。由于應用對象為艦船，這里僅考慮二維的平面運動，即

z(k)=(φgps,λgps,vE_gps,vN_gps)T

(7)

式中：φgps和λgps分別為GPS觀測到的船體位置的經度值和緯度值；vE_gps和vN_gps分別為GPS觀測到的東向速度和北向速度?；谑?5)和(6)，根據每一步帶有噪聲的觀測值集合{z1,z2,…,zk}得到系統狀態的估計值x(k)。

2 基本PF算法及其改進

2.1 基本PF算法

通過觀測序列{z1,z2，…,zk}來估計系統狀態x1,x2,…,xk。wk表示k時刻粒子的權值向量。通常系統的動態過程描述[16]為

在貝葉斯濾波框架的基礎上PF通過一組具有相關權值的隨機樣本表示后驗概率密度函數，并根據這些樣本和權重計算相應的估計值。在已知初始狀態概率密度p(x0|z0)的前提下，可以根據上一時刻狀態的后驗估計通過狀態轉移方程得到當前時刻的先驗概率密度p(xk|zi,i=1,2,…,k-1)，其預測方程為

p(xk|zi,i=1,2,…,k-1)=

(10)

根據貝葉斯原理，其狀態更新為

p(xk|zi,i=1,2,…,k)=

(11)

由于上式中后驗概率密度p(xk|zi,i=1,2,…,k)未知，故設計一個容易采樣的重要性分布函數q(xk)來代替重要性函數，其中

q(x0,x1,…，xk|zi,i=1,2,…,k)=

(12)

進一步通過當前時刻的觀測值yk對所有粒子的權值進行更新：

(13)

p(xk-1|zi,i=1,2,…,k-1)=

(14)

根據式(13)得每一粒子權值為

(15)

從而更新狀態概率密度為

(16)

PF算法存在一定的缺點，粒子的權值在似然更新后兩極化現象非常嚴重。大部分粒子權值趨于零，隨著迭代次數的增加必然會產生粒子退化現象。[17-18]可以采用重采樣或選取合適的重要性函數來克服這一問題。大多數PF算法(例如APF算法)選擇先驗概率密度函數為重要性函數，并利用最新的觀測值優化重要性函數，但當觀測噪聲較為明顯時，重要性函數與似然函數不能很好地匹配，只有極少的粒子能夠分布在有效的似然區間，造成粒子的權值迅速退化從而濾波器失效。針對這個問題，提出一種改進的PF(improved PF, IPF)算法，將最新的觀測值納入考慮范圍內，并根據當前的觀測值估計對應的狀態誤差，在確定粒子權值和重采樣之前對預測的粒子的誤差進行修正，從而有效抑制粒子權值退化同時提高濾波精度。

2.2 IPF算法

IPF算法具體步驟如下：

(17)

可以從似然函數(觀測方程)中進一步得到預測粒子的觀測值：

(18)

第二步，計算所有預測粒子的權值：

(19)

再對預測粒子的權值進行歸一化：

(20)

(21)

(22)

(23)

(24)

再根據式(25)修正粒子的權值，根據式(26)對修正后的權值進行歸一化處理。

(25)

(26)

進而得到狀態變量xk的后驗估計：

(27)

3 仿真分析

為驗證上述IPF算法有效性，結合INS/GPS船用組合導航系統數學模型，對EKF、APF和IPF算法應用于船用組合導航系統中的效果進行仿真對比。設某船的初始時刻位置為北緯38.874°、東經121.530°，初始速度為0，初始航向為50°。加速度計的隨機漂移為0.000 2g，零偏為0.001 2g。陀螺儀的隨機漂移為0.2(°)/h,測量白噪聲為0.04(°)/h，常值漂移為0.1(°)/h。INS的對準誤差為0.03°，初始速度誤差分別為0.4 m/s和0.5 m/s，初始位置誤差分別為4 m和5 m，初始姿態誤差均為0.1°。GPS觀測到的經度值和緯度值的標準偏差均為5×10-5，速度標準偏差分別為0.007 m/s和0.010 m/s。仿真中的粒子數為100，仿真時間為1 000 s，共進行60次蒙特卡洛仿真。圖1為在同一條件下對基于EKF、APF和IPF算法的INS/GPS組合導航系統進行仿真得到的船舶二維軌跡。

a)EKF算法

b)APF算法

c)IPF算法圖1 采用3種不同濾波算法的組合導航系統與INS、GPS給出的船舶二維軌跡對比

從圖1可以看出：由于INS的傳感器存在漂移誤差，INS的跟蹤軌跡與真實軌跡存在很大的誤差，且隨時間的推移逐漸偏離了真實軌跡。由于系統具有非線性，在這種情況下用EKF算法得到的軌跡誤差明顯大于用PF算法得到的軌跡誤差。進一步比較圖1b與1c可以看出：由于INS的傳感器存在較大的漂移誤差，采用APF的跟蹤效果并不是很理想；IPF算法在這種情況下表現出很好的濾波效果，其跟蹤軌跡與真實軌跡幾乎完全重合。

圖2～4分別為基于EKF、APF和IPF算法的位置和速度誤差曲線，其位置和速度的均方根誤差見表1。由圖2和表1可知，用EKF算法得到的位置和速度都存在較大的誤差。通過對3種算法對應的位置誤差和速度誤差的進一步比較可以看出：EKF算法在精度上遠遠低于PF算法，說明對于非線性系統，PF的效果要優于KF的效果；IPF算法對應的位置和速度誤差都明顯小于前兩種算法(EKF、APF算法)對應的位置和速度誤差，能夠很好地將位置誤差控制在±5 m之內，將速度誤差控制在±0.5 m/s之內，對于異常點的處理表現出更好的魯棒性。仿真結果表明，IPF算法與EKF、APF算法相比，能夠很好地克服PF算法中重要性函數與似然函數不匹配的問題，提高組合導航的濾波精度，且在慣性導航存在較大的觀測誤差時體現出很好的魯棒性。

a)位置b)速度

圖2基于EKF算法的位置和速度誤差曲線

a)位置b)速度

圖3 基于APF算法的位置和速度誤差曲線

圖4 基于IPF算法的位置和速度誤差曲線表1 基于INS/GPS的二維位置和速度均方根誤差

4 結束語

為解決粒子濾波(PF)算法在INS/GPS船用組合導航中進行濾波時重要性函數與似然函數不匹配造成的粒子權值嚴重退化、濾波精度迅速下降的問題，以及在傳感器具有較大的漂移誤差時，無法通過當前的觀測值對粒子狀態進行有效修正的問題，本文基于PF算法提出一種改進的粒子濾波(IPF)算法,根據當前的觀測值估計出觀測誤差，通過觀測方程的反函數求得相應的狀態誤差，進一步利用慣性導航方程準確地預測粒子狀態，實現對船舶軌跡的跟蹤。根據INS/GPS船用組合導航系統的數學模型，對IPF算法、擴展卡爾曼濾波(EKF)算法和輔助粒子濾波(APF)算法進行仿真對比。仿真結果表明，IPF算法能夠很好地解決上述問題，并保持很好的濾波精度。

參考文獻：

[1] 秦永元. 慣性導航[M]. 2版. 北京: 科學出版社, 2014: 3-5.

[2] 謝木生. 卡爾曼濾波在INS/GPS組合導航中的應用研究[D]. 長沙: 中南大學, 2013.

[3] 秦永元, 張洪鉞, 汪叔華, 等. 卡爾曼濾波與組合導航原理[M]. 西安: 西北工業大學出版社, 1998: 2-5.

[4] 于婷, 徐愛功, 付心如, 等. 一種自適應卡爾曼濾波組合導航定位方法[J]. 導航定位學報, 2017, 5(3): 101-104.

[5] 楊宏, 李亞安, 李國輝. 一種改進擴展卡爾曼濾波新方法[J]. 計算機工程與應用, 2010, 46(19): 18-20.

[6] WANG Yuru, TANG Xianglong, CUI Qing. Dynamic appearance model for particle filter based visual tracking[J]. Pattern Recognition, 2012, 45(12): 4510-4523.

[7] 王法勝, 魯明羽, 趙清杰, 等. 粒子濾波算法[J]. 計算機學報, 2014, 37(8): 1679-1694.

[8] DINI D H, MANDIC D P, JULIER S J. A widely linear complex unscented Kalman filter[J]. IEEE Signal Processing Letters, 2011, 18(11): 623-626.

[9] JOHANSEN A M, DOUCET A. A note on auxiliary particle filters[J]. Statistics & Probability Letters, 2008, 78(12): 1498-1504.

[10] PITT M K, SHEPHARD N. Filtering via simulation: auxiliary particle filters[J]. Journal of the American Statistical Association, 1999, 94: 590-599.

[11] TORMA P, SZEPESVRI C. Local importance sampling: a novel technique to enhance particle filtering[J]. Journal of Multimedia, 2006, 1(1): 32-43.

[12] LI Tiancheng, CORCHADO J M, BAJO J,etal. Effectiveness of Bayesian filters: an information fusion perspective[J]. Information Sciences, 2016, 329: 670-689.

[13] 孫楓, 唐李軍. 基于cubature Kalman filter的INS/GPS組合導航濾波算法[J]. 控制與決策, 2012, 27(7): 1032-1036.

[14] SAVAGE P G. Strapdown inertial navigation integration algorithm design part 1: attitude algorithms[J]. Journal of Guidance Control & Dynamics, 1998, 21(1): 19-28.

[15] SAVAGE P G. Strapdown inertial navigation integration algorithm design part 2: velocity and position algorithms[J]. Journal of Guidance Control & Dynamics, 1998, 21(2): 208-221.

[16] 向禮, 劉雨, 蘇寶庫. 一種新的粒子濾波算法在INS/GPS組合導航系統中的應用[J]. 控制理論與應用, 2010, 27(2): 159-163.

[17] 胡士強, 敬忠良. 粒子濾波算法綜述[J]. 控制與決策, 2005, 20(4): 361-365.

[18] 李天成, 范紅旗, 孫樹棟. 粒子濾波理論、方法及其在多目標跟蹤中的應用[J]. 自動化學報, 2015, 41(12): 1981-2002.