一種基于聯邦卡爾曼濾波器的多源信息融合定位算法

張 靖，陳鴻躍，陳 雨，劉宇航，孫 謙

0 引 言

現代戰爭的突發性和復雜性，要求陸基武器系統在公路及鐵路長時間、大范圍機動下具備精確打擊能力。車載定位系統提供的姿態、位置信息是陸基武器系統具備精確打擊能力的核心支撐之一。因此對車載定位系統長時間精度保持能力、復雜環境適應能力提出了較高要求。

基于多傳感器的組合導航成為解決這一問題的有效方法，一般采用卡爾曼濾波技術最優組合各個傳感器的信息。利用卡爾曼濾波技術對組合導航系統進行最優組合有兩種途徑：a）集中式卡爾曼濾波器，理論上可以得到誤差狀態的最優估計，但存在狀態維數高，計算量大，容錯性能差的問題[1]。b）分散化卡爾曼濾波器，其中聯邦卡爾曼濾波器[2]（Federated Kalman Filter）具備算法靈活性高、計算量小、容錯性能好的特點，可有效融合多種定位信息源。

文獻[3]將聯邦卡爾曼濾波器運用在全球定位系統/航位推算（GPS/DR）的組合導航中，并進行實地跑車試驗，驗證了利用聯邦卡爾曼濾波器進行組合導航的可行性。文獻[4]、文獻[5]建立了基于聯邦卡爾曼濾波器的捷聯慣性導航系統/全球定位系統/里程計（SINS/GPS/OD）組合導航數學模型，但沒有對分系統的桿臂誤差及安裝誤差角進行建模。文獻[6]改進了誤差狀態模型，采用集中式卡爾曼濾波器實現了SINS/OD組合導航系統，并對里程計刻度因子和安裝誤差角進行了估計。文獻[7]搭建的GPS/SINS/OD組合導航系統驗證了在GPS失效的時間段內，采用聯邦卡爾曼濾波器可增加系統的容錯性。文獻[8]通過改進聯邦濾波器對慣性導航系統/全球衛星導航系統/天文導航系統（INS/GNSS/CNS）進行仿真試驗，說明應用聯邦卡爾曼濾波器的定位系統精度仍有提升空間。

本文采用容錯性能高的無重置式聯邦卡爾曼濾波器作為多源信息融合的定位算法，建立了包括里程計和衛星定位桿臂誤差的組合導航模型。搭建了SINS/GNSS/里程計/高程計四種典型車載定位信息源的融合框架，通過仿真和實車試驗驗證了所提算法在計算量、容錯性、定位精度上的優勢。

1 開放式系統架構

選取車載定位領域最常用的信息源SINS、GNSS、里程計、高程計，搭建了一個典型的多信息源組合定位系統框架。如圖1所示，以SINS為公共參考系統，分別與 GNSS、里程計、高程計組成子濾波器，通過故障診斷與系統重構決定各子濾波器是否參與融合，可以實現定位信息源的即插即用。系統架構具備擴展能力，融合更多定位信息源時只需對算法框架做較小改動，可以具備開放性。

圖1 多源信息融合定位系統算法結構Fig.1 Multi-source Information Fusion System Algorithm Architecture

2 系統誤差模型

文中使用的坐標系有：慣性坐標系（i），地球坐標系（e），地理坐標系（g），導航坐標系（n），計算導航坐標系（～n）選取地理坐標系作為導航坐標系，載體坐標系（b），車體坐標系（ ～m）。

2.1 SINS/OD子濾波器

誤差狀態量選取為

式中 ψ為姿態誤差角，；δv為速度誤差，δp為位置誤差，δp = [ δ L δλ δh ]T；ε為陀螺常值零偏，?為加表常值零偏，；δk為里程計刻度系數誤差；，abδδ分別為里程計的航向、俯仰安裝誤差角；為里程計桿臂誤差，共21維狀態變量。

系統誤差方程為如下：

式中表示在導航系下導航系相對于慣性系的轉動角速度；表示的角速度誤差；×表示矢量叉乘；為由b系到n系的方向余弦矩陣。

式中 fb表示比力；表示在導航坐標系下相對于慣性系的地球自轉角速度；表示的角速度誤差；表示在導航系下導航坐標系相對于地球坐標系的轉動角速度；表示的角速度誤差；v為捷聯慣導相對地球表面的速度在導航坐標的表示。

式中 RM表示捷聯慣導所在位置處的子午圈曲率半徑； RN表示捷聯慣導所在位置處的卯酉圈曲率半徑；h表示捷聯慣導所在位置的高度。表示速度v在導航坐標系下東、北、天3個方向的速度分量。

將式（2）～（8）記作：

式中 F1( t)為狀態轉移矩陣， w1為系統噪聲向量。

2.2 SINS/GNSS子濾波器

誤差狀態量選取為

式中為GNSS桿臂誤差，共18維狀態變量。

系統誤差方程同式（2）～（6）、式（8），記作：

2.3 SINS/高程計子濾波器

誤差狀態量選取為

共15維狀態變量。

系統誤差方程同式（2）～（6），記作：

3 觀測方程

3.1 SINS/OD子濾波器

選擇一定時間內 SINS解算的位移增量與SINS/OD航位推算的位移增量差值作為觀測量，即：

式中 vI表示SINS真實速度； vod表示里程計在導航坐標系中的真實速度；表示里程計外桿臂產生的速度；v～D表示里程計測得的解算到 SINS載體坐標系的速度；θ表示由車體坐標系到SINS載體坐標系旋轉的安裝誤差角；表示車體坐標系到載體坐標系的方向余弦矩陣；表示在載體坐標系下導航坐標系相對于載體坐標系的轉動角速度。將上式記作：

式中1Z為觀測矩陣；1H為量測矩陣；1v為量測噪聲向量。

3.2 SINS/GNSS子濾波器

選擇SINS與GNSS的位置差值和速度差值作為觀測量。

3.2.1 位置觀測量

式中 pI表示SINS的位置； pGNSS表示衛星在導航坐標系中的位置；表示衛星外桿臂產生的位置偏差，其中：

整理得到：

式中 lGNSS為衛星桿臂；GNSS表示解算得到的外桿臂；δlGNSS表示外桿臂桿臂誤差，表示計算導航坐標系到導航坐標系的方向余弦矩陣，表示載體坐標系到計算導航坐標系的方向余弦矩陣。

3.2.2 速度觀測量

式中 Zv表示真實的速度觀測量； vI表示SINS的真實速度； vGNSS表示衛星接收機在導航坐標系中的真實速度；ngwv表示衛星接收機外桿臂產生的速度。得到實際測得的速度觀測量為

式中 v～I表示SINS解算的速度；v～gnw表示測得的衛星接收機外桿臂產生的速度； δ vI表示SINS真實速度與解算速度之間相差的速度誤差，衛星速度誤差 δ vGNSS為量測噪聲。記作：

式中2H為量測矩陣；2v為量測噪聲向量。

3.3 SINS/高程計子濾波器

式中表示SINS與高程計的高程觀測量； hI表示SINS解算得到的高度；表示高程計測得的高度；δh表示高度誤差。

將式（23）記作：

式中3H為量測矩陣；3v為量測噪聲向量。

4 聯邦卡爾曼濾波算法

系統采用無反饋重置式聯邦卡爾曼濾波器進行數據處理，如圖2所示，各局部濾波器相互獨立，沒有反饋重置帶來的相互影響，這提供了最高的容錯性。

對應的離散化系統誤差模型如下：

式中為第i個子濾波器的系統狀態向量；為第i個子濾波器的量測向量，上標 i = 1表示SINS/GNSS（衛星導航系統）子濾波器的量測，i=2表示SINS/OD（里程計）子濾波器的量測，i=3表示SINS/Baro-altimeter（高程計）子濾波器的量測；表示時刻k-1至時刻k的一步狀態轉移矩陣；為量測矩陣；Wk?1為系統噪聲向量，為量測噪聲向量；Wk?1和是互不相關的高斯白噪聲序列；的協方差陣為；的協方差陣為。

圖2 無反饋重置式聯邦卡爾曼濾波器Fig.2 Feedback-Free Federal Kalman Filter

i個子濾波器獨立進行時間更新和量測更新，得到狀態估計值和誤差協方差陣。則聯邦濾波算法如下：

a）子濾波器的時間更新。

式中表示第i個子濾波器在時刻k-1至時刻k的一步預測均方誤差；表示第i個子濾波器在時刻k的誤差協方差陣；表示第i個子濾波器在時刻k-1至時刻 k的一步預測狀態估計，子濾波器的誤差狀態維數見第 2 節，式（1）、（10）、（12）。

b）子濾波器的量測更新。

式中表示全局的狀態估計。

c）故障檢測。

利用量測值殘差（即新息）kr：

可以較好地檢測和隔離某些子系統的突變故障。因為?k/k?1包含了k以前的量測值Zi(i ＜ k )的信息，當無故障時， Hk/k?1=Z?k是對 Zk最好的預報估計，所以 rk應很小（理論上為零均值白噪聲）。當kZ發生故障時，kr會發生突變，據此可以檢測和隔離子系統故障。

d）最優信息融合。

式中為主濾波器中的協方差矩陣；為融合了主濾波器和子濾波器后的協方差矩陣；為融合后的狀態估計。在主濾波器中融合了15維公共誤差狀態。

e）信息分配。

式中 β(i)為信息分配系數，根據“信息守恒”原理，滿足：

在無重置式聯邦卡爾曼濾波器中 β(m)=0，

通過時間更新、量測更新、信息融合和信息分配的過程，子濾波器在噪聲分配過程中部分信息丟失，濾波結果是次優的，在將各個信息源組成的子濾波器結果融合過程中非最優性被重新合成，得到全局最優估計。

采用集中式卡爾曼濾波器對 SINS/GNSS/里程計/高程計 4種定位信息源進行融合，構建的濾波模型誤差狀態維數為24維。采用聯邦卡爾曼濾波器對相同濾波模型進行估計需要的誤差狀態維數是21維，相對于集中式卡爾曼濾波器減少了大量矩陣運算的計算量。若增加更多信息源，應用聯邦卡爾曼濾波器進行組合的定位系統誤差狀態維數不再增加。

5 仿真試驗

一組試驗在各信息源持續有效的情況下，采用集中式卡爾曼濾波器和聯邦式卡爾曼濾波器對同一組數據進行解算，對比兩種濾波器的定位精度，驗證采用聯邦卡爾曼濾波器進行組合導航的可行性。

另一組試驗在導航過程中分別關閉和開啟 GNSS和里程計，驗證算法的故障診斷與系統重構功能，提高定位系統在復雜環境下的定位可靠性。

5.1 路徑設計

起始點設置為：經度 E113.19458°，緯度N36.31295°，高度909 m。

車輛在進行240 s的初始對準后，靜止110 s，之后以 2 m/s2加速 10 s，勻速 200 s，以 0.4 m/s2減速 10 s，勻速200 s，再以0.4 m/s2加速10 s，之后勻速600 s，以0.4 m/s2減速10 s，勻速600 s，以2 m/s2加速10 s，勻速600 s，之后以0.4 m/s2減速10 s，勻速600 s。

全程共計用時3110 s，里程約為57 km。

5.2 誤差參數設置

以90型機抖激光陀螺為參考，取陀螺隨機常值零偏為0.002 (°)/h，隨機游走系數為0.0005 (°)/h；加速度計隨機常值零偏為 5 μg，隨機游走系數為0.5μ/g ；里程計刻度系數誤差為0；GPS經緯度誤差為10 m(1)σ，高程誤差20 m，GPS速度誤差為0.1 m/s；里程計桿臂誤差為[0.0149 m，0.1756 m，-1.0232 m]；GPS接收機桿臂誤差為[0.0149 m，0.1756 m，1.0232 m]。

5.3 仿真結果

5.3.1 各信息源持續有效

采用聯邦卡爾曼濾波器和集中式卡爾曼濾波器對上述數據進行仿真，東向、北向和天向定位誤差分別如圖3～5的縱坐標所示，單位為m，橫坐標為時間，去除初始對準的240 s，共計2870 s。

由圖 3～5可知，采用聯邦卡爾曼濾波器的東向誤差范圍為-3～2 m，北向誤差范圍為-2～3 m，天向誤差范圍為-6～4 m；采用集中式卡爾曼濾波器的東向誤差范圍為-3～2 m，北向定位誤差范圍為-2～3 m，天向誤差范圍為-6～8 m。采用兩種濾波器的水平定位誤差走勢一致，天向定位誤差采用聯邦卡爾曼濾波器優于集中式卡爾曼濾波器。所以，在各信息源持續有效時，采用聯邦式卡爾曼濾波器可以達到與集中式卡爾曼濾波器相當的定位精度。

圖3 東向誤差對比Fig.3 Longtitude Error Comparison

圖4 北向誤差對比Fig.4 Atitude Error Comparison

圖5 天向誤差對比Fig.5 Hight Error Comparison

5.3.2 故障仿真

對于SINS/OD子濾波器，設置 rk的閾值為[0.5 m，0.5 m，0.5 m]；對于SINS/GNSS子濾波器，設置 rk的閾值為[ 0 .5× 1 0?6°， 0 .5 × 1 0?6°，0.5 m/s，0.5 m/s，0.5 m/s]，此處高程觀測量不設閾值；對于SINS/高程計子濾波器，設置 rk的閾值為20 m。當 rk的絕對值超出閾值范圍時，對應濾波器不進行數據融合。

5.3.2.1 GNSS故障

假定在 600～1200 s定位過程中 GNSS失效，在1200 s之后GNSS重新有效，對上述數據進行仿真。

GNSS失效期間，定位算法進行了故障診斷，識別出了GNSS失效，關閉了SINS/GNSS子濾波器通道，定位算法主要由SINS/OD（里程計）組合進行導航，如圖 6所示，GNSS失效期間定位誤差有發散趨勢，這是由于 SINS/OD組合誤差隨時間積累這一特性導致。GNSS重新有效后，系統進行了重構，開啟了SINS/GNSS子濾波器通道，恢復了失效前的定位狀態。

圖6 在行駛過程中關閉GPS的誤差曲線Fig.6 Turn Off GPS During Driving Graph

5.3.2.2 里程計故障

假定在 600～1200 s定位過程中里程計失效，在1200 s之后里程計重新有效，對上述數據進行仿真。

里程計失效期間，系統定位誤差無明顯變化，如圖7所示。

圖7 在行駛過程中關閉里程計的誤差曲線Fig.7 Turn Off Odometer During Driving Graph

此時算法已關閉了 SINS/OD通道，主要由SINS/GNSS組合進行導航。表明各信息源持續有效時，算法辨識出定位精度高的SINS/GNSS子系統，在主濾波器信息融合中占據較大權重，在GNSS有效時，里程計的失效對定位精度的影響較小。

結合圖6、圖7可知，在GNSS或里程計失效期間，系統通過故障診斷和系統重構，具備了容錯能力，保證了定位精度，提高了定位算法的可靠性，保證了載體在復雜環境（地下、山洞、電磁環境等）中行駛的定位需求。

6 實車試驗

采用 SINS、GNSS、里程計和高程計構建一套組合定位系統，單機具體性能見表1。

表1 實車試驗設備條件Tab.1 Test Equipment Condition

將該組合定位系統安裝在某型號車上，跑車地點位于山西省，行車路線如圖 8所示，行駛過程中以差分GPS的定位結果為基準，每2 km記錄定位結果，進行精度檢測。

圖8 跑車行駛軌跡Fig.8 Driving Track

行車中采集 SINS、GNSS、里程計、高程計的原始數據，進行離線解算。

一組試驗在 2900～3500 s關閉里程計信號，在3500 s之后開啟。另一組試驗在 2900～3500 s關閉GNSS信號，在3500 s之后開啟。分別驗證在里程計和GNSS失效后，算法的定位精度和容錯性能。

定位誤差統計如表2所示，共24個位置點。第1列為定位點序號，2、3列為分別在2900～3500 s關閉GNSS或里程計的緯度（°）、經度（°）、高度（m）定位結果。第4列為差分GPS的定位結果，作為標準點檢測上述兩種情況的定位精度。5、6列為2、3列分別與第4列比較之后東、北、天3個方向的定位誤差。

表2 定位誤差統計Tab.2 Positioning Error Statistics

續表2

衛星定位間斷故障的定位結果，如圖9所示。其中水平誤差最大值 3.5 m。天向定位誤差最大值14.5 m，水平定位精度為 2.0 m，天向定位精度為12.8 m，定位精度計算見式（32），式中第i個采樣點數北向定位誤差為ix，m，東向定位誤差為iy，m，定位精度為ε。水平定位誤差在真值附近波動，定位精度較高，功能穩定。天向定位精度較低是由于高程計和GPS自身高程精度限制。

圖9 衛星間斷故障定位誤差曲線Fig.9 GPS Intermittent Fault Location Error Graph

里程計定位間斷故障的定位結果，如圖10所示。其中水平誤差最大值為8.1 m，天向定位誤差最大值為14.4 m，水平定位精度為 2.6 m，天向定位精度為13.5 m，定位精度計算方法同上。關閉里程計期信號間水平定位誤差從5 m增加到8 m，由于跑車過程中SINS/里程計組合定位精度較高，在主濾波器融合時占較大權重。在里程計信號恢復后，定位精度恢復到信號關閉之前狀態。

通過以上2組實車試驗，關閉定位子信息源時，搭建的定位算法通過故障診斷和系統重構保證了定位功能的穩定。因此，通過實車試驗驗證了本文搭建的基于聯邦卡爾曼的組合導航算法框架具備了良好的定位精度和容錯性能。

7 結 論

本文基于無重置式聯邦卡爾曼濾波器對4個典型信息源進行融合，并和集中式卡爾曼濾波器進行了比較。采用聯邦卡爾曼濾波算法降低了計算量，提高了算法的實用性，在保證定位精度的同時，提高了車載定位系統的容錯能力；融合算法框架具有良好的開放性，增加其他信息源（如數字磁圖、景象匹配等）時可以進行擴展；保證了車載定位系統在復雜環境下的定位精度，提高了陸基機動武器系統的實戰化水平。

卡爾曼濾波器的模型參數設置對濾波精度有一定影響，可在算法框架增加多模型自適應濾波算法，在不同應用環境中切換濾波參數，進一步提高定位系統的復雜環境適應能力。