有效點(diǎn)較少的動態(tài)場景下單目視覺SLAM算法

孫 平， 閆 冬

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

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

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

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

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

1 算法流程

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

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

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

2.1 動態(tài)特征點(diǎn)檢測算法

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

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

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

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

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

(1)

(2)

(3)

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

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

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

(4)

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

2.1.2 基于光流法的檢測

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

(5)

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

文中單目視覺SLAM算法是在ORB-SLAM2基礎(chǔ)上，增加了動態(tài)特征點(diǎn)檢測并剔除的功能，以此減少環(huán)境中的動態(tài)物體對視覺SLAM系統(tǒng)定位精度的影響.每輸入一幀圖片后，先進(jìn)行自我運(yùn)動估計(jì)和動態(tài)特征點(diǎn)檢測，然后剔除ORB-SLAM2中的動態(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)存，軟件平臺為Ubuntu16.04系統(tǒng).freiburg3_walking序列屬于高動態(tài)環(huán)境，一些動態(tài)物體的占比會超過50%以上(即有效點(diǎn)較少的情形).本文利用以下四個高動態(tài)圖像序列進(jìn)行實(shí)驗(yàn)分析，相機(jī)運(yùn)動類型如下：

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

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

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

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

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

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

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

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

3.2 與ORB-SLAM2算法對比

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

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

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

為了進(jìn)一步說明本文提出的單目視覺SLAM算法的定位效果，可視化對比了序列估計(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é)果表明，在兩種動態(tài)場景下，本文方法明顯減少了相機(jī)位姿的估計(jì)誤差.因此，在有效點(diǎn)較少的動態(tài)環(huán)境下，基于ARANSAC動態(tài)特征點(diǎn)剔除的單目視覺SLAM算法，能夠有效消除動態(tài)物體對視覺定位性能的影響，提高定位精度.

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

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

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

4 結(jié) 論

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