董伯麟,柴 旭

(合肥工業(yè)大學 機械工程學院,安徽 合肥 230009)

0 引言

近年來，隨著無人駕駛、虛擬現(xiàn)實等應用的空前發(fā)展，同時定位與地圖構建(SLAM)[1]被認為是實現(xiàn)這些應用的關鍵技術，所以SLAM也逐步從實驗室研究階段邁向市場應用。SLAM可描述為機器人搭載某種傳感器，在無場景信息情況下，在運動過程中進行自主定位同時構建增量式環(huán)境地圖。自1986年提出SLAM概念以來，它一直是機器人領域的熱點問題。

2007年，A.J.Davison等[2]提出的MonoSLAM是第一個實時單目視覺SLAM系統(tǒng)，它以特征點法為前端，以擴展卡爾曼濾波(EKF)為后端，其應用范圍較小，且易遇到特征丟失的情況。Klein等[3]提出的同步跟蹤與建圖(PTAM)實現(xiàn)了定位與建圖的并行化處理，且首次使用非線性優(yōu)化作為后端方案，引入關鍵幀機制，PTAM的缺陷與MonoSLAM類似。ORB-SLAM[4]作為現(xiàn)代較完善易用的系統(tǒng)，它支持多種傳感器模式，使用了三線程模式保證軌跡與地圖的一致性，ORB特征的計算非常耗時，地圖只能滿足定位需求。J.Engle等[5]提出的LSD-SLAM不需計算特征就可構建半稠密地圖，但也繼承了直接法的缺點，對光照敏感，在相機快速運動時易丟失。Forster等[6]提出了半直接法視覺里程計(SVO)，與其他方案相比，速度極快，實時性較好，但無建圖功能，位姿估計存在累積誤差，也不能進行重定位。M.Labbe等[7]提出的RTAB-MAP是深度相機(RGB_D) SLAM中的經(jīng)典方案，它提供了定位和建圖功能。

上述基于視覺傳感器的SLAM方案由于其傳感器和算法原因在實際應用中具有局限性。本文基于多傳感器融合將不同傳感器數(shù)據(jù)融合起來，讓各種傳感器的優(yōu)勢互補，從而提高SLAM系統(tǒng)的精度和魯棒性，設計了基于RGB-D與慣性測量單元(IMU)融合的多傳感器SLAM方案。

1 傳感器工作原理

1.1 IMU傳感器導航工作原理

IMU是一種慣性傳感器，它可利用傳感器特性獲取移動機器人載體自身的運動信息(如位置、姿態(tài)(俯仰角、偏航角、滾動角)、速度等)。由于最近幾年基于視覺慣性里程計(VIO)SLAM技術方案成為SLAM研究的熱點，IMU[8]也開始在組合導航系統(tǒng)中發(fā)揮重要作用。

本文采用的六自由度IMU主要由三軸加速度計和三軸陀螺儀組成。加速度計可測量慣性系統(tǒng)的三軸線加速度，進行一次積分可得到速度，二次積分可計算出位移。陀螺儀可測量系統(tǒng)的三軸角速度，通過一次積分獲得系統(tǒng)轉(zhuǎn)過的角度。圖1為IMU傳感器導航功能示意圖。其中加速度計和陀螺儀測量值都是基于IMU載體坐標系下的數(shù)據(jù)，需通過坐標系變換和積分運算得到在世界坐標系中的位姿。加速度計在慣性導航系統(tǒng)中主要起定位及修正姿態(tài)的作用，而陀螺儀負責姿態(tài)解算及輔助定位。

圖1 IMU傳感器導航功能示意圖

1.2 雙目結(jié)構光RGB-D工作原理

在視覺SLAM中，與基于單目或雙目傳感器的SLAM系統(tǒng)相比，RGB-D由于自身結(jié)構(通過結(jié)構光技術或時間飛行原理)可主動獲取圖像像素的景深信息，沒有單目尺度不確定性和雙目通過視差法計算深度導致計算量復雜的缺陷，RGB-D已廣泛使用在SLAM系統(tǒng)中。目前，RGB-D根據(jù)原理可分為雙目方案、結(jié)構光方案及飛行時間(TOF)方案。本文使用的RGB-D是基于“雙目+結(jié)構光”原理，其繼承了兩種方案的優(yōu)勢。圖2為結(jié)構光成像原理圖。投射器通過對發(fā)出的結(jié)構光編碼投射到物體表面，在其表面形成調(diào)制的光柵三維圖像，攝像機捕獲此信息獲得光柵二維圖像。由于投射器和攝像機在相機中位置固定，光柵顯示的位移與物體表面的深度信息對應，由光柵二維圖像便可構建待測物體的三維輪廓。

圖2 結(jié)構光成像原理圖

圖3為本文采用的RGB-D。RGB攝像頭可直接獲得應用場景的彩色圖像，雙目紅外攝像頭發(fā)射的紅外結(jié)構光遇到物體表面會反射獲得與彩色圖像對應的深度數(shù)據(jù)。與其他RGB-D相比，本文所用相機光學增強系統(tǒng)的作用是：普通雙目方案在室內(nèi)光線較暗或被測對象特征不明顯的場景下魯棒性較差；而結(jié)構光方案雖然適應普通弱光環(huán)境，但在強光下，由于其他紅外線的干擾導致測量效果受損。光學增強系統(tǒng)可主動調(diào)整系統(tǒng)亮度和圖像傳感器的曝光參數(shù)，對光照實現(xiàn)自適應，在室內(nèi)外能很好的識別物體，獲得更多的景深細節(jié)信息，優(yōu)化成像效果。

圖3 雙目結(jié)構光RGB-D

2 IMU-RGBD算法設計

2.1 算法框架圖

圖4為本文算法流程圖。設計了基于擴展卡爾曼濾波的IMU信息與RGB-D特征融合的方式來解決視覺SLAM所存在的問題。由RGB-D得到場景的彩色圖和深度圖，包含特征信息和深度數(shù)據(jù)(Depth)，兩種數(shù)據(jù)對應便可得三維空間信息，也即一個3D點。后續(xù)通過特征提取與匹配及RANSAC算法將錯誤匹配點對刪除，最后由迭代最近點算法(ICP)求出相鄰兩幀3D點間的位姿變信息(旋轉(zhuǎn)矩陣R和平移向量t)。同時，由IMU所測加速度和角速度數(shù)據(jù)，通過預積分使IMU數(shù)據(jù)頻率與相機獲取圖像的頻率一致，之后由數(shù)據(jù)處理計算出IMU的位姿信息(包括位置P、速度v及姿態(tài)四元數(shù)Q)。

圖4 本文算法框架圖

2.2 RGB-D位姿估計

RGB-D采集到的數(shù)據(jù)為空間3D點經(jīng)過相機成像后生成的2D圖像，在綜合評價尺度不變特征變換(SIFT)、加速魯棒特征(SURF)、ORB 3種特征點在相機運動、環(huán)境光照變化及計算量帶來的實時性問題后，選擇了ORB特征點作為檢測算子。

在得到匹配的特征點對后，通過迭代最近點初步估計相機的運動，得到場景的局部結(jié)構。

設pk為空間中一點，pk+1為該點在相機視頻流中下一幀所在位置，那么位姿變換可看作是對于?k，均有pk+1=Rpk+t，構建誤差項ek=pk+1-(Rpk+t)，構建最小二乘法使誤差項ek最小，從而達到3D特征點匹配精度最高的目的，即

(1)

根據(jù)R、t及相機的內(nèi)、外部參數(shù)對RGB圖像和深度圖像進行處理，即可把點云加起來得到點云地圖。但由于視覺相機總會遇到特征丟失的情況(如相機快速運動時，純旋轉(zhuǎn)運動，場景紋理復雜等)導致SVO所估計的相機運動過程不完整和定位不精確，因此，本文在此基礎上加入IMU傳感器進行輔助定位。

2.3 IMU位姿估計

IMU的測量模型為

(2)

IMU作為慣性傳感器用來測量物體的角速度和加速度，對短時間內(nèi)的快速運動很敏感。因此，其工作頻率很高才能進行測量。通常IMU采集數(shù)據(jù)的頻率與RGB-D采集圖像的頻率不一致，本文所用的IMU采集數(shù)據(jù)為100 Hz，而RGB-D采集圖像頻率為30 Hz。為了使相機在某時刻采集的圖像能與此時刻IMU數(shù)據(jù)對應，需將此時間段內(nèi)的IMU數(shù)據(jù)積分，預積分示意圖如圖5所示。根據(jù)IMU預積分理論模型，設i與j為相機在第k、k+1幀圖像時所對應的IMU數(shù)據(jù)時間段，時間間隔為Δt，在已知i時刻的IMU狀態(tài)量的情況下，則有：

(3)

圖5 預積分示意圖

2.4 基于擴展卡爾曼濾波(EKF)的RGB-DSLAM與IMU數(shù)據(jù)融合

IMU慣性導航系統(tǒng)的狀態(tài)誤差是進行(EKF)融合的基礎，在IMU的測量模型中定義系統(tǒng)的誤差狀態(tài)方程為

(4)

定義EKF中的狀態(tài)向量為

x=[p,v,Q,R,t]

(5)

在解算機器人運動狀態(tài)過程中可獲得兩種數(shù)據(jù)：控制數(shù)據(jù)uk記錄自身運動的傳感器數(shù)據(jù)(如IMU)；觀測數(shù)據(jù)zk記錄環(huán)境信息的傳感器數(shù)據(jù)(如相機)。假設系統(tǒng)狀態(tài)滿足馬爾可夫性，采用運動方程和觀測方程來描述系統(tǒng)的運動過程，系統(tǒng)的運動方程和觀測方程定義為

(6)

現(xiàn)將系統(tǒng)的狀態(tài)在k-1時刻進行一階泰勒展開，進行線性化處理，有：

(7)

式中：F為狀態(tài)轉(zhuǎn)移矩陣;H為測量矩陣。

在EKF的預測部分中的測量狀態(tài)向量為

(8)

協(xié)方差矩陣為

(9)

式中Q為過程噪聲。在進行狀態(tài)更新前，定義卡爾曼增益K為

(10)

式中S為測量噪聲矩陣。

在卡爾曼增益下，更新后的系統(tǒng)狀態(tài)向量為

(11)

更新后的系統(tǒng)協(xié)方差矩陣為

(12)

式中E為單位矩陣。

系統(tǒng)的整個運動狀態(tài)過程是根據(jù)運動方程上一個時刻狀態(tài)和當前時刻運動輸入來估計預測當前狀態(tài)。在視覺SLAM中，由于沒有運動輸入，一般是構建重投影誤差進行運動優(yōu)化估計。在多傳感器數(shù)據(jù)融合中加入了其他傳感器數(shù)據(jù)進行聯(lián)合估計，可對系統(tǒng)狀態(tài)進行更好的預測。

3 實驗結(jié)果與分析

本文是在ORB-SLAM2的基礎上融合IMU數(shù)據(jù)，為了驗證本文算法的效果，使用EuRoC數(shù)據(jù)集進行驗證。這個數(shù)據(jù)集是用微型飛行器在兩個不同的房間和工業(yè)廠房中飛行，根據(jù)光照、紋理、運動快慢和運動模糊，將序列分為esay、medium和difficult。采集到的視覺慣性數(shù)據(jù)包括相機圖片及同步的IMU數(shù)據(jù)，高精度外部運動捕捉系統(tǒng)采集到相機真實位姿(groundtruth)。

為了評估本文算法的定位精度和運行效率，在評估定位精度時，使用算法估計的移動機器人位姿和外部運動捕捉系統(tǒng)采集到的相機真實位姿的均方根誤差(RMSE)來衡量。均方根誤差計算式為

(13)

在4個不同場景和難度的數(shù)據(jù)集上進行測試，并與ORBSLAM2對比，結(jié)果如表1所示。

表1 均方根誤差與運行時間對比

均方根誤差值越小，說明估計軌跡與真實軌跡誤差越小，則定位精度越高。表1表明了本文算法在4種場景下定位精度比ORB SLAM2均提高，這是由于融合了IMU數(shù)據(jù)能在某些場景下彌補視覺數(shù)據(jù)的缺陷。在運行效率上，與ORB SLAM2相比，其運行時間整體增加25%，這是由于加入了松耦合，算法復雜度提升，導致運行時間提高。

圖6為算法在VR1_01數(shù)據(jù)集上的SVO效果圖。由圖可看出，房間左邊、前面及右邊由特征點組成的三面墻體較直觀地展示了系統(tǒng)建圖的效果。

圖6 VR1_01數(shù)據(jù)集構圖效果

4 結(jié)束語

本文提出了一種基于ORBSLAM2的改進SLAM算法，融合了慣性傳感器IMU測量的數(shù)據(jù)，對RGB-D的位姿估計的結(jié)果進行擴展卡爾曼濾波(EKF)融合，提升了視覺里程計的定位精度。但是由于采用的是基于松耦合數(shù)據(jù)融合方法，在運行效率上有所降低，后續(xù)可以考慮采用更好的數(shù)據(jù)融合方案來提升算法的時性，如采用直接法融合IMU無需計算特征點從而減少算法計算時間。