激光點云系列之三：點云配準(zhǔn)

作者 | 奚少華

在第一篇《詳解激光雷達(dá)點云數(shù)據(jù)的處理過程》和第二篇《激光雷達(dá)點云處理中遇到的問題及對策》的激光點云系列文中，筆者分別分析了激光雷達(dá)的點云處理流程和點云處理環(huán)節(jié)中遇到的典型問題及對策。在本篇文章中，筆者將重點分析激光點云在定位環(huán)節(jié)中的點云配準(zhǔn)技術(shù)。

由于受到視場角的限制，激光雷達(dá)在實時采集點云數(shù)據(jù)時，只能獲得有限視野范圍內(nèi)的點云圖像。為了獲得三維場景的點云數(shù)據(jù)，感知算法人員需要在已知的初始姿態(tài)信息下，將采集到的前后兩幀不同點云轉(zhuǎn)換到統(tǒng)一坐標(biāo)系下，將點云重合的部分拼接在一起——這就是點云配準(zhǔn)技術(shù)。

下文將逐一分析點云配準(zhǔn)具體是什么、點云配準(zhǔn)的技術(shù)方法、點云配準(zhǔn)面臨的問題及對策。

01、點云配準(zhǔn)在自動駕駛方面的應(yīng)用

點云配準(zhǔn)早先主要應(yīng)用于建筑行業(yè)中的建筑信息模型（BIM）、采礦行業(yè)中的礦區(qū)開采等，而在自動駕駛領(lǐng)域內(nèi)的作用主要有三類，分別為三維地圖構(gòu)建、高精地圖定位、姿態(tài)估計。

第一，三維地圖構(gòu)建。在建高精地圖時，自動駕駛系統(tǒng)通過激光雷達(dá)采集回來的相鄰幀點云進行點云配準(zhǔn)后，將不同位置采集回來的點云統(tǒng)一到一個坐標(biāo)系下，然后構(gòu)建出一個三維的高精度地圖。

第二，高精地圖定位。在自動駕駛車輛行駛時，車輛需要做到厘米級的精準(zhǔn)定位。例如，自動駕駛車輛需要估計其在地圖上的精確位置及車與道路路沿的距離。自動駕駛系統(tǒng)通過點云配準(zhǔn)技術(shù)，將實時采集到的點云數(shù)據(jù)與高精地圖的數(shù)據(jù)做匹配，為自動駕駛車輛給出精確的定位。

第三，姿態(tài)估計。自動駕駛系統(tǒng)通過點云配準(zhǔn)技術(shù)來估計車輛的相對姿態(tài)信息，然后并有利于對車輛做決策規(guī)劃。

02、點云配準(zhǔn)的工作原理與技術(shù)方法

2.1 點云配準(zhǔn)的工作原理

點云配準(zhǔn)的工作原理是激光雷達(dá)由于受到環(huán)境等各種因素的限制，在點云采集過程中單次采集到的點云只能覆蓋目標(biāo)物表面的一部分，為了得到完整的目標(biāo)物點云信息，就需要對目標(biāo)物進行多次掃描，并將得到的三維點云數(shù)據(jù)進行坐標(biāo)系的剛體變換，把目標(biāo)物上的局部點云數(shù)據(jù)轉(zhuǎn)換到同一坐標(biāo)系下。

通俗點來說，點云配準(zhǔn)的關(guān)鍵是如何尋找到初始點云和目標(biāo)點云之間的對應(yīng)關(guān)系，然后通過這個對應(yīng)關(guān)系將原始點云和目標(biāo)點云進行匹配，并計算出它們的特征相似度，最后統(tǒng)一到一個坐標(biāo)系下。

點云配準(zhǔn)通?？煞譃閮蓚€步驟，分別是粗配準(zhǔn)和精配準(zhǔn)。

粗配準(zhǔn)，即點云的初始配準(zhǔn)，指的是通過一個旋轉(zhuǎn)平移矩陣的初值，將兩個位置不同的點云盡可能地對齊。粗配準(zhǔn)的主流方法包括RANSAC、4PCS等。

經(jīng)過粗配準(zhǔn)之后，兩片點云的重疊部分已經(jīng)可以大致對齊，但精度還遠(yuǎn)遠(yuǎn)達(dá)不到自動駕駛車輛的定位要求，需要進一步做精配準(zhǔn)。

精配準(zhǔn)指的是在初始配準(zhǔn)的基礎(chǔ)上，進一步計算兩個點云近似的旋轉(zhuǎn)平移矩陣。精配準(zhǔn)的主流方法包括ICP、NDT、深度學(xué)習(xí)等。

2.2 點云配準(zhǔn)的技術(shù)方法

上文提到了一些點云配準(zhǔn)的具體方法，由于各家自動駕駛公司的技術(shù)水平與技術(shù)方案都不同，所以他們會采用不同的點云配準(zhǔn)方法——有些公司會在粗配準(zhǔn)或者精配準(zhǔn)過程中只采取一種方法，也有些公司會采取多種組合的方法。比如，在精準(zhǔn)配過程中，某些公司會采用ICP+深度學(xué)習(xí)的方式。該章節(jié)將詳細(xì)論述上文的幾種點云配準(zhǔn)方法。

2.2.1 粗配準(zhǔn)的技術(shù)方法

（1）RANSAC（RAndom SAmple Consensus,隨機采樣一致）

方法原理：該算法從給定的樣本集中隨機選取一些樣本并估計一個數(shù)學(xué)模型，將樣本中的其余樣本帶入該數(shù)學(xué)模型中驗證，如果有足夠多的樣本誤差在給定范圍內(nèi)，則該數(shù)學(xué)模型最優(yōu)，否則繼續(xù)循環(huán)該步驟。

RANSAC算法被引入三維點云配準(zhǔn)領(lǐng)域，其本質(zhì)就是不斷的對源點云進行隨機樣本采樣并求出對應(yīng)的變換模型，接著對每一次隨機變換模型進行測試，并不斷循環(huán)該過程直到選出最優(yōu)的變換模型作為最終結(jié)果。

具體步驟：

1）對點云進行降采樣和濾波處理，減少點云的計算量。

2）基于降采樣和濾波處理后的點云數(shù)據(jù)，進行特征提取。

3）使用RANSAC算法進行迭代采樣，獲取較為理想的變換矩陣。

4）使用所獲得的變換矩陣進行點云變換操作。

優(yōu)點：適用于較大點云數(shù)據(jù)量的情況，可以在不考慮點云間距離大小的情況下，都能實現(xiàn)點云的粗配準(zhǔn)。

缺點：存在配準(zhǔn)精度的不穩(wěn)定的問題。

（2）4PCS（4-Points Congruent Sets，全等四點集）

方法原理：該算法利用剛體變換中的幾何不變性（如向量/線段比例、點間歐幾里得距離），根據(jù)剛性變換后交點所占線段比例不變以及點之間的歐幾里得距離不變的特性，在目標(biāo)點云中盡可能尋找４個近似共面點（近似全等四點集）與之對應(yīng)，從而利用最小二乘法計算得到變換矩陣，基于RANSAC算法框架迭代選取多組基，根據(jù)最大公共點集（LCP）的評價準(zhǔn)則進行比較得到最優(yōu)變換。

具體步驟：

1）在目標(biāo)點云集合中尋找滿足長基線要求的共面四點基（基線的確定與輸入?yún)?shù)中overlap有很大關(guān)系，overlap越大，基線選擇越長，長基線能夠保證匹配的魯棒性，且匹配數(shù)量較少）。

2）提取共面四點基的拓?fù)湫畔ⅲ嬎闼狞c基間的兩個比例因子。

3）計算四種可能存在的交點位置，進而計算所有中長基線點對的交點坐標(biāo)，比較交點坐標(biāo)并確定匹配集合，尋找到對應(yīng)的一致全等四點。

4）尋找點云中所有的共面四點集合，重復(fù)上述步驟可得到全等四點集合，并尋找最優(yōu)全等四點對。

優(yōu)點：適用于重疊區(qū)域較小或者重疊區(qū)域發(fā)生較大變化場景點云配準(zhǔn)，無需對輸入數(shù)據(jù)進行預(yù)濾波和去噪。

缺點：不適合工程化應(yīng)用。

2.2.2 精配準(zhǔn)的技術(shù)方法

（1）ICP（Iterative Closest Point，最近點迭代法）

方法原理：選取兩片點云中距離最近的點作為對應(yīng)點，通過所有對應(yīng)點對求解旋轉(zhuǎn)和平移變換矩陣，并通過不斷迭代的方式使兩片點云之間的配準(zhǔn)誤差越來越小，直至滿足我們提前設(shè)定的閾值要求或迭代次數(shù)。

具體步驟：

1）計算源點云中的每一個點在目標(biāo)點集中的對應(yīng)近點。

2）求得使上述對應(yīng)點對平均距離最小的剛體變換，并求得平移參數(shù)和旋轉(zhuǎn)參數(shù)。

3）對求得的平移和旋轉(zhuǎn)矩陣進行空間變換，得到新的變換點集。

4）如果新的變換點集與參考點集滿足兩點集的平均距離小于某一給定閾值，或者迭代次數(shù)達(dá)到設(shè)定的最大值，則停止迭代計算，否則新的變換點集作為新的源點云繼續(xù)迭代，直到達(dá)到目標(biāo)函數(shù)的要求。

優(yōu)點：

1）不需要對點云集進行分割和特征提取。

2）在初值較好的情況下，可以得到很好的算法收斂性。

缺點：

1）在搜索對應(yīng)點的過程中，計算量較大，計算速度較慢。

2）對配準(zhǔn)點云的初始位置有一定要求，不合理的初始位置會導(dǎo)致算法陷入局部最優(yōu)。

3）ICP算法在尋找對應(yīng)點時，模型會將任何兩個點云之間的歐氏距離最近的點作為對應(yīng)點，這種假設(shè)會產(chǎn)生一定數(shù)量的錯誤對應(yīng)點。

（2）NDT（Normal Distribution Transform，正態(tài)分布點云算法）

方法原理：先對待配準(zhǔn)點云進行柵格化處理，將其劃分為指定大小的網(wǎng)格，通過正態(tài)分布的方式，構(gòu)建每個網(wǎng)格的概率分布函數(shù)，之后優(yōu)化求解出最優(yōu)變換參數(shù)，使得源點云概率密度分布達(dá)到最大，以實現(xiàn)兩個點云之間的最佳匹配。

具體步驟：

1）將空間劃分成各個格子cell（也叫體素voxel grid）。

2）將參考點云投到各個網(wǎng)格中。

3）基于網(wǎng)格中的點，計算格子的正態(tài)分布概率密度函數(shù)（probability density function, PDF）的參數(shù)。

4）將第二幅點云的每個點按轉(zhuǎn)換矩陣變換。

5）第二幅掃描點云的點落于參考點云的哪個網(wǎng)格，計算相應(yīng)的概率分布函數(shù)PDF。

6）求最大似然函數(shù)，得到最優(yōu)變換參數(shù)。

優(yōu)點：

1）對初始配準(zhǔn)的要求不高，即使初始值的誤差較大，也能有很好的效果。

2）不需要進行點云之間的特征匹配，避免了特征匹配中出現(xiàn)的問題，例如點云噪聲、物體移動、點云重合度對特征匹配的影響。

缺點：

1）對體素格子大小的要求較高，太大會導(dǎo)致配準(zhǔn)精度低，而太小會導(dǎo)致計算量加大。

2）配準(zhǔn)精度相比ICP算法略低。

（3）基于深度學(xué)習(xí)的點云配準(zhǔn)

深度學(xué)習(xí)除了應(yīng)用在感知層面，也應(yīng)用到了定位層面的點云配準(zhǔn)環(huán)節(jié)中。

基于深度學(xué)習(xí)的點云配準(zhǔn)指的是利用深度學(xué)習(xí)模型來提取原始點云的特征，從而獲取到點云的初始配置值，然后根據(jù)特征值再進一步地完成精配準(zhǔn)。近年來，常見的基于深度學(xué)習(xí)的點云配準(zhǔn)方法包括PointNetLK，Deep ICP，DCP，PRNet，IDAM，RPM-Net，3DRegNet，DGR等。

相比于其它傳統(tǒng)的配準(zhǔn)方法（ICP和NDT），基于深度學(xué)習(xí)的方法可以使得計算速度更快，并能學(xué)習(xí)到更高級的特征，從而達(dá)到更高的魯棒性。

根據(jù)配準(zhǔn)方法的結(jié)構(gòu)是完全由深度神經(jīng)網(wǎng)絡(luò)組成還是將非深度學(xué)習(xí)方法的一部分組件替換為基于深度學(xué)習(xí)的網(wǎng)絡(luò)，將基于學(xué)習(xí)的點云配準(zhǔn)方法分為部分深度學(xué)習(xí)的方法和端到端的深度學(xué)習(xí)方法。

部分深度學(xué)習(xí)的點云配準(zhǔn)方法是指直接用基于深度學(xué)習(xí)的組件替換掉非深度學(xué)習(xí)點云配準(zhǔn)方法中的某個組件，這就可能給原來的算法帶來速度或魯棒性的提升。部分深度學(xué)習(xí)的點云配準(zhǔn)方法最大的優(yōu)勢在于靈活性較大。

端到端的點云配準(zhǔn)方法是指從點云的輸入到最后的配準(zhǔn)結(jié)果都在一個完整的網(wǎng)絡(luò)中實現(xiàn)。端到端的點云配準(zhǔn)方法能夠最大程度地發(fā)揮深度學(xué)習(xí)方法的高效和智能，也能夠更好地發(fā)揮GPU的并行計算能力，有更快的計算速度。

當(dāng)前，自動駕駛行業(yè)內(nèi)應(yīng)用點云配準(zhǔn)技術(shù)仍屬于前期階段。某自動駕駛公司感知算法工程師說：“基于深度學(xué)習(xí)的點云配準(zhǔn)仍然處于早期階段，其應(yīng)用的精度無法保證，而且結(jié)合ICP等傳統(tǒng)方法的應(yīng)用也需要大量的計算時間?！?/p>

03、點云配準(zhǔn)面臨的問題及對策

3.1 配準(zhǔn)時點云數(shù)量過多的問題及對策

當(dāng)前，車上的數(shù)據(jù)都是先上傳到云端再處理，但這樣一方面數(shù)據(jù)的回傳成本很高，另一方面，無法做到數(shù)據(jù)處理的實時性，進而難以保障自動駕駛車輛的安全性，因此，需要將數(shù)據(jù)處理環(huán)節(jié)在車端完成。

現(xiàn)有量產(chǎn)車上的算力有限，無法直接處理龐大的點云數(shù)據(jù)，這成為了點云配準(zhǔn)時的一大障礙，而解決這一問題的主要方法就是點云精簡。

點云精簡是指將目標(biāo)點云數(shù)據(jù)集合中的無用點云去除。主要可以有兩種方式來實現(xiàn)：

1）去除冗余是指在數(shù)據(jù)配準(zhǔn)之后，將一些重復(fù)區(qū)域的數(shù)據(jù)予以去除，而這部分?jǐn)?shù)據(jù)的數(shù)據(jù)量大，并且多為無用數(shù)據(jù)，對建模的速度以及質(zhì)量有很大影響。

2）抽稀簡化是指由于激光雷達(dá)掃描獲得的點云數(shù)據(jù)密度過大、數(shù)量過多，其中一部分?jǐn)?shù)據(jù)對于后期建模用處不大，所以在滿足一定精度以及保持被測物體幾何特征的前提下，該部分?jǐn)?shù)據(jù)就需要進行精簡，以提高數(shù)據(jù)的實際運算速度、建模效率以及模型精度。

3.2 點云初始配準(zhǔn)的問題及對策

在前文中提到，初始配準(zhǔn)也叫作粗配準(zhǔn)，它是點云配準(zhǔn)的關(guān)鍵點，一般通過IMU和車輛輪速計的運動數(shù)據(jù)來獲取配準(zhǔn)的初始值，但當(dāng)前行業(yè)內(nèi)點云初始配準(zhǔn)的效率不高。

提高初始配準(zhǔn)的效率可以縮小不同視角下的點云之間的旋轉(zhuǎn)錯位和平移錯位，同時可以提高點云配準(zhǔn)整體的成功率和效率。

對于ICP算法來說，初始配準(zhǔn)的初始值效果會很大程度上影響ICP算法的精度。對于NDT算法來說，雖然初始值對于算法的效果不敏感，但是提高初始配準(zhǔn)的效率和質(zhì)量，也同樣能提升NDT算法的配準(zhǔn)精度。

那么，在自動駕駛領(lǐng)域內(nèi)，技術(shù)人員是如何提高點云的初始配準(zhǔn)效率？

一種是抓取目標(biāo)關(guān)鍵點。縱目科技感知算法工程師湯強說：“為了提高點云的初始配準(zhǔn)，最直接的方法就是提取選取一個關(guān)鍵目標(biāo)，比如一輛車，然后從這個關(guān)鍵目標(biāo)上選取幾個關(guān)鍵點去做旋轉(zhuǎn)矩陣和平移矩陣?！?/p>

另一種是特征提取，比如FPFH（快速點特征直方圖）。圖達(dá)通算法負(fù)責(zé)人許建博士說：“區(qū)別于ICP算法和NDT算法，基于特征提取的點云配準(zhǔn)方法可以提高點云配準(zhǔn)速度和精度。它通過提取的一些特征后（如棱角或者線、面），利用兩個點云間的空間拓?fù)浣Y(jié)構(gòu)，來進行點云之間的匹配。此外，實際工程應(yīng)用中，數(shù)據(jù)關(guān)聯(lián)、系統(tǒng)偏差估計等在線點云配準(zhǔn)技術(shù)也非常重要?！?/p>

3.3 同源點云配準(zhǔn)的問題及對策

同源點云配準(zhǔn)是指從同一類型的傳感器獲取點云數(shù)據(jù)，但在不同的時間和視角下進行的點云配準(zhǔn)。

在自動駕駛車輛上，除了一顆主激光雷達(dá)外，車端可能還會配置一些補盲激光雷達(dá)，或者采用兩顆以上的主激光雷達(dá)方案。這些激光雷達(dá)采集的點云都屬于是同源點云數(shù)據(jù)，但是從不同激光雷達(dá)采集回來的點云，由于從不同時間點和不同視角采集的情況，在點云配準(zhǔn)時會遇到一些問題。

一方面，多個激光雷達(dá)從不同角度采集回來的點云數(shù)據(jù)，它們的三維坐標(biāo)系不同，并且產(chǎn)生的噪點所在的坐標(biāo)系也不同。在做點云配準(zhǔn)時，點云數(shù)據(jù)需要統(tǒng)一到一個坐標(biāo)系下才可以實現(xiàn)更精準(zhǔn)的配準(zhǔn)效果。

許建說：“為了能讓多個激光雷達(dá)采集的點云統(tǒng)一到同一坐標(biāo)系下，在聯(lián)合標(biāo)定時，多目標(biāo)的點云配準(zhǔn)會比單目標(biāo)的點云配準(zhǔn)帶來更多的信息，使得配準(zhǔn)精度會更高。單一目標(biāo)的話，激光掃描到目標(biāo)物上的時，目標(biāo)邊緣點云不完整，會影響配準(zhǔn)的精度。多目標(biāo)的話，激光雷達(dá)采集的點云信息會更多。比如，目標(biāo)靜止的時候，多目標(biāo)之間就會形成拓?fù)浣Y(jié)構(gòu)信息，使得點云初始配準(zhǔn)的信息更豐富，有利于提高配準(zhǔn)整體的精度?！?/p>

另一方面，不同激光雷達(dá)之間的硬件性能指標(biāo)可能不同，這會造成同一目標(biāo)物表面覆蓋的點云數(shù)量可能都不同。

湯強說：“不同性能的激光雷達(dá)，它們的角分辨率參數(shù)都不同，意味著點云密度不同，導(dǎo)致點云配準(zhǔn)最終效果會受到影響。為了調(diào)節(jié)點云密度的不同，感知算法人員可以應(yīng)用點云精簡的方法，降低點云密度較高的激光雷達(dá)參數(shù)，也可以從硬件上進行調(diào)節(jié)，例如，技術(shù)人員可能會將兩個激光雷達(dá)的角分辨率調(diào)節(jié)至同一個水平上?！?/p>

3.4 跨源點云配準(zhǔn)的問題及對策

跨源點云配準(zhǔn)是指對不同類型傳感器的點云進行配準(zhǔn)。它的優(yōu)點是結(jié)合多個不同類型的傳感器各自的優(yōu)勢，為自動駕駛系統(tǒng)提供更豐富的三維點云信息。相比于同源點云配準(zhǔn)，跨源點云配準(zhǔn)尚處于學(xué)術(shù)階段，而其在自動駕駛領(lǐng)域的應(yīng)用也是預(yù)研階段。

某自動駕駛方案商的技術(shù)專家說：“跨源點云配準(zhǔn)是未來點云配準(zhǔn)技術(shù)的主要挑戰(zhàn)之一?！?/p>

具體來說，由于不同類型的傳感器之間的工作原理不同，傳感器之間的點云的分辨率、點云密度質(zhì)量、點云圖像畫面尺度、點云距離信息等都會不相同，而這些問題造成了跨源點云配準(zhǔn)的技術(shù)實現(xiàn)難度大大增加。

以4D成像雷達(dá)與激光雷達(dá)為例，雖然4D成像雷達(dá)可以與激光雷達(dá)一樣，能夠形成點云數(shù)據(jù)，但是4D成像雷達(dá)的點云密度或許只能達(dá)到某些低線數(shù)的激光雷達(dá)（如16線、32線等），而無法達(dá)到高線數(shù)激光雷達(dá)（如128線等）的點云密度。另外，在角分辨率參數(shù)上，4D成像雷達(dá)只能接近1°的參數(shù)水平，而某些激光雷達(dá)可以做到0.1°的水平。當(dāng)兩個傳感器掃描到同一個物體表面時，它們各自采集到的點云效果也會不同。例如在車輛前方較遠(yuǎn)處的點云數(shù)據(jù)中，激光雷達(dá)的點云質(zhì)量或許比4D毫米波的點云質(zhì)量會更好（前者的能量返回更多以及點云數(shù)量更多）， 造成了最終點云配準(zhǔn)的精度下降。

在現(xiàn)有的學(xué)術(shù)研究中，跨源點云配準(zhǔn)的方法已有很多，但主要是在增強現(xiàn)實、建筑施工等領(lǐng)域內(nèi)有應(yīng)用，在自動駕駛領(lǐng)域內(nèi)幾乎沒有相應(yīng)的應(yīng)用。下圖為筆者整理的一些跨源點云配準(zhǔn)的方法。比如，CSGM是將配準(zhǔn)問題轉(zhuǎn)化為圖匹配問題，并利用圖匹配理論來克服跨源點云配準(zhǔn)的問題。再比如，F(xiàn)MR是探討了深度學(xué)習(xí)在跨源點云配準(zhǔn)的方面的應(yīng)用。

?圖：跨源點云配準(zhǔn)方法

（數(shù)據(jù)來源：《2021最新關(guān)于點云配準(zhǔn)的全面綜述》）

參考文獻

【1】點云配準(zhǔn)，自動駕駛里的“滴血認(rèn)親”https://mp.weixin.qq.com/s/xjg0UOqOsukgx1swxBKwHA

【2】2021最新關(guān)于點云配準(zhǔn)的全面綜述https://mp.weixin.qq.com/s/SOtHePcis9S9hpN1oqP9Ug

【3】RANSAC算法思想與實現(xiàn)點云粗配準(zhǔn)https://blog.csdn.net/qq_42537872/article/details/126069525

【4】3D點云配準(zhǔn)算法-4PCS（4點全等集配準(zhǔn)算法）https://blog.csdn.net/weixin_42755079/article/details/126638986

【5】ICP點云配準(zhǔn)算法https://blog.csdn.net/

【6】點云配準(zhǔn)ICP&NDThttps://blog.csdn.net/weixin_49024732/article/details/124794910

【7】基于學(xué)習(xí)的點云配準(zhǔn)方法https://zhuanlan.zhihu.com/p/501503130