王運(yùn)夢,崔 龍,劉釗銘,張 峰

(1.中國科學(xué)院沈陽自動(dòng)化研究所,機(jī)器人學(xué)國家重點(diǎn)實(shí)驗(yàn)室,遼寧沈陽 110016; 2.中國科學(xué)院機(jī)器人與智能制造創(chuàng)新研究院,遼寧沈陽 110169;3.中國科學(xué)院大學(xué),北京 100049)

0 引言

ORB-SLAM系列憑借運(yùn)行速度快、定位準(zhǔn)確率高等優(yōu)勢受到了該領(lǐng)域內(nèi)專家學(xué)者們的廣泛關(guān)注。2017年,文獻(xiàn)[5]對最初始的ORB-SLAM進(jìn)行了改進(jìn),開源了一個(gè)支持單目、雙目、RGB-D 3種相機(jī)模式的ORB-SLAM2,針對靜態(tài)環(huán)境構(gòu)建整個(gè)系統(tǒng),其框架主要包括跟蹤、局部建圖和回環(huán)檢測三大線程,但是機(jī)器人在實(shí)際環(huán)境中會(huì)遇到各種各樣的動(dòng)態(tài)物體。動(dòng)態(tài)物體的干擾會(huì)導(dǎo)致位姿漂移、初始化和跟蹤失敗、累計(jì)誤差等問題,顯著降低定位和建圖的準(zhǔn)確性。

為了使SLAM系統(tǒng)更適合于動(dòng)態(tài)環(huán)境,很多研究者選擇在ORB-SLAM系統(tǒng)的基礎(chǔ)上,對算法進(jìn)行了改進(jìn)和優(yōu)化[6]。2018年,文獻(xiàn)[7]基于ORB-SLAM2提出了針對動(dòng)態(tài)環(huán)境的DS-SLAM,該算法使用RGB-D深度相機(jī),對圖像進(jìn)行分割,進(jìn)行八叉樹稠密建圖,對于人體等具有柔性復(fù)雜運(yùn)動(dòng)的物體識別準(zhǔn)確率較高、魯棒性較好,但是低動(dòng)態(tài)環(huán)境下的處理效果不如ORB-SLAM2;同年,文獻(xiàn)[8]提出基于ORB-SLAM2的可視化SLAM系統(tǒng):DynaSLAM,該算法使用Mask R-CNN網(wǎng)絡(luò)對動(dòng)態(tài)環(huán)境下的物體進(jìn)行實(shí)例分割,添加了動(dòng)態(tài)對象檢測和背景修復(fù)功能。當(dāng)不在具有先驗(yàn)動(dòng)態(tài)信息的物體上提取特征時(shí),跟蹤精度較高,例如汽車、自行車等,但是語義分割部分耗時(shí)較長,降低了系統(tǒng)整體的運(yùn)算速度;2022年,張恒等[9]在ORB-SLAM2的基礎(chǔ)上,將光流法和Mask R-CNN網(wǎng)絡(luò)相結(jié)合,檢測潛在的運(yùn)動(dòng)對象,進(jìn)行全動(dòng)態(tài)特征點(diǎn)的剔除,在TUM數(shù)據(jù)集下的實(shí)驗(yàn)效果較好。

回顧上述針對動(dòng)態(tài)環(huán)境構(gòu)建的SLAM算法,大部分采用語義分割算法對動(dòng)態(tài)物體進(jìn)行分割和剔除導(dǎo)致系統(tǒng)整體運(yùn)行速度緩慢。因此,為了提高系統(tǒng)的整體運(yùn)行效率,本文選擇在ORB-SLAM3系統(tǒng)框架的基礎(chǔ)上,使用YOLOv5目標(biāo)識別算法作為單獨(dú)的動(dòng)態(tài)物體檢測線程,與SLAM定位建圖線程同步進(jìn)行,在TUM單目數(shù)據(jù)集實(shí)驗(yàn)結(jié)果表明,本文所提算法在提高系統(tǒng)精度的同時(shí),顯著提升了整體系統(tǒng)運(yùn)行的效率。

1 系統(tǒng)框架及原理

1.1 YOLOv5原理

本文采用YOLO系列中穩(wěn)定性較高的YOLOv5[10]作為動(dòng)態(tài)物體檢測線程的主要算法,選擇深度和特征圖寬度最小的YOLOV5s網(wǎng)絡(luò)作為動(dòng)態(tài)物體檢測線程的主體網(wǎng)絡(luò),使用體量較小的coco128數(shù)據(jù)集對其進(jìn)行訓(xùn)練。該數(shù)據(jù)集包括128張常見物體類型的圖片,例如人體、汽車、電腦、書本、貓、狗等,能夠滿足室內(nèi)動(dòng)態(tài)物體類別的識別需求。

YOLOv5作為目前性能比最高的目標(biāo)識別算法,將單個(gè)卷積神經(jīng)網(wǎng)絡(luò)應(yīng)用于整個(gè)圖像,其運(yùn)算速度是“Faster R-CNN[11]”算法的百倍。改進(jìn)SLAM的動(dòng)態(tài)物體檢測線程使用YOLOv5的整體框架,如圖1所示,包括輸入端、特征提取網(wǎng)絡(luò)(Backbone)、特征融合網(wǎng)絡(luò)(Neck)、預(yù)測網(wǎng)絡(luò)(Prediction)和輸出端5部分。輸入端采用Mosaic數(shù)據(jù)增強(qiáng)方式,通過隨機(jī)地縮放、剪裁、排布對圖片進(jìn)行拼接,能夠?qū)υ紙D片自適應(yīng)地進(jìn)行錨框計(jì)算和圖片縮放,對于小目標(biāo)的檢測效果較好。Backbone的主干網(wǎng)絡(luò)采用CSP1_1和CSP1_3結(jié)構(gòu),在特征提取部分的Focus結(jié)構(gòu)使用32個(gè)卷積核對圖片進(jìn)行切片操作。Neck的主干網(wǎng)絡(luò)采用FPN結(jié)合PAN的網(wǎng)絡(luò)結(jié)構(gòu),其中包含的CSP2_1網(wǎng)絡(luò)結(jié)構(gòu)顯著增強(qiáng)了算法特征融合的能力。

圖1 YOLOv5算法基本框架

后處理部分采用加權(quán)NMS非極大值抑制,提高了最終物體判定的準(zhǔn)確性。預(yù)測網(wǎng)絡(luò)中的定位和物體檢測部分使用CIoU作為損失函數(shù),顯著加快了算法的收斂速度。GIoU損失函數(shù)公式如下:

(1)

式中:IoU為檢測的準(zhǔn)確率;A、B為任意檢測框;C為包圍A、B檢測框的最小面積。

針對上述問題,設(shè)計(jì)基于WIA-PA傳輸協(xié)議的航站樓環(huán)境參數(shù)實(shí)時(shí)無線監(jiān)測系統(tǒng)。通過開發(fā)具有外電源與鋰電池切換、光伏充電、數(shù)據(jù)無線傳輸?shù)裙δ艿臋z測器實(shí)現(xiàn)系統(tǒng)長時(shí)間可持續(xù)運(yùn)行。檢測器節(jié)點(diǎn)采用變周期數(shù)據(jù)傳輸機(jī)制,通過減少大量重復(fù)數(shù)據(jù)的傳輸實(shí)現(xiàn)系統(tǒng)低功耗運(yùn)行。監(jiān)測系統(tǒng)既可以實(shí)時(shí)掌握航站樓內(nèi)各區(qū)域環(huán)境狀態(tài),同時(shí)通過分析數(shù)據(jù)庫采集的數(shù)據(jù)進(jìn)而分析客流規(guī)律。

1.2 動(dòng)態(tài)物體檢測規(guī)則

動(dòng)態(tài)特征的提取會(huì)增加SLAM算法在構(gòu)建靜態(tài)地圖時(shí)的噪聲和運(yùn)行時(shí)間,一般室內(nèi)場景中的動(dòng)態(tài)物體主要是指人和動(dòng)物等會(huì)產(chǎn)生自主移動(dòng)的物體,靜態(tài)物體主要是生活中常見的不具備生命特征的物體,例如桌椅、冰箱和電腦等靜止物體。本文算法制定的動(dòng)態(tài)物體檢測規(guī)則如表1所示,YES表示物體在框內(nèi),NO表示物體不在框內(nèi),為避免剔除過多有效特征點(diǎn),當(dāng)物體同時(shí)位于或不位于動(dòng)態(tài)物體和靜態(tài)物體框內(nèi)時(shí),不剔除動(dòng)態(tài)特征點(diǎn),對應(yīng)為表中類別1、2、3;當(dāng)物體只位于靜態(tài)物體框內(nèi),不剔除動(dòng)態(tài)特征點(diǎn);當(dāng)物體只位于動(dòng)態(tài)物體框內(nèi)時(shí),則剔除特征點(diǎn),對應(yīng)為表中類別4。

表1 動(dòng)態(tài)物體檢測規(guī)則

1.3 本文系統(tǒng)框架

本文系統(tǒng)的整體框架如圖2所示,在ORB-SLAM3[13]原有框架的基礎(chǔ)上進(jìn)行了改進(jìn),使用YOLOv5作為動(dòng)態(tài)物體檢測線程。本文系統(tǒng)主要由動(dòng)態(tài)物體檢測線程、初始化與跟蹤線程、局部建圖線程、閉環(huán)與地圖合并線程4部分組成。

圖2 本文SLAM系統(tǒng)算法框架

在初始化與跟蹤線程中對圖像數(shù)據(jù)中的每一幀提取ORB特征點(diǎn)。ORB特征提取算法將FAST特征點(diǎn)的檢測方法與BRIEF特征點(diǎn)描述子結(jié)合起來,并在原來的基礎(chǔ)上做了改進(jìn)與優(yōu)化。通過構(gòu)建圖像金字塔保證特征點(diǎn)的尺度不變性,通過計(jì)算灰度質(zhì)心保證特征點(diǎn)的旋轉(zhuǎn)不變性。質(zhì)心的計(jì)算公式如(2)所示:

(2)

式中:C為質(zhì)心;m10、m00、m01為圖像塊的矩。

特征方向?yàn)?/p>

θ=arctan(m01/m10)

(3)

式中θ為角度。

與此同時(shí),動(dòng)態(tài)物體檢測線程對相同的輸入圖像數(shù)據(jù)進(jìn)行動(dòng)態(tài)特征點(diǎn)的劃分,剔除動(dòng)態(tài)特征點(diǎn)后進(jìn)行相機(jī)初始化和位姿跟蹤。使用恒速、參考關(guān)鍵幀和重定位跟蹤3種模式層層推進(jìn),極大地提高了跟蹤過程的魯棒性和穩(wěn)定性。

局部建圖線程接收跟蹤線程輸入的關(guān)鍵幀并進(jìn)行局部地圖優(yōu)化、刪除冗余關(guān)鍵幀等操作。將優(yōu)化后的關(guān)鍵幀發(fā)送給閉環(huán)與地圖合并線程,閉環(huán)與地圖合并線程對當(dāng)前關(guān)鍵幀進(jìn)行閉環(huán)檢測,判斷舊的關(guān)鍵幀和當(dāng)前關(guān)鍵幀是否形成閉環(huán),在檢測到閉環(huán)的時(shí)候要將當(dāng)前關(guān)鍵幀和閉環(huán)關(guān)鍵幀的位姿進(jìn)行融合,并對新加入的地圖進(jìn)行合并。為防止多個(gè)線程同時(shí)操作同一變量造成混亂,引入unique_lock鎖機(jī)制,四大線程同步并行,顯著提升了算法的整體運(yùn)行效率。

對于二維圖像點(diǎn)和三維地圖點(diǎn)之間的位姿匹配,本文所提算法使用最大似然PnP(perspective n point)[14]算法進(jìn)行求解,對于三維特征點(diǎn)之間的配準(zhǔn)采用ICP[15]算法。在初始化和閉環(huán)檢測部分使用光束平差法(bundle adjustment,BA)進(jìn)行全局優(yōu)化[16],該算法的本質(zhì)為最小化重投影誤差,主要原理是將二維圖像點(diǎn)通過位姿變換得到空間三維點(diǎn)。

(4)

式中:si為尺度值;K為相機(jī)內(nèi)參;ui、vi為觀測到的圖像像素坐標(biāo);ξ^為位姿;Xi、Yi、Zi為空間三維點(diǎn)的坐標(biāo)。

式(4)經(jīng)過移項(xiàng)等變換后得到最小化重投影誤差公式為

(5)

式中Pi為空間三維點(diǎn)的坐標(biāo)。

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

2.1 實(shí)驗(yàn)平臺

本文所提算法的實(shí)驗(yàn)平臺為臺式電腦,內(nèi)存為15.4 GB,硬件配置為IntelCorei7-10700FCPU,頻率為2.9 GHz,使用的顯卡版本為NVIDIA GeForce GTX 1660 SUPER,軟件平臺為Ubuntu20.04系統(tǒng)。

2.2 實(shí)驗(yàn)數(shù)據(jù)集

為驗(yàn)證本文算法的有效性,使用TUM數(shù)據(jù)集中動(dòng)態(tài)特征明顯的4組單目數(shù)據(jù)集進(jìn)行算法驗(yàn)證。數(shù)據(jù)集中所有圖像尺寸均為640 pixel×480 pixel,以無損png格式保存。

2.3 算法評價(jià)指標(biāo)

本文采用運(yùn)行時(shí)間和絕對位姿誤差(absolute pose error,APE)作為算法優(yōu)劣的評價(jià)指標(biāo)。APE通過計(jì)算軌跡與真值之間的的絕對姿態(tài)誤差,整體評估整條軌跡的全局一致性,本文主要通過平均誤差(ME)、均方根誤差(RMSE)、殘差平方和(SSE)和標(biāo)準(zhǔn)差(SD)4類誤差指標(biāo)對算法進(jìn)行評估,誤差值越接近于0,說明算法的準(zhǔn)確度越高。

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

4組數(shù)據(jù)集在動(dòng)態(tài)物體檢測線程的識別結(jié)果示例如圖3所示,由結(jié)果可知,該線程對動(dòng)態(tài)物體(人體)的識別準(zhǔn)確率高達(dá)70%以上,每一幀中動(dòng)態(tài)物體和靜態(tài)物體的檢測框產(chǎn)生重疊區(qū)域的概率高達(dá)80%。

(a)坐立狀態(tài)

(b)坐立狀態(tài)

(c)站立狀態(tài)

(d)站立狀態(tài)圖3 YOLOv5線程目標(biāo)識別示例

使用ORB-SLAM3算法提取到的特征效果如圖4所示,提取到的特征點(diǎn)已被小方格標(biāo)記出,ORB-SLAM3算法對于動(dòng)態(tài)物體的誤提取量較大,其在人體上提取到的特征約占整幀畫面的1/4。使用本文算法的特征提取效果如圖5所示,方形檢測框能夠較為準(zhǔn)確地標(biāo)注出動(dòng)態(tài)物體。通過與圖4未剔除效果對比可知,對于坐態(tài)和站立態(tài)的動(dòng)態(tài)人體,本文算法能夠較為準(zhǔn)確地識別并剔除錯(cuò)誤提取的動(dòng)態(tài)特征點(diǎn),對于不滿足判定條件的靜態(tài)物體不予剔除,能夠保證靜態(tài)地圖的準(zhǔn)確性。

(a)單幀a特征點(diǎn)提取

(b)單幀b特征點(diǎn)提取

(c)單幀c特征點(diǎn)提取

(a)坐立狀態(tài)

(c)坐立狀態(tài)

(d)站立狀態(tài)圖5 本文算法剔除動(dòng)態(tài)點(diǎn)效果示例

表2為3種算法在4組數(shù)據(jù)集上的運(yùn)行時(shí)間。ORB-SLAM2算法在數(shù)據(jù)集freiburg3_walking_static上的初始化失敗,導(dǎo)致沒有采集到運(yùn)行時(shí)間和誤差數(shù)據(jù)。由實(shí)驗(yàn)結(jié)果可知,本文算法在4組數(shù)據(jù)集上的運(yùn)行時(shí)間均為最短,整體效率相比ORB-SLAM3提升了20.65%,相比ORB-SLAM2提升了19.14%,體現(xiàn)了本文算法能夠快速運(yùn)行的優(yōu)異性能。

表2 算法運(yùn)行時(shí)間 s

圖6為本文算法在4組數(shù)據(jù)集上的APE誤差,表3為3種算法在4組數(shù)據(jù)集上經(jīng)過尺度縮放和軌跡對齊處理后的具體APE誤差對比。

(a)freiburg3_sitting_static數(shù)據(jù)集

(b)freiburg3_sitting_xyz數(shù)據(jù)集

(c)freiburg3_walking_static數(shù)據(jù)集

(d)freiburg3_walking_xyz數(shù)據(jù) wys圖6 本文算法的APE誤差

表3 算法誤差對比

由表3可知,本文算法比ORB-SLAM3算法的精度更高,平均平均誤差、平均均方根誤差、平均殘差平方和平均標(biāo)準(zhǔn)差誤差分別降低了4.776%,59.633%,78.788%、70.423%。由于ORB-SLAM2算法容易初始化和跟蹤失敗導(dǎo)致生成軌跡個(gè)數(shù)較少,甚至沒有軌跡數(shù)據(jù)生成,因此誤差數(shù)據(jù)較小。綜上所述,本文算法在初始化和運(yùn)行時(shí)間方面比ORB-SLAM2算法的效果更好,整體效果更優(yōu)。

3 結(jié)束語

傳統(tǒng)的單目視覺SLAM算法中沒有涉及處理動(dòng)態(tài)物體的干擾問題,為了排除動(dòng)態(tài)特征點(diǎn)的干擾,提高單目視覺SLAM系統(tǒng)的運(yùn)行效率,本文提出了一種基于YOLOv5和ORB-SLAM3相結(jié)合的動(dòng)態(tài)場景單目視覺SLAM算法,采用多線程并行的結(jié)構(gòu)保證了系統(tǒng)的實(shí)時(shí)性。在TUM動(dòng)態(tài)場景單目數(shù)據(jù)集上的實(shí)驗(yàn)結(jié)果表明:所提算法提高了動(dòng)態(tài)環(huán)境下系統(tǒng)的精度與魯棒性,其運(yùn)行效率比ORB-SLAM3提高了20.65%,比ORB-SLAM2提高了19.14%。本文僅對TUM數(shù)據(jù)集的室內(nèi)動(dòng)態(tài)場景進(jìn)行了實(shí)驗(yàn),未來可以針對室外動(dòng)態(tài)場景進(jìn)行實(shí)驗(yàn)驗(yàn)證。另外,為了提高SLAM系統(tǒng)整體的運(yùn)行效率,后續(xù)實(shí)驗(yàn)可以將ORB-SLAM3算法中的DBoW詞袋模型替換為創(chuàng)建速度更快的FBoW詞袋模型。