《NDT-LOAM: A Real-Time Lidar Odometry andMapping With Weighted NDT and LFA》深大学生

Abstract

         激光雷达同时定位和建图（Lidar-SLAM）从激光雷达上处理点云，并完成定位和建图。激光激光通常分为前端里程计和后端优化，可以并行运行以提高计算效率。前端里程计通过处理点云来估计激光雷达的运动，在点云配准中通常采用正态分布变换（NDT）算法。为了减少累积误差，本文提出了一种加权NDT结合局部特征调整（LFA）来处理点云，提高计算精度。

        根据范围的值及其表面特征对NDT栅格进行加权，构建新的权重成本函数。在实验中，我们在KITTI测程数据集上测试了NDT-LOAM，并与最先进的算法ALOAM/LOAM进行了比较。NDT-LOAM的平移平均漂移为0.899%，优于ALOAM和LOAM水平；此外，NDT-LOAM可以在10 Hz实时运行，而LOAM运行在1 Hz。结果表明，NDT-LOAM是一种实时的低漂移方法。此外，源代码被上传到GitHub，下载链接为https://github.com/BurryChen/lv_slam。

I. I NTRODUCTION

        

        （SLAM）问题在机器人界得到了广泛的研究，关键是如何构建或更新未知环境的地图，同时跟踪机器人的位置[1]-[3]。通常，SLAM系统分为前端和后端部分；具体来说，前端部分通常是里程计，机器人的运动是通过SLAM传感器（Camera或Lidar）的测量过程来估计的；后端完成地图构造，通过环路闭合检测和地图优化[4]减少累积漂移。SLAM技术已被广泛研究和应用于无人驾驶汽车、无人机、行星漫游车、新型家用机器人、室内定位系统，甚至在人体[5]-[8]内部。根据SLAM使用的传感器，主要分为视觉SLAM/[9]-[12]和Lidar SLAM/[13][13]-[20]。 

        视觉SLAM处理图像序列，估计机器人的运动，并对被包围的环境进行构图。Visual SLAM方法一般可以分为两类：间接（基于特征的）方法和直接方法[11]。类似地，这个概念可以扩展到激光雷达SLAM。激光雷达SLAM对激光雷达收集的点云进行处理，并根据点云匹配对运动进行估计。间接激光雷达SLAM方法以前计算原始传感器测量的中间表示对应，然后通过最小化再投影误差来估计传感器运动。对应的预算步骤通常是通过提取和匹配稀疏/密集的关键点（或特征点）和其他几何基元的参数表示来解决的，如线或曲线段。直接方法跳过这个预先计算的步骤，直接使用传感器测量（相机的光度测量或激光雷达的几何量），共同估计运动和对应点。

        与照相机相比，激光雷达可以获得大规模和高精度的测量数据，而不受光照条件[21]的影响。激光雷达是近年来SLAM应用中非常重要的传感器。在这项工作中，我们关注激光雷达光学测量，我们提出了一个实时激光雷达光学测量（NDT-LOAM）使用改进的加权正态分布变换（NDT）和关键帧匹配策略。根据NDT中的栅格的范围和表面特征进行不同的权重处理。进一步采用简单的启发式来选择该框架。与目前的激光雷达建图（LOAM）[15]方法相比，该方法在效率和精度上都显示了令人满意的性能。

II. R ELATED W ORK

         激光雷达在环境传感方面具有明显的优势，在SLAM中引起了广泛的关注。研究人员已经发表了大量关于激光雷达SLAM的工作，包括提高其效率，减少累积误差和地图优化。在基于图的SLAM流行之前，贝叶斯滤波技术是激光雷达SLAM社区[22]中模型获取的一种选择方法。基于贝叶斯的SLAM将SLAM问题作为移动测量平台的姿态状态估计问题，如Hector SLAM [23]、Gmapping[24]等。在不断合并新的观测数据的同时，它使用扩展卡尔曼滤波器（EKF）、粒子滤波器（PF）或信息滤波器（IF）来增强和更新姿态状态。基于图的SLAM也被称为全SLAM [25]，具有代表性的开源算法有Karto SLAM [24]、cartographer[14]等。

        它构造一个图的节点对应于平台提出在不同的时间点和边缘表示约束之间的姿态从观测，然后地图可以计算通过找到的空间配置节点，主要是符合测量建模的边缘[26]。

        对于具有六个自由度（6-DOF）的3D激光雷达SLAM的最新进展，我们提出了三种最先进的方法：Cartographer、LOAM和IMLS-SLAM。谷歌的Cartographer[14]结合了扫描到子地图匹配与循环闭包检测和图优化。局部子地图轨迹是使用一种称为相关扫描匹配（CSM）的算法创建的。在后端，所有扫描都使用像素精确的CSM与附近的子地图进行匹配，以创建循环闭包约束。在后端对子映射的约束图和扫描姿态进行了周期性的优化。因为CSM是基于概率网格的，而没有预先计算的对应关系，所以Cartographer是一个直接的方法。在该方法中，环路闭包检测和图优化可以纠正累积误差，提高建图精度。但是这些计算对于有限的CPU来说太耗时了，无法实时纠正位置错误。

        由卡内基梅隆大学（CMU）开发的LOAM多年来一直在KITTI测程基准上排名很高。它将SLAM问题分为两个模块。其中一个模块执行高频里程计，但精度较低，以估计激光扫描仪的速度。虽然没有必要，但如果有一个IMU可用，它可以提供一个运动先验和减轻大尺度，高频运动。第二个模块以较低一个数量级的频率运行，用于点云的精细匹配和配准。具体来说，之前在两个模块中找到并确定了对应关系，它们提取位于尖锐边缘和平面表面上的特征点，并将特征点分别与边缘线段和平面斑块进行匹配。显然，LOAM属于基于特征的方法。

        这两个模块的组合允许实时创建地图。然而，为了在KITTI里程计基准上达到可能的最大精度，激光雷达建图实际上以与激光雷达里程计基准相同的频率运行，并处理每个单独的扫描，运行在1 HZ，是传感器实时速度的10%。此外，研究人员还实现了一些LOAM变体，包括具有地面优化过程的LeGO-LOLOAM[18]、具有紧密耦合激光惯性里程计的LIO-SAM [19]、具有固态激光Livox的Loam_livox [18]、具有点网格特征的R-LOAM [27]、F-LOAM、A-LOAM等。

        近年来，IMLS-SLAM [13]是另一种仅基于三维激光雷达数据的低漂移SLAM算法。它依赖于一个扫描模型匹配框架。这种直接方法首先使用了基于激光雷达扫描的特定采样策略，但没有建立显式的对应关系。然后，它将模型定义为之前的局部激光雷达扫描，并使用隐式移动最小二乘（IMLS）曲面表示。这种方法需要1.3秒才能完成每次扫描，并且不能实时运行。

        在“同步定位和映射”的概念中，“同步”一词具体揭示了实时处理对里程计的重要性，SLAM的前端，同时追求高定位精度。上述方法比实时处理以更多的时间消耗为代价，获得了更高的里程计精度。它们不能解决处理效率和定位精度之间的矛盾。本文中的NDT-LOAM旨在解决准确性和效率方面的问题。它利用了改进的加权NDT和关键帧匹配策略，实现了基于三维激光雷达数据的低漂移、实时和最先进的SLAM解决方案。

III. M ETHODOLOGY

 

        一个完整的SLAM通常包括两部分：前端里程计和后端优化。本文以前端为重点，提出了一种高效的激光雷达里程计（NDT-LOAM）。系统架构如图1所示，它将SLAM前端问题分为两个模块：直接里程计（DO）和基于局部特征的调整（LFA）。

         DO模块利用了改进的加权正态分布变换（wNDT）和关键帧匹配策略Scan2Key。在NDT的对象函数中，小栅格根据其范围和维数特征，按不同的权重进行分布。此外，还采用了一个简单的启发式方法来选择关键帧。在原始的LOAM中，提取位于尖锐边缘和平面表面上的特征点，并将特征点分别与边缘线段和平面表面斑块进行匹配。显然，原始LOAM属于基于特征的方法。不同的是，我们的里程计采用正态分布变换（NDT）方法，而不是在里程计模块中直接有效地提取与匹配扫描的特征点，也就是说，基于NDT的里程计在图1中称为直接里程计（DO）。

        LFA模块类似于LOAM算法的映射，包括特征对应两个步骤和姿态调整两个步骤。利用初始姿态，对角特征和表面特征进行处理，并在当前帧与现有历史帧的局部点云图之间建立对应的特征。然后，姿态调整通过优化相应特征的代价函数来优化DO的初始姿态结果。

A. Classical NDT

         NDT首先由Biber和Straßer在2003年提出，[28]作为一种二维扫描配准的方法，然后由Martin马格努松[29]对三维点云进行了详细的描述。NDT映射并将点云转换为一个分段连续函数，由一组高斯正态分布的局部概率密度函数（PDFs）组成，每个函数描述了被占据空间单元的表面形状（二维情况中的正方形，或三维情况中的立方体）。

每个栅格中的高斯PDF可以解释为细胞内表面点p的生成过程。换句话说，最初假设p的位置是由这个分布得出的。该分布的均值μ和协方差计算为

B. Weighted NDT

 

         在经典的NDT中，所有有足够点的单元格都被等量地加权来建立配准目标函数。实际上，具有不同范围和不同维数特征的单元格对变换参数[15]的计算有不同的影响。首先，我们讨论了长测量范围与权重之间的关系。对于传感器中心附近的长测量范围，相同的旋转误差会导致点云的畸变，这意味着距离越长，约束越强。因此，我们引入了一个范围的权重wr

        其次，NDT也可以被描述为一种紧凑地表示一个曲面的方法。该转换将一个点云映射到一个平滑的表面表示，描述为一组局部PDF。每个具有平均μ和协方差的PDF描述了细胞中局部表面的形状和维数特征。

        每个单元中的协方差是一个对称的正定矩阵，它可以对特征向量和特征值进行特征值分解。特征值是正的和有序的，使λ1≥λ2≥λ3 > 0可以从协方差矩阵的特征值和特征向量推导出各种几何参数。在[30]中已经提出了几个指标，并选择了以下指标来表示单元格[31]内的线性、平面和球形指标：

 而，一个单元格的维数指数（1D、2D或3D）被定义为

 

        具有不同维数指标的栅格对NDT [13]有不同的影响。局部表面与二维平面越相似，局部PDF反映点云中的真实物理表面就越准确。因此，具有较高平面指数或二维特征的栅格对NDT配准的贡献更大，值得获得较高的权重。由于点云的稀疏性，有可能只有一条扫描线位于几个单元格中。这些具有线性行为或一维特征的栅格不能准确地呈现物理表面，应分布更少的权重。根据经验得到一维、二维和三维特征单元的权重比，当权重设置如下时，结果达到更高的精度：

 因此，加权NDT的目标函数为

 换句话说，Pk是由当前姿态参数变换后的点Pk的值与该点所属的单元格的中心之间的不同。梯度向量g的项是

 海塞矩阵为：

 参数ξ可以通过线搜索[32]的非线性迭代来求解，

 C. Keyframe Strategy

         数据积累和时间推断是减轻里程计漂移的重要步骤。特别是对于本文提出的具有稀疏点云的方法，与连续帧间的匹配相比，多帧的匹配更不容易受到错误的影响。因此，我们对激光雷达里程计法采用了一种关键帧匹配策略。如果时间戳t中的当前扫描帧St与时间戳t-1中的最后一个扫描帧St−1匹配，则将连续帧的匹配策略降级为scan2scan。当前扫描帧St与时间戳t-n（n≥1）中的最近的关键帧匹配，我们将关键帧匹配策略表示为Scan2Keyframe。值得注意的是，Scan2Key策略仅用于直接里程计，而不是LFA，如图1所示。

        此外，本文还没有讨论SLAM的后端，并且关键帧独立于循环闭包和全局优化。对于关键帧的选择，我们采用一个简单的启发式：如果相对于最后一个关键帧的转换达到距离阈值εdis，或者相对于最后一个关键帧的旋转达到阈值εdeg，或者从最后一个开始的时间间隔超过时间阈值εt，则该帧作为关键帧插入。在实际实验中，距离阈值通常比其他两个阈值更难，这是关键帧选择的第一个规则。

D. Local Feature Adjustment (LFA)

        LFA模块将当前扫描帧St中的特征与周围的点云图Qt−1进行匹配，以进一步细化姿态变换。LFA包括特征对应调整和姿态调整两个步骤。我们采用LOAM的Lidar建图模块来实现LFA，读者可以从[15]中获得详细的描述。 

        在对应的特征中，该算法从原始扫描点中提取点特征。点特征可以是角点或曲面点。上述里程计估计产生一个6自由度姿态估计，用作建图模块的初始姿态估计。

        使用初始的位姿将St转换为世界坐标，记为Stw。为了找到特征点的对应关系，我们将特征点存储在地图Qt−1的立方区域上。立方体中与转换点云St w的点相交的点被提取并存储在3Dkd树中。我们在Qt−1中找到了当前扫描特征点附近的特定区域内的对应点。我们将角对应表示为cC，表面对应表示为cS，c=p{p1，p2}，其中p1为的一个点。

        一旦找到特征对应，就可以利用角特征和表面特征对应的成本函数fC和fS分别形成姿态调整的残差块。代价函数的输出是残差的平方，由损失函数ρ加权。残差是形成对应关系的点之间的欧氏距离。剩余块是包裹在损失函数中的成本函数。所得到的残差块被添加到优化函数J中。姿态调整问题的定义为

         其中，cC和cS分别为角特征和曲面特征的点特征对应集。sC定义为sC =cC 1、cC 2、...，cnC，sS定义为sS =cS 1、c2 S，...，cS m，n和m分别为角和表面特征对应的个数。q为四元数，t为平移向量，在优化迭代过程中，它将扫描的特征点从激光雷达帧转换为映射帧。||•||2为欧几里得距离的平方，作为残差。

        通过优化后的变换，对特征对应估计进行第二次迭代，然后进行一个优化步骤，这是传统的LOAM算法的默认值。经过两次迭代后，使用最终优化的变换将当前扫描的特征点插入到地图中，并由地图构建模块进行说明。随后，更新后的地图可用于估计下一次激光雷达扫描的6自由度姿态。

实验：

略

使用LOAM中的特征点提取方法和局部地图进行匹配以获取更快的收敛速度。

代码：https://github.com/BurryChen/lv_slam