Cartographer三部曲（三）：Real-Time Loop Closure in 2D LIDAR SLAM

Real-Time Loop Closure in 2D LIDAR SLAM

主要貢獻：提出一種實時運行的激光slam，精度能達到5cm，前端採用CSM後端採用SPA，並採用分枝定界算法添加回環檢測功能。採用構建submap的形式使得系統能夠在大型環境中運行。

前端

scan

每當獲取到一幀掃描數據後快速的與子地圖進行匹配得到相對的座標變換關係
$T_{\xi} p=\underbrace{\left(\begin{array}{cc} \cos \xi_{\theta} & -\sin \xi_{\theta} \\ \sin \xi_{\theta} & \cos \xi_{\theta} \end{array}\right)}_{R_{\xi}} p+\underbrace{\left(\begin{array}{c} \xi_{x} \\ \xi_{y} \end{array}\right)}_{t_{\xi}}$

submaps

子地圖的表達 連續的幾幀激光數據構成子地圖（submap）。子地圖採用概率柵格地圖表達，首先將連續空間按照設定的精度柵格化，每一個柵格中都含有一個值代表該柵格被佔據的概率。

插入子地圖 計算擊中和錯過的柵格點，對這些點的概率按照如下公式進行更新。
$\begin{aligned} \operatorname{odds}(p) &=\frac{p}{1-p} \\ M_{\text {new }}(x) &=\operatorname{clamp}\left(\text { odds }^{-1}\left(\operatorname{odds}\left(M_{\text {old }}(x)\right) \cdot \operatorname{odds}\left(p_{\text {hit }}\right)\right)\right) \end{aligned}$

Ceres scan matching

調用Ceres庫進行幀間匹配的優化，其中誤差函數爲：

$\underset{\xi}{\operatorname{argmin}} \sum_{k=1}^{K}\left(1-M_{\mathrm{smooth}}\left(T_{\xi} h_{k}\right)\right)^{2}$
//TODO 文中並未對此過多的解釋，需要在源碼中進一步解讀

由於優化方法得到的是局部極值，因此初值的確定便顯得尤爲重要，在論文中採用IMU的值作爲初值可以提高精度。

迴環

迴環是整個論文的核心創新點，思路在於採用SPA對所有的子地圖和scan數據進行優化，並不斷檢測當前幀和一個窗口內的地圖的匹配程度若匹配程度高則達成迴環條件將結果立即加入優化中進行迴環。

優化問題

優化採用SPA方法，在第二篇論文中有詳細的介紹。

分枝定界算法

在一個窗口內尋找地圖和scan的最佳匹配是非常耗時的，因此採用分支定界算法對此進行加速從而達到了實時計算的效果。

節點選擇

分支定界算法將搜索區域採用樹的方式表達，對樹的搜索採用深度優先的準則，即在所有節點中每次都訪問得分最高的節點。

算法流程

根據節點的選擇可以得到如下的算法流程：

其中關鍵在於

• 將父節點分割成多個子節點
• 計算父節點的得分從而保證找到最優值

分支規則

將每個區域平分爲四塊，不斷的重複這個過程知道達到要求精度爲止。

計算得分上界

若是葉節點只需要直接計算得分即可，若非葉節點則需要滿足父節點得分不小於所有任意子節點的得分，這樣才能保證找到全局最優解。

爲了簡化計算，我們求父節點得分可以求在父節點代表的區域內每個激光得分的最大值之和。

$\begin{aligned} \operatorname{score}(c) &=\sum_{k=1}^{K} \max _{j \in \overline{\mathcal{W}}_{c}} M_{\text {nearest }}\left(T_{\xi_{j}} h_{k}\right) \\ & \geq \sum_{k=1}^{K} \max _{j \in \mathcal{W}_{c}} M_{\text {nearest }}\left(T_{\xi_{j}} h_{k}\right) \\ & \geq \max _{j \in \overline{\mathcal{W}}_{c}} \sum_{k=1}^{K} M_{\text {nearest }}\left(T_{\xi_{j}} h_{k}\right) \end{aligned}$

同時爲了加快速度會預先計算好預匹配地圖，即計算柵格地圖中每個點被測量爲佔用的概率，後續計算僅需要查表即可得到概率。