Google Cartographer SLAM 原理

本文前言

————————————————
版權聲明：本文爲CSDN博主「夢凝小築」的原創文章，遵循 CC 4.0 BY-SA 版權協議，轉載請附上原文出處鏈接及本聲明。
原文鏈接：https://blog.csdn.net/weixin_36976685/article/details/84994701

本人的研究方向爲激光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中的每個點束的姿態被表示爲，

A、Scans
一個scans即激光點雲圖，包含一個起點和許多的終點。起點稱爲origin，終點稱爲scan points，用 H 表示點雲集，其表達形式如下。

當獲得一個新的scans，並且要插入到submap中時，scans中點集在submap中的位置被表示成 ，其轉換公式如下：

B、Submaps
一個submap是通過幾個連續的scans創建而成的，由5cm*5cm大小的概率柵格 構造而成，submap在創建完成時，柵格概率小於表示該點無障礙，在與之間表示未知，大於表示該點有障礙。每一幀的scans都會生成一組稱爲hits的柵格點和一組稱爲misses的柵格點 ,如圖所示。

其中陰影帶叉的表示hits,陰影不帶叉的表示misses,每個hits中的柵格點被賦予初值,每個misses中的柵格點被稱爲,如果該柵格點在先前已經有值，則用下述對該柵格點的值進行更新（此處爲，類似）。

其中clamp是區間限定函數。

C、Ceres scan matching
每次獲得的最新的scan需要插入到submap中最優的位置，使我們scan中的點束的位姿經過轉換後落到submap中時，每個點的信度和最高。通過scan matching對進行優化，這裏的優化問題爲解最小二乘問題，其問題描述可表示爲：

其中 是線性評價函數，方法爲雙三次插值法，該函數的輸出結果爲（0，1）以內的數，在這之外的數可以生成，但不被考慮進去，通過這種平滑函數的優化，能夠提供比柵格分辨率更好的精度。該最小二乘問題在cartogrper中通過google自家的Ceres庫進行求解。

五、Closing Loops

Cartographer通過創建大量的submap來實現大場景建圖，submap在短時間內的準確度是可靠的，但長時間會存在累積誤差，爲了消除累積誤差，需要通過迴環檢測來優化所有submap的位姿。

A、Optimization problem
迴環的優化問題同樣爲非線性最小二乘問題，其問題描述可表示爲：

其中
Ξm 是submap的位姿
Ξs 是scan的位姿
這些位姿是在世界座標系下的。submap位姿和scan位姿之間存在約束條件
表示scan在submap座標系下的位姿，描述scan在哪一個submap座標匹配，是相應的協方差矩陣
殘差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是搜索空間，是該點對應的柵格點的M值，該式子可理解爲對於scan中的每一個點束插在該submap上時的信度和，信度越高則認爲越相似，我們需要在W空間中尋找出該信度和最大的匹配幀。
因此需要在W空間中尋找出pixel-accurate match的最優解。
顯然有一種方法是暴力匹配法，即在搜索空間範圍內中的每一幀與當前幀進行計算BBS式子的數值，求出最大值。
對於暴力匹配法來說，該方法的搜索步長爲1。我們假定搜索空間W

步長$r=1$ ,因此
$w_x = W_x, x_y = W_y, x_z = W_z$

其中$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，否則繼續返回搜索。
  將此算法應用在我們的迴環檢測中，現已知我們的搜索範圍（搜索最高精度，最底層），設置步長來對該問題進行優化搜索。

論文中的僞代碼如下：

下面貼出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方法。

————————————————
版權聲明：本文爲CSDN博主「夢凝小築」的原創文章，遵循 CC 4.0 BY-SA 版權協議，轉載請附上原文出處鏈接及本聲明。
原文鏈接：https://blog.csdn.net/weixin_36976685/article/details/84994701