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

1 煙臺(tái)南山學(xué)院智能科學(xué)與工程學(xué)院,山東省龍口市大學(xué)路12號(hào),265713 2 海軍航空大學(xué),山東省煙臺(tái)市二馬路188號(hào),264001 3 山東東海熱電有限公司,山東省龍口市東海工業(yè)園區(qū),265713

集中式卡爾曼濾波器是實(shí)現(xiàn)多傳感器組合導(dǎo)航系統(tǒng)的最優(yōu)估計(jì)方法,但是由于其狀態(tài)維數(shù)高,存在計(jì)算負(fù)擔(dān)重、容錯(cuò)性差、通信負(fù)擔(dān)重等缺點(diǎn)。隨著計(jì)算機(jī)技術(shù)的發(fā)展,計(jì)算負(fù)擔(dān)重的困難得到緩解,但對(duì)容錯(cuò)和估計(jì)精度的要求卻越來(lái)越高[1]?；诜植际綖V波方法,聯(lián)邦濾波器可增強(qiáng)系統(tǒng)的容錯(cuò)性與可靠性[2],且設(shè)計(jì)靈活,便于工程實(shí)現(xiàn),但其精度低于集中式卡爾曼濾波器[3]。為解決系統(tǒng)模型誤差導(dǎo)致的濾波精度下降的問(wèn)題,擴(kuò)展卡爾曼濾波、無(wú)跡卡爾曼濾波和容積卡爾曼濾波等非線性濾波相繼被提出[4],然而相對(duì)常規(guī)的線性卡爾曼濾波,這些方法存在工程實(shí)現(xiàn)難的缺陷。為克服量測(cè)噪聲統(tǒng)計(jì)特性不準(zhǔn)確導(dǎo)致的濾波精度下降的問(wèn)題,自適應(yīng)卡爾曼濾波技術(shù)不斷發(fā)展,主要包括基于新息的自適應(yīng)估計(jì)方法(IAE)和貝葉斯方法[5]。變分貝葉斯方法是對(duì)實(shí)際過(guò)程中實(shí)現(xiàn)難度較大的貝葉斯方法的改進(jìn),其核心思想是對(duì)貝葉斯密度分布函數(shù)進(jìn)行近似,可以實(shí)現(xiàn)量測(cè)噪聲統(tǒng)計(jì)特性的次優(yōu)估計(jì)。

本文在集中式卡爾曼濾波的基礎(chǔ)上推導(dǎo)了分布式卡爾曼濾波的一種表達(dá)式方法,該方法不僅保留了集中式卡爾曼濾波技術(shù)的最優(yōu)性,而且具有故障識(shí)別與自動(dòng)隔離、系統(tǒng)自動(dòng)重組的特點(diǎn)。以此為基礎(chǔ),結(jié)合變分貝葉斯估計(jì)方法提出一種組合導(dǎo)航系統(tǒng)的自適應(yīng)分布式濾波算法。

1 自適應(yīng)分布式濾波模型

1.1 集中式卡爾曼濾波

綜合考慮主導(dǎo)航系統(tǒng)及各子導(dǎo)航系統(tǒng),設(shè)多傳感器組合導(dǎo)航系統(tǒng)的狀態(tài)方程為:

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

(1)

各子導(dǎo)航系統(tǒng)的量測(cè)方程可表示為:

(2)

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

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

(3)

且

其中,

(4)

集中式卡爾曼濾波器的狀態(tài)估計(jì)方程為:

(5)

(6)

(7)

(8)

(9)

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

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

(10)

綜合以上,得到多傳感器系統(tǒng)的集中狀態(tài)估計(jì)表達(dá)式為:

(11)

1.2 分布式濾波器的一種表達(dá)形式

根據(jù)式(1)和式(2),與第i個(gè)子導(dǎo)航傳感器對(duì)應(yīng)的卡爾曼濾波方程為:

(12)

(13)

(14)

(15)

(16)

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

(17)

由式(10)可得:

(18)

從式(12)可推出:

(19)

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

(20)

(21)

類似于式(18),有:

(22)

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

(23)

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

(24)

以上分析說(shuō)明,式(24)的分布模型不僅具有集中卡爾曼濾波器的最優(yōu)特性,同時(shí)具有聯(lián)邦卡爾曼濾波器的較好的容錯(cuò)特性。

1.3 變分貝葉斯原理

當(dāng)量測(cè)噪聲方差未知、系統(tǒng)噪聲方差已知,包含量測(cè)噪聲的最優(yōu)貝葉斯分預(yù)測(cè)與更新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)

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

(27)

(28)

式中,m為量測(cè)方差的維數(shù)。

基于上述分析,第i個(gè)子導(dǎo)航傳感器對(duì)應(yīng)的自適應(yīng)卡爾曼濾波的預(yù)測(cè)過(guò)程為:

(29)

量測(cè)更新過(guò)程為:

(30)

式中,·/為點(diǎn)除運(yùn)算。

基于式(29)、(30),利用式(24)可完成多傳感器組合導(dǎo)航系統(tǒng)的自適應(yīng)分布式濾波。從式(29)、(30)看出,量測(cè)噪聲方差的估計(jì)與狀態(tài)估計(jì)相似,也包括預(yù)測(cè)和更新2個(gè)過(guò)程,避免了IAE方法中的滑動(dòng)窗和信息的存儲(chǔ),并可對(duì)協(xié)方差陣內(nèi)獨(dú)立變化的噪聲進(jìn)行單獨(dú)調(diào)整。綜上,本文提出的自適應(yīng)分布式組合導(dǎo)航系統(tǒng)方案見(jiàn)圖1。

圖1 自適應(yīng)分布式組合導(dǎo)航系統(tǒng)方案Fig.1 Adaptive distributed integrated navigation system scheme

2 仿真與分析

2.1 仿真算例模型

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

SINS/CNS子系統(tǒng)的量測(cè)方程采用姿態(tài)組合(轉(zhuǎn)換為地理系下的橫滾角、俯仰角、航向角)的模式,SINS/GNSS量測(cè)方程采用三維位置及三維速度的組合模式,SINS/ADS量測(cè)方程采用高度的組合模式以增強(qiáng)對(duì)高度通道的精確測(cè)量。

2.2 仿真條件設(shè)置

SINS/GNSS/CNS/ADS導(dǎo)航系統(tǒng)的仿真參數(shù)設(shè)置見(jiàn)表1。表中,CNS、GNSS和ADS的測(cè)量誤差均為正常情況下的測(cè)量誤差,選取仿真時(shí)間為3 600 s,SINS的輸出速率為100 Hz,GNSS、CNS和ADS的輸出速率為1 Hz,量測(cè)誤差均為高斯噪聲。

表1 仿真參數(shù)設(shè)置

本文僅考慮量測(cè)噪聲均方差異常的2種極端情況,即突變和緩變。為此,在GNSS子導(dǎo)航系統(tǒng)的各導(dǎo)航參數(shù)中分別加入量測(cè)噪聲均方差階躍變化和斜坡變化,而假設(shè)CNS及ADS的量測(cè)噪聲無(wú)異常,見(jiàn)表2,其中,t代表仿真時(shí)間。

表2 GNSS量測(cè)誤差均方差變化設(shè)置

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

2.3 仿真結(jié)果分析

分別對(duì)聯(lián)邦卡爾曼濾波器(FKF)與本文所提的自適應(yīng)分布式濾波(ADKF)算法進(jìn)行仿真實(shí)驗(yàn)。

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

圖2為當(dāng)GNSS量測(cè)噪聲均方差發(fā)生突變時(shí)2種方法的仿真結(jié)果?？梢钥闯?當(dāng)GNSS量測(cè)噪聲均方差突變時(shí),FKF算法的濾波精度非常差,主要原因是其相關(guān)子濾波器在處理GNSS時(shí)得到的故障檢測(cè)結(jié)果大于門限值,此時(shí)認(rèn)為與GNSS相連的子濾波器發(fā)生故障進(jìn)而進(jìn)行隔離。因?yàn)榻M合導(dǎo)航系統(tǒng)引入了ADS,增加了高度通道的可觀測(cè)性,故其高度與天向速度的精度較好。

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

圖3為利用ADKF算法對(duì)GNSS量測(cè)噪聲均方差發(fā)生突變時(shí)的跟蹤曲線?？梢钥闯?ADKF算法能夠較為準(zhǔn)確地跟蹤GNSS量測(cè)噪聲均方差的變化,進(jìn)而提高組合導(dǎo)航系統(tǒng)的濾波精度。

圖3 ADKF算法對(duì)GNSS量測(cè)噪聲均方差突變時(shí)的跟蹤Fig.3 Tracking of GNSS measurement noise RMS changes abruptly by ADKF algorithm

圖4為當(dāng)GNSS量測(cè)噪聲均方差發(fā)生緩變時(shí)2種方法的仿真結(jié)果,圖5為利用ADKF算法對(duì)GNSS量測(cè)噪聲均方差發(fā)生緩變時(shí)的跟蹤曲線。圖4、5進(jìn)一步證明了本文算法在處理量測(cè)噪聲均方差發(fā)生變化時(shí)的優(yōu)越性。

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

圖5 ADKF算法對(duì)GNSS量測(cè)誤差均方差緩變時(shí)的跟蹤Fig.5 Tracking of GNSS measurement noise RMS changes slowly by ADKF algorithm

3 結(jié) 語(yǔ)

本文提出組合導(dǎo)航系統(tǒng)的自適應(yīng)分布式濾波算法以提高組合導(dǎo)航系統(tǒng)在量測(cè)噪聲統(tǒng)計(jì)特性異常時(shí)的濾波精度。首先給出一種分布式濾波的表達(dá)形式,以便于故障的自動(dòng)隔離及系統(tǒng)的自動(dòng)重組,然后結(jié)合變分貝葉斯估計(jì)理論以自適應(yīng)調(diào)整量測(cè)噪聲的方差。仿真結(jié)果表明,本文算法能有效跟蹤量測(cè)噪聲方差的變化,提高組合導(dǎo)航系統(tǒng)的精度。

本文仿真分析僅考慮了GNSS量測(cè)噪聲方差發(fā)生變化的情況,對(duì)于其他子導(dǎo)航傳感器或多個(gè)子導(dǎo)航傳感器的量測(cè)噪聲方差發(fā)生變化的情況,本文算法也同樣適用。