Real-Time Correlative Scan Matching 翻譯和總結

Olson E B . Real-time correlative scan matching[C]// Robotics and Automation, 2009. ICRA '09. IEEE International Conference on. IEEE, 2009.
此爲谷歌開源的cartographer激光雷達掃描系統的參考文獻之一，系統中前端的scan to submap和後端的迴環優化部分都使用correlation scan matching作爲laser scan的匹配思想和策略。屬於SLAM中圖優化部分的數據關聯部分，也就是基於laser scan的匹配結果得到兩幀激光數據之間的位姿變換，同時可得到機器人的局部位姿（translation and rotation）。

摘要

通過激光雷達記錄的兩幀激光掃描數據得到兩幀之間相對位置關係的掃描匹配（Scan Matching）是移動機器人工作中的重要環節之一。作者認爲當時的一些算法爲了對計算性能進行拖鞋，採用了一些先驗的啓發式方法對問題進行求解。當然，這些啓發式的方法並不完美，特別是當先驗知識比較匱乏時，容易產生較差的結果。

1.概覽

該論文介紹了一類基於兩次相關激光掃描進行掃描匹配的方法。作者使用概率框架來處理此問題：找到一個剛體變換，能夠最大化已經觀測到的數據概率。此外，作者對整個疑似剛體變換區域均進行搜索，而不是信任一個局部搜索算法去找到全局最小值（當初始化存在噪聲時，該方法表現較差，作者後面會詳細列出結果）。該疑似區域可以基於命令驅動的運動或者輪式/視覺里程計作爲先驗來得到。

論文主要的貢獻是：

• 描述了一種相關掃描匹配方法的理論和實踐優勢
• 在傳統微處理器上實現了一種基於多分辨率匹配的實時處理
• 描述了將基於相關匹配的方法在GPU上實現的過程，解放CPU去處理其他任務
• 以常規的使用場景評估了論文提出的方法和其他三種不同的類似方法

作者認爲，論文所提方法的性能和魯棒性，以及可實時處理的特點可較好的適配於對魯棒性和精確度都有較高要求的機器人平臺。除了包括SLAM系統平臺外，任何導航和定位系統都可從中受益。

2.研究現狀

作者的工作與Konolige的相關定位方法很類似，並從中受益[15]。儘管所需要處理的問題公式描述是一致的，但是作者提出了新的解決方法。

3.實現方法

3.1 概率模型

作者根據圖2所示的圖模型建立掃描匹配問題的模型。機器人在運動$u$的驅動下，從$x_{i-1}$移動到$x_{i}$。觀測$z$與環境模型m和機器人的位置相互獨立。

目標是計算機器人位置的後驗分佈$p(x_i|x_{i-1}, u, m, z)$，應用貝葉斯法則並移除相關環境，可得：
$p(x_i|x_{i-1}, u, m, z) \propto p(z|x_i, m)p(x_i|x_{i-1}, u) \tag 1$
上式子中第一項是觀測模型，第二項是機器人的運動模型，可由控制輸入、輪速計或者imu等其他傳感器獲得。
儘管可以從多維高斯分佈得到運動模型，但是觀測模型則比較難計算，而且在結構上也更加複雜。觀測模型一般都會出現類似於圖1所示的多峯值現象。

本篇論文的核心貢獻便是爲了計算等式（1）中機器人的後驗位置分佈而提出的一種有效計算觀測模型分佈$p(z|x_i, m)$的方法。爲了找到$p(z|x_i, m)$的局部極值（爲了進一步找到最大似然結果），儘管之前已經提到使用爬山（hill-climbing）算法，但是本論文提出的方法將更加完善的對觀測概率分佈進行建模。作者認爲新方法可以獲得更魯棒的後驗概率估計和更合理的不確定度估計。
與之前的其他研究類似，作者同樣假設每個激光雷達掃描返回的$z_j$是獨立的，因此可以得到：
$p(z|x_i, m) = \prod_jp(z_j|x_i, m)$
對於一幀單個激光雷達的掃描$z_j$而言，其概率分佈可以認爲是從一個特殊的方位觀測得到的當前地圖m的可視表明。這便需要進行昂貴的射線投射來完成探測。與其他方法類似，作者忽略可見性和障礙物的影響，只考慮與地圖m表明的距離來估計$z_j$的概率。
在SLAM系統中，模型m可以基於之前的激光雷達觀測得到，也可以作爲已知的地圖先驗信息得到。

4.2 柵格查找表

$p(z|x_i, m)$的計算可以通過建立一個2D查找表來實現，通過預先計算每個世界座標系位置$(x, y)$處雷達觀測的對數概率查找表實現。
柵格化基於地圖m實現。對於地圖中的每個觀測點$m_i$，計算$m_i$處的觀測到附近點p的環境概率。對地圖中的每個點重複此過程，在查找表中記錄每個點的最大概率。由於查找表必須是視角獨立的，作者基於傳感器模型估計潛在香蕉分佈作爲一個徑向對稱分佈（Since our lookup table must be viewpoint independent, we approximate the potentially banana-shaped distribution arising from the sensor model (which has independent range and bearing noise) as a radially-symmetric distribution. 沒太理解）。可視化的查找表見圖3：

正如之前提到的，該模型m可以來源於預先已知的場景掃描地圖，也可以是基於上一步對環境的觀測進行估計。建圖也是一個很複雜的問題，一般要求對SLAM問題進行全部求解。
在作者的論文中，作者簡單的使用前一步的激光雷達掃描結果作爲模型（reference scan）。該方法的優點在於實現容易，魯棒（不會因爲與之前的數據關聯錯誤而發散），而且或許更實用，此方法作爲本論文所提方法的輔助方法進行使用。更加複雜的實現可以通過綜合多個雷達掃描（比如：CAMERA的Vasco）或者從雷達點中提取連續表面來建立細節更豐富的模型。

4.3 流程概述

作者的目的是通過等式（1）估計$p（z|x_i, m）$的分佈，該分佈可以用來得到後驗分佈$p（x_i|x_{i-1}, u, m, z）$。與其他方法不同的是，不知關注最大後驗估計，而且關注後驗分佈的本身，因此可以得到一個合理的測量不確定度。該分佈不可以用簡單的公式表達，需要用數值結果來評估。下面兩部分，將會描述兩種用於快速計算$p（z|x_i, m）$在$x_i$上的分佈。

4.4 多分辨率實現

爲了在傳統微處理器上使用該方法，此部分描述一種基於相關的快速匹配方法。因此傳統的微處理器對於大規模數據$p（z|x_i, m）$的計算很困難，因此本方法就是爲了解決此問題。要進行計算並最小化數值評估的計算量，需要先滿足1）在大尺度區域對分佈進行特徵化；2）精確定位最大概率值。下面分三步描述該方法，從一個經典的實現方法到多分辨率實現方法逐漸過渡。
1）暴力求解：從原理上講，需要在點的3D空間計算$p（z|x_i, m）$；三個維度對應未知T的三個參數：$\Delta x, \Delta{y}, \theta$。經典方法的實現包含3層嵌套循環；對每一個體素（voxel），都需要計算其概率並記錄。對每一個單個的體素，還涉及第4個循環，在待查詢scan裏迭代遍歷每個點，投影，並在查找表中找到其對應的概率誤差。這種方法特別慢，在後面的結果部分可見。
2）計算2D片：暴力求解比較慢的一個原因之一是對於每一個體素（待遍歷3維空間裏的一個小立方體，不同的體素對應不同的x，y，$\theta$）都需要對待查詢scan中的所有點進行投影。這是不太好的一種處理方式，因爲對於一個給定的方向角$\theta$, 被投影的待查詢點只是單純的與$\hat x, \hat y$搜索方向上的純位移相關。換句話說，可以通過在最外層迭代遍歷$\theta$，此表示待查詢點經過了合適的旋轉，在內部兩層（$\hat x, \hat y$）循環中簡單地對查詢點進行位移轉換（在xy尺度上不需要重複進行旋轉投影）。此操作還可以進一步進行加速，令位移變換搜索的步長大小與查找表的分辨率相一致，這樣只需要按索引移動便可以實現xy尺度上按步長迭代的位移搜索。在後面的結果部分中可見，該方法要顯著快於暴力搜索方法。
3) 多分辨率實現：最終基於CPU的實現方法是使用了兩個在不同分辨率上生成的查找表。

第一個是高分辨率（分辨率是3c m）, 第二個是低分辨率實現（30c m）。

在分辨率比較低時，在參考掃描幀裏的細節可能會丟失。因此，作者在計算低分辨率查找表時，將其值設定爲對應高分辨率柵格地圖中區域的最大值。這能夠確保在低分辨率地圖上計算出的概率總是至少與高分辨率地圖中的一樣大。換句話說，這能夠保證我們不錯過最大值。

整體方案策略是使用低分辨率圖快速確認可能或者不可能包含全局最大值的區域。目的是最小化高分辨率地圖裏的搜索範圍。方法具體步驟如下：

​ (1) 使用低分辨率查找表在整個3D搜索窗口進行遍歷和計算概率$p(z|x_{i}, m)$；

​ (2) 在尚未遍歷的3D空間裏查找最匹配的體素，定義此匹配分值爲$L_i$, 如果$L_i < H_{best}$, 則中斷當前遍歷，繼續遍歷下一個體素。此處$H_{best}$是最優的額掃描匹配對齊分值；

（3）在體素$i$內部基於高分辨率查找表再次進行搜索遍歷，假設當前體素的log概率是$H_i$。需要注意的是，$H_i <= L_i$，因爲低分辨率圖會對對數概率過擬合（因爲低分辨率查找表其值對應高分辨率查找表區域的最大值）。如果$H_i > H_{best}$，則令$H_{best}=H_i$。

這種多分辨率匹配方法非常快，使得實時相關掃描匹配成爲可能。正如結果部分所示，這結果也是非常魯棒。

4.5 GPU方法

GPU具有非常大的計算吞吐量，而且適合去進行批量計算$p(z｜x_i, m)$。作者認爲他們是第一個在機器人建圖和定位領域使用GPU的用戶。

作者將其實現寫成了一個OpenGL GLSL的着色程序，而不是像Nvidia的CUDA類似的特定程序。儘管這要求將問題轉換爲3維渲染操作（比如繪製紋理多邊形計算函數？，computing functions by drawing textured polygons），但是相關掃描匹配方法比較簡單，可以直接進行。

與CPU中的實現方法一致，作者還是對每一"片"都去固定$x_i$的方向，然後切片式的計算$p(z|x_i, m)$. 片段着色器以兩個紋理作爲輸入：一個1維數組，包含待查詢點；一個2維的紋理對應查找表。查找表的實現與CPU中也一致。

每個片段對一個特定的$x_i$計算$p(z|x_i, m)$，此值通過紋理座標傳遞到渲染器。多邊形中的每個片段根據在多邊形頂點指定的紋理座標自動接收線性插值紋理座標。換句話說，就是當我們將多邊形投射到屏幕上時，紋理座標一點也不與紋理對應，其通過片段着色器作爲應用於待查詢點的剛體運動變換來體現。

片段渲染器與傳統的CPU方法很類似：每個片段迭代遍歷待查詢點，根據局部$x_i$投影每一個點，接着在查找表中獲取log概率。渲染器簡單的將每個像素的log概率加在一起，並將結果輸出到幀緩衝器中。主機CPU可以接着檢查幀緩衝器，繼而計算最大似然概率結果。

在主機CPU端，爲每一個我們希望計算的$x_i$的方向圈定一個四邊形。一個單獨的四邊形對應一組固定方向下的位移。基於GPU的結果見圖1.需要主要的是，圖中的顏色編碼是使用了可視化輔助：每一個GPU輸出的像素值是個標量。

對GPU不是很懂，這段看的也不是很明白，寫的也不是很清楚。

4.6 計算協方差

在許多應用中，$x_i$的最大似然估計是有意義的。但是，作者同時也進行了一個合理的不確定度估計。

一旦在較大範圍對$x_i$值使用損失函數進行了計算，則可以基於此數據構建一個多維高斯分佈。令$x_i^{(j)}$是$x_i$ 的第$j$次計算評估：

基於計算的$p(z|x_i,m)$估計的掃描匹配的額不確定度考慮了大量的不確定量：包括傳感器自己的噪聲，待查詢點與模型的關聯誤差。該方法的缺點是高斯結果只根據已經進行過計算評估的數據採樣點進行擬合。任何還沒有被採樣的高概率區域也不會反應在高斯概率模型中，導致過擬合。因此，在更大的$x_i$範圍進行$p(z|x_i,m)$概率估計非常重要，這方面本文的方法考慮更周到。可以從圖5中兩種case看到本方法的協方差估計結果。

5. 實驗和結果

5.1 實驗設計

基於地圖中的已有的數據，再結合地圖信息進行仿真測試。

5.2 不同方法比較

1）ICP: The Iterative Closest Point（ICP）算法以1m的匹配限制爲條件，並以歐式距離作爲距離度量。變量是對稱的：對兩幀掃描中的每一個點，都在另一個掃描中去找最近點。比如，假設掃描幀a中的點$a_i$最接近的點是$b_j$, 而且在掃描b中的點$b_k$最接近點$a_i$。如果$b_i$與$a_j$的距離大於$b_k$的兩倍，則對應的（$a_i$, $b_i$）被刪除。$b_i$和$a_j$是什麼，難道不是$a_i$和$b_j$嗎這種優化方式可以幫助重新確認環境中的某一部分只在一個掃描幀中可見的情況，並剔除可能產生錯誤結果的遠距離對應關係。在上述的例子中，很可能$b_j$沒有對應的參考點。額外的對稱變量可以提供額外的約束關係，幫助減少匹配誤差。最後，基於找到的對應匹配點對使用**Horn’s算法[6]**計算優化剛體變換。

2） ICL：The Iterative Closest Line（ICL）算法在論文實驗中將待查詢點與最近的線段使用歐拉距離關聯。線段由不超過1m部分的連續點對生成。假設待查詢點$q_i$已經和一個線段$s$關聯起來了：則在$s$上與點$q_i$距離最近的點和$q_i$構成點對，繼而被傳遞到Horn’s算法中進行剛體變換計算。

3）爬山法：Hill-Climbing，本文所採用的爬山方法是在Vasco掃描匹配器之後建模的，該匹配器與CARMEN的分佈一致。這是一個沿着方向軸重複嘗試的局部搜索方法，如果匹配度提高則接受當前步長。初始步長相對比較大（x/y軸爲1m，$\theta$方向爲10度）；當沒有步長能夠使得誤差減小時，將步長大小減半。當步長大小到達預定的最小值時（x/y軸0.001m，$\theta$方向0.05度），搜索結束。與本文中方法一致，Vasco的優化也是在概率上，而且也使用了一個log概率查找表。爲了與本文的相關匹配方法進行直接比較，本文方法也使用相同的柵格查找表。

初始實驗包含大約25000次迭代，對每一次迭代，選擇新的位姿，並在之前的先驗估計中加入噪聲，並運行每個掃描匹配器。在圖4中，在z軸畫出來了平均誤差，同時x軸表示先驗的初始位置誤差, y軸表示先驗的旋轉誤差。（圖形對稱是因爲先驗的誤差可能是正的也可能是負的。）

一般情況下，超過幾釐米的誤差或者幾度都是不可接受的：這會導致機器人的定位誤差積累非常快。直觀而言，我們希望在誤差更大的結果裏反應出更大的先驗噪聲水平。ICP, ICL和爬山法的表現確實如此。還有值得注意的是，爬山法的誤差稍微比ICP的要高一些，甚至初始幾乎無噪聲的時候也要高一些。
本篇論文的核心結果是：針對先驗的噪聲，基於相關掃描匹配的的結果誤差更小而且更魯棒。付出的代價就是稍微多一些計算量，但是在下一部分可見，多出的計算量也是可以控制的。

重點：查找表的分辨率比較影響匹配的結果精度。作者對在0.001m~1m的查找表分辨率基於本文方法進行了誤差量化，發現當分辨率小於1.5cm時結果誤差已經沒有提升，分辨率爲3.1cm的只是稍微差一點。當分辨率大於6cm時，結果誤差會急速惡化。基於論文中的數據，作者推薦的柵格分辨率是3cm。

5.3 性能

本文提出的相關方法確實比ICL, ICP和爬山算法要慢一些，這應該沒什麼驚訝的（因爲本質上還是屬於遍歷的算法，其他三種屬於迭代的算法，相對來說複雜度低一些）。相關掃描匹配方法的初始目的就是爲了提升結果的質量。但是，基於相關掃描匹配的方法也夠實時使用（圖7）。實驗使用的CPU是Inter的Core2 6600，主頻2.4GHz。儘管CPU是多核的，但是實驗中只使用了一個線程。此外對跨兩代的GPU進行了評估，7600和Nvidia 7600GS以400MHz運行，以及GTX260以650MHz運行。柵格分辨率是1/32米，$\theta$的遍歷步長大小爲1度。

ICP和ICL的計算複雜度基於我們的實現，也表現很好（見圖7），爬山算法明顯速度佔優勢。儘管他們的計算複雜度隨初始不確定度的增大而增大，但是當初始不確定度較大時其結果幾乎完全錯誤（見圖4）。

相關掃描匹配方法的複雜度與先驗不確定度的三次方成比例增長。對於相對比較小的不確定度，上述幾種方法均基本可以實時實現。在這種情況下，搜索範圍隨着激光雷達掃描的頻率增大而縮小，因爲頻率更新越快，則機器人的定位誤差累積越小。比如更新頻率爲75Hz, 當車輛以37m/s的速度行駛時，需要設置0.5m的搜索窗口（37m/s*1/75s=0.5m）。方向角的搜索窗口類似，如果機器人的旋轉速度爲240rpm，則需要20度的搜索窗口。然而，即使是75Hz的更新頻率，多分辨率相關匹配算法也可以以足夠快的速度（8.4ms）滿足實時運算。

當應用在迴環中時，實時性相對來說不那麼要求苛刻，關鍵點是需要找到正確的對應關係。機器人只需要間斷性的完成一個大回環即可（或許沒幾分鐘一次）。在進行迴環操作時，往往會出現比較大的不確定度搜索窗口。如圖7所示，相關掃描匹配方法（特別是多分辨率實現方法）可以以10Hz處理4m的位移搜索窗口和90度的旋轉搜索窗口，作者認爲這這也可以適應迴環處理（個人覺得此處的例子不恰當，90度/4m的迴環搜索區域還是太小了）。

對於實時系統，運行時的動態性能也是一個比較大的挑戰。ICP、ICL，爬山算法以及多分辨率相關算法運行復雜度是數據獨立的。ICP和ICL在處理某些結果時話費了大約2.5倍長的時間（圖8）。單分辨率的相關方法表現更好，而GPU實現的表現還要差一些。這些不一致的表現只發生在小搜索窗口下：當搜索窗口比較大時，GPU方法的表現還是比較一致的。

結論

本篇論文提出一類基於相關的掃描匹配方法。該方法相對初始噪聲非常魯棒，而且確實比當時的現有方法表現更好。作者在CPU和GPU上均可以實時實現該魯棒算法。

本文提出的算法是一種概率驅動算法，可以很方便的與其他概率先驗合併，也能夠比較方便的計算協方差估計。相關的工作可見http://april.eecs.umich.edu.。