強(qiáng)跟蹤卡爾曼濾波器的研究與應(yīng)用

劉昱鑫,周 鑫,曹玉波*

(1.吉林化工學(xué)院 信息與控制工程學(xué)院,吉林 吉林132022;2.浙江中控技術(shù)股份有限公司 重點項目工程部,江蘇 無錫 214026)

隨著科技與信息化飛速發(fā)展,無人機(jī)搭載傳感器數(shù)量越來越多,對其精確度的要求也越來越高。然而,無人機(jī)上搭載眾多電磁設(shè)備會相互干擾,對無人機(jī)飛行造成影響。為解決上述問題,需要對干擾信號進(jìn)行濾波處理[1]。無人機(jī)常用的濾波算法有互補(bǔ)濾波、卡爾曼濾波[2]。其中,互補(bǔ)濾波相對容易實現(xiàn),計算成本低,但其估計精度略低于卡爾曼濾波的估計精度[3]。

卡爾曼濾波是由魯?shù)婪蚩柭?等人在1960年提出的用于克服維納濾波缺點的一種最優(yōu)線性狀態(tài)估計方法[4]。卡爾曼濾波能夠利用有限的,不直接的,包含噪聲的測量信息去估計那些缺失的信息[5]。強(qiáng)跟蹤濾波是1990年由清華大學(xué)周東華教授提出的用于非線性系統(tǒng)的新型濾波器。強(qiáng)跟蹤濾波有更好的模型參數(shù)失配的魯棒性,對于狀態(tài)突變有很強(qiáng)的跟蹤能力[6]。本研究設(shè)計一種改進(jìn)的濾波算法,在傳統(tǒng)卡爾曼濾波的基礎(chǔ)上引入強(qiáng)跟蹤的思想,提高濾波的準(zhǔn)確性與快速性。對無人機(jī)自駕儀估計的速度信號進(jìn)行濾波驗證,結(jié)果表明相比于卡爾曼濾波,強(qiáng)跟蹤卡爾曼濾波響應(yīng)更快,超調(diào)更小,精度更高。

1 離散卡爾曼濾波算法

卡爾曼濾波是一種利用線性系統(tǒng)的狀態(tài)方程,通過系統(tǒng)輸入與輸出數(shù)據(jù),對系統(tǒng)狀態(tài)進(jìn)行最優(yōu)估計的算法。卡爾曼濾波根據(jù)“預(yù)測—實測—修正”的順序進(jìn)行遞推[7],通過迭代算法獲得被測信號的估計值。在迭代的過程中削減了過程噪聲與測量噪聲,因此卡爾曼濾波器可以對被測信號進(jìn)行精確估計[8]。

離散卡爾曼濾波算法分為兩步,一步是預(yù)測;一步是更新。在一個濾波周期內(nèi),根據(jù)上一時刻的后驗估計與誤差協(xié)方差,對這一時刻的先驗估計與先驗誤差的協(xié)方差進(jìn)行預(yù)測,并計算出這一時刻的卡爾曼增益與后驗估計,最后更新誤差的協(xié)方差,用于下一個濾波周期的計算。

1)預(yù)測

(1)

(2)

2)更新

(3)

(4)

(5)

其中,A為狀態(tài)轉(zhuǎn)移矩陣;C為觀測矩陣。Q與R分別是過程噪聲的協(xié)方差矩陣和測量噪聲的協(xié)方差矩陣。

對于未知的信號s(t),在t+Δt時刻按泰勒公式展開成二階,

(6)

對式(6)求一階導(dǎo)數(shù)和二階導(dǎo)數(shù),

(7)

(8)

式中,Δt為采樣時間;o為泰勒展開余項。得到卡爾曼濾波系統(tǒng)狀態(tài)空間模型。

(9)

2 強(qiáng)跟蹤卡爾曼濾波

對于傳統(tǒng)的離散卡爾曼濾波器算法而言,迭代過程過度依賴過去的數(shù)據(jù)。在測量信號發(fā)生較大變化時,會導(dǎo)致系統(tǒng)累計誤差增加,從而使濾波信號產(chǎn)生滯后。為了使傳統(tǒng)的離散卡爾曼濾波器更好地適應(yīng)測量信號變化較大的情況,引入強(qiáng)跟蹤濾波的思想。在預(yù)測先驗誤差的協(xié)方差時引入漸消因子,減弱過去數(shù)據(jù)對當(dāng)前濾波值的影響,從而減小系統(tǒng)的累計誤差。通過調(diào)整誤差協(xié)方差矩陣及卡爾曼增益矩陣,促使濾波殘差序列保持互相正交,從而提升濾波器的快速性[9]。

加入漸消因子的先驗誤差協(xié)方差的預(yù)測過程如下所示。

(10)

其中,λ為對角矩陣,主對角線元素λi,k(i=1,2,3)計算式為

(11)

(12)

式中Nk和Mk分別定義為

(13)

(14)

(15)

(16)

即保證不同時刻的殘差序列相互正交。當(dāng)系統(tǒng)穩(wěn)定時,式(16)自然成立,此時強(qiáng)跟蹤卡爾曼濾波退化為傳統(tǒng)的卡爾曼濾波。

3 強(qiáng)跟蹤卡爾曼濾波器的實現(xiàn)與應(yīng)用

強(qiáng)跟蹤卡爾曼濾波器算法流程如圖1所示,在一個濾波周期內(nèi),首先對該時刻的先驗估計與新息協(xié)方差進(jìn)行預(yù)測。通過新息協(xié)方差矩陣計算出衰減因子,從而得到誤差協(xié)方差。計算出該時刻的卡爾曼增益與后驗估計,最后更新誤差的協(xié)方差,用于下一個濾波周期的先驗估計與新息協(xié)方差的計算。

圖1 強(qiáng)跟蹤卡爾曼濾波器算法流程圖

為了檢驗強(qiáng)跟蹤卡爾曼濾波器的濾波性能,在室外空曠場地對無人機(jī)進(jìn)行定點模式飛行。分別使用卡爾曼濾波器、強(qiáng)跟蹤卡爾曼濾波器對無人機(jī)自駕儀中估計出來的速度信號進(jìn)行濾波處理。實驗參數(shù)設(shè)置如表1所示。

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

使用外部傳感器估計的速度信號作為實際的速度信號。以水平側(cè)向通道為例,水平側(cè)向通道實際速度與濾波后速度對比圖如圖2所示。

時間/s圖2 濾波效果對比圖

由圖2可知,輸入信號經(jīng)過濾波器處理后,大部分的噪聲信號被濾除。在10 s到30 s之間輸入信號發(fā)生小幅度的改變,相比之下,強(qiáng)跟蹤卡爾曼濾波輸出的結(jié)果超調(diào)更小。

在30 s到37 s之間,輸入信號處于平穩(wěn)階段,強(qiáng)跟蹤卡爾曼濾波退化為傳統(tǒng)的卡爾曼濾波,兩者濾波效果相同;在37 s到60 s之間,輸入信號發(fā)生較大變化,卡爾曼濾波有明顯的滯后。而強(qiáng)跟蹤卡爾曼濾波由于引入漸消因子,減弱了陳舊數(shù)據(jù)對當(dāng)前數(shù)據(jù)的影響,擺脫了由累計誤差產(chǎn)生的束縛,使得濾波的快速性得到提升,對實際速度信號響應(yīng)得也更快,兩種濾波器的均方根誤差對比如表2所示。

表2 兩種濾波器均方根誤差

從表2可以看出強(qiáng)跟蹤卡爾曼濾波均方誤差遠(yuǎn)小于卡爾曼濾波均方誤差,說明強(qiáng)跟蹤卡爾曼濾波精度更高。

4 結(jié) 論

卡爾曼濾波器是目前應(yīng)用最廣泛的狀態(tài)估計方法之一,常常被用于通信、雷達(dá)、導(dǎo)航、自動控制等領(lǐng)域。本研究針對無人機(jī)速度估計存在噪聲問題,設(shè)計一種對未知信號進(jìn)行估計的離散強(qiáng)跟蹤卡爾曼濾波算法。引入漸消因子對傳統(tǒng)卡爾曼濾波器進(jìn)行改進(jìn),加快濾波器的響應(yīng)速度。通過控制無人機(jī)定點飛行,得到無人機(jī)估計的速度信號,并對濾波算法進(jìn)行驗證。結(jié)果表明強(qiáng)跟蹤卡爾曼濾波提高了濾波器的精度和實時性。