有效點較少的動態場景下單目視覺SLAM算法

孫 平， 閆 冬

(沈陽工業大學 人工智能學院， 沈陽 110870)

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

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

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

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

1 算法流程

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

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

2 基于動態特征點剔除的單目視覺SLAM算法

2.1 動態特征點檢測算法

本文提出的動態特征點檢測算法由兩個模塊組成，可分為基于ARANSAC算法的自我運動估計和基于光流法的檢測.

2.1.1 基于ARANSAC算法的自我運動估計

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

1) 生成最小樣本.將輸入圖像平均分成8×8的區域，然后隨機選取4個區域，分別在所選區域內取一個特征點，構建最小模型.

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

(1)

(2)

(3)

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

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

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

(4)

式中：σi為最佳模型對應區域有效點占比；pi=1/N，N為有效區域的個數；m為常數4；KS為迭代次數；η為設定的常數.

2.1.2 基于光流法的檢測

自我運動估計后，估計圖像中的點與當前幀中的點會被統一在相同的坐標系下，利用Lucas Kanade計算特征點在當前幀和估計圖像之間的光流值，并設定閾值τ來判斷特征點是靜態的還是動態的，其表達式為

(5)

2.2 剔除動態特征點的單目視覺SLAM算法

文中單目視覺SLAM算法是在ORB-SLAM2基礎上，增加了動態特征點檢測并剔除的功能，以此減少環境中的動態物體對視覺SLAM系統定位精度的影響.每輸入一幀圖片后，先進行自我運動估計和動態特征點檢測，然后剔除ORB-SLAM2中的動態特征點并計算剩余特征點的描述子，用靜態特征點進行SLAM.

3 實驗結果與分析

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

1) freiburg3_walking_static(fr3/w/static)：相機維持微小運動.

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

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

3.1 動態特征點檢測實驗

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

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

圖2 動態特征點檢測算法實驗對比結果Fig.2 Experimental comparison results for dynamic feature point detection algorithms

利用本文提出的ARANSAC動態特征點檢測算法，可以準確地估計透視變換矩陣，進而準確地補償相機運動，因此能夠正確區分場景中的動態特征點和靜態特征點，這對視覺SLAM后續穩定工作起到至關重要的作用.實驗表明，即使在有效點較少的情況下，本文提出的基于ARANSAC檢測算法依然能夠正確區分場景中的動態特征點.

3.2 與ORB-SLAM2算法對比

為了體現本文算法在動態環境下的定位性能，實驗中，采用絕對軌跡誤差(ATE)和相對位姿誤差(RPE)作為評價指標，表1～3分別為本文SLAM算法和ORB-SLAM2算法的對比結果.

表1 絕對軌跡誤差結果Tab.1 Results of absolute trajectory error

表2 旋轉漂移結果Tab.2 Results of rotating drift

表3 平移漂移結果Tab.3 Results of translation drift

由表1～3結果可知，在六個運動序列實驗中，fr1/desk序列屬于靜態序列，對比ATE和RPE指標可知，本文所提算法的均方根誤差和標準偏差均變化不大，可以看出本文算法和ORB-SLAM2算法的估計結果非常相似，保持在同一個精度水平.fr3/s/xyz序列屬于小范圍動態環境，對比ATE和RPE指標可知，本文所提算法的均方根誤差和標準偏差均略有下降，可以看出本文算法的定位精度略優于ORB-SLAM2算法.

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

為了進一步說明本文提出的單目視覺SLAM算法的定位效果，可視化對比了序列估計值與真實值之間的誤差，分別給出了fr3/w/half和fr3/w/xyz序列在ORB-SLAM2和本文算法上運行的ATE和RPE結果，如圖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序列在兩種算法下的相機軌跡，可以看出，ORB-SLAM2算法估計的相機軌跡在fr3/w/half序列下發生了較大偏移，最大誤差可達1.50 m左右，而本文算法估計的相機軌跡與原始軌跡基本吻合，誤差在0.12 m左右；圖5、6分別給出了fr3/w/xyz序列在兩種算法下的相機軌跡，可以看出，ORB-SLAM2算法估計的相機軌跡在fr3/w/xyz序列下也發生了較大偏移，最大誤差可達2.60 m左右，同樣本文算法估計的相機軌跡與原始軌跡基本吻合，誤差在0.10 m左右.上述結果表明，在兩種動態場景下，本文方法明顯減少了相機位姿的估計誤差.因此，在有效點較少的動態環境下，基于ARANSAC動態特征點剔除的單目視覺SLAM算法，能夠有效消除動態物體對視覺定位性能的影響，提高定位精度.

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

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

從表4中跟蹤每幀圖像的平均耗時可以看出，本文算法比ORB-SLAM2算法運行時間略長，這主要是因為本文算法不僅要跟蹤每幀圖像，還要在跟蹤線程中加入動態特征點檢測及剔除功能，減少動態物體對算法定位性能的影響.雖然本文算法運行時間有一定延長，但是在很小時間代價下可以明顯地提高單目視覺SLAM算法在動態復雜場景下的定位精度，實時性也能夠滿足一般機器人系統的要求.

4 結 論

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