基于IMU/視覺融合的導航定位算法研究

董伯麟,柴 旭

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

0 引言

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

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

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

1 傳感器工作原理

1.1 IMU傳感器導航工作原理

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

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

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

1.2 雙目結構光RGB-D工作原理

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

圖2 結構光成像原理圖

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

圖3 雙目結構光RGB-D

2 IMU-RGBD算法設計

2.1 算法框架圖

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

圖4 本文算法框架圖

2.2 RGB-D位姿估計

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

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

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

(1)

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

2.3 IMU位姿估計

IMU的測量模型為

(2)

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

(3)

圖5 預積分示意圖

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

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

(4)

定義EKF中的狀態向量為

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

(5)

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

(6)

現將系統的狀態在k-1時刻進行一階泰勒展開，進行線性化處理，有：

(7)

式中：F為狀態轉移矩陣;H為測量矩陣。

在EKF的預測部分中的測量狀態向量為

(8)

協方差矩陣為

(9)

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

(10)

式中S為測量噪聲矩陣。

在卡爾曼增益下，更新后的系統狀態向量為

(11)

更新后的系統協方差矩陣為

(12)

式中E為單位矩陣。

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

3 實驗結果與分析

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

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

(13)

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

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

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

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

圖6 VR1_01數據集構圖效果

4 結束語

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