粒子滤波详解【3/3】：增强蒙特卡洛定位AMCL

增强蒙特卡洛定位 §

基于粒子滤波的定位算法称为蒙特卡洛定位（Monte Carlo Localization，MCL），也就是我们上一章提到的SIR滤波。但实际定位时，MCL仍存在几个比较大的问题：

1. 无法解决机器人绑架问题；
2. 粒子数无法动态调整，存在冗余问题； 增强蒙特卡洛定位（Augmented_MCL，AMCL）算法在MCL的基础上，针对上述问题提出了对应的解决方案，使得其成为机器人领域一个非常基础但实用的算法。其是ROS/ROS2 Navigation包官方定位算法，并且AMCL代码接近十年都没有过更改。 同时AMCL也在一些工业机器人场景使用，如国内机器人公司仙工。

这里按照概率机器人这本经典书中的公式，把AMCL过一遍。 首先，给出MCL算法流程如下：

第3行采样次数为$M$，也就是所谓的粒子数为$M$。第4行是”撒粒子“步骤，对应先前说到的先验采样，选用$\mathbf{x}$的条件先验概率密度函数作为建议分布，对于机器人定位而言，建议分布对应机器人的运动模型，如轮式编码器模型。第5行计算粒子权重，根据观测方程，即测量模型给出。第9-10行为重采样步骤。

随机粒子 §

机器人绑定问题指的是机器人定位突然丢失或者突然错误。这种情况在MCL中很显著，当粒子数较少或者粒子分布范围较大时，此时重采样，正确位姿周围可能没有对应的粒子，导致定位失败。解决方案是随机撒一些新的粒子。接下来的问题是撒多少粒子，以及按照什么分布撒？

当定位失败时，观测值的似然概率会落在一个很小的值，因为此时状态$\mathbf{x}$不在合理范围内。粒子滤波中，似然概率就是粒子权重，粒子集权重平均$w_{\mathrm{avg}}$为：

$$w_{\mathrm{avg}}=\frac{1}{M}\sum _{m=1}^M{w}_t^{[m]}\approx p\left(z_t\mid z_{1:t-1},u_{1:t},m\right)$$

但是，似然概率小，不一定对应定位失败，有可能传感器数据噪声本来就很大，或者处于全局定位过程。所以引入一个长期的似然概率平均值$w_{\text{slow }}$和短期的似然概率平均值$w_{\text{fast }}$，通过判断两者有没有明显分化，来识别定位发散的情况，并在此时撒入新的粒子。

这里引入了一个指数移动平均（Exponential Moving Average），在时间维度上去平滑得到$w_{\text{slow }}$和$w_{\text{fast }}$：

$$\begin{array}{rl}& w_{\text{slow }}^k={\alpha }_{\text{slow }}{w}_{\text{avg }}^{k-1}+\left(1-{\alpha }_{\text{slow }}\right)w_{\text{slow }}^{k-1}\\ & w_{\text{fast }}^k={\alpha }_{\text{fast }}{w}_{\text{avg }}^{k-1}+\left(1-{\alpha }_{\text{fast }}\right)w_{\text{fast }}^{k-1}\end{array}$$

指数移动平均（Exponential Moving Average）

定义如下：

$$y[n]=\alpha x[n]+(1-\alpha )y[n-1]$$

其中$y[n]$是当前输出，$y[n-1]$是之前的输出，$x[n]$是当前输入。$\alpha$在0和1之间，等于1时，则不平滑滤波，等于0时，则一直不更新。$\alpha$越大，受到最近输入的影响越大。 “指数“的含义是之前输入对当前输出的影响指数衰减，可由下式看出：

\begin{aligned} y[n] & =\alpha x[n]+(1-\alpha) y[n-1] \ & =\alpha x[n]+(1-\alpha)(\alpha x[n-1]+(1-\alpha) y[n-2]) \ & =\alpha x[n]+(1-\alpha)(\alpha x[n-1]+(1-\alpha)(\alpha x[n-2]+(1-\alpha) y[n-3])) \ & \ldots \ & =\alpha \sum_{k=0}^n(1-\alpha)^k x[n-k] \end{aligned}

通过设置$0\le {\alpha }_{\text{slow }}\ll {\alpha }_{\text{fast }}$，那么当定位失败，$w_{\text{avg }}^k$突然变小，$w_{\text{fast }}^k$将会更快反应出这个变化，随之变小。因此，随机撒入新的粒子需要满足以下条件：

$${\omega }_{new}=1.0-\frac{w_{\text{fast }}}{w_{\text{slow }}}>0.0$$

粒子对应的权重为$1.0-w_{\text{fast }}/w_{\text{slow }}$，即似然概率越小，代表定位结果越差，对应撒的新的粒子的权重就越高。注意，这里提到的定位失败也包含定位结果比较差的情况。

既然机器人碰到绑架问题了，那么说状态先验概率密度函数不灵了，先验采样也失效了，此时应切换到似然采样，将似然函数作为建议分布。参考上一篇文章采样重要性重采样滤波。

对应的AMCL代码流程如下：

与MCL相比，核心变化在13行，当检测到定位失败时，切换到似然采样重新采样，并重新赋予粒子权重。

KLD采样 §

定位未收敛时，我们希望撒入尽可能多的粒子，覆盖整个状态空间。但当定位收敛时，粒子聚拢在真值附近，只需要采样小部分粒子就可以定位。问题来了，怎么确定需要采样多少个粒子呢？

很直接的思路，就是看我们采样近似的后验概率分布函数与真实分布的差异是否足够小，足够小的时候就不需要再去采样了。这个差距叫做KL距离（Kullback-Leibler distance）：

$$K(p,q)=\sum _{x}p(x)\ln \frac{p(x)}{q(x)}$$

显然，我们是不知道真实分布的，没办法直接求KL距离。但是可以假设后验概率分布函数$\underline{X}$服从多项分布$\underline{X}=\sim {\mathrm{Multinomial}}_k(n,\underline{p})$：$\underline{X}=\left(X_1,\dots ,X_k\right)$分布在$k$个不同的格子，总采样数为$n$且$X_1+X_2+\dots +X_k=n$。落入每个格子的概率为$\underline{p}=p_1,p_2,\dots ,p_k$且$p_1+p_2+\cdots +p_n=1$。显然，$\underline{p}$的似然估计是$\underline{\hat{p}}=n^{-1}\underline{X}$。接下来我们看一下似然比统计量${\lambda }_n$。简单来说，似然比统计量就是看采样的样本信息是否符合真实概率分布：

$$\log {\lambda }_n=\sum _{j=1}^k{X}_j\log \left(\frac{{\hat{p}}_j}{p_j}\right)=n\sum _{j=1}^k{\hat{p}}_j\log \left(\frac{{\hat{p}}_j}{p_j}\right)$$

其收敛于${\chi }^2$分布，具体推导可以参考Mathematical Statistics and Data Analysis：$$ 2 \log \lambda_n \rightarrow \chi_{k-1}^2 \quad \text { as } \quad n \rightarrow \infty

$$再回头看，最大似然估计和真实分布之间的KL距离可以写作：$$

K(\widehat{p}, p)=\sum_{j=1}^k \widehat{p}_j \log \left(\frac{\widehat{p}_j}{p_j}\right)

我们认为，当KL距离小于某个值$\varepsilon$时，采样粒子数足以近似后验概率分布函数了，即要求：

K(\underline{\hat{p}}, \underline{p}) \leq \epsilon

$$做一下变换，即$$

\begin{aligned} &2nK(\underline{\hat{p}}, \underline{p}) \leq 2n\epsilon \ \Rightarrow & \ \ \ \ \ \chi_{k-1}^2 \leq 2n\epsilon \end{aligned}

我们要选择一个合适的$n$值，保证卡方分布取值小于$2n\epsilon$的概率足够大，我们预先设定这个大概率为$1-\delta$。对于卡方分布有：

P\left(\chi_{k-1}^2 \leq \chi_{k-1,1-\delta}^2\right)=1-\delta

所以，$n$的取值应为：

n=\frac{1}{2 \epsilon} \chi_{k-1,1-\delta}

此时，最大似然估计和真实分布的KL距离小于$\epsilon$的概率为$1-\delta$。 通过Wilson-Hilferty变换，$\chi_{k-1,1-\delta}^2$可以近似求得，大家可以自己去看一下，这里给出$n$的值如下：

n=\frac{1}{2 \epsilon} \chi_{k-1,1-\delta}^2 \doteq \frac{k-1}{2 \epsilon}\left{1-\frac{2}{9(k-1)}+\sqrt{\frac{2}{9(k-1)}} z_{1-\delta}\right}

其中，$z_{1-\delta}$为标准正态分布的上$1-\delta$分位数。翻译一下，就是符合标准正态分布的随机变量$Z$取值大于$z_{1-\delta}$的概率为$1-\delta$。$z_{1-\delta}$取值为3的示意图如下[正太分布表](https://www.shuxuele.com/data/standard-normal-distribution-table.html)： ![[Pasted image 20231216142935.png]] 另外，$k$的取值反映了粒子覆盖的面积，可以通过直方图来统计。直方图可基于多维栅格或KD树进行统计。直方图$H$中的格子要么为空，要么被至少一个粒子占据。当定位未收敛时，粒子覆盖范围较大，对应$k$取值也变大，那么采样粒子数$n$也会变大，两者几乎呈线性关系。当$\epsilon=0.01$，$z_{1-\delta}=3$时，KLD采样粒子数$n$与$k$的关系如下图所示： ![[Pasted image 20231218094534.png]] 总结，KLD采样在每次粒子滤波迭代过程中，动态确定采样粒子数，使得采样近似的后验概率密度函数与真实分布的误差小于$\epsilon$的概率为$1-\delta$。 增加了KLD采样后的AMCL算法为： ![[Pasted image 20231216151028.png]] 其中第12~19行实现了KLD采样，当新采样粒子落到直方图空格子上时，$k$的值加1，$M_x$随之增大。当实际采样数$M$不断增大，直方图非空格子变少，$k$值不会一直上升，那么$M_x$基本固定，最终$M$会超过阈值$M_x$，触发采样停止条件。这样就实现了动态采样：1）当初始全局定位时，粒子数撒的范围较大，对应$k$值较大，那么应采样的粒子数就得很大；2）正常定位时，粒子数都比较集中，对应$k$值较小，那么所需采样的粒子数也就很小。 ------ *参考：* 1. Thrun S. Probabilistic robotics[J]. Communications of the ACM, 2002, 45(3): 52-57. 2. [CSDN - 自适应粒子滤波及实现：如何计算粒子数](https://blog.csdn.net/Mark_SLAM/article/details/81266527)