一種點線特征融合的雙目同時定位與地圖構(gòu)建方法

蔣 林， 夏旭洪， 韓 璐， 邱存勇， 張 泰， 宋 杰

(1.西南石油大學電氣信息學院，成都 610500；2.中國石油華北油田通信有限公司，任丘 062550)

同時定位與地圖構(gòu)建技術(simultaneous localization and mapping，SLAM)最早起源于機器人領域，是移動機器人實現(xiàn)自主導航和定位的核心技術之一。如今更是被用于增強現(xiàn)實/虛擬現(xiàn)實(AR/VR)、無人機、自動駕駛等領域。SLAM技術主要用來解決兩個核心問題：一是構(gòu)建未知環(huán)境的模型，二是對機器人進行定位[1]。近年來,隨著計算機視覺的發(fā)展和視覺傳感器的廣泛應用，SLAM問題的解決方案也越來越多樣化。陳超等[2]對基于Kinect視覺傳感器和激光傳感器信息融合的移動機器人進行了研究。將Kinect視覺傳感器獲取的深度圖像轉(zhuǎn)換為三維點云，通過過濾點云、投影及轉(zhuǎn)化為激光掃描數(shù)據(jù)等操作，與激光傳感器的掃描數(shù)據(jù)進行數(shù)據(jù)級的信息融合，并使用Gmapping算法進行二維地圖構(gòu)建，但該方法只能構(gòu)建二維環(huán)境地圖，且激光傳感器價格昂貴。

目前，主流的純視覺SLAM方法大致可以分為三類[3]:基于濾波器、基于關鍵幀優(yōu)化和基于直接跟蹤法。其中，基于關鍵幀優(yōu)化的視覺SLAM通常需要進行特征點的提取與匹配，根據(jù)特征點的匹配關系計算相機運動，最后運行集束調(diào)整(bundle adjustment， BA)對相機位姿和三維路標進行優(yōu)化，因此對環(huán)境特征的豐富程度和圖像質(zhì)量十分敏感。然而，在低紋理環(huán)境中通常很難找到足夠數(shù)量的可靠點特征，導致跟蹤失敗。在人們生活環(huán)境中的建筑物、街道、走廊、辦公室等都具有明顯的線條特征,因此基于線特征的視覺SLAM算法逐漸受到關注。Zuo等[4]在ORB-SLAM2的基礎上設計了一種點線綜合特征的SLAM方法，并推導了后端優(yōu)化過程中殘差的雅可比矩陣。Lu等[5]設計了一種融合點和線特征的RGB-D視覺里程計，該算法從RGB-D數(shù)據(jù)中提取3D點和線。還證明了融合這兩種類型的特征在運動估計中產(chǎn)生的不確定性小于在其工作中單獨使用任何特征類型。但該方法不具備SLAM中重要的回環(huán)檢測與校正環(huán)節(jié)，不是一個完整的SLAM系統(tǒng)。

為此，設計一種基于點、線特征融合的雙目SLAM方法。采用基于關鍵幀優(yōu)化的框架，改進了跟蹤線程，并利用點線特征共同構(gòu)建環(huán)境地圖。從而改善定位精度提升魯棒性。基于特征提取的SLAM算法中，一般特征提取和跟蹤是順序執(zhí)行的，即完成特征提取后依次進行匹配、跟蹤、位姿估計。特征提取會耗費較多時間，從而影響算法實時性。分離出特征提取線程，即對當前幀進行運動估計的同時特征提取線程繼續(xù)進行下一幀的特征提取。最終實驗表明，本文方法具備較好的實時性。

1 特征提取與運動估計

視覺SLAM的前端，主要完成特征的提取與匹配，并實時進行位姿求解，得到粗略的運動軌跡。

1.1 特征提取與匹配

選用ORB(oriented FAST and rotated BRIEF)算子來提取特征點，該算子具有良好的旋轉(zhuǎn)不變性、尺度不變性和抗噪能力，其描述符的二進制性質(zhì)可以實現(xiàn)快速、高效的特征匹配。為了確保特征點的均勻分布，對圖像進行分塊，根據(jù)小區(qū)域圖像的紋理豐富程度設置不同閾值，分別提取ORB特征。進行雙目匹配時為了盡可能的剔除局外點，采用如下策略。①左圖與右圖對應點互為最佳匹配;②匹配點的描述子距離必須小于設定閾值;③最優(yōu)匹配與次優(yōu)匹配的描述子距離比小于0.5。

線特征的提取算法采用LSD(line segment detector)算法[6]并計算了每條線段的LBD(line band discriptor)[7]描述子。LSD算法提取速度快、不需要調(diào)整額外的參數(shù)，并且每條線段都具有主方向，在進行線段匹配時可以排除一些誤匹配對。在雙目匹配與幀間匹配過程中，對線段特征采取類似點特征的處理方式以確保線段匹配的有效性。

1.2 空間直線參數(shù)化

空間直線具有四個自由度，不像空間點可以很容易利用四維齊次坐標表示，使用過參數(shù)化的表示方法會帶來多余的自由度及約束條件。設空間直線經(jīng)過點X1=(x1,y1,z1,r1)和X2=(x2,y2,z2,r2)，在如圖1所示的Plücker坐標表示下，空間直線L可以用一個六維向量來描述：

(1)

(2)

式(2)中：Rcw和tcw分別表示坐標系的旋轉(zhuǎn)和平移變換;Lw和Lc分別為直線在世界坐標系與相機坐標系下的坐標表示；nc和vc為相機坐標系下構(gòu)成Plücker坐標的兩個向量。

圖1 Plücker坐標表示Fig.1 The Plücker line coordinates

由于Plücker坐標表示方式存在約束v⊥n，不利于后端的非線性優(yōu)化過程，若直接進行優(yōu)化，則在優(yōu)化過后無法保證該約束條件。Bartoli等[8]提出了一種最小參數(shù)正交表示方法，通過對Plücker坐標的正交三角分解(QR分解)得到:

(3)

1.3 運動估計

SLAM系統(tǒng)中，根據(jù)參考幀與當前幀之間的特征匹配關系，最小化特征的重投影誤差來求解相機的運動。記由參考幀到當前幀的位姿變換矩陣為T，對應的李代數(shù)為ξ，則特征點的重投影誤差可描述為

(4)

(5)

此時參考幀到當前幀的位姿變換ξ*可由ξ的最大似然估計給出。假設所有的觀測數(shù)據(jù)均受0均值的高斯噪聲所影響，此時對ξ的最大似然估計ξ*等價于求解以下的最小二乘問題。

(6)

式(6)中：Ni、Nj分別代表參考幀到當前幀的點和線特征的匹配數(shù)量;ΩΔpi、ΩΔlj分別表示點和線特征觀測的協(xié)方差矩陣;ρ為魯棒核函數(shù)。重投影誤差Δp和Δl相對于位姿增量δξ的雅可比矩陣如式(7)、式(8)所示[4]:

(7)

(8)

式中：pc和lc分別為點特征與線特征在像素坐標系下的表示;Lc為線特征在相機坐標系下的Plücker坐標表示。

為了盡可能的剔除匹配異常值對位姿估計的影響，迭代結(jié)束后，將誤差大于閾值σ的全部局外點剔除，對所有剩下的內(nèi)點重新進行位姿估計，得到的結(jié)果作為運動估計的最終結(jié)果。

運動估計完成后采用文獻[9]的方法選取關鍵幀。記上一關鍵幀為i，當前幀為i+u，計算α:

(9)

其中，

h(ξ)=3[1+ln(2π)+0.5ln|Ωξ|]

(10)

式中：Ωξ為運動估計的協(xié)方差矩陣，當α>0.9時將當前幀選取為關鍵幀。

2 地圖構(gòu)建與優(yōu)化

利用視覺里程計進行運動估計時僅估計了兩個相鄰時間上的幀間運動，沒有考慮與其他關鍵幀之間的聯(lián)系，其誤差較大且存在累計誤差。對局部地圖進行優(yōu)化可以減小誤差，檢測到閉環(huán)后可以用閉環(huán)約束來降低系統(tǒng)的累計誤差。

2.1 局部地圖構(gòu)建與優(yōu)化

局部地圖包括當前關鍵幀和共視圖中與當前關鍵幀直接相連的關鍵幀，以及這些關鍵幀所觀測到的三維點和線的集合。插入新的關鍵幀后，運行集束調(diào)整對局部地圖進行優(yōu)化。記ψ為所有需要被優(yōu)化的變量，包含相機位姿，特征點以及空間直線的三維坐標。通過最小化關鍵幀和路標的重投影誤差，來提煉最優(yōu)的相機位姿和三維路標信息，即：

(11)

式(11)中：Kl為局部地圖中關鍵幀的集合；Pl為局部地圖中三維路標點的集合；Ll為局部地圖中空間線段的集合;eij為第j個路標點在第i幀的投影誤差;eik為第k條空間線段在第i幀的投影誤差;ρ為魯棒核函數(shù)。

其中誤差項關于位姿的雅克比矩陣同式(7)、式(8)，點特征重投影誤差關于3維世界坐標pw的雅可比矩陣為

(12)

線特征重投影誤差關于空間直線正交增量δθ的雅可比矩陣為

(13)

式(13)中:Lw為線特征在世界坐標系下的Plücker坐標表示。

BA優(yōu)化的復雜度將隨關鍵幀數(shù)目增加而增大，從而導致運算時間加長，占用內(nèi)存更大。因此，如果某一關鍵幀的所有特征的90%能夠被不少于3個關鍵幀所觀測到，那么將該關鍵幀從地圖中刪除。同時，如果地圖中的某一特征被少于3個關鍵幀所觀測到，則認為該特征不夠穩(wěn)定，也將刪除該特征。

2.2 閉環(huán)檢測

當插入新的關鍵幀后，分別利用點、線視覺詞典[10]將其轉(zhuǎn)換為詞包向量vp、vl。利用詞包向量在地圖中搜索與之具有相同詞匯的所有關鍵幀{Ki}，并分別計算點、線的相似性分數(shù){spi,sli}，最后對分數(shù)進行加權(quán)得到最終的相似性評分：

si=λspi+(1-λ)sli

(14)

采取ORB-SLAM2[11]相似的策略尋找閉環(huán)關鍵幀。成功檢測到閉環(huán)后，則對當前關鍵幀和閉環(huán)幀進行特征匹配，并求解當前幀與閉環(huán)幀之間的位姿變換。

2.3 位姿圖的建立與優(yōu)化

通常地圖中點線特征的數(shù)量要遠大于關鍵幀的數(shù)量，因此同時對所有的路標和位姿進行BA優(yōu)化是相當耗時的。而位姿圖只考慮位姿信息，可以在極短的時間內(nèi)完成位姿優(yōu)化。系統(tǒng)閉環(huán)校正前后的對比如圖2所示。

圖2 有無閉環(huán)校正的對比實驗效果Fig.2 Comparison of experimental results with or without closed-loop correction

位姿圖中將關鍵幀位姿作為頂點，關鍵幀之間相對位姿變換ΔTij(對應的李代數(shù)為ξij)作為邊。頂點用李群表示為T1,T2,…,Tn，對應的李代數(shù)表示為ξ1,ξ2,…,ξn。兩個頂點之間相連的邊用李群表示為

(15)

式(15)對應李代數(shù)的表達為

(16)

由于誤差的存在，兩個節(jié)點之間相對運動的估計值與真實值的誤差eij可以定義為

(17)

記K為所有位姿節(jié)點的集合，則總體目標函數(shù)為

(18)

通過求解式(18)可以對整個運動過程中的相機位姿進行全局優(yōu)化，有效降低系統(tǒng)的累積誤差。

3 實驗結(jié)果及分析

為了驗證所提方法的有效性，首先在KITTI[12]數(shù)據(jù)集上進行實驗，該數(shù)據(jù)集通過自主駕駛車搭載多種傳感器進行采集，包括64線激光雷達、GPS(global positioning system)/IMU(inertial measurement unit)組合導航系統(tǒng)以及兩個灰度相機和兩個彩色相機，高精度的組合導航系統(tǒng)可以作為系統(tǒng)的真值。

與ORB-SLAM2的雙目算法對比實驗如圖3所示，其中所用計算機CPU為Inter Core i7，主頻2.2 GHz，內(nèi)存為8 G。

圖3 部分序列估計軌跡對比Fig.3 Comparison of partial sequence estimation trajectories

將本文方法與ORB-SLAM2的雙目算法關鍵幀位姿之間的相對誤差(RMSE)[13]進行對比分析，在KITTI數(shù)據(jù)集下不同序列的相對平移誤差(trel/m)和相對旋轉(zhuǎn)誤差(Rrel/°)，如表1所示。

表1 算法在KITTI數(shù)據(jù)集上的誤差比較Table 1 Comparison of errors of algorithms on KITTI datasets

續(xù)表1

序列ORB-SLAM2本文方法平移誤差/m旋轉(zhuǎn)誤差/(°)平移誤差/m旋轉(zhuǎn)誤差/(°)071.150.531.350.69087.421.357.891.77096.881.149.581.25102.900.847.601.54

圖3(a)、圖3(c)、圖3(d)、圖3(e)均包含了不同數(shù)目的閉環(huán)，而圖3(b)、圖3(f)則為不包含閉環(huán)。其中數(shù)據(jù)集中序列00、05[圖3(a)、圖3(c)]包含閉環(huán)數(shù)目較多，具有大量閉環(huán)約束因而誤差也較??；序列07、09[圖3(d)、圖3(e)]只包含一個閉環(huán)，序列07的相對平移誤差為1.35 m，序列09的相對平移誤差為9.58 m。然而，序列03、10[圖3(b)、圖3(f)]不包含閉環(huán)，其相對平移誤差也僅為1.38、7.60 m。由圖3可知，無論是在不包含閉環(huán)、包含少量閉環(huán)以及包含較多閉環(huán)的情況下，本文方法均能較好的完成定位與建圖的任務。能獲得更豐富的環(huán)境信息，在部分數(shù)據(jù)集上定位精度有所提升，具備較高可靠性。最后評估相同數(shù)據(jù)集下系統(tǒng)各線程的處理時間，實驗結(jié)果如圖4～圖6所示。

圖4 KITTI00序列跟蹤線程時間對比Fig.4 Time comparison of the KITTI00 sequence tracking thread

由圖4可知，在KITTI數(shù)據(jù)集00序列的測試中ORB-SLAM2方法的跟蹤線程平均每幀約為110 ms，本文算法在分離特征提取線程后跟蹤線程平均每幀為78 ms。

圖5 KITTI00序列局部建圖線程時間對比Fig.5 Time comparison of the KITTI00 sequence local mapping thread

圖5為局部建圖線程時間對比。由圖5可知，本文算法在局部建圖線程過程中的動態(tài)響應更快，較ORB-SLAM2方法的響應時間短130 ms左右。

圖6 KITTI00序列閉環(huán)線程時間對比Fig.6 Time comparison of the KITTI00 sequence closed loop thread

圖6為閉環(huán)線程時間對比，由于閉環(huán)檢測過程中加入了線段特征，所以閉環(huán)線程時間明顯高于ORB-SLAM2，但閉環(huán)檢測幾乎不影響前端線程的實時性。

圖7 關鍵幀數(shù)量對比Fig.7 Number comparison of key frames

圖7為部分數(shù)據(jù)集下關鍵幀數(shù)量對比。本文方法關鍵幀數(shù)量約為ORB-SLAM2的70%左右，在關鍵幀存儲上占用的內(nèi)存更少。

實驗結(jié)果表明，雙目SLAM算法能夠成功完成SLAM任務，該方法在保證定位精度的同時能夠得到更為豐富的環(huán)境地圖信息，通過改進跟蹤線程，提升了系統(tǒng)的幀率，保證了算法的實時性。

4 結(jié)論

提出一種點線特征融合的雙目SLAM算法，算法采用點和線段兩種特征共同進行位姿估計，并構(gòu)建點線地圖模型。通過從跟蹤線程中分離特征提取線程，提高了系統(tǒng)的幀率?；贙ITTI數(shù)據(jù)集實驗結(jié)果驗證了該算法滿足實時性要求，具備較強的魯棒性，并能夠較準確地進行定位與構(gòu)圖。后續(xù)研究中希望引入IMU傳感器，利用IMU數(shù)據(jù)與相機數(shù)據(jù)進行融合，進一步提升系統(tǒng)魯棒性，使系統(tǒng)適應更多復雜環(huán)境。