基于平面匹配與目標檢測的視覺SLAM算法

婁 路，張 忍，李一天，隗寒冰，王桂平

(1.重慶交通大學 信息科學與工程學院，重慶 400074；2.重慶交通大學 機電與車輛工程學院，重慶 400074)

0 引 言

同時定位與建圖(simultaneous localization and mapping，SLAM)由SMITH等[1]提出，而視覺SLAM(visual-simultaneous localization and mapping，V-SLAM)是指用相機作為傳感器，在未知環境中估計相機位姿的同時建立周圍環境模型。目前V-SLAM算法逐漸成熟，MUR-ARTAL等[2]提出的ORB-SLAM2算法在ORB-SLAM[3]基礎上，增加雙目和RGB-D模式，能夠實現地圖重用、閉環檢測和重新定位的功能。YU等[4]提出的DS-SLAM是一種適用于動態環境中的語義SLAM算法，增加了語義分割模塊和運動一致性檢查模塊，雖然在動態環境中提高了定位精度，但運行速率下降。BESCOS等[5]提出的DynaSLAM因為增加了動態物體檢測和背景修復功能模塊，所以導致檢測耗時較長，實時性較差。WU等[6]提出的YOLO-SLAM包含輕量級目標檢測網絡，并結合一種新的幾何約束方法來過濾檢測區域的動態特征，該算法雖能達到實時效果，但無法應對低紋理、光照不足等環境問題。為解決V-SLAM遇到的這些問題，一些結合點和線特征來估計相機運動的方法[7-9]，或者結合點和面特征來估計相機位姿的方法被陸續提出來了[10-13]。

盡管V-SLAM算法已經取得了重大成就，但大多數算法只使用點特性作為特征信息，在低紋理環境下或者在光照變化場景下仍很難工作。本文提出一種基于單目和RGB-D視覺，融合快速目標檢測和平面匹配的三維重建SLAM算法(fastest yolo plane mapping SLAM，FPM-SLAM)，實驗結果表明，該算法同時滿足戶外和室內的工作場景需求，并解決動態復雜環境下可能存在的定位精度下降甚至失敗的問題。

1 本文算法介紹

1.1 FPM-SLAM算法框架

本文提出的算法利用Yolo-Fastest[14]快速檢測目標的特性，將檢測到的運動物體上的特征點剔除掉，再結合室內結構化平面特性，應對可能存在的低紋理、光線較暗等情況。FPM-SLAM算法整體流程如圖1所示，圖2是FPM-SLAM算法的可視化示意圖。FPM-SLAM算法在獲得RGB圖像和深度圖像后，同時進行跟蹤、可移動物體檢測、平面匹配模塊處理，主要步驟如下：

圖1 FPM-SLAM算法整體流程

圖2 FPM-SLAM算法流程可視化

(1)RGB圖像傳入目標檢測模塊，Yolo-Fastest算法檢測到感興趣區域(region of interest，ROI)，并將結果傳入跟蹤模塊。

(2)深度圖傳入平面匹配模塊，從深度圖像中恢復點云地圖，利用CAPE[16]方法完成平面提取，再利用幾何約束方法進行平面匹配并將結果傳入跟蹤模塊。

(3)跟蹤模塊首先提取ORB特征點，然后完成軌跡地圖初始化。跟蹤模塊等待可移動物體檢測模塊傳入的結果，剔除位于ROI上的ORB特征點，然后將關鍵幀傳入局部建圖模塊和稠密建圖模塊。

(4)跟蹤模塊將關鍵幀傳入稠密建圖模塊，重建三維點云，通過統計濾波剔除離群點，體素格濾波降采樣，最后保存生成的稠密三維點云地圖。

(5)局部建圖為了增加局部地圖點數目，關鍵幀之間會重新進行特征匹配，生成新的地圖點；局部BA優化共視圖里的關鍵幀位姿和地圖點，刪除不準確的地圖點和冗余關鍵幀。

(6)閉環模塊通過詞袋法檢測是否閉環，計算當前關鍵幀和閉環候選關鍵幀之間的位姿，進行閉環矯正。

(7)最后全局BA模塊優化所有的關鍵幀及其地圖點。

當機器人在戶外工作時，FPM-SLAM算法只運行①②④⑥⑦模塊。

1.2 目標檢測算法

本文算法框架中的可移動物體檢測模塊采用了Yolo-Fastest[14]模型。Yolo-Fastest提出的超輕量級網絡模型，由于搭配了移動端輕量級神經網絡EfficientNet-lite[15]，該模型在運行性能方面表現很好，訓練好的模型只有1.3 MB。圖3和圖4分別是EfficientNet-lite與其它網絡模型的預測速度對比和模型體積對比，帶五角星的線表示與之對比的骨干網絡，從圖中可以看出EfficientNet-lite網絡模型體積小而且預測速度快，使用的參數和計算數量非常低，計算量僅為250 mflops，在移動終端可以達到178 fps。本文采用Yolo-Fastest訓練Pascal VOC 2014數據集，該數據集有20個類別，訓練環境：win11操作系統，Intel i5 CPU，內存16 GB，RTX3060，顯存12 GB；參數設置100 000次迭代，Batch為64。最終將訓練模型融合到FPM-SLAM算法中，既滿足檢測精度的要求，也能夠完成對動態場景下的實時性檢測。

圖3 EfficientNet-Lite與其它網絡模型的預測速度對比

圖4 EfficientNet-Lite與其它網絡模型體積對比

1.3 平面匹配算法

本文提出了一個同時使用點和平面的視覺SLAM算法來實現室內環境下機器人相機位姿的估計。室內環境中有許多平行和垂直的平面結構(如桌面、地面、墻面等)，使用這些結構約束可以幫助實現平面的長期關聯，使得累積誤差更小，解決低紋理環境中定位下降問題。采集圖像中普遍存在平面特征，與點特征相比，平面特征具有受測量噪聲影響小、容易提取、計算量低等優點。圓柱體和平面提取(cylinder and plane extraction，CAPE)是Proen?a等[16]提出的一種快速三維點云平面和圓柱體提取方法，利用分割算法從稠密三維重建點云提取平面和圓柱體。本文采用CAPE方法只對平面特征進行快速提取并匹配，與現有的方法相比，該方法更快且平面分割結果更具有一致性，平面生成匹配方法見表1。首先將輸入的深度圖像轉換為三維點云，然后將點云分割成平面網格，通過主成分分析(principal component analysis，PCA)來計算網格的參數，再利用平面的法向量進行直方圖的統計，從而完成每個網格的平面分割，最終采用平面匹配從候選平面中計算出最佳配對平面。

表1 平面生成匹配方法

平面匹配階段，本算法利用RGB-D相機，從深度圖像中生成點云來提取平面。RGB-D相機提供RGB圖像和對齊的深度圖，深度圖保存了每個像素的距離信息，所以本文使用針孔相機模型(式(1))恢復點云，其中 (X，Y，Z)T是三維空間中的坐標點，K是內參矩陣，Z是深度值，P點在像素平面的像素坐標為u=(u，v)T，(cx，cy) 表示像素坐標系下的中心坐標，fx和fy是相機的焦距

(1)

由深度圖像生成的點云被組織起來，具有類似網格的圖像結構，有組織的結構可以從點云中快速分割平面，最終利用局部地圖中的所有點和平面對當前位姿進行了優化。其次增加幾何約束，給定兩個Hessian平面方程A{N，d} 和B{N′，d′}， 圖5給出了CAPE算法中兩個平面匹配的幾何表示，平面匹配需要滿足以下3個條件：

圖5 CAPE算法中兩個平面的幾何表示

(1)平面A和平面B法向量之間的夾角arccos(N·N′) 小于一定的角度θ；

(2)平面A和平面B各自原點到平面距離之間的差值 |d-d′| 小于一定閾值?；

為了從候選平面之中選擇最佳的平面，本文選擇產生最小平面間距離的候選平面，則兩個平面A和平面B之間的距離如式(2)所示，其中C和C′分別是平面A和平面B上的點

(2)

1.4 稠密三維點云重建

本文算法可在動態環境下輸出稠密三維點云地圖，用于機器人導航與避障等高級應用。三維稠密點云建圖從跟蹤模塊中獲得新的關鍵幀，利用關鍵幀的變換矩陣和深度圖像來生成局部點云，具體流程如圖6所示。

圖6 稠密建圖流程

(1)讀取一幀RGB圖像和深度圖像，根據相機的內參矩陣估計相機位姿，同時跟蹤模塊會提取ORB特征點，并判斷當前幀是否為關鍵幀，將得到的所有關鍵幀的深度圖像轉化為3D點云。

(2)統計濾波去除離群點，因為離群點的存在讓局部點云特征的估計變得困難，可能導致點云配準失敗，所以統計濾波會對每個點進行統計，計算每一點到其鄰域所有點的平均距離，剔除大于平均距離的鄰域點。如果得到的是高斯分布，可以計算出均值μ和標準差σ，標準差的倍數設置th=1， 對每個點的鄰域點的個數設置為50，那么去除鄰域距離大于μ+th*σ區間之外的點。

(3)體素網絡濾波器降采樣，由于多視角生成的點云存在重疊部分，這會占用大量存儲空間。在體素濾波器中，用體素的重心來近似替代體素中其它點，相當于降采樣。本文把分辨率設成0.01，表示只存儲0.01×0.01×0.01格子中的一個點，體素中的大量點云被刪除，僅保留體素重心的一點，因此點云存儲空間大幅減少。

(4)進行點云拼接，得到由三維點組成的稠密點云地圖。點云的創建需要提取圖像中具有深度信息的所有像素，如果是帶顏色通道的點云，則需要存儲每個點云對應的RGB值。點云拼接的作用是將連續幀的點云拼接在一起，從而創建出整個環境的點云地圖。在視覺SLAM算法中，通常將第一次出現的關鍵幀設置為世界坐標系，后續生成的關鍵幀需要以首幀為參考，將對應的點云轉換到世界坐標系下，與之前的點云進行拼接，從而形成完整的點云地圖。設點Pc=(Xc，Yc，Zc)T是相機坐標系下的坐標，點Pw=(Xw，Yw，Zw)T為世界坐標系下的坐標，Tcw為相機坐標系到世界坐標系的變換矩陣，R和t分別為旋轉和平移矩陣，具體公式如下

(3)

(5)如果還有數據幀輸入，則繼續(1)操作；否則保存稠密三維點云數據并結束。

2 實驗結果及分析

本文在戶外自動駕駛公開數據集KITTI[17]和室內機器人數據集TUM[18]上對FPM-SLAM算法進行評估，并與ORB-SLAM2、DynaSLAM、DS-SLAM算法的實驗結果進行比較，由于跟蹤結果需要在單目中對齊和縮放，本文使用絕對軌跡誤差(absolute trajectory error，ATE)的均方根誤差(root mean square error，RMSE)來評估該算法在動態場景中的定位精度。均方根誤差描述相機估計位姿與真值位姿之間的差值，RMSE值越小表示誤差越小。假設相機估計軌跡為Testi，i， 真實軌跡Tgt，i， 其中i=1，…，N， 那么相機的絕對軌跡誤差的均方根誤差計算公式如下

(4)

2.1 在TUM RGB-D數據集上的評估

TUM RGB-D數據集[18]包含了微軟Kinect傳感器在室內環境下采集的RGB和深度圖像。數據記錄了速率(30 Hz)的640×480分辨率圖像，真值軌跡是由具有8個高速跟蹤攝像機(100 Hz)的高精度運動捕捉系統獲得。在“坐著”的序列中，兩個人坐在一張桌子前聊天，做著手勢，運動幅度很小。在“行走”序列中，兩個人繞著鏡頭走，然后坐在一張桌子前。由于該數據集一些序列是高度動態的，它將影響傳統視覺SLAM算法的準確性。這些序列可分為以下幾種相機運動類型：

(1)halfsphere表示相機沿半徑為0.5 m的半球面軌跡移動。

(2)xyz表示相機沿xyz軸移動。

(3)rpy表示相機在俯仰角、偏航角、滾轉角上旋轉。

(4)static表示相機保持靜止不動。

FPM-SLAM算法在TUM RGB-D數據集上運行部分序列(fr3_walking_rpy、fr3_walking_xyz)實驗結果如圖7、圖8所示。圖7(a)、圖8(a)顯示了FPM-SLAM算法提取ORB特征點的效果，輸入一張RGB圖像到跟蹤模塊中，當算法檢測到可移動物體(比如圖像中的人)時，用矩形框標記并剔除掉可移動物體上的ORB特征點，再對圖像完成后續工作。在圖7(b)光線較暗的環境中，算法提取的ORB特征點較少，容易導致跟蹤丟失，因此將深度圖輸入到平面匹配模塊，從深度圖中恢復點云并分割成多個平面(對應圖7(b)地面匹配的兩個平面)，使用平面匹配算法解決這一問題。在圖8(b)中，辦公桌上有明顯的平面特征，算法進行ORB特征點匹配時，也進行平面匹配(對應圖8(b)桌面匹配的兩個平面)。圖7(c)、圖8(c)顯示了估計位姿和真實位姿的誤差軌跡圖，圖中虛線部分表示真值軌跡，實線部分表示相機估計位姿軌跡，柱線條越靠近下端，表示軌跡誤差越小，越靠近柱線條上端，表示絕對軌跡誤差越大。因為本文的算法性能在這兩個序列中表現良好，所以是實線呈現，虛線的真值軌跡被實線覆蓋。

圖7 fr3_walking_rpy序列效果

圖8 fr3_walking_xyz序列效果

表2顯示了在TUM RGB-D數據集的6個序列中，FPM-SLAM分別與幾種經典SLAM算法ORB-SLAM2、DynaSLAM、DS-SLAM的絕對軌跡均方根誤差對比結果。在表2中，前4個序列是高度動態的環境(有人在走動)，后兩個序列是低動態的環境(有人坐著打手勢)。總之，本文所提算法在TUM RGB-D數據集中的結果與DynaSLAM和DS-SLAM相似。當在高動態環境中消除動態對象(如行人)上的特征點時，可以提高定位精度(如fr3_w_halfsphere、fr3_w_xyz、fr3_w_pry、fr3_w_static序列)。但在低動態環境下(如fr3_s_xyz、fr3_s_halfsphere)，FPM-SLAM算法的軌跡誤差高于DynaSLAM算法，這是因為在去除位于可移動物體區域的特征點后，剩余的特征點大部分位于較遠區域，導致定位精度下降。

表2 TUM數據集絕對軌跡誤差的RMSE對比/m

FPM-SLAM算法在TUM RGB-D數據集上運行的部分序列(fr3_walking_xyz、fr3_walking_halfsphere)建圖效果如圖9、圖10所示。圖9(a)、圖10(a)顯示未剔除移動物體的建圖效果，由于人的走動給建圖帶來了不便，重建的點云圖中有許多重影；圖9(b)、圖10(b)顯示了剔除移動物體后的建圖效果，重影消失，重建效果較好。

圖9 fr3_walking_xyz建圖效果

圖10 fr3_walking_halfsphere建圖效果

2.2 在KITTI數據集的實驗

KITTI數據集[17]是由德國卡爾斯魯厄理工學院和美國豐田理工學院聯合采集，用于評估自動駕駛場景中的計算機視覺算法。采集車上安裝了兩臺高分辨率的彩色和灰度相機，RTK-GPS提供了精確的運動軌跡真實值。數據集包含了中等城市街道、鄉村道路和高速公路上的單目、雙目相機、激光雷達、GPS等多種傳感器數據，提供了11個序列帶有真值軌跡進行測試評估。

FPM-SLAM算法在KITTI數據集的實驗結果如圖11、圖12所示。圖11(a)、圖12(a)顯示了FPM-SLAM算法提取ORB特征點的效果，輸入一張RGB圖像到跟蹤模塊中，當算法檢測到可移動物體(比如圖像中的車)時，用矩形框標記并剔除掉可移動物體上的ORB特征點，再對圖像完成后續工作。圖11(b)、圖12(b)顯示了具有多個閉環的城市環境軌跡和稀疏重建。圖11(c)、圖12(c)顯示了估計位姿和真實位姿的誤差軌跡圖，采用KITTI00和KITTI05序列對定位軌跡的精度進行評估。

圖11 KITTI_00序列效果

圖12 KITTI_05序列效果

表3顯示了3種算法在KITTI數據集的11個序列中軌跡精度的比較。從表中可以看出，本文的算法在某些情況下的結果與ORB-SLAM2和DynaSLAM相似。當場景中有許多動態物體時(如移動的汽車、自行車和行人等)，該算法可以有效地提高SLAM(如KITTI03、KITTI04、KITTI09)的定位精度。但當大多數物體處于靜止狀態，FPM-SLAM比DynaSLAM有更大的誤差，因為這些序列(如KITTI00和KITTI02)包含許多近距離的靜止車輛，大多數ORB特征點在較遠地區，當剔除車輛上的特征點，這可能導致定位精度下降。

表3 KITTI數據集單目絕對軌跡誤差的RMSE對比/m

2.3 算法實時性評估

為測試算法的實時性，本文分別選取TUM數據集“高動態”3個序列，“低動態”兩個序列。測試圖像分辨率設置為640×480，由于SLAM算法的跟蹤模塊對圖像實時性要求高，所以本文只對比跟蹤模塊所耗費時間。本實驗的環境采用Intel i5 CPU，內存16 GB進行測試。

表4中DynaSLAM算法在測試序列的跟蹤模塊算法平均耗時2.667 s；本文提出的FPM-SLAM算法平均耗時29 ms。在fr3_s_halfsphere序列中，FPM-SLAM算法在跟蹤模塊中耗時0.026 ms，算法整體運行速率22.20 fps；DynaSLAM算法耗時2.138 s，算法整體運行速率0.48 fps。因此，本文算法較DynaSLAM算法有明顯提升，滿足室內環境實時跟蹤的需求。

表4 跟蹤模塊耗時對比/s

3 結束語

本文提出FPM-SLAM算法，結合點和面特征及深度學習，提高視覺SLAM在低紋理和動態復雜環境下的精確性和魯棒性。但在光照條件差的環境下，算法的魯棒性有待提升，后續將引入IMU傳感器提升算法的定位精度；在稠密三維地圖重建部分，即使經過統計學濾波、體素格濾波后，最后得到的地圖文件仍然很大，后續將轉換為八叉樹地圖減少存儲空間。本文主要創新點如下：

(1)將輕量級目標檢測算法Yolo-Fastest融入所提出的算法中，在戶外KITTI數據集上的實驗中，與ORB-SLAM2相比，本文的方法絕對軌跡誤差RMSE指數顯著降低66.67%；

(2)將平面特征匹配方法融入到所提出的算法中，在室內TUM數據集上實驗，本文的方法在大多數測試序列中效果比DS-SLAM和DynaSLAM要好，與ORB-SLAM2算法比較，絕對軌跡誤差RMSE指數顯著降低98.77%，而且運行速度得到了有效提升，跟蹤模塊平均耗時僅29 ms；

(3)將稠密建圖方法融入到所提算法中，剔除掉動態物體后生成稠密三維點云地圖，可用于機器人后續執行導航、避障和環境理解等高級應用任務。