基于容錯型聯邦強跟蹤濾波的多星座組合導航算法研究＊

張 輝，吳 鵬，張國柱，雍少為

（1．國防科學技術大學電子科學與工程學院衛星導航研發中心，湖南 長沙410073；2．61081部隊，北京100094）

0 引 言

目前，已有北斗系統（COMPASS）、GPS和GLONASS三個全球導航系統能夠提供導航定位服務。但是，即便是目前最成熟的衛星導航系統GPS也存在一些缺陷。例如，在航空領域，飛行器經常工作在高動態環境下，而且對導航定位系統的精度和可靠性要求也較高，GPS不能滿足作為單一導航系統的要求［1］。多星座組合導航定位具有可用衛星數量多、覆蓋性好、定位精度高、可靠性高等優點，正逐步成為衛星導航領域的研究熱點之一。

多星座組合導航系統信息處理多采用的方法有集中式融合和分布式融合。由于集中式融合方案存在實時計算困難、容錯性能較差的問題，因此，大多采用分布式融合［2］，其中最著名的就是由Carlson提出的聯邦濾波算法［3］。

文獻［4］將GPS、GLONASS和GALILEO三系統視為三個聯邦成員，構成針對GNSS系統的聯邦濾波器，仿真結果表明:定位精度在一定程度上得到提高。但是，常規聯邦濾波器中的子濾波器為擴展卡爾曼濾波器（EKF），它是建立在H2估計準則基礎上的，要求準確已知系統模型和噪聲統計特性［5］，而且在系統達到穩定狀態下，將喪失對突變系統狀態的跟蹤能力［6－7］，這在實際應用中帶來很大局限性。針對這一問題，文獻［8］提出了一種基于卡爾曼濾波的多星座組合導航雙重自適應聯合濾波算法，具有較好的機動目標跟蹤能力，提高了多星座組合導航系統的定位精度和可靠性。但由于“當前”統計模型中的系統加速度參數amax和a－max在跟蹤過程中不能自適應，使得系統方差的調整有限，對于機動加速度大范圍變動或突變的目標，其狀態跟蹤的快速性和精度的協調很難令人滿意［9］。另外，由于采用了重置－融合聯邦濾波結構，容錯性能較差。

針對EKF的缺陷，周東華等在文獻［10］中提出了著名的強跟蹤濾波器（STF）。STF與通常的濾波器相比有以下優良特性:（1）較強的關于模型參數失配的魯棒性；（2）較低的關于噪聲及初值統計特性的敏感性；（3）極強的關于突變狀態的跟蹤能力，并在濾波器達到穩態時仍保持這種能力；（4）適中的計算復雜性。

主要研究載體在高動態環境下的多星座組合導航系統信息處理算法。此時，系統狀態變化劇烈，動態噪聲和觀測噪聲難以確定，同時，對導航定位的精度和可靠性要求較高。針對這一問題，在采用容錯性能好、運算速度快的容錯型聯邦濾波器的基礎上，改進子濾波器為強跟蹤濾波器，增強了算法對模型誤差的魯棒性和對系統狀態的跟蹤能力。仿真結果證明:本算法優于傳統的聯邦濾波算法。

1 容錯型聯邦強跟蹤濾波器結構設計

聯邦濾波器主要分為四種基本結構:融合－重置式（FR）、零重置式（ZR）、無重置式（NR）和重調式（RS）等。它們的性能對比如表1所示。

表1 聯邦濾波算法四種基本結構性能比較

NR結構聯邦濾波較其他結構的聯邦濾波有如下優勢［11］:

1）容錯性能強:各子濾波器獨立工作，由于沒有主濾波器到子濾波器的信息重置，避免了一個傳感器故障造成各子濾波器交叉感染，容錯性最好。

2）運算速度快:由于不需要重置，可以在子濾波器向主濾波器發送完狀態估計值和方差后立即進行下一次運算，運算速度最快。

NR結構唯一不足是精度較FR結構稍有下降（但仍比任何一個子系統的精度高）。由于NR的特點，這種方案被認為是容錯型聯邦濾波結構（FTFF）．

為同時發揮多星座組合導航系統精度和可靠性優勢，加上導航系統的實時性運算要求，選用容錯型聯邦濾波器。如前所述，容錯型聯邦濾波器的子濾波器是擴展卡爾曼濾波器，存在著對模型誤差的魯棒性較差、對系統狀態的跟蹤性能不強的問題，不能滿足高動態載體導航定位的要求。因此，采用強跟蹤濾波器替換原來的子濾波器，構成容錯型聯邦強跟蹤濾波器（FTFSTF），達到增強算法的魯棒性和跟蹤能力的目的。算法具體結構如圖1所示。

圖1 容錯型聯邦強跟蹤濾波器結構

其中，全局算法采用容錯型聯邦濾波結構；各衛星導航系統對應的子濾波器設計為強跟蹤濾波器，根據狀態方程和量測方程進行濾波，輸出建立在單一導航系統量測基礎上的系統狀態估計結果；各子系統配置實時的故障檢測和隔離模塊（FDI），檢測正常的狀態估計結果被送往主濾波器；主濾波器對接收到的各子濾波器估計結果進行最優融合，得到組合系統對系統狀態的最終估計＾Xg，Pg，且無對子濾波器的狀態重置。

2 FTFSTF算法描述

2．1 系統模型建立

2．1．1 系統狀態方程

由于各衛星導航系統的導航定位原理相同，因此，可以采用相同的系統模型。

式中:τx，τy，τz分別為加速度時間相關常數；τξ為鐘漂的時間相關常數；sx（t），sy（t），sz（t），sξ（t）分別為（0，σ2x），（0，σ2y），（0，σ2z），（0，σ2ξ）的高斯白噪聲。

離散化后的系統狀態方程為

式中

同理可得Φy，Φz．

Wk的協方差為

2．1．2 系統觀測方程

取觀測量矩陣為

式中:ρi、f′di分別為偽距觀測量和線性化的多普勒觀測量；m為觀測到的衛星數。計算公式為

式中:Vsi表示衛星i的當前速度；Vu表示用戶當前速度；Ai＝［hxihyihzi］為用戶到衛星i的方向余弦矢量。

將上面兩式合并在一起并離散化，則系統觀測方程為

式中

式中:σ2iρ為衛星i的偽距觀測方差；σ2if為衛星i的多普勒觀測方差。需注意的是:由于COMPASS導航星座是由GEO、IGSO和MEO三種軌道類型的衛星組成，各種衛星的觀測噪聲是不同的。

2．2 子濾波器算法

子濾波器為強跟蹤濾波器。強跟蹤濾波器，通過在線選擇增益Kk使得狀態殘差估計最小，且輸出的殘差序列保持處處正交，從而強迫濾波器保持對實際系統狀態的跟蹤。

考慮由式（2）與式（5）構成的非線性系統，在擴展卡爾曼濾波器的估計誤差協方差陣中引入次優漸消矩陣，對不同數據通道進行漸消，使不同時刻的殘差序列處處正交，則構成一種強跟蹤濾波器—帶多重次優漸消因子的擴展卡爾曼濾波器（SMFEKF）［10］。

SMFEKF的具體算法為

式中LMDk＝diag｛λ1，k，λ2，k，…，λn，k｝，λi，k≥1，i＝1，2…n為對應狀態變量的多重次優漸消矩陣。

考慮到導航系統的實時性運算要求，漸消矩陣采用適合在線運算的一步次優算法［10］。

若由系統先驗信息可大致確定

則

式中:

式中:0．95≤ρ≤0．995為遺忘因子；β為弱化因子，它的引入可以使狀態估計更加平滑。β的取值可憑經驗選定，也可通過仿真由下面準則確定［12］

可以看出，當系統狀態發生突變時，估計誤差γkγTk的增大將引起誤差方差陣Vok增大，相應的時變漸消因子λi，k增大，濾波器的跟蹤能力增強。當LMDk為單位矩陣時，SMFEKF算法便退化為EKF算法。

2．3 故障檢測與隔離

文獻［13］給出一種適合組合導航系統故障檢測和隔離的方法——殘差χ2檢驗法，計算量小，實時性強，設計靈活。基本方法如下

故障判斷準則為

式中:θik為故障檢測函數；γik為殘差；Aik為殘差方差；TD為預設故障檢驗門限。

當主濾波器檢測到子濾波器i發生故障后，可以很快的拒絕接納它的信息，而融合剩余子濾波器的信息，得到系統的最優估計。

2．4 主濾波器信息融合

主濾波器按照下式將n個子濾波器的估計結果進行融合。

式中:Xik、Pik分別表示第i個子濾波器的估計值和方差；Xgk、Pgk分別表示全局最優估計和方差。

3 算法仿真及結果分析

仿真中，飛機的初始位置為北緯28．05°，東經112．78°，高度300m，初始速度100m／s，航向正東。仿真進行了500s．其中，前200s為水平勻速飛行，201s～380s經歷了加速、快速盤旋上升、減速等快速強機動的飛行狀態，最后進入水平勻速飛行，飛行軌跡如圖2所示。濾波器濾波周期T＝1 s．相關模型參數選取為:COMPASS系統中τx＝τy＝τz＝60；σ2x＝σ2y＝σ2z＝（4．0）2，τξ＝0．1，σ2ξ＝（15．0）2:GPS系統中τx＝τy＝τz＝100，σ2x＝σ2y＝σ2z＝（3．0）2，τξ＝0．1，σ2ξ＝（10．0）2；GLONASS系統中，τx＝τy＝τz＝30，σ2x＝σ2y＝σ2z＝（5．0）2，τξ＝0．1，σ2ξ＝（20．0）2．觀測衛星高度截止角為10°，偽距觀測誤差和偽距等效誤差分別選取為COMPASS:IGSO和MEO衛星取15m、15m，GEO衛星取20 m、20m；GPS:10m，10m；GLONASS:30m，30 m．多普勒觀測誤差和多普勒等效誤差分別選取為 COMPASS:0．15m／s，0．15m／s；GPS:0．1m／s，0．1m／s；GLONASS:0．3m／s，0．3m／s．STF中:ρ＝0．95，β＝1．9，由于衛星導航系統中位置和速度是可直接觀測量，其他誤差無法直接觀測，而速度狀態相對于位置狀態更容易發生“突變”，相應的賦予更大的漸消因子比例系數會更有助于對易變狀態的跟蹤，所以取

為了驗證算法的有效性，設計了3組實驗，分別對比分析了FTFSTF算法與常規聯邦濾波算法在機動跟蹤性能、濾波精度和容錯性能等方面的優劣。常規聯邦濾波器中，子濾波器為EKF，整體采用FR聯邦濾波結構，標記為EKF＋FR．FTFSTF子濾波器采用STF，整體采用NR聯邦濾波結構，標記為STF＋NR．

圖2 飛行軌跡仿真

實驗一:FTFSTF跟蹤性能仿真分析

賦予位置誤差為100m的狀態初值，分別采用FTFSTF與常規聯邦濾波算法對載體進行定位測速解算，仿真結果如圖3、圖4所示。

仿真結果表明:當賦予不準確的狀態初值時，系統收斂到穩定狀態（定位誤差7m），常規聯邦濾波器用了約98s，FTFSTF只用了8s，FTFSTF較常規聯邦濾波器收斂速度有較大幅度的提高；當載體在處于一般機動狀態時，FTFSTF與常規聯邦濾波器性能相當；當載體進行快速機動時，FTFSTF體現出較強的跟蹤性能，但濾波精度有所下降，而常規聯邦濾波器則出現較大的估計誤差。這驗證了FTFSTF采用的強跟蹤濾波算法具有較強的自適應性:對于導航系統而言，當載體處于常規機動狀態或者穩定狀態時，STF漸消矩陣取單位陣，退化為EKF；當載體處于強機動狀態時，STF通過實時調整漸消矩陣使狀態殘差序列處處正交來保持對系統狀態的強跟蹤性能。

實驗二:FTFSTF濾波精度仿真分析

主要分析FTFSTF采用的NR聯邦濾波結構與FR聯邦濾波結構在濾波精度上的差別。由于當載體處于常規機動狀態或者穩定狀態時，STF退化為EKF，所以，研究飛機前200s的勻速飛行階段，進行FTFSTF與常規聯邦濾波器的性能對比分析。同時，賦予位置誤差為20m的狀態初值。仿真結果如圖5、圖6和表2所示。其中，濾波精度用均方根誤差（RMSE）來衡量。

表2 常規聯邦濾波器和FTFSTF濾波精度統計

結果表明:FTFSTF采用的NR聯邦濾波結構較之FR聯邦濾波結構濾波精度下降很小。

實驗三:FTFSTF容錯性能仿真分析

主要分析FTFSTF采用的NR結構相對FR結構在容錯性能上的區別。假設GLONASS系統的2號星在80s～150s發生緩變故障，偽距中加入速率為0．5m／s的緩變誤差，且FDI模塊沒有檢測到這一故障，其他仿真條件同實驗二，仿真結果如圖7和圖8所示。

仿真結果表明:GLONASS系統發生緩變故障時，由于常規聯邦濾波器采用FR結構，導致各子濾波器發生交叉感染，整體濾波精度下降；而FTFSTF采用NR結構，不存在主濾波器到子濾波器的信息反饋，各子濾波器獨立工作，彼此之間不存在耦合，保證了整體濾波精度，容錯性能更好。

4 結 論

針對多星座組合導航在高動態場合應用的特點，提出了基于容錯型聯邦強跟蹤濾波的多星座組合導航算法。通過對COMPASS／GPS／GLONASS組合導航系統建模仿真，驗證了該算法的有效性。結果表明:該算法具有較強的容錯性能和機動目標跟蹤性能，能夠有效提高多星座組合導航定位的精度和可靠性。由于采用了無重置聯邦濾波結構和漸消矩陣的一步次優算法、殘差χ2檢驗算法等實用算法，該算法具有計算量適中、容錯性強、易于實現等優點，具有一定的工程實用價值。

但研究過程中發現，當載體做快速機動時，該算法的濾波精度有所下降。如何在保持強跟蹤性能的同時，提高濾波精度需要更進一步研究。

［1］ HEWITSON S ，WANG Jinling．GNSS receiver autonomous integrity monitoring（RAIM）performance analysis［J］．GPS Solutions，2006，10（3）:155－170．

［2］ 張寅生，程洪瑋，姚 靜，等．組合導航衛星系統定位技術進展［J］．全球定位系統，2009，34（5）:1－6．

［3］ CARLSON N A．Federated square filtering for decentralized parallel processes［J］．IEEE Trans．Aerospace and Electronic Systems，1990，26（3）:517－527．

［4］ 初海彬．多種衛星導航定位系統共用關鍵技術研究［D］．哈爾濱:哈爾濱工業大學，2004．

［5］ 付夢印，鄧志紅，張繼偉．Kalman濾波理論及其在導航系統中的應用［M］．北京:科學出版社，2003．

［6］ 周東華，葉銀忠．現代故障診斷與容錯控制［M］．北京:清華大學出版社，2000．

［7］ TERZIC B，JADRIC M．Design and implementation of the extend Kalman filter for the speed and motor position estimation of BLDC motor［J］．IEEE Transactions on Industrial Electronics，2001，48（6）:1065－1073．

［8］ 吳 玲，孫永榮，陳傳德．多星座組合導航自適應信息融合濾波算法［J］．航天控制，2010，28（6）:38－42．

［9］ 劉建書，李厚人，劉云龍．基于“當前”統計模型的交互式多模型算法［J］．系統工程與電子技術，2008，30（7）:1351－1354．

［10］ 周東華，席裕庚，張鐘俊．一種帶多重次優漸消因子的擴展卡爾曼濾波器［J］．自動化學報，1991，17（6）:689－695．

［11］ 韓崇昭，朱洪艷，段戰勝，等．多源信息融合［M］．2版．北京:清華大學出版社，2010．

［12］ BAI Mingli，ZHOU Donghua，SCHWARZ H．Identification of generalized friction for an experimental planar two－link flexible manipulator using strong tracking filter［J］．IEEE Trans．on Robotics and Automation，1999，15（2）:362－369．

［13］ 秦永元，張洪鉞，汪叔華．卡爾曼濾波與組合導航原理［M］．西安:西北工業大學出版社，1998．