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

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

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

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

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

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

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

(1)

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

(2)

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

(3)

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

(4)

(5)

(6)

(7)

(8)

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

2 質(zhì)量評價函數(shù)與量測噪聲等價權(quán)函數(shù)

2.1 基于先驗概率分布的量測信息質(zhì)量評價函數(shù)

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

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

H0：待檢測信息健康，μ=μ0；

H1：待檢測信息異常，μ=μ1。

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

(9)

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

由于

(10)

定義似然比公式為：

(11)

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

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

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

(12)

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

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

(13)

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

(14)

樣本似然比L(k)為：

(15)

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

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

(16)

其中：

量測新息服從正態(tài)分布，檢驗統(tǒng)計量與待檢測信號的低頻噪聲實時幅值相關(guān)，因此質(zhì)量評價函數(shù)值的大小可以反映被檢測量測信息實時質(zhì)量，且由于先驗概率的引入與新觀察數(shù)據(jù)的不斷填充，除了突變粗差外，小幅慢變噪聲也可以被檢測，對量測信息質(zhì)量的評價可以反映小幅慢變噪聲的影響。

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

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

考慮到對小幅慢變噪聲的檢測與抑制，量測信息根據(jù)質(zhì)量評價值被劃分為3個區(qū)間：有效信息、存疑信息、有害信息，質(zhì)量評價采用IGG方案，構(gòu)建權(quán)函數(shù)：

(17)

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

(18)

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

3 仿真實驗及分析

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

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

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

觀測模型為：

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

3.1 仿真實驗條件及實驗結(jié)果分析

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

圖1 載體運動軌跡Fig.1 Description of the vehicle trajectory

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

表1 多普勒計程儀量測信息異常噪聲起止時間

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

表2 超短基線量測信息異常噪聲起止時間

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

4 結(jié)論

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

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

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

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