基于UKF的GPS/SINS組合導航直接式濾波算法

王 林，林雪原

(1.海軍航空工程學院 研究生管理大隊，山東 煙臺 264000；2.信息融合研究所，山東 煙臺 264000)

?

基于UKF的GPS/SINS組合導航直接式濾波算法

王 林1，林雪原2

(1.海軍航空工程學院 研究生管理大隊，山東 煙臺 264000；2.信息融合研究所，山東 煙臺 264000)

基于無跡卡爾曼濾波(UKF)方法，使用姿態、速度、位置等9個導航參數組成狀態向量，以GPS系統輸出的速度、位置組成6維觀測向量，構建直接式結構的UKF濾波器。該濾波器能夠直接反映系統導航參數的動態過程，準確顯示運動狀態演變。針對GPS/SINS組合導航系統的特點，構建了GPS/SINS組合導航直接式卡爾曼濾波仿真驗證系統，仿真結果驗證了基于UKF的GPS/SINS組合導航直接式濾波算法的有效性，該直接式非線性濾波算法可使慣性組合導航系統的導航精度得到提高。

慣性組合導航；無跡卡爾曼濾波；直接式濾波

0 引言

卡爾曼濾波是一種基于模型的線性最小方差估計，其標準離散型算法具有遞推計算、使用計算機執行、適用于平穩或非平穩多維隨機信號估計等獨特優點。所以在隨機信號處理中，特別是在組合導航設計中應用廣泛。根據卡爾曼濾波方法設計的組合導航濾波器，成為組合導航系統的核心。標準卡爾曼濾波只適用于系統方程和量測均為線性時的估計，但是工程實踐中所遇到的物理系統數學模型往往是非線性的，即系統方程是非線性的，或者系統和量測都是非線性的。盡管擴展卡爾曼濾波(EKF)可解決系統和量測為非線性時的估計，但必須用泰勒級數的方法對原系統和量測作展開并舍去非線性項，再用基礎卡爾曼濾波算法對處理后的系統和量測方程作濾波計算，可見EKF本質上仍然是基礎卡爾曼濾波。由于在線性化處理過程中舍棄了非線性的高階項，EKF只適用于濾波對象非線性較弱的情況下，被估計對象非線性性質越強，所產生的誤差就會越大，嚴重的會引起濾波發散[1-2]。

為了解決強非線性條件下的估計問題，出現了基于UT變換的UKF算法，為以直接式濾波方法解決強非線性條件下的慣性組合導航提供了契機。

本文將UKF應用于組合導航，建立慣性組合導航系統的直接式濾波模型，以導航參數作為狀態變量，以GPS系統的速度和位置組成觀測向量，實現直接式濾波關鍵計算流程，以微小型飛行器作為對象進行仿真驗證。

1 UKF濾波算法

UKF和標準卡爾曼濾波都是線性的最小方差估計，都是建立在模型基礎上的算法，兩者的不同點在于計算最佳增益陣的方法。標準卡爾曼濾波在計算最佳增益陣時，建立在系統方程和量測方程均為線性的基礎上。而UKF在最佳增益陣的計算中，對系統方程以及量測方程的性質沒有任何具體的約束，所以UKF算法既可以在線性條件下使用，也可以在非線性條件下使用[2]。

設系統方程和量測方程為：

(1)

其中k-1,k表示離散時刻，Xk為狀態預測，Xk-1為上一時刻的狀態估計，其方差為Pk-1，uk-1為系統確定性控制項，Wk-1為系統噪聲，其方差為Qk-1，Zk為量測值，Vk為量測噪聲，其方差為Rk，系統和量測噪聲均服從高斯分布。UKF濾波要解決的問題是在已知狀態初始值X0，系統確定性控制序列u0,u1,…,uk-1以及量測值序列Z1,Z2,…,Zk的情況下，估計系統的狀態變量Xk。

UKF采用遞推計算方式，主要包括兩個過程：狀態預測計算和修正計算。已知狀態變量的維數為n，UKF用UT變換的方式獲取2n+1個采樣點(sigma點)，實現狀態及其相關統計量的遞推計算。在k時刻，2n+1個對稱的采樣sigma點及各點對應的權值為[1-2]:

(2)

UKF算法的核心是UT變換，它能夠產生代表狀態量統計特性的最少數量的采樣點，具有一些比較好的特性：UKF方法中的狀態變量傳遞過程中通過的是沒有被近似處理的系統，精度較高，模型較精確；UKF在保證精度的條件下，其計算量相比粒子濾波(PF)要少很多。這些特點使得UKF成為一種重要的非線性濾波技術[1]。

2 基于UKF的慣性組合導航直接式濾波模型

基于標準卡爾曼濾波及擴展卡爾曼濾波的間接式組合導航系統，使用線性化的慣性組合導航系統的誤差方程作為狀態方程，以慣性組合導航系統的誤差作為被估狀態。而基于UKF的GPS/SINS組合導航直接式濾波則以捷聯慣性導航系統的機械編排方程作為狀態方程，直接使用導航參數做被估狀態，是一種直接式濾波方法，狀態方程及量測方程無需線性化。

2.1 狀態方程與量測方程

采用“東北天”地理坐標系作為導航坐標系，慣性導航系統采用指北方位機械編排，狀態方程如式(3)所示。

(3)

(4)

濾波過程中，噪聲進入非線性系統中，參與系統狀態轉移，所以如式(4)所示將系統噪聲和量測噪聲與狀態向量一起組成增廣向量Xa，增廣向量維數n為21，增廣的方差陣為Pa。

狀態方程依據慣性導航系統工作原理展開式如式(5)所示。

(5)

基于UKF的直接式卡爾曼濾波的觀測方程直接反映觀測系統的量測原理，以GPS輸出的速度和位置作為觀測量，量測方程為式(6)所示，V為量測噪聲。

(6)

2.3 基于UKF的慣性組合導航直接式濾波流程

基于UKF的慣性組合導航直接式濾波系統以狀態向量、系統噪聲與量測噪聲組成增廣向量，以GPS輸出的速度和位置作為觀測量，其濾波流程分為三個部分：初始化及樣本點計算、時間更新、狀態更新。

2.3.1 初始化及樣本點計算

2.3.2 時間更新

2.3.3 狀態更新

獲得測量值及預測值后，進行新息計算，通過濾波增益矩陣，得到每個狀態對應的誤差修正量，UKF中新息及濾波增益的計算都使用到對狀態預測點集、測量預測點集均值和方差的計算。

通過sigma點集得到的狀態預測均值和方差為

估計觀測值的均值和方差為

計算狀態預測與量測估計的協方差為

計算濾波增益

通過增益矩陣，獲得各個狀態的誤差修正量，狀態和方差更新為

3 基于UKF的慣性組合導航直接式濾波仿真驗證

以微小型飛行器作為使用載體構建圖1所示的濾波仿真驗證系統，采用飛行器動態仿真和真實的導航傳感器測量誤差數據合并生成動態數據的方法，慣性元件和GPS誤差數據從實驗室實際測試的數據中注入。飛行軌跡按微小型飛行器性能特點及使用需求設定，包括如下過程：加速—爬升—平直飛行—爬升—平直飛行—轉彎—平直飛行—再一次爬升—平直飛行—轉彎—下滑—盤旋狀態，如圖2。仿真時間為3600s。

圖1 UKF直接式濾波仿真系統結構圖Fig.1 Structure chart of the UKF direct filtering demonstration system

圖2 飛行仿真的航跡Fig .2 Simulation of the flight trace

微小型飛行器的初始姿態定為：飛行器載體水平，航向90°，位置(118°，29°，50m)，UKF濾波初始參數：慣性器件噪聲參數，狀態初值和初始方差陣的值，UKF中的sigma點伸縮因子等。根據目前在用的慣性器件的性能，設定方差值，設陀螺噪聲的均方根為0.5(°)/s，設加速度計噪聲的均方根為10-3g，設GPS測速誤差為0.5m/s，定位誤差8m。

基于UKF的GPS/SINS組合導航仿真系統得到的姿態曲線如圖3所示，姿態誤差如圖4所示，定位誤差分別如圖5所示，系統的穩態誤差值如表1所示。

圖3 姿態曲線Fig.3 The curve of attitude curve

圖4 姿態誤差曲線Fig.4 The curve of attitude error

圖5 位置誤差曲線Fig.5 The curve of position error

誤差項姿態角/(°)橫滾俯仰航向最大值0.33410.29575.5703最小值-0.2491-0.1106-3.5843均 值-0.00090.0000-0.0422均方差0.02380.02920.5561誤差項位置/m經度緯度高度最大值6.99696.28757.5442最小值-6.1195-6.1117-7.7811均 值0.05430.0562-0.0365均方差1.92251.92301.3565

由仿真圖像及數據表明，基于UKF的慣性組合導航直接式濾波方法穩定，姿態誤差和位置誤差較小，具有較高的導航精度。在微小型飛行器特性的飛行條件下，該直接式濾波方法能夠適應于各種飛行狀態。

4 結論

本文研究了基于UKF的直接式慣性組合導航濾波方法，建立直接式結構的卡爾曼濾波器，以導航參數作為狀態變量，以GPS系統的速度和位置組成觀測向量，實現了直接式濾波計算流程。利用方向余弦矩陣和姿態角間的關系，實現了狀態向量中姿態角sigma點的生成與各sigma點姿態角誤差均值及方差的計算。本文構建了GPS/SINS組合導航直接式卡爾曼濾波仿真驗證系統，仿真結果驗證了該直接式非線性濾波方法的有效性，采用此方法可有效避免將非線性濾波模型進行線性化后的誤差，適于解決高機動條件下飛行器濾波及狀態估計。

[1] 劉建業，曾慶華，趙偉,等.導航系統理論與應用[M].西安:西北工業大學出版社,2010：296-366.

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

[3] 高社生，何鵬舉，楊波等.組合導航原理及應用[M].西安:西北工業大學出版社,2012:92-108.

[4] 林雪原，劉華章，史佩.一種低成本、低精度SINS/GPS組合導航系統級試驗研究[J].武漢大學學報(信息科學版)，2006(10).

[5] 林雪原，林炳榮.基于量測多尺度預處理的GPS/SINS組合導航系統[J].兵工自動化，2010(7).

[6] 林雪原.GPS/SINS組合導航系統與實現[J].計算機測量與控制，2007(12).

[7] 林雪原，駱卉子，范文強.一種改進的GPS動態定位濾波方法[J].海軍航空工程學院學報，2010(4).

[8] 林雪原.GPS/SINS組合導航系統的多尺度融合算法研究[J].電子科技大學學報，2011(9).

[9] 陸海勇.捷聯慣性導航系統中UKF濾波技術的應用研究[D].南京：碩士學位論文,南京航空航天大學, 2008.

[10] 周菊華.UKF濾波方法在捷聯慣導系統中的應用研究[D].南京：碩士學位論文,東南大學,2009.

[11] 徐佳.INS/GPS組合導航系統濾波算法的研究[D].沈陽：碩士學位論文,東北大學,2006.

[12] 劉錚.UKF算法及其改進算法的研究[D].長沙：碩士學位論文,中南大學,2009.

[13] 徐佳鶴，井龍偉.UKF濾波算法在INS/GPS組合導航系統中的研究[C]//.中國控制與決策學術年會論文集, 2007: 213-216.

[14] S J Julier, J K Uhlmann. Unscented Filtering and nonlinear Estimation[A]. Proceeding of the IEEE[C]//.vol.92 No3, March 2004.

Direct Kalman Filtering Algorithm for the GPS/SINS Integrated Navigation Based on UKF

WANG Lin1，LIN Xue-yuan2

(1.Naval Aeronautical and Astronautical University Graduate Students’ Brigade,Yantai 264000,China;2.Research Institute of Information Fusion, Yantai 264000,China)

A Kalman filter with direct structure is proposed based on UKF filter. The direct filter takes 9 parameters as state vectors such as attitude, velocity and position. And the six-dimension observation vectors are made up of velocity and position provided by Global Positioning System. The filter directly describes navigation parameters dynamic process of the system and reflects the evolution of motion state. A direct filtering demonstration system for the GPS/SINS Integrated Navigation is constructed according to the characteristics of GPS/SINS integrated navigation system. The simulation results verifies the effectiveness of the direct Kalman filtering method based on UKF and the filtering method can improve the navigation accuracy.

Inertial integrated navigation; Unscented Kalman filter; Direct filtering

2015 - 02 - 05；

2015-03-12。

國家自然科學基金(60874112)

王林(1982 - )，男，碩士研究生，主要從事有關信息傳輸與處理技術方向的研究。

E-mail:wl19820416@sohu.com

TP273

A

2095-8110(2015)03-0043-06