三維點云特征的工件識別與位姿估計

張 鐵，肖 卓，鄒焱飚，何英武

（1.華南理工大學機械與汽車工程學院，廣東 廣州 510640；2.廣州數控設備有限公司，廣東 廣州 510000）

1 引言

隨著機器人技術的發(fā)展，工業(yè)機器人逐漸應用于工業(yè)生產中的自動抓取及裝配中，需要對目標工件進行識別與位姿估計。工件的正確識別和準確的位姿估計都會影響工業(yè)生產的效率。因此，對工件的識別和位姿估計的研究具有重要的工程意義?；趫D像的物體識別與位姿估計已經有很多研究成果［1］，文獻［2］提出了一種基于區(qū)域劃分的平面工件識別與定位方法，該方法先利用LSD（Line Segment Detector）進行直線段的提取，然后將干擾的雜線段分類進行剔除，在保留的輪廓線段基礎上進行直線段的連接和區(qū)域劃分，最后利用各個區(qū)域的形狀特征向量和模板的形狀特征向量進行比對，完成工件的識別和定位。但是，圖像特征受到紋理、顏色、層次區(qū)分度、以及光照、分辨率、反射等外界環(huán)境的影響無法滿足現代工業(yè)要求。而通過三維掃描儀獲取3D點云數據已經非常便捷，將三維點云與位姿已知的模板點云進行匹配得到兩者之間的剛性變換，從而實現工件的識別和定位，成為位姿估計領域的研究熱點。

已有的三維點云工件識別與位姿估計方法是通過分析提取物體的特征點的幾何屬性、形狀屬性、結構屬性等特征進行比對、學習從而完成物體的識別?；诿枋鲎拥淖R別方法隨著描述子性能提升而不斷提高［3］，文獻［4］建立了定向點對特征的全局描述符，結合投票機制進行工件的識別與姿態(tài)估計；文獻［5］提出依靠歐幾里德距離評估特征描述符之間的相似性，訓練卷積神經網絡計算在描述符之間的相似性和差異性來計算描述符雖然該方法可以有效地捕獲對象標識和三維姿態(tài)，但其對3D物體識別的效果較差；文獻［6］給出一種隨機森林隨機抓取的位姿定位方法，提出立方體特征（CF）和基于段特征對（PSF）建立配對關系，采用隨機森林算法實現模板點云和場景點云對應關系查找；文獻［7］利用kinect采集到的深度信息及顏色與點云形狀特征實現目標的識別與定位；文獻［8］提出一種基于三維CAD模型生成測量對象點云數據庫，通過投票法識別并估計不同物體的6自由度姿態(tài)的方法。

綜上所述，這里針對存在干擾、目標遮擋、無紋理工件的場景中三維目標識別和姿態(tài)估計的問題，提出基于改進矢量構造的點云描述符SHOT（Signature of Histogram of Orientation），在估計點云法矢時將領域點距離信息考慮在內，構建工件點云特征；利用3D霍夫投票機制完成工件的識別，降低了錯誤的匹配概率；采用ICP（Iterative Closest Point）算法對點云進行精配準；實驗中，采用離線建立某一姿態(tài)下的模板點云特征描述庫，從不同的視角通過三維掃描儀獲取真實的場景點云，識別目標工件，計算其相對于模板點云的位姿。這里采用的模板點云特征匹配的目標識別及位姿估計的算法流程圖，如圖1所示。

圖1 3D物體識別與位姿估計框圖Fig.1 Block Diagrams for 3D Object Recognition and Pose Estimation

2 基于點云特征的識別

2.1 特征點提取

特征點提取是在整個流程圖中對模型點云和場景點云處理的第一步，對場景點云和模板點云采用統(tǒng)一重采樣，即通過建立點云的空間體素柵格進行下采樣，然后用每個體素內的重心代表該體素內的特征點F。

式中：n—點云中點的個數；(qxi，qyi，qzi)—每個點的沿x、y、z軸的坐標值。

2.2 點云法矢估計

對三維點云數據提取特征點后，以每個特征點的球形支持域內的局部坐標系建立SHOT描述符，因此建立局部坐標系需要具備唯一描述性和魯棒性，建立局部坐標系的過程如下，主要分為2步：

（1）由特征點F的k個最近鄰域點集P={pi，i=1，2...k}，建立協(xié)方差矩陣M，

式中：pi—k鄰域中的第i個點；p^—點集的質心

對M進行特征值分解，采用總體最小二乘法估計法線方向。為了減小計算量，將（2）中的質心替換為特征點。為了提高局部坐標系在雜亂場景中的可重復性，給較遠的點分配較小的權重，依次計算加權協(xié)方差矩陣M，可得：

式中：di=‖pi-F‖2—點pi與F之間的歐幾里德距離；

r—球面領域的半徑。

此時，求得加權協(xié)方差矩陣M特征向量即為局部坐標系的三個單位向量。

（2）由于特征值分解法給出的特征向量的符號定義不明確，為了實現局部坐標系的唯一性，對式（3）中的特征向量采用符號一致性的方法消除歧義，將最大和最小特征值及對應的特征向量分別定義為局部坐標系x軸和z軸的單位向量，則消除歧義的x軸定義為：

式中：x+—x軸的正半軸方向；x-—x軸的負半軸方向。

使用相同的過程消除z軸的歧義，最后y軸通過標準正交約束下的叉乘得到：

2.3 SHOT點云特征描述

（1）在特征點F處的r半徑球鄰域內建立局部坐標系，對球鄰域沿徑向、方位角方向、仰角等三個方向劃分空間網格，其中，2個徑向區(qū)，8個方位角區(qū)，2個仰角區(qū)，共分為32個空間區(qū)域，如圖2所示。

圖2 SHOT描述符的特征結構Fig.2 Signature Structure for SHOT

（2）對于每個區(qū)域由局部直方圖表示，每個直方圖分為11個小單元，各單元的值根據網格中相應部分的每個點的法線nvi與特征點法線nu之間的夾角θi余弦值來累計點數而構建，即：

最終SHOT描述符的維度為32×11=352。根據局部坐標系將所有直方圖有序地并排在一起。由于球形域上的空間細分，每個區(qū)域構建的單個直方圖組合在一起會產生邊界突變效應。另外在局部坐標系的擾動下，也會產生邊界效應。因此，對于每個點累計到特定的局部直方圖的單元格中，采用四次線性插值法，即對局部直方圖中相鄰的單元格和相鄰直方圖同一單元格內進行插值［9］。每個計數乘以每個維度的權重1-d，對于法矢夾角余弦的直方圖相鄰單元格插值，如圖3所示。d的定義如式（9）所示，在方位角方向、仰角方向和徑向方向插值類似。

圖3 法矢夾角余弦插值Fig.3 Interpolation on Normal Cosines

式中：s—相鄰單元格間距。

3 特征點對的對應分組

為當前場景和庫中的模板計算出描述符之后，通過kd樹關聯(lián)描述符空間中接近的模型－場景描述符對確定點對點的對應關系結果，如圖4所示。藍色圓圈表示特征點，虛線左邊表示模型點云，右邊表示場景點云。在三維物體識別中，通過加強幾何一致性來消除偽對應，即特征點對的對應分組。

圖4 配對關系（3D綠線表示正確的匹配，紅線表示偽對應）Fig.4 Pairing Relationship（Green Lines Identify Correct Matches and Red Ones Represent Wrong Correspondences）

采用類似于［10］中提出的方法去除偽對應，在三維空間中，利用模型點云與當前場景點云之間的對應關系，通過局部坐標系將每個場景特征點與模型質心的相對位置相關聯(lián)，使得每個對應的場景特征可以在三維霍夫空間中進行投票，足夠多的票數來支持工件在三維空間中給定位置的存在，如圖5所示。

圖5 引入局部參考系的坐標變換Fig.5 The Coordinate Transformation by the Introduce of Local Reference Frame

在圖5中，模型特征點為，場景特征點為，模型點云和場景點云的質心分別為CM，CS，CM和之間的向量表示為：

為保證向量在不同坐標系下具有旋轉平移不變性，將向量轉化到局部參考系（即為特征點坐標系）中，通過以下變換：

于是，每個特征點與其對應的向量關聯(lián)在一起，對每對場景特征點與模型特征點建立的聯(lián)系（?），由于局部參考系的旋轉平移不變性，所以有=。將轉換到場景的全局參考系中，可得：

根據這些轉換關系，場景特征點通過向量在3D霍夫空間中投票，在霍夫參數空間中，峰值的存在表明場景中存在目標。

4 基于ICP算法的點云精確配準

隨機樣本一致性算法［11］提供初始的場景點云相對于模板點云的變換矩陣，把變換矩陣應用于模型點云中，縮小兩幅點云之間的旋轉和平移錯位，提高精確配準的效率和趨向，為了進一步提高配準精度，采用ICP算法［12］進一步優(yōu)化，其實現的主要步驟為：

（1）場景點云S={pi，i=1，2…ns}表示目標點云，模型點云M={qi，i=1，2...nm}表示源點云，ns、nm分別表示兩幅點云的規(guī)模。

（2）在M中選取一定數量的點qi，k，在S中尋找最近點pi，k，確定初始最近對應點集(pik，qik)；

（3）計算兩幅點云之間的旋轉矩陣R和平移矩陣T；

（4）計算轉換后的源點云與目標點云之間的均方誤差函數：

（5）當均方誤差低于預先設定的閥值τ時，終止迭代，當前的R和T為最終的配準矩陣。否則更新配準變換后的點集pi，k，繼續(xù)（2）、（3）、（4）的步驟，直到滿足閾值要求。k表示迭代次數，N表示隨機選取點集中的元素個數。

5 場景點云相對模型點云的位姿估計

位姿估計通過平移向量和旋轉矩陣表示，旋轉矩陣的表達又包括2種方式，即歐拉角和四元數［13］。設最終的變換矩陣為：

T=[t1t2t3]T—平移向量。

則場景點云坐標系下和模型點云坐標系下的六自由度位姿估計為：

對應的四元數為q=[q0q1q2q3]，其中；

建立機器人視覺系統(tǒng)的坐標轉換關系，如圖6所示。機器人采用基于模板點云配準的方法獲取工件位姿，引導工業(yè)機器人抓取工件，具體的變換矩陣如下式：

圖6 Eye-to-Hand機器人系統(tǒng)各坐標系關系Fig.6 The Relationship Between the Coordinate Systems of the Eye-to-Hand Robot System

6 實驗結果與分析

為驗證方法的有效性，實驗使用三維掃描儀獲取曲面工件的點云數據進行實驗，點云數據中包括點的三維坐標（x，y，z）和強度信息（intensity），點云分辨率為2064×1544。工件設計圖和實物圖，如圖7（a）、圖7（b）所示，實驗平臺，如圖7（c）所示。

圖7 工件設計圖、工件實物圖、實驗平臺示意圖Fig.7 Workpiece Design Drawing，Physical Drawing of Workpiece，Experiment Platform

使用三維掃描儀采集單個工件點云，并對其進行處理得到紅色表示的模板點云，如圖8所示。同樣得到黃色表示的場景點云，如圖9所示。其中，對工件沒有遮擋，工件放置于不同的位姿下，如圖9（a）、圖9（c）所示。對工件有部分遮擋的情況，如圖9（b）所示。工件分別與其他物體相互接觸，呈現不同程度的遮擋以及噪聲，如圖9（f）～圖9（h）所示。為了提高計算效率，采用RANSAC算法擬合場景中的平面點云模型，分割出平面點云，去除背景點、離群點，得到最終匹配的場景點云，如圖10所示。

圖8 建立的模型點云Fig.8 Template Point Cloud

圖9 原始場景點云圖Fig.9 Original Scene Point Cloud

圖10 預處理后的場景點云Fig.10 Preprocessed Scene Point Cloud

將獲取的場景點云和模板點云進行實驗，根據經驗將相關參數設置為：對模型點云和場景點云分別使用寬度為10mm和30mm的三維體素化進行特征點采樣。對于點法線的計算，取周圍10個點計算法線；為特征點建立描述符的鄰域搜索半徑20mm；設置初始匹配的平方距離為0.25；局部參考系的半徑設置為15mm；3D霍夫投票空間中，設置每個bin的大小為0.4，閥值設置為5.0。為了直觀判斷在場景點云中識別出工件的識別結果，將基于SHOT描述符的場景點云和模板點云的匹配結果進行可視化顯示表示，如圖11所示。綠色實線表示場景點云和模板點云建立特征點對應關系的對應對，淺黃色的點云表示模板點云在三維空間中的初始位置，紅色點云表示根據隨機樣本一致性算法得到的初始位姿轉換后的模板點云位姿?？梢钥吹剑瑘鼍包c云（a～f）均與模板點云建立了正確的配對關系，表明采用到的場景點云中的工件實例均被正確識別。對場景點云（a～f）進一步采用ICP算法優(yōu)化位姿關系，在ICP算法中，最大迭代次數為100次，兩次轉換之間的差值設為1e－6，配準效果，如圖12所示。可以看到模型與場景點云基本準確配合。

圖11 3D對象場景中匹配局部描述符的結果Fig.11 Results of Matching Local Description in 3D Object Recognition Scenario

圖12 這里方法的最終結果配準結果Fig.12 ICP Algorithm Registration Results

對于圖12（a）所示的場景中，利用隨機樣本一致性算法得到模型點云與場景點云之間的初始位姿關系及利用ICP算法進一步精配準得到兩者間的位姿關系，兩者的乘積得到原始場景點云中的工件實例相對于模板點云的位姿轉換關系：

代入式（15）、式（16）將變換矩陣轉化為6自由度以及四元數表示形式：

利用兩組點云間的歐幾里德距離平方誤差的總和評估兩組點云的配準精度，配準誤差，如表1所示。表中第一行表示這里方法最終的配準，第二行表示單獨使用ICP算法的配準精度，單獨采用ICP算法的配準效果，如圖13所示。結果表明在精確配準前加入初始配準可以得到好的配準效果，使用這里方法的配準精度遠遠優(yōu)于單獨使用ICP算法。在實驗中，所采集到的場景點云中的工件實體均被正確識別，總的識別率為1。

圖13 單獨采用ICP算法的配準結果Fig.13 Registration Result Using ICP Algorithm Alone

表1 配準誤差（mm）Tab.1 Registration Errors（mm）

7 結論

為實現工業(yè)機器人能夠在復雜的環(huán)境中準確高效地識別和定位工件，提出了基于點云特征的方法進行了目標工件的識別和定位，改進局部坐標系的建立方法，SHOT描述符結合了特征和直方圖，具有更好的魯棒性；根據向量之間的關系，使用3D霍夫投票機制完成對工件的識別，最后利用了ICP算法精細化位姿。最后通過三維掃描儀采集的點云數據進行實驗，結果表明：基于特征點建立SHOT三維局部描述符的識別配準以及ICP精配準算法能夠準確識別工件，并且獲得比較高的配準精度，配準平均精度為0.010mm，從點云的轉換圖像中，可以看到模型點云與場景點云兩者間具有比較好的擬合效果，說明了方法的可行性。在許多機器人場景和應用中，基于三維點云的形狀特征為準確識別并估計目標物體的位姿提供了解決方案，豐富了工業(yè)機器人利用三維點云對物體位姿估計的方法。