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

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

0 引 言

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

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

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

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

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

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

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

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

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

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

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

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

2.2 MIMU/GPS位置子濾波器

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

2.3 MIMU/GPS速度子濾波器

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

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

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

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 試驗驗證

為了考核本文所設計的聯(lián)邦濾波算法的有效性、準確性,設計了Matlab理論仿真試驗.仿真過程中,假設車輛作勻速直線運動,時速80 km/h,初始航向角為 45°,初始姿態(tài)角誤差 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,且保持采樣同步.同時,為使仿真條件與實際情況盡量一致,在添加系統(tǒng)噪聲時,首先對實際組合系統(tǒng)中MIMU的輸出進行性能測試分析,得到該種 MIMU中陀螺和加速度計的噪聲特性,再將相同量級的噪聲加入仿真試驗中;同樣地,根據實際所測得GPS接收機的噪聲特性設定仿真試驗中接收機輸出噪聲的大小,根據測得的電子羅盤噪聲特性設定仿真試驗中電子羅盤輸出的噪聲大小.

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

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

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

4 結 論

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

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

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

[3] 張海龍,郝靜如,李啟光.聯(lián)邦卡爾曼濾波在捷聯(lián)慣導/全球定位/里程儀組合系統(tǒng)中的優(yōu)化設計[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] 黃麗.卡爾曼濾波在車載組合系統(tǒng)中的應用[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] 徐田來.車載組合導航信息融合算法研究與系統(tǒng)實現[D].哈爾濱:哈爾濱工業(yè)大學,2007.

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

[10] 劉明雍,周志遠,趙濤.聯(lián)邦濾波器的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)