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

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

0 引 言

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

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

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

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

1 開放式系統(tǒng)架構(gòu)

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

圖1 多源信息融合定位系統(tǒng)算法結(jié)構(gòu)Fig.1 Multi-source Information Fusion System Algorithm Architecture

2 系統(tǒng)誤差模型

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

2.1 SINS/OD子濾波器

誤差狀態(tài)量選取為

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

系統(tǒng)誤差方程為如下：

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

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

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

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

式中 F1( t)為狀態(tài)轉(zhuǎn)移矩陣， w1為系統(tǒng)噪聲向量。

2.2 SINS/GNSS子濾波器

誤差狀態(tài)量選取為

式中為GNSS桿臂誤差，共18維狀態(tài)變量。

系統(tǒng)誤差方程同式（2）～（6）、式（8），記作：

2.3 SINS/高程計子濾波器

誤差狀態(tài)量選取為

共15維狀態(tài)變量。

系統(tǒng)誤差方程同式（2）～（6），記作：

3 觀測方程

3.1 SINS/OD子濾波器

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

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

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

3.2 SINS/GNSS子濾波器

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

3.2.1 位置觀測量

式中 pI表示SINS的位置； pGNSS表示衛(wèi)星在導航坐標系中的位置；表示衛(wèi)星外桿臂產(chǎn)生的位置偏差，其中：

整理得到：

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

3.2.2 速度觀測量

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

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

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

3.3 SINS/高程計子濾波器

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

將式（23）記作：

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

4 聯(lián)邦卡爾曼濾波算法

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

對應的離散化系統(tǒng)誤差模型如下：

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

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

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

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

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

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

式中表示全局的狀態(tài)估計。

c）故障檢測。

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

可以較好地檢測和隔離某些子系統(tǒng)的突變故障。因為?k/k?1包含了k以前的量測值Zi(i ＜ k )的信息，當無故障時， Hk/k?1=Z?k是對 Zk最好的預報估計，所以 rk應很?。ɡ碚撋蠟榱憔蛋自肼暎?。當kZ發(fā)生故障時，kr會發(fā)生突變，據(jù)此可以檢測和隔離子系統(tǒng)故障。

d）最優(yōu)信息融合。

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

e）信息分配。

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

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

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

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

5 仿真試驗

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

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

5.1 路徑設計

起始點設置為：經(jīng)度 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 誤差參數(shù)設置

以90型機抖激光陀螺為參考，取陀螺隨機常值零偏為0.002 (°)/h，隨機游走系數(shù)為0.0005 (°)/h；加速度計隨機常值零偏為 5 μg，隨機游走系數(shù)為0.5μ/g ；里程計刻度系數(shù)誤差為0；GPS經(jīng)緯度誤差為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 仿真結(jié)果

5.3.1 各信息源持續(xù)有效

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

由圖 3～5可知，采用聯(lián)邦卡爾曼濾波器的東向誤差范圍為-3～2 m，北向誤差范圍為-2～3 m，天向誤差范圍為-6～4 m；采用集中式卡爾曼濾波器的東向誤差范圍為-3～2 m，北向定位誤差范圍為-2～3 m，天向誤差范圍為-6～8 m。采用兩種濾波器的水平定位誤差走勢一致，天向定位誤差采用聯(lián)邦卡爾曼濾波器優(yōu)于集中式卡爾曼濾波器。所以，在各信息源持續(xù)有效時，采用聯(lián)邦式卡爾曼濾波器可以達到與集中式卡爾曼濾波器相當?shù)亩ㄎ痪取?/p>

圖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的絕對值超出閾值范圍時，對應濾波器不進行數(shù)據(jù)融合。

5.3.2.1 GNSS故障

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

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

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

5.3.2.2 里程計故障

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

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

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

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

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

6 實車試驗

采用 SINS、GNSS、里程計和高程計構(gòu)建一套組合定位系統(tǒng)，單機具體性能見表1。

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

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

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

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

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

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

表2 定位誤差統(tǒng)計Tab.2 Positioning Error Statistics

續(xù)表2

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

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

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

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

7 結(jié) 論

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

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