基于李群的無跡卡爾曼濾波視覺SLAM算法

黃秀珍，伍一帆，李凱濤

(1.浙江理工大學 科技與藝術學院，浙江 紹興312369；2.杭州中威電子股份有限公司，浙江 杭州310000)

0 引言

如今，機器人的應用空間已走出實驗室面向各個領域，小到日常生活中的家用吸塵器、快遞分揀機等，大到國家領域中的深海采礦集礦機、無人機偵察等[1]。

在移動機器人領域，根據給定的環境地圖有效確定所處位置是一項基本任務，但在缺少地圖的情況下，機器人將無法進行自主定位[2]。機器人可以通過攜帶傳感器來獲取地圖以及估計自身位置和姿態。最常見的方法是結合衛星信號完成位姿估計，但是衛星導航容易受到環境的影響，出現較大誤差。而機器視覺SLAM算法可以搭載視覺傳感器的主體，在沒有環境先驗信息的情況下，在運動過程中建立環境的模型，同時估計自己的運動[3]。因此視覺SLAM算法具有很高的研究價值，也是目前機器人視覺的研究熱點。

基于濾波器的后端視覺SLAM定位算法最早出現于2002年，Davison等人[4]開始著手研究完整的視覺SLAM定位系統，實現了迭代擴展卡爾曼濾波(IEKF)的實時視覺SLAM系統，該研究首次建立了包含預測和噪聲模型的視覺SLAM系統?，F在人們以傳統卡爾曼濾波(KF)算法為基礎，涌現出大批基于擴展卡爾曼濾波、無跡卡爾曼濾波(UKF)和粒子濾波的位姿估計算法[5-6]。G.Loianno等人提出一個基于李群SE (3)的 UKF的視覺慣性里程計系統，以獲得一個獨特的、無奇異性的剛體姿態表示(SE(3)-UKF算法)[7]。Martin Brossard等人把視覺慣性和基于李群的無跡卡爾曼算法相結合[8],提出了L-UKF-LG算法和R-UKF-LG算法。本文以文獻[8]的算法為基礎，把UKF的系統狀態用李群表示，并構建視覺慣性緊耦合模型，對文獻[8]的算法進行改進，并在Matlab環境下對傳統UKF算法、傳統IEKF算法、SE(3)-UKF算法、L-UKF-LG算法和R-UKF-LG算法進行仿真分析。

1 UKF-LG視覺SLAM算法設計

本次仿真實驗算法優化了傳統UKF的系統狀態，采用了UKF-LG視覺SLAM算法。算法通過視覺傳感器對數量固定的三維路標進行觀測，設定路標不隨機器人的移動而產生變化，并采用慣性傳感器(IMU)觀測值和視覺測量值融合的方法解決了視覺慣導工作頻率不一致的問題，構建視覺慣性緊耦合模型[9]。下面將分別介紹視覺慣性緊耦合模型、系統狀態模型以及UKF-LG視覺SLAM算法流程。

1.1 視覺慣性緊耦合模型

視覺慣性緊耦合模型如圖1所示，IMU提供機器人當前時刻的線加速度和角加速度信息，相機提供路標點的觀測信息。系統狀態模型中的位姿信息需要對IMU數據計算得到的估計結果以及相機的觀測結果，并通過濾波器來得到最優的估計結果，UKF-LG算法對應于圖中的“濾波器”部分。另外，IMU輸出信息的速度和相機輸出圖像信息的速度并不是保持一致的，為了解決這一問題，設定IMU的信息輸入周期為濾波周期[10]。如果當前時刻只有IMU信息輸入，則緊耦合模型僅使用IMU的數據進行狀態估計和更新；如果圖像信息和IMU數據一起輸入，則緊耦合模型將兩個信息融合進行濾波，估計系統狀態并更新。

圖1 視覺慣性緊耦合框架Fig.1 Visual inertial tight coupling frame

1.2 系統狀態模型

對于一個裝載了IMU和視覺傳感器的機器人，想要得到它的位姿信息，就需要估計它的旋轉矩陣R、速度v、位移x、IMU偏差bk=[wk,b,ak,b]，以及空間中的n個路標在世界坐標系下的位置信息y=[y1,y2,…,yn][11]，具體的系統狀態模型為：

(1)

式中，旋轉矩陣R(k)表示從IMU坐標系到世界坐標系的變化，wk,i和ak,i表示k時刻IMU輸出的角速度和線加速度，gG表示當前位置的重力加速度，dt為測量周期。式(1)中的誤差均設定為方差為0的高斯白噪聲：

n=[nw,na,nbw,naw]～N(0,Q)。

(2)

對于式(1)中的狀態量，可以聯合起來構成特殊歐式群矩陣Xk∈SE(3)，特殊歐式群是一種李群，可以用來描述三維空間的旋轉和平移，Xk的表示形式為：

(3)

特殊歐式群矩陣Xk不能進行歐式加法運算，只能采用指數映射的計算方式，而IMU偏差bk滿足歐式加法運算，所以把這兩項寫出分塊矩陣的形式，系統的狀態方程表示為：

[Xk,bk]=f(Xk-1,Uk-bk-1,n),

(4)

式中，Uk=[wk,b,ak,b]為k時刻IMU測量到的角速度和線加速度。

觀測模型用來描述k時刻機器人自身位姿與觀測到的路標之間的關系，所以只有在視覺傳感器檢測到了路標后才有觀測方程[12]，k時刻的觀測方程中存放的是當前時刻所有特征點的空間信息。下面給出系統的觀測模型，對環境中路標的觀測值為Z(k)，觀測噪聲為ny：

(5)

式中，K為相機的內參矩陣；RC和tc分別表示從IMU 坐標系到相機坐標系的旋轉矩陣和平移量。

1.3 UKF-LG視覺SLAM算法流程

下面給出UKF-LG視覺SLAM算法流程。

1.3.1 預測階段

① 首先計算在李代數空間上2k+1維(k的取值由[Xk,bk]的維數確定)的sigma點的分布，sigma點的計算公式如下：

(6)

取λ=3-k，計算權值：

(7)

(8)

③ 求協方差矩陣P-，具體步驟為：

(9)

1.3.2 觀測更新階段

① 根據得到的協方差矩陣，重新計算sigma點：

(10)

(11)

(12)

1.3.3 濾波更新階段

① 計算卡爾曼增益Kk：

(13)

(14)

2 UKF-LG視覺SLAM算法Maltab實現

2.1 初始參數說明

本次仿真使用的代碼使用Euroc數據集[14]，在Matlab2018b環境下仿真。對傳統UKF算法、傳統IEKF算法[15]、SE(3)-UKF算法、L-UKF-LG算法和R-UKF-LG算法進行對比仿真以驗證算法準確度，視覺傳感器的采集頻率為20 Hz，IMU的采集頻率為200 Hz。相機的校準矩陣如下：

(15)

2.2 算法模塊框圖

算法的整體流程分為四大模塊：環境搭建、濾波法初始化、進行定位算法以及計算并顯示誤差。四大模板塊的具體內容如下：

① 環境搭建：設置了相機的采樣頻率和IMU采樣頻率；讀取數據集中通過相機拍攝的圖片、相機位姿的真實值以及IMU數據。

② 濾波器初始化：協方差矩陣的初始化以及過程噪聲和測量噪聲的設置；系統狀態的初始值以及相機的參數設置。

③ 進行定位算法：前文提到的幾種濾波法的預測階段、更新階段、系統狀態估計值的保存以及特征點集的更新。

④ 計算并顯示誤差：計算5種濾波法對機器人位置和姿態估計的標準誤差(RMSE)，并且繪制出曲線。主函數的流程圖如圖2所示。

圖2 主函數流程圖Fig.2 Main function flow chart

圖2中5個濾波法預測階段的代碼由**Propagation.m實現，其中的**表示對應的濾波法名稱(比如L-UKF的m文件是lUKFPropagation)；特征點檢測的代碼由ObserveLandmarks.m函數實現；狀態更新階段的代碼和預測階段類似，由**Update.m函數實現；記錄估計的姿態由UpdateTraj.m函數賦值給各個濾波法的狀態矩陣；特征點的補充由manageAmers.m函數運用線性三角形法實現。

3 仿真結果分析

為了比較5種濾波法在估計位姿上的性能，以5種濾波法對位置和姿態估計的均方根誤差(RMSE)作為評價指標。數據集以微型飛行器作為載具，并配置了視覺慣性傳感器在Vicon Room環境下采集數據，選擇V1_02_medium數據集中的cam0進行仿真。

圖3為在cam0數據集下5種濾波法在各個時刻對無人機的空間位置估計結果的誤差，可以看處幾種濾波器的位置誤差差距并不是很明顯。從圖中可以看出，在整個時刻里，本文算法對于位置的估計要低于傳統UKF算法，特別是在40～48 s后無人機發生大幅度的旋轉(從數據集拍攝的視頻可知)后，L-UKF-LG比其他濾波法能保持的誤差更低。

圖3 位置估計結果Fig.3 Location estimation results

圖4為5種濾波法在cam0數據集下的姿態估計結果的誤差。UKF花費了一定的時間才收斂在RMSE=1附近；40～48 s后無人機發生的大幅度旋轉對R-UKF-LG算法和IEKF算法的姿態比較結果RMSE產生了影響，數值明顯上升；SE(3)-UKF雖然使用了李群來表示狀態信息，但是沒有使用IMU來提供包括角速度在內的各種關于位姿的物理量信息，導致產生的誤差一直累計，所以它的姿態RMSE值隨著時間上升。相對于位置的比較結果而言，L-UKF-LG算法的姿態RMSE誤差比較小，特別是30 s以后，能很好地保持在0.6以內，收斂性比較好。

圖4 姿態估計結果Fig.4 Attitude estimation results

4 結束語

本文實現了UKF-LG視覺SLAM算法，并構建視覺慣性緊耦合模型，在Matlab環境下實現UKF-LG算法,給出了5種濾波器在兩個Euroc數據集中的仿真結果對比圖。以RMSE為判斷標準，分析了濾波器的定位性能，得出L-UKF-LG相對于傳統的UKF算法，有更低的位置和姿態的RMSE，以及更好的定位精確性。本文中UKF對移動機器人的定位雖然取得了一定效果，但仍有很多的情況沒有考慮，在狀態向量初始值等參數的選擇問題上還需要進一步研究和測試，找到能使算法性能達到最優或接近最優的參數組合。