Improved Techniques for Grid Mapping With Rao-Blackwellized Particle Filters-翻译

Improved Techniques for Grid Mapping With Rao-Blackwellized Particle Filters

Abstract

近年来，Rao-Blackwellized粒子滤波器（RBPF）被引入到解决同时定位和建图问题中。此方法使用粒子过滤器，其中每个粒子都携带环境的单个地图。因此，一个关键问题是如何减少粒子的数量。在这篇文章中，我们提出了在学习网格地图的RBPF中减少粒子数量的自适应技术。我们提出了一种计算精确建议分布的方法，不仅考虑了机器人的运动，而且还考虑了最近的观测。这大大降低了滤波器建议步骤中机器人姿态的不确定性。此外，我们还提出了一种选择性地执行重采样操作的方法，这大大减少了粒子耗尽的问题。在大型室内和室外环境中，用真实的移动机器人进行的实验结果说明了我们的方法比以前的方法的优势。

I. INTRODUCTION

地图绘制是移动机器人的基本任务之一。在文献中，移动机器人的建图问题通常被称为同时定位和映射（SLAM）问题[1]-[9]。这被认为是一个复杂的问题，因为对于定位，机器人需要一个一致的地图，而对于获取地图，机器人需要对其位置进行良好的估计。姿态估计和地图估计之间的这种相互依赖使得SLAM问题变得困难，需要在高维空间中寻找解决方案。
Murphy，Doucet及其同事[2]，[8]介绍了Rao-Blackwellized粒子滤波器（RBPF）作为解决SLAM问题的有效方法。Rao-Blackwellized方法的主要问题是它们的复杂性，用建立精确地图所需的粒子数来衡量。因此，减少粒子数量是这类算法面临的主要挑战之一。此外，重采样步骤可以潜在地消除正确的粒子。这种效果也称为粒子耗尽问题，或粒子贫化[10]。

在本文中，我们提出了两种方法来显著提高RBPFs的性能，并将其应用于解决栅格地图的SLAM问题：
• 一种建议分布，不仅考虑到机器人传感器精度并允许我们以高精度方式绘制粒子；
• 一种自适应重采样技术，它保持了粒子的合理多样性，并以这种方式使算法能够学习精确的映射，同时降低粒子耗尽的风险。

建议分布是通过评估与粒子相关的最可能姿态周围的可能性来计算的，该可能性是通过扫描匹配过程与里程信息相结合获得的。以这种方式，最新的传感器观测被考虑到创造下一代粒子。这使得我们能够根据一个比仅基于里程信息获得的模型更为富含信息（从而更为准确）的模型来估计机器人的状态。使用这种改进的模型有两个效果。考虑到当前的观测结果对机器人姿态的影响，该地图更加精确。这显著地减少了估计误差，因此需要更少的粒子来表示后验。第二种方法，自适应重采样策略，允许我们仅在需要时执行重采样步骤，这样，就保持了合理的粒子多样性。这将显著降低微粒耗尽的风险。

本文的工作是我们先前工作的扩展[11]，因为它进一步优化了建议分布，以更准确地绘制下一代粒子。此外，我们还添加了复杂性分析，对所使用的技术进行了形式化描述，并在本文中提供了更详细的实验。我们的方法已经在一系列大型室内外环境的系统实验中得到了验证。在所有实验中，我们的方法生成了高精度的度量地图。此外，所需粒子的数量比以前的方法低一个数量级。

本文的结构如下。在解释了如何使用Rao-Blackwellized滤波器来解决SLAM问题之后，我们在第三节描述了我们的方法。然后在第四节提供了实现细节。在第六节中介绍了在真实机器人上进行的实验。最后，第七节讨论了相关的方法。

II. RELATED WORK

根据Murphy[8]，SLAM的RBPF的关键思想是估计地图上的联合后验概率p(x_(1:t),m|z_(1:t),u_(1:t-1))和机器人的轨迹x_(1:t)=x₁,…,x_t 。在给定移动机器人获得的观测值z_(1:t)=z₁,…,z_t和里程测量值u_(1:t-1)=u₁,…,u_(t-1)的情况下，进行该估计。SLAM的RBPF使用以下因子分解：
这个分解允许我们首先估计机器人的轨迹，然后计算给定轨迹的地图。由于地图高度依赖于机器人的姿态估计，这种方法提供了一种有效的计算方法。这种技术通常被称为Rao-Blackwellization。
通常，（1）可以被有效地计算，因为x_(1:t)和z_(1:t) 是已经知道的。地图的概率密度函数可以通过已知位姿的建图方法“mapping with known poses”来计算。

为了估计潜在轨迹上的后验概率p(x_(1:t)│z_(1:t),u_(1:t-1) )，可以应用粒子滤波器。每个粒子代表机器人的一个潜在轨迹。此外，一个单独的地图与每个样本相关联。这些地图是根据观测结果建立的，轨迹由相应的粒子表示。
最常用的粒子滤波算法之一是（SIR）滤波器。用于建图的Rao-Blackwellized SIR滤波器，增量式地处理传感器观测值和里程计读数（当他们可获取的时候）。它更新代表地图和车辆轨迹的后验概率的样本集。这个过程可以概括为以下四个步骤。
1)采样：下一代粒子{x_t⁽ⁱ⁾ }是从{x_(t-1)⁽ⁱ⁾ }一代中通过从建议分布π采样获得的。通常，建议分布使用概率里程运动模型。
2）重要性权重：根据重要性抽样原则，为每个粒子分配一个单独的重要性权重{w_t(i)}

这些权重说明了这样一个事实，即建议分布一般不等于继承状态的目标分布。

3） 重采样：根据权值的比例重新分布采样粒子。这一步是必要的，因为只有有限数量的粒子用于近似连续分布。此外，重采样允许我们在目标分布与建议分布不同的情况下应用粒子过滤器。重采样后，所有粒子具有相同的权重。
4） 地图估计：对于每个粒子，根据该样本的轨迹x_(1:t)⁽ⁱ⁾和观测历史z_(1:t)，计算相应的Map估计 p(m_i |x_(1:t)⁽ⁱ⁾ ,z_(1:t))
这个方案的实现需要在获取新的观测时从头开始评估轨迹的权重。由于轨迹的长度随着时间的推移而增加，此过程将导致明显低效的算法。根据Doucet等人的说法[13] ，通过限制建议分布满足以下假设，获得了一个递归公式来计算重要性权重：
基于公式（2）和（3）权重可以按以下方式计算
这里η =1⁄(p(z_t |z_(1:t-1),u_(1:t-1))是由Bayes规则产生的标准化因子, 对所有粒子来说是一样的。

大多数现有的粒子滤波器应用依赖于（6）的递归结构。虽然通用算法指定了一个可用于学习地图的框架，但它保留了如何计算提案分布以及何时执行重采样步骤的开放性。在本文的其余部分中，我们描述了一种计算精确提案分布并自适应执行重采样的技术。

III. RBPF WITH IMPROVED PROPOSALSAND ADAPTIVE RESAMPLING

在文献中，已经提出了几种计算改进的建议分布和降低粒子耗尽风险的方法[13]-[15]。我们的方法应用了两个概念，这两个概念以前被认为是高效粒子滤波器实现的关键先决条件（参见Doucet等人。[13] ，即计算改进的建议分布和自适应重采样技术。

A. On the Improved Proposal Distribution

如第二节所述，需要从建议步骤中的建议分布中提取样本，以获得下一代粒子。直观地说，建议分布越接近目标分布，滤波器的性能越好。例如，如果我们能够直接从目标分布中提取样本，那么所有粒子的重要性权重都将相等，并且不再需要重采样步骤。不幸的是，在SLAM的情况下，一般来说，这种后验的封闭形式是无法获得的。
因此，典型的粒子滤波应用[16]，[7]使用里程运动模型作为建议分布。这种运动模型的优点是，对于大多数类型的机器人来说，它易于计算。此外，根据观测模型 p(z_t |m,x_t)计算重要性权重，用运动模型 p(x_t |x_(t-1),u_(t-1))代替（6）中的π就可以清楚了。
图1. 运动模型的两个组件。在区间L内，如果使用精确的传感器，两个函数的乘积由观测可能性决定。

然而，这种建议分布是次优的，特别是当传感器信息明显比基于里程计的机器人运动估计更精确时，通常情况下，如果机器人配备激光测距仪[例如，配备SICK的激光测量传感器（LMS）]。图1示出了观测似然的有意义区域实质上小于运动模型的有意义区域的情况。在这种情况下，当使用odometry模型作为建议分布时，各个样本的重要性权重可以彼此显著不同，因为只有一小部分提取的样本覆盖了状态空间中在观测模型下具有高可能性的区域（图1中的区域）。因此，需要相当多的样本来充分覆盖具有高观测可能性的区域。
一种常见的方法，特别是在定位中，是使用平滑似然函数，避免靠近有意义的区域的粒子获得太低的重要性权重。然而，这种方法丢弃了传感器收集到的有用信息，并且，至少在我们的经验中，常常导致在SLAM环境中不太准确的地图。
为了解决这个问题，可以在生成下一代样本时考虑最近的传感器观测。通过整合到建议分布中，可以将采样集中在观测似然的有意义区域上。根据Doucet[17]，该分布
是关于粒子权重方差的最优建议分布。利用这个建议，权重的计算变成
当为配备精确传感器（如激光测距仪）的移动机器人建模时，使用这种改进方案很方便，因为激光测距仪的精度会导致极大的峰值似然函数。在基于地标的SLAM的背景下，Montemerlo等人[6]提出了一种基于高斯近似的RBPF改进方案。这个高斯分布是为每个粒子计算的，使用一个卡尔曼滤波器来估计机器人位姿。这个当地图由一组特征表示时，如果影响特征检测的误差假设为高斯误差，则可以使用该方法。在本文中，我们将计算改进建议分布的思想转移到使用密集网格地图而不是基于地标的表示的情况。

B. Efficient Computation of the Improved Proposal

当使用栅格地图对环境进行建模时，由于观测似然函数的形状不可预测，因此无法直接获得建议分布的闭合形式近似。
理论上，可以使用自适应粒子滤波器获得建议分布的近似形式[15]。在这个框架中，通过计算（9）中给出的最优建议分布的抽样估计来构造每个粒子的建议分布。在SLAM环境下，首先要从运动模型中提取机器人的一组潜在姿态，然后用观测似然法对这些姿态进行加权，得到最优建议分布的近似。但是，如果观测可能性达到峰值，则必须从运动模型中采样的姿势样本的数量会很高，由于需要密集采样才能充分捕获典型的高似然小区域，这导致了与使用运动模型作为建议分布类似的问题：需要大量采样才能充分覆盖分布的有意义区域。
我们的观察结果之一是，在大多数情况下，目标分布只有有限数量的最大值，而且大多数情况下只有一个最大值。这使我们能够对仅覆盖这些最大值周围区域的位置进行采样。忽略分布中意义较小的区域可以节省大量计算资源，因为它需要较少的样本。在本文[11]的前一版本中，我们用区间L(i)内的常数k来近似p(x_t|x_t-1⁽ⁱ⁾,u_t-1)（参见图1），由下式给出：
在我们目前的方法中，我们考虑这两个组成部分，观察似然和运动模型在该区间L(i)。我们在扫描配准程序报告的似然函数的最大值附近局部近似后验p(x_t|m_t-1⁽ⁱ⁾,x_t-1⁽ⁱ⁾,z_t,u_t-1)。

为了有效地绘制下一代样本，我们基于该数据计算高斯近似N。与以往方法的主要区别在于，我们首先使用扫描匹配器来确定观测似然函数的有意义区域。然后，我们在有意义的区域采样，并根据目标分布评估采样点。对于每个粒子i，参数μ_t⁽ⁱ⁾和∑_t⁽ⁱ⁾ 对于区间L中的K个采样点{x_j}分别确定。此外，在计算平均μ⁽ⁱ⁾和方差∑⁽ⁱ⁾时，我们考虑了里程计信息. 我们估计高斯参数为
用归一化因子
通过这种方法，我们得到了最优分布的一个闭式近似，这使得我们能够有效地获得下一代粒子。使用这个建议分布，权重可以计算为
注意，η(i)是（17）中建议的高斯近似计算中使用的同一归一化因子。。

C. Discussion About the Improved Proposal

本节中的计算使我们能够分别确定每个粒子的高斯建议分布的参数。这个提议考虑到了最近的里程计读数和激光观测，同时允许我们进行有效的采样。与使用里程运动模型的情况相比，得到的密度具有更低的不确定性。为了说明这一事实，图2描绘了用我们的方法获得的典型粒子分布。对于无特征的直线走廊，如图2（a）所示，样品通常沿着走廊的主要方向分布。图2（b）示出了到达这种走廊末端的机器人。可以看出，走廊方向的不确定性降低，所有样本都围绕一个点居中。与此相反，图2（c）示出了从原始运动模型采样时的结果分布。
图2. 通常在映射过程中观察到的粒子分布。在开放道路中，粒子沿道路（a）分布。在死胡同中，所有尺寸（b）的不确定性都很小。之所以获得这样的后验概率，是因为在对下一代粒子进行采样时，我们明确地考虑了最近的观测结果。与此相反，原始里程计运动模型产生的峰值后验概率（c）较小。

如上所述，我们使用扫描匹配器来确定观测似然函数有意义的区域。这样，我们就把抽样的重点放在重要的区域。现有的扫描匹配算法大多在给定地图和机器人姿态的初始猜测的情况下，最大化观测概率。当似然函数为多模函数时（例如，关闭循环时），扫描匹配器为每个粒子返回最接近初始猜测的最大值。一般来说，可能会出现似然函数中的附加最大值丢失的情况，因为只报告了一个模式。然而，由于我们执行频繁的滤波器更新（每次移动0.5 m或旋转25 °后）并限制扫描匹配器的搜索区域，因此我们认为在对数据点进行采样以计算高斯建议分布时，分布只有一个模式。注意，在像循环闭合这样的情况下，过滤器仍然能够保留多个假设，因为重新进入循环时扫描匹配器的起始位置的初始猜测对于每个粒子是不同的。
然而，在某些情况下，至少在理论上，过滤器会变得过于自信。这可能发生在极为杂乱的环境中，并且当里程计受到噪音的严重影响时。解决这个问题的方法是跟踪扫描匹配器的多个模式，并对每个节点分别重复采样过程。然而，在我们使用真实机器人进行的实验中，我们从未遇到过这样的情况。
在过滤过程中，可能会发生扫描匹配过程失败，原因是观察不好或当前扫描与先前计算的地图之间的重叠区域太小。在这种情况下，使用图2（c）所示的机器人的原始运动模型作为建议分布。注意，这种情况很少发生在真实的数据集中（另见第VI-E节）。

D. Adaptive Resampling

对粒子滤波器的性能有主要影响的另一方面是重采样步骤。在重采样期间，具有低重要性权重的粒子通常被具有高权重的样本替换。一方面，由于仅使用有限数量的粒子来近似目标分布，因此需要重新采样。另一方面，重采样步骤可能从滤波器中去除好的样本，从而导致粒子贫化。因此，重要的是找到判断何时执行重采样步骤的标准。Liu[18]引入了所谓的有效样本大小来估计当前粒子集代表目标后验的程度。本文根据Doucet等人的公式计算了这个量。[13] 作为
其中w ⁽ⁱ⁾是指粒子i的标准化权重。
Neff背后的含义如下：如果样本来自目标分布，由于重要性抽样原则，它们的重要性权重将相等。目标分布的近似性越差，重要性权重的方差越大。由于Neff可被视为重要权重分散度的度量，因此它是一个有用的度量，用于评估粒子集与目标的近似程度。我们的算法遵循Doucet等人[13]提出的方法，以确定是否应执行重采样步骤。每次Neff下降到N/2的阈值以下时，我们都会重新采样，其中N是粒子数。在广泛的实验中，我们发现这种方法大大降低了重新放置好粒子的风险，因为重新采样操作的次数减少了，并且只在需要时执行。

Algorithm
算法1总结了整个过程。每次有一个新的测量元组（u_t-1，z_t）可用时，将分别为每个粒子计算建议，然后使用它来更新该粒子。这将导致以下步骤。
1）由粒子i表示的机器人姿势的初始猜测x_t^’(i)=x_(t-1)⁽ⁱ⁾⨁u_(t-1)，是从该粒子的先前姿势x_(t-1)⁽ⁱ⁾和自上次过滤器更新以来收集的里程测量值u_(t-1)获得的。这里，运算符⨁对应于标准姿势复合运算符[19]。
2） 基于地图m_(t-1)⁽ⁱ⁾执行扫描匹配算法，从初始猜测x_t^’(i)开始。扫描匹配器执行的搜索被限定在xt’(i)周围的有限区域内。如果扫描匹配报告失败，则会忽略根据运动模型计算的姿势和权重（步骤3和4）。
3） 在由扫描匹配器器得到的姿势x_t^’(i)周围选择一组采样点。基于这些点，通过逐点评估采样位置中的目标分布(z_t |m_(t-1)⁽ⁱ⁾ ,x_j)p(x_j |x_(t-1)⁽ⁱ⁾ ,u_(t-1))来计算建议分布的均值和协方差矩阵。在此阶段，还根据（17）计算加权因子。
4） 粒子i的新位姿x_t⁽ⁱ⁾是由改进的提议分布的高斯近似N(μ_t⁽ⁱ⁾,Σ_t⁽ⁱ⁾)得出的。
5） 更新重要性权重。
6） 粒子i的地图m⁽ⁱ⁾根据绘制的姿态x_t⁽ⁱ⁾和观测zt进行更新。
在计算下一代样本之后，根据Neff的值执行重采样步骤。