黄瓜采摘机械手运动规划与控制系统设计

王 燕

(洛阳理工学院 电气工程与自动化学院，河南 洛阳 471023)

黄瓜采摘机械手运动规划与控制系统设计

王 燕

(洛阳理工学院 电气工程与自动化学院，河南 洛阳 471023)

针对现有采摘机器人存在机构笨重、成本高、智能化水平低下等问题，研制了一套四自由度关节型采摘机械手。结合黄瓜采摘作业的特点，采用两段摆线运动组合的水平抓取运动规划算法，提出了基于DSP上位机运动控制器+CAN总线+DSP关节控制器的分布式控制系统。机械手定位精度测试实验结果表明：机械手定位精度可达6.9 mm，基本可满足黄瓜采摘作业的要求。

黄瓜采摘机械手；运动规划；控制系统

农业机器人是一类以农产品为操作对象、兼有人类部分信息感知和四肢行动功能、可重复编程的柔性自动化或半自动化设备。采摘机器人作为农业机器人的重要类型，具有很大的发展潜力。目前，日本、荷兰等国已研制出应用于温室和小型农场的西红柿、黄瓜、甘蓝等多种采摘机器人[1-3]，我国在该领域中的研究虽然还处于起步阶段，但也取得了一些可喜的成果，如中国农业大学研制的草莓、茄子采摘机器人和浙江大学研制的番茄收获机械手等[4-6]。综合国内外果蔬采摘机器人的研究文献发现，对于采摘机器人的研究大多集中在视觉系统对目标果实的识别和定位上，对机械手本体结构与控制系统的研究较少。很多农业采摘机器人大都直接购买现有的工业机械手，控制系统采用PC机+运动控制卡的结构形式，这就存在机构笨重、成本高、智能化水平不够高等问题。针对这种情况，研制了一套四自由度关节型采摘机械手，并提出了基于DSP上位机运动控制器+CAN总线+DSP关节控制器的控制系统结构，并通过实验验证了机械手及控制系统的性能。

1 黄瓜采摘机械手机械结构设计

图1 采摘机械手结构简图

黄瓜采摘机械手机械本体主要由机械臂与末端执行器组成。机械手实物如图1所示，由腰关节、肩关节、肘关节和腕关节4个旋转关节组成，属于关节式机械手，它的一端固定在基座上，另一端自由并安装末端执行器。机械臂前3个自由度来确定目标果实的位置，而为便于采摘，应使末端执行器在采摘过程中保持水平，在腕关节加一个旋转自由度，故4个自由度的机械臂即可满足采摘作业要求。

机械臂4个关节的机械结构形式基本相同，均采用直流力矩电机串接谐波减速器的传动方式，所设计的机械手利用CAN总线通信技术，所有关节只需4根连线(两根电源线和两根通信线)即可实现与上位机及关节之间的通信；各关节控制器具体执行各关节的位置和速度控制算法。智能关节的设计简化了系统布线，缩小了控制系统体积，可方便地实现机械臂关节的扩展，这为机器人构件的标准化、模块化提供了有利条件。

2 黄瓜采摘机械手运动规划算法

运动规划是根据末端执行器的作业要求来设计各关节位移、速度、加速度对时间的运动规律，规划函数必须保证关节变量及其前两阶导数的连续性，使规划的关节轨迹光滑，同时应尽量较少额外的运动如游移和抖振[7-8]。摆线运动因其计算简单，轨迹连续平滑，并能在有限区间的端点产生零速度和零加速度，特别适用于点到点的关节空间轨迹规划。在正则形式下，其运动方程可描述为[9-10]：

(1)

容易得到它的各阶导数为：

s′(τ)=1-cos2πτ，

(2)

s″(τ)=2πsin2πτ。

(3)

图2 基于两段摆线组合的平抓方式轨迹规划

图3 黄瓜采摘机械手控制系统总体框图

从初始工作状态位姿运动到目标黄瓜过程中的这段轨迹，如果直接采用基于摆线运动的规划，会碰撞其他果实，有时还会出现碰掉目标果实的严重后果。因此，采用两段摆线运动轨迹组合的平抓方式进行规划，保证机械手水平抓取的常态，如图2所示。

当采摘机器人由工作状态初始位姿A运动到目标黄瓜采摘点T时，首先控制机器人由A点运动到F点，在此段运动过程中保持腕关节水平；然后再由F点运动到T点。FT是平行于x轴的直线，即F点与T点的位置关系为：

xF=xT-Δx,yF=yT,zF=zT。

(4)

3 黄瓜采摘机械手控制系统设计

系统采用基于DSP的上位机运动控制器+下位机关节DSP控制器的多CPU、分布式控制结构，并采用有效支持分布式控制和实时控制的CAN总线通讯，以驱动各个关节协调工作。机械手运动规划控制系统框图如图3所示。

3.1 上位机控制系统硬件

上位机运动规划控制系统主要由电源模块、dsPIC30F4012最小系统电路、CAN总线通信模块、串口模块与固态继电器控制模块组成，用来完成逆运动学计算、轨迹规划、插补运算、末端执行器控制与视觉系统及下位机关节控制器的通信任务等。电源模块实现对运动控制器的供电，dsPIC30F4012最小系统是处理器能够正常工作的最低硬件要求，包括复位电路、时钟电路、ICD2仿真下载电路、LED测试电路。

dsPIC30F4012提供一个异步串行模块UART，可实现半双工或全双工通信，波特率范围为38 bps～1.875 Mbps[11]。由于dsPIC30F4012的U1TX和U1RX引脚上是TTL电平，而RS232接口的标准电平采用负逻辑；因此，必须把TTL电平与RS232电平进行相互转换，本系统采用MAXIM公司的MAX3221E芯片来完成电平转换。

图4 CAN总线接口电路图

CAN模块用于和机械手各关节的通信，黄瓜采摘机械手采用一点对多点的通信方式，上位机运动控制器和4个关节控制器的CAN模块均采用dsPIC30F4012内嵌标准CAN控制器和CAN收发器MCP2551组成。MCP2551是CAN控制器和物理总线之间的接口，工作速率高达1 Mbps，它的TXD端和RXD端分别接dsPIC30F4012的CANTX端和CANRX端，CANH和CANL连接到总线上，完成与总线的通信，如图4所示。

3.2 下位关节控制器硬件

下位关节控制器由dsPIC30F4012最小系统、H桥驱动电路、信号反馈电路、CAN总线通信模块、保护与故障处理模块、AS5045转角反馈模块组成。主要负责关节电机的位置控制、反馈信号处理、接收上位机控制命令并向上位机发送各关节位置信息等。

dsPIC30F4012中的电机控制PWM模块产生PWM信号，作为电机驱动电路的基极信号，H桥电路由4只场效应晶体管IRF3250组成，并由专用驱动控制芯片IR2104来驱动，从而控制直流电机的正反转。如图5所示。

图5 H桥驱动电路

电机位置传感器采用瑞士Austria Microsystems公司的AS5045无接触式磁旋转编码器。它是一个片上系统，整合了集成式Hall元件，提供用户可编程的零位置设定和诊断，内嵌DSP引擎，能够检测磁场的方向并计算出12位的二进制编码，可通过同步串行接口(SSI)访问绝对角度值，其分辨率达到0.087 9°，用于精确测量整个360°范围内的角度。测量角度时，只需简单地在芯片中心的正上方放置一个双极磁铁即可。

3.3 上位机运动控制系统软件

上位机运动控制系统主程序流程图如图6所示。

系统上电后，机械手首先从非工作状态初始位姿运动到工作状态初始位姿，然后检查是否有串口中断(视觉系统将目标黄瓜抓取点位置获取后发出)，如果有中断，则可进行采摘机械手的逆运动学运算，反之，继续查询；然后上位机运动控制器向关节控制器发送采集当前位置信息的指令，并通过CAN接收中断检查是否将信息发出，当上位机收到关节控制器的当前位置信息后，进行轨迹规划和插值运算，并将规划出的位置控制量通过CAN总线发到各关节控制器，驱动电机到达指定位置。当机械手到达目标采摘点后，上位机控制末端执行器完成黄瓜抓持与切割操作，最后机械手运动到黄瓜放置点，松开手抓，黄瓜在重力作用下落入收集器皿中，完成一根黄瓜的采摘任务。

图6 上位机控制系统主程序流程图

3.4 关节控制器软件

关节控制器整体流程图如图7所示，主要包括系统初始化及各功能模块初始化子程序、电流环调节子程序、CAN通信子程序、A/D采样中断子程序、位置环调节子程序、AS5045电机转角采集子程序等。

关节控制器上电进行初始化后，便检查是否有CAN接收中断。如果没有，则继续查询，反之，则开始接收上位机发送的位置指令；启动AD并采样流经关节电机电流，判断系统是否过流，如果是，则置位错误标志，并使电机停转；如系统处于正常工作状态，则调用电流环控制模块；然后采集AS5045编码器产生的反馈位置信号，进行PID调节，并将控制量通过PWM模块输出，从而控制关节电机达到期望位置。

4 黄瓜采摘机器人实验

为验证运动规划算法及控制系统设计的合理性，对机械臂进行了定位精度测试实验。以目标采摘区域中10个采摘点进行实验，实验环境如图8所示。

图7 关节控制器流程图 图8 采摘机器人定位精度实验平台

具体试验步骤如下：

(1)对于给定的采摘点，求其逆运动学方程，求解方法见文献[12]，以其中1组为例：px=613.4 mm、py=267.6 mm、pz=425 mm，代入逆运动学方程，可得对应的4个关节角度值：θ1=23.57°、θ2=76.49°、θ3=-64.14°、θ4=-12.36°。

(2)采用基于摆线运动的关节空间轨迹规划方法，由上位机运动控制器对机械手进行从工作状态初始位姿到目标采摘点的轨迹规划和插值运算，并将规划出的关节位置指令通过CAN总线发送到各关节控制器，从而驱动机械手到达指定位置。规划时间取3 s，步长N取200，采用定时插补方式实现。

(3)机械手运动到指定位置后，利用美国法如公司生产的便携式三坐标测量仪Platinum FaroArm测量机械臂末端位置坐标，其测量精度可达0.013 mm。测量结果如表1所示。

表1 机械手末端位置坐标测量结果

由表1可知：x轴方向误差较小，最大为4.3 mm；y轴和z轴方向较大，y轴最大误差为6.9 mm，z轴最大误差为-6.7 mm。尽管z轴误差偏大，但由于黄瓜果实的抓取点是在果蒂的中心处，果蒂的长度一般为30 mm～50 mm；而z轴误差方向又是垂直向下的，故z轴误差不会影响到正常的采摘作业。但y轴误差可能会造成末端手爪碰撞目标黄瓜而导致不能抓持黄瓜，影响采摘作业。机械手位置误差产生的原因主要有：(1)各组成连杆结构参数的误差，主要是由于加工装配引起的误差；(2)各关节的运动变量误差引起的位姿误差。

5 结 语

(1)设计了模块化智能关节并构建四自由度采摘机械手。该机械手结构简单紧凑，重量轻成本低，动作灵活平稳，具有足够的强度和承载能力。

(2)结合黄瓜采摘机器人自身结构特点与果实采摘作业要求，提出了基于摆线运动的水平抓取轨迹规划方法，将其应用于采摘机器人关节空间的轨迹规划，实现机器人快速平稳运动。

(3)提出了采用基于DSP的上位机运动控制器+CAN总线+下位机DSP关节控制器的分布式控制方案，搭建了控制系统的硬件平台，并进行了软件设计。

(4)机械手定位精度测试实验表明：定位精度可达6.9 mm，基本能满足黄瓜采摘作业的要求，只是定位误差需要进一步提高。

[1] Kondo N,Monta M.Basic study on chrysanthemum cutting sticking robot[C]//In proceedings of the international symposium on agricultural mechanization and automation,1997,1:93-98.

[2] Foglia M M,Reina G.Agricultural robot for radicchio harvesting[J].Journal of filed robotics,2006,23(6):363-377.

[3] Van Henten E J,Van Tuijl B J,Hemminget J,et al.An autonomous robot for harvesting cucumbers in greenhouses[J].Autonomous robots,2002,13(3):241-258.

[4] 徐丽明.草莓收获机器人系统的研究[D].北京:中国农业大学,2006:40-46.

[5] 宋健,孙学岩,张铁中等.开放式茄子采摘机器人设计与试验[J].农业机械学报,2009,40(1):143-147.

[6] 梁喜凤,王永维.番茄收获机械手奇异性分析与处理[J].农业工程学报,2006,22(1):85-88.

[7] Neelam R P,Kamal T S.Intelligent planning of trajectories for pick-and-place operations[C]//In Proc of International Conference on System,Man and Cybernetics,2000:55-60.

[8] Gasparetto A,Zanotto V.A new method for smooth trajectory planning of robot manipulators[J].Mechanism and Machine Theory,2007(42):455-471.

[9] Jorge Angeles.机器人机械系统原理理论、方法和算法[M].宋伟刚译.北京:机械工业出版社，2004:141-143.

[10] 庄鹏，姚正秋.基于摆线运动规律的悬索并联机器人轨迹规划[J].机械设计,2006(9):21-24.

[11] 何礼高.dsPIC30F电机与电源系列数字信号控制器原理与应用[M].北京:北京航天航空大学出版社,2007:32-42.

[12] Zhang L B,Wang Y,Yang Q H, et al.Kinematics and trajectory planning of a cucumber harvesting robot manipulator[J].International Journal of Agricultural and Biological Engineering,2009,2(1):1-7.

Design on Motion Planning and Manipulator Control System of Cucumber Picking Robot

WANG Yan

(Luoyang Institute of Science and Technology, Luoyang 471023, China)

Aiming at the existing problems of picking robot such as ponderous mechanism, high price, low intelligence, an articulated manipulator with four degree-of-freedom is developed. The horizontal grasping motion planning algorithm of two combined cycloidal motion is proposed, considering the characteristics of cucumber picking operation. And, the distributed control structure of the picking manipulator with DSP upper monitor+CAN bus+DSP joint controllers is presented. In order to verify performances of the manipulator and its control system, the positioning accuracy experiments are carried out, and results show that position accuracy of the manipulator can reach to 6.9mm, which basically meets the requirements of the cucumber picking operation.

cucumber picking manipulator; motion planning; control system

2016-12-29

王燕(1981-)，女，河南驻马店人，博士，讲师，主要从事机器人技术及智能仪表方面的研究.

河南省高等学校重点科研项目(16A413007).

10.3969/i.issn.1674-5403.2017.02.016

TP273+5

A

1674-5403(2017)02-0056-05