室內動態場景下基于稀疏光流與實例分割的視覺SLAM算法

張小勇，張 洪，高清源，湯多良，曹 毅

(1.江南大學 機械工程學院,江蘇 無錫 214122; 2.淮南職業技術學院 機電工程學院,安徽 淮南 232001)

隨著研究的不斷深入,移動機器人的應用場景變得愈加廣泛[1-2]。作為移動機器人實現智能導航的基礎的同時定位與建圖(simultaneous localization and mapping,SLAM),其與機器視覺相結合的視覺SLAM逐漸成為移動機器人領域的研究熱點[3-5]。傳統視覺SLAM算法雖在靜態場景中表現出較好的性能,但對動態對象的感知能力存在不足,若建圖時處于動態場景,可能出現動態特征點的誤匹配,影響定位精度,甚至出現運動目標跟蹤丟失的問題[6-7]。因此,研究動態場景下的高精度定位對移動機器人的應用至關重要[8]。

針對動態場景下的視覺SLAM算法的研究方法可分為兩類:一是基于多視幾何、光流等傳統幾何方法檢測異常點[8-11];二是基于深度學習方法對先驗的運動對象進行檢測分割[13-19]。

基于幾何的檢測方法在耗時方面存在優勢。例如:Zhang等[8]使用密集光流殘差檢測場景中的動態區域,同時使用與靜態特征融合的框架重建靜態背景,提出FlowFusion算法;Dai等[9]基于靜態特征之間相對位置各時刻恒定不變的特點,利用點相關性區分靜態和動態地圖點;Li等[10]對深度圖的邊緣點進行靜態加權,提出基于深度邊緣特征的實時RGB-D SLAM算法;Palazzolo等[11]基于截斷符號距離函數(truncated signed distance function, TSDF),利用純幾何的濾波方法去除動態目標,提出ReFusion算法。但是基于幾何檢測的方法在復雜環境下無法完整檢測潛在的動態對象,難以保證算法的定位精度[12]。

基于深度學習的方法以圖像分割算法為主、幾何檢測為輔。例如:Yu等[13]利用SegNet圖像分割網絡和運動一致性檢測的方法剔除動態特征點,提出DS-SLAM算法;Bescos等[14]利用Mask R-CNN實例分割網絡與多視幾何的方法,完成運動物體的檢測,提出動態視覺SLAM中經典的Dyna-SLAM算法;Zhong等[15]利用SSD目標檢測網絡對關鍵幀的動態對象進行檢測,根據檢測結果將特征點分為4種狀態并傳播至普通幀,提出Detect-SLAM算法;Cheng等[16]將具有深度信息的特征點重新投影到當前幀上,利用重投影偏移向量來檢測動態點,提出DM-SLAM算法;Ji等[17]利用SegNet圖像分割網絡對關鍵幀進行檢測以降低計算成本,通過深度圖像聚類與重投影誤差的方法識別動態區域以檢測未知的運動目標,提出TRS-SLAM算法。但是基于深度學習的方法跟蹤線程耗時多,難以保證算法的實時性[19]。

針對現有動態視覺SLAM算法難以同時兼顧算法定位精度與實時性問題,本研究提出一種基于稀疏光流與實例分割網絡的動態視覺SLAM算法。針對算法定位精度,利用Mask R-CNN實例分割網絡和Lucas-Kanade稀疏光流的方法剔除異常特征點;針對算法的實時性,設計異常特征點二次剔除模塊,降低實例分割網絡對跟蹤線程耗時的影響;最后通過公開數據集進行試驗驗證。

1 視覺SLAM算法

針對動態場景的視覺SLAM算法大多建立在ORB-SLAM2算法[20]的基礎上。相比傳統視覺SLAM算法,ORB-SLAM2算法先將視覺SLAM算法劃分為3個線程,即Tracking、Local Mapping、Loop Closing,再通過System總線程實現機器人的位姿估計與地圖構建,算法框架圖如圖1所示。其中Tracking線程負責相機的定位,主要包括ORB(oriented fast and rotated brief)特征點提取、相機位姿初始化、相機位姿估計及關鍵幀選取等步驟。

圖1 ORB-SLAM2算法框架圖Fig.1 System structure diagram of ORB-SLAM2

ORB-SLAM2的特征點提取算法為ORB算法。相比SIFT(scale-invariant feature transform)和SURF(speeded up robust features)算法,ORB算法在保證匹配準確性的前提下,運算速度得到大幅提高,算法特點如下:

ORB算法使用強度重心的方法,利用重心計算來明確特征點的方向,得到ORB的特征描述向量,使得ORB特征點具有旋轉不變性。

ORB算法利用BruteForceMatcher類中的match函數,對兩特征點的描述子進行“異”“或”運算得到它們之間的漢明距離,實現特征點幀間的匹配。漢明距離表示兩特征點之間的相似程度,如式(1)所示。

(1)

式中:d(A,B)為A和B特征點之間的漢明距離;Ai和Bi分別為A和B特征點的二進制描述符;⊕為“異”“或”運算符。

采用隨機采樣一致性算法(rand sample consensus,RANSAC)剔除特征匹配結果中的異常特征點,從而減少特征點的誤匹配,提高幀間匹配的準確率。

2 改進Tracking線程算法

Tracking線程用于在視覺SLAM算法中估計相機的位姿。工作原理是:先從圖像幀提取特征點,通過重定位來初始化位姿或是根據前一幀進行位姿估計;再將局部地圖與當前幀進行匹配,根據最小化投影誤差優化當前幀位姿;最后,根據關鍵幀選擇條件確定新的關鍵幀,將關鍵幀傳給Local Mapping線程。

為解決動態場景對定位精度造成的影響,傳統動態視覺SLAM算法通常在Tracking線程中融入幾何檢測算法或圖像分割網絡,這使得SLAM算法在高動態場景下難以兼顧算法的定位精度與實時性。本文基于Mask R-CNN實例分割網絡與Lucas-Kanade稀疏光流對傳統動態視覺SLAM算法的Tracking線程進行改進,以降低圖像分割網絡對Tracking耗時的影響。改進算法示意圖如圖2所示。

圖2 改進Tracking線程示意圖Fig.2 Schematic diagram of improved tracking algorithm

由圖2可知,Tracking線程的改進主要包括以下3點:

——在Tracking線程中融入Lucas-Kanade稀疏光流,通過連續幀特征點光流檢測異常特征點,完成異常特征點初步剔除,以保證算法的定位精度。

——基于ROS通信,將Mask R-CNN實例分割網絡獨立于Tracking線程之外,完成動態對象的檢測并保存動態圖像幀,以此降低圖像分割網絡對Tracking耗時的影響,提高算法實時性。

——異常特征點的二次剔除。先根據圖像分割模塊動態圖像幀對動態特征點容器進行更新,再利用容器進行特征點匹配,最后根據匹配結果完成異常特征點的二次剔除,進一步提高算法的定位精度。

2.1 基于Mask R-CNN的異常特征點二次剔除

為增強視覺SLAM算法對先驗動態對象的感知能力,從而提高算法在動態場景中的定位精度,在算法框架中融入圖像分割網絡。為便于與Dyna-SLAM算法進行對比,選取Mask R-CNN網絡對先驗動態對象進行檢測,基本框架如圖3所示。Mask R-CNN網絡在Faster R-CNN網絡的基礎上添加了預測目標掩膜的分支網絡,可在實現目標檢測的同時進行語義分割,具有準確率高、簡單直觀、易于使用等優點[21]。Mask R-CNN網絡選擇在COCO數據集[22]上進行預訓練,該數據集包含80多類對象,可滿足試驗動態對象檢測的要求。

圖3 Mask R-CNN算法框架圖Fig.3 Schematic diagram of Mask R-CNN framework

為消除動態場景對算法定位精度的影響,傳統基于深度學習的動態視覺SLAM算法通常在Tracking中加入圖像分割網絡來檢測并剔除動態特征點,以此保證動態場景下算法的定位精度。但此類算法在對每幀圖像進行位姿估計前需等待圖像的分割結果,使得算法跟蹤線程的耗時較多,故算法實時性不佳[19]。

為降低圖像分割對跟蹤線程耗時的影響,基于Mask R-CNN網絡提出異常特征點的二次剔除算法,如圖2所示。改進算法將圖像分割模塊置于Tracking線程之外,使得Tracking線程當前幀無需等待圖像分割模塊的動態幀檢測結果,直接調用動態特征點容器完成異常特征點的二次剔除并進行后續位姿估計。因此,改進算法能有效降低圖像分割網絡對Tracking線程耗時的影響,提高算法的實時性。異常特征點的二次剔除算法主要分為圖像分割模塊和異常特征點的二次剔除模塊兩部分。

圖像分割模塊。該模塊接收圖像幀并進行語義分割,完成先驗動態對象的檢測,為動態特征點容器的更新提供動態圖像幀,但不直接參與異常特征點的二次剔除。圖像分割模塊工作流程如圖4所示。圖像分割模塊接收Tracking線程當前RGB圖像幀,利用圖像分割網絡完成語義分割,語義分割圖如圖4(b)所示。對得到的語義分割結果圖進行圖像閾值分割,得到閾值分割圖(圖4(c))。將閾值分割圖與輸入圖像進行圖像的“與”運算,運算規則如式(2)所示,得到僅有動態對象的動態圖像幀,如圖4(d)所示。

圖4 圖像分割模塊示意圖Fig.4 Schematic diagram of image segmentation module

(2)

式中:Iinput、Ioutput和Imask分別為輸入圖像幀、動態圖像幀和閾值分割圖中目標位置的像素值。

異常特征點的二次剔除模塊。該模塊更新并利用動態特征點容器二次剔除當前圖像幀中存在的動態特征點,以降低動態特征點對算法定位精度的影響。異常特征點的二次剔除模塊處于Tracking線程中,此模塊執行前會判斷是否接收到新的動態圖像幀。若是,先對新的動態圖像幀進行ORB特征提取,再將提取的特征點與容器特征點進行匹配,篩選合適的特征點對容器進行更新后完成異常特征點的二次剔除;若否(即圖像分割模塊還沒有完成語義分割任務),則無需等待語義分割的完成,直接跳過容器的更新步驟,完成異常特征點的二次剔除。相比傳統基于深度學習的方法,基于Mask R-CNN實例分割網絡的二次剔除算法將圖像分割網絡置于Tracking線程之外,圖像分割模塊的耗時不會影響Tracking線程,只會影響二次剔除模塊動態特征點容器的更新頻率。因此,改進算法可在保留算法對動態對象感知能力的前提下,保證算法的實時性。由于圖像分割模塊耗時高于Tracking線程耗時,圖像分割模塊更新的動態圖像幀往往滯后于Tracking線程當前幀,具體滯后情況如圖5所示。平均滯后時間主要取決于圖像分割模塊選取的網絡模型,本文選取的Mask R-CNN網絡模型的平均滯后時間約為7幀。

圖5 動態圖像幀滯后示意圖Fig.5 Schematic diagram of lag of dynamic image frame

2.2 基于Lucas-Kanade光流的異常特征點剔除

低動態場景中動態圖像幀的更新滯后對二次剔除算法的剔除結果影響較小,但高動態場景中的更新滯后對二次剔除算法的剔除結果影響較大,這將影響算法的定位精度。本文在Tracking線程的二次剔除模塊前融入Lucas-Kanade稀疏光流,通過檢測相鄰幀的異常特征點,完成潛在動態特征點的初步剔除,以保證算法的定位精度。

Lucas-Kanade稀疏光流[23]基于3大假設:一是亮度恒定,即同一像素點在不同幀的亮度不變;二是運動微小,即相鄰幀的運動較小;三是空間一致,即目標區域內的像素點有相似的運動。

Lucas-Kanade稀疏光流通過對相鄰幀圖像中的特征點進行跟蹤,計算各特征點的光流矢量,由此判斷是否存在異常特征點。基于亮度恒定與運動微小兩個假設,得到圖像的約束方程,如式(3)所示。

Ixu+Iyv+It=0

(3)

式中:u和v分別為像素點沿x和y軸的速度矢量;Ix和Iy分別為圖像在x和y軸方向的梯度;It為時間方向的梯度。

基于空間一致性的假設,即鄰域內的光流為固定值,鄰域內像素點具有相似的運動。選取3×3的鄰域范圍,鄰域內所有像素點的運動可用式(4)表示。

(4)

選取ORB特征點作為跟蹤特征點,此特征點為變化明顯的角點,可有效避免跟蹤過程中出現的孔徑問題。再利用最小二乘法進行擬合優化求得速度矢量,可用式(5)表示。

(5)

基于ORB特征點與Lucas-Kanade稀疏光流,利用OpenCV庫的calcOpticalFlowPyrLK()函數,設計異常特征點的剔除算法。異常特征點的剔除流程如圖6所示。

圖6 基于Lucas-Kanade稀疏光流剔除算法的流程圖Fig.6 Flow chart of culling algorithm based on Lucas-Kanade sparse optical flow

異常特征點的剔除算法的具體步驟為:

Step1:接收當前輸入幀的RGB圖像,記為prevImg。檢測prevImg圖像幀的ORB特征點,完成稀疏光流點的初始化,再保存圖像幀與特征點的坐標數據。

Step2:接收下一幀的RGB圖像,記為nextImg。調用calcOpticalFlowPyrLK()函數,對nextImg和prevImg計算光流,進行ORB特征點跟蹤,得到特征點的跟蹤狀態status及其位置相對偏移誤差值errors。再根據誤差值errors完成誤差值閾值errors_value的設定。

其中,errors為特征點在前后兩幀圖像中位置的相對偏移誤差值。特征點位置的相對偏移誤差值及位置的偏移量定義可由式(6)和(7)表示。

(6)

dist[k]=abs(pre_keypoints[k].x-

next_keypoints[k].x)+abs(pre_keypoints[k].y-

next_keypoints[k].y)

(7)

式中:errors為特征點在前后兩幀圖像中位置的相對偏移誤差值;dist為特征點在前后兩幀圖像中位置的偏移量;pre_keypoints為前一幀圖像的特征點;next_keypoints為當前幀圖像的特征點。

誤差閾值errors_value的設定依據還包括跟蹤成功的特征點剩余的數量、位置相對偏移誤差值。誤差閾值的設定可在保證有充足的特征點用于位姿估計的情況下,盡可能地剔除所有的異常特征點。

Step3:根據nextImg的光流跟蹤結果,剔除跟蹤失敗的特征點,再對跟蹤成功的特征點進行篩選。若特征點的誤差值errors大于設定誤差值閾值errors_value,則視為異常特征點,剔除此類特征點。

Step4:檢查二次剔除后的剩余特征點是否充足,將nextImg剩余的ORB特征點數量與預設的特征點數量閾值進行比較。若nextImg剩余的ORB特征點數量小于閾值,則對nextImg圖像幀進行ORB特征提取,完成跟蹤特征點的補充。

Step5:如果當前幀圖像不是最后一幀,則將nextImg定義為prevImg,繼續執行步驟Step2～Step4完成特征點的光流跟蹤,否則退出算法循環。

為驗證基于Lucas-Kanade稀疏光流的剔除算法的有效性,在公開數據集上進行測試,特征點跟蹤效果的試驗結果如圖7所示。由圖7可知,在動態場景中,無論動態對象是否為先驗對象,在其發生明顯位移的情況下,基于Lucas-Kanade稀疏光流的剔除算法都可有效剔除大部分的異常特征點。

圖7 特征點跟蹤試驗結果圖Fig.7 Experiment results of feature point tracking

綜上,改進算法在傳統算法框架中融入Lucas-Kanade稀疏光流與Mask R-CNN圖像分割網絡,相比基于幾何檢測的算法,改進算法能有效提高算法定位精度。改進算法利用ROS通信將圖像分割模塊置于Tracking線程之外,可降低圖像分割網絡對Tracking線程耗時的影響。相比基于深度學習的算法,改進算法可大幅降低Tracking線程耗時,從而提高算法的實時性。

3 試驗驗證

為驗證改進算法在試驗場景中對定位精度與實時性的兼顧效果,通過公開數據集進行試驗驗證。

3.1 試驗評價指標

試驗采用的評價指標包括相機的絕對軌跡誤差ATE(absolute trajectory error)、Tracking線程單幀圖像的平均耗時。其中,相機的絕對軌跡誤差ATE為相機估計位姿與真實位姿的直接差值,ATE越小,算法的定位精度越高。其中,第i幀的ATE定義可由式(8)表示。

(8)

式中:Fi為第i幀的絕對軌跡誤差;Qi為真實位姿;Pi為估計位姿;S為估計到真實位姿的相似旋轉矩陣。

使用RMSE(root mean square error)對ATE進行統計分析,如式(9)所示。

(9)

3.2 公開數據集試驗驗證

試驗平臺為搭載Ubuntu16.04操作系統的筆記本電腦,處理器的型號為i7-8550U,主頻1.80 GHz,64位操作系統,運行內存為8 GB,顯卡為NVIDIA GeForce MX 150。

TUM RGB-D[24-25]公開數據集是德國慕尼黑工業大學提出的標準RGB-D數據集,常用于評估SLAM算法在各種場景下的性能。該數據集利用運動視覺捕捉系統記錄相機在真實場景中的連續軌跡數據,可提供序列對齊的RGB圖像、深度圖像和真實軌跡。

選取TUM RGB-D freiburg3數據集下6組子數據集進行試驗,即sitting_xyz(S_xyz)、sitting_halfsphere(S_half)、walking_static(W_static)、walking_xyz(W_xyz)、walking_rpy(W_rpy)、walking_halfsphere(W_half)。其中,walking和sitting分別表示高動態與低動態場景數據集。選用ORB-SLAM2、ReFuison、Dyna-SLAM、DM-SLAM、TRS-SLAM算法和本文算法進行對比試驗,試驗選取絕對軌跡誤差ATE的RMSE值作為評價指標。表1為各種算法下相機關鍵幀的絕對軌跡誤差。

表1 TUM數據集下相機關鍵幀絕對軌跡誤差的RMSE值Table 1 RMSE values of absolute trajectory error of camera key frame in TUM database m

分析表1可得:在TUM數據集下,相比ORB-SLAM2算法,本文算法在低動態場景中的相機關鍵幀絕對軌跡誤差的RMSE值依次降低30.20%、15.33%,在高動態環境中依次降低93.94%、93.49%、94.26%、98.31%。相比ReFusion算法,本文算法在低動態場景中依次降低81.28%、68.98%,在高動態環境中依次降低80.24%、84.79%、64.48%、80.49%。相比Dyna-SLAM算法,本文算法在S_xyz、W_half、W_xyz場景中依次降低6.45%、5.31%、3.43%,在W_rpy場景中受動態特征點容器的滯后效應影響較大,存在剔除不徹底的情況,定位精度下降。相比DM-SLAM和TRS-SLAM算法,本文算法在W_half場景中分別降低21.90%、26.21%,在W_static場景中分別降低17.72%、41.44%。

為進一步驗證試驗的結果,對部分算法在高動態場景中的相機絕對軌跡誤差對比圖進行分析。絕對軌跡誤差對比圖如圖8所示,每行的4幅分圖依次對應基于W_half、W_rpy、W_static、W_xyz數據集的分析結果,圖中黑色曲線為相機運動軌跡的真實值(groundtruth),藍色曲線為相機運動軌跡的算法估計值(estimated),紅色直線為相機運動軌跡真實值與估計值的差值(difference)。由圖8可知,在高動態場景中:ORB-SLAM2算法下的相機運動軌跡誤差最大,相機軌跡出現明顯偏移;ReFusion算法下的運動軌跡誤差相比ORB-SLAM2算法有所降低,但仍存在偏移情況;Dyna-SLAM算法與本文算法下的運動軌跡誤差相比其他兩種算法明顯降低,相機軌跡偏移量小。

圖8 TUM數據集下高動態場景下相機絕對軌跡誤差對比Fig.8 Comparison of absolute trajectory error of the camera in high dynamic scene under TUM database

Bonn RGB-D Dynamic Dataset[11]是用于評估RGB-D SLAM與三維重建的公開數據集,數據集的序列格式與TUM RGB-D相同。本文選取Bonn RGB-D公開數據集下的6組子數據集rgbd_bonn_crowd (Crowd)、rgbd_bonn_crowd2 (Crowd2)、rgbd_bonn_placing_nonobstructing_box (P_no_box)、rgbd_bonn_person_tracking (P_Tracking)、rgbd_bonn_removing_nonobstructing_box (R_no_box)、rgbd_bonn_person_tracking2 (P_Tracking2)進行試驗。以絕對軌跡誤差ATE的RMSE值和Mean值(均值)為評價指標,選取ORB-SLAM2、Refusion、Dyna-SLAM算法與本文算法進行對比試驗。表2為各種算法下相機關鍵幀的絕對軌跡誤差。

表2 Bonn數據集下相機關鍵幀的絕對軌跡誤差RMSE值Table 2 RMSE values of absolute trajectory error of camera key frame in Bonn database m

分析表2可得:相比ORB-SLAM2算法,本文算法的相機關鍵幀絕對軌跡誤差RMSE值在Bonn數據集的6個子集中依次降低98.62%、97.76%、93.76%、91.52%、96.32%、30.77%。相比ReFusion算法,本文算法依次降低83.57%、96.18%、86.55%、78.89%、70.10%、81.63%。相比Dyna-SLAM算法,改進算法在P_Tracking2和Crowd場景中受動態容器更新滯后效應影響較大,精度有所下降;在Crowd2、P_Tracking、P_no_box、R_no_box場景中依次降低14.28%、35.09%、30.77%、91.82%。

除定位精度外,視覺SLAM算法的實時性也是評價其算法優劣的重要性能指標。TUM與Bonn數據集下各種算法的跟蹤線程耗時如圖9所示。由圖9可知:Dyna-SLAM算法跟蹤線程耗時最多,且單幀圖像的耗時情況波動嚴重。ReFusion算法跟蹤線程耗時情況相比Dyna-SLAM算法有所降低,耗時波動情況有所改善。本文算法在TUM與Bonn數據集下跟蹤線程平均耗時分別為98.25與91.38 ms。相比ORB-SLAM2算耗時法,為提高算法的定位精度,改進算法耗時分別增加40.04與43.65 ms;相比ReFusion算法耗時分別降低77.33%與83.32%,相比Dyna-SLAM算法耗時分別降低88.72%與86.70%;算法的耗時波動情況相比ReFusion和Dyna-SLAM算法得到明顯改善,能夠保證算法的穩定運行。

圖9 TUM與Bonn數據集下不同算法跟蹤線程的單幀耗時Fig.9 Single-frame time cost of different algorithms in Tracking process under TUM and Bonn databases

綜上,在TUM與Bonn公開數據集中的各種動態場景下,本文改進算法的相機絕對軌跡誤差與跟蹤線程的耗時情況,相比傳統視覺SLAM算法明顯降低。說明改進算法可在保證算法定位精度的前提下,有效提高算法的實時性。

4 結 論

本文提出一種基于實例分割網絡與稀疏光流的動態視覺SLAM算法。在傳統SLAM算法的框架上對Tracking線程進行優化。改進算法利用稀疏光流完成異常特征點初步剔除,通過實例分割網絡設計異常特征點的二次剔除算法,使圖像分割網絡獨立于Tracking線程。該算法可降低圖像分割網絡對Tracking線程耗時的影響,提高算法的實時性。在TUM與Bonn公開數據集下,本文算法的定位精度有所提高;兩種數據集下跟蹤線程耗時相比ReFusion算法降低77.33%與83.32%,相比Dyna-SLAM算法降低88.72%與86.70%。