強跟蹤卡爾曼濾波器的研究與應用

劉昱鑫,周 鑫,曹玉波*

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

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

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

1 離散卡爾曼濾波算法

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

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

1)預測

(1)

(2)

2)更新

(3)

(4)

(5)

其中,A為狀態轉移矩陣;C為觀測矩陣。Q與R分別是過程噪聲的協方差矩陣和測量噪聲的協方差矩陣。

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

(6)

對式(6)求一階導數和二階導數,

(7)

(8)

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

(9)

2 強跟蹤卡爾曼濾波

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

加入漸消因子的先驗誤差協方差的預測過程如下所示。

(10)

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

(11)

(12)

式中Nk和Mk分別定義為

(13)

(14)

(15)

(16)

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

3 強跟蹤卡爾曼濾波器的實現與應用

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

圖1 強跟蹤卡爾曼濾波器算法流程圖

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

表1 參數設置

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

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

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

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

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

從表2可以看出強跟蹤卡爾曼濾波均方誤差遠小于卡爾曼濾波均方誤差,說明強跟蹤卡爾曼濾波精度更高。

4 結 論

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