首页 关于
树枝想去撕裂天空 / 却只戳了几个微小的窟窿 / 它透出天外的光亮 / 人们把它叫做月亮和星星
目录

第十二章：稀疏的扩展信息滤波器

12.1 介绍

在前两章中，我们讨论了SLAM算法的两个极端。我们已经注意到EKF SLAM算法时勤快的proactive。每当有新信息获得的时候， 它都会将新信息融合到一个概率分布中，这个过程的计算复杂度很高。GraphSLAM算法则不同，它只是在堆积信息。我们说这样的堆积信息的方法时懒散的lazy。 在获得数据的时候，GraphSLAM只是将它们记录下来。为了将收集到的信息转化成地图，GraphSLAM需要一些推理。这些推理是在所有数据都获得之后进行的。 所以GraphSLAM是一种离线的算法。

那么我们是否可以设计一种在线的滤波器算法，同时具有信息表达形式的效率。答案是是！但是需要很多近似。稀疏的扩展信息滤波器sparse extended information filter, SEIF实现了一种解决在线SLAM问题的信息方案。与EKF类似，SEIF积分机器人的历史位姿，只维护当前机器人位姿和地图的后验概率。 类似GraphSLAM算法，SEIF维护着所有知识的信息表达形式。如此一来，SEIF的更新过程就是一个懒散的信息迁移操作，它比EKF的勤快的概率更新好。 因此，SEIF可以看作是综合了两个方面的优点，它可以在线运行，而且效率很高。

做为一种在线技术，SEIF所维护的置信度建立在与EKF一样的状态向量上: $$ \begin{equation}\label{f1} y_t = \begin{pmatrix} x_t \\ m \end{pmatrix} \end{equation} $$ 其中\(x_t\)是机器人的状态，\(m\)是地图。那么已知关联度的后验概率就是\(p(y_t | z_{1:t}, u_{1:t}, c_{1:t})\)。

图12.1中显示了将GraphSLAM转换为在线的SLAM算法的关键思想。图中显示了在一个具有50个路标的仿真环境中，EKF SLAM算法的结果。 左图中描述了机器人的运动轨迹和对所有50个路标的概率学估计。EKF SLAM算法所维护的关键信息就是这些不同估计的协方差矩阵。 关联度，也就是归一化之后的协方差矩阵，如中间的图所示。两个轴列举了机器人的位姿(位置和方向角)，以及所有50个路标的2维位置。 深色的元素表示强关联。我们已经在介绍EKF SLAM时讨论过，在极端情况下，所有的特征坐标都将关联起来，表现出来就是关联度矩阵看起来跟棋盘一样。

图 12.1. 使用信息滤波器进行在线SLAM。左图:模拟机器人在50个路标之间运动。 中图:EKF的关联度矩阵，说明任何两个路标之间都有极强的关联性。右图:归一化的EKF信息矩阵，本身就是稀疏。 这个稀疏特性使得SLAM算法可以更高效的更新

如图12.1的右图所示，是一个信息矩阵\(\Omega_t\)，就像关联度矩阵那样进行了归一化操作。如上一章所述，归一化后的信息矩阵中的元素， 可以看作是约束，或者边，约束了地图中特征对的相对位置关系，对应单元的颜色越深，约束越强。这一描述说明，归一化后的信息矩阵就是稀疏的sparse。 只有一小部分的强链接起着主导作用，还有大量的边在归一化之后，它们的值就接近为0了。

每条边的强度都与对应特征间的距离有关系，只有当两个特征距离很近才会有很强的边出现。两个特征间的距离越大，边的强度就越弱。

这个稀疏特性完全不同于上一章。首先，在各个路标之间都有连边。在上一章中，这些连边是不存在的。其二，这种稀疏特性只是一种近似， 实际上归一化后的信息矩阵中的所有元素都是非零的，它们只是接近为0而已。

SEIF SLAM算法使用这一思想来维护一个稀疏的信息矩阵，只有临近的特征才被非零的元素连接。图12.2的右图中列举了如此得到的网络结构， 图中圆圈对应着点特征，虚线表示边，它们是对左图中信息矩阵的图形化表示。这份图标也显示了只与少数几个特征有关系的机器人。 这些特征被称为激活特征active features，在图中标记为黑色。存储一份稀疏的信息矩阵要求空间相对于地图中特征的数量是线性的。 更重要的是，SELF SLAM的所有基本更新都可以在常数时间内完成，无论地图中特征的数量。这个结果多少还是让人有些吃惊的， 因为在算法3.6中提到，信息滤波器中运动更新的朴素实现， 需要对整个信息矩阵求逆。

图 12.2. SEIF SLAM生成的特征网络示意图。左图中是一个稀疏的信息矩阵， 右图则是根据矩阵中非零元素建立连边的地图。正如正文中所述，SLAM算法的关键结构元素是，并不是所有的特征之间都有联系， 这也是我们常数时间方案的核心。

SEIF是一种在线的SLAM算法，维护了这样的一个稀疏的信息矩阵，而且在已知关联度的情况下，所有的更新过程所需要的时间都与地图的尺寸无关。 数据关联的搜索复杂度则是对数级的，这使得SEIF成为了本书中考虑的第一个高效的在线SLAM算法。

12.2 算法思想

我们以图例的形式，从SEIF更新的思想开始讲起。特别的，SEIF更新由4个步骤构成：一个运动更新过程、一个测量更新过程、一个稀疏化过程、 以及一个状态估计过程。

我们以图12.3所示的测量更新过程开始。图中两个子图分别显示了SEIF维护的信息矩阵，以及由信息连接定义的图。正如GraphSLAM那样， 感知到一个特征\(m_1\)将初始SEIF更新信息矩阵中的非对角元素，它连接了机器人的位姿估计\(x_t\)和观测到的特征\(m_1\)，如图12.3a的左图所示。

(a) (b)
图 12.3. 测量对于信息矩阵和特征关联网络的影响。 (a) 观测到\(m_1\)修改信息矩阵元素\(\Omega_{x_t, m_1}\)。(b) 类似的观测到\(m_2\)修改\(\Omega_{x_t, m_2}\)。

如图12.3b所示，感知到\(m_2\)将更新信息矩阵中连接机器人位姿\(x_t\)和特征\(m_2\)的元素。正如我们所看到的那样， 每次更新都对应着信息矩阵和信息向量的一次局部增加，这些增加操作只会触及信息矩阵和信息向量中连接机器人位姿状态和观测到的特征的元素。 和GraphSLAM算法一样，吸收测量值到SEIF中所需要的时间与地图的大小无关。

该算法的运动更新操作与GraphSLAM不同，作为一个滤波器，SEIF抛弃了过去的位姿估计。如图12.4所示，机器人的位姿发生了变化， 图a和图b分别描述了，运动之前和之后的信息状态。这个运动以很多形式影响信息状态。首先，削弱了机器人位姿与特征\(m_1,m_2\)之间的边。 这是因为机器人运动引入了新的不确定度，以至于我们丢失了机器人相对于地图的位置信息，但是这些信息并没有完全丢失。其次，在特征之间添加了新的信息。 这些信息的迁移说明，即使我们丢失了机器人位姿的信息，但是收获了地图中特征之间的相对位置信息。在运动之前，这些特征通过机器人的位姿间接的联系着， 运动之后就在这些特征之间建立了直接的联系。

(a) (b)
图 12.4. 运动对于信息矩阵和特征关联网络的影响。 (a)运动之前，(b)运动之后。如果运动是确定的，运动更新就会在任何两个激活特征之间，引入新边或者强化已经存在的边， 同时弱化机器人和这些特征之间的边。这一步引入了特征之间的边。

信息从机器人位姿的边迁移到特征之间的边，这是SEIF的关键要素。这是滤波器使用信息表解决在线SLAM问题的直接结果。通过积分出过去的位姿变量，我们就将位姿间的边丢弃了， 并将其中的信息映射到了信息矩阵中特征之间。这与上一节中讨论的GraphSLAM算法不同，它从来不会在地图中两个特征节点之间添加边。

在这个过程中建立起直接连接的一对特征，在更新之前它们必须是激活的，因此信息矩阵中联系它们与机器人位姿的关联单元一定非零。这在图12.4中得到了体现， 只在\(m_1\)和\(m_2\)之间建立了连接，而\(m_3\)因为没有被激活，所以没有参加更新。这说明通过控制激活路标的数量，我们可以控制运动更新的计算复杂度，以及信息矩阵中的连边数量。 如果激活的连边数量很少，那么运动更新的复杂度就会很小，同样的信息矩阵中对应特征间关系的非零元素就会少很多。

接着SEIF就执行了如图12.5所示的稀疏化操作。稀疏化过程涉及到移除机器人与激活特征之间的边，将激活特征转换为消极状态。在SEIF中边的移除，将导致邻接边之间的信息重新分布， 尤其是机器人位姿与激活特征之间。稀疏化操作的时间与地图的大小无关，但它是一种近似，将在机器人后验概率中瞬时一些信息。这种近似的好处是，它带来的真正的稀疏性， 使得滤波器可以高效的更新。

(a) (b)
图 12.5.稀疏化: 通过删除特征与机器人之间的边，令特征退出激活状态。为了以信息的形式补偿这一改变， 激活特征之间的边，以及它们与机器人之间的边都要更新。整个操作可以在常数时间里完成。

最后SEIF还有一个操作，并没有在图表中说明。这一过程涉及到期望估计在图上的传播。 正如我们已经在第三章中讨论的那样，扩展的信息滤波器需要估计状态\(μ_t\)，来对运动模型和测量模型更新。 SELF也需要一个状态估计来进行稀疏化。

显然，我们可以通过公式\(μ = \Omega^{-1}\xi\)来恢复状态估计，其中\(\Omega\)是信息矩阵，\(\xi\)是信息向量(译者按，原文中说的是\(\Omega\)是信息向量， \(\xi\)是信息状态，与上一章中的符号定义不符，遂改之)。但是如果每一次迭代都需要求解这样一个问题，计算量就很大了。 SELF通过一种松弛算法relaxation algorithm绕过了这一问题，它通过信息图传播状态估计。每一个局部状态估计更新都是根据信息图中邻居中最好的估计来进行的。 这个松弛算法将收敛到真实的均值\(μ\)。因为SEIF中的信息表是稀疏的，每个这样的更新操作都是常数时间就可以完成的。尽管有很多警告说，要想取得很好的效果， 那么更新的次数要比一个有限值大。为了保证计算量独立于状态空间的大小，SEIF在每个迭代过程中都执行了固定数量的这样的更新操作。如此得到的状态向量只是一个近似， 它将替代正确的均值估计用于每个更新过程。

12.3 SEIF SLAM算法

算法12.1中描述了SEIF的外层循环。该算法以信息矩阵\(\Omega_{t-1}\)、信息向量\(\xi_{t-1}\)、状态估计\(μ_{t-1}\)， 以及测量值\(z_t\)、控制量\(u_t\)和关联度向量\(c_t\)作为输入。算法的输出是新的状态估计，写成信息矩阵\(\Omega_t\)和信息向量\(\xi_t\)的形式。 此外SELF_SLAM_known_correspondences还输出改进后的估计\(μ_t\)。

算法12.1. 已知数据关联度，用于SLAM问题的稀疏扩展信息滤波器算法。

正如算法12.1所示，SEIF的更新过程由四个步骤构成。运动更新过程如算法12.2所示，将控制量\(u_t\)整合进滤波器的估计中。 它通过若干高效的计算做到这一点。具体的，这一更新过程只修改了信息矩阵和信息向量中与机器人位姿和激活特征有关的部分。 算法12.3中描述的是测量更新过程，它在已知关联度\(c_t\)的情况下整合了测量向量\(z_t\)。这个过程与运动更新一样是局部的。 它只更新了机器人位姿和地图中观测到的特征的信息值。稀疏化过程如算法12.4所描述的那样，是一个近似过程。它通过分别修改信息矩阵和信息向量移除了激活特征。 这个过程也是高效的，它只修改了机器人与激活路标之间的边。最后在算法12.5中讲述了状态估计更新，它使用了一种分期坐标下降技术来恢复状态估计\(μ_t\)。 这个过程又一次运用了SEIF的稀疏特性，在每个增量更新过程中，它只需要咨询其它少量的状态向量元素。

算法12.2. SEIF的运动更新过程

算法12.5. SEIF的分期状态更新过程

算法12.3. SEIF的测量更新过程

算法12.4. SEIF的稀疏化过程

总体上，SEIF的整个更新过程的时间复杂度是常数级的，整个处理时间与地图的大小无关。它与迄今为止唯一的另一个在线SLAM算法——EKF SLAM，形成了鲜明的对比。 EKF的每个更新过程都需要地图大小的二次方级的时间。但是，对于这样的一种常数时间SLAMconstant time SLAM，需要抱有一定的保留， 因为如果环境中有很多环路，那么状态估计的恢复就会是一种复杂的计算过程，目前还没有线性时间的解法。

12.4 SEIF的数学推导

暂略

12.5 稀疏化

12.5.1 一般思想

SEIF的关键步骤就是对信息矩阵\(\Omega_t\)稀疏化。正因为稀疏化是SEIF的基础，让我们在将其应用到信息滤波器之前，讨论一下它的一般术语。 稀疏化是一种近似，通过它一个后验概率分布可以由它的两个边缘分布来近似。假设\(a, b, c\)是随机变量的集合，那么它们的联合概率可以表示为\(p(a,b,c)\)。 为了稀疏化这一分布，我们必须移除变量\(a\)和\(b\)之间的直接联系。换句话说，我们将要用分布\(\tilde{p}\)来替代\(p\)，它具有如下的性质：\(\tilde{p}(a | b, c) = p(a | c), \tilde{p}(b | a, c) = p(b, c)\)。在多变量高斯函数中，很容易看出这个条件独立性等价于丢弃\(a\)和\(b\)之间的直接联系。信息矩阵中对应的元素为0。

\(\tilde{p}\)的一个好的估计是将边缘概率\(p(a,c), p(b,c)\)之积乘上一个比例系数得到的。这两个边缘概率都没有包含变量\(a\)和\(b\)之间的依赖关系， 因为它们都只包含了其中一个变量。因此，乘积\(p(a,c)p(b,c)\)并不包含\(a, b\)之间的直接关系。实际上\(a\)和\(b\)是在给定\(c\)的条件独立的。 但是\(p(a,c)p(b,c)\)不是\(a,b,c\)的一个合法概率分布。这是因为\(c\)在这个表达式中出现了两次。但是比上一个归一化系数\(p(c)\)就得到了一个概率分布，其中\(p(c) > 0\)。 $$ \begin{equation}\label{f20} \tilde{p}(a,b,c) = \frac{p(a,c)p(b,c)}{p(c)} \end{equation} $$ 为了理解这一近似的结果，我们做出如下的变换: $$ \begin{align}\label{f21} \tilde{p}(a,b,c) & = \frac{p(a,b,c)}{p(a,b,c)}\frac{p(a,c)p(b,c)}{p(c)} \\ & = p(a,b,c)\frac{p(a,c)}{p(c)}\frac{p(b,c)}{p(a,b,c)} \\ & = p(a,b,c)\frac{p(a|c)}{p(a|b,c)} \end{align} $$ 换句话说，移除了\(a\)和\(b\)之间的直接依赖关系，相当于条件概率\(p(a|b,c)\)近似等于\(p(a|c)\)。我们也注意到(没有证明)，在所有的\(q\)对\(p\)的近似中， 如果\(a\)和\(b\)关于\(c\)条件独立，那么这就是最接近与\(p\)的近似了。其近似程度可以用Kullback-Leiber散度来衡量，它是衡量一个概率分布与另一个的接近程度的非对称方法。

我们可以看到这样一个重要的事实，原始的\(p(a|b,c)\)至少具有和\(p(a|c)\)一样的信息量at least as informative，在\(\tilde{p}\)中用\(p(a|c)\)来替代\(p(a|b,c)\)。 这是因为\(p(a | b,c)\)的条件变量是\(p(a | c)\)的一个超集。对于高斯函数，这意味着近似\(p(a|c)\)的方差等于或者大于原始分布\(p(a | b,c)\)的方差。 此外，边缘概率\(\tilde{p}(a),\tilde{p}(b),\tilde{p}(c)\)的方差也分别大于或者等于\(p(a),p(b),p(c)\)的方差。换句话说，这种近似的方差不可能收缩。

12.5.2 SEIF的稀疏化

SEIF使用对后验概率\(p(y_t | z_{1:t}, u_{1:t}, c_{1:t})\)的稀疏化，来保持信息矩阵\(\Omega_t\)在所有时间都是稀疏的。这样一来， 它可以取消机器人位姿与地图中各独立的特征之间的边的激活状态。如果正确的完成了这一操作，它也就同时限制了特征对之间的边的数量。

让我们简单的考虑可能引入新边的两种情况。其一，观测到一个消极的特征并激活它，这样会在机器人位姿和这个特征之间添加一条边。其二，运动将使得两个激活特征之间添加一条边。 这一考量建议，控制激活特征的数量可以避免违反两个稀疏边界。因此，稀疏特性可以简单的通过保持激活特征在一个小的数目上来实现。

为了定义稀疏过程，我们先把所有的特征分为三个没有交集的子集: $$ \begin{equation}\label{f22} m = m^+ + m^0 + m^- \end{equation} $$ 其中，\(m^+\)是所有应该保持激活状态的激活特征。\(m^0\)则是我们将要取消激活状态的激活特征。换句话说，我们将要移除机器人位姿与\(m^0\)之间的边。\(m^-\)则是所有目前未激活的特征， 在稀疏化操作之后它们仍将保持未激活状态。因为\(m^+ \cup m^0\)包含了目前所有的激活特征，后验概率可以按照下面的形式计算: $$ \begin{align}\label{f23} p(y_t | z_{1:t}, u_{1:t}, c_{1:t}) & = p(x_t, m^+, m^0, m^- | z_{1:t}, u_{1:t}, c_{1:t}) \\ & = p(x_t | m^+, m^0, m^-, z_{1:t}, u_{1:t}, c_{1:t})p(m^+, m^0, m^- | z_{1:t}, u_{1:t}, c_{1:t}) \\ & = p(x_t | m^+, m^0, m^- = 0, z_{1:t}, u_{1:t}, c_{1:t})p(m^+, m^0, m^- | z_{1:t}, u_{1:t}, c_{1:t}) \end{align} $$ 在最后一步中我们应用了一个事实，如果我们知道了激活特征\(m^0\)和\(m^+\)，那么变量\(x_t\)就与未激活特征\(x^-\)无关。 我们可以为\(m^-\)赋予任何值而不会影响\(x_t\)的条件后验概率p(x_t | m^+, m^0, m^-, z_{1:t}, u_{1:t}, c_{1:t})。这里我们简单的选择了\(x^- = 0\)。

讨论了一般形式的稀疏化思想之后，现在用\(p(x_t | m^+, m^- = 0)\)来替代\(p(x_t | m^0, m^+, m^- = 0)\)，丢弃了对\(m^0\)的依赖关系。

$$ \begin{equation}\label{f24} \tilde{p}(x_t, m| z_{1:t}, u_{1:t}, c_{1:t}) = p(x_t | m^+, m^- = 0, z_{1:t}, u_{1:t}, c_{1:t}) p(m^0, m^+, m^- | z_{1:t}, u_{1:t}, c_{1:t}) \end{equation} $$

这个近似显然与下式等价: $$ \begin{equation}\label{f25} \tilde{p}(x_t, m| z_{1:t}, u_{1:t}, c_{1:t}) = \frac{p(x_t, m^+ | m^- = 0, z_{1:t}, u_{1:t}, c_{1:t})}{p(m^+ | m^- = 0, z_{1:t}, u_{1:t}, c_{1:t})} p(m^0, m^+, m^- | z_{1:t}, u_{1:t}, c_{1:t}) \end{equation} $$

12.5.3 稀疏化的数学推导

暂略

12.6 分期近似地图恢复

SEIF最终的更新过程是计算期望\(\mu\)。在本节中，我们将要省略符号表示中的时间索引，因为它在接下来讨论的技术中没有作用。所以我们将用\(\mu\)代替\(\mu_t\)。

在推导从信息形式中恢复状态估计\(\mu\)的算法之前，我们简单的讨论一下SEIF在什么时候都需要\(\mu\)的哪些部分。SEIF需要机器人位姿的状态估计\(\mu\)以及地图中的激活特征。 在三种不同的场合下需要这些估计:

  1. 均值或者说是期望用于运动模型的线性化，如算法12.2的第3、4和10所示。
  2. 在算法12.3得第6、8、10、13行中测量更新的线性化过程也需要用到这些估计。
  3. 最后还要用在算法12.4中第4行的稀疏化过程中。
但是，我们从不需要一个完整的向量\(\mu\)。我们所需要的只是机器人位姿的估计，以及所有激活特征的位置估计。这是\(\mu\)中所有状态变量的一个子集。 尽管如此，高效的计算这些估计需要一些额外的数学工具，因为即使是恢复所有变量的一个字节，通过\(\mu = \Omega^{-1}\xi\)恢复期望的完全过程需要对矩阵求逆， 或者用一些优化技术。

再说一遍，该算法的核心思想在于矩阵\(\Omega\)的稀疏特性。这一稀疏特性使得我们能够定义一个在线恢复状态变量的迭代算法，一边收集数据，一边估计和构造\(\xi\)和\(\Omega\)。 为了这样做，有必要将\(\mu = \Omega^{-1}\xi\)表述成一个优化问题。一会儿我们就会看到状态\(\mu\)具有如下的形式: $$ \begin{equation}\label{f32} \hat{\mu} = \arg\max_{\mu} p(\mu) \end{equation} $$ \(p(\mu)\)是定义在\(\mu\)上的高斯分布 $$ \begin{equation}\label{f33} p(\mu) = \eta \exp\left\{ -\frac{1}{2} \mu^T \Omega \mu + \xi^T \mu \right\} \end{equation} $$ 这里\(\hat{\mu}\)和\(\mu\)具有相同的形式和维度。To see that this is indeed the case, we note that the derivative of \(p(\mu)\) vanishes at \(\mu = \Omega^{-1}\xi\): $$ \begin{equation}\label{f34} \frac{\partial{p(\mu)}}{\partial{\mu}} = \eta(-\Omega\mu + \xi)\exp\left\{ -\frac{1}{2} \mu^T \Omega \mu + \xi^T \mu \right\} = 0 \end{equation} $$ 这意味着\(\Omega\mu = \xi\)，也就是\(\mu = \Omega^{-1}\xi\)。

这个变换说明恢复状态向量\(\mu\)等价于找到式(\(\ref{f33}\))的模式，现在就变成了一个优化问题。对于这个优化问题，我们现在就要描述一个迭代的爬山算法， 这要得益于信息矩阵的稀疏特性。

我们的方法是一个坐标下降法coordinate descent的实例。为了简化问题，这里我们描述只有一个坐标系的情况。在每一个测量更新过程中，我们的方法都迭代的重复这一优化过程\(K\)次。 那么式(\(\ref{f33}\))中的模式\(\hat{\mu}\)可以按照下式获得: $$ \begin{align}\label{f35} \hat{\mu} & = \arg\max_{\mu} \exp\left\{ -\frac{1}{2} \mu^T \Omega \mu + \xi^T \mu \right\} \\ & = \arg\min_{\mu} \frac{1}{2}\mu^T \Omega \mu - \xi^T\mu \end{align} $$ 我们注意到式(\(\ref{f35}\))中的最小化操作可以显式的写成各个坐标变量\(\mu_i\)的形式，\(\mu_i\)表示\(\mu\)中第\(i\)个坐标变量。 $$ \begin{align}\label{f36} \frac{1}{2}\mu^T \Omega \mu - \xi^T\mu = \frac{1}{2}\sum_i \sum_j \mu_i^T \Omega_{i,j} \mu_i - \sum_i \xi_i^T \mu_i \end{align} $$ 其中，\(\Omega_{i,j}\)为信息矩阵\(\Omega\)中坐标为\((i,j)\)的元素，\(\xi_i\)是向量\(\xi\)中的第\(i\)个元素。将该式对任意坐标变量\(\mu_i\)求偏导，有: $$ \begin{align}\label{f37} \frac{\partial}{\partial \mu_i} \left\{ \frac{1}{2}\sum_i \sum_j \mu_i^T \Omega_{i,j} \mu_i - \sum_i \xi_i^T \mu_i \right\} = \sum_j \Omega_{i,j}\mu_j - \xi_i \end{align} $$ 令该式等于零，就得到了给定所有估计\(\mu_j\)下，第\(i\)个坐标变量\(\mu_i\)的最优值(\(i \neq j\)): $$ \begin{align}\label{f38} \mu_i = \Omega_{i,i}^{-1}\left[\xi_i - \sum_{j \neq i}\Omega_{i,j}\mu_j \right] \end{align} $$ 这一表达式可以方便的写成矩阵的形式。这里我们定义\(F_i = \begin{pmatrix} 0 & \cdots & 0 & 1 & 0 & \cdots & 0 \end{pmatrix}\)来获取矩阵\(\Omega\)中的第\(i\)行元素 (译者注，这是一个行向量，除了第\(i\)个元素为1外，其余所有元素都为0。左乘在矩阵中就是获取矩阵的第\(i\)行元素，将其转置右乘在矩阵中就是获取第\(i\)列元素)。 如此我们可以写成矩阵的形式: $$ \begin{align}\label{f39} \mu_i = (F_i \Omega F_i^T)^{-1}F_i\left[ \xi - \Omega\mu + \Omega F_i^T F_i \mu \right] \end{align} $$ 这个考量源自于增量更新算法。重复地通过下式更新状态向量\(\mu_i\)的一些元素，以降低式(\(\ref{f39}\))左边与右边之间的差异。 $$ \begin{align}\label{f40} \mu_i \leftarrow (F_i \Omega F_i^T)^{-1}F_i\left[ \xi - \Omega\mu + \Omega F_i^T F_i \mu \right] \end{align} $$ 无限地对状态向量中地所有元素重复这一操作就可以收敛到正确的均值上(没有证明)。

容易看出，如果\(\Omega\)是稀疏的，那么式(\(\ref{f38}\))中求和项的元素数量，以及更新规则(\(\ref{f40}\))中向量乘法的数量，就是一个常数。因此，每个更新过程都需要常数级的时间。 为了维护SLAM算法的常数级时间特性，我们可以在每个时间点上提供一个固定的更新数目\(K\)。在很多次更新之后算法就会收敛。

这一近似的质量依赖于因子的数量，其中最主要的是地图中最大的环结构的尺寸。一般的，每个时间点只更新固定数量\(K\)可能不足以得到好的结果。而且， 有很多优化技术都比这里的坐标下降算法更高效。一个典型的例子就是在GraphSLAM中提到的共轭梯度。在实际应用中，依赖于高效的优化技术来恢复\(\mu\)是一种明智的选择。

(a) (b) (c)
(d) (e) (f)
图 12.6. 无稀疏化的SEIF(a)(b)(c) V.S. 4个激活特征稀疏化的SEIF(d)(e)(f)。这个对比是在一个具有50个路标的仿真环境中进行的。 (a)(d)分别显示了两种滤波器的连边，(b)(e)则是关联度矩阵，(c)(f)是归一化之后的信息矩阵。显然稀疏化之后的SEIF具有更少的连边，但是它的结果有些不可信，因为它的关联度矩阵表达的信息较少。

12.7 SEIF应该有多稀疏？

SEIF的稀疏程度应该有多大，是一个关键的问题。特别的，在SEIF中激活特征的数量决定了其稀疏程度的等级。稀疏程度是计算效率和结果的准确性之间权衡的结果。在使用SEIF的时候， 应该抓住这一权衡的精髓。

SEIF的"黄金标准"是EKF，EKF没有稀疏化操作，而且在恢复状态估计的时候它也不依赖于松弛技术。我们基于一个仿真的机器人世界进行对比，在这个世界中机器人感知距离、临近(proximity)、 以及路标的身份(identity)。

  1. 计算量(computation)。图12.7中对比了SEIF与EKF每个更新过程的计算量。这两种情况下，都是用优化方式实现的。图中列举了概率方法的最大计算量分支对比滤波器中信息表示方式。 EKF具有关于地图大小的二次方级的计算复杂度，SEIF复杂度较低是常数级的复杂度。
  2. 内存(memory)。图12.8中对比了EKF和SEIF的内存使用量。同样的EKF是二次方级的，SEIF则基本是线性的，这得益于它稀疏化的信息表示形式。
  3. 准确度(accuracy)。在这一点上EKF比SEIF效果好。这是因为SEIF为了维持稀疏特性的时候做出了近似，而且在恢复状态估计\(\mu_t\)的时候也做出了近似。如图12.9所示， 这两种方法的误差是地图大小的函数。
图12.7. SEIF与EKF的平均CPU时间对比 图12.8. SEIF与EKF的平均内存对比 图12.9. SEIF与EKF的均方根距离对比

我们可以通过仿真来感受稀疏度的作用。图12.10中，在一个具有50个路标的地图中，不同数量激活路标的SEIF的更新时间。可见随着激活特征的数量的减少，更新时间也在下降。 图12.11中则在相同的地图环境下，对比了不同数量激活路标的SEIF的近似误差与EKF的对比。图中的实线是SEIF输出的期望估计，而虚线则是准确的期望估计恢复\(\mu_t\)。 如图所示，6个激活特征能够提供比较有竞争力的结果，而且可以有效地节省EKF的计算量。对于过少的激活特征而言，误差就显著的增加了。SEIF的实现需要实验对比重要参数， 像这里的图表一样对比重要的因素。

图 12.10. EKF(最左边的数据点)与不同稀疏程度下SEIF的更新时间。随着激活特征数量的减少所需更新时间降低了。 图 12.11. EKF(最左边的数据点)与不同稀疏程度下SEIF的近似误差。这两幅图中的数据都是在一个有50个路标的地图中得到的。

12.8 增量数据关联

现在我们来看一下SEIF中的数据关联问题。我们第一个要关注的是熟悉的增量方法，它贪婪的确定最可能的关联度，然后就认定该值就是真值。 在10.3节中介绍EKF的数据关联技术时，就已经讨论了这样一种贪心算法。实际上， EKF和SEIF中所用的贪心数据关联算法的唯一区别在于数据关联概率的计算方式。根据经验来说，在信息滤波器中计算这一概率要比在EKF这种概率滤波器中要困难一点，因为信息滤波器并不跟踪协方差。

12.8.1 计算增量数据关联概率

\(t\)时刻的数据关联度向量标记为\(c_t\)。贪心的数据关联算法维护者一个数据关联的猜测集合，记为\(\hat{c}_{1:t}\)。所谓的增量形式是， 我们在计算\(\hat{c}_t\)的时候还要用到之前更新过程中的估计\(\hat{c}_{1:t-1}\)。数据关联过程就是在估计\(t\)时刻最可能的数据关联向量\(\hat{c}_t\)。它可以通过如下的最大似然估计来获得: $$ \begin{align}\label{f41} \hat{c}_t & = \arg \max_{c_t} p(z_t | z_{1:t-1}, u_{1:t}, \hat{c}_{1:t-1}, \hat{c}_t) \\ & = \arg \max_{c_t} \int p(z_t | y_t, c_t) p(y_t | z_{1:t-1}, u_{1:t}, \hat{c}_{1:t-1}) dy_t \\ & = \arg \max_{c_t} \int \int p(z_t | x_t, y_{c_t}, c_t)p(x_t, y_{c_t} | z_{1:t-1}, u_{1:t}, \hat{c}_{1:t-1}) dx_t dy_{c_t} \end{align} $$ 我们的传感器模型的标记\(p(z_t | x_t, y_{c_t}, c_t)\)显式的说明了它依赖于关联度变量\(c_t\)。准确的计算这一概率值是不可能在常数时间内完成的， 因为它需要计算地图中几乎所有变量的边缘概率。但是，用于高效稀疏化的基础近似方法也可以用在这里。

特别的，令\(m_{c_t}^+\)表示覆盖了机器人位姿\(x_t\)和路标\(y_{C_t}\)的马尔可夫魔毯Markov blanket。这个马尔可夫魔毯就是地图中所有连接到机器人的路标\(y_{c_t}\)的特征集合， 如图12.12所示。注意根据定义，\(m_{c_t}^+\)包含了所有的激活路标。\(\bar{\Omega}_t\)的稀疏特性确保了\(m_{c_t}^+\)中只包含固定数量的特征，无论地图的大小\(N\)。 如果关于\(x_t\)和\(y_{c_t}\)的马尔可夫魔毯没有交集，就在信息图中添加更多的特征，表示\(x_t\)和\(y_{c_t}\)之间的最短路径。

图 12.12. 马尔可夫魔毯。特征\(y_n\)和观测到的特征足以近似特征位置的后验概率，conditioning away all other features。

剩余的所有特征一起被称为\(m_{c_t}^-\): $$ \begin{equation}\label{f42} m_{c_t}^- = m - m_{c_t}^+ - \{y_{c_t}\} \end{equation} $$ 集合\(m_{c_t}^-\)中只包含那些对目标变量\(x_t\)和\(y_{c_t}\)有最小影响的特征。SEIF基本上是通过忽略这些非直接的影响，来对式(\(\ref{f41}\))中的概率\(p(x_t, y_{c_t} | z_{1:t-1}, u_{1:t}, \hat{c}_{t-1})\)近似： $$ \begin{align}\label{f43} p(x_t, y_{c_t} | z_{1:t-1}, u_{1:t}, \hat{c}_{1:t-1}) & = \int \int p(x_t, y_{c_t}, m_{c_t}^+, m_{c_t}^- | z_{1:t-1}, u_{1:t}, \hat{c}_{1:t-1})dm_{c_t}^+ dm_{c_t}^- \\ & = \int \int p(x_t, y_{c_t} | m_{c_t}^+, m_{c_t}^-, z_{1:t-1}, u_{1:t}, \hat{c}_{1:t-1}) p(m_{c_t}^+ | m_{c_t}^-, z_{1:t-1}, u_{1:t}, \hat{c}_{1:t-1}) p(m_{c_t}^- | z_{1:t-1}, u_{1:t}, \hat{c}_{1:t-1}) dm_{c_t}^+ dm_{c_t}^- \\ & \approx \int p(x_t, y_{c_t} | m_{c_t}^+, m_{c_t}^- = \mu_{c_t}^-, z_{1:t-1}, u_{1:t}, \hat{c}_{1:t-1}) p(m_{c_t}^+ | m_{c_t}^- = \mu_{c_t}^-, z_{1:t-1}, u_{1:t},\ \hat{c}_{1:t-1}) dm_{c_t}^+ \end{align} $$

如果该计算中特征集合与地图大小无关，那么这个概率值就可以在常数时间内计算完成，而一帮情况下这一条件总是成立的。在于上面提到的各种求导运算完全对比之后， 我们发现这一后验概率的近似仅仅是通过关联于两个目标变量的子矩阵得到的: $$ \begin{equation}\label{f44} \Sigma_{t:c_t} = F_{x_t, y_{c_t}}^T \left( F_{x_t,y_{c_t},m_{c_t}^+}^T \Omega_tF_{x_t, y_{c_t}, m_{c_t}^+} \right)^{-1} F_{x_t,y_{c_t}} \end{equation} $$ $$ \begin{equation}\label{f45} \mu_{t:c_t} = \mu_t F_{x_t, y_{c_t}} \end{equation} $$ 这一计算是常数时间的，因为其中的矩阵大小与\(N\)无关。从这个高斯函数中，可以很容易恢复式(\(\ref{f41}\))中期望的测量概率。

正如我们的EKF SLAM算法，当似然度\(p(z_t | z_{1:t-1}, u_{1:t}, \hat{c}_{1:t=1}, c_t)\)低于一个阈值\(\alpha\)就标记一个新的特征。我们就令\(\hat{c}_t = N_{t-1} + 1\)， 并且\(N_t = N_{t-1} + 1\)。否则地图的大小就保持不变，即\(N_t = N_{t-1}\)。选择使得数据关联概率最大的值作为\(\hat{c}_t\)的值。

最后告解一下，有时组合马尔可夫魔毯是不充分的，它不包含机器人位姿到考察路标之间的一条路径。当环境中存在较大的环路时，这一情况尤为突出。此时我们就需要扩展特征集合\(m_{c_t}^+\)， 使其至少包含一条\(m_{c_t}\)和机器人位姿\(x_t\)之间的路径。根据环路的尺寸不同，此时路标地数量就可能依赖于地图的大小\(N\)。我们将这样的扩展细节留作练习。

12.8.2 实际考量

一帮情况下，增量贪婪数据关联技术是很脆弱的，正如介绍EKF SLAM时讨论的那样。虚假的测量值很容易造成错误的关联，进而为SLAM估计引入显著的误差。 在SEIF中对于这一弱点的标准修复方式与EKF的类似，就是创建并维护一个临时路标列表provisional landmark list， 我们已经在10.3.3节中，在EKF SLAM的背景下，深度讨论过这一方法了。 临时的列表把那些之前没有观测到的新特征都添加进来，构成一个候选列表，独立于SEIF中维护的列表。在接下来的测量过程中，新采集到一个候选特征都要于等待列表中的所有候选特征对比一下， 增加匹配候选的关联度权重，对于那些看似不在附近的特征要降低它们的权重。当一个候选的权重超过了一个特定的阈值，就将之加入SEIF网络的特征中。

我们注意到数据关联不符合SEIF的常数时间特性。这是因为计算数据关联的时候，需要于多个特征进行对比。如果我们能够保证所有合适的特征都已经在SEIF中以一个短路径连接在激活特征集上， 那么就可能在常数的时间里完成数据关联。这样一来给定一个测量值，SEIF结构就能够搜索最可能的特征。然而，在第一次闭合环路的时候这个条件时不成立的， 此时正确的关联度可能于SEIF的邻接图相差甚远。

现在我们关注一个用于真实车辆的SEIF算法。这里所用的数据是SLAM领域中的一个常用的标准benchmark。这个数据集是通过在悉尼的一个公园中的户外汽车采集的。

图12.13和图12.14中分别显示了实验用车和场地。机器人上装备有一个SICK激光距离扫描器和一套测量转向角和前进速度的系统。激光传感器用于测量环境中的树木， 但它同时也收集了很多伪特征，比如说附近高速公路上运行的汽车。在环境的图中还叠加了原始的里程计，效果很差，这个积分出来的路径，在汽车运行了3.5公里之后就偏差了好几百米。 这在图12.14中可以看到，它显示了汽车的路径。里程计信息的质量很差，而且有很多伪特征，所以这个数据集特别适合用于测试SLAM算法。

图 12.13. 我们的实验用车中被有一个二维雷达距离扫描仪和一个差分GPS系统。汽车的运动通过一个线性可变差分传动传感器测量转向， 并用于轮子固连的编码器测量速度。在其背景中，可以看到测试环境Victoria公园。图片是由澳大利亚户外机器人研究中心的José Guivant和Eduardo Nebot提供的。 图 12.14. 测试环境:悉尼中Victoria公园中的一个350×350米的场地。图片中覆盖的是里程计读数中获得的积分路径。 数据和场地图片是由澳大利亚户外机器人研究中心的José Guivant和Eduardo Nebot提供的，运算结果是由斯坦福大学的Michael Montemerlo提供的。

如图12.15所示，是SEIF恢复的路径。这个路径与EKF所生成的路径相比很难找出量的差别。通过差分GPS测量的平均定位误差小于0.5米，这相比于整个长度为3.5公里的路径比起来小很多。 对应的路标地图如图12.16所示。同样的它与最好的EKF算法的正确性也有得一拼。SEIF的运行效率差不多两倍于EKF，而且所需要的内存数量是EKF的四分之一。这些节约还是比较小的， 但这只是一个小地图，而且大部分时间都是在预处理传感器的数据。对于更大的地图，节省的时间和空间要更多。

图 12.15. SEIF恢复的路径，准确度在±1m。图片由斯坦福大学的Michael Montemerlo提供 图 12.16. 在机器人路径上叠加了估计的路标位置。图片由斯坦福大学的Michael Montemerlo提供

12.9 分枝定界数据关联(Branch-and Bound Data Association)

在SEIF算法中，我们完全有可能定义一个完全不同的数据关联方法，而且可以证明它具有最优的结果，尽管可能需要指数的时间。这个技术有三个关键的内涵:

为了开发这个分枝定界数据关联算法，最好考虑一下定义在数据关联决策上序列上的数据关联树。在任何时间点，每个观测到的特征都可以关联到很多其它特征上， 或者认为是一个新的之前没有观测到的特征。历次数据关联选择之后的树，起始于时间点\(t = 1\)一直到当前的时候，正如图12.17a所示。显然树随着时间呈指数级增长， 因此穷举它是不太现实的。之前章节中描述的增量贪心算法，则是根据局部最可能的数据关联，沿着一条路径一直查找下去，如图12.17a中的灰色路径所示。

(a) (b) (c)
图 12.17. (a)数据关联树，其分枝因素随着地图中的路标数量而增长。(b) 基于树的SEIF维护者整个扩展节点边界的对数似然度， 使得我们可以找到备选的路径。(c)改进的路径。

显然，如果增量贪心算法成功了，那么它找到的路径是最优的。(译者按:不太可能吧，贪心算法不是只能找到局部的最优解吗，不能保证全局最优的呀？) 但是，增量贪心算法也可能失败。一旦做出了一个错误的决策，增量的方法是不能恢复的。此外，错误的数据关联决策将在地图中引入误差，之后的数据决策就会引入更多的误差。

12.9.1 递归搜索

接下来我们要讨论的算法是对增量贪心算法的推广，是对树的完全搜索可以得到最优的结果。当然搜索树中所有的分枝是不太实际的做法。但是如果我们在目前扩展的树的边界上，维护所有节点的对数似然值， 我们就可以保证这种最优性。如图12.17b所示，分枝定界SEIF算法维护的不仅仅是数据关联树上的一个路径，还有整个边界。每当扩展一个节点的时候，所有候选的结果都会被评估一遍， 并记录下对应的似然值。这点正如图12.17b所示，描述了整棵树边界的对数似然。

查找式(\(\ref{f41}\))中的最大值，意味着所选叶节点的对数似然值大于或者等于同样深度的其它叶节点的对数似然值。因为对数似然随着树的深度急剧下降，我们可以保证， 当所选叶节点的对数似然值大于或者等于任何边界上其它节点的对数似然值时，就找到了最优的数据关联。换句话说，当一个边界节点的对数似然值大于所选的叶节点的，就有机会通过修改过去的数据关联来提高数据的似然度。 我们的方法就简单的扩展这些边界节点。如果一个扩展触及到一个叶节点，并且它的值比目前最好的叶节点大，这个值将选作新的数据关联。否则当整个边界所具有的值都小于或者等于已选叶节点时，就终结搜索过程。 这个方法保证总是维护着当前最好的数据关联，但是，偶尔它还需要大量的搜索。

12.9.2 计算任意数据关联概率

要测试地图上两个特征是否需要连接，我们现在需要一种技术，为地图中任意两个特征计算它们相等的概率。这个测试基本上与GraphSLAM的关联度测试一样。但是，在SEIF中这个测试是一种近似， 准确的计算其对数似然值需要额外的计算开销。

算法12.6中列举了一个测试地图中两个特征是否一样的概率值算法，这一测试足以实现贪心的数据关联技术。其中关键的计算是恢复地图中一小部分特征集合\(B\)的联合协方差和平均向量。 为了确定地图中两个特征是否一样，SEIF必须考虑它们之间连边的信息。连边越多，结果就越准确，到那时需要的计算量就增加了。实际上，我们进场会面临着确定两个马尔可夫魔毯的特征问题。 一个特征的马尔可夫魔毯就是这个特征本身，以及在信息矩阵中以非零元素建立联系的所有其它特征。在大多数情况下，马尔可夫魔毯是相交的，如果它们不相交， 那么算法12.6就会在路标中间确定的那个一个路径，这两个路标一定是同时被同一个机器人观测到。

算法12.6接着用一种与高效稀疏化极为类似的技巧，裁剪出一个局部地信息矩阵和信息向量。SEIFs condition away features outside the Markov blankets。 因此，SEIF就有了一种高效的计算目标概率的技术，虽然是一种近似方法(because of the conditioning)，但是在实际应用中非常有效。

这个结果不仅使得SEIF可以做出数据关联决策，而且提供了一种计算这种决策的对数似然方法。这个过程输出结果的对数就是这种特定数据形式的对数似然值，沿着数据关联树中的路径求和， 就得到了特定关联情况下的全部数据对数似然值。

12.9.3 等价约束

一旦在数据关联搜索的过程中确定地图中两个特征是等价的，SEIF就在信息矩阵中添加一个软连接。假定第一个特征是\(m_i\)第二个特征是\(m_j\)。软连接通过如下形式的指数平方约束，约束了它们的位置 $$ \begin{equation}\label{f46} \exp \left\{-\frac{1}{2}(m_i - m_j)^Y C (m_i - m_j) \right\} \end{equation} $$ 其中\(C\)是一个对角惩罚矩阵，其形式为 $$ \begin{equation}\label{f47} C = \begin{pmatrix} \infty & 0 & 0 \\ 0 & \infty & 0 \\ 0 & 0 & \infty \end{pmatrix} \end{equation} $$ 实际应用中，\(C\)的对角元素都是用一个很大的整数替代的，这个数值越大，约束就越强。

容易看出未归一化形式的高斯式(\(\ref{f46}\))可以写作信息矩阵中\(m_i\)和\(m_j\)之间的连接。简单的定义了投影矩阵 $$ \begin{equation}\label{f48} F_{m_i - m_j} = \begin{pmatrix} \begin{matrix}0&\cdots&0\end{matrix} & \begin{matrix}1&0&0\end{matrix} & \begin{matrix}0&\cdots&0\end{matrix} & \begin{matrix}-1&0&0\end{matrix} & \begin{matrix}0&\cdots&0\end{matrix} \\ \begin{matrix}0&\cdots&0\end{matrix} & \begin{matrix}0&1&0\end{matrix} & \begin{matrix}0&\cdots&0\end{matrix} & \begin{matrix}0&-1&0\end{matrix} & \begin{matrix}0&\cdots&0\end{matrix} \\ \begin{matrix}0&\cdots&0\end{matrix} & \underbrace{\begin{matrix}0&0&1\end{matrix}} & \begin{matrix}0&\cdots&0\end{matrix} & \underbrace{\begin{matrix}0&0&-1\end{matrix}} & \begin{matrix}0&\cdots&0\end{matrix} \\ & m_i & & m_j & \end{pmatrix} \end{equation} $$ 这个矩阵将状态\(y_t\)映射称为\(m_i -m_j\)的差值中。因此式(\(\ref{f46}\))可以写作 $$ \begin{equation} \label{f49} \exp \left\{-\frac{1}{2}(F_{m_i - m_j} y_t)^T C (F_{m_i - m_j} y_t) \right\} = \exp \left\{-\frac{1}{2}y_t^T\left[F_{m_i - m_j}^T C F_{m_i - m_j} \right] y_t \right\} \end{equation} $$ 因此，为实现这一软约束，SEIF就需要将\(F_{m_i - m_j}^T C F_{m_i - m_j}\)添加到信息矩阵中，此时留下信息向量没有改变 $$ \begin{equation} \label{f50} \Omega_t \longleftarrow \Omega_t + F_{m_i - m_j}^T C F_{m_i - m_j} \end{equation} $$ 显然，添加项是稀疏的，它只包含了特征\(m_i\)和\(m_j\)之间非零的非对角元素。一旦添加了一个软连接，还可以通过逆运算来移除 $$ \begin{equation} \label{f51} \Omega_t \longleftarrow \Omega_t - F_{m_i - m_j}^T C F_{m_i - m_j} \end{equation} $$ 这个移除操作可以无视将该约束引入滤波器中的时间。但还是需要小心的记录下来，以保证SEIF从来不会移除一个不存在的数据关联约束，否则信息矩阵可能不再是半正定的了， 而且由此得到的置信度可能不是一个合法的概率分布。

12.10 实际考量

这种方法的任何有竞争力的实现，通常都只存在一个小数量的数据关联路径，在任何时间点都有不错的结果。当在室内环境中的环路中运行时，通常最多有三个不错的假设，一个闭包、一个左连续、一个右连续。 但是所有的很快就会变成不可能，所以递归的搜索树的次数应该比较小。

使数据关联更容易成功的一种方法时组合上负测量信息negative measurement information。距离传感器，在我们的实现中，可以返回所考量的物体存在于世界中的正信息和负信息。正信息就是物体检测。 负信息适用于探测物体于传感器之间的空间中。机器人未能检测出物体的情形，比传感器读数提供了在测量范围内一个物体不存在的信息，更贴近实际。

评估新的约束在整个似然度上的影响，需要考虑正负两方面的信息。它们都通过计算在位置估计上两个扫描是否匹配来获得。使用距离扫描器的时候， 正面和反面信息的组合是通过建立在一个扫描基础上局部地图上叠加一个扫描获得的。这样做时，可以直接确定两个局部地图的近似匹配概率，这种方法应用了正面和反面的信息。

本节的最后列举一些使用基于数据关联树的SEIF算法的实际结果。图12.18a中描述了增量ML数据关联结果，它的效果于常规地增量扫描匹配一样。显然，图中有些走廊出现了重影，这暴露了ML算法的缺陷。 相比之下，图12.18b中的地图就要更准确一些。图12.19a中描述了最近一次测量(不是整个路径)的对数似然值，当地图不一致的时候，它就有明显的下降。对于这一点，SEIF致力于搜索替代的数据关联。 很快它找到了"正确的"那个，并产生了图12.18b中的地图。图12.20中放大了出现问题的区域，也就是似然度下降的那一段。图12.19b中则显示了修正后的对数似然值。

图 12.18(a).使用增量ML扫描匹配构建的地图。 图 12.19(a).实际测量的对数似然关于时间的函数。其中较低的似然值是由于错误的赋值导致的。 图 12.20(a).在闭合大型环路时，机器人首先错误的假设还有第二个走廊存在。

图 12.18(b).全递归分枝定界数据关联技术下的地图。 图 12.19(b).通过树搜索递归地修复错误数据关联假设下地对数似然。成功的减少了数据曲线上的下沉。 图 12.20(b).但是当机器人遇到了走廊上的直角时就会产生不一致性。此时，算法迭代地进行搜索，以提高数据关联决策。

最后，在图12.21中对比了不同算法在一个有多个环路的大型建筑中的建图效果。

(a) 机器人路径 (b) 增量ML，在左边出现了不一致的地图
(c) FastSLAM，参见下一章 (d) 具有分枝定界数据关联的SEIF
图12.21. 图片由弗莱堡大学的Dirk Hähnel提供。

12.11 多机器人SLAM

SEIF还可以用于多机器人SLAM问题multi-robot SLAM problem。多机器人SLAM问题涉及到若干个独立探索地图的机器人，最终的目标是将它们的地图融合成为一个庞大的地图。 在许多方面，多机器人SLAM问题让人们联想到了单一地图构建问题，在这样的问题中，需要将不同时间点上的数据融合进一个单一的后验概率中。但是，多机器人问题在很多维度上都要更困难:

本节中，我们只描述实现一个多机器人建图算法的主要思想。我们将展现一种融合两幅已经建立起关联关系的地图的算法。我们还将讨论，但不会证明，在多机器人SLAM中建立全局关联度的技术。

12.11.1 地图融合

算法12.7中描述了一种融合已知关联度地图的关键过程。该算法以两个局部机器人后验概率作为输入，它们分别被表示成为信息表\(\Omega^j, \xi^j\)和\(\Omega^k, \xi^k\)的形式。 它还需要三个其它要素

  1. 一个线性化的排列元组\(d\)
  2. 一个相对旋转角度\(\alpha\)
  3. 一个特征关联度集合\(\mathcal{C}^{j,k}\)
排列向量\(d = \begin{pmatrix} d_x & d_y \end{pmatrix}^T\)以及转角\(\alpha\)声明了两个机器人坐标系的相对方位。具体的，第\(j\)个机器人位姿\(x^j\)和它的地图中的特征， 通过一个旋转\(\alpha\)和一个平移\(d\)，映射到了第\(k\)个机器人的坐标系中。这里我们用\(j \rightarrow k\)来表示第\(j\)个机器人的地图坐标在第\(k\)个机器人的坐标系中的表达。

  1. 对于第\(j\)个机器人的位姿\(x_t^j\) $$ \begin{equation} \label{f52} \begin{matrix} \underbrace{ \begin{pmatrix} x^{j \rightarrow k} \\ y^{j \rightarrow k} \\ z^{j \rightarrow k} \end{pmatrix} } & = \begin{pmatrix} d_x \\ d_y \\ \alpha \end{pmatrix} + \begin{pmatrix} \cos \alpha & \sin \alpha & 0 \\ -\sin \alpha & \cos \alpha & 0 \\ 0 & 0 & 1 \end{pmatrix} & \underbrace{ \begin{pmatrix} x^j \\ y^j \\ \theta^j \end{pmatrix} } \\ x_t^{j\rightarrow k} & & x_t^j \end{matrix} \end{equation} $$
  2. 对于第\(j\)个机器人的地图\(m_i^j\) $$ \begin{equation} \label{f53} \begin{matrix} \underbrace{ \begin{pmatrix} m_{i,x}^{j \rightarrow k} \\ m_{i,y}^{j \rightarrow k} \\ m_{i,s}^{j \rightarrow k} \end{pmatrix} } & = \begin{pmatrix} d_x \\ d_y \\ 0 \end{pmatrix} + \begin{pmatrix} \cos \alpha & \sin \alpha & 0 \\ -\sin \alpha & \cos \alpha & 0 \\ 0 & 0 & 1 \end{pmatrix} & \underbrace{ \begin{pmatrix} m_{i,x}^j \\ m_{i,y}^j \\ m_{i,s}^j \end{pmatrix} } \\ m_i^{j\rightarrow k} & & m_i^j \end{matrix} \end{equation} $$

这两个映射在算法12.7SEIF_map_fusion的第2和第5行中进行。这个过程涉及到对信息矩阵和信息向量的一个局部旋转和平移。后来在第6和第7行中，通过构建一个联合后验地图来进行地图融合。 在融合算法最后阶段，它获得了关联度列表\(\mathcal{C}^{j,k}\)。这个集合有特征对\((m_j, m_k)\)构成，它们分别对应着机器人\(j\)和机器人\(k\)的地图。 融合的过程以类似于12.9.3节软等价约束来进行。具体的，对于任何两个关联特征，我们在信息矩阵中联系着这两个特征的元素上，简单地添加较大的项。

我们注意到地图融合阶段还有一种实现方式，折叠了信息矩阵和向量中对应地行和列。下面的例子就说明了在滤波器中特征2和特征4的折叠操作，这一现象发生在我们的关联度列表生成特征2和特征4时一样的。 $$ \begin{equation}\label{f54} \begin{pmatrix} \Omega_{11} & \Omega_{12} & \Omega_{13} & \Omega_{14} \\ \Omega_{21} & \Omega_{22} & \Omega_{23} & \Omega_{24} \\ \Omega_{31} & \Omega_{32} & \Omega_{33} & \Omega_{34} \\ \Omega_{41} & \Omega_{42} & \Omega_{43} & \Omega_{44} \end{pmatrix} \rightarrow \begin{pmatrix} \Omega_{11} & \Omega_{12} + \Omega_{14} & \Omega_{13} \\ \Omega_{21} + \Omega_{41} & \Omega_{22} + \Omega_{42} + \Omega_{24} + \Omega_{44} & \Omega_{23} + \Omega_{43} \\ \Omega_{31} & \Omega_{32} + \Omega_{34} & \Omega_{33} \end{pmatrix} \end{equation} $$ $$ \begin{equation}\label{f55} \begin{pmatrix} \xi_1 \\ \xi_2 \\ \xi_3 \\ \xi_4 \end{pmatrix} \rightarrow \begin{pmatrix} \xi_1 \\ \xi_2 + \xi_4 \\ \xi_3 \end{pmatrix} \end{equation} $$ 折叠信息利用了信息状态的叠加特性。

12.11.2 地图融合的数学推导

暂略。

12.11.3 建立关联度

剩下的问题就是建立不同地图之间的关联度，并计算旋转角\(\alpha\)和平移\(\delta\)。目前已经有大量的可行方法，所以这里我们只介绍一种。显然这一问题是，有大量的特征可以在两个地图中得到匹配。

对于基于路标的地图，一个标准算法可能尝试捕获局部的配置，这些配置足够的贴近路标，以至于这样的局部配置对比就会提供关联度的较好候选。比如说，我们可能确定有\(m\)个特征在路标附近(\(m\)是一个较小的值)， 然后计算它们的相对距离和方向。这样产生的距离或者方向的向量就可以看作是一种统计量，用来对比两幅地图。使用哈希表或者kd树，我们可能很快就能够找到结果。 因此，对于问题——"在第\(j\)个机器人的地图中的这\(m\)个路标，在机器人\(k\)的地图中有\(m\)个路标与之对应吗？"我们可以高效的找到答案，最少是一个近似的结果。一旦一个初始关联度被确定了， 我们就可以很容易地，通过最小化两幅地图中这\(m\)个特征间的二次距离，来计算\(d\)和\(\alpha\)。

融合过程就可以按照下面地方式进行，首先使用从两幅地图中这\(m\)个局部特征计算得到的\(d, \alpha, \mathcal{C}^{j,k}\)来调用融合算子。接下来，按照算法12.6中的关联度测试， 筛选出那些概率值低于一个特定阈值的特征，从而识别出更多的一致路标。当找不到匹配的路标对时就终止算法。

统一地图中对应部分的对比，尤其是对不关联的地标附近的对比，将提供一个是否接受匹配结果的判据。形式上，一旦搜索终结了，如果地图融合的结果可以降低总体的似然度(以对数的形式)，就接受融合。 融合是通过对折叠的特征加上一个偏移量并乘以一个常数实现。这就高效的实现了一种贝叶斯MAP估计器，它具有对世界中特征数量的指数先验分布。

一般的，我们注意到搜索最优关联度是一个NP难问题。但是在实际应用中，爬山算法总是工作的很好。

12.11.4 示例

图12.22中给出了八个局部地图的例子。这些地图是通过将我们之前的banchmark数据集合拆分为8个不相交的子序列获得的，并分别在每个序列上运行了SEIF算法。

通过哈希表对\(m=4\)的局部特征进行关联度搜索来组合这些地图，SEIF可靠的得到了图12.23中显示的地图。当通过\(\mu = \Omega^{-1}\xi\)计算该地图的时候， 它不仅是对各个局部地图的叠加。实际上，在处理过程中每个地图都多少有些弯曲，这是对信息表附加的组合操作的结果。

图 12.22. 通过把数据拆分为8个序列而获得的8个局部地图 图 12.23. 多机器人SLAM的结果，通过本章中所描述的算法获得的。图片由Yufeng Liu提供。

图12.24是对三个飞行器的仿真。各个子图说明了通过地图的融合，各个地图的不确定度降低了。

(a) Step \(t = 3\) (b) Step \(t = 62\) (c) Step \(t = 65\) (d) Step \(t = 85\) (e) Step \(t = 89\) (f) Step \(t = 500\)
图 12.24. 不同时间点多机器人SLAM仿真截图。在第62到64步，飞行器1和飞行器2第一次穿过了相同的区域， 因此它们的局部地图中的不确定度缩小了。后来，在第85到89步中，飞行器2观测到了飞行器3观测到的路标，对于总体的不确定度影响较小。500步之后，所有的路标都被正确的定位了。

12.12 总结

本章中描述了一种高效的在线SLAM问题的解决方案——稀疏扩展信息滤波器sparse extended information filter, SEIF。类似于GraphSLAM，SEIF以信息表形式表示后验概率。 但不同的是，SEIF总是积分过去的位姿，因此构成了一种在线的SLAM算法。我们学习了:

SEIF时本书中讨论的第一个高效的在线SLAM算法。它结合了信息表达形式的优雅和积分过去位姿的思想。它是EKF的懒惰的姐妹(it is the lazy sister of the EKF)， EKF积极主动地将每个新测量值的信息通过特征网络传播开来，进而计算准确的联合协方差。SEIF很少累积这些信息，随着时间推移它会慢慢解决。在SEIF中基于树的数据关联也是懒惰的， 它只在必要的时候才考虑到达已知最好节点的备选路径。这点将在下一章中与粒子滤波器中的数据关联方法进行对比。

为了得到高效的在线算法，SEIF不得不做出各种近似，这使得它的精度比GraphSLAM和EKF要差一点。具体的，SEIF由两个局限性。其一，它和EKF一样只进行一次线性化。 GraphSLAM就可以重新线性化，能够显著的提高记过的准确度。其二，SEIF使用一种近似的方法来维护信息矩阵的稀疏特性。这种稀疏特性时GraphSLAM算法与生俱来的， 时整合的信息的自然属性。

尽管SEIF(已知关联度)的每个基本步骤都可以在常数时间内完成，有一点我们还是要小心的。如果SEIF应用于一个线性的系统，也就是我们不需要泰勒级数近似并且数据关联已知， 其更新时间确实就是常数的。但是，因为需要线性化，我们需要一个对信息状态均值\(\mu_t\)的估计。这个估计并不是以传统的信息滤波器的形式维护的，恢复它需要一定的时间。 我们的SEIF实现只是一种近似，而且其后验概率估计的质量也依赖于这一近似的质量。

12.13 参考文献

在之前的章节中已经讨论了信息论形式的SLAM的文献，到目前为止这些文献都时离线优化的方法。信息滤波器在SLAM领域中的历史相对比较短。在1997年，Csorba开发了一个信息滤波器， 维护了三个路标之间的相对信息。他可能时第一个观察到这种信息练剑可以维护全局的关联度信息，为需要二次方的线性内存的算法铺平了道路。Newman (2000); Newman and Durrant-Whyte (2001)开发了一种类似的滤波器， 但是留下了如何获取路标与路标之间的信息连接的问题。Leonard and Newman进一步的开发该算法使其成为一个高效的排列算法，并为之起了一个充满野心的名字consistent, convergent, and const-time SLAM。 他们成功的应用在一个使用合成孔径声纳的自主水下机器人上(Newman and Rikoski 2003)。该领域中另外一个具有深远影响的算法是Paskin’s (2003)提出的thin junction filter， 它用一个称为thin junction树(Pearl 1988; Cowell et al. 1999)的系数网络来表示SLAM的后验概率。Frese (2004)也提出了相同的思想，开发了一个类似的树，出于高效推理的目的，分解信息矩阵。 Julier and Uhlmann开发了一个可扩展的技术，称为covariance intersection，他稀疏的对后验概率做出了近似，这种方法可以被证明是可以防止过拟合的。 他们的算法被成功的应用到了NASA的火星漫游者探测器上(Uhlmann et al. 1999)。信息滤波器的观点也与Bulata and Devy (1996)的早期工作有关，他们的方法首先获取在局部路标尺度参考坐标系下的路标模型， 然后通过解决路标间相对信息来装配一个一致的全局地图。最后，一些离线的SLAM算法解决了完全SLAM问题，比如Bosse et al. (2004), Gutmann and Konolige (2000), Frese (2004), and Montemerlo and Thrun (2004)的方法，已经足够快，可以在有限的数据集上在线的运行了。

Gutmann and Konolige (2000)讨论了多机器人地图融合的问题。Nettleton et al.(2003)第一个将信息表达形式扩展到了多机器人SLAM问题中。他们领悟到信息的叠加特性可以不对称的整合不同机器人的局部地图。 他们还领悟到子地图的叠加可以形成高效的通信算法。据此算法，地图的融合有可能以机器人数量的对数时间完成。但是他们留下了如何排列这些地图的问题，后来被Thrun and Liu (2003)接受了。

SEIF算法是Thrun et al. (2002); see also Thrun et al. (2004a)开发的。据我们所知，这是第一个从滤波器的视角派生出创建特征对间信息连接的算法。Liu and Thrun (2003)开发了一种用于SEIF的贪心数据关联算法， 后来被Thrun and Liu (2003)扩展到了多机器人SLAM中。分枝定界数据关联算法要归功于Hähnel et al. (2003a)，基于早期的分枝定界算法Lawler andWood (1966) and Narendra and Fukunaga (1977)。 Kuipers et al. (2004)也平行的开展了类似的工作，他开发了类似的技术，虽然不是在信息论概念的背景下进行的。Thrun et al. 2004c将SEIF用于对一个废弃矿井建图，所构建的地图有\(10^8\)个特征。

本章中提及的Victoria公园数据集源自于Guivant et al. (2000)。

12.14 练习

暂略




Copyright @ 高乙超. All Rights Reserved. 京ICP备16033081号-1