NV-LIO：使用法向量的激光雷达-惯性里程计面向多楼层环境中的鲁棒 SLAM

文章：NV-LIO: LiDAR-Inertial Odometry using Normal Vectors Towards Robust SLAM in Multifloor Environments

作者：Dongha Chung and Jinwhan Kim

编辑：点云PCL

公众号致力于点云处理，SLAM，三维视觉，高精地图等领域相关内容的干货分享，欢迎各位加入，有兴趣的可联系dianyunpcl@163.com。未经作者允许请勿转载，欢迎各位同学积极分享和交流。

摘要

在过去的几十年里，已经开发了许多激光雷达-惯性里程计（LIO）算法，这些算法在各种环境中展示了令人满意的性能。大多数这些算法主要在开放的户外环境中进行了验证，但.在狭窄的室内环境中往往遇到挑战。在这样的室内环境中，由于激光雷达扫描的快速变化和重复的结构特征（如墙壁和楼梯），特别是在多楼层建筑中，可靠的点云配准变得具有挑战性。本文提出了NV-LIO，一种基于法向量的LIO框架，旨在多楼层结构的室内环境中实现同时定位和建图（SLAM）。该方法从激光雷达扫描中提取法向量，并利用它们进行对应搜索，以增强点云配准性能。为了确保稳健的配准，分析了法向量方向的分布，并检查了退化情况以调整匹配的不确定性。此外实现了基于视点的闭环模块，以避免被墙壁阻挡的错误对应关系。通过公共数据集和我们自己的数据集验证了所提出的方法。代码将公开发布在https://github.com/dhchung/nv_lio。

主要贡献

本文提出了NV-LIO，一种基于法向量的紧耦合LIO框架，旨在多楼层室内SLAM。该框架利用了将密集机械3D激光雷达扫描投影到距离图像中的能力，从中提取法向量，扫描帧之间的配准不仅考虑最近邻点，还考虑了法向量的角度差异，增强了配准过程中的对应搜索准确性。在匹配过程中，分析法向量方向的分布以评估退化的发生，调整匹配的不确定性。为了确保准确的闭环，在扫描帧和子图之间的匹配过程中采用了可见性分析，防止不同房间或楼层之间的错误对应。本文的主要贡献可以总结如下：

• 引入了一个紧耦合的LIO框架，利用密集机械激光雷达点云中的法向量在狭窄的室内环境中实现稳健的配准。

• 为了解决点云配准中的退化情况，特别是在长走廊或楼梯间场景中，提出了退化检测算法和相应的配准不确定性协方差矩阵计算方法。

• 在各种数据集中对所提出的算法进行了全面验证，包括在公共数据集和我们自己收集的数据集上的评估，如图1所示。验证结果展示了所提出方法的有效性和多样性。

图1：使用NV-LIO对KAIST的KI大楼（一栋五层建筑）进行的建图结果。（a）显示了总体的建图结果和激光雷达轨迹。（b）显示了建筑内部拍摄的照片与建图结果的比较。

主要内容

系统概述

图2展示了所提出算法的概述。通过积分IMU陀螺仪测量值来补偿扫描中的运动。通过使用球面投影对运动补偿后的点云进行投影，提取法向云。在使用惯性测量值对提取的法向云进行对齐后，通过关键帧子图之间的法向云配准确定相对位姿。此外，通过基于视点的闭环检测获得校正测量。这些配准结果作为相对位姿因子包含在图中，IMU测量值也通过IMU预积分添加到图中。通过这种位姿图优化，估计当前位姿和IMU偏置。

图2：所提出算法的框架

预处理

为了获得准确的法向量和配准结果，对激光雷达扫描进行运动补偿是至关重要的。为此利用IMU获取的角速度并结合估计的偏置，考虑到激光雷达点的接收频率（超过10000 Hz）远高于IMU的频率（100 Hz），基于初始接收到的点的时间戳，使用IMU估计的旋转进行时间插值。这个过程在激光雷达坐标系中进行，以便于后续的球面投影步骤。

球面投影与法向量提取

首先，对经过运动补偿的激光雷达点云进行球面投影以生成深度图像，深度图像的大小根据激光雷达点云的特性手动选择，包括通道数量、水平分辨率和视场等参数。法向量通过计算深度图像中点的水平和垂直方向的微分来获得。为了减少距离测量噪声的影响，不使用简单的相邻像素微分，而是采用窗口内的导数平均方法，得到的法向量从球面坐标转换为笛卡尔坐标，并在必要时进行反向调整，确保其方向一致性。最后，验证法向量是否与窗口内的邻近点形成共识。如果在窗口内的点到平面的距离小于一定阈值的点数不足，则认为该法向量无效。通过这一过程，提取出包含法向点的法向云。图3显示了从输入点云中提取法向量的整体过程。

图3：法向量提取过程

回环闭合检测

在多层室内环境中，由于重复的结构特征，全局回环检测算法往往难以有效工作。这一挑战在楼梯间尤为明显，楼梯间的重复特征可能导致在不同楼层之间的错误关联。因此本研究采用了一种基于当前位置信息进行局部匹配的回环检测方法，而非全局搜索。虽然ICP或GICP等技术常用于局部回环检测方法，它们通过半径搜索找到最近的点作为对应点，但在狭窄的室内空间中，这些方法常常会导致错位。这主要是因为室内环境通常由多个分割区域组成，即使激光雷达位置稍有变化，扫描结果也会有显著差异。为了解决这个问题，这里引入了一种基于视点的回环闭合检测方法，借鉴了文献[15]中的投影技术，以更好地进行对应点搜索。图4展示了基于视点的回环检测示例。首先，使用每个关键帧的位置构建一个kd-tree，然后选择最接近当前帧的关键帧。在这个过程中，当前帧之前的关键帧被排除在kd-tree之外。一旦识别出回环候选关键帧，将候选关键帧的法向云转换到当前帧的激光雷达姿态。然后，如图4c所示，将其投影以匹配当前激光雷达视点。在球面投影期间分配到相同像素的点中，只有最接近的点被保留。

图 4：使用基于视点的回环检测进行楼层间回环闭合检测示例

退化检测

在室内环境中，许多表面通常平行排列，导致退化情况的出现。例如，在楼梯间或长廊等环境（如图5所示），表面的法线向量仅沿两个方向分布，导致在剩余的方向上存在平移模糊性。例如，在楼梯间的情况下，形成楼梯间的墙壁的法线向量是水平分布的，这导致在水平方向上有很高的定位精度，但在垂直方向上可能存在模糊性。为了检测这种退化情况并确定合适的匹配不确定性，作者使用了法线向量的主成分分析。具体做法包括计算法线向量的协方差矩阵，并通过其特征值来判断法线向量的分布情况。最小特征值的大小可以指示是否存在退化情况。在匹配过程中，根据法线向量分布设置测量协方差，以在退化情况下调整匹配因子，保证定位精度和系统稳定性。

图5：楼梯间和长廊环境中的退化情况

位姿图

位姿图包括对初始姿态给定的先验因子，通过正常云配准获得的相对姿态因子，来自闭环检测的闭环因子，以及来自IMU预积分的IMU因子和恒定偏置因子。从上一次姿态图优化结果反映的偏置IMU测量被整合，以连续估计IMU频率下的当前帧。如果当前帧与上一帧之间的姿态差异超过一定阈值，则插入新的关键帧。姿态图的构建和优化使用了iSAM2框架。

实现细节

在球面图像投影中，使用的像素数量设置为通道数乘以1024。为了稳定地提取法线向量，在具有32个或更少通道的LiDAR中使用3x3窗口，而在通道数超过32的LiDAR中使用5x5窗口计算法线向量。法线云配准的距离阈值设置为0.5米，而下采样体素大小根据具体情景设置为0.4米或0.2米。对于关键帧，如果与上一个关键帧姿态的角度差异超过30度或距离差异大于阈值，则添加新的关键帧。此距离阈值根据建筑特征设置为1.0米或0.5米。闭环检测的距离阈值设定为10米。

实验

为了评估所提算法的性能，我们使用了多种数据集进行测试，这些数据集是在不同环境下使用不同类型的旋转式LiDAR捕获的，包括SubT-MRS数据集、Newer College数据集以及我们自己的数据集。为了评估所提方法在多层室内环境中的有效性，我们使用了SubT-MRS数据集的多层数据集进行评估。该数据集包括由装备有16通道LiDAR和IMU的四足机器人获取的数据，该机器人穿越建筑物的内部和外部，从一楼到三楼。Newer College数据集是在校园环境中使用手持设备捕获的，使用了LiDAR及其内置IMU进行评估。至于我们自己的数据集，它是在韩国科技院校园内的多个建筑物中使用手持设备获取的，如图6所示。我们的算法与LIOSAM、Fast-LIO2、Faster-LIO和DLIO等公开可用的先进算法进行了比较。所有测试都是在线进行的，使用装备有Intel i7-12700 CPU和12个核心的计算机。

图 6：本文自己数据集的数据采集系统

SubT-MRS 多层数据集：由于 SubT-MRS 数据集中缺乏真知轨迹，因此展示了定性结果，如图7所示。考虑到狭窄的走廊和楼梯，所有算法的体素大小均设为0.2米，基于关键帧的算法（如LIO-SAM、DLIO和NV-LIO）的关键帧距离设为0.5米。在评估过程中，Fast-LIO2 和 LIO-SAM 失败了，因此未包含在结果中。Faster-LIO 每次经过楼梯时都出现显著的漂移，导致地图形成不正确，如图7b所示。DLIO 在楼梯上生成了不正确的建图，观察到来自不同楼层的关键帧错误匹配（图7c）。相比之下，如图7d所示，NV-LIO 清晰地描绘了每层楼的边界，即使在楼梯环境中也提供了良好的状态估计，导致整体形成良好的地图结果。

图7:SubT MRS数据集的序列和结果

Newer College 数据集：Newer College 数据集的结果显示在表I和表II中。所有算法的下采样体素大小固定为0.4米，NV-LIO 的关键帧插入距离设定为1.0米进行评估。

结果表明，NV-LIO 需要额外的预处理，导致平均计算时间增加。然而，在室外环境中，它表现出与先进方法类似或更低的误差，同时在室内设置中显示出更大的稳健性。特别是在非常狭窄的环境（如楼梯数据集）或快速运动的情况下，如地下硬数据集中观察到的情况，它表现良好。

韩国科技院建筑数据集：图8展示了5层建筑的楼梯数据集的结果，该建筑在图1中展示。Fast-LIO2 和 Faster-LIO 虽然没有失败，但错误逐渐累积，导致楼梯的弯曲地图结果（图8b和8c）。DLIO 在开始时遇到了错误的关键帧匹配，导致整个楼梯少了一个楼层。它也因错误累积导致弯曲的建图问题（图8d）。另一方面，使用 NV-LIO 的建图结果（图8e）显示出垂直结构，保留了楼梯的实际形状。图8f和8g显示了从 Faster-LIO 和 NV-LIO 获得的结果的放大视图。如箭头所示，由于下层天花板与上层地板之间的不正确对应，Faster-LIO 未能正确建图，而NV-LIO 考虑了法线向量的方向，避免了不匹配，并导致正确结果。

图8：自己的数据集（楼梯间）的建图结果

此外，NV-LIO 在具有不同特征的建筑物中进行了评估。图9a展示了一个带有地下停车场和三层结构的体育馆形状建筑。图9b展示了一个包括五层和六层的研究实验室和教室的建筑，连接到三层建筑。图9c描述了一个由地下停车场和九层地上楼层组成的研究设施。NV-LIO 成功地进行了每个数据集的在线SLAM，持续约一小时，允许对建图结果进行定性评估。所有数据集的建图序列和结果都可在我们的代码存储库中找到。

图9：不同特征建筑物的测绘结果

总结

本文介绍了NV-LIO，这是一种基于法线向量的紧耦合LiDAR-惯性里程计框架，专为室内SLAM应用设计，NV-LIO利用从LiDAR扫描中提取的法线向量进行点云配准、退化检测和闭环检测，以确保在狭窄的室内环境中实现稳健的SLAM性能，通过公共数据集和我们自己的包含各种类型建筑的数据集对所提方法进行了评估。实验结果表明，NV-LIO在准确性和稳健性方面优于现有方法，尤其在狭窄走廊和楼梯等具有挑战性的室内场景中表现出色。