基于视觉定位的串并混联型百香果采摘机器人研究*

张日红，欧炬基，丁力行，李小敏，林桂潮，钟建鸣

(仲恺农业工程学院机电工程学院,广州市,510550)

0 引言

百香果广泛种植于热带和亚热带地区,在中国主要分布在广东、福建和广西等南方省份,大部分用于自然消费和工业加工[1],目前百香果采摘主要以人工作业为主,这种方式耗时长和成本高昂,不能满足现代农业的发展需求[2]。传统百香果种植方式多为平顶棚架,这种种植方式枝条杂乱,相互缠绕,容易滋生病虫害。双层垂帘式高密度种植采用的是立体的搭架种植方式,便于管理、采光充足、病害少、产量高[3-4],适合智能机器人采摘作业。

随着计算机图像处理技术、人工智能和电子技术的发展和成熟,经过国内外众多学者研究,各类不同的采摘机器人的样机研制与方法研究都有了重大突破。在样机研制方面,如苹果采摘机器人、番茄采摘机器人、猕猴桃采摘机器人、甜椒采摘机器人等[5-8],这些研究都极大地促进了采摘机器人的迅速发展。在方法研究方面,Kuznetsova等[9]提出一种经过预处理和后处理技术使YOLOv3算法,能够应用于苹果和橙子收获机器人机器视觉系统。Lü等[10]对分割出来的苹果利用边缘检测和改进的RHT变换对苹果进行识别,识别结果能基本满足机器人的采摘要求。宁政通等[11]相续使用改进的Mask R-CNN网络进行粗分割和区域生长法进行细分割的处理方法,提取果梗质心预测葡萄采摘点。张勤等[12]通过YOLOv4目标检测算法获取感兴趣区域,再融合RGB-D图像中的深度信息和颜色特征提取番茄串采摘点,耗时54 ms。

随着技术的不断发展,国内针对百香果采摘机器人的研究也具有较多研究基础。在机械采摘方面,如黄才贵等[13-14]设计了百香果采摘末端执行机构,和基于机器视觉的百香果采摘分级机器人平台。在百香果的检测方面,林营志等[15]利用YOLOv3深度卷积神经网络建立了复杂背景下的百香果果实定位模型,唐熔钗等[16]通过增强特征提升了模型对百香果小目标的识别能力,张展榜等[17]通过优化先验框尺寸与非极大抑制算法提高了模型在不同密集场景下的适应性和检测能力。Tu等[18]针对自然复杂生长场景下的百香果,在Faster R-CNN目标检测算法的基础上提出了一种多尺度的MS-FRCNN网络优化模型,提升了对百香果小目标的检测能力,涂淑琴等[19]运用FPN+ResNet-101网络模型对百香果目标进行了快速识别。综上所述,国内外面向百香果智能采摘的研究主要集中在机构设计和识别定位层面,针对百香果智能采摘技术的技术集成和试验应用研究甚少。因此,本文以垂帘式种植工艺的百香果为研究对象,综合运用深度神经网络设计一个融合深度信息的百香果采摘点识别定位系统。在此基础上,研制一款具备四自由度的串并混联型百香果采摘机器人,并进行运动学模型分析和手眼标定试验。

1 百香果采摘机器人设计

1.1 机器人系统组成

本文基于垂帘棚架式百香果种植场景,研制了一种串并混联型百香果采摘机器人,如图1所示。该机器人由串并混联型机械臂、末端剪切执行器、移动车体、深度相机、控制电箱、智能控制终端和移动电源所组成。串并混联型机械臂具备升降、伸缩、回转和摆头四个功能,整个机械臂安装于能在水平面上平动的移动车体上,每个车轮上配套的交流伺服电机为750 W。末端剪切执行器与RealSenseTMDepth Camera D435i深度相机(以下简称D435i深度相机)安装在机械臂的末端摆头电机上。智能控制终端选用配置NVIDIA GeForce GTX 2060Ti GPU和Intel Core i5-10400 CPU的计算机作为上位机。整机通过两台大功率移动电源供电。

图1 百香果采摘机器人系统组成

1.2 串并混联型机械臂结构

串并混联型机械臂可实现末端剪切执行器的伸缩、升降、回转和摆头四个功能的伺服运动。如图2所示,串并混联式机械臂采用由连杆和拉杆组成的双平行杆嵌套结构,可使末端剪切执行器始终保持水平姿态。机械臂的伸缩、升降和回转传动装置均布置于回转支撑齿轮上方的框架安装支架上,伺服电机的最大回转转速为3 000 r/min和最大扭矩为0.64 N·m,其动力均由伺服电机经蜗轮蜗杆减速器一次减速后分别通过水平、竖直螺杆和齿轮传动驱动实现。蜗轮蜗杆减速比为1∶20,丝杠导程为10 mm,回转齿轮减速比为20∶94。机械臂的摆头装置通过衔接连杆保持竖直状态,摆头伺服电机的最大回转转速为15 r/min,最大扭矩为50 N·m。

图2 串并混联式机械臂结构组成

1.3 末端剪切执行器设计

末端执行器保持水平的状态,可以对百香果的果梗进行剪切。如图3所示,安装在摆头伺服电机上的末端剪切执行器由电机支架、伺服夹爪电机、刀片托架和剪切刀片所组成,并与D435i深度相机构成“Eye in Hand”系统,具备较高的近距离目标捕获能力。伺服夹爪电机行程为40 mm,最大夹持力为65 N,最大速度为80 mm/s。

图3 末端剪切执行器结构图

1.4 百香果采摘机器人通信原理

串并混联型机械臂和末端剪切执行器的伺服驱动数据指令由三菱FX5U-80MT/ES可编程控制器(以下简称FX5U)发出,移动车体采用ARDUINO模块实现双轮差速伺服驱动控制。如图4所示,百香果采摘机器人通过D435i深度相机实时获取周围环境数据,主要包含RGB图像和深度距离信息,以USB串口通信的方式发送到百香果视觉识别与定位单元获取采摘点信息,经降维迭代求解和分析后得到目标点期望位姿Texp和采摘作业指令信号。

图4 百香果机器人通信原理

车体控制单元通过分析作业指令信号来输出移动车体运动信息,并采用USB串口通信方式与ARDUINO控制模块实时通信。机械臂定位单元以目标点期望位姿Texp作为已知输入条件求解机械臂的运动控制参数Pmc,采用MDBUS/TCP通信的方式将与FX5U实时通信。FX5U和ARDUINO控制模块通过输出数字信号给伺服驱动器,分别控制移动车体运动和机械臂与末端剪切执行器运行到目标采摘位置。

2 机械臂逆运动学降维迭代求解算法

正运动学是根据已知机械臂运动参数求解机械臂末端位姿,逆运动学是在已知机械臂末端位姿的条件下求解运动学控制参数。图5为机械臂等效结构示意图,根据表1的参数,研发设计的机械臂不满足解耦条件[20]中的“a×d=b×c”,求取非线性逆解难度大。为简化机械臂运动学模型和计算,本文结合张晴晖等[21]逆运动学降维求解的思想,先把回转和摆头两个关节去掉,将三维空间的数学模型降到二维空间,最后使用牛顿迭代的方法求得逆解。

表1 机械臂等效结构参数

图5 机械臂等效结构示意图

2.1 机械臂运动学建模

根据机械臂的双平行杆嵌套结构的特性对图2中的机械臂模型进行适当简化:将双平行杆简化为单平行杆。以回转机构中心原点O0,建立O0-X0Y0Z0基坐标系,以垂直移动模块和水平移动模块垂直交点O1,建立O1-X1Z1平面坐标系,以摆头伺服电机中心原点M,建立O2-X2Y2Z2坐标系。B、C、D和E为平行杆结构的四个端点。F点为DE延伸端的位置点,M点为末端剪切执行器的回转原点,N点为末端剪切执行器回转中心与末端剪切执行器水平段的交点,H点为末端剪切执行器中剪切刀片的中心点。表1列出了机械臂的等效结构参数。

如图5所示,θ为回转结构绕O0-X0Y0Z0坐标系Z0轴的旋转角度,φ为末端剪切执行器绕O2-X2Y2Z2坐标系Z2轴的旋转角度,α和β分别为CE和FE在O1-X1Z1平面坐标系下与X1轴正方向和负方向的夹角。在O1-X1Z1平面坐标系下,A(0,Az1)为机械臂的垂直移动滑块原点,C(Cx1,0)为机械臂的水平移动滑块原点。表2为机械臂关节运动参数范围。

表2 机械臂关节运动参数范围

2.2 机械臂运动学模型与工作空间

参照图5所示的机械臂等效结构示意图,根据式(1)将A点在Z1轴的坐标与C点在X1轴的坐标引入参数α和β加以表示。在O1-X1Z1平面坐标系下,不考虑θ和φ变化带来的作用,公式(2)通过几何降维处理获得F点坐标(Fx1,Fz1)。考虑θ和φ变化带来的作用,结合式(3),将H点的坐标映射到O0-X0Y0Z0基坐标系下,最后根据式(4)得到机械臂关于(α,β,θ,φ)的正运动学模型T。

(1)

(2)

(3)

(4)

串并混联型机械臂的运动学模型使用集合关系构建的,没有考虑移动车体带来的影响。试验种植基地中百香果棚架间距为2 m,结果高度集中在600～1 300 mm,试验移动车体高度为250 mm。图6中每个空间点是考虑移动车体带来的影响下,在世界坐标系o-xyz下,通过随机赋值机械臂的运动控制参数Pmc(Az1,Cx1,θ,φ)并求解正向运动学模型T得到的。仿真得到x轴和y轴上的工作区间比需要的大,z轴工作高度范围为578～1 317 mm,满足百香果采摘空间要求。

(a) x-y平面的工作空间

2.3 机械臂逆运动学求解算法

如图7所示,本文采用牛顿迭代法求出目标点期望位姿Texp的逆解数值解(α,β,θ,φ)。根据式(1)将φ和α转换为Az1和Cx1,最后得到机械臂的运动控制参数Pmc(Az1,Cx1,θ,φ)。试验结果表明,对于工作空间内的任意目标位姿,使用牛顿迭代法一般迭代160次就可以求得逆解数值解。按照采摘实际情况,在机器人右侧随机选取10个不同的位置进行逆运动学求解试验,设置的阈值为e=10-7,每次求解的平均迭代次数为163,平均耗时0.084 s。试验结果表明,基于降维迭代的逆运动学求解算法的精度和求解时间都满足机械臂求逆解的要求。

图7 牛顿迭代法求取逆解数值解

2.4 手眼标定

引导机械臂实施采摘,需要得到采摘点相对机械臂基坐标系的坐标。基于百香果采摘点定位方法获取百香果采摘点的像素坐标(up,vp)后,从D435i深度相机的深度信息中可以直接获取采摘点在相机坐标系下的坐标(xc,yc,zc)。“Eye in Hand”标定是求解相机坐标系到机械臂末端的变换矩阵TCE。将基坐标系固定不动,通过移动机械臂末端从不同角度拍摄n张标定板图片,可以获得n-1个“TATCE=TCETB”方程,采用Tsai等[22]所提出的方法求解得到TCM。由确定的正运动学模型T,结合式(5)可以得到采摘点在O0-X0Y0Z0基坐标系下的坐标(Px,Py,Pz)。

(5)

3 采摘点识别与定位算法

为解决在复杂背景下精准定位百香果采摘点的问题,根据百香果与果梗的连通关系,采用YOLO v5s目标检测算法快速识别与提取百香果ROI,以百香果ROI中心位置向上偏移构建果梗采摘点区域,融合深度信息搜索采摘点位置。

3.1 图像样本采集与数据预处理

百香果图像数据采集自广东省湛江科学研究院的紫色百香果种植基地,品种为紫香一号,垂帘式种植工艺,为避免叶面过于茂密,影响光照条件,在百香果生长期和结果期都进行剪枝操作。

为保证所制作数据集的多样性,在白天上午、中午与下午时段采集百香果RGB图像共2 560张,采集RGB-D深度图像412张。从所采集的RGB图像中筛选出质量稳定的图像1 852张,图像样本中主要包含无遮挡和遮挡的背景环境。其中1 552张用于数据集训练,300张用于数据集测试。为了提高百香果识别模型的泛化能力,对训练集的图像进行亮度、镜像翻转和添加噪声的数据增强操作,训练数据集扩增处理后所得到的共2 800张。

由于要进行百香果果实目标识别和采摘点定位研究,图像标注时,除了需要基于每个百香果的最小外接矩形进行标注,还要在最小外接矩形顶部包含百香果的一部分果梗,以保证矩形框内尽可能少地包含背景且尽可能多地包含采摘点区域。

3.2 快速提取百香果ROI

YOLO系列目标检测算法[23-25]是一阶段目标检测算法的经典代表,在保持较好性能的前提下将检测问题转换成回归问题。YOLO v5是在YOLO v4[26]的基础上改进,使其网络模型的权重文件大幅度减小,检测精度更高和运行速度更快,根据特征提取模块数量和卷积核数量不同,分为YOLO v5s、YOLO v5m、YOLO v5l与YOLO v5x四种架构。

如图8所示,本文采用YOLO v5s目标检测算法,在复杂背景下通过对输入图像全局检测,融合多尺度特征快速识别百香果目标。

(a) 原图

根据YOLO v5s目标检测模型检测结果,构建一个一维向量提取百香果ROI位置信息(xl,yl,w,h),其中xl,yl为百香果ROI在图像中左上角坐标,w,h为百香果ROI的宽度和高度。

3.3 百香果采摘点定位算法

百香果的果梗颜色与背景颜色相近,很难通过识别算法直接获取果梗位置。根据百香果与果梗具备连通关系并且基本没有支撑物干扰百香果垂帘生长,使用YOLO v5s快速识别并提取百香果ROI,以ROI中心点向上偏移构建果梗采摘点区域,再融合深度信息搜索采摘点。如图9所示,根据串并混联机械臂工作空间的范围,将ROI中心深度值dc在[0,800]mm内的ROI视为可采摘百香果目标,否则为不可采摘百香果目标。

图9 百香果采摘目标判别图

D435i深度相机基于结构光的方法采集深度信息,容易受到光线条件的影响[27],系统噪声的存在也会在捕获的深度图像中产生孔洞[28],在获取小目标的深度信息时会存在误差甚至缺失的问题。百香果的果梗细小,不能直接D435i深度相机获取采摘点的深度值。为减小D435i获取深度信息时带来的误差,以百香果ROI中心(uc,vc)向上偏移构建采摘点区域来预测果梗位置。据数据集统计,可采摘百香果的果径为4～6 cm。根据果梗与百香果ROI中心点的深度差关系,结合式(6)在采摘点区域建立有效采摘点集合P(X,Y)。

(6)

式中:d(u,v)——采摘点区域处点(u,v)的深度值。

从采摘点区域提取出有效采摘点集合P(X,Y)后,根据式(7)在P(X,Y)中搜索出深度值为dmin的点作为采摘点(up,vp)。

dmin=min[‖P(ux,vy)-Pdavg‖]

(7)

式中:dmin——P(X,Y)中与Pdavg最接近的深度值;

P(ux,vy)——P(X,Y)中(x,y)处的深度值;

Pdavg——P(X,Y)中的平均深度值。

如图10(b)所示,在百香果ROI顶部构建一个以(uc,0.07h)为中心、0.2w宽和0.1h高的矩形区域作为百香果采摘点区域。图10(c)为采摘点区域融合深度信息后的采摘点搜索结果。

图10 百香果采摘点区域定位过程图

4 系统集成与试验验证

4.1 百香果识别模型试验

进行YOLO v5s网络训练时,电脑配置情况为Intel i7-9750H Intel Xeon(R) Gold 5218 CPU,显卡为NVIDIA TITAN RTX GPU和24 GB运行内存。进行YOLO v5s网络测试电脑硬件情况为Intel Core i5-10400 CPU,显卡为NVIDIA GeForce GTX 2060Ti GPU和16 GB运行内存。batchsize设置为16,动量因子为0.937,参数训练采用SGD优化算法,权重衰减系数为0.000 5。

本研究训练使用2 800张训练集图像,如图11所示,模型一共训练300次,通过损失值评价模型训练效果,可以看出在迭代250次后,损失值降至0.27附近震荡并趋于稳定。

图11 YOLO v5s损失函数值和迭代次数曲线图

使用精确率(Precision)作为YOLO v5s模型检测精度的评价指标。对300张百香果测试集图像进行检测,总共检测出1 580个目标,包含1 509个百香果,百香果目标检测模型对百香果的识别精确率达到95.5%,满足百香果自动采摘的要求。

4.2 采摘点定位试验

4.2.1 定位算法模型试验

从412张深度图中随机选取100张质量过关的深度图,其中百香果主要包含无遮挡和遮挡两种生长情况,以采摘点位于果梗上判断为定位成功,否则定位失败。

如表3所示,在检测到的343个百香果目标中,可采摘目标总共有303个,其中无遮挡和遮挡分别有232个和71个,无遮挡和遮挡的定位成功率分别为91.4%和83.1%,总体定位成功率为89.4%。在定位试验过程中,对1 280×720的单帧图像平均识别时间约为27 ms,对单帧图像中单个百香果采摘点定位时间在6 ms左右。试验结果表明,所提出的采摘点识别定位算法的准确率和识别速度满足自动采摘要求。

表3 定位试验

4.2.2 采摘点定位算法测试试验与结果分析

试验种植基地中百香果结果位置最高1.3 m,最低位置0.5 m,两行百香果的宽度为2 m。对单侧百香果进行试验,末端执行器初始位置离地面0.7 m高,剪切刀片的开口设置为35 mm,D435i深度相机同时采集RGB数据和深度数据。

在进行单侧采摘试验的过程主要包括了搜索目标、参数求解、车体运动、机械臂运动、剪切百香果和机器人复位这几个步骤。百香果采摘机器人沿路径直线行驶,机械臂末端摆头90°;机器人系统通过百香果视觉识别与定位单元实时进行图像采集,驱动机械臂和车体的运动调整“Eye in Hand”系统的位置来搜索百香果目标;当检测到可采摘百香果,发送作业指令信号停止机械臂和车体的运动,同时对符合采摘要求的目标进行定位,通过PLC电气控制与驱动单元引导机械臂完成采摘任务,最后将机器人复位到执行采摘任务之前的位置,反复循环,直至采摘完成指定区内的所有目标。

试验过程中总共检测到30个可采摘百香果,如表4所示,主要分为无遮挡和有遮挡两种生长情况,总共完成采摘28个,总体成功采摘率为93.3%。虽然经过对百香果生长期和结果期进行剪枝操作,采摘过程中很少存在严重遮挡百香果的情况,仍然会存在藤蔓或叶子遮挡百香果法情况。试验中两个采摘百香果失败的原因都是末端剪切执行器提前接触到藤蔓或叶子推开百香果,使剪切刀片中心无法接近百香果采摘点位置。

表4 采摘试验

在30次的采摘试验中,单个百香果连贯采摘平均时间为30 s,最快采摘时间为25 s,最慢采摘时间为36 s。如图12所示,采摘过程中大部分时间都消耗在机器人搜索目标、机械臂运动定位和机械臂复位这三个步骤。

图12 采摘阶段不同步骤的所需平均时间

在采摘测试阶段,机械臂、末端剪切执行器和D435i深度相机可能发生干涉作用的情况。为保证机器人的稳定运行,保证智能采摘作业过程的安全性,在采摘测试时机器人并没有采用最大功率运行,而是选择尽可能满足技术需求的速度运行,机械臂运动定位和复位需要消耗更长的工作时间。

末端剪切执行器与D435i深度相机构成的“Eye in Hand”系统,具备较高的近距离目标捕获能力,但在远距离和大范围方面时的仍存在一定局限。如图12所示,由于百香果分布范围较广且不均匀,机器人在捕获目标的阶段需要消耗更多是时间。采摘机器人在视野范围内如果没有捕获到百香果,需要通过控制机械臂和移动车体的运动来调整末端执行器的位置来寻找百香果目标,在循环采摘的过程中需要花费较长时间。

5 结论

1) 针对垂帘式百香果种植环境,设计了一款百香果采摘机器人,包括串并混联型机械臂、末端剪切执行器、移动车体、深度相机、控制电箱、智能控制系统和移动电源。试验结果表明设计的百香果机器人满足自动采摘要求。

2) 针对复杂环境下百香果采摘点难以识别定位的问题,提出一种融合深度信息的目标检测与区域搜索的采摘点识别定位算法,通过YOLO v5s快速识别并提取百香果ROI,根据百香果垂帘生长以及与果梗具备连通关系,以ROI中心点向上偏移构建果梗采摘点区域并融合深度信息搜索采摘点,试验结果表明百香果识别模型对百香果识别精确率为95.5%,定位算法总体定位成功率为89.4%,对单帧图像平均识别时间约为27 ms,单个百香果采摘点定位时间在6 ms左右,满足百香果实时识别与定位的要求。

3) 为解决非线性求逆解难度大的问题,根据机械臂平面四连杆几何关系,提出降维迭代的逆运动学求解模型,使用牛顿迭代法获得了逆解数值解。试验结果表明,每次求解的平均迭代次数为163次,平均耗时0.084 s,满足机械臂求逆解的要求。

4) 在30次的采摘试验中,采摘成功率达到93.3%,采摘单个百香果平均时间为30 s(范围在25～36 s)。在采摘过程中,在针对存在有遮挡情况下的百香果,末端剪切执行器容易推开百香果导致采摘失败,机器人在捕获目标的阶段效率较低,为提高机器人采摘性能,后期工作可以研究高效的机械臂轨迹规划算法和增加一个范围较广的相机来捕获百香果目标的大概位置。