一種多傳感器組合導航系統的自適應分布式濾波算法

王 瑋 孫煒瑋 潘新龍 喬玉新 韓真真

1 煙臺南山學院智能科學與工程學院,山東省龍口市大學路12號,265713 2 海軍航空大學,山東省煙臺市二馬路188號,264001 3 山東東海熱電有限公司,山東省龍口市東海工業園區,265713

集中式卡爾曼濾波器是實現多傳感器組合導航系統的最優估計方法,但是由于其狀態維數高,存在計算負擔重、容錯性差、通信負擔重等缺點。隨著計算機技術的發展,計算負擔重的困難得到緩解,但對容錯和估計精度的要求卻越來越高[1]。基于分布式濾波方法,聯邦濾波器可增強系統的容錯性與可靠性[2],且設計靈活,便于工程實現,但其精度低于集中式卡爾曼濾波器[3]。為解決系統模型誤差導致的濾波精度下降的問題,擴展卡爾曼濾波、無跡卡爾曼濾波和容積卡爾曼濾波等非線性濾波相繼被提出[4],然而相對常規的線性卡爾曼濾波,這些方法存在工程實現難的缺陷。為克服量測噪聲統計特性不準確導致的濾波精度下降的問題,自適應卡爾曼濾波技術不斷發展,主要包括基于新息的自適應估計方法(IAE)和貝葉斯方法[5]。變分貝葉斯方法是對實際過程中實現難度較大的貝葉斯方法的改進,其核心思想是對貝葉斯密度分布函數進行近似,可以實現量測噪聲統計特性的次優估計。

本文在集中式卡爾曼濾波的基礎上推導了分布式卡爾曼濾波的一種表達式方法,該方法不僅保留了集中式卡爾曼濾波技術的最優性,而且具有故障識別與自動隔離、系統自動重組的特點。以此為基礎,結合變分貝葉斯估計方法提出一種組合導航系統的自適應分布式濾波算法。

1 自適應分布式濾波模型

1.1 集中式卡爾曼濾波

綜合考慮主導航系統及各子導航系統,設多傳感器組合導航系統的狀態方程為:

Xk+1=Φk+1,kXk+ΓkWk

(1)

各子導航系統的量測方程可表示為:

(2)

故集中式卡爾曼濾波器量測方程為:

Zk+1=Hk+1Xk+1+Vk+1

(3)

且

其中,

(4)

集中式卡爾曼濾波器的狀態估計方程為:

(5)

(6)

(7)

(8)

(9)

其中,式(8)中Pk+1|k+1也可寫為:

Pk+1|k+1=[I-Kk+1Hk+1]Pk+1|k

(10)

綜合以上,得到多傳感器系統的集中狀態估計表達式為:

(11)

1.2 分布式濾波器的一種表達形式

根據式(1)和式(2),與第i個子導航傳感器對應的卡爾曼濾波方程為:

(12)

(13)

(14)

(15)

(16)

展開式(5)右邊,合并后有:

(17)

由式(10)可得:

(18)

從式(12)可推出:

(19)

假定所有出現的矩陣的逆矩陣都是存在的,并且P0為非奇異的。由式(7)可得:

(20)

(21)

類似于式(18),有:

(22)

將式(22)代入式(21):

(23)

將式(18)、(20)、(23)代入式(17):

(24)

以上分析說明,式(24)的分布模型不僅具有集中卡爾曼濾波器的最優特性,同時具有聯邦卡爾曼濾波器的較好的容錯特性。

1.3 變分貝葉斯原理

當量測噪聲方差未知、系統噪聲方差已知,包含量測噪聲的最優貝葉斯分預測與更新2步[7-8]。

p(Xk-1,Rk-1|Z1:k-1)dXk-1dRk-1

(25)

p(Xk,Rk|Z1:k)∝

p(Zk|Xk,Rk)p(Xk,Rk|Z1:k-1)

(26)

雖然上述貝葉斯濾波是最優的,但后驗密度函數在實際中很難求解。變分貝葉斯原理是一種近似方法,對難以求解的后驗分布利用多個已知分布進行近似。由變分貝葉斯原理,式(25)和式(26)可轉換為如下的高斯分布和逆Gamma分布:

(27)

(28)

式中,m為量測方差的維數。

基于上述分析,第i個子導航傳感器對應的自適應卡爾曼濾波的預測過程為:

(29)

量測更新過程為:

(30)

式中,·/為點除運算。

基于式(29)、(30),利用式(24)可完成多傳感器組合導航系統的自適應分布式濾波。從式(29)、(30)看出,量測噪聲方差的估計與狀態估計相似,也包括預測和更新2個過程,避免了IAE方法中的滑動窗和信息的存儲,并可對協方差陣內獨立變化的噪聲進行單獨調整。綜上,本文提出的自適應分布式組合導航系統方案見圖1。

圖1 自適應分布式組合導航系統方案Fig.1 Adaptive distributed integrated navigation system scheme

2 仿真與分析

2.1 仿真算例模型

以SINS/GNSS/CNS/ADS多傳感器組合導航系統為例進行仿真與分析。選取式(1)中的系統狀態向量為X=[φE,φN,φU,δvE,δvN,δvU,δL,δλ,δh,εbx,εby,εbz,εrx,εry,εrz,?rx,?ry,?rz]T,其中,φE、φN、φU分別為平臺東、北、天向的誤差角;δvE、δvN、δvU分別為東、北、天向的速度誤差;δL、δλ、δh分別為緯度、經度、高度誤差;εbx、εby、εbz與εrx、εry、εrz分別為陀螺沿x、y、z軸的常值漂移誤差和一階Markov漂移誤差;?rx、?ry、?rz分別為加速度計沿x、y、z軸的一階Markov漂移誤差。

SINS/CNS子系統的量測方程采用姿態組合(轉換為地理系下的橫滾角、俯仰角、航向角)的模式,SINS/GNSS量測方程采用三維位置及三維速度的組合模式,SINS/ADS量測方程采用高度的組合模式以增強對高度通道的精確測量。

2.2 仿真條件設置

SINS/GNSS/CNS/ADS導航系統的仿真參數設置見表1。表中,CNS、GNSS和ADS的測量誤差均為正常情況下的測量誤差,選取仿真時間為3 600 s,SINS的輸出速率為100 Hz,GNSS、CNS和ADS的輸出速率為1 Hz,量測誤差均為高斯噪聲。

表1 仿真參數設置

本文僅考慮量測噪聲均方差異常的2種極端情況,即突變和緩變。為此,在GNSS子導航系統的各導航參數中分別加入量測噪聲均方差階躍變化和斜坡變化,而假設CNS及ADS的量測噪聲無異常,見表2,其中,t代表仿真時間。

表2 GNSS量測誤差均方差變化設置

與GNSS對應的子濾波器中,ρ、α、β的初始值選取為:ρ=[0.990.990.990.990.990.99]、α=[111111]、β=[2220.10.10.1]。在設定包含起飛、爬升、俯仰、加速、轉彎等各種機動飛行軌跡的基礎上,針對表2中GNSS量測噪聲均方差異常的2種情況,對算法性能進行仿真分析。

2.3 仿真結果分析

分別對聯邦卡爾曼濾波器(FKF)與本文所提的自適應分布式濾波(ADKF)算法進行仿真實驗。

其中,FKF方法采用基于殘差卡方檢測的、具有容錯功能的、帶有重置式的信息融合模式。

圖2為當GNSS量測噪聲均方差發生突變時2種方法的仿真結果。可以看出,當GNSS量測噪聲均方差突變時,FKF算法的濾波精度非常差,主要原因是其相關子濾波器在處理GNSS時得到的故障檢測結果大于門限值,此時認為與GNSS相連的子濾波器發生故障進而進行隔離。因為組合導航系統引入了ADS,增加了高度通道的可觀測性,故其高度與天向速度的精度較好。

圖2 GNSS量測噪聲均方差突變時誤差比較Fig.2 Comparison of error when GNSS measurement noise RMS changes abruptly

圖3為利用ADKF算法對GNSS量測噪聲均方差發生突變時的跟蹤曲線。可以看出,ADKF算法能夠較為準確地跟蹤GNSS量測噪聲均方差的變化,進而提高組合導航系統的濾波精度。

圖3 ADKF算法對GNSS量測噪聲均方差突變時的跟蹤Fig.3 Tracking of GNSS measurement noise RMS changes abruptly by ADKF algorithm

圖4為當GNSS量測噪聲均方差發生緩變時2種方法的仿真結果,圖5為利用ADKF算法對GNSS量測噪聲均方差發生緩變時的跟蹤曲線。圖4、5進一步證明了本文算法在處理量測噪聲均方差發生變化時的優越性。

圖4 GNSS量測噪聲均方差緩變時誤差比較Fig.4 Comparison of error when GNSS measurement noise RMS changes slowly

圖5 ADKF算法對GNSS量測誤差均方差緩變時的跟蹤Fig.5 Tracking of GNSS measurement noise RMS changes slowly by ADKF algorithm

3 結 語

本文提出組合導航系統的自適應分布式濾波算法以提高組合導航系統在量測噪聲統計特性異常時的濾波精度。首先給出一種分布式濾波的表達形式,以便于故障的自動隔離及系統的自動重組,然后結合變分貝葉斯估計理論以自適應調整量測噪聲的方差。仿真結果表明,本文算法能有效跟蹤量測噪聲方差的變化,提高組合導航系統的精度。

本文仿真分析僅考慮了GNSS量測噪聲方差發生變化的情況,對于其他子導航傳感器或多個子導航傳感器的量測噪聲方差發生變化的情況,本文算法也同樣適用。