自動(dòng)駕駛?cè)谠挘撼Ｓ玫狞c(diǎn)云配準(zhǔn)方法以及未來發(fā)展方向

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

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

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

? ? 定義

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

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

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

? ? 作用

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

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

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

? ? 方法

? ? 一、ICP

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

? 二、NDT

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

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

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

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

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

?

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

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

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

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

? ? ? 初值獲取

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

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

? ? 展望

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

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

編輯：黃飛

?