改進的UPF方法及其在MUAV組合導航中的應用

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

(1.西北工業大學 無人機特種技術國家重點實驗室, 陜西 西安 710065;2.西北工業大學 無人機研究所, 陜西 西安 710065;3.西北工業大學 航天學院, 陜西 西安 710072)

改進的UPF方法及其在MUAV組合導航中的應用

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

(1.西北工業大學 無人機特種技術國家重點實驗室, 陜西 西安 710065;2.西北工業大學 無人機研究所, 陜西 西安 710065;3.西北工業大學 航天學院, 陜西 西安 710072)

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

微小型無人機; 組合導航; 無跡粒子濾波; 自適應漸消濾波

0 引言

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

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

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

1 自適應漸消濾波

自適應漸消濾波是一種能夠有效利用系統最新量測信息,并可根據一定條件對參數進行自適應調節的濾波方法。根據漸消濾波思想,通過適當地改變協方差陣的權值,乘以一個加權因子,即可增強當前量測數據在狀態估計中的作用,以應對系統的突變及漂移,達到避免濾波器發散的目的。根據式(1)約束條件得到,多重次優漸消因子是設計殘差約束自適應漸消濾波的關鍵[5]。

(1)

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

(2)

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

(3)

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

(4)

其中:

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

(5)

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

(6)

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

2 帶有殘差約束的自適應漸消UPF方法

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

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

(7)

各個Sigma點的權值分別為:

(8)

(3)時間更新:利用UKF算法對粒子進行預測更新,由系統狀態方程對每個Sigma點進行非線性變換并進行加權處理,有:

(9)

一步預測均方誤差為:

(10)

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

(11)

(12)

(13)

(14)

計算濾波增益陣:

(15)

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

(16)

(17)

(18)

式中,i=1,2,…,N。歸一化權值,有:

(19)

(20)

(21)

(22)

(7)計算狀態估計:

(23)

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

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

3 組合導航系統數學模型

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

3.1 系統狀態方程

取MEMS-SINS的導航輸出參數誤差和微型GPS接收機的誤差狀態作為系統狀態,將微慣性傳感器的誤差考慮進來,可得20維系統狀態方程為:

(24)

式中,W為噪聲向量;F和G分別為狀態系數矩陣和誤差系數矩陣。且有:

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

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

(25)

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

3.2 系統量測方程

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

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

(26)

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

同理,MEMS-SINS給出的MUAV速度所對應于第j顆衛星的計算偽距率為:

(27)

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

最后得到系統量測方程為:

(28)

4 仿真結果及分析

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

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

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

5 結論

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

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

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

(4)該方法具有較為適中的計算量,能夠滿足MUAV組合導航系統對實時性的要求。

[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] 朱志宇.粒子濾波算法及其應用[M].北京:科學出版社,2010:27-32.

[3] 鮑其蓮,周媛媛.基于UKF的GPS/SINS偽距(偽距率)組合導航系統設計[J].中國慣性技術學報,2008,16(1):78-81.

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

[5] 劉錚.UKF算法及其改進算法的研究[D].長沙:中南大學,2009.

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

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

[8] 劉建業,曾慶化,趙偉,等.導航系統理論與應用[M].西安:西北工業大學出版社,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">網絡出版時間

時間:2013-08-21 18:46

邵瑋(1982-)，女，陜西西安人，博士研究生，主要研究方向為無人機組合導航及飛行控制。

(編輯：李怡)