何廣軍，康旭超

(1. 空軍工程大學(xué) 防空反導(dǎo)學(xué)院， 陜西 西安 710051； 2. 空軍工程大學(xué) 研究生院， 陜西 西安 710051)

高速巡航飛行器的發(fā)展對導(dǎo)航信息的準(zhǔn)確性提出了更高的要求，組合導(dǎo)航因其高精確性和容錯性備受關(guān)注[1-3]。分散式聯(lián)邦濾波器的并行式結(jié)構(gòu)克服了集中式濾波器在狀態(tài)變量增多情況下計算量大、實時性差的問題，在組合導(dǎo)航系統(tǒng)的設(shè)計中被廣泛應(yīng)用[4-7]。但飛行器在實際飛行中面臨環(huán)境多變、器件老化、受損等諸多不確定因素引起的導(dǎo)航系統(tǒng)故障，故障信息會通過全局濾波信息反饋對無故障子系統(tǒng)造成信息污染，嚴(yán)重降低了導(dǎo)航信息的準(zhǔn)確性。

對于故障子系統(tǒng)，通常采用聯(lián)邦濾波器調(diào)整信息分配系數(shù)的方法進行隔離[8-10]。文獻(xiàn)[8]通過先驗信息自適應(yīng)調(diào)整信息分配系數(shù)，減小了模型偏差和觀測粗差對濾波器性能的影響。文獻(xiàn)[10]依據(jù)故障程度構(gòu)造信息分配系數(shù)，自適應(yīng)調(diào)整量測噪聲矩陣，使故障子系統(tǒng)等價于量測噪聲趨于無窮大的正常系統(tǒng)，有效地提高了組合導(dǎo)航系統(tǒng)精度。文獻(xiàn)[11]針對軟故障難以在短時間發(fā)現(xiàn)的問題，提出了一種基于滑動殘差的卡方檢驗法。文獻(xiàn)[12]提出了一種基于四元數(shù)的改進聯(lián)邦濾波算法，具有較高的估計精度。文獻(xiàn)[13]針對χ2檢驗法在故障檢測中的不足，提出一種基于最小二乘支持向量機(Least Squares Support Vector Machine, LS-SVM)的故障檢測方法。文獻(xiàn)[14]從矩陣攝動理論角度出發(fā)，引入權(quán)衡因子以滿足不同環(huán)境下組合導(dǎo)航系統(tǒng)對精度和容錯性不同側(cè)重的需求。

以上所述均采用系統(tǒng)層面的故障隔離，即對故障子系統(tǒng)采用系統(tǒng)式分析方法，認(rèn)為各觀測量估計精度和收斂速度保持一致，給予相同的信息分配系數(shù)[11]。然而，實際上故障子系統(tǒng)中往往只有某個觀測量異常，這種系統(tǒng)式的分析方法極大地降低了導(dǎo)航子系統(tǒng)的信息利用率。對此，本文針對慣性/北斗/天文組合導(dǎo)航系統(tǒng)設(shè)計了一種基于矢量分配的容錯聯(lián)邦濾波算法，對子系統(tǒng)的各狀態(tài)變量進行獨立的故障診斷和信息分配，在隔離故障子系統(tǒng)錯誤信息的同時，增加了對正確觀測信息的利用，有效提高了故障條件下組合導(dǎo)航系統(tǒng)性能。

1 慣性/北斗/天文組合導(dǎo)航模型

1.1 組合導(dǎo)航系統(tǒng)狀態(tài)方程

選取東北天導(dǎo)航坐標(biāo)系為導(dǎo)航解算基本坐標(biāo)系，取位置誤差、速度誤差、姿態(tài)誤差以及陀螺隨機常值漂移和加速度計隨機常值漂移作為系統(tǒng)狀態(tài)變量：

X=[δL,δλ,δh,δve,δvn,δvu,φe,φn,

φu,εx,εy,εz,x,y,z]Τ

(1)

式中：δL、δλ、δh分別為緯度誤差、經(jīng)度誤差和高度誤差；δve、δvn、δvu分別為東、北、天方向速度誤差；φe、φn、φu分別為東、北、天方向數(shù)學(xué)平臺角誤差；εx、εy、εz均為陀螺隨機常值漂移；x、y、z均為加速度計隨機常值零偏。建立組合導(dǎo)航系統(tǒng)狀態(tài)方程：

(2)

其中，F(xiàn)i為第i個子系統(tǒng)的系統(tǒng)矩陣，Xi為第i個子系統(tǒng)的狀態(tài)變量，Gi為第i個子系統(tǒng)的系統(tǒng)噪聲矩陣，系統(tǒng)噪聲為：

W=[wεx,wεy,wεz,w,w,w]T

1.2 組合導(dǎo)航系統(tǒng)量測方程

針對慣性/北斗/天文組合導(dǎo)航系統(tǒng)，分別構(gòu)造慣性/北斗組合導(dǎo)航為子系統(tǒng)1、慣性/天文組合導(dǎo)航為子系統(tǒng)2，各子系統(tǒng)的量測方程如下。

子系統(tǒng)1：取慣性導(dǎo)航與北斗導(dǎo)航的位置信息和速度信息的差值作為觀測量。

(3)

其中：RM、RN分別為地球子午面及與其垂直且共法線的橢圓主曲率半徑；NGe、NGn、NGu和vGe、vGn、vGu分別為北斗衛(wèi)星導(dǎo)航沿東、北、天方向的位置信息誤差和速度信息誤差。

子系統(tǒng)2：取慣性導(dǎo)航與天文導(dǎo)航的姿態(tài)角誤差作為觀測量。

Z2(k)=HaX2(k)+Va(k)

(4)

其中，量測矩陣Ha=[03×6,I3×3,03×6]，Va(k)=[vCγ,vCθ,vCφ]T為星敏感器量測噪聲。

2 容錯聯(lián)邦濾波器的設(shè)計

2.1 基于觀測矢量的故障檢測

卡爾曼濾波中，故障與殘差有密切聯(lián)系，因此傳統(tǒng)的故障檢測多采用基于殘差的χ2檢測法，利用故障發(fā)生時殘差不再服從零均值的高斯白噪聲分布的特性，構(gòu)造故障檢測函數(shù)，通過分析故障檢測函數(shù)的分布特性來判斷故障是否發(fā)生[15]。但以往設(shè)計的故障檢測函數(shù)值均為標(biāo)量形式，僅僅將子系統(tǒng)觀測量看作一個整體進行系統(tǒng)層面的故障診斷，這往往與真實故障情境有較大出入。對此，本文設(shè)計了一種針對各觀測量的矢量故障檢測方法。

子系統(tǒng)i的故障檢測函數(shù)為：

(5)

(6)

其中，ri表示子系統(tǒng)i在k時刻的殘差值，則Λi為m行m列的矩陣，可以表示為：

(7)

2.2 故障條件下的信息分配方法

信息分配系數(shù)的選取直接影響聯(lián)邦濾波器的性能。不同的信息分配方案，狀態(tài)的估計精度與系統(tǒng)魯棒性之間往往存在矛盾[15]。對于故障子系統(tǒng)，若給予較大的信息分配系數(shù)，則不利于全局融合精度的提高；若給予較小的信息分配系數(shù)，在全局估計精度提高的同時，則增加了對錯誤量測信息的利用，不利于系統(tǒng)的穩(wěn)定，因此如何權(quán)衡狀態(tài)估計精度與系統(tǒng)魯棒性是設(shè)計聯(lián)邦濾波器的一個難點。對此，本文設(shè)計了一種信息分配方案，通過對狀態(tài)估計方差、系統(tǒng)噪聲方差及量測噪聲方差的動態(tài)調(diào)整，從而提高系統(tǒng)估計精度和魯棒性。

構(gòu)造子系統(tǒng)i的量測噪聲系數(shù)，則

(8)

其中

(9)

式中：m為子系統(tǒng)i的觀測量維數(shù)，不改變正常觀測量的量測噪聲，分配量測噪聲系數(shù)為1；對于判定發(fā)生故障的觀測量，通過令量測噪聲系數(shù)為0來增大其量測噪聲，并通過構(gòu)造量測噪聲系數(shù)矩陣Bi實現(xiàn)對量測噪聲方差陣的動態(tài)調(diào)整。Rireal(k)為子系統(tǒng)i的真實量測噪聲，則量測噪聲動態(tài)調(diào)整過程為：

(10)

由式(10)可知，調(diào)整后的故障觀測量測噪聲方差趨于無窮，從而實現(xiàn)故障觀測量的動態(tài)隔離。

系統(tǒng)誤差估計方差能夠?qū)崟r反映估計精度，可以通過方差陣特征值分解的方法構(gòu)造估計信息分配系數(shù)Gi[16]。提取系統(tǒng)i的協(xié)方差陣Pi進行特征值分解，則

(11)

其中，λ1,i,λ2,i，…，λn,i為子系統(tǒng)i(i=1,2)的協(xié)方差陣特征值，則估計精度系數(shù)定義為：

(12)

(13)

狀態(tài)估計方差越大，估計精度越低，對應(yīng)的分配系數(shù)越小。 為了保證信息分配后各子系統(tǒng)的狀態(tài)協(xié)方差陣依然為對稱陣，改進信息分配方式[16]，則

(14)

(15)

其中，Pg、Qg分別為全局融合后的協(xié)方差陣和系統(tǒng)噪聲矩陣，Pi、Qi分別為子系統(tǒng)i信息重置后的協(xié)方差陣和系統(tǒng)噪聲矩陣。

2.3 容錯聯(lián)邦濾波算法

根據(jù)濾波器承擔(dān)量測更新、時間更新以及信息分配任務(wù)的不同，聯(lián)邦濾波設(shè)計方法通?？梢苑譃橐韵聝煞N。 算法1：由各子濾波器完成量測更新后將信息送入主濾波器進行信息融合，融合后的結(jié)果在主濾波器內(nèi)完成時間更新，然后將估計信息分配給各子濾波器進行下一步的量測更新。 為了降低主濾波器發(fā)生故障后通過反饋對子濾波器的信息污染，本文采用算法2，即在各子濾波器內(nèi)完成量測更新與時間更新后將信息送入主濾波器進行信息融合，融合后的結(jié)果根據(jù)信息分配方法分配到各子濾波器進行下一步時間更新與量測更新[5]。

根據(jù)2.2節(jié)得到的量測噪聲系數(shù)及信息分配系數(shù)矩陣，設(shè)計容錯聯(lián)邦濾波算法如下：

1)子濾波器時間更新，即

Xi[(k+1)/k]=Fi[(k+1)/k]Xi(k)

(16)

(17)

2)故障診斷。 建立故障檢測函數(shù)Λi，對于觀測矢量的故障診斷方法在2.1節(jié)中已經(jīng)做了詳細(xì)介紹，本節(jié)不再贅述。 通過對觀測量的故障診斷，得到量測噪聲系數(shù)Bi，重構(gòu)量測噪聲矩陣Ri，減小濾波過程中對故障觀測量的利用。

(18)

3)子濾波器量測更新，即

(19)

Xi(k+1)=Xi[(k+1)/k]+Ki(k+1)·

{Zi(k+1)-Hi(k+1)Xi[(k+1)/k]}

(20)

Pi(k+1)=[I-Ki(k+1)Hi(k+1)]·
Pi[(k+1)/k][I-Ki(k+1)Hi(k+1)]T+
Ki(k+1)Ri(k+1)[Ki(k+1)]T

(21)

4)構(gòu)建信息分配矢量系數(shù)。 根據(jù)2.2節(jié)的信息分配方法，得到信息分配矢量系數(shù)Gi，即

(22)

5)主濾波器信息融合，則

(23)

(24)

6)子濾波器信息分配，則

(25)

(26)

Xi(k+1)=Xg(k+1)

(27)

由以上公式可得容錯聯(lián)邦濾波器結(jié)構(gòu)如圖1所示。

圖1 容錯聯(lián)邦濾波器結(jié)構(gòu)Fig.1 Structure of the fault-tolerant federated filter

3 容錯聯(lián)邦濾波器性能分析

3.1 系統(tǒng)全局估計精度分析

系統(tǒng)協(xié)方差陣能夠反映估計精度的大小。

(28)

由式(28)可知，系統(tǒng)i(i=1,2)的協(xié)方差與量測噪聲正相關(guān)。系統(tǒng)發(fā)生故障時，通過調(diào)整噪聲系數(shù)增大其故障觀測量的量測噪聲Ri，協(xié)方差Pi增大。根據(jù)式(24)可知，Pg/Pi反映了子系統(tǒng)i的狀態(tài)估計在全局估計中所占的比重。Pi增大使得故障子系統(tǒng)在信息融合中所占比重降低，從而降低了對故障子系統(tǒng)信息的利用程度。由于不同子系統(tǒng)觀測矩陣維數(shù)不同，以子系統(tǒng)1為例，通過對量測噪聲的動態(tài)調(diào)整，可以得到：

(29)

其中，n為狀態(tài)變量維數(shù)，j為子系統(tǒng)i的觀測量維數(shù)。觀測量Zj,i對應(yīng)的狀態(tài)估計精度為：

(30)

(31)

(32)

本文考慮到各狀態(tài)變量具有不同的收斂速度和估計精度，采用矢量形式的信息分配方法，對各狀態(tài)變量方差構(gòu)造不同的加權(quán)因子，一定程度上提高了濾波精度。

3.2 系統(tǒng)魯棒性能分析

導(dǎo)航子系統(tǒng)各狀態(tài)變量得到較大的信息分配系數(shù)將導(dǎo)致協(xié)方差Pjj減小，在全局估計精度提高的同時，降低了對正確觀測量的利用，魯棒性降低。針對傳統(tǒng)信息分配方法中估計精度與系統(tǒng)魯棒性矛盾的問題，本文通過對量測噪聲的動態(tài)調(diào)整加以改善。根據(jù)式(20)可推得：

(33)

對于故障子系統(tǒng)j，分配較小的量測噪聲系數(shù)Bj和信息分配矢量系數(shù)Gj，減少了異常觀測量Zj(k+1)在局部濾波中的作用，降低了Xj在全局估計中的影響比重。以往的容錯方法多對系統(tǒng)進行整體隔離，但實際情況往往并非所有觀測量均發(fā)生故障，在對故障觀測量隔離的同時，正確觀測量的狀態(tài)估計也將受到影響。下文以子系統(tǒng)1為例，對系統(tǒng)的姿態(tài)角、速度、位置三個狀態(tài)進行分析。根據(jù)式(19)～(20)可得：

(34)

其中，a(k)、v(k)、p(k)分別代表姿態(tài)、速度、位置狀態(tài)估計，Pv[k/(k-1)]、Pp[k/(k-1)]分別為速度、位置的預(yù)測估計方差，Rv,real(k)、Rp,real(k)分別為速度、位置的真實量測噪聲方差，nv、np分別為速度、位置觀測量的噪聲系數(shù)，rv(k)、rp(k)分別為速度、位置新息。

若子系統(tǒng)發(fā)生故障，速度信息觀測量Zv(k)異常，通過調(diào)整量測噪聲系數(shù)，即令nv=0，重構(gòu)的速度量測噪聲方差趨于無窮,可得：

(35)

從而減少了Zv(k)在局部濾波中的作用。對于正確觀測量Zp(k)，量測噪聲系數(shù)np=1，克服了對正確觀測信息同時隔離的缺陷。相對傳統(tǒng)的容錯濾波而言，本文算法僅減少了對錯誤觀測量Zv(k)的利用，故障系統(tǒng)的正確觀測量Zp(k)依然發(fā)揮其對預(yù)測狀態(tài)p[k/(k-1)]的修正作用，在對錯誤觀測信息隔離的同時，其他正常狀態(tài)并未受到影響，依然保持較高的濾波精度，提升了系統(tǒng)的魯棒性。

4 仿真實驗與分析

通過仿真實驗驗證所提算法對組合導(dǎo)航系統(tǒng)性能的影響。采用慣性/北斗/天文組合導(dǎo)航系統(tǒng)，飛行器初始位置為40.100 21°N，118.602 12°E，高度為100 m，初始速度誤差均為0.05 m/s，姿態(tài)角誤差均為10″，位置誤差分別為6 m、9 m、1 m，誤差陀螺漂移為0.01 (°)/h(1σ)，加速度計零偏為100 μg (1σ)，CNS測量誤差為10″，BDS位置誤差為10 m，速度誤差為0.1 m/s，仿真時間選取為800 s，濾波周期為1 s。

為檢驗無故障發(fā)生時本文算法對于組合導(dǎo)航系統(tǒng)全局估計精度的影響，分別對傳統(tǒng)聯(lián)邦濾波算法、自適應(yīng)聯(lián)邦濾波算法以及本文算法進行仿真實驗。其中，傳統(tǒng)聯(lián)邦濾波算法采用固定信息分配比為1 ∶1的信息分配方法，自適應(yīng)聯(lián)邦濾波算法采用自適應(yīng)標(biāo)量的信息分配方法，信息分配系數(shù)為：

(36)

仿真結(jié)果如圖2～4所示，導(dǎo)航誤差結(jié)果見表1。

表1 不同信息分配方式導(dǎo)航信息誤差對比

圖2 位置誤差曲線比較Fig.2 Comparison of position error curves

通過對比可知無故障發(fā)生時，采用矢量信息分配方法的性能優(yōu)于自適應(yīng)標(biāo)量信息分配的聯(lián)邦濾波算法，誤差估計較為穩(wěn)定，相較采用固定信息分配的方法，均方差值明顯減小，導(dǎo)航信息精度有較大提升。

圖3 速度誤差曲線比較Fig.3 Comparison of speed error curves

圖4 姿態(tài)誤差曲線比較Fig.4 Comparison of attitude error curves

傳統(tǒng)故障隔離算法對故障子系統(tǒng)進行系統(tǒng)級隔離，通過將故障子系統(tǒng)隔離實現(xiàn)容錯組合導(dǎo)航[10]。北斗導(dǎo)航系統(tǒng)通過4組以上的偽距方程聯(lián)立解算出載體的位置信息，解算公式如式(37)所示。

ρi=[(x-xsi)2+(y-ysi)2+(z-zsi)2]1/2+cΔt

t=1,2,3,4…

(37)

式中，x、y、z和xsi、ysi、zsi分別是載體和衛(wèi)星i在地球坐標(biāo)系中的位置坐標(biāo)。同理，可以通過聯(lián)立4組偽距率方程得到載體的速度信息，方程如下：

t=1,2,3,4,…

(38)

因此，要解算位置信息需要正確的偽距，而要得到速度信息需要正確的位置坐標(biāo)和偽距率。但在實際過程中，由于電離層折射、多經(jīng)效應(yīng)、信號接收誤差等多種不確定因素的影響，偽距、偽距率往往存在異常，從而影響位置、速度的解算。

為了驗證故障發(fā)生時本文算法的性能，設(shè)置如下模擬場景。在300～310 s時段內(nèi)，解算得到的速度、位置信息均為異常值，速度、位置誤差分別為2.5 m/s、2 m/s、2.3 m/s和100 m、95 m、90 m。450～500 s時段內(nèi)速度信息異常，誤差為3 m/s、3 m/s、3 m/s。600～620 s時段內(nèi)速度、位置誤差分別為3 m/s、3.2 m/s、2.8 m/s和110 m、100 m、110 m。將本文算法與傳統(tǒng)的故障隔離算法在不同時間段進行對比，仿真結(jié)果如圖5～7所示。其中，方法1為采用傳統(tǒng)故障隔離算法，方法2為采用本文容錯聯(lián)邦濾波算法。

圖5 速度誤差對比Fig.5 Comparison of velocity error

圖7 姿態(tài)誤差對比Fig.7 Comparison of attitude error

三個故障時段子系統(tǒng)速度信息存在異常，觀察圖5可知，兩種方法均能對異常速度信息進行有效隔離，修正后的速度誤差控制在0.2 m/s內(nèi)。

對圖6分析可知，采用方法1雖然對故障觀測量有較好的隔離效果，但同時也影響了非故障觀測量。在300～310 s、600～620 s的故障時段內(nèi)，采用兩種方法修正后的導(dǎo)航信息精度相差不大，這是由于此時的位置、速度信息均為故障量，兩種方法對速度、位置信息進行了同樣的隔離處理。本文算法的優(yōu)勢在于當(dāng)故障子系統(tǒng)并非所有觀測量異常時，只對故障量進行隔離，因此能夠充分利用正確的觀測信息。為更充分地體現(xiàn)本文算法的優(yōu)勢，在400～550 s時段內(nèi)對兩種算法進行仿真對比分析，速度、位置誤差對比效果如圖8～9所示。

圖6 位置誤差對比Fig.6 Comparison of position error

圖8 400～550 s速度誤差對比Fig.8 Comparison of velocity error in 400～550 s

圖9 400～550 s時段內(nèi)位置誤差對比Fig.9 Comparison of position error in 400～550 s

圖8中400～450 s以及500～550 s兩個時段為非故障段，觀察圖中可知采用兩種方法得到的導(dǎo)航結(jié)果均具有較高精度，證明了本文算法在系統(tǒng)正常情況下的可行性。450～500 s時段內(nèi)系統(tǒng)發(fā)生故障，假設(shè)偽距率異常僅導(dǎo)致速度信息異常，從圖中可以看出，在該故障段采用方法1得到的速度、位置誤差逐漸增加，這是由于方法1對所有觀測量進行隔離，相當(dāng)于濾波過程只進行時間更新導(dǎo)致誤差不斷積累。方法2只對故障觀測量進行隔離，正常的觀測信息仍然起到對誤差的修正作用，因此得到的誤差更小、更平穩(wěn)，仿真數(shù)據(jù)結(jié)果見表2。

通過對圖5～9以及表2數(shù)據(jù)綜合分析可知，與傳統(tǒng)的故障隔離方法對比，本文提出的容錯聯(lián)邦濾波算法有更好的容錯效果，尤其是在持續(xù)時間較長且非所有觀測量均發(fā)生故障時具有明顯的優(yōu)勢。能夠在有效隔離故障信息的同時，充分利用導(dǎo)航子系統(tǒng)正確導(dǎo)航信息，提升了系統(tǒng)魯棒性，具有較高的全局估計精度。

表2 400～550 s時段內(nèi)兩種算法導(dǎo)航數(shù)據(jù)誤差對比

5 結(jié)論

本文從組合導(dǎo)航系統(tǒng)精確性和魯棒性兩方面出發(fā)，設(shè)計了一種基于矢量分配的容錯聯(lián)邦濾波算法，可得到以下結(jié)論：

1)基于矢量形式的故障檢測方法，克服了常規(guī)容錯聯(lián)邦濾波算法中無法診斷導(dǎo)航子系統(tǒng)每個觀測量是否異常的缺陷，提高了組合導(dǎo)航的精確性。

2)通過量測噪聲系數(shù)實時調(diào)整量測噪聲矩陣，避免了隔離故障觀測量對正確觀測量狀態(tài)估計的影響，有效地提升了導(dǎo)航子系統(tǒng)的魯棒性。

3)主濾波器信息反饋采用矢量形式的信息分配方法，最大程度地發(fā)揮每個子系統(tǒng)的優(yōu)勢，充分利用了各子系統(tǒng)的狀態(tài)估計信息，提高了組合導(dǎo)航系統(tǒng)的精度。

該算法能夠應(yīng)用于多種組合導(dǎo)航系統(tǒng)，且保持了傳統(tǒng)聯(lián)邦濾波結(jié)構(gòu)計算量小、實時性高的優(yōu)點，具有較好的應(yīng)用前景。