ICRA2021| Intensity-SLAM：基于强度辅助的大规模环境定位和建图

摘要：SLAM是一项基于传感器感知（如LiDAR和相机）可用于估计机器人位置并重建环境的任务。它广泛应用于自动驾驶等机器人应用场景。传统基于 LiDAR 的 SLAM 算法主要利用场景上下文中的几何特征，而忽略了来自 LiDAR 的强度信息。最近一些基于深度学习的 SLAM 算法会考虑强度特征并以端到端的方式训练位姿估计网络。然而，它们需要大量的数据收集工作，并且它们对训练环境以外的环境的普遍性仍不清楚。本文中，我们接收了基于强度特征的 SLAM 系统。我们提出了一种新颖的完整 SLAM 框架，它利用了几何和强度特征。所提出的 SLAM 涉及基于强度的前端里程计估计和基于强度的后端优化。进行了包括室外自动驾驶和室内仓库机器人实验。其结果表明，所提出的方法优于现有的仅几何的 LiDAR SLAM 方法。

I 引言

定位是机器人技术的基本和基本主题之一。随着机器人工业的发展，机器人定位在过去的几十年里变得更加具有挑战性：从已知到未知环境，从简单到复杂环境，从静态到动态环境[1]，从短期到长期定位[2]。通常，固定锚（或路由器）设置在预定义的区域中，机器人位姿通过使用机器人与多个锚的距离来获得，例如超宽带(UWB)[3]和WI-FI定位[4]。但是，这些方法依赖于外部设置，并且主要用于小规模环境。为了解决传统定位方法的局限性，引入了同时定位和建图 (SLAM) 来估计机器人位姿 [5]。它独立于外部设置，因此在机器人应用中很有前景。根据使用的感知系统，SLAM可以进一步分为Visual SLAM（V-SLAM）和LiDAR SLAM。与 Visual SLAM 相比，LiDAR SLAM 对天气和光照等环境变化更加准确和鲁棒 [6]。

传统的 LiDAR SLAM 方法，如 LOAM[7] 和 HDL-Graph-SLAM [10]，主要关注仅依靠几何信息以最小化点云差异。这样，强度信息常常被忽略。请注意，强度信息与材料的反射率有关，并且对于不同的物体是不同的，这对于定位和物体识别很有用。因此，我们认为一个强大的 LiDAR SLAM 系统应该考虑到这样的强度信息。虽然最近有一些工作旨在使用强度信息来提高准确性，但它们主要采用卷积神经网络（CNN）来拟合原始激光扫描数据，没有任何特定的强度分析或公式化。此外，需要收集大量数据来训练神经网络，这在实际应用中可能很麻烦。

在之前的工作 [8] 中，我们已经证明强度信息对于稳健的回环检测非常有用。本文中，我们进一步利用强度信息来提高 SLAM 系统的定位精度。我们提出了一种新颖的 SLAM 框架，它使用几何和强度信息进行里程计估计。我们首先分析强度测量的物理模型。然后，我们在里程计估计公式中为现有的仅依靠几何的代价引入了额外的强度代价。最后，我们结合了基于强度的闭环检测和后端优化来进一步提高性能。所提出的框架使用室内仓库环境机器人和室外自动驾驶汽车进行了测试。结果表明，我们的方法在多种环境中提供了可靠和准确的定位，并且优于仅依靠几何的方法。本文的主要贡献如下：

1）我们提出了一种新颖的 SLAM 框架，它使用强度和几何信息进行定位估计，包括前端里程计估计和后端因子图优化。所提出的方法是开源的。

2）我们建议构建一个强度图来揭示强度分布，并将强度代价引入现有的仅依赖几何形状的 LiDAR SLAM 以估计机器人的位置。

3）对所提出的方法进行了全面的评估。更具体地说，我们的方法在仓库和自动驾驶场景中进行了测试。结果表明，对于多种场景，我们的方法优于现有的仅依靠几何方法。

本文的组织如下：第二部分回顾了现有 LiDAR SLAM 方法的相关工作。第三节描述了所提出方法的细节，包括强度校准、特征选择、里程计估计和基于强度扫描上下文 (ISC) 的图形优化。第四部分展示了实验结果并与一些现有工作进行了比较，然后是第五部分的结论。

II 相关工作

大多数现有的 LiDAR SLAM 工作都集中在环境的几何信息上。最流行的点云匹配方法之一的迭代最近点 (ICP) 方法，该方法将当前点与目标帧中的最近点进行匹配 [9]。ICP 通过最小化点对之间的欧几里德距离来迭代地寻找最佳点对应关系，直到变换矩阵收敛。该算法广泛用于 SLAM 系统，例如 HDL-Graph SLAM [10] 和 LiDAR-Only 里程计、定位[11]。然而，所有点都用于计算，这对于每次扫描数万个点的 LiDAR 来说计算成本很高。ICP 对噪声也很敏感。在自动驾驶等实际应用中，测量噪声（例如，路边树木的测量）可能很重要，随后会导致定位漂移。一种更健壮且计算效率更高的方法是利用几何特征。在 LiDAR Odometry And Mapping (LOAM) [7] 中，通过分析局部平滑度引入了一种简单的特征提取策略。根据局部平滑度将特征点分割为边缘特征和平面特征，然后分别通过边缘到边缘和平面到平面的匹配计算机器人位姿。类似的想法也用于轻型和地面优化的 LiDAR 里程计 (LeGO-LOAM) [12]，其目标是为无人地面车辆 (UGV) 提供计算效率高的 LiDAR SLAM。激光扫描首先被分割成地面点和非地面点。从地面点提取平面特征，从非地面点提取边缘特征。平面特征用于估计滚转和俯仰角以及 z 平移。结果随后用于匹配非地面点以计算 x 和 y 平移以及偏航角。对于接地机器人，LeGO-LOAM 实现了比 LOAM 更高的定位精度。然而，强度信息被忽略，在这些方法中只使用几何信息。

近年来，一些工作试图通过深度学习将强度信息引入 SLAM。在 DeepICP 中，提出了一种基于端到端学习的 3D 点云配准来寻找机器人位姿[13]。强度信息和几何通道都被引入深度特征提取 (DFE) 层以找到关键点。不是搜索最近的点，而是使用对应点生成 (CPG) 层根据学习到的一组候选者之间的匹配概率生成关键点对应关系，这使得反向传播成为可能。Deep-ICP 使用 KITTI [14] 和 Apollo-SouthBay 数据集 [15] 进行了验证，并且优于现有方法，例如 Generalized-ICP [16] 和正态分布变换 (NDT) [17]。Chen等还引入了一个端到端的 CNN 框架来识别后端 SLAM 的两次激光扫描的重叠 [18]。几何信息和强度信息通过 11 层网络进行训练以生成 3 维特征。通过对提取的特征应用另一个较小的层来估计重叠，而通过训练特征的互相关来估计偏航角变化。在 KITTI 数据集上的实验表明，从强度和几何信息估计的偏航角优于仅基于几何的深度学习框架。基于深度学习的方法采用端到端训练，无需进一步分析强度信息。然而，在实践中收集、标记和训练数据通常是困难和耗时的。此外，环境变化下的性能可能不一致，即训练后的网络可能在与训练数据相似的环境中表现良好，但在转移到另一个环境时会失败。

其他的一些工作尝试使用强度信息并将强度集成到位姿估计中，而无需端到端训练。Tian等提出了一种强度辅助 ICP方法，用于 2D-LiDAR 的快速配准 [19]。与传统的ICP算法相比，引入了目标函数，根据空间距离和强度残差确定初始刚体变换估计。迭代次数减少了 10 倍，因此所提出的算法能够在单核CPU 下实时运行。在 [20] 中，强度模型被合并到稀疏集束调整 (SBA) [21] 估计问题中。作者分析了强度测量的物理模型，并提出了一种增强 3D 定位、强度和表面法线的新测量模型。仿真结果表明，强度测量的添加以更少的地标实现了类似的 SLAM 精度。这项工作在 [22] 中进一步扩展，使用飞行时间 (ToF) 相机进行实验。关键点是通过尺度不变特征变换 (SIFT) [23] 或二元稳健不变可缩放关键点 (BRISK) [24] 从强度假想中提取特征来选择和跟踪的。该实验主要侧重于通过计算机视觉技术进行的视觉里程计 (VO)，但并未展示 LiDAR SLAM 在大规模环境中的性能。Khan等提出了一种数据驱动的方法来校准强度信息 [25]。然后通过最小化点对的强度残差而不是几何残差来解决扫描匹配。随后将其集成到 HectorSLAM [26] 中，与原始 Hector SLAM 相比，显着降低了漂移误差。但是，它仅限于小规模环境中的 2D 定位和建图。

III 数学模型

所提出的强度框架包括前端激光里程计和后端闭环检测。系统概述如图2所示。前端包括强度校准、特征提取和里程计估计，后端包括闭环检测和位姿图优化。

A. Intensity Calibration

激光雷达****激光束，测量到达时间和反射信号的能量。物体在传感器坐标中的位置由****角度和距离（即到达时间）确定。强度值由从反射信号接收的能量与****的激光功率之比确定。接收功率Pr的物理原理可确定为[21]：

其中Pe是****激光束的功率，Dr是接收器孔径，ηsys是系统****因子，ηatm是大气****因子，α是物体表面与激光束之间的入射角，ρ是物体的材料反射率。测量距离R和入射角α为外部参数。而ηsys和ηatm是常数参数。因此，强度测量I由[21]确定：

其中，ηall是一个常数。因此，表面反射率ρ仅与入射角α和测量距离R通过：

对于激光雷达扫描，该距离可以很容易地测量到。因此，可以通过分析局部法线来估计入射角。对于每个点p∈P3，我们可以搜索最近的两点p1和p2，以便局部表面法线n可以表示为：

因此，我们可以从具有几何读数P和强度读数I的原始扫描中推导出校准强度扫描I˜，特别是在一些传感器中默认使用距离测量部分校准，因此我们只将校准应用于入射角度。此外，低强度值往往会导致较低的信噪比(SNR)，这也降低了在低值[27]下进行强度测量的测距精度。低强度的点在实践中被忽略了。

B. Salient Point Selection and Feature Extraction

激光雷达扫描通常由数万个点组成。使用ICP等原始点云匹配方法的计算效率较低。此外，原始数据包含自动驾驶从路边树木上测量的点，这将降低匹配精度。因此，将点云与[7]，[13]特征相匹配更健壮和计算效率。本文使用了基于几何形状和强度信息的特征，而不是只使用几何形状的特征。校准的强度信息包含揭示不同物体分布的环境的反射率轮廓。因此，强度信息也有助于识别在多个帧中的相同特征。对于点Pi∈P及其强度值ηi˜∈i˜，我们搜索附近的Ni∈P，并计算局部距离分布σgi和强度分布σIi通过：

其中，wg和wI分别是几何形状和强度分布的权重。请注意，对于墙等平面，平滑度值较小；而对于拐角点或边缘点，平滑度值较大。因此，对于每次扫描，边缘特征从大σ点中选择Pε∈P，并从低σ点中选择平面特征Ps∈P。

C. Intensity Map Building

强度地图M包含了周围环境的反射率分布。对于大多数只有几何的SLAM，地图通过占用网格[28]或Octomap [29]维护和更新。三维空间被分割成网格单元，每个单元都由一个概率函数表示。类似的想法可以用于构造和更新一个强度图。我们使用强度测量I(ηi|z1:t)来表示每个网格单元mi[26]，而不是概率函数。更具体地说，对于时间t观察网格单元，表面反射率可以通过：

其中M(mi|z1:t)是当前的强度观测值，nmi是对mi的总观测次数。要注意，如果网格不包含对象，则强度将标记为0，因为没有反射信号。

D. Scan to Map Matching

激光里程计是估计当前帧到全局图之间的变换矩阵T∈SE(3)的任务。通过最小化几何误差和强度误差来计算最佳位姿估计：

1)几何残差：与LOAM[7]类似，通过将当前边缘和平面特征与全局地图匹配来计算几何误差。它可以通过最小化点到边和点到平面的残差来实现。给定边缘特征 pi∈Pε和变换点 pˆi = Tpi，我们可以从全局地图中搜索两个最近的点 pε1 和 pε2。点到边残差定义为：

2)强度残差：将特征与强度图匹配计算强度残差。可以通过最小化当前点pi（包括边缘特征和平面特征）与强度图中的转换点pˆi之间的强度残差来实现：

其中M(pˆi)是强度图M中的点强度值，为了在强度图中搜索强度信息，我们引入了三线性插值。虽然用最近的网格单元来表示强度更直接，但强度信息不太精确，特别是对于网格分辨率较低的大尺度建图。对于每个转换点pˆi=[xi，yi，zi]T，我们可以找到周围的八个网格单元。如图3所示，这些网格单元的强度测量结果标注为M1（pˆi）、M2（pˆi）、……、M8（pˆi）。使单元1的中心位置（最接近原点）为p1=[x1、y1、z1]T，并且单元8的中心位置（离原点最远）为p2=[x2、y2、z2]T，其中x2−x1、y2−y1和z2−z1分别为每个网格单元的宽度、高度和深度。目标点的强度估计计算为：

同样地，我们也可以推导出M34、M56和M78。

3)位姿估计：可通过最小化几何残余和强度残余来估计最终位姿：

它可以用LM算法[30]来求解。注意，通过假设恒定的角速度和线速度，相比假设相同的变换矩阵可以增加收敛速度。

E. Loop Closure Detection & Global Optimization

闭环检测的目标是从历史数据中识别出重新访问的场景。对于仅前端的 SLAM 系统，不可避免地会出现测量噪声的漂移。为了减少定位漂移，后端 SLAM 通过识别相同的位置来回环检测。我们使用关键帧选举来减少检索时间，因为闭环检测的计算成本通常很高。关键帧的选择基于以下标准：(1) 机器人的位移是显着的，即大于预先定义的阈值；(2)旋转角度变化显着；(3)经过的时间超过一定时间。对于大规模环境，阈值设置得更高以降低计算成本。所有关键帧都存储在后端维护的位姿图中。

对于每个关键帧，我们使用强度扫描上下文 (ISC) [9] 将当前帧提取为全局签名。与 GLAROT3D [31]、NBLD [32] 和扫描上下文 [33] 等仅几何描述符相比，ISC 在识别回环闭合时对旋转变化具有鲁棒性。ISC 是一个二维矩阵，通过将极坐标在方位角和径向方向上等分为 Ns 个扇区和 Nr 个环来计算。每个子空间由区域内点的最大强度表示。给定一个关键帧，我们可以从强度和几何信息中提取 ISC 描述符Ω。为了将查询 ISC 描述符Ωq与候选 ISC 描述符Ωc进行比较，让 Vqi 和 Vci 是 Ωq和Ωc 的第 i 列。相似度分数φ(Ωq,Ωc) 是通过取每个扇区的平均余弦距离来找到的：

可以通过在φ(Ωq,Ωc)上设置一个阈值来确定回环检测。

回环检测能够有效地识别环路对。然而，错误检测会导致位姿图优化失败。为了防止误报，使用几何一致性验证来检查候选帧的相似性。对于候选循环帧，我们从全局地图中搜索附近的线和平面信息。并且对于当前帧，通过最小化点到边缘和点到平面的距离来提取边缘和平面特征并将其匹配到相应的全局线和平面。当两帧不相关时，距离的总和通常很高。因此，可以通过对其设置阈值来过滤掉误报。确定重新访问的位置后，我们可以将两帧之间的边添加到位姿图中，并且可以应用全局优化来校正漂移[34]。

IV 实验评估

B. Evaluation Metric

为了评估定位的准确性，我们使用平均平移误差(ATE)和平均旋转误差(ARE)[15]：

实验结果：

1）在KITTI数据集上研究结果。从左到右为序列00、02、05、07和09。强度-SLAM的结果为绿色，参考值为红色。

2）KITTI数据集中05序列多种方法的对比

3）数据集上强度辅助方法和非强度辅助方法的结果分析

4）KITTI数据集中05序列上不同方法的性能比较

5）我们在仓库环境中的实际机器人上测试我们的方法的性能。图6(a)为实物图；图6(b)为SLAM结果，将机器人的轨迹绘制为红色。

V 结论

本文中，我们为基于 LiDAR 的定位系统提出了一种新颖的强度辅助完整 SLAM 框架。现有方法主要利用仅依靠几何特征来估计位置并忽略强度信息。我们从强度测量的物理模型开始，并说明强度信息的使用。我们引入了用于位姿估计的强度图和强度残差，以提高定位精度。此外，我们提出了一个由前端和后端系统组成的完整 SLAM 结构，即 Intensity-SLAM。在不同场景下进行了实验，包括仓库中的室内 AGV 和市区的室外自动驾驶。实验结果表明，通过集成强度信息，定位精度得到显着提高。与最先进的仅依靠几何方法相比，所提出的方法还实现了较低的平移误差和旋转误差。