組合導(dǎo)航的集中式容錯(cuò)卡爾曼濾波算法

趙琳， 康瑛瑤， 程建華， 吳謀炎

(哈爾濱工程大學(xué) 智能科學(xué)與工程學(xué)院，黑龍江 哈爾濱 150001)

隨著多源信息組合導(dǎo)航技術(shù)的發(fā)展，導(dǎo)航對(duì)系統(tǒng)容錯(cuò)性的需求也日益提高[1]。傳統(tǒng)多傳感器組合導(dǎo)航系統(tǒng)容錯(cuò)技術(shù)以聯(lián)邦濾波結(jié)合故障檢測(cè)方法為主要技術(shù)手段，以故障檢測(cè)函數(shù)與隔離門限判斷各子濾波器是否發(fā)生故障，隔離故障子濾波器，提高系統(tǒng)可靠性與精度[2-4]。傳統(tǒng)容錯(cuò)技術(shù)將量測(cè)信息分為“有故障”與“無(wú)故障”2種狀態(tài)，對(duì)聯(lián)邦濾波器進(jìn)行隔離式重構(gòu)，對(duì)于小幅值或斜坡慢變故障的檢測(cè)靈敏度差，濾波器可能受到污染，且在檢測(cè)門限附近存在組合模式切換頻繁的情況，降低了系統(tǒng)穩(wěn)定性[5]，除此之外，傳統(tǒng)的故障檢測(cè)技術(shù)，以子濾波器為檢測(cè)對(duì)象，有害量測(cè)發(fā)生時(shí)，對(duì)該子濾波器的所有量測(cè)信息實(shí)行無(wú)差別隔離處理，降低了有效信息利用率[6]。

集中濾波往往被認(rèn)為不具備容錯(cuò)能力，因此對(duì)多源信息組合導(dǎo)航系統(tǒng)來(lái)說(shuō)適用性低于聯(lián)邦濾波[7]。文獻(xiàn)[8]提出了一種矢量故障檢測(cè)方法，通過引入故障檢測(cè)矢量函數(shù)，以量測(cè)信息為檢測(cè)對(duì)象，實(shí)現(xiàn)對(duì)故障觀測(cè)信息的定位，為故障隔離提供依據(jù)，但這種硬切換方式仍然采用隔離式重構(gòu)方法。自適應(yīng)集中濾波器也具有一定的容錯(cuò)能力，其中，部分自適應(yīng)濾波方法在濾波過程中不斷修正或估計(jì)噪聲統(tǒng)計(jì)特性或利用可靠狀態(tài)實(shí)現(xiàn)信息預(yù)測(cè)，實(shí)現(xiàn)變權(quán)控制提升估計(jì)精度[9-11]?？共羁柭鼮V波[12-14]作為自適應(yīng)濾波的一種，以提升濾波算法實(shí)際抗差性能與估計(jì)結(jié)果可靠性為主要目標(biāo)，通過抗差估計(jì)原理實(shí)時(shí)檢測(cè)信息質(zhì)量，控制觀測(cè)異常，對(duì)強(qiáng)突變?cè)肼?階躍噪聲或沖擊噪聲)的檢驗(yàn)具有較高靈敏度，并通過構(gòu)建等價(jià)權(quán)函數(shù)，對(duì)有害信息進(jìn)行降權(quán)處理，抑制突變?cè)肼晫?duì)估計(jì)精度和可靠性的影響，但抗差卡爾曼濾波以量測(cè)信息的標(biāo)準(zhǔn)化殘差為對(duì)象評(píng)價(jià)觀測(cè)信息質(zhì)量并構(gòu)建自適應(yīng)因子，對(duì)狀態(tài)向量的一步預(yù)測(cè)依賴性強(qiáng)，無(wú)法抑制諸如斜坡噪聲、小幅慢變低頻量測(cè)噪聲。

針對(duì)上述問題，本文提出一種應(yīng)用于多傳感器融合組合導(dǎo)航技術(shù)的集中容錯(cuò)卡爾曼濾波算法，提升導(dǎo)航系統(tǒng)精度與可靠性。該算法以廣義極大似然型抗差估計(jì)(M估計(jì))為基礎(chǔ)，基于系統(tǒng)動(dòng)力學(xué)模型預(yù)測(cè)信息與量測(cè)信息構(gòu)建量測(cè)殘差，借助量測(cè)信息先驗(yàn)概率密度與極大后驗(yàn)概率準(zhǔn)則，建立涵蓋量測(cè)粗差、及小幅噪聲、慢變?cè)肼暤鹊皖l噪聲的量測(cè)信息質(zhì)量評(píng)估函數(shù)，基于量測(cè)信息質(zhì)量評(píng)估值對(duì)量測(cè)信息進(jìn)行分類，并設(shè)計(jì)權(quán)函數(shù)與自適應(yīng)因子，通過調(diào)節(jié)濾波參數(shù)實(shí)現(xiàn)有害信息降權(quán)處理與隔離，避免硬隔離的同時(shí)降低存疑信息對(duì)濾波器的污染，充分利用有效量測(cè)信息保證濾波效率，達(dá)到提升導(dǎo)航系統(tǒng)穩(wěn)定性、可靠性與精度的目的。

1 集中式自適應(yīng)容錯(cuò)卡爾曼濾波遞推方程

假設(shè)離散化后的組合導(dǎo)航濾波器動(dòng)力學(xué)模型與觀測(cè)模型為：

(1)

式中：Xk為k時(shí)刻系統(tǒng)n×1維狀態(tài)向量，主要包括慣性導(dǎo)航系統(tǒng)誤差、慣性器件主要誤差參數(shù)和量測(cè)信息主要誤差參數(shù)；Φk,k-1為系統(tǒng)n×n維狀態(tài)一步轉(zhuǎn)移矩陣；Γk為動(dòng)力學(xué)模型系統(tǒng)噪聲矩陣；Wk為系統(tǒng)噪聲向量；Zk為m×1維量測(cè)信息向量；Hk為m×n維量測(cè)矩陣；Vk為量測(cè)噪聲向量。在組合導(dǎo)航中，通常假設(shè)系統(tǒng)噪聲Wk與量測(cè)噪聲Vk為零均值高斯白噪聲。量測(cè)殘差rk為：

(2)

根據(jù)抗差M估計(jì)原則構(gòu)造自適應(yīng)容錯(cuò)卡爾曼濾波的極值條件：

(3)

根據(jù)極值條件，可得到自適應(yīng)卡爾曼濾波的遞推方程：

(4)

(5)

(6)

(7)

(8)

在組合導(dǎo)航實(shí)際應(yīng)用中，容錯(cuò)卡爾曼濾波算法通過對(duì)量測(cè)信息質(zhì)量進(jìn)行評(píng)價(jià)，建立適當(dāng)?shù)牧繙y(cè)信息等價(jià)權(quán)函數(shù)，平衡新息對(duì)狀態(tài)估計(jì)的貢獻(xiàn)，充分利用有效量測(cè)信息，屏蔽有害量測(cè)信息，從而達(dá)到提升導(dǎo)航系統(tǒng)容錯(cuò)性的目的，因此量測(cè)信息質(zhì)量評(píng)價(jià)方法與等加權(quán)函數(shù)的設(shè)計(jì)十分重要，是決定濾波算法容錯(cuò)性能的關(guān)鍵因素之一。

2 質(zhì)量評(píng)價(jià)函數(shù)與量測(cè)噪聲等價(jià)權(quán)函數(shù)

2.1 基于先驗(yàn)概率分布的量測(cè)信息質(zhì)量評(píng)價(jià)函數(shù)

為了實(shí)現(xiàn)對(duì)斜坡慢變?cè)肼暤淖R(shí)別與抑制，引入量測(cè)信息先驗(yàn)概率分布，設(shè)計(jì)量測(cè)信息質(zhì)量評(píng)價(jià)函數(shù)，再基于質(zhì)量評(píng)價(jià)函數(shù)設(shè)計(jì)量測(cè)噪聲等價(jià)權(quán)函數(shù)與自適應(yīng)因子，實(shí)時(shí)調(diào)節(jié)量測(cè)信息權(quán)重，提高組合導(dǎo)航系統(tǒng)容錯(cuò)性。

對(duì)于i時(shí)刻的量測(cè)信息，借助i時(shí)刻的量測(cè)殘差ri及量測(cè)殘差統(tǒng)計(jì)特性對(duì)該量測(cè)信息進(jìn)行質(zhì)量評(píng)價(jià)。根據(jù)量測(cè)信息統(tǒng)計(jì)特性進(jìn)行假設(shè)，假設(shè)健康量測(cè)信息的殘差均值為μ=μ0，則量測(cè)信息的殘差均值包含2個(gè)樣本類型：

H0：待檢測(cè)信息健康，μ=μ0；

H1：待檢測(cè)信息異常，μ=μ1。

量測(cè)信息也對(duì)應(yīng)屬于以上2個(gè)樣本類型。H0與H1樣本的條件概率密度分別為P(rij|H0)與P(rij|H1)，其中rij為第i次量測(cè)殘差ri向量中的第j個(gè)元素，即第i次量測(cè)信息向量中的第j個(gè)量測(cè)殘差。根據(jù)最大后驗(yàn)概率準(zhǔn)則，可得2種樣本類型的后驗(yàn)概率為：

(9)

式中：P(H0)為正常狀態(tài)的先驗(yàn)概率；P(H1)為故障狀態(tài)的先驗(yàn)概率。樣本發(fā)生判斷準(zhǔn)則為：

由于

(10)

定義似然比公式為：

(11)

此時(shí)，樣本發(fā)生判斷準(zhǔn)則可以被改寫為對(duì)似然比值大小的判斷。預(yù)先設(shè)計(jì)門限值TD，可得到：

式中門限TD值與判斷誤警率、漏檢率相關(guān)。

對(duì)于待檢測(cè)的第k次獲得的量測(cè)信息rkj，考慮從第1到k次逐次獲取的量測(cè)信息序列r1j,r2j,…,rkj，則rkj的樣本似然比L(k)為：

(12)

對(duì)似然比L(k)取對(duì)數(shù)變換，定義對(duì)數(shù)似然比為量測(cè)信息質(zhì)量評(píng)價(jià)函數(shù)，得到質(zhì)量評(píng)價(jià)函數(shù)遞推公式：

λ(k-1)+Δλ(k)

(13)

應(yīng)用于組合導(dǎo)航量測(cè)信息評(píng)價(jià)時(shí)，量測(cè)信息近似服從正態(tài)分布，則在H0與H1下待評(píng)價(jià)量測(cè)信息的條件概率密度分別為：

(14)

樣本似然比L(k)為：

(15)

其質(zhì)量評(píng)價(jià)函數(shù)λ(k)的遞推公式為：

λ(k-1)+Δλ(k)

(16)

其中：

量測(cè)新息服從正態(tài)分布，檢驗(yàn)統(tǒng)計(jì)量與待檢測(cè)信號(hào)的低頻噪聲實(shí)時(shí)幅值相關(guān)，因此質(zhì)量評(píng)價(jià)函數(shù)值的大小可以反映被檢測(cè)量測(cè)信息實(shí)時(shí)質(zhì)量，且由于先驗(yàn)概率的引入與新觀察數(shù)據(jù)的不斷填充，除了突變粗差外，小幅慢變?cè)肼曇部梢员粰z測(cè)，對(duì)量測(cè)信息質(zhì)量的評(píng)價(jià)可以反映小幅慢變?cè)肼暤挠绊憽?/p>

2.2 等價(jià)權(quán)函數(shù)與自適應(yīng)因子構(gòu)造

理想的權(quán)函數(shù)可以根據(jù)量測(cè)信息質(zhì)量，實(shí)現(xiàn)對(duì)量測(cè)信息權(quán)重的合理調(diào)節(jié)，有較強(qiáng)的容錯(cuò)能力(有效過濾有害觀測(cè)信息)，且具有較高的濾波估計(jì)效率(合理利用有效信息)。得到廣泛應(yīng)用的權(quán)函數(shù)有2段函數(shù)式和3段函數(shù)式，2段函數(shù)法包括常用的Huber法，丹麥法或選權(quán)法，3段函數(shù)法包括常用的IGG方案[11]。

考慮到對(duì)小幅慢變?cè)肼暤臋z測(cè)與抑制，量測(cè)信息根據(jù)質(zhì)量評(píng)價(jià)值被劃分為3個(gè)區(qū)間：有效信息、存疑信息、有害信息，質(zhì)量評(píng)價(jià)采用IGG方案，構(gòu)建權(quán)函數(shù)：

(17)

式中：|λk|≤c0時(shí)H0成立；|λk|≥c1時(shí)H1成立。因此，c0與c1的取值與H0及H1樣本判定的誤警率Pf及漏檢率Pm有關(guān)，通常情況下，1.0

(18)

由式(18)依次求得量測(cè)信息向量中所有元素對(duì)應(yīng)的等價(jià)噪聲協(xié)方差，更新量測(cè)噪聲協(xié)方差矩陣，代入式(4)～(8)中，即可完成集中式容錯(cuò)卡爾曼濾波器濾波更新。

3 仿真實(shí)驗(yàn)及分析

本文提出集中式自適應(yīng)容錯(cuò)卡爾曼濾波目的在于提高多源導(dǎo)航信息融合的組合導(dǎo)航系統(tǒng)的可靠性，因此，以某慣性/多普勒/超短基線組合導(dǎo)航系統(tǒng)為例，開展仿真實(shí)驗(yàn)，驗(yàn)證算法性能。

3.1 慣性/多普勒/超短基線組合導(dǎo)航濾波模型

選取慣性導(dǎo)航系統(tǒng)姿態(tài)、速度、位置誤差為主要待估計(jì)狀態(tài)量，選取多普勒計(jì)程儀速度信息、超短基線相對(duì)位置信息(斜距R、航向角α、俯仰角β)為量測(cè)信息，建立組合導(dǎo)航模型。此處，采用文獻(xiàn)[8]中已被驗(yàn)證的極區(qū)格網(wǎng)慣性/聲學(xué)組合導(dǎo)航濾波模型，其系統(tǒng)狀態(tài)模型為：

觀測(cè)模型為：

式中各矩陣見文獻(xiàn)[8]。

3.1 仿真實(shí)驗(yàn)條件及實(shí)驗(yàn)結(jié)果分析

設(shè)載體的初始位置為東經(jīng)126°，北緯75°，載體在仿真實(shí)驗(yàn)過程中，具有靜止，勻加速、勻速、勻減速、下潛等運(yùn)動(dòng)狀態(tài)，其中，加速度為0.2 m/s2，最大速度為5 m/s，載體運(yùn)動(dòng)軌跡如圖1所示。

圖1 載體運(yùn)動(dòng)軌跡Fig.1 Description of the vehicle trajectory

陀螺常值漂移為0.05(°)/h，加速度計(jì)零偏為6×10-5g，多普勒計(jì)程儀測(cè)速誤差小于0.3 m/s，超短基線測(cè)距誤差小于20 m，測(cè)向誤差小于1.5°。為了驗(yàn)證濾波算法的容錯(cuò)性能，分別在如表1所示時(shí)間段內(nèi)，向不同量測(cè)信息加入不同性質(zhì)的異常噪聲。仿真時(shí)長(zhǎng)為4 h，仿真結(jié)果如圖2～5所示。

表1 多普勒計(jì)程儀量測(cè)信息異常噪聲起止時(shí)間

圖2 量測(cè)噪聲與權(quán)值Fig.2 Description of measurement noise and weights

圖3 組合導(dǎo)航姿態(tài)誤差Fig.3 Description of the attitude errors

圖4 組合導(dǎo)航速度誤差Fig.4 Description of the velocity errors

圖5 組合導(dǎo)航位置誤差Fig.5 Description of the position errors

組合導(dǎo)航系統(tǒng)量測(cè)噪聲情況如表1、表2及圖2所示。圖2～5中的權(quán)值變化曲線即為不同觀測(cè)量相應(yīng)自適應(yīng)調(diào)節(jié)因子變化曲線。由圖2可知，量測(cè)信息出現(xiàn)異常噪聲時(shí)，量測(cè)信息權(quán)值可得到實(shí)時(shí)有效調(diào)節(jié)。對(duì)高頻量測(cè)粗差，量測(cè)信息權(quán)重可以得到有效調(diào)節(jié)。對(duì)于呈現(xiàn)斜坡增長(zhǎng)的噪聲，在噪聲幅值較小時(shí)，量測(cè)信息對(duì)應(yīng)權(quán)重值由α=1下降至0<α<1，當(dāng)量測(cè)噪聲幅值增長(zhǎng)到一定程度后，權(quán)值下降至0，該量測(cè)信息不再參與量測(cè)更新。此外，當(dāng)超短基線測(cè)向信息αβ異常時(shí)，若測(cè)距信息R輸出不包含異常噪聲，則僅測(cè)向信息被降權(quán)處理，測(cè)距信息權(quán)重不受影響；若測(cè)距信息R也含有異常噪聲，則所有超短基線信息被降權(quán)處理?？梢?，以量測(cè)信息為對(duì)象的容錯(cuò)集中濾波算法不僅可以有效抑制有害量測(cè)信息，還可以保證有效量測(cè)信息得到充分利用，保證濾波效率。

表2 超短基線量測(cè)信息異常噪聲起止時(shí)間

由圖3～5可知，使用傳統(tǒng)卡爾曼濾波算法時(shí)，量測(cè)信息異常噪聲無(wú)法被有效抑制，有害觀測(cè)信息被引入組合導(dǎo)航系統(tǒng)，導(dǎo)致導(dǎo)航精度與可靠性下降。容錯(cuò)卡爾曼濾波通過質(zhì)量評(píng)價(jià)函數(shù)實(shí)時(shí)調(diào)節(jié)量測(cè)信息的權(quán)重值，由表1、表2及圖3～5可知，基于普通卡爾曼濾波的組合導(dǎo)航系統(tǒng)，緯度最大誤差為37.36，均方差5.47；經(jīng)度最大誤差為179.33，均方差33.11；高度最大誤差為12.80，均方差3.28。基于容錯(cuò)卡爾曼濾波的組合導(dǎo)航系統(tǒng)，緯度最大誤差為3.78，均方差0.11；經(jīng)度最大誤差為5.41，均方差0.12；高度最大誤差為5.42，均方差0.11。由此可見，本文提出的容錯(cuò)卡爾曼濾波算法可以有效提高組合導(dǎo)航系統(tǒng)的可靠性和精度。

4 結(jié)論

1)該濾波算法通過引入量測(cè)信息的先驗(yàn)概率分布，基于極大后驗(yàn)準(zhǔn)則，建立了量測(cè)信息質(zhì)量評(píng)價(jià)函數(shù)，并借助質(zhì)量評(píng)價(jià)值設(shè)計(jì)了量測(cè)信息權(quán)函數(shù)與自適應(yīng)因子，通過調(diào)節(jié)濾波參數(shù)，實(shí)時(shí)調(diào)節(jié)了量測(cè)信息在濾波更新中所占權(quán)重。

2)該濾波算法充分利用有效信息，降權(quán)使用存疑信息并有效隔離有害信息，最終實(shí)現(xiàn)了提高組合導(dǎo)航系統(tǒng)容錯(cuò)性、可靠性和精度的目的。

3)通過仿真實(shí)驗(yàn)，充分驗(yàn)證了容錯(cuò)卡爾曼濾波在提高組合導(dǎo)航精度與可靠性方面的有效性。量測(cè)信息受污染時(shí)，降低了量測(cè)信息權(quán)重，對(duì)狀態(tài)一步預(yù)測(cè)給予了更多信任。

考慮到系統(tǒng)動(dòng)力學(xué)模型存在不確定性，如何對(duì)狀態(tài)一步預(yù)測(cè)的等價(jià)方差-協(xié)方差矩陣進(jìn)行自適應(yīng)調(diào)節(jié)，更加精確有效的平衡狀態(tài)更新與量測(cè)更新之間的相對(duì)權(quán)重，將是本文將來(lái)的研究方向。