組合導(dǎo)航中一種新息自適應(yīng)卡爾曼濾波算法

薛文婷,張 波,李署堅(jiān)

(北京航空航天大學(xué)電子信息工程學(xué)院,北京 100191)

0 引 言

卡爾曼濾波是在組合導(dǎo)航中常用的信息融合算法,使用標(biāo)準(zhǔn)卡爾曼濾波的前提條件是系統(tǒng)的動(dòng)力學(xué)特性和噪聲的統(tǒng)計(jì)特性已知,但在實(shí)際情況中由于全球定位系統(tǒng)接收機(jī)與航位推算(DR)器件及外部環(huán)境變化導(dǎo)致的不確定噪聲使得這個(gè)前提不能滿足,常導(dǎo)致濾波發(fā)散[1]。要控制發(fā)散首先需要判斷系統(tǒng)所處的不穩(wěn)定狀況,然后選擇不同的發(fā)散控制方式,新息可以用來作為判斷和調(diào)整濾波器增益的依據(jù)[2]。目前,國(guó)內(nèi)外對(duì)于自適應(yīng)卡爾曼濾波器的研究逐漸集中在兩個(gè)方面:基于新息自適應(yīng)估計(jì)(IAE)[3]的卡爾曼濾波器和多模型卡爾曼濾波器(MMAE)[4]。本文提出了一種基于IAE的卡爾曼濾波算法,可以通過實(shí)際新息的量測(cè)計(jì)算直接實(shí)現(xiàn)修正卡爾曼濾波增益,提高了在GPS測(cè)量發(fā)生較大變化時(shí)卡爾曼濾波器的精度和魯棒性[5]。通過仿真實(shí)驗(yàn)對(duì)比分析了這種方法與傳統(tǒng)卡爾曼濾波方法的定位誤差,研究結(jié)果表明基于新息的卡爾曼濾波具有優(yōu)于傳統(tǒng)卡爾曼濾波的定位結(jié)果且不易發(fā)散。

1 IAE爾曼濾波算法

1.1 卡爾曼濾波新息的概念

離散卡爾曼系統(tǒng)有以下兩個(gè)方程

Xk=φk,k-1Xk-1+Γk,k-1Wk-1，

(1)

Zk=HkXk+Vk，

(2)

式中:Xk為n維狀態(tài)向量;φk,k-1為n×n階狀態(tài)轉(zhuǎn)移矩陣;Γk,k-1為n×p階干擾矩陣;Wk-1為p維系統(tǒng)噪聲向量;Zk為m維觀測(cè)向量;Hk為m×n觀測(cè)矩陣;Vk為m維觀測(cè)噪聲向量。假定Wk與Vk為相互獨(dú)立的零均值白噪聲序列,分別有協(xié)方差陣

cov[WkWj]=Qxδkj，

(3)

cov[VkVj]=Rkδkj.

(4)

假定系統(tǒng)為一致完全能觀和一致完全能控,則濾波是穩(wěn)定的,激蕩濾波遞推充分多步以后,濾波誤差協(xié)方差陣Pk,k將趨于常數(shù)矩陣P+,增益矩陣Kk將趨于常數(shù)矩陣K,預(yù)測(cè)誤差協(xié)方差陣Pk,k-1將趨于常數(shù)矩P-,它們之間關(guān)系為

P-=φP+φT+ΓQΓT，

(5)

K=P-HT(HP-HT+R)-1，

(6)

P+=(I-KH)P-，

(7)

(8)

式中:rk為預(yù)報(bào)殘差,通常稱為新息,描述了濾波器的觀測(cè)量的估計(jì)值與實(shí)際觀測(cè)量之間的差值。

1.2 改進(jìn)的IAE卡爾曼濾波算法

以新息方式列寫卡爾曼濾波算法,狀態(tài)的一步預(yù)測(cè)為

(9)

狀態(tài)估計(jì)

(10)

濾波增益矩陣

(11)

一步預(yù)測(cè)誤差方差陣

(12)

誤差估計(jì)方差陣

Pk=(I-KkHk)Pk,k-1.

(13)

濾波器的新息序列rk狀態(tài)和其新息方差Crk的表達(dá)式為

(14)

(15)

(16)

基于零均值白色濾波新息更新序列,濾波的統(tǒng)計(jì)新息矩陣的更新方程為

(17)

(18)

式中:Δxk=Kkrk,將式(15)帶入式(11),即得改進(jìn)的IAE自適應(yīng)卡爾曼濾波增益的計(jì)算公式為

(19)

圖1 IAE卡爾曼濾波算法流程框圖

2 GPS/DR組合導(dǎo)航模型

本算法將GPS/DR信息進(jìn)行集中處理,運(yùn)動(dòng)模型為當(dāng)前統(tǒng)計(jì)模型。即當(dāng)前機(jī)動(dòng)目標(biāo)在以某一加速度機(jī)動(dòng)時(shí),它在下一瞬間的加速度取值是有限的,并且在當(dāng)前加速度的領(lǐng)域內(nèi)呈現(xiàn)某種統(tǒng)計(jì)分布。狀態(tài)量一般選取位置信息或位置差信息、速度或速度差信息、加速度或加速度差信息,便于直觀觀測(cè)運(yùn)動(dòng)狀態(tài)的變化。DR系統(tǒng)一般選取陀螺儀和里程計(jì)作為航位推算部分,考慮在狀態(tài)量中加入DR設(shè)備量,且知道陀螺和里程輸出均為單位時(shí)間內(nèi)變化量,所以考慮加入陀螺和里程 誤差量作為狀態(tài)量的一部分。

系統(tǒng)的狀態(tài)量選為

將GPS接收機(jī)輸出的定位信息er、en以及陀螺儀的輸出θ和里程計(jì)的輸出s作為外部觀測(cè)量。

當(dāng)前時(shí)刻狀態(tài)量與前一時(shí)刻狀態(tài)量及當(dāng)前時(shí)刻觀測(cè)量之間的關(guān)系表示為

X(k)=Φ(k,k-1)X(k-1)+μk,

(20)

Z(k)=H(k)X(k)+εk.

(21)

H[X(k,k-1)]=

3 仿真結(jié)果

對(duì)改進(jìn)的新息自適應(yīng)卡爾曼濾波算法進(jìn)行仿真驗(yàn)證,設(shè)定仿真條件為:接收機(jī)三軸定位誤差均為20 m, 接收機(jī)量測(cè)誤差方差為10 m, 接收機(jī)采樣周期為1 s,陀螺零漂為0.05(°)/h,里程計(jì)零漂0.05,東向和北向機(jī)動(dòng)加速度誤差方差1.5 m/s2,誤差相關(guān)時(shí)間0.1,東向和北向位置誤差相關(guān)時(shí)間設(shè)為0.01,微機(jī)械陀螺誤差和里程計(jì)誤差相關(guān)時(shí)間均為1 000 s,仿真時(shí)間設(shè)定為2 000 s,陀螺誤差均方差0.1°/h,里程計(jì)誤差均方差2 m.車輛沿著東北方向變加速行駛,初始速度為10 m/s,仿真時(shí)間1 600 s,采樣周期1 s.為觀測(cè)系統(tǒng)對(duì)外部量測(cè)干擾的效果,對(duì)GPS觀測(cè)數(shù)據(jù)進(jìn)行加噪處理,做如下設(shè)定:測(cè)量誤差選擇三種不同強(qiáng)度的誤差,1～300 s取1倍的觀測(cè)誤差,301～600 s取4倍的觀測(cè)誤差,601～900 s取9倍的觀測(cè)誤差,之后量測(cè)誤差恢復(fù)為1倍。常規(guī)的卡爾曼濾波算法模型始終采取預(yù)先設(shè)定的量測(cè)誤差。改進(jìn)的自適應(yīng)卡爾曼濾波采取有限記憶的滑動(dòng)新息方差,數(shù)據(jù)窗口大小N=20,以改進(jìn)卡爾曼增益K的計(jì)算。濾波效果如圖2～圖4和表1所示。

圖2 一般卡爾曼濾波位置誤差與觀測(cè)誤差

圖3 改進(jìn)IAE卡爾曼濾波位置誤差與觀測(cè)誤差

圖4 聯(lián)邦卡爾曼濾波與改進(jìn)的IAE濾波

表1新息自適應(yīng)卡爾曼濾波與聯(lián)邦卡爾曼濾波算法位置誤差統(tǒng)計(jì)特性

與參考軌跡的誤差范圍/m新息自適應(yīng)卡爾曼濾波聯(lián)邦卡爾曼濾波 小于1050.12%17.26% 小于2081.55%32.96% 小于3094.06%56.54% 小于40100%62.32% 小于50100%76.89%

4 結(jié)束語

在系統(tǒng)量測(cè)噪聲較大時(shí),改進(jìn)的新息卡爾曼濾波算法仍能保持較好的濾波效果,而傳統(tǒng)卡爾曼濾波算法則隨量測(cè)噪聲的變化出現(xiàn)了較大的發(fā)散,證明改進(jìn)的新息自適應(yīng)卡爾曼濾波算法能根據(jù)外界噪聲變化自適應(yīng)的調(diào)整濾波,在系統(tǒng)噪聲增強(qiáng)時(shí)候可以保持優(yōu)于聯(lián)邦卡爾曼濾波的定位精度及穩(wěn)定性。

[1]李 旦.組合導(dǎo)航自適應(yīng)卡爾曼濾波改進(jìn)算法研究[J].測(cè)控技術(shù),2011,3:114.

[2]卞鴻巍，李 安，覃方君，等.現(xiàn)代信息融合技術(shù)在組合導(dǎo)航中的應(yīng)用[M].北京:國(guó)防工業(yè)出版社.2010.

[3]MOHAMED A H, SCHWARZ K P. Adaptive Kalman filtering for INS/GPS[J]. Journal of Geodesy, 1999, 73(4):193-203.

[4]LOEBIS D, SUTTON R, CHUDLEY J,etal. Adaptive tuning of a Kalman filter via fuzzy logic for an intelligent AUV navigation system[J]. Control Engineering Practice, 2004, 12(12):1531-1539.

[5]卞鴻巍,楊艷娟，金志華，等.基于GPS姿態(tài)量測(cè)系統(tǒng)的綜合導(dǎo)航系統(tǒng)研究[M].彈箭與制導(dǎo)學(xué)報(bào),2003,23(5):8-11.