一种类球形果实采摘机械臂的抓取路径控制方法技术

一种类球形果实采摘机械臂的抓取路径控制方法，该方法解决了采摘机械臂逆解无解和所规划路径末端夹爪碰撞果实的问题，利用类球形果实可以以多种姿态完成抓取的优势，在所求取的机械臂末端姿态不能解出机械臂逆解时，使用遗传算法求解可到达果实位置的机械臂末端姿态，并通过移动采摘机器人位置进行多次求解，以提高机械臂逆解求取成功率，进而提高采摘成功率；提出了在采摘机器人采摘方向上添加插值点的方法，使采摘机器人抓取时沿着末端执行器开口朝向果实的方向到达果实位置，以提高采摘机器人的采摘成功率和降低果实损坏率。机器人的采摘成功率和降低果实损坏率。机器人的采摘成功率和降低果实损坏率。

【技术实现步骤摘要】
一种类球形果实采摘机械臂的抓取路径控制方法


[0001]本专利技术涉及到机器人应用
，涉及一种类球形果实采摘机械臂的抓取路径控制方法。

技术介绍

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

技术实现思路

[0005]为了解决上述采摘机械臂逆解无解和所规划路径末端夹爪碰撞果实的问题，利用类球形果实可以以多种姿态完成抓取的优势，在所求取的机械臂末端姿态不能解出机械臂逆解时，使用遗传算法求解可到达果实位置的机械臂末端姿态，并通过移动采摘机器人位置进行多次求解，以提高机械臂逆解求取成功率，进而提高采摘成功率；提出了在采摘机器人采摘方向上添加插值点的方法，使采摘机器人抓取时沿着末端执行器开口朝向果实的方向到达果实位置，以提高采摘机器人的采摘成功率和降低果实损坏率。
[0006]采摘机器人运用深度学习算法对果实进行识别，并利用深度相机采集到的果实信息对果实位置进行定位。所述采摘机器人使用软体末端执行器进行果实抓取，使用转动机械臂末端执行器的方法拧断果梗实现果梗分离。采摘机器人在完成果梗分离后将采到的果
实放到收获筐中，并回到初始位置继续进行识别和抓取。机械臂进行采摘时，其必须接收到目标果实的位置，以及末端执行器的姿态才能够求取逆解。在对非球形果实，如茄子、黄瓜等进行抓取时，由于其外形特征的影响，抓取时必须要以垂直于果实中心线的方向进行抓取，因此对其进行抓取除了要找到其中心点的三维空间坐标，还要找到适合进行抓取的末端执行器姿态才能够完成采摘。而对于类球形果实，如苹果、番茄等，在进行抓取时只要给定精确的果实位置中心点坐标，机械臂末端在保证避障的前提下可以以多种姿态对目标果实进行采摘。
[0007]本专利技术利用类球形果实可以以多种姿态进行抓取采摘的特点，提出了一种自适应的采摘机器人末端姿态计算方法，并且在所求取的机械臂末端姿态不能解出机械臂逆解时，使用遗传算法求解可到达果实位置的机械臂末端姿态，保证采摘机器人采摘成功率。
[0008]第一步，采摘机器人末端执行器抓取姿态计算；
[0009]所述采摘机器人采用移动轮式机器人平台安装六自由度采摘机械臂、采摘机械臂末端安装深度相机的形式进行采摘。
[0010]采摘机器人在到达采摘位置后，深度相机对环境进行感知和识别，并得到在相机坐标系下的果实中心点三维位置坐标a
c
(x
a0
,y
a0
,z
a0
)；相机坐标系下的果实中心点坐标a
c
可以经过由相机与机械臂标定得到的旋转四元数q1进行旋转转换为机械臂末端坐标系下的坐标a
t
，如式(1)所示：a
t
＝q1×
a
c
(1)
[0011]则相机坐标系下的果实中心点三维坐标a
c
(x
a0
,y
a0
,z
a0
)经过转换，转换为机械臂末端坐标系下的坐标a
t
(x
a
,y
a
,z
a
)。
[0012]在机械臂末端坐标系下计算果实中心点坐标a
t
(x
a
,y
a
,z
a
)和末端坐标系原点t(0,0,0)之间的方向向量a0，如式(2)所示：a0＝(x
a
,y
a
,z
a
)(2)
[0013]并对其进行单位化得到单位方向向量a，如式(3)所示：
[0014]机械臂末端执行器姿态由末端坐标系与机械臂底座坐标系之间的旋转四元数表示，以z轴为末端执行器的中心轴方向。在末端坐标系下，末端坐标系的z轴单位向量为z(0,0,1)，要想使机械臂末端执行器姿态方向与方向向量a相同，对z(0,0,1)进行旋转，使其方向与方向向量a相同，求取z旋转为a的旋转四元数q2，如式(4)所示：
[0015]式中：θ为z与a的夹角。u为z和a相垂直的方向向量。
[0016]为了方便求取和设h为z和a之间处的中间向量，则h如式(5)所示：h＝(z+a)/|z+a|(5)
[0017]则有可求得机械臂末端坐标系z轴旋转到旋转向量a的四元数q2由式(6)所示：q2＝a
·
h+a
ⅹ
h(6)
[0018]通过ROS读取当前末端坐标系与机械臂底座基坐标系的旋转四元数q3，则机械臂末端姿态q由q3左乘q2获得，如式(7)所示：
[0019]第二步，求取路径插值点
[0020]采摘机器人采用六自由度采摘机械臂进行果实抓取，在机械臂进行抓取时需要进行路径规划，如果不添加任何约束，则采摘机械臂可能从任何方向靠近果实，但是末端执行器的存在决定了采摘机器人只能从末端执行器开口朝向果实的方向到达果实位置进行抓取，否则机械臂从其他方向靠近果实都将导致末端执行器碰撞果实，不能正确抓取果实甚至碰撞果实造成破坏。提出了一种基于添加插值点的方法来约束机械臂，使得机械臂所规划路径能够沿着末端执行器开口朝向果实的方向靠近果实，避免末端执行器与果实发生碰撞。
[0021]在所求得末端执行器姿态q方向上接近果实位置处添加插值点b(插值点本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种类球形果实采摘机械臂的抓取路径控制方法，其特征在于以下步骤，第一步，采摘机器人末端执行器抓取姿态计算；所述采摘机器人采用移动轮式机器人平台安装六自由度采摘机械臂、采摘机械臂末端安装深度相机的形式进行采摘；采摘机器人在到达采摘位置后，深度相机对环境进行感知和识别，并得到在相机坐标系下的果实中心点三维位置坐标a
c
(x
a0
,y
a0
,z
a0
)；相机坐标系下的果实中心点坐标a
c
经过由相机与机械臂标定得到的旋转四元数q1进行旋转转换为机械臂末端坐标系下的坐标a
t
，如式(1)所示：a
t
＝q1×
a
c
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(1)则相机坐标系下的果实中心点三维坐标a
c
(x
a0
,y
a0
,z
a0
)经过转换，转换为机械臂末端坐标系下的坐标a
t
(x
a
,y
a
,z
a
)；在机械臂末端坐标系下计算果实中心点坐标a
t
(x
a
,y
a
,z
a
)和末端坐标系原点t(0,0,0)之间的方向向量a0，如式(2)所示：a0＝(x
a
,y
a
,z
a
)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(2)并对其进行单位化得到单位方向向量a，如式(3)所示：机械臂末端执行器姿态由末端坐标系与机械臂底座坐标系之间的旋转四元数表示，以z轴为末端执行器的中心轴方向；在末端坐标系下，末端坐标系的z轴单位向量为z(0,0,1)，要想使机械臂末端执行器姿态方向与方向向量a相同，对z(0,0,1)进行旋转，使其方向与方向向量a相同，求取z旋转为a的旋转四元数q2，如式(4)所示：式中：θ为z与a的夹角；u为z和a相垂直的方向向量；为求取和设h为z和a之间处的中间向量，则h如式(5)所示：h＝(z+a)/|z+a|
ꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(5)则有求得机械臂末端坐标系z轴旋转到旋转向量a的四元数q2由式(6)所示：q2＝a
·
h+a
ⅹ
h
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(6)通过ROS读取当前末端坐标系与机械臂底座基坐标系的旋转四元数q3，则机械臂末端姿态q由q3左乘q2获得，如式(7)所示：第二步，求取路径插值点在所求得末端执行器姿态q方向上接近果实位置处添加插值点b来保证所规划路径能够使采摘机器人沿着末端执行器开口方向靠近、包裹并抓取果实，避免末端执行器从侧面靠近果实与果实发生碰撞；...

【专利技术属性】
技术研发人员：王东，董永祥，连捷，王伟，
申请(专利权)人：大连理工大学，
类型：发明
国别省市：