華春生，郭偉豪

(遼寧大學(xué) 信息學(xué)院，遼寧 沈陽 110036)

0 引言

同步定位與地圖構(gòu)建(Simultaneous localization and mapping，SLAM)是指載體利用傳感器在未知環(huán)境移動(dòng)的過程中估計(jì)自身位姿的同時(shí)，進(jìn)行地圖的構(gòu)建.由于機(jī)器人的主要傳感器為視覺SLAM，它具有性價(jià)比高、體積小、可獲得豐富的環(huán)境信息等特點(diǎn)，所以近年來SLAM在機(jī)器人領(lǐng)域的應(yīng)用得到廣泛關(guān)注.研究者提出了很多優(yōu)秀的視覺SLAM算法，例如MonoSLAM[1]、PTAM[2](Parallel tracking and mapping)、LSD-SLAM[3]、DSO[4]、ORB-SLAM2[5]等.但是這些算法都是建立在所遇到的場(chǎng)景是靜態(tài)的假設(shè)基礎(chǔ)上，即圖像的信息變化只取決于相機(jī)本身的運(yùn)動(dòng).然而在實(shí)際的應(yīng)用場(chǎng)景中很難滿足這種假設(shè)，會(huì)不可避免地出現(xiàn)動(dòng)態(tài)物體，例如行走的人、移動(dòng)的物體等.雖然可以采用隨機(jī)抽樣一致性檢測(cè)(RANSAC)[6]來減少一些小物體的異常值，但是較大物體和移動(dòng)較多的情況卻很難處理.這不但會(huì)給系統(tǒng)帶來誤差還會(huì)影響到位姿估計(jì)和地圖的構(gòu)建.深度學(xué)習(xí)近年來發(fā)展迅速，將視覺SLAM和深度學(xué)習(xí)相結(jié)合的解決方案應(yīng)運(yùn)而生.利用深度學(xué)習(xí)技術(shù)來提取語義信息并融合到視覺SLAM中，不僅可以有效去除動(dòng)態(tài)點(diǎn)，而且可以在幾何和語義兩個(gè)層面上對(duì)周圍環(huán)境進(jìn)行一些高層次的理解，建立具有語義信息的三維地圖，為機(jī)器人執(zhí)行高級(jí)任務(wù)做準(zhǔn)備.Mclormac等[7]利用卷積神經(jīng)網(wǎng)絡(luò)和Whelan等[8]結(jié)合構(gòu)建語義地圖，都沒有考慮動(dòng)態(tài)物體移動(dòng)較多的問題.Runz等[9]和He等[10]將MASK-RCNN和基于幾何分割的算法相結(jié)合，實(shí)現(xiàn)了多物體跟蹤識(shí)別和重建，但是對(duì)非剛性物體無法重建.Bescos等[11]利用MASK-RCNN和多視角幾何相融合，能夠剔除動(dòng)態(tài)物體并進(jìn)行背景修復(fù)，但是，該算法會(huì)把有一定概率移動(dòng)的對(duì)象移除掉，例如靜止的車.這會(huì)導(dǎo)致留下的特征點(diǎn)太少，進(jìn)而影響到系統(tǒng)的位姿估計(jì)等.Lu等[12]提出了一種基于分布和局部的算法(DM-SLAM)，并設(shè)計(jì)了一種基于鄰域互斥的候選地圖選擇機(jī)制，減少了動(dòng)態(tài)物體的影響.這種算法局限性在于大量靜止物體出現(xiàn)在場(chǎng)景中，此后在跟蹤移動(dòng)物體時(shí)，算法就會(huì)失效.Yang等[13]提出語義和幾何約束算法提高了位姿估計(jì)精度，但是當(dāng)動(dòng)態(tài)對(duì)象占據(jù)當(dāng)前幀的一半時(shí)，基礎(chǔ)矩陣不能準(zhǔn)確反映整個(gè)幀，會(huì)導(dǎo)致對(duì)極約束的錯(cuò)誤.

由于在動(dòng)態(tài)環(huán)境下收到動(dòng)態(tài)對(duì)象的干擾(類似于行人等移動(dòng)物體的出現(xiàn))，一方面會(huì)導(dǎo)致定位精度的降低，累計(jì)誤差不斷增大，嚴(yán)重時(shí)會(huì)導(dǎo)致系統(tǒng)崩潰；另一方面保存在地圖中的動(dòng)態(tài)對(duì)象會(huì)嚴(yán)重影響地圖的使用價(jià)值.針對(duì)現(xiàn)有的視覺SLAM在動(dòng)態(tài)環(huán)境中存在的魯棒性與穩(wěn)定性的問題，本文在ORB-SLAM2系統(tǒng)的基礎(chǔ)上進(jìn)行改進(jìn)，提出動(dòng)態(tài)點(diǎn)檢測(cè)和語義分割相結(jié)合的算法，剔除動(dòng)態(tài)點(diǎn)，減少動(dòng)態(tài)目標(biāo)的影響，并構(gòu)建出三維語義地圖.

1 系統(tǒng)介紹

ORB-SLAM2算法是目前應(yīng)用最廣泛、系統(tǒng)最完整的視覺SLAM系統(tǒng)之一，同時(shí)支持單目、雙目和RGB-G相機(jī).因此本文將基于ORB-SLAM2算法進(jìn)行改進(jìn)，在提高精度的同時(shí)增加系統(tǒng)的穩(wěn)定性.改進(jìn)后的系統(tǒng)框架如圖1所示，在ORB-SLAM2算法的跟蹤線程里嵌入動(dòng)態(tài)點(diǎn)檢測(cè)，另外增加了語義分割線程和地圖構(gòu)建線程.系統(tǒng)的流程是先將RGB圖像序列同時(shí)輸入到跟蹤線程和分割線程，通過動(dòng)態(tài)點(diǎn)檢測(cè)算法和語義分割獲得的結(jié)果來檢測(cè)動(dòng)態(tài)物體，然后刪除動(dòng)態(tài)對(duì)象的特征點(diǎn).最后通過穩(wěn)定的靜態(tài)特征點(diǎn)來估計(jì)姿態(tài)，并構(gòu)建語義地圖.

圖1 系統(tǒng)框架

2 動(dòng)態(tài)特征點(diǎn)檢測(cè)與剔除

2.1 光流法

光流法是一種利用圖像序列中的像素在時(shí)間域上的變化和相鄰的圖像幀之間的相關(guān)性來求解前一幀和當(dāng)前幀之間的對(duì)應(yīng)關(guān)系，進(jìn)而求解對(duì)象運(yùn)動(dòng)信息的方法.光流法可以分為稀疏光流和稠密光流，稀疏光流只是針對(duì)圖像中的部分像素進(jìn)行計(jì)算，并且計(jì)算開銷要比稠密光流小.其中較為典型的是LK光流[14].本文利用LK光流提取特征點(diǎn)和求解基礎(chǔ)矩陣.

LK光流的使用需要遵循以下3個(gè)假設(shè)條件.

1)灰度不變假設(shè)：圖像中相同空間點(diǎn)的像素灰度值在每個(gè)圖像中是不變的.

2)小運(yùn)動(dòng)假設(shè)：相鄰幀間的運(yùn)動(dòng)位移要小.

3)空間一致假設(shè)：相鄰的像素要具有相似的運(yùn)動(dòng).

空間中某個(gè)點(diǎn)，在t時(shí)刻，假設(shè)其像素坐標(biāo)為(x，y)，在t+dt時(shí)刻設(shè)它運(yùn)動(dòng)到了(x+dx，y+dy)處.根據(jù)灰度不變假設(shè)，可以得到

I(x+dx，y+dy，t+dt)=I(x，y，t)

(1)

左邊進(jìn)行泰勒級(jí)數(shù)展開并保留一階項(xiàng)得到

(2)

由于灰度不變假設(shè)，故下一時(shí)刻的灰度等于之前的灰度，從而有

(3)

兩邊除以dt，得

(4)

其中令Vx=dx/dt，表示像素在x軸上的速度.令Vy=dy/dt，表示像素在y軸上的速度.令I(lǐng)x=?I/?x，Iy=?I/?y，分別表示像素在該點(diǎn)處x方向的梯度，y方向上的梯度.圖像灰度對(duì)連續(xù)時(shí)間的變化量可以記為It，上式可以寫成如下形式：

IxVx+IyVy=-It

(5)

考慮一個(gè)窗口的大小為ω×ω，即含有ω2個(gè)數(shù)量的像素，因此可建立如下方程：

AV=-b

(6)

V=-(ATA)-1ATb

(7)

這樣就可以求得像素在圖像間的運(yùn)動(dòng)速度，進(jìn)而求得像素在若干塊圖像中出現(xiàn)的位置.但以上3種假設(shè)在實(shí)際的應(yīng)用中不能完全滿足.當(dāng)物體移動(dòng)較快時(shí)往往會(huì)破壞這種假設(shè)，進(jìn)而會(huì)導(dǎo)致計(jì)算的光流值出現(xiàn)較大的誤差，為了改善此問題可以采用圖像金字塔LK光流法.圖像金字塔通過縮放等操作，獲得不同分辨率的圖像.計(jì)算光流時(shí)，可以首先從頂部的圖像計(jì)算，得到結(jié)果后，將其作為下一層要計(jì)算的初始值.這種方法的好處在于，當(dāng)原始圖像的像素運(yùn)動(dòng)較大時(shí)，對(duì)于金字塔的頂層來說，運(yùn)動(dòng)仍然是在一個(gè)很小的范圍內(nèi)，進(jìn)而滿足了基本假設(shè).

2.2 動(dòng)態(tài)點(diǎn)檢測(cè)

本文提出的動(dòng)態(tài)點(diǎn)檢測(cè)算法主要分為預(yù)檢測(cè)和極線約束兩個(gè)步驟.

圖2 對(duì)極幾何約束

第一步，預(yù)檢測(cè)：①使用稀疏LK光流得到上一幀和當(dāng)前幀圖像相匹配的特征點(diǎn)；②根據(jù)以下方法處理匹配的特征點(diǎn)，如果匹配的特征點(diǎn)特別靠近圖像的邊緣，則丟棄該對(duì)特征點(diǎn).

第二步，對(duì)極幾何約束：①對(duì)預(yù)檢測(cè)之后當(dāng)前幀和上一幀中剩余的特征點(diǎn)進(jìn)行特征匹配；②從匹配的特征點(diǎn)對(duì)中使用八點(diǎn)法來計(jì)算基礎(chǔ)矩陣F；③利用基礎(chǔ)矩陣和上一幀中的特征點(diǎn)計(jì)算出當(dāng)前幀對(duì)應(yīng)的極線；④計(jì)算當(dāng)前幀中的匹配特征點(diǎn)到極線的距離，和預(yù)設(shè)閾值比較，如果大于閾值則將其視為動(dòng)態(tài)點(diǎn).

(8)

式中A，B，C為線向量.對(duì)于當(dāng)前幀的每個(gè)匹配點(diǎn)，計(jì)算其到極線的距離，如果大于預(yù)設(shè)閾值dthr，則視為動(dòng)態(tài)點(diǎn).距離計(jì)算公式為

(9)

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

通過動(dòng)態(tài)點(diǎn)檢測(cè)算法檢測(cè)完動(dòng)態(tài)點(diǎn)后，無法獲得完整動(dòng)態(tài)區(qū)域邊界輪廓，而語義分割可以完整地分割出動(dòng)態(tài)區(qū)域.考慮到本系統(tǒng)是為實(shí)際場(chǎng)景的應(yīng)用而設(shè)計(jì)的，所以要在實(shí)時(shí)性和準(zhǔn)確性之間取得平衡.本文采用SegNet[15]語義分割網(wǎng)絡(luò)模型，并使用Pascal VOC數(shù)據(jù)集訓(xùn)練該模型.SegNet網(wǎng)絡(luò)是一種適合實(shí)時(shí)應(yīng)用的快速、小存儲(chǔ)空間的網(wǎng)絡(luò)模型，它可以將圖像中的對(duì)象進(jìn)行分割，能夠達(dá)到像素的級(jí)別.本文將動(dòng)態(tài)點(diǎn)檢測(cè)結(jié)果和語義分割結(jié)果相結(jié)合，去除動(dòng)態(tài)點(diǎn)的示意圖如圖3所示，圈出來的點(diǎn)為檢測(cè)出的動(dòng)態(tài)點(diǎn)，語義分割可以分割出精確的輪廓.如果檢測(cè)出一定數(shù)量的動(dòng)態(tài)點(diǎn)是在語義分割的動(dòng)態(tài)對(duì)象區(qū)域內(nèi)，則將該對(duì)象視為動(dòng)態(tài)物體，然后將該對(duì)象內(nèi)的特征點(diǎn)去除，而剩下的特征點(diǎn)可以用于后續(xù)的跟蹤和建圖.通過這種方式可以極大地提高魯棒性和精度.

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

3 地圖構(gòu)建

3.1 單幀點(diǎn)云生成

單幀點(diǎn)云是構(gòu)建全局地圖的基礎(chǔ)，每一個(gè)點(diǎn)都包含位置信息和深度信息，根據(jù)相機(jī)模型將二維圖像上的點(diǎn)映射到三維空間，從而得到對(duì)應(yīng)的點(diǎn)云.根據(jù)相機(jī)模型，空間點(diǎn)與對(duì)應(yīng)的像素坐標(biāo)之間變換關(guān)系如下：

(10)

其中f表示焦距，c表示相對(duì)于原點(diǎn)的偏移量，K表示相機(jī)內(nèi)參.設(shè)圖像中某一點(diǎn)的深度為d，則對(duì)應(yīng)的空間點(diǎn)為

(11)

進(jìn)而得到

(12)

將所有的像素按照式(12)進(jìn)行轉(zhuǎn)換即可得到相應(yīng)的點(diǎn)云.

3.2 點(diǎn)云的拼接與濾波

圖4 點(diǎn)云拼接示意圖

將關(guān)鍵幀對(duì)應(yīng)的點(diǎn)云生成后進(jìn)行拼接即可形成局部點(diǎn)云地圖.由于局部點(diǎn)云在拼接的過程中會(huì)出現(xiàn)關(guān)鍵幀重疊的情況，導(dǎo)致視野重疊區(qū)域存在很多位置相近的點(diǎn)，極大地浪費(fèi)了存儲(chǔ)空間，因此需要進(jìn)行體素濾波.通過體素濾波后可以使相應(yīng)的空間內(nèi)只有一個(gè)點(diǎn)，從而節(jié)約了存儲(chǔ)空間.點(diǎn)云拼接的示意圖如圖4所示.數(shù)學(xué)表達(dá)式為

(13)

其中：Ck表示單幀點(diǎn)云，Tk表示相機(jī)位姿，m表示局部點(diǎn)云.

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

4.1 實(shí)驗(yàn)環(huán)境

ORB-SLAM2算法作為視覺SLAM領(lǐng)域最為經(jīng)典的算法之一，因其優(yōu)秀的穩(wěn)定性和實(shí)用性被廣泛應(yīng)用到各個(gè)領(lǐng)域，而本文的算法也是在其基礎(chǔ)上進(jìn)行改進(jìn).因此本文算法實(shí)驗(yàn)的各種數(shù)據(jù)將與ORB-SLAM2相比較，以此來驗(yàn)證在動(dòng)態(tài)環(huán)境下本文算法的魯棒性.本文使用TUM RGB-D公開數(shù)據(jù)集中的walking序列進(jìn)行測(cè)試，分別是freiburg3_walking_static，freiburg3_walking_xyz，freiburg3_walking_halfsphere.walking序列中兩名測(cè)試人員會(huì)圍繞桌子來回走動(dòng)，屬于高動(dòng)態(tài)場(chǎng)景.

實(shí)驗(yàn)環(huán)境為Ubuntu 18.04，實(shí)驗(yàn)平臺(tái)為一臺(tái)配備Intel Xeon CPU E5-2678 v3，12GB內(nèi)存和一個(gè)NVIDIA Tesla K80 GPU的計(jì)算機(jī).絕對(duì)軌跡誤差(Absolute trajectory error，ATE)和相對(duì)位姿誤差(Relative pose error，RPE)[16]是用來評(píng)價(jià)算法優(yōu)劣的最常用的指標(biāo).本文將用以上兩項(xiàng)指標(biāo)作為算法精度評(píng)估的指標(biāo).絕對(duì)軌跡誤差用于衡量估計(jì)軌跡與真實(shí)軌跡的一致程度，即軌跡準(zhǔn)確度.相對(duì)位姿誤差用于評(píng)估在一定時(shí)間間隔內(nèi)的局部軌跡精度，并可用于評(píng)價(jià)位姿估計(jì)時(shí)產(chǎn)生的漂移量.

4.2 誤差分析

如圖5(a)所示，在以活動(dòng)的人為對(duì)象的動(dòng)態(tài)場(chǎng)景中，從表1和圖6、圖7可以觀察出原始的ORB-SLAM2算法估計(jì)軌跡存在很大的誤差.而經(jīng)過本文的改進(jìn)，在加入了語義分割之后可以精確地分割出動(dòng)態(tài)目標(biāo)，把動(dòng)態(tài)對(duì)象(即人身上)的特征點(diǎn)去除.不僅消除了動(dòng)態(tài)對(duì)象對(duì)位姿估計(jì)的影響，而且定位精度也取得了很大的提升，圖6最能直觀地看出增加了語義分割后定位精度的提升效果.

圖5 動(dòng)態(tài)環(huán)境測(cè)試圖

實(shí)驗(yàn)的結(jié)果如表1所示，其中第一列有3種相機(jī)運(yùn)動(dòng)類型：①static表示相機(jī)處于靜止?fàn)顟B(tài)；②xyz表示相機(jī)會(huì)沿著xyz軸運(yùn)動(dòng)；③halfsphere表示的是相機(jī)會(huì)按照半球面來運(yùn)動(dòng).其中提升率計(jì)算公式為

(10)

式中α表示ORB-SLAM2算法，β表示本文算法.在實(shí)驗(yàn)結(jié)果中計(jì)算了均方根誤差(Root mean squared error，RMSE)，平均值(Mean)和標(biāo)準(zhǔn)差(Standard deviation，S.D.)這3個(gè)參數(shù)，平均值反映估計(jì)位姿的精度，均方根誤差和標(biāo)準(zhǔn)差更能體現(xiàn)系統(tǒng)的穩(wěn)定性和魯棒性.

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

由表1可知，在高動(dòng)態(tài)場(chǎng)景下，本文算法的RMSE數(shù)值相比較于ORB-SLAM2算法平均下降了95.32%，反映系統(tǒng)魯棒性的標(biāo)準(zhǔn)差S.D.則下降了94.00%.實(shí)驗(yàn)結(jié)果表明了在高動(dòng)態(tài)環(huán)境中，本文算法比ORB-SLAM2算法具有更好的魯棒性和穩(wěn)定性.

圖6(a)是ORB-SLAM2算法在數(shù)據(jù)集walking_xyz下的相機(jī)軌跡圖，圖6(b)是本文算法所估計(jì)的相機(jī)軌跡圖.圖中虛線表示的是數(shù)據(jù)集的真值，實(shí)線表示的是各個(gè)算法的相機(jī)估計(jì)軌跡值.通過觀察圖中兩種線條的重合度，明顯可以看出本文算法估計(jì)的相機(jī)軌跡和真值基本重合，相比較于ORB-SLAM2算法，準(zhǔn)確度有很大的提升.圖7(a)和(b)展示了ORB-SLAM2算法和本文算法的絕對(duì)軌跡誤差圖.通過圖7(a)可以觀察出ORB-SLAM2算法的絕對(duì)軌跡誤差主要分布在0.3 m到0.5 m之間，還有一些時(shí)間的誤差快達(dá)到了1.0 m，誤差在0.1 m內(nèi)的時(shí)間則極短.而從圖7(b)中可以看出本文算法估計(jì)的軌跡誤差基本都是在0.015 m到0.028 m的區(qū)間內(nèi)，最小的誤差能達(dá)到0.005 m，最大的誤差都不超過0.08 m.從圖6和圖7中都能非常直觀地看出本文算法相較ORB-SLAM2算法，在動(dòng)態(tài)環(huán)境中的軌跡誤差有很大幅度的降低，大大提高了位姿估計(jì)的準(zhǔn)確度.

圖6 軌跡真實(shí)值與估計(jì)軌跡

圖7 絕對(duì)軌跡誤差圖

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

在語義建圖實(shí)驗(yàn)中，本文選擇了walking_static數(shù)據(jù)集.在此序列中會(huì)有兩個(gè)人坐在椅子上，或者圍繞著桌子進(jìn)行走動(dòng)，屬于高動(dòng)態(tài)場(chǎng)景.圖8(a)展示了ORB-SLAM2算法構(gòu)建的地圖，從圖中可以看出ORB-SLAM2沒有去除動(dòng)態(tài)目標(biāo)，會(huì)影響機(jī)器人執(zhí)行路徑規(guī)劃等任務(wù).圖8(b)展示了經(jīng)過語義分割和動(dòng)態(tài)點(diǎn)檢測(cè)算法結(jié)合去除動(dòng)態(tài)特征點(diǎn)之后的語義地圖效果，從圖中可以看出本文算法去除了場(chǎng)景中大部分動(dòng)態(tài)特征點(diǎn)，建立了點(diǎn)云地圖，可以用機(jī)器人執(zhí)行高級(jí)任務(wù).

圖8 ORB-SLAM2算法與本文算法構(gòu)建地圖效果

5 結(jié)束語

本文為了解決環(huán)境中出現(xiàn)動(dòng)態(tài)對(duì)象而影響視覺SLAM算法的精度和缺乏語義信息的問題，基于ORB-SLAM2算法提出了一個(gè)動(dòng)態(tài)點(diǎn)檢測(cè)算法，嵌入到ORB-SLAM2算法的追蹤線程中，并增加一個(gè)語義分割線程和稠密建圖線程.通過動(dòng)態(tài)點(diǎn)檢測(cè)算法與語義分割相結(jié)合去除動(dòng)態(tài)對(duì)象上的特征點(diǎn)，以減少動(dòng)態(tài)對(duì)象對(duì)系統(tǒng)位姿估計(jì)的影響，并建立了語義地圖.在公開數(shù)據(jù)集上的實(shí)驗(yàn)結(jié)果表明，在動(dòng)態(tài)場(chǎng)景下本文算法比ORB-SLAM2算法大幅度提升了位姿估計(jì)的準(zhǔn)確度，提高了系統(tǒng)的魯棒性和穩(wěn)定性.在未來的工作中將改進(jìn)稠密語義建圖的功能，使之用于機(jī)器人執(zhí)行高級(jí)任務(wù).