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

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

0 引言

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

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

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

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

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

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

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

(1)

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

(2)

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

(3)

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

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

(4)

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

(5)

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

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

(6)

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

狀態(tài)方程為

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

(7)

觀測(cè)方程為

Yk=g(Xk)+vk

(8)

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

(9)

(10)

(11)

(12)

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

σk=HkΦk-1ek-1

(13)

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

ek≈Φk-1ek-1

(14)

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

(15)

2 實(shí)驗(yàn)驗(yàn)證

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

表1 實(shí)驗(yàn)設(shè)備參數(shù)表

圖1 機(jī)載實(shí)驗(yàn)裝置圖

圖2 飛行軌跡圖

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

圖4 機(jī)載北向速度誤差曲線圖

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

圖6 機(jī)載東向速度誤差曲線圖

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

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

3 結(jié)論與展望

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

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

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

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