顧 濤，陳 帥，譚聚豪，王 琮，陳安升

(1.南京理工大學自動化學院，南京 210094；2.北京自動化控制設備研究所，北京 100074)

0 引言

目前，比較典型的組合導航系統(tǒng)有慣性/衛(wèi)星、慣性/合成孔徑雷達(Synthetic Aperture Radar，SAR)圖像匹配組合導航系統(tǒng)，以及慣性/天文(Celestial Navigation System，CNS)組合導航系統(tǒng)等，但是這些單一模式的組合導航系統(tǒng)并不能提供高精度的位置和速度信息，也不能滿足導航系統(tǒng)長時間穩(wěn)定可靠運行的要求。慣性/衛(wèi)星組合導航系統(tǒng)憑借衛(wèi)星導航系統(tǒng)對速度和位置的精確測量可以獲得高精度的位置和速度信息，但衛(wèi)星信號容易受到電磁干擾或遮擋，導致衛(wèi)星信號失鎖，在軍事應用中其精度和可靠性難以得到保證[1-2]；慣性/天文組合導航系統(tǒng)利用天體測量信息和慣性測量信息獲取高精度導航參數(shù)，具有自主性強和姿態(tài)精度高的特點，但天文導航系統(tǒng)也會受到可觀測星光這些自然環(huán)境的影響[3-4]。

基于以上，單一導航系統(tǒng)輔助慣性導航系統(tǒng)的組合模式已經(jīng)無法滿足導航系統(tǒng)高精度和高可靠性的需求。根據(jù)導航系統(tǒng)的環(huán)境特點和工作特性，以技術成熟和具有獨特優(yōu)勢的慣性/衛(wèi)星組合導航系統(tǒng)為基礎，融合具有不同工作特點的導航傳感器的輸出信息組成基于多源信息融合的組合導航系統(tǒng)，從而提高了車載導航系統(tǒng)的精度和可靠性。傳統(tǒng)聯(lián)邦濾波算法中，子濾波器的信息因子分配系數(shù)采用有重置的系數(shù)平均分配法，即βm=0,βi=1/N的分配方式進行聯(lián)邦濾波[5]，這種方式把每個導航子傳感器的噪聲特性默認為一樣的。然而，系統(tǒng)中的傳感器噪聲特性各不相同，此時仍采用均勻分配會導致系統(tǒng)狀態(tài)估計量的不準確，從而降低導航精度。文獻[6]中提出了一種基于協(xié)方差矩陣跡的信息因子分配方式，由于需要經(jīng)過狀態(tài)轉移矩陣運算后，反饋給下一時刻的協(xié)方差陣，這種方法在容錯性上會存在滯后。

本文主要采用基于聯(lián)邦濾波捷聯(lián)慣性導航系統(tǒng)(Strapdown Inertial Navigation System，SINS)/全球導航衛(wèi)星系統(tǒng)(Global Navigation Satellite System，GNSS)/里程計(Odometry，OD)/高度計的多源聯(lián)邦濾波組合導航模型，如圖1所示。并設計了一種基于預測殘差的自適應信息因子分配方法，使得信息因子分配系數(shù)以導航傳感器的精度為基礎進行實時地自適應分配，從而有效提高了聯(lián)邦濾波精度。為了滿足地面車載情況下的長時間復雜環(huán)境需要，設計了一種改進的χ2故障檢測法，與傳統(tǒng)χ2故障檢測法相比，該算法能更快地識別檢測出傳感器緩變故障，在傳感器出現(xiàn)故障時能夠更迅速地隔離故障的子濾波器，并將其對組合導航系統(tǒng)的影響降至最低，提高了組合導航系統(tǒng)的精度和可靠性。

圖1 SINS/GNSS/OD/高度計組合導航聯(lián)邦濾波模型

1 SINS/GNSS/OD/高度計組合導航濾波器結構設計

聯(lián)邦濾波器是一種如圖1所示的兩級濾波結構濾波器，公共參考子系統(tǒng)SINS的輸出Xk一方面給主濾波器，另一方面給各子濾波器作為量測值[7]。而各子系統(tǒng)的輸出只給各自的濾波器。將各子濾波器的局部估計值Xi及其協(xié)方差陣Pi送入主濾波器，和主濾波器的估計值一起進行融合得到全局估計值。

系統(tǒng)信息在主、子濾波器間的分配方式是基于信息分配原則的[9]，即滿足

(1)

其中，βm為主濾波器信息分配系數(shù)；βi為第i個子濾波器對應的信息分配系數(shù)。

系統(tǒng)信息在子濾波器中的具體分配方法如下

(2)

主濾波器最優(yōu)信息融合為

(3)

2 多源導航系統(tǒng)數(shù)學模型

2.1 聯(lián)邦子濾波器基本模型

慣導的誤差模型在文獻[10]中有詳細介紹，本文不再贅述，誤差模型具體如下：

在本文的聯(lián)邦濾波組合導航系統(tǒng)中，SINS作為公共參考系統(tǒng)，系統(tǒng)誤差狀態(tài)方程取

XSINS=[φEφNφUδVEδVNδVUδL

δλδhεbxεbyεbzεrxεryεrz

(4)

1)聯(lián)邦濾波子濾波器狀態(tài)方程為

(5)

其中，XI(t)同上文XSINS，為捷聯(lián)慣導系統(tǒng)誤差狀態(tài)；誤差系數(shù)矩陣GI(t)和狀態(tài)系數(shù)矩陣FI(t)在文獻[10]中均有定義，不再贅述。

2)聯(lián)邦濾波子濾波器量測方程為

ZI(t)=HI(t)X(t)+VI(t)

(6)

其中，ZI為量測誤差矩陣;HI和VI根據(jù)不同的組合選取有所不同。

2.2 聯(lián)邦濾波子濾波器數(shù)學模型

2.2.1 SINS/GNSS松組合濾波子系統(tǒng)

SINS/GNSS濾波子系統(tǒng)采用速度、位置松組合的方式，以慣導和北斗衛(wèi)星導航系統(tǒng)(BeiDou Navigation Satellite System，BDS)兩者的速度之差和位置之差作為子系統(tǒng)觀測量，則SINS/BDS子系統(tǒng)的量測方程為

(7)

式中，vIE、vIN、vIU為慣性導航系統(tǒng)的東、北、天方向的速度誤差，vGE、vGN、vGU為BDS的東、北、天方向的速度誤差；LI、λI、hI為慣性導航系統(tǒng)的緯度、經(jīng)度、高度，LG、λG、hG為BDS的緯度、經(jīng)度、高度；VG為量測噪聲。

2.2.2 SINS/OD組合濾波子系統(tǒng)

OD是測量車輛行駛路程的儀器，采用脈沖計數(shù)的方式測量采樣時間間隔內載體的路程增量，可以提供載體的前向速度信息，脈沖數(shù)經(jīng)刻度因子轉換為速度測量值[12]。

本文中，以慣導與OD的組合方式選取速度組合，慣導和OD兩者的速度之差作為子系統(tǒng)觀測量，則SINS/OD子系統(tǒng)的量測方程為

(8)

式中，vIE、vIN、vIU為慣性導航系統(tǒng)的東、北、天方向的速度誤差，vOE、vON、vOU為OD的東、北、天方向的速度誤差；VO為量測噪聲。

2.2.3 SINS/高度計組合濾波子系統(tǒng)

氣壓高度表是以氣壓敏感元件作為傳感器感受大氣壓力，再根據(jù)氣壓與高度的函數(shù)關系確定高度值的間接測量儀器，可以測量載體的絕對高度以及標準氣壓高度[13]。

以慣導和高度計的組合方式選取高度組合，慣導與高度計之間的高度之差作為子系統(tǒng)量測值，則SINS/高度計的量測方程為

ZA(t)=hI-hA=HA(t)X(t)+VA(t)

(9)

式中，hI為慣性導航系統(tǒng)的高度，hA為氣壓高度計的高度；VA為量測噪聲。

3 自適應分配聯(lián)邦濾波

傳統(tǒng)聯(lián)邦濾波采用各子濾波器信息分配等分的原則進行分配，本文設計了一種基于預測殘差的聯(lián)邦濾波自適應分配方法，通過實時調整分配因子，提高了系統(tǒng)的濾波精度和性能。

Kalman濾波是否收斂可以依據(jù)狀態(tài)誤差的協(xié)方差陣P的對角線元素進行判斷，其對角線元素與狀態(tài)誤差量中的相應元素相對應。如果在經(jīng)過一次濾波后其相應狀態(tài)誤差協(xié)方差陣P中對應元素項小于設定的閾值，則認為濾波收斂。

可以基于子濾波器的殘差對傳感器相應的子濾波器精度進行判定。當精度高時，Kalman濾波器的殘差rk是零均值白噪聲；當傳感器精度變低時，殘差的均值就不再為0。因此對殘差的檢驗可以確定各導航傳感器的精度。

系統(tǒng)殘差可以表示為

(10)

系統(tǒng)殘差的協(xié)方差理論值為

(11)

系統(tǒng)殘差的協(xié)方差理論值可以表示為

(12)

因此，通過式(11)和式(12)構建自適應分配因子

(13)

式中，c的范圍為0.8～1，本文中取1，其中

(14)

根據(jù)式(14)可推導得到

(15)

為了確保信息分配因子遵循信息守恒原理，需將上述分配因子進行歸一化。

4 故障檢測隔離算法實現(xiàn)

在進行多源信息融合的過程中，需要對傳感器的狀態(tài)進行檢測和判斷，在傳感器故障時進行實時隔離，防止其進入主濾波器中污染濾波結果，以保證濾波器正常工作，因此需要多源融合故障檢測算法來提高系統(tǒng)的精度和穩(wěn)定性。

本文設計了一種改進的χ2殘差檢測法，提出了在殘差特征值的基礎上對檢測閾值進行模糊邏輯和加權處理，具體方法如下：

定義殘差向量

(16)

從當前時刻往前選取n個殘差向量構成殘差矩陣

(17)

定義殘差方差矩陣

H=RTR

(18)

矩陣H的特征值最大值有界，即

λmax≤S

(19)

選取N個矩陣的最大特征值以提高數(shù)據(jù)的準確性，具體為

(20)

此時，故障判斷準則為

為了減少計算量，n和N的取值不宜過大，本文中設為n=2,N=3。

式中，λ是服從自由度為m的χ2分布，即λ～χ2(m) (m根據(jù)濾波器量測方程的維數(shù)進行確定)；TD為預先設置的門限值，這樣就可以根據(jù)χ2分布表、信息的置信度以及實際濾波計算情況設置濾波發(fā)散的門限值。如此一來，既可以保證判斷的靈敏度，又可以防止出現(xiàn)誤判的現(xiàn)象，有效地保證了濾波的可靠性和系統(tǒng)的安全性。

當某一個子濾波器檢測出故障時，系統(tǒng)舍棄該濾波器的信息，主濾波器的估計值舍棄故障濾波器的信息而使用其他子濾波器的值進行最優(yōu)估計。

當系統(tǒng)加入故障檢測算法后，聯(lián)邦濾波自適應分配系統(tǒng)形成如圖2所示的兩級檢測系統(tǒng)進行工作。第一級為子濾波器將濾波結果送入故障檢測算法進行判斷，對故障的子濾波器進行隔離；第二級為判斷為無故障的濾波器送入主濾波器進行自適應信息因子分配。

圖2 聯(lián)邦濾波自適應分配兩級結構

5 實驗與分析

為了驗證本文設計的基于聯(lián)邦濾波的SINS/GNSS/OD/高度計多源組合導航性能，進行了車載多源融合實驗。

5.1 實驗條件

本文使用的導航系統(tǒng)基于自研BDS，SINS采用MSI3200微慣性測量單元(Micro Inertial Measurement Unit，MIMU)，OD采用某車載里程計，氣壓高度計采用BMP280氣壓傳感器。由于BDS目前收星情況不如全球定位系統(tǒng)(Global Positioning System，GPS)，本次實驗將自研BDS作為聯(lián)邦濾波中的GNSS傳感器，而高精度UBLOX衛(wèi)星導航系統(tǒng)作為基準進行參照。

實驗地點為南京麒麟科技園周邊，跑車路線選取直線，測試路線如圖3所示。

圖3 車載運行軌跡

本次實驗分為2組，第1組在傳感器無故障條件下進行車載實驗，通過對采集數(shù)據(jù)進行仿真，驗證信息因子自適應分配的精度；第2組在仿真中加入突變故障和緩變故障，用于驗證改進的殘差故障檢測算法的性能。

5.2 實驗結果與分析

第1組跑車實驗的行駛路徑如圖3所示，將450s運動過程中所得到的誤差情況分別列于圖4～圖9中，位置和速度的平均誤差(1σ)如表2和表3所示；第2組實驗中，在一段時間內加入突變故障與緩變故障，主要對GPS發(fā)生故障時進行仿真驗證，故障具體設置如表1所示，算法的故障檢測值如圖10和圖11所示。

圖4 東向速度誤差對比

圖5 北向速度誤差對比

圖6 天向速度誤差對比

圖7 緯度誤差對比

圖8 經(jīng)度誤差對比

圖9 高度誤差對比

表1 故障設置表

表2 平均分配組合誤差信息(1σ)

表3 自適應分配組合誤差信息(1σ)

圖10 傳統(tǒng)殘差χ2故障檢測值

圖11 改進殘差χ2故障檢測值

由表2～表3和圖4～圖9可知，由于在車載條件下，各導航傳感器在不同時刻根據(jù)環(huán)境的變化精度有所不同，當加入基于預測殘差的自適應分配后，位置和速度信息的精度明顯得到提高，其中，位置精度較平均分配提升約1～2m，速度精度較平均分配提升約0.1m/s左右。

由表1和圖10～圖11可知,在加入突變故障后，傳統(tǒng)和改進殘差χ2故障檢測法均能迅速且準確地檢測出故障;當加入緩變故障時，傳統(tǒng)χ2殘差檢測法在緩變故障發(fā)生后延遲58s才檢測出來，而改進χ2殘差檢測法則在故障發(fā)生后延遲30s檢測出來，所以該算法較傳統(tǒng)χ2殘差檢測法靈敏度得到提升，能夠提前檢測出緩變故障。

6 結論

本文主要提出了一種基于聯(lián)邦濾波的SINS/GNSS/OD/高度計多源組合導航算法：

1)針對各導航傳感器精度不同的問題，設計了一種基于預測殘差的信息因子自適應分配算法，使得分配系數(shù)可以反映各傳感器狀態(tài)變量的特性；并對算法進行仿真實驗，結果分析顯示了該算法能夠充分利用各子系統(tǒng)的工作特性，從而提高了多源組合導航系統(tǒng)的導航精度。

2)針對實際情況下導航傳感器易出現(xiàn)突變故障和緩變故障的情況，設計了一種改進的殘差故障檢測算法；并通過分析驗證，該算法較傳統(tǒng)故障檢測算法能夠更快地識別緩變故障，從而提高了多源組合導航系統(tǒng)的容錯性。