三維點(diǎn)云的兩步校準(zhǔn)法及其應(yīng)用研究

汪 威，張開(kāi)顏，劉亞川，黃玉春

（1.湖北工業(yè)大學(xué)機(jī)械工程學(xué)院，湖北武漢430068；2.武漢大學(xué)遙感信息工程學(xué)院，湖北武漢430072）

點(diǎn)云校準(zhǔn)是指先獲取待處理點(diǎn)云與標(biāo)準(zhǔn)點(diǎn)云的相對(duì)位姿，再將待處理點(diǎn)云進(jìn)行平移和旋轉(zhuǎn)變換后，使其與標(biāo)準(zhǔn)點(diǎn)云位姿相同的過(guò)程。大部分文獻(xiàn)將點(diǎn)云位姿測(cè)算及相應(yīng)的剛體變換過(guò)程稱(chēng)為點(diǎn)云配準(zhǔn)，其中應(yīng)用最廣泛的是由Besl等［1］提出的迭代最近點(diǎn)（iterative closest point，ICP）法，該方法利用最近點(diǎn)對(duì)的歐式距離對(duì)點(diǎn)云間的變換矩陣進(jìn)行動(dòng)態(tài)最小二乘擬合，其配準(zhǔn)精度極高。但是，為了避免在匹配點(diǎn)對(duì)過(guò)程中陷入局部最優(yōu)，要求2個(gè)點(diǎn)云的相對(duì)位姿不能太大，故在實(shí)際應(yīng)用中通常先對(duì)點(diǎn)云進(jìn)行粗配準(zhǔn)，在得到接近位姿后再用ICP法進(jìn)行精配準(zhǔn)［2-3］。

點(diǎn)云粗配準(zhǔn)依賴于點(diǎn)云特征參數(shù)的選取［4-6］。Rusu等［7］將小范圍內(nèi)點(diǎn)與點(diǎn)之間的夾角、距離等幾何信息轉(zhuǎn)換為高維度的點(diǎn)特征直方圖（fast point feature histograms，F(xiàn)PFH），并將其作為點(diǎn)云特征參數(shù)，但由于FPFH的維度較高，使得點(diǎn)對(duì)匹配過(guò)程的運(yùn)算量較大［8］。張廣鵬等［9］利用主成分分析法求得了點(diǎn)云的主軸坐標(biāo)系，并將其作為人臉點(diǎn)云的特征參數(shù)，但是，該方法的求解效果依賴于點(diǎn)云外形的對(duì)稱(chēng)性。Stoyanov等［10］對(duì)點(diǎn)云所在空間進(jìn)行網(wǎng)格劃分，選取其中某一片點(diǎn)云為模板并構(gòu)建高斯混合模型，以其作為點(diǎn)云特征參數(shù)。張琮毅等［11］根據(jù)點(diǎn)云的顏色，采用不同點(diǎn)云特征參數(shù)對(duì)特征空間進(jìn)行初始匹配：對(duì)于無(wú)顏色的點(diǎn)云，計(jì)算其FPFH作為特征參數(shù)；對(duì)于有顏色的點(diǎn)云，計(jì)算其方向直方圖簽名作為特征參數(shù)。

然而，上述點(diǎn)云粗配準(zhǔn)方法在選取、匹配特征參數(shù)時(shí)運(yùn)算量過(guò)大，導(dǎo)致其執(zhí)行效率難以滿足實(shí)際應(yīng)用的要求。因此，本文以具有平面特征的工件的三維點(diǎn)云為研究對(duì)象，提出了一種三維點(diǎn)云兩步校準(zhǔn)法：在不損失精度的前提下，利用密度聚類(lèi)算法提取平面特征，并結(jié)合二維圖像模板匹配算法實(shí)現(xiàn)點(diǎn)云的快速校準(zhǔn)。最后將提出的三維點(diǎn)云兩步校準(zhǔn)法應(yīng)用于高壓輸電線塔塔座自動(dòng)焊接系統(tǒng)，以識(shí)別塔座的位姿信息。

1 三維點(diǎn)云兩步校準(zhǔn)法的原理分析

1.1 特征參數(shù)選取

機(jī)械零件常常包含具有平面、球面和圓柱面等幾何特征的多個(gè)表面。通過(guò)激光掃描得到機(jī)械零件的三維點(diǎn)云，再利用統(tǒng)計(jì)學(xué)方法提取點(diǎn)云中的某些幾何特征是點(diǎn)云位姿測(cè)算的有效手段之一。以具有平面特征的工件的三維點(diǎn)云為研究對(duì)象，對(duì)兩步校準(zhǔn)法的原理進(jìn)行分析。首先，從三維點(diǎn)云中隨機(jī)選取3個(gè)點(diǎn)：A（Ax，Ay，A）z、B（Bx，By，B）z和C（Cx，Cy，C）z，利用式（1）計(jì)算由上述3個(gè)點(diǎn)構(gòu)成的三角形的面積S，并基于平面矢量法建立3個(gè)點(diǎn)在直角坐標(biāo)系內(nèi)xoy平面上的投影位置與三角形面積S之間的關(guān)系，如圖1所示。由式（1）可知，投影點(diǎn)的位置排序決定了三角形面積S的正負(fù)性。

圖1 xoy平面內(nèi)投影點(diǎn)的位置與三角形面積的關(guān)系Fig.1 Relationship between position of projection point and triangle area in xoy plane

根據(jù)S的正負(fù)性對(duì)投影點(diǎn)進(jìn)行順時(shí)針排序：首先，輸入隨機(jī)選取的3個(gè)點(diǎn)在xoy面內(nèi)的投影點(diǎn)集合P；然后，根據(jù)投影點(diǎn)的x坐標(biāo)，按由小到大的順序?qū)ν队包c(diǎn)進(jìn)行排序，設(shè)排序后的投影點(diǎn)集合I={A ，B，C} ；最后，將I中各點(diǎn)的坐標(biāo)代入式（1），計(jì)算得到三角形面積S，若S＜0，交換I中B和C的位置索引。

完成投影點(diǎn)排序后，計(jì)算I中3個(gè)投影點(diǎn)所在平面的單位法向量。因I中3個(gè)投影點(diǎn)為順時(shí)針排列，故可確定該單位法向量的方向。對(duì)工件的三維點(diǎn)云進(jìn)行n（n為大于1的任意整數(shù)）次抽樣后，得到平面單位法向量集合F={F1，F(xiàn)2，…，F(xiàn)n} 。

但是，在獲取工件點(diǎn)云的過(guò)程中，激光三維輪廓傳感器多是沿垂直于工件主平面的方向進(jìn)行掃描，導(dǎo)致所采集的工件主平面的點(diǎn)云密度比其他平面的大，故點(diǎn)云中任意不共線的三點(diǎn)所在平面的單位法向量趨近于主平面的單位法向量V。針對(duì)這一現(xiàn)象，本文采用密度聚類(lèi)算法從平面單位法向量集合F中提取V［12-13］。為闡述提取原理，引入以下概念。

定義下標(biāo)變量i、j、k為F中元素的位置索引，變量Nmin為密度閾值，ε為半徑參數(shù)。

定義Fi的ε-鄰域集合為Nε（F）i，Nε（F）i={Fj∈F|dist(Fj，F(xiàn)i)≤ε}，其中dis（t）為計(jì)算兩點(diǎn)間歐式距離的函數(shù)，|Nε（F）i|表示Fi的ε-鄰域集合中的元素個(gè)數(shù)。

核心點(diǎn)：對(duì)于Fi∈F，若其ε-鄰域集合中的元素個(gè)數(shù)|Nε（F）i|≥Nmin，則Fi為核心點(diǎn)。

密度直達(dá)：若Fj∈Nε（F）i，且Fi為核心點(diǎn)，則稱(chēng)Fj與Fi關(guān)于ε和Nmin密度直達(dá)。

密度可達(dá)：對(duì)于集合F，若任意Fi與Fi+1關(guān)于ε與Nmin密度直達(dá)，則F1與Fn關(guān)于ε和Nmin密度可達(dá)。

密度相連：對(duì)于 Fi與 Fj，若存在核心點(diǎn) Fk使Fi與Fk及Fj與 Fk均關(guān)于ε和 Nmin密度可達(dá)，則稱(chēng)Fi與 Fj密度相連。

圖2為密度聚類(lèi)算法的原理示意圖，其中三角形樣本點(diǎn)為核心點(diǎn)，Nmin=5。所有與某核心點(diǎn)密度直達(dá)的圓形樣本點(diǎn)在該核心點(diǎn)的ε-鄰域范圍內(nèi)構(gòu)成一個(gè)點(diǎn)簇，而不在核心點(diǎn)ε-鄰域范圍內(nèi)的樣本點(diǎn)則定義為噪聲。若某一個(gè)點(diǎn)簇的鄰域半徑內(nèi)存在另一個(gè)點(diǎn)簇，則這2個(gè)點(diǎn)簇密度相連，將其合并為一個(gè)點(diǎn)簇。通過(guò)合并，點(diǎn)簇的邊界可以不斷擴(kuò)展。

圖2 密度聚類(lèi)算法的原理示意圖Fig.2 Schematic diagram of principle of density clustering algorithm

采用密度聚類(lèi)算法從點(diǎn)云的平面單位法向量集合F中提取主平面的單位法向量V。算法的輸入為平面單位法向量集合F、半徑參數(shù)ε和密度閾值Nmin，輸出為主平面的單位法向量V，具體程序如下。

1.2 點(diǎn)云傾斜度校正

令激光三維輪廓傳感器的掃描主平面為xoy平面，縱向測(cè)量方向?yàn)閦軸方向。按右手定則建立基坐標(biāo)系oG-xGyGzG，另定義一個(gè)初始位置與基坐標(biāo)系oG-xGyGzG完全重合的剛體坐標(biāo)系oB-xByBzB。將工件主平面單位法向量與基坐標(biāo)系的xoy面單位法向量（即z軸）的夾角θ定義為兩平面間的傾斜度。

通過(guò)密度聚類(lèi)算法確定工件主平面單位法向量V=（Vx，Vy，Vz）。設(shè)Vstd為標(biāo)準(zhǔn)位姿模板圖像主平面的單位法向量（目標(biāo)向量），Vstd=（0，0，1），則Vstd與V的夾角θ可由式（2）求得。

通過(guò)變換矩陣GRB實(shí)現(xiàn)點(diǎn)云的傾斜度校正。設(shè)BP為點(diǎn)云在剛體坐標(biāo)系oB-xByBzB中的坐標(biāo)，GP為經(jīng)傾斜度校正后點(diǎn)云在基坐標(biāo)系oG-xGyGzG中的坐標(biāo)。在點(diǎn)云傾斜度校正過(guò)程中，定義點(diǎn)云繞x軸的旋轉(zhuǎn)角為α，繞y軸的旋轉(zhuǎn)角為β。則有：

其中：

式中：versθ=1-cosθ

可得：

1.3 點(diǎn)云的位置與方向校準(zhǔn)

基于點(diǎn)云中各點(diǎn)的高度（z坐標(biāo)），將經(jīng)傾斜度校正后的點(diǎn)云轉(zhuǎn)換為點(diǎn)云灰度圖，轉(zhuǎn)換公式為：

式中：GPx、GPy和GPz分別為點(diǎn)云中各點(diǎn)在基坐標(biāo)系中的坐標(biāo)；Su、Sv分別為點(diǎn)云灰度圖在寬度方向和高度方向上的量化因子；uP、vP分別為點(diǎn)云中各點(diǎn)在點(diǎn)云灰度圖中對(duì)應(yīng)的像素坐標(biāo)；（fuP，vP）為灰度圖中像素點(diǎn)（uP，vP）的灰度。

設(shè)標(biāo)準(zhǔn)點(diǎn)云的灰度圖為模板，其像素坐標(biāo)的集合M={Mx，My}，待處理點(diǎn)云灰度圖的像素坐標(biāo)集合D={Dx，Dy} ，其中Mx、Dx和My、Dy分別表示點(diǎn)云中各點(diǎn)x、y坐標(biāo)對(duì)應(yīng)的點(diǎn)云灰度圖中像素坐標(biāo)的集合。則點(diǎn)云在主平面內(nèi)進(jìn)行位置和方向校準(zhǔn)時(shí)的變換矩陣G滿足：

式中：dx、dy為點(diǎn)云沿x、y方向的平移量；γ為點(diǎn)云繞z軸的旋轉(zhuǎn)角。

采用圖像模板匹配算法求解變換矩陣G［14-15］。先求解點(diǎn)云灰度圖各像素點(diǎn)的梯度響應(yīng)值g(uP，vP)，再設(shè)置合適的閾值獲取點(diǎn)云灰度圖中具有較大灰度梯度的像素點(diǎn)，即邊緣像素點(diǎn)。

設(shè)標(biāo)準(zhǔn)點(diǎn)云灰度圖的邊緣像素點(diǎn)坐標(biāo)的集合為m，待處理點(diǎn)云灰度圖的邊緣像素點(diǎn)坐標(biāo)的集合為d。當(dāng)m與d中的像素點(diǎn)正確匹配時(shí)，可確定變換矩陣G，此時(shí)兩像素點(diǎn)集間歐式距離和SD達(dá)到最小。

設(shè)置迭代次數(shù)為K，利用式（7）求解變換矩陣G，具體步驟如下：

步驟1：在m中搜索d的最近點(diǎn)作為匹配點(diǎn)。

步驟2：基于得到的所有匹配點(diǎn)，利用式（7）得到估計(jì)變換矩陣GK。

步驟3：應(yīng)用估計(jì)變換矩陣GK對(duì)邊緣像素點(diǎn)進(jìn)行變換，得到新的邊緣像素點(diǎn)坐標(biāo)的集合mK-1=GKmK。

步驟4：設(shè)置閾值δ，若滿足|SDK-1-SDK|＜δ，令G=GK，并利用式（5）計(jì)算點(diǎn)云的平移量dx、dy與旋轉(zhuǎn)角γ；否則K=K-1，返回步驟1）直到滿足閾值條件或K=1。

2 三維點(diǎn)云兩步校準(zhǔn)法的應(yīng)用與驗(yàn)證

2.1 高壓輸電線塔塔座位姿測(cè)量

高壓輸電線塔塔座由多塊板件焊接而成。在焊接前，將塔座各板件通過(guò)點(diǎn)焊固定在一起，并置于自動(dòng)焊接系統(tǒng)的工作臺(tái)上，如圖3（a）所示。由于塔座各構(gòu)件的一致性較差，每次焊接前均需要進(jìn)行人工示教，導(dǎo)致焊接效率較低，因此設(shè)計(jì)了一套可在線識(shí)別塔座位姿的自動(dòng)焊接系統(tǒng)，可為焊接機(jī)器人提供路徑規(guī)劃的依據(jù)，其結(jié)構(gòu)如圖3（b）所示。在該自動(dòng)焊接系統(tǒng)中，固定在工作臺(tái)上方的激光輪廓傳感器通過(guò)掃描得到待焊接塔座的三維點(diǎn)云，然后基于標(biāo)準(zhǔn)塔座的點(diǎn)云（通過(guò)掃描處于標(biāo)準(zhǔn)位姿的塔座得到），利用兩步校準(zhǔn)法對(duì)采集的三維點(diǎn)云進(jìn)行校準(zhǔn)，獲取待焊接塔座的相對(duì)位姿。所設(shè)計(jì)的高壓輸電線塔塔座自動(dòng)焊接系統(tǒng)的工作流程如圖4所示。

圖3 高壓輸電線塔塔座自動(dòng)焊接系統(tǒng)及其結(jié)構(gòu)組成Fig.3 Automatic welding system for tower base of high-voltage transmission line and its structure composition

選用的激光輪廓傳感器的型號(hào)為KEYENCE LJ-V7200。在掃描過(guò)程中，x軸方向的采樣間距為100 μm，y軸方向的采樣間距為50 μm，采樣速率約為1.56 m/s。工控機(jī)配置CPU Intel i5-6500，內(nèi)存為8 G的Windows7 64位操作系統(tǒng)。利用激光輪廓傳感器掃描高壓輸電線塔塔座并完成數(shù)據(jù)預(yù)處理，得到的三維點(diǎn)云的樣本點(diǎn)總數(shù)Nt≈34 000個(gè)。

圖4 高壓輸電線塔塔座自動(dòng)焊接系統(tǒng)工作流程Fig.4 Working process of automatic welding system for tower base of high-voltage transmission line

2.2 抽樣次數(shù)優(yōu)化與主平面單位法向量提取

圖5為對(duì)高壓輸電線塔塔座點(diǎn)云抽樣500，2 500，12 500次時(shí)獲得的平面單位法向量集合F的散點(diǎn)分布情況。由圖5可知，隨著抽樣次數(shù)的增加，點(diǎn)簇的聚集效應(yīng)趨于明顯，即抽樣次數(shù)n=12 500時(shí)，平面單位法向量集合的分布具有較好的集聚效果。但抽樣次數(shù)過(guò)多會(huì)導(dǎo)致數(shù)據(jù)處理時(shí)間過(guò)長(zhǎng)，過(guò)少則將影響位姿識(shí)別精度。因此選擇合理的抽樣次數(shù)是采用密度聚類(lèi)算法提取主平面單位法向量V時(shí)的重要環(huán)節(jié)。

圖5 抽樣次數(shù)不同時(shí)平面單位法向量集合的散點(diǎn)分布情況Fig.5 Scattered distribution of unit normal vector set of plane under different sampling times

為得到最優(yōu)的抽樣次數(shù)，定義抽樣次數(shù)n與點(diǎn)云中樣本點(diǎn)總數(shù)Nt的比值為w。圖6為w對(duì)5組高壓輸電線塔塔座點(diǎn)云對(duì)應(yīng)的主平面單位法向量V的聚類(lèi)效果的影響。由于V為單位向量，其變化量可用變換前后末端點(diǎn)間的距離Dtan來(lái)表征。由圖6可知，隨著w的增大，高壓輸電線塔塔座主平面的單位法向量V的變化量趨于收斂；當(dāng)w≥0.38時(shí)，Dtan趨近于0。

2.3 高壓輸電線塔塔座位姿識(shí)別實(shí)驗(yàn)

設(shè)置抽樣次數(shù)n=13 000，密度聚類(lèi)算法中的參數(shù)ε=0.015，Nmin=20。利用兩步校準(zhǔn)法對(duì)高壓輸電線塔塔座的位姿進(jìn)行識(shí)別。圖7為傾斜度校正過(guò)程中位姿不同的高壓輸電線塔塔座點(diǎn)云。

圖6 w對(duì)主平面單位法向量聚類(lèi)效果的影響Fig.6 Influence of w on clustering effect of principal plane unit normal vector

圖7 傾斜度校正過(guò)程中位姿不同的高壓輸電線塔塔座點(diǎn)云Fig.7 Point clouds of tower base of high-voltage transmission line with different poses during inclination correction

利用式（4）將圖7（a）、圖7（c）對(duì)應(yīng)的點(diǎn)云轉(zhuǎn)換為點(diǎn)云灰度圖，并利用1.2節(jié)中的公式獲取待處理點(diǎn)云與標(biāo)準(zhǔn)點(diǎn)云的相對(duì)位姿，即平移量dx、dy與旋轉(zhuǎn)角γ，如圖8所示。

圖8 待處理點(diǎn)云與標(biāo)準(zhǔn)點(diǎn)云的相對(duì)位姿Fig.8 Relative pose between pending point cloud and standard point cloud

為了測(cè)試兩步校準(zhǔn)法對(duì)高壓輸電線塔塔座位姿識(shí)別的精度、穩(wěn)定性以及耗時(shí)，對(duì)8組位姿不同的塔座點(diǎn)云進(jìn)行校準(zhǔn)。各組點(diǎn)云的理論相對(duì)位姿參數(shù)如表1所示，其中第1組為標(biāo)準(zhǔn)點(diǎn)云，第2至第7組為待處理點(diǎn)云。

表1 8組點(diǎn)云的理論相對(duì)位姿參數(shù)Table 1 Theoretical relative pose parameters of eight groups of point cloud

利用兩步校準(zhǔn)法獲得待處理點(diǎn)云的相對(duì)位姿參數(shù)，并計(jì)算其與理論相對(duì)位姿參數(shù)的殘差，結(jié)果如表2所示。

表2 基于兩步校準(zhǔn)法的點(diǎn)云位姿校準(zhǔn)殘差Table 2 Residuals of pose alignment for point cloud based on two-step alignment method

由表2可知，基于兩步校準(zhǔn)法獲得的點(diǎn)云旋轉(zhuǎn)角的最大誤差約為0.1°，平移量的最大誤差約為0.2 mm。表3為利用兩步校準(zhǔn)法校準(zhǔn)點(diǎn)云位姿的耗時(shí)。由表3可知，利用兩步校準(zhǔn)法校準(zhǔn)點(diǎn)云位姿的最長(zhǎng)耗時(shí)約為9 s。

表3 基于兩步校準(zhǔn)法的點(diǎn)云位姿校準(zhǔn)耗時(shí)Table 3 Time-consuming of pose alignment for point cloud based on two-step alignment method

采樣一致性法是一種在實(shí)際工程應(yīng)用中較為常用的粗配準(zhǔn)算法［16］。為了驗(yàn)證本文兩步校準(zhǔn)法的實(shí)用性，選用點(diǎn)云算法庫(kù)（point cloud library，PCL）中的采樣一致性法對(duì)8組位姿不同的高壓輸電線塔塔座點(diǎn)云進(jìn)行校準(zhǔn)，校準(zhǔn)殘差和耗時(shí)分別如表4和表5所示。

表4 基于采樣一致性法的點(diǎn)云位姿校準(zhǔn)殘差Table 4 Residuals of pose alignment for point cloud based on sample consensus method

表5 基于采樣一致性法的點(diǎn)云位姿校準(zhǔn)耗時(shí)Table 5 Time-consuming of pose alignment for point cloud based on sample consensus method

由表4和表5可知，基于采樣一致性法獲得的點(diǎn)云旋轉(zhuǎn)角的最大誤差約為1.6°，平移量的最大誤差約為7.0 mm，最長(zhǎng)耗時(shí)為38 s。

根據(jù)上述結(jié)果可知，在對(duì)具有平面特征的工件進(jìn)行位姿識(shí)別時(shí)，本文提出的兩步校準(zhǔn)法能夠較準(zhǔn)確地獲得工件的相對(duì)位姿變化信息，且在耗時(shí)上比采樣一致性法約縮短了76%。

3 結(jié) 論

1）針對(duì)具有平面特征的工件的位姿識(shí)別，提出了一種基于傾斜度校正與圖像模板匹配算法的三維點(diǎn)云兩步校準(zhǔn)法。通過(guò)順時(shí)針排序與密度聚類(lèi)算法，解決了點(diǎn)云特征參數(shù)的選取問(wèn)題。

2）通過(guò)分析抽樣次數(shù)對(duì)點(diǎn)云特征參數(shù)提取的影響，得到了最優(yōu)抽樣次數(shù)的選擇規(guī)律，為兩步校準(zhǔn)法在實(shí)際工程中的應(yīng)用提供了依據(jù)。

3）將兩步校準(zhǔn)法應(yīng)用于高壓輸電線塔塔座自動(dòng)焊接系統(tǒng)，對(duì)塔座位姿進(jìn)行識(shí)別，并與基于采樣一致性法的位姿識(shí)別結(jié)果進(jìn)行對(duì)比。實(shí)驗(yàn)結(jié)果表明兩步校準(zhǔn)法在識(shí)別具有平面特征的工件的位姿時(shí)優(yōu)于采樣一致性法。

研究結(jié)果表明本文提出的三維點(diǎn)云兩步校準(zhǔn)法法在先進(jìn)制造與智能檢測(cè)領(lǐng)域具有廣闊的應(yīng)用前景。后續(xù)將在已有基礎(chǔ)上，通過(guò)提取點(diǎn)云的母線、曲率和信息熵等特征來(lái)進(jìn)一步確定點(diǎn)云的位姿。將方法的適用對(duì)象從平板類(lèi)工件擴(kuò)展到含曲面的工件，并結(jié)合人工智能算法進(jìn)一步提高算法的快速性和適應(yīng)性。