NV-LIO: 基于法向量的激光雷达-惯性里程计

在过去的几十年里，已经开发了许多激光雷达-惯性里程计（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估计的旋转进行时间插值。该过程在激光雷达坐标系中进行，以便后续的球面投影步骤。

球面投影与法向量提取

在对运动补偿后的激光雷达点云进行球面投影时，会生成一个深度图像。根据激光雷达点云的特性（如通道数、水平分辨率和视场角），手动选择深度图像的大小。通过指定参数（最大和最小垂直视场角、深度图像的高度和宽度），可以计算出图像的垂直和水平分辨率，并将每个点建图到图像坐标。法向量通过对深度图像中点的深度值进行水平方向和垂直方向的差分计算得到。在室外环境中，相邻像素覆盖的区域较大，可以减小测距噪声的影响；而在狭窄的室内环境中，相同区域较小，测距噪声影响显著增大。因此，采用基于窗口的差分方法，假设窗口内的导数值相似，通过计算并平均水平方向和垂直方向上的导数值来减少测距噪声的影响。随后，将球面坐标系中的法向量转换为笛卡尔坐标系中的法向量，基于每个像素的方位角和仰角进行变换。由于所有像素的变换矩阵是固定的，可以提前预计算并存储以节省计算时间。如果检测到的表面与光线方向平行或朝向光线，则反转法向量。最后，通过计算点与其邻域内法向量形成的平面的距离，验证法向量是否与邻域内的点一致。如果有超过三分之一的点到平面的距离小于5厘米，该法向量被认为有效。通过这一过程，最终获得包含法向量的法向量云。图3展示了从输入点云中提取法向量的整体过程。

图3：法向量提取过程

法向量云配准

扫描到扫描的匹配可能会导致不可靠的配准结果，尤其是在激光雷达进行滚转或俯仰旋转以及扫描旋转轴的平移时。为了解决这一问题，我们采用了扫描到子图的匹配方法。子图通过将前序关键帧的法向量云累积在最后一个关键帧的坐标系中生成。

为了准确进行对应搜索和快速匹配，当前查询帧通过IMU整合从其最后获得的位姿转换到初始位姿。通过了解目标和查询帧的世界坐标系，可以确定这两帧之间的初始相对位姿。利用这一信息，目标帧被转换到查询帧的坐标系中进行匹配。为了加快匹配速度，当前法向量云和子图通过体素网格过滤器进行下采样。

为了实现稳定的匹配，必须满足两个标准：首先，点对点距离在距离阈值之内；其次，法向量的方向差异在角度阈值之内。为此，我们首先利用kd树选择在距离阈值内的子图点，然后计算所选点与查询点之间法向量方向的角度差异。如果角度差异在阈值范围内，则将这两点视为对应对。

随后使用高斯-牛顿方法计算相对位姿，利用对应对计算每对的残差成本函数。通过解决优化问题，可以计算出查询帧与目标帧之间的相对位姿。结果的相对位姿被转换为相对位姿因子并添加到因子图中。

闭环检测

全局闭环检测算法在具有重复结构特征的多楼层室内环境中常常面临挑战，特别是在楼梯间，特征的重复性可能导致在楼层间转换时与不同楼层的点云错误关联。因此，本研究采用了一种闭环检测方法，重点在于匹配当前位置信息附近的关键帧，而不是进行全局搜索。

虽然像ICP或GICP这样的技术通常使用半径搜索来找到最近点作为对应，但在局部闭环检测中，往往会导致对齐不准确，尤其是在狭窄的室内空间中。这主要是因为室内环境通常由多个分割区域组成，即使在激光雷达位置稍有变化时，点云之间也会存在显著差异。为了解决这一问题，我们引入了一种基于视点的闭环检测方法，灵感来源于投影技术，以提高对应搜索的效果。

在基于视点的闭环检测中，首先使用每个关键帧的位置构建kd树，然后选择离当前帧最近的关键帧。在此过程中，紧接着当前帧的关键帧被排除。一旦识别出闭环检测的候选关键帧，将候选关键帧的法向量云转换为当前帧的激光雷达位姿。然后将其投影到当前激光雷达的视点以进行匹配。在球面投影期间，分配到同一像素的点中，仅分配最近的点。

然而，由于激光雷达点云的稀疏特性，这种投影方法可能未能排除不应从当前视点看到的点。为了解决这个问题，我们利用与光线方向形成90度角的法向量点。为了排除通过这些点的法向量点，选择在光线方向超过90度角的点。随后，通过基于径向距离偏差和与下采样当前法向量云投影图的角度偏差过滤投影点，获得对应对。与法向量云配准方法相似，匹配的点对应对经过优化以估计相对位姿，然后作为闭环因子插入位姿图中。

图4：基于视点的楼层间闭环检测示例

退化检测

在室内环境中，许多表面往往平行排列，导致退化情况的发生。楼梯间或长走廊等环境（如图5所示）便是此类情况的典型例子，其中表面的法向量仅分布在两个方向上，导致在剩余方向上的平移模糊。例如，在楼梯间中，形成楼梯间的墙壁的法向量在水平方向上分布，导致在水平方向上的高定位精度，但在垂直方向上则可能存在模糊性。

为了计算适合这种退化情况的匹配不确定性，可以利用匹配法向点的法向量分布。通过对法向量进行主成分分析（PCA）来获得此分布。首先，计算法向量的协方差矩阵。随后，使用特征值分解对协方差矩阵进行分解，得到特征向量和特征值。最小特征值指示如果小于特定阈值，则表示发生了退化情况。通过这种方式，可以在匹配时插入基于法向量分布的测量协方差。

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

位姿图 SLAM 框架

位姿图由初始位姿的先验因子、通过法向云注册获得的相对位姿因子、来自闭环检测的闭环因子以及来自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数据集和我们自己的数据集。

图6: 自己的数据集的数据采集系统

为了评估所提方法在多层室内环境中的有效性，采用了SubT-MRS数据集中的多层数据集进行评估。该数据集包含由配备16通道LiDAR和IMU的四足机器人获得的数据，机器人从一层到三层穿越建筑的内部和外部。Newer College数据集是在校园环境中使用手持设备捕获的，并使用LiDAR及其内置IMU进行评估。我们的数据集是在KAIST校园的各种建筑中使用手持设备获取的，如图6所示。这里将算法与最新的公开算法进行比较，包括LIOSAM、Fast-LIO2、Faster-LIO和DLIO。所有测试均在配备12核Intel i7-12700 CPU的计算机上在线进行。

实验结果

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

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

Newer College数据集： Newer College数据集的结果如表I和表II所示。所有算法的降采样体素大小固定为0.4米，NV-LIO的关键帧插入距离设置为1.0米进行评估。评估是基于提供的与IMU帧对齐的真实轨迹进行的。绝对位姿误差通过在Umeyama对齐后使用翻译均方根误差进行测量。结果表明，NV-LIO需要额外的预处理，导致平均计算时间增加。然而，与最新的户外方法相比，它显示出相似或更低的误差，并在室内环境中表现出更大的鲁棒性。特别是在非常狭窄的环境（如楼梯数据集）或快速运动的场景（如地下硬数据集）中，其性能良好。

图8: 我们自己的数据集（楼梯）的建图结果

KAIST建筑数据集： 图8展示了五层建筑的楼梯数据集结果，如图1所示。LIO-SAM在进入楼梯间时未能注册，导致建图失败，无法进一步进行（图8a）。虽然Fast-LIO2和Faster-LIO没有失败，但在上楼梯的过程中错误逐渐累积，导致楼梯的曲线建图结果（图8b和8c）。DLIO在开始时遭遇错误的关键帧匹配，导致整座楼梯缺失一层，并且由于错误的累积而导致相同的曲线建图问题（图8d）。另一方面，使用NV-LIO的建图结果（图8e）显示出垂直结构，保持了楼梯的实际形状。图8f和图8g分别表示Faster-LIO和NV-LIO结果的放大视图。如箭头所示，Faster-LIO由于下层天花板与上层地板之间的不正确对应而未能正确建图，而NV-LIO考虑了法向量的方向，避免了不匹配，从而得出正确结果。此外，NV-LIO在具有不同特征的建筑中也进行了评估。图9a描绘了一个带有地下停车场的体育馆形状建筑和一个开放中心的三层结构。图9b展示了一个包含五层和六层的研究实验室和教室的建筑，与三层建筑相连。图9c表示一个包含地下停车场和九层地上层的研究设施。NV-LIO成功地在每个数据集上进行了在线SLAM，持续约一小时，允许对建图结果进行定性评估。

图9: 具有不同特征建筑的建图结果

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