衣明悅，石 晶，趙梓杉

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

前言

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

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

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

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

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

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

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

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

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

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

2 兩種經(jīng)典點(diǎn)云配準(zhǔn)算法

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

2.1 基于迭代最近點(diǎn)（ICP）的點(diǎn)云配準(zhǔn)算法

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

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

（1）計(jì)算源點(diǎn)云P中的每個(gè)點(diǎn)在目標(biāo)點(diǎn)云Q中的最近點(diǎn)；

（2）利用SVD分解計(jì)算旋轉(zhuǎn)矩陣R和平移矩陣t，使得目標(biāo)函數(shù)E（R，t）最??；

（3）對(duì)源點(diǎn)云P通過求得的旋轉(zhuǎn)矩陣R和平移矩陣t進(jìn)行旋轉(zhuǎn)和平移變換，得到新的對(duì)應(yīng)點(diǎn)集P'；

（4）計(jì)算新的點(diǎn)集P'與目標(biāo)點(diǎn)云Q之間的平均距離d；

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

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

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

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

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

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

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

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

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

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

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

3 總結(jié)

本文主要介紹了兩種點(diǎn)云匹配算法，兩種算法特點(diǎn)具體對(duì)比如表1所示：

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

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