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

汪 威，張開顏，劉亞川，黃玉春

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

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

點(diǎn)云粗配準(zhǔn)依賴于點(diǎn)云特征參數(shù)的選?。?-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)對匹配過程的運(yùn)算量較大［8］。張廣鵬等［9］利用主成分分析法求得了點(diǎn)云的主軸坐標(biāo)系，并將其作為人臉點(diǎn)云的特征參數(shù)，但是，該方法的求解效果依賴于點(diǎn)云外形的對稱性。Stoyanov等［10］對點(diǎn)云所在空間進(jìn)行網(wǎng)格劃分，選取其中某一片點(diǎn)云為模板并構(gòu)建高斯混合模型，以其作為點(diǎn)云特征參數(shù)。張琮毅等［11］根據(jù)點(diǎn)云的顏色，采用不同點(diǎn)云特征參數(shù)對特征空間進(jìn)行初始匹配：對于無顏色的點(diǎn)云，計算其FPFH作為特征參數(shù)；對于有顏色的點(diǎn)云，計算其方向直方圖簽名作為特征參數(shù)。

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

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

1.1 特征參數(shù)選取

機(jī)械零件常常包含具有平面、球面和圓柱面等幾何特征的多個表面。通過激光掃描得到機(jī)械零件的三維點(diǎn)云，再利用統(tǒng)計學(xué)方法提取點(diǎn)云中的某些幾何特征是點(diǎn)云位姿測算的有效手段之一。以具有平面特征的工件的三維點(diǎn)云為研究對象，對兩步校準(zhǔn)法的原理進(jìn)行分析。首先，從三維點(diǎn)云中隨機(jī)選取3個點(diǎn)：A（Ax，Ay，A）z、B（Bx，By，B）z和C（Cx，Cy，C）z，利用式（1）計算由上述3個點(diǎn)構(gòu)成的三角形的面積S，并基于平面矢量法建立3個點(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ù)性對投影點(diǎn)進(jìn)行順時針排序：首先，輸入隨機(jī)選取的3個點(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），計算得到三角形面積S，若S＜0，交換I中B和C的位置索引。

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

但是，在獲取工件點(diǎn)云的過程中，激光三維輪廓傳感器多是沿垂直于工件主平面的方向進(jìn)行掃描，導(dǎo)致所采集的工件主平面的點(diǎn)云密度比其他平面的大，故點(diǎn)云中任意不共線的三點(diǎn)所在平面的單位法向量趨近于主平面的單位法向量V。針對這一現(xiàn)象，本文采用密度聚類算法從平面單位法向量集合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）為計算兩點(diǎn)間歐式距離的函數(shù)，|Nε（F）i|表示Fi的ε-鄰域集合中的元素個數(shù)。

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

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

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

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

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

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

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

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

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

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

通過變換矩陣GRB實現(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)云傾斜度校正過程中，定義點(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)云灰度圖中對應(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)對應(yīng)的點(diǎn)云灰度圖中像素坐標(biāo)的集合。則點(diǎn)云在主平面內(nèi)進(jìn)行位置和方向校準(zhǔn)時的變換矩陣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)正確匹配時，可確定變換矩陣G，此時兩像素點(diǎn)集間歐式距離和SD達(dá)到最小。

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

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

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

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

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

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

2.1 高壓輸電線塔塔座位姿測量

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

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

選用的激光輪廓傳感器的型號為KEYENCE LJ-V7200。在掃描過程中，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個。

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

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

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

圖5 抽樣次數(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對5組高壓輸電線塔塔座點(diǎn)云對應(yīng)的主平面單位法向量V的聚類效果的影響。由于V為單位向量，其變化量可用變換前后末端點(diǎn)間的距離Dtan來表征。由圖6可知，隨著w的增大，高壓輸電線塔塔座主平面的單位法向量V的變化量趨于收斂；當(dāng)w≥0.38時，Dtan趨近于0。

2.3 高壓輸電線塔塔座位姿識別實驗

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

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

圖7 傾斜度校正過程中位姿不同的高壓輸電線塔塔座點(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）對應(yīng)的點(diǎn)云轉(zhuǎn)換為點(diǎn)云灰度圖，并利用1.2節(jié)中的公式獲取待處理點(diǎn)云與標(biāo)準(zhǔn)點(diǎn)云的相對位姿，即平移量dx、dy與旋轉(zhuǎn)角γ，如圖8所示。

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

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

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

利用兩步校準(zhǔn)法獲得待處理點(diǎn)云的相對位姿參數(shù)，并計算其與理論相對位姿參數(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)云位姿的耗時。由表3可知，利用兩步校準(zhǔn)法校準(zhǔn)點(diǎn)云位姿的最長耗時約為9 s。

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

采樣一致性法是一種在實際工程應(yīng)用中較為常用的粗配準(zhǔn)算法［16］。為了驗證本文兩步校準(zhǔn)法的實用性，選用點(diǎn)云算法庫（point cloud library，PCL）中的采樣一致性法對8組位姿不同的高壓輸電線塔塔座點(diǎn)云進(jìn)行校準(zhǔn)，校準(zhǔn)殘差和耗時分別如表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)耗時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，最長耗時為38 s。

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

3 結(jié) 論

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

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

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

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