基于聯(lián)邦濾波的SINS/GNSS/OD/高度計(jì)多源組合導(dǎo)航算法研究

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

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

0 引言

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

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

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

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

1 SINS/GNSS/OD/高度計(jì)組合導(dǎo)航濾波器結(jié)構(gòu)設(shè)計(jì)

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

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

(1)

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

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

(2)

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

(3)

2 多源導(dǎo)航系統(tǒng)數(shù)學(xué)模型

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

慣導(dǎo)的誤差模型在文獻(xiàn)[10]中有詳細(xì)介紹，本文不再贅述，誤差模型具體如下：

在本文的聯(lián)邦濾波組合導(dǎo)航系統(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)慣導(dǎo)系統(tǒng)誤差狀態(tài)；誤差系數(shù)矩陣GI(t)和狀態(tài)系數(shù)矩陣FI(t)在文獻(xiàn)[10]中均有定義，不再贅述。

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

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

(6)

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

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

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

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

(7)

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

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

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

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

(8)

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

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

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

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

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

(9)

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

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

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

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

可以基于子濾波器的殘差對傳感器相應(yīng)的子濾波器精度進(jìn)行判定。當(dāng)精度高時(shí)，Kalman濾波器的殘差rk是零均值白噪聲；當(dāng)傳感器精度變低時(shí)，殘差的均值就不再為0。因此對殘差的檢驗(yàn)可以確定各導(dǎo)航傳感器的精度。

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

(10)

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

(11)

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

(12)

因此，通過式(11)和式(12)構(gòu)建自適應(yīng)分配因子

(13)

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

(14)

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

(15)

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

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

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

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

定義殘差向量

(16)

從當(dāng)前時(shí)刻往前選取n個(gè)殘差向量構(gòu)成殘差矩陣

(17)

定義殘差方差矩陣

H=RTR

(18)

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

λmax≤S

(19)

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

(20)

此時(shí)，故障判斷準(zhǔn)則為

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

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

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

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

圖2 聯(lián)邦濾波自適應(yīng)分配兩級結(jié)構(gòu)

5 實(shí)驗(yàn)與分析

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

5.1 實(shí)驗(yàn)條件

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

實(shí)驗(yàn)地點(diǎn)為南京麒麟科技園周邊，跑車路線選取直線，測試路線如圖3所示。

圖3 車載運(yùn)行軌跡

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

5.2 實(shí)驗(yàn)結(jié)果與分析

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

圖4 東向速度誤差對比

圖5 北向速度誤差對比

圖6 天向速度誤差對比

圖7 緯度誤差對比

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

圖9 高度誤差對比

表1 故障設(shè)置表

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

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

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

圖11 改進(jìn)殘差χ2故障檢測值

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

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

6 結(jié)論

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

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

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