基于MatLab的多目標獼猴桃無損采摘路徑規(guī)劃

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

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

0 引言

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

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

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

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

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

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

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

2 基于Kinect的獼猴桃目標識別

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

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

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

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

表1 目標點的空間坐標Table 1 Spatial coordinates of the target point mm

由以上的目標點可以得到目標點在空間的三維散點圖[6-8]。目標點在三維空間的分布如圖4所示。

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

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

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

3.1 D-H建模

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

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

選擇機械臂的初始位置，并建立起連桿坐標，如圖7所示。

圖6 UR5型機器人的連桿坐標系Fig.6 The connecting rod coordinate system of UR5 robot

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

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

續(xù)表2

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

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

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

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

由圖8可知：機械臂是可以到達所有的目標點。由此證明，所選用的機械臂可以滿足采摘任務(wù)。

3.2 機械臂運動學(xué)建模

由于機械臂均為旋轉(zhuǎn)關(guān)節(jié)，由此來計算UR5機械臂的運動學(xué)方程。利用Robotics Tools box工具箱中的Link函數(shù)和robot函數(shù)來建立機械臂的研究對像[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機械臂均為旋轉(zhuǎn)關(guān)節(jié)，則可以求得機械臂各關(guān)節(jié)的位移及末端關(guān)節(jié)的軌跡[14]。其計算公式為

(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機械臂采摘多目標獼猴桃的軌跡規(guī)劃

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

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

圖9 簇狀獼猴桃的生長狀況Fig.9 Diagram of the workspace of UR5 manipulator

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

其中，中心點的求解公式為

(8)

(9)

代入以上19個點的在XOY平面內(nèi)的坐標，可以求得其中心點為C(111.95，54.631 6)。以此點為中心點可以求得目標點到該點的距離di(i=1,2,3,4，...)，則當以此距離為半徑畫圓時，可以得到從目標1～目標19到中心點的距離依次為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)圓上的目標依次為目標3、目標12、目標13、目標4、目標9、目標16、目標2、目標1、目標6、目標5、目標7、目標16、目標19、目標14、目標8、目標15、目標10、目標18、目標17、目標11，即UR5機械臂的采摘順序。

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

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

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

位置點為

(10)

姿態(tài)點為

(11)

當λ為0時，插補點對應(yīng)起始點；當λ為1時，插補點對應(yīng)終點；當λ在0～1的范圍內(nèi)時，對應(yīng)的是中間的插補點。

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

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

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

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

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

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

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

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

6 結(jié)論

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

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