【论文阅读】基于贝叶斯广义核推理的可通行区域建图

标题：Bayesian Generalized Kernel Inference forTerrain Traversability Mapping
作者：Tixiao Shan，Jinkun Wang，Brendan Englot and Kevin Doherty
来源：CoRL 2018
代码:
https://github.com/TixiaoShan/traversability_mapping/tree/master/traversability_mapping

1. 主要工作

本文提出贝叶斯广义核推理来解决可通行区域建图问题：
【1】将BGK高程推断应用于解决地形建图中遇到的稀疏数据问题；
【2】仅对选定位置的高程数据进行可通行性计算，从而减轻了计算负担；
【3】通过BGK可通行性推断来估计其他位置的可通行性。

这个框架可以使用稀疏激光雷达数据和与小型UGV兼容的硬件来实时建立可通行区域地图，是贝叶斯广义核推断在可通行性地图上的第一次应用。

2. 系统流程

3. 基于贝叶斯广义核推理的可通行区域建图

3.1 贝叶斯广义核推理

给定观测： D = { ( x i , y i ) i = 1 : N } \mathcal{D}=\left\{\left(\mathbf{x}_{i}, y_{i}\right)_{i=1: N}\right\} D={(xi​,yi​)i=1:N​}，试图推断在潜空间上参数化的目标值的概率分布：
$\Theta :p\left(y^{\ast }\mid {\mathbf{x}}^{\ast },\mathcal{D}\right)\propto \int p\left(y^{\ast }\mid {\mathbf{\theta }}^{\ast }\right)p\left({\mathbf{\theta }}^{\ast }\mid {\mathbf{x}}^{\ast },\mathcal{D}\right)d{\mathbf{\theta }}^{\ast }$

其中，
$p\left({\mathbf{\theta }}^{\ast }\mid {\mathbf{x}}^{\ast },\mathcal{D}\right)\propto {\int }_{{\mathbf{\theta }}_{1\cdot N}}{\prod }_{i=1}^{N}p\left(y_i\mid {\mathbf{\theta }}_i\right)p\left({\mathbf{\theta }}_{1:N},{\mathbf{\theta }}^{\ast }\mid {\mathbf{x}}_{1:N},{\mathbf{x}}^{\ast }\right)d{\mathbf{\theta }}_{1:N}$，

为与目标输入相关的潜参数的后验分布。

在高斯过程中，假设所有参数 $\theta$ 是相关的，而在贝叶斯广义核推理中，假设在给定目标输入的情况下，与观测输入相关的参数条件独立：
$p\left({\mathbf{\theta }}_{1:N},{\mathbf{\theta }}^{\ast }\mid {\mathbf{x}}_{1:N},{\mathbf{x}}^{\ast }\right)=$${\prod }_{i=1}^{N}p\left({\mathbf{\theta }}_i\mid {\mathbf{x}}_i,{\mathbf{\theta }}^{\ast },{\mathbf{x}}^{\ast }\right)p\left({\mathbf{\theta }}^{\ast }\mid {\mathbf{x}}^{\ast }\right),$

将其代入上式可得：

$p\left({\mathbf{\theta }}^{\ast }\mid {\mathbf{x}}^{\ast },\mathcal{D}\right)\propto$ ${\prod }_{i=1}^{N}p\left(y_i\mid {\mathbf{\theta }}^{\ast },{\mathbf{x}}^{\ast },{\mathbf{x}}_i\right)p\left({\mathbf{\theta }}^{\ast }\mid {\mathbf{x}}^{\ast }\right)$

其中，$p\left(y_i\mid {\mathbf{\theta }}^{\ast },{\mathbf{x}}^{\ast },{\mathbf{x}}_i\right)$ 为扩展似然分布。

通过构造一个平滑的扩展似然模型，可以得到（见参考文献[1]，2.1节）：

$$p\left({\mathbf{\theta }}^{\ast }\mid {\mathbf{x}}^{\ast },\mathcal{D}\right)\propto \prod _{i=1}^{N}p{\left(y_i\mid {\mathbf{\theta }}^{\ast }\right)}^{k\left({\mathbf{x}}_i,{\mathbf{x}}^{\ast }\right)}p\left({\mathbf{\theta }}^{\ast }\mid {\mathbf{x}}^{\ast }\right)...(1)$$

其中，$k(\cdot ,\cdot )$为核函数。

如果似然模型来自指数族，并且假设相应的共轭先验，则可以精确地确定后验。

3.2 稀疏核

$k\left(\mathbf{x},{\mathbf{x}}^{\ast }\right)=\{\begin{array}{ll}\frac{2+\cos \left(2\pi \frac{d}{l}\right)}{3}\left(1-\frac{d}{l}\right)+\frac{1}{2\pi }\sin \left(2\pi \frac{d}{l}\right),& \text{ if }d\le l\\ 0,& \text{ otherwise }\end{array}$

其中，$d={\Vert \mathbf{x}-{\mathbf{x}}^{\ast }\Vert }_2$，核的支持区间为$[0,l]$，这使得能够在对数线性时间内执行精确的推理。

3.3 贝叶斯核高程回归

假设高度$y$服从高斯分布：$y\sim \mathcal{N}\left(\mu ,{\sigma }^2\right)$，其中方差 ${\sigma }^2$ 已知，共轭先验也服从高斯分布：$\mu \sim \mathcal{N}\left({\mu }_0,{\sigma }^2/\lambda \right)$，其中，超参数$\lambda$反映对先验的置信度，$\lambda =0$ 表示没有先验，$\lambda \to \infty$ 表示具有十分充足的先验知识。

由公式（1）可知：

$p\left({\mu }^{\ast }\mid {\mathbf{x}}^{\ast },\mathcal{D}\right)\propto {\prod }_{i=1}^N\exp \left\{-\frac{1}{2}\frac{{\left(y_i-\mu \right)}^2}{{\sigma }^2}k\left({\mathbf{x}}_i,{\mathbf{x}}^{\ast }\right)\right\}\exp \left\{-\frac{1}{2}\frac{{\left(\mu -{\mu }_0\right)}^2}{{\sigma }^2}\lambda \right\}$

可得后验参数的均值和方差：

$$\mathbb{E}\left[{\mu }^{\ast }\mid \lambda ,\mathcal{D},{\mathbf{x}}^{\ast }\right]=\frac{\lambda {\mu }_0+{\sum }_{i=1}^{N}k\left({\mathbf{x}}_i,{\mathbf{x}}^{\ast }\right)y_i}{\lambda +{\sum }_{i=1}^{N}k\left({\mathbf{x}}_i,{\mathbf{x}}^{\ast }\right)}$$

$$\mathrm{Var}\left[{\mu }^{\ast }\mid \lambda ,\mathcal{D},{\mathbf{x}}^{\ast }\right]=\frac{{\sigma }^2}{{\lambda }^{\ast }}$$

其中，${\lambda }^{\ast }=\lambda +{\sum }_{i=1}^{N}k\left({\mathbf{x}}_i,{\mathbf{x}}^{\ast }\right)$，

从而，由 $\mathbb{E}\left[y^{\ast }\mid \lambda ,\mathcal{D},{\mathbf{x}}^{\ast }\right]=\mathbb{E}\left[{\mu }^{\ast }\mid \lambda ,\mathcal{D},{\mathbf{x}}^{\ast }\right]$ 可以得到后验预测分布的均值，即通过推理得到的高程数据 $y$，进一步得到稠密的高程图$m_e$.

3.4 贝叶斯核可通行性分类

将每个网格单元的可通行性建模为满足伯努利二元分布的随机变量，$y\sim \mathrm{Ber}(\theta )$，通过对参数 ${\theta }^{\ast }$ 的估计，来进行可通行性分类。

采用的共轭先验满足Beta分布，$\theta \sim \mathrm{Beta}\left({\alpha }_0,{\beta }_0\right)$，其中，${\alpha }_0$，${\beta }_0$为超参数。

后验依然满足Beta分布：

$$p\left({\theta }^{\ast }\mid {\mathbf{x}}^{\ast },\mathcal{D}\right)\propto {\theta }^{{\alpha }^{\ast }-1}(1-\theta {)}^{{\beta }^{\ast }-1}$$

其中，

$${\alpha }^{\ast }={\alpha }_0+\sum _{i=1}^{N}k\left({\mathbf{x}}_i,{\mathbf{x}}^{\ast }\right)y_i$$$${\beta }^{\ast }={\beta }_0+\sum _{i=1}^{N}k\left({\mathbf{x}}_i,{\mathbf{x}}^{\ast }\right)\left(1-y_i\right)$$

从而可以得到后验预测分布的均值和方差：

$$\mathbb{E}\left[y^{\ast }\mid {\alpha }_0,{\beta }_0,\mathcal{D},{\mathbf{x}}^{\ast }\right]=\frac{{\alpha }^{\ast }}{{\alpha }^{\ast }+{\beta }^{\ast }}$$ $$\mathrm{Var}\left[y^{\ast }\mid {\alpha }_0,{\beta }_0,\mathcal{D},{\mathbf{x}}^{\ast }\right]=\frac{{\alpha }^{\ast }{\beta }^{\ast }}{{\left({\alpha }^{\ast }+{\beta }^{\ast }\right)}^2}$$

3.4.1 可通行性训练数据

网格单元的可通行性主要由3个特征决定，陡度$h$，倾斜度$s$，粗糙度$r$：

$$v={\alpha }_1\frac{h}{h_{\text{crit }}}+{\alpha }_2\frac{s}{s_{\text{crit }}}+{\alpha }_3\frac{r}{r_{\text{crit }}}...(2)$$

其中，${\alpha }_1，{\alpha }_2，{\alpha }_3$ 分别代表权重，相加之和为1，$h_{\text{crit }}，s_{\text{crit }}，r_{\text{crit }}$，分别代表可能会引起机器人侧翻，卡住的最大的陡度、倾斜度、粗糙度。

$v$ 在[0,1]之间，小值表示局部地形平坦光滑，大值表示地形粗糙，如果有其中一个特征超过其临界值，则相应的单元格被标记为不可通行。

其中，陡度$h$，倾斜度$s$，粗糙度$r$的计算参考[2]（II C）.

当网格单元的高度由于最新测量的到来而发生改变时，至少在机器人半径内的所有相邻单元的可通行性需要重新计算。然而，使用等式(2)直接计算可通行性涉及平面拟合和特征值分解，难以实时使用。

因此，仅对与激光雷达点直接相交的网格单元进行可通行性计算，再通过BGK可通行性推断来估计其他位置的可通行性。

3.4.2 可通行性推断

可通行地图$m_v$中每个网格单元的状态：

state $=\{\begin{array}{ll}\text{ traversable, }& \text{ if }v<v_{th},{\sigma }^2<{\sigma }_{th}^2\\ \text{ non-traversable, }& \text{ otherwise }\end{array}$

$v$是该单元格预测的可通行性的均值，$v_{th}$是可通行性的阈值，方差的阈值为 ${\sigma }_{th}^2$，当方差${\sigma }^2$大于 ${\sigma }_{th}^2$时，单元格也被标记为不可通过。

4. 实验结果

本文在仿真和实际环境进行了测试。

4.1 仿真环境

4.1.1 结构化环境-City

图1：结构化环境仿真：上图(a)中是Gazebo中模拟城市环境的俯视图。(b)是可通行区域地图的ground truth。（c）是由基线法生成的可通行区域地图。(d) 是由BGK+Trav方法生成的可通行区域地图。(e)是本文提出的方法生成的可通行区域地图。(f)是计算出来的BGK${}^{+}$方差图。白色颜色表示方差低，洋红色表示方差高。

4.1.2 非结构化环境-Aerial

图2：非结构化环境仿真：空中模拟地形模型如图(a)所示. （b),（c）,（d）,（e）分别是ground truth，基线法，BGK+Trav，BGK${}^{+}$。(f)是BGK${}^{+}$中的方差图。

图3：ROC曲线

表1：不同建图方法的量化结果

4.2 大型的城市环境

图4：使用BGK${}^{+}$生成的大场景城市可通行区域地图：左边的图是卫星照片。中间的图是BGK${}^{+}$方法生成的可通行区域地图。右图是一些代表性场景的结果。

所提出的方法成功地区分了可通行区域和不可通行区域.

5. 结论

本文提出将贝叶斯广义核推理用于可通性地形图。

本文的框架独特之处在于它的组成，具有两个顺序的步骤。第一个步骤实现了高度推断来解决可用点云的稀疏性，然后第二个步骤实现了可通性推断来减少穷举可通性计算的负担。

本文提出的框架已经在模拟数据和真实数据中进行了验证，为激光雷达实时构建地形图提供了效率和准确性。未来的工作将考虑协方差信息的实际使用，以更好地支持复杂环境下的安全导航。

参考文献

[1] W. R. V ega-Brown, M. Doniec, and N. G. Roy. Nonparametric Bayesian inference on multi-variate exponential families. Advances in Neural Information Processing Systems, pp. 2546-
2554, 2014.
[2] A. Chilian and H. Hirschmuller. Stereo Camera Based Navigation of Mobile Robots on Rough
Terrain. Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Sys-
tems, pp. 4571-4576, 2009.