Google Cartographer SLAM 原理 (Real-Time Loop Closure in 2D LIDAR SLAM 论文详细解读)

本文前言

  *转载请注明出处 @梦凝小筑

  本人的研究方向为激光SLAM,因此对于Google Cartographer 的经典算法十分感兴趣,但是苦于该算法的论文是英文写作,且该论文有着公式多,解释少的特点。因此在看了原论文和网上的各种论文解读,都没有能够完全把这块硬骨头吃下去。

  机缘巧合,本人研究生课程高等运筹学大作业需要运用和Google Cartographer 中的闭环检测 相同的方法,结合Cartographer的源代码,才彻底将Cartographer论文搞懂。因此在这里写一篇比较完整的总结,方便大家相互学习讨论!

贴出本人参考的一些链接

官方资料1:https://google-cartographer-ros.readthedocs.io/en/latest/going_further.html

官方资料2:https://google-cartographer.readthedocs.io/en/latest/

论文链接1:https://static.googleusercontent.com/media/research.google.com/zh-CN//pubs/archive/45466.pdf

论文链接2:https://april.eecs.umich.edu/pdfs/olson2009icra.pdf

参考链接:https://zhehangt.github.io/2017/05/01/SLAM/Cartographer/CartographerPaper/

 

Cartographer 论文解读

一、Introduction

    在建图上应用SLAM并不是一个新的概念,这里不再作为本文的重点。

    本文的贡献在于:提出了一种新的基于激光数据的回环检测方法,这种方法可以减少计算量,可以满足大空间的建图需要,并且对大规模数据进行实时优化。

    hector并没有应用到回环检测,而google在此基础上进行改进,并应用上了回环检测,,还是值得学习和借鉴的。

二、Related Work

    相关工作不再赘述,直接搬用索哥的总结

    相关工作中首先介绍了scan matching的几种方法:

    1.scan-to-scan matching是基于激光SLAM中最常用来估计相关位姿的。但是非常容易造成累积误差。

    2.scan-to-map matching 可以减少累积误差,因为scan-to-map每次都通过高斯-牛顿法获得局部最优的位姿,前提是要有一个比较好的初始化位姿估计

    3.pixel-accurate scan matching 可以进一步减小局部误差,但是计算量比较大。这个方法同样可以用来检测回环。

    4.从laser scan 中提取特征,从而减少计算量。histogram-based matching用于回环检测。用机器学习做laser scan的特征检测。

    之后介绍了处理累积误差的两种方式

    1.基于粒子滤波的优化。粒子滤波在处理大场景地图时,由于粒子数的极具增长造成资源密集。

    2.基于位姿图的优化。与视觉SLAM的位姿图优化大同小异,主要是在观测方程上的区别。

三、System Overview

    Cartographer 能产生一个精度为5cm的2D栅格地图。

    Cartographer在前端匹配环节区别与其它建图算法的主要是使用了Submap这一概念,每当或得一次laser scan的数据后,便与当前最近建立的Submap去进行匹配,使这一帧的laser scan数据插入到Submap上最优的位置。(这里用的是高斯牛顿解最小二乘问题)在不断插入新数据帧的同时该Submap也得到了更新。一定量的数据组合成为一个Submap,当不再有新的scan插入到Submap时,就认为这个submap已经创建完成,接着会去创建下一个submap,具体过程如下图。(这里没有理解什么情况下新的scan不会再匹配到该Submap上)

 

    因此这里scan matching的本质是当前laser scan与多个邻近的laser scan之间进行的。

    通过scan matching得到的位姿估计在短时间内是可靠的,但是长时间会有累积误差。因此Cartographer应用了回环检测对累积误差进行优化。所有创建完成的submap以及当前的laser scan都会用作回环检测的scan matching。如果当前的scan和所有已创建完成的submap在距离上足够近,则进行回环检测。这里为了减少计算量,提高实时回环检测的效率,Cartographer应用了branch and bound(分支定界)优化方法进行优化搜索。如果得到一个足够好的匹配,则会将该匹配的闭环约束加入到所有Submap的姿态优化上。

四、LOCAL 2D SLAM

    Cartographer结合了local 和 global 两种方式进行2d SLAM。

    local方式就是前端匹配中通过submap进行scan matching。

    global方式就是回环检测,因为是对全局的submap进行匹配

    scans中的每个点束的姿态被表示为,\large \xi =\left (\xi _{x}, \xi _{y},\xi _{\Theta }\right )

A、Scans

    一个scans即激光点云图,包含一个起点和许多的终点。起点称为origin,终点称为scan points,用 H 表示点云集,其表达形式如下。

                                                            \large H=\left \{ h_{k} \right \}_{k=1,...,K}

    当获得一个新的scans,并且要插入到submap中时,scans中\large \left \{ h_{k} \right \}点集在submap中的位置被表示成\large T_{\xi } ,其转换公式如下:

B、Submaps

    一个submap是通过几个连续的scans创建而成的,由5cm*5cm大小的概率栅格\large \left [ p_{min},p_{max} \right ] 构造而成,submap在创建完成时,栅格概率小于\large p_{min}表示该点无障碍,在\large p_{min}\large p_{max}之间表示未知,大于\large p_{max}表示该点有障碍。每一帧的scans都会生成一组称为hits的栅格点和一组称为misses的栅格点 ,如图所示。

    其中阴影带叉的表示hits,阴影不带叉的表示misses,每个hits中的栅格点被赋予初值\large p_{hits},每个misses中的栅格点被称为\large p_{miss},如果该栅格点在先前已经有\large p值,则用下述对该栅格点的值进行更新(此处为\large p_{hits}\large p_{miss}类似)。

    其中clamp是区间限定函数。

C、Ceres scan matching

    每次获得的最新的scan需要插入到submap中最优的位置,使我们scan中的点束的位姿经过转换后落到submap中时,每个点的信度和最高。通过scan matching对\large T_{\xi }h_{k}进行优化,这里的优化问题为解最小二乘问题,其问题描述可表示为:

    其中\large \large M_{smooth} 是线性评价函数,方法为双三次插值法,该函数的输出结果为(0,1)以内的数,在这之外的数可以生成,但不被考虑进去,通过这种平滑函数的优化,能够提供比栅格分辨率更好的精度。该最小二乘问题在cartogrper中通过google自家的Ceres库进行求解。

五、Closing Loops

    Cartographer通过创建大量的submap来实现大场景建图,submap在短时间内的准确度是可靠的,但长时间会存在累积误差,为了消除累积误差,需要通过回环检测来优化所有submap的位姿。

A、Optimization problem

    回环的优化问题同样为非线性最小二乘问题,其问题描述可表示为:

   其中

    Ξm \large =\left \{ \xi _{i}^{m} \right \}_{i=1,...,m} submap的位姿

    Ξs\large =\left \{ \xi _{j}^{s} \right \}_{j=1,...,s} scan的位姿

   这些位姿是在世界坐标系下的。submap位姿和scan位姿之间存在约束条件

   \large \xi _{i}^{j}表示scansubmap坐标系下的位姿,描述scan在哪一个submap坐标匹配,\large \Sigma _{i}^{j}是相应的协方差矩阵

    残差E的计算公式如下:

    个人理解:

    首先回环优化,我们需要检测到回环,再进行优化。如何检测回环呢,前文也提到过,如果当前的scan和所有已创建完成的submap中的某个laser scan的位姿在距离上足够近,那么通过某种 scan match策略就会找到该闭环。这里为了减少计算量,提高实时回环检测的效率,Cartographer应用了branch and bound(分支定界)优化方法进行优化搜索,如果得到一个足够好的匹配,到此处,回环检测部分已经结束了,已经检测到了回环得存在。接下来要根据当前scan的位姿和匹配到得最接近的submap中的某一个位姿来对所有的submap中的位姿进行优化,即使残差E最小。回环检测与回环优化过程中scan和submap的关系如图所示:

    即优化local中的submap与gloab中的submap之间的位姿的误差,又因为所有的submap位姿存在着约束,因此,即是对所有submap的位姿进行优化,使error最小。

B、Branch-and-bound scan matching

    回环检测即是一种匹配过程,即当获得新的scan时,在其附近一定范围搜索最优匹配帧,若该最优匹配帧符合要求,则认为是一个回环。首先,该匹配问题可以描述为如下式子:

    其中W是搜索空间,\large \large M_{nearest}是该点对应的栅格点的M值,该式子可理解为对于scan中的每一个点束插在该submap上时的信度和,信度越高则认为越相似,我们需要在W空间中寻找出该信度和最大的匹配帧。

    因此需要在W空间中寻找出pixel-accurate match的最优解。

    显然有一种方法是暴力匹配法,即在搜索空间范围内中的每一帧与当前帧进行计算BBS式子的数值,求出最大值。

    对于暴力匹配法来说,该方法的搜索步长为1。我们假定搜索空间W

                                                          

    步长 r=1 ,因此

                                                              \large w_{x}=W_{x}        \large w_{y}=W_{y}       \large w_{\theta }=W_{\theta }

                                                  \large \overline{W}=\left \{ -w_{x},...,w_{x} \right \}*\left \{ -w_{y},...,w_{y} \right \}*\left \{ -w_{\theta },...,w_{\theta } \right \}

     其中W_{x}=W_{y}=7m ,搜索空间是7m*7m

     暴力匹配的 algorithm 如下 ,这样逐个遍历的方法显然是缓慢的                                       

*branch and bound

    为了提高搜索效率,Cartogrpher采用了branch and bound(分支定界) 的方法,本人研一课程高等运筹学也运用了该方法求解整数规划问题,在大作业中,我也运用该方法求解了JSP问题,主要思路来源也是参考Cartogrpher这部分算法的源代码。首先我简单讲解下 branch and bound 对该方法比较熟悉的读者可跳过。

    分枝界限法是由三栖学者查理德·卡普(Richard M.Karp)在20世纪60年代发明,成功求解含有65个城市的旅行商问题,创当时的记录。“分枝界限法”把问题的可行解展开如树的分枝,再经由各个分枝中寻找最佳解。

    其主要思想:把全部可行的解空间不断分割为越来越小的子集(称为分支),并为每个子集内的解的值计算一个下界或上界(称为定界)。在每次分支后,对凡是界限超出已知可行解值那些子集不再做进一步分支。这样,解的许多子集(即搜索树上的许多结点)就可以不予考虑了,从而缩小了搜索范围。

    或许有些读者还是不太理解,下面贴一张图进行讲解。

    这张图就已经比较好的描述了分支定界的思想,还有它为什么能够缩小搜索范围的情况下依然能求到最优解。若先前提到的暴力匹配是枚举法,则分支定界是一种隐式枚举法,。

    分支定界进行分支的过程,是不断提高搜索精度的过程,或者可以说增加约束的过程,整个分支树的最底层的所有枚举情况,便是最高搜索精度的枚举集合,便是暴力匹配搜索范围的全部整搜索空间。但是分支定界并没有真正的去求解所有枚举情况的目标函数(BBS)值。

    再来看这张图假设我们需要去计算检测匹配的点为如图所示16个

  • 则我们第一层搜索精度最低,只列举其中两个,并优先考虑靠左(优先考虑可能性最高的)。
  • 对其继续分层,将其精度提高一倍,又可以列举出两个,并优先考虑靠左。
  • 这样直至最底层,计算出该情况下的目标函数BBS(值),最左的底层有两个值A和B,我们求出最大值,并将其视为best_score
  • 然后我们返回上一层还未来得及展开的C,计算C的目标函数BBS(值)并让它与best_score比较,若best_score依旧最大,则不再考虑C,即不对其进行分层讨论。
  •  若C的目标函数BBS(值)更大,则对其进行分层,计算D和E的值,我们假设D值大于E,则将D与best_score对比
  •  若D最大,则将D视为best_score,否则继续返回搜索。

    将此算法应用在我们的回环检测中,现已知我们的搜索范围(搜索最高精度,最底层),设置步长来对该问题进行优化搜索。

   a, 首先计算顶层线性搜索空间:

    假设 W_{x} = 1000 w_{x} = W_{x}/2^{depth}

    其中depth是自己定义的,如果是8 顶层步长 r=2^{depth}=256

    那么顶层的线性搜索空间大小就是1000/256,大约为4

    角度搜索步长由cell大小(1 pixel),和scan的最大扫描距离决定,大约为300

    所以顶层candidate数量是:4×4×300

    论文中用下面两个公式规定了步长的范围

   b,计算其余层结构

   一个candidate可构建四个子Candidate

    角度参数不变

    对x_index_offsety_index_offset加入新的偏移量

    C层搜索空间:

 

    c,搜索算法

     1,构建顶层C0,计算所有C0 target 得分,从大到小排序

    2C0 \rightarrow C1 ,计算这四新的C1target 得分,从大到小排序

    3,重复步骤 2 ,直到 depth = Max , 得到最底层的四个Cdepth 并计算target得分,得分最高的作为 best_score

    4,返回倒数第二层,将best_score与剩下三个Cdepth-1 比较,若best_score大于任何一个得分,则无须进入其它分支,继续返回best_score

    5,重复 4 直到遍历整个分支树 返回的结果为最优结果

  

    论文中的伪代码如下:

下面贴出Cartogrpher branch and bound 的核心源代码和部分注释

Candidate2D FastCorrelativeScanMatcher2D::BranchAndBound(
    const std::vector<DiscreteScan2D>& discrete_scans,
    const SearchParameters& search_parameters,
    const std::vector<Candidate2D>& candidates, const int candidate_depth,
    float min_score) const {
  if (candidate_depth == 0) {
    // Return the best candidate.
    return *candidates.begin();
  }

  Candidate2D best_high_resolution_candidate(0, 0, 0, search_parameters);//讨论分层并计算目标函数值
  best_high_resolution_candidate.score = min_score;//更新下best score
  for (const Candidate2D& candidate : candidates) { //在分支定界中for循环用来分层
    if (candidate.score <= min_score) { //若该值不优,则减枝
      break;
    }
    std::vector<Candidate2D> higher_resolution_candidates;
    const int half_width = 1 << (candidate_depth - 1);
    for (int x_offset : {0, half_width}) {
      if (candidate.x_index_offset + x_offset >
          search_parameters.linear_bounds[candidate.scan_index].max_x) {
        break;
      }
      for (int y_offset : {0, half_width}) {
        if (candidate.y_index_offset + y_offset >
            search_parameters.linear_bounds[candidate.scan_index].max_y) {
          break;
        }
        higher_resolution_candidates.emplace_back(
            candidate.scan_index, candidate.x_index_offset + x_offset,
            candidate.y_index_offset + y_offset, search_parameters);
      }
    }
    ScoreCandidates(precomputation_grid_stack_->Get(candidate_depth - 1),
                    discrete_scans, search_parameters,
                    &higher_resolution_candidates);
    best_high_resolution_candidate = std::max(
        best_high_resolution_candidate,
    //下面利用递归,继续搜索
        BranchAndBound(discrete_scans, search_parameters,
                       higher_resolution_candidates, candidate_depth - 1,
                       best_high_resolution_candidate.score));
  }
  return best_high_resolution_candidate;
}

    

    后面是一些结果对比,大家自行看论文。

 

结语

    从18年11月将该算法搞懂到现在陆陆续续的更新,今天才全部编写完,很多地方还是不是理解的特别的到位,希望感兴趣的可以指正错误,或者一起讨论见解和想法,也希望本人研究生毕业能在此基础上发表一些创新且实用的slam方法。

 

©️2020 CSDN 皮肤主题: 大白 设计师:CSDN官方博客 返回首页