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

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

（2.河南省機(jī)器人與智能系統(tǒng)重點(diǎn)實(shí)驗(yàn)室 洛陽(yáng) 471003）

1 引言

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

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

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

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

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

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

藥盒為規(guī)則體，其表面光滑，多為特征均勻的矩形或方形平面，相比于通過(guò)手爪抓取藥盒，通過(guò)吸盤吸取藥盒更加快速準(zhǔn)確，根據(jù)藥盒的尺寸大小和質(zhì)量設(shè)計(jì)了真空吸盤，配備真空泵完成藥盒的抓取，吸盤實(shí)物如圖2 所示。系統(tǒng)硬件組成部分主要包括UR5 機(jī)器人、COMATRIX 深度相機(jī)、計(jì)算機(jī)、真空吸盤、真空泵等，如圖3 所示。相機(jī)相對(duì)機(jī)器人安裝位置可分為eye-in-hand 和eye-to-hand 兩種方式，本系統(tǒng)設(shè)計(jì)采用eye-in-hand 方式。選用COMATRIX 深度相機(jī)，深度圖分辨率（Depth Map）200 萬(wàn)個(gè)點(diǎn)，每個(gè)點(diǎn)都有獨(dú)立的（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è)計(jì)

3.1 場(chǎng)景點(diǎn)云數(shù)據(jù)獲取及預(yù)處理

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

3.1.1 去除載物臺(tái)背景

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

如圖4 所示，直通濾波去除載物臺(tái)前后對(duì)比，圖4（a）和（b）為場(chǎng)景原圖，圖4（c）和（d）為場(chǎng)景一去除載物臺(tái)前后對(duì)比，圖4（e）和（f）為場(chǎng)景二去除載物臺(tái)前后對(duì)比。

圖4 去除載物臺(tái)

3.1.2 體素化降采樣

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

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

圖5 體素降采樣

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

3.1.3 統(tǒng)計(jì)濾波離群點(diǎn)刪除

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

圖6 統(tǒng)計(jì)濾波去除離群點(diǎn)

3.2 場(chǎng)景目標(biāo)點(diǎn)云分割

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

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

圖7 表面法線估計(jì)

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

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

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

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

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

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

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

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

圖9 點(diǎn)云表面法線估計(jì)

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

4 實(shí)驗(yàn)與結(jié)果分析

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

4.1 場(chǎng)景分割測(cè)試實(shí)驗(yàn)

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

圖10 處理結(jié)果

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

4.2 抓取測(cè)試實(shí)驗(yàn)

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

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

圖11 吸盤姿態(tài)

5 結(jié)語(yǔ)

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