本文最后更新于2020年5月1日,已超过 6 个月没更新!

基于图优化理论SLAM算法最早于1997年被提出来,但由于使用标准技术解决误差最小化问题具有相当高的复杂性,因此在当时并没有受到足够重视。直到2010年左右,随着对SLAM问题结构的深入研究以及稀疏线性代数领域的发展,使得解决优化问题成为了可能。基于图优化理论的 SLAM 算法目前已经在速度和准确性方面属于最先进的技术之一。


1. Graph-based SLAM

图优化SLAM分为前端和后端两部分,前端负责构图,主要由帧间匹配(scan-match)和回环检测(loopclosure)两部分来进行构图;后端主要负责对位姿估计进行优化。

数学概念:
  • 用一个图(Graph)来表示SLAM问题
  • 图中的节点来表示机器人的位姿
  • 两个节点之间的边表示两个位姿的空间约束
  • Graph-based SLAM:构建图,并且找到一个最优的配置(各节点的位姿),让预测与观测的误差最小


2. 非线性二乘在SLAM中的应用

2.1 图的构建

1) 里程计测量



机器人从节点i运动到节点i+1,里程计测量得到此运动信息。并在对应节点中连上一条边,边为里程计测量值。

2) 回环检测



节点i和节点j观测到同样的环境信息,两者进行匹配得到相对位姿。并在对应的节点中连一条边,边为匹配的相对位姿。用信息矩阵来描述本次匹配的可靠性。

2.2 误差函数

  • 观测值z_{ij} = (dx,dy,d\theta)表示经过匹配计算得到的节点x_i和节点x_j的相对位姿。即节点x_j在节点x_i坐标系下的坐标。
  • 预测值为里程计的测量值,图中x_ix_j即为里程计测量得到的坐标,因此预测值z'_{ij} = X_i^{-1} X_j,其中X_i表示x_i对应的转换矩阵。
  • 因此误差函数e_{ij}(x)=t2v(z_{ij}^{-1} z'_{ij})。其中t2v表示把转换矩阵转换到对应的位姿。

误差函数矩阵形式:

    \[e_{ij}(x) = \begin{bmatrix}      R_{ij}(x) = (R_i^T(t_j - t_i) - t_{ij})\\      \theta_j - \theta_i - \theta_{ij} \end{bmatrix}\]

对应Jacobian矩阵:

    \[\frac{\partial{e_{ij}(x)}}{\partial{x_i}} =  \begin{bmatrix} -R_{ij}^T R_i^T & R_{ij}^T \frac{\partial{R_i^T}}{\partial{\theta}}(t_j - t_i)\\ 0 & -1 \end{bmatrix}\]

    \[\frac{\partial{e_{ij}(x)}}{\partial{x_i}} =  \begin{bmatrix} R_{ij}^T R_i^T & 0\\ 0 & 1 \end{bmatrix}\]

2.3 误差函数的线性化

误差函数:

    \[e_{ij}(x+\Delta{x}) = e_{ij}(x) + J_{ij} \Delta{x}\]

    \[J_{ij} = \frac{\partial{e_{ij}(x)}}{\partial{x}}\]

因为误差函数只跟x_ix_j有关,因此具有下列的性质:

    \[\frac{\partial{e_{ij}(x)}}{\partial{x}} = (0 \cdot\cdot\cdot \frac{\partial{e_{ij}(x_i)}}{\partial{x_i}} \cdot\cdot\cdot \frac{\partial{e_{ij}(x_j)}}{\partial{x_j}} \cdot\cdot\cdot 0)\]

    \[J_{ij} = (0 \cdot\cdot\cdot A_ij \cdot\cdot\cdot B_ij \cdot\cdot\cdot 0)\]

Jacobian矩阵的形式:

Jacobian是一个稀疏的向量,因此其会导致H矩阵的稀疏性:

H和b的最终形式:

H矩阵为稀疏矩阵,可以利用此特征进行快速求解。

2.4 固定坐标系

  • 观测值观测到的值两个位姿之间的相对位姿
  • 满足相对位姿约束的解有无穷多组
  • 为了让解唯一,必须加入一个约束条件让某一个位姿固定,一般选择第一个位姿。

第一个位姿为:

    \[\Delta{x_1} = 0\]

也即等价于:

    \[\begin{bmatrix}1 & 0 & 0\\0 & 1 & 0\\0 & 0 & 1\end{bmatrix} \Delta{x_1} = \begin{bmatrix} 0\\0\\0 \end{bmatrix}\]

加入此约束并求解线性系统:

    \[H\Delta{x} = -b\]

可以得到其等价于:

    \[H_{11} += \begin{bmatrix}1 & 0 & 0\\0 & 1 & 0\\0 & 0 & 1\end{bmatrix}\]

2.5 构建线性系统

已知误差项和Jacobian矩阵A_{ij}B_{ij}

向量𝑏的更新为:

    \[b_i^T += e_{ij}^T \Omega_{ij} A_{ij} \qquad b_j^T  += e_{ij}^T \Omega_{ij} B_{ij}\]

矩阵H的更新为:

    \[H_{ii} += A_{ij}^T \Omega_{ij} A_{ij} \qquad H_{ij} += A_{ij}^T \Omega_{ij} B_{ij}\]

    \[H_{ji} += B_{ij}^T \Omega_{ij} A_{ij} \qquad H_{jj} += B_{ij}^T \Omega_{ij} B_{ij}\]

2.6 求解

  • 已知矩阵𝐻和向量𝑏
  • 求解线性方程组:\Delta{x} = -H^{-1} b
  • 不断进行迭代,直至收敛:x = x + \Delta{x}

伪代码如下所示:


3. Cartographer

3.1 简介

Cartographer是谷歌(Google)于2016年10月推出一种激光SLAM算法, 该算法是目前效果最好的激光SLAM算法,可以根据激光雷达等传感器测量的数据生成分辨率为5cm的实时栅格地图。
其采用基于图优化方法的SLAM理论框架,分为前端和后端两部分:前端主要负责Scan-to-Submap和回环检测,将处理后的激光雷达数据与子图进行匹配,同时当生成个子图且没有新的数据帧插入的时候会进行局部回环检测。子图创建完成后,如果找到了与当前估计位姿最佳的匹配结果,则将其添加到回环约束中。后端主要负责对位姿估计进行优化,采用分支定界和预先计算的网格来实现全局闭环检测。

3.2 特性

  • 基于图优化的SLAM算法;
  • 比较完善的匹配系统,包含建图和定位;
  • 目前效果最好的开源激光SLAM系统;
  • 有人在专门的维护,不断增加新的特性。

源码仓库:cartographer
官网提供的ROS集成仓库:cartographer_ros


Try and fail, but don't fail to try.