改進(jìn)的UPF方法及其在MUAV組合導(dǎo)航中的應(yīng)用

邵瑋, 祝小平, 周洲, 趙剛

(1.西北工業(yè)大學(xué) 無人機(jī)特種技術(shù)國家重點(diǎn)實(shí)驗(yàn)室, 陜西 西安 710065;2.西北工業(yè)大學(xué) 無人機(jī)研究所, 陜西 西安 710065;3.西北工業(yè)大學(xué) 航天學(xué)院, 陜西 西安 710072)

改進(jìn)的UPF方法及其在MUAV組合導(dǎo)航中的應(yīng)用

邵瑋1, 祝小平2, 周洲1, 趙剛3

(1.西北工業(yè)大學(xué) 無人機(jī)特種技術(shù)國家重點(diǎn)實(shí)驗(yàn)室, 陜西 西安 710065;2.西北工業(yè)大學(xué) 無人機(jī)研究所, 陜西 西安 710065;3.西北工業(yè)大學(xué) 航天學(xué)院, 陜西 西安 710072)

針對微小型無人機(jī)組合導(dǎo)航系統(tǒng)中出現(xiàn)的非線性環(huán)節(jié),以及微小型無人機(jī)特殊的飛行環(huán)境易導(dǎo)致GPS信號因受到遮擋而時(shí)斷時(shí)續(xù)的情況,提出一種改進(jìn)的無跡粒子濾波(UPF)方法。利用帶有殘差約束的自適應(yīng)漸消方法對UPF進(jìn)行約束和改進(jìn),通過加強(qiáng)最新量測信息在狀態(tài)估計(jì)中的作用,達(dá)到迅速應(yīng)對系統(tǒng)突變、減緩濾波器發(fā)散的目的,增強(qiáng)系統(tǒng)魯棒性。仿真結(jié)果表明,相比于無跡卡爾曼濾波(UKF)和粒子濾波(PF),該方法能有效改善濾波性能、抑制濾波發(fā)散、提高組合導(dǎo)航系統(tǒng)的定位精度,且在GPS重新捕獲信號時(shí)具有更快速的重定位能力。

微小型無人機(jī); 組合導(dǎo)航; 無跡粒子濾波; 自適應(yīng)漸消濾波

0 引言

微小型無人機(jī)(Micro Unmanned Aerial Vehicles, MUAV)組合導(dǎo)航系統(tǒng)中通常會存在一定的非線性環(huán)節(jié),其微慣導(dǎo)(MEMS-SINS)/GPS組合導(dǎo)航系統(tǒng)偽距/偽距率組合方式中的量測方程即為非線性方程,在這種情況下,使用線性卡爾曼濾波(KF)方法很難解決導(dǎo)航數(shù)據(jù)融合的問題。

目前,解決組合導(dǎo)航系統(tǒng)中非線性模型濾波問題時(shí)使用較多的方法有兩類:一類是近似求解非線性函數(shù),將系統(tǒng)方程中的非線性部分線性化,忽略或逼近高階項(xiàng)的方法,如擴(kuò)展卡爾曼濾波方法(EKF);另一類是利用采樣的方法近似求解非線性密度函數(shù),從而得到狀態(tài)量的后驗(yàn)估計(jì)值,如無跡卡爾曼濾波方法(UKF)[1]和粒子濾波方法(PF)[2]。文獻(xiàn)[3]將UKF方法應(yīng)用于GPS/SINS組合導(dǎo)航系統(tǒng),通過與KF方法和自適應(yīng)濾波方法相比較,證明UKF方法提高了濾波精度。文獻(xiàn)[4]設(shè)計(jì)了一種基于交互多模型的UKF算法,用于解決GPS/DR組合導(dǎo)航系統(tǒng)中的非線性問題,相比于單個UKF方法,其具有估計(jì)精度高、適用于動態(tài)系統(tǒng)等特點(diǎn)。上述研究均在認(rèn)定系統(tǒng)可以時(shí)刻捕獲GPS信號的情況下進(jìn)行的,然而MUAV常穿梭于建筑物內(nèi)部、城市巷道等復(fù)雜環(huán)境中執(zhí)行任務(wù),這樣的飛行環(huán)境會對GPS信號的接收產(chǎn)生很大影響,造成GPS信號時(shí)斷時(shí)續(xù),從而導(dǎo)致組合導(dǎo)航系統(tǒng)出現(xiàn)模型誤差增大和量測信息發(fā)生較大突變的情況。

針對這一問題,本文以非線性濾波方法為基礎(chǔ),提出一種基于改進(jìn)的UPF方法的MUAV組合導(dǎo)航算法。通過在粒子濾波框架下使用UKF方法產(chǎn)生更加準(zhǔn)確的重要性分布,利用帶有殘差約束的自適應(yīng)漸消方法對UPF進(jìn)行約束和改進(jìn),使系統(tǒng)充分利用最新量測信息,在GPS失效時(shí)能夠快速反應(yīng),減緩濾波發(fā)散的速度;同時(shí)有效減輕了粒子退化及貧化問題,提高了濾波精度。

1 自適應(yīng)漸消濾波

自適應(yīng)漸消濾波是一種能夠有效利用系統(tǒng)最新量測信息,并可根據(jù)一定條件對參數(shù)進(jìn)行自適應(yīng)調(diào)節(jié)的濾波方法。根據(jù)漸消濾波思想,通過適當(dāng)?shù)馗淖儏f(xié)方差陣的權(quán)值,乘以一個加權(quán)因子,即可增強(qiáng)當(dāng)前量測數(shù)據(jù)在狀態(tài)估計(jì)中的作用,以應(yīng)對系統(tǒng)的突變及漂移,達(dá)到避免濾波器發(fā)散的目的。根據(jù)式(1)約束條件得到,多重次優(yōu)漸消因子是設(shè)計(jì)殘差約束自適應(yīng)漸消濾波的關(guān)鍵[5]。

(1)

這種方法使誤差序列在每一步相互正交,以此作為約束,使濾波器能夠自主校正估計(jì)誤差。通常情況下,上述條件很難滿足,只能要求等式近似成立即可,于是有:

(2)

這樣問題轉(zhuǎn)化為選擇一個合適的漸消矩陣,使得Cj,k中(Pxkzk-KkC0,k)一項(xiàng)為0,根據(jù)漸消思想可知一步預(yù)測狀態(tài)方程為[6]:

(3)

ηi,k=max{1,αick}

(4)

其中:

ck= tr[C0,k]/{tr{diag[α1,α2,…,αn]×

(5)

式中,C0,k為誤差陣,更新公式為:

(6)

式中,0<ρ≤1為遺忘因子。

2 帶有殘差約束的自適應(yīng)漸消UPF方法

在非線性系統(tǒng)中,本文將自適應(yīng)漸消濾波和UPF方法相結(jié)合,提出一種改進(jìn)的UPF方法。該方法結(jié)合了UKF和PF的優(yōu)點(diǎn),用UKF方法產(chǎn)生的Sigma點(diǎn)作為采樣點(diǎn),對狀態(tài)向量后驗(yàn)分布進(jìn)行近似,提高了粒子濾波效果;而帶有殘差約束的自適應(yīng)漸消濾波則充分利用系統(tǒng)最新量測信息,使系統(tǒng)具有更好的應(yīng)對突變的能力。

帶有殘差約束的自適應(yīng)漸消UPF方法的具體步驟如下:

(7)

各個Sigma點(diǎn)的權(quán)值分別為:

(8)

(3)時(shí)間更新:利用UKF算法對粒子進(jìn)行預(yù)測更新,由系統(tǒng)狀態(tài)方程對每個Sigma點(diǎn)進(jìn)行非線性變換并進(jìn)行加權(quán)處理,有:

(9)

一步預(yù)測均方誤差為:

(10)

zk| k-1=h(χk| k-1)

(11)

(12)

(13)

(14)

計(jì)算濾波增益陣:

(15)

在此處引入帶有殘差約束的自適應(yīng)漸消濾波方法,利用漸消UKF思想計(jì)算漸消因子,根據(jù)式(1)的約束條件,可以求得狀態(tài)更新后的濾波值和狀態(tài)后驗(yàn)方差陣分別為:

(16)

(17)

(18)

式中,i=1,2,…,N。歸一化權(quán)值,有:

(19)

(20)

(21)

(22)

(7)計(jì)算狀態(tài)估計(jì):

(23)

返回第(2)步重復(fù)上述過程。

通過以上步驟可以看出,該方法由于考慮了當(dāng)前最新量測信息的影響,因而能夠進(jìn)一步提高濾波精度,有效地提高M(jìn)UAV的導(dǎo)航定位精度;利用帶有殘差約束的自適應(yīng)漸消方法對UPF改進(jìn)后,使MUAV能應(yīng)對系統(tǒng)狀態(tài)突變,當(dāng)MUAV重新捕獲GPS信號時(shí),該方法能夠使濾波值迅速收斂。

3 組合導(dǎo)航系統(tǒng)數(shù)學(xué)模型

本文中MUAV的MEMS-SINS/GPS組合導(dǎo)航系統(tǒng)采用偽距/偽距率工作方式,其特點(diǎn)是量測方程為非線性,因此本文先以采用這種方式的MUAV組合導(dǎo)航系統(tǒng)為研究對象,建立濾波模型。

3.1 系統(tǒng)狀態(tài)方程

取MEMS-SINS的導(dǎo)航輸出參數(shù)誤差和微型GPS接收機(jī)的誤差狀態(tài)作為系統(tǒng)狀態(tài),將微慣性傳感器的誤差考慮進(jìn)來,可得20維系統(tǒng)狀態(tài)方程為:

(24)

式中,W為噪聲向量;F和G分別為狀態(tài)系數(shù)矩陣和誤差系數(shù)矩陣。且有:

=[φEφNφUδvEδvNδvUδLδλ

δhεbxεbyεbzεrxεryεrz

(25)

式中,狀態(tài)量φE,φN,φU,δvE,δvN,δvU,δL,δλ,δh分別為微慣導(dǎo)平臺誤差角、速度誤差和位置誤差;εb和εr分別為微陀螺漂移誤差中的隨機(jī)常數(shù)誤差和一階馬爾柯夫過程隨機(jī)誤差;為微加速度計(jì)漂移誤差中的一階馬爾柯夫過程隨機(jī)誤差;δtu為時(shí)鐘誤差引起的等效距離誤差;δtru為時(shí)鐘頻率誤差引起的等效速度誤差。

3.2 系統(tǒng)量測方程

組合導(dǎo)航系統(tǒng)以微型GPS接收機(jī)給出的偽距、偽距率信息為觀測量,假設(shè)MUAV在地球坐標(biāo)系(e系)下真實(shí)坐標(biāo)為(x,y,z),MEMS-SINS給出的MUAV在e系下坐標(biāo)為(xI,yI,zI),第j顆衛(wèi)星在e系中坐標(biāo)為(xsj,ysj,zsj),則MEMS-SINS給出的MUAV的位置所對應(yīng)于第j顆衛(wèi)星的計(jì)算偽距為:

δtu+vρj(j=1,2,3,4)

(26)

式中,vρj為偽距測量誤差。

同理,MEMS-SINS給出的MUAV速度所對應(yīng)于第j顆衛(wèi)星的計(jì)算偽距率為:

(27)

在導(dǎo)航定位中,MUAV在e系中的位置通常用地理經(jīng)度、緯度和高度表示。為方便計(jì)算,可利用坐標(biāo)轉(zhuǎn)換公式將其轉(zhuǎn)換為空間直角坐標(biāo),轉(zhuǎn)換過程詳見文獻(xiàn)[8],此處不再贅述。

最后得到系統(tǒng)量測方程為:

(28)

4 仿真結(jié)果及分析

為了驗(yàn)證本文所提算法,對其進(jìn)行計(jì)算機(jī)仿真。結(jié)合MUAV的實(shí)際情況,選取東北天地理坐標(biāo)系作為導(dǎo)航坐標(biāo)系,設(shè)定仿真條件為:MUAV初始位置為北緯34.2°,東經(jīng)108.7°,飛行高度100 m,初始航向角35°,初始俯仰角和滾轉(zhuǎn)角均為0°,飛行軌跡包括起飛、爬升、平飛、轉(zhuǎn)彎等;微陀螺儀常值漂移10(°)/h,隨機(jī)噪聲均方差10 (°)/h;微加速度計(jì)常值誤差1×10-3g,隨機(jī)誤差均方差2×10-4g;微型GPS接收機(jī)隨機(jī)偽距測量誤差為15 m,隨機(jī)偽距率測量誤差0.1 m/s;接收機(jī)時(shí)鐘偏差為20 m,時(shí)鐘偏差噪聲為0.1 m/s。仿真時(shí)間1 800 s,組合導(dǎo)航系統(tǒng)濾波周期為1 s。設(shè)定在600 s時(shí)GPS停止工作,800 s時(shí)恢復(fù),將本文方法與UKF方法和PF方法進(jìn)行比較,三種方法的仿真結(jié)果如圖1所示。

圖1 三種方法仿真結(jié)果比較Fig.1 Comparison of simulation results obtained with the three methods

由仿真結(jié)果可以看出,所提方法在速度估計(jì)誤差、位置估計(jì)誤差和姿態(tài)估計(jì)誤差等方面比UKF方法均有較大程度的改進(jìn);在位置估計(jì)誤差、姿態(tài)估計(jì)誤差兩方面較標(biāo)準(zhǔn)PF方法有一定改進(jìn);在速度估計(jì)誤差方面效果與PF方法接近。該方法可將東向、北向和天向速度的估計(jì)誤差分別控制在0.15 m/s,0.15 m/s和0.2 m/s以內(nèi);位置估計(jì)誤差在5 m以內(nèi);俯仰角、滾轉(zhuǎn)角估計(jì)誤差在0.2 arcmin左右,偏航角估計(jì)誤差在0.8 arcmin左右。

5 結(jié)論

(1)與傳統(tǒng)的PF方法相比,改進(jìn)的UPF方法能夠比普通的粒子濾波方法更準(zhǔn)確地逼近狀態(tài)向量的后驗(yàn)分布,提高了濾波精度。

(2)對系統(tǒng)狀態(tài)突然發(fā)生變化的情況具有良好的反應(yīng)能力;通過漸消因子自適應(yīng)的調(diào)節(jié)權(quán)值,使有效信息得以充分利用,較好地抑制了濾波發(fā)散,增強(qiáng)了系統(tǒng)的魯棒性;在GPS能夠正常使用的情況下,該方法可使系統(tǒng)具有更精確的導(dǎo)航能力。

(3)采用權(quán)值優(yōu)選算法克服粒子濾波中常出現(xiàn)的粒子貧化問題,較好地保持了粒子的多樣性。

(4)該方法具有較為適中的計(jì)算量,能夠滿足MUAV組合導(dǎo)航系統(tǒng)對實(shí)時(shí)性的要求。

[1] Julier S,Uhlmann J,Durrant-Whyte H F.A new method for the nonlinear transformation of means and covariance in filters and estimators[J].IEEE Transactions on Automatic Control,2000,45(3):477-482.

[2] 朱志宇.粒子濾波算法及其應(yīng)用[M].北京:科學(xué)出版社,2010:27-32.

[3] 鮑其蓮,周媛媛.基于UKF的GPS/SINS偽距(偽距率)組合導(dǎo)航系統(tǒng)設(shè)計(jì)[J].中國慣性技術(shù)學(xué)報(bào),2008,16(1):78-81.

[4] 臧榮春,崔平遠(yuǎn),崔祜濤,等.基于IMM-UKF的組合導(dǎo)航算法[J].控制理論與應(yīng)用,2007,24(4):634-638.

[5] 劉錚.UKF算法及其改進(jìn)算法的研究[D].長沙:中南大學(xué),2009.

[6] 周東華,席裕庚,張鐘俊.一種帶多重次優(yōu)漸消因子的擴(kuò)展卡爾曼濾波器[J].自動化學(xué)報(bào),1991,17(6):689-695.

[7] 張琪,胡昌華,喬玉坤.基于權(quán)值選擇的粒子濾波算法研究[J].控制與決策,2008,23(1):117-120.

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

ImprovedUPFalgorithmanditsapplicationinINSofMUAV

SHAO Wei1, ZHU Xiao-ping2, ZHOU Zhou1, ZHAO Gang3

(1.National Key Laboratory of Special Technology on UAV, NWPU, Xi’an 710065, China;2.The UAV Research Institute, NWPU, Xi’an 710065, China;3.College of Astronautics, NWPU, Xi’an 710072, China)

In allusion to the nonlinear component in INS of MUAV, and the intermittent GPS signal caused by serious sheltering in special flight environment, an improved unscented particle filter (UPF) method is proposed. The UPF is constrained and improved by using self-adaptive fading method with residual constraints. By strengthening the effect of the new measurements in state estimation, the purpose of quick response to the sudden changes of the system and filter divergence slowed down could be reached and the system robustness is enhanced. The simulation results show that, compared with UKF and PF, the new method can effectively improve the filter performance, suppress the filter divergence, enhance the positioning precision of the integrated navigation system, and reposition more quickly when GPS recaptures the signals.

micro UAV; integrated navigation; unscented particle filtering; self-adaptive fading filtering

V249.328

A

1002-0853(2013)05-0462-05

2013-05-23;

2013-06-03; < class="emphasis_bold">網(wǎng)絡(luò)出版時(shí)間

時(shí)間:2013-08-21 18:46

邵瑋(1982-)，女，陜西西安人，博士研究生，主要研究方向?yàn)闊o人機(jī)組合導(dǎo)航及飛行控制。

(編輯：李怡)