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

Abstract

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.

1. 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.

2. 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 $d^n(X,X_g)$ 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.

3. Trajectory Optimization Design

3.1. Kinematics Modeling

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).

$$\begin{array}{l}{}_i{}^{i-1}T=Rot(x_{i-1},{\alpha }_{i-1})Trans(x_{i-1},a_{i-1})Rot(z_i,{\theta }_i)Trans(z_i,d_i)\\ =\left[\begin{array}{cccc}\cos {\theta }_i& -\sin {\theta }_i& 0& a_{i-1}\\ \sin {\theta }_i\cos {\alpha }_{i-1}& \cos {\theta }_i\cos {\alpha }_{i-1}& -\sin {\alpha }_{i-1}& -d_i\sin {\alpha }_{i-1}\\ \sin {\theta }_i\sin {\alpha }_{i-1}& \cos {\theta }_i\sin {\alpha }_{i-1}& \cos {\alpha }_{i-1}& d_i\cos {\alpha }_{i-1}\\ 0& 0& 0& 1\end{array}\right]\end{array}$$

(1)

$${}_5{}^{0}T=\left[\begin{array}{cccc}{n}_x& o_x& a_x& p_x\\ n_y& o_y& a_y& p_y\\ n_z& o_z& a_z& p_z\\ 0& 0& 0& 1\end{array}\right]$$

(2)

$${}_5{}^{0}T={}_1{}^{0}T\left({\theta }_1\right){}_2{}^{1}T\left({\theta }_2\right){}_3{}^{2}T\left({\theta }_3\right){}_4{}^{3}T\left({\theta }_4\right){}_5{}^{4}T\left({\theta }_5\right)$$

(3)

Both sides of formula (3) left multiply the inverse matrix ${}_1{}^{0}T{}^{-1}$, then:

$$\left[\begin{array}{cccc}{c}_1& s_1& 0& 0\\ -s_1& c_1& 0& 0\\ 0& 0& 1& -d_1\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}{n}_x& o_x& a_x& p_x\\ n_y& o_y& a_y& p_y\\ n_z& o_z& a_z& p_z\\ 0& 0& 0& 1\end{array}\right]={}_1{}^{0}T{}^{-1}{}_5{}^{0}T,$$

(4)

$${}_1{}^{0}T{}^{-1}{}_5{}^{0}T=\left[\begin{array}{cccc}{c}_{234}{c}_5& -c_{234}{s}_5& -s_{234}& a_3{c}_{23}+a_2{c}_2+a_4{c}_{234}-d_5{s}_{234}\\ -s_5& -c_5& 0& 0\\ -s_{234}{c}_5& s_{234}{s}_5& -c_{234}& -a_3{s}_{23}-a_2{s}_2-d_5{c}_{234}-a_4{s}_{234}\\ 0& 0& 0& 1\end{array}\right],$$

(5)

The matrix of formula (4) is represented as L, and the matrix of formula (5) is represented as R. Take L (2, 4) = R (2, 4), that is, the elements in the second row and the fourth column of the left and right sides of the equation are equal, then:

$$-s_1{p}_x+c_1{p}_y=0,$$

(6)

Taking into account the formula (6), an analytical expression for the joint angle ${\theta }_1$ is obtained:

$${\theta }_1=a\tan 2\left(p_y,p_x\right),$$

(7)

According to formula (4) and formula (5), take L (3,3) = R (3,3), L (3,4) = R (3,4), then:

$${\theta }_2=a\tan 2\left(k,\pm \sqrt{{\left(a_3\sin {\theta }_3\right)}^2+{\left(a_3\cos {\theta }_3+a_2\right)}^2-k^2}\right)-a\tan 2\left(a_3\sin {\theta }_3,a_3\cos {\theta }_3+a_2\right),$$

(8)

According to formula (4) and formula (5), take L (1,4) = R (3,4), L (3,4) = R (3,4), then:

$$\{\begin{array}{l}\cos {\theta }_3=\frac{A^2+B^2-a_3^2-a_2^2}{2a_2{a}_3}\\ \sin {\theta }_3=\pm \sqrt{1-{(\frac{A^2+B^2-a_3^2-a_2^2}{2a_2{a}_3})}^2}\\ {\theta }_3=a\tan 2(\sin {\theta }_3,\cos {\theta }_3)\end{array},$$

(9)

In the formula, $k=d_1+d_5{a}_z-a_4\sqrt{1-a_z^2}-p_z$; $A=c_1{p}_x+s_1{p}_y+a_4{a}_z\pm d_5\sqrt{1-a_z^2}$; $B=p_z-d_1-d_5{a}_z\pm a_4\sqrt{1-a_z^2}$.

Both sides of formula (3) left multiply the inverse matrix ${}_2{}^{0}T{}^{-1}$, then:

$$\left[\begin{array}{cccc}{c}_1{c}_2& c_2{s}_1& -s_2& d_1{s}_2\\ -c_1{s}_2& -s_1{s}_2& -c_2& d_1{c}_2\\ -s_1& c_1& 0& 0\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}{n}_x& o_x& a_x& p_x\\ n_y& o_y& a_y& p_y\\ n_z& o_z& a_z& p_z\\ 0& 0& 0& 1\end{array}\right]={}_2{}^{0}T{}^{-1}{}_5{}^{0}T,$$

(10)

$${}_2{}^{0}T{}^{-1}{}_5{}^{0}T=\left[\begin{array}{cccc}{c}_{34}{c}_5& -c_{34}{s}_5& -s_{34}& a_2+a_4{c}_{34}-d_5{s}_{34}+a_3{c}_3\\ s_{34}{c}_5& -s_{34}{s}_5& c_{34}& d_5{c}_{34}+a_4{s}_{34}+a_3{s}_3\\ -s_5& -c_5& 0& 0\\ 0& 0& 0& 1\end{array}\right],$$

(11)

According to formula (10) and formula (11), take L (1,3) = R (1,3), L(2,3) = R(2,3), then:

$$\{\begin{array}{l}{\theta }_{34}=a\tan 2\left(\begin{array}{l}-\cos {\theta }_1\cos {\theta }_2{a}_x-\cos {\theta }_2\sin {\theta }_1{a}_y+\sin {\theta }_2{a}_z,\\ -\cos {\theta }_1\sin {\theta }_2{a}_x-\sin {\theta }_1\sin {\theta }_2{a}_y-\cos {\theta }_2{a}_z\end{array}\right)\\ {\theta }_4={\theta }_{34}-{\theta }_3\end{array},$$

(12)

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.

3.2. Objective Function and Motion Constraints

3.2.1. 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:

$$T_{all}=\min {\displaystyle \sum _{j=1}^{m-1}{h}_j},j=1,2,3,\cdots ,m-1,$$

(13)

In the formula, $T_{all}$ is the total time that the pickup manipulator runs from the initial position to the final position, and $h_j$ 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.

3.2.2. 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:

$$\max \left\{\left|{\dot{\theta }}_i(t)\right|\right\}\le {\dot{\theta }}_i{}_{\max }\left(i=1,2,3,\cdots ,5\right),$$

(14)

In the formula, ${\dot{\theta }}_i(t)$ is the angular velocity of the joint $i$ at time $t$, and ${\dot{\theta }}_{i\max }$ is the maximum angular velocity of the joint $i$.

According to the conditions and work requirements of the pickup manipulator, the settings of joint constraints are shown in

Table 1. Constraints on each joint.

Joint	${\dot{\mathit{\theta }}}_{\mathit{i}\mathbf{max}}/(°/\mathbf{s})$	${\ddot{\mathit{\theta }}}_{\mathit{i}\mathbf{max}}/(°/{\mathbf{s}}^2)$
1	120	50
2	120	50
3	120	50
4	150	75
5	150	75

.

Constraint of angular acceleration

The angular acceleration of the joint $i$ should meet the conditions:

$$\max \left\{\left|{\ddot{\theta }}_i(t)\right|\right\}\le {\ddot{\theta }}_{i\max }\left(i=1,2,3,\cdots ,5\right),$$

(15)

In the formula, ${\ddot{\theta }}_i(t)$ is the angular acceleration of the joint $i$ at time $t$, and ${\ddot{\theta }}_{i\max }$ is the maximum angular acceleration of the joint $i$.

3.3. Adaptive Genetic Algorithm

3.3.1. 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 [$t_0$,$t_f$]. If the time variable is represented by a binary coding of length $l$, [$t_0$,$t_f$] can be equally divided into $2^l-1$ segments. Thus, the accuracy of the time parameter can be adjusted by adjusting the value of the coding length $l$. The longer the length of the binary coding, the better the performance of the time division, and the higher the accuracy of the time variable, as shown in formula (14). In this work, we take the binary code length $l$ = 9, $t_f$ = 5.0, $t_0$ = 0. An encoding example of the time target is 110001010.

$$\delta =\frac{t_f-t_0}{2^l-1},$$

(16)

Decoding is the reverse operation of the encoding, that is, the process of converting the binary coding of the time-optimal target into a real number. If the binary coding corresponding to the time- optimal target is $x:x_1{x}_2\dots x_{l-1}{x}_l$, the decoding expression corresponding to the time variable of the pickup manipulator is:

$$t=t_0+\frac{t_f-t_0}{2^l-1}{\displaystyle \sum _{i=1}^l{x}_i{2}^{l-i}},$$

(17)

3.3.2. 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.

3.3.3. 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:

$$f_{}=\{\begin{array}{l}\frac{1}{h_j},\max \left\{\left|{\dot{\theta }}_i(t)\right|\right\}\le {\dot{\theta }}_{i\max } \mathrm{and} max\left\{\left|{\ddot{\theta }}_i(t)\right|\right\}\le {\ddot{\theta }}_{i\max }\\ \frac{1}{h_j^{\max }},\mathrm{Others}\end{array},$$

(18)

In the formula, $h_j^{\max }$ represents the maximum value within the time limit of the time individual $j$ that does not meet the motion constraint. $\max \left\{\left|{\dot{\theta }}_i(t)\right|\right\}$ is the maximum angular velocity of joint $i$ in the time range of $h_j$, $\max \left\{\left|{\ddot{\theta }}_i(t)\right|\right\}$ is the maximum angular acceleration of joint $i$ in the time range of $h_j$.

3.3.4. 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:

• Calculate the fitness $f(x_i)$ of individuals in the time population of the pickup manipulator and $i=1,2,\cdots ,N$, $x_i$ is the initial population, $N$ is the number of the initial population.
• Calculate the probability that an individual will be selected:

  $$p_i=\frac{f(x_i)}{{\displaystyle \sum _{u=1}^{N}f(x_u)}},i=1,2,3,\cdots ,N,$$

  (19)
• Calculate the cumulative probability of the individual:

  $$q_i={\displaystyle \sum _{u=1}^{i}p(x_u)},i=1,2,3,\cdots ,N,$$

  (20)
• Generate a uniformly distributed random number $r$ within [0, 1];
• If $r\le q\left[1\right]$, select individual 1, otherwise, select individual $k$ such that $q\left[k-1\right]\le q\left[k\right]$ is valid;
• 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 $p_i$ 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 $q_i$. When the wheel stops rotating, the area pointed by the pointer will be selected into the next generation population.

(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.

Two pairs of individuals are respectively set as: $A=(a_1,a_2,\dots ,a_{n-1},a_n)$,

$B=(b_1,b_2,\dots ,b_{n-1},b_n)$. Single point cross operation is carried out on individuals A and B, and the two newly encoded individuals produced are:

$$\{\begin{array}{l}{A}_1=(a_1,a_2,\dots ,a_{k-1},a_k,b_{k+1},b_{k+2},\dots ,b_{n-1},b_n)\\ B_1=\left(b_1,b_2,\dots ,b_{k-1},b_k,a_{k+1},a_{k+2},\dots ,a_{n-1},a_n\right)\end{array},$$

(21)

(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.

(5) Adaptive Dynamic Adjustment

For the adaptive genetic algorithm, the adaptive genetic algorithm proposed by Algethami et al. can dynamically adjust the value of $p_c$ and $p_m$ according to the fitness value [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):

$$p_c=\{\begin{array}{ll}\frac{p_{c1}+p_{c2}}{2}+\frac{(p_{c1}-p_{c2})}{2}\cos (\frac{f-f_{avg}}{f_{\max }-f_{avg}}\pi ),& f\ge f_{avg}\\ p_{c1},& f<f_{avg}\end{array},$$

(22)

$$p_m=\{\begin{array}{ll}\frac{p_{m1}+p_{m2}}{2}+\frac{(p_{m1}-p_{m2})}{2}\cos (\frac{f^{\prime }-f_{avg}}{f_{\max }-f_{avg}}\pi ),& f^{\prime }\ge f_{avg}\\ p_{m1},& f^{\prime }<f_{avg}\end{array},$$

(23)

In the formula, $p_{c1}$ is the cross probability when the individual fitness is the smallest, $p_{c2}$ is the cross probability when the individual fitness is the largest. $p_{m1}$ is the mutation probability when the individual fitness is the smallest, and $p_{m2}$ is the mutation probability when the individual fitness is the largest.

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 $p_{c1}$ = 0.7, $p_{c2}$ = 0.1, $p_{m1}$ = 0.08, $p_{m2}$ = 0.01 according to experience and repeated experiments.

4. Obstacle Avoidance Planning

4.1. 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.

4.1.1. 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:

$$\{\begin{array}{l}{U}_{att}(X)=\frac{1}{2}\eta d^2(X,X_g)\\ d^2(X,X_g)={\Vert X-X_g\Vert }^2=\left[{(x-x_g)}^2+{(y-y_g)}^2\right]\end{array},$$

(24)

In the formula, η refers to attraction potential field gain factor, $X=(p_x,p_y,p_z)$ refers to the position of the end-effector, $X_g$ refers to the position of the target point, $d(X,X_g)$ refers to the Euclidean distance between the end-effector and the target point.

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:

$$F_{att}(X)=-\nabla U_{att}(X)=-\eta d(X,X_g)\frac{\partial d(X,X_g)}{\partial X},$$

(25)

4.1.2. 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:

$$\{\begin{array}{ll}{U}_{rep}(X)=\frac{1}{2}\epsilon {(\frac{1}{d\left(X,X_{obs}\right)}-\frac{1}{d_0})}^2& ,X-X_{obs}\le d_0\\ U_{rep}(X)=0& ,X-X_{obs}>d_0\end{array},$$

(26)

In the formula, $\epsilon$ refers to repulsion potential field gain factor, $X_{obs}$ refers to the position of obstacles in the work environment, $d_0$ 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:

$$F_{rep}(X)=-\nabla U_{rep}(X)=\{\begin{array}{l}\epsilon (\frac{1}{d\left(X,X_{obs}\right)}-\frac{1}{d_0})\frac{1}{{(X-X_{obs})}^2}\frac{\partial d\left(X,X_{obs}\right)}{\partial X},X-X_{obs}\le d_0\\ 0,X-X_{obs}>d_0\end{array},$$

(27)

4.1.3. 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:

$$\{\begin{array}{l}U(X)=U_{att}(X)+{\displaystyle \sum _1^n{U}_{rep}(X)}\\ F(X)=F_{att}(X)+{\displaystyle \sum _1^n{F}_{rep}(X)}\end{array},$$

(28)

4.2. 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.

4.2.1. 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 $d^n(X,X_g)$ 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 $d_0$ the obstacle, the obstacle boundary is used as the collision condition. The distance $d(X,X_{obs})$ of the current point of the pickup manipulator to the obstacle boundary is calculated, and the repulsion potential field is constructed under this distance.

Thus, the mathematical expression of the improved repulsive potential field function is:

$$\{\begin{array}{l}{U}_{{}_{rep}}^{*}(X)=\frac{1}{2}\epsilon {(\frac{1}{d(X,X_{obs})-r}-\frac{1}{d_0})}^2{d}^n(X,X_g),X-X_{obs}\le d_0\\ U_{{}_{rep}}^{*}(X)=0,X-X_{obs}>d_0\end{array},$$

(29)

The improved repulsion force is generated by a negative gradient of the improved repulsion potential field function. The function expression relationship is:

$$F_{{}_{rep}}^{*}(X)=-\nabla U_{{}_{rep}}^{*}(X)=\{\begin{array}{l}{F}_{{}_{rep1}}^{*}+F_{{}_{rep2}}^{*},X-X_{obs}\le d_0\\ 0,X-X_{obs}>d_0\end{array},$$

(30)

When $X-X_{obs}>d_0$, it means that the distance between the end-effector and the obstacle exceeds the boundary affected by the repulsive potential field. The repulsion force is set to zero. When $X-X_{obs}<d_0$, the pickup manipulator is within the influence of the repulsion potential field. At this time, the repulsion force generated by the obstacle is composed of the repulsion component $F_{rep1}^{*}$ and the compensating attraction component $F_{rep2}^{*}$. The functional expressions are:

$$\{\begin{array}{l}{F}_{rep1}^{*}=\epsilon (\frac{1}{d(X,X_{obs})-r}-\frac{1}{d_0})\frac{1}{{(X-X_{obs})}^2}\frac{\partial d(X,X_{obs})}{\partial X}{d}^n(X,X_g),X-X_{obs}\le d_0\\ F_{rep2}^{*}=-\frac{n}{2}\epsilon {(\frac{1}{d(X,X_{obs})-r}-\frac{1}{d_0})}^2{d}^{n-1}(X,X_g)\frac{\partial d(X,X_g)}{\partial X},X-X_{obs}\le d_0\end{array},$$

(31)

In the formula, $F_{rep1}^{*}$ represents the repulsion force generated by the obstacle to the manipulator, pointing from the obstacle direction to the manipulator. $F_{rep2}^{*}$ is the compensation of attraction force for the target point unreachable, pointing from the direction of the pickup manipulator to the target point. The modes of the respective vectors of $F_{rep1}^{*}$ and $F_{rep2}^{*}$ are:

$$\{\begin{array}{l}\Vert F_{rep1}^{*}\Vert =\epsilon (\frac{1}{d(X,X_{obs})-r}-\frac{1}{d_0})\frac{1}{{(X-X_{obs})}^2}{d}^n(X,X_g),X-X_{obs}\le d_0\\ \Vert F_{rep2}^{*}\Vert =\frac{n}{2}\epsilon {(\frac{1}{d(X,X_{obs})-r}-\frac{1}{d_0})}^2{d}^{n-1}(X,X_g),X-X_{obs}\le d_0\end{array},$$

(32)

Compared with the original repulsion force $F_{rep}$, the improved repulsion component $F_{rep1}^{*}$ becomes smaller, but the direction is unchanged. The increased compensating attraction component $F_{rep2}^{*}$ points to the target point. So, the improved resultant repulsion force $F_{rep}^{*}$ becomes smaller and the direction tends toward the target point. The improved resultant repulsion force $F_{rep}^{*}$ is superimposed on the original attraction force, which makes the improved resultant force $F^{*}$ closer to the target point and solves the detection of the unreachable target.

The different values of the adjustment factor $n$ will have different effects on the improved repulsion function $F_{rep}^{*}$. The different values of $n$ are analyzed below.

(1) When 0 < $n$ < 1, the mathematical characteristics of $d^n(X,X_g)$ in $F_{rep1}^{*}$ and $d^{n-1}(X,X_g)$ in $F_{rep2}^{*}$ are analyzed, and the results are shown in Figure 8.

Analyzing the data in Figure 8, it can be founded that when $X\to X_g$, $d^n(X,X_g)\to 0$, $d^{n-1}(X,X_g)\to \infty$, then:

$$\{\begin{array}{l}\underset{X\to X_g}{\lim }\Vert F_{rep1}^{*}\Vert =\underset{X\to X_g}{\lim }\left[\epsilon (\frac{1}{d(X,X_{obs})-r}-\frac{1}{d_0})\frac{1}{{(X-X_{obs})}^2}{d}^n(X,X_g)\right]=0\\ \underset{X\to X_g}{\lim }\Vert F_{rep2}^{*}\Vert =\underset{X\to X_g}{\lim }\left[\frac{n}{2}\epsilon {(\frac{1}{d(X,X_{obs})-r}-\frac{1}{d_0})}^2{d}^{n-1}(X,X_g)\right]=\infty \end{array},$$

(33)

It can be found that when the pickup manipulator gradually approaches the target point, the improved repulsion component $F_{rep1}^{*}$ approaches zero. The compensated attraction component $F_{rep2}^{*}$ approaches infinity. Under the action of the resultant potential, the pickup manipulator can smoothly move to the target point.

(2) When $n$ = 1, the mathematical characteristics of $d^n(X,X_g)$ in $F_{rep1}^{*}$ and $d^{n-1}(X,X_g)$ in $F_{rep2}^{*}$ are analyzed, and the results are shown in Figure 9.

Analyzing the data in Figure 9, it can be found that when $X\to X_g$, $d^n(X,X_g)\to 0$, $d^{n-1}(X,X_g)\to c$, then:

$$\{\begin{array}{l}\underset{X\to X_g}{\lim }\Vert F_{rep1}^{*}\Vert =\underset{X\to X_g}{\lim }\left[\epsilon (\frac{1}{d(X,X_{obs})-r}-\frac{1}{d_0})\frac{1}{{(X-X_{obs})}^2}{d}^n(X,X_g)\right]=0\\ \underset{X\to X_g}{\lim }\Vert F_{rep2}^{*}\Vert =\underset{X\to X_g}{\lim }\left[\frac{n}{2}\epsilon {(\frac{1}{d(X,X_{obs})-r}-\frac{1}{d_0})}^2{d}^{n-1}(X,X_g)\right]=c\end{array},$$

(34)

It can be found that when the manipulator gradually approaches the target point, the improved repulsion component $F_{rep1}^{*}$ approaches zero. And the compensated attraction component $F_{rep2}^{*}$ approaches the constant c. The manipulator can smoothly move toward the target point under the action of the resultant potential.

(3) When $n$ > 1, the mathematical characteristics of $d^n(X,X_g)$ in $F_{rep1}^{*}$ and $d^{n-1}(X,X_g)$ in $F_{rep2}^{*}$ are analyzed, and the results are shown in Figure 10.

Analyzing the data in Figure 10, it can be founded that when $X\to X_g$, $d^n(X,X_g)\to 0$, $d^{n-1}(X,X_g)\to 0$, then:

$$\{\begin{array}{l}\underset{X\to X_g}{\lim }\Vert F_{rep1}^{*}\Vert =\underset{X\to X_g}{\lim }\left[\epsilon (\frac{1}{d(X,X_{obs})-r}-\frac{1}{d_0})\frac{1}{{(X-X_{obs})}^2}{d}^n(X,X_g)\right]=0\\ \underset{X\to X_g}{\lim }\Vert F_{rep2}^{*}\Vert =\underset{X\to X_g}{\lim }\left[\frac{n}{2}\epsilon {(\frac{1}{d(X,X_{obs})-r}-\frac{1}{d_0})}^2{d}^{n-1}(X,X_g)\right]=0\end{array},$$

(35)

It can be found that when the manipulator gradually approaches the target point, the improved repulsion component $F_{rep1}^{*}$ and the compensated attraction component $F_{rep2}^{*}$ approaches to zero at the same time. The manipulator can still move smoothly to the target point under the influence of the original potential attraction force $F_{att}$.

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 $F_{rep2}^{*}$ tends to be constant, and the transition is smooth. So, this work takes $n$ as 1 for operation.

4.2.2. 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.

5. Experimental Verification

5.1. Simulation Conditions

The experimental object is a five-DOF pickup manipulator and the kinematics parameter are $d_1$ = 240 mm, $a_2$ = 330 mm, $a_3$ = 210 mm, $a_4$ = 50 mm, $d_5$ = 90 mm. The selected obstacle radius is $r$ = 10 mm.

5.2. Experimental Verification

5.2.1. The Comparative Experiment of the Improved Artificial Potential Field Method and the Rapidly-Exploring Random Tree* Method

Create a work environment for the pickup manipulator, set the initial point of the end-effector $X_0$ = [1 cm, 18 cm], target point $X_g$ = [60 cm, 60 cm], and the center of obstacle M = [50 cm, 50 cm] 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. Path points of the end-effector obtained by the improved artificial potential field method.

Pi	x/cm	y/cm	Pi	x/cm	y/cm
1	1	18	14	32.772	40.617
2	3.444	19.74	15	35.216	42.357
3	5.888	21.48	16	37.66	44.097
4	8.332	23.219	17	40.104	45.837
5	10.776	24.959	18	42.548	47.577
6	13.22	26.699	19	44.992	49.316
7	15.664	28.439	20	47.436	51.056
8	18.108	30.179	21	47.708	54.044
9	20.552	31.918	22	50.407	55.352
10	22.996	33.658	23	53.107	56.66
11	25.44	36.398	24	55.807	57.968
12	27.884	37.138	25	58.507	59.276
13	30.328	38.878	26	60	60

. 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. Path points of the end-effector obtained by the Rapidly-exploring Random Tree* (RRT*) method.

Pi	x/cm	y/cm
1	1	18
2	10.82	19.889
3	19.264	25.247
4	27.761	30.519
5	37.056	34.209
6	45.685	39.262
7	55.679	38.93
8	66.288	54.877
9	60	60

.

Figure 12 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 P₂₀. Figure 13 shows the trajectory of the end-effector 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 P₆ and is much higher than the improved artificial potential field method. That reduces the working efficiency of the manipulator.

5.2.2. Simulation Experiment of the Improved Artificial Potential Field Method and Cosine Adaptive Genetic Algorithm

(1) General Obstacle Environment

Create a work environment for the pickup manipulator, set the initial point of the end-effector $X_0$ = [59 cm, 0 cm, 15 cm], target point $X_g$ = [45 cm, 20 cm, 0 cm], and the center of obstacle M = [53 cm, 10 cm, 20 cm] 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 4. Path points of the end-effector.

Pi	x/cm	y/cm	z/cm
1	59	0	15
2	57.28	2.458	13.333
3	55.592	4.915	11.667
4	53.839	7.373	10
5	50.863	7.753	8.333
6	48.366	9.416	6.667
7	47.457	12.274	5
8	46.548	15.133	3.333
9	45.638	17.992	1.667
10	45	20	0

. The established Kinematics model is used to calculate the joint angle, and the results are shown in

Table 5. Joint angles of the pickup manipulator at path points.

Pi	θ1/(°)	θ2/(°)	θ3/(°)	θ4/(°)	θ5/(°)
1	0	0	0	0	0
2	2.46	−9.41	29.06	−19.65	2.46
3	5.05	−11.60	39.94	−28.34	5.05
4	7.80	−12.52	47.88	−35.36	7.80
5	8.67	−14.87	60.99	−46.12	8.67
6	11.02	−15.11	68.87	−53.76	11.02
7	14.50	−12.85	68.52	−55.66	14.50
8	18.01	−10.33	67.19	−56.86	18.01
9	21.52	−7.56	64.86	−57.31	21.52
10	23.96	−4.64	61.91	−57.28	23.96

.

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. Time optimization results of the pickup manipulator at each path point.

Pi	Joint 1/(s)	Joint 2/(s)	Joint 3/(s)	Joint 4/(s)	Joint 5/(s)
1	0	0	0	0	0
2	0.5577	1.0568	1.8787	1.2329	0.4697
3	0.5577	0.5284	1.1448	0.9100	0.4697
4	0.5871	0.3523	0.9687	0.7632	0.4697
5	0.3229	0.5284	1.2329	0.9393	0.2642
6	0.5284	0.1761	0.9687	0.7926	0.4403
7	0.6458	0.5284	0.2055	0.4110	0.5284
8	0.6458	0.5577	0.4110	0.3229	0.5284
9	0.6458	0.5871	0.5284	0.2348	0.5284
10	0.5577	0.5871	0.5871	0.0587	0.4697

.

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. Comparison of time optimization results in the obstacle environment.

Path Points	Before Optimization/(s)	Optimized/(s)	Time Difference/(s)	Optimization Rate/(%)
1-2	2	1.8787	0.1213	6.1
2-3	2	1.1448	0.522	26.1
3-4	2	0.9687	1.0313	51.6
4-5	2	1.2329	0.7671	38.4
5-6	2	0.9687	1.0313	51.6
6-7	2	0.6458	1.3542	67.7
7-8	2	0.6458	1.3542	67.7
8-9	2	0.6458	1.3542	67.7
9-10	2	0.5871	1.4129	70.6

.

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 to conduct the motion simulation in ADAMS(Adams 2016, MSC, LA, America and 1963). Set the simulation time t = 8.72 s and the simulation step size = 200 for the simulation, and the results are shown in Figure 14 and Figure 15.

Figure 14 depicts the overall motion state of the pickup manipulator under the obstacle environment. 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.

(2) Obstacle Environment with the Problems of Local Minimum Value and Unreachable Target

Create a work environment with the problems of local minimum value and unreachable target for the pickup manipulator, set the initial point of the end-effector $X_o$ = [60 cm, 0 cm, 15 cm], target point $X_g$ = [45 cm, 10 cm, 15 cm], and the center of obstacle M = [49 cm, 6 cm, 16 cm] 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 8. Path points of the end-effector.

Pi	x/cm	y/cm	z/cm
1	60	0	15
2	57.504	1.664	15
3	55.008	3.328	15
4	52.512	4.992	15
5	50.265	6.98	15
6	52.572	8.9	15
7	49.603	9.33	15
8	47.792	11.722	15
9	45.238	10.147	15
10	45	10	15

.

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. 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. Comparison of time optimization results in obstacle environment.

Path Points	Before Optimization/(s)	Optimized/(s)	Time Difference/(s)	Optimization Rate/(%)
1-2	2	1.4579	0.5421	27.11
2-3	2	1.2524	0.7476	37.38
3-4	2	0.9785	1.0215	51.08
4-5	2	0.8317	1.1683	58.42
5-6	2	0.9497	1.0503	52.52
6-7	2	0.9883	1.0117	50.59
7-8	2	0.6262	1.3738	68.69
8-9	2	0.91	1.09	54.5
9-10	2	0.3131	1.6869	84.35

.

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.

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.

6. Conclusions

(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 $d^n(X,X_g)$ 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.