佟云昊，席志紅

哈爾濱工程大學(xué) 信息與通信工程學(xué)院，黑龍江 哈爾濱 150001

同步定位與地圖構(gòu)建[1]（simultaneous localization and mapping，SLAM）是指在陌生環(huán)境、周圍環(huán)境信息未知的情況下，移動機器人作為載體可通過自身搭載的視覺傳感器采集信息，然后估計出自身的位置和感知周圍的環(huán)境信息。因其效率高和功耗低等諸多特點在許多場景應(yīng)用廣闊，當(dāng)前，成熟的SLAM 系統(tǒng)已經(jīng)應(yīng)用到移動機器人等智能家居上，但是，傳統(tǒng)的SLAM 系統(tǒng)中依舊存在一些問題值得改進：移動機器人獲取的信息比較單一，傳統(tǒng)的視覺里程計中的關(guān)鍵幀不包括語義信息，在實際的機器人運行場景中，所選取的關(guān)鍵幀包含了大量的匹配誤差和動態(tài)點，以至于會給機器人的定位造成干擾。

近年來，隨著深度學(xué)習(xí)發(fā)展日漸完善，基于深度學(xué)習(xí)的移動機器人室內(nèi)定位與建圖的方法受到了學(xué)者們的廣泛關(guān)注。ORB-SLAM2 系統(tǒng)[2]是當(dāng)前相對成熟的SLAM 框架之一，因其穩(wěn)定性高、魯棒性好[3]而得到眾多研究者使用，但是在動態(tài)環(huán)境下，單純的ORB-SLAM2 框架容易被環(huán)境因素干擾，且只能創(chuàng)建稀疏的點云地圖，不能為后續(xù)的導(dǎo)航工作提供有效環(huán)境信息。Girshick 等[4]在2014 年提出了區(qū)域卷積神經(jīng)網(wǎng)絡(luò)(region-based convolutional neural networks, R-CNN)模型，具有里程碑意義的把卷積神經(jīng)網(wǎng)絡(luò)(convolutional neural networks, CNN ）融入到目標檢測系統(tǒng)，采用選擇搜索的方法生成出候選區(qū)，在候選區(qū)中采用卷積神經(jīng)網(wǎng)絡(luò)生成特征向量，最后進行檢測物體的類別判斷。在采用R-CNN 網(wǎng)絡(luò)模型進行特征提取中，會出現(xiàn)重復(fù)提取的現(xiàn)象，Girshick 等[5]在2015 年對R-CNN 網(wǎng)絡(luò)模型進行改進，提出了快速卷積神經(jīng)網(wǎng)絡(luò)(Fast R-CNN)和更快速卷積神經(jīng)網(wǎng)絡(luò)(Faster R-CNN)，其中Fast R-CNN 提高了檢測速度，合并了分類問題和邊框回歸問題。Faster RCNN 是為了提高選擇候選區(qū)域速度的問題，將選擇搜索策略改為區(qū)域推薦網(wǎng)絡(luò)，使目標檢測實現(xiàn)端點到端點的計算，從而提高了目標檢測的準確度和速度。Redmom 等[6]在2016 年提出了基于目標檢測的YOLO 算法，該算法采用全連接網(wǎng)絡(luò)，在數(shù)據(jù)輸出層輸出目標的位置和所屬類別，盡管其精度較Fast R-CNN 有所下降，但是YOLO 算法極大的提高了檢測速度，使得實時的目標檢測成為可能。

本文針對移動機器人在室內(nèi)環(huán)境定位容易受到動態(tài)環(huán)境干擾造成相機位姿不準確的問題，提出了一種基于目標檢測的室內(nèi)動態(tài)場景SLAM 系統(tǒng)，將YOLOv4 網(wǎng)絡(luò)[7]與ORB-SLAM2 算法相結(jié)合。 同時，引入LK 光流法[8]對圖像中可能殘留的動態(tài)特征點進行過濾。 此外，只使用靜態(tài)特征點進行特征匹配來解決攝像機的位姿問題，從而消除動態(tài)物體對SLAM 系統(tǒng)的影響。 利用點云拼接技術(shù)建立了無動態(tài)物體干擾的稠密點云[9]地圖，并轉(zhuǎn)化為八叉樹[10]地圖，極大地減少了地圖的存儲空間，進而為后續(xù)的導(dǎo)航系統(tǒng)做準備。

1 本文SLAM 系統(tǒng)框架

在動態(tài)環(huán)境中，ORB-SLAM2 算法受到移動對象的影響，導(dǎo)致定位精度降低、魯棒性差。為了解決這個問題，提出了一種基于對象檢測線程的運動對象檢測ORB-SLAM2 算法。同時，實現(xiàn)了網(wǎng)絡(luò)對象檢測在提取特征點的同時，利用YOLOv4目標檢測網(wǎng)絡(luò)檢測輸入圖像中的目標前端。獲取圖像中的語義信息后，動態(tài)對象在圖像中是確定的。根據(jù)物體檢測結(jié)果，模塊可以刪除動態(tài)特征點添加到跟蹤線程。

因為環(huán)境中有一些潛在動態(tài)的和一些模糊的物體，在特征匹配時找出所有匹配兩幀圖像特征點之間的最小距離和最大距離，當(dāng)距離大于兩倍的最小距離時，即認為匹配有誤。然后,根據(jù)特征點匹配結(jié)果，采用串聯(lián)LK 光流法檢測并去除動態(tài)特征點進行極外幾何約束。最后，只使用剩余的靜態(tài)特征點進行估計相鄰幀之間的姿勢。本文中給出了改進算法的流程如圖1 所示。

圖1 本文SLAM系統(tǒng)框架

2 YOLOv4 目標檢測

標準的目標檢測網(wǎng)絡(luò)是由數(shù)據(jù)輸入、骨干網(wǎng)絡(luò)、頸部、頭部4 個部分構(gòu)成。YOLOv4 算法是通過把原YOLOv3 中的主干網(wǎng)絡(luò)DarkNet53[12]和CSPNet[13]相結(jié)合，使殘差網(wǎng)絡(luò)的基礎(chǔ)層映射分為2 部分，并且采用跨階層級融合了2 部分，該方法不僅提升了目標檢測的速度和精度，同時也減少了內(nèi)存占用。YOLOv4 算法在骨干網(wǎng)絡(luò)使用中，采用Mish 函數(shù)[14]代替了原來系統(tǒng)中的Leaky ReLU。并且將空間金字塔池化引入系統(tǒng)，利用4 種不同尺度的最大池化處理上層輸出，增加了其感受野。頸部通過采用PANe 代替原YOLOv3 中的FPN，作為參數(shù)集合的方法，將原網(wǎng)絡(luò)修改后，采用張量連接的方式代替捷徑鏈接。頭部采用YOLOv3 的網(wǎng)絡(luò)設(shè)計進行多尺度測量，一般采用608×608 大小的圖片作為輸入，根據(jù)結(jié)構(gòu)圖，把3 個不相同層級的特征圖進行融合，產(chǎn)生3 個不同特征尺寸的輸出，分別為13×13×255、26×26×255和52×52×255 的檢測結(jié)果，提高網(wǎng)絡(luò)對不同尺寸特征圖的目標檢測能力。YOLOv4 目標檢測網(wǎng)絡(luò)結(jié)構(gòu)如圖2 所示。

圖2 YOLOv4 網(wǎng)絡(luò)結(jié)構(gòu)

在現(xiàn)實的場景中，動態(tài)的人、貓、狗等是主要的動態(tài)因素，YOLOv4 網(wǎng)絡(luò)會將輸入的圖片目標檢測，生成對應(yīng)邊界框的掩膜區(qū)域，本文系統(tǒng)經(jīng)過YOLOv4 網(wǎng)絡(luò)產(chǎn)生的結(jié)果如圖3 所示。

圖3 YOLOv4 目標檢測結(jié)果

3 LK 光流約束

光流是一種描述特征點在圖像之中運動的屬性，隨著時間變化，同一個像素會在圖像中不斷運動，而跟蹤這種流動的過程叫做光流法。計算部分像素的運動成為稀疏光流，稀疏光流以Lucas-Kanade 光流（LK 光流）為代表。LK 光流法基于3 個假設(shè)：1)每個圖像中的像素亮度在連續(xù)幀之間不變；2) 幀與幀之間時間間隔較短；3) 相鄰的2 個像素具有相似的運動[15-18]。 根據(jù)假設(shè)1)，灰度是恒定的，可以通過。

式中：t和t+dt為相鄰圖像幀對應(yīng)的時間，I(x,y,t)和I(x+dx,y+dy,t+dt)為圖像中像素點的位置。根據(jù)假設(shè)2)，由于相鄰圖像幀之間的時間間隔較小，以式(1)右側(cè)進行泰勒級數(shù)展開為

但是在LK 光流中需要引入額外的約束來計算特征點的速度u和v。 根據(jù)假設(shè)3)，即相鄰像素有相似的運動，選擇一個以特征點為中心的3×3的窗口，窗口內(nèi)的9 個像素有相同的運動。 可以對9 個像素同時計算為

對鴨坯進行統(tǒng)一前處理，之后進行烤制實驗。具體為紅外蒸汽烤制溫度220℃，烤制時間40min，蒸汽噴射2次，分別為烤制的第10min和第30min，每次噴射時間為2、3、4、5、6s；最后紅外干烤10min。

在極坐標幾何約束下，對目標檢測后的準靜態(tài)特征點計算光流大小。 然后通過求解其均值和標準差可以判斷特征點是否為動態(tài)特征點。

式中：Li為第i個特征點的光流尺寸，Lavg和Lstd分別為各特征點光流尺寸的均值和標準差，Lthr1和Lthr2為預(yù)置閾值[19]。 若第i個特征點的光流大小Li滿足上述關(guān)系，那么判定特殊點i為動態(tài)特征點，反之為靜態(tài)特征點。從圖4 可以看出，動態(tài)特征點在光流尺寸上較靜態(tài)特征點有顯著變化，圖片中的2 個人被判斷為動態(tài)特征，所以2 個人身上覆蓋光流點，而桌子上的顯示器沒有運動，被判斷為靜態(tài)特征。該方法可有效幫助區(qū)分動態(tài)、靜態(tài)特征點。

圖4 LK 光流法區(qū)分動靜態(tài)特征點

4 動態(tài)特征點剔除

在動態(tài)環(huán)境下傳統(tǒng)的視覺SLAM 算法極易造成相機位姿估計不準確的問題，本文系統(tǒng)通過在ORB-SLAM2 算法的基礎(chǔ)上進行改進，在原框架下添加目標檢測線程，當(dāng)RGDB-D 圖像幀進入系統(tǒng)，跟蹤線程和語義分割線程并行，對圖像幀進行圖像處理。采用YOLOv4 目標檢測算法對動態(tài)區(qū)域判斷，通過先驗的動態(tài)信息直接選擇特征物體種類的矩形區(qū)域。使用LK 光流法跟蹤特征點，根據(jù)特征點到極線的距離，通過矩形區(qū)域中動態(tài)特征點的數(shù)量以及所占比例進行篩選，判斷是否為特征點。本文算法經(jīng)過YOLOv4 目標檢測和LK 光流法篩選出數(shù)據(jù)集中動態(tài)特征，并進行剔除，只采用顯示器、桌子等靜態(tài)特征點進行系統(tǒng)定位，有效地提高了定位精度。傳統(tǒng)的ORBSLAM2 算法和本文算法在數(shù)據(jù)集運行的過程如圖5、圖6 所示?？梢钥闯霰疚乃惴ㄔ谶\行過程中，2 個人作為動態(tài)區(qū)域被框出，在框內(nèi)已經(jīng)沒有特征點，進而也就不會影響系統(tǒng)的定位。

圖5 傳統(tǒng)ORB-SLAM2 系統(tǒng)

圖6 本文系統(tǒng)

5 實驗測試與結(jié)果分析

本文采用的是由德國慕尼黑大學(xué)制作的TUM RGB-D 公開數(shù)據(jù)集，評估本文系統(tǒng)在動態(tài)環(huán)境下的定位精度。本文分別選用walking-xyz、walkinghalfsphere、walking-static 等3 個公開的動態(tài)數(shù)據(jù)集對本文系統(tǒng)和傳統(tǒng)ORB-SLAM2 系統(tǒng)進行對比測試，評估其在動態(tài)場景中的改進效果。

5.1 相機位姿誤差分析

本文使用2 種評估視覺SLAM 算法定位精度的評價指標[20]、絕對軌跡誤差（absolute trajectory error，ATE）和相對位姿誤差（relative pose error，RPE）。

絕對軌跡誤差可以直觀地反映出算法估計軌跡精度，是直接計算估值位姿和真實位姿的差值來顯示相機軌跡誤差。相對位姿誤差是指在某個固定的時間段內(nèi)，系統(tǒng)估計位姿和相機真實位姿的差值[21]，也稱旋轉(zhuǎn)漂移誤差。均方根誤差(root mean squared error, RMSE ) 反映的是相機軌跡估計值與真實值之間的偏差。假設(shè)整個過程中的觀測次數(shù)為m，對應(yīng)得到的估計值為x1,x2,···,xm，真實值為y1,y2,···,ym，均方根誤差的數(shù)學(xué)表達式為

標準偏差(standard deviation, Std)反映的是SLAM 系統(tǒng)相機軌跡估計的離散程度，數(shù)學(xué)表達式為

式中u為誤差平均值。

以上客觀的評價指標，可以有效地反映出SLAM 系統(tǒng)在經(jīng)過不同系統(tǒng)的穩(wěn)定性與可靠性。ORB-SLAM2 和本文系統(tǒng)分別在3 個數(shù)據(jù)集上測試的絕對軌跡誤差對比結(jié)果如表1 所示，相對位姿誤差對比結(jié)果如表2 所示。

表1 絕對位姿誤差對比結(jié)果

表2 相對位姿誤差對比結(jié)果

由表1 可以看出，整體上相比于ORB-SLAM2系統(tǒng)，本文SLAM 系統(tǒng)在動態(tài)場景下絕對軌跡誤差的RMSE 分別下降98.08%、93.95% 和83.72%，Std 分別下降98.2%、93.26% 和86.95%，由表2 顯示，在同樣的動態(tài)場景下，旋轉(zhuǎn)漂移-相對位姿誤差的RMSE 分別下降 55.56%、33.33%和80.76%，Std 分別下降53.33%、26.67%和87.5%。分析其原因，是由于本文系統(tǒng)能夠有效地剔除實驗環(huán)境中動態(tài)因素，只采用動態(tài)檢測篩選后的靜態(tài)環(huán)境特征點，用來相機位姿估計，提高了相機的定位精度。利用evo 工具對傳統(tǒng)的ORB-SLAM2 系統(tǒng)與本文系統(tǒng)繪制相機軌跡，評估估計軌跡和真值的誤差，系統(tǒng)在walking-xyz數(shù)據(jù)集上的實驗結(jié)果如圖7、圖8 所示。ORB-SLAM2 系統(tǒng)與本文系統(tǒng)在walking-xyz數(shù)據(jù)集上的絕對軌跡誤差和旋轉(zhuǎn)漂移誤差結(jié)果如圖9~圖12 所示。

圖7 ORB-SLAM2 系統(tǒng)相機估計軌跡和真實軌跡及誤差

圖8 本文系統(tǒng)相機估計軌跡和真實軌跡及誤差

圖9 ORB-SLAM2 系統(tǒng)絕對位姿估計誤差曲線

圖10 本文系統(tǒng)絕對位姿估計誤差曲線

圖11 ORB-SLAM2 系統(tǒng)相對位姿估計誤差曲線

圖12 本文系統(tǒng)相對位姿估計誤差曲線

5.2 地圖構(gòu)建

傳統(tǒng)的ORB-SLAM2 系統(tǒng)所創(chuàng)建稀疏點云地圖中缺少環(huán)境信息，環(huán)境信息不準確，無法用于后續(xù)的導(dǎo)航工作。在相機的運行過程中，相機頻率較高，但是相鄰的圖像幀之間變化較小。為了避免計算量過大，本文利用關(guān)鍵幀篩選，通過設(shè)置閾值，將沒有超過閾值的圖像幀，進入建圖線程，保證進入建圖線程的圖像幀需要處理的數(shù)據(jù)可行性，避免點云地圖冗余，創(chuàng)建點云地圖。本文利用目標檢測結(jié)果剔除室內(nèi)場景中的先驗動態(tài)對象，然后根據(jù)相機提供的深度圖和RGB 彩色圖，結(jié)合相機內(nèi)參以及相機的位姿（相機外參），將稀疏點云加起來，實現(xiàn)點云拼接，構(gòu)建無動態(tài)環(huán)境干擾的靜態(tài)稠密點云地圖。加入外點去除濾波器和降采樣濾波器對點云進行濾波處理，得到噪聲小的稠密點云地圖如圖13 所示。

圖13 稠密點云地圖

稠密點云地圖可將地圖環(huán)境信息保存更加完整，可以較清楚地顯示細節(jié)，本文在稠密點云地圖的基礎(chǔ)上進一步建立八叉樹地圖如圖14 所示。

圖14 八叉樹地圖

數(shù)據(jù)顯示，稠密點云地圖所占空間為12.5 MB，八叉樹地圖所占空間為3.3 MB，八叉樹地圖所占空間約為稠密點云地圖的26.4%，能夠節(jié)約大量的存儲空間。

6 結(jié)論

在室內(nèi)動態(tài)環(huán)境中，SLAM 系統(tǒng)容易受到周圍動態(tài)環(huán)境干擾，從而降低位姿估計精度，本文提出了一種基于ORB-SLAM2 框架的室內(nèi)動態(tài)場景SLAM 系統(tǒng)。

本文系統(tǒng)采用YOLOv4 目標檢測網(wǎng)絡(luò)對環(huán)境進行語義分割，獲取語義信息，再聯(lián)合LK 光流法對動態(tài)目標進行檢測，判定動態(tài)物體區(qū)域，在對動態(tài)特征點進行跟蹤前，篩出動態(tài)信息，只使用靜態(tài)特征點進行位姿估計，有效地減少動態(tài)信息對相機位姿估計影響，并同時提高地圖構(gòu)建的準確性。

在地圖構(gòu)建方面，本文實現(xiàn)了稠密點云地圖，并為移動機器人提供導(dǎo)航地圖，構(gòu)建八叉樹地圖，減少存儲空間，為后續(xù)實現(xiàn)機器人導(dǎo)航提供基礎(chǔ)。

然而，本文SLAM 系統(tǒng)在構(gòu)建地圖實時性方面仍有待提高，目標檢測網(wǎng)絡(luò)方面仍需進一步加強，提升網(wǎng)絡(luò)性，減少運行時間和相機位姿估計誤差，保證后續(xù)移動機器人的避障和路徑規(guī)劃。