多源信息融合的組合導航自適應聯邦濾波算法

段 睿, 張小紅,2, 朱 鋒

(1. 武漢大學測繪學院, 湖北 武漢 430079;2. 地球空間信息技術協同創新中心, 湖北 武漢 430079)

0 引 言

捷聯慣導(strapdown inertial navigation system, SINS)具有完全獨立自主、不受外界干擾、全天候工作的優點,能夠連續輸出穩定、低噪音的導航信息;在全球衛星導航系統(global navigation satellite system, GNSS)以及其他傳感器的輔助下,以SINS為核心構建高精度定位、測速、測姿一體化的組合導航系統,是實現地理信息數據自動化采集的關鍵[1-2]。

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

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

本文則從各子系統的驗前新息出發,采用新息單位權中誤差來計算聯邦濾波器的分配系數,從而構建一種新的自適應聯邦濾波算法,通過動態調節聯邦濾波中的信息分配系數,對觀測粗差和模型偏差引起的誤差予以削弱,實現多源信息的優化融合。

1 多源信息融合的數學模型

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

(1)

GNSS可以提供位置和速度信息,從而對SINS進行觀測更新。里程計通過量測載體的前向速度來提供更新信息。零速修正基于載體靜止時在e系下的速度為零這一原理,提供三軸速度信息進行更新。在地面車載導航中,不發生側滑和跳動時,載體的側向速度和垂向速度可認為為零,這樣就可以利用此二維速度信息對載體運動進行約束,從而對系統進行更新,這種更新一般稱作非完整性約束。其中,非完整性約束和里程計由于觀測值的互補性,可以組成三軸速度觀測信息進行更新[17]。

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

圖1 多源信息松組合結構圖Fig.1 Loosely-coupled structure of multi-sources information

2 聯邦Kalman濾波

在多源信息融合導航系統中,聯邦濾波器因為其計算量小、精度高、容錯性能好而被廣泛用于容錯估計器的設計。由于各觀測信息源相互獨立,多源信息融合的聯邦濾波器可表示為

(2)

聯邦濾波器通過信息分配系數βi對濾波器進行調節[7]:

(3)

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

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

圖2給出了在多源信息融合中利用聯邦濾波器進行融合的系統結構圖。可以看到,各子濾波器的結果經過主濾波器融合之后得到當前歷元的全局最優狀態和方差,然后利用信息分配系數βi對全局狀態和方差進行分配后又反饋給各子濾波器,從而進行下一次濾波。

圖2 多源信息融合聯邦濾波結構Fig.2 Structure of federated Kalman filter in multi-sensor fusion system

3 自適應分配系數的確定

前面提到,聯邦濾波器通過分配系數βi對濾波器進行控制和調整,其值的確定直接決定了濾波器的結構和性能。

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

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

(4)

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

(5)

文獻[20]從理論上證明了聯邦濾波和自適應濾波的等效性,因此可以利用自適應濾波的原理對聯邦濾波的分配系數進行確定[21-22];同時,利用抗差自適應濾波的權函數對自適應濾波進行調整[23],可得

(6)

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

4 測試與分析

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

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

4.1 測試1

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

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

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

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

圖6為采用兩種解算方法的姿態誤差。從圖6可以看出,采用自適應聯邦濾波后,航向角誤差從0.3°減小到0.1°,水平姿態角則從0.2°左右減小到0.05°以內。同時可以看出,GNSS位置和速度觀測粗差對航向角的干擾較強,里程計和零速修正則較弱,這與不同觀測信息對航向角的可觀測性強弱有關。

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

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

4.2 測試2

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

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

圖7 自適應分配系數Fig.7 Adaptive factor of multi-sources measurements

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

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

5 結 論

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

[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] 秦永元,張洪鉞,汪叔華. 卡爾曼濾波與組合導航原理[M]. 西安: 西北工業大學出版社, 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] 房建成,李學恩,申功勛. INS/CNS/GPS智能容錯導航系統研究[J]. 中國慣性技術學報, 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].武漢:武漢大學, 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版.武漢:武漢大學出版社, 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].大地測量與地球動力學,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.