论文翻译—2D NDT算法论文

正态分布变换：一个新的方法来匹配激光扫描

相关信息：

题目：The Normal Distributions Transform: A New Approach to Laser Scan Matching

来源：Proceedings 2003 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS 2003)

作者：Biber, Peter, and Wolfgang Straßer

---

摘要

摘要—2D范围内的扫描匹配是许多定位和建图算法的基本组件。大多数扫描匹配算法需要找到使用的，如点或线等特征之间的对应。我们为范围扫描提供了一个可选的表达，即正态分布变换。与占用栅格类似，我们将2D平面分成小的单元格。对于每一个单元格，我们分配一个正态分布，它局部地建模了测量一个点的概率。变换的结果是分段连续且可微的概率密度，我们可以使用牛顿算法将其和另一个扫描进行匹配。因此，不需要建立明确的对应关系。

我们详细介绍了该算法，并展示了其在相对位置跟踪以及同时定位和建图（SLAM）中的应用。真实数据的初步结果表明，即使不使用里程计数据，该算法也能够可靠并实时地建立未变化的室内环境地图（参见视频）。

1. 绪论

毫无疑问，同时定位与建图（SLAM）是移动机器人系统的一个基本能力。激光测距仪是获取需要输入的流行传感器，因为它们在各种各样的情况下都具有很高的可靠性以及低噪声。很多SLAM算法都是基于匹配两个距离扫面（range scans）或者将距离扫面匹配到地图上的能力。这里我们提出了一个新的方法来实现扫面匹配的低级任务，并且展示了如何用它来建图。

本文的主要目的是介绍正态分布变换（后面将用NDT表示）以及其在匹配一个扫面和另一个扫描或多个扫面的应用。NDT将从单个扫描中重建的离散2D点集，转换成在2D平面上定义的分段连续且可微的概率密度。概率密度由一组很容易计算的正太分布组成。然后将第二次扫描与NDT的匹配定义为，第二次扫描的对齐的点，在此密度下得分和的最大值。

我们也提出了一个针对SLAM问题的，完美符合我们的匹配方案的简单算法。但是提出的匹配方案却不依赖于这一算法。因此，我们在第二章的相关工作中，只回顾了涉及到匹配两帧扫描的方法，而没有回顾像Thrun[15]或Gutmann[6]中方法一样构建整个地图的方法。无论如何，该方法的一个组件是一个感知模型，即在给定地图和位姿估计条件下的单个扫描的可能性。由于我们的方法恰好可以得到这种模型的度量，因此我们相信，我们的扫描匹配器也能够被集成到更复杂的SLAM算法中。

本文剩余部分组织如下：第三章引入NDT，第四章给出扫描匹配方法的概述，并定义将扫描与NDT比较的度量，它在第五章中使用牛顿算法进行优化。扫描匹配器在第六章中被应用于位置跟踪，并且在第七章中被应用于简单的SLAM方法中。我们最终展示了在真实数据中的一些结果，并以展望未来工作的方式总结本文。

2. 先前的工作

匹配两个距离扫描的目的是找到进行扫描的两个位置之间的相对位姿。大多数成功的算法的基础是建立两个扫描间的图元(primitives)的对应关系。由此，可以推导测量误差并使其最小化。Cox使用点作为图元并将它们与在先验模型中给出的线进行匹配[3]。在Amos的工作中[7]，这些线是从扫描中提取的。Gutmann将从扫描中提取的线与模型[8]中的线进行匹配。匹配点和点这种最通用的方法是由Lu 和 Milios[9]提出的。这本质上是ICP(Iterated Closest Point，迭代最近点)算法([1],[2],[18])应用到激光扫描匹配中的变种。我们与Lu 和 Milios使用相同的建图策略。如[10]中所提到，我们没有建立精确的地图，而是使用选择的已经恢复位姿的扫描的集合作为隐式地图。

所有的这些方法都必须建立明确的对应。我们的方法在这一点上有所不同，因为我们从不需要建立图元间的对应关系。也有一些其它避免解决对应问题的方法。在[12]中，Mojaev结合局部极坐标占用栅格的相关性和概率里程计模型来确定位姿（使用激光扫描仪和声呐）。Weiss 和 Puttkammer[17]使用角度直方图来恢复两个位姿之间的旋转。然后，使用在找到最常见的方向后计算出的x和y直方图，来恢复平移。可以通过使用第二个主方向来扩展此方法[7]。

我们的工作也被计算机视觉技术所启发。如果概率密度这个词被图像强度所代替，那么我们的方法与特征跟踪[13]或构成全景图[14]具有相似的结构，这些技术在每个相应位置使用图像梯度来估计参数。这里用的是正态分布的导数。与图像梯度相反，它们可以被解析地计算出来（即有解析解）。

3. 正态分布变换

这一章描述单帧激光扫描的正态分布变换（NDT）。这是本文的主要贡献。然后，在下面的章节中相对简单地描述NDT在位置跟踪和SLAM方面的应用。

NDT通过局部正态分布的集合来建模一帧激光扫描的所有重建2D点。首先，将机器人周围的2D空间分为整齐的固定大小的单元格（cell）。然后，对于每个至少包含3个点的单元格，进行如下操作：

1）收集包含在这个box中的所有2D点 ${\mathbf{x}}_{i=1..n}$

2）计算平均值 $\mathbf{q}=\frac{1}{n}{\sum }_i{\mathbf{x}}_i$

3）计算协方差矩阵 $\Sigma =\frac{1}{n}{\sum }_i({\mathbf{x}}_i-\mathbf{q})({\mathbf{x}}_i-\mathbf{q}{)}^t$

将在该单元格中测量一个样本点$\mathbf{x}$的概率建模为正态分布 $N(q,\Sigma )$:

$$p(\mathbf{x})\sim exp(-\frac{(\mathbf{x}-q{)}^t{\Sigma }^{-1}(\mathbf{x}-q)}{2})\text{\hspace{1em}(1)}$$

和占用栅格类似，NDT建立平面的一个固定的细分。

可微分的描述。在我们展示例子之前，我们必须注意两个实现细节。但是占用栅格表示一个单元被占用的概率，而NDT则表示在单元格中每个位置测量一个样本(点)的概率。我们使用$100cm\times 100cm$大小的单元格。

这样的表达有什么用呢？现在，我们以概率密度的形式对2D平面进行了分段连续且可微分的描述。在我们展示例子之前，我们必须主意两个实现细节。

为了尽量减少离散化的影响，我们决定使用四个重叠的栅格。即，首先放置单个边长为$l$的一个栅格，然后第二个，水平地平移$\frac{l}{2}$，第三个垂直地平移$\frac{l}{2}$，最终第四个，水平并垂直平移$\frac{l}{2}$。现在每个2D点落入了四个单元格中。在本文的其余部分中，不会明确考虑到这一点，并且我们将描述我们的算法，就像每个点只有一个单元格。因此，如果计算出一个点的概率密度，则可以通过默认理解来完成，即评估所有四个单元格的密度并将结果相加。

第二个问题是，对于一个测量的无噪声的世界中的线，协方差矩阵会变得奇异且不可逆。实际上，协方差矩阵有时会接近奇异值。为了防止这种影响，我们检查$\Sigma$的较小特征值是否至少是较大特征值的0.001倍，如果不是，则将其设为该值。

图1显示了激光扫描的示例和NDT结果的可视化。通过评估在每个点的概率密度建立可视化，亮的区域意味着高概率密度。下一章展示，如何使用该变换来对齐两帧激光扫描。

NDT的一个例子：原始的激光扫描及其概率密度结果

4. 扫描对齐

两个机器人坐标系之间的空间映射$T$如下：

$$T:\left(\begin{array}{c}{x}^{\prime }\\ y^{\prime }\end{array}\right)=\left(\begin{array}{ccc}cos\varphi & -sin\varphi & \\ sin\varphi & cos\varphi & \end{array}\right)\left(\begin{array}{c}x\\ y\end{array}\right)+\left(\begin{array}{c}{t}_x\\ t_y\end{array}\right)\text{\hspace{1em}(2)}$$

其中$(t_x,t_y{)}^t$和$\varphi$分别描述了两个坐标系之间的旋转和平移。扫描对齐的目标是使用在两个位置得到的激光扫描来恢复这些的参数。给定两帧扫描（第一帧和第二帧），提出的方法的大纲如下：

1）创建第一帧扫描的NDT。

2）初始化估计参数（用0值或者使用里程计数据）

3）对于第二帧扫描中的每个样本点：根据参数将重建的2D点映射到第一次扫描的坐标系中。

4）确定每个映射的点所对应的正态分布。

5）通过评估每个映射的点的分布并将结果相加，来确定参数的得分。

6）通过尝试优化得分，计算一个新的参数估计。这通过执行牛顿算法的一个步骤即可完成。

7）回到第3步，直到满足收敛判据。

前四个步骤很简单：如上一章所描述的那样构建NDT。如上所述，里程计数据可以被用来初始化估计。使用$T$并找到对应的正态分布来完成对第二帧扫描的建图，是在NDT栅格中的一个简单查找。

现在，使用以下符号详细描述其余部分：
$\mathbf{p}=(p_i{)}_{i=1..3}^t=(t_x$