基于改進(jìn)容積卡爾曼濾波的慣性/光流組合自主測速方法

閆寶龍，趙東花，劉曉杰，吳新冬，閆德利，王晨光，申 沖

(1. 中北大學(xué)儀器科學(xué)與動態(tài)測試教育部重點實驗室，太原 030051；2. 中北大學(xué)儀器與電子學(xué)院，太原 030051；3. 中北大學(xué)信息與通信工程學(xué)院，太原 030051)

0 引言

隨著無人機、無人車等無人運動平臺的發(fā)展，具有自主導(dǎo)航能力的慣性導(dǎo)航系統(tǒng)(Inertial Navigation System，INS)以及高精度的全球定位系統(tǒng)(Global Positioning System，GPS)等逐漸發(fā)展成為研究熱點。但是，在復(fù)雜環(huán)境下，GPS信號易受干擾，INS單獨工作誤差易隨時間積累，而利用視覺傳感器進(jìn)行導(dǎo)航參數(shù)測量不存在誤差隨時間漂移的問題，且具有體積小、成本低、自主性強等優(yōu)點。因此，視覺/慣性組合導(dǎo)航可以校正INS因積分造成的組合導(dǎo)航誤差，逐漸成為研究熱點[1]。

N.Gageilk[2]和D.Honegger[3]等利用光流傳感器完成了無人機的定點懸停。2015年，M.Bloesch等提出了魯棒視覺慣性里程計[4],該算法將視覺信息與慣性測量信息進(jìn)行緊耦合，在保持精度的同時減少了計算量。文獻(xiàn)[5]使用雙輸入光流估計網(wǎng)絡(luò)(FlowNetCorr)[6]的卷積層部分實現(xiàn)視覺特征提取，再使用慣性信息與圖像的融合特征進(jìn)行姿態(tài)解算，能夠提升里程計的解算精度，但由于FlowNetCorr層數(shù)較多，訓(xùn)練成本較大，不適合包含深度信息的全導(dǎo)航參數(shù)估計。文獻(xiàn)[7]運用Lucas-Kanade(LK)算法和塊匹配算法，從兩幀圖像中得到圖像光流信息，再與INS的輸出信息和高度計輸出信息進(jìn)行融合，運用卡爾曼濾波(Kalman Filter，KF)得出無人機在慣性坐標(biāo)系下的速度。文獻(xiàn)[8]提出了一種光流法和改進(jìn)ORB特征點匹配算法融合的單目視覺里程計方法，利用光流和改進(jìn)ORB基于閾值循環(huán)運行，相較于傳統(tǒng)的光流視覺里程計，精度得到了提高。文獻(xiàn)[9]基于Elman神經(jīng)網(wǎng)絡(luò)預(yù)測速度，在環(huán)境適宜的情況下，在線訓(xùn)練神經(jīng)網(wǎng)絡(luò)模型。同時，使用基于INS動態(tài)誤差模型的卡爾曼濾波器，通過融合INS及速度數(shù)據(jù)得到誤差向量，從而對導(dǎo)航參數(shù)進(jìn)行補償修正，可在一定時間內(nèi)得到較為準(zhǔn)確的位置和姿態(tài)。

目前，KF已經(jīng)廣泛應(yīng)用于導(dǎo)航定位及運動控制中[10]。由于KF只能應(yīng)用于線性系統(tǒng)，而實際中所有的運動系統(tǒng)幾乎都是非線性的，因此通過對非線性方程進(jìn)行線性化來處理各種非線性問題，提出了擴(kuò)展卡爾曼濾波器(Extended Kalman Filter，EKF)。由于EKF使用一階泰勒展開近似非線性方程，引入了高階項截斷誤差，因此只適用于弱非線性系統(tǒng)[11]。容積卡爾曼濾波器(Cubature Kalman Filter，CKF)基于三階球面徑向容積準(zhǔn)則，構(gòu)造2n個相同權(quán)值的容積點，經(jīng)過非線性函數(shù)轉(zhuǎn)換后給出下一時刻系統(tǒng)狀態(tài)的預(yù)測，無需對非線性系統(tǒng)進(jìn)行線性化就可以適用于非線性運動系統(tǒng)[12]。

由于載體各個系統(tǒng)之間的數(shù)據(jù)輸出頻率不同，若直接使用CKF方法，將導(dǎo)致各個系統(tǒng)數(shù)據(jù)在數(shù)據(jù)融合時由于頻率不一致造成融合精度不高。此外，在純慣性導(dǎo)航系統(tǒng)或者純光流測速系統(tǒng)長時間單獨運行時，測速會因為累積誤差逐漸增大而發(fā)散。因此，本文采用一種基于多速率殘差校正的CKF方法對慣導(dǎo)以及光流數(shù)據(jù)進(jìn)行數(shù)據(jù)融合。該方法首先使用金字塔LK光流算法得到兩個圖像之間的運動信息，再運用多速率CKF融合INS數(shù)據(jù)與光流數(shù)據(jù)，對系統(tǒng)速度進(jìn)行估計。具體為當(dāng)光流數(shù)據(jù)與INS數(shù)據(jù)同時存在時，進(jìn)行殘差和狀態(tài)估計誤差的計算；在光流數(shù)據(jù)的采樣間隔，即只有INS數(shù)據(jù)時，利用上一時刻的狀態(tài)估計誤差對當(dāng)前時刻的狀態(tài)估計誤差進(jìn)行迭代計算，并用當(dāng)前時刻的狀態(tài)誤差估計系統(tǒng)殘差，再利用殘差對當(dāng)前時刻的狀態(tài)估計量進(jìn)行補償，以提高融合系統(tǒng)的速度測量精度。

1 改進(jìn)CKF的慣性/光流測速原理

由于在光流數(shù)據(jù)的采樣間隔無法對系統(tǒng)狀態(tài)量進(jìn)行補償，因此為了提高慣性/光流組合導(dǎo)航系統(tǒng)的測速精度，本文使用改進(jìn)CKF，即多速率殘差CKF對INS數(shù)據(jù)和光流傳感器的數(shù)據(jù)進(jìn)行融合。把載體在導(dǎo)航坐標(biāo)系的速度作為狀態(tài)量，使用光流傳感器的輸出作為觀測量估計載體速度信息，具體算法過程如下：

1)時間更新。容積點ξi及權(quán)值wi如式(1)所示

(1)

其中，I表示n階單位陣，i=1,2,…，2n，下述公式中i同義。假設(shè)k-1時刻的狀態(tài)初值及其協(xié)方差陣為Xk-1和Pk-1，以狀態(tài)協(xié)方差陣的平方根Sk-1代替協(xié)方差陣進(jìn)行時間和觀測更新，其中容積點的更新為

(2)

其中，f(·)為已知函數(shù)。

(3)

其中，Qk是量測噪聲協(xié)方差。

2)觀測更新。對容積點Xi,k|k-1進(jìn)行預(yù)測更新，如式(4)所示

(4)

經(jīng)過容積點傳播計算得到的觀測預(yù)測值Zi,k|k-1表達(dá)式如式(5)所示

(5)

其中，H是系統(tǒng)量測方程；vk-1是服從高斯分布，期望為0且協(xié)方差為R的高斯白噪聲。

狀態(tài)方程與觀測方程的互協(xié)方差陣及增益矩陣如式(6)所示

(6)

其中，濾波器的設(shè)計過程如下:

狀態(tài)方程為

Xk+1=AXk+Bkμk+ωk

(7)

觀測方程為

Yk=g(Xk)+vk

(8)

多速率殘差校正的濾波方法不同于低融合度濾波法，低融合度濾波法利用數(shù)據(jù)的時間戳進(jìn)行同一時刻的數(shù)據(jù)判定，并對同一時刻的數(shù)據(jù)進(jìn)行融合;而多速率殘差校正的濾波方法可以在光流系統(tǒng)解算速度的同時對光流狀態(tài)誤差和殘差進(jìn)行計算，若不存在光流系統(tǒng)解算速度時，可根據(jù)上一時刻的狀態(tài)估計誤差對當(dāng)前時刻狀態(tài)量進(jìn)行估計，并用當(dāng)前時刻誤差估計殘差。其系統(tǒng)的狀態(tài)估計誤差如式(9)，殘差如式(10)所示

(9)

(10)

(11)

(12)

其中，Φk-1是狀態(tài)轉(zhuǎn)移矩陣。將式(10)展開，并代入式(9)，得

σk=HkΦk-1ek-1

(13)

根據(jù)狀態(tài)估計誤差和殘差的關(guān)系,可得

ek≈Φk-1ek-1

(14)

由于光流輸出數(shù)據(jù)的間隔，濾波增益非常小，因此狀態(tài)最優(yōu)估計為

(15)

2 實驗驗證

為了驗證本文提出的基于多速率殘差校正CKF的慣性/光流組合測速方法的有效性，采用搭載有光流測速相機裝置的無人機進(jìn)行機載實驗驗證，見圖1。速度基準(zhǔn)(BASE)由SPAN-KVH1750分體式閉環(huán)光纖慣性/衛(wèi)星組合導(dǎo)航系統(tǒng)提供，速度測量精度為0.03m/s，飛行軌跡見圖2。實驗設(shè)備如表1所示。無人機在300m高空進(jìn)行多組實驗，試驗結(jié)果如圖3～圖6所示。最后分別使用EKF算法、基于多速率殘差校正的擴(kuò)展卡爾曼算法(EKF-ERC)、CKF算法及基于多速率殘差校正的容積卡爾曼算法(CKF-ERC)進(jìn)行比較，試驗結(jié)果如表2所示。

表1 實驗設(shè)備參數(shù)表

圖1 機載實驗裝置圖

圖2 飛行軌跡圖

圖3 機載北向速度測試數(shù)據(jù)

圖4 機載北向速度誤差曲線圖

圖5 機載東向速度測試數(shù)據(jù)圖

圖6 機載東向速度誤差曲線圖

表2 各算法解算誤差的方差與均方差

由圖3～圖6可知，無人機飛行時，本文改進(jìn)的CKF算法能夠較好地跟進(jìn)基準(zhǔn)值，而且機載東向速度均方根誤差由EKF的0.4724m/s降低到0.2964m/s，精度提升了37.26%;機載北向速度均方根誤差由EKF的0.2266m/s降低到0.06m/s,精度提升了73.45%。由此可以看出，與現(xiàn)有其他光流測速方法相比，本文提出的改進(jìn)CKF算法的速度測量精度有所提升。

3 結(jié)論與展望

本文針對無人高空飛行時，KF算法在慣性/光流組合測速數(shù)據(jù)融合時出現(xiàn)由于各系統(tǒng)輸出數(shù)據(jù)頻率不一致導(dǎo)致融合精度有限的問題，提出了一種基于多速率殘差校正的改進(jìn)CKF算法來提高慣性/光流組合系統(tǒng)的速度測量精度,并使用無人機搭載實驗進(jìn)行了驗證，結(jié)果表明：

1)使用多速率殘差濾波器改進(jìn)卡爾曼算法，針對光流采樣間隔時，利用殘差和狀態(tài)估計誤差對系統(tǒng)估計量進(jìn)行更新與計算，可以在保證系統(tǒng)輸出速率的同時，提高測速精度。

2)由于無人機的不規(guī)則運動造成速度非線性，EKF算法及其改進(jìn)算法解算精度較差;CKF算法精度較低，但是基于多速率殘差校正的CKF算法仍能夠提升測速精度。

在未來慣導(dǎo)/光流組合測速技術(shù)的研究中，在光流算法前提條件的限制下，對光流算法中的特征點提取等方面進(jìn)行改進(jìn)，并開展光照強弱以及高空飛行時氣流對光流測速平臺影響等研究，進(jìn)一步提高無人機飛行時的測速精度。