張莎莎，王周宇，陳禮鵬，莫 浩，崔永杰

(西北農(nóng)林科技大學(xué) 機(jī)械與電子工程學(xué)院，陜西 楊凌 712100)

0 引言

我國(guó)是世界上獼猴桃種植面積最大、產(chǎn)量最高的國(guó)家。獼猴桃果實(shí)營(yíng)養(yǎng)豐富，附加值高，但目前其果實(shí)主要依靠人工采摘，存在著采摘時(shí)間集中、勞動(dòng)強(qiáng)度大等問題。隨著科技的進(jìn)步，使用采摘機(jī)器人代替人工作業(yè)將成為可能。因此，采摘機(jī)器人怎樣采摘能夠達(dá)到無損并且使采摘效率最大化是采摘作業(yè)的關(guān)鍵技術(shù)之一。

近年來，很多國(guó)家在果蔬采摘機(jī)器人方向做了大量的研究工作，研制了多種果蔬采摘機(jī)械臂用以采摘不同的果蔬。1993 年，日本岡山大學(xué)的 N.Kondo等[1]人針對(duì)西紅柿傳統(tǒng)栽培系統(tǒng)研究出了七自由度的多功能模塊式黃瓜收獲機(jī)器人；新西蘭的Scarfe A J等研制了獼猴桃采摘機(jī)械臂；意大利巴里理工大學(xué)的 Mario M. Foglia 等人研制了甘藍(lán)采摘機(jī)器人；英國(guó) Silsoe 研究院的 J.N.Reed 等人研制了蘑菇采摘機(jī)器人[2-3]。1997年，東北林業(yè)大學(xué)的陸懷民等開發(fā)設(shè)計(jì)了林木球果采集機(jī)器人；2004-2005 年間，中國(guó)農(nóng)業(yè)大學(xué)的張鐵中、徐麗明等設(shè)計(jì)了草莓采摘機(jī)器人；2007年，中國(guó)農(nóng)業(yè)大學(xué)的湯修映、張鐵中等人研制了六自由度的圓柱型黃瓜采摘機(jī)器人 FVHR-I；西北農(nóng)林科技大學(xué)的崔永杰等人從2012-2017年致力于獼猴桃采摘機(jī)器人的研制，并研發(fā)出了一套能夠精準(zhǔn)采摘獼猴桃的機(jī)器人系統(tǒng)[3-5]。

目前，針對(duì)棚架結(jié)構(gòu)和簇生特點(diǎn)作物采摘的研究依然較少，尤其是針對(duì)多關(guān)節(jié)機(jī)械臂無損采摘多目標(biāo)獼猴桃的路徑規(guī)劃研究更少。因此，針對(duì)多目標(biāo)獼猴桃采摘的路徑規(guī)劃研究有重要的理論價(jià)值和現(xiàn)實(shí)意義。

1 獼猴桃的生長(zhǎng)環(huán)境

本文數(shù)據(jù)在西北農(nóng)林科技大學(xué)眉縣獼猴桃實(shí)驗(yàn)基地采集。獼猴桃果實(shí)是藤蔓植物，果枝生長(zhǎng)通過搭架來輔助的，目前最普遍的結(jié)構(gòu)是棚架式，結(jié)構(gòu)相對(duì)規(guī)整，果枝下垂的比較少。就西北農(nóng)林科技大學(xué)眉縣試驗(yàn)站棚架式獼猴桃而言，其架子的上方采用8號(hào)鐵絲構(gòu)成“井”字型結(jié)構(gòu)，果枝沿鐵絲網(wǎng)生長(zhǎng)，果實(shí)成簇的自然下垂，如圖1所示。

圖1 獼猴桃栽培模式Fig.1 The cultivation pattern of kiwi fruit tree

棚架式獼猴桃架子之間的橫向距離約為4m，縱向距離約為3m，果實(shí)成熟之后均勻分布在鐵絲上，高度分布在1.3～1.7m之間。獼猴桃為簇生植物，每簇在3～5個(gè)之間，單個(gè)獼猴桃尺寸平均值長(zhǎng)為62.95mm、寬47.02mm、厚51.4mm[5]。

2 基于Kinect的獼猴桃目標(biāo)識(shí)別

基于棚架式生長(zhǎng)的獼猴桃，其采摘環(huán)境中包括的背景有樹葉、樹干、天空及雜草等。本文選取圖2中的獼猴桃坐標(biāo)作為采摘對(duì)象。為了去除雜草及枝葉遮擋等因素的影響，從獼猴桃的下方來拍攝獼猴桃。圖3為圖像上獲取目標(biāo)的特征點(diǎn)，經(jīng)過Kinect傳感器的坐標(biāo)轉(zhuǎn)換得到目標(biāo)點(diǎn)的三維坐標(biāo)。

圖2 簇狀獼猴桃原始圖像Fig.2 The original image clusters of Kiwifruit

圖3 簇狀獼猴桃最終識(shí)別效果Fig.3 The final recognition result of Kiwifruit

由圖2獲得的獼猴桃的最終識(shí)別像素坐標(biāo)，經(jīng)過Kinect傳感器坐標(biāo)轉(zhuǎn)換可以得到圖中19個(gè)目標(biāo)點(diǎn)的空間坐標(biāo)，如表1所示。

表1 目標(biāo)點(diǎn)的空間坐標(biāo)Table 1 Spatial coordinates of the target point mm

由以上的目標(biāo)點(diǎn)可以得到目標(biāo)點(diǎn)在空間的三維散點(diǎn)圖[6-8]。目標(biāo)點(diǎn)在三維空間的分布如圖4所示。

圖4 目標(biāo)獼猴桃在空間的分布Fig.4 The target distribution of Kiwifruit in the space

根據(jù)獼猴桃在空間不同的分布來對(duì)機(jī)械臂的采摘路徑進(jìn)行規(guī)劃，從而實(shí)現(xiàn)獼猴桃的無損和高效率采摘。

3 UR5機(jī)械臂D-H參數(shù)選定

3.1 D-H建模

UR5型機(jī)械臂為六關(guān)節(jié)串聯(lián)機(jī)器人。6個(gè)關(guān)節(jié)為全旋轉(zhuǎn)關(guān)節(jié)，由Universal Robot為公司提供其結(jié)構(gòu)尺寸，如圖5所示。其采用傳統(tǒng)的D-H建模法對(duì)各個(gè)關(guān)節(jié)建立連桿坐標(biāo)系，如圖6所示。

圖5 UR5機(jī)械臂的結(jié)構(gòu)圖Fig.5 Structure diagram of UR5 manipulator

選擇機(jī)械臂的初始位置，并建立起連桿坐標(biāo)，如圖7所示。

圖6 UR5型機(jī)器人的連桿坐標(biāo)系Fig.6 The connecting rod coordinate system of UR5 robot

根據(jù)所建立的機(jī)器人連桿坐標(biāo)系，可以得到UR5型機(jī)器人的D-H參數(shù)，如表2所示。

表2 UR5型機(jī)器人的D-H參數(shù)Table 2 The D-H Parameters of UR5 robots

續(xù)表2

由UR5機(jī)械臂的D-H參數(shù)表在MatLab中建立其三維模型，如圖7所示。

圖7 UR5機(jī)械臂的三維示意圖Fig.7 Three dimensional diagram of UR5 manipulator

根據(jù)機(jī)械臂的構(gòu)型可以得到機(jī)械臂的運(yùn)動(dòng)范圍的工作空間[9-12]，如圖8所示。

圖8 UR5機(jī)械臂的工作空間圖Fig.8 Diagram of the workspace of UR5 manipulator

由圖8可知：機(jī)械臂是可以到達(dá)所有的目標(biāo)點(diǎn)。由此證明，所選用的機(jī)械臂可以滿足采摘任務(wù)。

3.2 機(jī)械臂運(yùn)動(dòng)學(xué)建模

由于機(jī)械臂均為旋轉(zhuǎn)關(guān)節(jié)，由此來計(jì)算UR5機(jī)械臂的運(yùn)動(dòng)學(xué)方程。利用Robotics Tools box工具箱中的Link函數(shù)和robot函數(shù)來建立機(jī)械臂的研究對(duì)像[13]，即

Link1=link ([pi/2 0 0 89.2 0], 'standard');

Link2=link ([0 -425 0 0 0], 'standard');

Link3=link ([0 -392.5 0 0 0], 'standard');

Link4=link ([pi/2 0 0 109.3 0], 'standard');

Link5=link ([-pi/2 0 0 93 0], 'standard');

Link6=link ([0 0 0 82 0], 'standard');

其中，a2=425，a3=392.43，d1=89.2，d4=109，d5=93，d6=82。

由于UR5機(jī)械臂均為旋轉(zhuǎn)關(guān)節(jié)，則可以求得機(jī)械臂各關(guān)節(jié)的位移及末端關(guān)節(jié)的軌跡[14]。其計(jì)算公式為

(1)

(2)

(3)

(4)

(5)

(6)

最終求得T06為

T06=T01·T12·T23·T34·T45·T56

(7)

其中，SI=sinθi，CI=cosθi，a2=425mm，a3=392.43mm，d1=89.2mm，d4=109mm，d5=93mm，d6=85mm。

4 UR5機(jī)械臂采摘多目標(biāo)獼猴桃的軌跡規(guī)劃

4.1 簇狀多目標(biāo)獼猴桃采摘順序規(guī)劃

由于獼猴桃果實(shí)成簇狀生長(zhǎng)，因此在采摘的過程中盡量采用由外向里的順序采摘，用來防止因末端執(zhí)行器采摘中間獼猴桃時(shí)對(duì)周圍的獼猴桃產(chǎn)生碰撞等損傷。圖9所示為簇狀獼猴桃的生長(zhǎng)狀況。如果先采摘中間的9號(hào)獼猴桃，2號(hào)、12號(hào)、3號(hào)和13號(hào)獼猴桃會(huì)有被末端執(zhí)行器損傷的可能；但是，如果采取由7、6、5、4、3、2、1、11、10、9、8、12、13、14號(hào)的先后順序采摘獼猴桃，就可以有效地較少因采摘帶來的獼猴桃損傷問題。

圖9 簇狀獼猴桃的生長(zhǎng)狀況Fig.9 Diagram of the workspace of UR5 manipulator

本文針對(duì)獼猴桃在采摘過程中容易因采摘而產(chǎn)生損傷的問題，選用由外向里的采摘方式。具體的方法為：先標(biāo)定相機(jī)，以相機(jī)為世界坐標(biāo)系中的原點(diǎn)，由相機(jī)獲得圖片中獼猴桃的三維坐標(biāo)。首先，忽略獼猴桃三維坐標(biāo)中的Z坐標(biāo)，只考慮獼猴桃三維坐標(biāo)中的X和Y值，主要是獼猴桃坐標(biāo)中的Z值對(duì)于XOY平面內(nèi)獼猴桃的方位沒有影響；其次，通過求解所有目標(biāo)獼猴桃的中心點(diǎn)C，并以此中心點(diǎn)為圓心，計(jì)算各個(gè)目標(biāo)獼猴桃的三維坐標(biāo)到中心點(diǎn)的距離di(i=1,2,3,4，...)；最后，以幾何中心點(diǎn)為圓心、以di為半徑做同心圓。機(jī)械臂先采摘外圓上的獼猴桃，再向里采摘內(nèi)圓的獼猴桃，依次向里。以上文中所采集到的19個(gè)目標(biāo)獼猴桃的三維坐標(biāo)為仿真的對(duì)象，應(yīng)用MatLab來實(shí)現(xiàn)UR5機(jī)械臂采摘獼猴桃的軌跡規(guī)劃。

其中，中心點(diǎn)的求解公式為

(8)

(9)

代入以上19個(gè)點(diǎn)的在XOY平面內(nèi)的坐標(biāo)，可以求得其中心點(diǎn)為C(111.95，54.631 6)。以此點(diǎn)為中心點(diǎn)可以求得目標(biāo)點(diǎn)到該點(diǎn)的距離di(i=1,2,3,4，...)，則當(dāng)以此距離為半徑畫圓時(shí)，可以得到從目標(biāo)1～目標(biāo)19到中心點(diǎn)的距離依次為154.9、156.8、274.5、224.4、141.5、141.6、141.4、63.6、172.4、38.5、11.9、273.6、225.3、83.1、57.2、136.4、12.25、15.4、106.48mm。根據(jù)其距離，可以得到外圓到內(nèi)圓上的目標(biāo)依次為目標(biāo)3、目標(biāo)12、目標(biāo)13、目標(biāo)4、目標(biāo)9、目標(biāo)16、目標(biāo)2、目標(biāo)1、目標(biāo)6、目標(biāo)5、目標(biāo)7、目標(biāo)16、目標(biāo)19、目標(biāo)14、目標(biāo)8、目標(biāo)15、目標(biāo)10、目標(biāo)18、目標(biāo)17、目標(biāo)11，即UR5機(jī)械臂的采摘順序。

4.2 UR5機(jī)械臂空間直線軌跡路徑規(guī)劃

UR5機(jī)械臂各個(gè)關(guān)節(jié)均為旋轉(zhuǎn)關(guān)節(jié)，依靠關(guān)節(jié)的旋轉(zhuǎn)使末端執(zhí)行器運(yùn)動(dòng)。關(guān)節(jié)空間的軌跡規(guī)劃以機(jī)械臂的關(guān)節(jié)角度、角速度和角加速度為目標(biāo)進(jìn)行規(guī)劃。本文采用對(duì)機(jī)械臂進(jìn)行空間直線軌跡規(guī)劃，使其路徑最短。

在已知起點(diǎn)和終點(diǎn)的情況下，進(jìn)行直線插補(bǔ)的運(yùn)算。設(shè)起點(diǎn)為A1(x1,y1,z1,?1,β1,γ1)，終點(diǎn)為A2(x2,y2,z2,?2,β2,γ2)。其中，x、y、z為笛卡爾坐標(biāo)，?、β、γ為RPY的變換姿態(tài)角。由式(10)與式(11)求得直線軌跡插補(bǔ)點(diǎn)A(x,y,z,?,β,γ)的位置和姿態(tài)。

位置點(diǎn)為

(10)

姿態(tài)點(diǎn)為

(11)

當(dāng)λ為0時(shí)，插補(bǔ)點(diǎn)對(duì)應(yīng)起始點(diǎn)；當(dāng)λ為1時(shí)，插補(bǔ)點(diǎn)對(duì)應(yīng)終點(diǎn)；當(dāng)λ在0～1的范圍內(nèi)時(shí)，對(duì)應(yīng)的是中間的插補(bǔ)點(diǎn)。

由于獼猴桃的采摘是由下向上的，所以需要控制末端關(guān)節(jié)到達(dá)終點(diǎn)時(shí)的姿態(tài)垂直向上。由于在采摘時(shí)是由末端執(zhí)行器來完成采摘的任務(wù)，因此對(duì)機(jī)械臂的速度控制為先加速后減速，當(dāng)?shù)竭_(dá)目標(biāo)時(shí)速度減為零。

5 軌跡規(guī)劃算法的驗(yàn)證

本文選用MatLab中的Simulink模塊來搭建機(jī)械臂的整套仿真系統(tǒng)[15-16]，可以得到UR5機(jī)械臂末端的x、y、z隨時(shí)間的變化曲線，如圖10所示。機(jī)械臂運(yùn)動(dòng)的初始點(diǎn)為(-501.72，-109，-142.99)，圖中顯示的是末端關(guān)節(jié)在X、Y、Z方向隨時(shí)間的位置變化。其中，data1、data2和data3分別代表末端關(guān)節(jié)在x、y、z方向的位置隨時(shí)間的變化?？梢钥闯觯耗┒岁P(guān)節(jié)到達(dá)目標(biāo)的先后順序與預(yù)先設(shè)定規(guī)劃的相同。

圖10 UR5機(jī)械臂末端關(guān)節(jié)隨時(shí)間的位置變化圖Fig.10 The position change diagram of the end joint over time of UR5 manipulator

圖11為末端關(guān)節(jié)速度在x、y、z方向隨時(shí)間變化的曲線。其中，data1、data2、data3分別為末端關(guān)節(jié)在x、y、z方向的速度隨時(shí)間的變化。由于機(jī)械臂的末端在采摘的過程中速度由零開始做加速運(yùn)動(dòng)，當(dāng)?shù)竭_(dá)目標(biāo)獼猴時(shí)速度逐漸減為零。由圖11可以看出：機(jī)械臂依次采摘19個(gè)目標(biāo)，在該過程機(jī)械臂的速度也沒有發(fā)生突變，順利地完成采摘任務(wù)。

圖11 UR5機(jī)械臂末端關(guān)節(jié)速度隨時(shí)間的變化圖Fig.11 The change of the velocity of the end joint over time of UR5 manipulator

由于本實(shí)驗(yàn)所用的末端執(zhí)行器采摘獼猴桃是由下向上垂直采摘獼猴桃，這就要求機(jī)械臂的末端保持垂直向上。其中，data1、data2、data3所以在該過程中需要保持機(jī)械臂末端的位姿垂直向上。由其仿真的結(jié)果可以看出(見圖12)：雖然在采摘的中間過程有突變，但是其很快恢復(fù)，并沒有影響采摘。

圖12 UR5機(jī)械臂末端關(guān)節(jié)位姿姿態(tài)隨時(shí)間的變化圖Fig.12 The change of the gesture of the end joint over time of UR5 manipulator

6 結(jié)論

1)本文分別建立了一套減少多目標(biāo)獼猴桃采摘中損傷的采摘算法。該算法規(guī)定了多目標(biāo)獼猴桃采摘順序的準(zhǔn)則，并采用最短距離的采摘方法，提高了采摘效率，且針對(duì)獼猴桃特有的采摘模式，有效地控制了其末端位姿垂直向上。

2)通過對(duì)采摘過程的仿真分析，驗(yàn)證了該算法的有效性。該算法的提出為獼猴桃采摘機(jī)器人高效、無損采摘提供了新的方法。