0%

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

前言

这是我阅读 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。在上一篇中已经总结了 SLAM 的问题定义以及怎么从 SLAM 中构建一个图,这一篇主要看一下如何根据这个图对问题进行求解。

位姿和地图特征点求解

目前为止,我们只有一个以位姿和特征点为节点的图,并不能求解出这些值。在 GraphSLAM 中,这些做法是通过线性化约束之后构建出一个信息矩阵 $\Omega$ 和信息向量 $\xi$,并通过以下 $\Sigma = \Omega^{-1}$ 和 $\mu = \Sigma \xi$ 来进行求解。这个过程是一个求解线性方程组的过程,注意到这里 $\Sigma$ 的维度可能会非常高(位姿和特征点的数量可能会非常多),所以我们需要思考怎么高效地对方程进行求解。

要回答这个问题,我们需要思考一下机器人实际运动的场景。假设每个特征点都只在一个时间被看到过一次的话,即对每一个特征点 $m_j$ 都只和一个位姿 $x_t$ 有关联,那么这样构造出来的图会线性的(对每一个特征点节点,都只会有另一个位姿节点和自身对应的位置有值)。这样如果通过重新排列的话,我们可以将其排列成带对角矩阵(band-diagonal),即所以非零值都在对角线附近,这样我们解 $\mu = \Omega^{-1}\xi$ 的时间可以降低到线性时间。

相对于以上的例子,实际情况大部分是一个特征点会在(时间上可能会相隔比较久)多个位置(对应机器人的不同位姿)被观测到。这种情况的发生非常普遍,因为机器人大部分会进行往复的运动,或者延一定路线来回走。在这种情况下,即存在特征点 $mj$ 同时被位姿 $x{t1}$ 和 $x{t2}$ 观测到,且 $t_2 \gg t_1$。这样相当于在图中构成了一个环,对于 $x{t1}$ 和 $x{t_2}$ 来说有两个连接:

  • 通过控制量 $u{t_1+1}, u{t1+2}, …, u{t2-1}, u{t_2}$ 连接
  • 通过 $m_j$ 连接

这种情况下,我们不能通过重新排列节点将矩阵转为带对角矩阵。事实上,在解 $\Omega^{-1}\xi$ 的过程中,可以使用别的技巧例如共轭梯度的方法,不用显式计算出矩阵的逆。

而 GraphSLAM 的算法采用了一种降维的方法,通过利用信息矩阵里的信息从而实现变量的消除。假设我们想要从信息矩阵 $\Omega$ 和信息向量 $\xi$ 中去掉,我们可以通过去掉 $mj$ 和 $x{t1}$ 的连接以及 $m_j$ 和 $x{t2}$ 的连接,并在 $x{t1}$ 和 $x{t_2}$ 添加新的连接(和通过 $m_j$ 形成的连接等效),如下图所示。

上图中,首先通过去掉 $m1$,在 $x{1}$ 和 $x{2}$ 原有的连接的基础上添加新的约束;然后去掉 $x_3$,在 $x{2}$ 和 $x_{3}$ 原有的连接的基础上添加新的约束,同时增加一个 $x_2$ 和 $x_4$ 之间的约束。而对于 $m_2$ 和 $m_4$ 由于他们本来就值关联一个位姿,所以可以直接去掉。(在这种情况下,我们只关注机器人位姿,而特征点在机器人位姿计算出来之后才求解。)

下面进行更广泛以及精确的推论,令 $\tau(j)$ 作为 $m_j$ 被观测到位姿的集合。即:

  • $x_t\in \tau(j) \Leftrightarrow \exist i: c_t^i = j$

$ct$ 是位姿观测到的所有特征点序号。根据这个定义,对于特征点 $m_j$ 而言,我们知道它只会跟 $\tau(j)$ 中的位姿 $x_t$ 产生连接,而不会跟其他位姿产生连接。那么我们可以通过对 $\tau(j)$ 中任意两个位姿 $x_t, x{t’}$ 添加连接,从而将 $m_j$ 和 $\tau{j}$ 中所有连接去除(在信息矩阵和信息向量相应位置的值设为 0)。值得注意的是,这个操作只是对信息矩阵进行局部的修改(时间花费不高)。在去除掉所有特征点之后,信息矩阵 $\Omega$ 和信息向量 $\xi$ 的维度会大大降低(因为缺少了所有特征点),而对于位姿节点来说,解(或者说约束)仍然是等效的。

这样做的好处是,我们将原来高维度的 SLAM 问题转变为一个更小的问题:将一个很大的矩阵 $\Omega$ 和 $\xi$ 降维变成 $\tilde{\Omega}$ 和 $\tilde{\xi}$,新的信息矩阵和信息向量只定义在机器人位姿上。(实际中,特征点数量可能会远远高于机器人位姿数量)。因此,这个时候我们可以通过 $\tilde{\Sigma} = \tilde{\Omega}^{-1}$ 和 $\tilde{\mu} = \tilde{\Sigma}\tilde{\xi}$ 求解出机器人位姿的后验概率。注意这里 $\tilde{\mu} = \mu$ (因为等效约束)。但是这种方法并没有去除环,所以最终求解时间还是会高于线性时间。

接下来, GraphSLAM 进行特征点位置的求解。方法是对于每一个特征点建立一个新的信息矩阵 $\Omega_j$ 和信息向量 $\xi_j$。信息量定义在 $m_j$ 和 $\tau{j}$ 中的所有位姿,而这个时候位姿则设为上一步中求解出的 $\tilde{\mu}$。所以我们只需要求解出一个解即可,可以直接对矩阵求逆进行求解。显然 $\Omega_j$ 只包含跟 $m_j$ 有关联的值,所以求逆只需要花费线性时间。

通过上面的方法,可以看出用图来表示问题 SLAM 是很自然的,求解 SLAM 思路概括如下:

  • 构建图:向一个大的信息矩阵根据每一次观测和每一次控制输入通过线性化约束加入局部信息(小矩阵)
  • 通过去除特征点并在特征点之间引入新的约束降低矩阵维度
  • 求解机器人位姿
  • 通过解出来的机器人位姿反求特征点位置

GraphSLAM 算法

前面的介绍大部分都只是内容上的概括,不涉及具体操作。本节将分步骤进行 GraphSLAM 算法的介绍。显然,算法主要的难度在于怎么实现将位姿和特征点的连接(即 $p(zt^i|x_t, m)$ 和 $p(x_t| u_t, x{t-1})$)转化为信息矩阵的新信息。由于信息矩阵是线性的,所以这个操作还涉及到约束的线性化。

解的初始化

要线性化约束首先需要有一个初始解。设定初始解的方法有很多种,例如可以通过一次 EKF-SLAM 的解来作为初始解。GraphSLAM 的初始化方法则更简单:

  • $t = 0$, 直接初始化为0,即: $\mu = (0, 0, 0)^T$
  • $t > 0$ 的情况则利用运动模型函数进行链式计算

伪代码如下:

构建信息矩阵 $\Omega$ 和信息向量 $\xi$

要构建信息矩阵和信息向量,首先需要将非线性约束线性化。线性化部分的算法伪代码 GraphSLAM_initilize如下所示:

代码中涉及很多关于数学的部分会在后面数学推导的章节进行阐述,这里主要讲一下基本思路。GraphSLAM_linearize 主要接受四个参数:

  • 控制输入:$u_{1:t}$
  • 观测值:$z_{1:t}$
  • 观测值对应的相关系数:$c_{1:t}$
  • 位姿初值:$\mu_{0:t}$

首先,GraphSLAM_initialize 首先利用 $u{1:t}$ 根据上一章节提到的方法利用控制输入量 $u{1:t}$ 对 $\mu_{0:t}$ 进行初始化,然后分别进行运动约束和观测约束的线性化即, $\Omega$ 和 $\xi$ 的构建。

从第 4 行到第 9 行是对控制量的处理,主要是利用非线性运动模型 $g(x{t-1}, u_t)$ 进行线性近似,$R_t$ 是运动模型的协方差矩阵,主要利用了 $\mu{0:t}$ 来进行泰勒展开;这些步骤主要添加了 $x{t - 1}$ 和 $x{t}$ 的约束进入信息矩阵和信息向量中。

第 10 行到第 21 行是针对观测量的处理。同样涉及到对非线性观测模型 $h(x_t, m_j)$ 进行泰勒展开线性化,$Q_t$ 是观测协方差矩阵。这里注意一下算法假定我们在某一个位置观测到的特征点跟全局地图特征点的对应关系是已知的,体现在第 13 行,另外涉及到角度的运算一定要正则化到 $[-\pi, \pi]$ 的合理范围内。第 18 , 19 行分别是对 $\Omega$ 和 $\xi$ 的更新。单一位姿和单一特征点的约束矩阵应该是 $5\times 5$,这里对矩阵分成四部分:左上角$3 \times 3$ 作为位姿自身的相关加在 $x_t$ 和自身的对应位置上(矩阵的对角部分),右下角 $2\times 2$ 作为特征点自身的信息矩阵加在 $m_j$ 和自身的对应位置上,另外两个 $3\times 2$ 和 $2\times 3$ 矩阵加在 $x_t$ 和 $m_j$ 的对应位置上,对信息向量的更新也是类似操作。由于矩阵是稀疏的,这一步的操作对时间(位姿的数量)是线性的。

简化图

下一步则是进行对图的简化和降维。算法伪代码 GraphSLAM_reduce 如下所示:

这部分函数利用 $\Omega$ 和 $\xi$ (根据所有位姿和特征点定义)作为输入参数,输出简化后的 $\tilde{\Omega}$ 和 $\tilde{\xi}$ (只包含位姿的信息)。从第 4 行到第 9 行大致包含了对每一个特征点的去除,注意这里只是大致上描述了一下,没有包括准确的实现,步骤大致如下,针对每一个特征点 $j$:

  • (第 5 行)找出观测到该相关点的所有位姿集合 $\tau(j)$
  • 从当前的信息向量 $\xi$ 中在 $x{\tau(j)}$ 和 $m_j$ 的位置减去 $\tilde{\Omega}{\tau(j),j}\tilde{\Omega}_{j,j}^{-1}\xi_j$
  • 从当前的信息矩阵 $\Omega$ 在 $x{\tau(j)}$ 和 $m_j$ 的位置减去 $\tilde{\Omega}{\tau(j),j}\tilde{\Omega}{j,j}^{-1}\tilde{\Omega}{\tau(j),j}$
  • 去掉 $\tilde{\Omega}$ 和 $\tilde{\xi}$ 中所有和 $j$ 有关联的行和列

通过对所有特征点重复上述步骤可以将 $\Omega$ 和 $\xi$ 进行降维,另外这一步的操作也是线性的。

求解机器人位姿和地图特征点

简化后接下来就可以进行求解了,主要是关于机器人位姿和后验概率(均值和方差)以及地图中所有特征点的均值(不包括方差)。这一部分算法 GraphSLAM 的伪代码如下所示:

第 2 , 3 行通过简化后的信息矩阵直接求解获得机器人位姿的均值和方差。这一部分可以直接求逆,也可以利用共轭梯度等技巧优化求解。第 4 到 第 7 行主要是对地图特征点均值的求解。

GraphSLAM 算法中解的精度很大程度上取决于均值的选取。均值的选取好坏会导致泰勒展开的精度,从而影响算法精度。为了尽可能减少泰勒展开线性化的错误,可以进行迭代更新初值,即重复进行 GraphSLAM_linearizeGraphSLAM_reduceGraphSLAM_solve。每次迭代利用上一次求得的解作为初值。论文中提到只有初值错误很高(超过 20 度)的时候才有必要迭代计算,并且一般来说 3 次左右迭代次数就足够了。

算法总结

下图总结了 GraphSLAM 的基本思路:

  1. 设初值
  2. 线性化
  3. 简化图
  4. 求解
  5. 重复 2 ~ 4 步直至收敛
  6. 返回机器人位姿均值方差,和地图特征点的均值

结语

本篇介绍了 GraphSLAM 的基本思路和算法实现,但是没有对其进行数学推导,下一篇将介绍关于算法中的数学部分的推导。