基于MatLab的多目标猕猴桃无损采摘路径规划

2019-12-21张莎莎王周宇陈礼鹏崔永杰

农机化研究 2019年4期

张莎莎，王周宇，陈礼鹏，莫浩，崔永杰

(西北农林科技大学机械与电子工程学院，陕西杨凌 712100)

0 引言

我国是世界上猕猴桃种植面积最大、产量最高的国家。猕猴桃果实营养丰富，附加值高，但目前其果实主要依靠人工采摘，存在着采摘时间集中、劳动强度大等问题。随着科技的进步，使用采摘机器人代替人工作业将成为可能。因此，采摘机器人怎样采摘能够达到无损并且使采摘效率最大化是采摘作业的关键技术之一。

近年来，很多国家在果蔬采摘机器人方向做了大量的研究工作，研制了多种果蔬采摘机械臂用以采摘不同的果蔬。1993 年，日本冈山大学的 N.Kondo等[1]人针对西红柿传统栽培系统研究出了七自由度的多功能模块式黄瓜收获机器人；新西兰的Scarfe A J等研制了猕猴桃采摘机械臂；意大利巴里理工大学的 Mario M. Foglia 等人研制了甘蓝采摘机器人；英国 Silsoe 研究院的 J.N.Reed 等人研制了蘑菇采摘机器人[2-3]。1997年，东北林业大学的陆怀民等开发设计了林木球果采集机器人；2004-2005 年间，中国农业大学的张铁中、徐丽明等设计了草莓采摘机器人；2007年，中国农业大学的汤修映、张铁中等人研制了六自由度的圆柱型黄瓜采摘机器人 FVHR-I；西北农林科技大学的崔永杰等人从2012-2017年致力于猕猴桃采摘机器人的研制，并研发出了一套能够精准采摘猕猴桃的机器人系统[3-5]。

目前，针对棚架结构和簇生特点作物采摘的研究依然较少，尤其是针对多关节机械臂无损采摘多目标猕猴桃的路径规划研究更少。因此，针对多目标猕猴桃采摘的路径规划研究有重要的理论价值和现实意义。

1 猕猴桃的生长环境

本文数据在西北农林科技大学眉县猕猴桃实验基地采集。猕猴桃果实是藤蔓植物，果枝生长通过搭架来辅助的，目前最普遍的结构是棚架式，结构相对规整，果枝下垂的比较少。就西北农林科技大学眉县试验站棚架式猕猴桃而言，其架子的上方采用8号铁丝构成“井”字型结构，果枝沿铁丝网生长，果实成簇的自然下垂，如图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的猕猴桃目标识别

基于棚架式生长的猕猴桃，其采摘环境中包括的背景有树叶、树干、天空及杂草等。本文选取图2中的猕猴桃坐标作为采摘对象。为了去除杂草及枝叶遮挡等因素的影响，从猕猴桃的下方来拍摄猕猴桃。图3为图像上获取目标的特征点，经过Kinect传感器的坐标转换得到目标点的三维坐标。

图2 簇状猕猴桃原始图像Fig.2 The original image clusters of Kiwifruit

图3 簇状猕猴桃最终识别效果Fig.3 The final recognition result of Kiwifruit

由图2获得的猕猴桃的最终识别像素坐标，经过Kinect传感器坐标转换可以得到图中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

根据猕猴桃在空间不同的分布来对机械臂的采摘路径进行规划，从而实现猕猴桃的无损和高效率采摘。

3 UR5机械臂D-H参数选定

3.1 D-H建模

UR5型机械臂为六关节串联机器人。6个关节为全旋转关节，由Universal Robot为公司提供其结构尺寸，如图5所示。其采用传统的D-H建模法对各个关节建立连杆坐标系，如图6所示。

图5 UR5机械臂的结构图Fig.5 Structure diagram of UR5 manipulator

选择机械臂的初始位置，并建立起连杆坐标，如图7所示。

图6 UR5型机器人的连杆坐标系Fig.6 The connecting rod coordinate system of UR5 robot

根据所建立的机器人连杆坐标系，可以得到UR5型机器人的D-H参数，如表2所示。

表2 UR5型机器人的D-H参数Table 2 The D-H Parameters of UR5 robots

续表2

由UR5机械臂的D-H参数表在MatLab中建立其三维模型，如图7所示。

图7 UR5机械臂的三维示意图Fig.7 Three dimensional diagram of UR5 manipulator

根据机械臂的构型可以得到机械臂的运动范围的工作空间[9-12]，如图8所示。

图8 UR5机械臂的工作空间图Fig.8 Diagram of the workspace of UR5 manipulator

由图8可知：机械臂是可以到达所有的目标点。由此证明，所选用的机械臂可以满足采摘任务。

3.2 机械臂运动学建模

由于机械臂均为旋转关节，由此来计算UR5机械臂的运动学方程。利用Robotics Tools box工具箱中的Link函数和robot函数来建立机械臂的研究对像[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机械臂均为旋转关节，则可以求得机械臂各关节的位移及末端关节的轨迹[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机械臂采摘多目标猕猴桃的轨迹规划

4.1 簇状多目标猕猴桃采摘顺序规划

由于猕猴桃果实成簇状生长，因此在采摘的过程中尽量采用由外向里的顺序采摘，用来防止因末端执行器采摘中间猕猴桃时对周围的猕猴桃产生碰撞等损伤。图9所示为簇状猕猴桃的生长状况。如果先采摘中间的9号猕猴桃，2号、12号、3号和13号猕猴桃会有被末端执行器损伤的可能；但是，如果采取由7、6、5、4、3、2、1、11、10、9、8、12、13、14号的先后顺序采摘猕猴桃，就可以有效地较少因采摘带来的猕猴桃损伤问题。

图9 簇状猕猴桃的生长状况Fig.9 Diagram of the workspace of UR5 manipulator

本文针对猕猴桃在采摘过程中容易因采摘而产生损伤的问题，选用由外向里的采摘方式。具体的方法为：先标定相机，以相机为世界坐标系中的原点，由相机获得图片中猕猴桃的三维坐标。首先，忽略猕猴桃三维坐标中的Z坐标，只考虑猕猴桃三维坐标中的X和Y值，主要是猕猴桃坐标中的Z值对于XOY平面内猕猴桃的方位没有影响；其次，通过求解所有目标猕猴桃的中心点C，并以此中心点为圆心，计算各个目标猕猴桃的三维坐标到中心点的距离di(i=1,2,3,4，...)；最后，以几何中心点为圆心、以di为半径做同心圆。机械臂先采摘外圆上的猕猴桃，再向里采摘内圆的猕猴桃，依次向里。以上文中所采集到的19个目标猕猴桃的三维坐标为仿真的对象，应用MatLab来实现UR5机械臂采摘猕猴桃的轨迹规划。

其中，中心点的求解公式为

(8)

(9)

代入以上19个点的在XOY平面内的坐标，可以求得其中心点为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。根据其距离，可以得到外圆到内圆上的目标依次为目标3、目标12、目标13、目标4、目标9、目标16、目标2、目标1、目标6、目标5、目标7、目标16、目标19、目标14、目标8、目标15、目标10、目标18、目标17、目标11，即UR5机械臂的采摘顺序。

4.2 UR5机械臂空间直线轨迹路径规划

UR5机械臂各个关节均为旋转关节，依靠关节的旋转使末端执行器运动。关节空间的轨迹规划以机械臂的关节角度、角速度和角加速度为目标进行规划。本文采用对机械臂进行空间直线轨迹规划，使其路径最短。

在已知起点和终点的情况下，进行直线插补的运算。设起点为A1(x1,y1,z1,∂1,β1,γ1)，终点为A2(x2,y2,z2,∂2,β2,γ2)。其中，x、y、z为笛卡尔坐标，∂、β、γ为RPY的变换姿态角。由式(10)与式(11)求得直线轨迹插补点A(x,y,z,∂,β,γ)的位置和姿态。

位置点为

(10)

姿态点为

(11)

当λ为0时，插补点对应起始点；当λ为1时，插补点对应终点；当λ在0～1的范围内时，对应的是中间的插补点。

由于猕猴桃的采摘是由下向上的，所以需要控制末端关节到达终点时的姿态垂直向上。由于在采摘时是由末端执行器来完成采摘的任务，因此对机械臂的速度控制为先加速后减速，当到达目标时速度减为零。

5 轨迹规划算法的验证

本文选用MatLab中的Simulink模块来搭建机械臂的整套仿真系统[15-16]，可以得到UR5机械臂末端的x、y、z随时间的变化曲线，如图10所示。机械臂运动的初始点为(-501.72，-109，-142.99)，图中显示的是末端关节在X、Y、Z方向随时间的位置变化。其中，data1、data2和data3分别代表末端关节在x、y、z方向的位置随时间的变化。可以看出：末端关节到达目标的先后顺序与预先设定规划的相同。

图10 UR5机械臂末端关节随时间的位置变化图Fig.10 The position change diagram of the end joint over time of UR5 manipulator

图11为末端关节速度在x、y、z方向随时间变化的曲线。其中，data1、data2、data3分别为末端关节在x、y、z方向的速度随时间的变化。由于机械臂的末端在采摘的过程中速度由零开始做加速运动，当到达目标猕猴时速度逐渐减为零。由图11可以看出：机械臂依次采摘19个目标，在该过程机械臂的速度也没有发生突变，顺利地完成采摘任务。