1.本发明涉及到机器人应用技术领域，涉及一种类球形果实采摘机械臂的抓取路径控制方法。


背景技术：

2.随着植物大棚、植物工厂的规模庞大化以及人力成本的提高，果蔬采摘与收获的智能化成为各国农业的迫切需求。伴随着计算机、图像处理等领域的技术突破，果蔬采摘机器人得到了迅速的发展。如美国华盛顿大学研发的苹果采摘机器人、挪威生命科学大学开发的草莓采摘机器人、以色列内盖夫本哈古里安大学开发的应用于商业温室中的甜椒采摘机器人等。采摘机器人技术日趋成熟并逐渐向商用方向发展。
3.六自由度机械臂是最常用的采摘机械臂之一。机械臂包括主要工作空间、灵活工作空间和非灵活工作空间。灵活工作空间和非灵活工作空间是主要工作空间的子集，两者互不相交并共同构成了主要工作空间。六自由度机械臂的末端执行器可以到达主要工作空间中的任意一点，但是其不能以任意末端姿态到达非灵活工作空间中的任意一点，只能以部分姿态到达非灵活工作空间中的点。在进行果实抓取时，如果只以一种固定算法求取一个机械臂末端姿态并进行逆解计算，则经常会出现抓取处于非灵活工作空间中果实无解的情况，导致采摘机器人无法实现抓取。由于采摘空间狭小，通常使用小型机械臂进行采摘，而小型机械臂的非灵活工作空间占主要工作空间比重很大，为了提高采摘成功率，必须解决非灵活工作空间中果实使用固定抓取姿态的抓取时逆解无解的问题。而采摘机器人逆解的求取又必须先得到末端执行器的末端姿态和末端位置才能够进行，必须找到合适的机械臂末端姿态使机械臂能够完成抓取。
4.另一方面，对于使用六自由度机械臂进行采摘的采摘机器人在采摘路径规划过程中，如果只给定目标果实的位置和姿态，而不添加其他约束，那么机械臂末端执行器可能从果实的侧面等其他方向而非末端执行器开口朝向果实的方向到达果实位置，这样就会导致末端执行器碰撞果实，致使采摘失败甚至果实掉落损坏。


技术实现要素：

5.为了解决上述采摘机械臂逆解无解和所规划路径末端夹爪碰撞果实的问题，利用类球形果实可以以多种姿态完成抓取的优势，在所求取的机械臂末端姿态不能解出机械臂逆解时，使用遗传算法求解可到达果实位置的机械臂末端姿态，并通过移动采摘机器人位置进行多次求解，以提高机械臂逆解求取成功率，进而提高采摘成功率；提出了在采摘机器人采摘方向上添加插值点的方法，使采摘机器人抓取时沿着末端执行器开口朝向果实的方向到达果实位置，以提高采摘机器人的采摘成功率和降低果实损坏率。
6.采摘机器人运用深度学习算法对果实进行识别，并利用深度相机采集到的果实信息对果实位置进行定位。所述采摘机器人使用软体末端执行器进行果实抓取，使用转动机械臂末端执行器的方法拧断果梗实现果梗分离。采摘机器人在完成果梗分离后将采到的果
实放到收获筐中，并回到初始位置继续进行识别和抓取。机械臂进行采摘时，其必须接收到目标果实的位置，以及末端执行器的姿态才能够求取逆解。在对非球形果实，如茄子、黄瓜等进行抓取时，由于其外形特征的影响，抓取时必须要以垂直于果实中心线的方向进行抓取，因此对其进行抓取除了要找到其中心点的三维空间坐标，还要找到适合进行抓取的末端执行器姿态才能够完成采摘。而对于类球形果实，如苹果、番茄等，在进行抓取时只要给定精确的果实位置中心点坐标，机械臂末端在保证避障的前提下可以以多种姿态对目标果实进行采摘。
7.本发明利用类球形果实可以以多种姿态进行抓取采摘的特点，提出了一种自适应的采摘机器人末端姿态计算方法，并且在所求取的机械臂末端姿态不能解出机械臂逆解时，使用遗传算法求解可到达果实位置的机械臂末端姿态，保证采摘机器人采摘成功率。
8.第一步，采摘机器人末端执行器抓取姿态计算；
9.所述采摘机器人采用移动轮式机器人平台安装六自由度采摘机械臂、采摘机械臂末端安装深度相机的形式进行采摘。
10.采摘机器人在到达采摘位置后，深度相机对环境进行感知和识别，并得到在相机坐标系下的果实中心点三维位置坐标ac(x
a0
,y
a0
,z
a0
)；相机坐标系下的果实中心点坐标ac可以经过由相机与机械臂标定得到的旋转四元数q1进行旋转转换为机械臂末端坐标系下的坐标a
t
，如式(1)所示：a
t
＝q1×ac
(1)
11.则相机坐标系下的果实中心点三维坐标ac(x
a0
,y
a0
,z
a0
)经过转换，转换为机械臂末端坐标系下的坐标a
t
(xa,ya,za)。
12.在机械臂末端坐标系下计算果实中心点坐标a
t
(xa,ya,za)和末端坐标系原点t(0,0,0)之间的方向向量a0，如式(2)所示：a0＝(xa,ya,za)(2)
13.并对其进行单位化得到单位方向向量a，如式(3)所示：
14.机械臂末端执行器姿态由末端坐标系与机械臂底座坐标系之间的旋转四元数表示，以z轴为末端执行器的中心轴方向。在末端坐标系下，末端坐标系的z轴单位向量为z(0,0,1)，要想使机械臂末端执行器姿态方向与方向向量a相同，对z(0,0,1)进行旋转，使其方向与方向向量a相同，求取z旋转为a的旋转四元数q2，如式(4)所示：
15.式中：θ为z与a的夹角。u为z和a相垂直的方向向量。
16.为了方便求取和设h为z和a之间处的中间向量，则h如式(5)所示：h＝(z+a)/|z+a|(5)
17.则有可求得机械臂末端坐标系z轴旋转到旋转向量a的四元数q2由式(6)所示：q2＝a
·
h+a
ⅹ
h(6)
18.通过ros读取当前末端坐标系与机械臂底座基坐标系的旋转四元数q3，则机械臂末端姿态q由q3左乘q2获得，如式(7)所示：
19.第二步，求取路径插值点
20.采摘机器人采用六自由度采摘机械臂进行果实抓取，在机械臂进行抓取时需要进行路径规划，如果不添加任何约束，则采摘机械臂可能从任何方向靠近果实，但是末端执行器的存在决定了采摘机器人只能从末端执行器开口朝向果实的方向到达果实位置进行抓取，否则机械臂从其他方向靠近果实都将导致末端执行器碰撞果实，不能正确抓取果实甚至碰撞果实造成破坏。提出了一种基于添加插值点的方法来约束机械臂，使得机械臂所规划路径能够沿着末端执行器开口朝向果实的方向靠近果实，避免末端执行器与果实发生碰撞。
21.在所求得末端执行器姿态q方向上接近果实位置处添加插值点b(插值点：机械臂执行动作时末端必须经过的点)来保证所规划路径能够使采摘机器人沿着末端执行器开口方向靠近、包裹并抓取果实，避免末端执行器从侧面靠近果实与果实发生碰撞。在末端坐标系下，令插值点b与果实位置a(xa,ya,za)之间的距离为1.5r，r为所采摘果实的平均半径，则机械臂进行抓取时必将先以末端执行器开口朝向果实的方向经过果实前方1.5r处的b点，后到达最终采摘点即果实中心点位置a(xa,ya,za)，避免了末端执行器从其他方向靠近果实与果实发生碰撞。在机械臂末端坐标系下的插值点b在果实位置a之前1.5r处，方向与机械臂末端姿态方向向量a(xa,ya,za)相同，设插值点b到末端坐标系坐标原点t(0,0,0)的向量为b，k为缩放系数，则由式(8)和式(9)求得b。
[0022][0023][0024]
其中|a|为向量a的模，r为所采摘果实的平均半径。
[0025]
求得向量b后即可得b点在末端坐标系的坐标通过采摘机械臂底座基坐标系与末端坐标系的转换四元数q3，将b
t
转换到机械臂底座坐标系中，如式(10)所示：
[0026]bc
＝q3×bt
ꢀꢀ
(10)
[0027]
其中bc为插值点b在机械臂底座基坐标系下的坐标。添加果实中心点位置ab、末端执行器末端姿态q和插值点b，使用解析法求取机械臂关节角，如果逆解存在，则机械臂执行抓取，如果逆解不存在，则执行第三步。
[0028]
第三步，采摘机器人末端姿态循环搜索
[0029]
类球形果实的抓取可以以任何姿态进行抓取，但是机械臂的逆解求取必须先确定末端姿态，因此，只要找到能够使机械臂能够到达采摘点的姿态即可完成采摘。如果机械臂以姿态q进行抓取时逆解不存在，则使用遗传算法搜索能够使机械臂到达果实位置存在逆解的姿态。将果实简化为球体，则其可行抓取姿态必定是朝向采摘机器人一面的半球，而所求取末端执行器抓取姿态q为机械臂朝向果实一面的中心，因此，只需要求取以末端执行器末端姿态q为中心半球面内的抓取姿态。将所求得末端执行器姿态四元数q转换为欧拉角(θ，ψ，)，则设置种群规模为20-30，每个个体变化范围为(θ-90，θ+90)，(ψ-90，ψ+90)，适应度为添加插值点后所求出逆解关节角的个数，求出的逆解关节
角越多，则个体适应度越高。经过1000-2000次迭代，如果求出存在六个关节逆解的末端姿态，则停止迭代，进行抓取，如果未找到逆解，则进行第四步。
[0030]
第四步，采摘机器人位置移动
[0031]
根据所使用采摘机械臂工作空间进行移动距离计算，以工作空间为球体，半径为n的六自由度采摘机械臂为例。首先计算目标果实与采摘机器人之间的距离，如果果实中心点位置ab距离采摘机械臂底座基坐标系原点0.6n以上，则令移动机器人向靠近果实方向行进0.1n并执行第一步。如果果实中心点位置ab距离采摘机械臂底座基坐标系原点0.6n及以下，则令移动机器人向远离果实方向行进0.1n并执行第一步。如果还未能实现抓取，则放弃采摘此果实。
[0032]
本发明利用球形果实可使用多种姿态进行抓取的优势，根据采摘机器人机械臂末端位置和目标果实计算合适的抓取姿态，并且在所求取的机械臂末端姿态不能解出机械臂逆解时，使用遗传算法求解可到达果实位置的机械臂末端姿态，并通过移动采摘机器人位置进行多次求解，提高了采摘机械臂逆解求取成功率，从而提高了采摘成功率。使用在果实中心点前方添加插值点的方法，保证采摘机器人以开口朝向果实的方向靠近果实，避免了采摘机器人从侧面靠近果实与果实发生碰撞导致的抓取失败和果实损坏，提高了采摘成功率并降低了果实损坏率。
附图说明
[0033]
图1为采摘流程图。
[0034]
图2为采摘机器人移动规则图。
[0035]
图3为采摘机器人坐标系关系图。
[0036]
图4a为机械臂正在从侧面靠近果实示意图。
[0037]
图4b为机械臂从侧面到达果实位置后发生碰撞导致抓取失败示意图。
[0038]
图5a为机械臂正以末端执行器开口朝向果实的方向靠近果实示意图。
[0039]
图5b为机械臂末端执行器到达果实中心点位置并完成抓取示意图。
具体实施方式
[0040]
下面结合附图，对本发明进行详细的说明。
[0041]
1.应用类球形果实采摘机器人抓取控制方法进行果实采摘的流程图如图1所示。采摘机器人在到达采摘位置后，对环境进行感知，深度相机对环境信息进行采集并使用深度学习算法对果实进行识别和定位。对识别到的果实进行编号和计数，如果此果实经过少于2次的抓取计算，则将果实位置信息从相机坐标系转换到机械臂末端坐标系下，并根据果实中心点位置和末端坐标系原点的相对位置求取机械臂末端姿态，以机械臂末端坐标系的原点到果实中心点位置的向量为方向向量a，求取末端坐标系的z轴旋转至方向向量a的四元数并右乘坐标系底座基坐标系至末端坐标系的旋转四元数，所得四元数即所求机械臂末端姿态。在求得机械臂末端姿态后，求取方向向量a上距离果实中心点1.5r处的插值点，并将插值点转换到底座基坐标系中。添加果实中心点位置ab、末端执行器姿态q和插值点b，使用解析法求取机械臂关节角，如果逆解存在，则机械臂执行抓取，如果逆解不存在，则使用遗传算法求取具有逆解的末端姿态，如果找到了能够使逆解存在的末端姿态，则机械臂执
行抓取，如果没有找到，则移动采摘机器人，以工作空间为球体，半径为n的六自由度采摘机械臂为例，如图二所示为移动采摘机器人规则，当果实中心点位置距离采摘机械臂底座基坐标系的距离大于0.6n，则采摘机器人朝向果实方向移动0.1n，当果实中心点位置距离采摘机械臂底座基坐标系原点的距离不大于0.6n，则采摘机器人远离果实方向移动0.1n。在采摘机器人完成移动后进行果实感知和定位，并判断此果实的计算次数，如果不小于2次则放弃抓取此果实。在对该采摘区域内的果实均进行过采摘或至少2次的尝试抓取后，前往下一采摘点进行采摘。
[0042]
2.如图3所示，采摘机械臂的三个常用坐标系：采摘机械臂底座的基坐标系、末端执行器的末端坐标系、深度相机的相机坐标系。此时采摘机械臂处于初始位置，采摘机械臂末端姿态即末端坐标系的姿态，末端执行器的开口方向即末端坐标系的z轴方向。要想实现采摘机器人自主抓取，首先通过深度相机识别和定位果实，获取在相机坐标系下的果实位置，然后通过相机坐标系与机械臂末端坐标系的旋转四元数进行转换，如式(1)所示，将果实位置坐标从相机坐标系下转换到机械臂末端坐标系下。此时在机械臂末端坐标系下构建方向向量a，如图三所示，方向向量a由机械臂末端坐标系原点指向果实中心点，以向量a的方向进行果实抓取，计算机械臂末端坐标系的z轴转换到方向向量a的方向，如式(4)-(6)所示。最后，将求得的旋转四元数与基于机械臂底座基坐标系下的机械臂末端当前位置四元数相乘，如式(7)所示，即可求得最终的机械臂末端姿态四元数。
[0043]
3.采摘机械臂在进行抓取时，如果不添加约束，那么其将可能以任何可能的方式靠近目标果实，如图4所示，图4a为机械臂正在从侧面靠近果实，图4b为机械臂从侧面到达果实位置后发生碰撞导致抓取失败。因此本发明提出了一种通过添加插值点的方法来约束机械臂所规划的路径。如图3所示，方向向量a已由式(2)-(3)求出，在方向向量a上距离果实中心点位置1.5r处添加插值点b，且在插值点b处采摘机器人末端姿态保持抓取姿态q，这样既可保证机械臂可以由末端执行器开口方向抓取果实，避免碰撞而导致采摘失败。添加插值点进行抓取如图5所示，图5a为机械臂正以末端执行器开口朝向果实的方向靠近果实，图5b为机械臂末端执行器到达果实中心点位置并完成抓取。