前言
最近准备开始阅读 The GraphSLAM algorithm with applications to large-scale mapping of urban structures,同时记录一下阅读过程中遇到的问题和笔记。 这篇论文提出了用于解决 Offline SLAM 中的 GraphSLAM 算法,对于想要了解关于图优化 SLAM 的数学原理来说,是很好的阅读材料。作者是 Stanford 的大牛教授,也参与写作了 SLAM 入门必读神书 《Probabilistic Robotics》。本篇笔记主要先归纳一下前两部分:介绍以及离线 SLAM 的基本定义以及从 SLAM 中构建图。
摘要
这篇论文提出一种用来处理线下 SLAM 问题,也就是 GraphSLAM,来作为 SLAM 后端优化的一种方法。GraphSLAM 通过将 SLAM 中的后验概率转换为图像化的网格,再用消除变量的方法来简化这个图,从而降低后端优化问题的维度。所以 GraphSLAM 可以用来处理超过 10^8 个节点的图。这篇论文还提出了一种对于数据处理的贪心算法,以及在一个 GPS 测量的数据集进行了测试。
介绍
介绍里面作者首先列举了一系列以往的关于 SLAM 方面的工作,大部分都是通过滤波的方法进行实现,尤其是以 EKF (Extended Kalman Filter) 为主。 EKF 作为一种已经被提出数十年的算法,已经非常成熟也被后来的很多工作进行了一定的扩充。
但是不可否认,关于滤波的方法有一个关键的缺点是: 数据没有进行保留,对于每一个数据只处理一次然后就被丢弃了。 这使得如果我们在线下进行重建地图工作的时候几乎不可能利用上所有的数据点。后续有人发现了 SLAM 问题其实很自然可以形成一个稀疏图,并且通过稀疏图的特性可以优化所有约束条件最终得到机器人的一系列位姿信息。
而这篇论文提出的 GraphSLAM 也是用来进行建图,它通过从数据以及一系列软约束中提取出特征,并通过在全局一致估计的情况解除所有约束,获得所有时间机器人的位姿以及地图。所以除了把 GraphSLAM 作为一种建图的工具以外,还可以把它作为一种从数据中构建一系列非线性约束并生成(线性化的)稀疏信息矩阵的方法。同时,作为在 SLAM 的应用领域,除了可以处理大规模数据以外,它还能将 GPS 信息也一并处理。
从 SLAM 问题构建图
离线 SLAM 问题介绍
首先先对 SLAM 问题进行一个数学定义,在这篇论文里面有以下定义:
$x_{1:t}$
表示机器人从 t = 1 到当前 t 时刻的位姿集合$m$
作为环境(地图),有一系列特征点$m_j$
组成- 同时机器人在某一个时刻可以对环境进行感知,通过观察多个特征来获得多个观测值
$z_t^i$
,i 表示在该时间内机器人的第 i 个观测值。一般来说会假定$z_T^i$
是一个范围观测量。
另外用一个观测函数 $h$
描述观测值和位姿,环境之间的关系,如下所示:
$$
z_t^i = h(x_t, m_j, i) + \epsilon_t^i
$$
由于观测都会带有误差,所以用一个高斯随机变量$\epsilon_t^i$
来描述,其均值为 0,方差为 $Q_t$
,$m_j$
是该观测值观测的对特征对象。用概率的表示方法,观测值还能写成以下形式:
$$
p(z_t^i|x_t, m) = \text{const.} \exp(-\frac{1}{2}(z_t^i - h(x_t, m_j, i))^TQ_t^{-1}(z_t^i - h(x_t, m_j, i)))
$$
部分机器人采用 GPS 作为传感器,则观测对象变成了机器人本身的位姿,观测方程可以表示如下:
$$
z_t^i = h(x_t, i) + \epsilon_t^i
$$
GraphSLAM 可以适用于以上两种观测函数(将地图特征点作为观测对象和将机器人位姿作为观测对象)。
讲完了观测函数,我们需要关注机器人位姿本身变化的控制函数,也就是 SLAM 里面预测这一步。在 $t$
时刻,机器人的状态转变只和前一时刻的位姿 $x_{t-1}$
和 当前的输入控制量 $u_t$
,状态转变函数可以用以下函数 $g$
来表示:
$$
x_t = g(u_t, x_{t-1}) + \delta_t
$$
$\delta_t \sim \mathcal{N}(0, R_t)$
作为控制指令的噪声,这个函数也可以作为机器人的运动学模型。同样,用概率的方式,转变函数也可以写成以下形式:
$$
p(x_t|u_t, x_{t-1}) = \text{const.} \exp(-\frac{1}{2}(x_t - g(u_t, x_{t-1}))^TR_t^{-1}(x_t - g(u_t, x_{t-1}))
$$
同样,离线 SLAM 的问题可以定义为:给定 $1$
到 $t$
时刻的观测值 $z_{1:t}$
和 控制量 $u_{1:t}$
,需要求出$1$
到 $t$
时刻的机器人位姿 $x_{1:t}$
和地图 $m$
满足的分布,即求:
$$
p(x_{1:t}, m | z_{1:t}, u_{1:t})
$$
GraphSLAM 的基本思想
上图展示了 GraphSLAM 的一个例子。图中涉及到以下六个节点:
$x_0, x_1, ..., x_4$
四个时刻的机器人的位姿$m_1, m_2$
两个地图特征点
而对于边的话,这个图里包含了两种边:
- 两个相邻位姿之间的相对运动运动构成的边(实线)
- 某一位姿和特征点的观测误差构成的边(虚线)
图中的每一条边都代表了一个非线性约束,用运动和观测模型的似然函数来表示,所以看做是一个信息约束。所有这些约束加起来就构成了以非线性最小二乘问题。如果要计算出地图的后验概率的话,需要将这些非线性约束线性化。而线性化的结果是一个稀疏的信息矩阵和信息向量,利用稀疏性我们可以通过变量消除来将整个图转变成维度低很多,同时只包含机器人位姿节点的图;接下来只需要用标准的推断方法可以计算器机器人经过路径(位姿)的后验概率。
图的构建
假设我们现在有一系列观测值 $z_{1:t}$
和对应的相关变量 $c_{1:t}$
和控制量 $u_{1:t}$
。通过上面的描述,我们可以从中构建一个图。这个图中包含以下节点:
- 机器人位姿
$x_{1:t}$
- 地图中的特征点
$m_{j}$
图中的每一条边对应以下一种事件:
- 一次运动产生两个相邻机器人位姿节点之间的一条边
- 一次观测产生一个机器人位姿节点和特征点的一条边
在 GraphSLAM 中,每条边分别代表了位姿和特征点节点之间的软约束。如果是对于一个线性系统的话,所有这些约束可以用一个统一的信息矩阵 $\Omega$
和信息向量 $\xi$
来表示。并且我们可以发现,随着每一次观测和控制输入,$\Omega$
和 $\xi$
都会进行一次局部更新,这种更新是以相加的形式出现的,这一点要弄清楚。
如上图所示,展示了一个图以及信息矩阵的构建过程:
-
首先考虑一次测量值
$z_t^i$
,这次测量提供了在机器人$t$
时刻的位姿$x_t$
和地图中$j = c_t^i$
特征点之间的信息。在 GraphSLAM 中,这个信息用一个在$x_t$
和$m_j$
之间的约束来表示。我们可以把这条边想象成弹簧质量系统里面的一个“弹簧”。约束可以表示成以下形式,其中$h$
表示观测模型函数,$Q_t$
表示测量噪声的方差矩阵:$$ (z_t^i - h(x_t, m_j, i))^TQ_t^{-1}(z_t^i - h(x_t, m_j, i)) $$
上图 a 中就表示将这样一条边加入图的过程。不过注意一下这个这个约束可能会退化(即没有关联到机器人所有位姿的变量),暂时还不用关注这个情况。
上图 b 中表示了添加一条相对运动形成的边进入图的过程,分别是加在相邻两个位姿 $x_{t-1}$
和 $x_{t}$
的对应行和列之间,边中值的大小取决于运动模型中的噪声方差矩阵 $Q_t$
,表示运动过程中的不确定性(漂移程度),不确定性越大,对应的值的模会越小(因为取协方差矩阵的逆)。这条由前后两个时刻运动形成边可以表示成以下形式,其中 $g$
表示运动学模型函数:
$$
(x_t - g(u_t, x_{t-1}))^TR_t^{-1}(x_t - g(u_t, x_{t-1}))
$$
通过将所有测量值和控制量通过上述方式加入图中,我们就会获得一个包含很多软约束的图。约束的数量和时间成线性关系,所以这是一个稀疏图。将所有约束相加成以下形式:
$$
\begin{aligned}
J_{GraphSLAM} = x_0^T\Omega_0x_0 &+ \sum_t(x_t-g(u_t. x_{t-1}))^TR_t^{-1}(x_t-g(u_t. x_{t-1}))\\
&+\sum_t\sum_i(z_t^i-h(y_t,c_t^i,i))^TQ_t^{-1}(z_t^i-h(y_t,c_t^i,i))
\end{aligned}
$$
这个函数定义在所有位姿变量 $x_{1:t}$
和 特征点位置 $m_j$
上,同时还添加了一个额外约束 $x_0^T\Omega_0x_0$
相当于将机器人的起始点位置固定在 $(0, 0, 0)$
上,并用 $\Omega_0$
表示起始点的协方差矩阵。
对于信息矩阵 $\Omega$
来说,通常来说非对角线都为 0, 除了以下情况:
- 对于两个相邻的位姿
$x_{t-1}$
和${x_t}$
之间会有表示控制量的信息边引入 - 对于一个位姿
$x_t$
和在该位姿观测到的特征点$m_j$
也会有一条边引入
值得注意的是,两个特征点之间的对应的元素永远为 0,表示我们从头到尾都不会获得两个特征点之间相对位置的信息;对于 SLAM 中,我们的观测都是针对一个特征点和位姿的相对信息。
结语
本篇只是简单总结了一下介绍部分和离线 SLAM 问题的定义以及怎么从一个 SLAM 问题中构建出一个图,下一篇笔记会总结论文中如何对图求解以及具体的算法。