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 u u的驅動下，從 x i − 1 x_{i-1} xi−1移動到 x i x_{i} xi。觀測 z z z與環境模型m和機器人的位置互相獨立。
目标是計算機器人位置的後驗分布 p ( x i ∣ x i − 1 , u , m , z ) p(x_i|x_{i-1}, u, m, z) p(xi∣xi−1,u,m,z)，應用貝葉斯法則并移除相關環境，可得：
p ( x i ∣ x i − 1 , u , m , z ) ∝ p ( z ∣ x i , m ) p ( x i ∣ x i − 1 , u ) (1) p(x_i|x_{i-1}, u, m, z) \propto p(z|x_i, m)p(x_i|x_{i-1}, u) \tag 1 p(xi∣xi−1,u,m,z)∝p(z∣xi,m)p(xi∣xi−1,u)(1)
上式子中第一項是觀測模型，第二項是機器人的運動模型，可由控制輸入、輪速計或者imu等其他傳感器獲得。
盡管可以從多元高斯分布得到運動模型，但是觀測模型則比較難計算，而且在結構上也更加複雜。觀測模型一般都會出現類似于圖1所示的多峰值現象。
本篇論文的核心貢獻便是為了計算等式（1）中機器人的後驗位置分布而提出的一種有效計算觀測模型分布 p ( z ∣ x i , m ) p(z|x_i, m) p(z∣xi,m)的方法。為了找到 p ( z ∣ x i , m ) p(z|x_i, m) p(z∣xi,m)的局部極值（為了進一步找到最大似然結果），盡管之前已經提到使用爬山（hill-climbing）算法，但是本論文提出的方法将更加完善的對觀測機率分布進行模組化。作者認為新方法可以獲得更魯棒的後驗機率估計和更合理的不确定度估計。
與之前的其他研究類似，作者同樣假設每個雷射雷達掃描傳回的 z j z_j zj是獨立的，是以可以得到：
p ( z ∣ x i , m ) = ∏ j p ( z j ∣ x i , m ) p(z|x_i, m) = \prod_jp(z_j|x_i, m) p(z∣xi,m)=j∏p(zj∣xi,m)
對于一幀單個雷射雷達的掃描 z j z_j zj而言，其機率分布可以認為是從一個特殊的方位觀測得到的目前地圖m的可視表明。這便需要進行昂貴的射線投射來完成探測。與其他方法類似，作者忽略可見性和障礙物的影響，隻考慮與地圖m表明的距離來估計 z j z_j zj的機率。
在SLAM系統中，模型m可以基于之前的雷射雷達觀測得到，也可以作為已知的地圖先驗資訊得到。
4.2 栅格查找表
p ( z ∣ x i , m ) p(z|x_i, m) p(z∣xi,m)的計算可以通過建立一個2D查找表來實作，通過預先計算每個世界坐标系位置 ( x , y ) (x, y) (x,y)處雷達觀測的對數機率查找表實作。
栅格化基于地圖m實作。對于地圖中的每個觀測點 m i m_i mi，計算 m i m_i mi處的觀測到附近點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（z|x_i, m） p（z∣xi,m）的分布，該分布可以用來得到後驗分布 p （ x i ∣ x i − 1 , u , m , z ） p（x_i|x_{i-1}, u, m, z） p（xi∣xi−1,u,m,z）。與其他方法不同的是，不知關注最大後驗估計，而且關注後驗分布的本身，是以可以得到一個合理的測量不确定度。該分布不可以用簡單的公式表達，需要用數值結果來評估。下面兩部分，将會描述兩種用于快速計算 p （ z ∣ x i , m ） p（z|x_i, m） p（z∣xi,m）在 x i x_i xi上的分布。
4.4 多分辨率實作
為了在傳統微處理器上使用該方法，此部分描述一種基于相關的快速比對方法。是以傳統的微處理器對于大規模資料 p （ z ∣ x i , m ） p（z|x_i, m） p（z∣xi,m）的計算很困難，是以本方法就是為了解決此問題。要進行計算并最小化數值評估的計算量，需要先滿足1）在大尺度區域對分布進行特征化；2）精确定位最大機率值。下面分三步描述該方法，從一個經典的實作方法到多分辨率實作方法逐漸過渡。
1）暴力求解：從原理上講，需要在點的3D空間計算 p （ z ∣ x i , m ） p（z|x_i, m） p（z∣xi,m）；三個次元對應未知T的三個參數： Δ x , Δ y , θ \Delta x, \Delta{y}, \theta Δx,Δy,θ。經典方法的實作包含3層嵌套循環；對每一個體素（voxel），都需要計算其機率并記錄。對每一個單個的體素，還涉及第4個循環，在待查詢scan裡疊代周遊每個點，投影，并在查找表中找到其對應的機率誤差。這種方法特别慢，在後面的結果部分可見。
2）計算2D片：暴力求解比較慢的一個原因之一是對于每一個體素（待周遊3維空間裡的一個小立方體，不同的體素對應不同的x，y， θ \theta θ）都需要對待查詢scan中的所有點進行投影。這是不太好的一種處理方式，因為對于一個給定的方向角 θ \theta θ, 被投影的待查詢點隻是單純的與 x ^ , y ^ \hat x, \hat y x^,y^搜尋方向上的純位移相關。換句話說，可以通過在最外層疊代周遊 θ \theta θ，此表示待查詢點經過了合适的旋轉，在内部兩層（ x ^ , y ^ \hat x, \hat y x^,y^）循環中簡單地對查詢點進行位移轉換（在xy尺度上不需要重複進行旋轉投影）。此操作還可以進一步進行加速，令位移變換搜尋的步長大小與查找表的分辨率相一緻，這樣隻需要按索引移動便可以實作xy尺度上按步長疊代的位移搜尋。在後面的結果部分中可見，該方法要顯著快于暴力搜尋方法。
3) 多分辨率實作：最終基于CPU的實作方法是使用了兩個在不同分辨率上生成的查找表。
第一個是高分辨率（分辨率是3c m）, 第二個是低分辨率實作（30c m）。
在分辨率比較低時，在參考掃描幀裡的細節可能會丢失。是以，作者在計算低分辨率查找表時，将其值設定為對應高分辨率栅格地圖中區域的最大值。這能夠確定在低分辨率地圖上計算出的機率總是至少與高分辨率地圖中的一樣大。換句話說，這能夠保證我們不錯過最大值。
整體方案政策是使用低分辨率圖快速确認可能或者不可能包含全局最大值的區域。目的是最小化高分辨率地圖裡的搜尋範圍。方法具體步驟如下：
(1) 使用低分辨率查找表在整個3D搜尋視窗進行周遊和計算機率 p ( z ∣ x i , m ) p(z|x_{i}, m) p(z∣xi,m)；
(2) 在尚未周遊的3D空間裡查找最比對的體素，定義此比對分值為 L i L_i Li, 如果 L i < H b e s t L_i < H_{best} Li<Hbest, 則中斷目前周遊，繼續周遊下一個體素。此處 H b e s t H_{best} Hbest是最優的額掃描比對對齊分值；
（3）在體素 i i i内部基于高分辨率查找表再次進行搜尋周遊，假設目前體素的log機率是 H i H_i Hi。需要注意的是， H i < = L i H_i <= L_i Hi<=Li，因為低分辨率圖會對對數機率過拟合（因為低分辨率查找表其值對應高分辨率查找表區域的最大值）。如果 H i > H b e s t H_i > H_{best} Hi>Hbest，則令 H b e s t = H i H_{best}=H_i Hbest=Hi。
這種多分辨率比對方法非常快，使得實時相關掃描比對成為可能。正如結果部分所示，這結果也是非常魯棒。
4.5 GPU方法
GPU具有非常大的計算吞吐量，而且适合去進行批量計算 p ( z ｜ x i , m ) p(z｜x_i, m) p(z｜xi,m)。作者認為他們是第一個在機器人建圖和定位領域使用GPU的使用者。
作者将其實作寫成了一個OpenGL GLSL的着色程式，而不是像Nvidia的CUDA類似的特定程式。盡管這要求将問題轉換為3維渲染操作（比如繪制紋理多邊形計算函數？，computing functions by drawing textured polygons），但是相關掃描比對方法比較簡單，可以直接進行。
與CPU中的實作方法一緻，作者還是對每一"片"都去固定 x i x_i xi的方向，然後切片式的計算 p ( z ∣ x i , m ) p(z|x_i, m) p(z∣xi,m). 片段着色器以兩個紋理作為輸入：一個1維數組，包含待查詢點；一個2維的紋理對應查找表。查找表的實作與CPU中也一緻。
每個片段對一個特定的 x i x_i xi計算 p ( z ∣ x i , m ) p(z|x_i, m) p(z∣xi,m)，此值通過紋理坐标傳遞到渲染器。多邊形中的每個片段根據在多邊形頂點指定的紋理坐标自動接收線性插值紋理坐标。換句話說，就是當我們将多邊形投射到螢幕上時，紋理坐标一點也不與紋理對應，其通過片段着色器作為應用于待查詢點的剛體運動變換來展現。
片段渲染器與傳統的CPU方法很類似：每個片段疊代周遊待查詢點，根據局部 x i x_i xi投影每一個點，接着在查找表中擷取log機率。渲染器簡單的将每個像素的log機率加在一起，并将結果輸出到幀緩沖器中。主機CPU可以接着檢查幀緩沖器，繼而計算最大似然機率結果。
在主機CPU端，為每一個我們希望計算的 x i x_i xi的方向圈定一個四邊形。一個單獨的四邊形對應一組固定方向下的位移。基于GPU的結果見圖1.需要主要的是，圖中的顔色編碼是使用了可視化輔助：每一個GPU輸出的像素值是個标量。
對GPU不是很懂，這段看的也不是很明白，寫的也不是很清楚。
4.6 計算協方差
在許多應用中， x i x_i xi的最大似然估計是有意義的。但是，作者同時也進行了一個合理的不确定度估計。
一旦在較大範圍對 x i x_i xi值使用損失函數進行了計算，則可以基于此資料建構一個多元高斯分布。令 x i ( j ) x_i^{(j)} xi(j)是 x i x_i xi 的第 j j j次計算評估：
基于計算的 p ( z ∣ x i , m ) p(z|x_i,m) p(z∣xi,m)估計的掃描比對的額不确定度考慮了大量的不确定量：包括傳感器自己的噪聲，待查詢點與模型的關聯誤差。該方法的缺點是高斯結果隻根據已經進行過計算評估的資料采樣點進行拟合。任何還沒有被采樣的高機率區域也不會反應在高斯機率模型中，導緻過拟合。是以，在更大的 x i x_i xi範圍進行 p ( z ∣ x i , m ) p(z|x_i,m) p(z∣xi,m)機率估計非常重要，這方面本文的方法考慮更周到。可以從圖5中兩種case看到本方法的協方差估計結果。
5. 實驗和結果
5.1 實驗設計
基于地圖中的已有的資料，再結合地圖資訊進行仿真測試。
5.2 不同方法比較
1）ICP: The Iterative Closest Point（ICP）算法以1m的比對限制為條件，并以歐式距離作為距離度量。變量是對稱的：對兩幀掃描中的每一個點，都在另一個掃描中去找最近點。比如，假設掃描幀a中的點 a i a_i ai最接近的點是 b j b_j bj, 而且在掃描b中的點 b k b_k bk最接近點 a i a_i ai。如果 b i b_i bi與 a j a_j aj的距離大于 b k b_k bk的兩倍，則對應的（ a i a_i ai, b i b_i bi）被删除。 b i b_i bi和 a j a_j aj是什麼，難道不是 a i a_i ai和 b j b_j bj嗎這種優化方式可以幫助重新确認環境中的某一部分隻在一個掃描幀中可見的情況，并剔除可能産生錯誤結果的遠距離對應關系。在上述的例子中，很可能 b j b_j bj沒有對應的參考點。額外的對稱變量可以提供額外的限制關系，幫助減少比對誤差。最後，基于找到的對應比對點對使用**Horn’s算法[6]**計算優化剛體變換。
2） ICL：The Iterative Closest Line（ICL）算法在論文實驗中将待查詢點與最近的線段使用歐拉距離關聯。線段由不超過1m部分的連續點對生成。假設待查詢點 q i q_i qi已經和一個線段 s s s關聯起來了：則在 s s s上與點 q i q_i qi距離最近的點和 q i q_i qi構成點對，繼而被傳遞到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.。