第十一章：GraphSLAM算法

11.1 介绍

在上一章中介绍的EKF SLAM算法有很多限制。其中就有其平方级的更新复杂度，以及EKF的线性化技术，该技术只在EKF中对每一个非线性项进行了一次线性化操作。本章中，我们介绍另一种SLAM算法，称之为GraphSLAM。相比于EKF算法，GraphSLAM算法用于解决完全SLAM问题full SLAM problem。该方法提供了一个离线问题的解决方案，计算所有的位姿和地图上所有的特征。正如我们将在本章中看到的那样，完全SLAM问题的后验概率本身就是一个稀疏图sparse graph。该图导致了各种非线性的二次约束。对这些约束的优化过程就会产生极大似然地图和对应的机器人位置集合。这一思想可以在大量的SLAM文献中看到，将之命名为GraphSLAM是因为它抓住这一思想的本质。

图11.1示例了GraphSLAM算法。图中描述的是GraphSLAM算法从5个标记为$x_0, \cdots, x_4$的位姿和两个地图特征$m_1, m_2$中提取出来的图。图中有两种类型的边，运动边和测量边。运动边连接的是连续的两个机器人位姿，测量边所连接的则是位姿以及对应位姿所能测量到的地图特征。图中的每一条边都对应着一个非线性约束，在后续的内容中我们会看到，这些约束都会表示为运动模型和测量模型的似然度的负对数形式，因此称为信息约束information constraints。把约束添加到图中对于GraphSLAM而言是微不足道的，因为它并没有引入多少计算量。如图11.1中所示，最小化所有约束的和就是一个最小二乘问题least squares problem。

图 11.1. 有4个位置和两个地图特征的GraphSLAM示例。图中的节点是机器人的位姿和特征定位。图中有两种类型的边，实线的边连接着机器人位置变换，虚线的边标志着机器人位姿与特征之间的练习，表示如果机器人在对应位置上就应该看到的特征。GraphSLAM中的每一条边都是一个非线性的二次约束。运动约束构成了运动模型，测量约束形成了测量模型。GraphSLAM的目标函数是这些约束的求和，最小化目标函数就可以得到最可能的地图和机器人运行路径。

为了计算地图的后验概率，GraphSLAM算法对这些约束做出了线性化操作。其线性化的结果就是一个信息矩阵和一个信息向量，其基本形式与第3章中提到的信息滤波器一致。信息矩阵继承了GraphSLAM构建的图的稀疏特性。这一特性使得GraphSLAM算法能够应用各种消元算法，将图转换为只定义在机器人位姿上的更小的图。路径后验地图就可以通过标准推理技术来求得。GraphSLAM也计算了一个地图和与该地图对应的边沿后验概率。全地图后验概率自然也是地图尺寸的二次方，因此一般不可恢复。

EKF SLAM和GraphSLAM是SLAM算法的两个极端。EKF SLAM和GraphSLAM之间的主要区别在于信息的表示形式。EKF SLAM通过一个协方差矩阵和均值向量来表示信息， GraphSLAM用软约束的图来表示信息。在EKF中更新协方差矩阵的计算代价很大，但是图的更新过程计算代价就很小了。

计算效率的提升是有代价的，在恢复地图和路径的时候GraphSLAM还需要额外的推理，但是EKF在任何时刻都维护者对地图和机器人位姿的最优估计。图的构建需要一个专门的步骤，在该步骤中将信息转换为状态的估计。这一操作在EKF SLAM中是不存在的。

有人将EKF SLAM称作是勤快的SLAM算法proactive SLAM，因为该算法会立即处理任何新获得的信息，用于提升对世界状态的估计。相比之下GraphSLAM则是一个懒散的SLAM算法lazy SLAM，它只是简单的将信息添加到图中却不去处理它。这一差别有着重要的意义，它使得GraphSLAM可以处理比EKF算法大几个数量级的地图。

EKF SLAM和GraphSLAM之间还有很多其它不同之处。作为一个完全SLAM问题的解决方案，GraphSLAM计算整个机器人路径的后验概率，因此并不是一个增量式的算法。 EKF SLAM则不同，它是一个滤波器，只维护当前时刻机器人位姿的估计。EKF SLAM使得机器人能够一直更新地图，但是GraphSLAM更适合从一个固定大小的数据集中萃取出地图信息。 EKF SLAM算法可以在机器人的整个生命周期中维护一幅地图，而不必关心从获取数据开始总共经过了多少个时间步数。

因为在构建地图的时候，GraphSLAM要用到所有的数据，它可以使用更好的线性化方法和数据关联技术。在EKF SLAM中，线性化操作和测量数据的关联度都是基于$t$时刻的数据来计算的。在GraphSLAM中，所有的数据都是用作线性化和数据关联度的。因此GraphSLAM算法可以修正过去的数据关联估计，并进行不止一次的线性化操作。实际上，在建图过程中GraphSLAM是在三个步骤之间循环迭代进行的：构造地图、计算关联度变量、以及对测量和运动模型线性化，如此获得那些量的最优估计。得益于此， GraphSLAM算法在准确性上要由于EKF SLAM算法。

但是，相比于EKF SLAM算法而言，GraphSLAM算法不是没有限制。其中一个我们已经讨论过，就是图的尺寸随着时间线性的增长，而EKF算法在用于估计的内存分配上基本不存在时间的依赖性。另一个限制与数据关联度有关，EKF SLAM的数据关联概率可以简单的通过后验概率的协方差矩阵计算得到，而GraphSLAM则需要额外的推理才可以得到。关于这一不同点，将在后续文章中加以解释，那时我们将为GraphSLAM定义一个显式的算法来计算关联度。到底哪个算法更好还真是一个问题，因为没有哪一个算法在所有的维度上都比其他算法更有优势。

本章中首先描述GraphSLAM算法背后的思想和基本的更新过程。然后从数学上推导各种更新过程并证明在特定的线性近似条件下的正确性。在讨论完一个实际的GraphSLAM算法的实现之后，我们将推导一个用于数据关联度的技术。

11.2 算法思想

GraphSLAM背后的思想非常简单，从数据中提取软约束，并将之用稀疏图的形式表示出来。它通过将这些约束转换到全局的关联度估计，从而获得地图和机器人的路径。这些约束通常都是非线性的，但在求解它们的时候都要对其线性化，将之转换为信息矩阵的形式。因此GraphSLAM本质上是基于信息论的技术。我们将从两个方面来讨论GraphSLAM：构建具有非线性约束的稀疏图、线性化约束的稀疏信息矩阵。

11.2.1 图的建立

假设我们有一组测量值$z_{1:t}$及其对应的关联度变量$c_{1:t}$，以及控制量集合$u_{1:t}$。GraphSLAM将这些数据转换为图，图中的节点就是机器人的位姿$x_{1:t}$，以及地图的特征$m = {m_j}$。图中的每一条边都对应着一个事件，运动事件生成一条连接着两个机器人位姿的边，测量事件则在位姿和地图中的特征之间构建了边。边表示GraphSLAM中位姿和特征之间的软约束。

对于一个线性系统，这些约束对应着大系统方程中的信息矩阵和信息向量的元素。按照惯例，令$\Omega$表示信息矩阵，$\xi$表示信息向量。后面我们会提到，每个测量值和控制量都会对$\Omega$和$\xi$进行一次局部更新local update，其表现形式就是在图中增加了一条边。实际上，将控制量和测量值整合进$\Omega$和$\xi$的规则就是局部地增加，在一定程度上将信息表示为一种增量additive quantity。

图11.2举例说明了构建图及其信息矩阵的过程。首先考虑一个测量值$z_t^i$，它提供了$t$时刻机器人位姿$x_t$和特征$j = c_t^i$之间的信息。在GraphSLAM中这一信息会被表述为$x_t$和$m_j$之间的一个约束。我们可以将这个边看作是弹簧-质块模型spring-mass model中的弹簧，约束形式为 $$ \begin{equation}\label{f1} \left(z_t^i - h(x_t, m_j) \right)^T Q_t^{-1} \left(z_t^i - h(x_t, m_j) \right) \end{equation} $$ 其中$h$是已经很熟悉的测量方程，$Q_t$则是测量噪声中的协方差。图11.2a中显示了GraphSLAM将这样的一条边添加进图的过程。

图11.2a. 观测到路标$m_1$

现在考虑机器人的运动。控制量$u_t$提供了$t-1$时刻和$t$时刻机器人的相对位置关系的信息。这一信息可以以如下的形式作为一个约束添加到图中: $$ \begin{equation}\label{f2} \left(x_t - g(u_t, x_{t-1}) \right)^T R_t^{-1} \left(x_t - g(u_t, x_{t-1}) \right) \end{equation} $$ 其中，$g$就是机器人运动学模型，$R_t$则是运动噪声的协方差。如图11.2b所示，添加一个这样的边到图中。图中还显示了在信息矩阵中新增的从位姿$x_{t-1}$到$x_t$之间的元素。这个更新过程也是增量的。这一数值反映了因为运动噪声$R_t$而残存的不确定度。噪声越小，添加到$\Omega$和$\xi$的数值就越大。(译者按:原文中提到的噪声是测量噪声，个人以为是一段笔误，这里翻译为运动的噪声)

图11.2b. 从$x_1$到$x_2$的机器人运动

综合了所有的测量值$z_{1:t}$和控制量$u_{1:t}$，我们就得到了软约束形式的稀疏图。图中约束的数量随着时间线性的增长，因此所得的图是稀疏的。图中所有约束的和为: $$ \begin{equation}\label{f3} J_{\mathrm{GraphSLAM}} = x_0^T \Omega_0 x_0 + \sum_t \left(x_t - g(u_t, x_{t-1}) \right)^T R_t^{-1} \left(x_t - g(u_t, x_{t-1}) \right) + \sum_t \sum_i \left(z_t^i - h(x_t, c_t^i) \right)^T Q_t^{-1} \left(z_t^i - h(x_t, c_t^i) \right) \end{equation} $$ 它是定义在位姿变量$x_{1:t}$以及地图$m$中所有的特征上的函数。注意到这一表达式还有一个固定形式的约束anchoring constrains$x_0^T \Omega_0 x_0$。这个约束将机器人的初始位姿锚定在地图上的绝对坐标$\begin{pmatrix} 0 & 0 & 0 \end{pmatrix}^T$上。

图11.2c. 几步之后的约束

在关联的信息矩阵中$\Omega$中，其非对角元素除了两种特殊的情况全为0。这两种特殊情况是，任何两个邻接的位姿状态$x_{t-1}$和$x_t$对应着一个非零的值，表示由控制量$u_t$所引入的信息边。如果机器人在位姿$x_t$上能够观测到特征$m_j$那么信息矩阵中对应着$x_t$-$m_j$的元素就是非零的。所有的特征对所对应的信息矩阵元素都为0。这反映了我们从来没有采集到特征之间的相对位置关系，在SLAM中我们采集到的全是测量值信息，约束了机器人和特征之间的位置关系。因此，信息矩阵就是稀疏的，除了线性数量的元素外其余各元素均为0。

11.2.2 推论

显然，无论是图的形式还是信息矩阵的形式都可以给我们提供地图和路径信息。在GraphSLAM中地图和路径是通过线性化的信息矩阵$μ = \Omega^{-1} \xi$获得的。这个操作要求我们求解一个由线性方程构成的系统。这带来了一个问题，我们该如何高效的恢复地图估计$μ$和协方差矩阵$\Sigma$。

这一复杂问题的答案依赖于世界的拓扑逻辑。如果每个特征都只能在一处看到，那么这个图就是线性约束表示的。那么$\Omega$就可以通过线性变换写成带对角阵(band-diagonal matrix)的形式，所有的非零元素都出现在对角元素的两侧。方程$μ = \Omega^{-1} \xi$就可以在线性的时间里解出。这一思想描述的是只遍历一次的无环世界，因此每个特征都可以看作是一个短的连续的时间段内的特征。

但是，更一般的情况是同一个特征可能被多次观测到，而且每次观测到的时间间隔很大。这可能是由于机器人在走廊里来来回回的运动，或者世界本身就是有环的。在任何一种情况下，都有特征$m_j$在两个完全不同的时间点所对应的位姿$x_{t_1}, x_{t_2}$上被观测到，其中$t_2 \gg t_1$。在我们的约束图中，这就产生了一个环形依赖， $x_{t_1}$和$x_{t_2}$通过控制序列$u_{t_1 + 1}, u_{t_1 + 2}, \cdots, u_{t_2}$连接起来，并分别在$x_{t_1}$和$m_j$以及$x_{t_2}$和$m_j$之间建立了观测边。这些边的存在，使得我们不能通过简单的线性变换得到带对角阵，因此求解地图过程更复杂。实际上，因为$\Omega$的逆与一个向量相乘，所以其结果可以通过诸如共轭梯度这样的优化算法得到，而不需要显示的计算一个矩阵的逆。因为实际的实际总是有着各种各样的环，所以这一过程就更有意义了。

GraphSLAM使用了一种称为因式分解的手段factorization trick，我们可以将之看作是通过信息矩阵传播的信息，实际上它是著名的求解矩阵逆的消元法的一种推广形式。假设我们想要从信息矩阵$\Omega$和信息状态$\xi$中移除一个特征$m_j$。在弹簧质块的模型中，这就相当于移除一个节点和连接在该节点上的所有边。后面我们会看到，这可以通过一个十分简单的操作来实现，我们可以通过在任何共有$m_j$的两个位姿节点之间添加一条边，来移除所有$m_j$和观测到$m_j$的位姿节点之间的边。

这一过程如图11.3所示，移除了两个地图特征$m_1$和$m_3$，对于这个地图而言移除$m_2$和$m_4$无足轻重。特征的移除修改了观测到特征的位姿对之间的边。如图11.3b所示，这一操作可能会在图中新增边，特征$m_3$的移除在位姿$x_2$和$x_4$之间新增了一条边。


(a). 移除$m_1$修改了连接$x_1$和$x_2$之间的边	(b). 移除$m_3$在$x_2$和$x_4$之间新增了一条边	(c). 移除所有特征之后的结果
图11.3. GraphSLAM算法中的图化简。移除特征相关的边，只留下了位姿状态之间的边。

形式化的，令$\tau(j)$为观测到$m_j$的位姿集合，即$x_t \in \tau(j) \Longleftrightarrow \exists i : c_t^i = j$。那么我们就知道特征$m_j$只与集合$\tau(j)$中的位姿有关联。在构建过程中，$m_j$不再与其它任何位姿或者特征节点有关联。现在我们可以将所有$m_j$与$\tau(j)$之间的边都设定为0，并在任何两个节点$x_t, x_{t'} \in \tau(j)$之间引入一条新边。类似的，还要更新信息向量中所有对应于位姿$\tau{j}$的值。这一操作的重要特点就是，它是局部的，只涉及到一小部分约束。当移除了所有到$m_j$的边之后，我们就可以安全的将$m_j$从信息矩阵和信息向量中移除。得到的信息矩阵就更小了，因为它缺少了元素$m_j$。但是，该信息矩阵的后验概率在数学形式上，与移除$m_j$之前的后验概率等价。这个等价关系是符合直觉的，我们用一系列直接与位姿节点相连的边替代了连接在$m_j$的边。这些边所构成的合力是一样的，只是$m_j$没有连接。

这一削减过程的优点就是，我们可以逐渐的将推理问题转换为一个更小的问题。通过解决$\Omega$和$\xi$中的每一个特征$m_j$，最终我们会得到一个只定义在机器人路径上的更小的信息矩阵$\tilde{\Omega}$和信息向量$\tilde{\xi}$。这一约简过程可以以地图尺寸的线性时间进行，实际上它就是一种信息形式的消元求矩阵逆的方法，在这种方法中我们还维护了一个信息状态。关于机器人路径的后验概率现在就可以写作$\tilde{\Sigma} = \tilde{\Omega}^{-1}$和$\tilde{μ} = \tilde{\Sigma}\xi$。不幸的是，我们的消元法并不能消除后验概率中的环，剩余的推理过程可能不止需要线性的时间。

最后一步，GraphSLAM算法恢复了特征的位置。概念上，就是对每一个$m_j$构建一个新的信息矩阵$\Omega_j$和信息向量$\xi_j$。它们都是定义在特征$m_j$和观测到该特征的位姿集合$\tau(j)$上的。它包含了$m_j$到$\tau(j)$之间的原始连接，以及取值为$\tilde{μ}$的没有不确定度的位姿$\tau(j)$。从这一信息形式中，使用通常的的矩阵求逆的方法，就可以很容易的计算出$m_j$的位置。显然，$\Omega_j$只包含了那些关联在$m_j$上的元素，因此矩阵的逆运算可以以集合$\tau(j)$中位姿数量的线性时间来完成。

显然图的形式是一种更自然地表示形式。完全SLAM问题可以通过局部地将信息添加到一个大信息图中来解决，图中的每一条边对应着一次测量值$z_t^i$和控制量$u_t$。为了将这些信息转换为地图和路径的估计，首先进行线性化，然后将位姿和特征之间的关系逐渐的转换为位姿对之间的关系。如此得到只约束了机器人位姿的结构，可以通过矩阵求逆解得。一旦恢复了位姿，特征的位置就可以根据初始的特征位姿信息逐个解出。

11.3 GraphSLAM算法

下面我们将更精细准确的说明GraphSLAM算法的各个计算步骤。完全GraphSLAM算法fully GraphSLAM algorithm将被描述为几个步骤。实现的主要难度在于简单的增加信息的算法，蕴含了将形式为$p(z_t^i | x_t, m)$和$p(x_t | u_t, x_{t-1})$的条件概率转换为信息矩阵中的一个边。信息矩阵元素都是线性的，因此这一步也涉及到了线性化$p(z_t^i | x_t, m)$和$p(x_t | u_t, x_{t-1})$。在EKF SLAM中，线性化是通过计算在平均位姿$μ_{0:t}$处的雅可比矩阵得到的。为了构建我们的初始信息矩阵$\Omega$和向量$\xi$，我们需要一个对于所有位姿$x_{0:t}$的初始估计$μ_{0:t}$。

有很多种方法可以找到适用于线性化的初始均值$μ$。比如说，我们可以运行一遍EKF SLAM算法并用它的估计值来进行线性化。本章中，我们将使用一种更简单的技术，只是将运动模型$p(x_t|u_t, x_{t-1})$简单地链接起来构成初始估计。该算法如算法11.1所示，称为GraphSLAM_initialize。该算法以控制量$u_{1:t}$为输入，输出是位姿估计序列$μ_{0:t}$。它将初始位姿定义为0，然后用速度运动模型依次估计后续位姿。因为我们只关注平均位姿向量$μ_{0:t}$， GraphSLAM_initialize只应用了运动模型中的确定性部分，而且也没有考虑任何测量值。

算法11.1. GraphSLAM算法中初始化平均位姿向量$μ_{1:t}$。

一旦有了初始的估计$μ_{0:t}$，GraphSLAM算法就可以构建完全SLAM信息矩阵$\Omega$和关联的信息向量$\xi$。它是通过对图中的边线性化得到的，算法11.2中描述了算法GraphSLAM_linearize。

该算法中用到了很多数学符号，它们的意义将在后面推导该算法的时候得到明确。GraphSLAM_linearize以控制量$u_{1:t}$、测量值$z_{1:t}$、关联度变量$c_{1:t}$、以及平均位姿估计$μ_{0:t}$做为输入，然后通过线性化操作逐渐的构建信息矩阵$\Omega$和信息向量$\xi$，不断地以子矩阵的形式添加每个测量值和控制量的信息。

特别的，在GraphSLAM_linearize中的第二行初始化信息元素。在第3行中，使用"无穷"信息元素将初始位姿$x_0$修正为$\begin{pmatrix} 0 & 0 & 0 \end{pmatrix}^T$。这一过程是必须的，否则的话最后得到的矩阵可能是奇异的。这也说明一个事实，只通过相对信息我们是不能得到绝对估计的。

GraphSLAM_linearize在其第4到第9行中整合了控制量。在其中的第5行和第6行中计算的位姿$\hat{x}$和雅可比矩阵$G_t$是对非线性控制函数$g$的线性化近似。如这些公式所示，线性化过程所用的正是位姿估计$μ_{0:t-1}$其中$μ_0 = \begin{pmatrix} 0 & 0 & 0 \end{pmatrix}^T$。这将导致第7行和第8行分别更新$\Omega$和$\xi$。这些元素将分别添加到$\Omega$和$\xi$对应的行和列中。这一添加操作正是要将新的约束包含进SLAM的后验概率中，这正是上一节中讨论的算法思想的实践。

测量值是在GraphSLAM_linearize的第10到21行中整合的。第11行中的矩阵$Q_t$正是我们所熟悉的测量噪声协方差。第13到17行，对测量函数进行了泰勒展开。这里使用的是6.6节中定义的基于特征的模型。我们需要特别关注第16行的实现，因为角度的表示可以任意的偏移$2\pi$。

这一计算过程在第18和19行中进行测量更新时终止。在第18行中添加到$\Omega$的矩阵尺寸是$6 × 6$。我们将它分解为一个描述位姿$x_t$的$3×3$的矩阵、一个描述特征$m_j$的$3×3$的矩阵、以及两个$3×3$的矩阵用于描述$x_t$和$m_j$之间关系的矩阵。将之对应地添加到$\Omega$的行和列中。

类似的，添加到信息向量$\xi$的向量的列维度是5。它被分为3维和2维的两个向量，并分别将它们添加到对应$x_t$和$m_j$的元素中。

GraphSLAM_initialize的结果就是信息向量$\xi$和矩阵$\Omega$。我们已经注意到$\Omega$是稀疏的，它只在主对角线、相邻位姿之间、位姿与地图特征之间对应的子矩阵处有非零值。该算法的运行时间是线性的，正比于获取数据的时间步数。

GraphSLAM算法的下一步就是消减信息矩阵和向量的维度。它是通过算法11.3中描述的GraphSLAM_reduce来实现的。该算法以定义在整个地图特征和位姿空间下的$\Omega$和$\xi$做为算法的输入，输出是完全定义在位姿空间上的削减后的矩阵$\tilde{\Omega}$和向量$\tilde{\varepsilon}$，需要注意该矩阵和向量与地图特征没有关系。

削减过程通过GraphSLAM_reduce的第4到9行，逐个移除特征$m_j$来实现。记录$\tilde{\Omega}, \tilde{\xi}$中每个元素的确切索引是很繁琐的，因此算法11.3中只提供了基本的思想。

第5行中计算了机器人观测到特征$j$的位姿集合$\tau(j)$。接着从当前的$\tilde{\Omega}$中提取出两个子矩阵:$\tilde{\Omega}_{j,j}$和$\tilde{\Omega}_{\tau(j), j}$。 $\tilde{\Omega}_{j,j}$是$m_j$和$m_j$之间的二次子矩阵，$\tilde{\Omega}_{\tau(j),j}$则是$m_j$与位姿变量$\tau(j)$之间的非对角元。

它也从信息状态向量$\tilde{\xi}$中提取出对应于第$j$个特征的元素，并标记为$\varepsilon_j$。接着在第6行和7行中将其对应信息从$\tilde{\Omega}$和$\tilde{\varepsilon}$减去。

在该操作之后，$m_j$所对应的行和列的元素都为0。接着就把这些行列移除，对应的削减了$\tilde{\Omega}$和$\tilde{\xi}$的维度。循环这一操作直到所有的特征都被移除了， $\tilde{\Omega}$和$\tilde{\xi}$中只保留了位姿变量。GraphSLAM_reduce的复杂度也是关于时间步数$t$的线性复杂度。

GraphSLAM算法的最后一步就是计算机器人路径上所有位姿的均值和方差，以及地图中所有特征的平均位置估计。这是通过算法11.4的GraphSLAM_solve来实现的。第2行和第3行中计算路径估计$μ_{0:t}$，通过对约简的信息矩阵$\tilde{\Omega}$求逆，并将其结果与信息向量相乘。紧接着，GraphSLAM_solve在第4到第7行中依次计算每个特征的地址。该算法返回的是机器人路径和所有地图特征的均值，以及机器人路径的协方差。我们注意到还有一种更高效的计算$μ_{0:t}$的方法，可以绕过对矩阵求逆。这将在本章的最后，介绍使用标准最优化技术的GraphSLAM算法时解释。


算法11.3. GraphSLAM算法约简信息矩阵。	算法11.4. 更新后验概率$μ$的算法。

GraphSLAM算法的解的质量依赖于GraphSLAM_initialize对初始均值估计的质量。这些估计的$x$-$y$要素，以一种线性的形式影响对应的模型，因此线性化并不依赖于这些值。对于$μ_{0:t}$中的方向变量则不是这样。这些变量的初始化误差会影响泰勒近似的准确度，最终会影响到算法的结果。

为了降低线性化过程中以内泰勒近似而带来的误差，常会在同一个数据集上运行GraphSLAM_linearize, GraphSLAM_reduce, GraphSLAM_solve多次。每次迭代都以上次迭代输出的均值向量$μ_{0:t}$作为输入，并输出一个新的，改进后的估计。只有当初始位姿估计具有很大偏差的时候，才需要对GraphSLAM进行迭代优化。通常迭代几次就可以得到比较好的结果了。

算法11.5. 用于已知关联度的完全SLAM问题的GraphSLAM算法。

算法11.5总结了最终的算法。它初始化了均值，然后就重复构造过程、约简过程、和求解过程。通常两三次迭代就能够收敛了，输出的均值$μ$就是对机器人路径和地图的最好的估计。

11.4 GraphSLAM算法的数学推导

暂略

11.5 GraphSLAM算法的数据关联度

GraphSLAM算法的数据关联度Data association in GraphSLAM与在EKF SLAM算法一样，使用关联度变量来实现。GraphSLAM搜索一个最好的关联度向量，而不是计算关联度的完整分布。因此，找到一个关联度向量就是一个搜索问题。但实践证明，对GraphSLAM中的关联度定义做一些微小的改变会比较方便。我们将关联度定义在地图中的特征对上，而不是特征的测量值。具体的，我们说$c(j,k) = 1$，如果$m_j$和$m_k$对应着世界中相同的物理特征，否则$c(j,k) = 0$。这种基于特征的关联度与之前章节中定义的关联度在逻辑上是等价的，但是它简化了基本算法的陈述。

搜索关联度空间的技术是贪心的，就像EKF那样。搜索最优关联度数值的每个阶段都是在提高一个合适的对数似然函数。但是，因为GraphSLAM在同一时间可以访问所有的数据，所以就有可能推出比EKF中所用的增量方法更有效的关联度技术。特别的：

在搜索的任何时间点，GraphSLAM都可以考虑任何特征的关联度。不需要顺序的处理观测到的特征。
关联度搜索可以组合地图的计算。假设两个观测到的特征都应着地图中统一物理特征，就可以影响到地图结果。通过组合这样的关联度假设到地图中，其他关联度假设将或多或少的受到影响。
GraphSLAM中的数据关联决策可以是undone的。数据关联的优势依赖于其他数据关联决策的值。在搜索的早期看起来不错的选择，在搜索的晚些时间可能就是个较差的结果。为了协调这样一种情况，GraphSLAM可以高效的取消之前的数据关联决策。

现在我们要描述一个特定的关联度搜索算法，来探索前两个特点。这个算法并没有考虑第三个特点，它仍然是贪心的，而且会顺序的搜索可能的关联度空间，直到获得一个不错的地图。但是，就像所有的贪心算法那样，我们的方法会陷入局部最大值，真实的关联度空间大小是地图中特征数量的指数级汉书。但是，我们将要满足爬山算法，并在下一章中探讨一种穷举算法。

11.5.1 未知关联度的GraphSLAM算法

我们的算法的关键部分就是关联度的似然值测试likelihood test for correspondence。特别的，GraphSLAM关联度是基于一个简单的测试:地图中两个不同的特征$m_j$和$m_k$，对应着世界中同一个物理特征的概率是多少？如果这个概率值超过了一个阈值，我们就接受这个假设并合并地图中的两个特征。

算法11.8中描述了关联度测试的算法，测试的输入是两个特征索引$j$和$k$，我们尝试计算这两个特征对应着物理世界中同一个特征的概率。为了计算这个概率，我们的算法使用了很多量：由$\Omega$和$\xi$描述的信息形式的SLAM后验概率，GraphSLAM_solve的输出结果，描述了期望向量$μ$和路径协方差$\Sigma_{0:t}$。

算法11.8. GraphSLAM关联度测试。它接收信息形式的SLAM后验概率，以及GraphSLAM_solve的输出作为输入。输出$m_j$与$m_k$关联的后验概率。

关联度测试以如下的形式进行:首先，它计算两个考察特征的边沿后验概率。这个后验概率在算法11.8中的第2和第3行中，被表示成信息矩阵$\Omega_{[j,k]}$和向量$\xi_{[j,k]}$。这一步的计算使用了很多信息形式$\Omega,\xi$的子元素，期望特征位置由$μ$表示，路径方差为$\Sigma_{0:t}$。接着，它计算了一个新的高斯随机变量的参数，描述了$m_j$和$m_k$之间的差异，标记为差异度变量$\Delta_{j,k} = m_j - m_k$。在算法第4行和第5行中计算了信息参数$\Omega_{\Delta_{j,k}}, \xi_{\Delta_{j,k}}$，第6行计算关联度差异的期望，第7行返回$m_j$和$m_k$的差异为0的概率。

关联度测试为我们提供了一个算法，让GraphSLAM搜索数据关联度。算法11.9描述了这样一种算法。它首先用唯一的数值初始化关联度变量，接着的四个步骤(第3到7行)与算法11.5中已知关联度的GraphSLAM算法一样。接着这个一般的SLAM算法就可以进行数据关联度搜索了。特别的对于地图中每两个不同特征构成的特征对，都在算法11.9的第9行中计算关联度概率。如果这个概率超过了一个阈值$\mathcal{X}$，就在第11行中将这两个特征的关联度向量设定为同一个值。

算法11.9. 求解未知关联度的完全SLAM问题的GraphSLAM算法。算法的内层循环可以通过选择特征对$m_j, m_k$的探针，以及在求解方程组之前累积多个关联度，来提高效率。

GraphSLAM算法在第12到14行中，迭代地依次构建(construction)、约简(reduction)、和求解(solution)SLAM后验概率。如此在新构建地图地时候，后继的关联度测试需要考虑到之前的关联度决策。当内存循环中找不到更多的特征时就终止地图构建。

显然GraphSLAM算法并不是很高效。具体的，它测试了所有特征对地关联度，而不是相邻特征的。此外，只要找到了单一关联度就需要重新构建一次地图，并不是批量的处理关联特征集合。当然对这些问题的修改也很直接，GraphSLAM算法的一个好的实现将比我们这里讨论的基本算法更细致。

11.5.2 关联度测试的数学推导

暂略

11.6 关于效率的考虑

GraphSLAM的实际实现依赖于很多额外的洞察以及提高效率的技术。到目前为止，我们所讨论的GraphSLAM算法最大的缺点可能就是，在算法的一开始，我们假设检测到的所有特征都是不同的。我们的算法需要一个一个的判定它们。只要特征的数量足够大，这种方法将慢地不能忍。此外，它将忽略了一个重要地限制，就是在任何时间点上，同一个特征只能观测到一次而不是两次。

现有的GraphSLAM算法的实现探索了很多可能。

在运行完全GraphSLAM解决方案之前，通常都会以一个较高的似然度立即确认特征的身份。比如说，通常会将一些短片段编译到局部子图local submaps中，比如局部占用栅格地图。 GraphSLAM推理就只在这些局部占用栅格地图中进行，两幅地图的匹配就是地图中相对位姿的概率学约束。这样一种层级技术可以显著降低SLAM算法的复杂度，同时保持GraphSLAM算法的一些关键特性。尤其是在大型数据集上执行的数据关联。

很多机器人都装备有传感器，在同一时间可以感测到大量的特征。比如说激光距离扫描器，在一次扫描过程中就可以看到好几打特征。对于任何这样的扫描，我们一般假设不同的测量值实际上对应着环境中的不同特征，因为不同的扫描对应着不同的方向角。这正是在第七章中提到的互斥原则mutual exclusion principle。因此就有$i \neq j \rightarrow c_t^i \neq c_t^j$。同一个扫描中，没有两个测量值是对应着世界中相同的特征的。

我们的成对的数据关联技术是不能够包含上述的约束的。特别的，它可能给同一个特征$z_s^k$赋予两个不同的测量值$z_t^i, z_t^j, s \neq t$。为了克服这一问题，通常会在同一时间关联整个测量向量$z_t, z_s$。这就涉及到计算$z_t, z_s$中所有特征的联合概率。这一计算就对我们的成对计算进行了扩展，并且在数学形式上是很直接的。

本章中所说的GraphSLAM算法并没有使用撤销数据关联undo a data association的能力。一旦做出了数据关联的决策，那么在未来的搜索过程中将不能撤销。数学上，在信息框架下撤销过去的数据关联决策是很直接的。我们可以在上述的算法中以任何形式修改任何两个测量值的关联度变量。但是判定是否应该撤销一个数据关联的过程是很困难的，因为没有测试来判定两个之前关联的特征是否应该区分开。一个简单的实现包括，撤销数据关联、重新构建地图、测试我们的判定是否还需要关联度。这样的方法可以以计算的形式引入，因为它并没有提供判定那些数据关联应该被测试的方法。检测不可能的关联度的机制不再本书讨论的范畴中，但是在实现这种算法的时候需要考虑到这点。

最后，GraphSLAM算法并没有考虑到负信息negative information。实际上，没有看到某个特征也是一种信息。但是，我们的简单公式并没有涉及到这一方面的计算。

实际上，我们是否可以利用负信息依赖于传感器模型，以及世界特征的模型。有时，我们可能必须计算阻塞的概率，例如某一路标的距离和方向角测量值，这是使用特定类型传感器的技巧。现代的实现确实考虑了负信息，但是通常是通过近似来替代对应的概率计算。我们将在下一章中给出这样的例子。

11.7 实验应用

下面我们来看一下GraphSLAM实现的结果。如图11.4是一个在废弃的矿区中运动的机器人。

图 11.4. 这是一个客户定制的1500磅的挖掘机器人，装备有板载计算机，激光距离传感器，瓦斯和陷落传感器，视频记录装备。该机器人用于对废旧矿区建图。

机器人收集到的地图如图11.5所示，这是一个占用栅格地图，使用成对扫描匹配技术来恢复机器人的位置。成对扫描匹配可以看作是一种GraphSLAM的实现，但是关联度只是建立在连续两个扫描之间。这种方法输出的结果有一个明显的缺陷如图11.5所示。


图 11.5. 通过成对扫描匹配获得的矿井地图。场景的半径接近250米。这副地图显然不一致，有些通道不止一次被显示在地图上。图片是由弗莱堡大学的Dirk Hähnel提供的。	图 11.6. 矿井地图的框架，局部地图的可视化处理。

为了应用GraphSLAM算法，我们的软件将地图拆分为若干小的局部子地图，每个子地图对应着5米的机器人运动。在这5米中，地图相对比较准确，漂移较小，因此这种扫描匹配方法基本上没有什么缺陷。每个子地图的坐标就变成了GraphSLAM中的位姿节点。邻接子地图通过它们之间的相对运动约束连接在一起。其输出结果如上图11.6所示。

接下来，我们进行递归数据关联搜索，使用两个重叠地图的关联度分析来实现关联度测试，使用一个高斯对匹配函数近似从而获得高斯匹配约束。图11.7列举了这一数据关联的过程: 每当GraphSLAM新建了一个信息形式的约束，就在图中用一个圆圈出来。图中列举了这一迭代搜索过程，只有当其它关联度得到了传播，才可以发现特定的特定的关联度，其它的关联度实在搜索的过程中得到发现的。最终的模型是稳定的，继续进行数据关联搜索也不会对结果产生什么改变。在图11.8中以2维地图的形式显示了一个栅格地图。这个地图还称不上完美，这是主要是因为局部地图匹配约束的粗糙实现方式，但它也比通过增量扫描匹配的结果要好了很多。


(a)	(b)	(c)	(d)	(e)	(f)
图 11.7. 数据关联搜索

读者应该注意到还有其它信息论的SLAM技术可以产生类似的结果。图11.9中是Bosse et al. (2004)在相同的数据集上生成的地图，他们使用的方法称为Atlas SLAM。该算法将地图拆分为子地图，并通过信息论相对链来维护这些子地图间的关系。更多的细节参见参考文献的综述。


图 11.8. 经过数据关联优化后的最终地图。图片由弗莱堡大学的Dirk Hähnel提供。	图 11.9. 通过Bosse et al. (2004).提出的Atlas SLAM算法生成的矿井地图。图片由MIT的Michael Bosse, Paul Newman, John Leonard, and Seth Teller提供。

11.8 其它优化技术

读者可能还记得GraphSLAM算法的中心目标函数$J_{\mathrm{GraphSLAM}}$是一个非线性二次方函数，如式($\ref{f3}$)所示。GraphSLAM通过一系列的线性化、消元、优化操作最小化该函数。 GraphSLAM_solve所提供的推理技术不是那么高效。如果我们所关心的纯粹是地图和路径，而不关心协方差，那么算法11.4中第2行的求逆运算就可以避免。如此的实现方式在计算效率上更高。

推理效率的关键是函数$J_{\mathrm{GraphSLAM}}$的形式。这个函数是一般的最小二乘的形式，所以有很多不同的算法可以使其最小化，比如说梯度下降法、Levenberg Marquardt、共轭梯度算法等。

图11.10a中显示了一个使用共轭梯度的算法最小化$J_{\mathrm{GraphSLAM}}$的结果。这些数据是一个接近$600×800$米的户外环境地图，是由图11.10b中所示的机器人采集的。


(a)	(b)
图11.10. (a) 斯坦福校园的一个三维地图。(b) 基于Segway RMP平台采集这些数据的移动机器人。这是DARPA MARS资助的一个项目。图片是由斯坦福大学的Michael Montemerlo提供的。

图11.11示例了只通过位姿数据的数据集合，对于一个完全排列的地图和路径，进行排列的过程。这个数据集包含有接近$10^8$个特征和$10^5$个位姿。对于如此大的数量级，EKF算法是不能处理的。因为算法11.4要对矩阵$\Omega$求逆，使用共轭梯度最小化$J_{\mathrm{GraphSLAM}}$只需要几秒钟。出于这个原因，这种算法的现代实现都在使用现代最优化技术，来替代这里所讨论的较慢的技术。感兴趣的读者可以查看参考文献中列举的其它最优化技术。


(a)	(b)
图11.11. 斯坦福校园地图的2维切片。使用共轭梯度排列前(a)后(b)对比。这样一个优化方法只需要几秒钟。图片是由斯坦福大学的Michael Montemerlo提供的。

11.9 总结

本章介绍针对完全SLAM问题的GraphSLAM算法。

GraphSLAM算法解决的是完全SLAM问题。它计算了机器人路径以及地图的后验概率，因此GraphSLAM是一族算法，不是EKF SLAM算法那样的在线算法。
GraphSLAM算法从数据集中构建了一个软约束的图。特别的，测量值被映射到位姿和感知到的特征之间的非线性约束。控制命令则映射到连续两个位姿之间的软约束。这个图天生就是稀疏的。边的数量是节点数量的线性函数，每个节点只是连接到有限多个其它节点上，无论图的大小。

GraphSLAM只是通过位姿和特征之间的边，以及位姿序列对间的边，把这些信息记录到图中。但是，这些信息表示并没有提供地图或者机器人路径的估计。
所有的这些约束的和构成了函数$J_{\mathrm{GraphSLAM}}$。对机器人路径和地图的最大似然估计可以通过最小化函数$J_{\mathrm{GraphSLAM}}$得到。
GraphSLAM将图映射到一个同构的信息矩阵和信息向量中进行推理，信息矩阵和向量定义在所有的位姿变量和整个地图上。GraphSLAM算法的关键内涵就是其信息结构式稀疏的。测量值提供了测量时刻地图特征与机器人位姿之间的相对关系的信息。在信息空间中，它们构成了这些变量之间的约束。类似的，运动提供了两个连续的位姿之间的信息。这一稀疏特性继承自稀疏图。
vanilla GraphSLAM算法通过一个有三个步骤构成的迭代过程恢复地图：通过泰勒展开构建线性信息表达，从地图中移除节点来约简信息构图，求解最优化问题以获得机器人位姿。这三个过程能够有效的解析信息，并产生对整个路径和地图的一致的后验概率。因为GraphSLAM是一个批处理算法，我们可以重复运行线性化步骤来提高结果。
其它的实现通过对函数$J_{\mathrm{GraphSLAM}}$的非线性最小二乘优化进行推理。但是这样的技术只能够得到后验概率的形式，不能给出其协方差。
GraphSLAM中的数据关联通过计算两个特征具有相同世界坐标的概率来实现。因为GraphSLAM是一个批处理算法，这一操作可以在任何时间，对任何两个特征对进行。这就是一个迭代的贪心搜索算法，遍历所有的数据关联变量，迭代的确定可能关联于地图中同一特征的特征对。
GraphSLAM的实际实现通常使用了一些技巧，来降低计算复杂度，避免错误的数据关联。特别的，实际的实现倾向于通过提取局部地图来降低数据复杂度，并且使用每个地图作为基本的元素。它们还希望在同一时间匹配多个特征，并且在数据关联搜索过程中利用负信息。
我们简单的提供了一些分治思想的GraphSLAM算法的结果，这些算法使用占用栅格地图表示距离扫描。除了这些近似之外，我们发现数据关联和推理技术在大尺度的建图问题上有较好的结果。
我们也提供了一些对于这些最小二乘问题的共轭梯度实现。我们注意到GraphSLAM中一般的目标函数可以通过最小二乘技术得到优化。特定的技术，比如共轭梯度，比GraphSLAM算法中基本的优化技术快很多。

正如介绍中提到的那样，EKF SLAM和GraphSLAM是SLAM算法中的两个极端。处于这两个极端之间的算法将在下面两章中介绍。更多技术的综述也可以在后续章节中的参考文献中看到。

11.10 参考文献

图推理技术在机器视觉和图像测量学中很常见，它们与structure from motion and bundle adjustment有关(Hartley and Zisserman 2000; B et al. 2000; Mikhail et al. 2001)。第一个提及相对的、类图约束的SLAM文献要追溯到Cheeseman and Smith (1986) and Durrant-Whyte (1988)。但是这些方法并没有进行任何全局松弛，或者说是优化。本章中提到的算法基本上是建立在Lu and Milios (1997)的影响深远的论文上的。他们是历史上第一个将SLAM先验知识表示为机器人位姿间的边，并使用全局优化算法来从这些约束中生成地图。他们最初的算法使用机器人位姿变量为参考，对距离扫描进行全局一致的排列，这点与标准EKF中将位姿看作是积分得来的思想不同。通过分析里程计和激光距离扫描，他们的方法生成了位姿之间的约束，可以看作是GraphSLAM中的边。但是他们并没有使用信息形式来描述他们的方法。Lu and Milios’s (1997)算法第一次被Gutmann and Nebel (1997)成功的应用， Gutmann and Nebel (1997)还报告了可能因为大量的矩阵求逆运算而导致的数据的不稳定。Golfarelli et al. (1998)第一个建立了SLAM问题与弹簧质块模型之间的关系， Duckett et al. (2000, 2002)则提供了第一个有效解决这一问题的技术。Frese and Hirzinger (2001)讨论了协方差和信息矩阵之间的关系，Araneda (2003)开发了更详细的图模型。

Lu和Milios开启了离线SLAM算法的研究，到目前它也具有和EKF算法平起平坐的重要地位。Gutmann and Konolige将他们的实现与一个马尔科夫定位过程结合来建立有环路的环境中的关联度。 Bosse et al. (2003, 2004)开发了Atlas，这是一个层级建图框架，基于解耦的随机建图范式，保留了子地图间的相对信息。它使用类似Duckett et al. (2000)的优化算法来排列多个子地图。 Folkesson and Christensen (2004a,b)开发了SLAM角度的优化算法，将梯度下降法应用到对数似然度形式的SLAM后验概率上。他们的图SLAMGraphical SLAM算法降低了环路闭合时的路径变量的数量。这个约简在数学形式上是一种近似，因为地图被轻微的删减了一些。梯度下降法显著的提高了该算法的速度。Konolige (2004) and Montemerlo and Thrun (2004)将共轭梯度引入到了SLAM领域中，该方法比梯度下降法更高效。他们不仅约简了大型环路的变量数量，而且报告说是可以在短短的几秒钟时间里排列$10^8$个特征到地图上。文中提到的Levenberg Marquardt技术，是Levenberg (1944) and Marquardt (1963)最小二乘优化的上下文中的方法。Frese et al. (2005)分析了信息形式的SLAM算法的效率，并用多栅格优化方法，开发了高效的优化技术，他报告说他的技术可以加速几个量级，是目前最高水平的优化技术。Dellaert (2005)开发了高效的分解技术，用于构建GraphSLAM约束图，特别关注于将约束图变换到更紧凑的形式，同时还保留稀疏性。

需要提及的是，用来维护局部要素间相对关联的思想，是之前章节中讨论的很多子地图技术的核心，尽管它很少被显式的提及。 Guivant and Nebot (2001);Williams (2001); Tardós et al. (2002); Bailey (2002)报告说用于记录子地图间相对关系的数据结构，很容易就映射成为信息论的概念了。尽管很多这样的算法都是滤波器，它们或多或少都共享着本章所讨论的信息形式。

就我们所知，这里所描述的GraphSLAM算法还没有以任何形式出版过，本书的早期草稿称这个算法为扩展信息表extended information form。但是GraphSLAM于上述那些文献密切相关，建立在Lu and Milios’s (1997)具有深远影响的算法之上。GraphSLAM这个名称与Folkesson and Christensen (2004a)的Graphical SLAM很像。在本章中我们之所以选择这个名字，是因为约束图是整个SLAM研究线路的基础。很多研究者已经开发了信息形式的滤波器，解决的是在线SLAM问题，而不是完全SLAM问题。在后续的章节中我们还会讨论这样的算法，显式的处理滤波器问题。

GraphSLAM所描述的SLAM问题与讨论了几十年的空间地图表示法有关系，我们已经在第9章的参考文献中讨论了空间地图表示法。信息表示法统共有两种不同的地图表示范式：拓扑逻辑和尺度。这一分类的背后是几十年的关于空间表示方法的讨论(Chown et al. 1995)。关于拓扑逻辑的讨论，已经在第9章的参考文献中罗列了出来。拓扑逻辑表示法的关键特征保留了一个事实，就是它们只描述了地图中不同元素之间的相对关系。因此，它们就没有寻找关联度尺度并嵌入相对信息的问题的困扰。目前先进的拓扑逻辑方法倾向于在链接中扩展尺度信息，比如说两个位置之间的距离。

与拓扑逻辑表示法类似，信息论方法收集邻接物体(路标和机器人)之间的相对信息。但是，相对地图信息被通过推理转换为尺度形式。在线性高斯的情况下，这一转换时无损的而且不可逆。计算完全的后验概率，包括协方差，需要矩阵求逆。第二个矩阵逆操作让我们回到了相对约束的初始形式。因此，拓扑逻辑形式和尺度形式存在一定的互补，就好像信息论和概率形式(EKF和GraphSLAM)，之间的关系。所以是否有某种一般的数学框架能够统一拓扑逻辑和尺度这两种形式？这一观点目前还没有被主流的研究社区所接受。

11.11 练习

暂略。


(a). 移除\(m_1\)修改了连接\(x_1\)和\(x_2\)之间的边	(b). 移除\(m_3\)在\(x_2\)和\(x_4\)之间新增了一条边	(c). 移除所有特征之后的结果
图11.3. GraphSLAM算法中的图化简。移除特征相关的边，只留下了位姿状态之间的边。


算法11.3. GraphSLAM算法约简信息矩阵。	算法11.4. 更新后验概率\(μ\)的算法。