容錯(cuò)聯(lián)邦強(qiáng)跟蹤卡爾曼濾波算法在組合導(dǎo)航中的應(yīng)用研究＊

馬云峰

（濰坊學(xué)院，山東 濰坊 261061）

容錯(cuò)聯(lián)邦強(qiáng)跟蹤卡爾曼濾波算法在組合導(dǎo)航中的應(yīng)用研究＊

馬云峰

（濰坊學(xué)院，山東 濰坊 261061）

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

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

1 引言

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

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

2 聯(lián)邦強(qiáng)跟蹤卡爾曼濾波器的設(shè)計(jì)

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

圖1 聯(lián)邦強(qiáng)跟蹤卡爾曼濾波器的結(jié)構(gòu)

2.1 位置子濾波器

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

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

位置子濾波器的觀(guān)測(cè)方程為

式中，

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

2.2 速度子濾波器

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

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

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

式中，

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

2.3 主濾波器及信息分配

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

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

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

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

式中，

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

（2）各子濾波器與主濾波器進(jìn)行時(shí)間修正［5］

式中，

（3）各子濾波器進(jìn)行量測(cè)修正

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

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

2.4 系統(tǒng)的三層容錯(cuò)設(shè)計(jì)

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

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

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

（2）子濾波器的故障檢測(cè)

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

（3）主濾波器的故障檢測(cè)

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

3 濾波算法在MSINS／GPS淺組合中的應(yīng)用仿真

3.1 仿真軌跡及仿真條件的設(shè)置

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

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

3.2 仿真結(jié)果

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

圖2 有速度跳變故障時(shí)集中式卡爾曼濾波誤差曲線(xiàn)

4 結(jié)論

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

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

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

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

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

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

［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］袁信，俞濟(jì)祥，陳哲.導(dǎo)航系統(tǒng)［M］.北京：北京航空工業(yè)出版社，1992.

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

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

［6］陶俊勇.組合導(dǎo)航系統(tǒng)信息融合理論及故障檢測(cè)與診斷技術(shù)研究［D］.長(zhǎng)沙：國(guó)防科技大學(xué)，2000.

（責(zé)任編輯：肖恩忠）

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

山東省自然科學(xué)基金項(xiàng)目（Y2007G64）

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

V249.32 文獻(xiàn)標(biāo)識(shí)碼：A 文章編號(hào)：1671－4288（2011）06－0110－06