賴秋玲　禹素萍　丁紳一

摘要：移動機器人同時定位與建圖（SLAM- Simultaneous Locating And Mapping）與路徑規(guī)劃是實現(xiàn)智能化機器人的前提。該系統(tǒng)首先通過HCR（Home Care Robot）機器人搭載激光雷達和RGB-D傳感器，在ROS的運行環(huán)境下，分別實現(xiàn)基于ORB-SLAM2和RGB-D SLAM算法框架的同時定位與建圖，獲取三維點云圖。然后將三維點云圖與激光雷達生成的二維平面圖進行數(shù)據(jù)融合，得到含有較豐富環(huán)境信息的環(huán)境二維柵格地圖，并利用A*算法路徑搜索算法，對機器人進行路徑規(guī)劃。實驗結(jié)果表明融合了環(huán)境三維點云信息的地圖，更適合用于路徑規(guī)劃。

關(guān)鍵詞：同時定位與建圖；RGB-D傳感器；激光雷達；路徑規(guī)劃

中圖分類號：TP242 文獻標(biāo)識碼：A 文章編號：1009-3044（2017）33-0024-03

Abstract： Mobile robot simultaneous locating and mapping （SLAM） and path planning are the prerequisites for realizing intelligent robots. In this system， the HCR （Home Care Robot） robot was equipped with Lidar and an RGB-D sensor. In the ROS operating environment， the SLAM Based on the ORB-SLAM2 and RGB-D SLAM algorithms were implemented respectively and three - dimensional point cloud map was generated. Then， the three - dimensional point cloud map was combined with the two - dimensional map generated by the Lidar to get a grid map with the rich environment information. Finally， the A * algorithm path search algorithm was applied to plan the path for the robot. Experiment results show that the grid map which integrates the three-dimensional cloud information is more suitable for path planning.

Key words： SLAM； RGB-D sensor； Lidar； path planning

1 概述

同時定位與建圖（Simultaneous Localization and Mapping—SLAM）是移動機器人領(lǐng)域的熱門研究課題[1]。文獻[2]已對現(xiàn)有的SLAM算法做了較全面的總結(jié)。SLAM的發(fā)展主要分為兩個階段：第一階段為經(jīng)典階段（1986-2004），在這個階段學(xué)者們?yōu)閷崿F(xiàn)SLAM提出了基于貝葉斯的濾波器方法，包括卡爾曼濾波（Kalman Filtering）、擴展卡爾曼濾波（Extended Kalman Filtering）[3]、Rao-Blackwellized粒子濾波[4]等。第二階段為算法分析階段（2004-2016），該階段主要對SLAM的基本性質(zhì)進行研究，包括可觀察性、收斂性和一致性。在此時期，人們理解了稀疏性[5]對高效地實現(xiàn)SLAM的關(guān)鍵作用，并開源了較多的SLAM庫，例如RGB-D SLAM和ORB-SLAM。ORB-SLAM2[6]是目前較優(yōu)秀的稀疏性開源SLAM框架，它優(yōu)化了ORB-SLAM[7]，支持標(biāo)定后的RGB-D相機。

隨著傳感器技術(shù)的發(fā)展，RGB-D相機、激光雷達等傳感器簡化了SLAM的實現(xiàn)難度，而傳感器信息融合[8]也不斷地被應(yīng)用到機器視覺領(lǐng)域。

因為激光雷達僅能檢測到一定高度的平面信息，所以靠單一的激光雷達，并不能得到更全面的環(huán)境信息，而RGB-D相機則具有較大的視角，可以獲取更豐富的環(huán)境信息。因此該系統(tǒng)利用RGB-D相機，構(gòu)建基于ORB-SLAM2和RGB-D SLAM算法框架的SLAM系統(tǒng)，對比兩種經(jīng)典SLAM算法，將較好的三維點云圖與激光雷達平面圖進行環(huán)境信息融合，并在此融合地圖上實現(xiàn)機器人路徑規(guī)劃。

2 軟硬件平臺與系統(tǒng)架構(gòu)

該系統(tǒng)的軟件部分使用ROS（Robot Operating System）系統(tǒng)[9]，機器人為HCR（Home Care Robot）機器人。電腦配置為Intel core I7處理器，操作系統(tǒng)為Ubuntu14.04。軟件系統(tǒng)結(jié)構(gòu)如圖1所示。

軟件系統(tǒng)結(jié)構(gòu)的主要分為以下三部分：

（1） 解算機器人位姿：由RGB-D相機采集環(huán)境的彩色和深度信息，經(jīng)由ORB-SLAM2算法或者RGB-D SLAM算法，解算出機器人的當(dāng)前位姿[XmYmθm]。

（2） 獲得環(huán)境地圖：通過ORB-SLAM2算法或者RGB-D SLAM算法，拼接融合關(guān)鍵幀生成環(huán)境三維點云圖，通過ROS的第三方庫Octomap[10]得到三維網(wǎng)格地圖。Octomap用八叉樹來存儲環(huán)境信息，遍歷八叉樹，將概率大于0.5的全部方塊投影得到二維平面地圖。然后將此平面地圖與激光雷達生成的地圖進行或操作，得到融合后的二維柵格地圖。

（3） 實現(xiàn)路徑規(guī)劃：將解算出的當(dāng)前位姿和機器人模型導(dǎo)入到生成的二維地圖中，將當(dāng)前位姿作為起始點，確定一個目標(biāo)點，通過A*算法，搜索出一條路徑，并生成機器人控制命令。endprint

2.1 基于ORB-SLAM2和RGB-D SLAM算法框架的實時定位與建圖

隨著傳感器技術(shù)不斷地發(fā)展，各類SLAM算法框架也應(yīng)運而生。ORB-SLAM2和RGB-D SLAM是近幾年較受關(guān)注的SLAM算法框架。

ORB-SLAM2是目前較優(yōu)秀的SLAM算法系統(tǒng)框架，如圖2所示，該系統(tǒng)主要由三個平行線程組成。

（1） 相機位姿跟蹤線程：首先對輸入幀進行預(yù)處理。然后相機位姿跟蹤，若不插入關(guān)鍵幀則不啟動局部地圖線程。局部地圖跟蹤是將當(dāng)前幀與局部地圖進行匹配，利用最小化投影誤差來優(yōu)化位姿。最后依據(jù)是否很長時間沒有插入關(guān)鍵幀或者跟蹤失敗等因素來判斷是否生成關(guān)鍵幀。

（2） 局部地圖線程：該線程主要是負(fù)責(zé)處理新生成的關(guān)鍵幀，剔除地圖中新添加的質(zhì)量不高的地圖點。而后進行地圖點的融合并進行局部BA（Bundle Adjustment）[11]優(yōu)化，最后將那些地圖點超過90%以上能被其他至少3個關(guān)鍵幀共同檢測到的關(guān)鍵幀進行剔除，以減少重復(fù)率，節(jié)省存儲空間。

（3） 閉環(huán)檢測及矯正線程：利用基于DBoW2[12]的場景識別進行閉環(huán)檢測，而后通過優(yōu)化位姿圖來實現(xiàn)閉環(huán)矯正。

在閉環(huán)檢測及矯正線程結(jié)束之后，觸發(fā)第四個線程進行全局的BA優(yōu)化，并更新地圖。該算法利用ORB[13]特征點完成跟蹤，建圖，以及場景識別的任務(wù)。這些ORB特征點對于旋轉(zhuǎn)和尺度變換有較強魯棒性，不受光線變換的影響并且提取速度較快，可以滿足實時性的需求。

而RGB-D SLAM[14]主要由前端與后端兩部分組成，如圖3所示，前端部分處理深度相機獲取的環(huán)境信息，解算出環(huán)境信息的空間幾何位置，后端用g2o庫將前端部分得到的機器人位姿進行優(yōu)化。最終得到環(huán)境三維點云圖和機器人位姿。

2.2 基于A*算法的路徑規(guī)劃

該系統(tǒng)采用A*算法來對機器人進行路徑規(guī)劃。A*算法[15]是一種啟發(fā)式的算法，它被廣泛地用于解決各類搜索問題，例如機器人路徑規(guī)劃，網(wǎng)絡(luò)游戲以及UAV（Unmanned Aerial Vehicles）避障。A*算法的基本思想是結(jié)合Dijkstra算法所用的離起始點最近的頂點信息和Greedy best-first search算法采用的離目標(biāo)點最近的頂點信息。通常用公式（1）來說明A*算法，g（n）表示從起始點到任意頂點n的實際消耗，即走的路徑長度，h（n）表示從頂點n到目標(biāo)點的啟發(fā)式預(yù)測消耗。在每一次循環(huán)中，算法找到一個n使得f（n）最小，從而搜索到目標(biāo)點的最小消耗，即最短路徑。

[fn=gn+h（n）] （1）

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

3.1 室內(nèi)ORB-SLAM2，RGB-D SLAM與激光雷達建圖結(jié)果

通過ORB-SLAM2算法框架和RGB-D SLAM算法，實驗結(jié)果如圖4所示，（a）為實驗場景圖，（b） 激光雷達生成的二維平面圖，（c）為ORB-SLAM2生成的三維點云圖， （d）為RGB-D SLAM生成的三維點云圖。從實驗結(jié)果看出，ORB-SLAM2算法框架的閉環(huán)檢測效果優(yōu)于RGB-D SLAM算法。RGB-D SLAM算法通過計算關(guān)鍵幀中的特征點描述子來進行回環(huán)檢測，而ORB-SLAM2是通過基于DBoW2的場景識別來進行閉環(huán)檢測，而且具有閉環(huán)修正模塊，較好地修正了地圖。在實驗過程中發(fā)現(xiàn)，ORB-SLAM2算法的實時性也較RGB-D SLAM好。ORB-SLAM2算法采用的是多線程方式，而RGB-D SLAM是單線程，在同樣的硬件設(shè)備條件下，ORB-SLAM2運行速度較快，可以滿足實際應(yīng)用中對實時性的要求。ORB-SLAM2算法的基于DBoW2的場景識別模型具有全局重定位能力，在跟蹤丟失之后，可以進行重新定位，魯棒性更優(yōu)。

<

其中左上角（a）圖為場景圖；右上角（b）圖激光雷達平面圖；左下角（c）圖為ORB-SLAM2生成的三維點云圖；右下角（d）圖為RGB-SLAM生成的三維點云圖。

從以上的實驗結(jié)果圖分析，該系統(tǒng)選用ORB-SLAM2生成的三維點云圖經(jīng)過Octomap處理，得到如圖5所示的三維網(wǎng)格環(huán)境地圖。Octomap使用八叉樹的數(shù)據(jù)結(jié)構(gòu)存儲地圖，首先遍歷八叉樹節(jié)點，刪除值小于0.5的節(jié)點（大于0.5表示該區(qū)域被占概率較大），然后將其投影得到二維平面地圖，為了便于使用A*算法，將障礙物方格化，地圖中標(biāo)示1的為障礙物，0為可移動空間（灰色區(qū)域），最后將投影得到的平面圖與激光雷達平面圖進行或操作，得到融合了環(huán)境三維點云信息和激光雷達信息的平面圖。

3.2 機器人路徑規(guī)劃結(jié)果

分別在激光雷達柵格地圖和融合三維環(huán)境點云信息的柵格地圖上，使用A*路徑搜索算法，對機器人路徑進行規(guī)劃。圖中所繪路線即為A*算法結(jié)果，黑色方塊為終點。實驗結(jié)果如圖6所示。由路徑規(guī)劃結(jié)果看出，在激光雷達地圖上進行導(dǎo)航出現(xiàn)失誤，即規(guī)劃的線路中經(jīng)過了障礙物占有的區(qū)域，而在融合了點云信息的環(huán)境地圖上，規(guī)劃的路線正確。這在一定程度上說明融合了三維環(huán)境點云信息的激光雷達地圖比未融合信息的激光雷達地圖包含較豐富的環(huán)境信息，有助于對機器人路徑規(guī)劃的實現(xiàn)。

4 結(jié)論

該系統(tǒng)實現(xiàn)了室內(nèi)移動機器人SLAM與路徑規(guī)劃，并比較了兩種經(jīng)典SLAM算法框架即ORB-SLAM2和RGB-D SLAM算法， 融合三維點云信息和激光雷達信息，實現(xiàn)機器人路徑規(guī)劃。實驗結(jié)果顯示ORB-SLAM2算法總體來說優(yōu)于RGB-D SLAM。融合了環(huán)境三維點云信息的激光雷達地圖，更有利于對機器人進行路徑規(guī)劃。但該系統(tǒng)在以下幾個方面不足，首先，將三維網(wǎng)格地圖投影生成二維平面地圖時存在噪聲影響，會對障礙物判斷產(chǎn)生一定誤差。其次，當(dāng)運行RGB-D SLAM算法的時候，光照對其影響較大，所以有一定的誤差產(chǎn)生。endprint

參考文獻：

[1] 李貴亞. 基于立體視覺的機器人SLAM算法研究[D]. 哈爾濱工業(yè)大學(xué)， 2015.

[2] Cadena C， Carlone L， Carrillo H， et al. Simultaneous Localization And Mapping： Present， Future， and the Robust-Perception Age[J]. 2016， 32（6）.

[3] 時也， 吳懷宇， 徐文霞，等. 基于擴展卡爾曼濾波器的移動機器人SLAM研究[J]. 電子設(shè)計工程， 2012， 20（1）：104-106.

[4] 羅元， 傅有力， 程鐵鳳. 基于改進Rao-Blackwellized粒子濾波器的同時定位與地圖構(gòu)建[J]. 控制理論與應(yīng)用， 2015， 32（2）：267-272. （下轉(zhuǎn)第37頁）

（上接第26頁）

[5] Bichucher V， Walls J M， Ozog P， et al. Bathymetric factor graph SLAM with sparse point cloud alignment[C]// Oceans. IEEE， 2015：1-7.

[6] Murartal R， Tardos J D. ORB-SLAM2： an Open-Source SLAM System for Monocular， Stereo and RGB-D Cameras[J]. 2016.

[7] Mur-Artal R， Montiel J M M， Tardós J D. ORB-SLAM： A Versatile and Accurate Monocular SLAM System[J]. IEEE Transactions on Robotics， 2015， 31（5）：1147-1163.

[8] Belter D， Labcki P， Skrzypczynski P. Estimating terrain elevation maps from sparse and uncertain multi-sensor data[C]// IEEE International Conference on Robotics and Biomimetics. IEEE， 2012：715-722.

[9] 張建偉， 張立偉， 胡穎，等. 開源機器人操作系統(tǒng)——ROS[M].北京：科學(xué)出版社，2012.

[10] 王允華， 李建勝. 基于Octomap的無人機室內(nèi)導(dǎo)航可飛行區(qū)域分析[J].測繪與空間地理信息，2017（2）：95-97.

[11] Zhao L， Huang S， Yan L， et al. Large-scale monocular SLAM by local bundle adjustment and map joining[C]// International Conference on Control Automation Robotics & Vision. IEEE， 2010：431-436. [12] Galvez-López D， Tardos J D. Bags of Binary Words for Fast Place Recognition in Image Sequences[J]. IEEE Transactions on Robotics， 2012， 28（5）：1188-1197.

[12] Galvez-López D， Tardos J D. Bags of Binary Words for Fast Place Recognition in Image Sequences[J]. IEEE Transactions on Robotics， 2012， 28（5）：1188-1197.

[13] 白雪冰， 車進， 牟曉凱，等. 結(jié)合快速魯棒性特征改進ORB的特征點匹配算法[J].計算機應(yīng)用，2016，36（7）：1923-1926.

[14] Elghor E C， Roussel D， Ababsa F， et al. 3D Planar RGB-D SLAM System[J]. 2016.

[15] 郝會龍， 任鴻翔， 肖方兵. 基于A算法的虛擬船員路徑規(guī)劃仿真[J].計算機工程與設(shè)計，2015（3）：733-737.endprint