基于聯邦濾波的多源融合導航算法

譚聚豪，陳安升，張博雅，陳 帥，溫哲君

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

0 引言

現有的導航系統信息源種類多樣，單一的導航源有GPS、BDS、GLONASS、INS、雷達測速儀、地磁匹配、圖像視覺導航以及天文導航等。然而往往在復雜多變的環境下，單一的導航系統可能會失效，其導航精度將不再可靠。多源融合組合導航系統基于信息融合技術，將來自不同導航源的同構或異構導航信息按照相應的融合算法進行融合，從而得到最佳的融合結果。相對于單一導航源可能會在某一時間或地點失效，多源融合導航可以充分利用每一個導航源的優勢，提高導航解算的糾錯能力，增加觀測的冗余度，從而提供較好的定位與導航服務。

捷聯慣導系統（Strapdown Inertial Navigation System，SINS）是一種不向外部輻射能量、也不依靠外界信息的獨立式導航系統，可以穩定地輸出導航所需要的信息，缺點是其導航精度隨時間而逐漸變差。因此，SINS常與其他導航源進行組合，從而提高長時間導航的精度和可靠性［1-2］。

我國北斗三號系統基本建成，其導航范圍已經覆蓋全球，其全球定位精度為10m，測速精度為0.2m/s，授時精度為20ns，高精度的導航性能可以為全球提供準確的定位、導航和授時服務；Doppler測速儀是測量載體速度的儀器，其基本原理是利用Doppler頻移和速度的關系，具有測速范圍較寬、測量精度較高的優點；里程計（ODO）固定在車輛傳動軸上，在車輛行駛過程中，產生固定數量的脈沖，通過脈沖計數可以得到車輛行駛的里程數，進而得到車輛行駛的前向速度；氣壓高度計則根據氣壓和海拔的變化關系測得載體的高度信息。

目前，已有許多學者對不同信息源以及多種信息源的組合導航進行了研究。文獻［3］提出了一種魯棒優化的因子圖算法應用在多傳感器融合中；文獻［4］采用了一種基于因子圖的增量平滑算法在慣性導航中進行多傳感器的信息融合；文獻［5］提出了一種SINS/GPS/BD2/DVL組合導航聯邦濾波算法，并將其應用到了船舶航行中；文獻［6］提出了一種SINS/GPS/CNS組合導航聯邦濾波算法，針對組合系統模型線性化、易導致濾波發散等問題，進行了研究驗證；文獻［7］將粒子濾波引入聯邦濾波結構中，得到了一種混合聯邦—粒子濾波算法，且以INS/BD/LoranC組合導航系統為對象進行了仿真；文獻［8］根據組合導航Kalman濾波器中新息信息與誤差的關系，利用新息信息的協方差陣對聯邦濾波器的信息分配系數進行了構造；文獻［9］提出了一種自適應聯邦濾波在PPP/INS緊組合中的應用，證明了聯邦濾波和自適應濾波的有機結合，且兩者可等同使用。

本文主要采用SINS/BDS/Doppler測速儀/里程計/氣壓高度計的多源組合導航模型，針對多源融合非線性的特點，采用聯邦濾波融合算法。算法中主濾波器不進行濾波，負責把子濾波器的估計值進行最優融合。此外，在子濾波器中引入殘差故障檢測理論，同時結合自適應濾波的理論，對子濾波器中的新息協方差陣進行判斷。接著，利用檢測結果進行聯邦濾波器信息分配系數的構造，進而形成自適應容錯聯邦濾波器，實現多信息源的最優融合。最后，生成時空基準統一、連續、可靠的PNT服務信息。

1 聯邦濾波組合導航系統模型

1.1 聯邦濾波器設計方案

本文所設計的聯邦濾波器中，以SINS作為公共的信息參考系統，使用了5個信息源，所以設置了4個子濾波器和1個主濾波器。每個子濾波器處理自己的量測信息得到局部估計，再在主濾波器中進行最優估計。此外，本文選用有重置的聯邦濾波器，且采用了一種自適應聯邦信息分配因子，在出現某一子信息源出現故障時，能夠對故障很好地進行隔離，不影響濾波器的正常濾波，使系統保持較好的穩定性和魯棒性。圖1為聯邦濾波器結構圖。

1.2 聯邦濾波器狀態方程

慣性導航系統作為公共的信息參考系統，其誤差模型選用通用的誤差模型，并采用東北天地理坐標系作為導航坐標系。系統的公共狀態變量為

圖1 聯邦濾波器結構圖Fig.1 Structure diagram of federal filter

式（1）中，φE、φN、φU為東北天三個方向上的姿態誤差角，δVE、δVN、δVU為東北天三個方向上的速度誤差，δL、δλ、δh為緯度誤差、經度誤差、高度誤差，分別為載體系下陀螺、加速度計三個軸上的隨機漂移和常值偏置，則誤差狀態方程可以表示為

式（2）中，FSINS為SINS系統的狀態轉移矩陣，GSINS為SINS系統的噪聲驅動矩陣，WSINS為陀螺和加速度計三個軸上均值為零、方差為QSINS的白噪聲，各矩陣的具體計算形式詳見文獻［10］。

1.3 聯邦濾波器量測方程

本文設計的聯邦濾波器主濾波器無信息分配，不用主濾波器進行濾波，主濾波器只對每個子濾波器的局部估計值進行融合進而得到全局估計，下面介紹各子濾波器的量測方程。

（1）SINS/BDS的量測方程

SINS位置信息表示為

SINS的速度信息表示為

BDS的位置信息表示為

BDS的速度信息表示為

則SINS/BDS的量測方程為

式（3）～式（6）中，λRef、LRef、hRef為經度、緯度和高度的基準參考值，vRef_E、vRef_N、vRef_U是載體各軸向的速度基準參考值，PE、PN、PU為BDS東北天三個方向上的位置誤差，BE、BN、BU為BDS東北天三個方向上的速度誤差。式（7）中，H1和V1的具體形式如下

量測噪聲V1看作是均值為零的白噪聲。

（2）SINS/Doppler測速儀的量測方程

Doppler測速儀的速度信息表示為

則SINS/Doppler測速儀的量測方程為

式（11）中，DE、DN、DU為Doppler測速儀東北天三個方向上的速度誤差，H2、V2的具體形式如下

量測噪聲V2看作是均值為零的白噪聲。

（3）SINS/ODO的量測方程

ODO 輸出載體系的速度信息表示為

將ODO輸出的載體速度分解到導航坐標系，并與SINS系統的速度作差，則得觀測方程為

量測噪聲V3看作是均值為零的白噪聲。

（4）SINS/氣壓高度計的量測方程

氣壓高度計的高度信息表示為

式（17）中，AU為高度計的測量誤差，可得SINS/氣壓高度計的量測方程為

2 聯邦濾波器設計

2.1 聯邦濾波器融合方法

在本文設計的聯邦濾波器中，每個子濾波器的狀態變量相同（SINS公共狀態變量），所以不按照傳統的方法在子濾波器中進行單獨的時間更新以及噪聲協方差陣Qf和誤差狀態協方差陣Pf的信息分配，而在主濾波器中進行噪聲協方差陣Qf和誤差狀態協方差陣Pf的時間更新。子濾波器中只完成量測更新，且利用方差上界技術和信息分配方法在不改變子濾波狀態估計情況下保證各子濾波器狀態估計是獨立的、不相關的，這樣子濾波器和主濾波器均為最優估計。將上述設計的子濾波器的狀態方程和量測方程進行離散化，可以得到

系統信息在主濾波器和子濾波器間的信息分配方法是遵守信息分配守恒原則的，即滿足

式（23）中，βm為主濾波器信息分配系數，βi為第i個子濾波器對應的信息分配系數。

由于本文設計的聯邦濾波器采用了主濾波器無信息分配，僅對子濾波器估計進行全局融合并對子濾波器進行反饋，因此信息分配又可表示為

系統信息在子濾波器中的具體分配方法，也即子濾波器中進行的量測更新方法可用下面的式子表示

式（25）中，為主濾波器的狀態估計，Pf、Qf分別為主濾波器的狀態誤差協方差陣和系統噪聲協方差陣，為子濾波器的狀態估計，Pi、Qi分別為子濾波器狀態估計對應的狀態誤差協方差陣和子濾波器系統噪聲協方差陣。

由式（21）可以得到子濾波器協方差陣的時間更新過程，為

由式（27）可知，子濾波器的狀態誤差協方差陣Pi，k-1和噪聲協方差陣Qi，k-1擴大為γi，k-1Pf，k-1和γi，k-1Qk-1后，其預測協方差陣也得到了擴大。

子濾波器的預測和濾波估計方程為

子濾波器的濾波增益為

子濾波器的估計均方差為

在子濾波器中完成了局部估計后，主濾波器根據每個子濾波器的估計值進行全局最優估計，主濾波器的最優融合方法如下

式（32）中，為系統誤差狀態量的全局最優估計結果，Pf為估計狀態量的協方差陣。

2.2 時空基準統一方法

針對多傳感器輸出頻率不同、數據通信延時等情況，由于慣性系統具有短時精度高的特點，采用遞推法將對應時刻慣性數據的量測值變化量補償到低頻率傳感器上作為融合時刻的真實值，這樣就把其他傳感器的滯后數據遞推到了融合時刻。

空間基準統一就是將不同坐標系下的測量信息經過旋轉變換統一到基準坐標系下，基準坐標系為導航坐標系。

此外，子濾波器的濾波周期以輸出頻率最低的傳感器為準，主濾波器周期大于等于子濾波器周期。

2.3 聯邦濾波器自適應信息分配方法

本文設計的聯邦濾波器采用了兩級濾波結構，為了得到全局最優估計，采用了對子濾波器產生的局部估計量進行信息分配。在Kalman濾波中，新息單位權重誤差的計算無需利用驗后信息，沒有信息分配的滯后性，同時新息單位權重誤差與驗后殘差平方和等價［10］。因此，新息單位權重誤差可以表示為

式（33）中，ε＝Zk-Hkk/k-1為各子濾波器的量測殘差，H為量測矩陣，Pk｜k-1為預測方差，Rk為觀測值方差。當無故障發生時，殘差ε的方差Pk為均值為零的噪聲；當有故障發生時，其均值就不再為零了。這樣就可以自適應對故障進行判斷和隔離，提高系統的抗干擾能力。

在每次濾波前進行計算，令分配系數βi＝σ0。并且可以利用自適應濾波的原理對聯邦濾波的分配系數進行確定［8］，同時結合自適應濾波的權函數和殘差χ2故障檢測理論對自適應分配因子進行調整，故可得

式（34）中，c1和c2兩個閾值的確定，文獻［11］是依靠經驗值，而本文在確定時采用了殘差χ2故障檢測理論［12］，c1取值為0.9，c2取值為7.8。

3 實驗與分析

為了對本文設計的聯邦濾波器的性能進行驗證，進行了基于車載運行軌跡的半實物仿真實驗和車載多源融合實驗。

3.1 多源融合仿真實驗

在仿真實驗中，將某組車載運行軌跡注入到軌跡發生器中，并根據軌跡參數對傳感器的精度做限制，且模擬設置某個傳感器的故障狀態，再進行融合，驗證算法的容錯性和可靠性。

仿真中，設置BDS系統的定位誤差在10m以內，測速誤差在2m/s以內；Doppler測速儀的測量誤差在0.5m/s；里程計的測速誤差在0.8m/s以內；高度計的測量誤差在5m內。SINS作為公共信息系統，仿真中采用3個子傳感器與之進行不同的組合，具體仿真結果如圖2～圖7。

圖2 緯度誤差Fig.2 Errors of latitude

圖3 經度誤差Fig.3 Errors of longitude

圖4 高度誤差Fig.4 Errors of height

圖5 東向速度誤差Fig.5 Errors of eastward speed

圖6 北向速度誤差Fig.6 Errors of northbound speed

圖7 天向速度誤差Fig.7 Errors of sky speed

從位置誤差和速度誤差的曲線可以看出，不同傳感器故障下的融合誤差均逐漸變小且趨于平緩，水平誤差穩定在1.5m 以內，高度誤差穩定在1m 以內，水平速度誤差穩定在 0.3m/s 左右，天向速度誤差穩定在0.15m/s 左右，算法可以實現故障隔離后仍穩定、可靠地工作。

3.2 車載多源融合實驗

車載實驗在空曠的郊區進行，實驗中采用本文前面所述的傳感信息源進行融合，來驗證本文算法框架的可行性和合理性。實驗中以某高精度組合導航產品為基準，具體實驗結果如圖8 ～圖16所示。

圖8 車載運行軌跡Fig.8 Running track of vehicle

圖9 位置誤差Fig.9 Errors of position

圖10 速度誤差Fig.10 Errors of speed

圖11 緯度對比Fig.11 Comparison of latitude

圖12 經度對比Fig.12 Comparison of longitude

圖13 高度對比Fig.13 Comparison of height

圖14 東向速度對比Fig.14 Comparison of eastward speed

圖15 北向速度對比Fig.15 Comparison of northbound speed

圖16 天向速度對比Fig.16 Comparison of sky speed

從上述曲線可以看出，水平、垂直位置誤差均在3m左右，且逐漸變小趨于平穩；水平速度誤差在0.2m/s左右，趨于平穩；天向速度誤差在0.15m/s以內，趨于平穩。整個過程的位置和速度曲線與基準相比，均能實現平穩、良好跟蹤，表明車載實驗下算法也能夠可靠的工作。

3.3 實驗結果與分析

在仿真實驗中，不同傳感器故障時聯邦濾波器均可以有效進行隔離，自適應的融合其他子濾波器的估計值進行最優融合。定位效果較好的是“BDS+ Doppler測速儀+高度計”的組合，其緯度誤差為0.7962m（1σ），經度誤差為0.9495m（1σ），高度誤差為1.4188m（1σ）。此外，不同傳感器的組合根據其測量特性會提高載體相應測量數據的精度。比如高度計參與組合的方式中，其高度誤差均較小，最小為1.4188m（1σ）。在車載實驗中，各個傳感器可以正常融合實現可靠定位，整個過程中的位置和速度與基準相比誤差較小，其緯度誤差為1.7828m（1σ），經度誤差為1.5656m（1σ），高度誤差為1.5026m（1σ），具體實驗結果對比如表1所示。

表1 實驗結果對比Table 1 Comparison of experiment results

對上述實驗結果分析可知，車載實驗和仿真實驗的定位誤差均在1.8m以內，定位精度較好。整個實驗表明，本算法可以實現多傳感器可靠的融合定位，且在復雜變化的環境中實現了傳感器故障的有效隔離，提高了測量精度，輸出了可靠的定位導航信息。

4 結論

本文提出了一種多源融合的聯邦濾波組合導航算法，搭建了聯邦濾波的多源融合模型，詳細設計了各子濾波器和主濾波器，且對信息分配因子進行了自適應分配以及將各子濾波器的估計結果進行最優融合，并通過實驗驗證了多源融合的聯邦濾波算法的可行性。算法能夠進行不同導航傳感器的自由融合且能夠取得較好的定位結果，具有不錯的研究意義和實際價值。