基于UKF的GPS/SINS組合導(dǎo)航直接式濾波算法

王 林，林雪原

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

?

基于UKF的GPS/SINS組合導(dǎo)航直接式濾波算法

王 林1，林雪原2

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

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

慣性組合導(dǎo)航；無(wú)跡卡爾曼濾波；直接式濾波

0 引言

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

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

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

1 UKF濾波算法

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

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

(1)

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

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

(2)

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

2 基于UKF的慣性組合導(dǎo)航直接式濾波模型

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

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

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

(3)

(4)

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

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

(5)

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

(6)

2.3 基于UKF的慣性組合導(dǎo)航直接式濾波流程

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

2.3.1 初始化及樣本點(diǎn)計(jì)算

2.3.2 時(shí)間更新

2.3.3 狀態(tài)更新

獲得測(cè)量值及預(yù)測(cè)值后，進(jìn)行新息計(jì)算，通過(guò)濾波增益矩陣，得到每個(gè)狀態(tài)對(duì)應(yīng)的誤差修正量，UKF中新息及濾波增益的計(jì)算都使用到對(duì)狀態(tài)預(yù)測(cè)點(diǎn)集、測(cè)量預(yù)測(cè)點(diǎn)集均值和方差的計(jì)算。

通過(guò)sigma點(diǎn)集得到的狀態(tài)預(yù)測(cè)均值和方差為

估計(jì)觀測(cè)值的均值和方差為

計(jì)算狀態(tài)預(yù)測(cè)與量測(cè)估計(jì)的協(xié)方差為

計(jì)算濾波增益

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

3 基于UKF的慣性組合導(dǎo)航直接式濾波仿真驗(yàn)證

以微小型飛行器作為使用載體構(gòu)建圖1所示的濾波仿真驗(yàn)證系統(tǒng)，采用飛行器動(dòng)態(tài)仿真和真實(shí)的導(dǎo)航傳感器測(cè)量誤差數(shù)據(jù)合并生成動(dòng)態(tài)數(shù)據(jù)的方法，慣性元件和GPS誤差數(shù)據(jù)從實(shí)驗(yàn)室實(shí)際測(cè)試的數(shù)據(jù)中注入。飛行軌跡按微小型飛行器性能特點(diǎn)及使用需求設(shè)定，包括如下過(guò)程：加速—爬升—平直飛行—爬升—平直飛行—轉(zhuǎn)彎—平直飛行—再一次爬升—平直飛行—轉(zhuǎn)彎—下滑—盤旋狀態(tài)，如圖2。仿真時(shí)間為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點(diǎn)伸縮因子等。根據(jù)目前在用的慣性器件的性能，設(shè)定方差值，設(shè)陀螺噪聲的均方根為0.5(°)/s，設(shè)加速度計(jì)噪聲的均方根為10-3g，設(shè)GPS測(cè)速誤差為0.5m/s，定位誤差8m。

基于UKF的GPS/SINS組合導(dǎo)航仿真系統(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

誤差項(xiàng)姿態(tài)角/(°)橫滾俯仰航向最大值0.33410.29575.5703最小值-0.2491-0.1106-3.5843均 值-0.00090.0000-0.0422均方差0.02380.02920.5561誤差項(xiàng)位置/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的慣性組合導(dǎo)航直接式濾波方法穩(wěn)定，姿態(tài)誤差和位置誤差較小，具有較高的導(dǎo)航精度。在微小型飛行器特性的飛行條件下，該直接式濾波方法能夠適應(yīng)于各種飛行狀態(tài)。

4 結(jié)論

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

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

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

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

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

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

[6] 林雪原.GPS/SINS組合導(dǎo)航系統(tǒng)與實(shí)現(xiàn)[J].計(jì)算機(jī)測(cè)量與控制，2007(12).

[7] 林雪原，駱卉子，范文強(qiáng).一種改進(jìn)的GPS動(dòng)態(tài)定位濾波方法[J].海軍航空工程學(xué)院學(xué)報(bào)，2010(4).

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

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

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

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

[12] 劉錚.UKF算法及其改進(jìn)算法的研究[D].長(zhǎng)沙：碩士學(xué)位論文,中南大學(xué),2009.

[13] 徐佳鶴，井龍偉.UKF濾波算法在INS/GPS組合導(dǎo)航系統(tǒng)中的研究[C]//.中國(guó)控制與決策學(xué)術(shù)年會(huì)論文集, 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。

國(guó)家自然科學(xué)基金(60874112)

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

E-mail:wl19820416@sohu.com

TP273

A

2095-8110(2015)03-0043-06