基于RGB-D相機數(shù)據(jù)的SLAM算法

洪 亮 ，馮 常

（1.中國科學(xué)院光電技術(shù)研究所四川省成都市610209；2.中國科學(xué)院大學(xué)北京100049）

同步定位與建圖，又名Simultaneous Localiza?tion and Mapping（SLAM），是機器人導(dǎo)航、無人駕駛和AR等技術(shù)的基礎(chǔ)。從1986年提出以來，已經(jīng)發(fā)展了30余年，目前已經(jīng)形成了基于濾波的方法和基于優(yōu)化的方法兩種主流思路。早期SLAM主要使用激光雷達作為傳感器[1]，但價格昂貴，近幾年，用視覺傳感器實現(xiàn)的SLAM方案展現(xiàn)了很好的效果，2010年，微軟推出一款廉價RGB-D相機Kinect，RGB-D相機通過同時獲得彩色圖和對應(yīng)的深度圖[2-3]，使得稠密點云地圖的構(gòu)建更加容易實現(xiàn)。

2012年，Henry等人[4]首先將Kinect用于室內(nèi)稠密地圖重建，2016年，Mut-Artal等人開源的ORB_SLAM2方案支持RGB-D相機，但是只能構(gòu)建稀疏的地圖。KinectFusion和ElasticFusion方案雖然能構(gòu)建三維稠密的地圖，但平臺需要配置GPU來實現(xiàn)[5]，Endres的方案雖然不需要使用GPU設(shè)備，但重建效率不高[6]?；谝陨系膯栴}，本文提出一種成本低廉、基于圖優(yōu)化框架的SLAM方案，通過使用RGB-D相機數(shù)據(jù)，匹配得到較為精確的相機位姿估計結(jié)果，實現(xiàn)室內(nèi)環(huán)境的三維稠密點云地圖的快速構(gòu)建，且對電腦的配置要求不高。

1 算法方案

早期SLAM主要使用基于濾波器的方法，近些年，基于優(yōu)化的方法展現(xiàn)出優(yōu)勢[7]，本文采用基于圖優(yōu)化的視覺SLAM框架，系統(tǒng)可以分為3個部分：前端、后端和建圖[8]。

在前端，機器人通過攜帶的RGB-D相機采集室內(nèi)環(huán)境的數(shù)據(jù)作為輸入，即連續(xù)的RGB圖和對應(yīng)的深度圖序列。以相機采集第一幅圖像時的位置為原點，建立世界坐標系。為了得到整個環(huán)境的三維圖，需要將圖像序列關(guān)聯(lián)起來，即建立連續(xù)圖像之間的位姿變換關(guān)系。在RGB圖中提取均勻化的ORB特征，對連續(xù)的兩幅圖進行特征匹配，并用隨機采樣一致性（RANSAC）方法優(yōu)化匹配結(jié)果，將優(yōu)化后的匹配點對作為初值，用PnP求解兩幅圖的位姿變換矩陣，并用Bundle Adjustment優(yōu)化兩幀之間的位姿變換矩陣。

由于相機在采集數(shù)據(jù)時幀率大概在30Hz[9]，及兩幅連續(xù)幀之間變化一般不大，所以并不是每一幀圖像都會對三維地圖的構(gòu)建有作用，冗余幀會降低系統(tǒng)的效率，通過設(shè)置關(guān)鍵幀選取機制去掉冗余幀。得到所有連續(xù)關(guān)鍵幀之間的位姿變換矩陣后，送入后端進行優(yōu)化。

在后端，采用位姿圖優(yōu)化的方式，以O(shè)RB特征為路標點，將機器人在世界坐標系中的位置作為優(yōu)化變量，即圖優(yōu)化中的點，將幀與幀的匹配得出的兩個位姿之間的位姿變換矩陣作為邊的初始值，使用g2o工具對其進行優(yōu)化，得到最終的機器人全局位姿估計。

在建圖模塊，將關(guān)鍵幀對應(yīng)的深度圖生成點云圖，并通過優(yōu)化后的位姿變換矩陣將當前幀的點云圖變換到世界坐標系中，實現(xiàn)室內(nèi)環(huán)境三維稠密點云地圖的構(gòu)建。

本文采用TUM的數(shù)據(jù)集[10]，最終快速構(gòu)建了全局一致的三維稠密點云圖，該地圖很好地反應(yīng)了室內(nèi)環(huán)境的情況。

算法流程圖如圖1所示。

圖1 算法流程圖

2 相機跟蹤

2.1 ORB特征提取

常用的特征提取和描述的方法有SIFT、SURF、Shi-Tomasi和ORB[11]等。SIFT和SURF算法的提取效率不高，Shi-Tomasi是Harris算法的改進，不具有尺度不變性。

文中采用的ORB（Oriented FAST and Rotated BRIEF）特征是Ethan Rublee在2011年提出的一種新的特征檢測與描述算法[12]，并在OpenCV中實現(xiàn)，相對于SIFT和SURF，ORB具有更快的速度。ORB以改進的FAST角點為關(guān)鍵點，以BRIEF為描述子，該描述子特有的2進制串的表現(xiàn)形式不僅節(jié)約了存儲空間，同時大大縮短了匹配的時間，這使得ORB的計算速度非常快。但原始的ORB特征算法有集中分布的特點，為了解決這一問題，提高匹配精度，本文采用ORB_SLAM2中均勻化的方法，將均勻化后的ORB作為特征。

本方案采用暴力匹配的方法來實現(xiàn)兩幅圖像間的特征匹配，基于BRIEF描述子的特點，使用漢明距離來度量兩個特征點的相似程度。同時，采用隨機采樣一致性（RANSAC）算法去除誤匹配，該算法對噪聲大的數(shù)據(jù)具有良好的魯棒性。

2.2 位姿估計

2.2.1 針孔相機模型

相機將環(huán)境中的三維點映射到二維圖像平面通常采用針孔相機模型來表示。若RGB圖像中某像素點坐標為p（u，v），深度值為d，該點在相機坐標系下的坐標為：

圖2 針孔相機模型

其中，fx和fy指相機在x、y兩個軸上的焦距，cx和cy指相機的光圈中心，s為深度縮放因子，這些構(gòu)成了相機的內(nèi)參，可以通過相機標定來獲得。

P（X，Y，Z）點的世界坐標Pw需要根據(jù)相機當前位姿來求：

其中K為相機的內(nèi)參矩陣，由式（1）中的內(nèi)參組成，相機的位姿R和t即為相機的外參，外參表示了相機的軌跡和位置，通過外參可以得到兩幀之間的變換矩陣T，精確的位姿求解算法是實現(xiàn)機器人定位的關(guān)鍵。

文中通過求出當前幀與上一幀之間的位姿變換矩陣，再與之前的變換矩陣作連乘，即可以得到機器人此刻在世界坐標系的位置和姿態(tài)。

2.2.2 PnP求解與優(yōu)化

完成兩幅圖像的匹配后，接下來可以求解相機的位姿變換矩陣，由于本文采用RGB-D相機，每一幅RGB圖都有一幅與之對應(yīng)的深度圖，所以2D-2D、3D-2D、3D-3D這3種方法均能求解。文獻[13]指出，2D-2D的對極幾何方法存在初始化和尺度的問題，而3D到2D運動估計比3D到3D的方法更精確，文中采用 PnP（Perspective-n-Point）算法來求解3D-2D問題，得到兩幀的初始位姿估計。

為了得到更精確的位姿，將得到的初始位姿作為初值，用非線性優(yōu)化的方法求取優(yōu)化的位姿變換矩陣，這是一個兩幀圖像之間的Bundle Adjustment問題，即最小化重投影誤差問題[8]，通過最小化公式（4）來求。

其中Pi為空間點的坐標，ui為該點對應(yīng)的像素坐標，相機初始位姿的李代數(shù)為ξ，通過迭代優(yōu)化來得到優(yōu)化后的相機位姿。

2.3 關(guān)鍵幀選擇機制

在求解位姿的同時，系統(tǒng)啟動關(guān)鍵幀選取機制：

1）由于相機快速運動，部分圖像存在模糊的問題，為了保證關(guān)鍵幀圖像質(zhì)量較高，需要在兩兩匹配時設(shè)置最小內(nèi)點閾值；

2）由于相機采集數(shù)據(jù)的頻率較高，相鄰圖像之間的位姿變換較小，設(shè)置每兩個連續(xù)關(guān)鍵幀之間最小間隔5幀；

3）設(shè)置一個相機的運動范圍，避免過大或過小。

通過關(guān)鍵幀選擇機制，使計算效率大大提高，在保證了全局地圖質(zhì)量的情況下，同時減輕了后端優(yōu)化的計算壓力。

3 后端位姿圖優(yōu)化

相機跟蹤部分又稱為視覺里程計，由于兩幀之間的位姿估計存在一定的誤差，而且誤差會累積，所以整個視覺里程計會存在不同程度的漂移。在一個局部范圍內(nèi)，視覺里程計能夠滿足定位和建圖精度，但是如果要建立較大范圍全局一致的三維環(huán)境地圖模型，需要引入后端優(yōu)化機制，來減小視覺里程計的累積誤差。

早期優(yōu)化采用濾波的方法，通過建立機器人的運動方程和觀測方程，優(yōu)化位姿和路標[8]。

uk為k時刻運動數(shù)據(jù)，xk為k時刻機器人位置，zk,j為觀測數(shù)據(jù)，yj為路標點，wk和vk,j都為噪聲數(shù)據(jù)。通過估計xk和yj來解決機器人定位和建圖問題。

目前，基于優(yōu)化的方法展現(xiàn)出一定的優(yōu)勢[14]。后端優(yōu)化的本質(zhì)是一個狀態(tài)估計問題。本文的SLAM系統(tǒng)只有視覺傳感器，沒有IMU等慣性測量單元，所以只有觀測方程，沒有運動方程。

本方案在后端采用位姿圖優(yōu)化的方法，圖中的路標點（特征點）為固定點，將相機位姿作為待優(yōu)化的變量，即圖優(yōu)化中的節(jié)點，通過幀與幀的匹配得出兩個位姿之間的相對運動估計，作為邊的初始值，從而轉(zhuǎn)化成圖優(yōu)化的問題。文中使用g2o優(yōu)化工具箱，采用L-M方法進行迭代優(yōu)化[15]。

圖3 位姿圖優(yōu)化

4 地圖構(gòu)建

早期基于激光雷達的方案主要建立的是環(huán)境的二維地圖模型，Velodyne公司推出的三維激光雷達雖然能建立環(huán)境的三維點云圖，但是價格昂貴?；谝曈X的方案不僅成本低廉，且能恢復(fù)環(huán)境的原貌[16]。

目前，視覺方案建立的三維地圖模型可以分為3種，第一種為稀疏地圖，如ORB_SLAM[17]，第二種為半稠密地圖，如LSD_SLAM[18]，第三種為稠密地圖，如RGBD SLAM[11]。

文中采用三維稠密點云地圖來重建室內(nèi)環(huán)境，該地圖模型能很好的表示環(huán)境情況。

5 實驗分析

5.1 特征匹配

圖4為初始匹配結(jié)果，篩選前大概有500組匹配，但存在很多錯誤匹配的情況，圖5為使用RANSAC去除了外點后的ORB特征匹配情況，篩選后約有50組匹配，同時匹配也比較準確。

5.2 方案效率

本文將從運算效率上與文獻[6]提出的RGB-D SLAM進行對比，計算平均每幀的處理時間，本實驗采用四核i5、內(nèi)存為4G的電腦，RGB-D SLAM運行在一個四核8G內(nèi)存的電腦上，對比結(jié)果如表1所示。

圖4 初始匹配結(jié)果

圖5 優(yōu)化后匹配結(jié)果

表1 本文方案與RGBDSLAM方案對比

文中采用TUM的兩個數(shù)據(jù)集進行測試，與文獻[6]中的數(shù)據(jù)進行對比，從表1可以看出，文中方案處理速度約為RGB-D SLAM方案的2倍，能快速實現(xiàn)三維室內(nèi)環(huán)境的重建。

5.3 建 圖

建圖實驗采用TUM的fr1_xyz數(shù)據(jù)集，該數(shù)據(jù)通過手持深度相機在實驗室桌面往復(fù)運動來采集RGB圖和對應(yīng)的深度圖。該數(shù)據(jù)集約有780幀圖片，本實驗處理了前700幀。

在加入后端優(yōu)化前，建立的室內(nèi)環(huán)境地圖如圖6和圖7所示?？梢钥吹剑瑑?yōu)化前的地圖中有很多雜亂的點云，桌面邊緣輪廓線較模糊，墻邊的凳子沒有構(gòu)建出來。

圖6 優(yōu)化前正面

加入后端優(yōu)化機制后，散亂的點云收斂在一起，桌面邊緣輪廓清晰，墻邊的凳子構(gòu)建較完善，從優(yōu)化后的地圖模型看出，形成了更加清晰一致的全局地圖。

圖7 優(yōu)化前側(cè)面

圖8 優(yōu)化后正面

圖9 優(yōu)化后側(cè)面

從不同的角度去觀察這個三維地圖模型，文中算法構(gòu)建出的三維稠密點云地圖都能很好地反應(yīng)室內(nèi)環(huán)境的情況。

6 結(jié)束語

文中提出了一種基于RGB-D相機數(shù)據(jù)的快速低成本SLAM算法方案。本SLAM方案只需要一個搭載在機器人上的深度相機作為傳感器[19-21]，如微軟的Kinect相機。前端在RGB圖提取中均勻化的ORB特征，通過跟蹤相機位姿，得到位姿變換矩陣。挑選關(guān)鍵幀送入后端，用g2o工具箱進行全局的位姿優(yōu)化，通過位姿變換矩陣將當前幀的點云圖變換到世界坐標系中，實現(xiàn)三維稠密點云地圖的構(gòu)建。通過與RGB-D SLAM進行對比，本方案的速度是RGBD SLAM的2倍，能實現(xiàn)室內(nèi)環(huán)境地圖的快速構(gòu)建，通過加入了后端優(yōu)化機制，構(gòu)建出全局一致的室內(nèi)環(huán)境模型。

目前，本算法方案還不能達到完全實時的狀態(tài)，且由于Kinect相機采集距離有限，對于大場景還不適用。因此下一步將通過優(yōu)化算法方案來提高效率和魯棒性。

參考文獻：

[1]Khairuddin A R，Talib M S，Haron H.Review on simultaneous localization and mapping（SLAM）[C]//Control System，Computing and Engineering（ICCSCE），2015 IEEE International Conference on.IEEE，2015:85-90.

[2]辛菁，茍蛟龍，馬曉敏，等.基于Kinect的移動機器人大視角3維 V-SLAM[J].機器人，2014，36（5）:560-568.

[3]李永鋒，張國良，王蜂，等.基于快速視覺里程計和大回環(huán)局部優(yōu)化模型的改進VSLAM算法[J].機器人，2015，37（5）:557-565.

[4]Henry P，Krainin M，Herbst E，et al.RGB-D mapping:Using Kinect-style depth cameras for dense 3D modeling of indoor environments[J].The International Jo-urnal of Robotics Research，2012，31（5）:647-663.

[5]Hsiao M，Westman E，Zhang G，et al.Keyframebased Dense Planar SLAM[C]//P-roc.International Conference on Robo-tics and Automation（ICRA），IEEE.2017:5110-5117.

[6]Endres F，Hess J，Engelhard N，et al.An evalua?tion of the RGB-D SLAM syst-em[C]//Robotics and Automation（ICRA），2012 IEEE Internation?al Conference on.IEEE，2012:1691-1696.

[7]Strasdat H，Montiel J M M，Davison A J.Visual SLAM:why filter[J].Image and Vision Computing，2012，30（2）:65-77.

[8]高翔，張濤.視覺SLAM十四講[M].北京：電子工業(yè)出版社，2017.

[9]付夢印，呂憲偉，劉彤，等.基于RGB-D數(shù)據(jù)的實時 SLAM 算法[J].機器人，2015，37（6）:683-692.

[10]Sturm J，Engelhard N，Endres F，et al.A bench?mark for the evaluation of RGB-D SLAM systems[C]//Intelligent Robots and Systems（IROS），2012 IEEE/RSJ International Conference on.IEEE，2012:573-580.

[11]Endres F，Hess J，Sturm J，et al.3-D mapping with an RGB-D camera[J].IEEE Transactions on Robotics，2014，30（1）:177-187.

[12]Rublee E，Rabaud V，Konolige K，et al.ORB:An efficient alternative to SIFT or SURF[C]//Computer Vision（ICCV），2011 IEEE international confer?ence on.IEEE，2011:2564-2571.

[13]Fraundorfer F，Scaramuzza D.Visual odometry:Part II:Matching，robustness，optimization，and applications[J].IEEE Robotics& Automation Magazine，2012，19（2）:78-90.

[14]梁明杰，閔華清，羅榮華.基于圖優(yōu)化的同時定位與地圖創(chuàng)建綜述[J].機器人，2013，35（4）:500-512.

[15]Kümmerle R，Grisetti G，Strasdat H，et al.g2o:A general framework for graph optimization[C]//Ro?botics and Automa-tion（ICRA），2011 IEEE In?ternational Conference on.IEEE，2011:3607-3613.

[16]劉浩敏，章國鋒，鮑虎軍.基于單目視覺的同時定位與地圖構(gòu)建方法綜述[J].計算機輔助設(shè)計與圖形學(xué)學(xué)報，2016，28（6）:855-868.

[17]Mur-Artal R，Montiel J M M，Tardos J D.ORBSLAM:a versatile and accurate monocular SLAM system[J].IEEE Transa-ctions on Robotics，2015，31（5）:1147-1163.

[18]Engel J，Sch?ps T，Cremers D.LSD-SLAM:Large-scale direct monocular SLAM[C]//European Conference on Compu-ter Vision.Springer，Cham，2014:834-849.

[19]徐震，邵波，王云鵬.無線傳感器網(wǎng)絡(luò)分布式數(shù)據(jù)采集功率控制研究[J].電力信息與通信技術(shù)，2017（2）:115-120.

[20]朱菊香.磁阻傳感器在角位移測量中的非線性校正[J].自動化與儀器儀表，2015（12）:52-53.

[21]陽桂蓉.開環(huán)霍爾電流傳感器磁芯設(shè)計[J].自動化與儀器儀表，2017（2）:74-76.