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

Authors: Lufeng Luo; Hanjin Wen; Qinghua Lu; Haojie Huang; Weilin Chen; Xiangjun Zou; Chenglin Wang
Journal: Complexity
 (2018)

---

## Abstract

Collision-free autonomous path planning under a dynamic and uncertainty vineyard environment is the most important issue which needs to be resolved firstly in the process of improving robotic harvesting manipulator intelligence. We present and apply energy optimal and artificial potential field to develop a path planning method for six degree of freedom (DOF) serial harvesting robot under dynamic uncertain environment. Firstly, the kinematical model of Six-DOF serial manipulator was constructed by using the Denavit-Hartenberg (D-H) method. The model of obstacles was defined by axis-aligned 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 automatically 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. After obtaining the space coordinates of grape picking point and anticollision bounding volume, the path points were drew out by the proposed method. 10 times picking tests for grape anticollision path planning were implemented on the developed simulation system, and the success rate was up to 90%. The results showed that the proposed path planning method can be used to the harvesting robot.


---

## Body

## 1. 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 time-consuming 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]: grid-based 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 axis-aligned 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: Section2 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.


## 2. 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.


### 2.1. Kinematical Modelling for Six-DOF Serial Robot

The shape of the six degree of freedom series harvesting robot is as shown in Figure1(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.Figure 1
Harvesting robot shape and its joints coordinate systems. Note thatO0 is the base coordinate of harvesting robot, Oi is the local coordinate system of the ith(i=1,2,…,6) linkages, Oh is the coordinate system of end effector, αi-1 is twist angle, αi-1 is length of linkages, θi is joint angle, and di is offset of linkages.
(a)
Harvesting robot shape (b)
Origin coordinate and joints coordinate systems of harvesting robot


#### 2.1.1. 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 O0 and the local coordinate system of each joint is Oi(i=1,2,…6) and the end-effector coordinate system is Oh. The coordinate system of each joint of the picking robot is shown by Figure 1(b).Table 1
D-H parameters of harvesting robot.
linkagesi Twist angleαi-1/(°) Length of linkagesai-1/mm Joint angleθi/(°) Offset of linkagesdi/mm Angle range/(°) Note/mm 1 0 0 θ 1 0 -170~170 2 -90 a 1 θ 2 0 -60~150 a 1 = 155 3 0 a 2 θ 3 0 -170~+75 a 2 = 360 4 -90 a 3 θ 4 d 4 -190~+190 a 3 = 155 5 90 0 θ 5 0 -125~+125 d 4 = 365 6 -90 0 θ 6 0 -360~+360


#### 2.1.2. Forward and Inverse Kinematic Modeling

The motion of the harvesting robot is driven by the forward kinematics model, which is to calculate the position and attitude of the end-effector coordinate systemOh relative to the base coordinate system O0 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.(1)Tii-1=ci-si0ai-1sicos⁡αi-1cicos⁡αi-1-sin⁡αi-1-disin⁡αi-1sisin⁡αi-1cisin⁡αi-1cos⁡αi-1dicos⁡αi-10001where si indicates sin⁡θi, ci indicates cos⁡θi (i=1,2,3,…,6), αi-1 is the twist angle, αi-1 is the length of linkages, and di is the offset of linkagesThe transformation matrix of the manipulatorT60 can be obtained by multiplying continuously the transformation matrix of each connecting rod Tii-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 (n,o,a,p) and the basic coordinate system [18].(2)T60=T10θ1T21θ2⋯T65θ6=nxoxaxpxnyoyaypynzozazpz0001where(3)nx=c1c23c4c5c6-s4s6-s23s5c6+s1s4c5c6+c4s6ny=s1c23c4c5c6-s4s6-s23s5c6-c1s4c5c6+c4s6nz=-s23c4c5c6-s4s6-c23s5c6ox=-c1c23c4c5s6+s4c6-s23s5s6-s1s4c5s6-c4c6oy=-s1c23c4c5s6+s4c6-s23s5s6+c1s4c5s6-c4c6oz=s23c4c5s6+s4c6+c23s5s6ax=c1-c23c4s5-s23c5-s1s4s5ay=s1-c23c4s5-s23c5+c1s4s5az=s23c4s5-c23c5px=c1a3c23-d4s23+a1+a2c2py=s1a3c23-d4s23+a1+a2c2pz=-a3s23-d4c23-a2s2c23=cos⁡θ2+θ3s23=sinθ2+θ3In 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 systemOh can be calculated by the cutting point position. The attitude of the end-effector (n,o,a,p) 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 end-effector 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.(4)θ1=arctan⁡pypxθ2=arctan⁡a2s3-d4c1px+s1py-a1-a3+a2c3pza3+a2c3c1px+s1py-a1-d4-a2s3pz-θ3θ3=arctan⁡a3d4-arctan⁡ka32+d42-k2θ4=arctan⁡-axs1+ayc1-axc1c23-ays1c23+azs23θ5=arctan⁡-axc1c4c23+s1s4-ays1c4c23-c1s4+azs23c4ax-c1s23+ay-s1s23+az-c23θ6=arctan⁡-nxc1s4c23-s1c4-nys1s4c23+c1c4+nzs4s23nxc5c1c4c23+s1s4-c1s5s23+nyc5s1c4c23-c1s4-s1s5s23-nzc4c5s23+s5c23where k=px2+py2+pz2+a12-2a1c1px+s1py-a32-a22-d42/2a2To 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 T60. The following T60 was calculated by using Matlab.(5)T60=0.35360.3536-0.8660217.5000-0.6124-0.6124-0.5000376.7211-0.70710.7071-0.0000-53.23090001.0000Then, the obtainedT60 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.


### 2.2. 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 ofN intervals each of which is defined by the minimal and maximal value of the corresponding coordinate for the points in S. 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 (x,y,z) in the AABB box satisfy the following conditions: xmin≤x≤xmax, ymin≤y≤ymax, and zmin≤z≤zmax, and the points Pmin=[xminyminzmin] and Pmax=[xmaxymaxzmax] were two important points of the AABB box. The center pointc can be calculated by the equation (c = (Pmin + Pmax)/2). A bounding box of the obstacle can clearly be represented by using the two points Pmin and Pmax.Figure 2
The bounding volume of obstacles such as cylinder and sphere.


### 2.3. 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 withn degrees of freedom, setting E(Ɛ1,Ɛ2,…,Ɛn) as a set of active joint values of it, in which Ɛi is the rotation angle of the i-th active joint of the manipulator. If the point P represents a point on the manipulator, then the relative position relationship between the point and the obstacle can be expressed as follows [21]:(6)ΓPE=1,thepointPisinsidetheobstacle0,therestwhereГ(P(E)) is relative position relationship between the point P and the obstacle when the manipulator is driven with the joint value and P(E) is the position coordinate of the point P on the manipulator in the basic coordinate system when the joint value is E. If the rods of the manipulator are uniformly dispersed into m points according to the step size dmin (mm), m discrete points constitute a point set Plink. The relative positional relation between the whole arm driven with the joint value E and the obstacle can be expressed as follows:(7)ΓΕ=∑i=1mΓPiEin which Г(E) 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 Г(E)=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:(8)ΩE=1ΓE=10ΓE=0whereΩ(E) is the value of the point in the joint configuration space Ω with the element in the joint value E of the manipulator as the coordinate and represents the relative positional relationship with the obstacle when the manipulator is in the position corresponding to E. If the manipulator interferes with the obstacle, Ω(E) is equal to 1, otherwise Ω(E) is equal to 0. So the set Hpart (E1,E2,…Ed) 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 d 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 Hpart.(9)Ψ=ΩHpart=∑j=1dΩEj=0where Ψ is the relative positional relationship between the manipulator the obstacle when the joint value set is Hpart.Therefore, the obstacle path planning is performed on the manipulator; that is, a setHpart of joint values is searched in the joint configuration space Ω, so that the set is connected to the initial configuration joint value Es and the target joint value Eg 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.Figure 3
The motion space of the harvesting robot.
(a)
Motion space in X, Y and Z coordinates (b)
Motion space in X and Y coordinates (c)
Motion space in X and Z coordinates (d)
Motion space in Y and Z coordinates


## 2.1. Kinematical Modelling for Six-DOF Serial Robot

The shape of the six degree of freedom series harvesting robot is as shown in Figure1(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.Figure 1
Harvesting robot shape and its joints coordinate systems. Note thatO0 is the base coordinate of harvesting robot, Oi is the local coordinate system of the ith(i=1,2,…,6) linkages, Oh is the coordinate system of end effector, αi-1 is twist angle, αi-1 is length of linkages, θi is joint angle, and di is offset of linkages.
(a)
Harvesting robot shape (b)
Origin coordinate and joints coordinate systems of harvesting robot


### 2.1.1. 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 O0 and the local coordinate system of each joint is Oi(i=1,2,…6) and the end-effector coordinate system is Oh. The coordinate system of each joint of the picking robot is shown by Figure 1(b).Table 1
D-H parameters of harvesting robot.
linkagesi Twist angleαi-1/(°) Length of linkagesai-1/mm Joint angleθi/(°) Offset of linkagesdi/mm Angle range/(°) Note/mm 1 0 0 θ 1 0 -170~170 2 -90 a 1 θ 2 0 -60~150 a 1 = 155 3 0 a 2 θ 3 0 -170~+75 a 2 = 360 4 -90 a 3 θ 4 d 4 -190~+190 a 3 = 155 5 90 0 θ 5 0 -125~+125 d 4 = 365 6 -90 0 θ 6 0 -360~+360


### 2.1.2. Forward and Inverse Kinematic Modeling

The motion of the harvesting robot is driven by the forward kinematics model, which is to calculate the position and attitude of the end-effector coordinate systemOh relative to the base coordinate system O0 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.(1)Tii-1=ci-si0ai-1sicos⁡αi-1cicos⁡αi-1-sin⁡αi-1-disin⁡αi-1sisin⁡αi-1cisin⁡αi-1cos⁡αi-1dicos⁡αi-10001where si indicates sin⁡θi, ci indicates cos⁡θi (i=1,2,3,…,6), αi-1 is the twist angle, αi-1 is the length of linkages, and di is the offset of linkagesThe transformation matrix of the manipulatorT60 can be obtained by multiplying continuously the transformation matrix of each connecting rod Tii-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 (n,o,a,p) and the basic coordinate system [18].(2)T60=T10θ1T21θ2⋯T65θ6=nxoxaxpxnyoyaypynzozazpz0001where(3)nx=c1c23c4c5c6-s4s6-s23s5c6+s1s4c5c6+c4s6ny=s1c23c4c5c6-s4s6-s23s5c6-c1s4c5c6+c4s6nz=-s23c4c5c6-s4s6-c23s5c6ox=-c1c23c4c5s6+s4c6-s23s5s6-s1s4c5s6-c4c6oy=-s1c23c4c5s6+s4c6-s23s5s6+c1s4c5s6-c4c6oz=s23c4c5s6+s4c6+c23s5s6ax=c1-c23c4s5-s23c5-s1s4s5ay=s1-c23c4s5-s23c5+c1s4s5az=s23c4s5-c23c5px=c1a3c23-d4s23+a1+a2c2py=s1a3c23-d4s23+a1+a2c2pz=-a3s23-d4c23-a2s2c23=cos⁡θ2+θ3s23=sinθ2+θ3In 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 systemOh can be calculated by the cutting point position. The attitude of the end-effector (n,o,a,p) 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 end-effector 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.(4)θ1=arctan⁡pypxθ2=arctan⁡a2s3-d4c1px+s1py-a1-a3+a2c3pza3+a2c3c1px+s1py-a1-d4-a2s3pz-θ3θ3=arctan⁡a3d4-arctan⁡ka32+d42-k2θ4=arctan⁡-axs1+ayc1-axc1c23-ays1c23+azs23θ5=arctan⁡-axc1c4c23+s1s4-ays1c4c23-c1s4+azs23c4ax-c1s23+ay-s1s23+az-c23θ6=arctan⁡-nxc1s4c23-s1c4-nys1s4c23+c1c4+nzs4s23nxc5c1c4c23+s1s4-c1s5s23+nyc5s1c4c23-c1s4-s1s5s23-nzc4c5s23+s5c23where k=px2+py2+pz2+a12-2a1c1px+s1py-a32-a22-d42/2a2To 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 T60. The following T60 was calculated by using Matlab.(5)T60=0.35360.3536-0.8660217.5000-0.6124-0.6124-0.5000376.7211-0.70710.7071-0.0000-53.23090001.0000Then, the obtainedT60 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.


## 2.1.1. 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 O0 and the local coordinate system of each joint is Oi(i=1,2,…6) and the end-effector coordinate system is Oh. The coordinate system of each joint of the picking robot is shown by Figure 1(b).Table 1
D-H parameters of harvesting robot.
linkagesi Twist angleαi-1/(°) Length of linkagesai-1/mm Joint angleθi/(°) Offset of linkagesdi/mm Angle range/(°) Note/mm 1 0 0 θ 1 0 -170~170 2 -90 a 1 θ 2 0 -60~150 a 1 = 155 3 0 a 2 θ 3 0 -170~+75 a 2 = 360 4 -90 a 3 θ 4 d 4 -190~+190 a 3 = 155 5 90 0 θ 5 0 -125~+125 d 4 = 365 6 -90 0 θ 6 0 -360~+360


## 2.1.2. Forward and Inverse Kinematic Modeling

The motion of the harvesting robot is driven by the forward kinematics model, which is to calculate the position and attitude of the end-effector coordinate systemOh relative to the base coordinate system O0 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.(1)Tii-1=ci-si0ai-1sicos⁡αi-1cicos⁡αi-1-sin⁡αi-1-disin⁡αi-1sisin⁡αi-1cisin⁡αi-1cos⁡αi-1dicos⁡αi-10001where si indicates sin⁡θi, ci indicates cos⁡θi (i=1,2,3,…,6), αi-1 is the twist angle, αi-1 is the length of linkages, and di is the offset of linkagesThe transformation matrix of the manipulatorT60 can be obtained by multiplying continuously the transformation matrix of each connecting rod Tii-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 (n,o,a,p) and the basic coordinate system [18].(2)T60=T10θ1T21θ2⋯T65θ6=nxoxaxpxnyoyaypynzozazpz0001where(3)nx=c1c23c4c5c6-s4s6-s23s5c6+s1s4c5c6+c4s6ny=s1c23c4c5c6-s4s6-s23s5c6-c1s4c5c6+c4s6nz=-s23c4c5c6-s4s6-c23s5c6ox=-c1c23c4c5s6+s4c6-s23s5s6-s1s4c5s6-c4c6oy=-s1c23c4c5s6+s4c6-s23s5s6+c1s4c5s6-c4c6oz=s23c4c5s6+s4c6+c23s5s6ax=c1-c23c4s5-s23c5-s1s4s5ay=s1-c23c4s5-s23c5+c1s4s5az=s23c4s5-c23c5px=c1a3c23-d4s23+a1+a2c2py=s1a3c23-d4s23+a1+a2c2pz=-a3s23-d4c23-a2s2c23=cos⁡θ2+θ3s23=sinθ2+θ3In 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 systemOh can be calculated by the cutting point position. The attitude of the end-effector (n,o,a,p) 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 end-effector 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.(4)θ1=arctan⁡pypxθ2=arctan⁡a2s3-d4c1px+s1py-a1-a3+a2c3pza3+a2c3c1px+s1py-a1-d4-a2s3pz-θ3θ3=arctan⁡a3d4-arctan⁡ka32+d42-k2θ4=arctan⁡-axs1+ayc1-axc1c23-ays1c23+azs23θ5=arctan⁡-axc1c4c23+s1s4-ays1c4c23-c1s4+azs23c4ax-c1s23+ay-s1s23+az-c23θ6=arctan⁡-nxc1s4c23-s1c4-nys1s4c23+c1c4+nzs4s23nxc5c1c4c23+s1s4-c1s5s23+nyc5s1c4c23-c1s4-s1s5s23-nzc4c5s23+s5c23where k=px2+py2+pz2+a12-2a1c1px+s1py-a32-a22-d42/2a2To 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 T60. The following T60 was calculated by using Matlab.(5)T60=0.35360.3536-0.8660217.5000-0.6124-0.6124-0.5000376.7211-0.70710.7071-0.0000-53.23090001.0000Then, the obtainedT60 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.


## 2.2. 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 ofN intervals each of which is defined by the minimal and maximal value of the corresponding coordinate for the points in S. 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 (x,y,z) in the AABB box satisfy the following conditions: xmin≤x≤xmax, ymin≤y≤ymax, and zmin≤z≤zmax, and the points Pmin=[xminyminzmin] and Pmax=[xmaxymaxzmax] were two important points of the AABB box. The center pointc can be calculated by the equation (c = (Pmin + Pmax)/2). A bounding box of the obstacle can clearly be represented by using the two points Pmin and Pmax.Figure 2
The bounding volume of obstacles such as cylinder and sphere.


## 2.3. 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 withn degrees of freedom, setting E(Ɛ1,Ɛ2,…,Ɛn) as a set of active joint values of it, in which Ɛi is the rotation angle of the i-th active joint of the manipulator. If the point P represents a point on the manipulator, then the relative position relationship between the point and the obstacle can be expressed as follows [21]:(6)ΓPE=1,thepointPisinsidetheobstacle0,therestwhereГ(P(E)) is relative position relationship between the point P and the obstacle when the manipulator is driven with the joint value and P(E) is the position coordinate of the point P on the manipulator in the basic coordinate system when the joint value is E. If the rods of the manipulator are uniformly dispersed into m points according to the step size dmin (mm), m discrete points constitute a point set Plink. The relative positional relation between the whole arm driven with the joint value E and the obstacle can be expressed as follows:(7)ΓΕ=∑i=1mΓPiEin which Г(E) 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 Г(E)=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:(8)ΩE=1ΓE=10ΓE=0whereΩ(E) is the value of the point in the joint configuration space Ω with the element in the joint value E of the manipulator as the coordinate and represents the relative positional relationship with the obstacle when the manipulator is in the position corresponding to E. If the manipulator interferes with the obstacle, Ω(E) is equal to 1, otherwise Ω(E) is equal to 0. So the set Hpart (E1,E2,…Ed) 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 d 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 Hpart.(9)Ψ=ΩHpart=∑j=1dΩEj=0where Ψ is the relative positional relationship between the manipulator the obstacle when the joint value set is Hpart.Therefore, the obstacle path planning is performed on the manipulator; that is, a setHpart of joint values is searched in the joint configuration space Ω, so that the set is connected to the initial configuration joint value Es and the target joint value Eg 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.Figure 3
The motion space of the harvesting robot.
(a)
Motion space in X, Y and Z coordinates (b)
Motion space in X and Y coordinates (c)
Motion space in X and Z coordinates (d)
Motion space in Y and Z coordinates


## 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 Figure4.Figure 4
Schematic diagram for the path planning of the grape harvesting robot.


### 3.1. 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-of-freedom 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(x0,y0,z0), there are n bunches of fruits in the visual area to be picked, and the space position coordinates are (xi,yi,zi), and the corresponding space variables of the manipulator are (θ1i,θ2i,θ3i,θ4i,θ5i,θ6i), in which i=1,2,…n.. 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)P=∑j=1nθj-θj+1in which P is the function of optimization objective, θis the motion angle of joint, and n 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:(11)P′=∑i=1mωiJi=∑i=1m∑j=1nωiθi,j-θi,j+1where P′ indicates the objective function of the manipulator that can embody the energy consumption of the manipulator motion, m is the number of the joints, ωi is the weight of the objective function of the ith joints, and θi,j indicates the angle value of the ith joint when the manipulator is locating the jth space point.


### 3.2. Harvesting Path Generating Based on Artificial Potential Field Theory

In the process of grape harvesting, collision-free 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.


#### 3.2.1. 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 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 fieldUsum(X) can be defined by the following [24]:(12)UsumX=UattX+UrepXin which Uatt(X) indicates the gravitational field generated by the target, Urep(X) indicates the repulsion field generated by the obstacles, X=x,y,zT is the position of the end-effector in the workspace. Setting the position of the target as Xg, and the gravitational field Uatt(X) can be defined by the following:(13)UattX=12kX-Xg2where k is a constant about the gravitational field and then the gravitation Fatt(X) can be solved by calculating the negative gradient value of the gravitational field, which is described as Formula (14).(14)FattX=-∇UattX=kXg-XTo define the mathematical expression about the repulsive force field, the following two conditions need firstly be conformed [25]: (1) the artificial potential field Usum(X) must be a continuous-differential function, and when X is equal to Xg, the value of Usum(X) is 0 (minimum value). (2) Under the effect of the artificial potential field Usum(X), the system can be stable. Setting X0 as the space position of the obstacle, the repulsive force field Urep(X) can be defined by the following:(15)UrepX=12η1X-X0-1ρ2,X-X0<ρ0,X-X0≥ρ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 Frep(X) can be solved by calculating the negative gradient value of the repulsive force field Urep(X), which is described as the following:(16)FrepX=-∇UrepX=η1X-X0-1ρ1X-X02∂X-X0∂X,X-X0<ρ0,X-X0≥ρ


#### 3.2.2. Sampling-Based Path Points Generating Method

The array of the path point sets(xi,yi,zi) 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 (x0,y0,z0), the terminal coordinates (xt,yt,zt) (the cutting point on the peduncles of grape cluster), and the bounding box model of obstacles R(Os,Oe) 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.Algorithm 1:Sampling-based path point generating method.
Input:the current point coordinates of the end-effector (x0,y0,z0), the terminal coordinates ( x t , y t , z t ) (the cutting point on the peduncles of grape cluster), the bounding box model of obstaclesR(Os,Oe) Output: the path points (xi,yi,zi). 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.


## 3.1. 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-of-freedom 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(x0,y0,z0), there are n bunches of fruits in the visual area to be picked, and the space position coordinates are (xi,yi,zi), and the corresponding space variables of the manipulator are (θ1i,θ2i,θ3i,θ4i,θ5i,θ6i), in which i=1,2,…n.. 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)P=∑j=1nθj-θj+1in which P is the function of optimization objective, θis the motion angle of joint, and n 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:(11)P′=∑i=1mωiJi=∑i=1m∑j=1nωiθi,j-θi,j+1where P′ indicates the objective function of the manipulator that can embody the energy consumption of the manipulator motion, m is the number of the joints, ωi is the weight of the objective function of the ith joints, and θi,j indicates the angle value of the ith joint when the manipulator is locating the jth space point.


## 3.2. Harvesting Path Generating Based on Artificial Potential Field Theory

In the process of grape harvesting, collision-free 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.


### 3.2.1. 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 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 fieldUsum(X) can be defined by the following [24]:(12)UsumX=UattX+UrepXin which Uatt(X) indicates the gravitational field generated by the target, Urep(X) indicates the repulsion field generated by the obstacles, X=x,y,zT is the position of the end-effector in the workspace. Setting the position of the target as Xg, and the gravitational field Uatt(X) can be defined by the following:(13)UattX=12kX-Xg2where k is a constant about the gravitational field and then the gravitation Fatt(X) can be solved by calculating the negative gradient value of the gravitational field, which is described as Formula (14).(14)FattX=-∇UattX=kXg-XTo define the mathematical expression about the repulsive force field, the following two conditions need firstly be conformed [25]: (1) the artificial potential field Usum(X) must be a continuous-differential function, and when X is equal to Xg, the value of Usum(X) is 0 (minimum value). (2) Under the effect of the artificial potential field Usum(X), the system can be stable. Setting X0 as the space position of the obstacle, the repulsive force field Urep(X) can be defined by the following:(15)UrepX=12η1X-X0-1ρ2,X-X0<ρ0,X-X0≥ρ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 Frep(X) can be solved by calculating the negative gradient value of the repulsive force field Urep(X), which is described as the following:(16)FrepX=-∇UrepX=η1X-X0-1ρ1X-X02∂X-X0∂X,X-X0<ρ0,X-X0≥ρ


### 3.2.2. Sampling-Based Path Points Generating Method

The array of the path point sets(xi,yi,zi) 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 (x0,y0,z0), the terminal coordinates (xt,yt,zt) (the cutting point on the peduncles of grape cluster), and the bounding box model of obstacles R(Os,Oe) 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.Algorithm 1:Sampling-based path point generating method.
Input:the current point coordinates of the end-effector (x0,y0,z0), the terminal coordinates ( x t , y t , z t ) (the cutting point on the peduncles of grape cluster), the bounding box model of obstaclesR(Os,Oe) Output: the path points (xi,yi,zi). 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.


## 3.2.1. 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 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 fieldUsum(X) can be defined by the following [24]:(12)UsumX=UattX+UrepXin which Uatt(X) indicates the gravitational field generated by the target, Urep(X) indicates the repulsion field generated by the obstacles, X=x,y,zT is the position of the end-effector in the workspace. Setting the position of the target as Xg, and the gravitational field Uatt(X) can be defined by the following:(13)UattX=12kX-Xg2where k is a constant about the gravitational field and then the gravitation Fatt(X) can be solved by calculating the negative gradient value of the gravitational field, which is described as Formula (14).(14)FattX=-∇UattX=kXg-XTo define the mathematical expression about the repulsive force field, the following two conditions need firstly be conformed [25]: (1) the artificial potential field Usum(X) must be a continuous-differential function, and when X is equal to Xg, the value of Usum(X) is 0 (minimum value). (2) Under the effect of the artificial potential field Usum(X), the system can be stable. Setting X0 as the space position of the obstacle, the repulsive force field Urep(X) can be defined by the following:(15)UrepX=12η1X-X0-1ρ2,X-X0<ρ0,X-X0≥ρ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 Frep(X) can be solved by calculating the negative gradient value of the repulsive force field Urep(X), which is described as the following:(16)FrepX=-∇UrepX=η1X-X0-1ρ1X-X02∂X-X0∂X,X-X0<ρ0,X-X0≥ρ


## 3.2.2. Sampling-Based Path Points Generating Method

The array of the path point sets(xi,yi,zi) 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 (x0,y0,z0), the terminal coordinates (xt,yt,zt) (the cutting point on the peduncles of grape cluster), and the bounding box model of obstacles R(Os,Oe) 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.Algorithm 1:Sampling-based path point generating method.
Input:the current point coordinates of the end-effector (x0,y0,z0), the terminal coordinates ( x t , y t , z t ) (the cutting point on the peduncles of grape cluster), the bounding box model of obstaclesR(Os,Oe) Output: the path points (xi,yi,zi). 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.


## 4. 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.


### 4.1. 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 Figure5.Figure 5
Schematic diagram for the path planning of the grape harvesting robot.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.


### 4.2. 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 Table2. The relationship between path length L and key point Pi can be described by Formula (17).(17)L=∑1≤i≤Kxi-xi-12+yi-yi-12+zi-zi-12where K indicates the number of sampling points of the path and (xi,yi,zi) is the coordinates of the i-th sampling points.Table 2
Statistical results of the virtual tests for grape harvesting path-planning.
Number of objectives Harvesting optimal sequence Length of path (L/cm) Number of sampling points (K) Collision(Y/N) Average time of planning (ms) 3 ②→③→① 635.78 108 N 152 2 ②→① 448.67 72 N 108 4 ③→④→①→② 985.23 172 N 215 5 ②→④→③→①→⑤ 1362.45 206 N 261 3 ①→③→② 723.33 102 N 163 4 ②→③→④→① 1061.58 170 N 203 5 ⑤→③→①→②→④ 1472.65 216 Y 274 2 ①→② 463.65 91 N 98 3 ②→③→① 756.87 127 N 151 4 ③→②→④→① 1045.26 162 N 199From the Table2 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.Figure 6
The path points drew by the proposed path-planning method.
(a) (b) (c)


## 4.1. 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 Figure5.Figure 5
Schematic diagram for the path planning of the grape harvesting robot.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.


## 4.2. 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 Table2. The relationship between path length L and key point Pi can be described by Formula (17).(17)L=∑1≤i≤Kxi-xi-12+yi-yi-12+zi-zi-12where K indicates the number of sampling points of the path and (xi,yi,zi) is the coordinates of the i-th sampling points.Table 2
Statistical results of the virtual tests for grape harvesting path-planning.
Number of objectives Harvesting optimal sequence Length of path (L/cm) Number of sampling points (K) Collision(Y/N) Average time of planning (ms) 3 ②→③→① 635.78 108 N 152 2 ②→① 448.67 72 N 108 4 ③→④→①→② 985.23 172 N 215 5 ②→④→③→①→⑤ 1362.45 206 N 261 3 ①→③→② 723.33 102 N 163 4 ②→③→④→① 1061.58 170 N 203 5 ⑤→③→①→②→④ 1472.65 216 Y 274 2 ①→② 463.65 91 N 98 3 ②→③→① 756.87 127 N 151 4 ③→②→④→① 1045.26 162 N 199From the Table2 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.Figure 6
The path points drew by the proposed path-planning method.
(a) (b) (c)


## 5. Discussion

Traditional path planning methods (such as skeleton, cell decomposition and subgoal graph etc) are very effective for lower degree of freedom robot (2 to 3 DOF). With the increase in the degree of freedom of robots, these algorithms will be out of work. A collision-free autonomous path planning methodolgy for six-DOF serial harvesting manipulator under a dynamic vineyard environment was presented in this study. The kinematical modelling of Six-DOF serial manipulator, the model of obstacles and the configuration space of harvesting robot was firstly introduced in detail. 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.


## 6. 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.



---

*Source: 3563846-2018-11-01.xml*