孫 平， 閆 冬

(沈陽(yáng)工業(yè)大學(xué) 人工智能學(xué)院， 沈陽(yáng) 110870)

即時(shí)定位與地圖構(gòu)建(simultaneous localization and mapping，SLAM)在無(wú)人駕駛、增強(qiáng)現(xiàn)實(shí)技術(shù)等領(lǐng)域都發(fā)揮著重要作用.相對(duì)于傳統(tǒng)傳感器，視覺(jué)傳感器采集的圖像可以提供更多的環(huán)境信息，得到了研究者的廣泛關(guān)注，提出了許多基于視覺(jué)傳感器的SLAM算法，如ORB-SLAM2[1]，LSD-SLAM[2]、DSO[3]等，然而上述SLAM算法均假設(shè)相機(jī)所處環(huán)境是靜態(tài)的，當(dāng)場(chǎng)景中存在動(dòng)態(tài)物體時(shí)其定位性能將明顯下降.

實(shí)際上，在視覺(jué)SLAM應(yīng)用中總會(huì)遇到運(yùn)動(dòng)的物體，如路上行駛的汽車、走路的行人等.這些動(dòng)態(tài)物體會(huì)給視覺(jué)SLAM帶來(lái)誤差，并得到錯(cuò)誤的位姿估計(jì)，最終會(huì)導(dǎo)致定位和建圖精度的嚴(yán)重下降.盡管ORB-SLAM2采用RANSAC算法能去除小的運(yùn)動(dòng)物體引入的外點(diǎn)[4]，從而提高動(dòng)態(tài)環(huán)境下的魯棒性，然而當(dāng)動(dòng)態(tài)物體在圖像中占比較大時(shí)，該算法就無(wú)法解決此場(chǎng)景下的視覺(jué)SLAM準(zhǔn)確定位問(wèn)題.

目前，關(guān)于動(dòng)態(tài)環(huán)境下的視覺(jué)SLAM準(zhǔn)確定位問(wèn)題已經(jīng)得到了一些研究成果，主要分為直接法[5-7]和多傳感器融合方法[8-9].在直接法中，主要通過(guò)稠密的場(chǎng)景流、光流檢測(cè)稀疏特征和計(jì)算圖像的強(qiáng)度差來(lái)檢測(cè)動(dòng)態(tài)物體；在多傳感器融合方法中，主要通過(guò)慣性數(shù)據(jù)補(bǔ)償特征點(diǎn)運(yùn)動(dòng)，并進(jìn)行濾波和最小化能量函數(shù)來(lái)消除動(dòng)態(tài)物體的影響.然而在上述方法中，直接法需要額外的深度信息，多傳感器融合法需要額外的傳感器，在僅使用RGB圖像作為唯一輸入的情況下，上述兩種方法均不能解決有效點(diǎn)較少的動(dòng)態(tài)復(fù)雜場(chǎng)景下視覺(jué)SLAM準(zhǔn)確定位問(wèn)題.

鑒于以上分析，本文考慮了僅使用單目視覺(jué)傳感器，在有效點(diǎn)較少的動(dòng)態(tài)復(fù)雜場(chǎng)景下，提出了一種基于動(dòng)態(tài)特征點(diǎn)剔除的單目視覺(jué)SLAM算法，主要工作有：1)針對(duì)有效點(diǎn)較少的情形，提出了ARANSAC算法，從而準(zhǔn)確地估計(jì)圖像間的透視變換矩陣，并用其補(bǔ)償相機(jī)的運(yùn)動(dòng)；2)提出了一種基于光流法識(shí)別動(dòng)態(tài)特征點(diǎn)的算法，在有效點(diǎn)較少的動(dòng)態(tài)場(chǎng)景中，該算法可以準(zhǔn)確地檢測(cè)出動(dòng)態(tài)特征點(diǎn)；3)在TUM數(shù)據(jù)集上進(jìn)行了仿真，并與ORB-SLAM2算法進(jìn)行對(duì)比，結(jié)果證明了基于動(dòng)態(tài)特征點(diǎn)剔除的單目視覺(jué)SLAM算法的有效性.

1 算法流程

本文提出的算法利用單目視覺(jué)傳感器捕獲原始RGB圖像后，在ORB-SLAM2算法中提取特征點(diǎn)并進(jìn)行動(dòng)態(tài)特征點(diǎn)的檢測(cè)與剔除，從而僅利用場(chǎng)景中的靜態(tài)特征點(diǎn)進(jìn)行SLAM，即場(chǎng)景中的動(dòng)態(tài)物體不再影響相機(jī)的定位，整體流程如圖1所示.

圖1 基于動(dòng)態(tài)特征點(diǎn)剔除的單目視覺(jué)SLAM算法框架Fig.1 Framework of monocular vision SLAM algorithm based on dynamic feature point elimination

2 基于動(dòng)態(tài)特征點(diǎn)剔除的單目視覺(jué)SLAM算法

2.1 動(dòng)態(tài)特征點(diǎn)檢測(cè)算法

本文提出的動(dòng)態(tài)特征點(diǎn)檢測(cè)算法由兩個(gè)模塊組成，可分為基于ARANSAC算法的自我運(yùn)動(dòng)估計(jì)和基于光流法的檢測(cè).

2.1.1 基于ARANSAC算法的自我運(yùn)動(dòng)估計(jì)

自我運(yùn)動(dòng)估計(jì)模塊是以上一幀和當(dāng)前幀的連續(xù)兩幀圖像作為輸入，通過(guò)提取上一幀ORB特征點(diǎn)并采用Lucas Kanade方法[10]尋找其在當(dāng)前幀的匹配點(diǎn).如果匹配對(duì)中心3×3圖像塊的像素差太大則會(huì)放棄該匹配，以此減少誤匹配的影響.考慮環(huán)境中有效點(diǎn)較少的情況(場(chǎng)景中靜態(tài)特征點(diǎn)的數(shù)量少于動(dòng)態(tài)特征點(diǎn)的數(shù)量)，提出ARANSAC算法估計(jì)前后兩幀圖像間的透視變換矩陣，并用透視變換矩陣扭曲上一幀獲得一個(gè)估計(jì)圖像，目的是使上一幀中的點(diǎn)轉(zhuǎn)換到當(dāng)前幀的坐標(biāo)系下.ARANSAC算法的具體步驟如下：

1) 生成最小樣本.將輸入圖像平均分成8×8的區(qū)域，然后隨機(jī)選取4個(gè)區(qū)域，分別在所選區(qū)域內(nèi)取一個(gè)特征點(diǎn)，構(gòu)建最小模型.

2) 構(gòu)建模型評(píng)價(jià).給定一個(gè)評(píng)價(jià)模型，計(jì)算該模型的透視變換矩陣，并對(duì)所有匹配特征點(diǎn)對(duì)進(jìn)行投影，從而判斷該特征點(diǎn)是有效點(diǎn)還是無(wú)效點(diǎn).評(píng)價(jià)模型為

(1)

(2)

(3)

式中：i為有效區(qū)域；εi為一個(gè)區(qū)域中有效點(diǎn)的比例；xi為有效區(qū)域的中心位置；A為輸入圖像的面積；C為有效點(diǎn)的協(xié)方差矩陣；s為最終的模型評(píng)價(jià)值.

3) 設(shè)定終止條件.當(dāng)最佳模型無(wú)效點(diǎn)的概率滿足式(4)條件時(shí)，立刻停止迭代，即

[6] 趙同彬,郭偉耀,韓 飛,等.工作面回采速度影響下煤層頂板能量積聚釋放分析[J].煤炭科學(xué)技術(shù),2018,46(10):37-44.

(4)

式中：σi為最佳模型對(duì)應(yīng)區(qū)域有效點(diǎn)占比；pi=1/N，N為有效區(qū)域的個(gè)數(shù)；m為常數(shù)4；KS為迭代次數(shù)；η為設(shè)定的常數(shù).

2.1.2 基于光流法的檢測(cè)

自我運(yùn)動(dòng)估計(jì)后，估計(jì)圖像中的點(diǎn)與當(dāng)前幀中的點(diǎn)會(huì)被統(tǒng)一在相同的坐標(biāo)系下，利用Lucas Kanade計(jì)算特征點(diǎn)在當(dāng)前幀和估計(jì)圖像之間的光流值，并設(shè)定閾值τ來(lái)判斷特征點(diǎn)是靜態(tài)的還是動(dòng)態(tài)的，其表達(dá)式為

(5)

2.2 剔除動(dòng)態(tài)特征點(diǎn)的單目視覺(jué)SLAM算法

文中單目視覺(jué)SLAM算法是在ORB-SLAM2基礎(chǔ)上，增加了動(dòng)態(tài)特征點(diǎn)檢測(cè)并剔除的功能，以此減少環(huán)境中的動(dòng)態(tài)物體對(duì)視覺(jué)SLAM系統(tǒng)定位精度的影響.每輸入一幀圖片后，先進(jìn)行自我運(yùn)動(dòng)估計(jì)和動(dòng)態(tài)特征點(diǎn)檢測(cè)，然后剔除ORB-SLAM2中的動(dòng)態(tài)特征點(diǎn)并計(jì)算剩余特征點(diǎn)的描述子，用靜態(tài)特征點(diǎn)進(jìn)行SLAM.

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

為驗(yàn)證本文算法的有效性，本文用TUM數(shù)據(jù)集中的freiburg1序列和freiburg3序列進(jìn)行算法驗(yàn)證，實(shí)驗(yàn)計(jì)算機(jī)硬件為聯(lián)想筆記本電腦，CPU為Intel i5，4GB內(nèi)存，軟件平臺(tái)為Ubuntu16.04系統(tǒng).freiburg3_walking序列屬于高動(dòng)態(tài)環(huán)境，一些動(dòng)態(tài)物體的占比會(huì)超過(guò)50%以上(即有效點(diǎn)較少的情形).本文利用以下四個(gè)高動(dòng)態(tài)圖像序列進(jìn)行實(shí)驗(yàn)分析，相機(jī)運(yùn)動(dòng)類型如下：

1) freiburg3_walking_static(fr3/w/static)：相機(jī)維持微小運(yùn)動(dòng).

3) freiburg3_walking_rpy(fr3/w/rpy)：相機(jī)沿著r-p-y三個(gè)方向旋轉(zhuǎn).

4) freiburg3_walking_halfsphere(fr3/w/half)：相機(jī)沿半球形軌跡移動(dòng).

3.1 動(dòng)態(tài)特征點(diǎn)檢測(cè)實(shí)驗(yàn)

本文提出的基于ARANSAC動(dòng)態(tài)特征點(diǎn)檢測(cè)算法的目標(biāo)是從所有提取的特征點(diǎn)中正確區(qū)分出場(chǎng)景中的動(dòng)態(tài)特征點(diǎn).為了驗(yàn)證該算法的有效性，分別從TUM數(shù)據(jù)集中的fr3/w/rpy、fr3/w/static、fr3/w/xyz和fr3/w/half序列選取連續(xù)兩幀圖像(圖像中的兩個(gè)人正在桌子前行走，位置不斷變化)進(jìn)行實(shí)驗(yàn)，并與基于RANSAC的檢測(cè)算法進(jìn)行對(duì)比，結(jié)果如圖2所示.

圖2中，對(duì)于每一列，最上面是原始的RGB圖像，中間為基于RANSAC算法的檢測(cè)結(jié)果，最下面是基于文中ARANSAC算法的檢測(cè)結(jié)果.彩色點(diǎn)表示從當(dāng)前圖像中提取的特征點(diǎn)，紅點(diǎn)是動(dòng)態(tài)的，綠點(diǎn)是靜態(tài)的.從圖2中可以看出，對(duì)于fr3/w/xyz序列，圖像中運(yùn)動(dòng)物體占比較少，此時(shí)基于RANSAC算法和基于ARANSAC算法的檢測(cè)結(jié)果基本一致，都能正確檢測(cè)出場(chǎng)景中的動(dòng)態(tài)物體.但是當(dāng)運(yùn)動(dòng)物體在圖像中占比較大時(shí)，提取的動(dòng)態(tài)特征點(diǎn)增多，如fr3/w/rpy序列動(dòng)態(tài)特征點(diǎn)占比在71.17%左右，fr3/w/static占比在68.04%左右，fr3/w/half占比在55.17%左右，此時(shí)利用RANSAC算法將會(huì)錯(cuò)誤地估計(jì)透視變換矩陣，從而不能準(zhǔn)確地補(bǔ)償相機(jī)運(yùn)動(dòng)，最終錯(cuò)誤地識(shí)別了正在運(yùn)動(dòng)的人身上提取的特征點(diǎn)為靜態(tài)點(diǎn)，而場(chǎng)景中靜態(tài)物體(如放置的椅子和電腦)反而被識(shí)別為動(dòng)態(tài)的.

圖2 動(dòng)態(tài)特征點(diǎn)檢測(cè)算法實(shí)驗(yàn)對(duì)比結(jié)果Fig.2 Experimental comparison results for dynamic feature point detection algorithms

利用本文提出的ARANSAC動(dòng)態(tài)特征點(diǎn)檢測(cè)算法，可以準(zhǔn)確地估計(jì)透視變換矩陣，進(jìn)而準(zhǔn)確地補(bǔ)償相機(jī)運(yùn)動(dòng)，因此能夠正確區(qū)分場(chǎng)景中的動(dòng)態(tài)特征點(diǎn)和靜態(tài)特征點(diǎn)，這對(duì)視覺(jué)SLAM后續(xù)穩(wěn)定工作起到至關(guān)重要的作用.實(shí)驗(yàn)表明，即使在有效點(diǎn)較少的情況下，本文提出的基于ARANSAC檢測(cè)算法依然能夠正確區(qū)分場(chǎng)景中的動(dòng)態(tài)特征點(diǎn).

3.2 與ORB-SLAM2算法對(duì)比

為了體現(xiàn)本文算法在動(dòng)態(tài)環(huán)境下的定位性能，實(shí)驗(yàn)中，采用絕對(duì)軌跡誤差(ATE)和相對(duì)位姿誤差(RPE)作為評(píng)價(jià)指標(biāo)，表1～3分別為本文SLAM算法和ORB-SLAM2算法的對(duì)比結(jié)果.

表1 絕對(duì)軌跡誤差結(jié)果Tab.1 Results of absolute trajectory error

表2 旋轉(zhuǎn)漂移結(jié)果Tab.2 Results of rotating drift

表3 平移漂移結(jié)果Tab.3 Results of translation drift

由表1～3結(jié)果可知，在六個(gè)運(yùn)動(dòng)序列實(shí)驗(yàn)中，fr1/desk序列屬于靜態(tài)序列，對(duì)比ATE和RPE指標(biāo)可知，本文所提算法的均方根誤差和標(biāo)準(zhǔn)偏差均變化不大，可以看出本文算法和ORB-SLAM2算法的估計(jì)結(jié)果非常相似，保持在同一個(gè)精度水平.fr3/s/xyz序列屬于小范圍動(dòng)態(tài)環(huán)境，對(duì)比ATE和RPE指標(biāo)可知，本文所提算法的均方根誤差和標(biāo)準(zhǔn)偏差均略有下降，可以看出本文算法的定位精度略優(yōu)于ORB-SLAM2算法.

實(shí)驗(yàn)中，fr3/w四個(gè)運(yùn)動(dòng)序列屬于高動(dòng)態(tài)環(huán)境，與ORB-SLAM2算法相比，本文算法在增加了動(dòng)態(tài)特征點(diǎn)檢測(cè)與剔除功能后，相機(jī)位姿估計(jì)值與真實(shí)值之間的誤差大大降低.對(duì)比ATE指標(biāo)可知，本文所提算法的均方根誤差和標(biāo)準(zhǔn)偏差均明顯下降，最佳效果分別降低97.99%和96.11%；對(duì)比RPE指標(biāo)可知，旋轉(zhuǎn)漂移的均方根誤差和標(biāo)準(zhǔn)偏差最佳效果分別降低92.89%和96.44%，平移漂移的均方根誤差和標(biāo)準(zhǔn)偏差最佳效果分別降低94.88%和94.00%.結(jié)果表明，在有效點(diǎn)較少的動(dòng)態(tài)環(huán)境下，本文算法對(duì)四個(gè)運(yùn)動(dòng)序列估計(jì)的軌跡與原始軌跡之間的誤差大大降低，提出的單目視覺(jué)算法能夠顯著提高視覺(jué)SLAM的定位精度.

為了進(jìn)一步說(shuō)明本文提出的單目視覺(jué)SLAM算法的定位效果，可視化對(duì)比了序列估計(jì)值與真實(shí)值之間的誤差，分別給出了fr3/w/half和fr3/w/xyz序列在ORB-SLAM2和本文算法上運(yùn)行的ATE和RPE結(jié)果，如圖3～6所示.

圖3 兩種算法在fr3/w/half序列上的ATE圖Fig.3 ATE graphs of two algorithms on fr3/w/half sequence

圖4 兩種算法在fr3/w/half序列上的RPE圖Fig.4 RPE graphs of two algorithms on fr3/w/half sequence

圖5 兩種算法在fr3/w/xyz序列上的ATE圖Fig.5 ATE graphs of two algorithms on fr3/w/xyz sequence

圖6 兩種算法在fr3/w/xyz序列上的RPE圖Fig.6 RPE graphs of two algorithms on fr3/w/xyz sequence

圖3、4分別給出了fr3/w/half序列在兩種算法下的相機(jī)軌跡，可以看出，ORB-SLAM2算法估計(jì)的相機(jī)軌跡在fr3/w/half序列下發(fā)生了較大偏移，最大誤差可達(dá)1.50 m左右，而本文算法估計(jì)的相機(jī)軌跡與原始軌跡基本吻合，誤差在0.12 m左右；圖5、6分別給出了fr3/w/xyz序列在兩種算法下的相機(jī)軌跡，可以看出，ORB-SLAM2算法估計(jì)的相機(jī)軌跡在fr3/w/xyz序列下也發(fā)生了較大偏移，最大誤差可達(dá)2.60 m左右，同樣本文算法估計(jì)的相機(jī)軌跡與原始軌跡基本吻合，誤差在0.10 m左右.上述結(jié)果表明，在兩種動(dòng)態(tài)場(chǎng)景下，本文方法明顯減少了相機(jī)位姿的估計(jì)誤差.因此，在有效點(diǎn)較少的動(dòng)態(tài)環(huán)境下，基于ARANSAC動(dòng)態(tài)特征點(diǎn)剔除的單目視覺(jué)SLAM算法，能夠有效消除動(dòng)態(tài)物體對(duì)視覺(jué)定位性能的影響，提高定位精度.

為了說(shuō)明本文所提算法的實(shí)時(shí)性，表4給出了本文算法和ORB-SLAM2算法在各個(gè)模塊中的運(yùn)行時(shí)間.

表4 算法耗時(shí)比較Tab.4 Time consuming comparison of algorithms ms

從表4中跟蹤每幀圖像的平均耗時(shí)可以看出，本文算法比ORB-SLAM2算法運(yùn)行時(shí)間略長(zhǎng)，這主要是因?yàn)楸疚乃惴ú粌H要跟蹤每幀圖像，還要在跟蹤線程中加入動(dòng)態(tài)特征點(diǎn)檢測(cè)及剔除功能，減少動(dòng)態(tài)物體對(duì)算法定位性能的影響.雖然本文算法運(yùn)行時(shí)間有一定延長(zhǎng)，但是在很小時(shí)間代價(jià)下可以明顯地提高單目視覺(jué)SLAM算法在動(dòng)態(tài)復(fù)雜場(chǎng)景下的定位精度，實(shí)時(shí)性也能夠滿足一般機(jī)器人系統(tǒng)的要求.

4 結(jié) 論

本文研究了有效點(diǎn)較少的動(dòng)態(tài)復(fù)雜場(chǎng)景下視覺(jué)SLAM準(zhǔn)確定位問(wèn)題，提出了一種基于ARANSAC動(dòng)態(tài)特征點(diǎn)剔除的單目視覺(jué)SLAM算法.考慮動(dòng)態(tài)場(chǎng)景下有效點(diǎn)較少的情況，提出了ARANSAC算法估計(jì)圖像間的透視變換矩陣，并用透視變換矩陣扭曲上一幀獲得一個(gè)估計(jì)圖像，使得上一幀中的點(diǎn)被統(tǒng)一到當(dāng)前幀的坐標(biāo)系下.通過(guò)光流法計(jì)算特征點(diǎn)在當(dāng)前幀和估計(jì)圖像之間的光流值，進(jìn)而對(duì)動(dòng)態(tài)特征點(diǎn)和靜態(tài)特征點(diǎn)進(jìn)行判斷，然后用獲得的靜態(tài)特征點(diǎn)進(jìn)行SLAM運(yùn)算.通過(guò)仿真結(jié)果對(duì)比分析，在有效點(diǎn)較少的動(dòng)態(tài)復(fù)雜場(chǎng)景下，本文算法顯著減少了位姿估計(jì)誤差，有效提高了單目視覺(jué)SLAM的定位精度.