基于電磁追蹤系統(tǒng)的點(diǎn)云配準(zhǔn)方法研究

孟曉亮， 張立曄， 房 超

(山東理工大學(xué) 計(jì)算機(jī)科學(xué)與技術(shù)學(xué)院，山東 淄博 255049)

1 引 言

近年來(lái)，隨著計(jì)算機(jī)視覺(jué)技術(shù)的快速發(fā)展，結(jié)構(gòu)光三維測(cè)量技術(shù)也被廣泛應(yīng)用于制造業(yè)、醫(yī)學(xué)、機(jī)器視覺(jué)等領(lǐng)域。對(duì)于一個(gè)完整的物體，需要從不同視角進(jìn)行多次測(cè)量，然后把它們拼合成一個(gè)整體才能反映物體表面的完整信息，這個(gè)過(guò)程稱為點(diǎn)云配準(zhǔn)[1]，也稱為點(diǎn)云拼接。

點(diǎn)云配準(zhǔn)按照其配準(zhǔn)精度可以分為粗配準(zhǔn)(也稱初始配準(zhǔn))和精配準(zhǔn)。目前，精配準(zhǔn)方法中應(yīng)用最為廣泛的是Besl等人[2]提出的迭代最近點(diǎn)(ICP)算法及其改進(jìn)算法[3-4]，該算法通過(guò)迭代最近點(diǎn)作為匹配點(diǎn)，計(jì)算獲得使點(diǎn)對(duì)距離偏差最小的剛性變換，其配準(zhǔn)效果的好壞依賴于粗配準(zhǔn)的準(zhǔn)確度，因此需要先對(duì)點(diǎn)云進(jìn)行粗配準(zhǔn)?，F(xiàn)有的粗配準(zhǔn)方法包括：(1) 基于點(diǎn)云自身特征的配準(zhǔn)。孟禹等人[5]提出一種基于采樣球的點(diǎn)云粗配準(zhǔn)方法，通過(guò)尋找重疊區(qū)域內(nèi)近似重合的3個(gè)點(diǎn)集，實(shí)現(xiàn)點(diǎn)云的粗配準(zhǔn)；陸軍等人[6]采用法向量和曲率加權(quán)計(jì)算、特征點(diǎn)提取、對(duì)應(yīng)關(guān)系篩選方法，提高了配準(zhǔn)的準(zhǔn)確度和速度；伍夢(mèng)琦等人[7]利用點(diǎn)云的幾何特征和圖像特征完成了點(diǎn)云的粗配準(zhǔn)。此類方法針對(duì)點(diǎn)云特征明顯的物體可以實(shí)現(xiàn)很好的配準(zhǔn)，但針對(duì)特征較少和重疊區(qū)域較小的物體，其配準(zhǔn)效果會(huì)受影響。(2) 基于輔助特征的配準(zhǔn)。劉曉利等的[8]借助粘貼在被測(cè)物體表面的標(biāo)志點(diǎn)求取坐標(biāo)系轉(zhuǎn)換矩陣，但該方法不適用于軟質(zhì)物體與易損物體的測(cè)量。孫軍華等人[9-10]使用方格特征的平面靶標(biāo)，利用方格角點(diǎn)的匹配求取坐標(biāo)系轉(zhuǎn)換矩陣。魏新國(guó)等人[11]采用圓形特征的平面靶標(biāo)作為中介，求解前后測(cè)量位置的坐標(biāo)系轉(zhuǎn)換矩陣。此類方法不影響被測(cè)物表面自身的特征，但其測(cè)量范圍受限，靈活性較低。(3) 基于機(jī)械定位的配準(zhǔn)。徐永安等人[12]借助旋轉(zhuǎn)的轉(zhuǎn)臺(tái)以實(shí)現(xiàn)點(diǎn)云的自動(dòng)配準(zhǔn)，在系統(tǒng)標(biāo)定準(zhǔn)確的情況下，能夠得到較好的拼接準(zhǔn)確度。但是此類方法的測(cè)量范圍受限于機(jī)械結(jié)構(gòu)的尺寸，系統(tǒng)靈活性較低，且工業(yè)現(xiàn)場(chǎng)測(cè)量時(shí)不便于攜帶。

綜上所述，目前的點(diǎn)云粗配準(zhǔn)方法都有一定的局限性。為此，本文提出一種基于電磁追蹤系統(tǒng)的點(diǎn)云粗配準(zhǔn)方法，利用電磁追蹤系統(tǒng)高準(zhǔn)確度的位移和姿態(tài)角信息，通過(guò)不同坐標(biāo)系間的快速轉(zhuǎn)換，實(shí)現(xiàn)點(diǎn)云的快速粗配準(zhǔn)，再使用ICP精配準(zhǔn)方法，實(shí)現(xiàn)點(diǎn)云的準(zhǔn)確配準(zhǔn)。最后，通過(guò)配準(zhǔn)實(shí)驗(yàn)驗(yàn)證所提方法可有效提高粗配準(zhǔn)的速度，并為ICP精配準(zhǔn)提供良好的初值。

2 測(cè)量及配準(zhǔn)系統(tǒng)組成

本文配準(zhǔn)過(guò)程中使用的被測(cè)物點(diǎn)云信息由結(jié)構(gòu)光三維測(cè)量系統(tǒng)獲得。測(cè)量系統(tǒng)由一個(gè)工業(yè)相機(jī)及鏡頭、一個(gè)數(shù)字投影機(jī)和一臺(tái)計(jì)算機(jī)組成，配準(zhǔn)系統(tǒng)借助電磁追蹤系統(tǒng)的發(fā)射器和接收器獲取準(zhǔn)確的位移和姿態(tài)角信息，如圖1所示。

圖1 測(cè)量及配準(zhǔn)系統(tǒng)示意圖

電磁追蹤系統(tǒng)(FASTRAK)是美國(guó)Polhemus公司的一款電磁定位產(chǎn)品，由于其具有設(shè)備搭建簡(jiǎn)單、定位準(zhǔn)確度高、速度快等特點(diǎn)，能實(shí)時(shí)返回準(zhǔn)確的位移和姿態(tài)角信息。本文將借助該系統(tǒng)實(shí)現(xiàn)點(diǎn)云的粗配準(zhǔn)。電磁追蹤系統(tǒng)主要由電磁系統(tǒng)單元、發(fā)射器和接收器等組成。

電磁追蹤系統(tǒng)的發(fā)射器發(fā)射電磁波，接收器通過(guò)傳感器接收電磁波，依據(jù)電磁系統(tǒng)單元計(jì)算發(fā)射器和接收器之間準(zhǔn)確的位移和姿態(tài)角信息，通過(guò)USB傳輸，就可以連接計(jì)算機(jī)得到接收器相對(duì)于發(fā)射器的位移(x,y,z)和姿態(tài)角(γ,φ,ω)6個(gè)參數(shù)的數(shù)據(jù)。其中，x、y、z分別為接收器相對(duì)于發(fā)射器X軸、Y軸和Z軸的位移。γ為繞發(fā)射器Z軸旋轉(zhuǎn)的角度，φ為繞發(fā)射器Y軸旋轉(zhuǎn)的角度，ω為繞發(fā)射器X軸旋轉(zhuǎn)的角度。電磁追蹤系統(tǒng)的位移準(zhǔn)確度為0.076 mm，姿態(tài)角準(zhǔn)確度為0.015°。

3 配準(zhǔn)原理

本文采用結(jié)構(gòu)光三維測(cè)量系統(tǒng)獲取點(diǎn)云，并通過(guò)平移、旋轉(zhuǎn)被測(cè)物體獲取不同視角下的點(diǎn)云信息。將接收器與被測(cè)物體綁定，則接收器與被測(cè)物體之間的運(yùn)動(dòng)為剛體運(yùn)動(dòng)，通過(guò)接收器與發(fā)射器之間準(zhǔn)確的位移和姿態(tài)角信息，發(fā)射器坐標(biāo)系作為中間坐標(biāo)系，將不同視角下獲得的點(diǎn)云由世界坐標(biāo)系轉(zhuǎn)換到發(fā)射器坐標(biāo)系下，再由發(fā)射器坐標(biāo)系轉(zhuǎn)換到接收器坐標(biāo)系下，即實(shí)現(xiàn)了點(diǎn)云的粗配準(zhǔn)。本文以兩視角下的點(diǎn)云配準(zhǔn)為例，闡述其配準(zhǔn)原理。

3.1 基于平面的坐標(biāo)系轉(zhuǎn)換方法

采用結(jié)構(gòu)光三維測(cè)量系統(tǒng)獲取的點(diǎn)云都是相對(duì)于測(cè)量系統(tǒng)標(biāo)定時(shí)的世界坐標(biāo)系而言。不同的空間坐標(biāo)系測(cè)量同一個(gè)平面時(shí)，將獲得不同的平面方程，而被測(cè)量的平面唯一存在?？赏ㄟ^(guò)獲取空間某一平面上的點(diǎn)云分別在世界坐標(biāo)系和發(fā)射器坐標(biāo)系中的擬合平面求解坐標(biāo)系之間的轉(zhuǎn)換關(guān)系。

首先，獲取空間平面在世界坐標(biāo)系和發(fā)射器坐標(biāo)系下的平面方程。世界坐標(biāo)系下，采用結(jié)構(gòu)光三維測(cè)量系統(tǒng)，獲得平面的點(diǎn)云坐標(biāo)，利用最小二乘法擬合得到世界坐標(biāo)系下的平面方程：

awxw+bwyw+cwzw+dw=0

.

(1)

發(fā)射器坐標(biāo)系下，讓接收器在空間平面上滑動(dòng)，利用電磁追蹤系統(tǒng)軟件記錄接收器中心點(diǎn)坐標(biāo)序列，同樣采用最小二乘法擬合出平面方程。由于接收器有一定的尺寸，接收器在測(cè)量平面上滑動(dòng)時(shí)，得到的接收器中心點(diǎn)坐標(biāo)序列所在平面與結(jié)構(gòu)光測(cè)量系統(tǒng)擬合得到的平面平行，通過(guò)沿法線方向補(bǔ)償一個(gè)深度值即可將兩平面重合，并得到發(fā)射器坐標(biāo)系下的平面方程：

atxt+btyt+ctzt+dt=0

.

(2)

其次，在平面上選取3組不共線的3個(gè)點(diǎn)，設(shè)在世界坐標(biāo)系下3點(diǎn)分別為W1、W2、W3，在發(fā)射器坐標(biāo)系下的3點(diǎn)分別為T1、T2、T3。在選取這3組對(duì)應(yīng)點(diǎn)時(shí)，令xw=xt，yw=yt，zw和zt的值可根據(jù)式(1)和式(2)得到，令

，

(3)

，

(4)

令

ki=Ti-CW

，

(5)

，

(6)

式中，i=1, 2, 3，問(wèn)題轉(zhuǎn)化為求取如下目標(biāo)函數(shù)：

.

(7)

根據(jù)奇異值分解(SVD)理論，有

.

(8)

可得

H=UΛVT

.

(9)

進(jìn)而可以得到旋轉(zhuǎn)矩陣RWT和平移向量TWT：

RWT=VUT

，

(10)

TWT=CT-RWTCW

.

(11)

求出RWT和TWT后，利用如下公式，將測(cè)得的點(diǎn)云由世界坐標(biāo)系轉(zhuǎn)換到發(fā)射器坐標(biāo)系下：

，

(12)

式中，[xT,yT,zT]T為發(fā)射器坐標(biāo)系下的點(diǎn)云，[xW,yW,zW]T為結(jié)構(gòu)光三維測(cè)量系統(tǒng)獲取的點(diǎn)云。

3.2 發(fā)射器坐標(biāo)系與接收器坐標(biāo)系轉(zhuǎn)換關(guān)系的求取

配準(zhǔn)過(guò)程中，發(fā)射器坐標(biāo)系作為中間坐標(biāo)系，因此還需要將點(diǎn)云由發(fā)射器坐標(biāo)系轉(zhuǎn)換到接收器坐標(biāo)系下，其轉(zhuǎn)換公式如下：

，

(13)

，

(15)

式中，(x,y,z)和(γ,φ,ω)可借助電磁追蹤系統(tǒng)軟件實(shí)時(shí)獲取。

3.3 粗配準(zhǔn)步驟

(1) 根據(jù)3.1節(jié)中描述的方法，求取發(fā)射器坐標(biāo)系與世界坐標(biāo)系的轉(zhuǎn)換關(guān)系。

(2) 將接收器與被測(cè)物體綁定，形成剛性連接。并將被測(cè)物體放在結(jié)構(gòu)光三維測(cè)量系統(tǒng)下進(jìn)行測(cè)量，獲取該視角下的點(diǎn)云信息，并記為(xA,yA,zA)，如圖1所示，記錄下此時(shí)接收器相對(duì)于發(fā)射器坐標(biāo)系的位移和姿態(tài)角(x1,y1,z1,γ1,φ1,ω1)。

(3) 對(duì)被測(cè)物體旋轉(zhuǎn)一定角度，獲取旋轉(zhuǎn)后該視角下的點(diǎn)云信息，并記為(xB,yB,zB)，記錄下此時(shí)接收器相對(duì)于發(fā)射器坐標(biāo)系的位移和姿態(tài)角(x2,y2,z2,γ2,φ2,ω2)。

(4) 將兩視角下的點(diǎn)云根據(jù)3.1節(jié)中的方法由世界坐標(biāo)系轉(zhuǎn)換到發(fā)射器坐標(biāo)系下，再根據(jù)3.2節(jié)中的方法由發(fā)射器坐標(biāo)系轉(zhuǎn)換到接收器坐標(biāo)系下，合并兩點(diǎn)云數(shù)據(jù)，即實(shí)現(xiàn)了點(diǎn)云的粗配準(zhǔn)。

3.4 ICP精配準(zhǔn)

ICP算法即迭代最近點(diǎn)算法，給定兩視角下具有部分重疊的點(diǎn)云MA和MB以及它們之間的初始旋轉(zhuǎn)矩陣R0和平移矩陣T0。假設(shè)在初始位置，點(diǎn)云MA中任意一點(diǎn)MAi和離點(diǎn)云MB中的最近一點(diǎn)MBi為一對(duì)匹配點(diǎn)，針對(duì)MA中的所有點(diǎn)查找其在MB中的最近點(diǎn)，然后求取最佳的R和T，使目標(biāo)誤差函數(shù)最小。接著更新點(diǎn)云的相對(duì)位置，重復(fù)以上過(guò)程，不斷迭代，直至目標(biāo)誤差函數(shù)收斂或達(dá)到設(shè)定值，這就是ICP精配準(zhǔn)算法的計(jì)算過(guò)程。目標(biāo)誤差函數(shù)的表達(dá)式如下：

，

(16)

式中，N為匹配點(diǎn)對(duì)的個(gè)數(shù)。

通過(guò)本文提出的粗配準(zhǔn)方法，可實(shí)現(xiàn)兩視角下被測(cè)物體的快速粗配準(zhǔn)，為ICP精配準(zhǔn)提供良好的初值。結(jié)合ICP精配準(zhǔn)方法，可實(shí)現(xiàn)不同視角下點(diǎn)云的準(zhǔn)確配準(zhǔn)。

4 配準(zhǔn)實(shí)驗(yàn)

本文采用一個(gè)大恒工業(yè)相機(jī)(型號(hào)為DH-HV3151UC，分辨率為2 048×1 536)和一個(gè)數(shù)字投影機(jī)(型號(hào)為Infocus 82，分辨率為1 024×768)搭建結(jié)構(gòu)光三維測(cè)量系統(tǒng)，并固定好電磁追蹤系統(tǒng)，通過(guò)結(jié)構(gòu)光三維測(cè)量方法[13]獲取單視角下被測(cè)物體的點(diǎn)云信息，系統(tǒng)各部分位置如圖2所示。

圖2 配準(zhǔn)系統(tǒng)組成

4.1 平面配準(zhǔn)實(shí)驗(yàn)

為評(píng)價(jià)本文所提方法的配準(zhǔn)誤差，針對(duì)不同深度(距離投影機(jī)的遠(yuǎn)近)位置的平面進(jìn)行有重疊的兩視角平移配準(zhǔn)實(shí)驗(yàn)，通過(guò)計(jì)算配準(zhǔn)后重疊區(qū)域點(diǎn)云最近點(diǎn)距離的平均值來(lái)評(píng)價(jià)配準(zhǔn)誤差，并采用ICP方法進(jìn)行精配準(zhǔn)，配準(zhǔn)誤差結(jié)果如表1所示。

由表1可以看出，粗配準(zhǔn)后平面的配準(zhǔn)誤差在0.40 mm以下，經(jīng)ICP精配準(zhǔn)后，配準(zhǔn)誤差在0.04 mm 以下，所提粗配準(zhǔn)方法能為ICP精配準(zhǔn)提供良好的初值，驗(yàn)證了所提方法的有效性，且所提方法可對(duì)被測(cè)物體進(jìn)行任意旋轉(zhuǎn)或平移，配準(zhǔn)過(guò)程更加靈活。

表1 不同深度平面的配準(zhǔn)誤差 (mm)

4.2 復(fù)雜表面配準(zhǔn)實(shí)驗(yàn)

為進(jìn)一步驗(yàn)證本文所提方法的配準(zhǔn)效果，采用石膏像作為配準(zhǔn)對(duì)象，進(jìn)行兩視角下的點(diǎn)云配準(zhǔn)實(shí)驗(yàn)，配準(zhǔn)后的結(jié)果如圖3所示。

圖3 石膏像配準(zhǔn)結(jié)果

圖3(a)和圖3(b)為采用結(jié)構(gòu)光三維測(cè)量方法獲取的不同視角下的石膏像點(diǎn)云，圖3(c)為采用本文所提粗配準(zhǔn)方法進(jìn)行粗配準(zhǔn)后的結(jié)果。進(jìn)行粗配準(zhǔn)后，再結(jié)合ICP精配準(zhǔn)方法進(jìn)行精配準(zhǔn)，精配準(zhǔn)后的結(jié)果如圖3(d)所示。從圖中可以看出，精配準(zhǔn)后的石膏像視覺(jué)效果良好。

精配準(zhǔn)后的配準(zhǔn)誤差和迭代次數(shù)的關(guān)系如圖4所示?？梢钥闯觯捎肐CP方法進(jìn)行精配準(zhǔn)的配準(zhǔn)誤差，在迭代次數(shù)為10次左右時(shí)趨于收斂，配準(zhǔn)誤差約為0.04 mm。這也間接地表明本文所提粗配準(zhǔn)方法可為ICP精配準(zhǔn)提供良好的初值，使其在迭代過(guò)程中能快速收斂。

圖4 配準(zhǔn)誤差和迭代次數(shù)的關(guān)系

5 結(jié) 論

本文提出一種基于電磁追蹤系統(tǒng)的點(diǎn)云配準(zhǔn)方法，利用電磁追蹤系統(tǒng)發(fā)射器和接收器之間準(zhǔn)確的位移和姿態(tài)角信息，獲得坐標(biāo)系轉(zhuǎn)換矩陣，并提出一種基于平面的坐標(biāo)系轉(zhuǎn)換方法。配準(zhǔn)實(shí)驗(yàn)結(jié)果表明，本文所提粗配準(zhǔn)方法能夠?yàn)镮CP精配準(zhǔn)提供良好的初值，使ICP精配準(zhǔn)在迭代過(guò)程中能快速收斂，且配準(zhǔn)后效果良好，精配準(zhǔn)后石膏像的配準(zhǔn)誤差約為0.04 mm，驗(yàn)證了本文所提方法的有效性。但由于金屬會(huì)影響電磁追蹤系統(tǒng)的測(cè)量準(zhǔn)確度，因此目前該方法主要適用于非金屬物體表面的配準(zhǔn)。