基于D-H法的农业采摘机器人运动协作控制系统设计

闵小翠，李 鹏

(广州华立科技职业学院，广州 511325)

0 引言

农业机器人是农产品收成工作中的重要辅助工具，农业机器人以牵引车为核心，在定位、导航等技术的支持下，通过传感器、嵌入式智能系统来控制农用机械等机械的运动，从而实现多种农田作业。采摘机器人是农业机器人中的一种，是以农产品采摘为主要工作内容的机器人，采摘机器人的研制和应用，对提高劳动生产率具有重要的现实意义。作物的生长是在时空上发生变化的，其生长的环境是多变的、未知的。因此，农业采摘机器人既要具备与生物的柔性相适应的加工能力，又要适应多变的自然环境。同时，部分农作物表皮较为脆弱，且形态多样，生长的程度也各不相同。采摘机器人一般是同时进行采摘与移动作业，为了提高农产品的采摘效率，一般采摘工作都由多个采摘机器人共同完成，这种工作方式虽能有效节省工作时间，但也提高了机器人之间的碰撞发生概率，为了降低工作状态下农业采摘机器人之间的产生的负面影响，设计并开发了机器人运动协作控制系统。

运动控制是一种自动化的分支，它利用伺服机构来控制机械的位置和速度等运动参数。文献[1]中提出的基于EtherCAT和TwinCAT3的机器人控制系统，通过以太网控制自动化技术实现系统的基本控制功能，并以TwinCAT3作为上位监控工具，实现实时控制指令的调节。文献[2]中提出了基于无线网络的机器人控制系统，该系统通过无线网络进行控制指令的传输，而文献[3]中提出的基于模糊PID的机器人控制系统，以模糊PID技术作为控制原理，通过模糊控制理论与调节控制理论的结合，实现系统完整的控制逻辑。文献[4]提出考虑气象因素的农业采摘机器人轨迹控制方法。提出了一种基于云存储技术的目标果实动态监测方法。基于 RBF神经网络设计了最优运动模型，并采用该模型的最优运动模型，实现了基于 RBF神经网络的采摘机器人轨迹控制。

上述系统为现阶段发展较为成熟的研究成果，然而在实际的运行过程中，传统控制系统存在明显的控制效果不佳的问题，主要体现在控制误差与机器人碰撞事故两个方面，为此引入D-H法。D-H方法是将一个坐标系固定于机械臂的各个连杆上，再利用4x4齐次转换矩阵来表示两个相邻的连杆之间的空间关系。利用D-H法对农业采摘机器人运动协作控制系统进行优化设计。利用D-H法构建农业采摘机器人运动协作数学模型，利用传感器设备实时量化机器人的位姿信息，并通过模拟采摘流程，分配机器人的运动协作任务。从位置和姿态等多个方面，确定机器人的运动协作控制目标，通过受力分析来求解机器人实际作用力，计算控制量，实现农业采摘机器人的运动协作控制功能。

1 农业采摘机器人运动协作控制硬件系统设计

1.1 农业采摘机器人传感器

装设运动传感器的目的是获取农业采摘机器人的实时运动信息，为运动协作控制量的计算提供初始数据。

1.1.1 位置传感器

优化设计的位置传感器采用光电转换器作定位器件，只提供高、低两种信号，使用的振荡器是由高速CMOS电路74HC00组成，光电转换器则选择3 mm直径的红外发射接收二极。优化设计位置传感器的实物图及工作电路如图1所示。

图1 位置传感器

1.1.2 碰撞传感器

为了避免农业采摘机器人末端在执行采摘作业过程中被障碍物阻碍，在终端操作机构的前端装设了一个碰撞传感器来感知障碍。利用FSR-408型力敏电阻器制造用于机器人避障的碰撞传感器，并将FSR-408型多个碰撞传感器装在机械手前臂上，实现对作业中遇到的障碍物进行实时检测。在采摘机器人的操作中，主要是对大臂和小臂进行操作，以完成对作物的采集[5]。通过碰撞传感器的安装，可以在运动时检测到障碍物。同时，对各方向的障碍物进行实时探测，若有障碍物出现，则按该方向的感应器进行探测，并采取避障措施。在信号获取过程中，每个传感器都要进行编码，以便于在程序设计中进行信号的处理。优化设计碰撞传感器的特点是自动复位，高重复精度，扭矩大。

在实际的装设过程中，将位置传感器安装在农业采摘机器人的中心位置以及各个关节位置，而碰撞传感器主要安装在农业采摘机器人操作元件末端。

1.2 农业采摘机器人运动协作控制器

运动协作控制器是系统控制功能运行的主要部件，控制器选用LPC11U6x作为核心部件，能根据定位盘的坐标进行控制。运动协作控制器的实物图及内部组成结构如图2所示。

图2 协作控制器内部组成框图

从图2中可以看出，优化设计的运动协作控制器采用“陀螺仪+编码器”模式，产生PWM信号作为电机驱动电路的基极信号，通过对4个FETIRF3205构成H桥电路。信号反馈部分由电流反馈和电压反馈两个部分组成，其中电流反馈电路主要包括运算放大器、电压比较器和AD模块。通过采样电阻，将电动机的电流直接输入到dsPIC30F4012的AD模块中，并将其作为电流回馈[6]。电压反馈的基本原理是：当电枢电流超出设定值时，比较器会输出一个低电平，并在此过程中，会发出一种提示系统出现故障的中断信号。电压电路具有电流检测、电流限制及过流保护等特点。

1.3 农业采摘机器人运动驱动器

在运动控制系统中，必须以作业要求和生产环境为前提，以价格和技术水平作为评判依据，进行驱动器设备的选择。由于采摘作业一般都是野外作业，安装液压、气压等都比较困难，而且会给步行系统带来负担，所以采取了电动驱动方式[7]。优化设计控制系统中选择EC45150W直流电机，该电机属于永磁同步电机。根据控制信号与驱动信号的作用原理，实现运动协作控制器与驱动器之间的连接。

1.4 农业采摘机器人运动协作通信模块

系统通信模块设计的目的是为控制指令的传输提供硬件支持，同时也为实时传感数据以及机器人之间的信息传输提供通信环境[8]。优化设计控制系统中布设的通信环境支持同步传输和异步传输两种方式，同步模式是一种连续传送多个字符的串口通讯方法，在通讯过程中，收发端的字节和比特之间存在着严格的时序关系，而发送和接收信息的两方则依靠同步信号来实现接收和接收的同步。在传送过程中，每一个字节所占用的时间间隔都是相等的，不仅字节间没有间隙，而且字节必须是连续的，当没有数据需要传送时，发送者要将剩下的时间用一个字节同步序列来填补。在多主机模式中，接收端的位同步信号是由发送者来提供的，而组同步信号则是通过接收端在完成了位同步之后捕获的。在主从模式下，通过主控制器生成位同步和组同步[9]。异步通讯没有严格的时序关系，而且没有固定的时序。字节是逐帧传输，每个帧的传输都是从开始位到终止位。接收端通过检测起始位和终止位来与发送者进行同步。除了路由器以及普通通信节点外，将优化的传感器接入到通信网络中，作为移动通信节点。

2 农业采摘机器人运动协作控制系统软件功能设计

在控制硬件系统的支持下，利用D-H法构建农业采摘机器人的数学模型，并对采摘机器人的运动状态进行求解。根据机器人的采摘任务，将采摘任务分配给多个机器人，通过对农业采摘机器人采摘流程的模拟，确定每个机器人实际的运动控制目标[10]。根据机器人的采摘目标，计算机器人的实际作用力，利用传感器设备采集采摘机器人的实时运动数据，通过当前运动数据与控制目标数据的对比，计算得出机器人的运动协作控制量，在控制指令的作用下，完成系统的控制功能。

2.1 基于D-H法的农业采摘机器人数学模型构建

利用D-H法得出农业采摘机器人中任意两个关节之间的连接模型如图3所示。

图3 农业采摘机器人关节连杆D-H模型表示法

按照图3表示的D-H法得出机器人中所有结构的模型构建结构[11]，按照机器人的组成结构，实现多个组成部件的连接。D-H法支持下，得出农业采摘机器人末端执行器位姿为：

(1)

式中，(ψx，ψx，ψz)、(λx，λy，λz)和(δx，δy，δz)分别对应的是法线向量、方位向量和接近向量，(px，py，pz)表示采摘机器人末端执行器在D-H基准坐标系中的位置坐标[12]。由此可以得出任意两个组成元素之间的变换矩阵为：

(2)

式中，θij和ϑij分别为关节转角和扭角，Lij为关节之间的连杆长度，χij表示关节偏距。同理可以得出所有连接机构之间的变换结构，并由此反映出农业采摘机器人的正向运动机理[13]。在此基础上，通过对机器人模型的逆运动学求解，即利用矩阵的逆对公式(2)进行多种变形，即可得出机器人中各个关节角的求解结果。另外，假设农业采摘机器人的旋转中心与几何中心重合，则其动力学模型可以量化描述为：

M(p)a+Q(p)v+Ff(p)+G(p)=c

(3)

式中，M(p)、Q(p)、Ff(p)和G(p)对应的是惯性矩阵、哥氏力与离心力作用合力、摩擦力以及重力，c为机器人的控制输入，该变量包括位置控制分量和角度控制分量两个部分[14]。将运动学求解结果以及动力学方程融合到机器人连接结构中，实现农业采摘机器人数学模型的构建。

2.2 农业采摘机器人实时运动位姿描述

在构建的农业采摘机器人数学模型下，利用硬件系统中的传感器设备实现对运动位姿的实时采集，并以量化形式输出。

(4)

式中，(x(t)，y(t)，z(t))表示机器人运动实时位置，(x0，y0，z0)表示机器人的初始位置，φ为姿态角，L为机器人的尺寸长度，t为时刻，v0和vvehicle分别为机器人的初始移动速度和当前移动速度。在实际位姿数据采集过程中，需要根据传感器的工作频率对机器人的位姿数据进行更新，更新结果如下：

b(t+1)=b(t)+κgainR

(5)

式中，b(t)和b(t+1)分别为t和t+1时刻的位姿数据，可分为位置数据和姿态角数据两个部分，R为位姿观测的协方差矩阵，κgain为卡尔曼增益参数[15]。按照上述流程完成农业采摘机器人中所有组成构件的实时运动位姿数据采集结果。

2.3 农业采摘机器人运动协作任务分配

根据农业采摘机器人的基本组成结构，农作物的采摘任务采用主-从作业模式，即主作业手用来采集农作物目标，而从作业手主要用来切断农作物果实与树枝之间的连接，根据作业手的工作内容，对机器人的运动任务进行划分，分配给主、从作业手，并消解作业手运动过程中可能存在的冲突现象[16]。假设输入到农业采摘机器人中的运动作业为U，则分配给主、从作业手的运动任务可以分别表示为：

(6)

式中，Umain、Ufrom分别为分配给主、从作业手的运动任务，d()为距离求解函数，Pmain，0和Pfrom，0分别表示机器人上主、从作业手的初始位置，Pcrops为采摘农作物目标的位置信息，Pbreakpoint为采摘过程中农作物的切断位置[17]。农业采摘机器人运动协作分配任务的约束条件为：

Pmain(t)-Pfrom(t)≠0

(7)

公式(5)表示的是农业采摘机器人作业过程中，任意时刻主作业手和从作业手的位置均不重合，由此实现机器人运动冲突的消解。

2.4 农业采摘机器人运动协作控制目标确定

充分考虑农业采摘机器人作业任务，通过机器人采摘作业流程的模拟，确定各个机械手以及机械手之间的控制目标。图4表示的是农业采摘机器人的采摘作业流程。

图4 农业采摘机器人作业流程图

在工作状态下的农业采摘机器人首先利用内置的视觉元件收集周围的环境信息，通过对图像特征的提取与匹配，确定当前环境中是否存在采摘对象，并确定农作物采集目标的实际位置，标记为(xcrops，ycrops，zcrops)，则采集目标位置即为主作业手的控制目标，而从作业手的控制目标可以表示为：

(8)

式中，(xbreakpoint，ybreakpoint，zbreakpoint)表示从作业手的控制目标位置，变量lcrops、wcrops和hcrops分别表示采摘目标的长度、宽度和高度，上述变量可通过视觉图像识别直接得出[18]。在已知起始节点与目标节点的情况下，得出主、从作业手的移动轨迹为：

(9)

将相关系数代入到公式(7)中，即可得出主作业手运动轨迹Ymain和从作业手运动轨迹Ybreakpoint的初始规划结果。为避免机器人在采摘过程中出现的碰撞现象，需要对运动轨迹中的障碍物进行识别，若初始规划轨迹中存在障碍物，则需要对该轨迹点进行移动处理，并重新连接移动轨迹点和原轨迹中的前后轨迹点，最终通过对规划轨迹的平滑处理，将得出的轨迹处理结果作为农业采摘机器人运动协作的控制目标。通过上述算法模型，实现了农业采摘机器人运动协作控制系统软件的运行，农业采摘机器人运动协作控制系统界面图如图5所示。

图5 农业采摘机器人运动协作控制系统界面图

2.5 农业采摘机器人作用力求解

当机器人协作采摘一个农作物时，机器人的两个作业手与被采摘物体形成一个闭环，具体的受力情况如图6所示。

图6 机器人协作采摘对象受力关系图

机器人主从作业手在采摘对象上施加的力Fresultant和力矩ζresultant可以分别表示为：

(10)

式中，Fmain和Ffrom分别为主从作业手的实际作用力，ζi为力矩分量，pi为机器人质心相对于机器人末端的位置向量[19]。在实际的求解过程中，作业手实际作用力的计算公式如下：

(11)

式中，mcrop为采摘作物目标的质量，o为作业手与采摘目标中心之间的夹角，κrigidity和rbranch分别为农作物连接树枝的刚度系数和半径值。将公式(11)的计算结果代入到公式(10)中，得出农业采摘机器人作用力以及力矩的具体求解结果。

2.6 农业采摘机器人运动协作控制功能实现

结合当前农业采摘机器人各个作业手的实际运动数据与采摘作业的目标数据，利用公式(12)计算出农业采摘机器人的运动协作控制量。

(12)

将实时位姿数据与运动目标求解结果参数代入到公式(12)中，即可得出机器人运动位置、姿态角以及作用力控制量的计算结果。根据控制量的计算结果，利用控制器设备生成控制指令，分别作用在机器人的各个关节上，控制指令的作用逻辑如图7所示。

图7 机器人运动协作控制指令作用逻辑图

在实际控制任务执行过程中，首先执行机器人采摘放置程序初始化，然后驱动主作业手到达待采摘目标下方，张开末端主作业手，从作业手运行至待剪断新梢位置，主作业手在力矩驱动下运行到收集台后张开释放采摘作物，释放完成抓手复位，完成一次采摘任务后判断分配的采摘任务是否全部执行完毕，若存在未执行的采摘任务，则继续循环上述控制步骤，否则跳出循环，农业采摘机器人复位，将采摘完成指令反馈给控制器[20]。

3 系统测试

为了测试优化设计的基于D-H法的农业采摘机器人运动协作控制系统是否达到预期效果，设计仿真实验。确定农业采摘机器人的运动协作控制目标，并在控制系统的作用下获取机器人的实际运动数据，通过两者之间的比对，得出反映系统控制精度功能的测试结果，除此之外，此次系统测试实验还对机械臂的协作控制能力进行测试，最终从控制精度和协作能力两个方面，体现出优化设计系统在控制功能方面的优势。

3.1 配置农业采摘机器人样机

此次系统测试实验选择GSCZ型号的果蔬采摘机器人作为控制对象，该机器人具有信息获取、成熟度判别等功能，从组成结构方面来看，由机架、履带移动机构、采摘操作装置、传输机构、双目视觉机构等部分组成，其中采摘操作部分中，主作业手为果实抓手，从作业手为切断装置。农业采摘机器人的外形尺寸为1 200 mm*800 mm*1 800 mm，整机质量为120 kg，机器人的最大移动速度为6.0 m/s，采摘模块和移动模块的总功率分别为80 W和120 W，主从作业手的工作空间均能达到800 mm*300 mm。

3.2 选择机器人采摘对象

选择哈密瓜、黄瓜、樱桃和苹果种植基地作为机器人的采摘作业环境，其中哈密瓜和黄瓜的果实均结在藤上，樱桃和苹果均长在树上，因此上述4种果实的采摘机理相同，其采摘高度在准备机器人样机的采摘范围内。选择果蔬种植基地内部分区域内成熟的果蔬作为采摘对象，在开始实验之前统计采摘对象的数量，并确定各个对象的生长位置信息。

3.3 生成机器人运动协作控制任务

按照空间顺序对采摘对象进行标号，并根据收集的位置信息生成机器人运动协作控制任务，其中部分任务的生成情况如表1所示。

表1 农业采摘机器人运动协作控制任务

根据机器人的位姿初始值，得出机器人的采摘轨迹，从而得出农业采摘机器人任意时刻运动控制目标的生成结果。

3.4 设置系统控制功能测试指标

从控制精度和协作控制能力两个方面设置量化测试指标，其中控制精度测试指标具体包括：位置控制误差、位置控制方差、姿态角控制误差和作用力控制误差，其中位置控制误差的数值结果如下：

εp=|xactual-Δx|+|yactual-Δy|+|zactual-Δz|

(13)

式中，(xactual，yactual，zactual)为机器人的实际位置坐标。

位置控制方差的数值结果如下：

(14)

式中，n为获取的机器人位置个数。

另外姿态角和作用力误差的测试结果如下：

(15)

式中，φactual、φtarget、Factual和Ftarget分别为姿态角和作用力的实际值与目标值。另外协作控制能力的测试指标为机器人的碰撞次数，该指标的测试结果为：

Ncollision=Ntask+Nexternal

(16)

式中，Ntask和Nexternal分别表示机器人作业手之间的碰撞次数及其与外界环境之间的碰撞次数。最终计算得出控制误差越小、碰撞次数越少，证明对应系统的控制功能越优。

3.5 系统测试过程与结果分析

根据系统的功能测试内容，将实验分为控制精度测试和碰撞测试两个部分，在实验开始之前，首先需要对农业采摘机器人运动协作控制系统进行开发，并调试系统运行程序与环境。首先将优化设计系统的硬件部分装设到机器人样机上，利用编程工具实现系统软件部分的开发，并导入到机器人内部的运行器件中。随机生成控制指令，观察机器人样机是否能够按照控制内容完成指定动作，从而完成系统硬件与软件的调试。为了体现出优化设计系统在控制功能方面的优势，设置传统的基于无线网络的机器人控制系统(文献[2]方法)和基于模糊PID的机器人控制系统(文献[3]方法)作为实验的对比系统，分别标记为对比系统1和对比系统2，按照上述流程完成对比系统的开发与调试工作。

3.5.1 控制精度测试

将农业采摘机器人样机放置在种植基地环境中，将生成的控制任务逐一输入到控制系统中，通过数学建模、位姿描述、任务分配、控制量计算等步骤，完成相应的采摘工作。统计机器人作业手的位置数据，通过公式(13)的计算，得出不同系统作用下机器人位置控制误差及方差的测试结果，如图8所示。

图8 农业采摘机器人位置控制误差测试对比结果

从图8中可以直观地看出，与两个对比系统相比，优化控制系统作用下机器人位置误差更小，最大值为14 mm。机器人位置的方差也更小，约为9 mm。另外机器人姿态角和作用力控制误差的测试结果，如表2所示。

表2 机器人姿态角和作用力控制误差测试数据表

将表2中的数据代入到公式(13)中，综合主、从作业手两个部分，得出两种对比系统下机器人姿态角控制误差的平均值分别为0.25°和0.15°，平均作用力控制误差分别为2.13 N和1.0 N，优化设计系统下，机器人姿态角和作用力的平均控制误差分别为0.04°和0.4 N。

3.5.2 碰撞测试

在农业采摘机器人运动过程中，记录作业手之间以及作业手与外界环境之间的碰撞次数，直接得出反映系统协作控制能力的测试结果，如表3所示。

表3 系统协作控制能力测试数据表

通过公式(15)的计算，得出3种控制系统作用下，机器人碰撞次数的平均值为5.75次、4.25次和1.25次。

4 结束语

农业采摘机器人是一种在农业机械化生产中非常重要的应用，具有广阔的发展前景。在此次研究中，利用D-H法对采摘机器人运动协作控制系统进行优化设计，实验结果表明，所设计基于D-H法的农业采摘机器人运动协作控制系统能够实现对机器人采摘过程的精准控制，减小控制误差和机器人碰撞概率，从而降低了机器人采摘失败的概率，有效提高了农业采摘机器人的工作效率。

采用D-H法进行农业采摘机器人运动协作控制，具有工作效率高、采摘精度高的优点，但在实际应用过程中，对多种农产品的控制参数不同，难以完成多种农产品同时采摘的工作。因此，在未来研究中，将以同时实现不同种类农产品的采摘为目标，进一步完善基于D-H法的农业采摘机器人运动协作控制系统。