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

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

0 引言

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

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

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

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

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

1 聯(lián)邦濾波組合導航系統(tǒng)模型

1.1 聯(lián)邦濾波器設(shè)計方案

本文所設(shè)計的聯(lián)邦濾波器中，以SINS作為公共的信息參考系統(tǒng)，使用了5個信息源，所以設(shè)置了4個子濾波器和1個主濾波器。每個子濾波器處理自己的量測信息得到局部估計，再在主濾波器中進行最優(yōu)估計。此外，本文選用有重置的聯(lián)邦濾波器，且采用了一種自適應(yīng)聯(lián)邦信息分配因子，在出現(xiàn)某一子信息源出現(xiàn)故障時，能夠?qū)收虾芎玫剡M行隔離，不影響濾波器的正常濾波，使系統(tǒng)保持較好的穩(wěn)定性和魯棒性。圖1為聯(lián)邦濾波器結(jié)構(gòu)圖。

1.2 聯(lián)邦濾波器狀態(tài)方程

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

圖1 聯(lián)邦濾波器結(jié)構(gòu)圖Fig.1 Structure diagram of federal filter

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

式（2）中，F(xiàn)SINS為SINS系統(tǒng)的狀態(tài)轉(zhuǎn)移矩陣，GSINS為SINS系統(tǒng)的噪聲驅(qū)動矩陣，WSINS為陀螺和加速度計三個軸上均值為零、方差為QSINS的白噪聲，各矩陣的具體計算形式詳見文獻［10］。

1.3 聯(lián)邦濾波器量測方程

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

（1）SINS/BDS的量測方程

SINS位置信息表示為

SINS的速度信息表示為

BDS的位置信息表示為

BDS的速度信息表示為

則SINS/BDS的量測方程為

式（3）～式（6）中，λRef、LRef、hRef為經(jīng)度、緯度和高度的基準參考值，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系統(tǒng)的速度作差，則得觀測方程為

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

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

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

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

2 聯(lián)邦濾波器設(shè)計

2.1 聯(lián)邦濾波器融合方法

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

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

式（23）中，βm為主濾波器信息分配系數(shù)，βi為第i個子濾波器對應(yīng)的信息分配系數(shù)。

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

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

式（25）中，為主濾波器的狀態(tài)估計，Pf、Qf分別為主濾波器的狀態(tài)誤差協(xié)方差陣和系統(tǒng)噪聲協(xié)方差陣，為子濾波器的狀態(tài)估計，Pi、Qi分別為子濾波器狀態(tài)估計對應(yīng)的狀態(tài)誤差協(xié)方差陣和子濾波器系統(tǒng)噪聲協(xié)方差陣。

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

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

子濾波器的預(yù)測和濾波估計方程為

子濾波器的濾波增益為

子濾波器的估計均方差為

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

式（32）中，為系統(tǒng)誤差狀態(tài)量的全局最優(yōu)估計結(jié)果，Pf為估計狀態(tài)量的協(xié)方差陣。

2.2 時空基準統(tǒng)一方法

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

空間基準統(tǒng)一就是將不同坐標系下的測量信息經(jīng)過旋轉(zhuǎn)變換統(tǒng)一到基準坐標系下，基準坐標系為導航坐標系。

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

2.3 聯(lián)邦濾波器自適應(yīng)信息分配方法

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

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

在每次濾波前進行計算，令分配系數(shù)βi＝σ0。并且可以利用自適應(yīng)濾波的原理對聯(lián)邦濾波的分配系數(shù)進行確定［8］，同時結(jié)合自適應(yīng)濾波的權(quán)函數(shù)和殘差χ2故障檢測理論對自適應(yīng)分配因子進行調(diào)整，故可得

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

3 實驗與分析

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

3.1 多源融合仿真實驗

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

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

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

圖3 經(jīng)度誤差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

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

3.2 車載多源融合實驗

車載實驗在空曠的郊區(qū)進行，實驗中采用本文前面所述的傳感信息源進行融合，來驗證本文算法框架的可行性和合理性。實驗中以某高精度組合導航產(chǎn)品為基準，具體實驗結(jié)果如圖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 經(jīng)度對比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左右，且逐漸變小趨于平穩(wěn)；水平速度誤差在0.2m/s左右，趨于平穩(wěn)；天向速度誤差在0.15m/s以內(nèi)，趨于平穩(wěn)。整個過程的位置和速度曲線與基準相比，均能實現(xiàn)平穩(wěn)、良好跟蹤，表明車載實驗下算法也能夠可靠的工作。

3.3 實驗結(jié)果與分析

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

表1 實驗結(jié)果對比Table 1 Comparison of experiment results

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

4 結(jié)論

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