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

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

0 引 言

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

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

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

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

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

1 容錯型聯(lián)邦強跟蹤濾波器結(jié)構(gòu)設計

聯(lián)邦濾波器主要分為四種基本結(jié)構(gòu):融合－重置式（FR）、零重置式（ZR）、無重置式（NR）和重調(diào)式（RS）等。它們的性能對比如表1所示。

表1 聯(lián)邦濾波算法四種基本結(jié)構(gòu)性能比較

NR結(jié)構(gòu)聯(lián)邦濾波較其他結(jié)構(gòu)的聯(lián)邦濾波有如下優(yōu)勢［11］:

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

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

NR結(jié)構(gòu)唯一不足是精度較FR結(jié)構(gòu)稍有下降（但仍比任何一個子系統(tǒng)的精度高）。由于NR的特點，這種方案被認為是容錯型聯(lián)邦濾波結(jié)構(gòu)（FTFF）．

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

圖1 容錯型聯(lián)邦強跟蹤濾波器結(jié)構(gòu)

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

2 FTFSTF算法描述

2．1 系統(tǒng)模型建立

2．1．1 系統(tǒng)狀態(tài)方程

由于各衛(wèi)星導航系統(tǒng)的導航定位原理相同，因此，可以采用相同的系統(tǒng)模型。

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

離散化后的系統(tǒng)狀態(tài)方程為

式中

同理可得Φy，Φz．

Wk的協(xié)方差為

2．1．2 系統(tǒng)觀測方程

取觀測量矩陣為

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

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

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

式中

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

2．2 子濾波器算法

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

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

SMFEKF的具體算法為

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

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

若由系統(tǒng)先驗信息可大致確定

則

式中:

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

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

2．3 故障檢測與隔離

文獻［13］給出一種適合組合導航系統(tǒng)故障檢測和隔離的方法——殘差χ2檢驗法，計算量小，實時性強，設計靈活?；痉椒ㄈ缦?/p>

故障判斷準則為

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

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

2．4 主濾波器信息融合

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

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

3 算法仿真及結(jié)果分析

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

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

圖2 飛行軌跡仿真

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

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

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

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

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

表2 常規(guī)聯(lián)邦濾波器和FTFSTF濾波精度統(tǒng)計

結(jié)果表明:FTFSTF采用的NR聯(lián)邦濾波結(jié)構(gòu)較之FR聯(lián)邦濾波結(jié)構(gòu)濾波精度下降很小。

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

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

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

4 結(jié) 論

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

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

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

［2］ 張寅生，程洪瑋，姚 靜，等．組合導航衛(wèi)星系統(tǒng)定位技術(shù)進展［J］．全球定位系統(tǒng)，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］ 初海彬．多種衛(wèi)星導航定位系統(tǒng)共用關鍵技術(shù)研究［D］．哈爾濱:哈爾濱工業(yè)大學，2004．

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

［6］ 周東華，葉銀忠．現(xiàn)代故障診斷與容錯控制［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］ 劉建書，李厚人，劉云龍．基于“當前”統(tǒng)計模型的交互式多模型算法［J］．系統(tǒng)工程與電子技術(shù)，2008，30（7）:1351－1354．

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

［11］ 韓崇昭，朱洪艷，段戰(zhàn)勝，等．多源信息融合［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］．西安:西北工業(yè)大學出版社，1998．