基于視覺傳感器的半直接SLAM算法*

彭 虎, 董 翔, 李 騰, 樊 淵

(安徽大學 電氣工程與自動化學院,安徽 合肥 230601)

0 引 言

當RGB-D相機發布后,許多研究開始專注于研究以RGB-D相機作為傳感器的視覺同時定位和地圖創建(vision simultneous localization and mapping,VSLAM)系統[1]。目前VSLAM方法主要分為兩種:特征點法和直接法[2]。在基于特征點法的VSLAM中,Mur-Artal R等人[3]使用圖形優化技術開發了ORB-SLAM2,系統圍繞著ORB特征點進行優化計算,同時提升了效率和精度;但它計算量大,而且在特征點缺失的區域,建圖效果不理想。相比于特征點法,直接法對特征點缺失區域,可以保證追蹤的穩定性和實時性。Sch?ps T等人[4]通過對像素點對之間的基線立體配準提出了BAD SLAM,它是針對像素進行的,在CPU上實現了半稠密點云地圖的構建;但和其他直接法一樣，其對相機內參和曝光比較敏感,而且在相機快速運動時容易丟失。

針對以上問題,本文提出了一種快速的半直接SLAM(fast semi-direct SLAM,FSD-SLAM)算法。利用基于子圖的特征增強模塊和改進的相機位姿估計方法,使得系統能夠在無結構和特征缺失等特殊場景中實現精確、穩定的相機運動估計。再使用增量式動態協方差縮放算法優化相機位姿,提高了優化的效率和實時性,并將優化后的相機位姿提供給地圖構建程序,獲得最終的三維重建地圖。

1 FSD-SLAM系統框架

1.1 初始化與特征增強

FSD-SLAM系統框架如圖1所示。當視頻幀輸入時,計算相鄰兩幀特征并進行匹配。當匹配到足夠多數量的特征時,求解與特征關聯的像素點深度和像素點的三維坐標,完成初始化。

圖1 系統的總體框架

提取針對運動模糊和無紋理環境的關鍵點對于VSLAM系統起著重要作用。基于子圖的特征增強模塊可以對FAST關鍵點[5]進行特征增強,根據機器人測距的可靠性,通過對三維特征點的增強生成子圖,方便系統穩定地提取圖像特征點。系統完成初始化后,將視頻幀添加到一個固定長度的視頻時間軸Qt隊列中,子圖的特征增強模塊從Qt頭部提取RGB輸入幀,并將處理之后具有特征點增強的幀更新回隊列當中,得到具有FAST特征點積累的RGB幀。圖2顯示了這個模塊在不充分的特征環境中如何工作。第i個子圖中特征R的增強三維點li表示如下

(1)

圖2 在不充足特征的環境下通過增強三維特征點實現子地圖的概念

1.2 相機位姿估計

在特征點法中的重投影誤差基礎上加權融合了匹配特征點對的光度誤差,通過優化求解這個誤差方程來計算相機位姿。基于特征點法的相機運動估計的方法如圖3所示。

圖3 特征點法相機位姿估計示意

在利用匹配特征點對重投影誤差的相機位姿估計方法中,假設三維空間點P11在連續相鄰兩幀圖像上的二維像素點坐標分別為p11和p13,如果相機位姿估計準確,則真實位置p12和p13為同一個點；如果位姿估計不正確,投影點p13與p12之間必然會存在著一個重投影誤差e。不失一般性,考慮任意一個空間點P1i,e定義如下

(2)

對于全部的特征點,總的重投影誤差方程定義如下

(3)

式中p1i為三維空間坐標點P1i在后一幀上對應的二維像素坐標點；T,K以及Z2i分別為估計的相機位姿、相機內參以及P1i對應相機坐標系下的Z軸坐標；N為特征匹配總數。

另外,通過光度誤差來求解相機的運動估計示意圖如圖4所示。

圖4 光度誤差示意

假定在前一個相機幀中的任意一個像素點p21對應世界坐標系下的空間點為P21,利用相機旋轉和平移到下一幀圖像后對應的像素位置為p22。如果相機位姿估計的不正確,則兩者會存在一個光度誤差。根據以上描述,某個像素的光度誤差可以定義為

(4)

所有對應像素對的誤差函數之和定義為

(5)

式中p2i為前面一個相機幀中的二維像素坐標點；P2i為像素點p2i對應的三維空間點坐標；T,K以及Z2i分別為估計的相機位姿、相機內參以及P2i對應相機坐標系下的Z軸坐標；I1()和I2()為提取前面一個相機幀和后面一個相機幀中對應像素的光度信息；M為像素點總數。

將相互匹配的特征點對的重投影誤差和對應像素的光度誤差進行加權融合,可以求出相機位姿的估計值如下所示

(6)

式中λ為設定的權值系數,它是用來線性地調整相互匹配特征點對的重投影誤差和其對應光度誤差在相機位姿優化中所起的作用。其中,權重λ取經驗值0.5。

1.3 全局優化

隨著輸入圖像的增多,需要去優化閉環檢測的信息以及不同時刻相機位姿估計的誤差。閉環檢測采用基于視覺詞袋[6]的快速檢測方法。具有代表性全局優化算法有SC(switchable constraints)算法[7]。SC算法需要同時求出開關變量、位姿和閉環的聯合優化,所以計算量大而且實時不強。本文提出來的算法是對SC算法的改進。不失一般性考慮兩個節點k和l之間的邊,可以將誤差分為兩個塊,第一個塊考慮除kl之外的所有邊,第二個塊僅考慮邊kl。可以將目標函數重新定義為

(7)

式(7)表明,SLAM問題的解包括節點狀態向量X*和開關變量S*。其中,sij為開關變量,Ξij為開關先驗,xi為時間i處的機器人姿態,zij為通過測距法或傳感器測量獲得的兩個姿態之間的轉換,而f(xi,xj)為預測xi和xj之間的測量值函數。里程表和傳感器測量值的協方差為Σ和Λ。

令e=X*,S*,整理式(7)得

(8)

式中g(·)為不包含邊kl以外所有的誤差,h(·)為包含邊kl的誤差。一旦優化器收斂,則對整個函數中所有變量的偏導數為零。所以對式(8)中的開關變量skl求偏導有

(9)

聯合式(8)和式(9),化簡求解h(·)并對其求導得到

(10)

則易知0≤h≤Φ。此時需要考慮所有節點k和l之間的邊,對于所有的開關變量,都滿足約束0≤h≤Φ。于是有

(11)

由式(11)可以求得

(12)

至此就找到了最優的開關變量s的取值區間。大大縮小了原SC算法給出的開關變量取值區間,減少計算量的同時,增加了全局優化的準確性。

2 實驗結果與分析

2.1 TUM數據集上定量的評估結果

為了驗證本文系統中特征增強模塊的性能,對TUM[8]數據集中fr3_lon-g_office序列的每一幅圖像,分別用F-SD-SLAM和SURF、FAST以及SIF-T三種流行的特征提取算法進行特征提取,計算每一幅圖像的特征提取效率,對整個序列分別取Min、Max、Mean和RMSE,結果如圖5。由實驗結果可以看出,FSD-SLAM可以在短時間里提取更多的特征點,具有更好的特征提取效率。

圖5 與其他方案相比的特征提取效率

對于本文系統優化算法準確性的評估,以優化后的絕對軌跡誤差(absolute tracking of error,ATE)和相對姿態誤差(relative pose error,RPE)[9]為評價標準,選取了TUM數據集中的freiburg1_room序列測試優化的工作效果,并與具有代表性的RGB-D SLAM[10]優化的效果做了對比。測試時軌跡經過優化后的ATE與RPE結果如圖6所示。由實驗結果可以看出,在相同的視頻序列freiburg1_room下FSD-SLAM算法相比于RGB-D SLAM算法優化后得到的ATE和RPE要小的多。

圖6 優化后估計軌跡和地面實況軌跡之間的ATE和相機位姿的RPE

定量的評價此時優化后的ATE,結果如表1所示。

表1 Freiburg1_room序列測試優化后的絕對軌跡誤差 m

定量評價優化后的RPE,分為平移誤差和旋轉誤差,結果分別如表2和表3所示。由表1和表2的結果分析可知,FSD-SLAM優化后的定位精度達到0.472 %,RGB-D SLAM優化后的定位精度為0.341 %,FSD-SLAM優化后的定位精度比RGB-D SLAM優化后的定位精度提高了38個百分點。

表2 Freiburg1_room序列測試優化后的相對位姿平移誤差m

表3 Freiburg1_room序列測試優化后的相對位姿旋轉誤差(°)

進一步驗證本文系統在視覺特征稀缺以及低紋理環境下的魯棒性。其中評估的標準是將系統運行20次,系統跟丟的次數。表4的實驗結果表明,ORB-SLAM2系統在fr3_nostructure_notexture_far、fr3_nostructure_notexture_near、fr3_structure_notexture_far和fr3_structure_notexture_near這些圖像序列中都跟丟了,而FSD-SLAM能很好地跟蹤這些序列。其在大多數無紋理和無結構的環境中,具有很少的跟丟次數。

表4 FSD-SLAM與ORB-SLAM2魯棒性比較

2.2 室內環境中定性的評估結果

真實環境中的實驗平臺為turtlebot2,配備的傳感器為KinectV2。從啟動機器人到結束工作,對應的持續運動時長為48.90 s,以及3.986 m的地面實況軌跡長度,0.432 m/s的平均平移速度和29.882 rad/s平均角速度。測試比較了本文方法和幾個當前具有代表性的SLAM方法在相機位姿估計上的ATE和RPE,這幾個SLAM方法分別為RGB-D SLAM、HOOFR SLAM[11]、MIS-SLAM[12]、BAD SLAM[4]和PL-SLAM[13],結果如表5所示。

表5 在室內環境下FSD-SLAM與其他方法相比較的ATE與RPE m

圖7顯示了系統得到的初始點云圖和全局稠密點云圖。

圖7 系統生成的初始點云圖和全局稠密點云圖

由表5的實驗結果可以看出,本文方法優于了其他實時的SLAM方法,實現了更低的錯誤率。同時由圖7可知,運用本文系統能夠重建室內環境的稠密全局點云地圖。

3 結 論

本文提出的FSD-SLAM方法在初始化與特征增強部分,使用了基于子圖的特征增強模塊,有效地解決了弱紋理環境下特征的提取問題；在相機位姿估計部分,加權融合了圖像特征點匹配對的重投影信息和特征對應像素對的光度信息,并結合增量式動態協方差縮放算法對估計的相機位姿進行全局優化,使得系統在無結構和特征缺失等特殊的場景中仍然能實現精確的相機運動估計和環境地圖重建。TUM數據集和實際室內場景中的測試實驗均表明：FSD-SLAM保持較高的定位精度,并且在無結構和特征缺失等特殊環境下具有較好的魯棒性。