隨著激光雷達(dá)、4D毫米波雷達(dá)逐漸成為汽車領(lǐng)域的頂流，其輸出的點云(Point Cloud)也成為繼像素之后描述車輛所處三維世界的一種主要數(shù)據(jù)格式。點云其實就是一個數(shù)據(jù)集，不同類型傳感器輸出的點云包含的數(shù)據(jù)略有差異。對于激光雷達(dá)來說，其輸出的點云一般包括三維空間坐標(biāo)（x，y，z）、反射強(qiáng)度信息、時間戳等數(shù)據(jù)。
?

但是，受傳感器視場角限制，單個傳感器往往只能獲得有限視野范圍內(nèi)的點云，如何才能生成完整場景的三維點云，以便支持后續(xù)的物體識別、分類及路徑規(guī)劃？自動駕駛車輛預(yù)先存儲了某一區(qū)域的高精度點云地圖，如何通過車輛傳感器實時獲取的點云確定車輛當(dāng)前的位置信息？在初始姿態(tài)信息已知，如何通過車輛傳感器采集的前后兩幀點云估計相對姿態(tài)變化信息？

? 點云配準(zhǔn)作為點云處理整個Pipeline中的重要一環(huán)，便是解決上述問題的良方妙藥。自動駕駛?cè)谠挼诙钠冢覀兙蛠?a href="http://www.asorrir.com/v/tag/858/" target="_blank">科普一下什么是點云配準(zhǔn)，點云配準(zhǔn)的作用，常用的點云配準(zhǔn)方法以及未來的發(fā)展方向。

? ? 定義

? 點云配準(zhǔn)（Pont Cloud Registration）,又名點云拼接、點云注冊，對于兩幀有重疊信息的點云，通過求解變換矩陣（旋轉(zhuǎn)矩陣R和平移矩陣T），使得重疊部分點云變換到同一個統(tǒng)一的坐標(biāo)系下。

? 點云配準(zhǔn)史上經(jīng)歷了人工、半自動、自動配準(zhǔn)階段。人工配準(zhǔn)，指的是工程師通過手眼調(diào)整兩幅點云，直至達(dá)到人眼意義上的配準(zhǔn)。半自動配準(zhǔn)，是指依賴儀器進(jìn)行配準(zhǔn)。自動配準(zhǔn)，是指利用一定的算法，自動完成兩幀點云的配準(zhǔn)，這也是本文所要重點介紹的內(nèi)容。

? 點云配準(zhǔn)包含粗配（Coarse Registration）準(zhǔn)和精配準(zhǔn)（Fine Registration）兩步。粗配準(zhǔn)指的是在兩幀點云位置相差較大，相對位姿完全未知的情況下進(jìn)行較為粗糙的配準(zhǔn)，目的是為后續(xù)精配準(zhǔn)提供較好的變換初值。精配準(zhǔn)在給定初始變換矩陣條件下，進(jìn)一步優(yōu)化得到更精確的變換。

? ? 作用

? 地圖構(gòu)建。在高精地圖制作環(huán)節(jié)中，制作點云地圖是第一步。通過對不同位置采集的連續(xù)幀點云進(jìn)行配準(zhǔn)，可以將不同位置的多幀點云統(tǒng)一到同一坐標(biāo)系，構(gòu)建場景的完整點云地圖。

? 高精定位。對于自動駕駛來說，獲取準(zhǔn)確的定位是一切豐滿理想實現(xiàn)的前提。刨去Tesla，其他絕大多數(shù)自動駕駛公司都將高精地圖作為實現(xiàn)準(zhǔn)確定位的必備武器。而通過點云配準(zhǔn)技術(shù)，可以將車輛傳感器實時掃描到的點云與已經(jīng)預(yù)存的高精點云地圖進(jìn)行匹配，并將得到的定位結(jié)果與GNSS輸出、IMU輸出、里程計輸出等融合，實現(xiàn)車輛最終高精定位結(jié)果輸出。

? 姿態(tài)估計。通過對車輛傳感器實時獲得的前后兩幀點云進(jìn)行配準(zhǔn)，可以估計出車輛的相對姿態(tài)信息。

? ? 方法

? ? 一、ICP

? ICP（Iterative Closest Point，迭代最近點）應(yīng)該是最為經(jīng)典的配準(zhǔn)方法之一，從1992年P(guān).J.Besl提出，到今天（2022年），已經(jīng)整整三十年，但I(xiàn)CP的光芒卻絲毫未減。從最早的點到點ICP，發(fā)展出了點到線ICP，點到面ICP及面到面ICP等方法，共同奠定ICP方法在點云配準(zhǔn)領(lǐng)域的劃時代的意義。

? 最早的點到點ICP方法的核心思想很簡單：空間位置上同一個點在兩幀點云中距離是最近的，利用平移旋轉(zhuǎn)變換后，讓這兩幀點云在相同位置的坐標(biāo)盡可能拉近甚至相等，以達(dá)到配準(zhǔn)的效果。

? 點到點ICP方法包含對應(yīng)點搜索和變換估計兩個階段，對應(yīng)點搜索是指在兩幀點云中找到每個點的匹配點，變換估計就是利用對應(yīng)關(guān)系來估計變換矩陣。這兩個階段將不斷進(jìn)行迭代，使得對應(yīng)關(guān)系越來越準(zhǔn)確，從而找到最佳的變換矩陣。 點到點ICP方法實現(xiàn)過程用數(shù)學(xué)語言可以描述為：

? （1）給定待配準(zhǔn)點云PS，也就是當(dāng)前傳感器掃描到的點云，里面由n個三維點pi組成。同時我們有目標(biāo)點云Pt，可以是預(yù)先建好的高精點云地圖，也可以是其他位置傳感器采集的但有重疊區(qū)域的點云，里面同樣包含一系列三維點pj組成。

（2）而我們要做的就是尋找一個3x3旋轉(zhuǎn)矩陣R和3x1平移矩陣t，使得下式取值最小。

（3）采用四元數(shù)法或SVD方法求解上R和t的值。

? 一步到位尋找兩幀點云最近點，需要將待配準(zhǔn)點云中每個點依次和目標(biāo)點云中每個點進(jìn)行距離計算，不僅計算復(fù)雜度高，而且計算耗時久。因此通常設(shè)置距離閾值來加速這一過程，當(dāng)待配準(zhǔn)點云中的一個點與目標(biāo)點云中的一個點距離小于一定閾值時就認(rèn)為找到了對應(yīng)點，不再遍歷待配準(zhǔn)點云和目標(biāo)點云中每個點。

? 經(jīng)過一次迭代，調(diào)整一些對應(yīng)點對的權(quán)重以及剔除一些不合理的對應(yīng)點對，我們可以得到一個臨時變換點云。通過計算代價函數(shù)，我們便可以得到臨時變換點云和目標(biāo)點云對應(yīng)的最優(yōu)變換。然后利用這個臨時點云和目標(biāo)點云再進(jìn)行比較，再得到一個變換點云及最優(yōu)變換，不斷迭代這一過程，直到找到待配準(zhǔn)點云中每一個點在目標(biāo)點云中的最近點及全局最優(yōu)變換。

? 形象化的配準(zhǔn)過程如下圖所示。在給定良好初值的兩幀點云中，通過不斷迭代，待配準(zhǔn)點云沿箭頭指向方向不斷向目標(biāo)點云靠近，并最終實現(xiàn)目標(biāo)點云和待配準(zhǔn)點云的配準(zhǔn)。
?

點到點ICP方法以簡單著稱，不需要對點云進(jìn)行分割和特征提取，不需要訓(xùn)練數(shù)據(jù)，可以較好的推廣到未知場景。且在初值較好情況下，嚴(yán)密的數(shù)學(xué)理論可以保證較好的精度和收斂性。

? 但是點到點ICP方法遍歷所有點云，在點云數(shù)量較大時，計算量是驚人的。同時點到點ICP方法只考慮點與點的距離，缺少對點云特征信息的利用，在所處環(huán)境不斷變化以及測量誤差影響下，會產(chǎn)生較大的配準(zhǔn)誤差。而最致命的問題是，點對點ICP方法初始值敏感，容易陷入局部最優(yōu)，導(dǎo)致配準(zhǔn)失敗。
? 當(dāng)上圖在初值不好的情況下，如下圖所示。按照對應(yīng)點距離最小原則，待配準(zhǔn)點云將按箭頭指示順時針方向旋轉(zhuǎn)。這種情況雖然會導(dǎo)致兩邊對應(yīng)點距離變遠(yuǎn)，但是刨去兩邊的剩余大部分點距離在減少。這也就是點到點ICP方法因初值不好帶來的局部最優(yōu)問題。
?

而為了解決遍歷點云中所有點引入的高計算量，點到點ICP方法引入特征點的概念。特征點是指點云中具有代表性的點，能夠盡可能代表點云的有效空間信息，比如拐角點，凸凹分界處的點等。通過對點云做特征點提取，然后再使用點到點ICP方法，可以顯著減少運算量，保持較高的精度。

? 而為了優(yōu)化局部最優(yōu)問題，出現(xiàn)了一些ICP改進(jìn)算法，包括點到面ICP，面到面ICP等。

? 點到面ICP考慮待配準(zhǔn)點云頂點到目標(biāo)點云頂點所在面的距離，比起直接計算點到點距離，考慮了目標(biāo)點云的局部結(jié)構(gòu)，精度更高，不容易陷入局部最優(yōu)。但是點到面ICP是一個非線性問題，速度比較慢，效率較低。

? 面到面ICP考慮待配準(zhǔn)點云頂點所在的面和目標(biāo)點云頂點所在面的距離，同時考慮了待配準(zhǔn)點云和目標(biāo)點云的局部結(jié)構(gòu)，精度更高，更不易陷入局部最優(yōu)。但是同樣作為一個非線性優(yōu)化問題，速度更慢，效率更低。

? 因此點到點ICP方法常用于粗配準(zhǔn)完成，已有較好初值的精配準(zhǔn)階段。

? 二、NDT

? NDT（Normal Distribution Transform，正態(tài)分布變換），將目標(biāo)點云（可以是高精點云地圖）轉(zhuǎn)換為多維變量的正態(tài)分布，并采用最優(yōu)化技術(shù)尋找最優(yōu)變換，使得待配準(zhǔn)點云經(jīng)過變換之后在目標(biāo)點云中的概率密度之和最大。

? NDT方法實現(xiàn)過程用數(shù)學(xué)語言可以描述為：

? （1）將目標(biāo)點云劃分為一個個規(guī)格的網(wǎng)格(對于2D點云來說，是2D平面；對于3D點云來說，是3D立方體)，同時保證每個網(wǎng)格中最少包含6個點。

? （2）假設(shè)每個網(wǎng)格中的點均服從正態(tài)分布，并計算每個網(wǎng)格中點的均值向量q和協(xié)方差矩陣C。

? （3）基于以上信息，對網(wǎng)格中每個點X進(jìn)行正態(tài)分布建模N(q，C)，我們便可以得到這個網(wǎng)格單元中每個點的高斯概率密度函數(shù)P(X)。

?

式P(X)中，c為歸一化常數(shù)。同時我們看到本式中對協(xié)方差矩陣C求逆，如果單元格中的點數(shù)太少會導(dǎo)致協(xié)方差矩陣條件數(shù)過大而導(dǎo)致誤差過大，故上文要求單元格至少包含六個點。同時每個P(X) 可以看作是單元格表面的近似表達(dá)，描述了單元格表面的位置以及它的方向和平滑度。
? 隨后我們會給定一個初始變換矩陣，將待配準(zhǔn)點云變換并貼近目標(biāo)點云，這樣一來待配準(zhǔn)點云上的點就會落在目標(biāo)點云劃分的網(wǎng)格單元中。以一個網(wǎng)格單元舉例，將待配準(zhǔn)點云中落入這個網(wǎng)格單元中的點Y帶入上文的P(X)中，可得到這個點在這個網(wǎng)格單元中的概率值，這個值是一個似然值。
? 同時為了減少格子離散化，不連續(xù)的影響，我們對這個網(wǎng)格單元分別向下、向左、向下后再向左平移，這樣我們可得到另外三個網(wǎng)絡(luò)單元。而這個點在另外三個網(wǎng)格單元中分別會有一個概率值，將這四個值加起來，作為待配準(zhǔn)點云這個點總的得分（Score）。
?

落在目標(biāo)點云網(wǎng)格單元中的每個點都有一個Score值，將所有Score值相乘就是我們的目標(biāo)函數(shù)了，而我們最終的目的就是找到最優(yōu)的參數(shù)使得目標(biāo)函數(shù)得分最大（最大似然），那我們也便認(rèn)為配準(zhǔn)了。可以理解為，在目標(biāo)點云中點出現(xiàn)概率大的地方，待配準(zhǔn)點云應(yīng)盡量占據(jù)這些位置。
? NDT方法通過網(wǎng)格劃分技術(shù)，可以支持更大、更稠密的點云地圖配準(zhǔn)。且在配準(zhǔn)過程中不利用對應(yīng)點的特征計算和匹配，所以相比ICP效率更高，計算資源消耗更少。且當(dāng)需要配準(zhǔn)的兩幀點云重疊率低，并且結(jié)構(gòu)化不明顯（平面較少）時，NDP比點ICP更加準(zhǔn)確。

? 三、深度學(xué)習(xí)

? 深度學(xué)習(xí)在自動駕駛領(lǐng)域可謂無往不利，點云配準(zhǔn)領(lǐng)域也不例外。基于深度學(xué)習(xí)的點云配準(zhǔn)算法不斷被提出，包括PointNetLK、DCP、IDAM、RPM-Net、3DRegNet等，且這些深度學(xué)習(xí)模型在實驗室中已經(jīng)證明性能與速度上均遠(yuǎn)超ICP和NDP方法。而在復(fù)雜的實際場景效果如何，值得我們持續(xù)關(guān)注。

? ? ? 初值獲取

? ? 對于ICP來說，初值的好壞在很大程度上決定了兩幀點云配準(zhǔn)的結(jié)果。對于NDT來說，雖說對初值不敏感（兩幀點云位置3m以內(nèi)，角度+/-45°范圍），但良好的初值有利于提高配置的效率和質(zhì)量。不同的車輛配置決定了不同的初值獲取方法。對于只有激光雷達(dá)配置的車輛，可以采用上一幀的變換結(jié)果作為初值。
? 對于配置有IMU和里程計的車型，IMU的加速度測量比較準(zhǔn)，積分出來的速度和位移會有累計誤差。里程計的速度測量比較準(zhǔn)，而根據(jù)運動模型計算出來的加速度會有一定誤差。因此針對此種配置，使用IMU的加速度測量結(jié)果和里程計的速度測量結(jié)果融合一個初值，會是非常可靠的一個結(jié)果。

? 對于配置GNSS的車型，如果GNSS還能輸出航向角，那么直接用GNSS的輸出作為初值。對于無法輸出航向角的GNSS,通過連續(xù)的位置坐標(biāo)，也可以計算一個平面運動的航向角。

? ? 展望

? ? 單一傳感器的點云配準(zhǔn)已有成熟的算法，但可以看到，每種方法都存在局限性，無法滿足自動駕駛?cè)珗鼍暗狞c云配準(zhǔn)需求，如何取長補(bǔ)短成為單一傳感器點云配準(zhǔn)發(fā)展的重要道路。

? 隨著多傳感器融合成為主旋律，不同類別傳感器獲取的點云進(jìn)行配準(zhǔn)就顯得重要且必須。但由于這方面的研究剛剛起步，僅有一些實驗室階段的優(yōu)化策略和深層次的神經(jīng)網(wǎng)絡(luò)模型，如何訓(xùn)練出可以上路的神經(jīng)網(wǎng)絡(luò)模型，值得萬眾期待。
? ?

編輯：黃飛

?