>
返回

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

前言

这是我阅读 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 采取的方法是搜索一个最佳相关性向量(对该时刻的所有观测点),而不是计算所有相关性之间的分布。所以这变成了一个搜索问题。另外,在 GraphSLAM 中,相关性是定义在地图中的一对特征点。即:

  • 如果 $m_j$$m_k$ 是世界中的同一个物理点,则有 $c(j,k) = 1$
  • 否则,$c(j,k) = 0$

关于搜索相关性空间的方法是贪心算法,和 EKF 一致。GraphSLAM 用对数似然函数来评价匹配质量,用贪心算法每一步搜索都可以提高匹配质量。另外,和 EKF 不同的是, GraphSLAM 可以在同一时间内获取所有数据,可以可以用比 EKF 中更有效的方法,具体如下:

  • 在搜索过程中, GraphSLAM 可以考虑任何特征点的集合,而不用考虑他们出现的顺序。
  • 匹配搜索可以和求解地图同时进行。假设两个两个搜索到的特征点关联了同一个实体点,通过将这个匹配假设引入地图中,会对地图其他匹配假设有影响。
  • GraphSLAM 中的数据处理决定可以不做完。数据处理的优劣取决其他对其他数据的处理;对于某一个最初是较优的选择的匹配在后续可能会发现其实不好,而 GraphSLAM 可以随时修改(去除)这个匹配关系。

下面进行某一个特定的匹配搜索算法,这个算法会涉及到上面提到的前两点,但不会涉及第三点(去除过去的匹配关系)。下面提到的数据处理算法是贪心算法的一种,并且其会逐渐搜索可能的匹配关系使得地图质量提高。但是跟别的贪心算法类似,这个算法可能会限制在局部最优值。

对未知匹配关系的 GraphSLAM 算法

算法中关键一点是对匹配关系的似然测试。在 GraphSLAM 中,匹配关系是基于一个简单的测试:对于地图中两个不同的匹配点 $m_j$$m_k$,他们对应同一个实体点的可能性有多高? 如果可能性超过一个给定的阈值,我们就接受这个假设并且在地图中把这两个特征点合并

关于匹配性测试的算法如下图所示,输入是两个特征点索引 $j$$k$,输出是他们关联到同一个实体点的可能性。除此之外,算法还利用以下参数:

  • 信息矩阵和信息向量表示的 SLAM 的后验概率
  • Graph_solve 的求解结果 位姿和特征点的均值$\mu$ 和位姿的方差 $\Sigma_{0:t}$

如上图所示,算法的步骤如下:

  • 计算两个特征点之间的边际后验概率。用 $\Omega_{[j,k]}$$\xi_{[j,k]}$ 来表示。这一步的原理也是根据上一篇笔记中提到的边缘化引理,详见:The GraphSLAM algorithm with applications to large scale mapping of urban structures: Part 3
  • 接下来,算法计算新的高斯随机变量 $\Delta_{j,k} = m_j - m_k$的参数,即信息参数 $\Omega_{\Delta_{j,k}}, \xi_{\Delta_{j,k}}$
  • 最后计算差值变量的期望,最后一行返回 $m_j$$m_k$ 相差为 0 的概率。

通过这个测试算法,我们可以在 GraphSLAM 中同时进行数据的处理,如下图所示。

上图中,和之前的 GraphSLAM 算法不同的地方在于:

  • 首先对匹配关系进行了初始化(每个观测值赋予一个独特值)
  • 在重复迭代求解的过程,加入了一步对匹配关系的测试,对于特征点集合中任意两个不匹配(尚未匹配)的点进行匹配性测试如果匹配性高于一个阈值则设为匹配,重复进行求解,直到没有出现新的匹配关系为止

可以发现,其实这个算法不算特别高效,因为:

  • 它测试了所有的特征点的点,而没有考虑距离等因素;
  • 并且每当找到一个新的匹配关系就重新地图重建,而不是对多个新的匹配关系进行统一重建

匹配性测试的数学推导

同样,论文中进行了上一部分的匹配性测试的数学推导以验证其正确性。首先论文定义了一个后验随机变量 $\Delta_{j,k} = m_j - m_k$ 来衡量两个两特征点之间的位置差异。两个特征点只有位置相同的时候才会被考虑是相同的,因此通过计算 $\Delta_{j,k}$ 的后验概率,我们就可以知道我们想要得到的匹配概率。

首先,需要计算如下联合条件概率:

$$
p(m_j, m_k | z_{1:t}, u_{1:t}, c_{1:t}) = \int p(m_j, m_k | x_{1:t}, z_{1:t}, c_{1:t}) p(x_{1:t} | z_{1:t}, u_{1:t}, c_{1:t}) dx_{1:t}
$$

我们信息形式的边际后验概率设为 $\xi_{[j,k]}$$\Omega_{[j,k]}$。这里要注意以下,下标加方括号的原因是要和联合信息矩阵中的子矩阵区分开。

上式中的分布可以通过边缘化引理获得。假设 $\Omega$$\xi$ 是全状态向量 $y_{0:t}$ 的联合后验概率的信息形式, $\tau(j)$$\tau(k)$ 分别表示机器人观测到 $j$ 特征点和 $k$ 特征点的位姿集合。 GraphSLAM 给出了位姿向量的均值 $\tilde{\mu}$,为了应用边缘化引理,我们应该利用 GraphSLAM_solve 的结果,GraphSLAM_solve 已经计算出特征点 $m_j$$m_k$ 的均值,我们只需要为联合特征点重新选择一下结果:

$$
\mu_{[j,k]} = \Omega_{jk,jk}^{-1}(\xi_{jk} + \Omega_{jk,\tau(j,k)}\mu_{\tau(j,k)})
$$

这里 $\tau(j,k) = \tau(j) \cup\tau(k)$,表示机器人观测到 $j$$k$ 的位姿。

对于联合后验概率,我们还需要计算协方差矩阵。这一部分中并没有在 GraphSLAM_solve 中计算,因为对于多个特征点的联合概率需要特征点数量的平方空间开销。但是对于特征点对的协方差是比较容易计算的。假设 $\Sigma_{\tau(j,k),\tau(j,k)}$$\Sigma_{0:t}$ 的关于 $\tau(j,k)$ 中所有位姿的子矩阵。这里的 $\Sigma_{0:t}$ 已经通过 GraphSLAM_solve 计算出来,而通过边缘化引理,我们可以计算关于 $(m_j,m_k)^T$ 的边际后验概率信息矩阵和向量:

$$
\begin{aligned}
\Omega_{[j,k]} &= \Omega_{jk,jk} - \Omega_{jk,\tau(j,k)}\Sigma_{\tau(j,k),\tau(j,k)}\Omega_{\tau(j,k),jk} \\
\xi_{[j,k]} &= \Omega_{[j,k]}\mu_{[j,k]}
\end{aligned}
$$

所以对于联合概率分布有:

$$
p(m_j, m_k | z_{1:t}, u_{1:t}, c_{1:t}) = \eta \exp\Big\{-\frac{1}{2}\begin{bmatrix}
m_j \\ m_k
\end{bmatrix}^T\Omega_{[j,k]}\begin{bmatrix}
m_j \\ m_k
\end{bmatrix} + \begin{bmatrix}
m_j \\ m_k
\end{bmatrix}^T\xi_{[j,k]}\Big\}
$$

通过这个表示方法,我们还能直接定义想要的匹配概率,下面考虑如下随机变量:

$$
\begin{aligned}
\Delta_{j,k} &= m_j - m_k \\
&= \begin{bmatrix}
    1 \\ -1
\end{bmatrix} ^T
\begin{bmatrix}
    m_j \\ m_k
\end{bmatrix}\\
&= \begin{bmatrix}
    m_j \\ m_k
\end{bmatrix}^T
\begin{bmatrix}
    1 \\ -1
\end{bmatrix}
\end{aligned}
$$

将这个形式放入高斯分布中,有:

$$
\begin{aligned}
p(\Delta_{j,k}|z_{1:t}, u_{1:t}, c_{1:t}) &= \eta \exp\Bigg\{-\frac{1}{2}\Delta_{j,k}^T\underbrace{\begin{bmatrix}
1 \\ -1
\end{bmatrix}^T\Omega_{[j,k]}\begin{bmatrix}
1\\-1
\end{bmatrix}}_{=:\Omega_{\Delta_{j,k}}}\Delta_{j,k} + \Delta_{j,k}^T\underbrace{\begin{bmatrix}
1\\-1
\end{bmatrix}^T\xi_{[j,k]}}_{=:\xi_{\Delta_{j,k}}}\Bigg\} \\
&= \eta \exp\Big\{-\frac{1}{2}\Delta_{j,k}^T\Omega_{\Delta_{j,k}} + \Delta_{j,k}^T\xi_{\Delta_{j,k}}\Big\}
\end{aligned}
$$

为了计算这个概率,首先将高斯分布改写如下:

$$
p(\Delta_{j,k}|z_{1:t}, u_{1:t}, c_{1:t}) = |2\pi\Omega_{\Delta_{j,k}}^{-1}|^{-\frac{1}{2}}\exp\Big\{-\frac{1}{2}(\Delta_{j,k} - \mu_{\Delta_{j,k}})^T\Omega_{\Delta_{j,k}^{-1}}(\Delta_{j,k} - \mu_{\Delta_{j,k}})\Big\}
$$

其中,均值为:

$$
\mu_{\Delta_{j,k}} = \Omega_{\Delta_{j,k}}^{-1}\xi_{\Delta_{j,k}}
$$

$\Delta_{j,k} = 0$ 代入进去求得:

$$
p(\Delta_{j,k} = 0 | z_{1:t}, u_{1:t}, c_{1:t}) = |2\pi\Omega_{\Delta_{j,k}}^{-1}|^{-\frac{1}{2}}\exp\Big\{-\frac{1}{2}\mu_{\Delta_{j,k}}^T\Omega_{\Delta_{j,k}^{-1}}\mu_{\Delta_{j,k}}\Big\}
$$

上面的表达式表示了两个特征点之间匹配的可能性。

实验 & 总结

论文中提及了一系列实验,这里不一一总结了,有兴趣的话可以直接读论文。实验中的观测数据有用激光雷达,也有用 GPS 的。总的来说,这篇论文提出了图优化 SLAM 的基本思路,包括了特征点变量的消除以及特征点匹配的方法。

Built with Hugo
Theme Stack designed by Jimmy