基于逆运动学降维求解与YOLO v4的果实采摘系统研究

张晴晖 孔德肖 李俊萩 钟丽辉

(西南林业大学大数据与智能工程学院， 昆明 650224)

0 引言

实现对机械臂运动控制和果实空间坐标定位是采摘设备设计与实现的重要环节。随着现代科技的发展，智能识别算法、机械领域新技术也越来越多地应用到果蔬采摘设备设计中，这些新技术在提高农业生产自动化和智能化的同时，也降低了生产成本、提高了工作效率。目前，国内外学者对采摘设备进行了大量的研究和实验[1-12]。文献[4]设计了一种采摘机器人，该机器人通过激光测距检测到水果，再通过计算机精确定位，并控制机械系统实现采摘；文献[5]基于单目视觉与超声检测对振荡果实采摘进行了识别与定位研究；文献[6]结合ARM开发技术设计了一套机械臂运动控制和避障系统，并通过Matlab进行三维仿真实验，结果表明，系统具有较好的实时性、可靠性和稳定性；文献[7]设计了一种芒果采摘机，该设备可确定正确的茎部切割位置，以减少乳胶渗出，保护果实免受伤害，并防止果实掉落；文献[8]以ARM92440处理器为核心，利用BP神经网络进行机器人视觉识别和定位，研制了一种苹果采摘机器人系统，实验表明，该系统能有效识别果实，并完成抓取工作；文献[9]采用多尺度多层感知器(MLP)和卷积神经网络(CNN)对苹果特征进行学习，再使用分水岭分割(WS)和循环霍夫变换(CHT)算法对苹果图像进行分割，提高了检测精度。文献[10]针对采摘过程中番茄收获机器人果实串采摘点位置难以确定的问题，提出基于果梗骨架角点计算方法，利用双目视觉图像采集系统采集了60组果实串图像，并获取果梗采摘点位置信息，结果表明，采摘点位置成功率为90%；文献[11]针对苹果采摘定位问题提出基于YOLO v3(You only look once)深度卷积神经网络的苹果定位方法，为机器人在复杂环境下快速识别苹果提供了理论依据；文献[12]为实现温室环境下农业采摘机器人对番茄果实的快速、精确识别，提出了一种改进型多尺度YOLO算法 (IMs-YOLO)，并通过番茄温室大棚采摘实验验证了模型的可行性和准确性。上述研究针对采摘方法、果实识别准确率以及果实采摘过程保护进行了研究，但关于多种果实识别采摘及设备远程控制的研究相对较少。

本文使用YOLO v4目标检测识别算法对果实进行像素定位和识别，训练多种果实的识别模型，并结合Android开发技术、服务器和网络通信技术，对多种果实进行智能识别及采摘，为降低机械臂逆运动学求解时的分析难度和计算量，通过降维方式进行机械臂逆运动学解算。

1 系统总体设计

1.1 总体框架

图1为果实采摘系统的连接及功能框图，系统中树莓派和手机端APP使用Socket通信协议与服务器进行连接，实现数据的互传。树莓派对实时图像拍摄和上传，完成机械臂逆运动学解算和运动轨迹规划，通过GPIO管脚与六自由度机械臂连接，输出控制信号，控制机械臂运动到指定位置，实现果实的采摘。服务器使用YOLO v4识别算法对树莓派上传的图像数据进行果实目标检测和像素定位，并对果实空间坐标进行解算，再将解算结果发送到树莓派。手机端APP可以实现与采摘设备的远程连接，控制采摘设备的启动和停止，可实时查看采摘设备的现场采摘视频，还可以通过手机端选择需要采摘的果实类型。

图1 果实采摘系统连接及功能框图Fig.1 System connection and function diagram of fruit picking system

1.2 设计流程

系统的设计流程如图2所示。打开手机APP以及采摘设备开机上电，APP和采摘设备自动连接至服务器，服务器端将采摘设备的IP信息发送到手机APP，在APP选中IP地址即可进行采摘设备的连接。设备连接成功后，可通过手机APP对采摘设备进行远程控制，此时树莓派将采摘现场的实时图像上传至服务器进行图像识别和空间定位，而后服务器将空间定位坐标返回树莓派。树莓派根据空间坐标进行机械臂逆运动学解算和机械臂运动轨迹规划，最终由树莓派输出控制信号，控制机械臂进行果实采摘。采摘过程中，可以点击APP中的查看按钮查看采摘现场的实时视频。

图2 系统总体流程图Fig.2 System flow chart

2 采摘系统机械臂设计

2.1 硬件结构

对果实进行采摘的是一个六自由度的机械臂，图3a为其实物图。该机械臂由6个6125型数字舵机构成旋转关节，单个舵机的扭矩为25 kg·cm，6个舵机与其它金属零件组成机械臂。

图3 机械臂实物图Fig.3 Physical drawing of mechanical arm

机械臂爪子部分可根据采摘不同的果实进行更换，这样可以使采摘机器人更好地完成果实采摘任务。当采摘的果实直径较小时，选用如图3b所示的开口较小的爪子进行采摘，该爪子最大的开口为55 mm。果实直径较大时，选用如图3c所示的开口较大的爪子进行采摘，该爪子最大的开口为125 mm。该机械臂参数见表1。

表1 机械臂参数Tab.1 Parameters of mechanical arm

2.2 机械臂逆运动学降维求解

机械臂的运动学分为正运动学和逆运动学两种。正运动学是设定好机械臂各个关节角，来实现机械臂的运动控制；逆运动学是已知一个在机械臂运动范围之内的空间坐标，根据已知的空间坐标来求解得到机械臂各个关节所需旋转的角度，最终控制机械臂运动到指定位置。在机械臂的实际运用中，更多的是实现机械臂的逆运动学求解。本文设计的采摘系统，通过图像识别算法来指定果实的空间坐标，进行机械臂逆运动学求解使机械臂运动到果实位置，最终实现采摘。

机械臂的逆运动学求解，通常是在三维坐标系中实现。首先需要通过右手定则确定机械臂各个关节的正旋转方向。之后得到各个关节的旋转矩阵和平移矩阵，将旋转矩阵和平移矩阵相乘得到一个新的矩阵，该矩阵描述了机械臂末端在空间的坐标及姿态。通过该矩阵就能得到机械臂末端和各个关节间旋转角的关系，通过这个关系可实现机械臂的逆运动学求解。但是这需要进行大量的矩阵运算和三角函数运算，并且可能求得多个解，使得机械臂的逆运动学求解变得很复杂。为了降低运算量，该采摘系统中结合使用的机械臂，对机械臂各个关节进行拆分，最终通过二维坐标系实现机械臂逆运动学求解，大大简化了计算。

2.2.1机械手臂D-H模型与坐标系建立

在分析机械手臂的运动过程时，需要先建立机械手臂中各个关节的坐标，图4a为根据D-H模型建立的机械臂各个关节的坐标图[13-16]。该图中使用通用的机械手臂关节符号来表示机械臂的结构。圆形关节表示此处的旋转轴垂直于屏幕，圆柱形关节表示此处的旋转轴平行于屏幕。该六自由度机械臂由3个圆形关节、2个圆柱关节以及1个控制机械臂末端爪子开合的关节组成。图4b为机械臂的三维建模图[17]，通过Matlab中的Robotic Toolbox对机械臂进行建模。

图4 机械臂关节坐标与建模图Fig.4 Joint coordinates and modeling diagram of mechanical arm

2.2.2机械手臂逆运动学角度求解过程实现

为了提高机械臂控制的实时性，通过降低维度的方法来进行分析得到逆运动学求解。求解过程中，先将底部云台的旋转关节和控制末端机械爪转动及开合的关节去掉，这样就可以在XOY二维平面进行机械臂姿态的分析。

图5 机械臂XOY平面图Fig.5 XOY plane diagram of mechanical arm

根据机械臂正运动学方程的DH表示法，建立图5中坐标系原点与机械臂末端的总变换矩阵为

(1)

(2)

其中cθ123表示cos(θ1+θ2+θ3)，sθ123表示sin(θ1+θ2+θ3)，sθ12表示sin(θ1+θ2)，cθ12表示cos(θ1+θ2)，sθ1表示sinθ1，cθ1表示cosθ1。

式(2)表示机械臂末端连杆坐标系相对于机械臂的描述[20-23]。

由式(2)可以得到机械臂末端的坐标为

Px=L0cθ1+L1cθ12+L2cθ123

(3)

Py=L0sθ1+L1sθ12+L2sθ123

(4)

根据几何方法有

α=θ1+θ2+θ3

(5)

由式(3)～(5)组成方程组即可求解得到图5所示的机械臂3个旋转关节的姿态，求解过程如下：

将式(5)分别代入式(3)、(4)得

Px=L0cosθ1+L1cos(θ1+θ2)+L2cosα

(6)

Py=L0sinθ1+L1sin(θ1+θ2)+L2sinα

(7)

为了方便计算，令

m=L2cosα-Px
n=L2sinα-Py

化简式(6)、(7)有

(8)

通过求解一元二次方程得

其中

由此求得θ1，重复上述算法就可以求解得到θ2、θ3。

由于该机械臂中控制爪子开合的关节和靠近爪子处的圆柱关节在转动过程中不会对机械臂末端的位置产生影响，本文求解机械臂逆运动学时，不对这2个关节进行求解。所以在求解出θ1、θ2、θ3后，只需要求解得到底部舵机云台处的旋转关节的姿态，即可完成机械臂逆运动学的求解。

图6是完成了图4a机械臂坐标建立之后的ZOX平面图。通过该图进行求解得到机械臂底部舵机云台处旋转关节的姿态。根据给定坐标中的Z和X坐标即可得到θ

(9)

完成了θ、θ1、θ2、θ3求解之后，就完成了该项目中所使用的机械臂逆运动学角度的求解。

图6 机械臂ZOX平面图Fig.6 ZOX plane diagram of mechanical arm

2.3 果实识别与空间坐标定位

2.3.1果实识别模型训练

实际采摘过程中，树莓派摄像头拍摄的视频图像中除了果实之外还有树叶、树枝和树干等其它非果实物体。如图7a所示，系统为了更好地在复杂环境中进行果实检测、识别以及像素定位，使用OpenCv计算机视觉和机器学习软件库来进行视频的处理，处理之后通过基于深度学习框架Darknet的YOLO[24]目标检测算法来进行果实目标检测与果实像素定位。Darknet框架具有CNN底层实现以及YOLO(You only look once)目标检测的功能。Darknet框架通过C语言实现，具有容易安装、移植性好和支持CPU、GPU(CUDA)2种计算方式的特点。在性能上YOLO v4的运行速度比EfficientDet快2倍，并且性能相当，将YOLO v3的平均精确率(AP)和帧率(FPS)分别提高了10%和12%。

图7 果实识别与目标检测Fig.7 Recognition and target detection of fruit

进行模型训练时，对各种果实在无遮挡光照充足的条件下实景拍摄图像500幅，对每种果实的500幅图像进行4种处理方法，包括90°、180°和270°旋转以及进行X轴镜像处理，最终得到每种果实的训练数据集图像为2 500幅，其中2 000幅图像用来进行模型训练，500幅图像用来模型验证，训练集和验证集比例为4∶1。确定好训练集之后使用图像标注工具对图像中果实进行标注，并生成PASCAL VOC格式的xml标注文件，生成的标记文件中使用了5个参数(class_fruit,x,y,w,h)对图像中的果实进行描述，class_fruit表示果实的种类，x和y表示果实在图像的中心坐标，w和h表示目标框的长和宽。之后，对YOLO v4中的配置文件进行修改。该项目中对配置文件的修改如下：batch(每一批训练的样本数量)为16，subdivisions(batch分割数量)为16，max_batches(迭代次数)为6 000，steps(学习率)为4 800、5 400(0.8倍batch为4 800，0.9倍batch为5 400)。进行模型训练的硬件环境为Intel(R)Core(TM)i7-7700HQ CPU 2.80 GHz(8CPUs)处理器，16 GB运行内存，显存为8 GB的GTX 1080Ti显卡，软件环境为Ubuntu 19.10操作系统。该系统训练了苹果、油桃、黄杏和李子4种果实的识别模型。

训练好模型之后即可对树莓派上传的实时图像进行果实目标检测，检测之后输出图像中果实的置信度、种类以及像素点尺寸和中心像素位置，得到的像素尺寸和像素位置用于后期果实的空间坐标定位。图7b是识别后的图像，从图像中可以看到果实被蓝绿色边框框出，果实标签为“lizi”，置信度为100%。

2.3.2果实空间坐标定位

得到果实空间坐标是实现果实采摘非常关键的步骤，该系统通过得到果实相对于摄像头的坐标来确定果实坐标，如图8所示。图中分别建立了果实和摄像头的XOY平面图和XOZ平面图，P表示摄像头拍摄到的图像，A表示果实，a表示果实A在图像中的尺寸，ay和ax表示a在图像中x方向和y方向各占据的像素点数量。

图8 果实空间位置定位Fig.8 Spatial location of fruit

在坐标定位中首先要得到图像中像素点与实际物体尺寸的转换关系，使用摄像头拍摄1元硬币的方式来得到转换比率，1元硬币的直径为3 cm，在拍摄距离为1 m时，其在图像中占163像素，得到转换比率k=163/3=54.3像素/cm。图中Py和Pz表示a中心点在y轴和z轴到中轴线的偏移量，根据相似原理，k=Py/YL，k=Pz/ZL，由此可以得到果实空间坐标中的y坐标和z坐标计算公式分别为

YL=Py/k

(10)

ZL=Pz/k

(11)

根据单目摄像头测距原理，计算果实空间坐标中的x坐标，计算公式为

D=WF/P

(12)

式中D——果实与摄像头的相对距离

W——果实宽度

F——摄像头焦距

P——果实在图像中占的像素数

树莓派摄像头焦距F为543像素。如图8所示W=ax/k，P=ax，D=XL，将计算公式修改为

XL=F/k

(13)

确定果实A的空间坐标为(XL,YL,ZL)。

2.4 机械臂运动轨迹规划与采摘控制

2.4.1五项式插值法推导

系统首先对果实空间坐标进行定位，然后进行逆运动学求解，之后再对机械臂的运动轨迹进行规划。系统中使用了五项式插值法对机械臂运动轨迹进行规划，该方法在得到机械臂各关节的起始角和停止角的前提下，可实现对角度、角速度和角加速度的约束。该方法的数学推导过程如下：

在五项式插值法中设定了6个待定系数a0～a5，得到起始角和终止角、角速度和角加速度随时间变换的函数关系。式中θ(t)表示角度随时间变换的函数关系，θ′(t)和θ″(t)是对θ(t)进行一次求导和二次求导之后的结果，分别表示角速度和角加速度随时间变换的函数关系为

(14)

在已知起始角和终止角的前提下，使用θ0和θf表示起始角和终止角，并且约束起始速度和终止速度，对式(14)进行推导得到的函数约束关系为

(15)

将式(15)代入式(14)求解得到各个系数为

(16)

2.4.2五项式插值法实现单个电机运动控制

在完成五项插值法数学推导之后，通过Python编程语言编写五项式插值法程序。表2为电机运动角、角速度和角加速度的约束关系。运行程序之后得到图9所示的角度、角速度和角加速度随约束时间的变化曲线。通过曲线图可以看出电机旋转的角度和角加速度均随时间变化，这也意味着电机在旋转过程中处于平稳的转动状态，这样可以保证机械臂的关节电机在工作过程中损耗较小，且使得机械臂整体运动情况更加协调与稳定。

表2 电机运动角度、角速度和角加速度约束关系Tab.2 Constraint of angle, angular velocity and angular acceleration of motor motion

图9 电机运动轨迹规划曲线Fig.9 Curves of motor motion trajectory planning

2.4.3单次采摘流程与速度设定

图10为采摘系统的单次采摘流程图，系统开始工作后，树莓派控制机械臂进入初始状态，同时树莓派拍摄图像并上传，在服务器中使用训练好的识别模型对果实进行识别，并根据空间坐标定位算法计算果实的空间坐标并返回到树莓派中。之后树莓派进行逆运动学解算，得到机械臂从初始状态运动到果实位置各个关节旋转的角度，并且根据旋转角使用五项式插值法进行运动轨迹规划。规划后树莓派输出对应的控制信号，控制机械臂运动以实现果实的采摘。采摘完成之后进行果实投放，最终机械臂回到初始状态，等待下一次采摘指令。

图10 单次采摘过程的流程图Fig.10 Flow chart of single picking process

在该系统中为了防止机械臂运动过快导致系统不稳定以及容易碰撞到果实，设定五项式插值法中进行运动轨迹规划的约束时间为5 s。根据采摘流程图可以得到，进行一次果实采摘需要通过五项式插值法进行3次运动轨迹规划。设3次五项式插值法消耗的时间为T1=15 s，其它各个流程步骤消耗的时间为T2，则采摘系统完成单次采摘任务的总时间为T=T1+T2=15+T2。由于时间T2包括了整个系统的各个环节的运作时间，会受到网络速度、服务器以及树莓派的运算速度的影响，无法计算得到一个精准的时间，只有通过实际测试获知。

3 采摘系统试验

图11a为系统自动采摘李子的现场测试。测试中通过APP界面中的“设备管理”对采摘设备进行连接，图11b所示是在手机APP中选择采摘的果实为李子，点击确定按钮后采摘设备开始工作。

图11 果实实地采摘测试图Fig.11 Test chart of fruit picked in field

与树莓派连接的摄像头进行视频拍摄并上传到服务器中，服务器调用训练好的李子识别模型对上传的视频进行识别并对果实进行空间位置定位，然后将识别结果返回树莓派，由树莓派控制机械臂进行采摘。当用户发送查看视频请求指令到服务器时，服务器将树莓派上传的原始视频提供到手机端进行显示。图11c为手机端APP查看到当前设备的IP、已采摘的果实数量、采摘的果实种类以及实时画面。

表3为机械臂姿态解算测试结果，测试过程中通过树莓派将实时图像上传到服务器中，服务器中调用YOLO v4识别算法训练得到的识别模型对上传的图像进行果实目标检测以及像素定位，根据像素定位计算得出空间坐标，并将果实的空间坐标返

表3 机械臂姿态解算测试结果Tab.3 Test results of attitude calculation of manipulator

回到树莓派中，树莓派根据接收到的空间坐标对机械臂进行逆运动学解算，对不同空间坐标实现机械臂逆运动学解算得到θ、θ1、θ2、θ3，如表3所示。

表4为使用该果实采摘设备采摘不同类别果实的测试结果。此次采摘测试在光照充足的条件下，对无遮挡且果实生长稀疏的情况下进行采摘测试。测试过程中根据不同果实尺寸来选择爪子型号，对每种果实进行采摘测试的次数均为60次，在此基础上得到采摘过程中机械臂的最大和最小电流、完成不同种类果实的采摘总时间Ta。

表4 不同种类果实采摘测试结果Tab.4 Picking test results of different kinds of fruits

表5为采摘系统的单次采摘时间，表中Ta为对各种果实采摘60次的总时间，单次果实采摘时间计算公式为T=T1+T2，由于T2是一个不确定的值，导致单次采摘具体时间T也无法准确确定，这里采用平均时间Tn来代替T，Tn=Ta/60表示平均单个果实采摘时间，则有单次果实采摘时间为：T=Tn=T1+T2，由表5中可以看出，单次最长采摘时间为17.98 s，最短单次采摘时间为16.58 s。

表5 单次采摘时间计算结果Tab.5 Calculation of single picking time

表6为对不同种类果实训练后的模型进行测试的结果，每种果实的训练集图像均为2 000幅，测试集图像为500幅，在Ubuntu 19.10操作系统中使用GPU进行模型训练，用训练后的模型进行测试得到的各类平均精确率的平均值(mAP)、准确率、召回率和交并比(IOU)如表6所示。

表6 不同种类果实的训练模型测试结果Tab.6 Training model test results of different kinds of fruits %

通过实际采摘测试，该采摘系统可以实现对苹果、油桃、黄杏、李子4种果实的自动采摘，系统工作稳定，但是目前系统设计与研究处于实验阶段，还有一些不足之处：

(1)果实识别模型采用的数据集均是在光照充足的条件下拍摄的无遮挡果实图像，所以采摘系统目前只能在光照充足的条件下对无遮挡的果实进行采摘。如果需要设计更好的采摘系统，则需要更多情况下的果实实景图像来进行模型训练。

(2)目前采摘系统中只是通过更改爪子型号来实现对直径较大和直径较小的果实进行采摘，由于爪子是金属类物体，采摘过程中会导致果实表皮受损，需要更换采摘和夹取方式来保护果实。

(3)目前该系统是通过人工方式移动或安放采摘设备到某一个位置来进行采摘测试，而实际的采摘系统因根据不同种类水果的果树高度来设计采摘机械臂以及整个采摘系统。

4 结论

(1)基于树莓派、六自由度机械臂、手机APP、服务器设计了智能果实采摘系统。

(2)相比于传统的水果采摘系统，本文设计的水果采摘系统通过YOLO v4训练了多种果实识别模型，可以实现对多种果实的自动采摘。

(3)该系统中结合实际使用的机械臂，对机械臂进行关节拆分，可以通过降维的方式来实现机械臂逆运动学求解，降低了分析难度和运算量。

(4)在机械臂运动控制中，使用了五项式插值法来实现对机械臂的运动轨迹规划，此法可使机械臂各个旋转关节的电机平稳转动，使机械臂运动更加协调与稳定，减小机械臂工作过程中电机的损耗。

(5)该系统设计中结合了Android开发技术以及网络通信技术，可以通过手机APP端对采摘设备进行远程控制以及查看实时采摘视频。