Real-Time Loop Closure in 2D LIDAR SLAM 翻譯和總結（一）

Hess W , Kohler D , Rapp H , et al. Real-Time Loop Closure in 2D LIDAR SLAM[C]// 2016 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2016.
此篇論文是谷歌發表，主要講了cartographer激光SLAM的實時迴環方法，以及大致框架。

摘要

便攜式激光測距儀，尤其是激光雷達LIDAR，以及SLAM技術，是一種有效獲取建築平面圖的工具和方法。實時生產和可視化建築平面圖可以幫助人們實時評估數據的質量和覆蓋範圍。基於有限的計算資源構建這樣的便攜式數據採集和處理平臺是非常有必要的。作者提出一種揹包建圖平臺，可以以5cm的分辨率實現實時建圖和迴環。爲了實現實時的迴環，文中使用了一種稱爲分支定界的方法計算scan-to-submap的匹配，並作爲約束條件。文中與其他方法進行了實驗對比，在性能方面，該方法相對現有方法還是具有較大優勢。

1. 引言

建築平面圖在大多數場合都非常有用。調查顯示一般採集建築平面圖都是基於當初的建築設計時的CAD圖紙和激光尺測量來進行。這些方法不僅慢，而且因爲直線的採集通過人的主觀先驗確定，這些不總是能夠精確描述空間的真實特徵。使用SLAM，使得快速精確測量建築尺寸和複雜度成爲可能，而且這相對於手工測量可以降低好幾個數量級的時間消耗。
在此領域使用SLAM並不是非常新的想法，論文的重點也不在於此。論文的主要貢獻是提出了一種基於激光雷達數據減小計算迴環的時間複雜度的新方法。這種方法可以用來對非常大的建築進行建圖，可達上萬平方米，同時可以實時的優化整個結果。

2.研究現狀

scan-to-scan匹配一般被很頻繁的用在激光SLAM中進行相對位姿估計，比如[1]-[4]。就其本身而言，然而，scan-to-scan匹配很快就會產生累計誤差。
scan-to-map匹配可以幫助限制誤差的積累。文獻[5]中使用高斯-牛頓在一個線性插值地圖中尋找局部最優匹配。在缺乏比較好的初始先驗的情況下，通過高幀率的激光雷達數據來作爲先驗，局部優化的scan-to-map方法是有效且魯棒的。在不穩定平臺上，基於IMU將激光束投影到水平面上實現重力的估計。
像素級的掃描匹配方法，比如[1]中，可以進一步減小局部誤差積累。儘管計算消耗更大，這種方法也可以有效用來進行迴環檢測。[4]中作者嘗試通過在激光點雲中提取特徵來匹配，從而減小計算量。其他用於迴環檢測的方法還有直方圖匹配[6]，掃描點雲中的特徵檢測，以及機器學習[7]。
兩種常用來進一步減小殘餘局部誤差積累的方法分佈是粒子濾波和圖優化SLAM[2], [8]。
粒子濾波必須在每一個粒子中維護一個全局系統狀態的表示。對於基於網格的SLAM，這很快會隨着地圖的變大而增大資源的消耗，測試用例是用3km的軌跡採集22000$m^2$的的場地。[9]中使用更小維度的特徵表示，其不要求每一個粒子對應一個grid map，可以進一步減小資源消耗。當需要存在一個持續更新的grid map時，[10]中建議計算submaps，只有在必要的時候才更小地圖，這樣final map就是所有子圖柵格化的表示。
基於圖優化的方法整個過程構建一系列節點，用來表示位姿和特徵。圖中的邊來自觀測構成的約束。好多不同的方法可以用來基於所有的約束來最小化整體誤差，比如[11], [12]。[13]中描述了一種用於戶外的SLAM系統，其基於圖優化SLAM，採用局部scan-to-sacn匹配，基於子圖特徵的直方圖實現局部重疊地圖的匹配。

3.系統概述

谷歌的cartographer提供了一種使用揹包設備進行實時室內建圖的解決方案，可以生成分辨率爲5cm的2D柵格圖。在建築中行走的過程中，可以看到系統是如何逐漸進行建圖的。假設在短期內，基於當前激光雷達掃描匹配估計的最優位姿是足夠準確的，然後基於此位姿將當前激光掃描幀插入到一個submap中。掃描匹配是相對一個最近的submap進行，因此其只依賴於最近的幾幀掃描，所以在世界座標系中的位姿估計誤差會逐漸累積。
爲了在一般的硬件上實現比較好的性能，cartographer沒有采用粒子濾波。爲了處理世界座標系下的誤差累積，間隔性地運行一次位姿優化（pose optimization ）。當一個submap構建完成時，不會再有新的laser scan被插入其中，只會在迴環優化中用來進行掃描匹配。所有已經構建完成的submaps以及scans都會自動用來進行迴環檢測和優化。如果他們基於當前的位姿估計距離足夠近，則基於一個掃描匹配會嘗試在此submap中去找對應的最匹配的scan。如果在當前估計的位姿附近的搜索窗口裏找到了一個比較好的匹配結果，則其會被作爲一個迴環約束（loop closing constraint）加入到圖優化問題中。通過每隔幾秒完成一次優化，採集人員的體驗就是當到達一個重複位置時，這個迴路會立刻閉合。這會導致一個淺實時約束，迴環的掃描匹配需要比新的scan插入的更快，否則這種現象感官上就會明顯滯後。cartographer通過一個分支定界方法和對每個已完成的submap進行網格預計算來實現快速回環。

4.local 2D SLAM

系統將獨立的local方法和global方法整合到一起形成2D SLAM系統。兩種過程都是基於激光雷達的觀測信息優化包含2個$(x, y)$位移和1個旋轉$\theta$的pose $\xi = (\xi_x, \xi_y, \xi_{\theta})$，優化完位姿的激光雷達觀測信息進一步會作爲激光掃描建圖結果（scans）在一個不穩定的平臺上，比如作者們使用的揹包平臺，使用和激光雷達固定在一個水平面上的IMU將優化完成的scans投影到2D水平世界座標系中。
在local方法中，每一幀連續的掃描都會與一個子圖（稱爲submap M）使用非線性優化進行匹配，從而對齊當前scan與submap，此過程進一步也被稱爲掃描匹配（scan matching）。Scan mathching隨着時間累積的誤差會在global方法中進行消除，這在下一部分進行敘述。

4.1 Scans

Submap的構建過程是重複對齊scan和submap座標系的迭代過程，並進一步插入作爲submap裏的掃描幀。單掃描幀的原點是（0， 0），令激光掃描幀裏的激光點集合爲$H=\{h_k\}_{k=1,...K}, h_k \in R^2$。在submap裏的掃描幀的pose爲$\xi$可以表示爲變換$T_\xi$，其可以將掃描幀裏的掃描點從scan座標系（激光雷達座標系）轉換到submap座標系。

4.2 Submaps

幾個連續的scans構建成一個submap。這些submaps以概率柵格的形式表示$M$：$r*r$–>$[p_{min}, p_{max}]$，此柵格地圖基於給定分辨率$r$的離散化網格點進行構建， 比如5cm。這些概率值可以被認爲是一個柵格點是否有障礙的概率。對每一個柵格點，定義關聯的$pixel$，其包含所有最靠近該柵格點的所有點此處的最靠近的範圍如何定義？下圖是什麼意思？pixel表示4領域柵格嗎。

每當一個scan被插入概率柵格中時，都會計算一組hits柵格點和另一組不相交的misses柵格點集。對於每個hit, 將距離最近的柵格點也插入到hit集合中。對於每個miss, 將其每個pixel關聯的柵格點插入其中，該柵格點與scan原點和每個scan點之間的射線相交（激光點射線會穿過這些miss柵格點），這裏不包括已經插入的hit集合。如果之前未被觀測過的柵格屬於hit或者misses集合，則會被分配一個概率$p_{hit}$
或者$p_{miss}$。如果柵格$x$已經被觀測過，則按照下面公式更新其對應的概率值：
$odd(p) = p/(1-p) \tag2$
$M_{new}(x) = clamp(odds^{-1}(odds(M_{old(x)})*odd(p_{hit}))) \tag3$
對於misses的更新同上，clamp表示區間限定函數，將新的概率值限定在$[p_{min}, p_{max}]之間$。

4.3 Ceres scan matching

在將一個掃描scan插入到一個submap中之前，使用基於Ceres的掃描匹配器（scan mathcher）優化求解該掃描幀相對當前局部local submap的位姿$\xi$。掃描匹配器負責找到submap中能夠最大化當前掃描點概率的位姿。作者將此此過程構建爲如下所示的非線性最小二乘問題：
$argmin\sum_{k=1}^K(1-M_{smooth}(T_{\xi}h_k))^2 \tag {CS}$
上式中$T_\xi$基於掃描幀的相對位姿將掃描幀點雲$h_k$從激光雷達掃描座標系轉換到submap座標系， K表示當前掃描幀的所有掃描點數量。函數$M_{smooth}：R^2->R$是局部submap概率值的平滑函數，此處使用雙三次插值算法（即對轉換完成的submap下的位姿附近的點進行概率查找，然後基於此函數進行概率平滑得到submap中的匹配概率值），其輸出在[0, 1]之外的值會被剔除。
基於此平滑函數的數學優化一般可以的得到優於柵格分辨率的位姿精度。因爲這是一個局部優化，需要要求有一個比較好的初始估計值。IMU可以用來估計掃描匹配時位姿的旋轉角分量$\theta$，並作爲初始估計值。當沒有IMU時，可以通過提高掃描匹配的頻率，或者進行像素級的掃描匹配（比如ICP，ICL等）方法來進行位姿估計，但是會增加計算消耗。

5.迴環

因爲掃描幀scans只相對於包含幾個最近scans的一個submap進行匹配，所以上述的local方法總會緩慢的產生誤差積累。對於幾十個連續的scans來說，這種累積誤差非常小。
當處理更大的空間時，會產生很多小的submap。進一步，作者提出全局方法，基於$Sparse Pose Adjustment$對所有submaps和scans的位姿進行優化。scans被插入到submap中的相對位姿會被存下來，在迴環優化的時候使用。除此之外，一旦submap不再更新，則所有的（one scan, one submap）對都會被考慮進迴環中。一個幀掃描器scan matcher會一直在後端運行，如果發現了一個匹配度很高的候選幀，則對應的相對位姿（對應的匹配到的候選幀的相對位姿作爲當前幀的位姿觀測加入？）會被加入到優化問題中。

5.1 優化問題

迴環優化，與scan matching也類似，爲了將額外的其他數據考慮進去，也構建了一個運行增加殘差進去的非線性最小二乘問題。每過幾秒或者幾個節點，則使用$Ceres$基於下式計算一個結果：
$argmin({\Xi}^m, \Xi^s) = \frac{1}{2}\Sigma_{ij}\rho(E^2(\xi_i^m, \xi_j^s; \Sigma_{ij}, \xi_{ij})) \tag{SPA}$
上式中世界座標系下的submap位姿$\Xi^m=\{{\xi_i^m}\}_{i=1, ..., m}$，scan位姿$\Xi^s=\{{\xi_j^s}\}_{i=1, ..., n}$，都會基於添加的約束constraints進行優化。 這些約束由submap與scan之間的相對位姿$\xi_{ij}$和關聯的協方差$\Sigma_{ij}$組成（在local方法構建submap的過程中產生，還有一些在global方法迴環檢測部分產生）。對於一對兒submap $i$ 和 scan $j$， 位姿$\xi_{ij}$描述了在該submap座標系下這個scan被匹配到的最優位置（前端Ceres Scan Matching得到）。協方差$\Sigma_{ij}$可以根據文獻[15]中所述方法進行計算，也可以在local方法中基於公式（CS）使用Ceres的協方差估計特點進行計算。對於這樣一個約束，殘差$E$可以通過下式計算：
$E^2(\xi_i^m, \xi_j^s; \Sigma_{ij}, \xi_{ij})) = e(\xi_i^m, \xi_j^s; , \xi_{ij})^T\Sigma_{ij}^{-1}e(\xi_i^m, \xi_j^s; , \xi_{ij}) \tag4$
$e(\xi_i^m, \xi_j^s; , \xi_{ij}) = \xi_{ij} - \left(\begin{matrix} R^{-1}_{\xi_i^m}(t_{\xi_i^m} - t_{\xi_j^s}) \\ \xi_{i;\theta}^m-\xi_{j:\theta}^s \end{matrix} \right) \tag5$
損失函數$\rho$目的時用來減少外點的影響，比如使用Huber loss，當掃描匹配在優化問題里加入了不正確的約束時，容易在SPA裏出現這種較大的外點誤差。比如，這種情況會比較容易發生在對稱環境中，比如辦公室隔間，其他處理外點的方法可參見[16]。
接下篇（Real-Time Loop Closure in 2D LIDAR SLAM 翻譯和總結（二））