基于矢量分配的組合導航容錯聯邦濾波算法*

何廣軍，康旭超

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

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

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

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

1 慣性/北斗/天文組合導航模型

1.1 組合導航系統狀態方程

選取東北天導航坐標系為導航解算基本坐標系，取位置誤差、速度誤差、姿態誤差以及陀螺隨機常值漂移和加速度計隨機常值漂移作為系統狀態變量：

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

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

(1)

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

(2)

其中，Fi為第i個子系統的系統矩陣，Xi為第i個子系統的狀態變量，Gi為第i個子系統的系統噪聲矩陣，系統噪聲為：

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

1.2 組合導航系統量測方程

針對慣性/北斗/天文組合導航系統，分別構造慣性/北斗組合導航為子系統1、慣性/天文組合導航為子系統2，各子系統的量測方程如下。

子系統1：取慣性導航與北斗導航的位置信息和速度信息的差值作為觀測量。

(3)

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

子系統2：取慣性導航與天文導航的姿態角誤差作為觀測量。

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

(4)

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

2 容錯聯邦濾波器的設計

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

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

子系統i的故障檢測函數為：

(5)

(6)

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

(7)

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

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

構造子系統i的量測噪聲系數，則

(8)

其中

(9)

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

(10)

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

系統誤差估計方差能夠實時反映估計精度，可以通過方差陣特征值分解的方法構造估計信息分配系數Gi[16]。提取系統i的協方差陣Pi進行特征值分解，則

(11)

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

(12)

(13)

狀態估計方差越大，估計精度越低，對應的分配系數越小。 為了保證信息分配后各子系統的狀態協方差陣依然為對稱陣，改進信息分配方式[16]，則

(14)

(15)

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

2.3 容錯聯邦濾波算法

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

根據2.2節得到的量測噪聲系數及信息分配系數矩陣，設計容錯聯邦濾波算法如下：

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

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

(16)

(17)

2)故障診斷。 建立故障檢測函數Λi，對于觀測矢量的故障診斷方法在2.1節中已經做了詳細介紹，本節不再贅述。 通過對觀測量的故障診斷，得到量測噪聲系數Bi，重構量測噪聲矩陣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)構建信息分配矢量系數。 根據2.2節的信息分配方法，得到信息分配矢量系數Gi，即

(22)

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

(23)

(24)

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

(25)

(26)

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

(27)

由以上公式可得容錯聯邦濾波器結構如圖1所示。

圖1 容錯聯邦濾波器結構Fig.1 Structure of the fault-tolerant federated filter

3 容錯聯邦濾波器性能分析

3.1 系統全局估計精度分析

系統協方差陣能夠反映估計精度的大小。

(28)

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

(29)

其中，n為狀態變量維數，j為子系統i的觀測量維數。觀測量Zj,i對應的狀態估計精度為：

(30)

(31)

(32)

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

3.2 系統魯棒性能分析

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

(33)

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

(34)

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

若子系統發生故障，速度信息觀測量Zv(k)異常，通過調整量測噪聲系數，即令nv=0，重構的速度量測噪聲方差趨于無窮,可得：

(35)

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

4 仿真實驗與分析

通過仿真實驗驗證所提算法對組合導航系統性能的影響。采用慣性/北斗/天文組合導航系統，飛行器初始位置為40.100 21°N，118.602 12°E，高度為100 m，初始速度誤差均為0.05 m/s，姿態角誤差均為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。

為檢驗無故障發生時本文算法對于組合導航系統全局估計精度的影響，分別對傳統聯邦濾波算法、自適應聯邦濾波算法以及本文算法進行仿真實驗。其中，傳統聯邦濾波算法采用固定信息分配比為1 ∶1的信息分配方法，自適應聯邦濾波算法采用自適應標量的信息分配方法，信息分配系數為：

(36)

仿真結果如圖2～4所示，導航誤差結果見表1。

表1 不同信息分配方式導航信息誤差對比

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

通過對比可知無故障發生時，采用矢量信息分配方法的性能優于自適應標量信息分配的聯邦濾波算法，誤差估計較為穩定，相較采用固定信息分配的方法，均方差值明顯減小，導航信息精度有較大提升。

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

圖4 姿態誤差曲線比較Fig.4 Comparison of attitude error curves

傳統故障隔離算法對故障子系統進行系統級隔離，通過將故障子系統隔離實現容錯組合導航[10]。北斗導航系統通過4組以上的偽距方程聯立解算出載體的位置信息，解算公式如式(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分別是載體和衛星i在地球坐標系中的位置坐標。同理，可以通過聯立4組偽距率方程得到載體的速度信息，方程如下：

t=1,2,3,4,…

(38)

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

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

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

圖7 姿態誤差對比Fig.7 Comparison of attitude error

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

對圖6分析可知，采用方法1雖然對故障觀測量有較好的隔離效果，但同時也影響了非故障觀測量。在300～310 s、600～620 s的故障時段內，采用兩種方法修正后的導航信息精度相差不大，這是由于此時的位置、速度信息均為故障量，兩種方法對速度、位置信息進行了同樣的隔離處理。本文算法的優勢在于當故障子系統并非所有觀測量異常時，只對故障量進行隔離，因此能夠充分利用正確的觀測信息。為更充分地體現本文算法的優勢，在400～550 s時段內對兩種算法進行仿真對比分析，速度、位置誤差對比效果如圖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時段內位置誤差對比Fig.9 Comparison of position error in 400～550 s

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

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

表2 400～550 s時段內兩種算法導航數據誤差對比

5 結論

本文從組合導航系統精確性和魯棒性兩方面出發，設計了一種基于矢量分配的容錯聯邦濾波算法，可得到以下結論：

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

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

3)主濾波器信息反饋采用矢量形式的信息分配方法，最大程度地發揮每個子系統的優勢，充分利用了各子系統的狀態估計信息，提高了組合導航系統的精度。

該算法能夠應用于多種組合導航系統，且保持了傳統聯邦濾波結構計算量小、實時性高的優點，具有較好的應用前景。