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

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

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

? ? 定義

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

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

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

? ? 作用

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

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

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

? ? 方法

? ? 一、ICP

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

? 二、NDT

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

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

? （1）將目標點云劃分為一個個規(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進行正態(tài)分布建模N(q，C)，我們便可以得到這個網(wǎng)格單元中每個點的高斯概率密度函數(shù)P(X)。

?

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

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

? 三、深度學習

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

? ? ? 初值獲取

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

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

? ? 展望

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

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

編輯：黃飛

?