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.
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.
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:
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).
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 are and , a two-point equation of the straight line is established.
Let . After sorting, a parametric equation is obtained, which is shown in Equation (4).
The standard equation for the ball is shown in Equation (5).
among them, is the coordinates of the center of the circumscribed ball; 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 . The value of parameter can be used to determine whether the intersection of the line and the ball is within the range of to . When , its intersection is within the range of the line segment, otherwise it is not. Therefore, by analyzing the solutions and of the equation, the positional relationship between the ball and the line can be judged. The collision analysis [16] is shown in Table 2.
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:
among them, is the position coordinate of the th step of the controlled object, that is, the current position point; is the position coordinate of the target point; is the gain coefficient of the attractive force; is the shortest distance between and . For Equation (6) to find a negative gradient [22] for , the attraction function is:
Then the attractive component of point on the three coordinate axes is:
among them, , and are the angles between the line connecting the point and the obstacle center and the , and axes, respectively.
The obstacle is treated by the method in Section 3.1, and the radius of the th circumscribed ball is , and the center of the ball is . The direction vector pointed by the obstacle to is denoted as , as shown in Equation (9). At this time, the position coordinate of the closest point [21] of and the circumscribed ball can be calculated, as in the Equation (10).
among them,
The acquisition of the relevant repulsion is transformed into a point-to-point calculation. The repulsive potential field function is:
among them, is the gain coefficient of the repulsive force; is the influence distance of the obstacle; is the shortest distance between and . For Equation (12) to find a negative gradient for , the repulsion function is:
Then the repulsive component of point on the three axes is:
among them, , and are the angles between the line connecting the point and the obstacle center and the , and axes, respectively.
After combining the attractive force and the repulsive force, the resultant force at point can be obtained.
among them, , and are the resultant force components on the three coordinate axes to point , as shown in Equation (16).
According to the resultant force and the step size , the position of the next step can be determined, and the components are as shown in Equation (17).
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 Section 4.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 .
4.2.2. Variable Parameter Is Used
In the normal case, 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 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 obstacle is , its influence distance is , where 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 number , initial group , maximum number of iterations , variables , and so on.(2)The spatial transformation of the solution yields an approximate solution set .(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 solutions and contemporary optimal chromosomes .(4)Take as the global optimal solution and as the global optimal chromosome .(5)In the loop, set , and obtain a new population by updating and mutating .(6)Transform the optimization result in the unit space into the solution space to get the solution of the optimization problem.(7)If , update the contemporary optimal solution . Otherwise .(8)If , output the global optimal solution , and end. Otherwise, return to step (5).(9)At this time, the angle of each joint angle of the arm is , so that the corresponding end position 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 as . Given the separation distance .(2)A random point is generated in the working space of the manipulator.(3)Find the node closest to in the RRT and calculate the Euclidean distance from to . If , the new node is equal to . Otherwise, is equal to the point after extends in the direction of .(4)Determine whether the space segment of to collides with an obstacle. If it collides, round off and return to step (2). Otherwise, add 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 as .(6)The direction in which points to 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 Sections 2.1 and 3.1, respectively.(2)Parameter initialization. For example, the initial angle is denoted as , the starting point is marked as , the target point is marked as , the step size is recorded as , the gain coefficient of the attractive force is denoted as , the gain coefficient of the repulsive force is denoted as , the joint angle combination is recorded as (initialized as ), the corresponding end position is denoted by (initialized as ), the dynamic virtual target point is denoted by , and the switching instruction is denoted by (initialized to ).(3)Is it judged that is established? If it is established, , if it is not established, then and , where is derived from the method of Section 4.2.1 and 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 and , where , and 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 Section 2.2, obtain the position information of the end and each joint angle.(6)Collision detection is performed on each link using the method in Section 3.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 as ) and its corresponding angle combination (denoted as ).(8)It is judged whether in step (7) reaches the vicinity of the target point , and if so, it ends, otherwise, it goes to step (9).(9)Determine whether the 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 , then return to step (3). Otherwise, the manipulator is driven according to the angle information at this time, that is, , , and then returns to step (3).
The corresponding flow chart is shown in Figure 1:
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 Table 3.
Record the total number of steps required for the manipulator to complete the task as . During the movement, the angle changes of the three joints are respectively recorded as , , and , where , then their cumulative angle changes are:
Therefore, the cumulative angle change of the entire manipulator is:
During the movement, the coordinate value of the end position of the manipulator is recorded as , and the change of the end position is as follows:
That is, is the distance between two points and . is the starting position coordinate. Therefore, the end moving distance during the entire movement is approximately:
5.1. Simulation Experiment of Single Obstacle Path Planning
Let the coordinates of the target point be , and calculate the spherical center coordinates and radius of the obstacle according to the established obstacle model as and , 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.
It can be seen from the Figure 2 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. is only 52 when the process is completed. The cumulative angular change of the entire manipulator is 4.1783 rad, and the cumulative end moving distance 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, , , , 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.
It can be seen from Figure 4 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. is only 59 when the process is completed. The cumulative angular change of the entire manipulator is 4.2158 rad, and the cumulative end moving distance 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.
It can be obtained from Table 5 (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.
Data Availability
The data used to support the findings of this study are included within the article. The source program used to support the findings of this study is included within the supplementary information file.
Conflicts of Interest
The authors declare that they have no conflicts of interest.
Supplementary Materials
The source program used to support the findings of this study is included within the supplementary information file. (Supplemental Materials)