0%

The GraphSLAM algorithm with applications to large scale mapping of urban structures: Part 3

前言

这是我阅读 The GraphSLAM algorithm with applications to large-scale mapping of urban structures的第三篇笔记。关于第一篇可以看 The GraphSLAM algorithm with applications to large scale mapping of urban structures: Part 1。在第二篇中已经总结了 GraphSLAM 的基本思路和具体算法,这一篇主要是进行 GraphSLAM 算法原理的具体推导。

GraphSLAM 的数学推导分为以下几步:

  • 推导出一个关于 SLAM 全后验概率的递归公式
  • 对每一个后验概率进行单独分析其对 SLAM 问题的影响
  • 推导回复机器人位姿和地图特征点必要的公式

SLAM 后验概率

首先,我们用 $y$ 来表示 SLAM 中的状态量,包括一个或多个机器人位姿 $x$ 和地图 $m$,定义如下:

根据以上定义, SLAM 问题的后验概率是:

其中:

  • $z_{1:t}$ 是观测值
  • $c_{1:t}$ 是观测值观测到特征点的序号
  • $u_{1:t}$ 是控制输入量

通过贝叶斯法则,后验概率可以写成以下形式:

其中, $\eta$ 是和 $p(z_t)$ 有关的常系数。由于观测值只跟当前时刻状态和特征点系数有关,上面等式右边第一项可以简化为:

同样,我们可以将 $y{0:t}$ 分成 $x_t$ 和 $y{0:t-1}$ 两部分,所以等式右侧第二项可以化为:

所以 SLAM 的后验概率公式可以转化为:

以上是后验概率关于 t 的迭代公式,将其展开至 $t = 0$可以写成:

上式中,$zt^i$ 是 $t$ 时刻第 $i$ 个观测值。这里注意一下,先验概率 $p(y_0)$ 可以分成 $p(x_0)$ 和 $p(m)$ 两部分。而在 SLAM 中,我们通常假定没有对地图的先验知识,所以在后面计算中会将 $p(y_0)$ 用 $p(x_0)$ 代替,而把 $p(m)$ 吸收进系数 $\eta$ 中。

负对数后验概率

上式中的概率可以用对数来简化表示和便于计算,所以对数-SLAM的后验概率可以写成以下形式:

根据之前的假设,我们假定运动误差和观测误差都是服从高斯分布,所以:

  • 机器人运动模型的概率服从 $\mathcal{N}(g(ut, x{t-1}), R_t)$,其中 $g$ 是运动模型函数,是确定性的;$R_t$ 则是运动误差的协方差矩阵
  • 观测模型服从 $\mathcal{N}(h(x_t, c_t^i, i), W_t)$,其中 $h$ 是观测模型函数,是确定性的;$Q_t$ 则是观测误差的协方差矩阵

所以对上述后验概率的各部分我们分别可以写成:

对于先验概率,我们同样可以将其表示成高斯分布的函数(但是是确定性的,通过将协方差矩阵设为无穷小),将其均值和信息矩阵设为:

其中,

在实际实现的时候,对于 $\infty$ 我们只需要用一个很大的正数代替即可。后验概率的负对数形式就可以表示为:

这个和最初在介绍 GraphSLAM 图构建的时候提出的 $J_{GraphSLAM}$ 是等效的,除了一些常数项的忽略。从形式来看,上面的等式将 SLAM 问题中要求的后验概率用三部分二次项表示了出来,分别是:先验、运动模型、观测。

泰勒展开

上面的后验概率公式是建立在函数 $g$ 和 $h$ 的二次型,而不是我们想要估计的量(位姿和地图)。GraphSLAM 将其进行线性化,得到:

其中, $\mut$ 是对状态向量 $y_t$ 的估计。通过这样的线性化,我们将对数似然概率转化为关于 $y{0:t}$ 的二次项,如下:

通过展开和重新排列可以得到:

上式中, $x{t-1:t} := [x_t \quad x{t - 1}]^T$,因此有: $[xt - G_tx{t-1}] = x_{t-1:t}^T[1 \quad -G_t]^T$

如果我们将上式中所有的二次项 (quadratic term) 收集进矩阵 $\Omega$,所有的一次项 (linear term) 收集进向量 $\xi$,则上式可以写成:

构建信息矩阵

从上式中,我们可以推导出的 GraphSLAM_linearize 中的实现是正确的。关于具体的算法参考我的上一篇笔记: The GraphSLAM algorithm with applications to large scale mapping of urban structures: Part 2,初始化分为:

  • 先验 :通过二次项 $x_0^T\Omega_0 x_0$,进行初始化:
  • 控制量:从上式中,我们可以发现控制量 $u_t$ 对整体信息矩阵和信息向量的更新如下:
  • 观测:同样,观测量 $z_t^i$ 对整体信息矩阵和信息向量的更新如下:

从上面的式子可以发现,对于非对角线元素,我们只更新了和位姿有关系的部分(位姿和位姿或者特征点和位姿),所以对于特征点和特征点(不是同一个)的值始终为 0。

信息矩阵简化

为了将信息矩阵简化,首先需要将后验概率分解成如下形式:

对于第一部分有:

实际中这个积分算不了,因为 $m$ 中包含的变量可能会非常多。所幸的是对于高斯变量,这个积分有解析解,主要利用了下表展示的对于高四遍了的边缘化引理:

我们可以将 $\Omega$ 和 $\xi$ 按机器人位姿 $x_{0:t}$ 和地图 $m$ 分成:

根据边缘化引理,有:

因为 $\Omega_{m,m}$ 是块对角矩阵,所以它的逆也很好算:

其中, $\Omega_{j,j} = F_j^T\Omega F_j$ 是地图中关联第 $j$ 个特征点的子矩阵,如下:

通过这样,简化后的信息矩阵和信息向量可以表示为:

上式中, $\Omega{x{0:t}, j}$ 只在 $\tau(j)$ 中包含的位姿的位置的值不为 0,$\tau(j)$是观测到特征点的位姿集合。以上就是关于 GraphSLAM_reduce 的推导,这个边缘化的过程也可以理解成变量消除的过程。

求解机器人路径和地图

最后是求解机器人路径和地图。关于机器人位姿均值和方差的部分比较简单,如下:

剩下是求 $p(m | z{1:t}, y{1:t}, c_{1:t})$,这里要用到另一个引理:

通过这个引理,求解地图特征点均值和方差的公式如下:

因为我们知道 $\Omega_{m,m}$ 是块对角矩阵,我们可以分解为:

其中,对每一个 $p(mj | z{1:t}, y{1:t}, c{1:t})$,

值得注意的是,这里算出来是特征点根据机器人位姿的条件概率。所以在算法中只返回了特征点的均值,没有方差(方差只有机器人位姿的部分)。

结语

目前关于 GraphSLAM 的大部分内容已经总结完了,但是还存在一些问题:目前我们假设观测到的特征点都能和地图中的特征点一一对应,但是实际中可能不会这样,那么我们怎么进行处理呢?下一篇将主要讲数据处理还有论文中的实验和最后的总结。