The Normal Distributions Transform: A New Approach to Laser Scan Matching
正態分佈變換：鐳射掃描匹配的新方法
摘要：匹配 2D 範圍掃描是許多定位和建圖演算法的基本組成部分。大多數掃描匹配演算法需要找到所使用的特徵（即點或線）之間的對應關係。我們提出了範圍掃描的替代表示法，即正態分佈變換。與佔用網格類似，我們將 2D 平面細分為單元。對於每個單元格，我們分配一個正態分佈，該分佈對測量點的機率進行區域性建模。變換的結果是分段連續且可微的機率密度，可用於使用牛頓演算法匹配另一次掃描。因此，不必建立明確的對應關係。我們詳細介紹了該演算法，並展示了其在相對位置跟蹤以及同步定位和地圖構建 (SLAM) 中的應用。真實資料的初步結果表明，即使不使用里程計資料，該演算法也能夠可靠且實時地繪製未修改的室內環境（參見影片）。
一、引言
毫無疑問，同步定位與建圖（SLAM）是移動機器人系統的基本能力。鐳射測距掃描器是獲得所需輸入的流行感測器，主要是因為它們在國外的情況下具有高可靠性和低噪聲。許多 SLAM 演算法基於匹配兩個範圍掃描或將範圍掃描與地圖匹配的能力。在這裡，我們提出了一種掃描匹配低階任務的新方法，並展示了我們如何使用它來構建地圖。
本文的主要目的是介紹正態分佈變換（以下稱為NDT）及其在一次掃描與另一次掃描或一次掃描與多個其他掃描的匹配中的應用。 NDT 將從單次掃描重建的離散 2D 點集轉換為在 2D 平面上定義的逐個連續且可微的機率密度。該機率密度由一組可以輕鬆計算的正態分佈組成。然後將第二次掃描與 NDT 匹配定義為最大化第二次掃描在該密度上的對齊點得分的總和。
我們還提出了一種針對 SLAM 問題的簡單演算法，該演算法很好地融入了我們的匹配方案。然而，所提出的匹配方案並不依賴於該演算法。因此，我們僅回顧第二節相關工作中有關兩次掃描匹配的方法，而不回顧像 Thrun [15] 或 Gutmann [6] 的方法那樣構建整個地圖的方法。然而，這種方法的一個組成部分是感知模型，即給定 amap 和 apose 估計的單次掃描的可能性。由於我們的方法可以精確測量此類模型，因此我們相信，我們的掃描匹配器也可以整合到更復雜的 SLAM 演算法中。
本文的其餘部分組織如下：第三節介紹了 NDT，第四節概述了掃描匹配方法，並定義了將 ascan 與 aNDT 進行比較的度量，該度量在第五節中使用牛頓演算法進行了最佳化。掃描匹配器應用於第六節中的位置跟蹤和第七節中的簡單 SLAM 方法。我們最終展示了一些真實資料的結果，並對未來的工作進行了展望。
II.P 先前工作
匹配兩個範圍掃描的目標是找到進行掃描的兩個位置之間的相對位置。最成功演算法的基礎是在兩次掃描的基元之間建立對應關係。由此，可以匯出誤差測量並將其最小化。 Cox 使用點作為基元並將它們與先驗模型中給出的線相匹配 [3]。在阿莫斯專案 [7] 中，這些線條是從掃描中提取的。 Gutmann 將從 ascan 中提取的線條與模型中的線條進行匹配 [8]。 Lu 和 Milios [9] 提出了最通用的方法，即點對點匹配。這本質上是應用於鐳射掃描匹配的 ICP（迭代最近點）演算法 ([1]、[2]、[18]) 的變體。我們與 Lu 和 Milios 分享我們的對映策略。如[10]中所示，我們不構建顯式地圖，而是使用選定掃描的集合及其恢復的姿勢作為隱式地圖。
在所有這些方法中，必須建立明確的對應關係。我們的方法在這一點上有所不同，因為我們永遠不需要在基元之間建立對應關係。還有其他方法可以避免解決對應問題。在[12]中，Mojae v 將區域性極地佔用網格的相關性與機率測距模型相結合，用於姿態確定（使用鐳射掃描器和聲納）。 Weiss 和 Puttkammer [17] 使用角度直方圖來恢復兩個姿勢之間的旋轉。然後使用找到最常見方向後計算的 x 和 y 直方圖來恢復平移。這種方法可以透過使用第二個主要方向來擴充套件[7]。
我們的工作也受到計算機視覺技術的啟發。如果用影像強度代替單詞機率密度，我們的方法與特徵跟蹤[13]或全景圖合成[14]具有相似的結構。這些技術使用每個相關位置的影像梯度來估計引數。這裡，使用正態分佈的導數。與影像梯度相反，這些可以透過分析計算。
III.正態分佈變換
本節描述單次鐳射掃描的正態分佈變換(NDT)。這意味著本文的核心貢獻。使用 NDT 進行位置跟蹤和 SLAM（在以下各節中描述）相對簡單。 NDT 透過區域性正態分佈的集合對一次鐳射掃描的所有重建 2D 點的分佈進行建模。首先，機器人周圍的二維空間被規則地細分為大小恆定的單元。然後，對於每個包含至少三個點的單元格，執行以下操作：
現在，在該單元中包含的 2D 點 x 處測量樣本的機率由正態分佈 N (q; ∑) 建模：
與佔用網格類似，NDT 建立了平面的常規細分。但是，佔用網格表示單元被佔用的機率，而 NDT 則表示測量單元內每個位置的樣本的機率。我們使用 100 釐米 x 100 釐米的單元尺寸。
這個表示有什麼用呢？我們現在以機率密度的形式對二維平面進行了分段連續且可微的描述。在展示示例之前，我們必須注意兩個實現細節。為了最大限度地減少離散化的影響，我們決定使用四個重疊的網格。也就是說，首先放置一個邊長為 l 的單個單元格，然後放置第二個網格，水平移動 L/2 ，第三個，垂直移動 L/2 ，最後移動第四個，水平和垂直移動 L/2 。沒有一個 2D 點落入四個單元格中。本文的其餘部分不會明確考慮這一點，我們將描述我們的演算法，就好像每個點只有一個單元格一樣。所以計算一個點的機率密度時，預設是對所有四個單元格的密度進行評估，然後將結果相加。
第二個問題是，對於無噪聲的測量世界線，協方差矩陣將變得奇異並且無法反轉。在實踐中，協方差矩陣有時會接近奇異值。為了防止這種影響，我們檢查的較小特徵值是否至少是較大特徵值的 0:001 倍。如果不是，則設定為此值。
圖 1. NDT 示例：原始鐳射掃描和生成的機率密度。
圖 1 顯示了鐳射掃描示例和所得 NDT 的視覺化。視覺化是透過評估每個點的機率密度建立的，明亮區域表示高機率密度。下一節將展示如何使用此轉換來對齊雙鐳射掃描。
IV.掃描對準
兩個機器人座標系之間的空間對映 T 由下式給出
其中 
描述兩個幀之間的平移和旋轉。掃描對準的目標是使用在兩個位置進行的鐳射掃描來恢復這些引數。考慮到兩次掃描（第一次和第二次），所提出的方法的概要如下：
1) 建立第一次掃描的NDT。
2) 初始化引數的估計（透過零或使用里程計資料）。
3）對於第二次掃描的每個樣本：根據引數將重建的二維點對映到第一次掃描的座標系中。
4) 確定每個對映點對應的正態分佈。
5) 引數的得分是透過評估每個對映點的分佈並對結果求和來確定的。
6) 透過嘗試最佳化分數來計算新的引數估計。這是透過執行牛頓演算法的一個步驟來完成的。
7) 轉到3，直到滿足收斂標準。
前四個步驟很簡單：上一節描述了構建 NDT。如上所述，里程計資料可用於初始化估計。使用 T 完成第二次掃描的對映，並在 NDT 網格中進行簡單查詢即可找到相應的正態分佈
現在使用以下符號詳細描述其餘部分：
如果評估具有引數 ∑i 和 qi 的所有點 xi 的正態分佈之和最大，則根據 p 的對映可以被認為是最優的。我們將此總和稱為 p 的分數。定義為：
該分數將在下一節中進行最佳化。
V.使用牛頓演算法進行最佳化
由於最佳化問題通常被描述為最小化問題，因此我們將採用我們的符號來表示此約定。因此，本節中要最小化的函式是分數。牛頓演算法迭代地找到引數 p =( pi)t ，從而最小化函式 f 。每次迭代都求解以下方程：
其中 g 是 f 與條目的轉置梯度
H 是 f 的 Hessian 矩陣，有條目
該線性系統的解是一個增量 p，它被新增到當前估計中：
如果 H 是正定的，f (p) 最初將沿 p 的方向減小。如果不是這種情況，則將 H 替換為 H0 = H + I，選擇這樣的方式，使得 H0 安全地為正定。有關最小化演算法本身的實際細節可以在 [4] 中找到。
該演算法現在應用於函式得分。透過收集方程3的所有被加數的偏導數來構建梯度和Hessian矩陣。為了更簡短的表示法並避免混淆引數編號i和鐳射掃描樣本i的索引，省略了樣本編號的索引i。另外，我們寫
很容易驗證，q 對 p 的偏導數等於 x0i 的偏導數。得分的一個被加數 s 由下式給出
對於這樣的被加數，梯度的條目是（使用鏈式法則）：
q 對 pi 的偏導數由 T 的雅可比矩陣 JT 給出（參見方程 2）：
Hessian H 中的被加數條目由下式給出：
q 的二階導數為（參見方程式 11）：
從這些方程可以看出，構建梯度和 Hessian 矩陣的計算成本很低。每個點只需呼叫一次指數函式和少量乘法。三角函式僅取決於（角度的當前估計），因此每次迭代只能呼叫一次。接下來的兩節將使用該演算法進行位置跟蹤和 SLAM。
VI.位置跟蹤
本節描述如何應用掃描匹配演算法來從給定時間 t = tstart 開始跟蹤當前位置。下一節將這種方法擴充套件到 SLAM。此時全域性參考座標系由機器人區域性座標系定義。相應的鐳射掃描在下文中被稱為關鍵幀。跟蹤是針對該關鍵幀執行的。在時間 tk 時，演算法執行以下步驟：
1) 讓 為時間 tk 1 和 tk 之間運動的估計（例如來自里程計）。
2) 根據 繪製時間 tk 1 的位置估計。
3) 使用當前掃描、關鍵幀的NDT和新位置估計來執行最佳化演算法。
4) 檢查關鍵幀是否足夠“接近”當前掃描。如果是的話，迭代。否則將最後一次成功匹配的掃描作為新的關鍵幀。
ascan 是否仍然足夠接近的決定基於一個簡單的經驗標準，涉及關鍵幀和當前幀之間的平移距離和角距離以及所得分數。為了對位置跟蹤有用，該演算法必須實時執行：在 1.4 GHz 機器上構建 ascan 的 NDT 需要大約 10 毫秒。對於掃描之間的小移動，最佳化演算法通常需要大約 1-5 次迭代（很少超過 10 次）。一次迭代需要2ms左右，所以實時性沒有問題。
VII. SLAM 的應用
我們將地圖定義為關鍵幀及其全域性姿勢的集合。本節描述當機器人到達未知區域時如何相對於該地圖進行定位以及如何擴充套件和最佳化該地圖。
A.關於多次掃描的定位
對於地圖中的每個掃描i，角度i（或旋轉矩陣Ri）和平移向量（tx；t y）it=Ti相關聯。這些描述了全域性座標系中掃描 i 的位姿。當前機器人位姿由旋轉矩陣 R 和平移向量 T 表示。從機器人座標系到掃描 i 的座標系的對映 T 0 由下式給出：
只需要進行一些小的改變即可使第五節的演算法適應這種情況。掃描 i 的二維點的對映現在透過應用 T 0 來計算。此外，T 0 的雅可比行列式和二階偏導數現在變得稍微複雜一些。對映的雅可比行列式現在由下式給出：
T 0 的二階偏導數現在由下式給出：
最佳化演算法的梯度和 Hessian 矩陣可以透過對所有重疊掃描求和來構建。但我們找到了一種替代方案，速度更快，並且產生同樣好的結果：對於在機器人位置採集的每個掃描樣本，確定掃描，其中評估機率密度的結果最大。僅此掃描用於此樣本和當前迭代。這樣，除了找到上述最大值之外，為最佳化演算法構建梯度和 Hessian 矩陣所需的操作與重疊關鍵幀的數量無關。
B. 新增新的關鍵幀並最佳化地圖
在每個時間步，地圖由一組關鍵幀組成，其姿態位於全域性座標系中。如果當前掃描與地圖的重疊太小，則地圖將由上次成功匹配的掃描擴充套件。然後，每個重疊掃描都分別與新的關鍵幀進行匹配，從而產生兩次掃描之間的相對姿勢。維護一個圖，其中儲存成對匹配結果的資訊。
在此圖中，每個關鍵幀都由陽極表示。節點儲存全域性座標系中關鍵幀姿態的估計。兩個節點之間的邊表示相應的掃描已成對匹配，並保持兩個掃描之間的相對位置。
新增新的關鍵幀後，透過最佳化在所有關鍵幀的引數上定義的誤差函式來細化地圖。成對配準的結果用於為每個匹配對定義二次誤差模型，如下所示： 兩次掃描的全域性引數也定義兩次掃描之間的相對位置。令 p 為全域性引數定義的相對位姿與成對匹配結果定義的相對位姿之間的差。然後我們對分數進行建模使用二次模型將這兩次掃描作為 p 的函式
因此，score 是兩兩匹配收斂時的最終分數，H 是由此獲得的 Hessian 矩陣。該模型是透過分數在 p = 0 到二次項之間的泰勒展開而匯出的。請注意，線性項缺失，因為我們圍繞極值點展開。現在，該分數在所有邊緣上求和並進行最佳化。
如果關鍵幀的數量變大，則無法再在實時條件下執行這種最小化（自由引數的數量為 3N 3，其中 N 是關鍵幀的數量）。因此，我們僅對地圖的子圖進行最佳化。該子圖是透過收集所有關鍵幀來構建的，從新關鍵幀的節點出發，遍歷不超過三個邊即可到達該子圖。我們現在僅針對屬於該子圖中包含的關鍵幀的引數來最佳化上面的誤差函式。當然，如果必須關閉迴圈，我們就必須對所有關鍵幀進行最佳化。
八、結果
我們現在展示的結果（以及第 6 節中的示例）是在不使用里程計的情況下執行的。這應該證明該方法的穩健性。當然，正如 Thrun 在[15]中已經指出的那樣，只要世界上存在任何二維結構，這才有可能。
圖2 中呈現的構建地圖。 2是透過駕駛機器人離開實驗室，沿著走廊，沿著走廊，然後返回實驗室而獲得的。
圖 2：使用我們的掃描匹配器構建的地圖。長度以釐米為單位。顯示的是關鍵幀集和估計軌跡（參見影片）。更多影片可以在作者的主頁上找到[11]。
因此，這種情況既需要地圖的擴充套件，又需要相對於地圖的定位。該機器人在 20 分鐘的行程中收集了 28430 次鐳射掃描，行程約 83 米。使用 SICK 鐳射掃描器進行掃描，覆蓋範圍為 180 度，角解析度為 1 度。為了模擬更高的速度，僅使用每第五次掃描。模擬速度約為 35 cm=s，每秒掃描次數約為 23 次。地圖是使用組合策略構建的。第 4 節的位置跟蹤器應用於每次掃描，而我們透過推斷結果來初始化引數最後一個時間步是線性的。每第十次掃描，應用第七節的程式。
圖 2 顯示了生成的地圖。顯示的是最終地圖包含的 33 個關鍵幀。仔細觀察還發現，我們的掃描匹配演算法可以容忍環境中的微小變化，例如開啟或關閉的門。在 1.4 GHz 機器上處理 ine 的所有幀需要 58 秒，即每秒掃描 97 次。透過將我們當前的實現從 Java 移植到更快的語言，也許可以獲得更快的速度。
IX. 結論和未來工作
我們提出了範圍掃描的一種新表示方法，即正態分佈變換（NDT）。該變換可用於匯出用於匹配另一掃描的分析表示式。我們還展示瞭如何將我們的掃描匹配器應用於位置跟蹤問題和 SLAM 問題。我們的方法的主要優點是：
- 不必在點或特徵之間建立明確的對應關係。由於這是大多數方法中最容易出錯的部分，因此我們在沒有對應關係的情況下更加穩健。
- 所有導數都可以透過分析計算。這既快速又正確。
問題當然是：一切都可以透過區域性正態分佈很好地建模嗎？
到目前為止，我們的測試都是在室內環境中進行的，這從來都不是問題。計劃在結構較少的環境（最好是室外）中進行進一步的測試。我們還打算系統地比較我們的方法與 Lu 和 Milios 的方法的收斂半徑。
X.致謝
Peter Biber 得到了巴登符騰堡州的資助，對此深表謝意。我們還要感謝 Andreas Zell 教授和 Achim Lilienthal 教授讓我們能夠使用機器人和資料採集軟體。我們還要感謝 Sven Fleck 進行了多次富有成效的討論併為影片提供了幫助。