動態環境下結合語義的魯棒視覺SLAM*

王金戈， 鄒旭東， 仇曉松， 蔡浩原

(1.中國科學院 電子學研究所 傳感技術國家重點實驗室,北京 100190;2.中國科學院大學 電子電氣與通信工程學院,北京 100049)

0 引 言

傳統的同時定位與地圖構建(simultaneous localization and mapping，SLAM)技術建立在靜態環境下，不考慮環境物體的運動。而實際環境中，人的走動、車輛的來往都會造成環境動態變化，從而使SLAM系統建立的地圖無法保持長時間的一致性，基于視覺的特征也會因為物體的運動而變得不穩定。在倉儲、無人駕駛等定位精度要求較高的領域，移動中的人對SLAM定位精度的影響會導致位姿漂移、跟蹤失敗、誤差累積等問題。

為了使SLAM在動態環境下正常工作，需要避免使用處于動態物體上的特征點，因此,需要事先計算出動態物體的位置。目前常用的動態物體提取方法都是基于幾何特征[1,2]，當面對更加極端的動態環境時，如人靠近鏡頭的走動，依然會失效。

本文提出了一種結合語義的魯棒視覺SLAM—Dynamic-SLAM算法，采用深度學習技術在語義層面實現對動態物體的檢測，并對動態物體特征點進行剔除，以消除其在SLAM定位與建圖中的誤差。實驗證明，該方案在各種動態環境中都取得了較好的定位精度和魯棒性。

1 系統框架

本文在ORB-SLAM2[3～5]的基礎上，增加基于語義的動態物體判定模型，并優化基于特征點的視覺里程計算法，使其能夠舍棄附著在動態物體上的特征點，只采用非動態物體的特征點參與位姿估計和非線性優化，從而避免動態物體特征點的干擾。在物體檢測部分，提出了基于運動模型的物體檢測補償算法，進一步提高了物體檢測精度。

單目相機實時采集的圖像作為SLAM定位與建圖模塊和物體檢測模塊的輸入，物體檢測模塊的輸出經過語義校正模塊后實時反饋給SLAM定位與建圖模塊，SLAM定位與建圖模塊最后給出定位和建圖結果。

1.1 物體檢測

本文采用文獻[10]提出的SSD(single shot multibox detector)物體檢測網絡，該網絡使用VGG16的基礎網絡結構，保留前5層不變，利用Atrous[11]算法將fc6和fc7層轉換成2個卷積層，再在后面增加3個卷積層和1個平均池化層。使用不同網絡層的信息來模擬不同尺度下的圖像特征，最后通過非最大抑制得到最終的檢測結果。由于舍棄了最初的候選框生成階段，使得整個物體檢測流程能夠在單一網絡下完成，從而實現較高的檢測效率(46 FPS，Titan X)和檢測精度(77.2 %)。

在動態環境SLAM中，動態物體檢測的成功與否直接決定了系統的其他模塊是否能夠正常執行。一旦發生漏檢，相鄰兩張圖像間的巨大差異將會導致特征點數量急劇變化，從而導致系統的不穩定。為了能夠穩定、有效地剔除動態特征點，必須在物體檢測時獲得足夠高的檢測精度。在常規的物體檢測任務中，由于各個圖片間不具有明顯的關聯，無法通過上下文信息提高檢測精度。但在SLAM中，由于視頻幀按照時間序列抵達，可以借助前若干幀的檢測結果預測下一次的檢測結果，從而彌補下一次可能出現的漏檢或誤檢。基于這一思想，本文提出了相鄰幀漏檢補償模型，該模型基于一個合理的假設：“動態物體的運動速度不會超過某個閾值”。用X表示動態物體的坐標，Vth表示動態物體運動速度的閾值，FPS表示幀率，兩者之間應該滿足X

漏檢補償流程如下：

1)當前幀K1進入SSD網絡，輸出檢測到的物體列表，列表中的每一項包括檢測出的物體的位置坐標X1i(0

2)若對于前一幀K0的檢測結果中的每一項X0j(0

3)修正后的檢測結果列表作為動態物體判定原始數據。

1.2 動態物體判定

本文在語義的層面上提出了基于先驗知識的動態物體判定方法。SLAM系統如果不從語義層面理解周圍的環境，就無法真正區分哪些是動態的，哪些是靜態的，只能在短時間內找出運動的物體，而無法保證長時間的一致性。因此，本文將物體檢測的結果與先驗知識相結合，給出動態物體判定模型。根據人的先驗知識，對物體的動態特性評分，0分為靜態物體，10分為動態物體，常見物體在該區間上所處的大致位置如圖1所示。

圖1 常見物體的動態特性評分

圖中只列出了一部分常見的物體，其他物體的評分可以根據具體應用設定合適的分數。將物體分數與一個事先定義的閾值相比較，分數高于閾值時判定為動態物體，低于閾值時則判定為靜態物體。閾值的大小視情況而定，通常可設為5。

1.3 動態環境SLAM

在ORB-SLAM2原有框架的基礎上，增加了物體檢測線程和基于語義的校正模塊。新增的模塊與ORB-SLAM2已有的3個線程的關系如圖2所示。在新的視頻幀抵達后，同時傳入物體檢測線程和跟蹤線程，兩者并行地對圖像進行處理。物體檢測線程采用前述的SSD物體檢測網絡計算出物體的類別和位置，進一步由基于語義的校正模塊將其中的物體分為動態物體和靜態物體，最后把動態物體的位置提供給跟蹤線程。

圖2 Dynamic-SLAM流程框圖

跟蹤線程對每一幀圖像提取ORB特征[12]，通過與參考幀的特征匹配，得到2張圖像間特征點的對應關系，利用這些對應關系估計相機位姿。在初始化完成的情況下，相機位姿估計是一個PnP(perspective-n-point)問題，求解該問題的方法有很多[13,14]，本文采用以集束調整(bundle adjustment)[15]為代表的非線性優化方法，該方法可以充分利用所有匹配結果，得到位姿的最優估計。構建非線性優化問題，最小化重投影誤差如下

式中ξ將觀測到的像素坐標與3D點按照當前位姿ξ投影后的2D坐標求差，該誤差即為重投影誤差。優化目標是找到一個相機位姿ξ，使得重投影誤差最小。

在動態環境中，受到運動物體的影響，動態物體上特征點的重投影誤差會處于過高的水平，導致相機位姿ξ無法收斂到最優值，定位誤差顯著增大。為此Dynamic-SLAM的追蹤線程在提取ORB特征后，根據當前檢測到的動態物體的位置實施特征點剔除操作，將動態物體其上的特征點予以剔除。在后續的局部地圖匹配、相機位姿估計和非線性優化中，只利用靜態物體上的特征點，保證整個過程中重投影的一致性，使SLAM系統不受動態物體干擾。

為了保證SLAM系統的實時性，物體檢測和跟蹤分處2個線程，設計了安全高效且支持并發操作的數據結構Detection來傳遞檢測結果，并使用互斥鎖Unique_lock保證不發生訪問沖突，在寫入操作執行前需事先獲取鎖。物體檢測線程和跟蹤線程的處理速度并不一致，采取異步讀寫共享變量的方式實現線程間通信，最大限度地利用CPU時間。

2 實驗與結果分析

本文設計并實施了一系列實驗來驗證Dynamic-SLAM系統在動態環境下的魯棒性和定位精度。使用TUM RGB-D benchmark[16]中的Walking_rpy數據集驗證物體識別的準確率和檢出率，使用2段采集的數據集驗證動態物體干擾下初始化和定位的魯棒性，使用TUM RGB-D Benchmark中的Walking_xyz數據集驗證一般動態環境下的定位精度。

實驗運行環境為Intel Core i5-7300HQ(4核2.5 GHz)，8 GB內存，NVIDIA GeForce GTX1050Ti顯卡，4 GB顯存。

2.1 物體檢測

在該項測試中，采用Walking_rpy數據集，在連續487幀圖片中，SSD的原始檢測結果成功檢出401次，失敗86次，檢出率82.3 %。經過運動補償后的檢測結果成功檢出486次，失敗1次，檢出率達到99.8 %。圖3所示為物體檢測結果每隔30幀的抽樣，左側為SSD的原始檢測結果，右側為經過相鄰幀漏檢補償后的檢測結果，方框為檢測到的物體位置，左下角標簽標注了物體類別。實驗表明，相鄰幀漏檢補償模型大大提高了物體檢測的檢出率，為后續的SLAM定位與建圖模塊打下了良好的基礎。

圖3 物體檢測測試

2.2 動態物體干擾下的初始化測試

初始化的成功與否關系到后續的定位是否準確。由于初始時刻尚無事先建立的地圖，只能通過幀間匹配來確定相機的運動，導致動態環境下的初始化變得尤為困難。在動態物體的干擾下，SLAM很容易錯誤初始化。實驗中，將攝像頭放置在桌面上固定不動，測試人的來往走動對初始化會造成怎樣的影響。

實驗結果如圖4所示。可以看到，ORB-SLAM 2在面對動態物體時，不能分辨前景和背景物體，無法排除動態物體的干擾，特征點大多聚集在動態物體之上，到第4幅圖時已經錯誤初始化。而Dynamic-SLAM成功檢測出動態物體的位置，并將其上的特征點剔除，從而避免了錯誤初始化。

2.3 抗動態物體干擾測試

針對人始終存在于相機視野中的情況設計了一個測試集，分別測試ORB-SLAM2和Dynamic-SLAM的運行效果。如圖5所示。

圖5 抗動態環境干擾測試

圖6 抗動態環境干擾測試定位和建圖結果

圖5(a)中，為ORB-SLAM 2的特征提取(點狀為特征點)，圖5(b)為Dynamic-SLAM的動態物體識別和特征提取。圖6(a)ORB-SLAM 2的定位與建圖結果，6(b)Dyna-mic-SLAM的定位與建圖結果。可以發現，由于無法區分動態物體和靜態物體，ORB-SLAM 2提取的特征點大多集中在人身上，使得系統把人當成背景環境，從而導致定位的結果完全依賴于人和相機之間的相對運動。在圖6(a)中可以明顯地看到，相機在運動一段時間后就完全停止了，地圖點也不再更新。而Dynamic-SLAM能夠自動選取靜態環境的特征，定位結果是一條連續的直線，建立的地圖也基本與實際場景相吻合。

2.4 TUM動態環境數據集測試

本文選擇TUM RGB-D benchmark中的Walking_xyz數據集，該數據集的場景中有2個人在辦公桌周圍來回走動，運動幅度大，且在視野中占據了不小的比例，本文測試兩種算法在該場景下的定位精度和性能。

圖7(a)為ORB-SLAM2的實時畫面，圖7(b)為Dynamic-SLAM的實時畫面。記錄了所有關鍵幀的定位結果，與真實值比較并計算誤差。

圖7 Walking_xyz數據集測試

從實驗結果可以看出，在均方根誤差、平均誤差、誤差中間值、誤差標準差、最小誤差和最大誤差這6個指標中，Dynamic-SLAM(分別為1.68,1.59,1.74,0.55,0.54,2.81 cm)都明顯優于ORB-SLAM2(分別為2.17,2.05,2.02,0.68,0.95,4.01 cm)。以均方根誤差為標準，Dynamic-SLAM的精度比ORB-SLAM 2提高了22.6 %。在性能方面，記錄了2個算法的運行時間，多次運行取平均值的方法Dynamic-SLAM的性能比ORB-SLAM 2提高了10 %。雖然增加了物體檢測流程，但由于物體檢測放在一個獨立線程中，并利用了GPU加速，非但沒有拉低整個系統的運行速度，反而使運行時間縮短了。這得益于無效特征點的減少，使系統只借助于有效的特征點進行計算，從而節約了位姿估計和非線性優化的時間。

3 結 論

本文提出了一種動態環境下結合語義的魯棒視覺SLAM算法，實驗結果表明，改進后的算法在動態環境下的定位和建圖精度更高，魯棒性更強。與目前State-Of-the-Art的視覺SLAM算法ORB-SLAM 2相比，在動態環境數據集下的定位精度提高22.6 %，性能提高10 %。