基于容錯(cuò)型聯(lián)邦強(qiáng)跟蹤濾波的多星座組合導(dǎo)航算法研究＊

張 輝，吳 鵬，張國(guó)柱，雍少為

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

0 引 言

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

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

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

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

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

1 容錯(cuò)型聯(lián)邦強(qiáng)跟蹤濾波器結(jié)構(gòu)設(shè)計(jì)

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

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

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

1）容錯(cuò)性能強(qiáng):各子濾波器獨(dú)立工作，由于沒(méi)有主濾波器到子濾波器的信息重置，避免了一個(gè)傳感器故障造成各子濾波器交叉感染，容錯(cuò)性最好。

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

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

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

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

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

2 FTFSTF算法描述

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

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

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

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

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

式中

同理可得Φy，Φz(mì)．

Wk的協(xié)方差為

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

取觀測(cè)量矩陣為

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

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

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

式中

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

2．2 子濾波器算法

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

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

SMFEKF的具體算法為

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

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

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

則

式中:

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

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

2．3 故障檢測(cè)與隔離

文獻(xiàn)［13］給出一種適合組合導(dǎo)航系統(tǒng)故障檢測(cè)和隔離的方法——?dú)埐瞀?檢驗(yàn)法，計(jì)算量小，實(shí)時(shí)性強(qiáng)，設(shè)計(jì)靈活。基本方法如下

故障判斷準(zhǔn)則為

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

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

2．4 主濾波器信息融合

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

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

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

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

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

圖2 飛行軌跡仿真

實(shí)驗(yàn)一:FTFSTF跟蹤性能仿真分析

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

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

實(shí)驗(yàn)二:FTFSTF濾波精度仿真分析

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

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

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

實(shí)驗(yàn)三:FTFSTF容錯(cuò)性能仿真分析

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

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

4 結(jié) 論

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

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

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

［2］ 張寅生，程洪瑋，姚 靜，等．組合導(dǎo)航衛(wèi)星系統(tǒng)定位技術(shù)進(jìn)展［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)星導(dǎo)航定位系統(tǒng)共用關(guān)鍵技術(shù)研究［D］．哈爾濱:哈爾濱工業(yè)大學(xué)，2004．

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

［6］ 周東華，葉銀忠．現(xiàn)代故障診斷與容錯(cuò)控制［M］．北京:清華大學(xué)出版社，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］ 吳 玲，孫永榮，陳傳德．多星座組合導(dǎo)航自適應(yīng)信息融合濾波算法［J］．航天控制，2010，28（6）:38－42．

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

［10］ 周東華，席裕庚，張鐘?。环N帶多重次優(yōu)漸消因子的擴(kuò)展卡爾曼濾波器［J］．自動(dòng)化學(xué)報(bào)，1991，17（6）:689－695．

［11］ 韓崇昭，朱洪艷，段戰(zhàn)勝，等．多源信息融合［M］．2版．北京:清華大學(xué)出版社，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］ 秦永元，張洪鉞，汪叔華．卡爾曼濾波與組合導(dǎo)航原理［M］．西安:西北工業(yè)大學(xué)出版社，1998．