Collision-Free Path-Planning for Six-DOF Serial Harvesting Robot Based on Energy Optimal and Artificial Potential Field

1College of Mechanical and Electrical Engineering, Foshan University, 18 Jiangwan Road, Foshan, 528000, China 2Key Laboratory of Key Technology on Agricultural Machine and Equipment, Ministry of Education, South China Agricultural University, 483 Wushan Road, Guangzhou 510642, China 3College of Mechanical and Electrical Engineering, Chongqing University of Arts and Sciences, 319 Honghe Road, Yongchuan, Chongqing 402160, China


Introduction
Planning optimal paths is an important branch in the research field of intelligent robot and an ideal path planning method is very important for improving the performance of robots [1,2].In agricultural production, fruit harvesting is a timeconsuming and labour-intensive procedure, and the study on timely harvesting of fruits using robotics manipulator is becoming a significant challenge for researchers [3].To accomplish the task of fruit harvesting successfully, automatic obstacle-avoidance path planning of harvesting manipulator is one of the key technical problems of fruit harvesting robot [4].In the process of solving the path planning problem, if the environment information is completely knowable, the time global planning can be used to obtain an optimal path to reach the target state from the initial state [5].However, the harvesting robot usually operates in a nonstructural and complex natural environment, and its work space often involves various uncertainties which robots must evade, such as branches and immature fruits which distributed around ripe fruits (harvesting target).So the dynamic uncertainty of the obstacle and the location of the picking target make the path planning a difficult problem.In such case, it can only be based on the real-time detection of the environmental information and reprogramming to get the moving path from the current state to the target state.
In recent years, scholars from different parts of the world have proposed many methods and strategies for solving anticollision path for automatic robot in uncertain environment and dynamic obstacle environment [6].These studies can be divided into four primary categories [7]: gridbased approaches, potential field approaches, sample-based approaches, and discrete optimization approaches.In [7], a real-time dynamic path planning method for autonomous driving that avoids both static and moving obstacles was presented.In [8], two efficient sampling-based approaches combining two RRT variants were developed to solve a complex path planning problem, and a path planning method which uses mutual information was proposed for the multi-AUV (Autonomous Underwater Vehicle) system.Cui et al. [9] presented an adaptive path planning algorithm for multiple AUVs to estimate the scalar field over a region of interest.To make the manipulator avoid collisions with the plant stem in sweet-pepper harvesting, Bac et al. [10] presented a stem localization of sweet-pepper plants using the support wire as a visual cue.The objectives of this study were to find the obstacles so that the robot can plan a collision-avoidance path.To conduct an undamaged robotic grape-harvesting, Luo et al. [11] focused their works on locating the spatial coordinates of the cutting points on a peduncle of grape clusters for the end-effector and determining the bounding volume of the grape clusters for the motion planner of the manipulator.A three-dimensional CAD model used in [12] has solved the problem of collision between the cucumber picking robot and the stem, but the amount of collision calculation is very large.To decrease the movement uncertainty of industrial robots, Gao et al. [13] presented a parameter identification method based on the D-H model, in where the redundant parameters are particularly addressed.Experimental studies based on a 6 DOF industrial robot show the proposed method can improve the movement accuracy of the industrial robot significantly.To make the robotic manipulator work efficiently and safely in a dynamic unstructured environment, Wei et al. [14] proposed an autonomous obstacle avoidance dynamic path-planning method for a robotic manipulator based on an improved RRT algorithm.For solving serial manipulators path-planning when grabbing target in obstacle workspace, Dai et al. [15] presented a solution based on screw theory and ant colony algorithm.In [16], a new approach to time-optimal path parameterization (TOPP) based on reachability analysis was proposed.
Through the above analysis, we can see that the path planning strategies applied in unstructured environments in recent years focused on autonomous driving, autonomous underwater vehicle, and industry robots.Although some domestic and foreign scholars have carried out some research on the path planning of fruit and vegetable picking robot, the research on collision-free autonomous path planning for the grape harvesting robot under a dynamic vineyard environment is still in the initial stage.
To conduct an undamaged robotic grape-harvesting in an unstructured vineyard, a collision-free autonomous path planning method based on minimum energy and artificial potential field for grape harvesting robot was developed in this study.Firstly, the kinematical model of six-DOF serial harvesting manipulator was constructed by using the D-H method, the model of obstacles was defined by the axisaligned bounding box, and then the configuration space of harvesting robot was described by combining the obstacles and arm space of robot.Secondly, the harvesting sequence in path planning was computed by energy optimal method, and the anticollision path points were generated based on the artificial potential field and sampling searching method.Finally, to verify and test the proposed path planning algorithm, a virtual test system based on virtual reality was developed.The innovation of this study is the development of the energy optimal harvesting sequence planning method and the anticollision path points generating method based on the configuration space of six-DOF serial harvesting manipulator.
The paper is organized as follows: Section 2 shows several basic models for harvesting robot that will be used in the path planning strategies.Section 3 describes the plan planning method that was designed based on energy minimum and artificial potential field.Section 4 performed an experiment to validate the performance of the developed plan planning method.Section 5 summarizes and discusses the experiment results.The paper ends with the conclusions and future work.

Basic Models Building for Harvesting Robot
The task of obstacle avoidance path planning of the six degree of freedom manipulator is to search for a series of continuous joint angle values under the condition of the given obstacle, the starting, and target position of the manipulator, which can drive the manipulator to move from the starting position to the target position safely without collision.To elaborate the principle and process of the path planning method proposed in this paper in detail, it is necessary to establish the robot coordinate system, kinematic model, obstacle model, and the configuration space model, which are the design basis of the path planning algorithm.
. .Kinematical Modelling for Six-DOF Serial Robot.The shape of the six degree of freedom series harvesting robot is as shown in Figure 1(a), which is made up of six rotating joints.They are responsible for adjusting the end-effector to the appropriate picking position so that the actuator can clamp the grapes and cut the peduncles.The end-effector is composed of three functional parts: clamper, pallet, and scissor.The harvesting manipulator is made up of rigid link and rigid joint, and its motion is realized by controlling the joints of the manipulator.

. . . Establishing D-H Coordinate System for Harvesting Arm.
To establish the forward and reverse kinematics models of the harvesting robot, the Denavit-Hartenberg (D-H) method [17] was adopted in this study.The D-H table (see Table 1) is built on the basis of the structure and parameters of the robot.And then the coordinate system of the picking robot is set up, in which the reference coordinate system is O 0 and the local coordinate system of each joint is O i (i = 1, 2, . . .6) and the end-effector coordinate system is O h .The coordinate system of each joint of the picking robot is shown by Figure 1(b model, which is to calculate the position and attitude of the end-effector coordinate system O h relative to the base coordinate system O 0 by a given set of joint values.The transformation matrix between two adjacent connecting rods can be calculated by the D-H parameters in Formula (1) and the Table 1.
where   indicates sin   ,   indicates cos   ( = 1, 2, 3, . . ., 6),  -1 is the twist angle,  -1 is the length of linkages, and   is the offset of linkages The transformation matrix of the manipulator 0 6  can be obtained by multiplying continuously the transformation matrix of each connecting rod −1   (i = 1, 2, . . .6), which is a function with the six joint variables ( 1 ,  2 ,  3 ,  4 ,  5 ,  6 ) and can be described by Formula (2).It represents the position relationships between the end-effector attitude (, , , ) and the basic coordinate system [18]. where In the robotics grape harvesting, the three-dimensional coordinates of the cutting point on the peduncle of grape cluster are obtained by the visual system of robot, and the target position of the end-effector coordinate system O h can be calculated by the cutting point position.The attitude of the end-effector (, , , ) can be derived by combining with the growth posture of grape clusters.In the case of knowing the relative position between the target position of the endeffector and the current position, the inverse kinematics model is responsible for calculating the satisfied joint variable of the manipulator which can control the manipulator move from the current position to the target position.To construct the inverse kinematics model, the inverse transformation method [19] was used in this study, and the calculation formulas of these joint variable ( 1 ,  2 ,  3 ,  4 ,  5 ,  6 ) are as shown Formula (4).So the relation between the position of the terminal connecting rod and the joint variable of the manipulator can be established. where 2 )/2 2 To verify the correctness of the kinematics model, the values of a set of joint variables ( 1 = /3,  2 = -/3,  3 = /3,  4 = /6,  5 = /2,  6 = /4) is artificially allocated and then these values were input to the forward kinematic model (Formula (2)) to solve the terminal pose matrix 0 6 .
The following 0 6  was calculated by using Matlab.Then, the obtained 0 6  seen as known conditions was input to the inverse kinematic model (Formula ( 4)) to solve the angle values of the robot joints, and the joint values ( 1 = 60.0000, 2 = -60.0000, 3 = 60.0000, 4 = 29.9993, 5 = 90.0000,and  6 = 45.0022) were calculated.It can be seen that the result is basically consistent with the variable values allocated by manual, which shows the correctness of the kinematics model of the robot.
. .Obstacles Modeling.In the collision detection, the bounding volume is a simple geometric space, which contains objects of complex shape.The purpose of adding a bounding volume to an object is to quickly carry out collision detection or to filter the exact collision detection.The bounding body types include sphere, axis aligned bounding box (AABB), directed bounding box (OBB), 8-DOP, and convex hull.The obstacles in the actual picking environment are usually not regular geometry, so it is difficult to describe them with a precise model [20].To facilitate the collision avoidance path-planning, the axis-aligned bounding box (AABB) was adopted to establish the obstacles model approximately in this study.Although this modeling method expands the obstacle domain to a certain extent, it greatly decreases the calculation amount of the interference detection between the manipulator and the obstacles and effectively improves the efficiency of the path planning and also makes the planned path more secure.
The axis-aligned minimum bounding box for a given point set is its minimum bounding box subject to the constraint that the edges of the box are parallel to the coordinate axes.It is simply the Cartesian product of  intervals each of which is defined by the minimal and maximal value of the corresponding coordinate for the points in .In the obstacles modeling, the farthest and nearest points in X, Y and Z directions from the vertex collection of the obstacle are obtained firstly, and then the AABB box was built by using these points.The AABB box is a simple hexahedron, and each of whose side is parallel to a coordinate plane.Its length, width and height can be different from each other.In Figure 2, two simple 3D objects and their AABB are drawn.Some important properties of the AABB can be described as follows.Firstly, the points (, , ) in the AABB box satisfy the following conditions:  min ≤  ≤  max ,  min ≤  ≤  max , and  min ≤  ≤  max , and the points P min = [ min  min  min ] and P max = [ max  max  max ] were two important points of the AABB box.The center point c can be calculated by the equation (c = (P min + P max )/2).A bounding box of the obstacle can clearly be represented by using the two points P min and P max .
. .Configuration Space Modelling for Harvesting Robot.To solve a collision avoidance path for the harvesting robot, it is very importance to determine the configuration space of the manipulator.For a manipulator with  degrees of freedom, setting (@ 1 , @ 2 , . . ., @ n ) as a set of active joint values of it, in which @ i is the rotation angle of the -th active joint of the manipulator.If the point  represents a point on the manipulator, then the relative position relationship between the point and the obstacle can be expressed as follows [21]: where D(()) is relative position relationship between the point  and the obstacle when the manipulator is driven with the joint value and () is the position coordinate of the point  on the manipulator in the basic coordinate system when the joint value is .If the rods of the manipulator are uniformly dispersed into  points according to the step size  min (mm),  discrete points constitute a point set  link .The relative positional relation between the whole arm driven with the joint value  and the obstacle can be expressed as follows: in which D() is the relative positional relationship between the driven manipulator and the obstacle when the joint value is E. It can be known from formula (6) and formula (7) that the manipulator does not interfere with the obstacle only if D() = 0.The joint configuration space Ω is established with the joint values of the manipulators of n degrees of freedom.If the set of the joint value E of the manipulator is represented by H, then any point in the joint configuration space Ω uniquely corresponds to one of the elements in the set H and vice versa.In other words, the joint configuration space Ω and the set H are bijective.
Therefore, the relative positional relationship between the driven manipulator and the obstacle with the joint value E can be expressed by the following formula in the joint configuration space: where Ω() is the value of the point in the joint configuration space Ω with the element in the joint value  of the manipulator as the coordinate and represents the relative positional relationship with the obstacle when the 6 Complexity manipulator is in the position corresponding to .If the manipulator interferes with the obstacle, Ω() is equal to 1, otherwise Ω() is equal to 0. So the set  part ( 1 ,  2 , . . .  ) can represent a set of continuously varying poses of the manipulator, the set represents a set of discrete points in the joint configuration space Ω.If a set of discrete points is connected, a path can be obtained, where  is the number of the path control points.Only when the following formula is satisfied, does the driven manipulator not collide with the obstacle when the joint value set is  part .
where Ψ is the relative positional relationship between the manipulator the obstacle when the joint value set is  part .Therefore, the obstacle path planning is performed on the manipulator; that is, a set  part of joint values is searched in the joint configuration space Ω, so that the set is connected to the initial configuration joint value  s and the target joint value  g of the manipulator.Meanwhile, it satisfies the collision-free condition on the path as shown in (9).
To calculate the configuration space of the joint arm of the harvesting robot, a simulation model of the robot was established by using two functions of "link" and "drivebot" that were provided by Matlab Robotics Toolbox Toolbox [22].And then the motion range of the harvesting manipulator in the actual working environment is calculated and analyzed to carry better out the path-planning of picking operation.According to the structure of the robot, the motion space of the robot is mainly determined by the first three joints (1 to 3 linkages) and the latter three joints (4 to 6 linkages) have no effect on the motion space.The first three joints determine the position of the end-effector, and the last three joints determine the posture of the end-effector.At present, there are mainly three methods for solving the motion space of the robots: analytic methods, numerical methods, and graphic methods.The analytic method is to use algebraic method to obtain the equation about the structure parameters of the robot.The advantage of this method is that it can accurately describe the relationship between the structure parameters of the robot and the space of the robot, but its computation is large.In this study, we used analytic method and take Matlab to solve the robot's motion space, and the motion space of the harvesting robot can be shown as Figure 3.

Path-Planning Based on Energy Optimal and Artificial Potential Field
The work process of the grape harvesting robot is as follows: Firstly, the picking robot acquires the information on number of fruits in the field of view and the position of each fruit through the visual system.Secondly, the picking sequence is planned.The picking order of all fruits is sorted according to certain principles.Thirdly, controller has robot arm move to the picking point of first bunch of grape rapidly and the end effector performs the picking action.Fourthly, the second bunch of fruit is picked according to the above same procedure until all the fruit is harvested.Fifth, the robot arm returns to the initial posture, and the picking robot controls the mobile platform to move forward and then starts the next stage of picking.Therefore, the path planning of the picking robot work process is divided into two stages.The first stage is to plan the picking sequence and the second stage is to plan the path point for each bunch of fruit picking.The schematic diagram for the path planning of the grape harvesting robot can be shown as Figure 4.

. . Harvesting Sequence Planning Based on Minimum Energy.
As the requirement of multiobjectives harvesting, with the aim to get the minimum sum of weighted rotating joint angles in joint space, the objective function on six-degree-offreedom harvesting robot arm task planning based on minimum energy principle is proposed.By calculating the value of objective function, the picking sequence with the minimum energy consumption can be obtained.In this paper, an objective function to determine the harvesting sequence of grape bunches is proposed.The optimal harvesting sequence is finally determined by calculating the function value, thus solves the problem of harvesting task planning.
The harvesting sequence can be described in mathematically as assuming that the current manipulator is in the initial position of the harvesting ( 0 ,  0 ,  0 ), there are  bunches of fruits in the visual area to be picked, and the space position coordinates are ( i ,  i ,  i ), and the corresponding space variables of the manipulator are ( 1i ,  2i ,  3i ,  4i ,  5i ,  6i ), in which  = 1, 2, . . ... Manipulator is required to search for an optimal harvesting sequence from the initial position.All the fruits are picked sequentially and then returned to the original position.It is assumed that the motion parameters (including angular velocity and angular acceleration) of all joints of a manipulator are the same.In order to minimize the energy in the process of motion, when a joint of the arm carry out harvesting task, it selects the proper sequence of harvesting to minimize the total angle of rotation, which indicates that the operating time and energy consumption of the joint under this sequence to complete the harvesting task are minimal.The optimization objective of single joint is (10) in which  is the function of optimization objective,  is the motion angle of joint, and  is the number of the space points that end effector need to move through.However, there are multiple joints for the harvesting manipulator in this study, if the minimum sum of the single joint angle changes was took as the optimization objective, and then each joint would have an optimal objective.At this time, the problem of the harvesting sequence of the manipulator is transformed into a multiobjective optimization problem.Considering the angle changes of each joint of the manipulator may not reach the minimum at the same time in the same harvesting sequence, so it is necessary to coordinate the sub objective functions of each joint to achieve the optimal objective function and the optimum of each joint when determine the optimal objective function of the whole  manipulator.Moreover, the power of each joint motor is different, according to the value of the power of each joint motor of the manipulator, different weights should be given to each joint angle, and different weights should also be given to the sub targets of each joint of the manipulator.So the sequence planning of the six degree of freedom harvesting manipulator based on the principle of minimum energy can be described by the following: , −  ,+1      (11) where   indicates the objective function of the manipulator that can embody the energy consumption of the manipulator motion,  is the number of the joints,  i is the weight of the objective function of the th joints, and  i,j indicates the angle value of the th joint when the manipulator is locating the th space point.
. .Harvesting Path Generating Based on Artificial Potential Field eory.In the process of grape harvesting, collisionfree path planning under a vineyard environment is the most important issue which need be resolved firstly to avoid damage the fruits.At present, there are two main path planning methods [23]: model-based global path planning and sensor-based local path planning.In this study, the local path planning method based on artificial potential field was adopted.
. . .Principle of Artificial Potential Field.The path planning method based on artificial potential field is a virtual force method proposed by Khatib [24].Its basic idea is to design the motion of the robot in the surrounding environment into an abstract artificial gravitational field.The artificial potential field includes the gravitational field and the repulsion field, in which the object produces the gravitational attraction to the object and guides the object to move toward it.The obstacles repel other objects to avoid collisions with it.The resultant Input: the current point coordinates of the end-effector ( 0 ,  0 ,  0 ), the terminal coordinates ( t ,  t ,  t ) (the cutting point on the peduncles of grape cluster), the bounding box model of obstacles ( s ,  e ) Output: the path points ( i ,  i ,  i ).
1. Find all collision-free sampling points.2. for each sampling points p do (a) calculate the probability that the current point is the candidate path points.(b) find the point whose probability is maximum (c) consider the points as a candidate path point.end 3. Repeat the above process until the terminal coordinates is reached.
Algorithm 1: Sampling-based path point generating method.
force of every point on a path is equal to the sum of all repulsive forces and gravitational forces on this point.
The basic idea of the proposed path planning in this study is that the harvesting point is attractive to the motion of the harvesting manipulator by the virtual force field, the anticollision bounding body of grape cluster and other obstacles are repulsive to it, and the path planning is carried out through the interaction of attraction and repulsion force.The artificial potential field  sum () can be defined by the following [24]: in which  att () indicates the gravitational field generated by the target,  rep () indicates the repulsion field generated by the obstacles,  = (, , )  is the position of the end-effector in the workspace.Setting the position of the target as  g , and the gravitational field  att () can be defined by the following: where  is a constant about the gravitational field and then the gravitation  att () can be solved by calculating the negative gradient value of the gravitational field, which is described as Formula (14).
To define the mathematical expression about the repulforce field, the following two conditions need firstly be conformed [25]: (1) the artificial potential field  sum () must be a continuous-differential function, and when  is equal to  g , the value of  sum () is 0 (minimum value).(2) Under the effect of the artificial potential field  sum (), the system can be stable.Setting  0 as the space position of the obstacle, the repulsive force field  rep () can be defined by the following: where  is a constant about the repulsive force field and  is the maximum range of the impact of the obstacle.When the distance between the obstacle and the end-effector is larger than , the repulsive field would no longer to affect the motion of harvesting robot.The repulsive force  rep () can be solved by calculating the negative gradient value of the repulsive force field  rep (), which is described as the following: . . .Sampling-Based Path Points Generating Method.The array of the path point sets ( i ,  i ,  i ) are firstly established, and then the terminal pose of the end-effector is determined based on the current pose of the robot and the pose of grape cluster.The current point coordinates of the end-effector ( 0 ,  0 ,  0 ), the terminal coordinates ( t ,  t ,  t ) (the cutting point on the peduncles of grape cluster), and the bounding box model of obstacles ( s ,  e ) are given, respectively.According to the artificial potential field method, new sampling points are generated randomly in the neighbourhood and collision detection is performed on the sampling points.The probability that all collision-free sampling points are selected as the candidate path points is calculated, and the sampling point with the highest probability is added to the path array as the following path point.Repeat the above process until the terminal coordinates is reached.The pseudo code of sampling-based path point generating method is as shown in Algorithm 1.

Experiments and Results
To validate the performance of the proposed anticollision path planning algorithm in a complex orchard, a virtual harvesting robot simulation system was developed based on the virtual reality software platform, and then the performance of path planning method was investigated based on the developed simulation system.
. .Virtual Harvesting Robot Testing System.The virtual simulation system was implemented on the Windows 7 operating system by using the software tools such as Visual studio 2013, EON Studio, 3Dmax and Solidworks.In this study, the virtual harvesting robot is constructed according to the actual size and workspace of the 6-DOF robot prototype.Firstly, the 3D model of orchard scene and harvesting robot's body structure is constructed, and the kinematics model of the manipulator is set by D-H parameter method.Then the virtual grape clusters and their anticollision space bounding body are drawn under the virtual environment.The collision avoidance path points are computed by the proposed algorithm.Finally, the path is simulated by a three-dimensional visualization method.The end-effector on the harvesting robot is consists of three functional parts: clamping finger, pallet and scissor.The clamping mechanism is mainly responsible for clamping the peduncle of grape cluster.The pallet holds the grape cluster from behind and underneath, carrying part of grape gravity to prevent the grape from shaking and slipping.When the fingers and pallets lock up the grape cluster, the peduncle is cut by the scissors mechanism.The software interface is shown as Figure 5.
After the collision-free path is planned, the joint variables parameters of the manipulator are calculated by the inverse kinematics model, and then the forward kinematics method is used to control the robot's end-effector to the target position.In the virtual simulation system, the collision node in the EON platform was used to detect collision in this research.The node can detect the collision events between different geometric objects in the virtual environment, which can monitor the collision between the harvesting robot and the obstacle in real-time.To control the motion behavior of virtual harvesting robot, the modular programming and routing communication mechanism in EON platform were adopted.The main modules involved are movement, rotation, position sensor, angle sensor, time sensor, switch node, routing, and so on.The movement module is mainly used for the clamping behavior control of the end-effector.The rotation module is used to control the rotation of the six joints of the manipulator and the rotary cutting motion of the actuator scissors.The position and angle sensors are used to perceive the real-time travel and position of the joint motion.The time sensor is used to control the motion speed and acceleration of the joints of the manipulator, and the switch nodes are used to connect the communication between the modules.The communications among several function modules are carried out through routing mechanism in the virtual environment.The routing mechanism is responsible to mainly passes and monitors the running state of each module through events, and then broadcast the state parameters to other functional modules in real time, the coordination of the multifunctional modules is finally realized.Events are the data information transmitted between two domains, which include two types: Input and Output.The Input event is the information generated by other sending nodes to change the state of the receiving node, and the Output event is a state that the state of the node itself changes and outputs the information.When one of the behavior modules sends the command, the receiving node gets the instructions through the route, transfers the nodes through the event driven, and realizes the message driven mechanism.
. .Path-Planning Testing and Analysis.The path planning tests on multiple harvesting objectives were implemented base on the developed virtual simulation.The cluster number of the tested grape was set from 2 to 5, and their positions were randomly given in the configuration space of the manipulator.The harvesting sequence was firstly calculated by the energy optimal, and the path points were generated by artificial potential field.The simulation test platform are as follows: a laptop (Lenovo T430) with 4G RAM and Intel(R)Core(TM)i5-3230 M CPU@2.60 GHz, a Windows 7 operating system and a Visual C++ 2008 platform.The statistics and analysis are made on the path length, the number of sampling points, the collision situation and the average time of path planning in each harvesting.The results of the tests are shown as Table 2.The relationship between path length  and key point  i can be described by Formula (17).
where  indicates the number of sampling points of the path and ( i ,  i ,  i ) is the coordinates of the -th sampling points.
From the Table 2 we can see that there are 9 times is successful and 1 times failure in the 10 grape harvesting tests.The success rate was 90%, the path length was between 448.67 cm and 1472.65 cm, the number of sampling points was between 90 and 206, and the average time of path planning was between 98 ms and 273 ms.It can be seen that the reason for the failure of the test was the collision between the end effector and the anticollision space bounding body of the grape cluster.If the distance between the end effector and grape cluster is too small, the end effector will be easy to collide with the upper part of the grape cluster when the end effector move closely to the fruit peduncle for harvesting operation.Harvesting path-planning can be optimized by setting and adjusting the minimum safety margin between the cutting point and the anticollision space bounding volume.Moreover, the average time of path planning increases with the number of objective and the number of sampling points.Figure 6 shows a successful grape harvesting path planning.The white dots are the path points swept by the clamper center of the end effector during the harvesting.
And then the harvesting sequence and the anticollision path points were calculated by using the energy optimal and the artificial potential field.The virtual tests based on virtual reality were performed to validate the performance of the proposed algorithm.Through the virtual simulation test of the harvesting path planning, the three-dimensional space path points of the end-effector of the harvesting manipulator were calculated in real time in virtual environment and the collision detection is analyzed.The success rate was up to 90%.It can be known that the simulation system designed in this study has a good practical value to help test and improve the intelligent behavior algorithm of the robot.
To obtain an optimal harvesting path, taking the sum of the joint angle of the weighted rotation as the goal, the harvesting sequence planning objective function of the six-DOF harvesting manipulator is established by using the minimum energy principle and the minimum energy consumption sequence can be obtained by calculating the value of the target function.In addition, the mapping model of obstacles in the joint configuration space is built on the basis of sampling method, and the artificial potential field is used to search the obstacle avoidance path in the joint configuration space, which improves the generality of the algorithm.However, due to the proposed algorithm adopts random sampling strategy, the sampling points may collide with the obstacles, so it is necessary to resample.If the number of maximum sampling point is too small, the failure probability of the path planning increases.And if the number of maximum sampling point value is too large, it will cause the path to not exist or be difficult to find, or the planning time is too long.So the parameters need test repeatedly.

Conclusion
To conduct an undamaged robotic grape-harvesting in an unstructured vineyard, a path planning strategies based on minimum energy and artificial potential field for grape harvesting robot was developed in this study.The kinematical model of Six-DOF serial manipulator was firstly constructed by using the D-H method, the model of obstacles was defined by the axis-aligned bounding box, and then the configuration space of harvesting robot was described by combining the obstacles and robot.Then, the harvesting sequence in path planning was computed by energy optimal method, and the anticollision path points were generated based on the artificial potential field and sampling searching method.Finally, to verify and test the proposed path planning algorithm, a virtual test system based on virtual reality was developed.10 times harvesting tests for grape anticollision path planning were implemented on the developed simulation system, and the success rate was up to 90%.In conclusion, the developed approach can effectively plan a collision-free path for the grape harvesting robot in the complex vineyard environment.However, when facing multiple overlapped and adjoining grape clusters, the path planning is still an issue that needs to be solved, and will require further research.

Figure 2 :
Figure 2: The bounding volume of obstacles such as cylinder and sphere.
Motion space in Y and Z coordinates

Figure 3 :Figure 4 :
Figure 3: The motion space of the harvesting robot.

Figure 5 :
Figure 5: Schematic diagram for the path planning of the grape harvesting robot.
). Harvesting robot shape and its joints coordinate systems.Note that  0 is the base coordinate of harvesting robot,   is the local coordinate system of the th( = 1, 2, . . ., 6) linkages,  ℎ is the coordinate system of end effector,  -1 is twist angle,  -1 is length of linkages,   is joint angle, and   is offset of linkages.