席志紅 韓雙全 王洪旭

摘 要：針對(duì)動(dòng)態(tài)物體在室內(nèi)同步定位與地圖構(gòu)建（SLAM）系統(tǒng)中影響位姿估計(jì)的問題，提出一種動(dòng)態(tài)場景下基于語義分割的SLAM系統(tǒng)。在相機(jī)捕獲圖像后，首先用PSPNet（Pyramid Scene Parsing Network）對(duì)圖像進(jìn)行語義分割;之后提取圖像特征點(diǎn)，剔除分布在動(dòng)態(tài)物體內(nèi)的特征點(diǎn)，并用靜態(tài)的特征點(diǎn)進(jìn)行相機(jī)位姿估計(jì);最后完成語義點(diǎn)云圖和語義八叉樹地圖的構(gòu)建。在公開數(shù)據(jù)集上的五個(gè)動(dòng)態(tài)序列進(jìn)行多次對(duì)比測試的結(jié)果表明，相對(duì)于使用SegNet網(wǎng)絡(luò)的SLAM系統(tǒng)，

所提系統(tǒng)的絕對(duì)軌跡誤差的標(biāo)準(zhǔn)偏差有6.9%～89.8%的下降，平移和旋轉(zhuǎn)漂移的標(biāo)準(zhǔn)偏差在高動(dòng)態(tài)場景中的最佳效果也能分別提升73.61%和72.90%。結(jié)果表明，改進(jìn)的系統(tǒng)能夠顯著減小動(dòng)態(tài)場景下位姿估計(jì)的誤差，準(zhǔn)確地在動(dòng)態(tài)場景中進(jìn)行相機(jī)位姿估計(jì)。關(guān)鍵詞：語義分割;動(dòng)態(tài)場景;室內(nèi)場景;位姿估計(jì);視覺同步定位與地圖構(gòu)建;語義同步定位與地圖構(gòu)建

中圖分類號(hào)：TP242.6

文獻(xiàn)標(biāo)志碼：A

Abstract： To address the problem that dynamic objects affect pose estimation in indoor Simultaneous Localization And Mapping （SLAM） systems， a semantic segmentation based SLAM system in dynamic scenes was proposed. Firstly， an image was semantically segmented by the Pyramid Scene Parsing Network （PSPNet） after being captured by the camera. Then image feature points were extracted， feature points distributed in the dynamic object were removed， and camera pose was estimated by using static feature points. Finally， the semantic point cloud map and semantic octree map were constructed. Results of multiple comparison tests on five dynamic sequences of public datasets show that compared with the SLAM system using SegNet network， the proposed system has the standard deviation of absolute trajectory error improved by 6.9%-89.8%， and has the standard deviation of translation and rotation drift improved by 73.61% and 72.90% respectively in the best case in high dynamic scenes. The results show that the improved method can significantly reduce the error of pose estimation in dynamic scenes， and can correctly estimate the camera pose in dynamic scenes.

Key words： semantic segmentation; dynamic scene; indoor scene; pose estimation; Visual Simultaneous Localization And Mapping （VSLAM）; semantic Simultaneous Localization And Mapping （SLAM）

0 引言

同步定位與地圖構(gòu)建（Simultaneous Localization And Mapping， SLAM）是移動(dòng)機(jī)器人、無人機(jī)、無人駕駛等應(yīng)用的基礎(chǔ)技術(shù)。場景中存在運(yùn)動(dòng)物體時(shí)，動(dòng)態(tài)物體的不穩(wěn)定特征點(diǎn)被提取后會(huì)嚴(yán)重影響相機(jī)位姿估計(jì)，造成軌跡誤差偏大，甚至系統(tǒng)崩潰?；谝曈X的SLAM在動(dòng)態(tài)場景中仍然具有很大的挑戰(zhàn)性，而且目前視覺SLAM通?；趲缀涡畔⒔▓D，缺少對(duì)地圖信息的抽象理解，不能為移動(dòng)載體的感知和導(dǎo)航提供環(huán)境的語義信息，制約了感知和導(dǎo)航效果。因此語義信息與幾何信息結(jié)合構(gòu)建語義地圖成為了一個(gè)研究熱點(diǎn)。

深度學(xué)習(xí)與SLAM結(jié)合可以從幾何和語義兩個(gè)層次上感知場景，從而對(duì)環(huán)境內(nèi)容進(jìn)行抽象理解，緩解對(duì)環(huán)境特征的依賴，獲得高層次的感知[1]，提高移動(dòng)機(jī)器人對(duì)周圍環(huán)境的理解。Li等[2]將SLAM與卷積神經(jīng)網(wǎng)絡(luò)（Convolutional Neural Network， CNN）結(jié)合，選擇關(guān)鍵幀進(jìn)行語義分割，利用二維語義信息和相鄰關(guān)鍵幀之間的對(duì)應(yīng)關(guān)系進(jìn)行三維建圖。McCormac等[3]將ElasticFusion和CNN結(jié)合，利用稠密的SLAM系統(tǒng)ElasticFusion計(jì)算位姿并建出稠密的圖，卷積神經(jīng)網(wǎng)絡(luò)預(yù)測每個(gè)像素的物體類別，通過貝葉斯更新來把識(shí)別的結(jié)果和SLAM生成的關(guān)聯(lián)信息整合到稠密語義地圖中。在動(dòng)態(tài)場景中，Kim等[4]提出通過計(jì)算連續(xù)的深度圖像在同一平面上的投影的差異獲得場景中的靜態(tài)物體。Sun等[5]通過計(jì)算連續(xù)RGB圖像的強(qiáng)度差異、量化深度圖像的分割完成像素分類，區(qū)分動(dòng)態(tài)靜態(tài)物體。Yu等[6]提出的

DS-SLAM（Semantic visual SLAM towards Dynamic environments）在動(dòng)態(tài)場景下將SLAM與SegNet[7]網(wǎng)絡(luò)結(jié)合，利用語義信息和運(yùn)動(dòng)特征點(diǎn)檢測濾除每一幀的動(dòng)態(tài)物體，從而提高位姿估計(jì)準(zhǔn)確性，同時(shí)建立語義八叉樹地圖。Li等[8]提出了一種關(guān)鍵幀邊緣點(diǎn)的靜態(tài)加權(quán)方法用以表示一個(gè)點(diǎn)是靜態(tài)環(huán)境一部分的可能性，減少動(dòng)態(tài)對(duì)象對(duì)位姿估計(jì)的影響。Bescos等[9]將多視幾何和深度學(xué)習(xí)結(jié)合，實(shí)現(xiàn)沒有先驗(yàn)動(dòng)態(tài)標(biāo)記而具有移動(dòng)性的物體的檢測和分割，并且通過對(duì)動(dòng)態(tài)物體遮擋的背景幀進(jìn)行修復(fù)，生成靜態(tài)場景地圖。

本文針對(duì)室內(nèi)動(dòng)態(tài)場景下SLAM位姿估計(jì)和建立語義地圖問題展開研究，改進(jìn)了動(dòng)態(tài)場景中的語義SLAM系統(tǒng)，主要工作如下：

1）將ORB-SLAM2[10]與語義分割網(wǎng)絡(luò)PSPNet（Pyramid Scene Parsing Network）[11]相結(jié)合，減小了動(dòng)態(tài)物體內(nèi)的特征點(diǎn)對(duì)相機(jī)位姿估計(jì)的影響，減小了軌跡誤差。2）建立帶有語義信息的點(diǎn)云圖和語義八叉樹地圖，為導(dǎo)航定位等應(yīng)用提供語義地圖信息。3）在相同公開數(shù)據(jù)集慕尼黑工業(yè)大學(xué)TUM（Technische Universitt München） RGB-D[12]上與DS-SLAM進(jìn)行對(duì)比，評(píng)估本文系統(tǒng)的有效性。

1 本文系統(tǒng)

1.1 SLAM系統(tǒng)框架

ORB-SLAM2被認(rèn)為是目前最完整、最穩(wěn)定的SLAM系統(tǒng)之一，可以運(yùn)行單目、雙目和RGB-D相機(jī)。本文SLAM采用ORB-SLAM2來提供SLAM方案，并行運(yùn)行五個(gè)線程：語義分割、跟蹤、局部建圖、閉環(huán)檢測和稠密地圖構(gòu)建。本文SLAM系統(tǒng)框架如圖1所示，RGB-D相機(jī)捕獲的原始RGB圖像先在語義分割線程中處理，得到每個(gè)像素語義標(biāo)簽;然后由跟蹤線程提取ORB特征點(diǎn)，由移動(dòng)一致性檢查檢測潛在的外點(diǎn)，若ORB特征點(diǎn)落在語義分割預(yù)測的動(dòng)態(tài)物體中，則將這些特征點(diǎn)作為外點(diǎn)剔除，用剔除外點(diǎn)后穩(wěn)定的靜態(tài)點(diǎn)估計(jì)相機(jī)位姿。

1.2 語義分割DS-SLAM采用的語義分割網(wǎng)絡(luò)是基于全卷積神經(jīng)網(wǎng)絡(luò)（Fully Convolutional Network， FCN）[13]的SegNet，由VGG-16網(wǎng)絡(luò)修改得到，但FCN存在幾個(gè)問題：缺乏依據(jù)上下文推斷的能力，不能通過類別之間的關(guān)系彌補(bǔ)標(biāo)簽之間的關(guān)聯(lián);模型可能會(huì)忽略小的東西，而大的東西可能會(huì)超過FCN接受范圍，從而導(dǎo)致不連續(xù)的預(yù)測?？傊?，F(xiàn)CN不能很好地處理場景之間的關(guān)系和全局信息。而PSPNet提出了一個(gè)金字塔場景解析網(wǎng)絡(luò)，能夠?qū)㈦y解析的場景信息特征嵌入基于FCN預(yù)測框架中，將局部信息和全局特征融合到一起，并提出了適度監(jiān)督損失的優(yōu)化策略，既能獲取全局場景信息，還可以有效地處理場景之間的關(guān)系。因此，本文SLAM系統(tǒng)采用基于Caffe[14]的逐像素語義分割網(wǎng)絡(luò)PSPNet。在PASCAL VOC2012[15]數(shù)據(jù)集上訓(xùn)練的PSPNet總共劃分20個(gè)類別。PSPNet的結(jié)構(gòu)如圖2所示。輸入的圖片經(jīng)過卷積神經(jīng)網(wǎng)絡(luò)提取特征圖，提取后的特征圖經(jīng)過金字塔池化模塊，得到不同尺度下的帶有整體信息的特征，在上采樣后將金字塔生成的不同層次的特征圖連接，最后經(jīng)過卷積層獲得每個(gè)像素的分類。

1.3 移動(dòng)一致性檢查和外點(diǎn)剔除動(dòng)態(tài)點(diǎn)的檢查方法采用DS-SLAM的移動(dòng)一致性檢查。如果移動(dòng)一致性檢查得到的動(dòng)態(tài)點(diǎn)落在分割的對(duì)象內(nèi)，并且動(dòng)態(tài)點(diǎn)數(shù)量大于一定閾值，則該對(duì)象可以被視為動(dòng)態(tài)對(duì)象。動(dòng)態(tài)點(diǎn)檢測方法如下：

第1步 計(jì)算光流金字塔以獲得當(dāng)前幀中匹配的特征點(diǎn)。

第2步 如果匹配點(diǎn)對(duì)太靠近圖像邊緣或者匹配點(diǎn)對(duì)中心的3×3

圖像塊的像素差超過設(shè)定閾值，

則匹配點(diǎn)對(duì)將被丟棄。

第3步 通過隨機(jī)采樣一致性方法和匹配點(diǎn)對(duì)計(jì)算基礎(chǔ)矩陣，對(duì)于每一對(duì)匹配點(diǎn)，根據(jù)參考幀的像素坐標(biāo)和基礎(chǔ)矩陣計(jì)算極線，確定從匹配點(diǎn)到其對(duì)應(yīng)的極線的距離是否小于等于某個(gè)閾值：如果距離大于閾值，則認(rèn)為它是一個(gè)動(dòng)態(tài)點(diǎn)[6]。

其中：光流金字塔的計(jì)算采用LK（Lucas-Kanade）光流金字塔算法，假設(shè)亮度不變、運(yùn)動(dòng)幅度小、空間一致。畫面移動(dòng)過程中，圖像上每個(gè)像素的偏移量為（x， y），若第t幀A點(diǎn)的位置是（x1， y1），則第t+1幀時(shí)，假如A點(diǎn)的位置是（x2， y2），可以確定A點(diǎn)的運(yùn)動(dòng)為（u， v）=（x2， y2）-（x1， y1）。假設(shè)原圖是I（x， y，z，t），移動(dòng)后的圖像是I（x+δx，y+δy，z+δz，t+δt），兩者滿足圖像約束方程：

在運(yùn)動(dòng)足夠小的情況下，二階和高階泰勒級(jí)數(shù)展開式可以忽略，因此有

由式（2）等式兩邊同除dt可得：

由于LK算法假設(shè)是小位移，需要在多層圖像縮放金字塔上求解，每一層的求解結(jié)果乘以2后加到下一層即可解決位移大的問題。

基于幾何的方法提取動(dòng)態(tài)物體輪廓時(shí)間開銷大，而用語義分割的方法去提取輪廓耗時(shí)少，因此，在本文SLAM系統(tǒng)中采用了語義分割網(wǎng)絡(luò)，可以快速地獲得對(duì)象的完整輪廓。如果通過移動(dòng)一致性檢查產(chǎn)生一定數(shù)量的動(dòng)態(tài)點(diǎn)落在分割對(duì)象的輪廓中，則確定該對(duì)象正在移動(dòng)。如果確定分割的對(duì)象正在移動(dòng)，則移除位于對(duì)象輪廓中的所有特征點(diǎn)。通過這種方式，可以精確地消除外點(diǎn)。此外，錯(cuò)誤分割的影響也可以在一定程度上降低。

在實(shí)際場景中，人是動(dòng)態(tài)場景中的主要?jiǎng)討B(tài)物體，因此將人作為實(shí)驗(yàn)的動(dòng)態(tài)物體。在語義分割結(jié)果出來之后，如果沒有檢測到人，則所有ORB特征將直接與最后一幀匹配估計(jì)位姿;否則，使用移動(dòng)一致性檢查結(jié)果確定人員是否在移動(dòng)。如果人被確定為靜態(tài)，則直接估計(jì)位姿;否則在匹配之前刪除屬于人物輪廓的所有ORB特征點(diǎn)。這樣，可以顯著降低動(dòng)態(tài)對(duì)象的影響。

1.4 語義點(diǎn)云地圖和語義八叉樹地圖

系統(tǒng)構(gòu)建的語義點(diǎn)云地圖和語義八叉樹地圖，能夠較好地呈現(xiàn)出室內(nèi)的場景，語義分割識(shí)別出的物體被標(biāo)注了不同的顏色信息，并且場景中的動(dòng)態(tài)物體（在數(shù)據(jù)集中即為人）得到了很好的剔除。相對(duì)于語義點(diǎn)云地圖，語義八叉樹地圖所占的空間約是語義點(diǎn)云地圖的1%，能夠節(jié)省大量的硬盤空間，為機(jī)器人提供導(dǎo)航地圖，并且能夠提供語義信息。

2 實(shí)驗(yàn)與分析

2.1 數(shù)據(jù)集實(shí)驗(yàn)所需的數(shù)據(jù)集TUM RGB-D是TUM（德國慕尼黑工業(yè)大學(xué)）開源的大型數(shù)據(jù)集，包含RGB-D數(shù)據(jù)和地面實(shí)況數(shù)據(jù)，目的是為視覺測距和視覺SLAM系統(tǒng)的評(píng)估建立新的基準(zhǔn)。本文的數(shù)據(jù)集主要采用數(shù)據(jù)集中Asus Xtion采集的5個(gè)序列，分別為

。 freiburg3_sitting_static序列中兩個(gè)人坐在辦公桌前，相機(jī)保持在適當(dāng)位置，視為是低動(dòng)態(tài)序列，其他四個(gè)序列均為兩個(gè)人走過辦公室，視為高動(dòng)態(tài)序列。 freiburg3_walking_static序列相機(jī)保持在適當(dāng)位置， freiburg3_walking_xyz序列相機(jī)沿三個(gè)方向（x，? y，z）移動(dòng)，reiburg3_walking_halfsphere序列中相機(jī)在大約一米直徑的小半球上移動(dòng)， freiburg3_walking_rpy序列中相機(jī)沿主軸（滾轉(zhuǎn)俯仰偏航）在相同位置旋轉(zhuǎn)。

另外，數(shù)據(jù)集還提供了用于系統(tǒng)評(píng)估的方法——絕對(duì)軌跡誤差（Absolute Trajectory Error， ATE）和相對(duì)位姿誤差（Relative Pose Error， RPE）。其中：ATE代表軌跡的全局一致性，而RPE測量平移和旋轉(zhuǎn)漂移。

2.2 實(shí)驗(yàn)結(jié)果

在本節(jié)中，將展示本文SLAM系統(tǒng)實(shí)驗(yàn)結(jié)果以說明系統(tǒng)在公共數(shù)據(jù)集TUM RGB-D的動(dòng)態(tài)場景數(shù)據(jù)集中的性能。所有實(shí)驗(yàn)均在配備Intel i7 CPU，GTX1070 GPU和16GB內(nèi)存的計(jì)算機(jī)上進(jìn)行。

2.2.1 定量結(jié)果

定量比較結(jié)果顯示在表1～3中。實(shí)驗(yàn)對(duì)比了本文系統(tǒng)與DS-SLAM在數(shù)據(jù)集五個(gè)序列中的實(shí)驗(yàn)結(jié)果，評(píng)價(jià)指標(biāo)是均方根誤差（Root Mean Squared Error， RMSE）、平均誤差（Mean）、中值誤差（Median）和標(biāo)準(zhǔn)偏差（Standard Deviation， S.D.）。其中：均方根誤差（RMSE）描述估計(jì)值與真實(shí)值之間的偏差，因此值越小，代表的系統(tǒng)估計(jì)的軌跡越接近于真實(shí)值;平均誤差反映所有估計(jì)誤差的平均水平;中值誤差代表所有誤差的中等水平;標(biāo)準(zhǔn)偏差（S.D.）反映系統(tǒng)軌跡估計(jì)的離散程度。這幾種客觀評(píng)價(jià)算法表現(xiàn)出系統(tǒng)估計(jì)的軌跡與真實(shí)值之間的差距，反映了系統(tǒng)的穩(wěn)定性和可靠性。

從表1～3可看出，相對(duì)于DS-SLAM，本文SLAM系統(tǒng)可以使大多數(shù)高動(dòng)態(tài)序列的性能得到提高，在動(dòng)態(tài)程度較高的序列freiburg3_walking_xyz和freiburg3_walking_rpy中，提升明顯。結(jié)果表明，本文SLAM系統(tǒng)可以提高SLAM系統(tǒng)在高動(dòng)態(tài)場景中的魯棒性和穩(wěn)定性。但是，在低動(dòng)態(tài)序列中，例如fr3_sitting_static序列，性能的提高并不明顯，原因是DS-SLAM可以處理低動(dòng)態(tài)場景并獲得良好性能，因此可以改進(jìn)的空間有限。

2.2.2 定量結(jié)果

圖3～4顯示了在高動(dòng)態(tài)freiburg3_walking_rpy序列中，ORB-SLAM2、DS-SLAM和本文SLAM系統(tǒng)的ATE和RPE圖。圖5顯示了ORB-SLAM2、DS-SLAM和本文系統(tǒng)在freiburg3_walking_rpy上的ATE對(duì)比?？煽闯觯疚腟LAM系統(tǒng)的絕對(duì)軌跡誤差和相對(duì)位姿誤差均有不同程度的減少。

3 結(jié)語

動(dòng)態(tài)物體的存在對(duì)于軌跡和位姿的估計(jì)影響較大，對(duì)動(dòng)態(tài)物體剔除以減小軌跡和位姿誤差很有必要。為此，本文改進(jìn)系統(tǒng)使用了分割準(zhǔn)確率高的PSPNet作為分割網(wǎng)絡(luò)，用以分類場景中的物體，對(duì)于場景中的動(dòng)態(tài)物體上的動(dòng)態(tài)特征點(diǎn)予以剔除，利用穩(wěn)定的靜態(tài)特征點(diǎn)進(jìn)行動(dòng)態(tài)場景下的運(yùn)動(dòng)估計(jì)，繼而完成語義地圖的構(gòu)建，并通過實(shí)驗(yàn)對(duì)比驗(yàn)證了本文系統(tǒng)在減小軌跡和位姿誤差上的優(yōu)勢(shì)。在接下來的工作中，將研究運(yùn)動(dòng)模糊對(duì)動(dòng)態(tài)特征點(diǎn)的影響并優(yōu)化。

參考文獻(xiàn)（References）

[1] CADENA C， CARLONE L， CARRILLO H， et al. Past， present， and future of simultaneous localization and mapping： toward the robust-perception age[J]. IEEE Transactions on Robotics， 2016， 32（6）： 1309-1332.

[2] LI X， AO H， BELAROUSSI R， et al. Fast semi-dense 3D semantic mapping with monocular visual SLAM[C]// Proceedings of the IEEE 20th International Conference on Intelligent Transportation Systems. Piscataway： IEEE， 2017： 385-390.

[3] McCORMAC J， HANDA A， DAVISON A， et al. SemanticFusion： dense 3D semantic mapping with convolutional neural networks[C]// Proceedings of the 2017 IEEE International Conference on Robotics and Automation. Piscataway： IEEE， 2017： 4628-4635.

[4] KIM D H， KIM J H. Effective background model-based RGB-D dense visual odometry in a dynamic environment[J]. IEEE Transactions on Robotics， 2016， 32（6）： 1565-1573.

[5] SUN Y， LIU M， MENG M Q. Improving RGB-D SLAM in dynamic environments： a motion removal approach[J]. Robotics & Autonomous Systems， 2017， 89： 110-122.

[6] YU C， LIU Z， LIU X， et al. DS-SLAM： a semantic visual SLAM towards dynamic environments[C]// Proceedings of the 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway： IEEE， 2018： 1168-1174.

[7] BADRINARAYANAN V， KENDALL A， CIPOLLA R. SegNet： a deep convolutional encoder-decoder architecture for scene Segmentation[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence， 2017， 39（12）： 2481-2495.

[8] LI S， LEE D. RGB-D SLAM in dynamic environments using static point weighting[J]. IEEE Robotics and Automation Letters， 2017， 2（4）： 2263-2270.

[9] BESCOS B， FCIL J M， CIVERA J， et al. DynaSLAM： tracking， mapping， and inpainting in dynamic scenes[J]. IEEE Robotics and Automation Letters， 2018， 3（4）： 4076-4083.

[10] MUR-ARTAL R， TARDS J D. ORB-SLAM2： an open-source SLAM system for monocular， stereo， and RGB-D cameras[J]. IEEE Transactions on Robotics， 2017， 33（5）： 1255-1262.

[11] ZHAO H， SHI J， QI X， et al. Pyramid scene parsing network[C]// Proceedings of the 30th IEEE Conference on Computer Vision and Pattern Recognition. Piscataway： IEEE， 2017： 6230-6239.

[12] STURM J， ENGELHARD N， ENDRES F， et al. A benchmark for the evaluation of RGB-D SLAM systems[C]// Proceedings of the 2012 IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway： IEEE， 2012： 573-580.

[13] SHELHAMER E， LONG J， DARRELL T. Fully convolutional networks for semantic segmentation[J]. IEEE Transactions on Pattern Analysis， 2017， 39（4）： 640-651.

[14] JIA Y， SHELHAMER E， DONAHUE J， et al. Caffe： convolutional architecture for fast feature embedding[EB/OL]. [2019-02-10]. https：//arxiv.org/pdf/1408.5093.pdf.

[15] EVERINGHAM M， van GOOL L， WILLIAMS C K I， et al. The PASCAL Visual Object Classes Challenge 2012 （VOC2012） Resultst [EB/OL]. [2019-01-10].

http：//host.robots.ox.ac.uk/pascal/VOC/voc2012/.