基于聯邦濾波的SINS/GNSS/OD/高度計多源組合導航算法研究

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

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

0 引言

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

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

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

圖1 SINS/GNSS/OD/高度計組合導航聯邦濾波模型

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

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

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

(1)

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

系統信息在子濾波器中的具體分配方法如下

(2)

主濾波器最優信息融合為

(3)

2 多源導航系統數學模型

2.1 聯邦子濾波器基本模型

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

在本文的聯邦濾波組合導航系統中，SINS作為公共參考系統，系統誤差狀態方程取

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

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

(4)

1)聯邦濾波子濾波器狀態方程為

(5)

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

2)聯邦濾波子濾波器量測方程為

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

(6)

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

2.2 聯邦濾波子濾波器數學模型

2.2.1 SINS/GNSS松組合濾波子系統

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

(7)

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

2.2.2 SINS/OD組合濾波子系統

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

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

(8)

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

2.2.3 SINS/高度計組合濾波子系統

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

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

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

(9)

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

3 自適應分配聯邦濾波

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

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

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

系統殘差可以表示為

(10)

系統殘差的協方差理論值為

(11)

系統殘差的協方差理論值可以表示為

(12)

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

(13)

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

(14)

根據式(14)可推導得到

(15)

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

4 故障檢測隔離算法實現

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

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

定義殘差向量

(16)

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

(17)

定義殘差方差矩陣

H=RTR

(18)

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

λmax≤S

(19)

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

(20)

此時，故障判斷準則為

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

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

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

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

圖2 聯邦濾波自適應分配兩級結構

5 實驗與分析

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

5.1 實驗條件

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

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

圖3 車載運行軌跡

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

5.2 實驗結果與分析

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

圖4 東向速度誤差對比

圖5 北向速度誤差對比

圖6 天向速度誤差對比

圖7 緯度誤差對比

圖8 經度誤差對比

圖9 高度誤差對比

表1 故障設置表

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

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

圖10 傳統殘差χ2故障檢測值

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

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

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

6 結論

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

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

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