視覺SLAM在室內動態場景中的應用研究

徐少杰，曹雛清，王永娟

1.南京理工大學 機械工程學院，南京210094

2.安徽工程大學 計算機與信息學院，安徽 蕪湖241000

視覺SLAM（同時定位與建圖）作為移動機器人領域的核心技術已經發展了二十多年，在這期間涌現出了大量優秀的視覺SLAM 方法。這些方法按視覺里程計算法不同可分為兩個大類：一類是特征點法，一類是直接法[1]。PTAM[2]、RGBD-SLAM[3]、ORB-SLAM2[4]都是基于特征點的視覺SLAM 系統。PTAM 利用Bundle Adjustment（BA）首先實現了實時的SLAM系統，RGBDSLAM 是基于RGBD 相機的可實時構建稠密三維點云地圖的SLAM 系統，文獻[4]在單目的基礎上增加了對雙目和RGBD 相機的支持，是單目、雙目和RGBD 相機三種傳感器都可運行的SLAM 系統。由于特征點對于光照變化、運動、旋轉都不敏感，所以基于特征點法的SLAM系統比較穩定，魯棒性較好，但是提取特征點、計算描述子和特征匹配比較耗時。DTAM[5]、LSD-SLAM[6]都是基于直接法的視覺SLAM 系統。DTAM 首次利用直接法實現了實時稠密三維重建并提升了SLAM 系統的準確性和魯棒性，LSD-SLAM利用圖像灰度來實現定位和半稠密點云地圖構建?；谥苯臃ǖ腟LAM 系統節約了計算特征點和描述子的時間，計算速度較快。直接法假設同一空間點的像素灰度值在各個圖像中是固定不變的（灰度不變假設），由于環境中光照變化、相機自動曝光等的影響，這一假設很難實現，因此基于直接法的SLAM系統穩定性較差。

此外，無論是特征點法還是直接法的視覺SLAM系統均將環境視為靜態的，但是現實環境中不可避免地會出現動態物體，如室內環境中的人等。為了解決動態場景下的SLAM問題，研究者們已經做了一些嘗試。房金立等人[7]提出將光流法與實例分割結合的方式進行檢測并剔除動態特征點，以此來提高動態環境下的定位精度。DS-SLAM[8]采用語義分割結合光流法的方法，降低了動態物體對視覺SLAM的影響，構建出語義八叉樹地圖。Wang 等人[9]通過彩色圖像中的光流來檢測動態物體。然而光流法的假設太強，實際環境中很難能夠滿足假設。

為了解決上述問題，本文提出一種用目標檢測網絡檢測潛在動態物體所在的圖像區域，在特征提取后剔除分布于這一區域中的特征點，以此來降低動態物體對位姿估計帶來的影響，并對關鍵幀進行語義分割，在構建點云地圖時剔除動態地圖點，構建出無動態物體影響的三維稠密語義地圖。

1 動態環境下的位姿估計

本文選擇基于特征點法的ORB-SLAM2 來提供視覺SLAM 方案，框架如圖1 所示，分為5 個線程：目標檢測線程、跟蹤線程、局部地圖線程、回環檢測線程和語義建圖線程。數據處理流程為：RGB-D 相機采集的RGB圖和深度圖輸入跟蹤線程進行ORB 特征提取，同時將RGB 圖輸入目標檢測線程檢測出動態物體所在區域，跟蹤線程根據目標檢測結果剔除動態特征點，用剩余的穩定的靜態特征點進行相機位姿估計。關鍵幀傳入局部建圖線程進行局部BA，剔除冗余關鍵幀，然后將關鍵幀傳入回環檢測線程和語義建圖線程，回環檢測線程中進行回環檢測和全局BA，語義建圖線程中用語義分割網絡對關鍵幀進行分割，構建點云地圖時結合先驗知識剔除動態物體地圖點，最后結合位姿信息進行點云拼接構建出全局一致的不含動態物體的語義地圖。

圖1 本文SLAM系統框架

1.1 動態目標檢測

傳統目標檢測算法基于人工設計的特征，對于多樣性變化敏感，魯棒性差。為了解決以上問題目標檢測算法漸漸從傳統方法轉向基于卷積神經網絡的檢測方法。YOLOv4[10]中集成了很多優秀的方法，在相同檢測精度下檢測時間大大減少?，F階段的目標檢測器主要由四部分組成：Input、Backbone、Neck、Head。YOLOv4采用CSPDarknet53 作為Backbone，SPP 作為Neck 的附加模塊，PANet 作為Neck 的特征融合模塊，YOLOv3 作為Head。在MS COCO 數據集中AP 為43.5%，實際應用中可達65 frame/s，可實現實時檢測。本文采用YOLOv4 作為目標檢測網絡，檢測環境中的動態物體，室內環境下的動態點主要來自于環境中的行人。為了檢測行人，本文用PASCAL VOC數據集訓練YOLOv4，該數據集包含20 種物體，從中提取出“person”類別，共提取出4 015 張圖像用于訓練網絡。重訓練后的YOLOv4檢測結果如圖2所示。

圖2 動態目標檢測結果

1.2 動態特征點剔除

如圖2 所示，YOLOv4 的檢測結果是一個個的檢測框，假設一幀圖像上的所有特征點的集合是A={p1,p2,…,pn}，單個特征點坐標為pi=(ui,vi)，檢測框定義為d=(l,t,r,b)，其中l為檢測框的左邊的x坐標值，t為上邊的y坐標值，r為右邊的x坐標值，b為下邊的y坐標值。檢測框將圖像中的特征點分成兩類：在檢測框外的靜態特征點和檢測框內的動態特征點。設靜態特征點集合為B，動態特征點集合為C，對每個特征點進行如下判斷：如果點p滿足{(u,v)|l＜u ＜r,t ＜v ＜b}則p∈C，否則p∈B。在對當前幀提取特征點后按上述判斷條件判斷容器中的特征點是否屬于集合C，如果是，則將其刪除。動態特征點剔除效果如圖3所示。

從圖3（a）中可以看出在剔除之前有一些特征點分布在兩個人身上，這部分特征點將會影響位姿估計，不利于機器人準確定位，從圖3（b）中可以看出，經過剔除處理后的特征點已經全部都分布于靜態的物體之上，可以有效降低動態物體對機器人位姿估計的影響。

圖3 動態特征點剔除

1.3 位姿計算

通過對動態特征點的剔除，在計算機器人位姿時便可使用集合B中的靜態特征點來計算。設圖像中滿足條件的特征點pi=(ui,vi)的空間點為Pi=(Xi,Yi,Zi)，則特征點pi與空間點Pi的對應關系如下：

其中，s為尺度因子，K為相機內參矩陣，ξ為位姿的李代數，∧為李代數轉換為矩陣的運算符，e為誤差。

將式（1）改寫為矩陣形式：

將所有誤差累加求和，構建最小二乘問題：

由于p∈B，所以上式中的Pi均為固定值，這樣式（3）中的ξ*即為所求的相機位姿。

2 動態環境下的語義建圖

2.1 語義分割

上述工作雖然可以獲得動態環境下的相機位姿，但是構建語義地圖還需要按像素分割的語義分割圖像?？紤]到SLAM系統的實時性，所用語義分割網絡需要在準確率和分割速度達到一個平衡，因此本文采用基于Caffe框架的實時語義分割網絡SegNet[11]來對關鍵幀進行分割。訓練神經網絡的數據集是NYU-Depth V2，該數據集是一個室內靜態數據集，共包含室內場景中常見的13個類別，但是不包含動態的行人類別，因此添加上PASCAL VOC中的行人類別構成新的數據集用來訓練SegNet，訓練后的網絡分割結果如圖4所示。圖中不同種類用不同的顏色表示，其中行人用白色來表示。

2.2 三維語義點云地圖構建

本文利用PCL點云庫構建點云地圖，將地圖點定義為pcl：：PointXYZRGBA 類型，地圖點定義為q=(x,y,z,r,g,b)，其中(x,y,z)表示地圖點的空間坐標，(r,g,b)表示地圖點的顏色信息。

圖4 語義分割結果

設像素點p，對應的相機坐標系下的空間點P，則這兩點的對應關系為：

由公式（4）像素點可以投影到相機坐標系下的三維空間中，可將單幀圖像轉換成相機坐標系下的單片點云，但相機在運動中相機坐標系也在不斷變化，因此還需要將相機坐標系下的單片點云拼接到統一的世界坐標系中來構建全局一致的點云地圖。

在動態環境下動態物體在不停地運動，所在空間位置在不停地變化，但是所在的每個位置都會被相機記錄下來，這樣在構建點云地圖時會將每個時刻的動態物體都重建出來，造成地圖混亂，不利于機器人執行任務，因此需要在構建地圖時將動態的點濾除。濾除方法是按RGB 值濾除，如圖4 所示，表示動態的人的像素點的顏色RGB值為（255，255，255），因此在構建地圖時只要地圖點滿足(r,g,b)=(255,255,255)就將其濾除，這樣構建的地圖就可免除動態物體移動帶來的影響，構建出全局一致的三維語義點云地圖。

3 實驗結果

為驗證本文方法的有效性，采用TUM 數據集對本文方法進行實驗。TUM數據集是通過Kinect深度相機采集的不同室內場景的圖像序列，提供了采集圖像時的相機位姿真值，可供研究者們評估自己的SLAM系統性能。本文采用該數據集中的三個高度動態的場景數據walking_halfsphere、walking_static、walking_xyz 來進行測試。

3.1 位姿估計實驗

實驗時采用ATE[12]（Absolute Trajectory Error，絕對軌跡誤差）作為評價指標，絕對軌跡誤差指的是相機的位姿真值與SLAM 系統位姿估計值之間的差值。表1為本文算法和ORB_SLAM2 在三種數據集中的測試結果對比，其中的提升率計算方式為：

其中rmes、mean 和std 分別指的是均方根誤差、平均值和標準差。表2 為本文算法與其他同類方法的絕對軌跡誤差結果對比。

表1 絕對軌跡誤差對比

表2 同類算法結果對比

從表1可以看出，本文算法在對動態目標進行處理之后的位姿估計精度相對于ORB_SLAM2 的位姿估計精度有了很大的提升，平均提升率約為88.3%。表2 顯示了本文算法與其他同類算法的結果對比，結果表明本文算法在實驗的三個數據集中表現更優。圖5（a）是ORB_SLAM2 計算出的walking_xyz 序列中的相機軌跡，圖5（b）是本文算法計算出相機軌跡。其中灰色虛線表示相機軌跡的真值，彩色線表示計算值，右邊的彩色線條從藍到紅表示誤差值從小到大。圖6 是本文算法與ORB-SLAM2計算出的軌跡與真值間的誤差分布圖，圖6（a）是ORB-SLAM2的計算誤差分布圖，圖6（b）是本文算法的計算誤差分布圖。從圖6（a）中可以看出ORBSLAM2 的計算誤差主要分布在0.5 m 到1.1 m 之間，只有極短時間內的誤差能降到0.1 m左右，從圖6（b）中可以看出本文算法除了極短的時間段內的誤差較大之外絕大部分時間誤差都分布在0 m 到0.1 m 之間。從圖5和圖6 能直觀地看出本文算法相對于ORB_SLAM2 算法在動態環境中對相機位姿的計算準確率更高。

圖5 軌跡真實值與計算值曲線

圖6 絕對軌跡誤差分布對比

3.2 語義建圖實驗

本文選擇walking_static 序列作為語義建圖的圖像序列，在該序列中有兩個人在相機前繞著一張辦工桌走動，是一個高度動態的序列，若不考慮這兩個人的影響，建圖效果如圖7（a）所示，圖中只保留了行人走動密集的區域，這樣更加明顯地看出動態物體運動對地圖構建帶來的影響，地圖中出現了大量的“殘影”，這樣機器人在運動到此處時會認為前方有物體阻攔而無法通過。圖7（b）則是通過語義分割濾除了行人后構建的語義地圖，相比于圖7（a），圖7（b）不僅消除了運動的人帶來的影響，而且賦予了空間點語義信息，這樣不僅有利于機器人執行路徑規劃、導航等任務，而且可以執行諸如“到桌子上取一個杯子過來”這樣的高級任務。

圖7 處理前后地圖對比

4 結束語

為了解決傳統視覺SLAM 無法適用于高動態場景以及地圖缺少語義信息問題，提出了一種融合目標檢測網絡、語義分割網絡的語義SLAM方法。在位姿跟蹤階段利用YOLOv4 目標檢測網絡檢測出環境中動態物體所在區域，在特征提取時剔除分布在動態物體區域的特征點，保留靜態特征點，然后利用靜態特征點進行位姿跟蹤，減少動態物體帶來的負面影響，在地圖構建階段利用SegNet 語義分割網絡對關鍵幀進行語義分割，在構建點云時根據RGB 信息濾除動態物體，從而構建出不含動態物體的語義地圖。在TUM數據集上的實驗結果表明，本文算法相對于傳統視覺SLAM算法在動態環境中明顯擁有更好的效果。