室內動態(tài)場景下的語義SLAM

唐鼎新，袁 亮，冉 騰

(新疆大學 機械工程學院，新疆 烏魯木齊 830047)

0 引 言

傳統(tǒng)的SLAM建圖只包含幾何結構信息，并不能滿足現(xiàn)有的應用需求。SLAM進入機器人感知時代，需要更多的研究來為自主機器人實現(xiàn)真正魯棒的感知和導航[1]。

隨著深度學習在視覺領域的大量研究，計算機可以獲取環(huán)境中的語義信息，語義信息通過數(shù)據(jù)關聯(lián)融合到SLAM算法中，提高SLAM算法建圖的精確性和魯棒性，而且可以增強機器人任務規(guī)劃的能力，彌補人機交互中的不足。Mccormac J等[2]提出了SemanticFusion，將卷積神經(jīng)網(wǎng)絡與ElasticFusion[3]結合，實現(xiàn)了實時地圖構建與語義標記。然而沒有考慮到移動機器人工作環(huán)境復雜、動態(tài)性較強的問題。MaskFusion[4]將ElasticFusion和Mask-RCNN[5]結合，實現(xiàn)對動態(tài)目標像素級別的識別和分割，采用最小化空間幾何誤差和光度誤差計算相機位姿，但是該工程將動態(tài)目標從地圖上移除、跟蹤和重構僅限于剛性物體，而非剛性物體則無法處理，例如人。Yu等提出的DS-SLAM[6]是一種結合ORB-SLAM2[7]的系統(tǒng)，該系統(tǒng)采用語義分割方法SegNet[8]從RGB圖像中檢測出人所在的像素點，然后通過運動一致性來檢查落在人員身上的特征描述子，以確定它們是否是動態(tài)的，最后利用深度圖像中的靜態(tài)像素構建一個基于八叉樹的語義地圖。Zhang等[9]使用PSPNet的語義分割網(wǎng)絡獲取置信度，ORB-SLAM2系統(tǒng)獲取相機位姿，根據(jù)置信度和相機位姿更新Octomap中的八叉樹地圖，建立帶有語義的三維八叉樹地圖，然而該工程在動態(tài)環(huán)境下無法構建靜態(tài)語義八叉樹地圖，并且動態(tài)環(huán)境極易影響相機位姿的精確性，導致八叉樹地圖出現(xiàn)重疊和錯誤部分。

本文針對室內動態(tài)場景建立三維語義地圖問題展開研究，在Zhang等文獻[9]的基礎上加入Farneback光流法[10,11]，減少了ORB-SLAM2中動態(tài)物體內特征點對相機位姿估計的影響，解決了文獻[9]中在動態(tài)場景下構建三維語義地圖較差的問題。并且在建立語義八叉樹地圖的過程中根據(jù)語義分割的結果剔除室內預定義的動態(tài)目標，減少動態(tài)目標對地圖的影響，為導航定位等應用提供精確的語義地圖信息。

1 三維語義建圖框架

本文以文獻[9]的語義SLAM系統(tǒng)為框架，流程如圖1所示。系統(tǒng)首先接收RGB-D相機的彩色圖像和深度圖像，然后將彩色圖像和深度圖像分別輸入兩個不同的模塊。一個是ORB-SLAM2模塊，另一個是語義分割模塊，通過兩個模塊生成三維語義地圖。

圖1 語義SLAM流程

1.1 ORB-SLAM2

使用ORB-SLAM2作為語義SLAM后端。它有4個線程分別是跟蹤線程、閉環(huán)檢測線程、優(yōu)化線程和建圖線程，具體線程如圖2所示。首先在當前幀查找角點，估計灰度質心的方向向量，然后對前一步提取出特征點的周圍圖像區(qū)域進行描述，通過得到的ORB特征描述子進行匹配，匹配好的特征點用PnP估計相機位姿。將計算得到的相機位姿(位置、角度)通過ROS發(fā)布，此時Octomap訂閱相機位姿生成八叉樹地圖。

圖2 流程

1.2 三維語義地圖構建

語義分割模塊使用ADE20K數(shù)據(jù)集訓練PSPNet的模型，該模型基于PyTorch實現(xiàn)[12]。ADE20K數(shù)據(jù)集包含150個類的大型詞匯表，包括室內和室外場景，本文實驗建圖出現(xiàn)的標簽顏色如圖3所示。

當將點云輸入到Octomap中時，使用貝葉斯語義融合和訂閱的相機位姿更新八叉樹子節(jié)點的占據(jù)率、RGB顏色、語義顏色和置信度。首先將語義信息存儲在Octomap的每個子節(jié)點中，子節(jié)點只存儲CNN產(chǎn)生的置信度前三的語義顏色。如果兩幀之間的子節(jié)點語義信息存在變化，那么子節(jié)點有新的語義信息，將兩幀之間的語義信息進行語義融合，最后生成語義信息和點云融合的三維八叉樹地圖。

2 動態(tài)環(huán)境下語義地圖構建

2.1 Farneback光流法

為了消除動態(tài)目標對相機位姿估計的影響，本文采用光流法剔除動態(tài)目標區(qū)域的特征點，優(yōu)化相機位姿的流程如圖4所示，利用優(yōu)化的相機位姿建立三維語義地圖。Farneback光流法利用圖像序列中的像素在時間域上的變化與相鄰幀之間的相關性來查找上一幀和當前幀之間存在的對應關系，計算出相鄰幀之間物體的運動信息。

圖4 Farneback優(yōu)化流程

Farneback光流法是一種基于梯度的方法，假設圖像梯度和局部光流恒定。局部光流恒定就是對于任意的y∈N(x),d=?X/?t不變。梯度恒定如式(1)所示

(1)

將輸入圖像(灰度圖像)視為二維信號的函數(shù)，因此圖像像素點的灰度值可以用一個二維變量的函數(shù)。假設以感興趣的像素點為中心，構建一個局部坐標系。利用二次多項式近似表示圖像中每個像素點的鄰域并系數(shù)化處理，得到如式(2)所示

f(x,y)～xTAx+bTx+c

(2)

式中：x為二維列向量(x,y)T，A為一個2×2的對稱矩陣，b是一個2×1的矩陣。此處的系數(shù)確定后只針對所選像素點(x,y)而言，每個像素點對應一組系數(shù)。當輸入某一幀圖像時，在這一幀某個像素點二次多項式在理想的變換情況下，設初始的圖像信號f1如式(3)所示

f1(x,y)～xTA1x+b1Tx+c1

(3)

它的下一幀圖像信號f2為上一幀所選像素點二次多項式基礎上增加了全局位移d，如式(4)所示

f2(x,y)=f2(x-d)

(4)

如果A1是非奇異矩陣，將式(3)和式(4)中的系數(shù)等效可以求解得到d，如式(5)所示

(5)

最后通過構建目標函數(shù)來進行優(yōu)化求得位移e(x,y)，如式(6)所示

(6)

為了減少實際情況中噪聲的影響，選取感興趣像素點的鄰域，然后對目標函數(shù)加權后得到位移e(x,y)，即為每個點的光流值，如式(7)所示

(7)

為了消除動態(tài)區(qū)域的特征點，需要判斷像素點是否為動態(tài)點，然后對當前幀與上一幀間某像素點的位移設置一個閾值，當超過閾值時視為動態(tài)點否則為靜態(tài)點。在ORB-SLAM2中檢測動態(tài)點時，當前幀為關鍵幀時進行特征點的管理，刪除當前幀被視為動態(tài)點的特征點，從而得到優(yōu)化的相機位姿。

2.2 剔除語義地圖的動態(tài)目標

針動態(tài)目標嚴重影響三維語義地圖的建立，本文剔除語義地圖更新中的動態(tài)目標。首先動態(tài)目標會占據(jù)更多的地圖空間和八叉樹節(jié)點，影響建圖的效率和地圖的使用。其次在建圖過程中動態(tài)目標會影響靜態(tài)地圖的建立，導致靜態(tài)地圖的節(jié)點更新錯誤。本文剔除動態(tài)目標算法的原理如下：

在八叉樹中，用概率表示每個子節(jié)點是否被占據(jù)。由于更新節(jié)點時會受到噪聲等影響，所以每個節(jié)點通常用0到1之間的浮點數(shù)表示占據(jù)的概率。Octomap的原始論文中觀測數(shù)據(jù)為z，那么t時刻的第n個節(jié)點的概率如式(8)所示

(8)

應用此表達方式，八叉樹按照占據(jù)概率進行渲染，使用RGB-D數(shù)據(jù)和語義信息更新地圖。子節(jié)點存儲著語義標簽的語義信息，同時語義標簽帶有RGB顏色。根據(jù)語義標簽的RGB顏色，將某個物體的占據(jù)概率設置為0，則這個物體就會被渲染成透明，也就是停止在地圖中此物體的節(jié)點更新。在建圖時根據(jù)不同場景剔除不同的動態(tài)目標，比如室內預定義的動態(tài)目標“人”。最后生成靜態(tài)的語義八叉樹地圖。

3 實驗結果與分析

使用公開的標準數(shù)據(jù)集TUM驗證分析本文算法的性能，該數(shù)據(jù)集采用Microsoft Kinect深度攝像頭拍攝，RGB圖像分辨率為640×480。本文與ROS進行集成，在動態(tài)環(huán)境下建立三維語義地圖，在rviz下顯示語義的八叉樹地圖。實驗所用的硬件配置為Intel i5 CPU，8 GB內存和 NVIDIA GeForce GTX1030 GPU。在真實場景下所用的計算機硬件配位為Intel i5 CPU，8 GB內存和 NVIDIA GeForce GTX1030 GPU。機器人使用醫(yī)療服務型輪椅機器人如圖5所示。

圖5 醫(yī)療服務型輪椅機器人

3.1 動態(tài)環(huán)境下定位精度分析

TUM RGB-D數(shù)據(jù)集在動態(tài)環(huán)境中提供了多個序列，通過外部運動捕捉系統(tǒng)獲得精確的地面真實信息，如waking和sitting等。由于本文算法較好地解決了動態(tài)環(huán)境下的相機位姿估計問題，因此本文實驗主要對比算法在高動態(tài)環(huán)境下相機的定位精度。Walking序列為實驗的主要序列，因為有人在walking序列中來回行走，這些序列中的人可以看作是高動態(tài)對象，是最難處理的對象。Sitting序列也被使用，但它們被認為是低動態(tài)序列，因為其中的人只是移動了一點。根據(jù)相機的運動類型，同時分為了static、xyz、rpy、halfsphere。

ORB-SLAM2作為全局SLAM解決方案，是目前公認的最優(yōu)秀、最穩(wěn)定的SLAM算法之一，因此用它與本文算法進行了對比。另外，本文算法與ORB-SLAM2使用絕對軌跡誤差(absolute trajectory error，ATE)和相對位姿誤差(relative pose error，RPE)在定位精度上進行分析，其中ATE代表軌跡的全局一致性，而RPE測量平移和旋轉漂移。

本文算法與ORB-SLAM2算法進行對比，采用ATE和RPE評價對walking序列中的xyz進行對比實驗，此序列為動態(tài)環(huán)境下最常用的序列。本文算法對比估計的相機位姿與真實相機位姿之間的偏差，點劃線越短代表本文算法位姿估計越精確，如圖6所示。對比估計的相機運動軌跡與真實軌跡每米的平移漂移量，在相同時間內漂移越小，估計的位姿就越精確，如圖7所示。由此可見，本文算法的定位精度和穩(wěn)定性要明顯優(yōu)于ORB-SLAM2。

圖6 絕對軌跡誤差對比

圖7 相對位姿誤差對比

同時使用了4個評價標準將本文算法和ORB-SLAM2進行了比較，這4個評價標準分別是：均方根誤差(root mean square error, RMSE)、平均誤差(Mean)、中值誤差(Me-dian)和標準偏差(standard deviation,S.D.)。為了驗證算法的真實性，每個標準取5組數(shù)據(jù)求平均值，見表1～表3。

表1 絕對軌跡誤差ATE/cm

表2 平移漂移RPE/(cm/幀)

表3 旋轉漂移RPE/(度/秒)

從表1～表3可以看出在高動態(tài)序列中本文算法對定位精度有明顯的改善。Walking_static序列是最能體現(xiàn)動態(tài)環(huán)境的序列，此序列只有兩個高動態(tài)行走的人，相機并沒有移動。在ATE方面，RMSE的提升值高達94.84%，在RPE平移和旋轉方面RMSE提升值分別是88.58%和88.10%。

在實際應用中，實時性是評價SLAM系統(tǒng)性能的重要指標。測試Farneback光流法處理兩幀之間動態(tài)區(qū)域特征點所需的時間為94.086 ms，本文算法基本滿足實時性要求。

3.2 動態(tài)環(huán)境下三維語義地圖構建結果對比

為驗證本文提出的Farneback光流法和建圖中剔除動態(tài)目標對構建三維語義地圖具有良好的提升效果，將在高動態(tài)環(huán)境下建立兩組語義地圖進行對比。其中一組為文獻[9]的原版建圖效果，另一組地圖則加入Farneback光流法和剔除動態(tài)目標。

由圖8(a)白圈可以看出，由于動態(tài)目標對相機位姿的影響，導致構建的墻面交錯重疊，中間部分的壁畫也無法構建；左圖灰圈部分，建圖時動態(tài)目標與靜態(tài)地圖重疊，嚴重影響靜態(tài)地圖的建立。由圖8(b)可以看出，加入Farneback光流法后的地圖消除了墻壁交錯重疊，同時也構建出中間的壁畫；右圖由于剔除了動態(tài)目標，能夠清楚地看見椅子，并構建出一個良好的靜態(tài)地圖。

圖8 語義建圖對比

3.3 真實場景下三維語義地圖構建結果分析

為了驗證本文算法，將與文獻[9]、ORB-SLAM2、RTAB[13]對高動態(tài)環(huán)境進行真實場景建圖實驗。建圖場景分別是實驗室、走廊和辦公室，實驗室和走廊建立局部地圖，而辦公室建立全局地圖。

如圖9～圖14所示，白圈為相機位姿優(yōu)化前建立的三維語義八叉樹地圖效果，而灰圈為語義地圖未剔除動態(tài)目標的效果。通過對比，本文算法在真實場景下也同樣能實時建立精確的全局和局部靜態(tài)三維語義地圖。

圖9 實驗室靜態(tài)環(huán)境三維語義地圖構建對比

圖10 實驗室動態(tài)環(huán)境三維語義地圖構建對比

圖11 走廊靜態(tài)環(huán)境三維語義地圖構建對比

4 結束語

本文針對室內動態(tài)場景語義地圖構建較差的問題展開研究。國內外很多語義SLAM沒有考慮到動態(tài)目標對建圖的影響，動態(tài)場景下構建出的語義地圖也無法用于其它工作，并且構建出的語義地圖和稠密點云非常占用計算機的內存和性能。通過實驗對比，本文算法能在性能較低的計算機上實時構建存儲量小的靜態(tài)語義八叉樹地圖。

本文算法雖然能夠準確建立動態(tài)場景下靜態(tài)三維語義八叉樹地圖，但是還有很多問題值得探究。首先相機抖動較大和光照較強或較弱會影響相機位姿的估計，導致構建的三維語義地圖出現(xiàn)錯誤。其次沒有考慮到如何利用構建語義和SLAM相輔相成的數(shù)據(jù)關聯(lián)。后期本文算法會在這兩個方面進行改進。

圖12 走廊動態(tài)環(huán)境三維語義地圖構建對比

圖13 辦公室靜態(tài)環(huán)境三維語義地圖構建對比

圖14 辦公室動態(tài)環(huán)境三維語義地圖構建對比