付 豪，徐和根，張志明，齊少華

（同濟(jì)大學(xué)電子與信息工程學(xué)院，上海 201804）

0 引言

隨著計(jì)算機(jī)科學(xué)和傳感器技術(shù)的發(fā)展，機(jī)器人進(jìn)入快速發(fā)展階段。同步定位與地圖構(gòu)建（Simultaneous Localization And Mapping，SLAM）是機(jī)器人導(dǎo)航與定位的基礎(chǔ)技術(shù)之一［1］。由于相機(jī)具有豐富的場景感知能力和價(jià)格低廉等優(yōu)勢，近年來，采用相機(jī)作為主傳感器的視覺SLAM 受到了廣泛的關(guān)注［2］。在最近的研究中，大多數(shù)SLAM系統(tǒng)都采用了靜態(tài)場景假設(shè)，即假設(shè)場景不隨時(shí)間變化［3］。然而在實(shí)際的機(jī)器人應(yīng)用場景中，不可避免地會(huì)存在動(dòng)態(tài)物體。同時(shí)，為了讓機(jī)器人完成更復(fù)雜的任務(wù)，機(jī)器人對場景的理解能力受到了研究者的廣泛關(guān)注［4］。因此，在動(dòng)態(tài)環(huán)境下建立更利于機(jī)器人理解的語義地圖成為一個(gè)研究熱點(diǎn)。

為了解決動(dòng)態(tài)環(huán)境下的機(jī)器人定位問題，F(xiàn)ang 等［5］和Wang 等［6］采用光流法過濾場景中的移動(dòng)物體；Bakkay 等［7］采用基于光流法的改進(jìn)場景流來過濾移動(dòng)物體；Zhao 等［8］使用深度圖像和多視角幾何方法檢測動(dòng)態(tài)物體。這些算法對光照等環(huán)境變化敏感，魯棒性較差。深度學(xué)習(xí)的發(fā)展為這個(gè)問題帶來了新的解決方案?；诰矸e神經(jīng)網(wǎng)絡(luò)（Convolutional Neural Network，CNN），有許多優(yōu)秀的語義分割算法被提出，如SegNet［9］、DeepLab［10］、Mask-RCNN（Mask Region-based CNN）［11］等。語義分割算法可以實(shí)現(xiàn)圖片的像素級(jí)分類，從而獲得圖片中物體的邊界以及語義信息。在深度學(xué)習(xí)的基礎(chǔ)上，Zhong 等［12］使用SSD（Single Shot multibox Detector）檢測圖片中的物體，對于先驗(yàn)標(biāo)記為動(dòng)態(tài)的物體種類，過濾其上的所有特征點(diǎn)，再進(jìn)行后續(xù)的相機(jī)位姿估計(jì)。Bescos 等［13］利用Mask-RCNN 和多視角幾何方法檢測動(dòng)態(tài)物體，并在ORBSLAM2 系統(tǒng)的基礎(chǔ)上提出DynaSLAM 算法，通過Mask-RCNN和多視角幾何方法檢測動(dòng)態(tài)特征點(diǎn)，并將其過濾。Yu等［14］利用SegNet 獲取圖像中的語義信息，并結(jié)合運(yùn)動(dòng)一致性檢測來過濾動(dòng)態(tài)物體上的動(dòng)態(tài)點(diǎn)，在ORB-SLAM 的基礎(chǔ)上提出DSSLAM 算法。這些算法都需要先驗(yàn)性假設(shè)哪些種類的物體會(huì)發(fā)生運(yùn)動(dòng)，如人類。在地圖構(gòu)建上，DynaSLAM 建立了稠密的點(diǎn)云地圖，不利于機(jī)器人的存儲(chǔ)與使用，DS-SLAM 建立了語義八叉樹［15］地圖。

本文針對動(dòng)態(tài)場景下的相機(jī)位姿估計(jì)以及語義地圖構(gòu)建問題進(jìn)行研究，主要的工作如下：

1）結(jié)合了語義分割和光流，提出了一種新的動(dòng)態(tài)特征點(diǎn)過濾算法，在取得較好的過濾效果的同時(shí)降低了對先驗(yàn)知識(shí)的依賴性。

2）提出了一種動(dòng)態(tài)環(huán)境下的靜態(tài)語義地圖構(gòu)建算法。在保留主體信息的同時(shí)，極大降低了存儲(chǔ)空間的需求，能夠更好地應(yīng)用于后續(xù)地任務(wù)。

在公開的TUM 數(shù)據(jù)集上將本文提出的算法與ORBSLAM2、DS-SLAM 以及DynaSLAM 作對比，實(shí)驗(yàn)結(jié)果驗(yàn)證了本文算法的有效性。

1 系統(tǒng)設(shè)計(jì)

1.1 系統(tǒng)框架

ORB-SLAM2 系統(tǒng)作為目前最為完整、穩(wěn)定的開源視覺SLAM 系統(tǒng)之一，得到了研究人員的廣泛使用。本文所提出的動(dòng)態(tài)場景SLAM 算法也是在該系統(tǒng)框架上進(jìn)行改進(jìn)的。圖1 是本文提出的SLAM 算法的完整框架，在原有的跟蹤、局部建圖和回環(huán)檢測線程的基礎(chǔ)上，增加了動(dòng)態(tài)物體檢測和語義地圖構(gòu)建線程。圖中短虛線框選的部分為本文添加的線程，長虛線框選的部分為ORB-SLAM2 原有線程。首先，攝像機(jī)獲取的每一幀在進(jìn)行跟蹤線程之前要經(jīng)過語義分割網(wǎng)絡(luò)，得到包含物體語義標(biāo)簽的掩模，并聯(lián)合光流法計(jì)算物體的動(dòng)態(tài)概率。局部建圖和回環(huán)檢測線程與ORB-SLAM2 相同。最后，根據(jù)本文提出的關(guān)鍵幀選擇策略挑選部分關(guān)鍵幀用于構(gòu)建靜態(tài)語義地圖。本文算法通過物體動(dòng)態(tài)概率去除動(dòng)態(tài)點(diǎn)，建立靜態(tài)點(diǎn)云圖，并利用語義信息更新語義八樹圖；基于語義分割，對場景中的物體點(diǎn)云進(jìn)行分割與聚類，建立稀疏語義地圖。

圖1 本文算法總體框架Fig.1 Overall framework of proposed algorithm

1.2 動(dòng)態(tài)特征點(diǎn)過濾

本文提出基于語義和光流約束的動(dòng)態(tài)特征點(diǎn)過濾算法，整體框架如圖2所示。

圖2 動(dòng)態(tài)特征點(diǎn)過濾算法的框架Fig.2 Framework of dynamic feature point filtering algorithm

該算法首先提取當(dāng)前幀的ORB 特征，同時(shí)通過語義分割網(wǎng)絡(luò)得到物體的掩碼；接著，采用多視角幾何方法初步過濾當(dāng)前幀中不符合極線約束的特征點(diǎn)；然后，通過滑動(dòng)窗口法計(jì)算出當(dāng)前幀和5 幀前的稠密光流；最后，結(jié)合語義分割與光流得到物體的動(dòng)態(tài)概率。在有人、顯示器和椅子的圖中，人的動(dòng)態(tài)概率為0.971，顯示器的動(dòng)態(tài)概率為0.047，椅子的動(dòng)態(tài)概率為0.014。因?yàn)槿说膭?dòng)態(tài)概率大于閾值0.5，得以確定人是動(dòng)態(tài)物體。對人身上的特征點(diǎn)也進(jìn)行過濾，得到最終的靜態(tài)特征點(diǎn)。

1.2.1 語義分割網(wǎng)絡(luò)

本文采用DeepLab v3語義分割網(wǎng)絡(luò)。網(wǎng)絡(luò)采用編解碼結(jié)構(gòu)，并通過空洞卷積平衡精度和耗時(shí)。

本文在PASCAL VOC 數(shù)據(jù)集上訓(xùn)練DeepLab v3，該數(shù)據(jù)集包含20 個(gè)類別，如車輛、人類、椅子、顯示器等。使用TUM數(shù)據(jù)集中的RGB 圖片進(jìn)行測試，圖3 顯示了語義分割算法的結(jié)果。其中，紅色代表人類，綠色代表顯示器，藍(lán)色代表椅子。這些都是室內(nèi)場景下可能具有移動(dòng)屬性的物體。

圖3 語義分割網(wǎng)絡(luò)的分割結(jié)果Fig.3 Segmentation results of semantic segmentation network

1.2.2 幾何約束

語義分割網(wǎng)絡(luò)不能識(shí)別場景中的所有物體。為了過濾網(wǎng)絡(luò)無法識(shí)別的物體種類上的動(dòng)態(tài)特征點(diǎn)，本文采用幾何約束。對極幾何約束是攝像機(jī)運(yùn)動(dòng)學(xué)的重要組成部分，只與攝像機(jī)的內(nèi)參和相機(jī)位姿有關(guān)，如圖4所示。

圖4 對極幾何示意圖Fig.4 Schematic diagram of epipolar geometry

假設(shè)相機(jī)從不同的角度觀測到同一個(gè)空間點(diǎn)p。根據(jù)針孔相機(jī)模型，它在兩個(gè)圖像上的像素坐標(biāo)x=[u v1]T，即x1、x2符合約束：

其中：K代表相機(jī)的內(nèi)參矩陣；R、T分別表示兩個(gè)相機(jī)坐標(biāo)系間的旋轉(zhuǎn)與平移矩陣；s表示像素點(diǎn)的深度信息。

在理想情況下，兩張圖片中匹配的點(diǎn)對的坐標(biāo)符合約束：

其中，F(xiàn)為基礎(chǔ)矩陣（Fundamental matrix）。而在真實(shí)的場景下，由于相機(jī)采集的照片并非理想圖片，存在一定程度的畸變和噪聲，使得相鄰幀間的點(diǎn)無法完美匹配上極線l：

其中：X、Y和Z為極線的向量。點(diǎn)x2到極線l1的距離D為：

若距離D大于閾值，則認(rèn)為該點(diǎn)不符合極線約束。有兩種原因造成這種結(jié)果：首先，這些點(diǎn)本身是不匹配的；其次，這些點(diǎn)存在于動(dòng)態(tài)物體上，這些點(diǎn)隨著物體的運(yùn)動(dòng)而移動(dòng)，造成了不匹配，這些點(diǎn)為動(dòng)態(tài)點(diǎn)。因此，過濾所有不符合極線約束的特征點(diǎn)。

1.2.3 光流約束

光流法是運(yùn)動(dòng)檢測中的常用方法。光流法計(jì)算圖像序列中的像素在時(shí)間域上的變化以及幀間的關(guān)聯(lián)性，從而得出相鄰幀中物體的運(yùn)動(dòng)關(guān)系。光流法主要有以下三個(gè)假設(shè)：圖像中的像素亮度在連續(xù)幀間不會(huì)發(fā)生變化；幀間的時(shí)間間隔相對較短；相鄰像素具有相似的運(yùn)動(dòng)。從而有：

其中：I(u，v，t)代表像素點(diǎn)(u，v)在t幀的光強(qiáng)度。式（5）表明像素點(diǎn)(u，v)在dt時(shí)間內(nèi)移動(dòng)了(du，dv)像素距離。

U、V定義了光流值在單位時(shí)間內(nèi)橫縱坐標(biāo)下的像素偏移：

由于相機(jī)的采樣頻率較高（通常為30 frame/s），關(guān)聯(lián)的兩幀之間物體運(yùn)動(dòng)通常不明顯，為此，本文采用滑動(dòng)視窗法，設(shè)置視窗為5。前端采集當(dāng)前幀與5幀以前的圖像計(jì)算光流場。

本文采用DeepFlow 算法［16］計(jì)算稠密光流。由于攝像機(jī)的不規(guī)則運(yùn)動(dòng)會(huì)造成前景和背景的同時(shí)運(yùn)動(dòng)，因此很難判斷像素點(diǎn)的偏移是由攝像機(jī)運(yùn)動(dòng)還是物體運(yùn)動(dòng)引起的。如圖5所示，直接計(jì)算光流時(shí)，由于攝像機(jī)本身的運(yùn)動(dòng)，整個(gè)畫面都有較大的光流值。為了使光流可視化，每個(gè)點(diǎn)的像素值被填充為

圖5 直接計(jì)算光流的結(jié)果Fig.5 Results of direct calculation of optical flow

由于為了盡可能消除背景運(yùn)動(dòng)對光流的影響，根據(jù)1.2.2 節(jié)得到的靜態(tài)特征點(diǎn)對，采用隨機(jī)抽樣一致算法（RANdom SAmple Consensus，RANSAC）算法，計(jì)算兩幀間的基礎(chǔ)矩陣F。根據(jù)基礎(chǔ)矩陣，求解前第5幀的射影變換，之后，對變換后的圖片計(jì)算稠密光流，結(jié)果如圖6 所示。經(jīng)過變換后，計(jì)算出的光流場基本只包含運(yùn)動(dòng)物體，在這兩幀中只有部分人體被檢測出來。光流的可視化方法和圖4相同。

圖6 矯正后的光流計(jì)算結(jié)果Fig.6 Results of optical flow calculation after correction

從圖6 中可以看出，利用光流法難以得到物體清晰的輪廓，而采用語義分割的方法難以判斷物體是否處于運(yùn)動(dòng)狀態(tài)。本文算法對語義和光流約束進(jìn)行結(jié)合，對于圖片中的每一個(gè)像素點(diǎn)n，計(jì)算其動(dòng)態(tài)概率pn：

因此對于每一個(gè)物體，其動(dòng)態(tài)概率pobj為物體上所有像素點(diǎn)動(dòng)態(tài)概率的平均值：

在沒有先驗(yàn)的情況下（事先不指定運(yùn)動(dòng)物體的類別），如果pobj>0.5，則認(rèn)為該物體是動(dòng)態(tài)的，需要對該物體上的特征點(diǎn)進(jìn)行過濾；否則，則認(rèn)為該物體是靜態(tài)的，可以用于后續(xù)的跟蹤以及地圖構(gòu)建任務(wù)。

1.3 語義地圖構(gòu)建

地圖構(gòu)建是SLAM 系統(tǒng)的重要組成部分。在ORBSLAM2 系統(tǒng)中，只構(gòu)建了基于特征點(diǎn)的稀疏地圖，不利于機(jī)器人導(dǎo)航等任務(wù)的使用。本文算法使用RGB-D 圖像，結(jié)合語義信息和稠密運(yùn)動(dòng)概率，構(gòu)建靜態(tài)點(diǎn)云地圖，并更新語義八叉樹地圖，最終構(gòu)建出稀疏語義地圖，便于后續(xù)任務(wù)的使用。

1.3.1 關(guān)鍵幀選擇

對于SLAM 系統(tǒng)來說，視覺傳感器傳入的圖像數(shù)量非常多，為了減少系統(tǒng)資源的消耗，ORB-SLAM2 采取選擇關(guān)鍵幀的策略方法。但是，這種選擇策略是針對ORB-SLAM2 設(shè)計(jì)的，并不完全適合本文算法的語義地圖構(gòu)建。在本文中，對ORB-SLAM2的關(guān)鍵幀序列進(jìn)行了二次篩選。

對于關(guān)鍵幀序列，第一幀直接構(gòu)建地圖，后續(xù)的每一幀都需要滿足以下兩個(gè)條件：

首先，在一個(gè)新的關(guān)鍵幀傳入后，計(jì)算當(dāng)前關(guān)鍵幀和前一個(gè)地圖構(gòu)建關(guān)鍵幀的相機(jī)平移和旋轉(zhuǎn)。當(dāng)攝像機(jī)平移大于0.3 m 或旋轉(zhuǎn)角度大于5°時(shí)，將該關(guān)鍵幀加入到地圖構(gòu)建幀序列中，否則跳過該關(guān)鍵幀。

其次，在動(dòng)態(tài)場景中，物體可能會(huì)發(fā)生移動(dòng)。當(dāng)發(fā)生移動(dòng)時(shí)，即使攝像機(jī)位置保持穩(wěn)定，也需要更新地圖中的物體。采用了常用指標(biāo)交并比（Intersection over Union，IoU）評(píng)測物體檢測。在新增加的關(guān)鍵幀F(xiàn)i與上一幀F(xiàn)i-1中，計(jì)算所有靜態(tài)物體掩模的IoU：

1.3.2 靜態(tài)點(diǎn)云地圖構(gòu)建

本文在ORB-SLAM2 基礎(chǔ)上新增點(diǎn)云構(gòu)建線程。經(jīng)過1.3.1 節(jié)的關(guān)鍵幀過濾后，得到用于地圖構(gòu)建的關(guān)鍵幀序列。對于序列中的每一幀，利用式（10），結(jié)合SLAM 系統(tǒng)在運(yùn)行過程中獲得的相機(jī)位置姿勢信息和圖像的深度值，將點(diǎn)p從圖像坐標(biāo)系轉(zhuǎn)換到世界坐標(biāo)系中，構(gòu)建點(diǎn)云。

其中：K代表相機(jī)的內(nèi)參矩陣；R和T分別代表相機(jī)的旋轉(zhuǎn)和平移；d代表像素點(diǎn)的深度值；[u v1]T代表點(diǎn)p在圖片坐標(biāo)系下的位置；[a b b]T代表點(diǎn)p在空間坐標(biāo)系下的位置。

如果地圖中包含動(dòng)態(tài)物體，地圖將難以使用。在常見的抓取或避障任務(wù)中，包含動(dòng)態(tài)物體的地圖使得算法難以判斷物體是否還存在，障礙物是否可以通過。因此，結(jié)合1.3.1 節(jié)得到的動(dòng)態(tài)物體掩模以及光流值，對動(dòng)態(tài)物體進(jìn)行過濾。

對輸入圖像中的每一個(gè)像素點(diǎn)n，如果n是某個(gè)物體上的點(diǎn)，那么其動(dòng)態(tài)概率pn=pobj，否則按照式（7）計(jì)算。如果pn>0.5，該點(diǎn)被視為動(dòng)態(tài)點(diǎn)，不能用于點(diǎn)云地圖構(gòu)建。從圖7中可以看出，經(jīng)過過濾后的靜態(tài)點(diǎn)云地圖幾乎不包含動(dòng)態(tài)點(diǎn)。通過聯(lián)合多視角下的不同關(guān)鍵幀，對背景進(jìn)行填充，構(gòu)建靜態(tài)點(diǎn)云地圖。

圖7 根據(jù)動(dòng)態(tài)物體掩膜和光流過濾動(dòng)態(tài)點(diǎn)建立靜態(tài)點(diǎn)云圖Fig.7 Using moving object masks and optical flow to filter out dynamic points for constructing static point cloud map

1.3.3 語義八叉樹地圖構(gòu)建

雖然點(diǎn)云的表示方式很直觀，但點(diǎn)云圖也會(huì)存在一些不足。點(diǎn)云地圖占用存儲(chǔ)空間，而在大部分情況下，很多點(diǎn)的位置信息都是冗余的。點(diǎn)云地圖不能直接用于導(dǎo)航任務(wù)。因此，本文將點(diǎn)云轉(zhuǎn)化為八叉樹地圖，構(gòu)建全局語義八樹圖。

八叉樹地圖使用八叉樹來存儲(chǔ)地圖，最小節(jié)點(diǎn)大小由分辨率決定。由于存在攝像機(jī)的噪聲和動(dòng)態(tài)物體等干擾因素，同一個(gè)體素在不同時(shí)間處于不同的狀態(tài)。因此，每個(gè)體素的占用概率應(yīng)結(jié)合多次觀測的聯(lián)合概率得到：

將占據(jù)概率進(jìn)行Logit變換：

體素被占據(jù)的更新方式可以表示為：

其中：ZT=1 表示該體素在時(shí)間T被觀察到，ZT=0 表示該體素在時(shí)間T沒有被觀察到；locc=0.8為預(yù)設(shè)值。當(dāng)一個(gè)節(jié)點(diǎn)被反復(fù)觀察到時(shí)，其體素的占用概率會(huì)增加，與此同時(shí)，其語義信息也被賦予，當(dāng)一個(gè)節(jié)點(diǎn)沒有被觀察到時(shí)，其占用概率會(huì)降低。而動(dòng)態(tài)物體的運(yùn)動(dòng)概率越高，其占用概率會(huì)降低，反之則會(huì)增加。當(dāng)一個(gè)體素的占用概率大于預(yù)設(shè)閾值（0.8）時(shí)，該節(jié)點(diǎn)將在地圖中顯示出來。

1.3.4 稀疏語義地圖構(gòu)建

在某些情況下，系統(tǒng)更關(guān)注場景中目標(biāo)物體。例如，在抓取任務(wù)中，機(jī)器人手臂更關(guān)注待抓取目標(biāo)的位置信息。為了方便后續(xù)任務(wù)能夠更方便地搜索目標(biāo)，本文建立了一個(gè)稀疏語義地圖。

直接對點(diǎn)云進(jìn)行分割是難以得到準(zhǔn)確結(jié)果的。基于算法性能的限制，語義分割得到的掩模與物體也并不完全對應(yīng)，尤其是在物體邊界處。在大多數(shù)情況下，二維圖像中難以精確劃分的物體邊界在三維空間中距離較遠(yuǎn)，容易區(qū)分。因此，本文采用了一種基于語義分割的點(diǎn)云分割方法。該方法的步驟如下：

步驟1 利用關(guān)鍵幀投影到世界坐標(biāo)構(gòu)建三維點(diǎn)云；

步驟2 對構(gòu)建的點(diǎn)云進(jìn)行體素化濾波，過濾由圖片噪聲產(chǎn)生的點(diǎn)云；

步驟3 根據(jù)點(diǎn)云和圖片的對應(yīng)關(guān)系，將語義信息綁定到點(diǎn)云中；

步驟4 基于語義信息對點(diǎn)云進(jìn)行歐氏聚類，分割出每一個(gè)物體。

基于語義分割的點(diǎn)云分割結(jié)果如圖8所示。

圖8 基于語義分割的點(diǎn)云分割結(jié)果Fig.8 Point cloud segmentation results based on semantic segmentation

為了更簡單地表示物體，針對每一個(gè)物體的點(diǎn)云，通過主成分分析（Principal Component Analysis，PCA）獲取特征向量，構(gòu)建方向包圍盒（Oriented Bounding Box，OBB）。通過具有方向性的最小包圍盒表征物體，它很容易用于導(dǎo)航中的碰撞檢測等任務(wù)，結(jié)果如圖9（a）所示。

在得到每一類物體的邊界框后，根據(jù)1.3.3 節(jié)得到的物體動(dòng)態(tài)概率，去除動(dòng)態(tài)物體。如果pobj>0.5，則算法認(rèn)為該物體是動(dòng)態(tài)的，不能添加到地圖中。在這一幀中，pchair=0.03，pmonitor=0.07。因此，判斷人是動(dòng)態(tài)物體，將其過濾掉。如圖9（b）所示，利用顯示器和椅子的邊界框來構(gòu)建靜態(tài)稀疏語義地圖。

圖9 物體包圍盒Fig.9 Object bounding box

在動(dòng)態(tài)場景中，物體的位置可能會(huì)發(fā)生變化，所以需要更新地圖中物體的位置。本文算法將稀疏語義地圖以物體數(shù)據(jù)庫的形式存放，用于存儲(chǔ)物體的類別和位置信息。數(shù)據(jù)庫中包含以下信息：每一個(gè)物體的包圍框世界坐標(biāo)和它的物體種類屬性。本文算法按照如圖10所示的策略更新語義圖。

圖10 語義地圖更新流程Fig.10 Flow chart of semantic map updating

當(dāng)獲得一個(gè)新的靜態(tài)物體語義時(shí)，如果數(shù)據(jù)庫中沒有同類物體，則直接將新物體添加到數(shù)據(jù)庫中。當(dāng)數(shù)據(jù)庫中存在同類物體，如果新物體與數(shù)據(jù)庫中所有同類物體邊界框沒有重合，則將新物體添加到數(shù)據(jù)庫中。如果邊界框重合，即與數(shù)據(jù)庫中已有物體距離小于閾值，則算法認(rèn)為物體發(fā)生移動(dòng)，更新數(shù)據(jù)庫中物體的位置。

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

2.1 實(shí)驗(yàn)設(shè)置

實(shí)驗(yàn)主要在TUM 數(shù)據(jù)集上進(jìn)行測試，主要包括兩個(gè)方面：一是SLAM系統(tǒng)的跟蹤定位性能；二是地圖構(gòu)建任務(wù)。

TUM數(shù)據(jù)集是用于評(píng)估SLAM算法性能的經(jīng)典RGB-D數(shù)據(jù)集。它被廣泛用于測試SLAM 算法在動(dòng)態(tài)環(huán)境下的定位準(zhǔn)確性和魯棒性。該數(shù)據(jù)集包含39 個(gè)不同的序列，每個(gè)序列有640 × 480大小的彩色圖像、與之匹配的深度圖像和攝像機(jī)的真實(shí)軌跡。其中，相機(jī)的真實(shí)軌跡是由高速運(yùn)動(dòng)捕捉系統(tǒng)確定的，以確保軌跡的準(zhǔn)確性。它可以很容易地與SLAM 系統(tǒng)估計(jì)的軌跡進(jìn)行比較，以評(píng)估算法的魯棒性。

本測試運(yùn)行在一臺(tái)PC 上，CPU 為AMD 銳龍R52600，內(nèi)存為16 GB，顯卡為NVIDIA 1660。系統(tǒng)環(huán)境為Ubuntu 16.04，語義分割網(wǎng)絡(luò)采用Python3.6 編寫，SLAM 主程序使用C++編寫。

2.2 定位精度測試

SLAM 系統(tǒng)的定位精度是評(píng)價(jià)SLAM 系統(tǒng)性能以及魯棒性的重要部分。本文在TUM 數(shù)據(jù)集上進(jìn)行了兩類場景的測試，即高動(dòng)態(tài)場景和低動(dòng)態(tài)場景。在高動(dòng)態(tài)場景中，人在場景中持續(xù)行走，因此，以下將高動(dòng)態(tài)場景簡稱為W（walking）；在低動(dòng)態(tài)場景中，人坐在椅子上，沒有明顯的動(dòng)作，因此，以下將低動(dòng)態(tài)場景簡稱為S（sitting）。每個(gè)場景都包含四種不同的攝像機(jī)運(yùn)動(dòng)軌跡，分別是halfsphere、rpy、static 和xyz。在halfsphere（以下簡稱hs）軌跡中，相機(jī)沿著半球運(yùn)動(dòng)；在rpy 軌跡中，相機(jī)進(jìn)行搖擺、俯仰運(yùn)動(dòng)；在static（以下簡稱s）軌跡中，相機(jī)的位置幾乎沒有變化；在xyz 軌跡中，攝像頭沿著x、y和z軸運(yùn)動(dòng)。

本文使用絕對軌跡誤差（Absolute Trajectory Error，ATE）和相對位姿誤差（Relative Pose Error，RPE）來評(píng)估算法在定位上的性能。為了更好地反映SLAM 系統(tǒng)的魯棒性，采用平均數(shù)、中位數(shù)、標(biāo)準(zhǔn)差（Standard Deviation，SD）和均方根誤差（Root Mean Square Error，RMSE）作為評(píng)價(jià)指標(biāo)。其中：平均數(shù)和中位數(shù)可以更直觀地顯示誤差的整體情況；而標(biāo)準(zhǔn)差可以更好地反映系統(tǒng)的穩(wěn)定性；RMSE 對異常值比較敏感，可以反映系統(tǒng)的魯棒性。表1 為軌跡絕對誤差，表2 為RPE 的平移誤差，表3為RPE的旋轉(zhuǎn)誤差。

表1 絕對軌跡誤差結(jié)果比較Tab.1 Result comparison of absolute trajectory error

表2 相對位姿誤差的平移誤差結(jié)果比較Tab.2 Result comparison of translation error of relative pose error

表3 相對位姿誤差的旋轉(zhuǎn)誤差結(jié)果比較Tab.3 Result comparison of rotation error of relative pose error

表1～3 中，W 代表高動(dòng)態(tài)場景，S 代表低動(dòng)態(tài)場景，hs、rpy等分別代表對應(yīng)的運(yùn)動(dòng)軌跡。W-hs 即代表在高動(dòng)態(tài)場景下，相機(jī)以halfsphere 的軌跡進(jìn)行運(yùn)動(dòng)，其余的命名方式以此類推。通過對ATE和RPE的評(píng)估，從表1～3中可以看出，本文所提出的算法在高動(dòng)態(tài)場景下的性能相較ORB-SLAM2 有很大的提升。性能的提升Adv計(jì)算方法如式（15）所示：

其中：Ecom表示用于比較的算法的誤差；Eour表示本文所提出算法的誤差。從表1～3 中還可以看出，在所有的高動(dòng)態(tài)場景中，本文算法都有95%以上的提升。在W-xyz軌跡中，相機(jī)大范圍的運(yùn)動(dòng)，場景中的人也在不斷移動(dòng)，這對SLAM 系統(tǒng)來說是一個(gè)很大的挑戰(zhàn)。在W-xyz 軌跡的一些評(píng)價(jià)指標(biāo)中，本文算法相較ORB-SLAM2減小了98%的誤差。

在低動(dòng)態(tài)場景中，由于ORB-SLAM2也具有一定的動(dòng)態(tài)特征點(diǎn)過濾能力，本文算法表現(xiàn)與ORB-SLAM2相似。

同時(shí)，還將本文算法與最先進(jìn)的動(dòng)態(tài)場景下的SLAM 算法，即DynaSLAM 和DS-SLAM 在高動(dòng)態(tài)場景下對系統(tǒng)的定位性能進(jìn)行了比較，以ATE的RMSE作為比較項(xiàng)。

在實(shí)驗(yàn)室環(huán)境中，對本文算法進(jìn)行測試。由于沒有TUM數(shù)據(jù)集中的高速運(yùn)動(dòng)捕捉相機(jī)，難以確定相機(jī)的真實(shí)位姿，本文控制相機(jī)在實(shí)驗(yàn)室場景中沿著標(biāo)準(zhǔn)的矩形行走，矩形的大小為4 m×4 m。如圖11所示，場景中有人員在不斷走動(dòng)。

圖11 實(shí)驗(yàn)室環(huán)境圖片F(xiàn)ig.11 Photograph of laboratory environment

DynaSLAM 和DS-SLAM 算法的性能采用其原文中的數(shù)據(jù)，對比結(jié)果如表4 所示。從表4 可以看出，本文算法在所有數(shù)據(jù)集中的定位誤差都小于DS-SLAM；在W-rpy 和W-xyz 軌跡中，相較于DynaSLAM 有一定的定位性能提升。對比算法都需要先驗(yàn)地指定場景中的運(yùn)動(dòng)物體種類，而本文的算法無需先驗(yàn)知識(shí)，也能實(shí)現(xiàn)較好的性能。

表4 不同算法的絕對軌跡誤差對比 單位：mTab.4 Comparison of absolute trajectory error of different algorithms unit：m

圖12 顯示了ORB-SLAM2 和本文算法在高動(dòng)態(tài)場景下估計(jì)的軌跡。ORB-SLAM2 簡稱為ORB，W-hs-ORB 表示ORBSLAM2 算法在高動(dòng)態(tài)場景下，算法對hs 軌跡的估計(jì)結(jié)果，其他圖片的命名規(guī)則相同。圖中，黑色線條是真實(shí)的軌跡（ground truth），紅色線條（深灰）是SLAM 系統(tǒng)估計(jì)的軌跡（estimated），藍(lán)色線條（淺灰）是估計(jì)軌跡與真實(shí)軌跡之間的誤差（difference）。

從圖12 可以看出，ORB-SLAM2 算法在高動(dòng)態(tài)場景下，估計(jì)軌跡與真實(shí)值有較大的誤差。相比之下，本文算法與真實(shí)軌跡更加接近，可以有效去除動(dòng)態(tài)環(huán)境下的干擾，魯棒性較好。

圖12 不同算法在高動(dòng)態(tài)場景下估計(jì)的軌跡對比Fig.12 Comparison of trajectories estimated by different algorithms in highly dynamic scenes

不同算法估計(jì)的軌跡結(jié)果如圖13 所示。由于缺少實(shí)際軌跡的絕對世界坐標(biāo)，算法的性能難以量化。但從圖13 中可以看出，與ORB-SLAM2 相比，本文提出的算法估計(jì)的軌跡更接近4 × 4的矩形，算法在真實(shí)環(huán)境也有較好的表現(xiàn)。

圖13 實(shí)驗(yàn)室環(huán)境下不同算法估計(jì)的軌跡Fig.13 Trajectories estimated by different algorithms in laboratory environment

2.3 運(yùn)行時(shí)間

本文在ORB-SLAM2 的基礎(chǔ)上新增了動(dòng)態(tài)物體過濾以及語義地圖過濾線程。由于地圖構(gòu)建只對關(guān)鍵幀作處理，且該線程與主線程并行執(zhí)行，對算法運(yùn)行效率影響不大。對于每一幀輸入圖像，動(dòng)態(tài)物體過濾線程每一模塊的運(yùn)行時(shí)間如表5所示。

從表5 中可以看出，對于算法的實(shí)時(shí)性影響最大的是語義分割模塊。在目前的測試平臺(tái)下，算法還無法實(shí)現(xiàn)實(shí)時(shí)檢測。在未來，更換硬件條件更好的測試平臺(tái)或采用高精度且快速的語義分割網(wǎng)絡(luò)，系統(tǒng)的實(shí)時(shí)性可以得到進(jìn)一步提升。

表5 不同模塊的運(yùn)行時(shí)間Tab.5 Running times of different modules

2.4 建圖測試

本文算法構(gòu)建了三種類型的地圖，分別是點(diǎn)云地圖、語義八樹地圖和稀疏語義地圖。本節(jié)將會(huì)構(gòu)建三類地圖并進(jìn)行評(píng)估。由于本文的算法主要針對高動(dòng)態(tài)場景，所以本節(jié)只對算法在高動(dòng)態(tài)場景下的性能進(jìn)行比較。

2.4.1 點(diǎn)云地圖

在本節(jié)中，測試了三種方法的地圖構(gòu)建性能，分別為：直接根據(jù)ORB-SLAM2 建立點(diǎn)云地圖，根據(jù)光流過濾動(dòng)態(tài)物體，以及本文的基于語義和光流約束的靜態(tài)點(diǎn)云構(gòu)建算法。數(shù)據(jù)集的表示方法和前文相同，ORB-SLAM2 簡稱為ORB，OF 代表只用光流過濾動(dòng)態(tài)點(diǎn)。從圖14 來看，本文算法構(gòu)建的地圖更接近真實(shí)場景，而由ORB-SLAM2 構(gòu)建的地圖有很多動(dòng)態(tài)點(diǎn)。在W-static（W-s）軌跡中，攝像機(jī)位置相對固定，在ORBSLAM2 構(gòu)建的地圖中可以看到背景的大致輪廓。由于光流難以得到物體的完整輪廓，使得只使用光流時(shí)無法過濾掉所有的動(dòng)態(tài)點(diǎn)，其結(jié)果如圖14（b）、（e）所示，地圖中仍可以看到人體。本文算法利用語義約束提供了更好的物體邊界分割，使得過濾大部分動(dòng)態(tài)點(diǎn)成為可能。從圖14（c）、（f）中可以看出，本文算法建立的點(diǎn)云圖幾乎不包含動(dòng)態(tài)點(diǎn)，可以更好地表現(xiàn)靜態(tài)場景。

圖14 稠密點(diǎn)云地圖Fig.14 Dense point cloud map

2.4.2 語義地圖構(gòu)建

動(dòng)態(tài)場景下生成的語義八叉樹地圖如圖15（a）、（d）所示。為了便于可視化，代表顯示器的體素用綠色顯示，而代表椅子的體素用藍(lán)色顯示，其余體素顯示物體本身的顏色。圖中不存在典型的動(dòng)態(tài)物體如人類，說明較好地完成了靜態(tài)地圖的構(gòu)建工作。從圖中的體素顏色覆蓋區(qū)域可以看出，顯示器部分體素的語義得到較好的賦予，可以從圖中識(shí)別出來，但椅子部分相對較差。這主要是因?yàn)檎Z義分割網(wǎng)絡(luò)不能很好地分割出椅子。本文分割網(wǎng)絡(luò)在VOC 數(shù)據(jù)集上進(jìn)行訓(xùn)練，其中的椅子與TUM 數(shù)據(jù)集中的椅子差異較大，這使得算法對于椅子難以得到較好的分割結(jié)果。

圖15 語義地圖Fig.15 Semantic map

基于點(diǎn)云圖和語義信息，本文生成了一個(gè)稀疏語義地圖，如圖15（b）、（e）所示。圖中綠色（淺灰）包圍框代表顯示器，而藍(lán)色（深灰）包圍框代表椅子。該圖可以更直觀地了解物體的位置。由于缺乏數(shù)據(jù)集中的物體真實(shí)位置信息，本文將目標(biāo)的三維包圍框以覆蓋在點(diǎn)云圖的形式展示，可以用來直觀地了解檢測的準(zhǔn)確性，如圖15（c）、（f）所示，顯示器的邊界框相對準(zhǔn)確，而椅子的邊界框只包含椅子的一部分，主要原因是在語義分割網(wǎng)絡(luò)中很難獲得椅子的準(zhǔn)確像素級(jí)分割。如果對于場景中的物體有完整的數(shù)據(jù)集可以供網(wǎng)絡(luò)有監(jiān)督地學(xué)習(xí)，算法的性能可以得到一定程度的提升。

稀疏的語義地圖在保留主要信息的同時(shí)，可以極大降低存儲(chǔ)空間的要求，地圖文件的存儲(chǔ)需求如表6所示。從表6中可以看出，相較點(diǎn)云地圖，八叉樹地圖所需的存儲(chǔ)空間顯著降低，而本文建立的稀疏語義地圖存儲(chǔ)空間進(jìn)一步減少，更有利于嵌入式系統(tǒng)的存儲(chǔ)。

表6 地圖文件存儲(chǔ)空間對比Tab.6 Comparison of map file storage space

3 結(jié)語

為了減少動(dòng)態(tài)物體對SLAM 系統(tǒng)的干擾，本文提出了一種針對動(dòng)態(tài)環(huán)境的更為魯棒的視覺SLAM 算法。該算法建立在ORB-SLAM2的基礎(chǔ)上，利用語義和光流約束來過濾動(dòng)態(tài)物體。首先，利用語義分割網(wǎng)絡(luò)來獲取幀中每一類物體的像素級(jí)掩碼；然后，通過光流匹配得到每個(gè)物體的動(dòng)態(tài)概率，動(dòng)態(tài)概率高的物體將被過濾。在公共TUM 數(shù)據(jù)集上對本文算法以及其他先進(jìn)的SLAM 算法進(jìn)行對比，驗(yàn)證了本文算法在高動(dòng)態(tài)場景下具有更好的定位精度和魯棒性。在地圖構(gòu)建方面，本文構(gòu)建了靜態(tài)點(diǎn)云圖、語義八樹圖和稀疏語義圖。實(shí)驗(yàn)結(jié)果表明，本文算法有效地過濾掉了動(dòng)態(tài)物體，構(gòu)建了靜態(tài)地圖。同時(shí)，構(gòu)建的稀疏語義地圖在保留主要信息的同時(shí)，極大降低了存儲(chǔ)空間的要求。但是本文算法的性能受到語義分割結(jié)果的影響，不準(zhǔn)確的分割結(jié)果將對后續(xù)工作產(chǎn)生不利影響。在以后的工作中，考慮采用點(diǎn)云分割網(wǎng)絡(luò)來提高分割結(jié)果。