基于激光雷達的點云配準算法綜述

衣明悅，石 晶，趙梓杉

（遼寧工業大學 汽車與交通工程學院，遼寧 錦州 121001）

前言

目前主流的自動駕駛定位技術有基于全球導航衛星系統（GNSS）、基于航跡推算的定位方法，以及基于環境特征匹配的定位方法三種。其中基于環境特征匹配的定位方法主要采用車載相機和激光雷達獲取RGB圖像與激光點云數據，然后與高精地圖中存儲數據進行特征匹配，從而實現精確定位。

在點云地圖構建過程中由于激光雷達掃描距離存在限制，所以會進行多次連續掃描，然后將每一次掃描的點云數據進行配準拼接，最終形成完整連續的激光點云地圖。

點云配準是點云數據處理的基礎性工作。同時激光掃描的點云數據能夠以較小的內存獲得物體準確的拓撲結構和幾何結構，因而獲得越來越多學者的廣泛關注。在實際采集過程中，因為被測物體尺寸過大，物體表面被遮擋或者三維掃描設備的掃描角度等因素，單次的掃描往往得不到物體完整的集合信息。從而，為了獲得被測物體的完整幾何信息，就需要通過求解坐標之間轉換關系，將連續掃描的兩幀或多幀激光點云轉換到同一坐標系（scan-to-scan），或者將當前掃描點云已建立的地圖進行配準（scan-to-map）從而最終恢復車身位置和姿態的變化。

1 發展現狀

1.1 迭代最近點（ICP）算法國內外發展現狀

Besl[1]等在1992年提出的迭代最近點（ICP）算法，是最經典的點云配準算法，該算法計算較為簡單，但是該算法的缺點是不僅對初始位置要求較高，而且容易陷入局部最優結果。此外顯示測量中很難滿足傳統ICP算法在兩片點云間存在子集關系的要求[2]。因此針對ICP算法的缺陷，很多學者提出了ICP的改進算法。TakeshiMasuda等在1994年為了提高對應點搜索效率，提出了在當前幀的點云中選擇有代表性的點，再進行搜索對應點的步驟，減小對應點搜索的范圍，減少了點云匹配消耗的時間[3]。DevrimAkca在2005年為了加快對應點搜索，提升算法效率，提出了一種用于匹配三維點云的最小二乘匹配的算法，使用高斯一馬爾可夫模型估計點云之間的轉換參數，同時，提出了有效的空間劃分方法[4]。

為了解決實際中點云配準問題，國內一些學者也提出了改進匹配算法。張鴻賓等在2005年，提出了一種基于物體表面距離的匹配算法，主要思想是將物體表面用三角網格來近似表示，將網格間最近距離的均值作為評價標準，進而估計運動參數，該算法對物體匹配的準確性有所提高[5]。近年，在2019年趙明富等[6]提出了融合采樣一致性和ICP算法對初始位置相差較大的點云進行配準。2021年李慧慧等提出一種改進的ICP激光點云精確配準方法，該方法可以對初始位置相差較大且具有部分重疊的點云進行精確配準，同時提高運行效率并對噪聲具有相應的魯棒性[6]。

1.2 正態分布變換（NDT）算法國內外發展現狀

Biber和Straber在2003年第一次提出了NDT算法，當時主要用于二維平面點云數據之間的匹配[7]。厄勒布魯大學的Magnus-son在2006年首次在三維空間中使用NDT算法，并提出了3D-NDT算法，使得NDT算法突破了維度的限制[8]，能夠適用于利用機器人進行采集的點云數據配準。Cihan等在2013年提出了多層正態分布變換（ML-NDT）算法[9]，改進了正態分布變換算法，同年，JariSaarinen等人提出了一種新的三維建圖方法，NDT-OM算法主要采用遞歸更新策略在大規模動態環境中實現精確的三維地圖創建，并且可以無縫提供多種分辨率的地圖[10]。

國內也有很多學者也進一步研究和改進了正態分布變換（NDT）算法。蔡則蘇在2005年利用NDT算法根據自然環境特征有效地完成了室內環境下的自主定位，有效避免了點云之間的復雜匹配問題[11]。浙江大學的胡鳳軍等在2014年提出了將牛頓迭代算法應用到三維正態分布變換算法中，解決了在離散點云中配準算法收斂性差，容易產生局部最優的問題[12]。2021年荊路等提出了基于SAC-IA和NDT融合的點云配準方法，依據FPFH特征和SAC-IA算法對點云進行初始配準，然后在此基礎上使用NDT算法進行精配準該方法的配準精度不僅顯著提高，同時效率也有一定的提升。

2 兩種經典點云配準算法

在同步定位和建圖（SLAM）中，分為前端和后端，前端主要完成的工作就是點云配準工作，本章將分別介紹迭代最近點（ICP）算法與正態分布變換（NDT）算法的基本思想與流程。

2.1 基于迭代最近點（ICP）的點云配準算法

迭代最近點ICP（IterativeClosestPoint）的點云配準算法重復進行“確定對應關系的點集—計算最優剛體變換”的過程，直到收斂條件被滿足，其實質就是基于最小二乘法的最優匹配。該算法的基本原理是：求解兩組點云數據重疊區域內源點云P與目標點云Q之間的空間變換，使兩組點云模型之間的距離最小。設E（R，t）為源點云P在變換矩陣（R，t）下與目標點云Q之間的誤差，稱E（R，t）為目標函數，其中R為旋轉矩陣，t為平移矩陣。則求解最優變換矩陣的問題可以轉化為滿足minE（R，t）的最優解（R，t）。可用下列公式表示：

通過最小化目標函數來求解最優變換矩陣（R，t）。具體步驟為：

（1）計算源點云P中的每個點在目標點云Q中的最近點；

（2）利用SVD分解計算旋轉矩陣R和平移矩陣t，使得目標函數E（R，t）最小；

（3）對源點云P通過求得的旋轉矩陣R和平移矩陣t進行旋轉和平移變換，得到新的對應點集P'；

（4）計算新的點集P'與目標點云Q之間的平均距離d；

（5）如果平均距離d小于給定的閾值或大于預設的最大迭代次數，則停止迭代計算。否則返回第一步，直到滿足收斂條件為止。

2.2 基于正態分布變換（NDT）的點云配準算法

正態分布變換，是一種統計學模型。如果一組隨機向量滿足正態分布，那么概率密度函數可用下列公式表示：

其中D表示維數，表示均值向量，Σ表示隨機向量的協方差矩陣。NDT算法能夠通過概率的形式描述點云的分布情況，這種形式有利的減少了配準所需要的時間。具體算法流程為：

（1）網格化目標點云。利用立方體將激光點云所在的空間進行柵格化，使激光點云處于相應的立方體中；

（2）目標點云劃分好后，求出每一個網格內的正態分布概率密度函數；其中網格太大或太小都有風險，因此一般保證網格內至少有5個點；

（3）求出源點云相對目標點云的初始坐標變換參數，即旋轉矩陣R和平移矩陣t；這一步是為下一步變換參數的迭代提供了距離最優點較近的初值；

（4）源點云進行初始坐標轉換，并計算在目標點云網格中的概率；源點云根據上一步求出的（R，t），將坐標轉換到目標點云網格上，轉換后的源點云坐標，對應所在網格的正態分布概率密度函數，求出轉換后激光點坐標的概率；

（5）將每個點云的概率乘積作為目標似然函數。概率乘積最大時的似然函數，就是最優的坐標轉換，如式：

其中－logφ為最大似然函數的負對數形式，這種形式是為了使之后求導更方便。

最后利用牛頓迭代法，找出最優變換參數完成點云配準。

3 總結

本文主要介紹了兩種點云匹配算法，兩種算法特點具體對比如表1所示：

表1 兩種經典點云匹配方法特點

基于迭代最近點（ICP）的點云配準算法，可以獲得非常精確的配準效果不必對處理的點集進行分割和特征提取，同時有較好的初值時，可以得到很好的算法收斂性。相比于基于正態分布變換（NDT）的點云配準算法，ICP算法在搜索對應點的過程中，計算量非常大，且收斂域小，運行速度慢，同時因為標注ICP算法中尋找對應點時，認為歐氏距離最近的點就是對應點，這種假設也會生成的錯誤對應點。同 時基于正態分布變換（NDT）算法不僅擁有更好的魯棒性，相比于ICP算法效率也有了提高。