多源信息融合的組合導(dǎo)航自適應(yīng)聯(lián)邦濾波算法

段 睿, 張小紅,2, 朱 鋒

(1. 武漢大學(xué)測(cè)繪學(xué)院, 湖北 武漢 430079;2. 地球空間信息技術(shù)協(xié)同創(chuàng)新中心, 湖北 武漢 430079)

0 引 言

捷聯(lián)慣導(dǎo)(strapdown inertial navigation system, SINS)具有完全獨(dú)立自主、不受外界干擾、全天候工作的優(yōu)點(diǎn),能夠連續(xù)輸出穩(wěn)定、低噪音的導(dǎo)航信息;在全球衛(wèi)星導(dǎo)航系統(tǒng)(global navigation satellite system, GNSS)以及其他傳感器的輔助下,以SINS為核心構(gòu)建高精度定位、測(cè)速、測(cè)姿一體化的組合導(dǎo)航系統(tǒng),是實(shí)現(xiàn)地理信息數(shù)據(jù)自動(dòng)化采集的關(guān)鍵[1-2]。

在地面開(kāi)闊的環(huán)境下,集成GNSS/SINS的定位定姿系統(tǒng)(position and orientation system,POS)可以提供高精度、穩(wěn)定的外方位元素參考。然而,在地面復(fù)雜環(huán)境下,比如城市峽谷、隧道、高架橋等,由于信號(hào)遮擋及嚴(yán)重的多徑問(wèn)題,GNSS經(jīng)常失鎖,從而使導(dǎo)航系統(tǒng)誤差快速積累。在這種情況下,集成基于多傳感器的多源信息并實(shí)現(xiàn)優(yōu)化融合成為首選方案。里程計(jì)(odometer,OD)[3]、零速修正(zero velocity update, ZUPT)[4]、非完整性約束(non-holonomic constraints, NHC)[5-6]等多源信息由于獲取方便、成本低等優(yōu)勢(shì),被廣泛用作POS系統(tǒng)的外部更新信息源。

但由于這些信息的采集手段的限制,其觀測(cè)結(jié)果往往容易產(chǎn)生粗差甚至出現(xiàn)錯(cuò)誤,其動(dòng)態(tài)模型也容易出現(xiàn)偏差,此時(shí)設(shè)計(jì)容錯(cuò)性能好,能有效抵御故障信息的融合器顯得尤為重要。自從Carlson提出聯(lián)邦濾波結(jié)構(gòu)的分散化濾波器以來(lái),由于其具有設(shè)計(jì)靈活,計(jì)算量小等優(yōu)點(diǎn)而受到重視[7];另一方面,由于結(jié)構(gòu)上各子系統(tǒng)相互隔離,易于實(shí)現(xiàn)系統(tǒng)錯(cuò)誤的檢測(cè)與排除[8],大批學(xué)者對(duì)基于聯(lián)邦濾波器的多源信息融合方法進(jìn)行了研究。房建成等設(shè)計(jì)了一種基于自適應(yīng)模糊系統(tǒng)的聯(lián)邦濾波智能容錯(cuò)結(jié)構(gòu)方案,實(shí)現(xiàn)了SINS/GNSS/天文導(dǎo)航系統(tǒng)(celestial navigation system,CNS)組合系統(tǒng)的容錯(cuò)融合[9]。文獻(xiàn)[10]在GNSS/INS/OD組合系統(tǒng)中利用模糊自適應(yīng)卡爾曼濾波和加權(quán)的方法解決了傳統(tǒng)聯(lián)邦卡爾曼濾波器由于統(tǒng)計(jì)模型的不準(zhǔn)確造成精度下降的問(wèn)題,對(duì)結(jié)果有顯著改善,同時(shí)提高了系統(tǒng)的穩(wěn)定性。文獻(xiàn)[11]結(jié)合聯(lián)邦濾波和無(wú)跡濾波設(shè)計(jì)了無(wú)反饋的聯(lián)邦濾波器,實(shí)現(xiàn)了SINS/GNSS/CNS的優(yōu)化融合。利用聯(lián)邦濾波進(jìn)行融合的關(guān)鍵是其中信息分配系數(shù)的確定,它決定了濾波器的結(jié)構(gòu)和性能。文獻(xiàn)[12]提出了利用不同傳感器局部估計(jì)的濾波方差進(jìn)行自適應(yīng)確定分配系數(shù)的方法。文獻(xiàn)[13]利用Kalman濾波中新息易受觀測(cè)誤差和系統(tǒng)誤差影響的特點(diǎn),采用新息的協(xié)方差矩陣構(gòu)造自適應(yīng)分配系數(shù)。隨后,聯(lián)邦粒子濾波以及基于遺傳算法的自適應(yīng)信息分配方法也被用于多源信息融合[14-15]。

本文則從各子系統(tǒng)的驗(yàn)前新息出發(fā),采用新息單位權(quán)中誤差來(lái)計(jì)算聯(lián)邦濾波器的分配系數(shù),從而構(gòu)建一種新的自適應(yīng)聯(lián)邦濾波算法,通過(guò)動(dòng)態(tài)調(diào)節(jié)聯(lián)邦濾波中的信息分配系數(shù),對(duì)觀測(cè)粗差和模型偏差引起的誤差予以削弱,實(shí)現(xiàn)多源信息的優(yōu)化融合。

1 多源信息融合的數(shù)學(xué)模型

在多源信息松組合導(dǎo)航系統(tǒng)中,以SINS為參考系統(tǒng),GNSS等其他系統(tǒng)提供觀測(cè)更新。以e系作為導(dǎo)航坐標(biāo)系,則SINS的誤差模型為

(1)

GNSS可以提供位置和速度信息,從而對(duì)SINS進(jìn)行觀測(cè)更新。里程計(jì)通過(guò)量測(cè)載體的前向速度來(lái)提供更新信息。零速修正基于載體靜止時(shí)在e系下的速度為零這一原理,提供三軸速度信息進(jìn)行更新。在地面車(chē)載導(dǎo)航中,不發(fā)生側(cè)滑和跳動(dòng)時(shí),載體的側(cè)向速度和垂向速度可認(rèn)為為零,這樣就可以利用此二維速度信息對(duì)載體運(yùn)動(dòng)進(jìn)行約束,從而對(duì)系統(tǒng)進(jìn)行更新,這種更新一般稱(chēng)作非完整性約束。其中,非完整性約束和里程計(jì)由于觀測(cè)值的互補(bǔ)性,可以組成三軸速度觀測(cè)信息進(jìn)行更新[17]。

在本文中, GNSS,OD、零速修正、非完整性約束等多源信息將以松組合的形式參與融合,圖1給出了多源信息融合的結(jié)構(gòu)圖。

圖1 多源信息松組合結(jié)構(gòu)圖Fig.1 Loosely-coupled structure of multi-sources information

2 聯(lián)邦Kalman濾波

在多源信息融合導(dǎo)航系統(tǒng)中,聯(lián)邦濾波器因?yàn)槠溆?jì)算量小、精度高、容錯(cuò)性能好而被廣泛用于容錯(cuò)估計(jì)器的設(shè)計(jì)。由于各觀測(cè)信息源相互獨(dú)立,多源信息融合的聯(lián)邦濾波器可表示為

(2)

聯(lián)邦濾波器通過(guò)信息分配系數(shù)βi對(duì)濾波器進(jìn)行調(diào)節(jié)[7]:

(3)

式中,Q為過(guò)程噪聲方差。根據(jù)信息守恒原則,βi須滿(mǎn)足∑βi=1。

每次濾波前,依據(jù)式(3)對(duì)濾波參數(shù)進(jìn)行分配,進(jìn)入各子濾波器進(jìn)行濾波,然后進(jìn)入主濾波器按式(2)進(jìn)行融合。

圖2給出了在多源信息融合中利用聯(lián)邦濾波器進(jìn)行融合的系統(tǒng)結(jié)構(gòu)圖?？梢钥吹?各子濾波器的結(jié)果經(jīng)過(guò)主濾波器融合之后得到當(dāng)前歷元的全局最優(yōu)狀態(tài)和方差,然后利用信息分配系數(shù)βi對(duì)全局狀態(tài)和方差進(jìn)行分配后又反饋給各子濾波器,從而進(jìn)行下一次濾波。

圖2 多源信息融合聯(lián)邦濾波結(jié)構(gòu)Fig.2 Structure of federated Kalman filter in multi-sensor fusion system

3 自適應(yīng)分配系數(shù)的確定

前面提到,聯(lián)邦濾波器通過(guò)分配系數(shù)βi對(duì)濾波器進(jìn)行控制和調(diào)整,其值的確定直接決定了濾波器的結(jié)構(gòu)和性能。

一般而言,分配系數(shù)依據(jù)觀測(cè)值的先驗(yàn)信息在濾波前給予固定的值。而在多源信息松組合導(dǎo)航系統(tǒng)中,各觀測(cè)信息的觀測(cè)質(zhì)量并非固定不變。在開(kāi)闊環(huán)境下,GNSS觀測(cè)值質(zhì)量較好,而在地面復(fù)雜環(huán)境下由于信號(hào)的遮擋和中斷,其觀測(cè)質(zhì)量很差;里程計(jì)和非完整性約束在載體保持直線(xiàn)行駛時(shí),其數(shù)學(xué)模型和觀測(cè)值較為符合實(shí)際,而在載體發(fā)生強(qiáng)機(jī)動(dòng)、跳動(dòng)、側(cè)滑時(shí),其模型和觀測(cè)值可信度較低;對(duì)于零速修正,則需要準(zhǔn)確地探測(cè)出零速歷元,誤判會(huì)嚴(yán)重影響濾波結(jié)果。因此,在多源信息融合的聯(lián)邦濾波器中,需要考慮子系統(tǒng)數(shù)學(xué)模型和觀測(cè)值質(zhì)量的影響而自適應(yīng)地確定信息分配系數(shù)。

在估計(jì)理論中,驗(yàn)后殘差反映了估計(jì)結(jié)果的好壞[18],但其計(jì)算具有滯后性。而在Kalman濾波中,新息單位權(quán)中誤差的計(jì)算無(wú)需利用驗(yàn)后信息,同時(shí)可以證明,新息單位權(quán)中誤差與驗(yàn)后殘差平方和等價(jià)[19],因此新息單位權(quán)中誤差[18]可以表達(dá)為

(4)

式(4)的計(jì)算只需要驗(yàn)前信息,可以每次濾波前計(jì)算,令分配系數(shù)βi=σ0,則

(5)

文獻(xiàn)[20]從理論上證明了聯(lián)邦濾波和自適應(yīng)濾波的等效性,因此可以利用自適應(yīng)濾波的原理對(duì)聯(lián)邦濾波的分配系數(shù)進(jìn)行確定[21-22];同時(shí),利用抗差自適應(yīng)濾波的權(quán)函數(shù)對(duì)自適應(yīng)濾波進(jìn)行調(diào)整[23],可得

(6)

式中,c1,c2的取值依據(jù)經(jīng)驗(yàn)選取,一般而言c1取值為0.9左右,c2則可取10左右。

4 測(cè)試與分析

測(cè)試數(shù)據(jù)采用NovAtel SPAN系統(tǒng)采集,包含一臺(tái)戰(zhàn)術(shù)級(jí)IMU(iMAR FSAS,零偏小于0.75°/h,完整參數(shù)見(jiàn)文獻(xiàn)[24])和一臺(tái)雙頻測(cè)量型GPS接收機(jī)(NovAtel SPAN-SE),基站采用NovAtel OEM6接收機(jī)。IMU數(shù)據(jù)采樣率為200 Hz,GPS數(shù)據(jù)采樣率為1 Hz。數(shù)據(jù)采集于武漢市郊區(qū),整體觀測(cè)環(huán)境較好,基線(xiàn)長(zhǎng)度不超過(guò)7 km,數(shù)據(jù)軌跡和整段數(shù)據(jù)的可視衛(wèi)星數(shù)、位置精度因子(positional dilution of precision,PDOP)及雙差模糊度固定情況如圖3所示。整段數(shù)據(jù)的平均可視衛(wèi)星數(shù)和PDOP分別為10和1.3,且雙差模糊度全部固定,GPS定位結(jié)果可達(dá)厘米級(jí)。里程計(jì)觀測(cè)值由GPS測(cè)速結(jié)果進(jìn)行模擬,其噪聲標(biāo)準(zhǔn)差為(0.1 m/s)。測(cè)試結(jié)果以IE(Inertial Explorer 8.30)GNSS/SINS雙向平滑緊組合解算結(jié)果作為參考真值。

圖3 測(cè)試數(shù)據(jù)軌跡及觀測(cè)條件Fig.3 Trajectory of field test and observation condition

4.1 測(cè)試1

在GNSS觀測(cè)值,OD觀測(cè)值,零速探測(cè)結(jié)果上加入粗差,分別采用自適應(yīng)聯(lián)邦濾波(方法1)和傳統(tǒng)的聯(lián)邦濾波(方法2)進(jìn)行解算。

圖4、圖5給出了采用兩種解算方法的位置誤差和速度誤差。圖4、圖5中用灰色框分成了三部分:第一部分模擬了GNSS位置和速度誤差,第二部分模擬了里程計(jì)誤差,第三部分模擬零速誤差。從圖中可以看出自適應(yīng)聯(lián)邦濾波相對(duì)于傳統(tǒng)聯(lián)邦濾波而言,可以有效抵御觀測(cè)值粗差的結(jié)果的影響,位置誤差從超過(guò)2 m減小到0.2 m以?xún)?nèi)。速度誤差與位置誤差一致,最大速度誤差從超過(guò)0.4 m/s減小到0.05 m/s以?xún)?nèi)。

圖4 不同解算方法的位置誤差(測(cè)試1)Fig.4 Position errors of different schemes (test 1)

圖5 不同解算方法的速度誤差Fig.5 Velocity errors of different schemes

圖6為采用兩種解算方法的姿態(tài)誤差。從圖6可以看出,采用自適應(yīng)聯(lián)邦濾波后,航向角誤差從0.3°減小到0.1°,水平姿態(tài)角則從0.2°左右減小到0.05°以?xún)?nèi)。同時(shí)可以看出,GNSS位置和速度觀測(cè)粗差對(duì)航向角的干擾較強(qiáng),里程計(jì)和零速修正則較弱,這與不同觀測(cè)信息對(duì)航向角的可觀測(cè)性強(qiáng)弱有關(guān)。

圖6 不同解算方法的姿態(tài)誤差Fig.6 Attitude errors of different schemes

圖7給出了采用自適應(yīng)聯(lián)邦濾波的分配系數(shù),其中圖7(a)的紅色曲線(xiàn)為位置更新的新息單位權(quán)中誤差σ0。可以看到,當(dāng)出現(xiàn)粗差時(shí),σ0會(huì)急劇變大,此時(shí)由式(6)中的第三部分可知,該觀測(cè)信息的分配系數(shù)為零(圖中出現(xiàn)粗差的歷元分配系數(shù)為0,且與σ0突變處重合),即該觀測(cè)值不參與更新,這是自適應(yīng)聯(lián)邦濾波能抵御觀測(cè)粗差的主要原因。同時(shí),在圖7(b)中可以看到,總體而言GNSS速度觀測(cè)值分配系數(shù)比里程計(jì)的分配系數(shù)大,且里程計(jì)出現(xiàn)了較多的拒絕情況,這與兩者的精度差異是相符合的。

4.2 測(cè)試2

為了驗(yàn)證本文的自適應(yīng)聯(lián)邦濾波算法對(duì)動(dòng)態(tài)模型誤差的容錯(cuò)性能,在740～800 s共60 s歷元區(qū)間內(nèi)模擬觀測(cè)模型偏差。分別采用自適應(yīng)聯(lián)邦濾波(方法1)和傳統(tǒng)的聯(lián)邦濾波(方法2)進(jìn)行解算。圖8為采用兩種解算方法的位置誤差。

從圖8可以看出,在進(jìn)行多源信息融合時(shí),相較于傳統(tǒng)聯(lián)邦濾波,自適應(yīng)聯(lián)邦濾波解算時(shí)可以更有效地抵御動(dòng)態(tài)模型粗差對(duì)結(jié)果的干擾。對(duì)加入模型的歷元區(qū)間的結(jié)果進(jìn)行統(tǒng)計(jì),位置誤差RMS從(3.16E-1,8.10E-2,5.52E-2)減小到(2.32E-2,5.50E-3,2.05E-2),分別減小92%,93%和63%。

圖7 自適應(yīng)分配系數(shù)Fig.7 Adaptive factor of multi-sources measurements

圖8 不同解算方法的位置誤差(測(cè)試2)Fig.8 Position errors of different schemes (test 2)

第3節(jié)提到,自適應(yīng)濾波和聯(lián)邦濾波在理論上具有等效性,采用式(6)對(duì)聯(lián)邦濾波的信息分配系數(shù)進(jìn)行調(diào)整時(shí),等效于采用自適應(yīng)濾波器進(jìn)行解算,而自適應(yīng)濾波可以抑制由于模型不準(zhǔn)確引起的誤差[22]。因此,在多源信息融合導(dǎo)航系統(tǒng)中,采用自適應(yīng)聯(lián)邦濾波可以提高動(dòng)態(tài)模型偏差存在時(shí)的解算精度。

5 結(jié) 論

在多源信息融合時(shí),由于數(shù)學(xué)模型偏差和觀測(cè)粗差的存在,采用傳統(tǒng)的聯(lián)邦濾波往往容錯(cuò)性能不佳,使融合結(jié)果精度較差。本文研究了采用自適應(yīng)聯(lián)邦濾波對(duì)多源信息融合的方法,針對(duì)Kalman濾波中新息單位權(quán)中誤差與驗(yàn)后殘差平方和等價(jià)的特點(diǎn),采用驗(yàn)前信息計(jì)算聯(lián)邦濾波的分配系數(shù),并借助自適應(yīng)濾波原理對(duì)分配系數(shù)進(jìn)行調(diào)整,得到了自適應(yīng)聯(lián)邦濾波的模型。利用該模型對(duì)組合導(dǎo)航的多源信息進(jìn)行融合時(shí),可以有效降低觀測(cè)值粗差和數(shù)學(xué)模型偏差對(duì)濾波的干擾,提高了多源信息融合的解算精度。

[1] GROVES P D. Principles of GNSS, inertial, and multi-sensor integrated navigation systems[M]. 2nd ed. Boston/London: Artech House, 2013.

[2] SKALOUD J. Direct GEO-referencing in aerial photogrammetric mapping[J]. Photogrammetric Engineering & Remote Sensing, 2002, 68(3):207-210.

[3] KIM S B, BAZIN J C, LEE H K, et al. Ground vehicle navigation in harsh urban conditions by integrating inertial navigation system, global positioning system, odometer and vision data[J]. IET Radar, Sonar & Navigation, 2011, 5(8):814-823.

[4] LI X, MAO Y, LING X, et al. Applications of zero-velocity detector and Kalman filter in zero velocity update for inertial navigation system[C]∥Proc.of the IEEE Guidance, Navigation and Control Conference, 2014:1760-1763.

[5] WON D, AHN J, SUNG S, et al. Performance improvement of inertial navigation system by using magnetometer with vehicle dynamic constraints[J]. Journal of Sensors, 2015, 2015:1-11.

[6] ILYAS M, PARK S, BAEG S H. Improving unmanned ground vehicle navigation by exploiting virtual sensors and multi-sensor fusion[J]. Journal of Mechanical Science & Technology, 2014, 28(11):4369-4379.

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

QIN Y Y, ZHANG H Y, WANG S H. Kalman filter and the theory of integrated navigation[M]. Xi’an: Northwestern Polytechnical University Press, 1998.

[8] GENG K K, CHULIN N A. Applications of multi-height sensors data fusion and fault-tolerant Kalman filter in integrated navigation system of UAV[J].Procedia Computer Science, 2017, 103: 231-238.

[9] 房建成,李學(xué)恩,申功勛. INS/CNS/GPS智能容錯(cuò)導(dǎo)航系統(tǒng)研究[J]. 中國(guó)慣性技術(shù)學(xué)報(bào), 1999(1): 5-8.

FANG J C, LI X E, SHENG G X. Research on INS/CNS/GPS Intelligent fault tolerant navigation system[J]. Journal of Chinese Inertial Technology,1999(1): 5-8.

[10] CUI P, XU T. Data fusion algorithm for INS/GPS/Odometer integrated navigation system[C]∥Proc.of the 2nd IEEE Conference on Industrial Electronics and Applications,2007:1893-897.

[11] DENG H, LIU G, CHEN H, et al. The application of federated Kalman filtering in SINS/GPS/CNS integrated navigation system[J]. International Journal of Wireless & Microwave Technologies, 2012, 2(2):12-19.

[12] ZHANG H, LENNOX B, GOULDING P R, et al. Adaptive information sharing factors in federated Kalman filtering[J].IFAC Proceedings Volumes, 2002, 35(1): 79-84.

[13] WANG Q, CUI X, LI Y, et al. Performance enhancement of a USV INS/CNS/DVL integration navigation system based on an adaptive information sharing factor federated filter[J]. Sensors, 2017, 17(2):239.

[14] HU H, HUANG X, LI M, et al. Federated unscented particle filtering algorithm for SINS/CNS/GPS system[J]. Journal of Central South University of Technology,2010,17(4):778-785.

[15] WEI Q, FANG J C. Research on FKF method based on an improved genetic algorithm for multi-sensor integrated navigation system[J]. Journal of Navigation, 2012, 65(3): 495-511.

[16] 朱鋒.PPP/SINS組合導(dǎo)航關(guān)鍵技術(shù)與算法實(shí)現(xiàn)[D].武漢:武漢大學(xué), 2015.

ZHU F. The key technology and algorithm of PPP/SINS integration[D].Wuhan: Wuhan University, 2015.

[17] NIU X, NASSAR S, EL-SHEIMY N. An accurate land-vehicle MEMS IMU/GPS navigation system using 3D auxiliary velocity updates[J]. Navigation, 2007, 54(3): 177-188.

[18] 崔希璋,於宗儔,陶本藻,等. 廣義測(cè)量平差[M].2版.武漢:武漢大學(xué)出版社, 2009.

CUI X Z, YU Z C, TAO B Z, et al. Generalized surveying adjustment[M]. 2nd ed. Wuhan: Wuhan University Press, 2009.

[19] 陶本藻.卡爾曼濾波模型誤差的識(shí)別[J].大地測(cè)量與地球動(dòng)力學(xué),1999,19(4):15-20.

TAO B Z. Identification of Kalman filtering model errors[J]. Crustal Deformation and Earthquake, 1999, 19(4):15-20.

[20] LI Z K, GAO J X, WANG J, et al. PPP/INS tightly coupled navigation using adaptive federated filter[J]. GPS Solutions, 2017,21(1):1-12.

[21] YANG Y X, GAO W G. An optimal adaptive Kalman filter[J]. Journal of Geodesy, 2006, 80(4):177-183.

[22] YANG Y X, CUI X Q, GAO W G. Adaptive integrated navigation for multi-sensor adjustment outputs[J]. Journal of Navigation, 2004, 57(2): 287-295.

[23] YANG Y X, GAO W G, ZHANG X. Robust Kalman filtering with constraints: a case study for integrated navigation[J]. Journal of Geodesy, 2010, 84(6): 373-381.

[24] NOVATEL INC. SPAN IMU-FSAS: tactical grade, low noise IMU combines with NovAtel's GNSS technology to create a 3D position, velocity and attitude solution[EB/OL].[2017-08-25].https:∥www.novatel.com/assets/Documents/Papers/FSAS.pdf.