概率机器人（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}$}

以上是一个离散贝叶斯滤波器，其中后验概率为离散的概率值的集合，即 b e l ( x t ) = { p k , t } bel(x_t)=\{p_{k,t}\} bel(xt​)={ pk,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