基于遺忘PI自適應(yīng)濾波在車載導(dǎo)航中的應(yīng)用

郭秀靈,孫立功

(河南科技大學(xué)電氣工程學(xué)院,河南 洛陽 471023)

1 引言

隨著導(dǎo)航技術(shù)的不斷發(fā)展,車輛導(dǎo)航定位技術(shù)受到越來越多學(xué)者的關(guān)注。由于高精度的慣導(dǎo)產(chǎn)品價(jià)格高,難以大量應(yīng)用于民用車輛,因此,該領(lǐng)域的學(xué)者們也在不影響導(dǎo)航精度的情況下,通過提出新的導(dǎo)航算法來減少車載導(dǎo)航的成本,為民用車輛使用慣導(dǎo)設(shè)備創(chuàng)造了條件[1]。常規(guī)的卡爾曼濾波算法應(yīng)用于導(dǎo)航系統(tǒng)時(shí),需要精確的數(shù)學(xué)模型以及確切的系統(tǒng)噪聲和量測(cè)噪聲,但在實(shí)際的應(yīng)用中,量測(cè)噪聲和系統(tǒng)噪聲準(zhǔn)確的特征值很難獲得[2]。針對(duì)上述問題,可采用自適應(yīng)的卡爾曼濾波算法(Adaptive Kalman Filter, AKF)的非線性濾波來進(jìn)行估計(jì)出定位信息[3]。

自適應(yīng)卡爾曼濾波算法通過用測(cè)量數(shù)據(jù)來實(shí)現(xiàn)濾波推算,同時(shí),判斷系統(tǒng)的動(dòng)態(tài)是否發(fā)生變化。若動(dòng)態(tài)發(fā)生變化將其看成隨機(jī)干擾歸算到模型及噪聲中去,對(duì)模型參數(shù)和噪聲進(jìn)行估計(jì)和及時(shí)修正來適應(yīng)系統(tǒng)動(dòng)態(tài)的變化。自適應(yīng)卡爾曼濾波方法有多種,包括Sage-Husa自適應(yīng)濾波、自適應(yīng)擴(kuò)展卡爾曼濾波算法、漸消因子濾波[4-6]。其中Sage-Husa自適應(yīng)濾波運(yùn)用自適應(yīng)噪聲協(xié)方差矩陣可以修正一定的噪聲統(tǒng)計(jì)特性,但對(duì)于較高階次的系統(tǒng)時(shí)濾波輸出的結(jié)果易發(fā)散;自適應(yīng)擴(kuò)展卡爾曼濾波算法通過對(duì)新息協(xié)方差的測(cè)量與計(jì)算,對(duì)卡爾曼濾波器的增益矩陣進(jìn)行自適應(yīng)修正,但運(yùn)算過程中存在計(jì)算量大的問題,解算結(jié)果出現(xiàn)發(fā)散的情況;而漸消因子濾波能在一定程度上修正模型誤差引起的系統(tǒng)噪聲。

針對(duì)上述問題,本文提出了一種基于遺忘因子PI自適應(yīng)濾波算法在車載導(dǎo)航中的應(yīng)用。該算法將PI控制器與卡爾曼濾波器結(jié)合,可實(shí)現(xiàn)對(duì)觀測(cè)噪聲協(xié)方差矩陣進(jìn)行在線修正,來提高導(dǎo)航的精度。自適應(yīng)卡爾曼濾波是通過新息序列的協(xié)方差估計(jì)值和理論值構(gòu)造函數(shù)來選取遺忘因子,以此來限制卡爾曼濾波器的記憶長度,調(diào)整新息的權(quán)重,充分利用現(xiàn)時(shí)的觀測(cè)數(shù)據(jù),隨著時(shí)間的推進(jìn),濾波器逐漸消除建模不準(zhǔn)確或量測(cè)噪聲的多變的影響,達(dá)到濾波抑制濾波發(fā)散的目的[7-9]。

2 常規(guī)自適應(yīng)濾波模型建立

自適應(yīng)卡爾曼濾波能對(duì)觀測(cè)噪聲協(xié)方差矩陣進(jìn)行實(shí)時(shí)估計(jì)并修正,從而抑制發(fā)散,實(shí)現(xiàn)快速收斂,并減小誤差。自適應(yīng)濾波在進(jìn)行狀態(tài)估計(jì)時(shí),同時(shí)通過量測(cè)輸出在線實(shí)時(shí)地估計(jì)系統(tǒng)的噪聲參數(shù),但是對(duì)于車載導(dǎo)航系統(tǒng)來說,由于加速度計(jì)易受振動(dòng)以及運(yùn)動(dòng)加速度干擾,觀測(cè)噪聲協(xié)方差矩陣是時(shí)變的,實(shí)現(xiàn)所有的噪聲參數(shù)進(jìn)行估計(jì)是不可能的,因此,假設(shè)量測(cè)噪聲方差陣未知的。

2.1 常規(guī)自適應(yīng)系統(tǒng)模型

導(dǎo)航系統(tǒng)隨機(jī)系統(tǒng)狀態(tài)空間模型,如下

Xk=Φk/k-1Xk-1+Γk/k-1Wk-1

Zk=HkXk+Vk

(1)

其中Xk是n×1維的狀態(tài)向量,Zk是m×1維的量測(cè)向量;Φk/k-1,Γk/k-1,Hk是已知的系統(tǒng)結(jié)構(gòu)參數(shù),分別稱為n×n維的狀態(tài)一步轉(zhuǎn)移矩陣、n×l維的系統(tǒng)噪聲分配矩陣、m×n維的量測(cè)矩陣,Wk-1是l×1維的系統(tǒng)噪聲向量,Vk是m×1維的量測(cè)噪聲向量,兩者都是零均值的高斯白噪聲向量序列,服從正態(tài)分布,且它們之間互不相關(guān),即滿足

(2)

2.2 量測(cè)噪聲方差陣的求解

在自適應(yīng)濾波算法中,量測(cè)噪聲方差陣Rk是未知的。下面關(guān)于Rk的自適應(yīng)估計(jì)的方法計(jì)算[10-14]。

由自適應(yīng)卡爾曼濾波狀態(tài)估計(jì)計(jì)算方程得,k時(shí)刻的實(shí)測(cè)濾波殘差即新息為

(3)

(4)

(5)

2.3 常規(guī)自適應(yīng)的卡爾曼濾波的計(jì)算步驟

由上述分析,可得自適應(yīng)的卡爾曼濾波的公式如下

(6)

3 PI自適應(yīng)卡爾曼濾波算法

該算法對(duì)觀測(cè)噪聲協(xié)方差矩陣進(jìn)行調(diào)整,通過殘差方差的實(shí)際值和理論值的誤差值,利用PI調(diào)節(jié)器來進(jìn)行實(shí)時(shí)調(diào)節(jié)和控制。PI自適應(yīng)卡爾曼濾波算法,通過修改濾波算法公式實(shí)現(xiàn),將時(shí)變觀測(cè)噪聲協(xié)方差矩陣rk來代替Rk[15]。

定義殘差方差的理論值為

(7)

定義殘差方差的實(shí)際值為

Ck=(Zk-Hkk/k-1)

(8)

殘差方差理論值與實(shí)際值之間的差值可以用兩個(gè)矩陣的跡之差來表示,即

Ek=trace(Dk)-trace(Ck)

(9)

自適應(yīng)卡爾曼濾波算法的關(guān)鍵在于求解調(diào)整系數(shù)αk。殘差方差的理論值Dk是隨觀測(cè)噪聲協(xié)方差矩陣Rk逐漸遞增的,因此,通過調(diào)節(jié)Rk來改變殘差方差的理論值,使Ek的值在0附近。

在實(shí)際應(yīng)用中,通過引入PI控制器來更好地控制Ek的值?？刂破鞯恼{(diào)整系數(shù)αk的求解公式如下

(10)

式(10)中,Kp為比例系數(shù),可快速消除誤差,Ki為積分系數(shù),可用于消除靜態(tài)誤差。該P(yáng)I控制算法,為防止積分飽和,可通過引入積分分離來實(shí)現(xiàn)。積分開關(guān)系數(shù)β的設(shè)置如下

(11)

式(11)中,ε為開關(guān)閾值,可根據(jù)實(shí)際情況選定。當(dāng)殘差方差誤差Ek大于閾值時(shí),不使用積分環(huán)節(jié),防止飽和;反之,積分環(huán)節(jié)介入,來消除余差。

自適應(yīng)卡爾曼濾波增益計(jì)算公式如下

(12)

其中,rk=(1+αk)rk-1為第k次迭代時(shí)觀測(cè)噪聲協(xié)方差矩陣的估計(jì)值,αk為觀測(cè)噪聲協(xié)方差矩陣的調(diào)整系數(shù)。由此,可得PI自適應(yīng)的卡爾曼濾波的公式如下

(13)

4 遺忘因子的選取

為減輕濾波過渡收斂的情況,使得狀態(tài)估計(jì)能較好地適應(yīng)新的量測(cè)值變化,引入了遺忘濾波的算法,在濾波過程中修改系統(tǒng)噪聲Qk和量測(cè)噪聲Rk的權(quán)重,來逐漸減小歷史信息的權(quán)重,提高了新信息的權(quán)重,達(dá)到減小濾波器慣性的目的。遺忘濾波在自適應(yīng)系統(tǒng)建模的情況下能進(jìn)一步改善了濾波精度[6-8,12]。

該系統(tǒng)的狀態(tài)空間模型如式(1)所示。對(duì)于一個(gè)有限時(shí)間序列N,噪聲的方差陣變?yōu)?/p>

(14)

通過整理可得,帶由遺忘因子的自適應(yīng)卡爾曼濾波的公式如下

(15)

式(15)中,s為遺忘因子,取值范圍為0

由上述可知,本文采用自適應(yīng)遺忘濾波方法,隨著濾波的進(jìn)行,利用遺忘因子給舊的歷史數(shù)據(jù)越小的加權(quán)系數(shù),降低舊數(shù)據(jù)在濾波中的作用,增大新數(shù)據(jù)的加權(quán)系數(shù),以抑制濾波發(fā)散,提高新數(shù)據(jù)的利用率,從而來改善濾波器的動(dòng)態(tài)性能。

5 試驗(yàn)結(jié)果與分析

為驗(yàn)證具有遺忘PI自適應(yīng)卡爾曼濾波算法的有效性和精確性,通過500秒左右的跑車實(shí)驗(yàn)來進(jìn)行收集數(shù)據(jù)。通過加載該數(shù)據(jù)并通過matlab進(jìn)行驗(yàn)算,分別用常規(guī)的自適應(yīng)濾波算法、PI自適應(yīng)卡爾曼濾波算法、遺忘PI自適應(yīng)濾波算法來進(jìn)行解算,得到的行車軌跡與實(shí)際的行車軌跡(GPS高斯—克呂格投影)對(duì)比,如圖1、2、3所示。

圖1 常規(guī)的自適應(yīng)濾波算法解算軌跡

從圖1中,可以看出常規(guī)的自適應(yīng)濾波算法,在解算的過程中,受到環(huán)境的影響和其它因素的干擾的,解算出的軌跡與實(shí)際的軌跡進(jìn)行對(duì)比,部分誤差較大。為改善上述軌跡發(fā)生的誤差波動(dòng),通過采用PI自適應(yīng)濾波算法來實(shí)現(xiàn),結(jié)果有較好的修正,如圖2所示,但解算出的軌跡與實(shí)際軌跡相比還有誤差,如果繼續(xù)用這些波動(dòng)作為白噪聲,還會(huì)引起大的誤差。因此,PI自適應(yīng)濾波算法通過加入遺忘因子來加權(quán)系數(shù),抑制濾波的發(fā)散,從而有效克服波動(dòng),該算法解算出的軌跡如圖3所示,可以看出解算出的軌跡與實(shí)際軌跡相比,誤差極其小。

圖2 PI自適應(yīng)卡爾曼濾波算法解算軌跡

圖3 遺忘PI自適應(yīng)卡爾曼濾波算法解算軌跡

如圖4～6分別為常規(guī)自適應(yīng)濾波算法、PI自適應(yīng)卡爾曼濾波算法、遺忘PI自適應(yīng)濾波算法在東向、北向上解算出的位置誤差圖。從上面的誤差圖及軌跡圖可以看出,行車150～250秒時(shí)間,位置誤差波動(dòng)較劇烈。通過常規(guī)自適應(yīng)濾波算法解算時(shí),位置誤差波動(dòng)較為頻繁而且幅值較大。PI自適應(yīng)濾波算法解算時(shí),波動(dòng)沒有減少,但誤差的幅值減小了。而遺忘PI自適應(yīng)濾波算法,波動(dòng)減少了,同時(shí)誤差幅值也減小了。由此可得,具有遺忘因子PI自適應(yīng)濾波算法自適應(yīng)卡爾曼濾波算法的解算精度最高,其次是自適應(yīng)卡爾曼濾波算法,最后是常規(guī)自適應(yīng)濾波算法。在實(shí)際應(yīng)用中,隨機(jī)動(dòng)態(tài)系統(tǒng)的結(jié)構(gòu)參數(shù)和噪聲統(tǒng)計(jì)特性參數(shù)的獲取都會(huì)或多或少存在一些誤差,致使卡爾曼濾波的精度降低,嚴(yán)重時(shí)還很可能會(huì)引起濾波發(fā)散,而自適應(yīng)濾波算法能改善部分參數(shù)的影響,但是自適應(yīng)濾波并不是萬能的,在濾波計(jì)算回路不再是簡(jiǎn)單線性的,是一個(gè)較復(fù)雜的非線性系統(tǒng),實(shí)現(xiàn)起來較復(fù)雜,而遺忘PI自適應(yīng)濾波算法是在自適應(yīng)濾波算法的基礎(chǔ)上,通過PI控制實(shí)現(xiàn)在線修正,通過引入遺忘因子減輕過渡收斂癥狀,同時(shí)也減小歷史信息的權(quán)重,相對(duì)而言提高了新信息的權(quán)重,從而減少舊量測(cè)數(shù)據(jù)的影響,對(duì)噪聲矩陣的調(diào)整更加連續(xù),因此該算法濾波效果相對(duì)穩(wěn)定。

圖4 常規(guī)自適應(yīng)濾波解算位置誤差

圖5 PI自適應(yīng)濾波解算位置誤差

圖6 遺忘PI自適應(yīng)濾波解算位置誤差

6 結(jié)論

本文在自適應(yīng)卡爾爾曼濾波的基礎(chǔ)上提出了一種基于具有遺忘PI自適應(yīng)的卡爾曼濾波算法。通過對(duì)量測(cè)噪聲協(xié)方差矩陣進(jìn)行遺忘因子實(shí)時(shí)估計(jì)以及通過PI控制的在線調(diào)整,保證了該算法的精度,增強(qiáng)了其在工程上的應(yīng)用價(jià)值。實(shí)驗(yàn)結(jié)果表明,改進(jìn)后的算法能有效的抑制濾波發(fā)散,比常規(guī)的自適應(yīng)濾波的效果更好,導(dǎo)航的精度有極大提高,同時(shí)系統(tǒng)的穩(wěn)定性也有所提高。車載試驗(yàn)的結(jié)果也驗(yàn)證了該算法的正確性以及實(shí)用性,這也為車載導(dǎo)航的進(jìn)一步提高導(dǎo)航精度的研究提供有利的保證。