Research On Trajectory Planning Of Tomato Cluster Harvesting Manipulator And Anti-swing Control Of Tomato Cluster

Application of harvesting manipulators will help to improve agricultural production efficiency,and to provide support for improving production pattern,addressing labor shortage,and realizing large scale,precision and industrialization of agriculture.Due to the low efficiency and fruit damage of single-fruit picking robot,this paper took a kind of 7-DoF tomato cluster harvesting manipulator as the research object,the trajectory planning of the whole manipulator and the anti-swing positioning problem of the tomato cluster were studied.The modified Denavit-Hartenberg convention was used to establish the link coordinate frames of the manipulator,and the parameters of each link were calculated to obtain the kinematics model.The vector-product method was used to solve the Jacobian matrix of the manipulator.The correctness of the manipulator model was verified by Matlab/Robotics toolbox.The dynamics equation of the manipulator is established by the Lagrange approach.The polynomial interpolation method,linear interpolation method with parabolic transition,cycloid method and B-spline curve method were used to plan the movement of the end effector from the picking point to the reset point.Experiments show that the quintic polynomial interpolation meet the boundary conditions of joint path points.Planned trajectory is continuously smooth,and the algorithm has good real-time performance.Aiming at the problem that the tomato cluster will be swing and be damaged when the end effector holding the tomato cluster moving to another point.The input shaping approach as one of the active vibration control technique was introduced.Its working principle and design process were expatiated,and took the single DoF vibration system to verify the effectiveness of this method.The results show that the ZVD shaper and EI Shaper have better vibration suppression performance on the system with modeling errors.The physical properties of the tomato cluster were measured in the field to obtain average value of them.The Lagrange approach was used to construct the dynamics model of the end effector and tomato cluster system,and obtain the transfer functions among the input torque,the displacement of end effector and the swing angle by linearization processing.The system responses under the constant force input were verified,the results show that system modeling and linearization are effective.The anti-swing positioning control system of the end effector-tomato fruit cluster system was designed based on the double closed loop PID,and the positioning and anti-swing effect were confirmed.The robustness of the control system by changing the quality of the tomato cluster and the length of the stem was studied.The simulation experiments show that the double closed loop PID control has better effect on the end effector positioning and the swing angle suppression of the tomato cluster,but there is a shortcoming of long adjustment time.Therefore,the input shapers were designed based on the reduced-order system model.The simulation experiments show that the input shaper has good performance on system positioning and anti-swing control,strong robustness to system parameters.Finally,the experimental platform of the control system was built,and the STM32F103ZET6 is used as the main control chip and the stepping motor as the motion actuator to verify the designed control system.