概率机器人（Probability Robotics）笔记 Chapter 9: 占据栅格建图（Occupancy Grid Mapping）

1. 简介

前两章讲了定位的概率方法，是一个低维度感知问题，假设地图已知。
但有些情况地图未知，或者不准确。
因此建图可以让部署机器人更容易，也会让机器人更能适应环境变化。
建图是真正的自动机器人的核心竞争力之一。

建图难在：

1. 假设空间巨大:
   地图定义在连续空间上，因此无限高维度；即使离散化，维度也很高。
   因此贝叶斯滤波的方法不好使。
2. 建图是一个“鸡-蛋”问题:
   有地图定位很容易，有定位建图也不难；
   但机器人的里程计精度会逐渐降低，因此建图的时候需要同时定位。

影响建图的难度的因素有:
尺寸，感知和执行的噪声，感知模糊度（不同地方的相似度），闭环场景。

本章首先假设定位已知，然后介绍一系列算法，统称为occupancy grids。
占据栅格地图课从有噪、不确定的测量数据中构建连续的地图。
其思想是用一个在均匀分布的栅格中的随机变量场来表示地图。
每一个随机变量都是二值的，对应其位置是否被占据。
占据栅格地图算法实现了这些随机变量的后验概率估计。

这种地图的用处在于后加工（post-processing）。
因为SLAM产生的地图不适于路径规划与导航，
因此经常在SLAM之后使用占据栅格地图地图算法。

2. 占据栅格建图算法

目标是计算地图后验$p(m|z_{1:t},x_{1:t})$。
由于假设位姿信息已知，因此无需控制信息$u$。

算法使用的是高分辨率的栅格，最常用的领域是二维建筑平面图。
算法也可以泛化至三维，但计算量很大。

把第$i$个格子记为${\mathbf{m}}_i$；
栅格地图由有限多个格子构成，即$m={\sum }_i{\mathbf{m}}_i$。
每个${\mathbf{m}}_i$都可以取两个值，1为占据，0为自由。
表达式$p({\mathbf{m}}_i=1)$和$p({\mathbf{m}}_i)$均指格子被占据的概率。

地图后验的问题在于其维度。
解决的标准做法是估计所有小格子的概率，即$p({\mathbf{m}}_i|z_{1:t},x_{1:t}$。
这样就不能表征相邻格子之间的相关性，即假设格子间独立：
$p(m|z_{1:t},x_{1:t})={\Pi }_{i}p({\mathbf{m}}_i|z_{1:t},x_{1:t}$

可以使用二项贝叶斯滤波器来估计每个格子的值，参考章节4.1.4。
滤波器中使用log-odds表示，即
$l_{t,i}=\log \frac{p({\mathbf{m}}_i|z_{1:t},x_{1:t})}{1-p({\mathbf{m}}_i|z_{1:t},x_{1:t})}$
这种表达的优点是可以回避逼近0值或1值时的数值不稳定问题。
由log-odds ratio计算概率表达式：
$p({\mathbf{m}}_i|z_{1:t},x_{1:t})=1-\frac{1}{1+\exp \{l_{t,i}\}}$

算法 occupancy_grid_mapping($\{l_{t-1,i}\},x_t,z_t$):
1····for all cells ${\mathbf{m}}_i$ do
2········if ${\mathbf{m}}_i$ in perceptual field of $z_t$ then
3············$l_{t,i}=l_{t-1,i}+\text{inverse\_sensor\_model}({\mathbf{m}}_i,x_t,z_t)-l_0$
4········else
5············$l_{t,i}=l_{t-1,i}$
6········endif
7····endfor
8····return ${l_{t,i}}

算法中使用inverse_sensor_model(即$p({\mathbf{m}}_i|z_t,x_t)$)来更新。
对于锥状测距传感器的逆测量模型为：

1. 如果不在测量范围内，则为$l_0$。
2. 如果在，且$z_t^k<z_{max}$，且$|r-z_t^k|<\alpha /2$，则为$l_{\text{occ}}$
3. 如果在，且$r\le z_t^k$，则为$l_{free}$。
   $l_0$是log概率比值比的先验，即$l_0=\log \frac{p({\mathbf{m}}_i=1)}{p({\mathbf{m}}_i=0)}$

前面的算法都只使用传感器数据来更新地图，但还可以使用机器人占据的空间信息；
这个信息可以加进inverse传感器模型中。

2.1 多传感器融合

不同传感器建图的方法不同：
如对于双目，可以把得到的视差图投影到二维空间上，然后对结果用高斯核卷积。

有两种方法可以处理多种传感器的数据：

1. 使用不同的传感器模型来对一张地图进行更新。但是不同传感器检测到不同障碍物，贝叶斯滤波的结果不make sense。
2. 不同传感器分别构建地图，然后总的地图取最保守的估计（最大值），即
   ${\mathbf{m}}_i={\max }_k{\mathbf{m}}_i^k$。

3. 学习逆测量模型

3.1 对测量模型求逆

称$p({\mathbf{m}}_i|x,z)$为逆的原因是：
它根据由这个世界造成的测量数据，提供关于这个世界的信息。

下面推导逆测量模型。
根据贝叶斯定律：
$p({\mathbf{m}}_i|x,z)=\eta {\int }_{m:m(i)={\mathbf{m}}_i}p(z|x,m)p(m_{d}m$$
即对第$i$个格子取值为${\mathbf{m}}_i$的所有地图积分。
很明显，地图太多，无法积分，需要近似。
近似算法包括使用测量模型生成样本，然后使用函数近似器来近似这个逆。

3.2 由正向测量模型生成样本

生成($x_t^{{[k]} z_t}{[k]}\ \bm{m}_i^{[k]})样本的步骤如下：

1. 采样一个随机地图$m^{[k]}\sim p(m)$。
2. 在地图中采样一个位姿$x_t^{[k]}$。
3. 采样一个测量$z_t^{[k]}\sim p(z|x_t^{[k]},m^{[k]})$。
4. 由地图信息，提取${\mathbf{m}}_i$的occupancy真值。

这样一系列三元组可以作为训练集。

这个做法有点低效，因为它并没有利用一些逆传感器模型的特性，如：

1. 测量数据并不能带来距离之外的信息，也就是说可以只采样距离之内的格子。
2. 测量与格子和机器人的绝对位置无关，而与相对位置有关。
3. 相邻的格子应当相似。
4. 如果机器人携带相同的传感器，那么模型应当可以通用。

可以把以上作为限制条件，加入样本生成中，以减小数据集大小。

3.3 误差函数

为了训练函数，需要一个近似的误差函数，如使用BP算法的神经网络。
训练集的格式为：${\text{input}}^{[i]}\to \text{occ}({\mathbf{m}}_i{)}^{[i]}$。

假设每个训练数据都相互独立，记函数近似器的参数为$W$，则训练集的可能性为
${\Pi }_{i}p({\mathbf{m}}_i^{[k]}|{\text{input}}^{[k]},W)$;
其log为$J(W)={\sum }_i\log p({\mathbf{m}}_i^{[k]}|{\text{input}}^{[k]},W)$。
训练即最小化函数值J。

记函数近似器为$f({\text{input}}^{[k]},W)$，输出为0到1之间的数（即占据可能性）；
也就是：
$$p({\mathbf{m}}_i^{[k]}|{\text{input}}^{[k]},W)=\{\begin{array}{rl}f({\text{input}}^{[k]},W)& if{\mathbf{m}}_i^{[k]}=1\\ 1-f({\text{input}}^{[k]},W)& if{\mathbf{m}}_i^{[k]}=0\end{array}$$
即
$$p({\mathbf{m}}_i^{[k]}|{\text{input}}^{[k]},W)=f({\text{input}}^{[k]},W{)}^{{\mathbf{m}}_i^{[k]}}(1-f({\text{input}}^{[k]},W){)}^{1-{\mathbf{m}}_i^{[k]}}$$

将上式带入前文$J(W)$的定义，可以得到误差函数：
$$J(W)=-\sum _i{\mathbf{m}}_i^{[k]}\log f({\text{input}}^{[k]},W)+(1-{\mathbf{m}}_i^{[k]})\log (1-f({\text{input}}^{[k]},W))$$

3.4 更多考虑

在样例中，输入的数据是极坐标系下的相对坐标。

在写这本书的时候，学习逆向测量模型的工作较少。

4. 最大后验占据栅格建图

4.1 维护相关性的情况(The Case for Maintaining Dependencies)

前文假设地图可以解构成相互独立的小格子。
这样一个因数分解导致问题：
因为一个格子可能被多个测量覆盖，且相邻格子有相关性，，所以可能产生矛盾的测量；
导致某个格子的置信度取决于两个测量的频率。
本质上，这是因为测量覆盖多个格子，所以格子间有相关性。

4.2 前向模型下的占据栅格建图

有一个算法可以结合这些相关性，算法输出后验的模式(mode)。
模式的定义是地图的最大后验的log，也就是
$m^{\ast }={\text{argmax}}_m\log p(m|z_{1:t},x_{1:t})$。
地图的后验包括一个地图的先验和一个测量的可能性，即：
$\log p(m|z_{1:t},x_{1:t})=\log p(z_{1:t}|x_{1:t},m)+\log p(m)$
右式的第一项可以分解成所有的测量的log，即$\log p(z_t|x_t,m)$。
而第二项是先验的和，即$\log p(m)=M\log (1-p({\mathbf{m}}_i))+{\sum }_i(l_0{)}^{{\mathbf{m}}_i}$
由于上式左边与地图无关，因此最大后验的地图为：
$m^{\ast }={\text{argmax}}_m{\sum }_t\log p(z_t|x_t,m)+{\sum }_i(l_0{)}_i^{\mathbf{m}}$

可以使用爬坡算法来最大化这个log概率。
算法的优化起点是一张所有格子都free的地图，然后通过翻转某些格子，来提高概率。
对于这种算法，地图先验不能太靠近1，否则会返回一个全占据的地图。
这种算法只能返回一个局部极大值。但实践中，很少有局部极值。

算法 MAP_occupancy_grid_mapping($x_{1:t},z_{1:t}$):
1····set $m=\{0\}$
2····repeat until convergence
3········for all cells ${\mathbf{m}}_i$ do
4············$m_i={\text{argmax}}_{k=0,1}(l_0{)}^k+{\sum }_t\log \text{measurement\_model}(z_t,x_t,m\text{with}{\mathbf{m}}_i=k)$
5········endfor
6····endrepeat
7····return $m$

MAP的主旨是对整个时间轴的测量数据做统一优化。

MAP的问题和解决：

1. 不能提供不确定性：可以使用敏感度分析（一个格子的敏感度是翻转这个格子的值对地图后验的影响）来近似不确定性，但是这个近似太过自信。
2. 算法是批处理的算法：要求把所有数据都存下来，不能递增地运行。
3. 速度：可以以普通OG的结果作为优化的起点，会快一些。

5. 总结

本章中所有算法都需要位姿已知，因此并不能解决普遍的建图问题。

1. 标准的占据栅格建图算法独立估计每个格子的后验，是二值贝叶斯滤波器的应用。
2. 多种传感器的融合有两种方式：维护一张或多张地图，后者提取最消极的占据值。
3. 标准的占据栅格建图算法依赖逆向测量模型，是从effect推cause。
4. 可以学习逆向测量模型。
5. 标准占据栅格建图算法不能维护格子间的相关性。
6. 整个地图的后验通常无法计算，但可以用数据优化。但优化需要所有的数据，且不能计算地图的不确定性残差。