任工昌，張路平，劉朋，桓源

(陜西科技大學(xué)機(jī)電工程學(xué)院,陜西西安 710021)

0 前言

近年來，隨著機(jī)器人技術(shù)的高速發(fā)展，各種機(jī)器人已被廣泛運(yùn)用于醫(yī)療、服務(wù)、農(nóng)業(yè)和特種作業(yè)等多個領(lǐng)域。特別是近期疫情和人口老齡化問題的出現(xiàn)，加速了機(jī)器人向醫(yī)療和服務(wù)領(lǐng)域的應(yīng)用步伐[1]。目前，針對醫(yī)療和服務(wù)領(lǐng)域機(jī)器人的相關(guān)研究熱點(diǎn)主要集中在避障、機(jī)械臂的軌跡規(guī)劃或視覺識別等方面。但考慮到醫(yī)療和服務(wù)機(jī)器人的應(yīng)用場景，除機(jī)器人本體的相關(guān)技術(shù)研究外，如何使機(jī)器人更好地適用于各種復(fù)雜場景是促進(jìn)機(jī)器人實(shí)際應(yīng)用的關(guān)鍵。

現(xiàn)階段，醫(yī)療和服務(wù)機(jī)器人均通過攜帶機(jī)械臂幫助人類完成簡單的物品搬運(yùn)工作。機(jī)械臂在工業(yè)環(huán)境中應(yīng)用時，大多均是依照固定指令對物件進(jìn)行抓取和放置，物件形態(tài)及放置位置一旦發(fā)生變化，便難以完成[2]。很多研究人員都忽略了機(jī)械臂抓取技術(shù)與機(jī)械臂放置之間的差異，將機(jī)械臂抓取與機(jī)械臂放置歸為同一問題，未做深入研究。但是考慮實(shí)際運(yùn)用場景，在機(jī)械臂搬運(yùn)物品和放置過程中，如何在復(fù)雜環(huán)境下選取合理的放置區(qū)域是需要解決的關(guān)鍵問題。王永和李金澤[3]針對當(dāng)前機(jī)械臂抓取與放置方式固定、指令單一、難以應(yīng)對復(fù)雜未知情況的不足，提出一種基于深度強(qiáng)化學(xué)習(xí)與快速搜索隨機(jī)樹(Rapidly-exploring Random Tree，RRT)的機(jī)械臂抓放控制方法，經(jīng)過關(guān)鍵點(diǎn)選取以及RRT算法計(jì)算，根據(jù)任務(wù)需要將物件準(zhǔn)確放置于目標(biāo)位置，例如把杯子直立放在臺面上或把鞋子放在鞋架上，其研究重心為多類別的抓取放置。任工昌等[4]設(shè)計(jì)了一種拖車式醫(yī)用配送機(jī)器人，將立體循環(huán)車廂與機(jī)械手結(jié)合，可通過機(jī)械手將藥品放置于固定位置，實(shí)現(xiàn)醫(yī)院環(huán)境的藥品自主配送服務(wù)。

上述技術(shù)均屬于點(diǎn)對點(diǎn)式放置，放置位置固定且操作環(huán)境簡單。針對具有隨機(jī)障礙物的非結(jié)構(gòu)環(huán)境，如何實(shí)現(xiàn)可放置區(qū)域的識別和最佳放置點(diǎn)的選取仍有待研究。本文作者通過分析抓取與放置環(huán)境的差異性，提出一種基于三維點(diǎn)云信息的空置域識別和最佳放置點(diǎn)選取算法，可實(shí)現(xiàn)對復(fù)雜環(huán)境中空置域的識別和放置點(diǎn)的選取，對提高機(jī)器人在復(fù)雜環(huán)境下的作業(yè)能力具有積極作用。

1 問題描述和分析

研究解決機(jī)械臂在非結(jié)構(gòu)環(huán)境下放置的問題首先要理解并區(qū)分抓取與放置問題的差異性。兩者的差異主要體現(xiàn)在操作環(huán)境和操作流程的不同：(1)操作環(huán)境。分析目前的研究現(xiàn)狀，機(jī)器人抓取的操作環(huán)境大多為已知環(huán)境，大部分抓取技術(shù)實(shí)現(xiàn)的前提是已知抓取目標(biāo)的模型或位置，如室內(nèi)環(huán)境的模板匹配抓取或工業(yè)應(yīng)用中的工件識別抓??；相反，非結(jié)構(gòu)環(huán)境下機(jī)械臂放置面臨的首要難題是環(huán)境的未知性或隨機(jī)性，使得機(jī)械臂放置增加了難度和不確定性；(2)操作流程。機(jī)械臂抓取一般以識別目標(biāo)為前提，確定目標(biāo)相對于機(jī)械臂的坐標(biāo)，再通過機(jī)械臂進(jìn)行避障抓取。對于機(jī)械臂的放置，首先要對放置環(huán)境進(jìn)行實(shí)時識別分析，再確定可放置區(qū)域或可放置點(diǎn)，最后進(jìn)行避障放置。

2 空置域及放置點(diǎn)識別算法設(shè)計(jì)

針對空置域和放置點(diǎn)的識別流程如圖1所示。使用深度攝像頭獲取放置環(huán)境的三維點(diǎn)云信息，對獲取的點(diǎn)云圖進(jìn)行預(yù)處理后，采用統(tǒng)計(jì)濾波進(jìn)行離群點(diǎn)移除，從而減少數(shù)據(jù)處理量并降低噪聲對識別結(jié)果的影響[5]?；陬伾珔^(qū)域生長的聚類分割算法分割場景，得到無障礙桌面區(qū)域(空置域)，再利用平面擬合、點(diǎn)云投影等進(jìn)行處理，獲取平面空置域信息；通過對空置域進(jìn)行邊緣鄰域計(jì)算分析，獲取可放置區(qū)域，通過篩選可放置區(qū)域關(guān)鍵特征點(diǎn)識別獲取最終放置區(qū)域和放置點(diǎn)。

圖1 可放置域及放置點(diǎn)識別流程

2.1 點(diǎn)云預(yù)處理

利用KinectV2攝像頭獲取桌面環(huán)境點(diǎn)云信息，為避免光照等因素影響使得獲取的點(diǎn)云圖像存在噪點(diǎn)，需要對它進(jìn)行預(yù)處理。通過統(tǒng)計(jì)濾波去除空間中點(diǎn)云密集區(qū)域外的離群點(diǎn)，達(dá)到濾波降噪處理的目的[6]。

2.2 基于顏色區(qū)域生長的場景聚類分割

獲取的目標(biāo)場景實(shí)時點(diǎn)云數(shù)據(jù)中包括桌面及其障礙物信息，為得到可放置區(qū)域，需要將障礙物與桌面分割，從而得到無障礙物數(shù)據(jù)的桌面點(diǎn)云信息，便于后續(xù)處理。

本文作者選用基于顏色區(qū)域生長的聚類分割算法實(shí)現(xiàn)場景分割。該算法適合復(fù)雜室內(nèi)場景的分割，通過合并滿足相同色差閾值條件的點(diǎn)實(shí)現(xiàn)分割，其輸出的數(shù)據(jù)為分割后不同聚類組成的數(shù)組。將種子點(diǎn)鄰域內(nèi)色差小于閾值的點(diǎn)視為一個聚類，聚類之間的色差小于閾值時合并為一個聚類，且當(dāng)前聚類中點(diǎn)的數(shù)量小于聚類點(diǎn)數(shù)量的與最近的聚類合并在一起[7]。

2.3 基于隨機(jī)采樣一致性算法的平面擬合

由于受障礙物邊界信息的影響，經(jīng)分割后的無障礙桌面會出現(xiàn)表面不平整等情況。因此，為便于后續(xù)處理，需要對桌面的點(diǎn)云信息進(jìn)行平面擬合。隨機(jī)采樣一致性(Random Sample Consensus，RANSAC)算法是一種獨(dú)立于噪聲的方法，常用于在圖像中尋找平面[8]。通過 RANSAC 算法擬合平面，可以將數(shù)據(jù)點(diǎn)劃分為內(nèi)點(diǎn)和外點(diǎn)，內(nèi)點(diǎn)是滿足估計(jì)平面模型的點(diǎn)。擬合是為了通過迭代方式將更多的點(diǎn)形成平面模型，而將未擬合在平面模型內(nèi)的點(diǎn)看作為外點(diǎn)。RANSAC 平面擬合隨機(jī)采樣參數(shù)的估計(jì)可以有效降低噪聲點(diǎn)的影響。

2.4 空置域及放置點(diǎn)搜索算法設(shè)計(jì)研究

為得到物體的可放置區(qū)域和放置點(diǎn)坐標(biāo)，需要對得到的無障礙物桌面點(diǎn)云數(shù)據(jù)進(jìn)行處理分析。想要得到理想的放置區(qū)域需要考慮以下因素：(1)放置區(qū)域面積滿足要求；(2)放置區(qū)域沒有去除障礙物后的點(diǎn)云空洞缺陷。

2.4.1 算法思想

根據(jù)以上需求，提出一種基于邊緣鄰域搜索的空置域識別方法，并在此基礎(chǔ)上提出最優(yōu)放置點(diǎn)的選取算法。目前，點(diǎn)云的邊界點(diǎn)提取方法眾多，其中直接提取點(diǎn)云邊界的方法主要是基于點(diǎn)云微切面邊界提取的研究方法。其基本思想：首先，在所有點(diǎn)云中搜索到離該點(diǎn)最近的k個近鄰點(diǎn)；再將k個近鄰點(diǎn)依據(jù)最小二乘準(zhǔn)則投影到微切面上；最后在微切面上依據(jù)不同的判別方法提取邊界點(diǎn)[9]。

由于文中的研究對象為桌面點(diǎn)云，其本身為一整體平面，為提高算法搜索速度，降低數(shù)據(jù)處理量，經(jīng)點(diǎn)云旋轉(zhuǎn)，將其旋轉(zhuǎn)至與xoy面平行的平面，利用點(diǎn)云投影算法壓縮投影至xoy面，則搜索與z坐標(biāo)信息無關(guān)，避免了將三維點(diǎn)云投影到微切面的繁瑣過程，同時將算法處理的對象從空間三維點(diǎn)云轉(zhuǎn)變?yōu)槠矫娑S點(diǎn)云信息，大大減少了數(shù)據(jù)處理量。采用alpha shapes平面點(diǎn)云邊緣提取算法，提取經(jīng)投影平面化后的點(diǎn)云邊緣輪廓，通過遍歷識別邊緣點(diǎn)的半徑鄰域，確定不可放置的區(qū)域?yàn)橥恻c(diǎn)，則內(nèi)點(diǎn)為可放置區(qū)域，通過Kd-Tree算法搜索內(nèi)點(diǎn)中半徑為R的鄰域內(nèi)點(diǎn)云數(shù)量最多的點(diǎn)即為最優(yōu)放置點(diǎn)。其整體思路流程如圖2所示。

圖2 空置域及放置點(diǎn)搜索流程

2.4.2 alpha shapes邊緣提取算法

由EDELSBRUNNER等[10]提出的alpha shapes算法是一種簡單、有效的快速提取平面點(diǎn)云邊界點(diǎn)的算法。它克服了點(diǎn)云形狀不規(guī)則對算法的影響，可快速準(zhǔn)確提取邊界點(diǎn)，其原理如下。

如圖3所示，對于任意形狀的平面點(diǎn)云，有一半徑為α的圓，繞其進(jìn)行滾動。若α足夠小時，則點(diǎn)云中每一點(diǎn)均為邊界點(diǎn)：若α適當(dāng)增大到一定程度，其只在邊界點(diǎn)上進(jìn)行滾動，其滾動的軌跡為點(diǎn)云邊界[11]。

圖3 alpha shapes 邊界提取原理

其算法思路如下：

(1)對于一組平面點(diǎn)云，任選一點(diǎn)P,設(shè)定滾動圓半徑為α，搜索點(diǎn)P半徑2α內(nèi)的所有點(diǎn)云，記為點(diǎn)集Q;

(2)任選Q中一點(diǎn)P1(x1,y1)，計(jì)算過P與P1且半徑為α的兩個圓，如圖3(b)所示，P2與P3為圓心，則兩圓心坐標(biāo)為

(1)

其中：

(2)

S2=(x-x1)2+(y-y1)2

(3)

(3)計(jì)算去除P1(x1,y1)后，其余點(diǎn)P2與P3的距離，若所有點(diǎn)至這兩點(diǎn)的距離都大于α,則表明點(diǎn)P為邊界點(diǎn)P2；

(4)若剩余的點(diǎn)到點(diǎn)P2或P3的距離不全都大于α，則遍歷點(diǎn)集Q內(nèi)所有點(diǎn)輪換作為點(diǎn)P1。若存在某一點(diǎn)滿足以上條件，則表明該點(diǎn)為邊界點(diǎn)，終止對該點(diǎn)的判斷，判斷下一點(diǎn)；若Q中所有近鄰點(diǎn)中均不存在P1這樣的點(diǎn)，則表明點(diǎn)為非邊界點(diǎn)。

2.4.3 基于alpha shapes邊緣提取的可放置域識別算法

提取到邊界點(diǎn)后，對邊緣點(diǎn)進(jìn)行半徑鄰域遍歷搜索，半徑大小設(shè)為待放置物品的最大半徑，則外邊緣點(diǎn)與內(nèi)空洞邊緣點(diǎn)半徑鄰域的點(diǎn)云區(qū)域?yàn)椴豢煞胖命c(diǎn)，其余區(qū)域?yàn)榭煞胖脜^(qū)域。圖4所示為空置域識別提取算法原理，紅色為邊緣點(diǎn)，黑色為以R為半徑的邊緣點(diǎn)同心圓，黃色點(diǎn)為邊緣點(diǎn)R鄰域內(nèi)搜索到的無效點(diǎn)，剩余的綠色點(diǎn)為可放置點(diǎn)。

圖4 邊緣點(diǎn)鄰域提取原理

算法流程如下：

(1)讀取經(jīng)旋轉(zhuǎn)投影后的平面點(diǎn)云數(shù)據(jù)集合：

其中:N為進(jìn)行處理的點(diǎn)云個數(shù)；

(2)采用alpha shapes算法提取邊緣點(diǎn)數(shù)據(jù)，將它添加進(jìn)邊緣點(diǎn)集P1;

(3)在P對點(diǎn)云數(shù)據(jù)集合P1中的每一點(diǎn)Pi=[xi,yi,zi]T使用基于Kd 樹的搜索算法計(jì)算，查詢點(diǎn)半徑R內(nèi)的鄰域點(diǎn)，將搜索的鄰域點(diǎn)添加進(jìn)邊緣鄰域點(diǎn)集P2；

(4)從P中刪除集合P1與P2中的點(diǎn)云數(shù)據(jù)，則P中其余點(diǎn)為可放置區(qū)域點(diǎn)集；

2.4.4 基于點(diǎn)云半徑鄰域搜索的最優(yōu)放置點(diǎn)選擇算法

通過以上算法，得到可放置區(qū)域的點(diǎn)云數(shù)據(jù)集合，但這些點(diǎn)的分布有的靠近放置平面邊緣，有的距離障礙物過近，其安全性和可靠性較低，增加了機(jī)械臂的避障難度。針對以上問題，進(jìn)一步設(shè)計(jì)基于點(diǎn)云密度的最優(yōu)放置點(diǎn)選取算法。通過迭代搜索和Kd-Tree鄰域搜索算法，對得到的可放置區(qū)域點(diǎn)云數(shù)據(jù)進(jìn)行搜索，利用可放置區(qū)域點(diǎn)的無空洞最大鄰域半徑確定最終的最佳放置點(diǎn)。

點(diǎn)云為不均勻分布，假設(shè)已知其平均密度ρ、冗余變量δ、放置平面最大半徑Rmax、待放置物體的最大半徑Rmin，則半徑為Rs(cm)的無空洞點(diǎn)云數(shù)量m滿足：

0

(4)

(5)

文中最佳放置點(diǎn)為最大相同半徑鄰域內(nèi)點(diǎn)云數(shù)量最多的可放置點(diǎn)，則對于半徑為R=kRs的無空洞可靠鄰域，其點(diǎn)云數(shù)量mj應(yīng)滿足非線性關(guān)系：

mj=k2m

(6)

則有：

(7)

Rj=Rj+s

(8)

需縮小半徑時，有：

Rj=Rj-s

(9)

算法的流程如圖5所示，其詳細(xì)步驟如下：

圖5 放置點(diǎn)優(yōu)選算法流程

(3)確定點(diǎn)qj最大無空洞鄰域范圍。搜索點(diǎn)qj的Rj半徑內(nèi)的鄰域點(diǎn)數(shù)量mj；當(dāng)該點(diǎn)不滿足點(diǎn)云無空洞條件公式(7)時，則表明半徑過大，依據(jù)公式(9)縮小搜索半徑，若滿足無空洞條件時，則搜索鄰域可能是最大鄰域，依據(jù)公式(8)增加搜索半徑，進(jìn)一步確認(rèn)；同時，若mj≥m，則將當(dāng)前半徑鄰域作為該點(diǎn)最大無空洞鄰域；若mj

(4)若搜索半徑減小至Rj≤0時或搜索點(diǎn)在增加搜索半徑后，不滿足無空洞條件后，則表明已獲取到該點(diǎn)最大鄰域信息(最優(yōu)點(diǎn)坐標(biāo)qj、最大可靠鄰域半徑R、最優(yōu)點(diǎn)鄰域點(diǎn)數(shù)m)，則j=j+1，若j≤M,搜索下一個點(diǎn)，同時將當(dāng)前最大搜索半徑作為下一個點(diǎn)的初始搜索半徑(Rj=R)，跳轉(zhuǎn)至步驟(3)循環(huán)；

(5)若m≠0，則保留qj作為鄰域范圍最大點(diǎn)，輸出該點(diǎn)坐標(biāo)及鄰域信息;若m=0，則無解；

(6)算法結(jié)束。

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

利用實(shí)驗(yàn)室現(xiàn)有的醫(yī)藥配送機(jī)器人作為實(shí)驗(yàn)平臺，驗(yàn)證文中提出的算法。如圖6所示，該機(jī)器人裝有KinectV2攝像頭，前方為一個放置有部分物品的桌面，識別算法基于PCL點(diǎn)云庫和MATLAB點(diǎn)云工具箱進(jìn)行設(shè)計(jì)，利用ROS平臺進(jìn)行開發(fā)驗(yàn)證。

圖6 醫(yī)藥配送機(jī)器人及實(shí)驗(yàn)場景設(shè)計(jì)

首先，驅(qū)動相機(jī)采集數(shù)據(jù)。獲取實(shí)驗(yàn)環(huán)境的RGB-D圖和深度圖，如圖7(a)(b)所示。然后，采用棋盤格法對KinectV2相機(jī)進(jìn)行內(nèi)外參數(shù)標(biāo)定，根據(jù)標(biāo)定后的相機(jī)模型獲取實(shí)驗(yàn)環(huán)境的三維點(diǎn)云數(shù)據(jù)，如圖7(c)所示。

圖7 采集實(shí)驗(yàn)環(huán)境數(shù)據(jù)

獲取到實(shí)驗(yàn)所需點(diǎn)云數(shù)據(jù)后，對點(diǎn)云進(jìn)行預(yù)處理。經(jīng)過預(yù)處理后的點(diǎn)云數(shù)據(jù)如圖8所示,此時點(diǎn)云噪聲等冗余無效數(shù)據(jù)點(diǎn)被大量移除，提高了后續(xù)算法運(yùn)行的效率和魯棒性。

圖8 點(diǎn)云預(yù)處理結(jié)果

基于顏色區(qū)域生長的點(diǎn)云聚類分割，結(jié)果如圖9所示。通過區(qū)域色度信息將點(diǎn)云數(shù)據(jù)進(jìn)行類區(qū)分，經(jīng)過類分割提取后，獲取分離背景與障礙物的桌面，如圖10(a)所示，經(jīng)平面擬合后的桌面如圖10(b)所示。

圖9 基于顏色區(qū)域生長的點(diǎn)云聚類分割結(jié)果

圖10 桌面點(diǎn)云提取結(jié)果

對平面擬合后的桌面三維點(diǎn)云進(jìn)行旋轉(zhuǎn)、投影，壓縮為二維平面點(diǎn)云，只對其二維坐標(biāo)數(shù)據(jù)信息進(jìn)行處理，大大減少數(shù)據(jù)的處理量。圖11所示為點(diǎn)云平面化后的結(jié)果。

圖11 點(diǎn)云平面化

采用alpha shapes邊緣提取算法對壓縮后的二維點(diǎn)進(jìn)行邊緣的識別提取，圖12所示為采用alpha shapes對經(jīng)投影平面化后的點(diǎn)云進(jìn)行邊緣識別提取的結(jié)果。

圖12 alpha shapes邊緣識別提取結(jié)果

提取到邊緣點(diǎn)云數(shù)據(jù)后，通過對邊緣點(diǎn)云的半徑鄰域進(jìn)行遍歷搜索，如圖13所示，區(qū)分提取到可放置區(qū)域。

圖13 基于邊緣點(diǎn)鄰域搜索的可放置域識別結(jié)果

獲取可放置區(qū)域后，基于可放置區(qū)域的點(diǎn)云數(shù)據(jù)，采用文中提出的基于最優(yōu)放置點(diǎn)選取算法，獲取最優(yōu)放置點(diǎn)，即最大無空洞半徑鄰域內(nèi)點(diǎn)云數(shù)量最多的點(diǎn)。如圖14(a)所示。為更加直觀地展示其放置區(qū)域，采用基于Kd-Tree的半徑鄰域搜索算法，搜索顯示其最大半徑鄰域，如圖14(b)所示。

圖14 最優(yōu)放置點(diǎn)選取算法結(jié)果

4 總結(jié)

為解決室內(nèi)非結(jié)構(gòu)環(huán)境下機(jī)械臂放置物品區(qū)域的識別難題，本文作者基于三維點(diǎn)云信息處理和點(diǎn)云半徑鄰域搜索提出了一種空置域識別和最優(yōu)放置點(diǎn)選取算法。通過對點(diǎn)云信息進(jìn)行預(yù)處理、分割、平面擬合等操作，實(shí)現(xiàn)了空置域的識別。結(jié)合alpha shapes平面點(diǎn)云邊緣提取算法和半徑鄰域搜索算法得到可放置區(qū)域，并基于點(diǎn)云密度篩選出最優(yōu)放置點(diǎn)。通過實(shí)驗(yàn)驗(yàn)證了算法的可行性和有效性。該算法為機(jī)械臂在非結(jié)構(gòu)環(huán)境下的隨機(jī)放置問題提供了有效解決方案，對推進(jìn)移動機(jī)器人在復(fù)雜環(huán)境下的自主抓放技術(shù)具有積極意義。