概率机器人（Probabilistic Robotics）笔记 Chapter 8: 栅格定位与蒙特卡洛定位（Grid and Monte Carlo Localization）

1. 简介

本章介绍两个全局定位算法，与第七章介绍的高斯方法有所不同：

1. 可以处理原始传感器数据，不用提取特征，也可以处理负信息。
2. 非参数化，不受限于EKF的单模型分布。
3. 可以解决全局定位问题，有时甚至可以解决绑架问题。

第一个算法叫栅格定位。它使用一个直方图滤波器来表示后验置信。缺点是，如果栅格划分很细，则计算量很大。如果划分粗糙，则离散化时信息丢失会影响滤波器表现。

第二个算法叫蒙特卡洛定位（MCL）算法，也是目前最流行的。它使用粒子滤波器来估计后验。

2. 栅格定位

2.1 基础算法

算法伪代码：
1····for all $k$ do
2········${\bar{p}}_{k,t}={\Sigma }_i{p}_{i,t-1}\mathbf{m}\mathbf{o}\mathbf{t}\mathbf{i}\mathbf{o}\mathbf{n}\_\mathbf{m}\mathbf{o}\mathbf{d}\mathbf{e}\mathbf{l}(mean({\mathbf{x}}_k),u_t,mean({\mathbf{x}}_i))$
3········$p_{k,t}=\eta \mathbf{m}\mathbf{e}\mathbf{a}\mathbf{s}\mathbf{u}\mathbf{r}\mathbf{e}\mathbf{m}\mathbf{e}\mathbf{n}\mathbf{t}\_\mathbf{m}\mathbf{o}\mathbf{d}\mathbf{e}\mathbf{l}(z_t,mean({\mathbf{x}}_k),m)$
4····endfor
5····return {$p_{k,t}$}

以上是一个离散贝叶斯滤波器，其中后验概率为离散的概率值的集合，即$bel(x_t)=\{p_{k,t}\}$，而$p_{k,t}$是定义在格子${\mathbf{x}}_k$上的。

在基础的栅格定位中，对空间的划分是均匀（通常15厘米、5度分辨率）且时不变的。

2.2 栅格分辨率

栅格定位的一个关键变量是分辨率。有两种极端选择，都挺好用：

1. 粗糙、分辨率可变的栅格：把空间分解成与环境中的重要地点关联的一系列区域，可以由存在或缺失某一地标来定义。走廊环境中，地点可以与路口关联，因此分解的分辨率与环境结构相关。这样的粗糙表达通常与空间的拓扑表达相关。
2. 细致、分辨率固定的栅格：把空间均匀分割。

使用粗糙分辨率时，应当在测量和运动模型中对分辨率的粗糙度进行补偿，不能只评估格子的重心。例如，如果运动模型每秒更新一次，机器人运动10厘米，而分辨率1米，则由格子重心向任何方向运动，都会回到这个格子，因此状态永远不转换。

一种普遍的做法是放大测量和运动模型中的噪声。例如，可以把测距模型的主高斯锥的方差增大半个分辨率。再例如，运动模型可以加入一个随机进入相邻格子的概率，此概率与步长成正比。

2.3 计算考虑

当使用分辨率高的网格时，计算可能无法实时：运动模型对3D栅格进行卷积，是一个6D运算。测量模型是一个3D运算，但是计算所有scan的可能性的运算量非常大。

减小计算复杂度的方法：

1. 预缓存（Pre-caching）: 为了减少测量模型中光束追踪的计算量，可以提前进行一些计算，比如用光束模型时，可以缓存格子重心点的距离。
2. 传感器二次抽样：使用一部分光束。
3. 延迟运动更新：降低运动更新频率，如短时间内积分。可以把运算速度提高一个数量级。
4. 选择性更新：当更新栅格的置信度时，只更新后验概率大于某阈值的格子，可以把运算量降低几个数量级，但是不太适用于绑架问题。

在算力足够的情况下，栅格越细越好。

2.4 图示

略过…

3. 蒙特卡洛定位(MCL)

3.1 MCL算法

基于粒子滤波器，可以应用于局部定位和全局定位。这个算法非常流行，容易实现。

算法MCL（${\chi }_{t-1},u_t,z_t,m$）：
1····${\bar{\chi }}_t={\chi }_t=\varphi$
2····for $m=1$ to $M$ do
3········$x_t^{[m]}=\mathbf{s}\mathbf{a}\mathbf{m}\mathbf{p}\mathbf{l}\mathbf{e}\_\mathbf{m}\mathbf{o}\mathbf{t}\mathbf{i}\mathbf{o}\mathbf{n}\_\mathbf{m}\mathbf{o}\mathbf{d}\mathbf{e}\mathbf{l}(u_t,x_{t-1}^{[m]})$
4········$w_t^{[m]}=\mathbf{m}\mathbf{e}\mathbf{a}\mathbf{s}\mathbf{u}\mathbf{r}\mathbf{e}\mathbf{m}\mathbf{e}\mathbf{n}\mathbf{t}\_\mathbf{m}\mathbf{o}\mathbf{d}\mathbf{e}\mathbf{l}(z_t,x_t^{[m]},m)$
5········${\bar{\chi }}_t={\bar{\chi }}_t+⟨x_t^{[m]},w_t^{[t]}⟩$
6····endfor
7····for $m=1$ to $M$ do
8········draw $i$ with probability $\propto w_t^{[i]}$
9········add $x_t^{[i]}$ to ${\chi }_t$
10···endfor
11···return ${\chi }_t$

基础MCL算法使用M个粒子来代表置信度$bel(x_t)$。粒子滤波器首先使用运动模型的概率分布来采样粒子，然后用测量模型判断这个粒子的权重（不改变粒子分布），最后根据权重重新采样（原来权重高的粒子周围产生更多新粒子，但新产生的粒子权重一样）。初始置信度是均匀分布粒子，权重为$M^{-1}$。

3.2 MCL的特性

MCL可以近似任何形式的分布，而不是仅限于参数化的分布（如EKF），而近似的精确度由粒子的个数决定。粒子个数的选择要权衡计算的精确度和复杂度。一种常用策略是不停地采样，直到下一组控制和测量到来。这种可以自适应计算资源的方法，难以用于栅格定位和高斯方法的定位。

MCL可以表示非常复杂的多模型概率分布，并且可以与窄高斯分布无缝混合。这让MCL可以进行全局定位，同时有高精度的位置追踪。

3.3 随机粒子MCL（Random Particle MCL）: 失败恢复

MCL不可以解决绑架问题，也不能处理全局定位失败。有时，粒子只存在于某一个位姿周围，如果这个位姿是错的，算法无法恢复。

这个问题非常重要。任何概率的方法都会有所有粒子都错误的可能性，如MCL的二次采样，当粒子少或者空间很大的时候尤为明显。

解决这个问题的思路非常简单：增加随机的粒子。即使没有绑架的情况，这种方法也可以增加鲁棒性。

增加粒子的数目：可以加固定数量，也可以根据定位精度$p(z_t|z_{t-1},u_t,m)$。由于权重是精度的概率估计，也就是$$p(z_t|z_{t-1},u_t,m)\approx \frac{1}{M}{\Sigma }_{m=1}^M{w}_t^{[m]}$$

可以在一个时间窗口内求平均，来平滑这个估计。

增加粒子的分布：可以是位姿空间上的均匀分布，然后使用当前的观测设定权重，也可以在某些方法中根据观测的可能性直接分配粒子。

算法 Augmented_MCL:
01····static $w_{slow}$, $w_{fast}$
02····${\bar{\chi }}_t={\chi }_t=\varphi$
03····for $m=1$ to $M$ do
04········$x_t^{[m]}=\mathbf{s}\mathbf{a}\mathbf{m}\mathbf{p}\mathbf{l}\mathbf{e}\_\mathbf{m}\mathbf{o}\mathbf{t}\mathbf{i}\mathbf{o}\mathbf{n}\_\mathbf{m}\mathbf{o}\mathbf{d}\mathbf{e}\mathbf{l}(u_t,x_{t-1}^{[m]})$
05········$w_t^{[m]}=\mathbf{m}\mathbf{e}\mathbf{a}\mathbf{s}\mathbf{u}\mathbf{r}\mathbf{e}\mathbf{m}\mathbf{e}\mathbf{n}\mathbf{t}\_\mathbf{m}\mathbf{o}\mathbf{d}\mathbf{e}\mathbf{l}(z_t,x_t^{[m]},m)$
06········${\bar{\chi }}_t={\bar{\chi }}_t+⟨x_t^{[m]},w_t^{[t]}⟩$
07········ $w_{avg}=w_{avg}+\frac{1}{M}{w}_t^{[m]}$
08····endfor
09····$w_{slow}=w_{slow}+{\alpha }_{slow}(w_{avg}-w_{slow})$
10····$w_{fast}=w_{fast}+{\alpha }_{fast}(w_{avg}-w_{fast})$
11····for $m=1$ to $M$ do
12········with probability $max(0.0,1.0-w_{fast}/w_{slow})$ do
13············add random pose to ${\chi }_t$
14········else
15············draw $i\in \{1,...,N\}$ with probability $\propto w_t^{[i]}$
16············add $x_t^{[i]}$ to ${\chi }_t$
17········endwith
18····endfor
19····return ${\chi }_t$

第9行和第10行实在进行指数滤波，而系数${\alpha }_{slow},{\alpha }_{fast}$是衰减速率，$0\le {\alpha }_{slow}\ll {\alpha }_{fast}$。

3.4 修改提议分布（Proposal Distribution）

MCL的一个限制是它的提议机制，即使用运动模型作为提议分布，但试图近似这个分布下的产物。提议分布和目标分布的区别越大，就需要更多样本。

这导致了MCL中的一种失败模式：如果传感器理想（无噪声），则会定位失败，即使这个传感器并不会产生足够的定位信息。问题在于，EKF是结合了测量数据来生成新的均值，而MCL只使用运动模型来生成。

解决方案有两种，一种是放大测量模型中的噪声；另一种更好的是对于一小部分粒子，反转测量模型和运动模型的角色：首先根据测量模型生成新粒子，即$x_t^{[m]}\sim p(z_t|x_t)$，然后根据运动模型计算权重，即$w_t^{[m]}=\int p(x_t^{[m]}|u_t,x_{t-1})bel(x_{t-1})d{x}_{t-1})$。这种方法称为MCL with mixture distribution，或mixture MCL。这种模型不可以单独使用，因为生成粒子的时候完全无视粒子之前的位置。通常用这种模型生成5%的新粒子。

这种方法的问题在于，两个步骤都有可能难以实现。只有当测量模型的逆有closed form解时，才容易从测量模型中采样，但通常并非如此。运动模型中的积分难算，因为$bel(x_{t-1})$是用粒子表示的。

4. 动态环境中的定位

前文提到的定位算法都有一个关键限制：静态环境假设，或马尔科夫假设。概率机器人学可以把动态物体视作噪声，可有一定鲁棒性；但是假设不同时刻到来的噪声互相独立，而没有建模的动态障碍物通常都会有一定持续性。如果这种效应非常明显，基于静态假设的概率定位方法可能失败。

两种方法：一种将隐藏状态状态变量，由滤波器一同估计，数学上更泛化，但计算复杂度大，且变量的数量本身也是变量；另一种对传感器数据进行预处理，扔掉隐藏状态影响的测量数据，这种方法在特定受限的场景下工作良好。下面讨论第二种方法。

思路是探究传感器测量的生成，然后拒绝掉有可能是受动态物体影响的测量数据。在第六章中说，测量模型由四种模型组成，即$p_{hit},p_{short},p_{max},p_{rand}$，其中$p_{short}$是与意料之外的物体相关联的。定义变量${\bar{c}}_t^k$，可以分别取{hit，short，max，rand}四个值，那么根据贝叶斯定律，可以计算：

$$p({\bar{c}}_t^k=short|z_t^k,z_{1:t-1},u_{1:t},m)=\frac{p(z_t^k|{\bar{c}}_t^k=short,z_{1:t-1},u_{1:t},m)p({\bar{c}}_t^k=short)}{{\sum }_{c}p(z_t^k|{\bar{c}}_t^k=c,z_{1:t-1},u_{1:t},m)p({\bar{c}}_t^k=c)}$$

上式中$p(z_t^k|{\bar{c}}_t^k=c,z_{1:t-1},u_{1:t},m)=\int p(z_t^k|x_t,{\bar{c}}_t^k=c,m)\overline{bel}(x_t)d{x}_t$。

由于$p(z_t^k|x_t,{\bar{c}}_t^k=c,m)$可以简写为$p_{hit},p_{short},p_{max},p_{rand}$，所以可以得到想要的概率的表达式：

$$p({\bar{c}}_t^k=short|z_t^k,z_{1:t-1},u_{1:t},m)=\frac{\int p_{short}(z_t^k|x_t,m)z_{short}\overline{bel}(x_t)d{x}_t}{\int {\Sigma }_c{p}_c(z_t^k|x_t,m)z_c\overline{bel}(x_t)d{x}_t}$$

上面这些积分没有closed-form解，为了计算，需要用代表后验概率$\overline{bel}(x_t)$的样本来近似，这些样本可以是可能性栅格，也可以是MCL中的粒子。如果某个测量的上述概率超过一定阈值，那么这个测量会被认做是受到动态物体影响，进而舍弃。

下面给出一种结合了这种技巧的粒子滤波器：

算法 test_range_measurement($z_t^k,{\bar{\chi }}_t,m$):
01····$p=q=0$
02····for $m=1$ to $M$ do
03········$p=p+z_{hit}\cdot p_{hit}(z_t^k|x_t^{[m]},m)$
04········$q=q+{\sum }_c{z}_c\cdot p_c(z_t^k|x_t^{[m]},m)$
05····endfor
06····if $p/q\le \chi$ then
07········return accept
08····else
09········return reject
10····endif

使用以上算法的效果就是，意外地短的测量被筛除，而意外地长的测量被保留。这样一个不对称性可以扔掉被动态障碍物影响的测量，同时保持了定位失败恢复的能力。总的来讲，做测量的筛选总是个好主意。这样的不对称性可能并不适用于其他数据。

5. 实践考虑

关于定位算法的选择，首先要考虑的是，是否应当从传感器测量数据中提取特征。

关于前两章五种定位算法的比较：

\	EKF	MHT	粗糙(拓扑)栅格	细致(均匀)栅格	MCL
测量	地标	地标	地标	原始数据	原始数据
测量噪声	高斯	高斯	any	any	any
后验	高斯	混合高斯	直方图	直方图	粒子
内存效率	++	++	+	-	+
时间效率	++	+	+	-	+
易于实现	+	-	+	-	++
分辨率	++	++	-	+	+
鲁棒性	-	+	+	++	++
全局定位	no	no	yes	yes	yes

当实现一个定位算法时，应当调整参数。可以首先采集参考数据集，然后调参。这是因为无论模型多准确，永远会有没有建模的情况，并且系统误差也会影响结果。

6. 总结

1. 栅格算法用直方图表示后验。
2. 栅格算法通过分辨率平衡精确度和计算复杂度。为了补偿栅格的粗糙度，通常需要调整测量和运动模型。对于细致的栅格，应当选择性地更新格子。
3. MCL用粒子表征后验。通过粒子个数来平衡精度和计算难度。
4. 两种算法都可以完成全局定位。
5. 通过增加随机粒子，MCL可以解决绑架问题。
6. Mixture-MCL通过反转一部分粒子的生成方式，提高高精度传感器定位的表现，但实现难度较大。
7. 可以通过筛选传感器数据来应对动态物体。当使用距离传感器时，通常筛除意外地短的测量数据。