胡新宇，左 韜,2，張勁波，伍一維

（1.武漢科技大學 信息科學與工程學院，湖北 武漢 430081；2.武漢科技大學 冶金自動化與檢測技術(shù)教育部工程研究中心，湖北 武漢 430081）

引言

移動機器人與周圍環(huán)境進行智能交互，在環(huán)境中執(zhí)行類似于人的任務，是近些年的研究熱點[1]。同時定位與地圖構(gòu)建（SLAM，simultaneous localization and mapping）是移動機器人最基本的問題，要求機器人能在未知環(huán)境中探索、建模并構(gòu)建有效、可用的全局地圖，是該領(lǐng)域研究的前沿方向[2]。將深度學習融合到視覺SLAM技術(shù)中，構(gòu)建含有環(huán)境語義信息的三維稠密點云地圖，這對移動機器人理解環(huán)境中的語義信息，順利執(zhí)行高級復雜的任務具有舉足輕重的作用。

視覺SLAM是通過視覺傳感器，在沒有環(huán)境先驗信息的情況下，在運動過程中估計自身運動狀態(tài)并構(gòu)建環(huán)境地圖[3]。目前，主流的視覺SLAM算法可分為兩種[4]：一種是基于特征的方法，該算法是通過提取每個圖像中的特征，再進行匹配，從而恢復相機的姿態(tài)。如：KinectFusion[5]使用RGBD 相機，通過ICP算法恢復相機的姿態(tài)，并通過CUDA 進行加速，來構(gòu)建環(huán)境的稠密點云地圖；RGBD_SLAM[6]是通過跟蹤ORB 特征，利用G2O算法優(yōu)化相機位姿來構(gòu)建稠密地圖；ORB-SLAM[7]是目前為止在該領(lǐng)域最優(yōu)秀的SLAM系統(tǒng)框架，不僅支持單目、雙目以及深度相機，還包含回環(huán)檢測、重定位、自動初始化，可以穩(wěn)健、快速地構(gòu)建環(huán)境的稀疏點云地圖。另一種是直接法，根據(jù)圖像的強度值來估計相機位姿，常見的基于直接法的算法框架有：DSO_SLAM[8]、LSD_SLAM[9]、SVO_SLAM[10]等。

在計算機視覺領(lǐng)域，深度學習的出現(xiàn)推動了目標檢測的發(fā)展。文章通過卷積神經(jīng)網(wǎng)絡對環(huán)境中的物體進行檢測，獲取物體的類別和預測框。FCN[11]、PSPNet[12]和DeepLab[13]都可對二維圖像進行像素級的語義分割，R-CNN[14]、Fast-RCNN[15]、Faster-RCNN[16]可通過對目標產(chǎn)生一個候選區(qū)域，再通過分類器和回歸器回歸得到候選區(qū)域的精確位置，但由于會產(chǎn)生很多區(qū)域建議，導致對目標進行檢測的速度較慢，此類方法稱為two-stage 法。與之相比，目標檢測速度較快的是one-stage 法，如：SSD[17]和YOLO[18]都是在圖片不同位置采用多種尺度和長寬比進行抽樣，通過卷積神經(jīng)網(wǎng)絡提取特征，然后進行分類和回歸。

雖然ORB-SALM2是非常完善、實用的視覺SLAM系統(tǒng)，但它依舊存在一些問題，如：由于點云地圖中缺乏體積信息，移動機器人無從得知環(huán)境中哪些地方可通行，使得ORB-SLAM2 所構(gòu)建的稀疏點云地圖無法實際使用；所構(gòu)建的地圖中缺乏語義信息，不利于機器人理解環(huán)境內(nèi)容；當環(huán)境越大時，存儲點云地圖所占空間越大等問題。為解決以上問題，本文提出一種構(gòu)建語義地圖的方法，在ORB-SLAM2的基礎上增加目標檢測算法來構(gòu)建稠密點云語義地圖，并將其轉(zhuǎn)換為Octomap 進行存儲，首先使用檢測速度較快的YOLO v3算法及對象正則化來獲取物體的2D標簽，結(jié)合生成的稠密點云地圖得到含有語義信息的點云標簽，利用基于圖的分割算法對稠密點云進行分割和優(yōu)化，再將點云標簽與分割后的點云進行點云融合，進而構(gòu)建稠密點云語義地圖，將所構(gòu)建的點云語義地圖轉(zhuǎn)換為八叉樹地圖，以便移動機器人進行避障和導航，減小了點云地圖的存儲空間。

1 系統(tǒng)框架

1.1 語義SLAM系統(tǒng)

本文提出的語義SLAM系統(tǒng)框架如圖1所示。通過深度相機獲取環(huán)境的圖像序列，經(jīng)過目標檢測網(wǎng)絡YOLO v3檢測每一個關(guān)鍵幀圖像中的物體，從當前幀的彩色圖和深度圖得到每一個物體的地圖點，將所有地圖點坐標的均值作為物體的坐標，并計算該物體的質(zhì)心、種類和邊界框，給定每種類別的物體不同的顏色，將其作為2D標簽。同時，經(jīng)過ORB-SLAM2算法構(gòu)建環(huán)境的稀疏點云地圖，再通過含有2D標簽的彩色圖像及其對應的深度圖像以及關(guān)鍵幀來生成稠密點云標簽，對稠密點云地圖進行基于圖的分割和優(yōu)化，然后與生成的點云標簽進行融合得到效果較好的稠密點云語義地圖，為了方便移動機器人執(zhí)行更高級任務，減小存儲空間，將點云地圖轉(zhuǎn)換為Octomap。

圖1 語義SLAM系統(tǒng)框圖Fig.1 Block diagram of semantic SLAM system

構(gòu)建稠密點云語義地圖的具體步驟如下：

1）得到2D標簽。加載color.txt，里面存儲YOLO v3 能夠識別的80種類別物體的rgb值，讀取預訓練權(quán)重yolov3.weights、配置文件yolov3.cfg及訓練好的模型coco.names，對每個關(guān)鍵幀的彩色圖像進行目標檢測，得到物體的種類，給定不同物體不同的顏色并繪制物體的邊界框，將其作為2D標簽，若計算出的物體種類為“人”，則對2D標簽進行修改。

2）稠密點云標簽。通過關(guān)鍵幀、彩色圖像及深度圖像來產(chǎn)生稠密點云地圖，遍歷深度圖像，獲取圖中點 (m,n)處的值，計算該點的空間坐標并從含有2D標簽的彩色圖中獲取該點的顏色，再將該點插入稀疏點云tmp中，每新插入一個關(guān)鍵幀都執(zhí)行此操作，點云tmp 經(jīng)過變換矩陣T得到點云cloud，對cloud 進行泊松重建，得到每個物體的模型，然后將模型轉(zhuǎn)換為三維點云，得到稠密點云地圖，對稠密點云進行染色得到稠密點云標簽。

3）點云分割。將稠密點云作為輸入點云，進行參數(shù)設置，輸入點云經(jīng)濾波后對點云進行基于圖的分割，得到分割后的點云。

4）點云語義地圖。遍歷稠密點云標簽中的每一個點，每個點都存儲了空間位置xyz值和顏色信息rgb值，如果該點的rgb值與某一物體的rgb值相等，則認為該點是這個物體點云中的點，并對分割后的點云中該點的顏色信息進行更新，得到點云語義地圖。

1.2 ORB-SLAM2分析

ORB-SLAM2的原理框圖如圖2所示，它包含并運行3個線程：跟蹤、局部建圖以及全局回環(huán)檢測并優(yōu)化線程。跟蹤線程即前端里程計的主要部分 負責對每幅輸入的圖像提取ORB 特征點，與最鄰近的關(guān)鍵幀進行比較，計算特征點的位置，粗略估計相機位姿，如果跟蹤失敗，則通過詞袋法（DBoW2，bag of binary words）進行重定位（placerecognition），最終決定是否插入關(guān)鍵幀；局部建圖線程通過Bundle Adjustment 對局部空間內(nèi)的特征點及相機位姿進行優(yōu)化，刪除冗余的關(guān)鍵幀和低質(zhì)量的地圖點，得到更精確的相機位姿和特征點的空間位置；采用DBoW2算法對全局地圖和關(guān)鍵幀進行回環(huán)檢測，如果檢測到回環(huán)，通過Sim3 優(yōu)化計算里程計的累計誤差，然后將匹配到的所有點云進行對齊并融合重復的點，最后對相似性約束進行圖優(yōu)化，實現(xiàn)全局一致性。得到稀疏點云地圖，該地圖主要包括3D 地圖點、關(guān)鍵幀、共視關(guān)系及生長樹等。

圖2 ORB-SLAM2 原理圖Fig.2 Schematic of ORB-SLAM2

1.3 基于深度學習的目標檢測

本文使用當前檢測速度較快的YOLO v3[19]網(wǎng)絡結(jié)構(gòu)，如圖3所示。其基本思想是將每幅輸入圖像劃分成s×s個柵格并對這些柵格進行分類得到目標的類別和邊框。由于YOLO v3 采用多個logistic分類器在保證目標檢測的速度的同時又不降低多標簽分類的準確性。

圖3 YOLO v3網(wǎng)絡結(jié)構(gòu)Fig.3 YOLO v3 network structure

在YOLO v3的基礎上進行對象正則化，對顯然不屬于該環(huán)境的物體標簽進行修改，得到比較精確的物體標簽。如：靜態(tài)環(huán)境下，數(shù)據(jù)集中的泰迪熊會被YOLO v3檢測為“人”，但動態(tài)的“人”顯然不屬于該靜態(tài)數(shù)據(jù)集，因此需要對標簽“人”進行修正，改為“泰迪熊”。

本文使用對象級的Dense CRF模型對語義標簽進行優(yōu)化，吉布斯分布為

當P(x)最 小時得到最優(yōu)的標簽結(jié)果，Z為正則化因子，E(x)為能量函數(shù)：

式中：x表示物體的標簽；i,j∈(1,k)為環(huán)境中物體的數(shù)量；一元勢 ?u(xi)和 二元勢 ?p(xi,xj)分別為

式中：P(xi)為第i個物體標簽的概率分布；w(m)為線性組合權(quán)重；μ(xi,xj)為標簽的容錯函數(shù)，表示同一物體出現(xiàn)兩種不同的標簽的可能性，本文采用Potts模型：

式中 θα、θβ表示物體i與物體j間的歐式距離小于 α、β。本文中，高斯核函數(shù)特指同一位置出現(xiàn)兩種物體的概率pi,j的倒數(shù)，表達式如下：

式中：ηi,j表示出現(xiàn)物體i和 物體j同時出現(xiàn)的頻率；ηj表示出現(xiàn)物體j的頻率。

將目標檢測網(wǎng)絡、對象正則化和語義SLAM的視覺里程計緊密結(jié)合，減小了相機位姿的估計值與真實值的差，提高了建圖的精確度。具體表現(xiàn)為：其一，當輸入一幀圖像時，定義一個向量來存儲圖像中的所有物體，遍歷一個存儲YOLO v3 所能夠識別的80類物體的列表，根據(jù)不同種類的物體對應的類別序號各不相同，通過左上角點和右下角點得到該物體的邊界框，如圖4所示。其二，在語義SLAM的跟蹤線程中，添加了對物體的跟蹤，使得相機的絕對位姿誤差、絕對軌跡誤差及相對位姿誤差與ORB-SLAM2 方法相比有所減少。當采集RGBD 圖像和進行雙目初始化時，在地圖中添加當前幀中所有物體的地圖點，將屬于一個物體的所有地圖點的平均位置作為物體的位置，并對地圖中物體的序號和置信度進行更新；在跟蹤參考關(guān)鍵幀時，通過一個向量objs來存儲獲取到的當前幀中的所有物體的序號、種類和置信度，遍歷每個物體的地圖點，如果參考幀中該點所屬的物體與地圖中的物體為同一類物體，計算該點的空間位置，當重復出現(xiàn)同一物體時，更新地圖中物體的序號和置信度，如果出現(xiàn)新物體，則在地圖和向量objs中添加新物體的序號、種類和置信度。

圖4 目標檢測Fig.4 Object detection

1.4 點云分割

本文根據(jù)物體的類別進行點云分割，采用圖割算法對點云進行基于圖的分割和優(yōu)化，首先將ORB-SLAM2 構(gòu)建的稀疏點云結(jié)合對應的關(guān)鍵幀及深度信息生成稠密點云，將其作為原始點云，對原始點云進行超體素分割，共得到12982個體素。構(gòu)造一個加權(quán)無向鄰接圖G={v,e}來表示超體素的分割結(jié)果，如圖5所示。

圖5 加權(quán)無向鄰接圖Fig.5 Diagram of weighted undirected adjacency

其中頂點vi∈v表示曲面片，邊e為權(quán)重，表示兩個曲面片的相似度，實線表示同一物體的2個不同曲面片間的權(quán)重，虛線表示不同物體的2個曲面片間的權(quán)重。經(jīng)超體素分割后，環(huán)境中的每一個物體都包含多個曲面片，計算每個曲面片vi的重心ci和 法向量ni，根據(jù)法向量的局部凸度信息來決定是否在圖中添加邊，通過RANSAC算法對曲面片進行隨機采樣得到候選面P={p1,p2,p3,...,pm}，再計算每個曲面片的重心ci(x′,y′,z′)到 候選面pm的歐氏距離d：

其中：候選面滿足Ax+By+Cz+D=0，D為常數(shù)；分別為候選面在xyz軸上的截距。將距離d(ci,pm)<4.5 cm（經(jīng)驗值）范圍內(nèi)的所有候選面S(P)都作為屬于每類物體的可能的候選面，如（8）式所示：

然后進行優(yōu)化，得到屬于每個物體的可能的候選面：

最后，對候選平面進行基于圖的對象分割[20]，完成點云分割。

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

2.1 稠密點云地圖

本實驗在TUM 數(shù)據(jù)集上運行，使用的數(shù)據(jù)集名稱為l ong_office_household，共歷時87.09 s 采集到2585 張彩色圖及對應的深度圖，手持相機繞辦公桌一圈，該環(huán)境包含多種物體，其中YOLO v3可識別顯示屏、鍵盤、鼠標、椅子、泰迪熊、書、瓶子以及杯子8種類別的物體。本實驗的運行平臺使用i3-8100、主頻為3.6 GHz的CPU，GPU型號為Nvidia GeForce 1070，顯存8 G，使用Darknet 框架，操作系統(tǒng)為Ubuntu 16.04。

本文是將深度相機采集的圖像序列作為YOLO v3網(wǎng)絡的輸入，計算出物體的類別和預測框的坐標，并給定不同類別的物體不同的顏色，將其作為2D標簽，通過ORB-SLAM2算法構(gòu)建環(huán)境的稀疏點云地圖，再根據(jù)含有2D標簽的彩色圖像和相應的深度圖像以及關(guān)鍵幀來生成含有語義信息的稠密點云標簽，如圖6（a）、6（b）所示。從圖中可以看到，通過YOLO v3 能有效識別環(huán)境中的顯示屏、鍵盤、鼠標、椅子、泰迪熊、書、瓶子以及杯子，每個類別的物體對應的顏色各不相同。

圖6 語義標簽Fig.6 Semantic labels

由于點云標簽與分割后的點云數(shù)目相同，將兩者進行點云融合。遍歷點云標簽中的點云，每個點云不僅存儲了空間位置還存儲了顏色信息，當點云的顏色信息為某一個特定值時，則認為該點是屬于分割后的點云中某一物體中的點，對不同種類的物體賦予不同的顏色，從而得到點云語義地圖，如圖7（a）、7（b）所示。點云語義地圖中相同的顏色代表同一種類的物體，不同顏色代表不同種類的物體。

圖7 點云語義地圖Fig.7 Point cloud semantic map

2.2 系統(tǒng)性能評估

對本文提出的緊密結(jié)合ORB-SLAM2 和YOLO v3得到環(huán)境的稠密點云語義地圖，與ORB-SLAM2構(gòu)建的不含語義信息的稀疏點云地圖進行性能比較。首先，根據(jù)時間戳將計算得到的相機位姿的真實值與估計值進行對齊，然后計算兩者的差得到絕對軌跡誤差（ATE），并通過絕對位姿誤差（APE）來檢測所構(gòu)地圖的全局一致性，通過相對位姿誤差（RPE）評估系統(tǒng)的漂移性，以下實驗結(jié)果為分別使用兩種方法運行該數(shù)據(jù)集5次得到的誤差平均值，其對比結(jié)果如表1所示。

表1 誤差表Table1 Error table

從表1可知，與傳統(tǒng)的ORB-SLAM2 相比，在緊密結(jié)合Yolo v3 和ORB-SLAM2后能減小相機的軌跡誤差和位姿誤差，提高了構(gòu)建稠密點云語義地圖的精度，整個算法運行時間約為22.68 s，由于增加了語義信息的獲取，時間消耗也略有增加。絕對位姿誤差比較如圖8所示，相對位姿誤差比較如圖9所示，其中綠色線為本文方法得到的誤差，藍色線為ORB-SALM2算法得到的誤差。

圖8 絕對位姿誤差Fig.8 Absolute pose error

圖9 相對位姿誤差Fig.9 Relative pose error

2.3 八叉樹地圖

八叉樹地圖對于移動機器人執(zhí)行高級復雜的任務至關(guān)重要，如：避障、導航或抓取物體等。使用構(gòu)建的稠密點云地圖生成Octomap，地圖中存儲了點的空間信息，每個小方塊都表示是否被占據(jù)的概率，0為空閑，表示可通行，1為被占據(jù)，表示不可通行。分辨率設置的越高，方塊的體積越小，通過查看每個小方塊是否被占據(jù)可實現(xiàn)移動機器人不同精度的導航。

使用八叉樹的形式存儲地圖，解決了點云地圖中的點沒有體積信息、存儲空間大的問題，不同分辨率的Octomap 所占存儲空間也不同，如表2所示。根據(jù)實際情況，本文將分辨率設為0.05 pixel，使得地圖所占存儲空間由原來的194.30 MB減小至2.28 MB。

表2 占據(jù)存儲空間對比表Table2 Comparison of occupied storage space

將點云轉(zhuǎn)換為Octomap 并存儲顏色信息，設置葉子的大小為5 cm，生成的OctoMap如圖10所示，其中圖10（a）～10（h）分別為書、瓶子、椅子、泰迪熊、杯子、鍵盤、鼠標和顯示屏。

圖10 八叉樹地圖Fig.10 Octree map

3 結(jié)論

本文提出通過緊密結(jié)合目標檢測網(wǎng)絡YOLO v3 和ORB-SLAM2 來構(gòu)建環(huán)境的稠密點云語義地圖。將通過目標檢測及對象正則化得到的物體的2D標簽映射到點云地圖上，結(jié)合深度信息和關(guān)鍵幀生成稠密點云標簽，同時對稠密點云進行基于圖的分割，然后將點云標簽與分割后的點云進行融合得到點云語義地圖，其中相同顏色為同一種類的物體，不同顏色表示不同種類。本文在SLAM的里程計部分添加了對物體的跟蹤，使得相機的絕對位姿誤差和軌跡誤差與傳統(tǒng)的ORB-SLAM2相比分別減少了16.02%和15.86%，提高了建圖精度，將點云地圖保存為Octomap形式，減少了地圖的存儲空間。本系統(tǒng)并不能在ROS系統(tǒng)下實時構(gòu)建點云語義地圖，在未來的研究中將解決這一問題，并實現(xiàn)機器人的導航。