容錯聯(lián)邦強跟蹤卡爾曼濾波算法在組合導航中的應用研究＊

馬云峰

（濰坊學院，山東 濰坊 261061）

容錯聯(lián)邦強跟蹤卡爾曼濾波算法在組合導航中的應用研究＊

馬云峰

（濰坊學院，山東 濰坊 261061）

提出了一種改進的聯(lián)邦強跟蹤卡爾曼濾波算法，并將其應用于MSINS／GPS淺組合中。采用了三層容錯結構，能夠有效地檢測出數(shù)據(jù)粗大誤差、GPS速度跳變以及濾波器誤差估計的故障，及時進行故障隔離，從而使系統(tǒng)具有良好的容錯能力。仿真結果表明，聯(lián)邦強跟蹤卡爾曼濾波算法解決了量測相關、GPS速度跳變和初值選取敏感性等問題，可以提高系統(tǒng)的實時性、魯棒性和容錯性能。

捷聯(lián)慣性導航系統(tǒng)；全球定位系統(tǒng)；組合導航系統(tǒng)；卡爾曼濾波；強跟蹤；聯(lián)邦濾波；容錯

1 引言

Kalman濾波技術已經(jīng)廣泛地應用于組合導航系統(tǒng)中，但在MSINS／GPS淺組合中采用經(jīng)典的集中式卡爾曼濾波器還存在著濾波的實時性、濾波穩(wěn)定性、GPS速度跳變等問題。

1988年Carlson提出了用于容錯系統(tǒng)的聯(lián)邦卡爾曼濾波器信息融合算法，由于聯(lián)邦濾波器設計靈活、計算量小、容錯性能好而備受重視［1－2］，這種算法的特點是采用不同的信息分配策略可以獲得不同容錯性能的組合導航系統(tǒng)，這種化整為零的估計方法十分適合于多導航數(shù)據(jù)處理器進行并行計算，從而提高整個系統(tǒng)的實時性?？紤]到GPS輸出的位置和速度測量值是相關的，而且GPS的速度有時會發(fā)生突跳，若采用集中式卡爾曼濾波器極易造成濾波發(fā)散。為解決量測相關并提高系統(tǒng)的容錯性，在MSINS／GPS淺組合應用中設計一種聯(lián)邦容錯強跟蹤卡爾曼濾波器，采用有反饋的融合結構，實現(xiàn)對導航誤差的濾波估計，并能對故障進行隔離和系統(tǒng)重構。

2 聯(lián)邦強跟蹤卡爾曼濾波器的設計

濾波器結構如圖1所示。該方案采用兩個并行的強跟蹤卡爾曼子濾波器，分別對位置和速度進行估計，然后通過一個主濾波器融合各子濾波器的狀態(tài)估計，子濾波器的更新周期和主濾波器的融合周期均為1s；采用了三層故障檢測實現(xiàn)容錯設計，提高組合導航系統(tǒng)的故障隔離和重構的能力。

圖1 聯(lián)邦強跟蹤卡爾曼濾波器的結構

2.1 位置子濾波器

位置子濾波器的狀態(tài)變量包括MSINS導航計算參數(shù)誤差、陀螺儀測量誤差和加速度計測量誤差。狀態(tài)變量X1（t）為

MSINS／GPS淺組合的位置子濾波器狀態(tài)方程與純慣導系統(tǒng)的狀態(tài)方程形式相同［3］，即為

位置子濾波器的觀測方程為

式中，

式中，Pge、Pgn、Pgu為GPS接收機沿東北天方向的位置誤差。

2.2 速度子濾波器

速度子濾波器與位置子濾波器的狀態(tài)變量相同，設為X2（t）

速度子濾波器狀態(tài)方程與純慣導系統(tǒng)的狀態(tài)方程形式也相同，即為

速度子濾波器的量測方程Zv（t）為

式中，

式中，Vge、Vgn、Vgu為GPS接收機沿東北天方向的速度誤差。

2.3 主濾波器及信息分配

設主濾波器的狀態(tài)估計為＾Xf，估計誤差方差陣為Pf；位置、速度子濾波器的狀態(tài)估計分別為＾X1、＾X2，相應的估計誤差方差陣為P1、P2。系統(tǒng)信息P－1f、Q－1f由兩個子濾波器共同分享，主濾波器中不分配信息（β1＋β2＝1，βm＝0），只執(zhí)行各子濾波器的數(shù)據(jù)融合。

（1）假定初始時刻全局狀態(tài)估計為＾Xf0，其協(xié)方差陣為Pf0，系統(tǒng)噪聲的協(xié)方差矩陣為Qf0。根據(jù)信息守恒原理，把這些信息通過信息分配因子β1、β2分配到各子濾波器。

系統(tǒng)噪聲信息按照同樣的信息分配原則分配到各子濾波器。

信息分配策略采用文獻［4］中的方法，根據(jù)局部濾波器的估計精度因子來選定。取β1＋β2＝1，βm＝0，系統(tǒng)信息全部保留在子濾波器中。

式中，

式中，β1i、β2i稱為局部濾波器的估計精度因子；分別代表P1，P2的第i行第i列的元素。

（2）各子濾波器與主濾波器進行時間修正［5］

式中，

（3）各子濾波器進行量測修正

（4）主濾波器進行數(shù)據(jù)融合

把各子濾波器得到的狀態(tài)估計＾X1（k）、＾X2（k）及其協(xié)方差P1（k）、P2（k）進行融合。因Carlson聯(lián)邦濾波采用了信息分配原理，子濾波器的結果之間相互獨立，主濾波器的數(shù)據(jù)融合算法為

2.4 系統(tǒng)的三層容錯設計

實際測試表明，GPS接收機有時會給出錯誤的位置、速度數(shù)據(jù)。因此，如果在GPS數(shù)據(jù)進入卡爾曼濾波器之前，不將錯誤的GPS數(shù)據(jù)剔除，則經(jīng)反饋校正后，便污染了整個系統(tǒng)。為此，系統(tǒng)進行了三層容錯設計，即數(shù)據(jù)粗大誤差的剔除與隔離、子濾波器誤差估計值的故障檢測、主濾波器誤差估計值的故障檢測。

（1）速度數(shù)據(jù)粗大誤差判斷

若根據(jù)式（27）計算出的V1大于10 m／s（假設GPS速度最大跳變?yōu)?m／s），則GPS受到干擾，速度出現(xiàn)突變，上標E、N、U表示東、北、天方向。

（2）子濾波器的故障檢測

對于各子濾波器，前后兩次的誤差估值不應發(fā)生非常大的突變。如果有大的突變，則認為卡爾曼濾波器已受到第一層未檢測出的故障所污染，其濾波結果不送到主濾波器，并重新初始化該子濾波器。采用殘差χ2檢驗法［6］進行子濾波器誤差估計值的故障檢測。

（3）主濾波器的故障檢測

對于主濾波器，同樣前后兩次的誤差估值不應發(fā)生非常大的突變。如果有大的突變，則認為主濾波器已受到前兩層未檢測出的故障所污染，本次融合結果不再反饋到各子濾波器，并且不對MSINS進行反饋校正。同樣采用狀態(tài)χ2檢驗法進行主濾波器誤差估計值的故障檢測。

3 濾波算法在MSINS／GPS淺組合中的應用仿真

3.1 仿真軌跡及仿真條件的設置

設仿真軌跡由平飛、轉(zhuǎn)彎、加速、爬升等狀態(tài)組成，仿真軌跡中的載體先在高度為1000米的高空上由東北方向勻速飛行，接著加速飛行，然后拐彎向正東方向勻速飛行，最后爬升到一定高度勻速飛行。

仿真條件：系統(tǒng)噪聲Qk是由陀螺儀和加速度計敏感誤差的白噪聲部分構成的。給定陀螺儀和加速度計的參數(shù)為：光纖陀螺儀常值漂移10°／h，隨機噪聲均方差為10°／h；石英撓性加速度計常值誤差0.5mg，隨機誤差均方差為0.1mg，GPS的位置誤差為均方差15m的白噪聲，速度誤差為均方差0.1m／s的白噪聲，兩個濾波器的狀態(tài)初始值均為0。當300秒至400秒期間，GPS東向速度發(fā)生±4m／s的跳變。

3.2 仿真結果

有東向速度故障情況下的集中卡爾曼濾波估計誤差曲線和聯(lián)邦強跟蹤卡爾曼濾波器的濾波誤差曲線如圖2和圖3所示。

圖2 有速度跳變故障時集中式卡爾曼濾波誤差曲線

4 結論

（1）在正常情況下，聯(lián)邦強跟蹤卡爾曼濾波器的濾波精度與集中式卡爾曼濾波器的濾波精度相當。

（2）集中式卡爾曼濾波器對初值的選取較敏感，若初值的選取與實際相差較大，將導致濾波器的發(fā)散，而聯(lián)邦強跟蹤卡爾曼濾波器對初值選取敏感度較低。

（3）當GPS速度發(fā)生跳變故障后，集中式濾波器立即發(fā)散。這說明，在有故障發(fā)生時，集中式卡爾曼濾波器易受污染。

圖3 有速度跳變故障時聯(lián)邦強跟蹤卡爾曼濾波誤差曲線

（4）聯(lián)邦強跟蹤卡爾曼濾波器在速度發(fā)生跳變故障時，能夠根據(jù)檢驗結果迅速將速度子濾波器隔離，主濾波器拒絕接納該子濾波器的信息，僅接納并輸出位置子濾波器的信息。從圖3中看出，在300s－400s之間聯(lián)邦濾波器濾波精度降為單獨位置子濾波器估計的效果；當子濾波器速度跳變消失后，又可及時重新加入信息融合，聯(lián)邦濾波器估計濾波精度恢復至故障前。

（5）當系統(tǒng)噪聲統(tǒng)計特性未知、初始值選擇不當或GPS速度產(chǎn)生跳變時，與集中式Kalman濾波器相比，聯(lián)邦強跟蹤卡爾曼濾波器具有很強的魯棒性和容錯性，而且具有較高的精度。

［1］Carlson N A.Federated square root filter for decentralized parallel processes［J］.IEEE Trans：Aerospace and Electronic Systems，1990，26（3）：517－525.

［2］Carlson N A.Federated kalman filter simulation results［J］.Navigation，1994，41（3）：297－321.

［3］袁信，俞濟祥，陳哲.導航系統(tǒng)［M］.北京：北京航空工業(yè)出版社，1992.

［4］周哲.多傳感器信息融合及其在組合導航系統(tǒng)中的應用［D］.哈爾濱：哈爾濱工業(yè)大學，2000.

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

［6］陶俊勇.組合導航系統(tǒng)信息融合理論及故障檢測與診斷技術研究［D］.長沙：國防科技大學，2000.

（責任編輯：肖恩忠）

Application of Fault Tolerant Federated Strong Tracking Kalman Algorithm in Integrated Navigation System

MA Yun－feng
（Weifang University，Weifang 261061，China）

An improved federated strong tracking kalman filter has been proposed creatively and has been used in loose coupling MSINS／GPS integrated navigation system.Adopting three－layer fault－tolerant structure，it can effectively check the errors of data outlier，GPS's velocity jump and the malfunction of the filter state estimate.When the system fault has been diagnosed，it can be insulated in time，which can make this system have better ability of toleranting the faults.The simulation results show that this approach can resolve the problem of the correlation of GPS's position and velocity measurement data，GPS's velocity jump and the system's sensitivity to initial values.Thus，the real time character，robust and the ability of tolerating faults of the system has been improved.

strapdown inertial navigation system（SINS），global position system（GPS），integrated navigation system，kalman filter，strong tracking，federated filter，fault tolerant.

2011－05－18

山東省自然科學基金項目（Y2007G64）

馬云峰（1967－），男，山東臨朐人，濰坊學院信息與控制工程學院教授，工學博士。研究方向：組合導航技術。

V249.32 文獻標識碼：A 文章編號：1671－4288（2011）06－0110－06