論文翻譯—2D NDT算法論文

正態分佈變換：一個新的方法來匹配激光掃描

相關信息：

題目：The Normal Distributions Transform: A New Approach to Laser Scan Matching

來源：Proceedings 2003 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS 2003)

作者：Biber, Peter, and Wolfgang Straßer

---

摘要

摘要—2D範圍內的掃描匹配是許多定位和建圖算法的基本組件。大多數掃描匹配算法需要找到使用的，如點或線等特徵之間的對應。我們爲範圍掃描提供了一個可選的表達，即正態分佈變換。與佔用柵格類似，我們將2D平面分成小的單元格。對於每一個單元格，我們分配一個正態分佈，它局部地建模了測量一個點的概率。變換的結果是分段連續且可微的概率密度，我們可以使用牛頓算法將其和另一個掃描進行匹配。因此，不需要建立明確的對應關係。

我們詳細介紹了該算法，並展示了其在相對位置跟蹤以及同時定位和建圖（SLAM）中的應用。真實數據的初步結果表明，即使不使用里程計數據，該算法也能夠可靠並實時地建立未變化的室內環境地圖（參見視頻）。

1. 緒論

毫無疑問，同時定位與建圖（SLAM）是移動機器人系統的一個基本能力。激光測距儀是獲取需要輸入的流行傳感器，因爲它們在各種各樣的情況下都具有很高的可靠性以及低噪聲。很多SLAM算法都是基於匹配兩個距離掃面（range scans）或者將距離掃面匹配到地圖上的能力。這裏我們提出了一個新的方法來實現掃面匹配的低級任務，並且展示瞭如何用它來建圖。

本文的主要目的是介紹正態分佈變換（後面將用NDT表示）以及其在匹配一個掃面和另一個掃描或多個掃面的應用。NDT將從單個掃描中重建的離散2D點集，轉換成在2D平面上定義的分段連續且可微的概率密度。概率密度由一組很容易計算的正太分佈組成。然後將第二次掃描與NDT的匹配定義爲，第二次掃描的對齊的點，在此密度下得分和的最大值。

我們也提出了一個針對SLAM問題的，完美符合我們的匹配方案的簡單算法。但是提出的匹配方案卻不依賴於這一算法。因此，我們在第二章的相關工作中，只回顧了涉及到匹配兩幀掃描的方法，而沒有回顧像Thrun[15]或Gutmann[6]中方法一樣構建整個地圖的方法。無論如何，該方法的一個組件是一個感知模型，即在給定地圖和位姿估計條件下的單個掃描的可能性。由於我們的方法恰好可以得到這種模型的度量，因此我們相信，我們的掃描匹配器也能夠被集成到更復雜的SLAM算法中。

本文剩餘部分組織如下：第三章引入NDT，第四章給出掃描匹配方法的概述，並定義將掃描與NDT比較的度量，它在第五章中使用牛頓算法進行優化。掃描匹配器在第六章中被應用於位置跟蹤，並且在第七章中被應用於簡單的SLAM方法中。我們最終展示了在真實數據中的一些結果，並以展望未來工作的方式總結本文。

2. 先前的工作

匹配兩個距離掃描的目的是找到進行掃描的兩個位置之間的相對位姿。大多數成功的算法的基礎是建立兩個掃描間的圖元(primitives)的對應關係。由此，可以推導測量誤差並使其最小化。Cox使用點作爲圖元並將它們與在先驗模型中給出的線進行匹配[3]。在Amos的工作中[7]，這些線是從掃描中提取的。Gutmann將從掃描中提取的線與模型[8]中的線進行匹配。匹配點和點這種最通用的方法是由Lu 和 Milios[9]提出的。這本質上是ICP(Iterated Closest Point，迭代最近點)算法([1],[2],[18])應用到激光掃描匹配中的變種。我們與Lu 和 Milios使用相同的建圖策略。如[10]中所提到，我們沒有建立精確的地圖，而是使用選擇的已經恢復位姿的掃描的集合作爲隱式地圖。

所有的這些方法都必須建立明確的對應。我們的方法在這一點上有所不同，因爲我們從不需要建立圖元間的對應關係。也有一些其它避免解決對應問題的方法。在[12]中，Mojaev結合局部極座標佔用柵格的相關性和概率里程計模型來確定位姿（使用激光掃描儀和聲吶）。Weiss 和 Puttkammer[17]使用角度直方圖來恢復兩個位姿之間的旋轉。然後，使用在找到最常見的方向後計算出的x和y直方圖，來恢復平移。可以通過使用第二個主方向來擴展此方法[7]。

我們的工作也被計算機視覺技術所啓發。如果概率密度這個詞被圖像強度所代替，那麼我們的方法與特徵跟蹤[13]或構成全景圖[14]具有相似的結構，這些技術在每個相應位置使用圖像梯度來估計參數。這裏用的是正態分佈的導數。與圖像梯度相反，它們可以被解析地計算出來（即有解析解）。

3. 正態分佈變換

這一章描述單幀激光掃描的正態分佈變換（NDT）。這是本文的主要貢獻。然後，在下面的章節中相對簡單地描述NDT在位置跟蹤和SLAM方面的應用。

NDT通過局部正態分佈的集合來建模一幀激光掃描的所有重建2D點。首先，將機器人周圍的2D空間分爲整齊的固定大小的單元格（cell）。然後，對於每個至少包含3個點的單元格，進行如下操作：

1）收集包含在這個box中的所有2D點 $\mathbf{x}_{i = 1..n}$

2）計算平均值 $\mathbf{q}=\frac{1}{n}\sum_i{\mathbf{x}_{i}}$

3）計算協方差矩陣 $\Sigma = \frac{1}{n} \sum_i{(\mathbf{x}_{i} - \mathbf{q})(\mathbf{x}_{i} - \mathbf{q})^{t}}$

將在該單元格中測量一個樣本點$\mathbf{x}$的概率建模爲正態分佈 $N(q, \Sigma)$:

$p(\mathbf{x}) \sim exp(-\frac{(\mathbf{x}-q)^t \Sigma^{-1} (\mathbf{x}-q)}{2}) \tag{1}$

和佔用柵格類似，NDT建立平面的一個固定的細分。

可微分的描述。在我們展示例子之前，我們必須注意兩個實現細節。但是佔用柵格表示一個單元被佔用的概率，而NDT則表示在單元格中每個位置測量一個樣本(點)的概率。我們使用$100cm \times 100cm$大小的單元格。

這樣的表達有什麼用呢？現在，我們以概率密度的形式對2D平面進行了分段連續且可微分的描述。在我們展示例子之前，我們必須主意兩個實現細節。

爲了儘量減少離散化的影響，我們決定使用四個重疊的柵格。即，首先放置單個邊長爲$l$的一個柵格，然後第二個，水平地平移$\frac{l}{2}$，第三個垂直地平移$\frac{l}{2}$，最終第四個，水平並垂直平移$\frac{l}{2}$。現在每個2D點落入了四個單元格中。在本文的其餘部分中，不會明確考慮到這一點，並且我們將描述我們的算法，就像每個點只有一個單元格。因此，如果計算出一個點的概率密度，則可以通過默認理解來完成，即評估所有四個單元格的密度並將結果相加。

第二個問題是，對於一個測量的無噪聲的世界中的線，協方差矩陣會變得奇異且不可逆。實際上，協方差矩陣有時會接近奇異值。爲了防止這種影響，我們檢查$\Sigma$的較小特徵值是否至少是較大特徵值的0.001倍，如果不是，則將其設爲該值。

圖1顯示了激光掃描的示例和NDT結果的可視化。通過評估在每個點的概率密度建立可視化，亮的區域意味着高概率密度。下一章展示，如何使用該變換來對齊兩幀激光掃描。

NDT的一個例子：原始的激光掃描及其概率密度結果

4. 掃描對齊

兩個機器人座標系之間的空間映射$T$如下：

$T: \left(\begin{matrix} x' \\ y' \end{matrix}\right) = \left(\begin{matrix} cos\phi & -sin\phi & \\ sin\phi & cos\phi \end{matrix}\right) \left(\begin{matrix} x \\ y \end{matrix}\right)+\left(\begin{matrix} t_x \\ t_y \end{matrix} \right) \tag{2}$

其中$(t_x, t_y)^t$和$\phi$分別描述了兩個座標系之間的旋轉和平移。掃描對齊的目標是使用在兩個位置得到的激光掃描來恢復這些的參數。給定兩幀掃描（第一幀和第二幀），提出的方法的大綱如下：

1）創建第一幀掃描的NDT。

2）初始化估計參數（用0值或者使用里程計數據）

3）對於第二幀掃描中的每個樣本點：根據參數將重建的2D點映射到第一次掃描的座標系中。

4）確定每個映射的點所對應的正態分佈。

5）通過評估每個映射的點的分佈並將結果相加，來確定參數的得分。

6）通過嘗試優化得分，計算一個新的參數估計。這通過執行牛頓算法的一個步驟即可完成。

7）回到第3步，直到滿足收斂判據。

前四個步驟很簡單：如上一章所描述的那樣構建NDT。如上所述，里程計數據可以被用來初始化估計。使用$T$並找到對應的正態分佈來完成對第二幀掃描的建圖，是在NDT柵格中的一個簡單查找。

現在，使用以下符號詳細描述其餘部分：
$\mathbf{p} = (p_i )^t_{i=1..3} = (t_x , t_y , φ)^t$：要估計的參數向量。

$\mathbf{x_i}$：在第二幀掃描座標系下，第二幀掃描的激光掃描樣本點$i$的重建2D點。

$\mathbf{x'_i}$：根據參數$\mathbf{p}$將點$\mathbf{x_i}$映射到第一幀掃描座標系中的點，即$\mathbf{x'_i} = T(\mathbf{x_i, p})$。

$\mathbf{\Sigma_i , q_i}$：在第一幀掃描中查找的$\mathbf{x'_i}$所對應的正態分佈的協方差矩陣和均值。

如果評估所有具有參數$\mathbf{\Sigma}$和$\mathbf{q_i}$的點$\mathbf{x'_i}$的正態分佈的和最大，則可以認爲根據$\mathbf{p}$建立的地圖是最優的。我們稱這個和爲$\mathbf{p}$的分數。它被定義爲：

$score(\mathbf{p}) = \sum exp(\frac{-(\mathbf{x'_i} - \mathbf{q_i})^t \mathbf{\Sigma_i^{-1}}(\mathbf{x'_i} - \mathbf{q_i})}{2}) \tag{3}$

該得分在下一章被優化。

5. 使用牛頓算法優化

由於優化問題通常被描述爲最小化問題，因此我們將採用這種約定的表示法。因此，在本章中，要最小化的函數是$-score$。牛頓的算法迭代地找參數$\mathbf{p} = (p_i)^t$，使函數$f$最小化。每次迭代求解如下等式：

$\mathbf{H \Delta p = -g} \tag{4}$

其中$\mathbf{g}$是輸入爲
$g_i = \frac{\partial f}{\partial p_i} \tag{5}$
時，$f$的梯度的轉置，$\mathbf{H}$是輸入爲
$H_{ij} = \frac{\partial f}{\partial p_i \partial p_j} \tag{6}$
時，$f$的海塞矩陣。

將這個線性方程的$\mathbf{\Delta p}$加到當前的估計中：
$\mathbf{p \leftarrow p + \Delta p} \tag{7}$

如果$\mathbf{H}$是正定的，$f(\mathbf{p})$最初將沿$\mathbf{p}$的方向減小。如果$\mathbf{H}$不是是正定的，$\mathbf{H}$將被$\mathbf{H'} = \mathbf{H} + \lambda \mathbf{I}$代替，選擇$\lambda$確保$\mathbf{H'}$是正定的，關於最小化算法本身的實現細節可以在例如[4]中找到。

該算法現在被應用到函數$-score$中。通過集合等式3的所有和項的偏導數來構建梯度以及海塞（矩陣）。爲了使符號更短，並避免混淆參數號$i$和激光掃描樣本（點）$i$的索引，將不再使用樣本點號的索引$i$。另外，我們有：

$\mathbf{q = x'_i - q_i} \tag{8}$

可以很容易地驗證，$\mathbf{q}$關於$\mathbf{p}$的偏導數等於$\mathbf{x'_i}$的偏導數。然後$-score$的和$s$爲：

$s = -exp (\frac{-\mathbf{q^t \Sigma^{-1} q}}{2}) \tag{9}$

對於這樣和，梯度的輸入爲（使用鏈式法則）：

$\tilde{g_i} = -\frac{\partial s}{\partial p_i} = -\frac{\partial s}{\partial q} \frac{\partial q}{\partial p_i} = \mathbf{q^t \Sigma^{-1}} \frac{\partial q}{\partial p_i} \cdot exp (\frac{-\mathbf{q^t \Sigma^{-1} q}}{2}) \tag{10}$

$\mathbf{q}$關於$p_i$的偏導數由$T$的雅可比矩陣$\mathbf{J_T}$給出（參見等式2）：

$\mathbf{J_T} = \left(\begin{matrix} 1 & 0 & -x\cdot sin\phi -y \cdot cos\phi & \\ 0 & 1 & x \cdot cos\phi -y sin \cdot \phi \end{matrix}\right) \tag{11}$

在海塞（矩陣）$\mathbf{H}$中的一個和的輸入爲：

$\tilde{H}_{ij} = -\frac{\partial s}{\partial p_i \partial p_j} = -exp (\frac{-\mathbf{q^t \Sigma^{-1} q}}{2}) ((-\mathbf{q^t \Sigma^{-1}} \frac{\partial \mathbf{q}}{\partial p_i})(-\mathbf{q^t \Sigma^{-1}} \frac{\partial \mathbf{q}}{\partial p_j}) + (-\mathbf{q^t \Sigma^{-1}} \frac{\partial^2 \mathbf{q}}{\partial p_i \partial p_j}) + (-\frac{\partial \mathbf{q^t}}{\partial p_j} \mathbf{\Sigma^{-1}} \frac{\partial \mathbf{q}}{\partial p_i})) \tag{12}$

$\mathbf{q}$的二階導數爲（見等式11）：

$\frac{\partial^2 \mathbf{q}}{\partial p_i \partial p_j} = \left\{ \begin{aligned} \left(\begin{matrix} -x\cdot cos\phi + y\cdot sin\phi \\ -x\cdot sin\phi- y \cdot cos\phi \end{matrix}\right) ~~~i = j = 3\\ \left(\begin{matrix} 0 \\ 0 \end{matrix}\right) ~~~ otherwise \end{aligned} \right. \tag{13}$

從這些等式中可以看到，建立梯度和海塞（矩陣）的計算成本較低。每個點只有一個對指數函數的調用以及少量的乘法運算。三角函數僅取決於$\phi$（當前角度估計），因此每次迭代僅需調用一次。接下來的兩章將使用此算法來對SLAM進行位置跟蹤。

6. 位置跟蹤

這一章描述了掃描匹配算法如何被應用到，從給定的時間$t=t_{start}$跟蹤當前的位置。然後下一章將其擴展到SLAM 中。

全局參考座標系由在此時刻的局部機器人座標下定義。以下將各個激光掃描稱爲關鍵幀。對此關鍵幀進行跟蹤。在時刻$t_{k}$，算法執行如下步驟：

1）定義$δ$爲$t_{k-1}$到$t_{k}$時間內的移動（例如來自里程計）。

2）根據$δ$建立在$t_{k-1}$時刻估計的位置的地圖。

3）使用當前的掃描，關鍵幀的NDT以及新的位置估計執行優化算法。

4）檢查該關鍵幀離當前的掃描是否足夠“近”。如果是，迭代。否則，將最新的成功匹配的掃描作爲關鍵幀。

掃描是否仍然足夠接近的決定是基於簡單的經驗判據，該判據涉及關鍵幀和當前幀之間的平移和角度距離以及結果分數。爲了幫助位置跟蹤，算法必須能夠實時運行：在一個1.4GHz的機器上建立一個掃描的NDT需要大約10ms。對於掃描之間很小的移動，優化算法通常需要大約1-5次迭代（很少超過10次）。一次迭代大概需要2ms，所以實時沒有問題。

7. 應用到SLAM

我們將地圖定義爲關鍵幀在各自全局位姿下的集合。這一章描述了當機器人到達未知區域，如何相對於地圖定位並擴展及優化地圖。

A. 相對於多個掃描的定位

對於地圖中的每一幀掃描$i$，它關聯了一個角度$\phi_i$（或者一個旋轉矩陣$\mathbf{R_i}$）和一個平移向量$(t_x, t_y)^t_i = \mathbf{T}_i$。它們描述了掃描$i$在全局座標系下的位姿。當前機器人的位姿旋轉矩陣$\mathbf{R}$和平移向量$\mathbf{T}$表示：

$T': \left(\begin{matrix} x' \\ y'\end{matrix}\right) = \mathbf{R}^t_i(\mathbf{R}\left(\begin{matrix} x \\ y\end{matrix}\right) + \mathbf{T} - \mathbf{T_i})\tag{14}$

僅需要很小的變化就可以使第5章的算法適用這種情況。掃描$i$的2D點的建圖現在通過應用$T'$來計算。而且，$T'$的雅可比(矩陣)和二階導數現在變得稍微複雜一點。建圖的雅可比(矩陣)如下：

$\mathbf{J_{T'}} = \mathbf{R_i^t J_T} \tag{15}$

$T'$的二階導數如下：

$\frac{\partial^2 \mathbf{x'}}{\partial p_i \partial p_j} = \left\{ \begin{aligned} \mathbf{R_i}^t\left(\begin{matrix} -x\cdot cos\phi + y\cdot sin\phi \\ -x\cdot sin\phi- y \cdot cos\phi \end{matrix}\right) ~~~i = j = 3\\ \left(\begin{matrix} 0 \\ 0 \end{matrix}\right) ~~~ otherwise \end{aligned} \right. \tag{16}$

通過彙總所有重疊的掃描可以求得用於優化算法的梯度和海塞（矩陣）。但是我們找到了另一種選擇，它更快且能產生同樣好效果：對於在機器人位置獲得的掃描的每一個樣本點，確定評估概率密度的結果最大的掃描。只有該掃描被用於此樣本和當前的迭代，這樣，建立優化算法的梯度和海塞（矩陣）所需的操作，與重疊關鍵幀的數量無關，除了找到提到的最大值。

B. 加入新的關鍵幀並優化地圖

在每一時刻，地圖由各自全局座標系位姿下的關鍵幀組成。如果當前掃描與地圖的重疊太小，那麼通過最近的成功匹配的掃描來擴展地圖。然後，將每個重疊的掃描分別匹配到新的關鍵幀，產生兩次掃描之間的相對位姿。維護一個圖，其中包含成對匹配結果的信息。

在圖中，每一個關鍵幀由一個節點來表示。一個節點保存了在全局座標系中關鍵幀位姿的估計值。兩個節點之間的邊表示相應的掃描已成對匹配，並保持兩個掃描之間的相對位姿。

在一個新的關鍵幀加入後，通過優化基於所有關鍵幀的參數定義的誤差函數，來精化地圖。成對配準的結果用於爲每個匹配對定義二次誤差模型，如下所示：兩次掃描的全局參數還定義兩次掃描之間的相對姿態。令$\Delta p$爲全局參數定義的相對位姿與成對匹配結果定義的相對位姿之間的差。然後，我們對$\Delta p$的函數，兩次掃描的分數進行建模，使用二次模型：

$score'(\Delta p) = score + \frac{1}{2}(\Delta p)^t \mathbf{H}(\Delta p) \tag{17}$

因此，$score$是最終稿的的得分，當成對的匹配收斂，$\mathbf{H}$是由此獲得的海塞（矩陣）。該模型可通過對$score$在$\mathbf{\Delta p=0}$附件進行二階泰勒展開得到。注意，缺失了線性項，因爲我們擴展了一個極端點。現在，基於所有的邊對分數求和並優化g。

如果關鍵幀的數量變大，則無法在實時條件下執行此最小化操作（空閒參數的數量爲3N-3，其中N爲關鍵幀的數量）。我們因此只優化地圖的子圖。通過收集所有關鍵幀來構建該子圖，可以通過遍歷不超過三個邊來從新關鍵幀的節點到達此關鍵幀。現在，我們僅針對參數（屬於此子圖中包含的關鍵幀）對上面的誤差函數進行優化。當然，如果產生了閉環，我們必須在所有關鍵幀上進行優化。

8. 結果

我們現在提供的結果（以及第6節中的示例）是在不使用里程計的情況下得到的。這應該證明了該方法的魯棒性。當然，如Thrun在[15]中所所提到的，只要任何2D結構存在於世界上，這都是可能的。

在圖2中建立的地圖是通過控制機器人走出實驗室，順着走廊，逆着走廊，然後返回實驗室得到的。

使用我們掃描匹配器建立的地圖。長度單位爲cm。顯示的是關鍵幀的集合以及估計的軌跡（見視頻）。更多的視頻在作者主頁[11]

因此，這種情況既需要擴展地圖，又需要相對於地圖的定位。機器人在20分鐘運動了大約83米，採集了28430幀激光掃描。使用SICK激光獲取分辨率爲1°，角度爲180°的掃描。仿真的速度大約35cm/s，並且每秒大約23幀掃描。我們使用組合策略來創建該地圖。第4章的位置跟蹤器被應用到每一個掃描中，而我們通過線性地推斷最新時刻的結果來初始化參數【即以勻速模型預測下一時刻位姿】。每第十幀掃描，就會執行一次第七章的過程。

圖2展示了建圖的結果。其最終顯示的地圖由33個關鍵幀組成。仔細觀察也可以發現，我們的掃描匹配算法可以容忍環境中的小變化，例如開門或關門。在1.4GHz的機器上，離線處理所有的幀需要58秒，即每秒（處理）97幀掃描。通過將我們當前的實現從Java移植到更快的語言，也許可以提高（處理）速度。

9. 結論以及將來的工作

我們已經提出了一個距離掃描的新表達，即正態分佈變換（NDT）。可使用該變換來推導匹配另一個掃描的解析表達式。我們也展示瞭如何將我們的掃描匹配器應用於位置跟蹤問題和SLAM問題。我們方法的主要優點是：

• 點或特徵之間無需建立明確的對應關係。因爲這是在大部分方法中最容易出錯的部分，沒有對應，我們（的方法）更穩定。
• 可以解析地計算出所有導數，這樣快且正確。

當然，也存在問題：一切都能夠通過局部正態分佈進行充分地建模嗎？

迄今爲止，我們在室內環境的測試表現很好，這從來沒有問題。我們計劃在結構較少的環境中，最好是在戶外，進行進一步測試。我們還打算系統地比較我們的方法與Lu和Milios方法的收斂半徑。

10. 致謝

Peter Biber由Land BadenWuerttemberg贈款資助，對此深表謝意。我們還要感謝Andreas Zell教授和Achim Lilienthal，（我們纔有可能）使用機器人和數據採集軟件。我們還要感謝Sven Fleck進行了許多富有成果的討論，併爲視頻提供了幫助。

11. 參考文獻