王 林，林雪原

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

?

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

王 林1，林雪原2

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

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

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

0 引言

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

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

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

1 UKF濾波算法

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

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

(1)

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

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

(2)

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

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

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

2.1 狀態(tài)方程與量測方程

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

(3)

(4)

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

狀態(tài)方程依據(jù)慣性導航系統(tǒng)工作原理展開式如式(5)所示。

(5)

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

(6)

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

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

2.3.1 初始化及樣本點計算

2.3.2 時間更新

2.3.3 狀態(tài)更新

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

通過sigma點集得到的狀態(tài)預測均值和方差為

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

計算狀態(tài)預測與量測估計的協(xié)方差為

計算濾波增益

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

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

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

圖1 UKF直接式濾波仿真系統(tǒng)結(jié)構(gòu)圖Fig.1 Structure chart of the UKF direct filtering demonstration system

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

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

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

圖3 姿態(tài)曲線Fig.3 The curve of attitude curve

圖4 姿態(tài)誤差曲線Fig.4 The curve of attitude error

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

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

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

4 結(jié)論

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

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

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

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

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

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

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

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

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

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

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

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

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

[13] 徐佳鶴，井龍偉.UKF濾波算法在INS/GPS組合導航系統(tǒng)中的研究[C]//.中國控制與決策學術(shù)年會論文集, 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 - )，男，碩士研究生，主要從事有關(guān)信息傳輸與處理技術(shù)方向的研究。

E-mail:wl19820416@sohu.com

TP273

A

2095-8110(2015)03-0043-06