低成本車載組合導(dǎo)航在GPS失效時(shí)的算法設(shè)計(jì)

姜博文,王可東

(北京航空航天大學(xué) 宇航學(xué)院,北京 100191)

?

低成本車載組合導(dǎo)航在GPS失效時(shí)的算法設(shè)計(jì)

姜博文,王可東*

(北京航空航天大學(xué) 宇航學(xué)院,北京 100191)

在低成本MEMS-IMU/GPS車載組合導(dǎo)航應(yīng)用中,城市中建筑的遮擋會(huì)造成GPS失效,此時(shí)由于MEMS器件精度較低,導(dǎo)航精度會(huì)迅速降低甚至發(fā)散。針對(duì)此問(wèn)題,引入車輛的天向和側(cè)向速度約束作為虛擬觀測(cè),并且考慮到GPS失效時(shí)段由于低精度MEMS-IMU帶來(lái)的非線性,在不做小角度近似的條件下,推導(dǎo)出非線性速度約束作為虛擬觀測(cè),利用非線性UKF濾波算法進(jìn)行估計(jì),以進(jìn)一步改善導(dǎo)航性能。車載實(shí)驗(yàn)表明,提出的非線性速度約束算法能夠有效提高GPS失效時(shí)的精度,并且在GPS信號(hào)再次有效時(shí)能夠快速收斂。

GPS;MEMS;UKF;組合導(dǎo)航;慣性導(dǎo)航系統(tǒng)

MEMS-IMU/GPS的組合導(dǎo)航系統(tǒng)由于其低成本和小型化等優(yōu)點(diǎn)得到了越來(lái)越廣泛的應(yīng)用[1-2],但是在城市中GPS容易受高樓、橋梁或隧道等遮擋而失效,給組合系統(tǒng)帶來(lái)如下問(wèn)題[3]:一是當(dāng)GPS失效時(shí)組合系統(tǒng)運(yùn)行在純慣導(dǎo)解算狀態(tài),由于MEMS-IMU的精度較低,解算結(jié)果會(huì)迅速發(fā)散。針對(duì)此問(wèn)題,文獻(xiàn)[4]利用里程計(jì)的輸出作為GPS失效時(shí)的觀測(cè)信息來(lái)解決這個(gè)問(wèn)題,這種方案在工程中已有應(yīng)用,但是新引入額外的傳感器無(wú)疑增加了實(shí)現(xiàn)的成本。文獻(xiàn)[5]通過(guò)假設(shè)車輛側(cè)向和天向速度為零以限制慣導(dǎo)誤差的快速發(fā)散,這種方案實(shí)施簡(jiǎn)單,但是精度有限,而且只適合于簡(jiǎn)單的直線運(yùn)動(dòng)。文獻(xiàn)[6-8]等分別通過(guò)神經(jīng)網(wǎng)絡(luò)、遺傳算法、支持向量機(jī)等方法,在GPS信號(hào)有效時(shí)進(jìn)行在線訓(xùn)練,當(dāng)GPS信號(hào)失效時(shí)利用訓(xùn)練好的模型來(lái)估計(jì)誤差以達(dá)到提高精度的效果,但是這些方法由于巨大的計(jì)算量在應(yīng)用中受到了一定限制;第2個(gè)問(wèn)題是在低成本的IMU應(yīng)用中,當(dāng)GPS信號(hào)再次有效時(shí),在GPS失效期間累積的導(dǎo)航誤差已經(jīng)不是小量,近似的線性模型已經(jīng)不再適用,線性傳遞下的系統(tǒng)狀態(tài)和協(xié)方差已不能準(zhǔn)確描述系統(tǒng)信息,采用線性的KF(Kalman Filter)進(jìn)行濾波難以立即對(duì)GPS恢復(fù)后的狀態(tài)做出正確的估計(jì)。

針對(duì)上述問(wèn)題,本文考慮到城市應(yīng)用中GPS失效持續(xù)時(shí)間不長(zhǎng)但是比較頻繁,并且低精度MEMS-IMU在短時(shí)的失效內(nèi)能造成較強(qiáng)的非線性,因此在傳統(tǒng)線性速度約束(用速度誤差的線性函數(shù)作為觀測(cè))的基礎(chǔ)上建立了非線性的速度約束(用速度誤差的非線性函數(shù)作為觀測(cè))模型,在GPS失效時(shí)將速度約束作為虛擬的觀測(cè),并用UKF(Unscented KF)濾波算法在GPS失效時(shí)進(jìn)行估計(jì),提高估計(jì)精度,當(dāng)GPS信號(hào)恢復(fù)時(shí)候能快速收斂,改善導(dǎo)航性能。

1 組合系統(tǒng)模型

1.1 系統(tǒng)誤差模型

1.1.1 非線性誤差模型

記n為真實(shí)地理系,n′為計(jì)算系。設(shè)n系可以通過(guò)三次轉(zhuǎn)動(dòng)得到n′系,三次轉(zhuǎn)角依次為αz、αx和αy,根據(jù)這三次旋轉(zhuǎn)可算出n系到n′系的轉(zhuǎn)換矩陣:

其中cα、sα分別表示cosα、sinα。

則系統(tǒng)的狀態(tài)方程[9]可以表示為:

(1)

(2)

令15維狀態(tài)變量x=[αδvδpbabg]T其中α=[αxαyαz],δp=[δLδλδh]T,ba,bg分別為加速度計(jì)和陀螺的零偏。則式(1)表示的非線性系統(tǒng)可以寫為:

(3)

1.1.2 線性誤差模型

慣導(dǎo)在小角度近似下的線性誤差模型在文獻(xiàn)[10-12]已有詳細(xì)推導(dǎo),這里直接對(duì)式(1)進(jìn)行小角度線性化,結(jié)果如下:

(4)

(5)

1.2 測(cè)量模型

1.2.1GPS有效時(shí)

當(dāng)GPS有效時(shí),采用GPS輸出的速度和位置作為觀測(cè)量[11],觀測(cè)方程如下:

(6)

1.2.2GPS無(wú)效時(shí)

在GPS信號(hào)失效的情況下,引入車輛的運(yùn)動(dòng)學(xué)約束來(lái)抑制導(dǎo)航系統(tǒng)的誤差[14]。假定車輛沒(méi)有側(cè)滑并且在平坦的道路行駛,這在大多數(shù)城市中都是合理的。如果滿足這兩個(gè)假設(shè),車輛的側(cè)向速度和天向速度的真實(shí)值為0,如圖1所示。此時(shí)可以把這兩個(gè)值看作車輛的虛擬測(cè)量值[15]。

圖1 車輛運(yùn)動(dòng)學(xué)約束示意

假設(shè)IMU的測(cè)量軸和車輛的右前上方向完全重合,則車輛速度在載體系投影的理論值為:

(7)

(8)

式(7)和式(8)作差可得:

(9)

(10)

可以簡(jiǎn)記為

z=h(x,t)

(11)

(12)

測(cè)量噪聲的引入是由于考慮到IMU的測(cè)量軸并不可能完全重合與車輛的右前上方向,當(dāng)IMU的y軸與車輛前向有2°～3°的安裝誤差并且車輛以18 m/s～20 m/s的速度前行時(shí),會(huì)在側(cè)向和天向造成1 m/s的誤差[14];并且車輛的上下坡或者轉(zhuǎn)彎造成的影響也會(huì)引起相應(yīng)的誤差。

當(dāng)α足夠小時(shí),對(duì)式(10)做線性化近似可得:

(13)

式中:vn×代表詳細(xì)vn的斜對(duì)稱陣。上式與文獻(xiàn)[8]中結(jié)果一致,說(shuō)明了本文推導(dǎo)的正確性?？紤]到低精度的IMU在GPS失效時(shí)精度下降很快,因此本文在GPS失效時(shí)段將系統(tǒng)看作非線性,因此通過(guò)非線性濾波的手段對(duì)參數(shù)進(jìn)行估計(jì),下面介紹UKF濾波。

2 濾波算法

MEMS-IMU的低精度導(dǎo)致在GPS丟失時(shí)段系統(tǒng)有較強(qiáng)的非線性,傳統(tǒng)的KF濾波算法不再適用。文獻(xiàn)[16]從非線性均值和方差的傳播的角度出發(fā),通過(guò)UT(Unscented Transformation)變換直接進(jìn)行傳播,避免了非線性函數(shù)線性化近似,能夠獲得有比EKF(Extended KF)更高的精度[17]。文獻(xiàn)[18-19]對(duì)噪聲屬于加性噪聲的情況進(jìn)行了簡(jiǎn)化,由于本文中系統(tǒng)噪聲和測(cè)量噪聲都是加性噪聲,可以使用簡(jiǎn)化的UKF進(jìn)行濾波,步驟如下:

狀態(tài)初始化:

(14)

(15)

(16)

其中:

(17)

組合系統(tǒng)結(jié)構(gòu)如圖2和圖3所示。

圖2 GPS有效時(shí)的系統(tǒng)結(jié)構(gòu)

圖3 GPS無(wú)效時(shí)的系統(tǒng)結(jié)構(gòu)

3 車載實(shí)驗(yàn)

車載實(shí)驗(yàn)中,MEMS-IMU選用的是ADIS16375型三軸集成IMU,數(shù)據(jù)輸出率為200Hz,陀螺和加速度計(jì)初始零偏分別為±1°/s和±16mg;GPS接收機(jī)為u-blox的NEO-6M定位模塊,采樣頻率為1Hz,定位精度為2.5m,測(cè)速精度為0.1m/s;控制器為STM32F103型ARM芯片。采用和芯星通的UR370三頻高精度接收機(jī)提供基準(zhǔn),該接收機(jī)采用雙天線RTK(Real-TimeKinematic)測(cè)量方法,結(jié)合基準(zhǔn)站可實(shí)現(xiàn)流動(dòng)站厘米級(jí)的定位精度;此外,雙天線差分可提供姿態(tài)角信息,沿車軸縱向安裝兩個(gè)天線,可輸出車體的俯仰角和偏航角,姿態(tài)角精度由兩個(gè)天線間的基線長(zhǎng)度決定,基線長(zhǎng)度越長(zhǎng),定姿精度越高。試驗(yàn)中的基線長(zhǎng)度約為1m,定姿精度為0.5°左右。系統(tǒng)實(shí)物照片如圖4所示。

圖4 實(shí)驗(yàn)設(shè)備

收集跑車實(shí)驗(yàn)的數(shù)據(jù)做事后處理,截取持續(xù)120s的跑車數(shù)據(jù),在第60s～100s時(shí)間段人為設(shè)置數(shù)據(jù)遮擋40s,在GPS有效階段采用圖2所示的閉環(huán)松組合結(jié)構(gòu),當(dāng)GPS失效時(shí)引入速度約束的虛擬觀測(cè),結(jié)構(gòu)如圖3所示。首先考察引入速度約束對(duì)導(dǎo)航性能的改善情況,再將線性速度約束算法和本文提出的非線性速度約束算法作對(duì)比。其中卡爾曼濾波的Q陣值根據(jù)AID16375數(shù)據(jù)手冊(cè)中陀螺和加計(jì)的噪聲設(shè)置,R陣的值根據(jù)GPS速度和位置誤差的值設(shè)置。

如圖5所示為添加線性速度約束前后的導(dǎo)航結(jié)果對(duì)比,由圖可知,當(dāng)在60s～100s期間引入GPS阻斷時(shí),引入的線性速度約束可以較好地提升導(dǎo)航性能,由于此問(wèn)題已在文獻(xiàn)[10]中詳細(xì)討論,此處不再畫出具體的位置和姿態(tài)的誤差圖。

圖5 約束前后軌跡對(duì)比

如圖6～圖11所示為非線性速度約束與線性速度約束的導(dǎo)航性能對(duì)比,由圖可知,與線性速度約束算法相比,在GPS失效的40s中,非線性速度約束算法精度有較大提高,其中東向、北向和天向的最大位置誤差分別減小了約42%、67%和21%,由于改進(jìn)的非線性約束模型在GPS失效時(shí)能夠做出更準(zhǔn)確的估計(jì),所以位置誤差的發(fā)散得到抑制,非線性速度約束算法的姿態(tài)精度也高于線性速度約束算法;更重要的是,在GPS信號(hào)恢復(fù)后(100s之后),前者能迅速收斂到真值附近,而后者則不能,這是因?yàn)樵谠贕PS失效期間造成的慣導(dǎo)姿態(tài)誤差已經(jīng)不是小量,小角度假設(shè)下的線性模型已經(jīng)不再適用,線性傳播下的狀態(tài)和協(xié)方差也不能準(zhǔn)確描述系統(tǒng)信息,而非線性模型和濾波算法則較好地提高了導(dǎo)航性能。由于車載實(shí)驗(yàn)環(huán)境的復(fù)雜性,為了進(jìn)一步驗(yàn)證改進(jìn)算法的有效性,選取另一段跑車軌跡,屏蔽其10s～50s的信號(hào)(轉(zhuǎn)彎部分考慮在內(nèi))。

從圖12可以看出改進(jìn)算法的位置誤差明顯減小,此處不再畫出,下面畫出姿態(tài)誤差,如圖13和圖14所示。

圖9 天向位置誤差對(duì)比

圖6 軌跡對(duì)比圖

圖7 東向位置誤差對(duì)比

圖8 北向位置誤差對(duì)比

圖10 俯仰角誤差對(duì)比

圖11 偏航角誤差對(duì)比

圖12 軌跡對(duì)比

圖13 偏航角誤差對(duì)比

圖14俯仰角誤差對(duì)比

列出兩組實(shí)驗(yàn)的最大姿態(tài)誤差如表1所示。

表1 最大姿態(tài)誤差對(duì)比表

由于圖中位置誤差比較明顯,不再列出。

從圖中可以看出在轉(zhuǎn)彎時(shí)車輛動(dòng)態(tài)較大,低精度的IMU帶來(lái)非線性更加明顯,信號(hào)恢復(fù)后俯仰角需要更長(zhǎng)的時(shí)間收斂到正確狀態(tài),并且直線運(yùn)動(dòng)時(shí)偏航角可觀性較差,使其更加難以恢復(fù)到真值;改進(jìn)的算法能夠明顯抑制誤差發(fā)散,有利于組合導(dǎo)航性能的提升,進(jìn)一步驗(yàn)證了算法的有效性。

6 結(jié)論

本文以車載組合導(dǎo)航為研究對(duì)象,當(dāng)GPS失效時(shí),假設(shè)車輛天向和側(cè)向速度為零,引入速度方程作為虛擬觀測(cè),并用UKF濾波進(jìn)行非線性估計(jì)。車載實(shí)驗(yàn)結(jié)果表明,本文提出的非線性速度約束方法精度高于線性速度約束方法,并且在GPS重獲信號(hào)時(shí)也能夠保證狀態(tài)快速收斂到真值,提高了車載導(dǎo)航在GPS失效時(shí)的性能。后續(xù)研究中可以加入里程計(jì),引入前向的速度約束,進(jìn)行更高精度的組合導(dǎo)航。

[1] 牛小驥,曠儉,陳起金. 采用MEMS慣導(dǎo)的小口徑管道內(nèi)檢測(cè)定位方案可行性研究[J]. 傳感技術(shù)學(xué)報(bào),2016,1:40-44.

[2] 吳利青,徐德輝,熊斌. 基于MEMS技術(shù)的雙凸臺(tái)微型熱電能量采集器的仿真和制備[J]. 傳感技術(shù)學(xué)報(bào),2016,3:305-312.

[3] Zhang B,Chu H,Sun T,et al. Error Prediction for SINS/GPS After GPS Outage Based on Hybrid KF-UKF[J]. Mathematical Problems in Engineering,2015.

[4] Ilyas M,Yang Y,Qian Q S,et al. Low-Cost IMU/Odometer/GPS Integrated Navigation Aided with Two Antennae Heading Measurement for Land Vehicle Application[C]//Control and Decision Conference(CCDC),2013 25th Chinese. IEEE,2013:4521-4526.

[5] Godha S. Performance Evaluation of Low Cost MEMS-Based IMU Integrated with GPS for Land Vehicle Navigation Application[M]. Library and Archives Canada Bibliothèque et Archives Canada,2006.

[6] Zhang T,Xu X. A New Method of Seamless Land Navigation for GPS/INS Integrated System[J]. Measurement,2012,45(4):691-701.

[7] Chen L,Fang J. A Hybrid Prediction Method for Bridging GPS Outages in High-Precision POS Application[J]. Instrumentation and Measurement,IEEE Transactions on,2014,63(6):1656-1665.

[8] Hasan A M,Samsudin K,Ramli A R,et al. Automatic Estimation of Inertial Navigation System Errors for Global Positioning System Outage Recovery[J]. Proceedings of the Institution of Mechanical Engineers,Part G:Journal of Aerospace Engineering,2011,225(1):86-96.

[9] Yan G,Yan W,Yan W,et al. A SINS Nonlinear Error Model Reflecting Better Characteristics of SINS Errors[J]. Xibei Gongye Daxue Xuebao,2009,27(4).

[10] Shin E H,El-Sheimy N. Accuracy Improvement of Low Cost INS/GPS for Land Applications[M]. National Library of Canada=Bibliothèque nationale du Canada,2003.

[11] Titterton D,Weston J L. Strapdown Inertial Navigation Technology[M]. IET,2004.

[12] Groves P D. Principles of GNSS,Inertial,and Multisensor Integrated Navigation Systems[M]. Artech House,2013.

[13] Wu Z,Yao M,Ma H,et al. Improving Accuracy of the Vehicle Attitude Estimation for Low-Cost INS/GPS Integration Aided by the GPS-Measured Course Angle[J]. IEEE Transactions on Intelligent Transportation Systems,2013,14(2):553-564.

[14] Godha S. Performance Evaluation of Low Cost MEMS-Based IMU Integrated with GPS for Land Vehicle Navigation Application[M]. Library and Archives Canada=Bibliothèque et Archives Canada,2006.

[15] 徐田來(lái). 車載組合導(dǎo)航信息融合算法研究與系統(tǒng)實(shí)現(xiàn)[D]. 哈爾濱:哈爾濱工業(yè)大學(xué),2007.

[16] Julier S J,Uhlmann J K. Unscented Filtering and Nonlinear Estimation[J]. Proceedings of the IEEE,2004,92(3):401-422.

[17] Zhang P,Gu J,Milios E E,et al. Navigation with IMU/GPS/Digital Compass with Unscented Kalman Filter[C]//Mechatronics and Automation,2005 IEEE International Conference. IEEE,2005,3:1497-1502.

[18] Kol?s S,Foss B A,Schei T S. Constrained Nonlinear State Estimation Based on the UKF Approach[J]. Computers and Chemical Engineering,2009,33(8):1386-1401.

[19] Van Der Merwe R. Sigma-Point Kalman Filters for Probabilistic Inference in Dynamic State-Space Models[D]. Oregon Health and Science University,2004.

姜博文(1991-),男,碩士研究生,主要研究方向?yàn)榻M合導(dǎo)航;

王可東(1975-),男,博士,副教授,碩士生導(dǎo)師,主要研究方向?yàn)樾l(wèi)星/慣性組合導(dǎo)航、天文/慣性組合導(dǎo)航、最優(yōu)濾波算法和地形匹配算法等,wangkd@buaa.edu.cn。

Algorithm Design for a Low-Cost Vehicle Integrated Navigation System in the GPS Outage

JIANGBowen,WANGKedong*

(School of Astronautics,Beihang University,Beijing 100191,China;)

The performance of a micro-electro-mechanical system-inertial measurement unit(MEMS-IMU)/global positioning system(GPS)integrated system will be degraded significantly due to the poor performance of the MEMS-IMU if the GPS signals are blocked by the buildings. To resolve this problem,the up and side vehicle velocity constraints are modelled as the virtual measurements. The nonlinear velocity measurement equations are derived without the small angle approximation if the nonlinearity due to the low accuracy of the MEMS IMU is taken into account in the GPS outage. The Unscented Kalman Filter(UKF)is employed to improve the integration accuracy. The vehicle test results show that the proposed nonlinear algorithm with the velocity constrains can improve the integration accuracy significantly in the GPS outage and converge quickly when the GPS signals is reacquired.

GPS;MEMS;UKF;integrated navigation;inertial navigation system

2016-07-05 修改日期:2016-11-02

V249.32

A

1004-1699(2017)03-0412-06

C:7230

10.3969/j.issn.1004-1699.2017.03.014