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

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

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

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

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

1 三維點云兩步校準法的原理分析

1.1 特征參數(shù)選取

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

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

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

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

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

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

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

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

密度直達：若Fj∈Nε（F）i，且Fi為核心點，則稱Fj與Fi關于ε和Nmin密度直達。

密度可達：對于集合F，若任意Fi與Fi+1關于ε與Nmin密度直達，則F1與Fn關于ε和Nmin密度可達。

密度相連：對于 Fi與 Fj，若存在核心點 Fk使Fi與Fk及Fj與 Fk均關于ε和 Nmin密度可達，則稱Fi與 Fj密度相連。

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

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

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

1.2 點云傾斜度校正

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

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

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

其中：

式中：versθ=1-cosθ

可得：

1.3 點云的位置與方向校準

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

式中：GPx、GPy和GPz分別為點云中各點在基坐標系中的坐標；Su、Sv分別為點云灰度圖在寬度方向和高度方向上的量化因子；uP、vP分別為點云中各點在點云灰度圖中對應的像素坐標；（fuP，vP）為灰度圖中像素點（uP，vP）的灰度。

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

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

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

設標準點云灰度圖的邊緣像素點坐標的集合為m，待處理點云灰度圖的邊緣像素點坐標的集合為d。當m與d中的像素點正確匹配時，可確定變換矩陣G，此時兩像素點集間歐式距離和SD達到最小。

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

步驟1：在m中搜索d的最近點作為匹配點。

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

步驟3：應用估計變換矩陣GK對邊緣像素點進行變換，得到新的邊緣像素點坐標的集合mK-1=GKmK。

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

2 三維點云兩步校準法的應用與驗證

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

高壓輸電線塔塔座由多塊板件焊接而成。在焊接前，將塔座各板件通過點焊固定在一起，并置于自動焊接系統(tǒng)的工作臺上，如圖3（a）所示。由于塔座各構(gòu)件的一致性較差，每次焊接前均需要進行人工示教，導致焊接效率較低，因此設計了一套可在線識別塔座位姿的自動焊接系統(tǒng)，可為焊接機器人提供路徑規(guī)劃的依據(jù)，其結(jié)構(gòu)如圖3（b）所示。在該自動焊接系統(tǒng)中，固定在工作臺上方的激光輪廓傳感器通過掃描得到待焊接塔座的三維點云，然后基于標準塔座的點云（通過掃描處于標準位姿的塔座得到），利用兩步校準法對采集的三維點云進行校準，獲取待焊接塔座的相對位姿。所設計的高壓輸電線塔塔座自動焊接系統(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。工控機配置CPU Intel i5-6500，內(nèi)存為8 G的Windows7 64位操作系統(tǒng)。利用激光輪廓傳感器掃描高壓輸電線塔塔座并完成數(shù)據(jù)預處理，得到的三維點云的樣本點總數(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為對高壓輸電線塔塔座點云抽樣500，2 500，12 500次時獲得的平面單位法向量集合F的散點分布情況。由圖5可知，隨著抽樣次數(shù)的增加，點簇的聚集效應趨于明顯，即抽樣次數(shù)n=12 500時，平面單位法向量集合的分布具有較好的集聚效果。但抽樣次數(shù)過多會導致數(shù)據(jù)處理時間過長，過少則將影響位姿識別精度。因此選擇合理的抽樣次數(shù)是采用密度聚類算法提取主平面單位法向量V時的重要環(huán)節(jié)。

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

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

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

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

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

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

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

圖8 待處理點云與標準點云的相對位姿Fig.8 Relative pose between pending point cloud and standard point cloud

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

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

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

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

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

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

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

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

表5 基于采樣一致性法的點云位姿校準耗時Table 5 Time-consuming of pose alignment for point cloud based on sample consensus method

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

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

3 結(jié) 論

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

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

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

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