摘要

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

主要貢獻(xiàn)

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

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

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

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

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

內(nèi)容概述

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

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

實(shí)驗(yàn)

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

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

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

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

如表III所示，其他SLAM算法在走廊中更容易累積誤差，我們提出的VoxelMap++在很大程度上比其他方法更準(zhǔn)確，主要是因?yàn)槠矫婧喜⒖梢愿鼫?zhǔn)確地估計(jì)平面的表示并實(shí)時(shí)估計(jì)它們的協(xié)方差。

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

建圖效果如下

總結(jié)

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