基于MIMU/GPS/EC的聯邦卡爾曼濾波器設計?

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

(1.中北大學信息與通信工程學院,山西太原 030051;2.中北大學電子測試技術國防重點實驗室,山西太原 030051;3.電子科技集團第2研究所,山西太原 030024）

0 引 言

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

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

1 MIMU/GPS/電子羅盤組合導航系統信息融合方法

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

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

圖1 組合導航系統最優估計融合方案Fig.1 Optimal estimation fusion program for the designed integrated navigation system

2 組合系統數學模型建立

2.1 MIMU公共參考系統狀態方程

首先,建立MIMU公共參考系統狀態方程.考慮到MIMU的誤差主要來自傳感器自身的常值漂移和隨機誤差,以及標度因數、溫度特性等,為減小計算量,通常將模型簡化,重點考慮其主要誤差項及誤差特點[6].以MIMU作為公共參考系統,取狀態變量為

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

MIMU的狀態方程為:

式中:FI,GI,WI分別為狀態轉移矩陣、噪聲驅動矩陣和系統噪聲.

2.2 MIMU/GPS位置子濾波器

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

2.3 MIMU/GPS速度子濾波器

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

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

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

2.4 MIMU/電子羅盤姿態子濾波器

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

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

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

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

2.5 主濾波器融合算法

3 試驗驗證

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

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

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

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

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

4 結 論

本文針對MIMU/GPS/電子羅盤組合導航系統進行研究,為提高MIMU誤差的修正精度,引入GPS、電子羅盤輔助導航,構建位置、速度、姿態分別組合的無重置式聯邦卡爾曼濾波器,將MIMU、GPS、電子羅盤進行最優組合、形成優勢互補,為系統提供最高的容錯性能.所設計的濾波算法充分利用各種導航傳感器信息,有效提高了導航系統的綜合性能,具有設計簡單、可操作性強、精度高的優點.與純慣導系統相比,該系統具有更好的可靠性、容錯性和實時性,能夠輸出連續高精度的位置、速度和姿態信息,為進一步開展組合導航系統的研究試驗奠定了良好的基礎.

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

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

[3] 張海龍,郝靜如,李啟光.聯邦卡爾曼濾波在捷聯慣導/全球定位/里程儀組合系統中的優化設計[J].探測與控制學報,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].西安:西安電子科技大學,2010.

[6] 黃麗.卡爾曼濾波在車載組合系統中的應用[D].上海:上海交通大學,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].哈爾濱:哈爾濱工業大學,2007.

[9] 陳海明.容錯多傳感器組合導航系統算法研究及仿真實現[D].南京:南京航空航天大學,2011.

[10] 劉明雍,周志遠,趙濤.聯邦濾波器的SINS/GNS/DVS水下組合導航[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)