Trajectory Optimization of Pickup Manipulator in Obstacle Environment Based on Improved Artificial Potential Field Method

: In order to realize the technique of quick picking and obstacle avoidance, this work proposes a trajectory optimization method for the pickup manipulator under the obstacle condition. The proposed method is based on the improved artificial potential field method and the cosine adaptive genetic algorithm. Firstly, the Denavit – Hartenberg (D-H) method is used to carry out the kinematics modeling of the pickup manipulator. Taking into account the motion constraints, the cosine adaptive genetic algorithm is utilized to complete the time-optimal trajectory planning. Then, for the collision problem in the obstacle environment, the artificial potential field method is used to establish the attraction, repulsion, and resultant potential field functions. By improving the repulsion potential field function and increasing the sub-target point, obstacle avoidance planning of the improved artificial potential field method is completed. Finally, combined with the improved artificial potential field method and cosine adaptive genetic algorithm, the movement simulation analysis of the five-Degree-of-Freedom pickup manipulator is carried out. The trajectory optimization under the obstacle environment is realized, and the picking efficiency is improved.


Introduction
In the unstructured training fields such as golf, tennis, and table tennis, there are some disadvantages in picking balls such as high labor intensity, high risk, boring work, and low efficiency. Although the use of automated pickup devices such as dedicated discs [1] can improve picking efficiency, it is impossible to pick up objects between obstacles. How to realize the task of picking objects quickly in the obstacle environment, the pickup manipulator as the core part of the service robot is one of the effective picking up tools. However, at present, the pickup manipulator still has low work efficiency in the process of picking up objects, and it is difficult to avoid a collision in the obstacle environment [2]. Therefore, the trajectory optimization of the pickup manipulator in the obstacle environment needs to be solved urgently [3][4].
The operation of the traditional manipulator is usually limited to the specified path or angular displacement range [5]. In 2018, Alexander Reiter et al. proposed a time-optimal path design method based on a predetermined end-effector path and discussed the time-optimal path following predefined end-effector paths for kinematically redundant robots [6]. Giulio Trigatti et al. proposed a new method for trajectory planning of spraying for industrial robots in 2018. According to the contour trajectory of spraying, the proposed algorithm was used to define the motion law and limit the speed of the end-effector [7]. In 2018, Junsen Huang et al. proposed a robotic time-pulse comprehensive optimal trajectory planning method. The fifth-order B-spline interpolation method is used to interpolate the motion trajectory in the joint space, and then the trajectory is optimized by the elite non-dominated sorting genetic algorithm (NSGA-II) for trajectory planning of the surgical robot [8]. In 2019, Yi Fang et al. proposed a smooth and time-optimal S-curve trajectory planning method to meet the requirements of the high-speed and ultra-precise operation of robotic manipulators in modern industrial applications [9]. However, these methods do not consider the influence of the unstructured obstacle environment. When the pickup manipulator is in an unstructured environment, the presence of obstacles may affect the pickup manipulator. These obstacles will collide with the pickup manipulator, leading to a decrease in the working stability of the pickup manipulator and even damage to the motor and other parts. O. Khatib proposed a unique real-time obstacle avoidance approach for manipulators and mobile robots based on the "artificial potential field" concept and implemented in the COSMOS system for a PUMA 560 robot in 1985 [10]. Steven M. LaValle introduced the concept of a Rapidly-exploring Random Tree (RRT) as a randomized data structure that was designed for a broad lass of path planning problems in 1998 [11]. Jiankun Wang et al. proposed an improved RRT algorithm incorporating obstacle boundary information to deal with an optimal path in 2016 [12]. Wei, k et al. proposed a dynamic path planning method for robotic autonomous obstacle avoidance based on an improved RRT algorithm in 2018 [13]. This method is used for obstacle avoidance of robots in smart factories. In 2018, Li, A et al. proposed a dynamic trajectory planning method based on ACT-R (ideal rational adaptive control) cognitive model [14] and is used for active obstacle avoidance in electric vehicles. In 2019, Wang, X et al. proposed a method for obstacle avoidance with pure azimuth measurement in the absence of a model of obstacle motion [15]. Zhang, W et al. proposed a dynamic collision avoidance method based on collision risk assessment and improved speed barrier method [16] for uncertain dynamic obstacle environments in 2017. Jiankun Wang et al. described a socially compliant path planning scheme for robotic autonomous luggage trolley collection at airports in 2019 [17]. However, these methods do not consider the time-optimal trajectory optimization, and the work efficiency needs to be improved.
In order to improve the picking efficiency and avoid collision with obstacles, this work takes a five-Degree-of-Freedom pickup manipulator as the research object. The improved artificial potential field method and the cosine adaptive genetic algorithm are combined to finish the trajectory optimization and obstacle avoidance planning.

Methodology
To solve the collision problem of the pickup manipulators in complex environments featuring obstacles, an improved artificial potential field method is proposed for the obstacle avoidance planning. In order to solve the problem of the unreachable target, the distance term ) , ( g n X X d between the pickup manipulator and the target point is added based on the traditional artificial potential field. The influence of the obstacle boundary is also considered on the obstacle avoidance planning of the pickup manipulator. So, the radius r of the obstacle is introduced to the repulsion potential field function to adjusting the repulsion potential field. For the defect of local minimum value, this work establishes the virtual sub-target point to solve it. The improved artificial potential field method is used for obstacle avoidance planning and gets the path points. Then this work uses the cosine adaptive genetic algorithm to optimize the trajectory between the path points. By setting the genetic parameters, constructing the objective function, constructing the motion constraints, designing the fitness function, analyzing the three genetic operators, and the adaptive dynamic adjustment, the time-optimal trajectory planning of the cosine adaptive genetic algorithm is completed. The time-optimal trajectory of the pickup manipulator in the obstacle environment shortens the working time of the pickup manipulator and improves the picking efficiency, the overall process is shown in Figure 1.   Figure 2 shows the overall structure of the selected pickup manipulator with five-Degree-of-Freedom. The selected pickup manipulator is mainly composed of a base, links, joints, and an end gripper. The coordinate system of the selected pickup manipulator linkage, which is established by the D-H method [18][19][20], is shown in Figure 3. The kinematics modeling analysis is performed as follows.  According to the homogeneous transformation matrix in the formula (1) and the pose matrix in the formula (2), the expression of the pose matrix of the end-effector (3) is completed. With the above parameters, the inverse kinematics modeling can be completed by the analytical method. The results are shown in the formulas (4) to (12).

Kinematics Modeling
cos sin 0 sin cos cos cos sin sin sin sin cos sin cos cos Both sides of formula (3) left multiply the inverse matrix 1 0 1 − T , then: 234  5  234  5  234  3  23  2  2  4  234  5  234   55  0  1  0  15  234  5  234  5  234  3  23  2  2  5  234  4  234   00   0  0  0  1   c  c  c  s  s  a  c  a  c  a  c  d  s  sc  TT  s  c  s  s  c  a  s  a  s  d  c  a In the formula, Both sides of formula (3) left multiply the inverse matrix 1 0 2 − T , then: According to formula (10) and formula (11), take L (1,3) = R (1,3), L(2,3) = R(2,3), then: The inverse kinematics of the pickup manipulator is completed with the above computing. According to the position of the small ball detected by the binocular camera, the joint angle of the pickup manipulator can be calculated by using the inverse kinematics mode and realize the motion control.

Objective Function
For the trajectory planning of the pickup manipulator, the objective function is constructed with the shortest picking time. Then the time of each trajectory curve is optimized in turn. The established objective function of the pickup manipulator is: In the formula, all T is the total time that the pickup manipulator runs from the initial position to the final position, and j h is the time value obtained after decoding, m is the number of discrete path points that the pickup manipulator runs from the initial point to the picking target point.

Motion Constraints
The time-optimal trajectory planning of the pickup manipulator is carried out under the constraint of the kinematics parameter of each joint. The constraint conditions are the maximum value of angular velocity and angular acceleration of each joint.
The constraint of angular velocity The angular velocity of the joint i should meet the conditions: In the formula, According to the conditions and work requirements of the pickup manipulator, the settings of joint constraints are shown in Table 1.
In the formula,

Encoding and Decoding
The encoding of the genetic algorithm [21][22][23] is a process of converting the parameters from solution space into genetic spatial. The time target of trajectory planning is encoded by binary coding. The variation range of the optimization variables is relatively small; therefore, using a certain length of binary coding can ensure the accuracy of the solution. The time range of the pickup manipulator in the working state is set to [ shown in formula (14). In this work, we take the binary code length l = 9,

Initial Population
In the MATLAB software (R2015b, Mathworks, Natick, MA, America and 1984), the initial population of the time variable of the pickup manipulator is randomly generated by generating random numbers. Using the rand (n, l) command to generate a random matrix of n rows and l column, n represents the size of the population, and l represents the length of binary coding of individuals. Using the round (n, 1) command to round each element in the matrix off to the form of binary symbols 0 and 1, thereby generating an initial population of the pickup manipulator. The initial population is set to 30; that is, the population consists of 30 individuals.

Fitness Function
The fitness function is mapped from the objective function. In order to convert the minimum value of running time of the pickup manipulator to the maximum value of the fitness function, the fitness function is designed as: In the formula,

Operators
The genetic operator operation consists of three major operations: selection, crossover, and mutation. To deal with the time optimization of the pickup manipulator, they simulate the biological genetic evolution mechanism in different periods.
(1) Select Operation The selection operation is an operation of evaluating and selecting individuals in the time population according to the fitness function of the time-optimal trajectory planning of the pickup manipulator. The commonly used selection algorithm is the fitness proportional method, which is also called the roulette method [24] or Monte Carlo selection method. So, this work takes the roulette method to select individuals in the time population. The solution is to calculate the individual fitness in the population according to the fitness function. An individual with big fitness will be selected in the next generation because the probability of an individual in the population being selected is proportional to the fitness. The specific steps are as follows: a. Calculate the fitness ) ( N is the number of the initial population.
b. Calculate the probability that an individual will be selected: c. Calculate the cumulative probability of the individual: d. Generate a uniformly distributed random number r within [0, 1]; f. Repeat the process e for N times.
A visual depiction of the selection operation for the time individual of the pickup manipulator is shown in Figure 4. According to the probability i p in the time population, the individuals are sequentially assigned to the corresponding areas in the roulette and form a complete roulette. The position corresponding to each rotation of the turntable is uniform, and the rotational position is generated by a random function. The random number r represents the cumulative probability i q .
When the wheel stops rotating, the area pointed by the pointer will be selected into the next generation population.
Probability Accumulated probability (2) Cross Operation The cross operation is an operation of partially exchanging the coding of two individuals to be optimized to generate new individuals. Since the time population is small and the single-point cross operation is simple [25], the cross operation is realized by a single point crossing method. The working principle of the cross operation is shown in Figure 5.
Single point cross operation is carried out on individuals A and B, and the two newly encoded individuals produced are: , (

3) Mutation Operation
In the process of genetic algorithm operation, mutation takes an important role in restoring the population diversity and ensuring the global convergence of the algorithm. The mutation is an operation that randomly changes a certain bit in the individual coding with a small probability, which is usually set to 0.001-0.1. That is to say, it randomly replaces one of the bits 0 into 1 or replaces 1 with 0 in the binary coding. The principle of the mutation operation is shown in Figure 5.

(4) Termination Condition
The selection of the termination condition of the genetic algorithm is very important in the genetic algorithm. Improper selection of the termination condition will make the genetic algorithm convergence to the local best and make genetic algorithm early-maturing. The termination condition of the genetic algorithm is set according to the precision of the working time of the pickup manipulator and the requirement for rapid convergence in this work. In order to avoid converging to the local best, the maximum evolution algebra selected is set to 50 after multiple experiments.  [26]. In the traditional genetic algorithm, the crossover probability and the mutation probability are fixed. So it is difficult to adapt to the dynamic changes of the individual population and makes the search convergence and stability of the algorithm worse. In order to improve the optimization performance of the genetic algorithm, a cosine adaptive function is used to dynamically adjust the cross and mutation probability, as shown in the formula (22)-(23): In the formula,     Figure 6 shows the cross and mutation probability adjustment curve made by the cosine adaptive function. When the individual fitness is greater than the average fitness and is nearby, the cosine curve declines gradually and improves the cross and mutation probability in the adjustment process. It is beneficial to promote the change of the genetic structure of the individual and improve the ability of the global search in the population. When the individual fitness is near the maximum fitness value, the cosine curve is also smooth and reduces the probability of cross and mutation. It is beneficial to retain the excellent individual gene structure and promote the evolution direction of the population. In addition, when the individual fitness in the population is smaller than the current average fitness, the individual maintains the maximum crossover and mutation probability. It is conducive to expanding the diversity of the population and improving the overall optimization ability. In general, the optimization of the genetic algorithm by the cosine adaptive function is beneficial to improve the time optimization ability of the algorithm for the trajectory planning of the pickup manipulator. In the cosine adaptive function of the crossover and mutation probability, take

Artificial Potential Field Function
The artificial potential field function is a mathematical function expression of the virtual artificial potential field. The artificial potential field function includes the attraction potential field function, the repulsion potential field function, and the resultant potential field function. The negative gradients of the three potential field functions can generate attraction, repulsive force, and resultant force respectively, and make the manipulator moves under the resultant force finally.

Attraction Potential Field Function
The distance between the target point and the end-effector is proportional to the attraction potential field. The farther the distance is, the stronger the potential field is generated, and vice versa. Thus, the attraction potential field function can be expressed as: In the formula, η refers to attraction potential field gain factor, The attraction force of the target point on the manipulator is generated by the attraction potential field. It is a negative gradient of the attraction potential field function and draws the pickup manipulator towards the target point. The function expression is:

Repulsion Potential Field Function
The distance between the obstacle and the end-effector is inversely proportional to the repulsive potential field. The closer the distance is, the stronger the potential field is, and vice versa. When the manipulator is close to the obstacle infinitely, the repulsion potential field tends to infinity. When the distance between them is greater than the distance affected by the repulsion potential field, the obstacle no longer affects the pickup manipulator. That is to say, the repulsion potential field becomes zero. Thus, the repulsion potential field function can be expressed as: In the formula,  refers to repulsion potential field gain factor, obs X refers to the position of obstacles in the work environment, 0 d refers to the distance affected by the repulsion potential field.
The repulsion force of the obstacle on the pickup manipulator is generated by the repulsion potential field. It is the negative gradient of the repulsion potential field function and repels the pickup manipulator away from the obstacle. The function expression is:

Resultant Potential Field Function
The resultant potential field is superimposed by the attraction potential field and the repulsive potential field, and the resultant force is also superimposed by attraction force and repulsion force. The resultant potential field function and the resultant force function are:

Improved Method
The artificial potential field method [27,28] is used for obstacle avoidance planning. It has the advantages of a simple and intuitive mathematical model, low computational complexity, and good real-time performance. However, there may be unreachable target points or local minimum value defects during the operation. These defects make it difficult to complete the obstacle avoidance planning successfully [29]. Now, this work analyzes the two defects of the traditional artificial potential field method and improves them.

Improvement of Target Point Unreachable Defect
The target point is unreachable; that is, the obstacle is distributed near the target point. So the pickup manipulator will never move to the target point due to the repulsive force of the obstacle. In order to solve the problem of an unreachable target point in the special case, the repulsion potential field function of the traditional artificial potential field method is improved. In order to enable the pickup manipulator to reach the target point in the special case, the relative distance term ) , ( g n X X d between the pickup manipulator and the target point is increased based on the original repulsion potential field function. n is adjustment factor of the distance and the repulsion potential field can be adjusted according to the relative distance between the pickup manipulator and the target point.
In addition, the influence of the obstacle boundary is considered, and the obstacle radius r is introduced into the repulsion potential field function on the obstacle avoidance planning. Figure 7 shows that under the influence distance Thus, the mathematical expression of the improved repulsive potential field function is: The improved repulsion force is generated by a negative gradient of the improved repulsion potential field function. The function expression relationship is: In the formula, * 1 rep F represents the repulsion force generated by the obstacle to the manipulator, pointing from the obstacle direction to the manipulator.  (1) When 0 < n < 1, the mathematical characteristics of are analyzed, and the results are shown in Figure 8. Analyzing the data in Figure 8, it can be founded that when (33) It can be found that when the pickup manipulator gradually approaches the target point, the improved repulsion component   Analyzing the data in Figure 9, it can be found that when It can be found that when the manipulator gradually approaches the target point, the improved repulsion component  Analyzing the data in Figure 10, it can be founded that when It can be found that when the manipulator gradually approaches the target point, the improved repulsion component According to the above three cases, different values of the adjustment factor n can make the pickup manipulator move smoothly to the target point. When n is 1, the compensated attraction force * 2 rep F tends to be constant, and the transition is smooth. So, this work takes n as 1 for operation.

Improvement of the Local Minimum Value Defect
The local minimum value, that is, the repulsion force and the attraction force of the pickup manipulator reach the balance and fall into the local stable state. So, the resultant force becomes zero, and there is no driving force to get the pickup manipulator to the target point. Hu et al. used the angular migration in the X-Y plane to make end-effector jump out of local minimum value in 2012 [30]. In 2019, Huang et al. proposed a fuzzy improved APF algorithm and the problem of the local minimum value was overcome [31]. In order to solve the problem of local minimum value of the pickup manipulator, this work improves the artificial potential field method by increasing the virtual sub-target. Figure 11 shows that the sub-target point is set at the bottom of the obstacle influence boundary. The appearance of the sub-target point changes the original attraction potential field environment and breaks the original balance state of attraction force and repulsion force. Thus, the manipulator is separated from the local minimum point under the resultant potential field constructed by the sub-target. When the pickup manipulator reaches a new position and is no longer affected by the local minimum value, the virtual sub-target point is not working. The pickup manipulator continues to use the target point to perform the planning process with the artificial potential field method.   Figure 11. Force of improved local minimum value.

Simulation Conditions
The experimental object is a five-DOF pickup manipulator and the kinematics parameter are 1 The selected obstacle radius is r = 10 mm. to conduct the simulation experiment in MATLAB. The improved artificial potential field method is adopted for obstacle avoidance planning to obtain the path points of the end-effector, and the results are showed in Table 2. The rapidly-exploring Random Tree* (RRT*) method is adopted for obstacle avoidance planning to obtain the path points of the end-effector, and the results are showed in Table  3.  Figure12 shows the path point obtained by the improved artificial potential field method for obstacle avoidance planning. When the end-effector is not affected by obstacles, the improved artificial potential field method can plan a straight-line trajectory to reduce the running distance and improve the working efficiency of the pickup manipulator. When the manipulator is affected by obstacles, the improved artificial potential field method can plan the obstacle avoidance path and make the pickup manipulator successfully reach the target point. The minimum obstacle avoidance radius is 2.873 cm, and it can be found in table 2 at P20. Figure 13 shows the trajectory of the endeffector obtained by RRT* method for obstacle avoidance planning. Due to the randomness of the sampling of RRT* method, the generated path is often not the optimal path, which makes the manipulator run a longer distance. When the manipulator is affected by obstacles, the minimum obstacle avoidance radius is 11.566 cm, shown in Table 2 at P6 and is much higher than the improved artificial potential field method. That reduces the working efficiency of the manipulator.  The improved artificial potential field method is adopted for obstacle avoidance planning to obtain the path points of the end-effector, and the results are showed in Table 4. The established Kinematics model is used to calculate the joint angle, and the results are shown in Table 5.  The primary selection of the running time between the path points is 2 s, and the established cosine adaptive genetic algorithm is used to optimize the time of the trajectory planning. The time optimization results of each joint at each path point are shown in Table 6. In order to ensure that every joint reaches the joint position, the longest running time of the joints is taken as the running time of the pickup manipulator. The time optimization results in the obstacle environment are shown in Table 7. According to the time optimization results in Table 7, the running time of the pickup manipulator in each stage is shortened by 6.1%, 26.1%, 51.6%, 38.4%, 51.6%, 67.7%, 67.7%, 67.7%, and 70.6%, respectively. The overall time is shortened by 51.6%, and the optimization effect is obvious. Combine the joint angles in Table 5 and the time optimization results of each path point in Table 7 Figure 14a shows the initial state of the pickup manipulator under the obstacle environment. If the pickup manipulator moves in a straight line without obstacle avoidance planning, it will collide with the cylindrical obstacle. Figure 14b shows the end state of the pickup manipulator. The running curve of the pickup manipulator is obtained by using the improved artificial potential field to avoid an obstacle in the obstacle environment and then using the cosine adaptive genetic algorithm to optimize the trajectory. It can be seen from the trajectory curve that the pickup manipulator can successfully avoid the obstacle and smoothly reach the target point along the path of the obstacle avoidance planning, and the trajectory optimization. Figure 15 shows the displacement curve of the end-effector along each coordinate axes. It can be seen from Figure 15 that the trajectory of the end-effector along the coordinate axes during the movement is continuous, without a large fluctuation phenomenon, and the motion law is reasonable. It proves the correctness of the theoretical derivation of the improved artificial potential field method and the cosine adaptive genetic algorithm. The improved artificial potential field method is adopted for obstacle avoidance planning to obtain the path points of the end-effector, and the results are showed in Table 8. The primary selection of the running time between the path points is 2s, and the established cosine adaptive genetic algorithm is used to optimize the time of the trajectory planning. In order to ensure every joint reaches the joint position in the same time, the longest-running time of the joints is taken as the pickup manipulator running time. The time optimization results in the obstacle environment are shown in Table 9. According to the time optimization results in Table 9, the running time of the pickup manipulator in each path point is respectively shortened by 27.11%, 37.38%, 51.08%, 58.42%, 52.52%, 50.59%, 68.69%, 54.5%, and 84.35%. The overall running time is shortened by 53.85%, and the optimization effect is obvious. Combine the path points in Table 6 and the time optimization results of each path point in Table 9 to conduct the motion simulation in ADAMS. Set the simulation time t = 8.31 s and the simulation step size = 200 for simulation, and the result is shown in Figure 16.  Figure 16 shows the end state of the pickup manipulator. The running curve of the pickup manipulator is obtained by using an improved artificial potential field to avoid an obstacle in the obstacle environment and then using the cosine adaptive genetic algorithm to optimize the trajectory. It can be seen from the trajectory curve that the pickup manipulator can successfully avoid the obstacle and smoothly reach the target point along the path of obstacle avoidance planning and trajectory optimization.  Figure 17 shows the displacement curve of the end-effector along each coordinate axes. It can be seen from Figure 17 that the trajectory of the end-effector along the coordinate axes is continuous, without large fluctuation. The issues of local minimum values and unreachable targets are solved, and the motion law is reasonable. It proves the correctness of the theoretical derivation of the the improved artificial potential field method and the cosine adaptive genetic algorithm.

Experimental
According to the above two experimental results, the pickup manipulator can successfully avoid the obstacle and smoothly reach the target point along the path of obstacle avoidance planning and trajectory optimization. When the obstacle point is close to the target point, the improved artificial potential field method can avoid the end-effector swing between the obstacle point and the target point and solve the problem of the unreachable target. When the end-effector is in local minimum value, the improved artificial potential field method can make the end-effector jump out of the local minimum value quickly and solve the problem of the local minimum value of the artificial potential field method. Combining with the improved artificial potential field method and cosine adaptive genetic algorithm, the overall running time of the pickup manipulator is, respectively, reduced by 51.6% and 53.85%. During the movement, the trajectory of the end-effector is continuous, without large fluctuations.

Conclusion
(1) In order to shorten the working time of the pickup manipulator and improve its operating efficiency, this work establishes a time objective function for the pickup manipulator. According to the motion constraint conditions, the fitness function is designed, and the relation between fitness function and motion constraint is analyzed. The trajectory optimization model of the pickup manipulator is established by cosine adaptive genetic algorithm.
(2) In order to avoid collision between the pickup manipulator and the obstacle in the working environment, an improved artificial potential field method is proposed to analyze the obstacle avoidance planning of the pickup manipulator. For a defect of target unreachable of the traditional artificial potential field method, the distance term ) , ( g n X X d between the pickup manipulator and the target point is increased. The obstacle radius r is introduced into the repulsion potential field function. For a defect of the local minimum value, the virtual sub-target point is added for improvement. (3) This work designs a simulation platform of a five-DOF pickup manipulator. Combining improved artificial potential field method and cosine adaptive genetic algorithm to conduct trajectory optimization motion simulation on the five-DOF pickup manipulator. The results show that by combining the improved artificial potential field method and the cosine adaptive genetic algorithm, the pickup manipulator can perform obstacle avoidance planning. Moreover, the problems of unreachable targets and local minimums value are solved, and the trajectory optimization under obstacle avoidance conditions is effectively realized. However, the problem of the optimization of the obstacle avoidance path is not considered in this work.