Real-Time Correlative Scan Matching 翻译和总结

Olson E B . Real-time correlative scan matching[C]// Robotics and Automation, 2009. ICRA '09. IEEE International Conference on. IEEE, 2009.
此为谷歌开源的cartographer激光雷达扫描系统的参考文献之一，系统中前端的scan to submap和后端的回环优化部分都使用correlation scan matching作为laser scan的匹配思想和策略。属于SLAM中图优化部分的数据关联部分，也就是基于laser scan的匹配结果得到两帧激光数据之间的位姿变换，同时可得到机器人的局部位姿（translation and rotation）。

摘要

通过激光雷达记录的两帧激光扫描数据得到两帧之间相对位置关系的扫描匹配（Scan Matching）是移动机器人工作中的重要环节之一。作者认为当时的一些算法为了对计算性能进行拖鞋，采用了一些先验的启发式方法对问题进行求解。当然，这些启发式的方法并不完美，特别是当先验知识比较匮乏时，容易产生较差的结果。

1.概览

该论文介绍了一类基于两次相关激光扫描进行扫描匹配的方法。作者使用概率框架来处理此问题：找到一个刚体变换，能够最大化已经观测到的数据概率。此外，作者对整个疑似刚体变换区域均进行搜索，而不是信任一个局部搜索算法去找到全局最小值（当初始化存在噪声时，该方法表现较差，作者后面会详细列出结果）。该疑似区域可以基于命令驱动的运动或者轮式/视觉里程计作为先验来得到。

论文主要的贡献是：

• 描述了一种相关扫描匹配方法的理论和实践优势
• 在传统微处理器上实现了一种基于多分辨率匹配的实时处理
• 描述了将基于相关匹配的方法在GPU上实现的过程，解放CPU去处理其他任务
• 以常规的使用场景评估了论文提出的方法和其他三种不同的类似方法

作者认为，论文所提方法的性能和鲁棒性，以及可实时处理的特点可较好的适配于对鲁棒性和精确度都有较高要求的机器人平台。除了包括SLAM系统平台外，任何导航和定位系统都可从中受益。

2.研究现状

作者的工作与Konolige的相关定位方法很类似，并从中受益[15]。尽管所需要处理的问题公式描述是一致的，但是作者提出了新的解决方法。

3.实现方法

3.1 概率模型

作者根据图2所示的图模型建立扫描匹配问题的模型。机器人在运动$u$的驱动下，从$x_{i-1}$移动到$x_i$。观测$z$与环境模型m和机器人的位置相互独立。

目标是计算机器人位置的后验分布$p(x_i|x_{i-1},u,m,z)$，应用贝叶斯法则并移除相关环境，可得：
$$p(x_i|x_{i-1},u,m,z)\propto p(z|x_i,m)p(x_i|x_{i-1},u)\text{\hspace{1em}(1)}$$
上式子中第一项是观测模型，第二项是机器人的运动模型，可由控制输入、轮速计或者imu等其他传感器获得。
尽管可以从多维高斯分布得到运动模型，但是观测模型则比较难计算，而且在结构上也更加复杂。观测模型一般都会出现类似于图1所示的多峰值现象。

本篇论文的核心贡献便是为了计算等式（1）中机器人的后验位置分布而提出的一种有效计算观测模型分布$p(z|x_i,m)$的方法。为了找到$p(z|x_i,m)$的局部极值（为了进一步找到最大似然结果），尽管之前已经提到使用爬山（hill-climbing）算法，但是本论文提出的方法将更加完善的对观测概率分布进行建模。作者认为新方法可以获得更鲁棒的后验概率估计和更合理的不确定度估计。
与之前的其他研究类似，作者同样假设每个激光雷达扫描返回的$z_j$是独立的，因此可以得到：
$$p(z|x_i,m)=\prod _{j}p(z_j|x_i,m)$$
对于一帧单个激光雷达的扫描$z_j$而言，其概率分布可以认为是从一个特殊的方位观测得到的当前地图m的可视表明。这便需要进行昂贵的射线投射来完成探测。与其他方法类似，作者忽略可见性和障碍物的影响，只考虑与地图m表明的距离来估计$z_j$的概率。
在SLAM系统中，模型m可以基于之前的激光雷达观测得到，也可以作为已知的地图先验信息得到。

4.2 栅格查找表

$p(z|x_i,m)$的计算可以通过建立一个2D查找表来实现，通过预先计算每个世界坐标系位置$(x,y)$处雷达观测的对数概率查找表实现。
栅格化基于地图m实现。对于地图中的每个观测点$m_i$，计算$m_i$处的观测到附近点p的环境概率。对地图中的每个点重复此过程，在查找表中记录每个点的最大概率。由于查找表必须是视角独立的，作者基于传感器模型估计潜在香蕉分布作为一个径向对称分布（Since our lookup table must be viewpoint independent, we approximate the potentially banana-shaped distribution arising from the sensor model (which has independent range and bearing noise) as a radially-symmetric distribution. 没太理解）。可视化的查找表见图3：

正如之前提到的，该模型m可以来源于预先已知的场景扫描地图，也可以是基于上一步对环境的观测进行估计。建图也是一个很复杂的问题，一般要求对SLAM问题进行全部求解。
在作者的论文中，作者简单的使用前一步的激光雷达扫描结果作为模型（reference scan）。该方法的优点在于实现容易，鲁棒（不会因为与之前的数据关联错误而发散），而且或许更实用，此方法作为本论文所提方法的辅助方法进行使用。更加复杂的实现可以通过综合多个雷达扫描（比如：CAMERA的Vasco）或者从雷达点中提取连续表面来建立细节更丰富的模型。

4.3 流程概述

作者的目的是通过等式（1）估计$p（z|x_i,m）$的分布，该分布可以用来得到后验分布$p（x_i|x_{i-1},u,m,z）$。与其他方法不同的是，不知关注最大后验估计，而且关注后验分布的本身，因此可以得到一个合理的测量不确定度。该分布不可以用简单的公式表达，需要用数值结果来评估。下面两部分，将会描述两种用于快速计算$p（z|x_i,m）$在$x_i$上的分布。

4.4 多分辨率实现

为了在传统微处理器上使用该方法，此部分描述一种基于相关的快速匹配方法。因此传统的微处理器对于大规模数据$p（z|x_i,m）$的计算很困难，因此本方法就是为了解决此问题。要进行计算并最小化数值评估的计算量，需要先满足1）在大尺度区域对分布进行特征化；2）精确定位最大概率值。下面分三步描述该方法，从一个经典的实现方法到多分辨率实现方法逐渐过渡。
1）暴力求解：从原理上讲，需要在点的3D空间计算$p（z|x_i,m）$；三个维度对应未知T的三个参数：$\Delta x,\Delta y,\theta$。经典方法的实现包含3层嵌套循环；对每一个体素（voxel），都需要计算其概率并记录。对每一个单个的体素，还涉及第4个循环，在待查询scan里迭代遍历每个点，投影，并在查找表中找到其对应的概率误差。这种方法特别慢，在后面的结果部分可见。
2）计算2D片：暴力求解比较慢的一个原因之一是对于每一个体素（待遍历3维空间里的一个小立方体，不同的体素对应不同的x，y，$\theta$）都需要对待查询scan中的所有点进行投影。这是不太好的一种处理方式，因为对于一个给定的方向角$\theta$, 被投影的待查询点只是单纯的与$\hat{x},\hat{y}$搜索方向上的纯位移相关。换句话说，可以通过在最外层迭代遍历$\theta$，此表示待查询点经过了合适的旋转，在内部两层（$\hat{x},\hat{y}$）循环中简单地对查询点进行位移转换（在xy尺度上不需要重复进行旋转投影）。此操作还可以进一步进行加速，令位移变换搜索的步长大小与查找表的分辨率相一致，这样只需要按索引移动便可以实现xy尺度上按步长迭代的位移搜索。在后面的结果部分中可见，该方法要显著快于暴力搜索方法。
3) 多分辨率实现：最终基于CPU的实现方法是使用了两个在不同分辨率上生成的查找表。

第一个是高分辨率（分辨率是3c m）, 第二个是低分辨率实现（30c m）。

在分辨率比较低时，在参考扫描帧里的细节可能会丢失。因此，作者在计算低分辨率查找表时，将其值设定为对应高分辨率栅格地图中区域的最大值。这能够确保在低分辨率地图上计算出的概率总是至少与高分辨率地图中的一样大。换句话说，这能够保证我们不错过最大值。

整体方案策略是使用低分辨率图快速确认可能或者不可能包含全局最大值的区域。目的是最小化高分辨率地图里的搜索范围。方法具体步骤如下：

​ (1) 使用低分辨率查找表在整个3D搜索窗口进行遍历和计算概率$p(z|x_i,m)$；

​ (2) 在尚未遍历的3D空间里查找最匹配的体素，定义此匹配分值为$L_i$, 如果$L_i<H_{best}$, 则中断当前遍历，继续遍历下一个体素。此处$H_{best}$是最优的额扫描匹配对齐分值；

（3）在体素$i$内部基于高分辨率查找表再次进行搜索遍历，假设当前体素的log概率是$H_i$。需要注意的是，$H_i<=L_i$，因为低分辨率图会对对数概率过拟合（因为低分辨率查找表其值对应高分辨率查找表区域的最大值）。如果$H_i>H_{best}$，则令$H_{best}=H_i$。

这种多分辨率匹配方法非常快，使得实时相关扫描匹配成为可能。正如结果部分所示，这结果也是非常鲁棒。

4.5 GPU方法

GPU具有非常大的计算吞吐量，而且适合去进行批量计算$p(z｜x_i,m)$。作者认为他们是第一个在机器人建图和定位领域使用GPU的用户。

作者将其实现写成了一个OpenGL GLSL的着色程序，而不是像Nvidia的CUDA类似的特定程序。尽管这要求将问题转换为3维渲染操作（比如绘制纹理多边形计算函数？，computing functions by drawing textured polygons），但是相关扫描匹配方法比较简单，可以直接进行。

与CPU中的实现方法一致，作者还是对每一"片"都去固定$x_i$的方向，然后切片式的计算$p(z|x_i,m)$. 片段着色器以两个纹理作为输入：一个1维数组，包含待查询点；一个2维的纹理对应查找表。查找表的实现与CPU中也一致。

每个片段对一个特定的$x_i$计算$p(z|x_i,m)$，此值通过纹理坐标传递到渲染器。多边形中的每个片段根据在多边形顶点指定的纹理坐标自动接收线性插值纹理坐标。换句话说，就是当我们将多边形投射到屏幕上时，纹理坐标一点也不与纹理对应，其通过片段着色器作为应用于待查询点的刚体运动变换来体现。

片段渲染器与传统的CPU方法很类似：每个片段迭代遍历待查询点，根据局部$x_i$投影每一个点，接着在查找表中获取log概率。渲染器简单的将每个像素的log概率加在一起，并将结果输出到帧缓冲器中。主机CPU可以接着检查帧缓冲器，继而计算最大似然概率结果。

在主机CPU端，为每一个我们希望计算的$x_i$的方向圈定一个四边形。一个单独的四边形对应一组固定方向下的位移。基于GPU的结果见图1.需要主要的是，图中的颜色编码是使用了可视化辅助：每一个GPU输出的像素值是个标量。

对GPU不是很懂，这段看的也不是很明白，写的也不是很清楚。

4.6 计算协方差

在许多应用中，$x_i$的最大似然估计是有意义的。但是，作者同时也进行了一个合理的不确定度估计。

一旦在较大范围对$x_i$值使用损失函数进行了计算，则可以基于此数据构建一个多维高斯分布。令$x_i^{(j)}$是$x_i$ 的第$j$次计算评估：

基于计算的$p(z|x_i,m)$估计的扫描匹配的额不确定度考虑了大量的不确定量：包括传感器自己的噪声，待查询点与模型的关联误差。该方法的缺点是高斯结果只根据已经进行过计算评估的数据采样点进行拟合。任何还没有被采样的高概率区域也不会反应在高斯概率模型中，导致过拟合。因此，在更大的$x_i$范围进行$p(z|x_i,m)$概率估计非常重要，这方面本文的方法考虑更周到。可以从图5中两种case看到本方法的协方差估计结果。

5. 实验和结果

5.1 实验设计

基于地图中的已有的数据，再结合地图信息进行仿真测试。

5.2 不同方法比较

1）ICP: The Iterative Closest Point（ICP）算法以1m的匹配限制为条件，并以欧式距离作为距离度量。变量是对称的：对两帧扫描中的每一个点，都在另一个扫描中去找最近点。比如，假设扫描帧a中的点$a_i$最接近的点是$b_j$, 而且在扫描b中的点$b_k$最接近点$a_i$。如果$b_i$与$a_j$的距离大于$b_k$的两倍，则对应的（$a_i$, $b_i$）被删除。$b_i$和$a_j$是什么，难道不是$a_i$和$b_j$吗这种优化方式可以帮助重新确认环境中的某一部分只在一个扫描帧中可见的情况，并剔除可能产生错误结果的远距离对应关系。在上述的例子中，很可能$b_j$没有对应的参考点。额外的对称变量可以提供额外的约束关系，帮助减少匹配误差。最后，基于找到的对应匹配点对使用**Horn’s算法[6]**计算优化刚体变换。

2） ICL：The Iterative Closest Line（ICL）算法在论文实验中将待查询点与最近的线段使用欧拉距离关联。线段由不超过1m部分的连续点对生成。假设待查询点$q_i$已经和一个线段$s$关联起来了：则在$s$上与点$q_i$距离最近的点和$q_i$构成点对，继而被传递到Horn’s算法中进行刚体变换计算。

3）爬山法：Hill-Climbing，本文所采用的爬山方法是在Vasco扫描匹配器之后建模的，该匹配器与CARMEN的分布一致。这是一个沿着方向轴重复尝试的局部搜索方法，如果匹配度提高则接受当前步长。初始步长相对比较大（x/y轴为1m，$\theta$方向为10度）；当没有步长能够使得误差减小时，将步长大小减半。当步长大小到达预定的最小值时（x/y轴0.001m，$\theta$方向0.05度），搜索结束。与本文中方法一致，Vasco的优化也是在概率上，而且也使用了一个log概率查找表。为了与本文的相关匹配方法进行直接比较，本文方法也使用相同的栅格查找表。

初始实验包含大约25000次迭代，对每一次迭代，选择新的位姿，并在之前的先验估计中加入噪声，并运行每个扫描匹配器。在图4中，在z轴画出来了平均误差，同时x轴表示先验的初始位置误差, y轴表示先验的旋转误差。（图形对称是因为先验的误差可能是正的也可能是负的。）

一般情况下，超过几厘米的误差或者几度都是不可接受的：这会导致机器人的定位误差积累非常快。直观而言，我们希望在误差更大的结果里反应出更大的先验噪声水平。ICP, ICL和爬山法的表现确实如此。还有值得注意的是，爬山法的误差稍微比ICP的要高一些，甚至初始几乎无噪声的时候也要高一些。
本篇论文的核心结果是：针对先验的噪声，基于相关扫描匹配的的结果误差更小而且更鲁棒。付出的代价就是稍微多一些计算量，但是在下一部分可见，多出的计算量也是可以控制的。

重点：查找表的分辨率比较影响匹配的结果精度。作者对在0.001m~1m的查找表分辨率基于本文方法进行了误差量化，发现当分辨率小于1.5cm时结果误差已经没有提升，分辨率为3.1cm的只是稍微差一点。当分辨率大于6cm时，结果误差会急速恶化。基于论文中的数据，作者推荐的栅格分辨率是3cm。

5.3 性能

本文提出的相关方法确实比ICL, ICP和爬山算法要慢一些，这应该没什么惊讶的（因为本质上还是属于遍历的算法，其他三种属于迭代的算法，相对来说复杂度低一些）。相关扫描匹配方法的初始目的就是为了提升结果的质量。但是，基于相关扫描匹配的方法也够实时使用（图7）。实验使用的CPU是Inter的Core2 6600，主频2.4GHz。尽管CPU是多核的，但是实验中只使用了一个线程。此外对跨两代的GPU进行了评估，7600和Nvidia 7600GS以400MHz运行，以及GTX260以650MHz运行。栅格分辨率是1/32米，$\theta$的遍历步长大小为1度。

ICP和ICL的计算复杂度基于我们的实现，也表现很好（见图7），爬山算法明显速度占优势。尽管他们的计算复杂度随初始不确定度的增大而增大，但是当初始不确定度较大时其结果几乎完全错误（见图4）。

相关扫描匹配方法的复杂度与先验不确定度的三次方成比例增长。对于相对比较小的不确定度，上述几种方法均基本可以实时实现。在这种情况下，搜索范围随着激光雷达扫描的频率增大而缩小，因为频率更新越快，则机器人的定位误差累积越小。比如更新频率为75Hz, 当车辆以37m/s的速度行驶时，需要设置0.5m的搜索窗口（37m/s*1/75s=0.5m）。方向角的搜索窗口类似，如果机器人的旋转速度为240rpm，则需要20度的搜索窗口。然而，即使是75Hz的更新频率，多分辨率相关匹配算法也可以以足够快的速度（8.4ms）满足实时运算。

当应用在回环中时，实时性相对来说不那么要求苛刻，关键点是需要找到正确的对应关系。机器人只需要间断性的完成一个大回环即可（或许没几分钟一次）。在进行回环操作时，往往会出现比较大的不确定度搜索窗口。如图7所示，相关扫描匹配方法（特别是多分辨率实现方法）可以以10Hz处理4m的位移搜索窗口和90度的旋转搜索窗口，作者认为这这也可以适应回环处理（个人觉得此处的例子不恰当，90度/4m的回环搜索区域还是太小了）。

对于实时系统，运行时的动态性能也是一个比较大的挑战。ICP、ICL，爬山算法以及多分辨率相关算法运行复杂度是数据独立的。ICP和ICL在处理某些结果时话费了大约2.5倍长的时间（图8）。单分辨率的相关方法表现更好，而GPU实现的表现还要差一些。这些不一致的表现只发生在小搜索窗口下：当搜索窗口比较大时，GPU方法的表现还是比较一致的。

结论

本篇论文提出一类基于相关的扫描匹配方法。该方法相对初始噪声非常鲁棒，而且确实比当时的现有方法表现更好。作者在CPU和GPU上均可以实时实现该鲁棒算法。

本文提出的算法是一种概率驱动算法，可以很方便的与其他概率先验合并，也能够比较方便的计算协方差估计。相关的工作可见http://april.eecs.umich.edu.。