从定位错误中恢复：添加随机粒子

普通MCL虽然一定程度上解决了全局定位和连续定位问题，但是在“机器人劫持（kidnapping）”或者全局定位错误时没有恢复的能力。实际上，任何向MCL这样的随机算法都可能在重采样时丢弃所有的正确位姿附近的粒子，这个问题在粒子数量较少或者粒子较为分散是更加容易发生。

为此，AMCL中引入了Augmented_MCL，它的思想比较简单，就是通过添加随机粒子来应对“kidnapping”等问题，即使没有kidnapping，这一策略也可以增加定位的稳健性。但是添加随机粒子需要确定两个事情：

• Q1、应该添加多少随机粒子？
• Q2、添加的随机粒子应该从什么样的分布中采样？

A1 根据观测似然来决定添加多少粒子

对于第一个问题，可以考虑添加固定数量的随机粒子，但是更好的思路是根据当前定位的性能决定随机粒子的添加数量。这个思路的一种实现方法是监控传感器观测的概率\(p(z_t | z_{1:t-1}, u_{1:t}, m)\)，并将其与平均似然概率关联起来。在粒子滤波中，这个量可以很容易地从粒子权重获得。因为从定义上来讲，粒子的权重是对这个概率的一个随机估计。所以可以用

\[\frac{1}{M}\sum{{\omega_t}^{[m]}} \approx p(z_t | z_{1:t-1}, u_{1:t}, m) \]

来近似表达这个分布，并且进一步在多个时间步上对这个近似进行平滑。除了定位错误，观测概率可能较低的情况有很多，比如传感器噪声突然变得不正常之高，或者粒子在全局定位过程中发散等。为此，一个好的办法是维护观测似然的一个短期平均和一个长期平均（也就是对应到指数平滑方法exponential smooth）。

对于指数平滑，没有找到机器人非常相关的资料，暂且看下这个解释（用到指数这个概念应该就是不同时间步的权重是不同的，所以权重分布类似指数的泰勒展开那样）。

具体在AugmentedMCL上的应用，我的理解就是，对于平均似然，会维护短期平均值\(\omega_{fast}\)和长期平均值\(\omega_{slow}\)，\(\omega_{fast}\)计算的是短期的（最近的）几个时间步的似然概率的平均，而\(\omega_{slow}\)计算的是中长期的平均，如果定位稳定时，反应到观测似然上面就会比较稳定，则这两个平均值是几乎相等的；如果定位突然不稳定，观测似然突然下降，则会立马在短期平均上反应出来，也就是\(\omega_{fast}\)会明显反应出下降，但是中长期平均则由于时间步较长，反应就不明显。基于这样的原理就可以通过以

\[max\{0.0, 1.0 - \frac{\omega_{fast}}{\omega_{slow}} \} \]

的概率来增加粒子。

A2 直接根据观测似然来产生随机粒子

对于第二个问题，可以考虑直接根据观测似然来产生粒子。

目前暂且理解到这里。

以上大部分是对概率机器人相关内容的翻译加上我自己的一些理解。欢迎指出问题和交流讨论～