基于ORB-SLAM2的三維占據網格地圖的實時構建

王 飛, 王耀力

(太原理工大學信息與計算機學院,太原 030024)

同時定位與地圖構建(simultaneous localization and mapping,SLAM)。是指搭載特定傳感器主體,在沒有環境先驗信息的情況下,于運動過程中建立環境的模型,同時估計自己的運動[1-2]。按傳感器的不同,SLAM可以分為兩大類,激光SLAM(lidar SLAM)和視覺SLAM(visual SLAM)。激光SLAM理論研究已相對成熟,但由于激光傳感器動輒幾萬到幾十萬的價格,極大地限制了激光SLAM在移動機器人上的應用。因此,搭載視覺傳感器來實現定位和導航的視覺SLAM成為了近年來研究的熱點[3-5]。視覺SLAM使用視覺傳感器來獲得外界圖像信息,進而進行機器人位姿估算與定位。

自從Davision在2007年提出了第一個實時的單目視覺SLAM系統MonoSLAM[6]以來,各種SLAM系統都使用各自的方式對環境地圖實現了實時的構建。目前在V-SLAM 中,較流行的方案有MSCKF[7]、ROVIO[8-9]、ORB-SLAM2[10-11]等,然而以上SLAM系統僅輸出相機運動的軌跡圖及稀疏的點云地圖,但點云地圖形式不緊湊,因此無法為移動機器人提供導航和避障的支持。Santana等[12]在 2011 年介紹了一種基于單目相機使用平面信息(單應矩陣)構建2D地圖的方法,該方法在分割步驟通過判斷圖像特征點是否在同一平面來快速判斷是否有障礙物,但由于單目視覺沒有尺度信息,其所構建的 2D 網格地圖并不適用于機器人的定位和導航。Dia等[13]和Rakotovao等[14]介紹了一種新型的用于占據網格地圖構建的逆傳感器模型,然而并沒有SLAM方面的應用。文獻[15-16]將點云地圖進行投影后實現了二維占用網格地圖(occupy grid map)的構建,但二維地圖對空間中障礙物信息表示不清晰,重疊信息處理不明確。Meagher[17]和Freeman等[18]提出了一種八叉樹的存儲方式,將三維空間中的每個平面一分為二,得到八個大小相同的立方體,每個立方體用0-1中的數表達是否被占用,通過調整分割的次數,可以實現對地圖精度的調整,以這一存儲方式構造的八叉樹地圖是導航領域較為常見的方式[19-20]。但八叉樹的時間復雜度較大,計算代價較高。對于以SLAM為底層支撐的機器人系統來說,并不希望其占用太多的資源。

目前的V-SLAM系統多生成點云地圖,無法為導航和避障提供有效的支持,而現有構建三維網格地圖的方法由于計算代價高、時間復雜度大等缺點,無法實現三維網格地圖的實時構建。因此針對上述不足,利用一種高效的跳表結構來存儲地圖信息,減少計算成本,實現三維網格地圖的實時構建。

1 跳表地圖

自主機器人導航的關鍵是能夠從地圖中獲得足夠豐富的感知信息,從而允許機器人規劃路徑并避開障礙物,其中三維網格地圖在導航中占有重要地位。現提出一種基于SkipList存儲結構的三維地圖模型,并描述關鍵數據結構以及詳細的地圖存儲與查詢方法。

1.1 跳表結構

一般將樹形結構根據地圖坐標對體素進行分組。如圖1所示,根節點根據x的坐標劃分第一層,第一層根據y的坐標劃分第二層,第三層同理使用z坐標劃分。整棵樹的最大深度為3,深度為d3的節點為地圖體素,它可以用于存儲地圖數據,例如占用概率。然而,采用經典的樹結構來實現圖1所示的概念效率并不高,如果每個節點的子節點數無限多,而對每個節點的子節點都用普通鏈表進行儲,則執行隨機訪問將出現O(n)的復雜度,為了提升效率,采用SkipList數據結構[21]進行存儲。

圖1 體素分組示意圖Fig.1 The diagram voxel grouping

圖2 SkipList結構Fig.2 Structure of SkipList

如圖2所示,SkipList在普通鏈表的基礎上,從內部節點向上層拓展為一個新的鏈表,通過幾次延拓,形成一個沿橫向分層、沿縱向相互耦合的多個鏈表。為了查表方便,同層節點都按關鍵碼(key)排序。盡管每一列的節點相同,但這種重復不僅可以加快查找,而且只要策略得當,也不至于造成空間的實質浪費。得益于結構中的這種重復,使得跳表將有序鏈表中O(n)的隨機復雜度降低到O(lgn),因而為提高本文的時間效率提供了理論支持。

1.2 地圖的分組與排布

在存儲方式上,1.1節中對樹形結構進行了分組,相對應的,對整個占據網格地圖也應將其離散為大小相同的體素(voxel),將圖1中所示的概念實際實現為圖3。圖1的d1層用來在x軸上構造SkipList,稱其為xNode,d2層和d3層分別在在y軸和z軸上構造為yNode和zNode。通過三層的Skiplist嵌套實現了將地圖體素用Skiplist表示的目的,并且體素的坐標可以通過迭代其前驅節點來獲得。

SkipList數據結構的每個節點都可以通過key尋址,可以使用它將真實世界坐標映射到量化索引,即3D點P(x,y,z)的體素索引v(Ix,Iy,Iz)為

(1)

式(1)中：r表示地圖的分辨率。根據本文的數據結構,查詢一個體素v=f(Ix,Iy,Iz),相當于執行一個迭代序列h{g[f(Ix),Iy],Iz}=v,結合圖3,定義f()表示檢索xNode,g()表示檢索yNode,h()表示檢索zNode。三個查詢函數f()、g()和h()中的每一個都可以得到匹配和錯誤兩個結果,泛型函數φ[f(Ix)]可以在函數執行之初根據xNode將多個查詢函數定位到不同的SkipList樹的不同分支上,因此每個泛型函數φ[f(Ix)]均可以同時執行,這在很大程度上提升了本文的算法的執行效率。

圖3 SkipList嵌套示意圖Fig.3 Schematic diagram of SkipList nesting

1.3 并行化操作

嵌套的SkipList數據結構為算法提供了高度的并行化操作。此外,即使單個SkipList也可以使用基于鎖定的技術來實現一定程度的并行化,但這種算法通常是不可預測的,因此并不適合在實時的任務中進行使用,在1.2中提出了體素索引的概念,可以操作結構的高并行性,適用于實時任務。以下將從遍歷和更新兩種操作說明本文結構提高運行效率的具體措施。

遍歷操作:遍歷整棵樹(即到達地圖的每個體素)包括并行訪問所有第一層節點,并收集結果。由于網格地圖按照x坐標將不同組的體素分布在不同的第一層SKipList中,因此,可以同時操作不在同一組數據,即可以并行的實現體素的遍歷,該過程可表示為

(2)

更新操作:在執行更新操作時,無法預先知道其是否產生新的分支亦或僅更新現有體素的內容而不執行進一步搜索。只要兩個更新操作屬于兩個單獨的第一級分支,則不會發生沖突,避免了更新操作與其他操作并發的可能。假設傳感器觀測的三維點為C={P(x,y,z)},可以根據它們的第一個量化坐標將這兩點分組在子集中:在確保沒有并發內存訪問的情況下,并行地執行處理每個子集的集成操作。以上過程可表示為

(3)

基于以上兩種操作的并行化不僅有助于改善計算性能,而且有助于分離傳感器可能發生的地圖更新情況從而為本文算法的高效運行提供保障。

2 占用網格地圖模型

確定了網格地圖的存儲方式后,為使機器人能判斷某一位置是否可以通過,還需要將每一體素的占據概率存入圖中,該過程中傳感器根據不同的權重將信息融合入體素信息中,網格占用的概率可以表示為

(4)

W′(v)=W(v)+wi(v)

(5)

式中：P′(v)和P(v)分別表示體素v的新舊占用概率；W′(v)和W(v)表示體素v的新舊權重。通過式(4)和式(5)實現對地圖持續的更新。以下將對占據網格地圖的概率模型進行推導,從而獲得體素的概率P。在機器人位姿已知的情況下,假設網格地圖的概率模型為P(M|Z0:t),其中M為當前的地圖,Z0:t表示傳感器從開始到t時刻的測量值。

在占用網格地圖中,通過數值0、1、0.5 的概率來表示其單元格的狀態,其中0表示空閑,1表示被占用,0.5則表示狀態未知。如果用mi表示地圖中第i個單元格,其狀態可用二進制占用變量表示(占用或空閑)。在上文中已將占據網格地圖按一定的規則離散為不同的體素,而體素個數則由地圖分辨率決定,故每個體素的概率模型為

P(mi|z0:t)

(6)

在柵格地圖中,將一個點是空的概率表示為p(m=0),有障礙物表示為p(m=1),兩者的概率和為1。

(7)

以mi=0為例,0-t時刻的mi的概率為

(8)

對式(8)做Markov假設,可得

P(mi|z0:t-1,m)=P(zt|m)

(9)

P(zt|z0:t-1)=P(zt)

(10)

由于地圖被劃分為一系列體素,因此可將這一假設擴展為

P(mi|z0:t-1,mi)=P(zt|mi)

(11)

將式(9)～式(11)代入式(8)可得

(12)

同理可得m=0的情況:

(13)

為減少不必要的計算,將式(12)與式(13)取比值可得

(14)

進一步變換為

(15)

由于0-1概率的不穩定性,故采用概率對數值log-odds來描述,l為概率對數值,p為0-1的概率,l與p之間由logit變換描述為

(16)

其反變換為

(17)

可以看到,當l從-∞變到+∞時,p相應的從0變到1。而當l取值為0時,取p=0.5。因此存儲l來表示節點是否被占據,當不斷觀察到“占據”時,只需更新l的值即可。當查詢概率時,再用逆logit變換,將l轉換至概率p。

由以上分析,將式(16)帶入式(15)并簡化可得

(18)

(19)

通過式(19)可以看出,當前時刻地圖體素m的狀態為當前的觀測值加上先驗信息。通過這一機制,將體素狀態更新轉換為簡單的加減法,大大提高了地圖更新實現的效率。

3 ORB-SLAM2實現三維占據網格地圖

由Mur-Artal等提出的ORB-SLAM2系統,是PTAM的繼承者中非常有名的一位,它提出于2015年,是現代SLAM系統中非常完善和易用的系統之一,它代表著主流特征點SLAM的一個高峰。

3.1 ORB-SLAM2概述

ORB-SLAM 2是一種基于圖像特征和非線性優化的視覺 SLAM系統。它包括Tracking、LocalMapping和LoopCLose三個線程,并在全部處理進程中使用ORB特征來進行檢測和描述,并輔以DBoW詞袋模型進行回環檢測,在復雜度和準確度上得到了顯著的提高。其主要結構和流程如圖4所示。

圖4 ORB-SALM2流程Fig.4 The flowchart of ORB-SALM2

3.2 三維地圖構建框架

ORB-SLAM2連接Microsoft的Kinect V2相機進行真實場景的實時運行時,需要在ROS(robot operating system)系統中訂閱相機發布的彩色圖像和深度圖像,ORB-SLAM2獲取到相機節點和圖像后,進行關鍵幀和地圖點的提取,計算機器人的姿態,在建圖線程初步生成局部點云地圖,最終經過閉環檢測和全局優化生成全局點云地圖。將ORB-SLAM2生成的關鍵幀、地圖點以及機器人位姿通過ROS進行發布,SkipList Map在獲得信息后對地圖進行分組,并結合tf位姿變換,將信息存入SkipList中,通過高并行的更新和查找操作,實現三維網格地圖模型的構建,具體流程如圖5所示。

圖5 ORB-SLAM2的三維地圖構建框架Fig.5 Framework of 3D map construction of ORB-SLAM2

4 實驗結果與分析

為了驗證本文算法的有效性,首先對跳表地圖的效率進行驗證,其次將構建實驗環境,利用KinectV2在室內環境及走廊環境進行三維占據網格地圖的實時構建實驗。

4.1 效率對比實驗

本文算法以跳表(SkipList)為基礎進行構建,跳表是一種高效的詞典結構,它是基于有序鏈表結構來定義和實現,其對地圖的操作在平均意義下復雜度僅為O(lgn)。以下將在各種條件下,對地圖的插入、刪除和搜索與傳統的Octree進行對比。

實驗使用TUM數據集中具有代表性的rgbd dataset freiburg2 large with loop 和rgbd dataset freiburg3 long office household 兩組數據進行實驗。圖6表示的是新的測量值插入原有地圖進行更新操作時的性能,為了獲得更全面的評估,考慮了0.05、0.1、0.2 m三種不同的體素的分辨率。在更新過程中,地圖分辨率越低,SkipList結構的上層結構所含節點個數越少,故當分辨率變化時所耗費時間也將有較大的改變,而Octree中,分辨率的改變并不能有效減少結構的分支,因此在三種不同的分辨率下Octree執行更新操作時所用的時間基本一致且都大于本文使用的算法。

圖7所示為兩組數據集在三種不同分辨率下對所生成地圖進行遍歷所耗費的時間。由圖7可知,在遍歷整個地圖時,充分使用了1.2節與1.3節提出的高并行性,使整體遍歷時間均比Octree短,且分辨率越高差別越明顯。

圖6 插入時間對比Fig.6 Comparison in the insertion time

圖7 遍歷時間對比Fig.7 Comparison in the traversal time

表1對比了SkipList Map、KD-Tree和Octree在TUM四組數據集中的搜索時間。與圖6、圖7相比,表1額外考慮了Kd-tree,因為它在空間搜索任務中被廣泛采用。括號中的數值為其余兩種算法以本文方法為基準而得到的比值。從表1中可以看出,提出的SkipList Map在實際操作中較Kd-tree時間縮短約40%,較Octree縮短約50%,表明本文的結構能以更短的時間完成搜索操作。

表1 搜索時間對比Table 1 Comparison of search time

以上三組試驗表明,本文的算法在執行插入、更新和搜索時都能以更短的時間完成,與所期結果一致,因而驗證了本文算法具有良好的時間性能。

4.2 實驗平臺的搭建

以輪式機器人為實驗平臺,搭載NVIDIA的Jetson TX2核心板,它是一臺基于NVIDIA PascalTM架構的單模塊超級計算機,性能強大,外形小巧,節能高效,適合機器人等智能終端設備,因其優異的性能,本實驗選擇其作為算法的運行平臺。在TX2上運行的是Ubuntu16系統,配置ROS環境以及一些必要的功能包。本實驗所選相機為Microsoft Kinect V2,它的Depth傳感器,采用的是time of flight(TOF)的方式,通過從投射的紅外線脈沖反射回來的時間來獲得Depth的信息。相機的實際圖像由Color Camera獲得,通過對齊深度信息與彩色信息可以獲得任意點的深度,從而為構建三維地圖提供強有力的數據基礎。圖8所示即為實驗平臺。

圖8 輪式機器人實驗平臺Fig.8 The platform wheeled robot experiment

4.3 建圖實例

將使用4.2節中搭建的實驗平臺在室內環境及走廊環境進行兩組地圖構建實驗。

首先在實驗室內部進行實驗,最終獲得運動過程中產生的軌跡和識別到的點云信息如圖9(b)所示,在點云地圖中顯示了相機的運動軌跡,并能粗略標識出實驗室的大致輪廓,但無法對機器人的導航和定位提供有效的障礙物位置信息。實驗生成的三維網格地圖如圖9(a)所示。實驗中體素分辨率為0.05 m(即三維地圖由邊長為0.05 m的正方體構成),圖9(a)較好地呈現了兩側放置的桌子和物品,對真實環境中右上角的小物體也能清晰表示,為了更好地表示實驗室內部的空間信息,將Kinect攝像頭所在的0.2 m 高的平面設置為0 m,使用不同顏色表示不同高度,其中負值表示障礙處于攝像頭之下,顏色與高度的對應值由圖9(a) 的圖例給出。

圖9 實驗室場景Fig.9 The lab scene

將地圖通過ROS系統中的rviz(ROS visualization)顯示,并利用工具測量地圖中的尺寸信息,與真實環境的對比如表2所示。從表2中可以看到,地圖中的測量值與真實環境的誤差均在3～6 cm,足以支持一般情景的導航和避障。

表2 實驗地圖與真實環境尺寸對比Table 2 Comparison of experimental map and real environment size

在完成室內場景的實驗后,選擇走廊作為大場景環境下的實驗驗證。走廊場景下的運動軌跡和點云地圖如圖10(b)所示。使用0.05 m的體素大小構建的網格地圖如圖10(a)所示,其中不同顏色代表的不同高度由圖例給出,圖10(a)能夠清晰地表示走廊的墻壁和門框等不同物體,在樓道與電梯間之間也可以表現二者的連接部分。實驗說明本文的算法在大場景下也可以較好地實現三維網格地圖的實時構建。

圖10 走廊場景Fig.10 The corridor scene

5 結論

提出了一種基于ORB-SLAM2的跳表地圖(SkipList Map)算法,用于三維占據網格地圖實時構建。首先根據跳表數據結構進行網格地圖的重組與分布；其次討論了三維占據網格地圖的表示和更新的方法；最后研究了ORB-SLAM2應用該地圖的流程步驟。實驗部分首先對比了本文算法與Octree和kd-tree的性能差異,表明本文的算法優于后兩種結構,平均意義下時間復雜度僅為O(lgn)；其次展示了本文的算法在真實場景中的效果。實驗表明,本算法具有較強的實用性,能夠實時構建用于機器人導航和路徑規劃的三維網格占據地圖。