2020 ICRA论文《Real-Time Graph-Based SLAM with Occupancy Normal DistributionsTransforms》

德国University of Tubingen

 Abstract

         同步定位和建图（SLAM）是移动机器人技术中的基本问题之一。虽然大多数方法是基于占用网格图，但正态分布变换（NDT）和占用正态分布变换（ONDT）等混合物已被证明更准确地代表传感器测量。在这项工作中，我们稍微重新制定了(O)NDT匹配函数，使它成为一个最小二乘问题，可以用各种鲁棒的数值和解析的非线性优化器来解决。进一步，我们提出了一种新的全局(O)NDT扫描匹配器用于闭环检测。在我们的评估中，我们的NDT和ONDT方法能够优于我们从谷歌的cartographer实现中采用的基于占用网格地图的方法。

 

I. INTRODUCTION  

         同时定位和建图（SLAM）的问题是一个鸡和蛋的问题。一个正确的全局姿态需要建立一个一致的地图，而一个正确的地图对于精确的自定位是至关重要的。这种依赖的问题通常通过扫描匹配来解决。为了有效地合并闭环检测信息，当机器人重新进入之前访问过的位置时，可以应用基于图的方法。因此，节点表示机器人姿态，边描述这些姿态之间的约束，并由帧对帧匹配和闭环检测[1]构造。

        目前，SLAM大多被认为是可以解决的，特别是关于二维室内的情况。然而，根据机器人的设置和所使用的传感器，它仍然很困难。迄今为止最有利的SLAM系统是谷歌的cartographer[1]，它基于占用网格地图，同时支持2D和3D空间。虽然它无疑是一个非常复杂的框架，但我们仍然观察到，在将其应用于我们自己机器人的数据时存在困难。除了占用网格图外，也有还引入了正态分布变换（NDT）[2]和NDT与占用信息的组合，例如NDT-OM [3]、其增强版本NDT-OMFG [4]和ONDT [5]。由于其他工作的结果[6]，[7]表明，基于NDT的匹配算法可以比迭代最近点（ICP）更快，提供更好的结果，它们可能优于基于占用网格地图的SLAM解决方案。

        然而，尽管这种地图类型已经在SLAM上下文[2]、[8]、[9]、[10]中被多次使用，但大多数方法使用低效的手工匹配器，并且没有与最先进的基于占用网格地图的SLAM方法进行定量比较。

        在这项工作中，为了解决这些问题，我们研究了(O)NDT在基于图形的SLAM中的适用性，以解决这些问题。

        这项工作的贡献如下：

• 受谷歌的Cartographer的启发，我们修改了从[2The Normal Distributions Transform: A New Approach to Laser Scan Matching]中获得的NDT匹配函数，使其在无特征的环境中变得更加鲁棒，并将其扩展到ONDT。与占用网格地图相比，建图单元之间不需要插值，因为基于NDT的地图在空间上是连续的，因此得到的匹配器比基于占用网格地图的对应建图更快。
• 我们使用快速开源求解器以无推导的方式求解匹配函数。与手工制作的基于雅可比矩阵和海塞矩阵的方法相比，我们的方法易于实现，而且速度非常快。
• 我们提出了一个具有(O)NDT映射的全局扫描匹配器，类似于谷歌Cartographer中使用的相关扫描匹配器[11]。然而，我们的版本不需要一堆预先计算的网格，因此它的内存效率更高。
• 我们的实验表明，我们的ONDT和NDT版本在Rawseeds[12]、[13]数据集和我们自己的机器人记录的两个数据集上都优于基于占用网格地图的方法。

为了公平地比较不同的方法，我们在一个通用的SLAM框架中实现了不同的方法，该框架与所使用的建图类型[14]无关。 

II. R ELATED W ORK

        虽然占用网格图只包含占用概率，但NDT地图最初也被设计为一个规则网格，但单元格不是一个占用值，而是将属于其中的测量点编码为一个多元样本分布[2原始的二维NDT论文]。因此，一个较粗的网格分辨率就足够了。NDT-OM（FG）[3]、[4]和ONDT [5]是这两种可能性的组合，所涉及的网格单元格包含这两种信息类型。纯NDT图和ONDT图已经用于蒙特卡罗定位（MCL）[15]，[5]。因此，将样本分布的概率密度函数进行拉伸，然后直接作为一个类似似然场的传感器模型来加权姿态假设。 

        扫描匹配已经在Biber和斯特拉瑟[2]最初发表的NDT中讨论过。作者通过应用牛顿算法和人工导出的海塞矩阵和雅可比算法来最大化扫描和地图之间的相关分数。斯托亚诺夫等人[16 Fast and accurate scan registration through minimization of the distance between compact 3D NDT representations 3维NDT论文]应用相同的方法在三维和进一步介绍了分布到分布匹配，它可以用于扫描到地图匹配通过转换扫描到本地地图，由斯托亚诺夫等人[9]，或map to map的上下文。后者是由Einhorn等人[8]完成的，但没有执行scan to scan的匹配，而是使用车轮里程计将多次扫描积累到局部子图中。

        另一方面，Ulas和Temeltas [17]使用了不同分辨率的多层NDT地图，通过迭代细化进行扫描匹配。这种方法的缺点是，除了额外的内存需求外，在多个建图中插入点还会花费进一步的运行时间。作者还使用了基于雅可比矩阵和海塞矩阵的求解器。然而，在原则上，这类似于谷歌的cartographer[1]中使用的全局相关扫描匹配方法[11 CSM]。与[17 多层NDT地图]相比，它是一种类似于蛮力的方法，并在一个更大的搜索窗口上操作，以避免局部优化的问题。在基于图的SLAM上下文中，Jel‘ınek[10]将应用于基于NDT的地图的全局相关扫描匹配方法[11 CSM]与[9 map to map]的局部分布到分布匹配方法相结合。然而，杯状匹配步骤只进行了两层NDT图，并没有进行定量评估。

        一般来说，NDT SLAM的工作没有与基于占用网格地图的方法进行比较。仅仅将一些基于NDT的匹配器[6]、[7]与ICP进行了比较，ICP是一种帧到帧匹配的标准方法，不再在最先进的SLAM实现中使用。例如，谷歌的cartographer[1]使用了一种类似于NDT匹配器的相关性最大化方法来进行帧对帧的匹配。在这项工作中，我们使用一个通用的SLAM框架，支持不同的地图类型[14]来比较(O)NDT-和基于占用网格地图的版本。与cartographer类似，我们应用Ceres[18]的无推导求解器进行局部匹配，并使用相关扫描匹配器的[11]进行全局匹配。

III. PRELIMINARIES

 为了清晰起见，我们简要总结了关于ONDT和NDT地图[5]的基本知识，以及原始的NDT匹配函数和解决方案[2]。此外，我们还简要介绍了基本的SLAM结构、所利用的组件和相关参数[14]。

A. Occupancy Normal Distribution Transforms (ONDT)

 

        正如Biber和斯特拉瑟[2 二维NDT论文]在最初的出版物中提出的那样，我们的(O)NDT地图由多个相同分辨率的子地图组成，沿着不同的轴移动了分辨率的一半。这导致了一个有效的网格，有效分辨率r是子图分辨率的一半，并且需要减少离散化问题，见[5 Effificient Map Representations for Multi-Dimensional Normal Distributions Transforms 2018IROS]。 

        在每个子图中，测量一个点q的可能性由栅格j的样本分布Nj（µj，Σj）的非归一化概率密度函数得到，再乘以栅格j的占用概率pj，其中j是落入栅格q的概率。q的完全似然值是通过建立所有ns子图的平均值而得到的：

 如果使用NDT匹配代替ONDT，则概率pj不存在，可以在这个方程中设置为1。在2D情况下是ns = 4，在3D情况下是ns = 8。

B. Original Approach to NDT Matching

 

        Biber和Strasser [2]扫描NDT建图匹配的原始方法是最大化期望变换T的得分，这是所有扫描点q的似然值的和，变换T：

 

 在实际应用中，我们使用牛顿算法迭代地更新变换T，通过添加一个∆T来最小化函数f ：=−score(T)。∆T是通过求解下式得到的

 其中H和g是f的Hessian和f的梯度。对于g，需要推导出雅可比矩阵。

C. Graph-Based SLAM Framework

 

         我们使用的基于图的SLAM框架是由Kuhlmann [14 Multi-Modal SLAM for Outdoor Robots本文作者前一年开发的SLAM]开发的，与谷歌的Cartographer[1]密切相关并受到启发。作为Cartographer，它使用可配置的分割策略构建几个子地图，例如在每个插入的数据块的数字之后启动一个新的子地图。将扫描姿态的一个子集插入到姿态图中，例如在每个∆t、∆d或∆θ（时间、平移或旋转差）之后。从局部和全局匹配中，在匹配的节点与建图开始对应的节点之间生成局部和全局约束。

        在所有个节点之后，使用这些约束通过稀疏姿态调整（SPA）[19]来优化全局姿态图。与Cartographer相比，其主要优点是所有组件都是作为插件构建的，因此是可交换的，并且任何建图类型都可以被使用。

IV. L OCAL M ATCHER

 

        简要讨论的NDT匹配方法都是基于非线性函数的雅可比矩阵和海塞矩阵。显然，所涉及的牛顿或莱文伯格-马夸特算法都是手工实现的。这些方法的重新实现没有产生有希望的结果，而且大多很缓慢。在我们的测试中，我们用无推导求解器和以下代价函数获得了最好的结果。 

A. (O)NDT Cost Function

 

         原则上，我们希望将公式(2)中定义的分数最大化。然而，由于大多数求解器实现最小化了最小二乘问题，简单地使用Biber和斯特拉瑟[2]所做的负分数是不够的，因为负残差的平方将有效地最小化分数。与Cartographer[1]中使用的占用网格地图成本函数相同，因此我们可以最小化残差上的最小二乘问题。为了得到一个独立于测量点的数量的分数，我们将地图成本函数表示为

 不幸的是，这并不足以在无特征环境中成功匹配，例如，如果只有直直的墙可以观察到，因为行动的距离将被低估，如图2所示和讨论。

B. Translational and Rotational Cost Functions

 

         在Cartographer的实现中，这个问题可以通过两个修改来解决。首先，将未分配的网格单元处理为低占用概率为0.1的单元，通过激发未分配单元的扫描点的非零相关分数来强制分配新单元，如果机器人正在向前移动到以前未访问的区域，这是必需的。对于(O)NDT映射，这是没有用的，因为较高的网格单元大小和重叠面积导致在分布周围的边长约为分辨率r的平方面积被隐式地分配为free。

        其次，Cartographer的实现使用额外的平移和旋转成本函数来惩罚最终解T与初始猜测的偏差，这通常是通过基于车轮里程计的外推得到的。

        这一原理及其对匹配结果的影响如图2所示。另一方面，这意味着在没有特征的环境中，如果没有里程计或其他来源的初始猜测，我们的匹配器（可能还有占用网格地图版本）将会失败。

        根据成本函数的实现与Cartographer的实现一样：

 用是一个提取一个变换的第i个平移分量的函数，即xyz和

         与是一个提取一个变换的第i个旋转分量的函数，即ϕ，θ，ψ（滚动，俯仰，偏航）或在四元数的情况下，分量x，y，z，w。

        根据所使用的旋转表示方式，需要相应地配置解决方案空间。使用Ceres [18]，例如，可以应用所提供的QuaternionParameterization。它将一个点投影回有效的解决方案空间。如果使用ϕ、θ、ψ，这将意味着将角度限制在区间[−π，π)。

那么总成本函数是单个成本函数的加权和：

 注意，变换被定义为应用于初始估计的修正，因此它被初始化为恒等函数。

 图2  受公式(7)的动机，使用一个示范性的无特征环境，如两侧都有直墙的走廊，显示为根据(O)NDT数据结构的黄色重叠分布。扫描点用红色表示。(a)从最后一个姿态xt与车轮里程计数据外推，得到下一个姿态的初始猜测。(b)简单扫描匹配不惩罚偏离将将扫描与地图分布对齐，轨迹行动距离被低估。(c)使用(7)，扫描大多与分布对齐，但是xt+1更接近，新的地图扇区更有可能被分配。

C. Solvers

 为了找到T，任何无推导的非线性求解器都可以用来求解最小二乘最小化问题

 

         这可以用数值方法来解决，例如用ALGLIB [20]中的LM方法或NLopt [21]提供的不同方法，从中我们发现NEWUOA [22]最好，或者使用自动推导和适当的求解器来解析，例如用Ceres [18]。在我们的测试中，Ceres在合理的运行时间中提供了最好的结果。因为这也是Cartographer中使用的求解器，所以这是我们用来评估的。我们的匹配器和占用网格地图的Cartographer匹配器之间的主要区别是，占用网格映射单元需要插值，(O)NDT单元不插值，因为它们是每个定义的分段空间连续的。Cartographer通过双插值[1]解决这个问题，这需要在2D情况下16地图访问。然后将拟合的三次hermite样条自动微分，得到所需的雅可比矩阵。通过我们的(O)NDT映射，相关函数(1)可以直接区分，这可能要快得多，这取决于单个地图访问的成本和插值步骤。更多关于自动求导及其效率的信息可以在Ceres的网页[18]上找到。

V. G LOBAL M ATCHER

 

        我们用于回环闭合的全局扫描匹配器类似于在Cartographer[1]中实现的全局相关扫描匹配器[11 CSM]，但它不需要预先计算具有不同分辨率的网格建图堆栈。 

A. Loop Closure Candidates

 

         在我们的框架中，根据新插入的图节点所呈现的姿态只与它们在局部不匹配的地图进行全局匹配。闭环检测的选择不一定不可避免进一步限制。然而，例如，只匹配最大距离dmax的内的节点和所有图节点的小比例ρ的节点是有用的。

B. Brute Force Approach

        正如在[1]中所讨论的，Cartographer的全局相关扫描匹配器原则上在一个可配置的搜索窗口中执行穷强力搜索，线性窗口大小为，角窗口大小为。对于(O)NDT建图，需要最大的相关函数为(2)，用点的数量归一化，即

         根据该窗口和线性步长和角度步长生成候选解，在Cartographer中，从扫描中估计为匹配，以便在两个连续步骤[1]之间距离传感器原点最大的点最多相差。但是，我们使用作为参数，而不是将设置为r。

 

 C. Depth First Search (DFS)

         为了减少彻底的运行时间，我们以k个步骤迭代地优化搜索候选对象，类似于DFS。首先，我们对线性步长为(2k−1)的初始候选变换T进行采样。角步长在所有k个步长中保持不变，根据旋转的测量点可以预先计算，如在Cartographer的实现中,见[1]。然后根据式(9)对这些候选转换进行评分，并进一步研究分数不小于一个最小分数的转换。在这些周围，根据下一个迭代的线性步长产生新的候选对象。最后，以最佳的​​​​​​​作为匹配结果。在伪代码中，如算法1显示:

         在Cartographer中，这是类似的，但候选人的分数是基于k个不同分辨率的网格地图的堆栈来估计的。初始候选项使用分辨率为2k·r的网格图进行评分，最后一个使用原始网格图的网格图进行评分。这些地图是预先计算的，每个单元格被设置为地图中对应单元格的最大值，具有下一个更精细的分辨率，以实现一个真正的DFS。然而，这不能直接应用于(O)NDT，因为建图单元不能以这样的方式组合，因此所有可能的下采样转换的最大分数可以通过简单的查找来估计。因此，我们在每一步中都使用原始地图，这可能会导致忽略局部最优，这取决于采样分辨率。

 

VI. E VALUATION

         在下面，我们展示我们进行的实验，将我们的(O)NDT匹配器与基于占用网格地图的版本进行比较，这些版本在功能上等同于Cartograher方法的重新实现。

A. Datasets and Metrics

 

        为了研究不同机器人平台的良好混合，我们利用了Rawseeds  Bicocca数据集[12]、[13]的数据集25a和27a，以及两个自编数据集，分别用TurtleBot2和Summit XL记录。通过迭代应用具有极端参数的Cartographer离线工具，优化了我们的机器人的地面真实轨迹，如图3所示。机器人用于Rawseed数据集配备了两个SICK 激光扫描仪80米范围，我们TurtleBot2只是Hokuyo URG-04LX-UG01激光扫描器很短的范围约5.6米，和我们的峰会XL与SICK TiM561激光扫描仪范围约10米，其他传感器与此评估无关。 

        TurtleBot2数据集具有挑战性，因为使用的Hokuyo激光扫描仪的低范围导致大多数无特征扫描，类似于图2中所示的问题。Summit XL数据集也很难处理，因为机器人移动得非常快，特别是在旋转过程中，因此，连续扫描对之间的平移距离，特别是旋转距离相对较大。为了进行评估，我们测量了不同组件的平均运行时：将扫描或点云插入到地图中，局部和全局匹配。无论所使用的建图类型如何，框架开销都应该大致相同，因此会被忽略。此外，我们根据绝对轨迹误差（ATE）[23]和相对姿态误差（RPE）[24]估计了所实现的定位精度，这些指标是由Rawseeds基准测试套件[12]，[13]提供的。

其他：略

VII. C ONCLUSION

        在这项工作中，我们提出了在基于NDT和ONDT建图的SLAM上下文中的局部和全局扫描匹配的组件，这些组件简单且易于实现。我们实验评估了各种ONDT和NDT的SLAM配置，并与基于占用网格地图的配置进行了比较，后者使用功能接近谷歌的Cartographer实现的组件来实现。在所有被调查的数据集上，记录了非常不同的机器人，我们基于(O)NDT的配置在定位精度和随时间变化的误差方面都优于基于占用网格图的配置。总的来说，我们经历了基于NDT和ONDT的组件对不同的具有挑战性的影响和参数变化更加鲁棒。