基于目標檢測與深度信息關聯的RGB-D SLAM算法

張開平,黃宜慶,吳 桐

(1.安徽工程大學電氣工程學院,安徽,蕪湖 241000;2.安徽省電氣傳動與控制重點實驗室,安徽 蕪湖 241000;3.高端裝備先進感知與智能控制教育部重點實驗室,安徽 蕪湖 241000)

實時定位與建圖(Simultaneous Localization and Mapping, SLAM),是指機器人在沒有先驗信息的情況下,搭載特定的傳感器,通過一系列運動來獲取自身的位姿和構建周圍環境地圖的過程.視覺SLAM就是指將攝像頭作為自身的主要傳感器來采集數據,從而實現定位與建圖功能的方法.目前較為成熟的開源視覺SLAM方案主要有MonoSLAM(Real-Time Single Camera)[1],SVO[2],ORB-SLAM2[3],ORB-SLAM3[4]等.

傳統的SLAM系統在面對例如動態場景和欠紋理的極端環境時,系統的精確性和魯棒性會受到極大的挑戰.此類SLAM系統在工作時,只允許場景中出現少量的動態物體,當動態物體占據場景的大部分面積時,就會影響跟蹤過程的數據關聯,從而導致相機運動估計軌跡漂移等問題.為了提高SLAM在動態場景下的定位精度和魯棒性,研究者們開始嘗試在SLAM系統中去除這些由動態物體導致的影響.此類工作主要分為三類:1)基于多視圖幾何來實現動態點的剔除.Cheng等人提出了一種稀疏運動去除(SMR)模型[5],利用貝葉斯框架檢測每一幀的動態和靜態區域并去除動態區域,在檢測中考慮了相鄰幀的相似性和差異性來減少檢測的不穩定性.Derome等人[6]通過當前圖像和預測圖像之間的殘差獲得光流并計算變換矩陣,將當前幀變換為前一幀圖像來檢測運動物體.但是僅僅依靠幾何和光流方法會導致算法的魯棒性不足.2)通過深度學習對輸入幀進行處理,以獲得圖像的語義信息以剔除動態信息.Jing等人[7]基于ORB-SLAM3,提出使用SparseInst網絡[8]對圖像幀進行語義分割來獲取語義標簽和動態對象掩膜,隨后使用光流和極線約束結合動態掩膜進一步確定動態特征信息,最后設計獨立的點云線程來構建語義全局3D稠密點云地圖.此類方法相比于僅使用幾何方法具有更好的精確性,但是依賴于深度學習網絡的質量,結構簡單的網絡不能夠準確識別圖像中的物體,復雜的網絡會大大降低SLAM系統的運行速度并增加系統的運算量.3)結合多視圖幾何信息與深度學習來檢測動態物體.WEI等人[9]設計了一種自適應語義分割算法對ORB-SLAM2的跟蹤線程進行了改進,并使用全局BA算法優化相機位姿. DRSO-SLAM[10]計算特征點的光流場來跟蹤動態物體光流,并將語義分割網絡得到語義先驗信息作為掩膜信息來計算基礎矩陣,最后通過極線幾何方法去除動態點.DP-SLAM[11]提出了一種實時運動概率傳播方法,結合極線幾何和語義分割去除動態特征信息.Ayman等人提出了一種深度注意力模塊SLAM(DMA-SLAM)算法[12],通過與深度相關的自適應閾值和影響因子來考慮幾何和語義模塊中使用的深度影響.Zhang等人[13]提出了一種不受噪聲影響的運動邊界框算法,該算法可以檢測不同姿態下的人和物體的運動狀態,避免了在篩選動態特征信息時的誤剔除.目前大多數動態環境SLAM算法都是采用此類方法,但是當動態物體占據場景中的很大比例時,直接去除動態物體上的全部信息,會導致用于跟蹤的特征信息過少,造成軌跡丟失,對地圖構建造成影響.

為了解決在動態場景下的定位與建圖問題,本文構造了一種基于圖像深度信息與語義先驗信息相關聯的RGB-D SLAM算法,對于語義分割網絡的實時性問題,利用YOLOv5[13]目標檢測輕量級網絡對輸入圖像進行目標檢測得到動態物體框.對于防止誤剔除動態特征信息的問題,給出一種深度信息隨機采樣一致性(Depth-RANSAC)算法來剔除動態物體框內的動態特征點,保留框內的靜態特征點,提高對于動態特征信息檢測的精確性.

1 本文算法結構

本文算法在ORB-SLAM2的RGB-D模式基礎上進行改進,總框架如圖1所示,該框架是在ORB_SLAM2的基礎上加入了目標檢測線程和D-RANSAC特征點剔除算法.嵌入此線程和算法的目的是過濾被ORB-SLAM2提取的特征點集,這些點集中包括了動態特征點和靜態特征點.如果所有特征信息都被用來估計相機運動軌跡的話,則會造成估計軌跡的誤差,所以只需要靜態特征點來計算相機位姿.本文算法的目標檢測線程是基于YOLOv5網絡實現的,以獲取場景中的動態語義信息,D-RANSAC算法進一步篩選動態特征點,防止過多特征點被剔除而導致跟蹤失敗.

圖1 本文算法框架圖Figure 1 The framework diagram of this algorithm

當系統工作時,由RGB-D相機采集到的圖像會同時傳入跟蹤線程和目標檢測線程,首先在跟蹤線程中對輸入圖像提取FAST關鍵點[15]并計算BRIEF描述子[16],同時目標檢測線程處理傳入的圖像,生成場景中動態或潛在動態的物體框信息,并將結果傳入跟蹤線程.在跟蹤線程中,對提取的特征信息與目標檢測線程傳入的語義先驗信息進行擬合,并與場景深度信息相關聯,通過D-RANSAC算法剔除屬于動態物體上的特征點.過濾后的特征信息被用來進行位姿初始化和重定位,此后的流程類似于ORB-SLAM2.

目標檢測算法的目標是識別出場景中的各類物體以及獲得這些物體的位置信息.目前目標檢測算法可以分為two-stage和one-stage兩類.YOLO(You Only Look Once)[17]是Joseph Redmon和Ali Farhadi等人于2015年提出的基于單個神經網絡的目標檢測系統.YOLOv5是Glenn Jocher在2021年發布的一階段目標檢測算法,該算法在YOLOv4的基礎上做出了一系列改進,其速度和精度都取得了一定進步.

2 深度信息隨機采樣一致性(D-RANSAC)算法

ORB-SLAM2是基于ORB特征點來實現定位與建圖的SLAM系統,其提取的特征點質量關系到整個SLAM系統的魯棒性與精確性.動態特征信息會使系統在計算相機運動軌跡時產生誤差漂移.在DMS-SLAM[18]中,將YOLO目標檢測網絡獲得的動態框內的所有特征點全部去除,來實現動態信息的篩除,但是當人占場景的三分之二以上的時候,這種方法會剔除場景中的大多數特征點,從而導致SLAM系統跟蹤所需要的特征點數量不足,出現跟蹤失敗、系統的魯棒性下降、相機估計軌跡漂移等問題.

本文給出一種結合語義先驗信息和深度信息的動態點剔除方法,這種方法可以準確剔除動態框內屬于人身上的特征點,保留框內的靜態特征點.在目標檢測線程中會獲得當前場景中物體的bounding box.根據深度圖像可以區分處于不同深度場景的不同物體的輪廓,如圖2所示.借助這些信息,可以利用D-RANSAC算法來篩選落在人身上的特征點,從而保留那些屬于動態框內的靜態特征點.此算法是基于RANSAC(Random Sample Consensus)算法[19]改進而來,RANSAC是可以從一個包含有“外點”的點集中,通過迭代的方式來估計唯一數學模型的算法.本文D-RANSAC算法的使用有一些前提,首先是人需要占據bounding box的三分之二以上,其次落在人身上的特征點的深度值差異小于閾值,最后是人與周圍背景要有明顯深度差異.

圖2 深度圖像Figure 2 Depth image

深度隨機采樣一致性算法流程如下:首先將動態框內的特征點push一個集合Pdynamic,然后進入循環,當前迭代次數n小于我們初始化的迭代次數N時,在Pdynamic中隨機選取兩個特征點,獲得這兩個點在其深度圖像上的深度信息d1,d2,計算Dmod,如式(1)所示.

(1)

隨后遍歷Pdynamic中剩余的點,計算每個點與Dmod的差值,當差值小于閾值Th時,將此點加入內點集,更新內點集的數量s和迭代次數N,迭代完成時,輸出最終的內點集合.傳統RANSAC算法需要提前規定好算法所需的迭代次數,如果迭代次數過大,則會導致計算資源的浪費,若設計的迭代次數過小,則不能獲得比較精確的內點集.本文在D-RANSAC算法中加入了動態更新迭代次數算法,具體如下:

在Pdynamic中隨機選擇k個點來計算數學模型,則內點在全部點中的百分比P:

(2)

則k個點中至少有一個點為外點的概率為1-Pk.在N次迭代的條件下,每一次選擇的點集中都存在離散點的概率為(1-Pk)n,用Ptrue來表示從點集中隨機選擇的點在N次迭代中都是內點的概率,則:

Ptrue=1-(1-Pk)N

(3)

那么迭代次數N為

(4)

每個點為內點的概率P是先驗值,Ptrue是通過RANSAC計算得出正確數學模型的概率.RANSAC算法開始時P的準確值是未知的,在初始化時把P設為一個足夠大的值,在每次更新深度模型和內點集的時候,利用當前內點集合計算出P,由P更新迭代次數N,由此可以根據由當前計算出的深度模型所獲得的內點集合,動態更新RANSAC迭代次數,達到更精確的輸出結果和更快的計算速度.

3 實驗分析

為了進行對比,本節在特征信息提取、SLAM定位評估兩個方面對算法進行評估,并與ORB-SLAM2、YOLO-SLAM等兩種算法在TUM數據集上進行對比試驗.ORB-SLAM2使用RANSAC算法來實現對于動態點的剔除,以獲得相對準確的相機姿態.YOLO-SLAM是本文在ORB-SLAM2的基礎上,僅加入YOLOv5目標檢測網絡并將動態框內的所有特征信息去除來實現動態信息的篩選的SLAM系統.

3.1 特征信息提取

本小節對本文中提出的深度信息隨機采樣一致性算法進行實驗.實驗平臺是CPU為Intel i7-8700 ,GPU為GeForce GTX2080Super,內存為64GB的計算機.

實驗在TUM數據集進行,實驗結果如圖3所示.

圖3 特征信息提取實驗結果Figure 3 Experimental results of feature information extraction

如實驗結果所示,圖3(A)表示ORB-SLAM2工作時,會從動態物體上提取特征點信息,這些信息會導致相機的估計軌跡出現誤差.圖3(B)表示只使用YOLOv5目標檢測網絡來獲取動態物體并將動態框內的信息全部剔除.當動態物體占據場景的大部分時,會造成SLAM系統跟蹤所需的信息過少,從而導致跟蹤丟失.圖3(C)表示在圖3(B)的基礎上加入了深度信息隨機采樣一致性算法后,將動態框內屬于人身上的特征點篩除,保留了框內的靜態特征點信息,從而不會因為特征信息過少導致跟蹤丟失.

3.2 定位精確性檢驗

本節在TUM數據集上對本文算法進行了檢驗,并與ORB-SLAM2、YOLO-SLAM在定位評估方面做了定量分析.TUM數據集由在不同場景使用RGB-D相機記錄的39個序列組成,包含了測試、手持SLAM、三維物體重建、動態物體等多種針對不同任務的數據集.本節主要測試TUM數據集中的動態場景序列.

本文算法與ORB-SLAM2、YOLO-SLAM算法在高動態序列上的軌跡誤差如圖4所示.從圖4(A)可以看出沒有深度信息隨機采樣一致性算法的YOLO-SLAM2表現得比原始的ORB-SLAM2差,這說明只通過目標檢測網絡篩除全部的動態信息,會使SLAM系統跟蹤所需的特征信息過少,導致跟蹤丟失,造成了估計軌跡的誤差.在圖4的實驗結果中,本文算法表現最好,這表明深度隨機采樣一致性算法有效解決了YOLO-SLAM導致的跟蹤信息過少的問題,并在高動態場景中相較于ORB-SLAM2算法具有更好的性能.

本文算法與ORB-SLAM2、YOLO-SLAM的對比結果如表1～3所示.表1是各算法絕對軌跡誤差,可以看出本文算法在前四個高動態序列表現最好.其中,在動態場景較為復雜的“fr3/w/rpy”序列上,本文算法相較于ORB-SLAM2,相機估計軌跡在RMSE和STD精度提高了25%和15%,而與YOLO-SLAM相比較提高了15%.在前四個高動態序列中,本文算法相較于ORB-SLAM2在RMSE上平均提升55.5%,在STD上平均提升63.25%,相較于YOLO-SLAM,平均精度分別提高47.25%和38.75%.由此可見,本文算法在高動態環境下的精度均優于對比算法.

表1 絕對軌跡誤差對比結果

從表2看出,YOLO-SLAM在“fr3/w/rpy”、“fr3/w/static”和“fr3/w/half”三個動態序列上的精度相較于原始的ORB-SLAM2,取得了較差的結果.原因是YOLO-SLAM將目標檢測網絡中獲得的動態物體框內的特征信息全部去除,造成了跟蹤所需的信息過少,導致了跟蹤丟失.而本文給出的深度信息隨機采樣一致性算法去除了動態物體框內真正屬于動態物體(在本文中是人這一類別)上的特征信息,保留了靜態特征,所在高動態序列RPE平移和旋轉部分的對比中,本文算法表現最好.

表2 相對軌跡誤差對比結果

從表3可以看出,在低動態序列“fr3/s/xyz”和“fr3/s/static”中,本文算法相較于對比算法,提升并不明顯.這是因為ORB-SLAM2在低動態和靜態場景下工作的精度足夠好,所以在此場景下的本文算法的改進并不明顯.

表3 相對軌跡誤差旋轉部分對比結果

從圖5(A)可以看出,ORB-SLAM2在fr3/w/xyz序列中的估計軌跡在y方向的誤差達到了1.25m,YOLO-SLAM的估計軌跡與真實軌跡在y方向的誤差達到了0.6 m,而本文算法相較于對比算法具有更小的誤差,在y方向上只有0.3 m,在x方向只有0.2 m.因此在動態序列中,本文算法比對比算法具有更優的魯棒性.

圖5 估計軌跡與真實軌跡誤差圖Figure 5 Error diagram of estimated trajectory and real trajectory

4 結 語

本文構造了一種基于ORB-SLAM2的動態環境RGB-D SLAM系統,使用YOLOv5目標檢測網絡來對輸入圖像進行動態物體檢測,再將語義先驗信息與場景深度信息相關聯,使用D-RANSAC算法來剔除動態物體框內的動態特征點,保留框內的靜態特征點,利用保留下來的穩定靜態特征點來進行動態序列下的運動軌跡估計和地圖構建.通過實驗平臺對TUM數據集的檢測,結果表明本文算法在高動態環境下的相機姿態和定位精度上有明顯提高.本文算法在面對低動態場景以及靜態場景時,前者由于目標檢測網絡將人定義為高動態物體,當人是靜止或者是緩慢移動的時候,會將人身上的一些靜態特征點剔除,導致跟蹤效果下降.如何正確篩除當前正在運動的動態特征點時下一步工作的重點,未來考慮改進本文的D-RANSAC算法,使其適應更復雜的環境,使其在工作時具有更好的魯棒性.