NV-LIO：一种基于法向量的激光雷达-惯性系统（LIO）

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

作者：Dongha Chung, Jinwhan Kim

NV-LIO：一种基于法向量的激光雷达-惯性系统（LIO）NV-LIO利用从激光雷达扫描中提取的法向量进行云配准、退化检测和闭环检测，以确保在狭窄的室内环境中具有鲁棒的SLAM性能。所提出的方法通过公开数据集和我们的数据集进行了评估，涵盖了各种类型的建筑。https://mp.weixin.qq.com/s/i4kcbitGMv_HRVdo-YAaFA摘要：在过去的几十年里，已经开发了大量的激光雷达惯性测距（LIO）算法，在多样化的环境中表现出了令人满意的性能。然而，这些算法大多数主要在开放的户外环境中得到验证，它们在封闭的室内环境中经常会遇到挑战。

在室内环境中，尤其是在多层建筑中，由于激光雷达扫描的快速变化以及重复的结构特征，如墙壁和楼梯，稳定的点云配准变得问题重重。在本文中，我们提出了NV-LIO，这是一个基于法向量的LIO框架，专为室内多层结构环境中的同时定位与地图构建（SLAM）而设计。

我们的方法从激光雷达扫描中提取法向量，并利用它们进行对应搜索以提高点云配准性能。为了确保配准的鲁棒性，分析了法向量方向的分布，并检查了退化的情况以调整匹配不确定性。此外，还实现了一个基于视点的闭环模块，以避免被墙壁阻挡的错误对应关系。所提出的方法通过公共数据集和我们自己的数据集进行了验证。

01 引言

移动机器人领域通过传感器、执行器和控制算法的进步，兼容了更多的操作环境。这些进步正在重塑机器人领域，将它们的应用从自动驾驶扩展到包含消防机器人、安全机器人、送货机器人等多样化的环境，这些机器人在封闭的室内环境中需要克服各种障碍。这些机器人要在未知的室内环境中导航，就需要可靠的SLAM技术。

激光雷达传感器可以直接测量周围环境的深度信息，无论环境亮度如何，都能提供稳定的测量。利用这些功能，三维（3D）激光雷达主要用于通过为移动机器人应用匹配连续的激光雷达点云来进行定位和建图。在过去的几十年里，大量公共数据集为自动驾驶应用而发布，这些数据集的开发和验证推动了激光雷达-惯性测姿算法技术的显著进步。

然而，这些算法在室内空间中性能下降是众所周知的，在多层环境中经常会失败。与户外环境不同，室内环境的特点是空间狭小、墙壁单薄，形成多个分割区域。在这些区域中，由激光雷达扫描捕捉到的场景可能会因为墙壁和楼梯等重复的结构元素而迅速变化。由于这些环境因素，现有算法在点云配准过程中经常会失败。造成这种配准失败的原因之一是难以将墙两侧的点云对齐，这就是所谓的双边问题[1]。尽管墙壁有一定的厚度，但这些错位可能会在地图中将墙表现为没有厚度。另一个问题是固定参数问题。在狭窄的空间中，激光雷达扫描通常会在近距离产生密集的点云。然而，当使用固定参数进行下采样时，用于匹配的点的数量减少，可能导致对齐过程中的不准确或不匹配 [2]，[3]。

在本文中，我们提出了NV-LIO，一个基于法向量的紧耦合的多楼层室内SLAM的LIO框架。该框架利用了将密集旋转式的3D激光雷达扫描投影成范围图像的能力，从这些图像中我们提取法向量。扫描之间的配准不仅考虑了最近的邻居，还考虑了法向量的角度差异，从而提高了配准过程中对应搜索的准确性。在匹配过程中，分析法向量方向的分布以评估退化的发生，调整匹配的不确定性。为了确保准确的闭环，在扫描与子图之间的匹配过程中采用了可见性分析，防止了不同房间或楼层之间的错误对应。

本文的主要贡献可以总结如下：

• 我们引入了一种紧耦合的LIO框架，利用密集机械LiDAR点云中的法向量，在狭窄的室内环境中实现了鲁棒的配准。

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

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

02 相关工作

激光雷达（惯性）测距的关键组成部分在于点云配准的有效性。为了处理每个扫描中成千上万点的计算负载，已经出现了各种方法来减少计算时间。基于特征的方法从点云中提取关键特征进行匹配，而直接法涉及对点云进行降采样并使用迭代最近点（ICP）[4]或广义迭代最近点（GICP）[5]等技术。

在基于特征的算法中，LOAM [6] 利用点头运动的2D激光雷达扫描，基于相邻点之间的关系提取角点和平面点。LeGO-LOAM [7] 采用了LOAM的特征提取技术来处理3D激光雷达，从每个环中提取特征，并结合地面分割以提高准确性。

LIO-SAM [8] 采用了类似的特征提取方法，但引入了与IMU预积分的紧耦合LIO，展示了高性能。Fast-LIO [10] 采用了相同的特征提取方法，但引入了新的卡尔曼增益公式以加快计算速度。基于其速度优势，Fast-LIO2 [11] 引入了一种使用ikd-tree [12] 的直接匹配方法进行快速最近邻搜索，以实现高效的扫描到地图匹配，而不是传统的扫描到扫描匹配，从而获得更全局一致的性能。另一方面，Faster-LIO [13] 将Fast-LIO2的树结构替换为增量体素化（iVox）系统，以实现更快的计算速度。

虽然这些直接方法在户外环境中表现出卓越的性能，但它们在狭窄的室内环境中经常失败，其中点云在接近区域内密集排列。这些算法的快速计算依赖于点云的降采样，这可能无法在拥挤的室内环境中保留用于可靠配准的有意义点。

为了解决这个问题，已经提出了根据激光雷达扫描的分析自适应调整参数的方法。基于Faster-LIO，AdaLIO [2] 通过分析给定的激光雷达扫描自适应改变体素化大小、搜索半径和用于平面选择的残差边距，方便在退化情况下实现稳定的配准性能。在DLIO（DLIO：深度学习惯性测距）[14]和[3]中提出的工作中，提出了自适应关键帧插入方法，用以平衡高效的计算时间和可靠的点云配准。这些方法根据扫描分析插入关键帧，导致狭窄空间中关键帧密集，宽阔区域中关键帧稀疏。

SuMa [15] 采用两步过程，其中激光雷达点云首先投影为深度图像，然后提取法线以形成surfels。通过匹配surfel地图和测量的surfels进行SLAM，根据匹配结果更新surfels。为了加快对应搜索，采用渲染技术，将surfel地图投影到当前帧以找到图像中的对应关系。LIPS [16] 利用室内场景中平面（如办公室）的普遍性，从激光雷达扫描中提取平面原语，并使用匹配这些原语的技术。文献[1]中的方法首先为每个点提取法线向量，并使用聚类识别平面。其中介绍了一种名为前向ICP流动的方法，利用点到平面距离找到对应现有平面的新的扫描点，而不是在每次扫描中找到平面。在匹配过程中，如果平面法线向量与现有平面法线向量之间的角度差超过某个阈值，则不进行匹配，从而有效解决双侧问题。

03 方法

3.1 系统概述

系统状态 $\pmb{X}$ 表示为：

其中 $\pmb{R,t,v}$ 分别为全局参考坐标系中的旋转矩阵、位置和速度， $\pmb{b}_a$ 和 $\pmb{b}_g$ 分别为 IMU 框架中的 IMU 加速度和陀螺仪偏差。当设置一个与 IMU 的坐标系统不同的机体坐标系时，需要根据坐标变换对 IMU 加速度和角速度进行变换。为了方便和减少额外的计算，将系统的机体坐标系与 IMU 坐标系对齐。

关键帧 $\mathbb{K}$ 表示为：

其中 $\pmb{x}$ 为在扫描时身体在世界坐标系中的姿态， $\pmb{N}$ 为包含法线点 $P_i$ 的法线云， $P_i$ 由点坐标 $\pmb{p}_i$ 和法线向量 $\pmb{n}_i$ 组成。

与户外环境不同，在狭窄的室内空间中，当前扫描中地图点的可见性可能非常有限。由于这种特性，直接将扫描与地图匹配可能导致漂移，特别是在狭窄的走廊或楼层过渡期间，在返回相同位置时校正困难。因此，在本研究中，我们采用基于关键帧的姿态图 SLAM 框架。地图以关键帧的集合形式形成，允许它动态地响应校正。

图2为所提出算法的概述。通过使用 IMU 陀螺仪测量的姿态补偿扫描中的运动。使用球形投影从运动补偿的云中提取法线云。通过在由先前关键帧组成的小地图之间的法线云配准确定相对位姿。此外，通过基于视角的循环关闭获得校正测量。这些配准结果作为相对位姿因子包含在图中，并通过 IMU 预积分将 IMU 测量值添加到图中。通过这种位姿图优化，估计了当前位姿和 IMU 偏差。

3.2 预处理

为了实现准确的法线向量和配准结果，对激光雷达扫描的运动补偿是必要的。为此，我们利用了具有估计偏差的 IMU 获得的角速度。考虑到从激光雷达接收到的点频率（超过 10000 Hz）与 IMU 频率（100 Hz）的差异，我们使用 IMU 估计的旋转，基于初始传入点的时间戳进行时间插值。这个过程在激光雷达坐标系中进行，以便于后续的球形投影步骤。

3.3 球形投影与法线提取

在运动补偿的激光雷达点云上执行球形投影以生成深度图像。在此过程中，深度图像的大小是手动选择的，考虑到激光雷达点云的特征，如激光雷达通道数、水平分辨率和视场角（FoV）。给定最大垂直 $FOV(fov_{max})$ 、最小垂直 $FOV(fov_{min})$ 、深度图像高度（h）和宽度（w）的参数，垂直分辨率是 $ver_{res}=(fov_{max}-fov_{min})/h$ ，水平分辨率是 $hor_{res}=2\pi /w$ 。每个点 $(\pmb{p}(x,y,z))$ 的图像坐标 $(u,v)$ 如下：

法线向量可以通过在范围图像的水平方向( $\Delta r/ \Delta \psi$ )和垂直方向( $\Delta r/ \Delta \theta$ )对深度值( $r(u,v)$ )进行微分来计算：

其中 $\theta = \pi /2-(fov_{max}-v \times ver_{res})$ 表示极角， $\psi = \pi - u \times hor_{res}$ 表示方位角，c是一个缩放变量，使法向量成为一个单位向量。

在广阔的户外环境中，相邻像素覆盖的区域较广，减轻了距离测量噪声对法线向量计算的影响。相比之下，在狭窄的室内环境中，相同的区域要小得多，放大了距离测量噪声对计算结果的影响。考虑到这些因素，我们不再简单地使用相邻像素之间的微分，而是应用基于窗口的方法，假设窗口内的导数值相似。为窗口内每一对在水平和垂直方向上的导数值计算并求平均值，以减轻距离测量噪声的影响。在每个像素的方位角和俯仰角的基础上，将用球坐标表示的法向量 $\pmb{n}^s$ 转换为笛卡尔坐标 $\pmb{n}^c$ ， $\pmb{n}^c(u,v)=T(\theta ,\psi)\pmb{n}^s(u,v)$ ，其中 $T(\theta,\psi)$ 是转换矩阵。公式如下：

由于所有像素的转换矩阵保持恒定，通过预先计算并存储所有像素的转换矩阵来减少计算时间。

由于检测到的表面不能面向平行或射线( $\vec{\pmb{r}}=\pmb{p}/|\pmb{p}|$ )的方向，当法线向量与射线方向( $\pmb{n}^c·\vec{\pmb{r}}$ )的点积为正时，将法线向量反转。最后，为了验证法线向量是否与窗口内邻近点形成共识，计算了点与由其法线向量和邻近点形成平面之间的距离。如果窗口大小内小于三分之一的点与平面之间的点到平面距离在5厘米以内，则认为无效。通过这一过程，获得了包含法线点P的法线云N。图3显示了从输入云中提取法线的过程。

3.4 正常云配准（Normal Cloud Registration）

扫描对齐可能特别难以处理激光雷达在滚转或俯仰旋转以及扫描旋转轴上的运动。为了应对这个问题，我们采用了与LIO-SAM类似的扫描到子地图匹配方法。子地图是通过在前一个关键帧坐标系中累积前一个关键帧的法线云来生成的。对于最后一个关键帧 $\mathbb{K}_n$ ，增强前 $j$ 个关键帧的子地图 $N_n$ 如下所示：

其中 $N^k_i$ 表示将关键帧 $\mathbb{K}_i$ 中的法向量云转换为关键帧 $\mathbb{K}_k$ 的坐标系，∪表示法向量云的增强。

为了准确地进行对应搜索和快速匹配，我们IMU积分将当前查询帧 $\mathbb{K}_q$ 从其最后获得的姿态变换到初始姿态。知道目标帧和查询帧的世界坐标系，我们可以确定两个帧之间的初始相对姿态。利用这些信息，我们将目标帧转换为查询帧的坐标系，并继续进行匹配过程。之后，为了加快匹配速度，我们使用体素栅格滤波器对当前法线云 $N_q$ 和子地图 $N_n$ 进行下采样。

为了在结果法线云之间实现稳定的匹配，我们建立了满足以下两个条件的对：首先，点对之间的点到点距离在距离阈值内；其次，法线向量方向之间的差在角度阈值内。首先，我们使用kd-树在每个查询点的当前法线云中选择距离阈值内的子地图点。然后对于选中的子地图点按顺序，计算所选点与查询点之间的法线向量方向差。如果角度差在角度阈值内，这两个点被选为对应对。

之后，我们使用高斯-牛顿方法利用对应对计算相对位姿。每个对的残差成本函数计算为点到平面距离，目标帧相对于查询帧的相对位姿可以通过解决以下优化问题来计算：

得到相对位姿然后转换为相对位姿因子添加到因子图中。

3.5 循环关闭检测

全局循环检测算法在具有重复结构特征的多楼层室内环境中经常遇到困难。这种挑战在楼梯间尤为明显，其中特征的重复性可能导致与不同楼层的云关联错误。

尽管ICP或GICP等局部循环检测方法经常使用半径搜索找到最接近的点作为对应点，但它们在狭窄的室内环境中经常导致错位，这主要是因为室内环境通常由多个分割的区域组成，即使激光位移很小的变化也会导致激光扫描有显著的变化。

为了解决这个问题，我们引入了一种基于视角的循环关闭检测方法，灵感来自文献[15]中的投影技术，以改善对应搜索。图4说明了基于视角的循环检测的一个例子。使用每个关键帧的位置构建kd树，然后选择当前帧最近的帧。在此过程中，前于当前帧的关键帧将被立即排除在kd树之外。一旦识别出循环关闭候选关键帧，就将候选关键帧的法线云变换到当前帧的激光位姿中。然后，如图4c所示，将其投影以匹配当前激光的视角。在球形投影期间，分配给同一像素的点仅分配给最接近范围的点。

这种投影方法可能不会排除由于激光点云的稀疏性而本应从当前视角看不到的点。为了解决这个问题，我们利用了位于射线方向90度范围内的法线点，表示为N^+。由于这些点代表墙壁的另一侧，我们期望排除任何超出90度角的法线点。然后，通过将投影点基于径向距离偏差和与下采样的当前法线云投影图像的角偏差进行过滤（如图4b所示），获得对应点对（如图4d所示）。类似于3.4中的法线云配准方法，通过优化这些匹配的点对应对来估计相对位姿。然后将这些插入到位姿图中作为循环关闭因子。

3.6 退化检测

在室内环境中，许多表面通常平行排列，导致退化情况的发生。如图5所示，楼梯井或长走廊等环境就是这样的例子，其中表面的法线向量仅在两个方向上分布，导致沿剩余方向的平移模糊。例如，在楼梯井的情况下，形成楼梯井的墙面的法线向量在水平方向上分布，导致在水平方向上具有高定位精度，但在垂直方向上可能存在模糊。

为了计算退化情况下的适当匹配不确定性，可以使用匹配法线点中的法线向量的分布。这可以通过法线向量的主成分分析来获得，如下所示：首先计算法线向量的协方差矩阵C：

后，使用特征值分解将协方差矩阵C分解为 $C=V\Lambda V^{-1}$ ，其中V是由特征向量组成的矩阵，Λ是对角元素为特征值的矩阵：

其中 $\lambda _0 <\lambda _1 <\lambda _2$ 。可以使用特征值来近似法线向量的分布，其中最小的特征值 $\lambda _0$ ，如果它低于某个特定阈值，则表示退化情况。随后，每个特征值 $\lambda _i$ 对应一个特征向量 $v_i$ ，可以将测量协方差 $Q$ 设置为：

其中 $s$ 是一个给定的常数。当与前一个关键帧匹配时，我们使用这种基于分布的测量协方差在退化情况下插入因子。然而，在循环关闭等高概率错误匹配的情况下，如果检测到退化，我们避免插入循环因子以确保稳定性。

3.7 姿态图SLAM框架

姿态图由以下部分组成：对初始姿态的先验因子、通过普通点云配准获得的相对姿态因子、来自闭环检测的闭环因子，以及来自IMU预积分的IMU因子和恒定偏差因子。

在上一次姿态图优化结果中反映出的偏差IMU测量值被整合，以在IMU速率下持续估计当前帧。如果当前帧与上一帧之间的姿态差异超过某个阈值，则会插入新的关键帧。该姿态图是使用iSAM2框架[17]构建并优化的。

04 实验

在球面图像投影中使用的像素数量被设置为通道数乘以1024。为了稳定提取法向量，对于32个通道或更少的激光雷达，使用了3乘3的窗口；而对于超过32个通道的激光雷达，则使用5乘5的窗口来计算法向量。法向量云配准的距离阈值设置为0.5米，根据场景的不同，降采样体素大小设置为0.4米或0.2米。对于关键帧，如果与前一关键帧姿态的角度差超过30度，或者距离差异大于某个阈值，则会增加新的关键帧。这个距离阈值根据建筑物特征被设置为1.0米或0.5米。闭环检测的距离阈值被设置为10米。