基于MIMU/GPS/EC的聯(lián)邦卡爾曼濾波器設(shè)計?

張 樨,李 杰,蔣 竅,劉曉玲

(1.中北大學(xué)信息與通信工程學(xué)院,山西太原 030051;2.中北大學(xué)電子測試技術(shù)國防重點(diǎn)實(shí)驗(yàn)室,山西太原 030051;3.電子科技集團(tuán)第2研究所,山西太原 030024）

0 引 言

隨著組合導(dǎo)航水平的不斷提高,組合導(dǎo)航理論與組合導(dǎo)航技術(shù)也同步發(fā)展,其中最常用的組合導(dǎo)航算法即為卡爾曼最優(yōu)估計理論.作為組合導(dǎo)航系統(tǒng)的核心算法,卡爾曼濾波技術(shù)將各個導(dǎo)航系統(tǒng)的信息最優(yōu)地進(jìn)行組合,估計出系統(tǒng)的各種誤差狀態(tài),再用誤差狀態(tài)去校正系統(tǒng),從而構(gòu)成具有更高精度、更高容錯性能、更高可靠性的組合導(dǎo)航系統(tǒng)[1,2].利用卡爾曼濾波技術(shù)進(jìn)行最優(yōu)組合,通常有兩種途徑:集中式卡爾曼濾波和分散化卡爾曼濾波.集中式卡爾曼濾波是利用一個卡爾曼濾波器來集中地處理所有的導(dǎo)航信息,存在狀態(tài)維數(shù)高,計算量大、不利于故障診斷、容錯性差等不足,不利于濾波器的實(shí)時運(yùn)行和控制[3].

而在眾多的分散化卡爾曼濾波方法中,Carlson提出的聯(lián)邦濾波器(Federated Filter)因?yàn)樵O(shè)計靈活、算法簡單、計算量小、容錯性高、便于實(shí)時執(zhí)行等突出優(yōu)點(diǎn),致力于解決如何在保證系統(tǒng)容錯性能的同時,提高濾波器的精度,特別適用于組合導(dǎo)航系統(tǒng)的信息融合處理[4-5].本文針對MIMU/GPS/EC組合導(dǎo)航系統(tǒng)的特點(diǎn),提出并設(shè)計了位置、速度、姿態(tài)分別組合的無重置式聯(lián)邦卡爾曼濾波器,旨在進(jìn)一步增強(qiáng)系統(tǒng)的容錯性能、提高導(dǎo)航定位精度.

1 MIMU/GPS/電子羅盤組合導(dǎo)航系統(tǒng)信息融合方法

在分析MIMU、GPS、電子羅盤工作原理的基礎(chǔ)上,利用MIMU自主性強(qiáng)、可靠性高、輸出信息全面等諸多優(yōu)點(diǎn),選擇將其作為公共參考系統(tǒng),GPS為觀測子系統(tǒng)1,利用MI MU/GPS組合構(gòu)成位置子濾波器和速度子濾波器;電子羅盤選取為觀測子系統(tǒng)2,利用MIMU/電子羅盤構(gòu)成姿態(tài)子濾波器.子濾波器之間相互獨(dú)立,無反饋重置帶來的相互影響,為系統(tǒng)保證了最高的容錯性能.具體信息融合方案如圖1所示.

從圖1可以得出,MIMU/GPS組合采取位置、速度組合模式,將MIMU和GPS輸出的位置和速度的差值分別作為量測值,最優(yōu)地估計出MIMU的誤差,然后對MIMU進(jìn)行校正.對于電子羅盤,它在導(dǎo)航過程中只能提供三維姿態(tài)角,故MIMU和電子羅盤選擇姿態(tài)組合模式.將電子羅盤提供的三維姿態(tài)角與MIMU解算輸出姿態(tài)角取差值作為姿態(tài)角信息的量測值,通過濾波算法估計出系統(tǒng)姿態(tài)角誤差,然后對MIMU進(jìn)行反饋校正,進(jìn)一步提高姿態(tài)解算精度.

圖1 組合導(dǎo)航系統(tǒng)最優(yōu)估計融合方案Fig.1 Optimal estimation fusion program for the designed integrated navigation system

2 組合系統(tǒng)數(shù)學(xué)模型建立

2.1 MIMU公共參考系統(tǒng)狀態(tài)方程

首先,建立MIMU公共參考系統(tǒng)狀態(tài)方程.考慮到MIMU的誤差主要來自傳感器自身的常值漂移和隨機(jī)誤差,以及標(biāo)度因數(shù)、溫度特性等,為減小計算量,通常將模型簡化,重點(diǎn)考慮其主要誤差項(xiàng)及誤差特點(diǎn)[6].以MIMU作為公共參考系統(tǒng),取狀態(tài)變量為

式中:δφe,δφn,δφu分別為平臺東向、北向和天向的失準(zhǔn)角;δVe,δVn,δVu分別為系統(tǒng)東向、北向和天向的速度誤差;δL,δλ,δh分別為緯度、經(jīng)度和高度的誤差;εbx,εby,εbz為陀螺的常值漂移;εrx,εry,εrz為陀螺誤差模型的一階馬爾科夫過程;▽x,▽y,▽z為加速度計的常值漂移.

MIMU的狀態(tài)方程為:

式中:FI,GI,WI分別為狀態(tài)轉(zhuǎn)移矩陣、噪聲驅(qū)動矩陣和系統(tǒng)噪聲.

2.2 MIMU/GPS位置子濾波器

GPS系統(tǒng)的定位誤差主要包括GPS衛(wèi)星有關(guān)的誤差、與GPS信號傳播有關(guān)的誤差、與觀測和接收機(jī)有關(guān)的誤差等.為方便實(shí)際應(yīng)用,通常將各種誤差都投影到距離上進(jìn)行分析.雖然各種誤差源具有不同的統(tǒng)計特性,但如果觀測時間足夠長,所有的誤差都可以看作是零均值的隨機(jī)過程,因此對于GPS誤差也可當(dāng)成零均值隨機(jī)過程來處理.

2.3 MIMU/GPS速度子濾波器

選取公共狀態(tài)為速度子濾波器的狀態(tài)向量,即X2=XI.那么,速度子濾波器的狀態(tài)方程即為

式中:F2,G2,W 2分別等同于F I,G I,W I.

取MIMU輸出的速度信息與GPS接收機(jī)輸出的速度信息之差值作為速度子濾波器的量測值.建立速度子濾波器的量測方程如下

2.4 MIMU/電子羅盤姿態(tài)子濾波器

選取公共狀態(tài)為狀態(tài)向量,即 X3=XI.則姿態(tài)子濾波器的狀態(tài)方程

式中:F3,G3,W 3分別等同于F I,G I,W I.

選MIMU輸出的姿態(tài)信息與電子羅盤的姿態(tài)信息的差值為該子濾波器的量測值,并建立姿態(tài)子濾波器的量測方程如下

式中 :H3=[03×3I3×303×4];V3(t)=[PePnPu]T為電子羅盤的量測白噪聲.

2.5 主濾波器融合算法

3 試驗(yàn)驗(yàn)證

為了考核本文所設(shè)計的聯(lián)邦濾波算法的有效性、準(zhǔn)確性,設(shè)計了Matlab理論仿真試驗(yàn).仿真過程中,假設(shè)車輛作勻速直線運(yùn)動,時速80 km/h,初始航向角為 45°,初始姿態(tài)角誤差 0.05°,0.05°,0.1°,初始速度誤差 0.1m/s,軌跡起點(diǎn)位置設(shè)為東經(jīng) 112°、北緯 38°、高度840m,初始位置誤差 1m,具體初始條件如表1.

試驗(yàn)中,設(shè)置三個子濾波器為各自的采樣頻率,主濾波器的濾波頻率為 0.1 Hz,濾波總時間為500 s.其中,MIMU、電子羅盤、GPS的采樣頻率分別為1 000 Hz、10Hz、10 Hz,且保持采樣同步.同時,為使仿真條件與實(shí)際情況盡量一致,在添加系統(tǒng)噪聲時,首先對實(shí)際組合系統(tǒng)中MIMU的輸出進(jìn)行性能測試分析,得到該種 MIMU中陀螺和加速度計的噪聲特性,再將相同量級的噪聲加入仿真試驗(yàn)中;同樣地,根據(jù)實(shí)際所測得GPS接收機(jī)的噪聲特性設(shè)定仿真試驗(yàn)中接收機(jī)輸出噪聲的大小,根據(jù)測得的電子羅盤噪聲特性設(shè)定仿真試驗(yàn)中電子羅盤輸出的噪聲大小.

表1 仿真模型誤差源參數(shù)Tab.1 Error source param eters of the simulation m odel

圖2 純慣導(dǎo)解算與組合導(dǎo)航系統(tǒng)解算誤差對比圖Fig.2 Error comparison char t of inertial navigation and integrated navigation system

從圖2中可以看出,純慣導(dǎo)的解算誤差隨著時間的增長而不斷累積,輸出數(shù)據(jù)不斷偏移真實(shí)值,出現(xiàn)了嚴(yán)重的誤差發(fā)散現(xiàn)象.而組合導(dǎo)航系統(tǒng)通過聯(lián)邦卡爾曼濾波算法對MIMU解算輸出進(jìn)行實(shí)時修正,使組合系統(tǒng)的濾波精度得到了明顯改善,較好地抑制了誤差發(fā)散問題.濾波后系統(tǒng)位置誤差控制在1.8 m以內(nèi),速度誤差在0.9 m/s以內(nèi),濾波后航向角誤差控制在0.5°之內(nèi),有效克服了MIMU/GPS組合在加速度較小時航向誤差觀測性差的問題.結(jié)果表明,所設(shè)計的位置/速度/姿態(tài)組合的聯(lián)邦卡爾曼濾波算法對于提高系統(tǒng)精度、可靠性是十分有效的.此外,考慮到實(shí)際選用的是低成本MIMU,在仿真時為了與實(shí)際情況保持一致,所設(shè)置的MIMU精度也相對較低,若采用高精度傳感器集成 MIMU,則組合系統(tǒng)的精度還可以進(jìn)一步提高.

4 結(jié) 論

本文針對MIMU/GPS/電子羅盤組合導(dǎo)航系統(tǒng)進(jìn)行研究,為提高M(jìn)IMU誤差的修正精度,引入GPS、電子羅盤輔助導(dǎo)航,構(gòu)建位置、速度、姿態(tài)分別組合的無重置式聯(lián)邦卡爾曼濾波器,將MIMU、GPS、電子羅盤進(jìn)行最優(yōu)組合、形成優(yōu)勢互補(bǔ),為系統(tǒng)提供最高的容錯性能.所設(shè)計的濾波算法充分利用各種導(dǎo)航傳感器信息,有效提高了導(dǎo)航系統(tǒng)的綜合性能,具有設(shè)計簡單、可操作性強(qiáng)、精度高的優(yōu)點(diǎn).與純慣導(dǎo)系統(tǒng)相比,該系統(tǒng)具有更好的可靠性、容錯性和實(shí)時性,能夠輸出連續(xù)高精度的位置、速度和姿態(tài)信息,為進(jìn)一步開展組合導(dǎo)航系統(tǒng)的研究試驗(yàn)奠定了良好的基礎(chǔ).

[1] 劉俊,石云波,李杰.微慣性技術(shù)[M].北京:電子工業(yè)出版社,2005:186-190.

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

[3] 張海龍,郝靜如,李啟光.聯(lián)邦卡爾曼濾波在捷聯(lián)慣導(dǎo)/全球定位/里程儀組合系統(tǒng)中的優(yōu)化設(shè)計[J].探測與控制學(xué)報,2009,31(1):66-68.

Zhang Hailong,Hao Jingru,LiQiguang.Theop tim ization design of federal kaiman filter on integrated navigation system of SINS/GPS/odometer[J].Journalof Detection&Control,2009,31(1):66-68.(in Chinese)

[4] Gao Shesheng,Zhong Yongm in,Lia Wei.Robustadap tive filteringmethod for SINS/SAR integrated navigation system[J].Aerospace Science and Technology,2011,15:425-430.

[5] 李鵬程.組合導(dǎo)航及其濾波算法研究[D].西安:西安電子科技大學(xué),2010.

[6] 黃麗.卡爾曼濾波在車載組合系統(tǒng)中的應(yīng)用[D].上海:上海交通大學(xué),2009.

[7] Tariq S Abuhashim,Mamoun F Abdel-Hafez,Mohammad Ameen A I-Jarrah.Building a robust integrity monitoring algorithm for a low cost GPS-aided-INS system[J].International Journalof Control Automation and Systems,2010,8(5):1108-1122.

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

[9] 陳海明.容錯多傳感器組合導(dǎo)航系統(tǒng)算法研究及仿真實(shí)現(xiàn)[D].南京:南京航空航天大學(xué),2011.

[10] 劉明雍,周志遠(yuǎn),趙濤.聯(lián)邦濾波器的SINS/GNS/DVS水下組合導(dǎo)航[J].火力與指揮控制,2009,34(12):41-44.

Liu Mingyong,Zhou Zhiyuan,Zhao Tao.Research on SINS/DNS/DVS underwater integrated navigation based on federal filter[J].Fire Control&Command Control,2009,34(12):41-44.(in Chinese)