劉明雍, 郭 勇, 張立川, 2, 高沛林

?

一種基于UKF的MEMS/GPS組合導(dǎo)航算法

劉明雍1, 郭 勇1, 張立川1, 2, 高沛林1

(1. 西北工業(yè)大學(xué) 航海學(xué)院, 陜西 西安, 710072; 2. 水下信息與控制重點(diǎn)實(shí)驗(yàn)室, 陜西 西安, 710072)

針對(duì)微電子機(jī)械(MEMS)/全球定位(GPS)組合導(dǎo)航系統(tǒng)中, 強(qiáng)非線性引起的擴(kuò)展卡爾曼濾波(EKF)導(dǎo)航精度不高、濾波性能不穩(wěn)定、收斂速度慢等問(wèn)題, 研究了MEMS/GPS組合導(dǎo)航系統(tǒng)無(wú)跡卡爾曼濾波(UKF)算法, 避免了EKF繁重的求導(dǎo)計(jì)算。本文對(duì)2種濾波方法進(jìn)行了仿真比較, 結(jié)果表明, 用UKF的組合導(dǎo)航系統(tǒng)的誤差收斂速度比EKF快, 精度也比EKF高, UKF算法更適合于MEMS/GPS組合導(dǎo)航。

微電子機(jī)械/全球定位組合導(dǎo)航系統(tǒng); 擴(kuò)展卡爾曼濾波; 無(wú)跡卡爾曼濾波

0 引言

在計(jì)算機(jī)技術(shù)高速發(fā)展和低價(jià)慣性器件不斷涌現(xiàn)的背景下, 微電子機(jī)械系統(tǒng)(micro-electro- mechanical systems, MEMS)和GPS組合導(dǎo)航系統(tǒng)成為眾多領(lǐng)域?qū)Ш郊夹g(shù)的首選, 其關(guān)鍵技術(shù)是多傳感器信息最優(yōu)遞推融合算法。標(biāo)準(zhǔn)卡爾曼濾波在線性系統(tǒng)、高斯分布等假設(shè)的基礎(chǔ)上得到了一個(gè)最優(yōu)濾波算法[1-2], 但是大部分實(shí)際系統(tǒng)都包含非線性環(huán)節(jié), 屬于非線性系統(tǒng)。非線性系統(tǒng)濾波問(wèn)題可將非線性系統(tǒng)進(jìn)行線性化, 然后應(yīng)用線性系統(tǒng)濾波法,即廣泛應(yīng)用的擴(kuò)展卡爾曼濾波(extended Kalman filter, EKF)法。巴塞羅那自治大學(xué)的Quinchia, Alex和Carles等人最近就利用EKF方法對(duì)MEMS誤差模型進(jìn)行了分析與研究[3], 雖然結(jié)果也保持了卡爾曼濾波的優(yōu)點(diǎn)及計(jì)算上有效地迭代更新, 但實(shí)際應(yīng)用中對(duì)弱非線性系統(tǒng)效果較好, 對(duì)強(qiáng)非線性系統(tǒng)則容易發(fā)散, 且線性化過(guò)程存在高階項(xiàng)截?cái)嗾`差, 計(jì)算Jacobian矩陣時(shí)計(jì)算量大易出錯(cuò)等缺陷, 因此需要尋找更加適合實(shí)際應(yīng)用和精度更高的算法。

為解決以上問(wèn)題, 本文將Juliter和Uhlmann提出的無(wú)跡卡爾曼濾波(unscented Klaman filter, UKF)方法引入到了MEMS/GPS組合導(dǎo)航系統(tǒng)的濾波中[4-5], 這種濾波方法以UT(unscented transform)變換為基礎(chǔ)。比起在組合導(dǎo)航中應(yīng)用廣泛的EKF, 優(yōu)點(diǎn)如下。

1) 對(duì)非線性函數(shù)的概率密度分布進(jìn)行近似, 而不是對(duì)非線性函數(shù)進(jìn)行近似, 即使系統(tǒng)的模型復(fù)雜, 也不增加算法實(shí)現(xiàn)的難度;

2) 所得非線性函數(shù)統(tǒng)計(jì)量的準(zhǔn)確性可達(dá)3階(泰勒展開(kāi));

3) 不需要計(jì)算Jacobian矩陣, 可以處理不可導(dǎo)非線性函數(shù)。

Saeedi Sara和EI-Sheimy將UKF方法運(yùn)用到慣性導(dǎo)航系統(tǒng)(inertial navigation system, INS)和GPS組合導(dǎo)航系統(tǒng)中, 取得了較好的導(dǎo)航精度[6]。同時(shí)李濤等學(xué)者也將UKF方法應(yīng)用到INS/GPS組合導(dǎo)航系統(tǒng)中, 并與EKF方法進(jìn)行比較, 仿真結(jié)果表明, UKF在精度和收斂性方面要優(yōu)于EKF[7]。本文則將UKF濾波方法應(yīng)用到了MEMS/GPS組合導(dǎo)航系統(tǒng)中, 驗(yàn)證了UKF濾波方法的可行性, 并與EKF方法進(jìn)行比較, 仿真結(jié)果表明, 采用UKF的MEMS/GPS組合導(dǎo)航系統(tǒng)優(yōu)于采用EKF的組合導(dǎo)航系統(tǒng), 基本滿(mǎn)足低成本, 較高精度, 高可靠性的導(dǎo)航定位要求。

1 組合導(dǎo)航系統(tǒng)非線性模型

1.1 GPS系統(tǒng)誤差狀態(tài)方程的建立

1.2 MEMS/GPS組合導(dǎo)航系統(tǒng)的狀態(tài)方程

選用東北天地理坐標(biāo)系為導(dǎo)航坐標(biāo)系, 組合系統(tǒng)的誤差含有MEMS系統(tǒng)的數(shù)學(xué)平臺(tái)誤差、速度誤差、位置誤差和慣性?xún)x表誤差(陀螺漂移和加速度計(jì)隨機(jī)誤差)、GPS系統(tǒng)的位置和速度誤差。

導(dǎo)航誤差狀態(tài)方程為

相應(yīng)的誤差方程有

1.3 MEMS/GPS組合導(dǎo)航系統(tǒng)的觀測(cè)方程

1.4 基于UKF的濾波器設(shè)計(jì)

UKF不對(duì)非線性系統(tǒng)的狀態(tài)方程和觀測(cè)方程進(jìn)行線性化, 而是利用一系列近似高斯分布的采樣點(diǎn), 通過(guò)Unscented變換來(lái)進(jìn)行狀態(tài)與誤差協(xié)方差的遞推和更新, 在每次更新過(guò)程中, 采樣點(diǎn)都通過(guò)非線性誤差狀態(tài)方程進(jìn)行傳播并隨著量測(cè)方程變換, 此方法的精度不僅比EKF高, 同時(shí)還避免了對(duì)非線性方程的線性化過(guò)程。UKF使用的是離散時(shí)間非線性模型, 因此先采用4階Runger-Kutta法以數(shù)值積分的方法將系統(tǒng)模型進(jìn)行離散化處理得到的系統(tǒng)方程為

2) 計(jì)算Sigma點(diǎn), 設(shè)狀態(tài)變量為維, 計(jì)算2n+1個(gè)采樣點(diǎn)及其權(quán)值

3) 時(shí)間傳播方程

4) 量測(cè)更新方程

關(guān)于標(biāo)準(zhǔn)UKF方法的具體實(shí)現(xiàn)過(guò)程, 可參考文獻(xiàn)[9]。

2 仿真分析

為評(píng)估所建MEMS/GPS組合導(dǎo)航系統(tǒng)的性能, 將前面設(shè)計(jì)的算法應(yīng)用到MEMS/GPS組合導(dǎo)航系統(tǒng), 并且進(jìn)行了仿真。

圖1表明, 仿真到3600 s時(shí)EKF濾波算法對(duì)經(jīng)度的估計(jì)誤差為50 m, UKF濾波算法對(duì)經(jīng)度的估計(jì)誤差為45 m, 因此UKF對(duì)經(jīng)度誤差的估計(jì)略好于EKF。

圖1 經(jīng)度誤差曲線

圖2 緯度誤差曲線

圖2表明, 2種濾波算法的緯度誤差基本相同, 3 600 s的仿真過(guò)程中誤差基本保持在18 m以?xún)?nèi)。

綜上所述,選用UKF對(duì)組合導(dǎo)航進(jìn)行濾波實(shí)現(xiàn)是一種導(dǎo)航精度較好的導(dǎo)航方法, 能有效抑制由白噪聲引起的誤差的增量。

3 結(jié)束語(yǔ)

本文將UKF濾波方法引入到MEMS/GPS組合導(dǎo)航系統(tǒng)濾波中, 避免了EKF線性化誤差對(duì)于組合導(dǎo)航系統(tǒng)精度和濾波器穩(wěn)定的影響, 通過(guò)仿真比較了UKF與EKF的濾波效果。理論分析和仿真結(jié)果表明, UKF的收斂速度更快, 精度更高, 穩(wěn)定性更好。在MEMS/GPS系統(tǒng)中應(yīng)用UKF濾波, 真正實(shí)現(xiàn)了低成本、高精度的導(dǎo)航定位要求, 并可有效解決系統(tǒng)非線性加劇而引起的濾波發(fā)散問(wèn)題。

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

[2] 曹學(xué)偉, 石杏喜, 左常清. 基于EKF的DR/激光測(cè)距組合導(dǎo)航算法研究[J]. 山東科技大學(xué)學(xué)報(bào), 2009(2): 84-87.

Cao Xue-wei, Shi Xing-xi, Zuo Chang-qing. Study on DR/Laser Rangefinder Integrated Navigation Algorithm Based on EKF[J]. Journal of Shandong University of Science and Technology, 2009(2): 84-87.

[3] Quinchia A G, Carles F. Analysis and Modelling of MEMS Inertial Measurement Unit[C]//Localization and GNSS (ICL- GNSS), 2012 International Conference, 2012: 1-7.

[4] Julier S, Uhlmann J K. A General Method for Approximating Nonlinear Transformations of Probability Distributions[R]. Robotics Research Group, Department of Engineering Science, University of Oxford, 1994.

[5] Uhlmann J K. Simultaneous Map Building and Localization for Real Time Applications[R]. U.K: University of Oxford, 1994.

[6] Saeedi Sara, EI-Sheimy. The Merits of UKF and PF for Integrated INS/GPS Navigation Systems[C]//22nd International Technical Meeting of the Satellite Division of the Institute of Navigation, 2009: 1612-1618.

[7] Li Tao, Xu Xiao-su. The Application of EKF and UKF to the SINS/GPS Integrated Navigation Systems[C]//Infor- mation Engineering and Computer Science(ICIECS), 2010 2nd International Conference, 2010: 25-26.

[8] 李濤.非線性濾波方法在導(dǎo)航系統(tǒng)中的應(yīng)用研究[D].長(zhǎng)沙: 國(guó)防科學(xué)技術(shù)大學(xué), 2003.

[9] 張紅梅, 鄧正隆. UKF方法在陸地車(chē)輛組合導(dǎo)航中的應(yīng)用[J]. 中國(guó)慣性技術(shù)學(xué)報(bào), 2004, 16(4): 20-23.

Zhang Hong-mei, Deng Zheng-long. UKF Method for Land Vehicle Integrated Navigation System[J]. Journal of Chinese Inertial Technology, 2004, 16(4): 20-23.

UKF-Based Algorithm for MEMS/GPS Integrated Navigation System

LIU Ming-yong1, GUO Yong1, ZHANG Li-chuan1, 2, GAO Pei-lin1

(1. College of Marine Engineering, Northwestern Polytechnical University, Xi′an 710072, China; 2. Science and Technology on Underwater Information and Control Laboratory, Xi′an 710072, China)

To solve the problems of low navigation accuracy, slow convergence rate and unstable filtering accuracy caused by the strong nonlinearity of the extended Kalman filter(EKF) in micro electro mechanical system/global positioning system(MEMS/GPS) integrated navigation system, an unscented Kalman filter(UKF) is proposed for the MEMS/GPS integrated navigation system. The UKF avoids the heavy derivation in EKF. Simulation results show that the two filtering methods: the error convergence rate of UKF integrated navigation system is faster, the accuracy is higher than the EKF. Comparison between the two algorithms shows that MEMS/GPS integrated navigation system with UKF achieves faster error convergence rate, higher navigation accuracy and enhanced navigation performance.

micro electro mechanical system/global positioning system integrated navigation system; extended Kalman filter; unscented Kalman filter

TJ630.33

A

1673-1948(2013)05-0351-04

2012-12-26;

2013-03-28.

國(guó)家自然科學(xué)基金(51179156); 國(guó)家自然科學(xué)基金青年基金項(xiàng)目(51109179); 教育部博士點(diǎn)基金(20106102120057); 西北工業(yè)大學(xué)基礎(chǔ)基金(JC201228).

劉明雍(1971- ), 男, 教授, 博士生導(dǎo)師, 主要從事水下航行器導(dǎo)航與控制的研究.

(責(zé)任編輯: 楊力軍)