# Improved Manipulator Obstacle Avoidance Path Planning Based on Potential Field Method

Authors: Ming Zhao; Xiaoqing Lv
Journal: Journal of Robotics
 (2020)

---

## Abstract

Aiming at the existing artificial potential field method, it still has the defects of easy to fall into local extremum, low success rate and unsatisfactory path when solving the problem of obstacle avoidance path planning of manipulator. An improved method for avoiding obstacle path of manipulator is proposed. First, the manipulator is subjected to invisible obstacle processing to reduce the possibility of its own collision. Second, establish dynamic virtual target points to enhance the predictive ability of the manipulator to the road ahead. Then, the artificial potential field method is used to guide the manipulator movement. When the manipulator is in a local extreme or oscillating, the extreme point jump-out function is used in real time to make the end point of the manipulator produce small displacements and change the action direction to effectively jump out of the dilemma. Finally, the manipulator is controlled to avoid all obstacles and move smoothly to form a spatial optimization path from the start point to the end point. The simulation experiment shows that the proposed method is more suitable for complex working environment and effectively improves the success rate of manipulator path planning, which provides a reference for further developing the application of manipulator in complex environment.


---

## Body

## 1. Introduction

The manipulator obstacle avoidance path planning [1] refers to: planning a feasible path from the starting point to the end point of the manipulator end. During the operation, the manipulator is required to not collide with any obstacles. This is a more complicated three-dimensional obstacle path planning problem in space.Common manipulator obstacle avoidance path planning methods include grid method [2], free space method [3, 4], rapidly-exploring random tree (RRT) algorithm [5], probabilistic road map algorithm [6], particle swarm optimization [7], ant colony algorithm [8], genetic algorithm [9], artificial potential field method [10], and so on. Compared with other methods, the artificial potential field method has the advantages of simple configuration and high operating efficiency, but the algorithm belongs to the local optimization method. It is used in the manipulator of multi-link structure, sometimes there are problems such as stagnation and oscillation. In view of the shortcomings of the artificial potential field method in the obstacle avoidance path planning of manipulators, relevant scholars have done a lot of research. Literature [11] introduced the laws of dynamics to improve the artificial potential field method, and used the attraction velocity, and the repulsive velocity to construct the joint velocity of the joint space, and planned an obstacle avoidance path for the manipulator. However, the improved algorithm still has the problem of being easily trapped in local extremum. The literature [12] uses the navigation potential function method to deal with the local extremum problem, but the inverse kinematics solution is applied in the overall path planning, which is computationally complex, and inefficient. In literature [13], the solution of adding virtual obstacles is applied to solve the minimum value problem. However, before this scheme, a reasonable set of angle values needs to be obtained, and the related operations are not easy to implement. Literature [14] attracts the current point away from the local extremum by adding a virtual target point. However, geometric methods are used to determine virtual target points, and its applicability is not strong in complex environments. Literature [15, 16] combined with the random advantage of RRT algorithm to generate virtual target points to escape local extremum, but there are too many uncertain factors in the process, and the path is not guaranteed to be optimal.Based on the above analysis, it can be seen that the defects of the artificial potential field method restrict the completion quality of the manipulator obstacle avoidance path planning task. This paper starts from reducing the possibility of falling into local extremum and path optimization, by establishing the dynamic virtual target points to improve the ability of the manipulator to predict obstacles in front; by setting the extreme point jump function to help the manipulator to jump out of local extremum or oscillation. Among them, the random principle of the dynamic virtual target point reduces the possibility that the manipulator is trapped in the local extremum, and the extreme point jump-out function not only effectively solves the problem of extreme point, and oscillation but also reduces the unnecessary moving distance as much as possible, which is beneficial to improve the overall path quality.


## 2. Kinematics Modeling and Analysis of Manipulator

### 2.1. Kinematics Modeling of Manipulator

This paper takes PUMA560 as the research object, and its specific structure diagram is available from reference [17]. The PUMA560 is an articulated robot whose first three joints determine the position of the end and the last three joints determine the orientation of the end. Since this paper studies the path planning problem at the end of the manipulator, it does not involve the grasping action, so only the angle changes of the first three joints are concerned. The degrees of the last three joints are fixed to zero in the discussion that follows.According to the structural diagram of the manipulator and the coordinate system of the link parameters established by the D-H [18, 19] parameter method, the link parameters of the PUMA560 [17, 20] can be obtained, as shown in Table 1.Table 1
The link parameters of the PUMA560.
Linki
α
i
−
1 [°]
a
i
−
1
d
i
θ
i
Joint angle range [°]
1
0
0
0
θ
1
−160~160
2
−90
0
d
2
θ
2
−225~45
3
0
a
2
0
θ
3
−45~225
4
−90
a
3
d
4
θ
4
−110~170
5
90
0
0
θ
5
−100~100
6
−90
0
0
θ
6
−266~266
Among thema2=431.8mma3=20.32mmd2=149.09mmd4=433.07mm.


### 2.2. Kinematics Analysis of the Manipulator

After the parameters are determined, the manipulator can be analyzed for positive kinematics. In the case of a given base coordinate, the end position, and attitude can be obtained by the transformation formula. Its transformation formula is as follows:(1)Tii−1=cθi−sθi0ai−1sθicαi−1cθicαi−1−sαi−1−dsαi−1sθisαi−1cθisαi−1cαi−1dcαi−10001.Substituting the link parameters information of PUMA560 into Equation (1), the homogeneous transformation matrix between the links can be calculated. By multiplying the link transformation matrices, a matrix of end positions and poses can be obtained. That is, its positive kinematics equation, as shown in Equation (2).(2)60T=10Tθ121Tθ232Tθ343Tθ454Tθ465Tθ6.


## 2.1. Kinematics Modeling of Manipulator

This paper takes PUMA560 as the research object, and its specific structure diagram is available from reference [17]. The PUMA560 is an articulated robot whose first three joints determine the position of the end and the last three joints determine the orientation of the end. Since this paper studies the path planning problem at the end of the manipulator, it does not involve the grasping action, so only the angle changes of the first three joints are concerned. The degrees of the last three joints are fixed to zero in the discussion that follows.According to the structural diagram of the manipulator and the coordinate system of the link parameters established by the D-H [18, 19] parameter method, the link parameters of the PUMA560 [17, 20] can be obtained, as shown in Table 1.Table 1
The link parameters of the PUMA560.
Linki
α
i
−
1 [°]
a
i
−
1
d
i
θ
i
Joint angle range [°]
1
0
0
0
θ
1
−160~160
2
−90
0
d
2
θ
2
−225~45
3
0
a
2
0
θ
3
−45~225
4
−90
a
3
d
4
θ
4
−110~170
5
90
0
0
θ
5
−100~100
6
−90
0
0
θ
6
−266~266
Among thema2=431.8mma3=20.32mmd2=149.09mmd4=433.07mm.


## 2.2. Kinematics Analysis of the Manipulator

After the parameters are determined, the manipulator can be analyzed for positive kinematics. In the case of a given base coordinate, the end position, and attitude can be obtained by the transformation formula. Its transformation formula is as follows:(1)Tii−1=cθi−sθi0ai−1sθicαi−1cθicαi−1−sαi−1−dsαi−1sθisαi−1cθisαi−1cαi−1dcαi−10001.Substituting the link parameters information of PUMA560 into Equation (1), the homogeneous transformation matrix between the links can be calculated. By multiplying the link transformation matrices, a matrix of end positions and poses can be obtained. That is, its positive kinematics equation, as shown in Equation (2).(2)60T=10Tθ121Tθ232Tθ343Tθ454Tθ465Tθ6.


## 3. Collision Detection

### 3.1. Modeling of Obstacles

Obstacle modeling is one of the important steps in obstacle avoidance path planning. In order to ensure that there is no collision and to simplify the calculation as much as possible, this paper uses the circumscribed ball method [21] to establish the obstacle model.(i)
First wrap the obstacle in the smallest cube.(ii)
When the long side of the cube is less than 3 times the short side, make the circumscribed ball of the cube at this time. Otherwise, the cube is divided into two parts by a plane passing through the midpoint of each long side.(iii)
The wrapped information of the two parts is released, and the two parts are treated separately as obstacles. That is, steps (i), (ii), and (iii) are repeated until the obstacle is completely wrapped by the circumscribing ball.(iv)
After converting the obstacle into one or more circumscribed balls, obtain the center and radius of each circumscribed ball.


### 3.2. Collision Detection

The links of the manipulator have a certain cross-sectional area. In order to conveniently establish a collision detection system, each link is enveloped into a cylinder. The radius of the section of the cylinder is measured as the radius of the link of the manipulator. In order to reduce the complexity of the calculation, the radius of the link of the manipulator is added to the radius of each circumscribed ball of the obstacle. At this time, the positional relationship between the obstacle and the link of the manipulator is transformed into a simple ball-to-wire relationship. The collision detection process is as follows:When the position coordinates of the two ends of the link arexi,yi,zi and xi+1,yi+1,zi+1, a two-point equation of the straight line is established.(3)x−xixi+1−xi=y−yiyi+1−yi=z−zizi+1−zi.Letx−xi/xi+1−xi=y−yi/yi+1−yi=z−zi/zi+1−zi=t. After sorting, a parametric equation is obtained, which is shown in Equation (4).(4)x=txi+1−xi+xiy=tyi+1−yi+yiz=tzi+1−zi+zi.The standard equation for the ball is shown in Equation (5).(5)x−x02+y−y02+z−z02=r2,among them,x0,y0,z0 is the coordinates of the center of the circumscribed ball; r is the sum of the radius of the circumscribed ball and the radius of the manipulator link. Bring Equation (4) into Equation (5), and get a quadratic equation about t. The value of parameter t can be used to determine whether the intersection of the line and the ball is within the range of xi,yi,zi to xi+1,yi+1,zi+1. When 0≤t≤1, its intersection is within the range of the line segment, otherwise it is not. Therefore, by analyzing the solutions t1 and t2 of the equation, the positional relationship between the ball and the line can be judged. The collision analysis [16] is shown in Table 2.Table 2
Table of collision analysis.
Number of solutions
Range of solutions
Collision or not
0
No solution
No
1
0
≤
t
≤
1
Yes
1
t
<
0 or t>1
No
2
0
≤
t
1
≤
1 or 0≤t2≤1 or (t1<0 and t2>1) or (t1>1 and t2<0)
Yes
2
(t1<0 and t2<0) and (t1>1 and t2>1)
No


## 3.1. Modeling of Obstacles

Obstacle modeling is one of the important steps in obstacle avoidance path planning. In order to ensure that there is no collision and to simplify the calculation as much as possible, this paper uses the circumscribed ball method [21] to establish the obstacle model.(i)
First wrap the obstacle in the smallest cube.(ii)
When the long side of the cube is less than 3 times the short side, make the circumscribed ball of the cube at this time. Otherwise, the cube is divided into two parts by a plane passing through the midpoint of each long side.(iii)
The wrapped information of the two parts is released, and the two parts are treated separately as obstacles. That is, steps (i), (ii), and (iii) are repeated until the obstacle is completely wrapped by the circumscribing ball.(iv)
After converting the obstacle into one or more circumscribed balls, obtain the center and radius of each circumscribed ball.


## 3.2. Collision Detection

The links of the manipulator have a certain cross-sectional area. In order to conveniently establish a collision detection system, each link is enveloped into a cylinder. The radius of the section of the cylinder is measured as the radius of the link of the manipulator. In order to reduce the complexity of the calculation, the radius of the link of the manipulator is added to the radius of each circumscribed ball of the obstacle. At this time, the positional relationship between the obstacle and the link of the manipulator is transformed into a simple ball-to-wire relationship. The collision detection process is as follows:When the position coordinates of the two ends of the link arexi,yi,zi and xi+1,yi+1,zi+1, a two-point equation of the straight line is established.(3)x−xixi+1−xi=y−yiyi+1−yi=z−zizi+1−zi.Letx−xi/xi+1−xi=y−yi/yi+1−yi=z−zi/zi+1−zi=t. After sorting, a parametric equation is obtained, which is shown in Equation (4).(4)x=txi+1−xi+xiy=tyi+1−yi+yiz=tzi+1−zi+zi.The standard equation for the ball is shown in Equation (5).(5)x−x02+y−y02+z−z02=r2,among them,x0,y0,z0 is the coordinates of the center of the circumscribed ball; r is the sum of the radius of the circumscribed ball and the radius of the manipulator link. Bring Equation (4) into Equation (5), and get a quadratic equation about t. The value of parameter t can be used to determine whether the intersection of the line and the ball is within the range of xi,yi,zi to xi+1,yi+1,zi+1. When 0≤t≤1, its intersection is within the range of the line segment, otherwise it is not. Therefore, by analyzing the solutions t1 and t2 of the equation, the positional relationship between the ball and the line can be judged. The collision analysis [16] is shown in Table 2.Table 2
Table of collision analysis.
Number of solutions
Range of solutions
Collision or not
0
No solution
No
1
0
≤
t
≤
1
Yes
1
t
<
0 or t>1
No
2
0
≤
t
1
≤
1 or 0≤t2≤1 or (t1<0 and t2>1) or (t1>1 and t2<0)
Yes
2
(t1<0 and t2<0) and (t1>1 and t2>1)
No


## 4. Manipulator Obstacle Avoidance Path Planning

### 4.1. Theoretical Basis of Artificial Potential Field Method in Three-Dimensional Space

The artificial potential field method is an extremely important method in path planning. Its outstanding advantages are high efficiency and operability. The basic principle of the artificial potential field method is to place the controlled object in an abstract artificial potential field environment. The target point in the environment produces “attractive force” on it, and the obstacle produces a “repulsive force” on it, and the combined force of the two forces guides the controlled object to move.In three-dimensional space, the attractive potential field function of the artificial potential field method is:(6)UattXj=12kρ2Xj,Xg,among them,Xj=xj,yj,zj is the position coordinate of the jth step of the controlled object, that is, the current position point; Xg=xg,yg,zg is the position coordinate of the target point; k is the gain coefficient of the attractive force; ρXj,Xg=Xj−Xg is the shortest distance between Xj and Xg. For Equation (6) to find a negative gradient [22] for Xj, the attraction function is:(7)FattXj=−∇Uatt=kρXj,Xg.Then the attractive component of pointXj on the three coordinate axes is:(8)FattxXj,Xg=FattXj,Xg×cosαj,x,FattyXj,Xg=FattXj,Xg×cosαj,y,FattzXj,Xg=FattXj,Xg×cosαj,z,among them,αj,x,αj,y, and αj,z are the angles between the line connecting the point Xj and the obstacle center and the x,y, and z axes, respectively.The obstacle is treated by the method in Section3.1, and the radius of the ith i=1,2,…,n circumscribed ball is ri, and the center of the ball is X0,i=x0,i,y0,i,z0,i. The direction vector pointed by the obstacle to Xj is denoted as ei, as shown in Equation (9). At this time, the position coordinate Xi,j=xi,j,yi,j,zi,j of the closest point [21] of Xj and the circumscribed ball can be calculated, as in the Equation (10).(9)ei=xj−x0i,yj−y0i,zj−z0ixj−x0i2+yj−y0i2+zj−z0i2,(10)Xi,j=xi,j,yi,j,zi,j=x0i+riai,y0i+ribi,z0i+rici,among them,(11)aibici=xj−x0ixj−x0i2+yj−y0i2+zj−z0i2yj−y0ixj−x0i2+yj−y0i2+zj−z0i2zj−z0ixj−x0i2+yj−y0i2+zj−z0i2.The acquisition of the relevant repulsion is transformed into a point-to-point calculation. The repulsive potential field function is:(12)UrepXj=12η1ρXj,Xi,j−1ρ02ρXj,Xi,j≤ρ00ρXj,Xi,j>ρ0,among them,η is the gain coefficient of the repulsive force; ρ0 is the influence distance of the obstacle; ρXj,Xi,j=Xj−Xi,j is the shortest distance between Xj and Xi,j. For Equation (12) to find a negative gradient for Xj, the repulsion function is:(13)FrepXj=−∇Urep=η1ρXj,Xi,j−1ρ01ρ2Xj,Xi,j.Then the repulsive component of pointXj on the three axes is:(14)FrepxXj,Xi,j=FrepXj,Xi,j×cosβj,x,FrepyXj,Xi,j=FrepXj,Xi,j×cosβj,y,FrepzXj,Xi,j=FrepXj,Xi,j×cosβj,z,among them,βj,x,βj,y, and βj,z are the angles between the line connecting the point Xj and the obstacle center and the x,y, and z axes, respectively.After combining the attractive force and the repulsive force, the resultant forceFj at point Xj can be obtained.(15)Fj=Fjx+Fjy+Fjz,among them,Fjx,Fjy, and Fjz are the resultant force components on the three coordinate axes to point Xj, as shown in Equation (16).(16)Fjx=FattxXj,Xg+∑iFrepxXj,Xi,j,Fjy=FattyXj,Xg+∑iFrepyXj,Xi,j,Fjz=FattzXj,Xg+∑iFrepzXj,Xi,j.According to the resultant forceFj and the step size l, the position Xj+1=xj+1,yj+1,zj+1 of the next step can be determined, and the components are as shown in Equation (17).(17)xj+1=xj+l×FjxFj,yj+1=yj+l×FjyFj,zj+1=zj+l×FjzFj.According to the above theory, obstacle avoidance path planning in a three-dimensional space with respect to one point can be achieved. This laid the foundation for the more complex manipulator's obstacle avoidance path planning.


### 4.2. Obstacle Avoidance Path Planning Method for Manipulator

#### 4.2.1. Establish Dynamic Virtual Target Points

The artificial potential field method belongs to a kind of local optimization algorithm. In order to achieve better global optimization effect, this paper uses dynamic virtual target points to guide the manipulator movement. That is, before each step of action, a virtual target point is determined according to a certain principle, which attracts the end of the manipulator and guides the arm to move. The selection process of the virtual target point is as follows. First consider the end of the manipulator as a point. Then, according to the theory of Section4.1, taking the current coordinates of the point as the starting point and the target point as the end point, an optimal path for obstacle avoidance is planned. Calculate the total length of the path. When its length is small, the virtual target point is equal to the target point. Otherwise, a point on the path (which is a random point within the specified range) is selected as the virtual target point of the manipulator’s current action. Record the virtual target point here as Xgg.


#### 4.2.2. Variable Parameterρ0 Is Used

In the normal case,ρ0 is set to a fixed value, that is, the influence distance in the whole process adopts the same standard. However, when there are obstacles of different sizes in the space and the difference in the radius of the circumscribed sphere is large, if a small ρ0 is used at this time, the phenomenon that the controlled points are too close to the large obstacles is likely to occur. At this time, it is necessary to make a large adjustment to avoid obstacles, which will greatly increase the difficulty of control or lengthen the path length. In response to this problem, this paper proposes that the radius of the obstacle itself determines its influence distance. When the radius of the circumscribed ball of the ith obstacle is ri, its influence distance ρ0,i is ρ0,i=b∗ri, where b is a constant.


#### 4.2.3. Handling of Invisible Obstacles

Due to the complex structure of the manipulator, its end may collide with itself during operation. In order to reduce this possibility, the initial joint angle portion of the manipulator is treated as an invisible obstacle. The circumscribed ball center of the obstacle is the position coordinate of the starting joint angle, and the radius is set to 1.5 times the link radius. It participates in the calculation of all relevant repulsions.


#### 4.2.4. Set the Extreme Point Jump-Out Function

If the current joint angle has caused the resultant force at the end of the manipulator to be the smallest among all adjacent joint angles [23], but the end position corresponding to the joint angle does not reach the target point, the manipulator at this time has fallen into a local minimum point. In order to effectively solve this problem, this paper sets the extreme point jump-out function. This function is called in time when it is detected that the end position of the manipulator is repeated with a position that has passed before (including the case of oscillation). First, Bloch Quantum Genetic Algorithm (BQGA) [24] is used to generate a small displacement at the end of the manipulator. Then, the RRT algorithm [5, 15, 16] is used to change the direction of motion of the manipulator. Finally, under the joint action of the two algorithms, the local extremum is jumped out.The process of using BQGA to generate small displacements is as follows:(1)
Parameter initialization of BQGA. Current iteration numbert, initial group Qt, maximum number of iterations M−g, variables λ1,λ2,λ3, and so on.(2)
The spatial transformation of the solution yields an approximate solution setXt.(3)
Calculate fitness. Collision detection of the end of the manipulator. If it collides, the fitness function is set to infinity. Otherwise, calculate the fitness function (the resultant force at the end of the manipulator). Obtain contemporary optimal solutionsBX and contemporary optimal chromosomes BC.(4)
TakeBX as the global optimal solution GX and BC as the global optimal chromosome GC.(5)
In the loop, sett←t+1, and obtain a new population Qt by updating and mutating Qt−1.(6)
Transform the optimization result in the unit space into the solution space to get the solutionXt of the optimization problem.(7)
IffitBX<fitGX, update the contemporary optimal solution BX←GX,BC←GC. Otherwise GX←BX,GC←BC.(8)
Ift>M−g, output the global optimal solution GX=λ1,λ2,λ3, and end. Otherwise, return to step (5).(9)
At this time, the angle of each joint angle of the arm isT=T1+λ1T2+λ2T3+λ3000, so that the corresponding end position TT can be obtained according to the kinematics of the manipulator, that is, the process of completing the small displacement of the end of the manipulator.The process of changing the direction of motion using the RRT algorithm is as follows:(1)
Start with the current position of the end of the manipulator and record it asqstart. Given the separation distance Δl.(2)
A random pointqrand is generated in the working space of the manipulator.(3)
Find the nodeqnear closest to qrand in the RRT and calculate the Euclidean distance l from qnear to qrand. If l<Δl, the new node qnew is equal to qrand. Otherwise, qnew is equal to the point after qnew extends Δl in the direction of qrand.(4)
Determine whether the space segment ofqnear to qnew collides with an obstacle. If it collides, round off qnew and return to step (2). Otherwise, add qnew to the tree.(5)
Determine whether the maximum number of iterations has been reached. If not, return to step (2) to continue the loop. Otherwise, draw the path and mark the node at the end of the path as the virtual target point, denoted asXggg.(6)
The direction in whichqstart points to Xggg is taken as the next action direction of the manipulator.In the commonly used path planning method [16], the virtual target point method is also used to jump out of the local extremum. Its process is to create a virtual target point near the end, after the end needs to reach the virtual target point, and then move to the final target point. In contrast, the method in this paper only changes the direction of the action through the randomly generated virtual target point, and does not need to arrive, that is, after running one step, the virtual target point disappears instantly. This can improve work efficiency and ensure path quality.In general, the random principle of dynamic virtual target points reduces the likelihood that the manipulator will fall into local extremum. The dynamic virtual target point and the appropriate influence distance make it difficult for the end of the manipulator to reach a dangerous area that is too close to the obstacle, which improves the obstacle prediction ability of the manipulator to some extent. The invisible obstacle treatment better overcomes the problem of the structural structure of the manipulator. Setting the extreme point jump-out function solves the stagnation and oscillation of the manipulator. The application of these measures improves the effectiveness and applicability of the manipulator’s obstacle avoidance path planning system, and is conducive to planning a more ideal obstacle avoidance path.


### 4.3. Manipulator’s Obstacle Avoidance Path Planning Process

According to the above theory, the basic steps of the obstacle avoidance path planning of the manipulator are summarized as follows:(1)
The manipulator’s kinematics modeling and obstacle modeling using the methods of Sections2.1 and 3.1, respectively.(2)
Parameter initialization. For example, the initial angle is denoted asT0, the starting point is marked as X0, the target point is marked as Xg0, the step size is recorded as λ, the gain coefficient of the attractive force is denoted as k, the gain coefficient of the repulsive force is denoted as η, the joint angle combination is recorded as T (initialized as T=T0), the corresponding end position is denoted by TT (initialized as TT=X0), the dynamic virtual target point is denoted by Xg, and the switching instruction is denoted by sg (initialized to sg=0).(3)
Is it judged thatsg=0 is established? If it is established, Xg=Xgg, if it is not established, then Xg=Xggg and sg=0, where Xgg is derived from the method of Section 4.2.1 and Xggg is derived from the method of Section 4.2.4.(4)
Traverse the adjacent joint angles. There are three possible values for each joint angle. They areθi−λ,θi and θi+λ, where i=1,2,…,m, and m is the number of joint angles that can change the end position. Set the joint angle that does not affect the end position to zero.(5)
According to the kinematics analysis of the manipulator in Section2.2, obtain the position information of the end and each joint angle.(6)
Collision detection is performed on each link using the method in Section3.2. If it collides, it is rounded off, otherwise, the information of the angle combination and its corresponding end position information are saved.(7)
Calculate the resultant force at all end positions in step (6), find the end position where the resultant force is the smallest, and save the end position (denoted asTTnext) and its corresponding angle combination (denoted as Tnext).(8)
It is judged whetherTTnext in step (7) reaches the vicinity of the target point Xg0, and if so, it ends, otherwise, it goes to step (9).(9)
Determine whether theTTnext at this time is the same as the position that has passed before, and if so, call the extreme point jump-out function and set sg=1 , then return to step (3). Otherwise, the manipulator is driven according to the angle information at this time, that is, T=Tnext, TT=TTnext, and then returns to step (3).The corresponding flow chart is shown in Figure1:Figure 1
Flow chart of the obstacle avoidance path planning of the manipulator.


## 4.1. Theoretical Basis of Artificial Potential Field Method in Three-Dimensional Space

The artificial potential field method is an extremely important method in path planning. Its outstanding advantages are high efficiency and operability. The basic principle of the artificial potential field method is to place the controlled object in an abstract artificial potential field environment. The target point in the environment produces “attractive force” on it, and the obstacle produces a “repulsive force” on it, and the combined force of the two forces guides the controlled object to move.In three-dimensional space, the attractive potential field function of the artificial potential field method is:(6)UattXj=12kρ2Xj,Xg,among them,Xj=xj,yj,zj is the position coordinate of the jth step of the controlled object, that is, the current position point; Xg=xg,yg,zg is the position coordinate of the target point; k is the gain coefficient of the attractive force; ρXj,Xg=Xj−Xg is the shortest distance between Xj and Xg. For Equation (6) to find a negative gradient [22] for Xj, the attraction function is:(7)FattXj=−∇Uatt=kρXj,Xg.Then the attractive component of pointXj on the three coordinate axes is:(8)FattxXj,Xg=FattXj,Xg×cosαj,x,FattyXj,Xg=FattXj,Xg×cosαj,y,FattzXj,Xg=FattXj,Xg×cosαj,z,among them,αj,x,αj,y, and αj,z are the angles between the line connecting the point Xj and the obstacle center and the x,y, and z axes, respectively.The obstacle is treated by the method in Section3.1, and the radius of the ith i=1,2,…,n circumscribed ball is ri, and the center of the ball is X0,i=x0,i,y0,i,z0,i. The direction vector pointed by the obstacle to Xj is denoted as ei, as shown in Equation (9). At this time, the position coordinate Xi,j=xi,j,yi,j,zi,j of the closest point [21] of Xj and the circumscribed ball can be calculated, as in the Equation (10).(9)ei=xj−x0i,yj−y0i,zj−z0ixj−x0i2+yj−y0i2+zj−z0i2,(10)Xi,j=xi,j,yi,j,zi,j=x0i+riai,y0i+ribi,z0i+rici,among them,(11)aibici=xj−x0ixj−x0i2+yj−y0i2+zj−z0i2yj−y0ixj−x0i2+yj−y0i2+zj−z0i2zj−z0ixj−x0i2+yj−y0i2+zj−z0i2.The acquisition of the relevant repulsion is transformed into a point-to-point calculation. The repulsive potential field function is:(12)UrepXj=12η1ρXj,Xi,j−1ρ02ρXj,Xi,j≤ρ00ρXj,Xi,j>ρ0,among them,η is the gain coefficient of the repulsive force; ρ0 is the influence distance of the obstacle; ρXj,Xi,j=Xj−Xi,j is the shortest distance between Xj and Xi,j. For Equation (12) to find a negative gradient for Xj, the repulsion function is:(13)FrepXj=−∇Urep=η1ρXj,Xi,j−1ρ01ρ2Xj,Xi,j.Then the repulsive component of pointXj on the three axes is:(14)FrepxXj,Xi,j=FrepXj,Xi,j×cosβj,x,FrepyXj,Xi,j=FrepXj,Xi,j×cosβj,y,FrepzXj,Xi,j=FrepXj,Xi,j×cosβj,z,among them,βj,x,βj,y, and βj,z are the angles between the line connecting the point Xj and the obstacle center and the x,y, and z axes, respectively.After combining the attractive force and the repulsive force, the resultant forceFj at point Xj can be obtained.(15)Fj=Fjx+Fjy+Fjz,among them,Fjx,Fjy, and Fjz are the resultant force components on the three coordinate axes to point Xj, as shown in Equation (16).(16)Fjx=FattxXj,Xg+∑iFrepxXj,Xi,j,Fjy=FattyXj,Xg+∑iFrepyXj,Xi,j,Fjz=FattzXj,Xg+∑iFrepzXj,Xi,j.According to the resultant forceFj and the step size l, the position Xj+1=xj+1,yj+1,zj+1 of the next step can be determined, and the components are as shown in Equation (17).(17)xj+1=xj+l×FjxFj,yj+1=yj+l×FjyFj,zj+1=zj+l×FjzFj.According to the above theory, obstacle avoidance path planning in a three-dimensional space with respect to one point can be achieved. This laid the foundation for the more complex manipulator's obstacle avoidance path planning.


## 4.2. Obstacle Avoidance Path Planning Method for Manipulator

### 4.2.1. Establish Dynamic Virtual Target Points

The artificial potential field method belongs to a kind of local optimization algorithm. In order to achieve better global optimization effect, this paper uses dynamic virtual target points to guide the manipulator movement. That is, before each step of action, a virtual target point is determined according to a certain principle, which attracts the end of the manipulator and guides the arm to move. The selection process of the virtual target point is as follows. First consider the end of the manipulator as a point. Then, according to the theory of Section4.1, taking the current coordinates of the point as the starting point and the target point as the end point, an optimal path for obstacle avoidance is planned. Calculate the total length of the path. When its length is small, the virtual target point is equal to the target point. Otherwise, a point on the path (which is a random point within the specified range) is selected as the virtual target point of the manipulator’s current action. Record the virtual target point here as Xgg.


### 4.2.2. Variable Parameterρ0 Is Used

In the normal case,ρ0 is set to a fixed value, that is, the influence distance in the whole process adopts the same standard. However, when there are obstacles of different sizes in the space and the difference in the radius of the circumscribed sphere is large, if a small ρ0 is used at this time, the phenomenon that the controlled points are too close to the large obstacles is likely to occur. At this time, it is necessary to make a large adjustment to avoid obstacles, which will greatly increase the difficulty of control or lengthen the path length. In response to this problem, this paper proposes that the radius of the obstacle itself determines its influence distance. When the radius of the circumscribed ball of the ith obstacle is ri, its influence distance ρ0,i is ρ0,i=b∗ri, where b is a constant.


### 4.2.3. Handling of Invisible Obstacles

Due to the complex structure of the manipulator, its end may collide with itself during operation. In order to reduce this possibility, the initial joint angle portion of the manipulator is treated as an invisible obstacle. The circumscribed ball center of the obstacle is the position coordinate of the starting joint angle, and the radius is set to 1.5 times the link radius. It participates in the calculation of all relevant repulsions.


### 4.2.4. Set the Extreme Point Jump-Out Function

If the current joint angle has caused the resultant force at the end of the manipulator to be the smallest among all adjacent joint angles [23], but the end position corresponding to the joint angle does not reach the target point, the manipulator at this time has fallen into a local minimum point. In order to effectively solve this problem, this paper sets the extreme point jump-out function. This function is called in time when it is detected that the end position of the manipulator is repeated with a position that has passed before (including the case of oscillation). First, Bloch Quantum Genetic Algorithm (BQGA) [24] is used to generate a small displacement at the end of the manipulator. Then, the RRT algorithm [5, 15, 16] is used to change the direction of motion of the manipulator. Finally, under the joint action of the two algorithms, the local extremum is jumped out.The process of using BQGA to generate small displacements is as follows:(1)
Parameter initialization of BQGA. Current iteration numbert, initial group Qt, maximum number of iterations M−g, variables λ1,λ2,λ3, and so on.(2)
The spatial transformation of the solution yields an approximate solution setXt.(3)
Calculate fitness. Collision detection of the end of the manipulator. If it collides, the fitness function is set to infinity. Otherwise, calculate the fitness function (the resultant force at the end of the manipulator). Obtain contemporary optimal solutionsBX and contemporary optimal chromosomes BC.(4)
TakeBX as the global optimal solution GX and BC as the global optimal chromosome GC.(5)
In the loop, sett←t+1, and obtain a new population Qt by updating and mutating Qt−1.(6)
Transform the optimization result in the unit space into the solution space to get the solutionXt of the optimization problem.(7)
IffitBX<fitGX, update the contemporary optimal solution BX←GX,BC←GC. Otherwise GX←BX,GC←BC.(8)
Ift>M−g, output the global optimal solution GX=λ1,λ2,λ3, and end. Otherwise, return to step (5).(9)
At this time, the angle of each joint angle of the arm isT=T1+λ1T2+λ2T3+λ3000, so that the corresponding end position TT can be obtained according to the kinematics of the manipulator, that is, the process of completing the small displacement of the end of the manipulator.The process of changing the direction of motion using the RRT algorithm is as follows:(1)
Start with the current position of the end of the manipulator and record it asqstart. Given the separation distance Δl.(2)
A random pointqrand is generated in the working space of the manipulator.(3)
Find the nodeqnear closest to qrand in the RRT and calculate the Euclidean distance l from qnear to qrand. If l<Δl, the new node qnew is equal to qrand. Otherwise, qnew is equal to the point after qnew extends Δl in the direction of qrand.(4)
Determine whether the space segment ofqnear to qnew collides with an obstacle. If it collides, round off qnew and return to step (2). Otherwise, add qnew to the tree.(5)
Determine whether the maximum number of iterations has been reached. If not, return to step (2) to continue the loop. Otherwise, draw the path and mark the node at the end of the path as the virtual target point, denoted asXggg.(6)
The direction in whichqstart points to Xggg is taken as the next action direction of the manipulator.In the commonly used path planning method [16], the virtual target point method is also used to jump out of the local extremum. Its process is to create a virtual target point near the end, after the end needs to reach the virtual target point, and then move to the final target point. In contrast, the method in this paper only changes the direction of the action through the randomly generated virtual target point, and does not need to arrive, that is, after running one step, the virtual target point disappears instantly. This can improve work efficiency and ensure path quality.In general, the random principle of dynamic virtual target points reduces the likelihood that the manipulator will fall into local extremum. The dynamic virtual target point and the appropriate influence distance make it difficult for the end of the manipulator to reach a dangerous area that is too close to the obstacle, which improves the obstacle prediction ability of the manipulator to some extent. The invisible obstacle treatment better overcomes the problem of the structural structure of the manipulator. Setting the extreme point jump-out function solves the stagnation and oscillation of the manipulator. The application of these measures improves the effectiveness and applicability of the manipulator’s obstacle avoidance path planning system, and is conducive to planning a more ideal obstacle avoidance path.


## 4.2.1. Establish Dynamic Virtual Target Points

The artificial potential field method belongs to a kind of local optimization algorithm. In order to achieve better global optimization effect, this paper uses dynamic virtual target points to guide the manipulator movement. That is, before each step of action, a virtual target point is determined according to a certain principle, which attracts the end of the manipulator and guides the arm to move. The selection process of the virtual target point is as follows. First consider the end of the manipulator as a point. Then, according to the theory of Section4.1, taking the current coordinates of the point as the starting point and the target point as the end point, an optimal path for obstacle avoidance is planned. Calculate the total length of the path. When its length is small, the virtual target point is equal to the target point. Otherwise, a point on the path (which is a random point within the specified range) is selected as the virtual target point of the manipulator’s current action. Record the virtual target point here as Xgg.


## 4.2.2. Variable Parameterρ0 Is Used

In the normal case,ρ0 is set to a fixed value, that is, the influence distance in the whole process adopts the same standard. However, when there are obstacles of different sizes in the space and the difference in the radius of the circumscribed sphere is large, if a small ρ0 is used at this time, the phenomenon that the controlled points are too close to the large obstacles is likely to occur. At this time, it is necessary to make a large adjustment to avoid obstacles, which will greatly increase the difficulty of control or lengthen the path length. In response to this problem, this paper proposes that the radius of the obstacle itself determines its influence distance. When the radius of the circumscribed ball of the ith obstacle is ri, its influence distance ρ0,i is ρ0,i=b∗ri, where b is a constant.


## 4.2.3. Handling of Invisible Obstacles

Due to the complex structure of the manipulator, its end may collide with itself during operation. In order to reduce this possibility, the initial joint angle portion of the manipulator is treated as an invisible obstacle. The circumscribed ball center of the obstacle is the position coordinate of the starting joint angle, and the radius is set to 1.5 times the link radius. It participates in the calculation of all relevant repulsions.


## 4.2.4. Set the Extreme Point Jump-Out Function

If the current joint angle has caused the resultant force at the end of the manipulator to be the smallest among all adjacent joint angles [23], but the end position corresponding to the joint angle does not reach the target point, the manipulator at this time has fallen into a local minimum point. In order to effectively solve this problem, this paper sets the extreme point jump-out function. This function is called in time when it is detected that the end position of the manipulator is repeated with a position that has passed before (including the case of oscillation). First, Bloch Quantum Genetic Algorithm (BQGA) [24] is used to generate a small displacement at the end of the manipulator. Then, the RRT algorithm [5, 15, 16] is used to change the direction of motion of the manipulator. Finally, under the joint action of the two algorithms, the local extremum is jumped out.The process of using BQGA to generate small displacements is as follows:(1)
Parameter initialization of BQGA. Current iteration numbert, initial group Qt, maximum number of iterations M−g, variables λ1,λ2,λ3, and so on.(2)
The spatial transformation of the solution yields an approximate solution setXt.(3)
Calculate fitness. Collision detection of the end of the manipulator. If it collides, the fitness function is set to infinity. Otherwise, calculate the fitness function (the resultant force at the end of the manipulator). Obtain contemporary optimal solutionsBX and contemporary optimal chromosomes BC.(4)
TakeBX as the global optimal solution GX and BC as the global optimal chromosome GC.(5)
In the loop, sett←t+1, and obtain a new population Qt by updating and mutating Qt−1.(6)
Transform the optimization result in the unit space into the solution space to get the solutionXt of the optimization problem.(7)
IffitBX<fitGX, update the contemporary optimal solution BX←GX,BC←GC. Otherwise GX←BX,GC←BC.(8)
Ift>M−g, output the global optimal solution GX=λ1,λ2,λ3, and end. Otherwise, return to step (5).(9)
At this time, the angle of each joint angle of the arm isT=T1+λ1T2+λ2T3+λ3000, so that the corresponding end position TT can be obtained according to the kinematics of the manipulator, that is, the process of completing the small displacement of the end of the manipulator.The process of changing the direction of motion using the RRT algorithm is as follows:(1)
Start with the current position of the end of the manipulator and record it asqstart. Given the separation distance Δl.(2)
A random pointqrand is generated in the working space of the manipulator.(3)
Find the nodeqnear closest to qrand in the RRT and calculate the Euclidean distance l from qnear to qrand. If l<Δl, the new node qnew is equal to qrand. Otherwise, qnew is equal to the point after qnew extends Δl in the direction of qrand.(4)
Determine whether the space segment ofqnear to qnew collides with an obstacle. If it collides, round off qnew and return to step (2). Otherwise, add qnew to the tree.(5)
Determine whether the maximum number of iterations has been reached. If not, return to step (2) to continue the loop. Otherwise, draw the path and mark the node at the end of the path as the virtual target point, denoted asXggg.(6)
The direction in whichqstart points to Xggg is taken as the next action direction of the manipulator.In the commonly used path planning method [16], the virtual target point method is also used to jump out of the local extremum. Its process is to create a virtual target point near the end, after the end needs to reach the virtual target point, and then move to the final target point. In contrast, the method in this paper only changes the direction of the action through the randomly generated virtual target point, and does not need to arrive, that is, after running one step, the virtual target point disappears instantly. This can improve work efficiency and ensure path quality.In general, the random principle of dynamic virtual target points reduces the likelihood that the manipulator will fall into local extremum. The dynamic virtual target point and the appropriate influence distance make it difficult for the end of the manipulator to reach a dangerous area that is too close to the obstacle, which improves the obstacle prediction ability of the manipulator to some extent. The invisible obstacle treatment better overcomes the problem of the structural structure of the manipulator. Setting the extreme point jump-out function solves the stagnation and oscillation of the manipulator. The application of these measures improves the effectiveness and applicability of the manipulator’s obstacle avoidance path planning system, and is conducive to planning a more ideal obstacle avoidance path.


## 4.3. Manipulator’s Obstacle Avoidance Path Planning Process

According to the above theory, the basic steps of the obstacle avoidance path planning of the manipulator are summarized as follows:(1)
The manipulator’s kinematics modeling and obstacle modeling using the methods of Sections2.1 and 3.1, respectively.(2)
Parameter initialization. For example, the initial angle is denoted asT0, the starting point is marked as X0, the target point is marked as Xg0, the step size is recorded as λ, the gain coefficient of the attractive force is denoted as k, the gain coefficient of the repulsive force is denoted as η, the joint angle combination is recorded as T (initialized as T=T0), the corresponding end position is denoted by TT (initialized as TT=X0), the dynamic virtual target point is denoted by Xg, and the switching instruction is denoted by sg (initialized to sg=0).(3)
Is it judged thatsg=0 is established? If it is established, Xg=Xgg, if it is not established, then Xg=Xggg and sg=0, where Xgg is derived from the method of Section 4.2.1 and Xggg is derived from the method of Section 4.2.4.(4)
Traverse the adjacent joint angles. There are three possible values for each joint angle. They areθi−λ,θi and θi+λ, where i=1,2,…,m, and m is the number of joint angles that can change the end position. Set the joint angle that does not affect the end position to zero.(5)
According to the kinematics analysis of the manipulator in Section2.2, obtain the position information of the end and each joint angle.(6)
Collision detection is performed on each link using the method in Section3.2. If it collides, it is rounded off, otherwise, the information of the angle combination and its corresponding end position information are saved.(7)
Calculate the resultant force at all end positions in step (6), find the end position where the resultant force is the smallest, and save the end position (denoted asTTnext) and its corresponding angle combination (denoted as Tnext).(8)
It is judged whetherTTnext in step (7) reaches the vicinity of the target point Xg0, and if so, it ends, otherwise, it goes to step (9).(9)
Determine whether theTTnext at this time is the same as the position that has passed before, and if so, call the extreme point jump-out function and set sg=1 , then return to step (3). Otherwise, the manipulator is driven according to the angle information at this time, that is, T=Tnext, TT=TTnext, and then returns to step (3).The corresponding flow chart is shown in Figure1:Figure 1
Flow chart of the obstacle avoidance path planning of the manipulator.


## 5. Simulation Experiment

In order to verify the effectiveness of the path planning method proposed in this paper, simulation experiments were carried out with Matlab R2016b. The simulation experiment is from single obstacle to multi-obstacle, and compared with the simulation results of common methods. The initialization of the parameters is shown in Table3.Table 3
Table of manipulator initialization parameters.
Parameter
Value
T
0
/
rad
(0, 0, 0, 0, 0, 0)
λ/rad
0.01
π
k
10
η
1Record the total number of steps required for the manipulator to complete the task asssum. During the movement, the angle changes of the three joints are respectively recorded as Δθ1,s, Δθ2,s, and Δθ3,s, where s=1,2,…,ssum, then their cumulative angle changes are:(18)Δθ1=Δθ1,1+Δθ1,2+⋯+Δθ1,ssum,Δθ2=Δθ2,1+Δθ2,2+⋯+Δθ2,ssum,Δθ3=Δθ3,1+Δθ3,2+⋯+Δθ3,ssum.Therefore, the cumulative angle change of the entire manipulator is:(19)Δθsum=Δθ1+Δθ2+Δθ3.During the movement, the coordinate value of the end position of the manipulator is recorded asXss=1,2,…,ssum, and the change of the end position is as follows:(20)ΔXsXs,Xs−1=Xs−Xs−1.That is,ΔXsXs,Xs−1 is the distance between two points Xs and Xs−1. X0 is the starting position coordinate. Therefore, the end moving distance during the entire movement is approximately:(21)ΔXsum=ΔX1+ΔX2+⋯+ΔXssum.


### 5.1. Simulation Experiment of Single Obstacle Path Planning

Let the coordinates of the target point beXg0=0.3500,−0.2500,0.4000, and calculate the spherical center coordinates and radius of the obstacle according to the established obstacle model as Xob=0.4000,−0.3000,−0.1500 and r=0.2500, respectively. According to the flow chart of the obstacle avoidance path planning of the manipulator, the simulation results are shown in Figures 2 and 3.Figure 2
Path plan of an obstacle.Figure 3
Changes in the joint angle of each.It can be seen from the Figure2 that the manipulator avoids the obstacle and accurately reaches the vicinity of the target point, forming a relatively smooth path. It can be seen from Figure 3 that the angles of the joint angles of the manipulators change continuously in small amplitude during the stepping process, which ensures the stability of the movement of the manipulator. Observing the whole movement process of the manipulator, the joints and links do not collide with the obstacle. Under a certain safety margin, the end of the manipulator advances step by step toward the target point. ssum is only 52 when the process is completed. The cumulative angular change of the entire manipulator Δθsum is 4.1783 rad, and the cumulative end moving distance ΔXsum is 1.2232 m. In the experiment of single obstacles, there was no situation of falling into local extremum or oscillation, and the path planning task was completed smoothly.


### 5.2. Simulation Experiment of Multiple Obstacle Path Planning

Let the number of obstacles be three,Xg0=0.3000,−0.1500,0.7200, Xob=0.6000,0.3500,−0.1000;0.7000,−0.4000,0.3000;0.5000,−0.2000,−0.3000, r=0.1500,0.2500,0.1200, and other parameters remain unchanged. The simulation results from the starting point to the target point are shown in Figures 4 and 5. Table 4 records the coordinate information of each path point where the end position of the manipulator travels.Figure 4
Path plan of three obstacles.Figure 5
Changes in the joint angle of each.Table 4
Coordinate information of the path point of the manipulator.
Steps
T
T
x
y
z
Steps
T
T
x
y
z
1
0.4519
0.1491
−
0.4195
23
0.4730
0.1343
−
0.1498
2
0.4513
0.1491
−
0.4060
24
0.4775
0.1342
−
0.1350
3
0.4502
0.1491
−
0.3924
25
0.4775
0.1342
−
0.1350
…
…
26
0.4468
0.1351
−
0.1569
19
0.4627
0.1346
−
0.1789
…
…
20
0.4812
0.1340
−
0.1606
64
0.3251
−
0.1284
0.7131
21
0.4812
0.1340
−
0.1606
65
0.3150
−
0.1318
0.7233
22
0.4549
0.1349
−
0.1678
66
0.2000
−
0.1500
0.7200It can be seen from Figure4 that the manipulator can still avoid obstacles successfully reaching the vicinity of the target point in a complicated working environment with three obstacles. During the operation, the whole manipulator did not collide with the obstacles, and it was not too close to the obstacles, which proved that the manipulator can predict the obstacles in advance. It can be seen from Figure 5 that the joint angle of the manipulator changes continuously without abnormal fluctuation or static phenomenon. The overall movement of the manipulator is stable, and the end running path is smooth. ssum is only 59 when the process is completed. The cumulative angular change of the entire manipulator Δθsum is 4.2158 rad, and the cumulative end moving distance ΔXsum is 1.4400 m. Observing the coordinate information of each path point at the end of the manipulator in Table 4, the end position information is different. However, when observing the running process of the program, it was found that the manipulator had a phenomenon of repeating with the previous position in the 21th step and the 24th step, that is, the manipulator was caught in the local extreme value. After this happens, the main program calls the extreme point jump-out function twice, and adjusts the position of the end of the manipulator and its action direction in time. After only two adjustments, the arm jumped out of the local extremes that were trapped. The reason why the repeated position information does not appear in the process is because the adjusted information covers the repeated information when the extreme point jump-out function adjusts the manipulator. In this way, the manipulator will not stop moving during the whole operation. In this operation, the manipulator not only effectively addresses the extreme point and oscillation problems but also ensures the path quality as much as possible. Eventually, the manipulator runs stably along a more desirable path to the vicinity of the target point.


### 5.3. Comparison with Commonly Used Path Planning Methods

Literatures [15, 16] use the hybrid algorithm of artificial potential field method and RRT algorithm to realize the obstacle avoidance path planning of the manipulator. The main idea of the method is as follows: the artificial potential field method is used for path planning; when the manipulator is trapped in the local extremum, the RRT algorithm is used to establish the virtual target point to jump out of the local extremum; when the manipulator jumps out of the local extremum, the artificial potential field method continues to be used for path planning. This method is common in the existing literature. Therefore, this paper refers to it as the commonly used method for manipulator obstacle avoidance path planning. The disadvantage of using this method is that because the target point is fixed, the manipulator is more likely to fall into local extremum, and the path quality cannot be guaranteed during the process of jumping out of the local extremum. In order to further prove the effectiveness and superiority of the method in this paper, the following comparative experiments under the same conditions were carried out. The comparison data of the six groups are shown in Tables 5–7. Among them, the data results obtained by the method of this article are placed outside “()”, the data results obtained by the commonly used method are placed inside “()”, and ∞ indicates that the planning fails.Table 5
Comparison of data between two planning methods for an obstacle.
S/N
s
sum
Δ
θ
sum [rad]
Δ
X
sum [m]
Extreme point?
1
44 (45)
3.5500 (3.5500)
1.4616 (1.5082)
No (No)
2
52 (53)
4.2412 (4.2097)
1.3254 (1.5204)
No (No)
3
44 (46)
3.6757 (3.6128)
1.4493 (1.4825)
No (No)
4
51 (56)
3.8642 (4.0527)
1.4639 (1.5969)
No (No)
5
48 (48)
3.2358 (3.3615)
1.5348 (1.5828)
No (No)
6
50 (54)
3.8327 (4.0527)
1.5411 (1.4013)
No (No)
The corresponding histogram is shown in Figures6–8.Figure 6
A comparison chart ofssum when there is an obstacle.Figure 7
A comparison chart ofΔθsum when there is an obstacle.Figure 8
A comparison chart ofΔXsum when there is an obstacle.Table 6
Comparison of data between two planning methods for two obstacles.
S/N
s
sum
Δ
θ
sum [rad]
Δ
X
sum [m]
Extreme point?
1
64 (113)
5.5292 (8.4195)
1.4867 (2.2876)
No (Yes)
2
58 (53)
4.4726 (4.3354)
1.3254 (1.5356)
Yes (No)
3
70 (119)
5.9062 (8.8593)
1.5150 (2.1116)
No (Yes)
4
70 (∞)
5.8434 (∞)
1.5056 (∞)
No (∞)
5
72 (113)
5.9690 (11.0898)
1.5247 (2.5900)
No (Yes)
6
128 (∞)
6.4088 (∞)
1.5740 (∞)
Yes (∞)
The corresponding histogram is shown in Figures9–11, where the graph reaching the maximum represents ∞.Figure 9
A comparison chart ofssum when there are two obstacles.Figure 10
A comparison chart ofΔθsum when there are two obstacles.Figure 11
A comparison chart ofΔXsum when there are two obstacles.Table 7
Comparison of data between two planning methods for three or more obstacles.
S/N
s
sum
Δ
θ
sum [rad]
Δ
X
sum [m]
Extreme point?
1
78 (105)
6.5973 (8.9850)
1.5905 (2.2011)
No (Yes)
2
84 (150)
6.6602 (12.4093)
1.6221 (2.7519)
Yes (Yes)
3
91 (∞)
6.6403 (∞)
1.7230 (∞)
No (∞)
4
114 (130)
6.8801 (10.4301)
1.8434 (2.1753)
Yes (Yes)
5
∞ (∞)
∞ (∞)
∞ (∞)
∞ (∞)
6
143 (∞)
7.2257 (∞)
1.7650 (∞)
Yes (∞)
The corresponding histogram is shown in Figures12–14, where the graph reaching the maximum represents ∞.Figure 12
A comparison chart ofSsum when there are 3 or more obstacles.Figure 13
A comparison chart ofΔθsum when there are 3 or more obstacles.Figure 14
A comparison chart ofΔXsum when there are 3 or more obstacles.It can be obtained from Table5 (Figures 6–8) that in the case of an obstacle, the difference between the two methods is not large, and the path planning task can be completed well. However, it can be seen from the Table 6 (Figures 9–11) of the two obstacles that the probability of occurrence of extreme points in the commonly used methods begins to increase, and even the phenomenon of planning failure occurs. From the simulation data of three or more obstacles shown in Table 7 (Figures 12–14), when the environment is more complicated, the improved method proposed in this paper shows a big advantage in terms of the number of steps, the cumulative angle change, and the cumulative end moving distance. From the comprehensive analysis of the data in Tables 5–7, it can be seen that when the improved obstacle avoidance path planning method is applied, the manipulator can complete the task with fewer steps, and the overall angle change is small, and the end moving distance is obviously shortened. The success rate of path planning has also increased significantly. It can be calculated that the improved method proposed in this paper reduces the possibility of the manipulator falling into the extreme point by about 27.78%, and reduces the possibility of planning failure by about 22.22%. Observe the relevant simulation results of two or more obstacles and analyze the data that both methods are planned successfully. It can be obtained that compared with the commonly used method, the improved method of this paper reduces the planned step number by about 27.37%, reduces the cumulative angle change by about 31.66%, and shortens the cumulative end moving distance by about 28.77%, which fully reflects the effectiveness and superiority of the method.


## 5.1. Simulation Experiment of Single Obstacle Path Planning

Let the coordinates of the target point beXg0=0.3500,−0.2500,0.4000, and calculate the spherical center coordinates and radius of the obstacle according to the established obstacle model as Xob=0.4000,−0.3000,−0.1500 and r=0.2500, respectively. According to the flow chart of the obstacle avoidance path planning of the manipulator, the simulation results are shown in Figures 2 and 3.Figure 2
Path plan of an obstacle.Figure 3
Changes in the joint angle of each.It can be seen from the Figure2 that the manipulator avoids the obstacle and accurately reaches the vicinity of the target point, forming a relatively smooth path. It can be seen from Figure 3 that the angles of the joint angles of the manipulators change continuously in small amplitude during the stepping process, which ensures the stability of the movement of the manipulator. Observing the whole movement process of the manipulator, the joints and links do not collide with the obstacle. Under a certain safety margin, the end of the manipulator advances step by step toward the target point. ssum is only 52 when the process is completed. The cumulative angular change of the entire manipulator Δθsum is 4.1783 rad, and the cumulative end moving distance ΔXsum is 1.2232 m. In the experiment of single obstacles, there was no situation of falling into local extremum or oscillation, and the path planning task was completed smoothly.


## 5.2. Simulation Experiment of Multiple Obstacle Path Planning

Let the number of obstacles be three,Xg0=0.3000,−0.1500,0.7200, Xob=0.6000,0.3500,−0.1000;0.7000,−0.4000,0.3000;0.5000,−0.2000,−0.3000, r=0.1500,0.2500,0.1200, and other parameters remain unchanged. The simulation results from the starting point to the target point are shown in Figures 4 and 5. Table 4 records the coordinate information of each path point where the end position of the manipulator travels.Figure 4
Path plan of three obstacles.Figure 5
Changes in the joint angle of each.Table 4
Coordinate information of the path point of the manipulator.
Steps
T
T
x
y
z
Steps
T
T
x
y
z
1
0.4519
0.1491
−
0.4195
23
0.4730
0.1343
−
0.1498
2
0.4513
0.1491
−
0.4060
24
0.4775
0.1342
−
0.1350
3
0.4502
0.1491
−
0.3924
25
0.4775
0.1342
−
0.1350
…
…
26
0.4468
0.1351
−
0.1569
19
0.4627
0.1346
−
0.1789
…
…
20
0.4812
0.1340
−
0.1606
64
0.3251
−
0.1284
0.7131
21
0.4812
0.1340
−
0.1606
65
0.3150
−
0.1318
0.7233
22
0.4549
0.1349
−
0.1678
66
0.2000
−
0.1500
0.7200It can be seen from Figure4 that the manipulator can still avoid obstacles successfully reaching the vicinity of the target point in a complicated working environment with three obstacles. During the operation, the whole manipulator did not collide with the obstacles, and it was not too close to the obstacles, which proved that the manipulator can predict the obstacles in advance. It can be seen from Figure 5 that the joint angle of the manipulator changes continuously without abnormal fluctuation or static phenomenon. The overall movement of the manipulator is stable, and the end running path is smooth. ssum is only 59 when the process is completed. The cumulative angular change of the entire manipulator Δθsum is 4.2158 rad, and the cumulative end moving distance ΔXsum is 1.4400 m. Observing the coordinate information of each path point at the end of the manipulator in Table 4, the end position information is different. However, when observing the running process of the program, it was found that the manipulator had a phenomenon of repeating with the previous position in the 21th step and the 24th step, that is, the manipulator was caught in the local extreme value. After this happens, the main program calls the extreme point jump-out function twice, and adjusts the position of the end of the manipulator and its action direction in time. After only two adjustments, the arm jumped out of the local extremes that were trapped. The reason why the repeated position information does not appear in the process is because the adjusted information covers the repeated information when the extreme point jump-out function adjusts the manipulator. In this way, the manipulator will not stop moving during the whole operation. In this operation, the manipulator not only effectively addresses the extreme point and oscillation problems but also ensures the path quality as much as possible. Eventually, the manipulator runs stably along a more desirable path to the vicinity of the target point.


## 5.3. Comparison with Commonly Used Path Planning Methods

Literatures [15, 16] use the hybrid algorithm of artificial potential field method and RRT algorithm to realize the obstacle avoidance path planning of the manipulator. The main idea of the method is as follows: the artificial potential field method is used for path planning; when the manipulator is trapped in the local extremum, the RRT algorithm is used to establish the virtual target point to jump out of the local extremum; when the manipulator jumps out of the local extremum, the artificial potential field method continues to be used for path planning. This method is common in the existing literature. Therefore, this paper refers to it as the commonly used method for manipulator obstacle avoidance path planning. The disadvantage of using this method is that because the target point is fixed, the manipulator is more likely to fall into local extremum, and the path quality cannot be guaranteed during the process of jumping out of the local extremum. In order to further prove the effectiveness and superiority of the method in this paper, the following comparative experiments under the same conditions were carried out. The comparison data of the six groups are shown in Tables 5–7. Among them, the data results obtained by the method of this article are placed outside “()”, the data results obtained by the commonly used method are placed inside “()”, and ∞ indicates that the planning fails.Table 5
Comparison of data between two planning methods for an obstacle.
S/N
s
sum
Δ
θ
sum [rad]
Δ
X
sum [m]
Extreme point?
1
44 (45)
3.5500 (3.5500)
1.4616 (1.5082)
No (No)
2
52 (53)
4.2412 (4.2097)
1.3254 (1.5204)
No (No)
3
44 (46)
3.6757 (3.6128)
1.4493 (1.4825)
No (No)
4
51 (56)
3.8642 (4.0527)
1.4639 (1.5969)
No (No)
5
48 (48)
3.2358 (3.3615)
1.5348 (1.5828)
No (No)
6
50 (54)
3.8327 (4.0527)
1.5411 (1.4013)
No (No)
The corresponding histogram is shown in Figures6–8.Figure 6
A comparison chart ofssum when there is an obstacle.Figure 7
A comparison chart ofΔθsum when there is an obstacle.Figure 8
A comparison chart ofΔXsum when there is an obstacle.Table 6
Comparison of data between two planning methods for two obstacles.
S/N
s
sum
Δ
θ
sum [rad]
Δ
X
sum [m]
Extreme point?
1
64 (113)
5.5292 (8.4195)
1.4867 (2.2876)
No (Yes)
2
58 (53)
4.4726 (4.3354)
1.3254 (1.5356)
Yes (No)
3
70 (119)
5.9062 (8.8593)
1.5150 (2.1116)
No (Yes)
4
70 (∞)
5.8434 (∞)
1.5056 (∞)
No (∞)
5
72 (113)
5.9690 (11.0898)
1.5247 (2.5900)
No (Yes)
6
128 (∞)
6.4088 (∞)
1.5740 (∞)
Yes (∞)
The corresponding histogram is shown in Figures9–11, where the graph reaching the maximum represents ∞.Figure 9
A comparison chart ofssum when there are two obstacles.Figure 10
A comparison chart ofΔθsum when there are two obstacles.Figure 11
A comparison chart ofΔXsum when there are two obstacles.Table 7
Comparison of data between two planning methods for three or more obstacles.
S/N
s
sum
Δ
θ
sum [rad]
Δ
X
sum [m]
Extreme point?
1
78 (105)
6.5973 (8.9850)
1.5905 (2.2011)
No (Yes)
2
84 (150)
6.6602 (12.4093)
1.6221 (2.7519)
Yes (Yes)
3
91 (∞)
6.6403 (∞)
1.7230 (∞)
No (∞)
4
114 (130)
6.8801 (10.4301)
1.8434 (2.1753)
Yes (Yes)
5
∞ (∞)
∞ (∞)
∞ (∞)
∞ (∞)
6
143 (∞)
7.2257 (∞)
1.7650 (∞)
Yes (∞)
The corresponding histogram is shown in Figures12–14, where the graph reaching the maximum represents ∞.Figure 12
A comparison chart ofSsum when there are 3 or more obstacles.Figure 13
A comparison chart ofΔθsum when there are 3 or more obstacles.Figure 14
A comparison chart ofΔXsum when there are 3 or more obstacles.It can be obtained from Table5 (Figures 6–8) that in the case of an obstacle, the difference between the two methods is not large, and the path planning task can be completed well. However, it can be seen from the Table 6 (Figures 9–11) of the two obstacles that the probability of occurrence of extreme points in the commonly used methods begins to increase, and even the phenomenon of planning failure occurs. From the simulation data of three or more obstacles shown in Table 7 (Figures 12–14), when the environment is more complicated, the improved method proposed in this paper shows a big advantage in terms of the number of steps, the cumulative angle change, and the cumulative end moving distance. From the comprehensive analysis of the data in Tables 5–7, it can be seen that when the improved obstacle avoidance path planning method is applied, the manipulator can complete the task with fewer steps, and the overall angle change is small, and the end moving distance is obviously shortened. The success rate of path planning has also increased significantly. It can be calculated that the improved method proposed in this paper reduces the possibility of the manipulator falling into the extreme point by about 27.78%, and reduces the possibility of planning failure by about 22.22%. Observe the relevant simulation results of two or more obstacles and analyze the data that both methods are planned successfully. It can be obtained that compared with the commonly used method, the improved method of this paper reduces the planned step number by about 27.37%, reduces the cumulative angle change by about 31.66%, and shortens the cumulative end moving distance by about 28.77%, which fully reflects the effectiveness and superiority of the method.


## 6. Summary

Aiming at the problems existing in the commonly used path planning methods, this paper establishes a relatively complete mechanical arm obstacle avoidance path planning system by adopting a series of improvement measures such as establishing dynamic virtual target points and setting extreme point jump-out functions. The simulation results show that when dealing with complex working environment, the system still has the advantages of smooth running of the manipulator, continuous change of joint angle, less control steps, smaller cumulative angle change, and shorter cumulative end moving distance. It improves the success rate of path planning and also ensures path quality. The improved method proposed in this paper solves the problem of obstacle avoidance path planning of mechanical arm in three-dimensional space, which provides reference for manipulator control in saving energy and finding optimized path.



---

*Source: 1701943-2020-01-22.xml*