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

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

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

0 引言

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

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

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

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

1 改進CKF的慣性/光流測速原理

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

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

(1)

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

(2)

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

(3)

其中，Qk是量測噪聲協方差。

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

(4)

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

(5)

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

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

(6)

其中，濾波器的設計過程如下:

狀態方程為

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

(7)

觀測方程為

Yk=g(Xk)+vk

(8)

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

(9)

(10)

(11)

(12)

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

σk=HkΦk-1ek-1

(13)

根據狀態估計誤差和殘差的關系,可得

ek≈Φk-1ek-1

(14)

由于光流輸出數據的間隔，濾波增益非常小，因此狀態最優估計為

(15)

2 實驗驗證

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

表1 實驗設備參數表

圖1 機載實驗裝置圖

圖2 飛行軌跡圖

圖3 機載北向速度測試數據

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

圖5 機載東向速度測試數據圖

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

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

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

3 結論與展望

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

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

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

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