1. 筆者總結

機器人系統中，高效的地圖數據結構是保證整個系統效率的關鍵。常見的點云地圖存儲方式包括：關鍵幀集合、樹形結構（kdtree、octree）、voxels，而用于導航定位路徑規劃的地圖通常是 Gridmap、Octomap格式。然而對于高分辨率的雷達，占用柵格地圖的計算效率卻依舊面臨挑戰。

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

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

2. 摘要

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

3. 主要貢獻

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

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

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

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

4. 算法解析

系統框架

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

深度圖像柵格化

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

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

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

2D分段樹

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

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

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

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

占用狀態確定

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

占用柵格地圖

地圖結構

為了優化計算和內存效率之間的平衡，D-Map利用哈希網格映射來維持占用空間，利用八叉樹來維持未知空間。

哈希柵格地圖

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

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

八叉樹地圖

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

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

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

由節點表示的單元格的中心。

節點表示的單元格的大小。

初始化

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

占用柵格更新

占用狀態索引

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

5. 實驗

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

表1提供了上述數據集的進一步細節。

效率評估分析

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

精度評估分析

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

6. 實際應用

高分辨率實時三維地圖交互導航

自主無人機探索

地圖重建效果

占據柵格建圖效果

7. 總結

本文提出了一種新的占用映射框架D-Map，旨在為高分辨率激光雷達傳感器提供有效的占用更新。我們提出的框架由三個關鍵技術組成。首先，提出了一種通過深度圖像投影來確定任意大小單元格占用狀態的方法。其次，利用高效的樹上更新策略，開發了一種混合地圖結構。第三，引入了一種去除策略，該策略利用激光雷達的低誤報率從地圖中去除已知單元格。這樣可以減少需要更新的單元數量，降低地圖更新的成本，從而顯著提高效率。為了驗證我們提出的框架，我們對其準確性和效率進行了理論分析，并在各種激光雷達數據集上進行了廣泛的基準實驗。結果表明，與其他最先進的建圖方法相比，D-Map顯著提高了效率，同時保持了相當的精度和高存儲效率。另外演示了兩個真實世界的應用，以展示D-Map在基于高分辨率激光雷達的應用中的有效性和效率。未來，我們可以擴展我們的D-Map框架，以支持動態環境中的占用建圖，并利用并行處理。

審核編輯：黃飛

?