基于3D 視覺的工件識別及無序抓取系統設計

陳順強，廖 旭，羅 楨，李 波，黃傳富，王 鍵，李 力，柏俊杰

（重慶科技學院 電氣工程學院，重慶 401331）

隨著工業信息化技術蓬勃發展，目前工業生產中多使用機械臂來代替人工完成工件的分揀、裝配等任務。而在實際的工業生產中，工業機械臂的作業環境及需求相對復雜，例如機械臂的視覺部分經常會受到光照強度、相機成像距離、視點變化等因素的影響，而且一些特定的生產工藝下，還需要工業機械臂能從雜亂無章、相互遮擋或者重疊的物料中逐一完成抓取。對于上述條件限制，目前發展成熟的基于2D 相機搭配一般的工業機械臂抓取技術很難高效、準確地完成抓取任務，而且一般的工業抓取系統較為昂貴。

為了解決上述問題，目前已有很多學者提出可行的基于3D 視覺引導機械臂抓取的方法[1]?？驴朴耓2]使用2 個結構光攝像頭，將得到目標物件的深度點云數據進行三維重建，重建后的點云和模板點云進行配準，得到需要的抓取坐標，該方法缺點在于獲取到點云數據存在一定的噪聲和干擾，和模板點云比對必定存在差距，從而會影響到抓取坐標的準確性。WONG等[3]提出SegICP 方案使用CNN 網絡對大規模點云數據進行處理，實現了對點云數據的分割和配準，但是該方法識別性和速度不能兼容。

為了提升基于3D 視覺[4]引導機械臂完成無序抓取的準確性和實時性，本文融合了一些效果較好的點云處理算法[5-7]，對深度相機采集到的點云數據進行濾波、分割、配準等處理[8-9]，得到攜帶信息量高的點云，再使用GPD 算法獲取機械臂6DoF 抓取坐標[10]，將抓取坐標信息通過ROS 系統多機通信完成和機械臂的信息交互，引導機械臂完成無序抓取[11-12]。最后對該系統進行實驗測試。

1 整體系統結構

針對基于3D 視覺的工件識別及抓取系統，本文提出了以下系統框架，如圖1 所示。

圖1 系統整體框架圖

基于3D 視覺引導機械臂無序抓取系統包括3D 視覺處理模塊、機械臂運動控制模塊、手眼標定模塊3 個部分，其中3D 視覺處理模塊為系統的核心部分，手眼標定模塊為輔助部分，機械臂運動控制模塊為執行部分。

2 3D 視覺處理模塊設計

2.1 點云濾波處理

本文通過在上位機ROS 系統中訂閱濾波話題，將深度相機獲取的點云數據轉化為PCD 處理格式，通過調用PCL 庫中的半徑濾波、離群點濾波、體素網格濾波對點云數據進行濾波處理。

通過直通濾波來過濾掉深度相機大視角下的干擾點云數據，減小深度相機正前方觀測范圍，濾波效果如圖2 所示。

圖2 直通濾波效果圖

直通濾波已經將大范圍發散的點云濾除掉，深度相機采集的數據會存在不同密度的點云集，采用離群點濾波去除主體點云數據周圍存在稀疏的異常值，濾波效果如圖3 所示。

圖3 直通濾波效果圖

直通濾波和離群點濾波對數據進行濾波后，已經取得了較好的去噪效果，但剩下的點云數據密度相對較大，大量的點云數據運算將會消耗時間和空間資源，因此本文采用體素網格濾波來對數據進行下采樣，保留點云關鍵特征的基礎上去除冗余信息，達到精簡點云數據的目的。體素網格濾波效果如圖4 所示。

圖4 體素濾波效果圖

2.2 點云分割

2.2.1 基于RNASAC 算法的平面分割

由于目標工件之間往往是相互堆疊的，為了使機械臂完成單一性的無序抓取，必須將目標點云數據進行分割處理，本文采取RANSAC 算法對點云數據進行平面分割，去除試驗臺平面及背景的干擾，分割效果如圖5 所示。

圖5 RANSAC 算法進行平面分割效果圖

2.2.2 基于邊緣點云信息和改進歐氏聚類融合算法的目標分割

使用傳統的歐氏聚類算法對無序堆疊、相互粘連的工件點云數據進行分割時，若搜索半徑r 值選取不當會出現過分割和欠分割的情況，而且分割時間較長。為提升歐氏聚類算法點云分割的魯棒性、加快分割效率，本文提出一種基于邊緣點云信息和改進歐氏聚類融合算法來進行目標分割。

（1）基于法向量夾角突變的邊緣點去除

當2 個目標工件出現堆疊時，映射到其三維點云數據上，在空間中工件堆疊處邊緣點云法向量會出現方向上的突變?；谠撎匦?，本文對點云數據邊緣點部分進行提取并去除，算法步驟如下：

① 使用KD-Tree 搜索構建工件點云p 的拓撲關系。

② 為目標工件構建點云模板庫，設定法向量變化閾值u，領域半徑r。

③ pi為目標工件點云中的任意點，pi計算該點的搜索鄰域半徑內相鄰點法線夾角最大變化的均值δpi，若滿足δpi>u 則定義該點為邊緣點，反之為非邊緣點。其中，n0表示法向量的方向；Φ 為點云法線夾角變化最大值，可表示為：

則鄰域點法線夾角最大變化均值為：

④?對工件邊緣點進行剔除，剔除效果如圖6所示。

圖6 邊緣點剔除效果圖

（2）對剔除過邊緣點的點云數據使用改進的歐氏聚類算法進行分割

使用傳統的歐氏聚類對點云進行分割，其分割效率最主要影響因素就是聚類閾值的選取。本文提出一種自調節閾值的歐氏聚類分割方法，提升分割效率。算法步驟如下：

① 選取目標點云中任一點pb作為聚類中心點，使用k-dimensional 算法快速pb搜索最近鄰的N 個點；

② 判斷選取的聚類中心點pb是內部點還是邊緣點，若為內部點就可以增加距離閾值，反之減小距離閾值。本文使用聚類中心點pb到N 個近鄰點的平均距離dm來判斷pb為內部點還是邊緣點，dm表達式：

自調節閾值dre和近鄰點的平均距離dm存在密切關系，定義其關系表達式：

③ 通過上述公式得出自調節閾值dre，使用自調節dre替換初始聚類閾值，重復上述步驟對剩下的點云數據進行分割。

（3）填補邊緣點云數據

使用自調節閾值歐氏聚類算法完成對剔除邊緣的點云數據分割后，為了保障點云數據完整性，需要將剔除的邊緣點云數據補回，目標數據點云分割結果如圖7 所示。

圖7 目標粘連時分割效果圖

對于目標工件不同的擺放位置，做了50次分割實驗，點云的分割準確率統計結果如表1 所示。

表1 50 次目標工件不同擺放位置分割準確率 %

采用傳統的歐氏聚類算法和改進后的算法分別對隨意擺放的工件點云數據做了300 次分割實驗，并且進行分割精度和分割時間上的統計，實驗結果如圖8 所示。

圖8 分割算法改進前后分割準確率及時間對比圖

由實驗結果可知，基于邊緣點云信息和改進歐氏聚類融合算法相對于傳統的歐氏聚類算法分割時間平均降低50.6 ms，分割準確率平均提升3.2%。

2.3 點云目標識別及配準

2.3.1 改進FPFH 算法

本文對FPFH 算法進行改進，在FPFH 算法原本3 組特征法線夾角的基礎上再引進1 組特征元素，該特征元素為查詢點法向量與提取固定擬合平面的夾角。傳統FPFH 算法和改進的FPFH算法局部坐標表示圖如圖9、10 所示。

圖9 傳統FPFH 算法局部坐標表示

基于改進的FPFH算法特征提取計算流程如下：

① 圖10 中ps為需要計算FPFH 的點，pt為它的鄰域內一點，ns和nt分別為其法向量，式中u 為ps點的法向量，該局部坐標3 個軸的計算方法如下：

圖10 改進FPFH 算法局部坐標表示

② 由上訴公式得到點ps的一個局部坐標，n1

將原本FPFH 中的三元組擴充為四元組，分別為（α，Φ，θ，β）。

③ 分別計算ps與近鄰點的四元組，通過統計得到ps的簡化點特征直方圖，記作SPFPs。以近鄰點依次作為查詢點計算出對應的特征直方圖。

④查詢點ps計算快速點特征直方圖的公式如（13）所示，式中wi為權重（pq和pi之間的距離）。

⑤ 按照步驟1、2、3、4 計算點云中各點的4元組信息，分別將4 個元素對應的直方圖連接起來，即為點云樣本的改進FPFH 全局快速點特征直方圖。

2.3.2 點云識別及配準實驗

點云目標識別之前需要建立本地模板庫，對同一物體的多個角度采集點云信息，然后計算其改進FPFH 計算特征直方圖結果，并以PCD 文件格式保存下來，如圖11 所示。

圖11 不同視角下的目標與其改進FPFH 特征直方圖

定義待識別目標與不同模板的改進FPFH 特征直方圖的差異距離為d （Hh，Hk）均值，計算公式：

式中，Hh表示待識別目標的改進FPFH 特征向量；Hk表示點云模型庫內點云模型的改進FPFH 特征向量為實驗次數。差異距離d （Hh，Hk）均值越小即識別效果精確度越高。

使用本文提出的改進FPFH 以及模板庫制作方式和傳統FPFH 識別準確度（用d （Hh，Hk）值衡量）和識別速度對比。通過50 次實驗計算統計數據均值如表2 所示。為擬合平面法向量，根據該坐標可以計算ps點的4 組特征表示：

表2 傳統FPFH 和改進FPFH 識別準確度、速度對比

本文分別使用改進前后的FPFH 算法聯合ICP 算法對點云數據進識別配準實驗，配準效果圖如圖12 所示。

2.3.3 GPD 最佳抓取姿態檢測

對點云數據進行配準后，需要計算出最佳抓取坐標，本文使用GPD 算法擬合出最佳抓取坐標，在ROS 系統調用編譯好的GPD 算法的話題。抓取位姿在ROS 插件RVIZ 中的可視化效果圖如圖13 所示。

圖13 GPD 算法運行結果

3 抓取實驗

在實際場景中，搭建了無序分揀系統，整個分揀系統主要由Astra Pro 深度相機、機械臂、機械臂控制器、上位機等組成。本文采用以樹莓派4B 為主控，ROS 為操作系統，通過ROS 多機通訊將機械臂與上位機進行連接，通過訂閱上位機傳輸抓取坐標的話題，來控制機械臂完成抓取，抓取系統工作流程過程如圖14 所示。

將4 個待抓取工件擺放到桌面，分別針對工件無堆疊和有堆疊2 種場景進行50 次抓取實驗，同時記錄利用目標識別和位姿估計算法得到的位置坐標，并且統計識別坐標和實際坐標間的平均絕對值及相對誤差，以及抓取時間和抓取準確度。抓取實驗數據如表3 所示。

表3 抓取實驗數據表

其中實際坐標與識別坐標平均相對誤差由公式（15）求得：

式中，n 為總實驗次數；x，y，z 為實際坐標；x'，y'，z'為識別坐標。

實際坐標與識別坐標平均相對誤差越小，抓取精度越高。由表3 數據得出50 次抓取實驗的平均相對誤差為0.9%~1.4%，抓取時間在5 s~8 s間，其中工件無序無堆疊場景下的抓取成功率在94.2%以上，而工件有序有堆疊場景下的抓取平均成功率為88.2%。

為了驗證本文提出的基于邊緣點云信息和改進歐氏聚類融合對目標分割算法以及改進的FPFH 全局特征提取，對該3D 無序抓取系統的效率提升度。本文使用傳統的歐氏聚類算法以及FPFH 算法來完成點云數據的處理，并同樣使用該套系統完成抓取，進行50 次工件有堆疊情況下動物抓取實驗。改進前后抓取時間及實際坐標與識別坐標相對誤差如圖15 所示。

圖15 改進前后抓取時間及抓取姿態相對誤差數據對比

4 結論

本文提出一套基于3D 視覺的機械臂無序抓取系統方法，并通過實驗驗證改進算法的有效性?；谶吘夵c云信息和改進歐氏聚類融合算法相對于傳統的歐氏聚類算法點云數據分割準確度提升3.4%，分割時間平均提升50.8 ms。擴充FPFH 的特征元祖，并且建立點云識別的模板點云庫，使特征點云識別準確度提高。將改進、優化的算法和傳統算法應用到同一個抓取系統里進行效果比對，驗證了本文改進的算法能夠達到優化點云數據處理效果。實驗結果表明，本文所設計的基于3D 視覺的無序抓取系統能從工件堆疊的場景中成功抓取目標，而且抓取系統的準確度提升5.2%，抓取系統運行時間降低3 s~6 s。