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

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

? 點云配準作為點云處理整個Pipeline中的重要一環，便是解決上述問題的良方妙藥。自動駕駛圈黑話第二十四期，我們就來科普一下什么是點云配準，點云配準的作用，常用的點云配準方法以及未來的發展方向。

? ? 定義

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

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

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

? ? 作用

? 地圖構建。在高精地圖制作環節中，制作點云地圖是第一步。通過對不同位置采集的連續幀點云進行配準，可以將不同位置的多幀點云統一到同一坐標系，構建場景的完整點云地圖。

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

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

? ? 方法

? ? 一、ICP

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

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

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

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

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

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

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

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

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

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

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

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

? 而為了優化局部最優問題，出現了一些ICP改進算法，包括點到面ICP，面到面ICP等。

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

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

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

? 二、NDT

? NDT（Normal Distribution Transform，正態分布變換），將目標點云（可以是高精點云地圖）轉換為多維變量的正態分布，并采用最優化技術尋找最優變換，使得待配準點云經過變換之后在目標點云中的概率密度之和最大。

? NDT方法實現過程用數學語言可以描述為：

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

? （2）假設每個網格中的點均服從正態分布，并計算每個網格中點的均值向量q和協方差矩陣C。

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

?

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

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

? 三、深度學習

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

? ? ? 初值獲取

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

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

? ? 展望

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

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

編輯：黃飛

?