適用于高分辨率雷達(dá)的非光線投射占用柵格建圖方案

1. 筆者總結(jié)

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

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

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

2. 摘要

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

3. 主要貢獻(xiàn)

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

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

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

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

4. 算法解析

系統(tǒng)框架

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

深度圖像柵格化

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

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

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

2D分段樹(shù)

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

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

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

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

占用狀態(tài)確定

我們以五個(gè)小區(qū)為例介紹了我們確定小區(qū)占用狀態(tài)的方法的原理，如圖5所示，編號(hào)從1到5。我們首先根據(jù)單元格是否完全位于激光雷達(dá)的傳感區(qū)域內(nèi)對(duì)其進(jìn)行分類(lè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被確定為已知的，因?yàn)樗挥谟^察環(huán)境中所有物體的前面；網(wǎng)格1由于其位于對(duì)象后面而被確定為未知。網(wǎng)格2的占用狀態(tài)仍然是不確定的，因?yàn)樗囊徊糠治挥趯?duì)象的前面，而另一部分位于后面。關(guān)于其中一部分在感測(cè)區(qū)域內(nèi)的單元格，網(wǎng)格5被確定為未知，因?yàn)樗挥谖矬w后面。盡管位于物體前方，但由于網(wǎng)格4的位置不完全在內(nèi)部，因此網(wǎng)格4的占用狀態(tài)仍不確定。根據(jù)上述原理，D-Map將位于激光雷達(dá)傳感區(qū)域內(nèi)部或與之相交的單元投影到當(dāng)前深度圖像上，該圖像由最近的點(diǎn)云光柵化而成，這些點(diǎn)云表示環(huán)境中的對(duì)象。單元和對(duì)象之間的相對(duì)位置是通過(guò)比較它們的深度值來(lái)確定的。在Alg.1中描述了對(duì)深度圖像的占用狀態(tài)確定的過(guò)程。

占用柵格地圖

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

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

哈希柵格地圖

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

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

八叉樹(shù)地圖

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

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

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

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

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

初始化

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

占用柵格更新

占用狀態(tài)索引

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

5. 實(shí)驗(yàn)

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

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

效率評(píng)估分析

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

精度評(píng)估分析

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

6. 實(shí)際應(yīng)用

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

自主無(wú)人機(jī)探索

地圖重建效果

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

7. 總結(jié)

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

審核編輯：黃飛

?