AMR果蔬自动收获机器人动力学建模与实时控制

曹 东，田富洋，宋占华，闫银发，李法德，蔡占河

AMR果蔬自动收获机器人动力学建模与实时控制

曹 东1，田富洋1，宋占华1，闫银发1，李法德1，蔡占河2

(1.山东农业大学 机械与电子工程学院/山东省园艺机械与装备重点实验室，山东 泰安 271018；2.菏泽市产品检验检测研究院，山东 菏泽 274000)

现代农业的高速发展促使我国农业向精细农业、精准农业的方向深入发展，由此果蔬自动收获机器人的发展进入快车道。为此，设计研发了带有6自由度机械臂的果蔬自动收获机器人AMR1。该机器人为通用型果蔬自动收获机器人，可改装各种专用末端执行器以实现专用采摘。为了验证本机器人结构的合理性与实用性，通过利用SolidWorks软件进行了实体建模，并建立了相关的运动学、动力学模型，进一步设计了6自由度机械臂的PID-SMC控制算法，实时控制机械臂末端的轨迹跟踪。通过试验仿真，验证了所提出的动力学和控制模型，为果蔬自动收获机器人AMR1的进一步应用提供了参数数据。

机器人；自动收获；建模；仿真

0 引言

经济的高速发展与科技的不断进步促使农业生产模式的变革与生产新技术的不断融合，智能农业机械的技术条件日趋成熟，是当今农业机械发展的主流方向。农业机器人的应用与推广是现代农业向精细农业、精准农业发展的必由之路，而果蔬自动收获机器人是智能农业机械装备的典型。果蔬自动收获机器人是一种高度智能化和自动化的农业机械装备，集传感技术、图像识别技术、自动导航技术、通讯技术、计算机技术及系统集成技术等多种前沿科学技术于一身，代表了机电一体化的最高水平[1]。果蔬自动收获机器人作为新一代的生产工具，在提高果蔬生产力、改变果蔬收获模式及降低劳动力成本等方面彰显出极大的优越性，果蔬自动收获机器人的应用将带来农业生产的又一次技术革命。本文设计研发了果蔬自动收获机器人AMR1，并通过SolidWorks实体建模与ADAMS运动学、动力学仿真验证AMR1果蔬自动收获机器人结构的合理性与实用性，为后续的智能控制算法与基于机器视觉的自动收获程序设计做准备。

1 果蔬自动收获机器人AMR的建模

1.1 总体结构

本文设计的果蔬自动收获机器人AMR1整体机构由智能行走装置与自动收获装置构成。智能行走装置为四轮移动平台，采用后轮驱动、前轮转向的结构。自动收获装置由果蔬存储箱与6DOF(6R)采摘收获机械手构成[2]，整体机构如图1所示。

1.前轮转向机构 2.机械手末端夹持器 3.采摘机械臂 4.自动控制室 5.果实存储厢 6.后轮驱动机构图1 AMR果蔬自动收获机器人结构简图Fig.1 Sketch of Automatic harvesting robot modeling diagram

1.2 主要工作部件设计

根据调查结果显示，田垄间距因果蔬品种而异。以黄瓜、茄子、西红柿、芸豆为例，垄距一般在300～700mm之间，果蔬高平均在300～1 800mm之间，AMR1自动收获机器人车轮间距取均值500mm，车体高500mm。其前后轮均采用冗余伺服驱动，即每个车轮独立悬挂驱动，采用伺服电机配减速机输出驱动力是为了获取更高的位置精度。冗余驱动是考虑伺服电机相对步进电机、普通直流电机而言输出扭矩偏小，双驱以获取足够动力。后驱采用链传动以获取足够传动强度来适应田间作业，前驱与自动收获机械手的转向均采取同步带传动，以获取足够转向精度，达到自动收获机器人田垄间自动转向与机械手精确定位的目的，来提高收获效率。果蔬存储厢由果蔬存储室与自动控制室构成，自动收获机械手为6DOF型机械手，前3个关节确定机械手的方位，后3个关节模拟腕关节确定自动收获机械手的姿态。为了简化D-H参数，提高运动学逆解效率。6个旋转关节可串联在同一条直线上而不存在关节偏置，既增大了可操作空间，又节省了机器成本。底部腰关节600mm，可360°转向；腰关节上部为大臂和小臂，其长度分别为700、400mm；大臂和小臂都可270°转向。为了模拟腕部关节，实现末端执行器依据果实位置调整最佳姿态高效采摘，由文献[3]可知：自动收获机械手运动学逆解封闭,腕部三关节的旋转轴必须相交于一点，因此在小臂上部依次串联相互垂直的3个旋转微关节，旋转角度度依次为360°、270°和360°。

1.3 工作原理

自动收获机器人通过获取车体上部双目视觉图像信息确定果实位置，由控制程序根据反馈信息自动计算运动学逆解及相关动力学，如雅克比力距矩阵解算。向自动收获机械手发送采摘信号，调整每个关节姿态对果实进行精确采摘；然后，调整各控制参数(关节运动角度，伺服电机力矩等)，将果实放置后部的果蔬存储厢，依次循环采摘收获；到达田垄边界依据轨迹规划实现自动转向进行下一采摘收获工作。通过下一阶段虚拟样机在ADAMS中进行相关运动学、动力学仿真，以模拟较为复杂的机械系统运转，提高系统设计性能、节约时间和成本，同时验证本设计结构的合理性与高效性。

2 农业机器人操作臂运动学反解算法

将机械臂各关节变量表示为qi(i=1,2,…,qn)，该值由各关节传感器输出获得。根据文献[1],应用Denavit-Hartenberg方法，可以得到n自由度的农业机器人操作臂运动学方程为[3]

(1)

逆运动学的解法有多种，Pieper解法适用于本机器人，但机器人的强耦合性、强非线性及噪声等一系列不确定因素给精确的数学模型建立带来影响。由于BP神经网络具有较强的非线性映射能力，可以解决操作臂运动学反解问题，因此本机器人选用BP神经网络去拟合非线性的反解问题。

(2)

其中，Atan2(y,x)是双变量反正切函数。神经网络训练样本取关节变量qi(i=1,2,…,6)代入公式(1)随机获得。BP神经网络结构简图如图2所示。

神经网络在训练时选取2 000组数据，每组数据包括6个输入矢量和6个输出矢量；神经网络结构中输入矢量为(∂,β,γ,px,py,pz)；输出矢量为(q1,q2,q3,q4,q5,q6)；输出矢量根据各关节允许转动范围随机取数并组合在一起，输入矢量根据正运动学公式计算得出。神经网络选用单隐层，隐层神经元个数为21，训练误差曲线图如图3所示。

由图3可以看出：神经网络经过77次迭代均方根误差下降到目标误差0.015以内，5次迭代之后下降趋势较明显，耗时短、效率高且继续进行多次训练误差还可以进一步减小。根据实验结果，该方法基本满足本农业机器人操作臂控制要求。

图3 神经网络训练误差曲线图Fig.3 Error of Neural network training curve

3 农业机器人广义动力学模型

该农业机器人在摘取、喷雾及施肥等操作过程中，驱动轮需要进行欠驱动达到最优的前进路线，并且机械手需要更加灵活地到达目标位置，需要考虑手臂在运动过程中快速地避开农业作物，因此机器人系统中既含有驱动轮又含有被动轮。机械臂在抓取农产品目标过程中既含有主动铰又含有被动铰，该类机器人系统称为欠驱动农业机器人。

根据文献[4]，欠驱动农业机器人系统动力学方程可表述为

其中，Maa为主动关节系统的质量矩阵；Mpp为被动关节质量矩阵；Map为耦合质量矩阵。

对公式(3)按照空间算子代数理论对树型机械多体系统的质量矩阵进行分解，即

可得系统的动力学微分-代数方程组为

(4)

其中

相关算子参数可以参考文献[5]。

4 PID滑模变结构(PID-SMC)控制设计

根据方程(4)本文控制可以表示为

(5)

xd(0)=x(0)

(6)

式(6)可以说明一个可以执行的期望轨迹线，其初始的位置、速度应该与系统的初始位置、速度相同，这样避免了一个跟踪的过渡时间，提高了效率。

跟踪误差矢量记为

(7)

状态空间Rn中的滑动曲面S(x)记为

(8)

其中，λ是正常数，为进一步追踪减小误差，将积分部分用于滑动面，描述为以下公式，即

(9)

这种方法的主要目标是让滑动面s(x,t)斜率趋于0，因此可以选择公式(5)中的控制u，使在滑动面s(x,t)之外满足以下关系，即

(10)

其中，ζ是正常数，这就使得s(x,t)恒为0简化为一阶问题。式(10)表达的是：选s2(x,t)作为度量标准到滑动面的平方距离沿着系统的全部轨线减小并且当轨线进入滑面后就会一直停留在该滑面。

如果

(11)

消除倒数项，用到了一个从t=0到t=trch的积分

≤-ζ(trch-0)

(12)

式(12)中，t=trch是堆积到达滑动面的时间，所以假设S(trch=0)定义为

(13)

如果

(14)

如果

Strch=S(0)→error(x-xd)=0

(15)

假设S的定义为

(16)

(17)

假设二阶系统的定义为

(18)

(19)

(20)

其中，sgn(s)是符号函数，表达式为

(21)

(22)

通过使用积分控制，可以得到以下等式，即

(23)

用这种方法U的逼近值可以表示为

(24)

综上所述，多自由度机械臂的滑模变结构控制公式可以表示为

τ=τeq+τdis

(25)

τeq是名义动力学系统参数，可以表示为

(26)

τdis可以表示为

τdis=K·sgn(S)

(27)

代入等式(23)得

τ=τeq+K·sgn(S)

(28)

图4是本文六自由度机械臂的滑模变结构控制示意图。由等式(25)～式(28)得该机械臂的滑模变结构控制的表达式为

(29)

图4 六自由度机械臂的滑模变结构控制示意图Fig.4 Schematic diagram of sliding mode variable structure control for Six-DOF Robot Manipulator

5 仿真分析

5.1ADAMS仿真

由于ADAMS在实体建模方面的效率与细节处理效能上远不如SolidWorks/Proe/UG等专业三维设计软件，因此本文依据上文设计参数在SolidWorks进行实体建模，虚拟样机如图5所示。为提高ADAMS的仿真效率，在将三维模型导入ADAMS之前进行必要的零件的合并，精简后模型如图6所示。AMR果蔬自动收获机械手末端轨迹曲线如图7所示。

图5 AMR果蔬自动收获机器人实体模型Fig.5 Entity model of vegetable and fruit automatic harvesting robot AMR

图6 AMR果蔬自动收获机器人精简模型Fig.6 The simplified model of vegetable and fruit automaticharvesting robot AMR

图7 AMR果蔬自动收获机械手末端轨迹曲线Fig.7 The end-effector trajectory curve of vegetable and fruitautomatic harvesting robot AMR

模型自检无干涉，进入后处理模块观测各关节角度、角速度、角加速度变化，如图8～图10所示。

图8 AMR果蔬自动收获机械手6R关节角速度变化图Fig.8 The joint angular velocity variation of vegetable and fruitautomatic harvesting robot AMR

图9 AMR果蔬自动收获机械手6R关节角度变化图Fig.9 The joint angle variation of vegetable and fruit automaticharvesting robot AMR

图10 AMR果蔬自动收获机械手主要关节角加速度变化图Fig.10 The major joint angular acceleration variation of vegetable andfruit automatic harvesting robot AMR

整个收获过程中，自动收获机械手处于欠驱动状态，即采摘收获过程中机械手各关节顺序动作是为了[8]：一方面，在抓取果实过程中调整最佳位姿，避免触碰果实对果实造成损伤；另一方面，抓取后回收过程中障碍(避免自动收获机械手碰撞果实存储厢等)，顺利将果实放入果蔬采集箱内。由图8和图9可看出：角度角速度变化平顺，而角加速度力矩波动较大局部波幅较大。这是由于各关节顺序动作造成的伺服电机在启动的瞬时需克服阻力矩产生足够加速度，因此造成波形震动现象的发生[10]。上述变化曲线不仅说明的仿真的正确性，更为伺服电机的选型提供了参数依据，可根据曲线变化的峰值乘以动载荷系数推算本自动收获机器人各关节所需伺服电机的输出扭矩。

5.2 控制算法仿真

为验证本文提出的控制算法的效率和精度，利用如图11所示机器人，采用滑模变结构控制、PID反馈控制、滑模变结构PID控制对6自由度机械臂末端轨迹进行控制，仿真结果如图12～图17所示。试验结果表明：本文提出的控制算法高效有效，精度较高。

图 11 农业轮式机器人实物图Fig.11 Practicality picture of Agricultural Wheeled robot

图12 PID控制机器人末端轨迹Fig.12 The trajectory tracking of end-effector based on PID control

图13 PID控制机器人末端轨迹误差Fig.13 The trajectory tracking error of end-effector based on PID control

图14 滑模变结构控制机器人末端轨迹Fig.14 The trajectory tracking of end-effector based on SMC control

图15 滑模变结构控制机器人末端轨迹误差Fig.15 The trajectory tracking error of end-effector based on SMC control

图16 滑模变结构-PID控制机器人末端轨迹Fig.16 The trajectory tracking of end-effector based on SMC-PID control

图17 滑模变结构-PID控制机器人末端轨迹误差Fig.17 The trajectory tracking error of end-effector based onSMC-PID control

6 结论

本文通过SolidWorks建立果蔬自动收获机器人的

虚拟样机，利用BP神经网络解决运动学反解问题，建立了机器人在工作工程中的动力学模型，并在ADAMS中进行相关运动学、动力学仿真，获取采摘收获机械手的各项参数尤其是采摘收获过程中所需最大力矩。同时，进一步设计了农业机器人末端轨迹的滑模变结构PID控制算法。该研究为样机伺服电机的选型提供了可靠依据，降低了生产成本，缩短了设计周期，大大降低了特定采摘对象的自动收获机器人研发周期。通过对收获机器人机械臂的控制研究，找到较好的控制方案，提高了自动收获机器人采摘效率。

[1] 常勇,马书根,王洪光,等.轮式移动机器人运动学建模方法[J].机械工程学报,2010(5):30-36.

[2] 谷侃锋,赵明扬.轮式移动机器人沙地行驶控制建模与仿真研究[J].系统仿真学报,2008(18):5035-5039.

[3] 熊有伦.机器人技术基础[M].武汉:华中科技大学出版社,2014.

[4] 田富洋，吴洪涛，赵大旭. 一类柔性宏刚性微空间机器人广义动力学建模研究[J].宇航学报，2010,31(3):687-694.

[5]SunT,PeiH,PanY,etal.Neuralnetwork-basedslidingmodeadaptivecontrolforrobotmanipulators[J].Neurocomputing, 2011(74):2377-2384.

[6] 王燕,杨庆华,鲍官军,等.关节型果蔬采摘机械臂优化设计与试验[J].农业机械学报,2011(7):191-195.

[7]CYChen,THSLi,YCYeh.EP-basedkinematiccontrolandadaptivefuzzysliding-modedynamiccontrolforwheeledmobilerobots[J].InformationSciences,2009，179：180-195.

[8] 梁喜凤,周涛,王斌锐.果实收获机器人关节滑模控制系统设计与试验[J]. 农业机械学报,2016,47(3)：1-7.

[9]RobertS.Twoapproachesforfeedforwardcontrolandoptimaldesignofunderactuatedmultibodysystems[J].MultibodysystemDynmics, 2012,27:75-93.

[10]OscarC,HéctorN,JoséS,etal.AnewapproachfordynamicfuzzylogicparametertuninginAntColonyOptimizationanditsapplicationinfuzzycontrolofamobilerobot[J].AppliedSoftComputing, 2015,28:150-159.

[11]FranciscoG,RossomandoN,CarlosS,etal.AutonomousmobilerobotsnavigationusingRBFneuralcompensator[J].ControlEngineeringPractice,2011,19:215-222.

[12]Nakazonok,Ohnishi,Kinsjoh,etal.VibrationcontrolofloadforrotarycranesystemusingneuralnetworkwithGA-basedtraining[J].ArtificialLifeandRobotics,2008,13:98-101．

ID:1003-188X(2018)02-0007-EA

The Dynamics and Control of AMR Vegetable and Fruit Automatic Harvesting Robot

Cao Dong1, Tian Fuyang1, Song Zhanhua1, Yan Yinfa1, Li Fade1, Cai Zhanhe2

(1.College of Mechanical and Electronic Engineering, Shandong Agricultural University/Shandong Provincial Key Laboratory of Horticultural Machineries and Equipments，Tai’an 271018, China；2.Product Inspection and Research Institute of Heze, Heze 274000,China)

Abstract： The rapid development of modern agriculture promote Chinese Precision Agriculture development deeply. Thus, the fruit and vegetable automatic harvesting robot have already entered quick development stage. This paper designed and developed the fruit and vegetable automatic harvesting robot AMR1. The robot is a universal automatic harvesting robot for fruits and vegetables, it can achieve special picking by various special end effectors in order to verify the practicality of the robot structure. In this paper, we use SolidWorks software to carry on the entity modeling in order to verify Special-purpose of harvesting.We also established the kinematics and dynamics models.The PID-SMC control algorithm of 6 degree of freedom manipulator is designed further to real-time control of the trajectory tracking of robot arm. The simulation results show that the proposed dynamic and control model is reasonable.Data of the simulation reduced production costs and improved manufacturing efficiency.

robot; automatic harvesting; dynamics; modeling; simulation

2016-11-28

国家自然科学基金项目(51205238)

曹 东(1991-),男，山东济宁人，硕士研究生，(E-mail) sdaucd@126.com。

田富洋(1979-)，男，山东泰安人，副教授，硕士生导师，(E-mail)sdautfy@163.com。

S225;TP242

A

1003-188X(2018)02-0007-07