洪 亮 ，馮 常

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

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

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

1 算法方案

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

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

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

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

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

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

算法流程圖如圖1所示。

圖1 算法流程圖

2 相機(jī)跟蹤

2.1 ORB特征提取

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

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

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

2.2 位姿估計(jì)

2.2.1 針孔相機(jī)模型

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

圖2 針孔相機(jī)模型

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

P（X，Y，Z）點(diǎn)的世界坐標(biāo)Pw需要根據(jù)相機(jī)當(dāng)前位姿來求：

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

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

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

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

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

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

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

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

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

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

3）設(shè)置一個(gè)相機(jī)的運(yùn)動(dòng)范圍，避免過大或過小。

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

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

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

早期優(yōu)化采用濾波的方法，通過建立機(jī)器人的運(yùn)動(dòng)方程和觀測(cè)方程，優(yōu)化位姿和路標(biāo)[8]。

uk為k時(shí)刻運(yùn)動(dòng)數(shù)據(jù)，xk為k時(shí)刻機(jī)器人位置，zk,j為觀測(cè)數(shù)據(jù)，yj為路標(biāo)點(diǎn)，wk和vk,j都為噪聲數(shù)據(jù)。通過估計(jì)xk和yj來解決機(jī)器人定位和建圖問題。

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

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

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

4 地圖構(gòu)建

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

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

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

5 實(shí)驗(yàn)分析

5.1 特征匹配

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

5.2 方案效率

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

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

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

表1 本文方案與RGBDSLAM方案對(duì)比

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

5.3 建 圖

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

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

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

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

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

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

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

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

6 結(jié)束語

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

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

參考文獻(xiàn)：

[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的移動(dòng)機(jī)器人大視角3維 V-SLAM[J].機(jī)器人，2014，36（5）:560-568.

[3]李永鋒，張國良，王蜂，等.基于快速視覺里程計(jì)和大回環(huán)局部優(yōu)化模型的改進(jìn)VSLAM算法[J].機(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]付夢(mèng)印，呂憲偉，劉彤，等.基于RGB-D數(shù)據(jù)的實(shí)時(shí) SLAM 算法[J].機(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)化的同時(shí)定位與地圖創(chuàng)建綜述[J].機(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]劉浩敏，章國鋒，鮑虎軍.基于單目視覺的同時(shí)定位與地圖構(gòu)建方法綜述[J].計(jì)算機(jī)輔助設(shè)計(jì)與圖形學(xué)學(xué)報(bào)，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]朱菊香.磁阻傳感器在角位移測(cè)量中的非線性校正[J].自動(dòng)化與儀器儀表，2015（12）:52-53.

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