視覺SLAM在室內(nèi)動(dòng)態(tài)場(chǎng)景中的應(yīng)用研究

徐少杰，曹雛清，王永娟

1.南京理工大學(xué) 機(jī)械工程學(xué)院，南京210094

2.安徽工程大學(xué) 計(jì)算機(jī)與信息學(xué)院，安徽 蕪湖241000

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

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

為了解決上述問題，本文提出一種用目標(biāo)檢測(cè)網(wǎng)絡(luò)檢測(cè)潛在動(dòng)態(tài)物體所在的圖像區(qū)域，在特征提取后剔除分布于這一區(qū)域中的特征點(diǎn)，以此來降低動(dòng)態(tài)物體對(duì)位姿估計(jì)帶來的影響，并對(duì)關(guān)鍵幀進(jìn)行語(yǔ)義分割，在構(gòu)建點(diǎn)云地圖時(shí)剔除動(dòng)態(tài)地圖點(diǎn)，構(gòu)建出無動(dòng)態(tài)物體影響的三維稠密語(yǔ)義地圖。

1 動(dòng)態(tài)環(huán)境下的位姿估計(jì)

本文選擇基于特征點(diǎn)法的ORB-SLAM2 來提供視覺SLAM 方案，框架如圖1 所示，分為5 個(gè)線程：目標(biāo)檢測(cè)線程、跟蹤線程、局部地圖線程、回環(huán)檢測(cè)線程和語(yǔ)義建圖線程。數(shù)據(jù)處理流程為：RGB-D 相機(jī)采集的RGB圖和深度圖輸入跟蹤線程進(jìn)行ORB 特征提取，同時(shí)將RGB 圖輸入目標(biāo)檢測(cè)線程檢測(cè)出動(dòng)態(tài)物體所在區(qū)域，跟蹤線程根據(jù)目標(biāo)檢測(cè)結(jié)果剔除動(dòng)態(tài)特征點(diǎn)，用剩余的穩(wěn)定的靜態(tài)特征點(diǎn)進(jìn)行相機(jī)位姿估計(jì)。關(guān)鍵幀傳入局部建圖線程進(jìn)行局部BA，剔除冗余關(guān)鍵幀，然后將關(guān)鍵幀傳入回環(huán)檢測(cè)線程和語(yǔ)義建圖線程，回環(huán)檢測(cè)線程中進(jìn)行回環(huán)檢測(cè)和全局BA，語(yǔ)義建圖線程中用語(yǔ)義分割網(wǎng)絡(luò)對(duì)關(guān)鍵幀進(jìn)行分割，構(gòu)建點(diǎn)云地圖時(shí)結(jié)合先驗(yàn)知識(shí)剔除動(dòng)態(tài)物體地圖點(diǎn)，最后結(jié)合位姿信息進(jìn)行點(diǎn)云拼接構(gòu)建出全局一致的不含動(dòng)態(tài)物體的語(yǔ)義地圖。

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

1.1 動(dòng)態(tài)目標(biāo)檢測(cè)

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

圖2 動(dòng)態(tài)目標(biāo)檢測(cè)結(jié)果

1.2 動(dòng)態(tài)特征點(diǎn)剔除

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

從圖3（a）中可以看出在剔除之前有一些特征點(diǎn)分布在兩個(gè)人身上，這部分特征點(diǎn)將會(huì)影響位姿估計(jì)，不利于機(jī)器人準(zhǔn)確定位，從圖3（b）中可以看出，經(jīng)過剔除處理后的特征點(diǎn)已經(jīng)全部都分布于靜態(tài)的物體之上，可以有效降低動(dòng)態(tài)物體對(duì)機(jī)器人位姿估計(jì)的影響。

圖3 動(dòng)態(tài)特征點(diǎn)剔除

1.3 位姿計(jì)算

通過對(duì)動(dòng)態(tài)特征點(diǎn)的剔除，在計(jì)算機(jī)器人位姿時(shí)便可使用集合B中的靜態(tài)特征點(diǎn)來計(jì)算。設(shè)圖像中滿足條件的特征點(diǎn)pi=(ui,vi)的空間點(diǎn)為Pi=(Xi,Yi,Zi)，則特征點(diǎn)pi與空間點(diǎn)Pi的對(duì)應(yīng)關(guān)系如下：

其中，s為尺度因子，K為相機(jī)內(nèi)參矩陣，ξ為位姿的李代數(shù)，∧為李代數(shù)轉(zhuǎn)換為矩陣的運(yùn)算符，e為誤差。

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

將所有誤差累加求和，構(gòu)建最小二乘問題：

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

2 動(dòng)態(tài)環(huán)境下的語(yǔ)義建圖

2.1 語(yǔ)義分割

上述工作雖然可以獲得動(dòng)態(tài)環(huán)境下的相機(jī)位姿，但是構(gòu)建語(yǔ)義地圖還需要按像素分割的語(yǔ)義分割圖像?？紤]到SLAM系統(tǒng)的實(shí)時(shí)性，所用語(yǔ)義分割網(wǎng)絡(luò)需要在準(zhǔn)確率和分割速度達(dá)到一個(gè)平衡，因此本文采用基于Caffe框架的實(shí)時(shí)語(yǔ)義分割網(wǎng)絡(luò)SegNet[11]來對(duì)關(guān)鍵幀進(jìn)行分割。訓(xùn)練神經(jīng)網(wǎng)絡(luò)的數(shù)據(jù)集是NYU-Depth V2，該數(shù)據(jù)集是一個(gè)室內(nèi)靜態(tài)數(shù)據(jù)集，共包含室內(nèi)場(chǎng)景中常見的13個(gè)類別，但是不包含動(dòng)態(tài)的行人類別，因此添加上PASCAL VOC中的行人類別構(gòu)成新的數(shù)據(jù)集用來訓(xùn)練SegNet，訓(xùn)練后的網(wǎng)絡(luò)分割結(jié)果如圖4所示。圖中不同種類用不同的顏色表示，其中行人用白色來表示。

2.2 三維語(yǔ)義點(diǎn)云地圖構(gòu)建

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

圖4 語(yǔ)義分割結(jié)果

設(shè)像素點(diǎn)p，對(duì)應(yīng)的相機(jī)坐標(biāo)系下的空間點(diǎn)P，則這兩點(diǎn)的對(duì)應(yīng)關(guān)系為：

由公式（4）像素點(diǎn)可以投影到相機(jī)坐標(biāo)系下的三維空間中，可將單幀圖像轉(zhuǎn)換成相機(jī)坐標(biāo)系下的單片點(diǎn)云，但相機(jī)在運(yùn)動(dòng)中相機(jī)坐標(biāo)系也在不斷變化，因此還需要將相機(jī)坐標(biāo)系下的單片點(diǎn)云拼接到統(tǒng)一的世界坐標(biāo)系中來構(gòu)建全局一致的點(diǎn)云地圖。

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

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

為驗(yàn)證本文方法的有效性，采用TUM 數(shù)據(jù)集對(duì)本文方法進(jìn)行實(shí)驗(yàn)。TUM數(shù)據(jù)集是通過Kinect深度相機(jī)采集的不同室內(nèi)場(chǎng)景的圖像序列，提供了采集圖像時(shí)的相機(jī)位姿真值，可供研究者們?cè)u(píng)估自己的SLAM系統(tǒng)性能。本文采用該數(shù)據(jù)集中的三個(gè)高度動(dòng)態(tài)的場(chǎng)景數(shù)據(jù)walking_halfsphere、walking_static、walking_xyz 來進(jìn)行測(cè)試。

3.1 位姿估計(jì)實(shí)驗(yàn)

實(shí)驗(yàn)時(shí)采用ATE[12]（Absolute Trajectory Error，絕對(duì)軌跡誤差）作為評(píng)價(jià)指標(biāo)，絕對(duì)軌跡誤差指的是相機(jī)的位姿真值與SLAM 系統(tǒng)位姿估計(jì)值之間的差值。表1為本文算法和ORB_SLAM2 在三種數(shù)據(jù)集中的測(cè)試結(jié)果對(duì)比，其中的提升率計(jì)算方式為：

其中rmes、mean 和std 分別指的是均方根誤差、平均值和標(biāo)準(zhǔn)差。表2 為本文算法與其他同類方法的絕對(duì)軌跡誤差結(jié)果對(duì)比。

表1 絕對(duì)軌跡誤差對(duì)比

表2 同類算法結(jié)果對(duì)比

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

圖5 軌跡真實(shí)值與計(jì)算值曲線

圖6 絕對(duì)軌跡誤差分布對(duì)比

3.2 語(yǔ)義建圖實(shí)驗(yàn)

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

圖7 處理前后地圖對(duì)比

4 結(jié)束語(yǔ)

為了解決傳統(tǒng)視覺SLAM 無法適用于高動(dòng)態(tài)場(chǎng)景以及地圖缺少語(yǔ)義信息問題，提出了一種融合目標(biāo)檢測(cè)網(wǎng)絡(luò)、語(yǔ)義分割網(wǎng)絡(luò)的語(yǔ)義SLAM方法。在位姿跟蹤階段利用YOLOv4 目標(biāo)檢測(cè)網(wǎng)絡(luò)檢測(cè)出環(huán)境中動(dòng)態(tài)物體所在區(qū)域，在特征提取時(shí)剔除分布在動(dòng)態(tài)物體區(qū)域的特征點(diǎn)，保留靜態(tài)特征點(diǎn)，然后利用靜態(tài)特征點(diǎn)進(jìn)行位姿跟蹤，減少動(dòng)態(tài)物體帶來的負(fù)面影響，在地圖構(gòu)建階段利用SegNet 語(yǔ)義分割網(wǎng)絡(luò)對(duì)關(guān)鍵幀進(jìn)行語(yǔ)義分割，在構(gòu)建點(diǎn)云時(shí)根據(jù)RGB 信息濾除動(dòng)態(tài)物體，從而構(gòu)建出不含動(dòng)態(tài)物體的語(yǔ)義地圖。在TUM數(shù)據(jù)集上的實(shí)驗(yàn)結(jié)果表明，本文算法相對(duì)于傳統(tǒng)視覺SLAM算法在動(dòng)態(tài)環(huán)境中明顯擁有更好的效果。