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

(1.中國科學(xué)院 電子學(xué)研究所 傳感技術(shù)國家重點(diǎn)實(shí)驗(yàn)室,北京 100190;2.中國科學(xué)院大學(xué) 電子電氣與通信工程學(xué)院,北京 100049)

0 引 言

傳統(tǒng)的同時(shí)定位與地圖構(gòu)建(simultaneous localization and mapping，SLAM)技術(shù)建立在靜態(tài)環(huán)境下，不考慮環(huán)境物體的運(yùn)動(dòng)。而實(shí)際環(huán)境中，人的走動(dòng)、車輛的來往都會(huì)造成環(huán)境動(dòng)態(tài)變化，從而使SLAM系統(tǒng)建立的地圖無法保持長時(shí)間的一致性，基于視覺的特征也會(huì)因?yàn)槲矬w的運(yùn)動(dòng)而變得不穩(wěn)定。在倉儲(chǔ)、無人駕駛等定位精度要求較高的領(lǐng)域，移動(dòng)中的人對SLAM定位精度的影響會(huì)導(dǎo)致位姿漂移、跟蹤失敗、誤差累積等問題。

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

本文提出了一種結(jié)合語義的魯棒視覺SLAM—Dynamic-SLAM算法，采用深度學(xué)習(xí)技術(shù)在語義層面實(shí)現(xiàn)對動(dòng)態(tài)物體的檢測，并對動(dòng)態(tài)物體特征點(diǎn)進(jìn)行剔除，以消除其在SLAM定位與建圖中的誤差。實(shí)驗(yàn)證明，該方案在各種動(dòng)態(tài)環(huán)境中都取得了較好的定位精度和魯棒性。

1 系統(tǒng)框架

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

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

1.1 物體檢測

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

在動(dòng)態(tài)環(huán)境SLAM中，動(dòng)態(tài)物體檢測的成功與否直接決定了系統(tǒng)的其他模塊是否能夠正常執(zhí)行。一旦發(fā)生漏檢，相鄰兩張圖像間的巨大差異將會(huì)導(dǎo)致特征點(diǎn)數(shù)量急劇變化，從而導(dǎo)致系統(tǒng)的不穩(wěn)定。為了能夠穩(wěn)定、有效地剔除動(dòng)態(tài)特征點(diǎn)，必須在物體檢測時(shí)獲得足夠高的檢測精度。在常規(guī)的物體檢測任務(wù)中，由于各個(gè)圖片間不具有明顯的關(guān)聯(lián)，無法通過上下文信息提高檢測精度。但在SLAM中，由于視頻幀按照時(shí)間序列抵達(dá)，可以借助前若干幀的檢測結(jié)果預(yù)測下一次的檢測結(jié)果，從而彌補(bǔ)下一次可能出現(xiàn)的漏檢或誤檢?；谶@一思想，本文提出了相鄰幀漏檢補(bǔ)償模型，該模型基于一個(gè)合理的假設(shè)：“動(dòng)態(tài)物體的運(yùn)動(dòng)速度不會(huì)超過某個(gè)閾值”。用X表示動(dòng)態(tài)物體的坐標(biāo)，Vth表示動(dòng)態(tài)物體運(yùn)動(dòng)速度的閾值，F(xiàn)PS表示幀率，兩者之間應(yīng)該滿足X

漏檢補(bǔ)償流程如下：

1)當(dāng)前幀K1進(jìn)入SSD網(wǎng)絡(luò)，輸出檢測到的物體列表，列表中的每一項(xiàng)包括檢測出的物體的位置坐標(biāo)X1i(0

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

3)修正后的檢測結(jié)果列表作為動(dòng)態(tài)物體判定原始數(shù)據(jù)。

1.2 動(dòng)態(tài)物體判定

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

圖1 常見物體的動(dòng)態(tài)特性評分

圖中只列出了一部分常見的物體，其他物體的評分可以根據(jù)具體應(yīng)用設(shè)定合適的分?jǐn)?shù)。將物體分?jǐn)?shù)與一個(gè)事先定義的閾值相比較，分?jǐn)?shù)高于閾值時(shí)判定為動(dòng)態(tài)物體，低于閾值時(shí)則判定為靜態(tài)物體。閾值的大小視情況而定，通?？稍O(shè)為5。

1.3 動(dòng)態(tài)環(huán)境SLAM

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

圖2 Dynamic-SLAM流程框圖

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

式中ξ將觀測到的像素坐標(biāo)與3D點(diǎn)按照當(dāng)前位姿ξ投影后的2D坐標(biāo)求差，該誤差即為重投影誤差。優(yōu)化目標(biāo)是找到一個(gè)相機(jī)位姿ξ，使得重投影誤差最小。

在動(dòng)態(tài)環(huán)境中，受到運(yùn)動(dòng)物體的影響，動(dòng)態(tài)物體上特征點(diǎn)的重投影誤差會(huì)處于過高的水平，導(dǎo)致相機(jī)位姿ξ無法收斂到最優(yōu)值，定位誤差顯著增大。為此Dynamic-SLAM的追蹤線程在提取ORB特征后，根據(jù)當(dāng)前檢測到的動(dòng)態(tài)物體的位置實(shí)施特征點(diǎn)剔除操作，將動(dòng)態(tài)物體其上的特征點(diǎn)予以剔除。在后續(xù)的局部地圖匹配、相機(jī)位姿估計(jì)和非線性優(yōu)化中，只利用靜態(tài)物體上的特征點(diǎn)，保證整個(gè)過程中重投影的一致性，使SLAM系統(tǒng)不受動(dòng)態(tài)物體干擾。

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

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

本文設(shè)計(jì)并實(shí)施了一系列實(shí)驗(yàn)來驗(yàn)證Dynamic-SLAM系統(tǒng)在動(dòng)態(tài)環(huán)境下的魯棒性和定位精度。使用TUM RGB-D benchmark[16]中的Walking_rpy數(shù)據(jù)集驗(yàn)證物體識(shí)別的準(zhǔn)確率和檢出率，使用2段采集的數(shù)據(jù)集驗(yàn)證動(dòng)態(tài)物體干擾下初始化和定位的魯棒性，使用TUM RGB-D Benchmark中的Walking_xyz數(shù)據(jù)集驗(yàn)證一般動(dòng)態(tài)環(huán)境下的定位精度。

實(shí)驗(yàn)運(yùn)行環(huán)境為Intel Core i5-7300HQ(4核2.5 GHz)，8 GB內(nèi)存，NVIDIA GeForce GTX1050Ti顯卡，4 GB顯存。

2.1 物體檢測

在該項(xiàng)測試中，采用Walking_rpy數(shù)據(jù)集，在連續(xù)487幀圖片中，SSD的原始檢測結(jié)果成功檢出401次，失敗86次，檢出率82.3 %。經(jīng)過運(yùn)動(dòng)補(bǔ)償后的檢測結(jié)果成功檢出486次，失敗1次，檢出率達(dá)到99.8 %。圖3所示為物體檢測結(jié)果每隔30幀的抽樣，左側(cè)為SSD的原始檢測結(jié)果，右側(cè)為經(jīng)過相鄰幀漏檢補(bǔ)償后的檢測結(jié)果，方框?yàn)闄z測到的物體位置，左下角標(biāo)簽標(biāo)注了物體類別。實(shí)驗(yàn)表明，相鄰幀漏檢補(bǔ)償模型大大提高了物體檢測的檢出率，為后續(xù)的SLAM定位與建圖模塊打下了良好的基礎(chǔ)。

圖3 物體檢測測試

2.2 動(dòng)態(tài)物體干擾下的初始化測試

初始化的成功與否關(guān)系到后續(xù)的定位是否準(zhǔn)確。由于初始時(shí)刻尚無事先建立的地圖，只能通過幀間匹配來確定相機(jī)的運(yùn)動(dòng)，導(dǎo)致動(dòng)態(tài)環(huán)境下的初始化變得尤為困難。在動(dòng)態(tài)物體的干擾下，SLAM很容易錯(cuò)誤初始化。實(shí)驗(yàn)中，將攝像頭放置在桌面上固定不動(dòng)，測試人的來往走動(dòng)對初始化會(huì)造成怎樣的影響。

實(shí)驗(yàn)結(jié)果如圖4所示?？梢钥吹?，ORB-SLAM 2在面對動(dòng)態(tài)物體時(shí)，不能分辨前景和背景物體，無法排除動(dòng)態(tài)物體的干擾，特征點(diǎn)大多聚集在動(dòng)態(tài)物體之上，到第4幅圖時(shí)已經(jīng)錯(cuò)誤初始化。而Dynamic-SLAM成功檢測出動(dòng)態(tài)物體的位置，并將其上的特征點(diǎn)剔除，從而避免了錯(cuò)誤初始化。

2.3 抗動(dòng)態(tài)物體干擾測試

針對人始終存在于相機(jī)視野中的情況設(shè)計(jì)了一個(gè)測試集，分別測試ORB-SLAM2和Dynamic-SLAM的運(yùn)行效果。如圖5所示。

圖5 抗動(dòng)態(tài)環(huán)境干擾測試

圖6 抗動(dòng)態(tài)環(huán)境干擾測試定位和建圖結(jié)果

圖5(a)中，為ORB-SLAM 2的特征提取(點(diǎn)狀為特征點(diǎn))，圖5(b)為Dynamic-SLAM的動(dòng)態(tài)物體識(shí)別和特征提取。圖6(a)ORB-SLAM 2的定位與建圖結(jié)果，6(b)Dyna-mic-SLAM的定位與建圖結(jié)果?？梢园l(fā)現(xiàn)，由于無法區(qū)分動(dòng)態(tài)物體和靜態(tài)物體，ORB-SLAM 2提取的特征點(diǎn)大多集中在人身上，使得系統(tǒng)把人當(dāng)成背景環(huán)境，從而導(dǎo)致定位的結(jié)果完全依賴于人和相機(jī)之間的相對運(yùn)動(dòng)。在圖6(a)中可以明顯地看到，相機(jī)在運(yùn)動(dòng)一段時(shí)間后就完全停止了，地圖點(diǎn)也不再更新。而Dynamic-SLAM能夠自動(dòng)選取靜態(tài)環(huán)境的特征，定位結(jié)果是一條連續(xù)的直線，建立的地圖也基本與實(shí)際場景相吻合。

2.4 TUM動(dòng)態(tài)環(huán)境數(shù)據(jù)集測試

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

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

圖7 Walking_xyz數(shù)據(jù)集測試

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

3 結(jié) 論

本文提出了一種動(dòng)態(tài)環(huán)境下結(jié)合語義的魯棒視覺SLAM算法，實(shí)驗(yàn)結(jié)果表明，改進(jìn)后的算法在動(dòng)態(tài)環(huán)境下的定位和建圖精度更高，魯棒性更強(qiáng)。與目前State-Of-the-Art的視覺SLAM算法ORB-SLAM 2相比，在動(dòng)態(tài)環(huán)境數(shù)據(jù)集下的定位精度提高22.6 %，性能提高10 %。