摘要

局部幾何信息即法線和點(diǎn)分布在基于激光雷達(dá)的同時(shí)定位與地圖構(gòu)建（SLAM）中是至關(guān)重要，因?yàn)樗鼮閿?shù)據(jù)關(guān)聯(lián)提供了約束，進(jìn)一步確定了優(yōu)化方向，最終影響姿態(tài)的準(zhǔn)確性。然而即使在使用KD樹(shù)或體素圖的輔助下，估計(jì)法線和點(diǎn)分布也是耗時(shí)的任務(wù)。為了實(shí)現(xiàn)快速法線估計(jì)，我們研究了激光雷達(dá)掃描幀的結(jié)構(gòu)信息，提出了一種新穎的快速近似最小二乘（FALS）方法，通過(guò)預(yù)先計(jì)算的方位角信息，當(dāng)新的掃描幀到達(dá)時(shí)，估計(jì)法線僅需要點(diǎn)的距離信息，為了高效估計(jì)點(diǎn)的分布，我們將ikd樹(shù)擴(kuò)展到體素管理地圖，并在保持法線一致性的同時(shí)增量更新其點(diǎn)云分布。對(duì)于滿(mǎn)足基于法線的可見(jiàn)性和一致性檢查的掃描點(diǎn)，我們?cè)O(shè)計(jì)了一種穩(wěn)健且準(zhǔn)確的分層數(shù)據(jù)關(guān)聯(lián)方案，其中點(diǎn)到曲面的關(guān)聯(lián)優(yōu)先于點(diǎn)到平面的關(guān)聯(lián)，在分布趨于收斂后，我們進(jìn)一步固定體素以平衡時(shí)間消耗和表示的正確性，在多樣的公開(kāi)數(shù)據(jù)集上進(jìn)行的大量實(shí)驗(yàn)表明，與其他先進(jìn)方法相比，我們的系統(tǒng)具有明顯優(yōu)勢(shì)。

主要貢獻(xiàn)

地圖體素內(nèi)逐步更新點(diǎn)云分布以保持空間信息的正確性，同時(shí)與法線保持一致性。為了平衡時(shí)間消耗和表示的正確性，我們?cè)跀U(kuò)展的ikd樹(shù)上管理地圖，并在分布收斂后進(jìn)一步固定分布。本工作的主要貢獻(xiàn)如下：

Ring FALS一種利用特定激光雷達(dá)的結(jié)構(gòu)信息的新型快速近似最小二乘法法線估計(jì)器，與PCL相比快速而準(zhǔn)確，滿(mǎn)足LIO系統(tǒng)的實(shí)時(shí)要求。

考慮地圖體素內(nèi)點(diǎn)分布的魯棒且準(zhǔn)確的分層數(shù)據(jù)關(guān)聯(lián)方案，其中點(diǎn)到曲面的關(guān)聯(lián)優(yōu)先于點(diǎn)到平面的關(guān)聯(lián)，大尺度優(yōu)先于小尺度。

對(duì)公共數(shù)據(jù)集進(jìn)行廣泛實(shí)驗(yàn)，證明我們的LIO系統(tǒng)相對(duì)于其他最先進(jìn)的方法具有優(yōu)勢(shì)。為了使社區(qū)受益，該工作實(shí)現(xiàn)已在 https://github.com/tiev-tongji/LOG-LIO 開(kāi)源，還將Ring FALS作為獨(dú)立的法線工具開(kāi)源在 https://github.com/tiev-tongji/RingFalsNormal。

主要內(nèi)容

LOG-LIO的流程接收來(lái)自3D激光雷達(dá)和慣性測(cè)量單元（IMU）的輸入，如圖2所示。對(duì)于新的輸入掃描，我們首先使用Ring FALS來(lái)估計(jì)原始點(diǎn)的法線，在使用IMU測(cè)量校正畸變后，根據(jù)它們的局部幾何信息，在未畸變的點(diǎn)云和地圖之間執(zhí)行關(guān)聯(lián)。我們通過(guò)iEKF整合IMU的測(cè)量，并通過(guò)類(lèi)似FAST-LIO的方法優(yōu)化機(jī)體的姿態(tài)，在優(yōu)化之后，新的點(diǎn)被添加到由擴(kuò)展的ikd樹(shù)管理的地圖中，地圖體素內(nèi)的分布在考慮法線的同時(shí)進(jìn)行增量維護(hù)，以確保其準(zhǔn)確性，并在收斂后進(jìn)行固定。

圖2. LOG-LIO系統(tǒng)概述

圖3. 白色線條表示來(lái)自 Ring FALS 估計(jì)的法線

實(shí)驗(yàn)

表 I 顯示了用于 M2DGR 和 NTU VIRAL 數(shù)據(jù)集中 Velodyne-32 LiDAR 的單次掃描的平均法線估計(jì)處理時(shí)間。對(duì)于 Velodyne-32 LiDAR 的單次掃描約有 57,600 個(gè)點(diǎn)，與 PCL 相比，Ring FALS 的耗時(shí)僅為十分之一，與 OMP 版本相比甚至只有四分之一。對(duì)于 Ouster16 LiDAR，無(wú)論是單線程還是 OMP 版本，Ring FALS 的時(shí)間消耗都遠(yuǎn)小于 PCL，分析 PCL 的結(jié)果，盡管 Ouster-16 LiDAR 的點(diǎn)數(shù)少于 Velodyne-32，但耗時(shí)反而增加，原因是 PCL 法線估計(jì)中最耗時(shí)的部分之一是 KDtree 鄰域搜索，這與 KDtree 的結(jié)構(gòu)有關(guān)。

表格 II 報(bào)告了實(shí)驗(yàn)結(jié)果，可以看出，在室內(nèi)場(chǎng)景中，LOG、LOG-C 和 FAST-LIO2 的軌跡準(zhǔn)確性接近，并在大多數(shù)序列中優(yōu)于 LIO-SAM。

圖4顯示了數(shù)據(jù)集street10 的軌跡，進(jìn)行了定性比較，LOG 的出色表現(xiàn)證明與稀疏點(diǎn)場(chǎng)景中的地圖體素關(guān)聯(lián)更好地表示了局部幾何信息。

圖4. M2DGR 數(shù)據(jù)集序列 street10 中的定位估計(jì)，放大的彩色框圖像對(duì)應(yīng)于軌跡中相同顏色的框。LOG-LIO 在大多數(shù)實(shí)驗(yàn)中獲得最佳結(jié)果，緊隨其后的是 FAST-LIO2，而由于 LiDAR 點(diǎn)云和地圖的稀疏性，LIO-SAM 更容易失敗，如表格 III 所示

每個(gè)序列中 LOG-LIO 和 FAST-LIO2 的平均耗時(shí)，如表 IV 所示。

總結(jié)

本文提出的LOG-LIO是一種在線激光雷達(dá)慣性里程計(jì)法，它結(jié)合實(shí)時(shí)的法線和點(diǎn)分布估計(jì)來(lái)準(zhǔn)確表示局部幾何信息。同時(shí)還提出了一種高效的激光雷達(dá)點(diǎn)云法線估計(jì)方法，名為 Ring FALS，它預(yù)先計(jì)算了方位信息并僅利用距離信息來(lái)估計(jì)點(diǎn)的法線，LOG-LIO 通過(guò)擴(kuò)展的 ikd-tree 管理地圖，并在地圖體素內(nèi)逐步維護(hù)法線和點(diǎn)分布，分層數(shù)據(jù)關(guān)聯(lián)方案提供了精確的約束，從而實(shí)現(xiàn)更準(zhǔn)確的姿態(tài)估計(jì)，在各種環(huán)境中，LOG-LIO 在實(shí)驗(yàn)中表現(xiàn)出色，優(yōu)于現(xiàn)有的 LIO 系統(tǒng)。