基于IMU/視覺融合的導(dǎo)航定位算法研究

董伯麟,柴 旭

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

0 引言

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

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

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

1 傳感器工作原理

1.1 IMU傳感器導(dǎo)航工作原理

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

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

圖1 IMU傳感器導(dǎo)航功能示意圖

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

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

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

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

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

2 IMU-RGBD算法設(shè)計(jì)

2.1 算法框架圖

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

圖4 本文算法框架圖

2.2 RGB-D位姿估計(jì)

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

在得到匹配的特征點(diǎn)對(duì)后，通過迭代最近點(diǎn)初步估計(jì)相機(jī)的運(yùn)動(dòng)，得到場景的局部結(jié)構(gòu)。

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

(1)

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

2.3 IMU位姿估計(jì)

IMU的測量模型為

(2)

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

(3)

圖5 預(yù)積分示意圖

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

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

(4)

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

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

(5)

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

(6)

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

(7)

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

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

(8)

協(xié)方差矩陣為

(9)

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

(10)

式中S為測量噪聲矩陣。

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

(11)

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

(12)

式中E為單位矩陣。

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

3 實(shí)驗(yàn)結(jié)果與分析

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

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

(13)

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

表1 均方根誤差與運(yùn)行時(shí)間對(duì)比

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

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

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

4 結(jié)束語

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