摘要

本文介紹了VoxelMap++：一種具有平面合并功能的體素建圖方法，能夠有效提高基于LiDAR同時定位與建圖（SLAM）的準確性和效率。該地圖是一個包含一個平面特征的體素集合，具有3自由度的表示和相應(yīng)的協(xié)方差估計?？紤]到整個地圖將包含大量的共面特征（子平面），這些子平面的3自由度估計可以視為帶有更大平面協(xié)方差的測量。因此，我們設(shè)計了一個基于并查集的平面合并模塊，可以節(jié)省資源并進一步提高平面擬合的準確性。這個模塊可以區(qū)分不同體素中的子平面，并將這些子平面合并以估計父平面。合并后父平面的3自由度表示將比子平面更精確，不確定性將顯著減小，從而進一步提高了LiDAR慣性里程計的性能。在具有挑戰(zhàn)性的環(huán)境中進行的實驗，如走廊和森林，證明了我們的方法相對于其他最先進方法的高準確性和效率。順便說一下，我們的實現(xiàn)VoxelMap++在GitHub上開源，適用于非重復(fù)掃描LiDAR和傳統(tǒng)掃描LiDAR。

主要貢獻

本文提出了一種新穎的在線可合并體素或者說柵格建圖方法，采用3自由度平面表示，稱為VoxelMap++。具體而言，本文的貢獻包括：

我們通過使用最小二乘估計，將VoxelMap中的平面擬合和協(xié)方差估計方法從6自由度提升到3自由度。這一改進從工程實現(xiàn)的角度來看，進一步提高了協(xié)方差估計的效率，減少了內(nèi)存使用量，使VoxelMap++能夠輕松適應(yīng)各種資源受限的嵌入式平臺。

我們提出了一種新穎的在線體素合并方法，采用并查集。在體素中的每個共面特征（子平面）將被視為大平面（父平面）的測量，合并模塊不僅提高了平面擬合的準確性，降低了整個地圖的不確定性，還減少了地圖的內(nèi)存使用。

我們將VoxelMap++與其他最先進的算法在各種場景中進行了比較（結(jié)構(gòu)化、非結(jié)構(gòu)化和退化場景），展示了算法在準確性和效率方面的優(yōu)越性。

我們使VoxelMap++適應(yīng)不同類型的LiDAR（多旋轉(zhuǎn)LiDAR和非傳統(tǒng)固態(tài)LiDAR），并在GitHub上以易讀性和模塊化開源，以分享我們的發(fā)現(xiàn)并為社區(qū)做出貢獻。

內(nèi)容概述

VoxelMap++的流程如圖1所示，LiDAR原始點預(yù)處理方法和基于迭代誤差狀態(tài)卡爾曼濾波器的狀態(tài)估計方法與FASTLIO 類似。值得注意的是，本文的建圖方法可以適用于其他最先進的LiDAR慣性算法，無論其基于卡爾曼濾波器還是優(yōu)化。，在狀態(tài)估計后，新掃描中的每個點都將投影到相應(yīng)的體素中，然后構(gòu)建或更新由哈希表組織的體素地圖（鍵是體素ID，值是平面擬合結(jié)果P）。這些新的特征點將逐步用于進行3自由度平面擬合和協(xié)方差估計，這個模塊的復(fù)雜度不會隨著體素中點的數(shù)量增加而增加，因為用于擬合平面的所有值都是求和的形式，可以被緩存和逐步計算， 然后，收斂的平面將用于平面合并，在這個模塊中，體素中的子平面Pk將基于并查集合并為父平面Pf，與Pk相比，Pf的平面估計結(jié)果將更加準確，這顯然會改善LiDAR慣性里程計的定位結(jié)果。

圖1. VoxelMap++系統(tǒng)概述，本文的主要貢獻是用黃色表示的建圖模塊。平面合并算法如算法1所示

實驗

我們在具備2.9GHz 8核和16Gib內(nèi)存的筆記本電腦上，使用C++和機器人操作系統(tǒng)（ROS）實現(xiàn)了提出的VoxelMap++系統(tǒng)。實驗數(shù)據(jù)包括開源數(shù)據(jù)集M2DGR和我們自己收集的具有挑戰(zhàn)性的退化或非結(jié)構(gòu)化數(shù)據(jù)集，傳感器平臺如圖4所示。

圖4：我們的數(shù)據(jù)采集設(shè)備配備了Livox HAP激光雷達和內(nèi)置IMU的ZED 2i相機，這些設(shè)備已經(jīng)很好地固定在手推車上。表I提供了有關(guān)A-LOAM、LeGO-LOAM、LIO-SAM、LINS、FAST-LIO2、VoxelMap和我們提出的VoxelMap++在測試路線和評估結(jié)果方面的詳細信息?？梢郧宄乜闯觯@些數(shù)據(jù)集涵蓋了各種用于SLAM的環(huán)境，包括遠距離和短距離、室內(nèi)和室外、直線和曲線路線，這些場景足以說明結(jié)構(gòu)化的城市環(huán)境。

圖5顯示了我們方法的LiDAR軌跡以及所有樣本數(shù)據(jù)集序列上的實際軌跡。

圖5 我們方法LiDAR估計軌跡（藍色）與實際軌跡（紅色）在所有樣本序列上的情況。如表II所示，VoxelMap和VoxelMap++在非結(jié)構(gòu)化場景中比其他最先進的方法更具魯棒性和準確性。

如表III所示，其他SLAM算法在走廊中更容易累積誤差，我們提出的VoxelMap++在很大程度上比其他方法更準確，主要是因為平面合并可以更準確地估計平面的表示并實時估計它們的協(xié)方差。

我們提出的VoxelMap++的另一個優(yōu)勢是與其他先進方法相比，它在CPU和內(nèi)存資源使用方面更少，如表IV所示。

建圖效果如下

總結(jié)

本文提出了一種可合并的用于在線LiDAR慣性里程計的體素建圖方法，與其他方法相比，這種方法保持了具有3自由度表示和相應(yīng)協(xié)方差的平面特征，從而有效提高了計算速度并節(jié)省了內(nèi)存使用，為了提高平面擬合的精度充分利用了體素之間的關(guān)系，并在平面擬合收斂后基于并查集合并了共面體素。本文還展示了如何在基于迭代擴展卡爾曼濾波的LiDAR慣性里程計中實現(xiàn)所提出的建圖方法。在結(jié)構(gòu)化的開源數(shù)據(jù)集和我們自己的具有挑戰(zhàn)性的數(shù)據(jù)集上的實驗表明，我們的方法在性能上優(yōu)于其他最先進的方法。然而，該方法也存在一些缺點。例如，在動態(tài)場景（如關(guān)閉的電梯）中，魯棒性將顯著下降。因此將考慮從識別體素變化的角度對該方法進行優(yōu)化。