王太學(xué)，江智，江德港，李柏林，郭彩玲

(1.西南交通大學(xué) 唐山研究生院，河北 唐山 063000；2.西南交通大學(xué) 機(jī)械工程學(xué)院，成都 610031；3.唐山學(xué)院 河北省智能裝備數(shù)字化設(shè)計(jì)及過程仿真重點(diǎn)實(shí)驗(yàn)室，河北 唐山 063000)

0 引言

點(diǎn)云配準(zhǔn)是將不同視角下獲得的點(diǎn)云數(shù)據(jù)經(jīng)過一定的旋轉(zhuǎn)平移變換到同一坐標(biāo)系下的過程，廣泛應(yīng)用于計(jì)算機(jī)視覺[1]、三維重建[2]等領(lǐng)域。

目前國內(nèi)外學(xué)者已經(jīng)對點(diǎn)云配準(zhǔn)進(jìn)行了大量研究。最經(jīng)典的點(diǎn)云配準(zhǔn)算法是Besl等[3]提出的迭代最近點(diǎn)(iterative closest point，ICP)算法，該算法的配準(zhǔn)前提是要求輸入點(diǎn)云有較好的初始位置，才能實(shí)現(xiàn)精確配準(zhǔn)，但是算法本身需要多次迭代，收斂速率較慢，并且容易陷入局部最優(yōu)。針對經(jīng)典ICP算法的缺點(diǎn)，相關(guān)學(xué)者對ICP算法進(jìn)行了改進(jìn)。趙夫群等[4]將三維點(diǎn)云轉(zhuǎn)換成二維方位角，通過二維圖像和三維點(diǎn)的映射關(guān)系進(jìn)行坐標(biāo)變換，最后基于奇異值分解完成了ICP算法的精配準(zhǔn)；嚴(yán)劍鋒等[5]基于點(diǎn)云的幾何特征利用點(diǎn)云的曲率提取特征點(diǎn)，通過匹配特征點(diǎn)對完成粗匹配，為ICP算法提供較好的初值，提高了配準(zhǔn)精度和可靠性。點(diǎn)特征直方圖(point feature histogram，PFH)和快速點(diǎn)特征直方圖(fast point feature histogram，F(xiàn)PFH)[6-7]雖然在點(diǎn)云配準(zhǔn)的過程中也能提高點(diǎn)云配準(zhǔn)的精度，但是當(dāng)點(diǎn)云體積密度較大時(shí)，點(diǎn)云配準(zhǔn)效率會大大降低。馬偉麗等[8]通過隨機(jī)抽樣一致性算法結(jié)合四元數(shù)法計(jì)算配準(zhǔn)參數(shù)，完成了全局最優(yōu)參數(shù)的點(diǎn)云精確配準(zhǔn)，配準(zhǔn)效率卻有待提高；He等[9]提出一種基于PointNet++與ICP算法相結(jié)合的點(diǎn)云配準(zhǔn)算法，同時(shí)提取多個(gè)點(diǎn)云特征，實(shí)現(xiàn)了點(diǎn)云的快速魯棒配準(zhǔn)，但是對于稀疏點(diǎn)云的適應(yīng)性較差；Magnusson等[10]提出了3D-NDT(three-dimensional normal distributions transform)算法，把三維體素內(nèi)的點(diǎn)云數(shù)據(jù)集轉(zhuǎn)換成連續(xù)可微的概率分布函數(shù)，通過得分法實(shí)現(xiàn)點(diǎn)云的配準(zhǔn)；Aiger等[11]提出了4PCS(4-points congruent sets)算法，算法無需初始位置和濾波去噪，并允許存在離群值；在4PCS算法的基礎(chǔ)上，Mellado等[12]提出了super4PCS(super three-dimensional normal distributions transform)算法，運(yùn)用仿射變換不變性，利用共面四點(diǎn)完成點(diǎn)云匹配；Chetverikov等[13]提出一種trimmed ICP(TrICP)算法，通過尋找最小重疊參數(shù)實(shí)現(xiàn)點(diǎn)云的精確匹配，但是需要具有良好的初始位置估計(jì)值。

基于以上學(xué)者的研究現(xiàn)狀，為了改進(jìn)ICP算法需要較好的初值和收斂速度較慢的問題，本文提出一種基于主軸基向量間的關(guān)系進(jìn)行主軸校正的方法，利用兩組點(diǎn)云的三個(gè)點(diǎn)云主成分坐標(biāo)軸對應(yīng)基向量間的關(guān)系進(jìn)行主軸校正，為ICP算法輸入較好的初值，接著針對具有部分重疊的點(diǎn)云提出一種改進(jìn)ICP算法，利用最近鄰算法搜索重疊點(diǎn)云部分的有效對應(yīng)點(diǎn)對，利用有效點(diǎn)對進(jìn)行求解最優(yōu)變換參數(shù)完成點(diǎn)云的精確配準(zhǔn)。

1 點(diǎn)云粗配準(zhǔn)

1.1 主軸方向分析

PCA(principal component analysis)算法[14]是一種用于降維的統(tǒng)計(jì)方法，主要的作用是用來簡化數(shù)據(jù)集，通過線性變換求取數(shù)據(jù)集的主成分，從而實(shí)現(xiàn)降維。兩組點(diǎn)云數(shù)據(jù)經(jīng)過PCA算法處理后得到了初始的旋轉(zhuǎn)和平移矩陣。雖然其主軸基本共線，但仍然存在主軸方向相反的可能性[15]，需要對主軸方向校正才能為精配準(zhǔn)提供較好的初值。

設(shè)兩組點(diǎn)云P和Q經(jīng)過PCA變換后得到的坐標(biāo)軸分別為upvpwp、uqvqwq，原點(diǎn)分別為Op和Oq，其中u、v、w分別表示第一坐標(biāo)軸、第二坐標(biāo)軸和第三坐標(biāo)軸。當(dāng)對點(diǎn)云進(jìn)行旋轉(zhuǎn)和平移變換后，主軸方向出現(xiàn)相反的情況有7種，如表1所示，定義類別Ci(i=1，2，…，7)。其中“1”表示兩組點(diǎn)云的兩個(gè)對應(yīng)坐標(biāo)軸方向相同，“-1”表示兩組點(diǎn)云的兩個(gè)對應(yīng)坐標(biāo)軸方向相反。

表1 主軸方向相反情況

經(jīng)過PCA后以兩組點(diǎn)云的質(zhì)心為原點(diǎn)建立相對坐標(biāo)系，可能出現(xiàn)的點(diǎn)云坐標(biāo)系情況如圖1所示，其中圖1(a)為點(diǎn)云Q的坐標(biāo)系，圖1(b)至圖1(h)為點(diǎn)云P經(jīng)過PCA后可能出現(xiàn)坐標(biāo)軸反向的類型。

圖1 PCA后出現(xiàn)反向坐標(biāo)軸的類型

1.2 點(diǎn)云主軸校正

對于兩組已經(jīng)進(jìn)行處理后的點(diǎn)云數(shù)據(jù)，主軸基本對齊后可能出現(xiàn)表1中7種類別的任何一種，主軸校正的關(guān)鍵就是確定出現(xiàn)的是哪一類別，針對這一類別進(jìn)行主軸調(diào)整，得到較好的旋轉(zhuǎn)和平移矩陣初值。

圖2 確定主軸類別流程圖

步驟1：輸入兩組相似點(diǎn)云。

步驟2：利用PCA進(jìn)行點(diǎn)云數(shù)據(jù)處理，實(shí)現(xiàn)主軸對齊。

步驟3：利用SVD分解計(jì)算α、β和γ。

步驟4：計(jì)算αp·αq，若大于0，計(jì)算βp·βq，若大于0，計(jì)算γp·γq，若大于0，則主軸方向相同，若小于0，輸出類別為C1；若βp·βq<0，計(jì)算γp·γq，若大于0，輸出類別為C2，若小于0，輸出類別為C3；若αp·αq<0，計(jì)算βp·βq，若大于0，計(jì)算γp·γq，若大于0，輸出類別為C4，若小于0，輸出類別為C5；若βp·βq<0，計(jì)算γp·γq，若大于0，輸出類別為C6，若<0，輸出類別為C7。

步驟5：根據(jù)步驟4的判斷結(jié)果，對源點(diǎn)云P與目標(biāo)點(diǎn)云Q主軸方向相反的坐標(biāo)軸的基向量取反，即αp=-αp或βp=-βp或γp=-γp，從而完成Up校正，得到新矩陣UP0。

如圖3(a)所示，為兩組點(diǎn)云進(jìn)行PCA處理后的點(diǎn)云配準(zhǔn)情況。從圖中可以看到，兩組點(diǎn)云主軸已經(jīng)基本對齊，但出現(xiàn)了主軸u和w反向的情況。經(jīng)過算法初配準(zhǔn)后的結(jié)果如圖3(b)所示，實(shí)現(xiàn)了主軸方向的校正。

圖3 點(diǎn)云粗配準(zhǔn)圖

2 點(diǎn)云精配準(zhǔn)

兩組點(diǎn)云在經(jīng)過粗配準(zhǔn)后，初始位置關(guān)系已經(jīng)基本相近，運(yùn)用經(jīng)典ICP算法進(jìn)行迭代，最小化目標(biāo)函數(shù)求解最優(yōu)變換參數(shù)R和T即可完成精配準(zhǔn)。但是在實(shí)際點(diǎn)云數(shù)據(jù)獲取過程中，不同視角下的兩組點(diǎn)云在同一個(gè)坐標(biāo)系下會有一定重疊度但是并不會完全重合[16]，使得ICP算法在應(yīng)用時(shí)受限。

在點(diǎn)云配準(zhǔn)的過程中，并不是所有點(diǎn)對對配準(zhǔn)的貢獻(xiàn)都相同[17]，非重疊區(qū)域的點(diǎn)在配準(zhǔn)的過程中只會找到錯(cuò)誤點(diǎn)對[18]，因此要剔除錯(cuò)誤點(diǎn)對的影響。

針對具有部分重疊的點(diǎn)云配準(zhǔn)問題，本文提出了改進(jìn)ICP算法，先將兩組點(diǎn)云進(jìn)行粗配準(zhǔn)，之后利用最近鄰搜索算法尋找最近鄰點(diǎn)組成有效點(diǎn)對，基于有效點(diǎn)對迭代求解最優(yōu)變換參數(shù)完成配準(zhǔn)。

2.1 改進(jìn)ICP算法描述

2.2 改進(jìn)ICP算法步驟

步驟2：建立兩組點(diǎn)云的點(diǎn)對關(guān)系，表達(dá)如式(1)所示。

(1)

式中：ck代表一組點(diǎn)對關(guān)系；得到兩組點(diǎn)云的有效點(diǎn)對，記為Dl{l=1，2，3，…，s}，s為有效點(diǎn)對的個(gè)數(shù)，l為一組有效點(diǎn)對，s≤N且s≤M。

(2)

其中，將粗配準(zhǔn)得到的結(jié)果作為第一次迭代的初值，通過以上迭代步驟來獲得最優(yōu)解。

3 實(shí)驗(yàn)實(shí)例與分析

為驗(yàn)證本文算法性能，選取斯坦福大學(xué)點(diǎn)云數(shù)據(jù)庫中Bunny模型為實(shí)驗(yàn)對象，其中源點(diǎn)云的數(shù)量為31 797(圖4紫紅色點(diǎn)云)，目標(biāo)點(diǎn)云的數(shù)量為35 947(圖4藍(lán)色點(diǎn)云)。實(shí)驗(yàn)從初始位置和重疊度兩個(gè)方面對點(diǎn)云數(shù)據(jù)進(jìn)行配準(zhǔn)實(shí)驗(yàn)，分別驗(yàn)證算法在粗配準(zhǔn)和精配準(zhǔn)過程的性能。實(shí)驗(yàn)環(huán)境為Intel Xeon Silver 4110 2.10 GHz CPU，運(yùn)行平臺為MATLAB R2016a。

3.1 粗配準(zhǔn)

為檢驗(yàn)本文粗配準(zhǔn)算法性能，選取基于局部特征的配準(zhǔn)算法FPFH、4PCS、PFH和改進(jìn)PCA算法進(jìn)行對比分析。圖4(a)為點(diǎn)云的初始位置，圖4(b)～圖4(e)分別為FPFH、4PCS、PFH和改進(jìn)PCA算法對點(diǎn)云進(jìn)行粗配準(zhǔn)后的結(jié)果，四種算法的均方根誤差RMSE和所用時(shí)間如表2所示。

圖4 粗配準(zhǔn)測試

表2 粗配準(zhǔn)算法結(jié)果對比

從實(shí)驗(yàn)結(jié)果可以看出，本文算法在配準(zhǔn)時(shí)間和誤差上優(yōu)于其他三種算法。FPFH算法在配準(zhǔn)時(shí)沒有完全考慮中心點(diǎn)周圍鄰域的所有點(diǎn)對關(guān)系，使周圍的一些幾何信息丟失導(dǎo)致匹配誤差較大；4PCS算法精度有所提高，但每次需要隨機(jī)采樣，并且點(diǎn)云重疊率低也會導(dǎo)致其配準(zhǔn)時(shí)間增長；PFH算法需要計(jì)算鄰域點(diǎn)所有組合的特征值，使算法復(fù)雜度增高，導(dǎo)致算法的配準(zhǔn)時(shí)間較長且其平移誤差較大。

3.2 精配準(zhǔn)

為驗(yàn)證本文算法在不同重疊度下點(diǎn)云的配準(zhǔn)性能，選取三組具有不同重疊度的點(diǎn)云與三種精配準(zhǔn)算法TrICP、super4PCS、3D-NDT進(jìn)行對比分析。本文算法能夠自動計(jì)算出點(diǎn)云的重疊度，重疊度參數(shù)如表3所示(ε根據(jù)粗配準(zhǔn)的結(jié)果設(shè)定為0.8)，配準(zhǔn)后的結(jié)果如圖5、圖6和圖7所示，不同重疊度下的配準(zhǔn)誤差和時(shí)間如表3所示。

圖5 重疊度=0.092算法配準(zhǔn)結(jié)果

圖6 重疊度=0.684算法配準(zhǔn)結(jié)果

圖7 重疊度=0.671算法配準(zhǔn)結(jié)果

表3 配準(zhǔn)算法的效果比較

根據(jù)配準(zhǔn)結(jié)果得出，對于不同重疊度的點(diǎn)云，本文算法在配準(zhǔn)的時(shí)間和收斂誤差上都明顯低于其他三種算法，經(jīng)過改進(jìn)的PCA算法粗配準(zhǔn)后，利用KD-tree尋找最近點(diǎn)，并利用歐式距離較大的有效點(diǎn)對進(jìn)行精配準(zhǔn)，縮短了配準(zhǔn)時(shí)間和提高了配準(zhǔn)的精度。TrICP算法對點(diǎn)云的初始位置要求較高，在尋找最佳重疊時(shí)需多次迭代配準(zhǔn)，計(jì)算時(shí)間較長，并且在初始位置不好的情況下容易陷入局部最優(yōu)得到錯(cuò)誤匹配結(jié)果。super4PCS算法是對4PCS算法的改進(jìn)，利用共面四點(diǎn)的仿射不變性進(jìn)行點(diǎn)云配準(zhǔn)。雖然其配準(zhǔn)性能較高，但是需要人工選取樣本點(diǎn)，當(dāng)點(diǎn)云密度增大后算法配準(zhǔn)效率會降低。3D-NDT算法對參數(shù)設(shè)置較敏感，需要多次手動進(jìn)行參數(shù)的調(diào)整，而手動調(diào)參的過程也增加了算法時(shí)間的開銷。

對比同一算法，從實(shí)驗(yàn)數(shù)據(jù)可以看出，重疊度相差較大的兩組點(diǎn)云，重疊度越大，配準(zhǔn)效率越高，而對于重疊度近似的兩組點(diǎn)云，在保證良好初始位置的情況下，其配準(zhǔn)效率也會越高。而本文算法在重疊率較低和初始位置相差較大的情況下收斂速度較快，配準(zhǔn)精度較高，證明了本文算法的魯棒性。

3.3 牽引座支座點(diǎn)云配準(zhǔn)

為驗(yàn)證算法在實(shí)測數(shù)據(jù)上的性能，用FARO Design ScanArm掃描儀從不同方位獲取了汽車牽引座點(diǎn)云數(shù)據(jù)，利用TrICP算法、super4PCS算法、3D-NDT算法和本文算法對實(shí)測點(diǎn)云數(shù)據(jù)進(jìn)行配準(zhǔn)，配準(zhǔn)結(jié)果如圖8所示，其中圖8(a)為點(diǎn)云數(shù)據(jù)的獲取過程，圖8(b)為獲取兩組點(diǎn)云的初始位置，圖8(c)～圖8(f)分別為四種算法的配準(zhǔn)圖，并對局部的配準(zhǔn)效果進(jìn)行了放大，算法的均方根誤差和耗時(shí)如表4所示。

圖8 支座點(diǎn)云配準(zhǔn)結(jié)果

表4 不同算法對支座點(diǎn)云的配準(zhǔn)結(jié)果

從配準(zhǔn)結(jié)果和表中所得數(shù)據(jù)可以看出：3D-NDT算法表現(xiàn)最差，由于數(shù)據(jù)為實(shí)測數(shù)據(jù)，噪聲不可避免，算法在參數(shù)選取過程中難度增加，誤差增大；TrICP算法較3D-NDT算法在性能上有所提升，但受初始位置影響配準(zhǔn)效果仍不理想；super4PCS算法雖然配準(zhǔn)誤差較3D-NDT算法和TrICP算法稍有提高，但是隨著點(diǎn)云數(shù)量的增加，導(dǎo)致其選取樣本點(diǎn)的效率降低而匹配時(shí)間增加；本文算法在利用歐式距離較大的有效點(diǎn)對配準(zhǔn)過程中減少了配準(zhǔn)時(shí)間并增加了配準(zhǔn)精度，配準(zhǔn)精度為0.010 05 mm，配準(zhǔn)時(shí)間為41.012 5 s，配準(zhǔn)性能優(yōu)于其他三種對比算法，證明了本文算法的可行性。

4 結(jié)束語

針對ICP算法對點(diǎn)云配準(zhǔn)過程中需要輸入較好初值和收斂速度慢的問題，本文提出了一種融合PCA的改進(jìn)ICP算法。首先利用PCA算法對點(diǎn)云數(shù)據(jù)進(jìn)行處理實(shí)現(xiàn)兩組點(diǎn)云主軸對齊，定義主軸基向量，利用主軸基向量間的關(guān)系實(shí)現(xiàn)了主軸方向的校正，獲得點(diǎn)云配準(zhǔn)的初始位姿。對比FPFH、4PCS、PFH算法，本文粗配準(zhǔn)算法所用時(shí)間分別提高了20.2%、50.5%、44.4%，配準(zhǔn)精度分別提高了66.9%、48.5%、79.4%。在部分重疊區(qū)域的點(diǎn)云配準(zhǔn)中，粗配準(zhǔn)為精配準(zhǔn)提供了較好的初值，本文提出一種利用重疊區(qū)域的有效點(diǎn)對進(jìn)行ICP配準(zhǔn)算法，通過計(jì)算有效點(diǎn)對中歐式距離較大的點(diǎn)對來求解最優(yōu)變換參數(shù)，完成精配準(zhǔn)。通過在Bunny點(diǎn)云和支座點(diǎn)云配準(zhǔn)的實(shí)驗(yàn)表明，在重疊度較低和初始位置相差較大的情況下，本文算法在收斂精度和配準(zhǔn)時(shí)間上都優(yōu)于對比算法，同時(shí)彌補(bǔ)了傳統(tǒng)ICP算法收斂速度慢的不足，并為ICP算法提供了較好初值。但文本算法在點(diǎn)云較稠密或點(diǎn)云數(shù)量較多時(shí)，配準(zhǔn)誤差較大，之后將著重于研究大數(shù)據(jù)點(diǎn)云的配準(zhǔn)準(zhǔn)確性。