散亂堆疊藥盒三維視覺抓取系統(tǒng)設(shè)計*

甄瑞濤 韓建海，2 郭冰菁，2 李向攀，2

（1.河南科技大學(xué)機(jī)電工程學(xué)院 洛陽 471003）

（2.河南省機(jī)器人與智能系統(tǒng)重點實驗室 洛陽 471003）

1 引言

我國是世界最大的原料藥生產(chǎn)及出口國家，醫(yī)藥行業(yè)是我國國民經(jīng)濟(jì)的重要組成部分［1］。如此大的生產(chǎn)需求單靠人工勞動力，勞動力成本過高并且難以滿足需求，所以將視覺引導(dǎo)機(jī)器人抓取系統(tǒng)應(yīng)用于醫(yī)藥生產(chǎn)線上，可以靈活地完成生產(chǎn)、分揀、包裝等工作，提高醫(yī)藥生產(chǎn)的智能化和自動化，將大大降低勞動力成本。

場景分割與位姿估計是視覺引導(dǎo)機(jī)器人抓取的關(guān)鍵，場景分割實現(xiàn)抓取對象與載物臺的分離以及抓取對象之間的分割，位姿估計獲取目標(biāo)在相機(jī)坐標(biāo)系下的位置和姿態(tài)。場景分割可分為處理圖像數(shù)據(jù)的二維場景分割以及處理三維點云數(shù)據(jù)的三維場景分割。二維場景分割在弱紋理、顏色或亮度相近、堆疊或有遮擋等情況下，通常難以得到理想的分割結(jié)果，三維點云包含場景的深度信息，因此三維場景分割依據(jù)點云表面法線、曲率、凹凸性等特征便可以判斷物體之間位置的相對差異，完成分割［2～5］。本文選擇基于三維視覺進(jìn)行系統(tǒng)設(shè)計。

目前，對于散亂堆疊放置物體的抓取，現(xiàn)有的視覺引導(dǎo)機(jī)器人抓取系統(tǒng)多是通過基于卷積神經(jīng)網(wǎng)絡(luò)或深度神經(jīng)網(wǎng)絡(luò)的方法進(jìn)行場景分割，需要預(yù)先標(biāo)注大量的訓(xùn)練樣本進(jìn)行特征訓(xùn)練，且運算量巨大，對硬件有較高要求［6～9］；通過模板匹配的方法估計目標(biāo)位姿，需要預(yù)先建立模板庫，并且模板匹配運算量大耗時較長，當(dāng)目標(biāo)物大小和形狀等特征變化時，需要重新創(chuàng)建模板庫［10～13］。為此，針對藥盒這一規(guī)則體的堆疊抓取，根據(jù)藥盒大小多變且表面光滑的特征，本文設(shè)計了散亂堆疊藥盒三維視覺抓取系統(tǒng)，基于點云預(yù)處理與點云分割算法設(shè)計了堆疊藥盒場景分割算法，并且通過計算藥盒上表面點云的質(zhì)心以及質(zhì)心點表面法線獲取藥盒位姿，無需標(biāo)注訓(xùn)練樣本進(jìn)行特征訓(xùn)練，也無需建立模板庫，能夠很好地適應(yīng)不同大小的藥盒。系統(tǒng)整體實現(xiàn)了場景分割、位姿估計及抓取功能。

2 系統(tǒng)整體方案設(shè)計

首先，通過深度相機(jī)獲取堆疊藥盒場景點云數(shù)據(jù)；其次，對獲取的原始點云數(shù)據(jù)進(jìn)行預(yù)處理，提高數(shù)據(jù)的精確性和后期處理速度。預(yù)處理包括：直通濾波去除載物臺背景、體素化網(wǎng)格方法降采樣、統(tǒng)計濾波刪除離群點；然后，通過場景分割完成不同藥盒點云之間的分割以及藥盒上表面點云與側(cè)面點云的分割；最后，根據(jù)單個完整藥盒上表面點云的點個數(shù)設(shè)定的閾值判斷，先處理表層未被遮擋的藥盒，依據(jù)藥盒完整上表面點云進(jìn)行位姿估計，獲取單個藥盒上表面點云質(zhì)心以及質(zhì)心點表面法線，得到藥盒位姿，由機(jī)器人及安裝在末端的吸盤完成抓取。根據(jù)是否存在傾斜放置藥盒，分為場景一“散亂無傾斜堆疊”與場景二“散亂有傾斜堆疊”兩種場景對系統(tǒng)進(jìn)行測試試驗。系統(tǒng)工作流程如圖1所示。

圖1 系統(tǒng)工作流程圖

藥盒為規(guī)則體，其表面光滑，多為特征均勻的矩形或方形平面，相比于通過手爪抓取藥盒，通過吸盤吸取藥盒更加快速準(zhǔn)確，根據(jù)藥盒的尺寸大小和質(zhì)量設(shè)計了真空吸盤，配備真空泵完成藥盒的抓取，吸盤實物如圖2 所示。系統(tǒng)硬件組成部分主要包括UR5 機(jī)器人、COMATRIX 深度相機(jī)、計算機(jī)、真空吸盤、真空泵等，如圖3 所示。相機(jī)相對機(jī)器人安裝位置可分為eye-in-hand 和eye-to-hand 兩種方式，本系統(tǒng)設(shè)計采用eye-in-hand 方式。選用COMATRIX 深度相機(jī)，深度圖分辨率（Depth Map）200 萬個點，每個點都有獨立的（X，Y，Z）坐標(biāo)，Z 軸準(zhǔn)確度為200μm。選用UR5 機(jī)器人，有效負(fù)載5kg，工作范圍850mm，重復(fù)定位精度±0.1mm。

圖2 真空吸盤

圖3 系統(tǒng)硬件結(jié)構(gòu)

3 系統(tǒng)視覺算法設(shè)計

3.1 場景點云數(shù)據(jù)獲取及預(yù)處理

相機(jī)固定于機(jī)器人末端，設(shè)置機(jī)器人初始位置，即相機(jī)每次拍照位置，使相機(jī)位于垂直于載物臺正上方760mm 處，相機(jī)垂直于載物臺拍攝獲取場景數(shù)據(jù)。獲取的數(shù)據(jù)包含載物臺背景，并且由于相機(jī)誤差和環(huán)境因素的影響，獲取的原始點云數(shù)據(jù)存在噪聲，且密度過大，所以需要對原始點云數(shù)據(jù)進(jìn)行直通濾波、降采樣、離群點刪除等預(yù)處理操作。

3.1.1 去除載物臺背景

相機(jī)每次拍照相對載物臺的位置是固定的，即相機(jī)坐標(biāo)系下載物臺表面的深度值是不變的。藥盒有一定厚度，所以藥盒上表面的深度值小于載物臺表面的深度值。對三個坐標(biāo)維度分別進(jìn)行直通濾波，如式（1）所示，（xmin，xmax），（ymin，ymax），（zmin，zmax）分別是三個維度坐標(biāo)設(shè)定的閾值，在保證藥盒點云數(shù)據(jù)完整的情況下，將載物臺背景完全去除。

如圖4 所示，直通濾波去除載物臺前后對比，圖4（a）和（b）為場景原圖，圖4（c）和（d）為場景一去除載物臺前后對比，圖4（e）和（f）為場景二去除載物臺前后對比。

圖4 去除載物臺

3.1.2 體素化降采樣

通過體素化網(wǎng)格方法對去除載物臺背景的稠密藥盒點云數(shù)據(jù)降采樣，精簡冗余點。對輸入的點云創(chuàng)建一個三維的體素柵格，每個體素內(nèi)的所有點以該體素所含點的質(zhì)心替代。依據(jù)點云數(shù)據(jù)的密度，在保持點云形狀特征的前提下，設(shè)置體素葉子尺寸為1mm。如圖5 及表1 所示，對比了降采樣前后點云數(shù)據(jù)的變化。

表1 體素降采樣前后點云數(shù)據(jù)對比

圖5 體素降采樣

由采樣結(jié)果可知，在保持點云形狀特征的前提下，降采樣對藥盒點云數(shù)據(jù)進(jìn)行了大比率精簡，后續(xù)能夠有效地提升目標(biāo)點云分割速度。

3.1.3 統(tǒng)計濾波離群點刪除

由于相機(jī)測量誤差以及降采樣，會造成稀疏的離群點，使點云局部特征的估計變得復(fù)雜，導(dǎo)致錯誤的結(jié)果，從而致使點云分割等后期處理的失敗。采用統(tǒng)計濾波進(jìn)行離群點刪除，計算每個點與其鄰域點的平均距離并進(jìn)行統(tǒng)計分析，得到具有均值和標(biāo)準(zhǔn)差的高斯分布，根據(jù)全局距離均值和標(biāo)準(zhǔn)差設(shè)置合適的閾值，修剪掉平均距離不在標(biāo)準(zhǔn)范圍內(nèi)的離群點。設(shè)置對每個點分析的鄰域點個數(shù)為50，并將標(biāo)準(zhǔn)差倍數(shù)設(shè)置為2。離群噪聲去除前后對比如圖6 所示，統(tǒng)計濾波可以有效的去除離群點，為后期的目標(biāo)點云分割提供精確的點云數(shù)據(jù)。

圖6 統(tǒng)計濾波去除離群點

3.2 場景目標(biāo)點云分割

點云分割算法主要有區(qū)域生長、歐式聚類、隨機(jī)采樣一致性等方法［14～15］，本文主要任務(wù)是將不同藥盒的點云分割開，同時實現(xiàn)藥盒上表面點云與側(cè)面點云的分割，結(jié)合藥盒點云數(shù)據(jù)均為特征均勻的矩形或方形平面的特征，通過區(qū)域生長算法實現(xiàn)場景目標(biāo)點云的分割。

區(qū)域生長算法依據(jù)點法線之間的夾角大小進(jìn)行分割，比較種子點法線與其鄰域點法線之間的夾角，將夾角小于閾值的點存入同一聚類集群。為此，需要計算目標(biāo)點云表面法線。點云為多個離散的空間點組成，不是連續(xù)的光滑表面，因此需要在其每個點處根據(jù)鄰域點近似的計算表面法線。求取表面某點處的法線等同于估計該點處表面的相切面法線，就將求取表面法線的問題轉(zhuǎn)換成了通過最小二乘法擬合估計切平面的問題。因此，求取表面法線的方法可簡化為通過查詢點的鄰域元素創(chuàng)建一個協(xié)方差矩陣，求取該協(xié)方差矩陣的特征值和特征向量。對于點云中任意一點Pi，其相應(yīng)的協(xié)方差矩陣C 以及特征值和特征向量計算如式（2）和式（3）所示。求得的特征值有多個，對應(yīng)多個特征向量，選取最小特征值對應(yīng)的特征向量，即為所擬合切平面的法向量。如圖7 為藥盒點云表面法線估計結(jié)果。

圖7 表面法線估計

式中：K 是任意點Pi鄰域點的個數(shù)，為Pi鄰域點的三維質(zhì)心，λj是協(xié)方差矩陣C的第j個特征值，j是特征值λj對應(yīng)的特征向量。

算法流程：1）將曲率最小的點設(shè)置為初始種子點并將其加入種子點序列，設(shè)置一個空的聚類集群，由初始種子點開始生長；2）搜索當(dāng)前種子點鄰域內(nèi)的所有點，比較當(dāng)前種子點與其鄰域點之間的法線夾角，小于平滑閾值的鄰域點加入到當(dāng)前區(qū)域的聚類集群中，同時檢測當(dāng)前種子點鄰域內(nèi)所有點的曲率值，小于曲率閾值的點被作為新的種子點加入到種子點序列中；3）當(dāng)前種子點鄰域判斷結(jié)束后，將該種子點刪除，用加入的新的種子點繼續(xù)生長，直到種子點序列清空，便完成了一個區(qū)域的生長；4）從剩余的部分尋找曲率最小的點作為新的初始種子點，開始一個新區(qū)域的生長，重復(fù)1）～3）的生長步驟，完成多個區(qū)域的生長，最終完成點云的分割。

算法關(guān)鍵參數(shù)的設(shè)置：最小點簇的點數(shù)量為100；最大點簇的點數(shù)量為15000；鄰域點的搜尋方式設(shè)置為KdTree，因為獲取的目標(biāo)點云為無序的，需要通過KdTree 對無序的點云建立拓?fù)潢P(guān)系，依據(jù)所建立的拓?fù)潢P(guān)系對鄰域內(nèi)的點進(jìn)行快速訪問；搜尋的鄰域點數(shù)目設(shè)置為50；平滑度閾值設(shè)置為0.0527rad；曲率閾值設(shè)置為1.5。分割結(jié)果如圖8所示，實現(xiàn)了場景1 與場景2 中不同藥盒點云的分割。

圖8 區(qū)域生長分割

3.3 待抓取目標(biāo)位姿獲取

通過吸盤實現(xiàn)對藥盒的抓取，為使吸盤可以準(zhǔn)確牢固的吸住藥盒，需保證吸取藥盒時吸盤垂直于藥盒表面，且吸住藥盒的中心，因此需要獲取機(jī)器人基坐標(biāo)系下待抓取目標(biāo)的位姿。針對藥盒這一扁平規(guī)則體，本文提出通過計算藥盒上表面點云質(zhì)心以及質(zhì)心點表面法線獲取藥盒位姿。

對于每個藥盒上表面點云，其質(zhì)心Pc的計算方法如下，式（3）為通常物體求取質(zhì)心Pc的方法，mi為質(zhì)點對應(yīng)的質(zhì)量。對于點云質(zhì)心Pc的計算，令mi=1即可，藥盒點云質(zhì)心的計算如式（4）所示。

對于每個待抓取藥盒，依據(jù)點云分割獲取的單個藥盒上表面點云求取其表面法線，并以質(zhì)心點為索引獲取質(zhì)心點法線。如圖9 為分割出的單個藥盒質(zhì)心及質(zhì)心點法線的顯示。

圖9 點云表面法線估計

求取的質(zhì)心為centroid（x，y，z），對應(yīng)的質(zhì)心點法向量為normal（normal_x，normal_y，normal_z），其中normal_x、normal_y 和normal_z 為法向量的方向余弦，計算反余弦即可得到法線與相機(jī)坐標(biāo)系三個軸的夾角（α，β，γ），即獲得了藥盒在相機(jī)坐標(biāo)系下的位姿。通過機(jī)器人基坐標(biāo)系與機(jī)器人末端坐標(biāo)系的變換矩陣以及手眼標(biāo)定所建立的機(jī)器人末端與相機(jī)之間的坐標(biāo)轉(zhuǎn)換關(guān)系獲得機(jī)器人基坐標(biāo)系下藥盒的位姿。機(jī)器人末端所裝工具為吸盤已確定，所以工具坐標(biāo)相對于機(jī)器人末端坐標(biāo)的轉(zhuǎn)換矩陣是確定的，因此藥盒位姿可實現(xiàn)相機(jī)坐標(biāo)至基坐標(biāo)的轉(zhuǎn)換、基坐標(biāo)至工具坐標(biāo)的轉(zhuǎn)換，引導(dǎo)機(jī)器人完成抓取。

4 實驗與結(jié)果分析

為了測試系統(tǒng)的性能，分別從場景分割準(zhǔn)確性及抓取效果兩個方面進(jìn)行實驗分析。以6 種常見的不同尺寸大小的藥盒為實驗對象。

4.1 場景分割測試實驗

系統(tǒng)圖像處理單元采用Intel Core i7-8700k CPU、Ubuntu 16.04 64 位 操 作 系 統(tǒng)、點 云 庫PCL1.9.1，通過clion集成開發(fā)。為進(jìn)一步驗證場景分割算法的有效性，對兩場景進(jìn)行了多次數(shù)據(jù)采集，并通過算法處理，如圖10 為隨機(jī)獲取的兩場景復(fù)雜堆疊數(shù)據(jù)的處理結(jié)果，觀察分析處理結(jié)果發(fā)現(xiàn)：對于兩場景的表層藥盒均能夠準(zhǔn)確完整地將屬于不同藥盒的點云單獨分割出來，并且能夠?qū)崿F(xiàn)傾斜藥盒上表面與側(cè)面點云的分割；對于兩場景藥盒的復(fù)雜堆疊，會存在表層藥盒對下層藥盒橫斷遮擋的情況，造成下層單個藥盒的點云不在同一連續(xù)平面，從而使屬于同一藥盒的點云被聚類為多個簇。

圖10 處理結(jié)果

表層藥盒的精準(zhǔn)分割為接下來的位姿識別做準(zhǔn)備，表層未被遮擋的藥盒點云是完整的，而下層被遮擋的藥盒點云不完整，因此點云的點個數(shù)會明顯缺失，根據(jù)單個完整藥盒上表面點云的點數(shù)設(shè)定閾值，以此判斷識別表層未被遮擋的藥盒，先抓取表層藥盒，并根據(jù)表層單個藥盒點云的點個數(shù)設(shè)置處理優(yōu)先級，由表層至下層逐個完成抓取。

4.2 抓取測試實驗

系統(tǒng)能夠成功實現(xiàn)抓取，末端抓取力的控制是關(guān)鍵。系統(tǒng)通過吸盤抓取藥盒，吸盤吸附口直徑設(shè)計為4cm，配合真空泵可提供足夠的吸附力，機(jī)器人靜止時吸盤最大安全吸取質(zhì)量為2kg，為使機(jī)器人運動中吸盤可以牢固的吸住藥盒，需保證吸取藥盒質(zhì)量小于1.5kg，藥盒尺寸長寬應(yīng)大于4cm，滿足大部分藥盒。

分別對6 種不同大小的藥盒進(jìn)行了多次抓取，經(jīng)測試，系統(tǒng)能夠很好地完成藥盒的位姿估計和抓取任務(wù)，如圖11為抓取藥盒時吸盤的姿態(tài)。

圖11 吸盤姿態(tài)

5 結(jié)語

針對藥物生產(chǎn)線上藥品分揀、包裝等繁重的工作任務(wù)，設(shè)計了一種應(yīng)用于藥盒抓取的散亂堆疊藥盒三維視覺抓取系統(tǒng)。根據(jù)藥盒點云均為特征均勻的矩形或方形平面的特征，基于點云預(yù)處理與區(qū)域生長算法設(shè)計了場景分割算法；并且根據(jù)藥盒多為扁平規(guī)則體的特征提出通過計算藥盒上表面點云質(zhì)心以及質(zhì)心點表面法線的方法估計藥盒位姿。經(jīng)實驗證明，系統(tǒng)整體很好地實現(xiàn)了場景分割、位姿估計及抓取功能，能夠很好地適應(yīng)不同大小的藥盒，可以有效地提高藥物生產(chǎn)線的智能化和自動化以及降低人工成本，具有實際的應(yīng)用價值。