衣明悅，石 晶，趙梓杉

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

前言

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

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

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

1 發(fā)展現(xiàn)狀

1.1 迭代最近點（ICP）算法國內外發(fā)展現(xiàn)狀

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

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

1.2 正態(tài)分布變換（NDT）算法國內外發(fā)展現(xiàn)狀

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

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

2 兩種經(jīng)典點云配準算法

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

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

迭代最近點ICP（IterativeClosestPoint）的點云配準算法重復進行“確定對應關系的點集—計算最優(yōu)剛體變換”的過程，直到收斂條件被滿足，其實質就是基于最小二乘法的最優(yōu)匹配。該算法的基本原理是：求解兩組點云數(shù)據(jù)重疊區(qū)域內源點云P與目標點云Q之間的空間變換，使兩組點云模型之間的距離最小。設E（R，t）為源點云P在變換矩陣（R，t）下與目標點云Q之間的誤差，稱E（R，t）為目標函數(shù)，其中R為旋轉矩陣，t為平移矩陣。則求解最優(yōu)變換矩陣的問題可以轉化為滿足minE（R，t）的最優(yōu)解（R，t）?？捎孟铝泄奖硎荆?/p>

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

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

（2）利用SVD分解計算旋轉矩陣R和平移矩陣t，使得目標函數(shù)E（R，t）最?。?/p>

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

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

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

2.2 基于正態(tài)分布變換（NDT）的點云配準算法

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

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

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

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

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

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

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

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

最后利用牛頓迭代法，找出最優(yōu)變換參數(shù)完成點云配準。

3 總結

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

表1 兩種經(jīng)典點云匹配方法特點

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