一种采摘机械臂抓取运动规划方法

1.本发明属于机器人农业采摘领域，尤其涉及一种采摘机械臂抓取运动规划方法，适用于植物工厂或植物大棚中甜椒果实的自动化采摘。


背景技术：

2.随着中国人口老龄化进程加速，人口红利逐渐消失，导致劳动力成本上升，在新型工业化、城镇化和农业现代化背景下，农村人口外流，导致劳动力锐减。我国是农业大国，每年在果蔬番茄、甜椒、苹果、柑橘等成熟的季节需要大量的劳动力进行采摘，工人们经常需要顶着烈日进行果蔬采摘，劳动强度大、采摘效率低。为了降低收获成本，提高收获效率，因此非常有必要发展智慧农业，研发具有自主感知和精确抓取能力的果蔬采摘机器人。随着机器视觉，传感器，机器人等技术的不断的发展，越来越多的智能果蔬采摘机器人代替人类完成果蔬采摘任务，但是果蔬采摘机器人的设计仍然面临着很多亟待解决的难题。
3.首先第一点是视觉系统的识别不够精确。视觉系统就好比人的眼睛，只有看得准，才能够抓得准。农业采摘环境是一个非结构化的环境，采摘环境中由于枝叶遮挡和光照变化等因素的存在，会对果实果梗的识别精度产生很大的影响。此外，识别过程中果实果梗与果实相比较细，难以精准识别，因此提高识别算法的鲁棒性和精准性是非常有必要的。
4.第二点是采摘机器人的采摘效率低、系统不可靠。用于农业场景的采摘机器人，果实采摘过程中使用的是六自由度工业机械臂进行采摘，希望机械臂快速且高效按照某一特定姿态到达果实果梗的采摘点，进行果实采摘。机械臂的工作空间分为灵活工作空间和非灵活工作空间，灵活工作空间是能够以任意姿态到达的工作空间，非灵活工作空间下机械臂采摘过程中由于姿态的限制，导致机械臂常常会出现无解或者有解但是机械臂的运动会出现大幅度的变化，不能按照期望的路径去进行果实的采摘。这些会导致采摘机器人出现效率低，系统不可靠(成功率低)等问题，最终导致农民拒绝使用这些采摘机器人。因此在移动采摘领域如何确保采摘机器人快速可靠和高效地进行采摘的需求仍然很大，迫切需要开展研究。


技术实现要素：

5.为了克服采摘环境中遮挡、光照和果梗较细对果实果梗识别准确度的影响和解决采摘机器人效率低和系统不可靠的问题。本发提供一种采摘机械臂抓取运动规划方法。
6.本发明的具体技术方案为：
7.一种采摘机械臂抓取运动规划方法，包括步骤如下：
8.步骤1，广视角下采用双目相机1对采摘环境进行感知，对果实进行初识别，识别出果实的个数和相机坐标系下每个果实的三维坐标，得到初始采摘点；
9.步骤2，机械臂与目标果实之间定义预采摘平面，将步骤1得到的初始采摘点投影到预采摘平面，得到预采摘点；
10.步骤3，根据离线规划得到机械臂从起始位置到每一个预采摘点的轨迹；结合离线
规划的轨迹和在线规划对当前视角下要采摘的果实进行建模，规划机械臂依次到达预采摘点的最优采摘顺序；机械臂先到达规划采摘顺序后的一个预采摘点，然后近视角下通过双目相机2进行二次识别得到对应的目标采摘点，再通过在线规划进行采摘，采摘完成后回到预采摘点，然后根据规划的采摘顺序到达下一个预采摘点；
11.步骤4，机械臂重复步骤3，直到步骤2预采摘平面内所有预采摘点遍历后，回到机械臂的初始位置。
12.进一步地，步骤1所述的果实初识别采用yolov。
13.进一步地，步骤2预采摘平面的设定具体为：如图10所示，机械臂的工作空间是半径为rcm的球体，预采摘平面到果实的位置为10-15cm，预采摘平面到机械臂基坐标系的距离为(r-15,r-10)，预采摘平面长为rcm，宽为0.75rcm。箭头表示机械臂到达预采摘平面的位置和姿态。本发明中预采摘平面的大小设置可以根据需求进行调整，此所引申出的显而易见的变化或变动仍处于本发明创造的保护范围之中。
14.进一步地，步骤3，所述离线规划为采用强化学习算法进行训练，输出到达每个预采摘点的轨迹。
15.进一步地，步骤3，所述强化学习算法为ppo算法。
16.进一步地，步骤3，对机械臂的整个采摘过程，离线规划和在线规划进行进行建模，规划机械臂依次到达预采摘点的采摘顺序，构建能量成本函数，以机械臂的关节角度变化之和最小作为优化指标；建模成旅行商问题进行求解，规划到达预采摘点的顺序，即机械臂的最优采摘顺序。
17.进一步地，步骤3，所述的二次识别具体方法为，到达预采摘点后近视角下采用双目相机2进行果实的果梗识别，首先使用yolov识别果实，获取果梗的感兴趣区域。
18.进一步地，针对所述的感兴趣区域使用轻量型实例分割网络yolact，获取果实果梗的位置，得到目标采摘点。解决了识别过程中果实果梗与果实相比较细，难以精准识别以及采摘环境中由于枝叶的遮挡，光照变化较大的影响，
19.进一步地，步骤3，所述在线规划具体为采用ros机器人操作系统中的moveit框架控制机械臂进行采摘，在线规划过程中采用双向rrt进行机械臂的路径规划，采用仿生优化bioik(biologically inspired optimization inverse kinematics)算法进行机械臂的逆解求取。
20.为了解决采摘机器人的效率低，系统不可靠(成功率低)的问题，对机械臂的工作空间进行划分，定义预采摘平面把整个采摘规划过程分为离线规划和在线规划两个部分，离线规划部分基于强化学习算法中的近端策略优化ppo(proximal policy optimization)算法进行训练。基于yolov采用双目视觉相机1以广视角进行初识别获取甜椒果实的位置，根据甜椒果实的位置投影到预采摘平面，得到甜椒果实预采摘点的位置，通过强化学习输出到达每个预采摘点的轨迹。采摘过程中分为以下步骤：首先机械臂需要先到达预采摘点，然后进行二次识别和在线规划进行采摘，采摘完成后回到预采摘点，然后到达下一预采摘点。重复这个过程直到当前视角下果实采摘完，最后回到初始位置。采摘过程中在线规划和离线规划的示意图如图7所示。
21.机械臂的在线规划过程是机械臂到达预采摘平面上的点后开始计算的，过程如下：机械臂从预采摘点出发，到达目标采摘点进行采摘，然后回到预采摘点。把机械臂在线
规划过程中各关节角度变化之和作为机械臂消耗的能量，各关节角度变化之和越小，能量成本越小。通过图7示意图可以看出，机械臂在线规划过程中完成当前视角全部果实的采摘，所消耗的能量可以看成常数。
22.离线规划阶段过程是机械臂从出发点依次遍历所有预采摘点，然后回到初始位置的过程。在离线规划阶段，机械臂经过预采摘点顺序不一样，机械臂消耗的能量成本是不一样的。为了减少机械臂采摘过程的能量成本的消耗，提高机械臂的采摘效率，对离线规划部分和在线规划部分进行建模，建模成旅行商问题，规划最优采摘顺序。
23.由于采摘过程中机械臂在线规划消耗的能量的成本函数都是一样的，因此在规划最优采摘顺序过程中，在线规划过程能量成本函数可以看做是一个常数。机械臂从开始位置出发，依次通过全部预采摘点最后回到开始的位置，机械臂各关节角度变化之和最小即是最优的采摘顺序，所消耗的能量最小。通过遗传算法对最优采摘顺序进行求解，计算出机械臂到达预采摘点的最优采摘顺序。
24.采用离线规划和规划采摘顺序能够显著提高采摘机器人的成功率和采摘效率。
附图说明
25.图1是一种采摘机械臂抓取运动规划方法流程图。
26.图2是采摘机器人真实作业环境示意图。
27.图3是机械臂和2个双目相机坐标转换关系的示意图。
28.图4是强化学习仿真环境示意图。
29.图5是甜椒果梗的感兴趣区域示意图。
30.图6是果实投影到预采摘平面上的示意图。
31.图7是采摘机器人离线规划和在线规划示意图。
32.图8是采摘机器人二次识别和在线规划示意图。
33.图9是本发明的甜椒果梗识别算法yolact网络结构示意图。
34.图10是预采摘平面大小尺寸示意图。
具体实施方式
35.下面结合附图，对本发明进行详细的说明。
36.本发明提出一种采摘机械臂抓取运动规划方法。该方法是在植物工厂、植物大棚等规范化种植场景，实现农业采摘机器人的果实采摘作业为应用背景。本发明所述果实以甜椒为例，实现一种采摘机械臂抓取运动规划方法，流程图如图1所示。具体实施步骤如下：
37.一种采摘机械臂抓取运动规划方法，包括以下步骤：
38.步骤1、采摘机器人实验设备的搭建，实验设备一共由两部分组成，采摘机器人和预采摘平面。甜椒采摘过程中，预采摘平面把机械臂的运动规划分为两个部分，如图2所示，一个是离线规划部分，一个在线规划部分。由于甜椒的果梗在整个图像中像素点比较少，在甜椒的识别上也由两部分组成，在初始位置以广视角通过双目视觉相机1进行初识别，到达预采摘平面上的预采摘点后，通过双目视觉相机2近视角下进行精确识别。双目视觉相机1安装在如图3所示的位置，双目视觉相机1主要负责果实位置的初识别，相机1的相机坐标系为camera1_frame，通过相机标定(眼在手外)确定双目视觉相机1相机坐标系camera1_
frame与机械臂基坐标系base_link的转换关系。
39.通过(x,y,z,yaw1,pitch1,roll1)表示相机坐标系camera1_frame和机械臂基坐标系base_link的转换关系。(x,y,z,yaw1,pitch1,roll1)表示将base_link坐标系沿x轴旋转yaw1，转沿y轴旋转pitch1，沿z轴旋转roll1，再以base_link坐标系进行平移，沿x轴平移x，沿y轴平移y，沿z轴平移z。公式(1)表示将所获得的甜椒三维坐标从相机坐标系camera1_frame转换到机械臂基坐标系base_link下，用base_linkp表示。r
camera1_base_link
为从双目视觉相机1坐标系到机械臂基坐标系的旋转矩阵，t
camera1_base_link
为相机1坐标系到机械臂基坐标系的平移坐标。
40.base_link
p＝r
camera1_base_link
·
camera1_frame
p+t
camera1_base_link
ꢀꢀꢀ
(1)
41.将双目视觉相机2安装在六自由度优傲universalrobot机械臂末端上。通过手眼标定确定相机坐标系camera2_frame和机械臂末端坐标系tool0的转换关系。通过(x1,y1,z1,yaw,pitch,roll)表示相机坐标系camera2_frame和机械臂末端坐标系tool0的转换关系。(x1,y1,z1,yaw,pitch,roll)表示将tool0坐标系沿x轴旋转yaw，转沿y轴旋转pitch，沿z轴旋转roll，再以tool0坐标系进行平移，沿x轴平移x1，沿y轴平移y1，沿z轴平移z1。启动双目视觉相机和六自由度优傲机械臂，并加载机械臂和相机的urdf模型，该模型描述了机械臂的各个关节的位置姿态以及和相机坐标位置的转换关系。如图3所示，定义机械臂的基坐标系为base_link，相机2的坐标系为camera2_frame，机械臂末端坐标系为tool0，利用平移和旋转确定相机坐标系camera2_frame和机械臂基坐标的位置关系。公式(2)表示将所获得的甜椒三维坐标从相机2坐标系camera2_frame转换到机械臂末端坐标系tool0下，用
tool0
p表示。r
camera2_tool0
为从相机2坐标系到机械臂末端坐标系的旋转矩阵，t
camera2_tool0
为相机2坐标系到机械臂末端坐标系的平移坐标。公式(3)表示将从相机2所获得的甜椒三维坐标从机械臂末端坐标系tool0转换到机械臂基坐标系base_link下，
base_link
p表示以机械臂基坐标系base_link为参考系下甜椒的位置。r
tool0_base_link
为从机械臂末端坐标系到机械臂基坐标系的旋转矩阵，t
tool0_base_link
为机械臂末端坐标系到机械臂基坐标系的平移坐标。
42.tool0
p＝r
camera2_tool0
·
camera2_frame
p+t
camera2_tool0
ꢀꢀꢀ
(2)
43.base_link
p＝r
tool0_base_link
·
tool0
p+t
tool0_base_link
ꢀꢀꢀ
(3)
44.步骤2、基于强化学习进行离线学习，基于ppo强化学习算法控制机械臂以特定姿态到达预采摘平面，学习后的模型用于离线规划。真实环境搭建如图2所示，真实环境中预采摘平面中箭头的方向表示机械臂末端的抓取姿态，与预采摘平面垂直，箭头与采摘平面的交点为机械臂到达预采摘平面的位置，一个箭头所指的位置可以用(x,y,z,α,β,γ)表示，包含了机械臂末端要到达的位置以及抓取姿态。搭建仿真环境，在pybullet仿真环境中进行训练，仿真环境如图4所示。图中的白色实线为预定义的包围盒，机械臂训练过程中末端离开包围盒，给予一个负的奖励，训练终止，重新开始。图中的物体表示机械臂训练过程中的目标点，机械臂到达物体目标点的抓取姿态为(α,β,γ)，预采摘平面上的训练点通过函数f
random
(x,y,z)产生，函数f
random
(x,y,z)表示以机械臂基坐标系为参考系的预采摘平面上的位置坐标(x,y,z)，random(a,b)函数表示范围在a和b之间的随机数。
45.46.采用ppo强化学习算法控制机械臂以特定姿态到达预采摘平面，机械臂在学习过程中奖励函数定义如下：机械臂到达目标点时给予一个正的奖励，末端超出提前预定义的包围盒给予一个负的奖励，到达了机械臂运动的最大步数但是还未到达目标点给予一个负的奖励。通过计算机械臂末端的坐标和目标点的坐标的距离作为奖励函数的依据。机械臂当前状态下末端的坐标为(current.x,current.y,current.z)，目标点的坐标为(target.x,target.y,target.z)。通过公式8表示奖励函数的依据，在distance小于等于1，即机械臂末端到达以目标点为中心半径为1cm的球内，则认为机械臂到达了目标点。
47.sruare_dx＝(current.x-target.x)2ꢀꢀꢀ
(5)
48.sruare_dy＝(current.y-target.y)2ꢀꢀꢀ
(6)
49.sruare_dz＝(current.z-target.z)2ꢀꢀꢀ
(7)
50.distance＝sqrt(sruare_dx+square_dy+square_dz)
ꢀꢀꢀ
(8)
51.步骤3、采摘环境下，对甜椒果实的位置进行初识别。订阅双目视觉相机1的rgb图像，运用深度学习yolov识别果实的位置。在ros系统中配置darknet_ros，收集甜椒的数据集，通过标注软件进行标注，然后使用yolov进行训练，在ros系统中同时订阅相机的深度图像和rgb对齐的话题，采用双目相机进行图像的采集，图像分辨率为640*480，运行darknet_ros中的yolov进行甜椒的识别，ros系统会返回甜椒的识别的结果，会框选出甜椒的位置，会得到识别矩形框在640*480的像素位置，如图5中甜椒果实识别框a所示，矩形框的四个角代表的点的像素坐标为a1(x1,y1)，a2(x2,y2)，a3(x3,y3)，a4(x4,y4)，通过四个像素位置我们可以求出果实甜椒的中心位置c(xi,yi)，通过公式9求得：
52.根据甜椒果实的识别框求出甜椒果实中心的像素点，当深度学习算法识别到甜椒果实的个数为n时，通过公式9计算会得到n个甜椒果实中心的像素点坐标用c(xi,yi),i∈n表示，与双目视觉相机的深度图像进行匹配会得到双目视觉相机的世界坐标系下对应的坐标pi＝[xi,yi,zi],i∈n通过公式10求出。
[0053][0054]
式中(xi,yi)表示深度图像的像素坐标，z表示双目相机的深度值，f是相机的焦距，dx，dy分别表示横向单位像素和纵向单位像素所对应的物理距离，单位为mm，u0，v0分别为横向像素中点和纵向像素中点。
[0055]
步骤4、根据步骤3初识别的结果pi＝[xi,yi,zi],i∈n求出投影到预采摘平面上的点的位置。求出n个果实投影到预采摘平面上的坐标，把这些坐标位置作为机械臂的预到达的点，如示意图6所示。当有n个点投影到采摘平面上时，预采摘平面上的点记为ai(i＝1...n)。机械臂的初始位置双目视觉相机的世界坐标系下z轴与预采摘平面垂直，距离为d
l
，因此相机识别到的甜椒果实投影到预采摘平面上的位置为ai＝[xi,yi,dl],i∈n，通过
相机坐标系camera1_frame和机械臂基坐标系base_link的转换关系，求出所有预采摘点相对于机械臂基坐标系末端base_link的位置。
[0056]
base_link
pi(xi,yi,zi)＝r
camera1_base_link
·ai
(xi,yi,dl)+t
camera1_base_link
(i∈n)
ꢀꢀꢀ
(11)
[0057]
步骤5通过预采摘点的位置，规划机械臂的最优采摘顺序。加载步骤2中强化学习离线规划训练的文件，输入预采摘点的位置坐标，求出机械臂末端tool0到达预采摘点ai,i∈n的轨迹。机械臂在规划过程中由两部分组成，到达预采摘平面和二次规划进行采摘。机械臂在采摘过程中需要先到达预采摘点，然后进行进行二次感知识别进行采摘。机械臂在进行在线规划过程中无论采用何种采摘顺序，机械臂完成采摘消耗的能量成本是一样的。在离线规划阶段，机械臂经过预采摘点顺序不一样，机械臂消耗的能量成本是不一样的。为了减少机械臂采摘过程的能量成本的消耗，对离线规划部分和在线规划部分进行建模，构建能量成本函数。在线规划部分的成本用zc表示，离线阶段的规划成本用mc表示，机械臂从ai点运动到a
i+1
时经过的轨迹用公式12中的fc(ai,a
i+1
)函数表示，公式12中θn是一个6维的向量，表示机械臂的6个关节的角度位置。机械臂运动过程中各关节角度变化之和表示成本函数，机械臂通过各个预采摘点的关节角度变化之和越小，消耗的能量成本越低，函数d(fc(ai,a
i+1
))表示机械臂从ai点运动到a
i+1
时关节角度变化之和。
[0058]
fc(ai,a
i+1
)＝{θ1，θ2…
θn}
ꢀꢀꢀ
(12)
[0059][0060]
d(fc(ai,a
i+1
))表示机械臂从ai到a
i+1
的能量成本消耗，用tc表示能量成本消耗的总和。用公式14表示
[0061][0062]
如图7所示，机械臂完成当前视角全部果实的采摘所消耗的能量可以建模成旅行商问题，机械臂从开始位置出发，依次通过全部预采摘点最后回到开始的位置，机械臂各关节角度变化之和最小即是最优的采摘顺序，所消耗的能量最小。通过遗传算法对最优采摘顺序进行求解，计算出机械臂到达预采摘点的顺序。
[0063]
步骤6、控制机械臂按照规划的最优采摘顺序依次到达预采摘点，然后进行二次识别和在线规划进行采摘，如图8所示。二次识别过程基于近视角下通过相机2来完成。过程如下，机械臂到达预采摘点时进行甜椒果梗的识别，订阅双目视觉相机2的rgb图像，通过深度学习算法yolov识别出甜椒果实的位置，我们通过获取的果实位置进行推理，根据甜椒果实的位置，求出甜椒果梗的感兴趣区域，如图5识别框b所示。详细步骤如下，通过最小外接矩形框出甜椒果实的位置，求出甜椒果实的质心，通过甜椒果实的质心向上推理出甜椒果梗的感兴趣区域识别框b，通过识别框b得到甜椒果梗矩形框在640*480的像素位置。甜椒梗的识别框b通过下列方式求出，果实识别框a的长度保持不变，向上平移得到甜椒果梗区域b，如图5所示。框选区域b包括甜椒的果实、叶子、茎，和甜椒的果梗。然后通过轻量型实例分割网络yolact网络识别出甜椒果梗区域，得到采摘点。yolact网络识别甜椒果梗的示意图如图9所示。
[0064]
步骤7、到达采摘点完成甜椒的采摘。通过识别到的采摘点在线规划进行果实的采
摘，控制机械臂完成甜椒的采摘。然后控制机械臂到达下一预采摘点重复步骤7进行采摘，直到所有预采摘点遍历完毕。进入下一采摘地点，重复以上步骤进行采摘。
[0065]
在gazebo仿真环境中对本实施例算法进行了仿真对比实验，对比算法为不采用离线规划算法和不规划采摘顺序，以采摘成功率和采摘单个果实的平均时间作为评价指标，对比算法在识别到果实的位置后，以特定姿态先到达果实前方15cm处识别果梗，然后进行采摘，采摘完后回到初始位置。重复这个过程直到果实采摘完毕，果实的采摘顺序是随机的。仿真过程中设置了8个采摘果实，设置了10组实验，在10组实验过程中果实的位置是随机的。实验结果显示对比算法的采摘成功率为77％，单个果实的采摘时间为22s，采用离线规划和规划采摘顺序采摘成功率为92％，单个果实的采摘时间为16s。通过对比实验得出采摘成功率提高15％，采摘效率提高27.3％。实验表明采用离线规划和规划采摘顺序能够显著提高采摘机器人的成功率和采摘效率。