段 睿, 張小紅,2, 朱 鋒

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

0 引 言

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

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

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

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

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

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

(1)

GNSS可以提供位置和速度信息,從而對SINS進行觀測更新。里程計通過量測載體的前向速度來提供更新信息。零速修正基于載體靜止時在e系下的速度為零這一原理,提供三軸速度信息進行更新。在地面車載導(dǎo)航中,不發(fā)生側(cè)滑和跳動時,載體的側(cè)向速度和垂向速度可認(rèn)為為零,這樣就可以利用此二維速度信息對載體運動進行約束,從而對系統(tǒng)進行更新,這種更新一般稱作非完整性約束。其中,非完整性約束和里程計由于觀測值的互補性,可以組成三軸速度觀測信息進行更新[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)邦濾波器因為其計算量小、精度高、容錯性能好而被廣泛用于容錯估計器的設(shè)計。由于各觀測信息源相互獨立,多源信息融合的聯(lián)邦濾波器可表示為

(2)

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

(3)

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

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

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

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

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

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

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

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

(4)

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

(5)

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

(6)

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

4 測試與分析

測試數(shù)據(jù)采用NovAtel SPAN系統(tǒng)采集,包含一臺戰(zhàn)術(shù)級IMU(iMAR FSAS,零偏小于0.75°/h,完整參數(shù)見文獻[24])和一臺雙頻測量型GPS接收機(NovAtel SPAN-SE),基站采用NovAtel OEM6接收機。IMU數(shù)據(jù)采樣率為200 Hz,GPS數(shù)據(jù)采樣率為1 Hz。數(shù)據(jù)采集于武漢市郊區(qū),整體觀測環(huán)境較好,基線長度不超過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é)果可達厘米級。里程計觀測值由GPS測速結(jié)果進行模擬,其噪聲標(biāo)準(zhǔn)差為(0.1 m/s)。測試結(jié)果以IE(Inertial Explorer 8.30)GNSS/SINS雙向平滑緊組合解算結(jié)果作為參考真值。

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

4.1 測試1

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

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

圖4 不同解算方法的位置誤差(測試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°以內(nèi)。同時可以看出,GNSS位置和速度觀測粗差對航向角的干擾較強,里程計和零速修正則較弱,這與不同觀測信息對航向角的可觀測性強弱有關(guān)。

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

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

4.2 測試2

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

從圖8可以看出,在進行多源信息融合時,相較于傳統(tǒng)聯(lián)邦濾波,自適應(yīng)聯(lián)邦濾波解算時可以更有效地抵御動態(tài)模型粗差對結(jié)果的干擾。對加入模型的歷元區(qū)間的結(jié)果進行統(tǒng)計,位置誤差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 不同解算方法的位置誤差(測試2)Fig.8 Position errors of different schemes (test 2)

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

5 結(jié) 論

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

[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智能容錯導(dǎo)航系統(tǒng)研究[J]. 中國慣性技術(shù)學(xué)報, 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ù)與算法實現(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] 崔希璋,於宗儔,陶本藻,等. 廣義測量平差[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] 陶本藻.卡爾曼濾波模型誤差的識別[J].大地測量與地球動力學(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.