基于點(diǎn)密度調(diào)整的激光雷達(dá)點(diǎn)云配準(zhǔn)改進(jìn)方法?

王翊成 李明磊 魏大洲 吳伯春

（1.南京航空航天大學(xué)電子信息工程學(xué)院 南京 211106）（2.中國航空無線電電子研究所 上海 200233）

1 引言

激光雷達(dá)掃描（Light detection and ran-ging，Li-DAR）技術(shù)，采用非接觸式的測量方式，能夠在較短時(shí)間內(nèi)獲取三維空間中物體表面的觀測點(diǎn)云數(shù)據(jù)［1］。點(diǎn)云配準(zhǔn)是許多三維建模工作的基礎(chǔ)，為了得到待測物體的完整數(shù)據(jù)模型，需要確定一組坐標(biāo)轉(zhuǎn)化關(guān)系，將各個(gè)視角采集的點(diǎn)云融合到一個(gè)統(tǒng)一的坐標(biāo)系下，形成一個(gè)完整的點(diǎn)云數(shù)據(jù)模型。線掃LiDAR 設(shè)備采集數(shù)據(jù)時(shí)沿俯仰角方向掃描不是連續(xù)的，采集的點(diǎn)云數(shù)據(jù)會呈現(xiàn)明顯的環(huán)形條紋結(jié)構(gòu)。這些條紋結(jié)構(gòu)使點(diǎn)云數(shù)據(jù)具有極不均勻的點(diǎn)密度分布，對配準(zhǔn)產(chǎn)生負(fù)面影響。點(diǎn)云密度的不均勻會影響觀察點(diǎn)的定向法向量估計(jì)，而定向法向量在物體識別和表面重建中起著至關(guān)重要的作用［2］。為了改進(jìn)點(diǎn)云質(zhì)量，可以對點(diǎn)云數(shù)據(jù)進(jìn)行重采樣處理。在計(jì)算機(jī)圖形學(xué)領(lǐng)域，一些算法依賴于局部幾何分析來改進(jìn)點(diǎn)集曲面［3］。Lipman 開發(fā)了一種高效的、無參數(shù)化的局部最優(yōu)投影（Locally optimal projection，LOP）來處理異常值［4］。然而，當(dāng)輸入數(shù)據(jù)點(diǎn)的分布高度不均勻時(shí)，LOP 不能很好地工作。Huang 等［5］改進(jìn)了LOP 算法，提出了基于局部自適應(yīng)密度權(quán)重的WLOP算法，來處理原始數(shù)據(jù)中的不均勻分布。但在處理大規(guī)模數(shù)據(jù)時(shí)，WLOP 算法的效率較低。因此本文采用了一種基于超體素的重采樣技術(shù)來調(diào)整點(diǎn)云密度，使點(diǎn)云分布更加均勻，以支撐后續(xù)的精配準(zhǔn)操作。

目前常用的精配準(zhǔn)算法有Besl 等于1992 年提出的迭代最近點(diǎn)（Iterative Closest Points，ICP）算法［6］，該算法精度高、容易實(shí)現(xiàn)，但對初始位置要求較高。為此，國內(nèi)外研究人員對該算法進(jìn)行了許多改進(jìn)，戴靜蘭等［7］提出了一種基于曲率特征點(diǎn)和K-D樹搜索改進(jìn)的ICP算法，提高了ICP配準(zhǔn)的精度，但當(dāng)待配準(zhǔn)點(diǎn)云數(shù)據(jù)過大時(shí)，該算法配準(zhǔn)效率較低；李若白等［8］提出了一種基于特征點(diǎn)法向量夾角的改進(jìn)點(diǎn)云配準(zhǔn)算法，一定程度上減少了配準(zhǔn)算法的迭代次數(shù)。此外，還有PICP（Probability ICP）［9］、MICP（Modified ICP）［10］和CICP（Cluster ICP）［11］等許多變種算法。

本文結(jié)合線掃LiDAR 設(shè)備采集數(shù)據(jù)的特點(diǎn)提出了一種基于點(diǎn)密度調(diào)整的激光雷達(dá)點(diǎn)云配準(zhǔn)改進(jìn)方法，可有效提高配準(zhǔn)精度，減少配準(zhǔn)耗時(shí)。

2 算法設(shè)計(jì)

本算法首先將輸入點(diǎn)云分割為彈性超體素框架，以超體素為節(jié)點(diǎn)構(gòu)建K 最近鄰圖（K nearest neighbor，KNN）。然后使用該彈性框架來約束重采樣過程，使原先條紋狀點(diǎn)云更加均勻地分布。最后，以重采樣的點(diǎn)云為輸入，通過ICP 算法實(shí)現(xiàn)點(diǎn)云配準(zhǔn)。

2.1 點(diǎn)投影重采樣

點(diǎn)云重采樣算法流程圖如圖1 所示。第一階段是利用八叉樹算法對點(diǎn)云進(jìn)行子集抽樣，從而根據(jù)八叉樹節(jié)點(diǎn)計(jì)算靈活的支撐點(diǎn)。基于這些支撐點(diǎn)，實(shí)現(xiàn)基于分割的區(qū)域生長生成超體素，將其視為節(jié)點(diǎn)，進(jìn)一步生成作為彈性框架的K 最近鄰（KNN）圖［12］。在第二階段，使用基于超體素的框架來約束重采樣過程。重新采樣點(diǎn)被插入框架的網(wǎng)格中，并被調(diào)整為沿著法向量方向投射到場景表面上。然后，所有插入點(diǎn)的分布根據(jù)一個(gè)能使點(diǎn)均勻分布的能量函數(shù)進(jìn)行細(xì)化。基于超體素的圖就像一個(gè)篩子，使用迭代方法均勻分配重采樣點(diǎn)的分布。

圖1 基于點(diǎn)密度調(diào)整的激光雷達(dá)點(diǎn)云配準(zhǔn)流程圖

建立約束框架：超體素是一種過度分割的結(jié)果，將局部一致的點(diǎn)聚集成許多塊。種子點(diǎn)的選擇至關(guān)重要，為了得到較為一致的分割結(jié)果，很多方法采用平坦區(qū)域的點(diǎn)作為種子點(diǎn)［13］，但是這些方法對于突出結(jié)構(gòu)會失去適用性。因此本文采用八叉樹提取種子點(diǎn)。

假設(shè)輸入點(diǎn)云P={p1,p2,…,pn}，一個(gè)自適應(yīng)八叉樹可以提取其中一組葉子節(jié)點(diǎn)L={l1,l2,…,ln}作為種子點(diǎn)，大致表示場景的結(jié)構(gòu)［14］。使用包含P的最小三維邊界框作為根節(jié)點(diǎn)。然后，根節(jié)點(diǎn)被細(xì)分為8 個(gè)子立方體，大小相同，這樣遞歸地進(jìn)行空間分解以分割非空立方體，直到滿足閾值標(biāo)準(zhǔn)或達(dá)到最小體素大?。?5］。只有滿足特定深度的八叉樹節(jié)點(diǎn)才會被選作超體素的初始聚類中心。

其中的乘數(shù)5 是一個(gè)經(jīng)驗(yàn)值，它加強(qiáng)了空間鄰近度的權(quán)重，以產(chǎn)生更為緊湊的體素形狀。基本上，λDS的值應(yīng)接近1。

在一個(gè)超體素中心周圍2w*2w*2w（w=W/2Dept?）的三維空間中尋找相似的點(diǎn)。一旦每個(gè)點(diǎn)都與最近的一個(gè)節(jié)點(diǎn)相關(guān)聯(lián)，一個(gè)迭代步驟將每個(gè)聚類中心的位置Vj=[x,y,z]T和法向量nVj=[nx,ny,nz]T更新為屬于該聚類的所有點(diǎn)的平均值。新的聚類中心與原聚類中心之間的空間距離即為殘差。當(dāng)收斂時(shí)，較大的體素通過移動(dòng)擴(kuò)大出現(xiàn)在平滑區(qū)域，較小的體素通過移動(dòng)收縮出現(xiàn)在邊緣和角落，而均勻區(qū)域的體素大小相似。在模型中，體素可以看作是一個(gè)緊湊的表面貼片，一個(gè)體素表示模型中的一個(gè)節(jié)點(diǎn)。然后在歐幾里得空間中建立一個(gè)KNN 圖，該圖被作為一個(gè)約束框架，為后續(xù)重采樣提供鄰近點(diǎn)關(guān)系。

插入重采樣點(diǎn)：重采樣階段包括兩個(gè)步驟：一是獲取插入點(diǎn)的初始位置；二是優(yōu)化數(shù)據(jù)點(diǎn)的全局分布，如圖2 所示。對于一個(gè)給定的超體素節(jié)點(diǎn)集合V={V1,V2,…,Vm} ，定義某個(gè)節(jié)點(diǎn)Vi和它的鄰域集合NVi的最大間隙的中點(diǎn)為p0i，利用鄰域點(diǎn)集的法向量的平均值來估計(jì)初始的法向量npi。上述的最大間隙被定義為以Vi和它的領(lǐng)域集合NVi中的某點(diǎn)作為直徑的兩個(gè)端點(diǎn)構(gòu)成的最大圓形區(qū)域，且該區(qū)域內(nèi)沒有其他點(diǎn)。當(dāng)場景表面是彎曲形狀時(shí)，中點(diǎn)pi0并不總是在表面上，如圖2（a）所示。因此，需要將插入點(diǎn)投影到場景的潛在表面，以獲得實(shí)際位置。投影移動(dòng)是通過沿著法線方向npi移動(dòng)距離Δ來實(shí)現(xiàn)的：

圖2 重采樣點(diǎn)的插入過程

到目前為止，只考慮體素節(jié)點(diǎn)之間的約束，插入點(diǎn)的分布并不理想，如圖2（b）所示，需要作均勻優(yōu)化。

優(yōu)化點(diǎn)云分布：對于給定的超體素集合V和上一步得到的插入點(diǎn)集P′，目標(biāo)是通過最小化能量函數(shù)式（7）來調(diào)整插入點(diǎn)位置，使其均勻分布：

其中：

迭代公式中第一項(xiàng)實(shí)現(xiàn)對離群值和數(shù)據(jù)噪聲的抑制，第二項(xiàng)反映采樣點(diǎn)受到的框架約束的調(diào)整，系數(shù)μ用于平衡這兩項(xiàng)。求解最小化能量函數(shù)為迭代優(yōu)化過程，如式（10）所示。收斂時(shí)pk+1i為重采樣點(diǎn)的最終位置，最終得到完整的重采樣點(diǎn)云數(shù)據(jù)，如圖2（c）所示。

2.2 點(diǎn)云配準(zhǔn)

本文使用重采樣后的優(yōu)化點(diǎn)云數(shù)據(jù)作為配準(zhǔn)的輸入數(shù)據(jù)。配準(zhǔn)的關(guān)鍵是如何得到坐標(biāo)轉(zhuǎn)化參數(shù)旋轉(zhuǎn)矩陣R和平移矩陣T，使得兩視角下測得的匹配對應(yīng)三維數(shù)據(jù)經(jīng)坐標(biāo)變換后的距離最?。?6］。

經(jīng)典ICP 算法是一種迭代縮小的方法，通過最小化重疊區(qū)域之間的歐氏距離誤差度量來尋找兩個(gè)數(shù)據(jù)集之間的最優(yōu)配準(zhǔn)參數(shù)［17］。若存在兩個(gè)完全對應(yīng)的點(diǎn)集P（點(diǎn)數(shù)Np）和X（點(diǎn)數(shù)Nx），點(diǎn)集內(nèi)點(diǎn)數(shù)相等（Np=Nx）且匹配對應(yīng)。

ICP配準(zhǔn)流程如圖3所示：

圖3 ICP算法流程圖

1）通過重采樣得到目標(biāo)點(diǎn)云P（點(diǎn)數(shù)Np）和參考點(diǎn)云X（點(diǎn)數(shù)Nx）；

2）在點(diǎn)云P中尋找特征點(diǎn)集F并初始化；

3）計(jì)算點(diǎn)集F在點(diǎn)云X中的最近點(diǎn)Y；

4）計(jì)算坐標(biāo)轉(zhuǎn)化參數(shù)向量和誤差(q,dms)；

5）判斷誤差是否收斂，如果dk-dk+1<ε，ε為設(shè)定值且ε>0，滿足閾值條件，則收斂，否則跳轉(zhuǎn)至步驟3）繼續(xù)迭代計(jì)算；

6）得到最終坐標(biāo)變換矩陣R和T，完成配準(zhǔn)。

ICP 算法在較好的初值情況下，可以得到很好的算法收斂性以及精確的配準(zhǔn)結(jié)果。但I(xiàn)CP 算法對初始配準(zhǔn)條件要求相對嚴(yán)格，要求待配準(zhǔn)點(diǎn)云的重疊度很高，否則容易陷入局部最優(yōu)陷阱。此外，ICP 算法魯棒性較差，異常點(diǎn)對掃描匹配影響較大，因此本文對輸入點(diǎn)云進(jìn)行重采樣，以提高點(diǎn)云數(shù)據(jù)的質(zhì)量。

3 實(shí)驗(yàn)結(jié)果與分析

為驗(yàn)證基于點(diǎn)密度調(diào)整的激光雷達(dá)點(diǎn)云配準(zhǔn)改進(jìn)方法的優(yōu)勢，利用三維點(diǎn)云模型進(jìn)行重采樣實(shí)驗(yàn)，將未經(jīng)過點(diǎn)密度調(diào)整算法處理和處理過的數(shù)據(jù)分別采用經(jīng)典的ICP 算法和標(biāo)準(zhǔn)3D-NDT 算法［19］進(jìn)行配準(zhǔn)，對比實(shí)驗(yàn)結(jié)果。

3.1 ICP配準(zhǔn)對比實(shí)驗(yàn)

實(shí)驗(yàn)測試的點(diǎn)云數(shù)據(jù)由Velodyne 公司的VLP-16型線掃激光雷達(dá)采集得到。一組輸入點(diǎn)云配準(zhǔn)前的位姿如圖4 所示，紅色數(shù)據(jù)點(diǎn)是目標(biāo)點(diǎn)云，包含21112 個(gè)數(shù)據(jù)點(diǎn)；黑色數(shù)據(jù)點(diǎn)是源點(diǎn)云，包含15282 個(gè)數(shù)據(jù)點(diǎn)。墻面存在著明顯條紋式的結(jié)構(gòu)，數(shù)據(jù)點(diǎn)分布較為離散。

圖4 初始點(diǎn)云位姿圖（ICP）

首先直接使用原始數(shù)據(jù)進(jìn)行ICP 配準(zhǔn)，得到的配準(zhǔn)結(jié)果如圖5（a）所示，子圖（b）和（c）為配準(zhǔn)結(jié)果的細(xì)節(jié)圖。該算法耗時(shí)為5418.31ms，配準(zhǔn)迭代次數(shù)為45 次。由于線掃機(jī)制造成的條紋狀結(jié)構(gòu)，直接進(jìn)行ICP 配準(zhǔn)算法得到的配準(zhǔn)效果并不好，如（b）處門框存在較明顯的錯(cuò)位，（c）處墻面沒有對齊。

圖5 ICP配準(zhǔn)結(jié)果

使用重采樣點(diǎn)云配準(zhǔn)的結(jié)果如圖5（d）所示，（e）、（f）為同位置結(jié)果對比。本算法總耗時(shí)4243.34ms，其中包含點(diǎn)密度調(diào)整處理耗時(shí)2018ms、ICP 配準(zhǔn)耗時(shí)2225.34ms，配準(zhǔn)迭代次數(shù)為18次。

可以看到重采樣的點(diǎn)云固有結(jié)構(gòu)并沒有較大的改變，但原先突兀的條紋狀結(jié)構(gòu)得到了一定的改進(jìn)。這是因?yàn)橹夭蓸犹幚頊p少了線掃雷達(dá)方位角方向數(shù)據(jù)點(diǎn)密集排布的趨勢，而在俯仰角方向的條紋間隙間均勻插入了較多數(shù)據(jù)點(diǎn)，整體上降低了輸入點(diǎn)云的數(shù)據(jù)量。經(jīng)過點(diǎn)密度調(diào)整后，ICP 配準(zhǔn)時(shí)間縮短至未重采樣點(diǎn)云的41%，配準(zhǔn)算法迭代次數(shù)大大降低，提高了運(yùn)行效率。兩種算法的詳細(xì)對比數(shù)據(jù)如表1所示。

表1 不同ICP算法的配準(zhǔn)結(jié)果比較

3.2 3D-NDT配準(zhǔn)

除了使用ICP 算法驗(yàn)證外，本文還測試了重采樣點(diǎn)云對于NDT配準(zhǔn)算法的影響，如圖6所示。子圖（a）顯示了原始點(diǎn)云配準(zhǔn)前的相對位姿關(guān)系，目標(biāo)點(diǎn)云包含10258個(gè)數(shù)據(jù)點(diǎn)；源點(diǎn)云包含4108個(gè)數(shù)據(jù)點(diǎn)。

圖6 NDT算法配準(zhǔn)結(jié)果

直接使用原始數(shù)據(jù)進(jìn)行NDT 配準(zhǔn)，得到的配準(zhǔn)結(jié)果如子圖（b）所示。顯然，未經(jīng)過點(diǎn)密度調(diào)整的點(diǎn)云并不能在規(guī)定的20 次最大迭代次數(shù)內(nèi)完成配準(zhǔn)。經(jīng)過密度調(diào)整算法處理得到的配準(zhǔn)結(jié)果如子圖（c）所示。該算法總耗時(shí)1864.63ms，其中包含點(diǎn)密度調(diào)整處理耗時(shí)1212ms、NDT 配準(zhǔn)耗時(shí)652.63ms，配準(zhǔn)迭代次數(shù)為19 次，配準(zhǔn)誤差21.94cm。配準(zhǔn)數(shù)據(jù)統(tǒng)計(jì)如表2所示。

表2 不同NDT算法的配準(zhǔn)結(jié)果比較

超體素的框架建立可以被看作是散點(diǎn)數(shù)據(jù)到數(shù)據(jù)“骨架”的過渡，相當(dāng)于是一種從粗到細(xì)的機(jī)制。均勻密度調(diào)整算法可以被看作往抽象的數(shù)據(jù)“骨架”的表面填充均勻數(shù)據(jù)點(diǎn)的過程，從而使結(jié)果趨向于補(bǔ)償線掃激光雷達(dá)的稀疏數(shù)據(jù)，削減局部密集的冗余數(shù)據(jù)。在總體上，使數(shù)據(jù)近似呈現(xiàn)出更加均勻的正態(tài)分布，提高了NDT 算法的配準(zhǔn)效率和精度。此外，該方法也提供了更加豐富的局部表面信息，有助于局部特征提取、法向量估計(jì)等工作。

本文方法也存在一個(gè)問題：該算法不能確定一個(gè)小的間隙和孔是真實(shí)存在的空洞，還是掃描時(shí)數(shù)據(jù)缺失。因此，一些實(shí)際存在的空隙可能會在重采樣時(shí)被填補(bǔ)；一些數(shù)據(jù)點(diǎn)會被誤插入到墻角之間，影響配準(zhǔn)精度。為了解決該問題，未來的工作可能是先從點(diǎn)云數(shù)據(jù)中識別出空隙、墻面等結(jié)構(gòu)，再進(jìn)行重采樣點(diǎn)的插入和密度調(diào)整。

4 結(jié)語

點(diǎn)云配準(zhǔn)是三維重建的關(guān)鍵環(huán)節(jié)，決定了后續(xù)工作的準(zhǔn)確性。線掃激光雷達(dá)由于機(jī)械式的旋轉(zhuǎn)掃描機(jī)制，采集的點(diǎn)云數(shù)據(jù)會呈現(xiàn)明顯的環(huán)形條紋結(jié)構(gòu)。本文提出了一種基于點(diǎn)密度調(diào)整的激光雷達(dá)點(diǎn)云配準(zhǔn)改進(jìn)方法。將輸入點(diǎn)云分割為超體素框架，然后使用該彈性框架來約束重采樣過程，使點(diǎn)云分布更加均勻合理。實(shí)驗(yàn)表明，通過點(diǎn)密度調(diào)整算法處理的數(shù)據(jù)，能提高經(jīng)典ICP 和3D-NDT 算法的配準(zhǔn)精度，減少算法迭代次數(shù)，提高配準(zhǔn)效率。