1. 筆者總結(jié)

機器人系統(tǒng)中，高效的地圖數(shù)據(jù)結(jié)構(gòu)是保證整個系統(tǒng)效率的關(guān)鍵。常見的點云地圖存儲方式包括：關(guān)鍵幀集合、樹形結(jié)構(gòu)（kdtree、octree）、voxels，而用于導(dǎo)航定位路徑規(guī)劃的地圖通常是 Gridmap、Octomap格式。然而對于高分辨率的雷達，占用柵格地圖的計算效率卻依舊面臨挑戰(zhàn)。

今天筆者介紹的是一項來自香港大學(xué)火星實驗室新的工作，與傳統(tǒng)的柵格地圖構(gòu)建方式不同，這項工作基于深度圖像投影確定占用柵格狀態(tài)，使用了包含哈希柵格和八叉樹的混合地圖結(jié)構(gòu)，并且能夠增量更新，在計算量和內(nèi)存效率之間提供了平衡，從作者的真實應(yīng)用實驗中能夠看出地圖的輕量化、高效性。這里也推薦「3D視覺工坊」新課程《徹底理解dToF雷達系統(tǒng)設(shè)計[理論+代碼+實戰(zhàn)]》。

圖 1. 我們提出的框架 D-Map 作為實時高分辨率占用測繪模塊，用于古代堡壘中的自主無人機探索任務(wù)。(a) 無人機采集的高保真點云。(b) 場景鳥瞰圖。(c) 空中平臺搭載128通道激光雷達(OS1-128)執(zhí)行勘探任務(wù).

2. 摘要

占用地圖是機器人系統(tǒng)對未知和已知環(huán)境區(qū)域進行探索的基本組成部分。本文提出了一種用于高分辨率激光雷達傳感器的高效占用建圖框架，稱為D-Map。該框架引入了三個主要的創(chuàng)新點來提高占用地圖的計算效率。首先，我們使用深度圖像來確定區(qū)域的占用狀態(tài)，而不是傳統(tǒng)的光線投射方法。其次，我們在基于樹的地圖結(jié)構(gòu)上引入了一種高效的樹上更新策略。這兩種技術(shù)避免了對小柵格的冗余訪問，顯著減少了要更新的柵格數(shù)量。第三，我們利用激光雷達傳感器的低誤報率，在每次更新時從地圖中刪除已知柵格。這種方法不僅通過減小地圖尺寸來提高我們框架的更新效率，而且賦予它一個有趣的遞減特性，我們將其命名為D-map。為了支持我們的設(shè)計，我們對深度圖像投影的準(zhǔn)確性和占用更新的時間復(fù)雜性進行了理論分析。此外，我們在公共和私人數(shù)據(jù)集中對各種激光雷達傳感器進行了廣泛的基準(zhǔn)實驗。與其他最先進的方法相比，我們的框架顯示出卓越的效率，同時保持了相當(dāng)?shù)慕▓D精度和高內(nèi)存效率，我們展示了D-Map在手持設(shè)備和攜帶高分辨率激光雷達的無人機平臺上用于實時占用地圖的兩個真實世界應(yīng)用。此外，我們在GitHub上開源了D-Map的實現(xiàn)。

3. 主要貢獻

提出了一種基于深度圖像投影的占用狀態(tài)確定方法，以減輕傳統(tǒng)光線投射技術(shù)中的計算負(fù)載。這種基于投影的方法能夠確定任何大小的單元的占用狀態(tài)，從而允許在大規(guī)模環(huán)境中進行后續(xù)的高效更新。

提出了一種新的基于混合地圖結(jié)構(gòu)的樹上更新策略，用于更新占用狀態(tài)，在計算和內(nèi)存效率之間提供了良好的平衡。混合地圖結(jié)構(gòu)將未知空間存儲在八叉樹上，這使得能夠高效地表示大的未知空間，而占用的空間存儲在哈希柵格地圖上。就效率而言，所提出的策略允許在八叉樹上確定大小區(qū)的占用狀態(tài)，從而避免對小小區(qū)的不必要更新并提高效率。

利用激光雷達測量的低誤報率，在每次更新時直接刪除具有確定狀態(tài)（即占用或空閑）的柵格。這種方法使我們的地圖結(jié)構(gòu)具有遞減特性，因此我們將我們的框架稱為D-map，從而提供更高的計算效率和更少的內(nèi)存使用。

對所提出的占用狀態(tài)確定方法的準(zhǔn)確性以及D-Map中更新和查詢的時間復(fù)雜性進行了深入分析。具體來說，我們導(dǎo)出了一個分析函數(shù)來量化相對于深度圖像分辨率的精度損失。D-Map更新的時間復(fù)雜性分析為我們優(yōu)于依賴射線投射的最先進方法的卓越性能提供了理論支撐。

4. 算法解析

系統(tǒng)框架

圖2. D-Map的框架概述。藍色塊顯示 D-Map 的輸入，包括點云和相應(yīng)的傳感器里程計。橙色塊是D-Map的占用圖結(jié)構(gòu)，由用于維護占用空間的哈希網(wǎng)格圖和用于維護未知空間的八叉樹組成。綠色塊中提出了占用更新策略，該策略提取八叉樹上傳感區(qū)域內(nèi)的單元，并根據(jù)使用深度圖像的占用狀態(tài)確定方法進行操作。

深度圖像柵格化

圖3. 該圖說明了圖分辨率d、檢測范圍R和深度圖像分辨率之間的空間關(guān)系。

在為占用狀態(tài)確定做準(zhǔn)備時，將激光雷達傳感器捕獲的點云光柵化為當(dāng)前傳感器姿態(tài)的深度圖像。為了確保狀態(tài)確定的準(zhǔn)確性，深度圖像的分辨率應(yīng)該足夠小，使得從地圖到深度圖像的單元的投影面積大于一個像素。如圖3所示，我們使用以下公式確定相對于地圖分辨率d和LiDAR檢測范圍R的深度圖像分辨率：

然而，高分辨率地圖將產(chǎn)生巨大尺寸的高分辨率深度圖像，由于點云的數(shù)量遠小于深度圖像的大小，因此會有許多空像素。為了解決這個問題，我們將深度圖像分辨率與激光雷達角分辨率綁定，激光雷達角分辨是以旋轉(zhuǎn)方式發(fā)射和接收的兩個激光脈沖之間的最小角度。具體來說，我們將標(biāo)準(zhǔn)深度圖像分辨率定義為

2D分段樹

為了確定地圖中單元的占用狀態(tài)，采用了兩步過程，其中首先將單元格投影到深度圖像上，然后將投影的深度與相應(yīng)區(qū)域的最小和最大深度值進行比較。由于單元格在深度圖像上的投影面積隨單元位置而變化，因此采用2-D分段樹結(jié)構(gòu)來加快對深度圖像上最小值和最大值的有效查詢，如下所述。

分段樹是一種完全平衡的二叉樹，通過表示一組區(qū)間來有效地提供范圍查詢[53]。圖4描述了通過一維段樹查詢最小值的過程。分段樹是通過遞歸地將數(shù)組一分為二來構(gòu)建的，直到每個節(jié)點都包含一個元素。由于段樹上的每個節(jié)點代表數(shù)組的一個區(qū)間，因此區(qū)間中值的匯總信息，例如最小值和最大值在構(gòu)造過程中進行預(yù)處理，以加速后續(xù)查詢。查詢時，分段樹使用節(jié)點子集（圖4中的彩色節(jié)點）檢索查詢區(qū)間的最小表示。該結(jié)果是通過用比直接查詢更少的操作來匯總來自檢索到的節(jié)點的信息而獲得的。一維段樹上查詢的時間復(fù)雜度為O（log N），其中N是離散數(shù)組中元素的數(shù)量，而直接查詢的時間復(fù)雜性為O（N）。

圖 4 展示了在一維線段樹上快速查詢 [2, 7] 像素范圍內(nèi)最小值的示例。范圍查詢從線段樹的根開始，沿著樹遞歸搜索，直到當(dāng)前節(jié)點范圍被查詢范圍完全覆蓋，其中建樹時該節(jié)點上已經(jīng)保存的節(jié)點范圍的最小值為 被退回。在此示例中，范圍 [2, 7] 產(chǎn)生四個節(jié)點，分別表示范圍 [2, 2]、[3, 4]、[5, 6] 和 [7, 7]。從這四個節(jié)點有效地獲得范圍的最小值，而不是計算數(shù)組中的六個元素。

將一維分段樹擴展到二維結(jié)構(gòu)的方法包括構(gòu)建“分段樹的分段樹”。外層中的分段樹按行分割2-D陣列，并且在外層分段樹的每個節(jié)點上，構(gòu)造1-D內(nèi)部分段樹以保持覆蓋行上的列信息。對2-D分段樹的查詢首先在外部樹中搜索表示被查詢區(qū)域覆蓋的行的節(jié)點，然后遍歷相應(yīng)節(jié)點上的內(nèi)部分段樹以檢索覆蓋的列。最后，根據(jù)存儲在檢索到的節(jié)點上的信息來總結(jié)查詢區(qū)域上的結(jié)果。在大小為的二維數(shù)組上查詢二維段樹的時間復(fù)雜度為，而直接查詢導(dǎo)致時間復(fù)雜度給定從點云光柵化的深度圖像，我們構(gòu)建一個2-D分段樹，以保持每個樹節(jié)點上的最小和最大深度值，分別表示為和。此外，我們還跟蹤每個節(jié)點覆蓋區(qū)域內(nèi)點云占用的像素數(shù)量，表示為。

占用狀態(tài)確定

我們以五個小區(qū)為例介紹了我們確定小區(qū)占用狀態(tài)的方法的原理，如圖5所示，編號從1到5。我們首先根據(jù)單元格是否完全位于激光雷達的傳感區(qū)域內(nèi)對其進行分類。如圖5（b）中自上而下的視圖所示，網(wǎng)格1、網(wǎng)格2和網(wǎng)格3完全位于感應(yīng)區(qū)域內(nèi)，而網(wǎng)格4和網(wǎng)格5只有一部分位于感應(yīng)區(qū)域內(nèi)部。在完全內(nèi)部的單元格中，網(wǎng)格3被確定為已知的，因為它位于觀察環(huán)境中所有物體的前面；網(wǎng)格1由于其位于對象后面而被確定為未知。網(wǎng)格2的占用狀態(tài)仍然是不確定的，因為它的一部分位于對象的前面，而另一部分位于后面。關(guān)于其中一部分在感測區(qū)域內(nèi)的單元格，網(wǎng)格5被確定為未知，因為它位于物體后面。盡管位于物體前方，但由于網(wǎng)格4的位置不完全在內(nèi)部，因此網(wǎng)格4的占用狀態(tài)仍不確定。根據(jù)上述原理，D-Map將位于激光雷達傳感區(qū)域內(nèi)部或與之相交的單元投影到當(dāng)前深度圖像上，該圖像由最近的點云光柵化而成，這些點云表示環(huán)境中的對象。單元和對象之間的相對位置是通過比較它們的深度值來確定的。在Alg.1中描述了對深度圖像的占用狀態(tài)確定的過程。

占用柵格地圖

地圖結(jié)構(gòu)

為了優(yōu)化計算和內(nèi)存效率之間的平衡，D-Map利用哈希網(wǎng)格映射來維持占用空間，利用八叉樹來維持未知空間。

哈希柵格地圖

由于環(huán)境中的占用區(qū)域通常比自由和未知區(qū)域少，我們使用體素哈希技術(shù)在哈希網(wǎng)格圖中保持環(huán)境的占用空間，這允許在的時間復(fù)雜度下進行有效的更新和查詢操作。給定點p=[x，y，z]的哈希鍵值由哈希函數(shù)hash計算，定義如下：

其中d表示散列網(wǎng)格圖的分辨率。P、Q是大素數(shù)，而Q也用作哈希表的大小。mod是兩個整數(shù)之間的模計算。P和Q的值經(jīng)過仔細選擇，以最大限度地降低沖突概率，在我們的工作中，P和Q分別設(shè)置為116101和201326611。

八叉樹地圖

基于樹的映射結(jié)構(gòu)由于其高內(nèi)存效率而成為占用映射的常用方法。在各種基于樹的數(shù)據(jù)結(jié)構(gòu)中，八叉樹[58]脫穎而出，因為它在動態(tài)更新方面優(yōu)于其他空間數(shù)據(jù)結(jié)構(gòu)[59]。此外，八叉樹上的空間劃分自然允許在樹更新期間確定大單元的占用狀態(tài)。因此，我們利用八叉樹來組織未知空間。

在D-Map中，八叉樹上的節(jié)點包含以下元素：

點陣列包含其八個子節(jié)點的地址。如果節(jié)點是葉節(jié)點，則為空。

由節(jié)點表示的單元格的中心。

節(jié)點表示的單元格的大小。

初始化

為了初始化D-Map中的映射過程，環(huán)境的占用狀態(tài)被認(rèn)為是完全未知的。在實現(xiàn)中，給定感興趣區(qū)域的初始邊界框C bbx，八叉樹的根節(jié)點被初始化以表示未知立方體C根，其中心C被分配在C bbx的中心。根節(jié)點的大小L指定為邊界框C bbx的最長邊長，并且子節(jié)點的點陣列ChildNodes初始化為空。此外，哈希網(wǎng)格映射中的哈希表被初始化為空表。值得注意的是，初始邊界框并不限制映射區(qū)域，因為八叉樹和哈希網(wǎng)格映射都允許映射空間的動態(tài)增長。

占用柵格更新

占用狀態(tài)索引

D-Map中使用了兩個數(shù)據(jù)結(jié)構(gòu)，因此執(zhí)行兩個步驟以獲得地圖分辨率下的單元占用狀態(tài)。首先，如果單元的對應(yīng)節(jié)點存在于八叉樹上，則該單元被確定為未知。否則，該區(qū)域被確定為已知區(qū)域。隨后，查詢哈希地圖以確定其是否被占用。如果沒有，則該區(qū)域被確定為自由空間。這里也推薦「3D視覺工坊」新課程《徹底理解dToF雷達系統(tǒng)設(shè)計[理論+代碼+實戰(zhàn)]》。

5. 實驗

實驗在三個開放數(shù)據(jù)集和一個私有數(shù)據(jù)集上進行。第一個公開數(shù)據(jù)集是Kitti數(shù)據(jù)集，它通過Velodyne HDL-64E旋轉(zhuǎn)3D激光掃描儀在10 Hz下捕獲深度測量。考慮到將在我們的基準(zhǔn)實驗中使用的與基于網(wǎng)格的映射相關(guān)的巨大內(nèi)存消耗，我們選擇了序列Kitti 04、Kitti 06和Kitti 07來進行基準(zhǔn)評估。第二個公共數(shù)據(jù)集是FAST-LIO中提供的戶外序列MainBuilding，該序列使用半固態(tài)3D激光雷達傳感器Livox-Avia以10Hz收集數(shù)據(jù)。Octopap工作中提供的第三個公共數(shù)據(jù)集包括室內(nèi)序列FR-079和室外序列Freiburg。此外，我們在私人數(shù)據(jù)集Workshop上評估了遞減特性的好處，在該工作坊中，使用10 Hz的3D LiDAR Livox Avia手持掃描雜亂的室內(nèi)環(huán)境。值得注意的是，我們的激光雷達慣性里程計框架FAST-LIO2獲得了序列車間和主樓中占用地圖的里程計估計。

表1提供了上述數(shù)據(jù)集的進一步細節(jié)。

效率評估分析

表2總結(jié)了各種占用建圖方法的平均更新時間 圖12比較了兩個序列中占用更新的時間消耗

精度評估分析

我們使用Octopap的建圖結(jié)果作為真值進行精度評估。具體來說，我們通過對計算建圖區(qū)域內(nèi)具有正確占用狀態(tài)的單元格總數(shù)來衡量D-Map的準(zhǔn)確性。未知空間和自由空間的實驗結(jié)果如表3所示。

6. 實際應(yīng)用

高分辨率實時三維地圖交互導(dǎo)航

自主無人機探索

地圖重建效果

占據(jù)柵格建圖效果

7. 總結(jié)

本文提出了一種新的占用映射框架D-Map，旨在為高分辨率激光雷達傳感器提供有效的占用更新。我們提出的框架由三個關(guān)鍵技術(shù)組成。首先，提出了一種通過深度圖像投影來確定任意大小單元格占用狀態(tài)的方法。其次，利用高效的樹上更新策略，開發(fā)了一種混合地圖結(jié)構(gòu)。第三，引入了一種去除策略，該策略利用激光雷達的低誤報率從地圖中去除已知單元格。這樣可以減少需要更新的單元數(shù)量，降低地圖更新的成本，從而顯著提高效率。為了驗證我們提出的框架，我們對其準(zhǔn)確性和效率進行了理論分析，并在各種激光雷達數(shù)據(jù)集上進行了廣泛的基準(zhǔn)實驗。結(jié)果表明，與其他最先進的建圖方法相比，D-Map顯著提高了效率，同時保持了相當(dāng)?shù)木群透叽鎯π省Ａ硗庋菔玖藘蓚€真實世界的應(yīng)用，以展示D-Map在基于高分辨率激光雷達的應(yīng)用中的有效性和效率。未來，我們可以擴展我們的D-Map框架，以支持動態(tài)環(huán)境中的占用建圖，并利用并行處理。

審核編輯：黃飛

?