楊正世，李文國(guó)

(昆明理工點(diǎn)大學(xué)機(jī)電工程學(xué)院，云南 昆明 650500)

0 引言

點(diǎn)云配準(zhǔn)技術(shù)是研究不同視角，即不同參考坐標(biāo)下的一組點(diǎn)云或多組點(diǎn)云。通過某種算法，將具有一定重疊度的點(diǎn)云數(shù)據(jù)，通過平移旋轉(zhuǎn)縮放等操作變換到同一參考坐標(biāo)系下，進(jìn)而得到實(shí)體完整的點(diǎn)云數(shù)據(jù)信息[1]。它是整個(gè)三維點(diǎn)云數(shù)據(jù)處理中的關(guān)鍵步驟，不但為后期的點(diǎn)云數(shù)據(jù)建模提供了較為精確的點(diǎn)云數(shù)據(jù)，而且為構(gòu)建高精度的模型提供了保證。該技術(shù)廣泛應(yīng)用于3D游戲[2]、物體識(shí)別[3]、大壩測(cè)量[4]、文物保護(hù)[5]、醫(yī)療矯形[6]、機(jī)器人點(diǎn)位導(dǎo)航及自動(dòng)地圖構(gòu)建[7-8]等領(lǐng)域。

點(diǎn)云配準(zhǔn)的關(guān)鍵在于求取變換矩陣。目前常用的點(diǎn)云配準(zhǔn)算法，是由Paual J和Besl[9]在1992年提出的最近點(diǎn)迭代(iterative closest point,ICP)算法以及基于ICP算法的改進(jìn)算法。點(diǎn)云配準(zhǔn)優(yōu)化問題，可以轉(zhuǎn)化為求取一個(gè)有6個(gè)自由度的剛體變換M(R,T)，將兩片點(diǎn)云配準(zhǔn)到同一坐標(biāo)系下，使配準(zhǔn)點(diǎn)云之間的距離誤差達(dá)到最小。ICP算法對(duì)初始點(diǎn)云對(duì)的對(duì)齊要求非常高。如果初始點(diǎn)云對(duì)未能對(duì)齊，將會(huì)使ICP算法陷入局部最優(yōu)。初始點(diǎn)云對(duì)對(duì)齊，將減少迭代的次數(shù)，提高點(diǎn)云配準(zhǔn)的速度和精度。

目前，求解位置變換矩陣的主要方法如下。Beltrami和Jordan提出的奇異值分值(singular value decomposition，SVD)[10]法，求解的變換矩陣精度高，但是需多次迭代，會(huì)影響點(diǎn)云配準(zhǔn)的速度。文獻(xiàn)[11]對(duì)Horn[12]提出的四元素表示法進(jìn)行了細(xì)致的描述。列文伯格-馬夸爾特法(Levenberg-Marquardt,LM)[13]算法能夠借助高斯-牛頓算法以及梯度下降法的優(yōu)點(diǎn)，并對(duì)兩者的不足之處作了改善;但是其局部搜索耗時(shí)大，求得的變換矩陣精度不高。因此，本文提出了一種基于矩陣指數(shù)表示變換矩陣的點(diǎn)云配準(zhǔn)方法，從而提高求取變換矩陣的速度和精度，以及ICP算法的收斂速度和計(jì)算精度。

1 算法描述

1.1 目標(biāo)函數(shù)

如前文所述，點(diǎn)云配準(zhǔn)優(yōu)化問題，可以轉(zhuǎn)化為求取一個(gè)有6個(gè)自由度的剛體變換M(R,T)，將2片點(diǎn)云配準(zhǔn)到同一坐標(biāo)系下，使配準(zhǔn)點(diǎn)云之間的距離誤差達(dá)到最小。假設(shè)待配準(zhǔn)點(diǎn)云集P={p1,p2,…,pi}，1≤i≤n},Q={q1,q2,…,qi}，1≤i≤m。其中：n、m分別為P和Q中點(diǎn)云的總數(shù)。用R表示旋轉(zhuǎn)變換矩陣，T表示平移變換向量，則ε(R,T)表示源點(diǎn)集Q在變換矩陣M(R,T)的作用下與目標(biāo)點(diǎn)集P之間的誤差。因此，求解點(diǎn)云配準(zhǔn)優(yōu)化的問題就可以轉(zhuǎn)化為求解滿足 min[ε(R,T)]的最優(yōu)解，可以定義為：

(1)

式中:qi為點(diǎn)云集Q中的一個(gè)點(diǎn)云數(shù)據(jù)；pi為點(diǎn)云集P中的一個(gè)數(shù)據(jù)點(diǎn)；R∈SO(3)；T∈R3。

假如(pi，qi)為對(duì)齊的點(diǎn)云對(duì)，ni為它們之間單個(gè)點(diǎn)云的法向量，如需確定最佳的變換矩陣M(R,T)，以實(shí)現(xiàn)點(diǎn)云集P和點(diǎn)云集Q配準(zhǔn)，則可將式(1)變換為：

(2)

式中：ni為點(diǎn)云qi點(diǎn)的法向量；(piR+T-qi)為點(diǎn)云pi到點(diǎn)云qi的一種線性轉(zhuǎn)換；[(piR+T-qi)ni]2為piR+T點(diǎn)到qi平面距離的平方。

1.2 求解變換矩陣

(3)

(4)

由指數(shù)ex展開可知，當(dāng)|x|<1時(shí)，ex≈1+x。所以，當(dāng)坐標(biāo)旋量‖ω‖<1時(shí)，式(4)可以表示為：

(5)

T=v

(6)

將式(5)和式(6)代入式(2)，可得：

(7)

定義a=P×n，則式(2)可以重寫為：

(8)

要使式(8)有最小值，則要先求取函數(shù)ε(R,T)關(guān)于ω和v參數(shù)的偏導(dǎo)數(shù)。當(dāng)偏導(dǎo)數(shù)為零，即可求出參數(shù)的值。求解偏導(dǎo)公式為：

(9)

將式(9)表示為Ax=b的線性矩陣方程。其中：A為6×6的協(xié)方差矩陣，可以用已知的ai和ni表示；x為6×1的未知向量；b為6×1的向量，可以通過對(duì)應(yīng)的點(diǎn)云對(duì)(pi，qi)和ai和ni表示。求解Ax=b的線性矩陣方程，就可以得出旋轉(zhuǎn)矩陣R和平移向量T，即完成變換矩陣求解。從式(9)可知，求解變換矩陣只需要6個(gè)方程，算法易于實(shí)現(xiàn)，且不需要構(gòu)造其他的模型函數(shù)。在LM算法中，需要構(gòu)造模型函數(shù)f，并在其領(lǐng)域內(nèi)對(duì)估計(jì)參數(shù)向量p作線性近似變換。如未能較好地構(gòu)造模型函數(shù)構(gòu)，將直接影響變換矩陣的求解。在SVD算法中，需要計(jì)算點(diǎn)云的質(zhì)心，然后構(gòu)造協(xié)方差矩陣。該方法計(jì)算量大，迭代次數(shù)多。從式(9)可以看出，本文方法很好地避免了上述兩種算法的不足，能夠快速、精確地求出變換矩陣。

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

為了加快對(duì)點(diǎn)云對(duì)(pi，qi)最近點(diǎn)的搜索，采用K-D tree搜索法來尋求最近點(diǎn)，提高整體的點(diǎn)云配準(zhǔn)速度。如前文所述，ICP算法對(duì)初始位置對(duì)齊的點(diǎn)云對(duì)依耐性較高。采用隨機(jī)采樣一致性算法(random sample consensus，RANSAC)[15]去除錯(cuò)誤的點(diǎn)云，以減少計(jì)算迭代的次數(shù)，并結(jié)合本文算法來求出變換矩陣，完成點(diǎn)云配準(zhǔn)。 具體的點(diǎn)云配準(zhǔn)流程如圖1所示。

圖1 點(diǎn)云配準(zhǔn)流程圖

2 試驗(yàn)結(jié)果與分析

2.1 試驗(yàn)評(píng)估及試驗(yàn)環(huán)境說明

試驗(yàn)從以下2個(gè)方面進(jìn)行評(píng)估。

②在相同的條件下，分別與基于SVD算法的ICP算法和基于LM算法的ICP算法，比較配準(zhǔn)的精度和配準(zhǔn)的運(yùn)行時(shí)間。

本文所有試驗(yàn)的計(jì)算機(jī)配置為：AMD A8 2.1 GHz CPU、4 GB內(nèi)存，WIN7 64位操作系統(tǒng)。點(diǎn)云配準(zhǔn)方法采用C++語言在Visual 2010上編程實(shí)現(xiàn)，并調(diào)用PCL 1.7.2版本[16]的點(diǎn)云庫顯示和保存點(diǎn)云。

為了驗(yàn)證本文方法的有效性，試驗(yàn)中的點(diǎn)云數(shù)據(jù)采用斯坦福大學(xué)提供的Bunny、Happy和Dragon數(shù)據(jù)樣本。數(shù)據(jù)大小分別為：35 947個(gè)Bunny數(shù)據(jù)，32 328個(gè)Happy數(shù)據(jù)，22 998個(gè)Dragon數(shù)據(jù)。

2.2 配準(zhǔn)的結(jié)果展示

2.2.1 Bunny配準(zhǔn)結(jié)果

將Bunny的點(diǎn)云數(shù)據(jù)分別代入基于SVD算法、LM算法以及本文方法。3種算法各自迭代30次，Bunny點(diǎn)云配準(zhǔn)的結(jié)果如圖2所示。

圖2 Bunny點(diǎn)云配準(zhǔn)結(jié)果

從圖2可知，本文方法和SVD算法優(yōu)于LM算法。在Bunny的耳朵處，LM算法配準(zhǔn)得不是很好，出現(xiàn)大量的不配準(zhǔn)點(diǎn)。為了客觀地表示Bunny點(diǎn)云配準(zhǔn)的結(jié)果，分別對(duì)上面3種算法迭代不同的次數(shù)，比較其配準(zhǔn)時(shí)間和配準(zhǔn)精度，結(jié)果如表1所示。

表1 Bunny點(diǎn)云配準(zhǔn)精度和時(shí)間對(duì)比

從表1可以得出,本文方法配準(zhǔn)精度和速度明顯優(yōu)于LM算法和SVD算法。LM算法在迭代15次時(shí)配準(zhǔn)精度出現(xiàn)最高，精度數(shù)量級(jí)達(dá)到10-8；在迭代20次時(shí)精度降低，然后隨著迭代次數(shù)的增加精度提高。SVD算法在迭代30次時(shí)達(dá)到局部最優(yōu)，得到最佳的匹配，精度數(shù)量級(jí)達(dá)到10-14。本文方法在迭代15次時(shí)，精度數(shù)量級(jí)達(dá)就到10-16，明顯超過其他2種算法的最佳配準(zhǔn)精度數(shù)量級(jí)。在3種算法分別迭代40次時(shí)，LM算法在速度上是最慢的，本文方法最快，所用時(shí)間是SVD算法所要時(shí)間的6倍、LM算法的11倍。很明顯，本文方法在速度上明顯優(yōu)于其他2種算法。

2.2.2 Happy配準(zhǔn)結(jié)果

將Happy的點(diǎn)云數(shù)據(jù)分別代入3種算法。3種算法各自迭代30次的配準(zhǔn)結(jié)果如圖3所示。

由圖3可知，本文方法和LM算法優(yōu)于SVD算法。SVD算法在Happy的上部分右邊處配準(zhǔn)得不是很好。

同樣地，為了客觀表示Happy點(diǎn)云配準(zhǔn)的結(jié)果，分別對(duì)3種算法迭代不同的次數(shù)，比較其配準(zhǔn)時(shí)間和配準(zhǔn)精度，結(jié)果如表2所示。

圖3 Happy點(diǎn)云配準(zhǔn)結(jié)果

迭代次數(shù)LM算法精度/mt/msSVD算法精度/mt/ms本文方法精度/mt/ms103.56e-0646 9076.49e-0610 3132.10e-0710 098157.27e-0770 4423.13e-0617 4314.15e-1614 262202.48e-0797 3641.52e-0622 7884.84e-1614 689256.48e-09118 3567.15e-0728 1135.89e-1615 621301.27e-12141 5893.92e-0733 6566.87e-1615 789356.48e-16161 3703.37e-1439 3626.87e-1616 216408.90e-16167 4094.02e-1441 4416.87e-1616 542

由表2可知，LM算法從迭代初始次數(shù)時(shí)配準(zhǔn)精度就高于SVD算法，在迭代40次時(shí)配準(zhǔn)精度高于本文方法。但是，本文方法收斂速度仍然是3種算法中最快的。在迭代15次時(shí)，本文方法精度數(shù)量級(jí)達(dá)就到10-16、LM算法精度數(shù)量級(jí)為10-7、SVD算法精度數(shù)量級(jí)為10-6，證明本文方法配準(zhǔn)精度遠(yuǎn)高于其他2種算法。在配準(zhǔn)時(shí)間上，LM算法總大于SVD算法和本文方法。LM算法迭代10次所花時(shí)間大于SVD算法迭代40次的時(shí)間，本文方法的配準(zhǔn)時(shí)間仍然是最小的，明顯小于其他2種算法。

2.2.3 Dragon配準(zhǔn)結(jié)果

將Dragon的點(diǎn)云數(shù)據(jù)分別代入3種SVD算法。3種算法各自迭代30次，Dragon點(diǎn)云配準(zhǔn)結(jié)果如圖4所示。

從圖4可知，3種算法的配準(zhǔn)效果都較好。同樣地，為了客觀表示Dragon點(diǎn)云配準(zhǔn)的結(jié)果，分別采用上面3種算法迭代不同的次數(shù)，比較其配準(zhǔn)時(shí)間和配準(zhǔn)精度，結(jié)果如表3所示。

圖4 Dragon點(diǎn)云配準(zhǔn)結(jié)果

迭代次數(shù)LM算法精度/mt/msSVD算法精度/mt/ms本文方法精度/mt/ms103.39e-0636 7763.17e-067 5935.10e-087 174152.81e-0654 5614.42e-0711 5563.36e-127 531209.13e-1169 8184.14e-1414 5353.76e-177 900257.96e-1183 0254.66e-1414 8533.76e-178 156307.61e-1195 7604.66e-1415 2303.76e-178 289357.33e-11111 3114.66e-1415 5493.76e-178 366407.06e-11124 1174.66e-1415 7423.76e-178 741

由表3可知，從精度上看，LM算法是3種算法中最差的一種，在迭代次數(shù)超過25次時(shí)收斂幾乎保持不變，在整過迭代過程中沒有出現(xiàn)局部最優(yōu)值； SVD算法在迭代20次時(shí)超過LM算法，在迭代25次時(shí)出現(xiàn)局部最優(yōu)值；本文方法在迭代20次時(shí)就出現(xiàn)局部最優(yōu)值，得到最佳的配準(zhǔn)結(jié)果，配準(zhǔn)精度結(jié)果為10-17，在精度數(shù)量級(jí)上超過LM算法和SVD算法。從時(shí)間上看，本文方法在配準(zhǔn)時(shí)間仍然最少的，SVD算法次之，LM算法最多。本文方法在配準(zhǔn)精度和速度都好于其他兩種算法。

2.2.4 單片點(diǎn)云配準(zhǔn)結(jié)果

在以上試驗(yàn)中，分別比較了3種算法對(duì)完整點(diǎn)云數(shù)據(jù)的配準(zhǔn)結(jié)果。為了驗(yàn)證本文方法的普遍有效性，對(duì)單片非閉合的點(diǎn)云進(jìn)行配準(zhǔn)。數(shù)據(jù)采用了Bunny的頭部,點(diǎn)云數(shù)據(jù)量為23 317; Happy的底座,點(diǎn)云數(shù)據(jù)量為6 823; Dragon的右部分,點(diǎn)云數(shù)據(jù)量為12 414。分別采用3種算法對(duì)以上點(diǎn)云數(shù)據(jù)進(jìn)行處理，各自迭代30次，單片點(diǎn)云配準(zhǔn)結(jié)果如圖5所示。

圖5 單片點(diǎn)云配準(zhǔn)結(jié)果

由圖5可知，第1行中Bunny的頭部，3種算法配準(zhǔn)精度無明顯的差別；第2行中Happy的底座，LM算法和本文方法明顯優(yōu)于SVD算法；第3行中Dragon的右部分腿，LM算法比本文方法和SVD算法差。為了更加精確、直觀地描述3種算法的配準(zhǔn)精度和速度，將3種算法各自迭代30次，結(jié)果如表4所示。

表4 單片點(diǎn)云配準(zhǔn)精度和時(shí)間對(duì)比

由表4可知，在第1行Bunny中，LM算法在配準(zhǔn)精度上要高于SVD算法，但時(shí)間上要遠(yuǎn)大于SVD算法，本文方法在配準(zhǔn)精度和時(shí)間上都要優(yōu)于其他兩種算法；在第2行Happy中，SVD算法在配準(zhǔn)精度和時(shí)間上都好于LM算法，本文方法是三者算法中最好的；在第3行Dragon中，LM算法在配準(zhǔn)精度上高于SVD算法，本文方法遠(yuǎn)超LM算法和SVD算法，還是三者中最好的。從總體上來看，本文方法對(duì)單片非閉合的點(diǎn)云配準(zhǔn)精度最高，精度數(shù)量級(jí)達(dá)到10-17～10-16，與閉合的點(diǎn)云數(shù)據(jù)幾乎一致，其他2種算法在單片非閉合點(diǎn)云的配準(zhǔn)上誤差較大，與閉合的點(diǎn)云數(shù)據(jù)相差較大。

3 結(jié)束語

點(diǎn)云配準(zhǔn)是點(diǎn)云數(shù)據(jù)處理中最重要的一步，其關(guān)鍵在于求解位置變換矩陣。針對(duì)目前求解變換矩陣精度不高、耗時(shí)大的問題，提出一種新的求解變換矩陣的方法；結(jié)合K-D tree來加快最近點(diǎn)的搜索速度，并采用RANSAC剔除錯(cuò)誤的點(diǎn)云，以減少迭代的次數(shù)，最終完成點(diǎn)云的配準(zhǔn)。試驗(yàn)結(jié)果表明，本文方法在配準(zhǔn)精度和時(shí)間上優(yōu)于LM算法和SVD算法。本文方法的精度數(shù)量級(jí)達(dá)到10-17～10-16,是其他兩種算法不可達(dá)到的，由此證明了本文方法的有效性和時(shí)效性。下一步將對(duì)點(diǎn)云數(shù)據(jù)有噪聲和點(diǎn)云密集程度大的情況開展研究工作。