AGV path planning method based on artificial potential field method improved by quasi-annealing method

Abstract

The invention discloses an AGV path planning method based on an artificial potential field method improved by a quasi-annealing method, and the method comprises the steps: calculating a gravitational value through employing a gravitational function or an improved gravitational function according to whether the distance between an AGV and a target point exceeds half of the distance between the target point and a starting point; according to the distance between the AGV and the target point, using a traditional repulsive force function or an improved repulsive force function to calculate a repulsive force value; calculating resultant force; if the resultant force is 0, it is indicated that the AGV is in the local optimum dilemma, and if the resultant force is not 0, the calculated resultant force is used to calculate the moving point of the iteration; and judging whether the AGV arrives at a target point, if the AGV does not arrive at the target point, repeating the above contents to obtain a corresponding path point, and if the AGV already arrives at the target point, stopping path planning. The improved gravitational function is combined with the traditional gravitational function, the AGV can stably move towards the target point on the premise that the AGV does not collide with an obstacle, the improved repulsive force function is combined with the traditional repulsive force function, and the situation that the AGV cannot safely reach the designated point is avoided.

Description

Technical Field

[0001] The present invention belongs to path planning, and particularly relates to an AGV path planning method based on an improved artificial potential field method using simulated annealing method. Background Technique

[0002] Path planning refers to the process of finding the optimal or feasible path from a starting point to a target point in a given environment. It has wide applications in fields such as robotics, autonomous driving, logistics, game AI, etc. The core task of path planning is to generate a safe and efficient path considering factors such as obstacles, dynamic environment, motion constraints, etc. Path planning is one of the essential contents in the work of AGV. Common algorithms include A* algorithm, Dijkstra algorithm, RRT (Rapidly-Exploring Random Tree), etc. The working scenario of AGV is generally with multiple moving objects, so local path planning is used as the preferred path planning method for AGV. There are many local path planning methods, and the artificial potential field method is one of them. Inspired by the movement of electrons in an electric field, the site where the AGV is located can be constructed as a virtual electric field. The AGV can be regarded as an electron with a negative charge, and the target point and obstacles can be regarded as electrons with a positive charge and a negative charge respectively. According to physical knowledge, under the action of these two different charged electrons, a gravitational potential field and a repulsive potential field are generated, and the electron moves under the combined potential field, that is, the AGV can perform corresponding position changes. The gravitational potential field can generate a gravitational force to guide the forward direction of the AGV, while the repulsive potential field generates a repulsive force on the AGV, which can prevent the robot from colliding with obstacles. However, at the same time, the generation of the repulsive force requires that the AGV is within its action range, otherwise the AGV's movement is only affected by the gravitational force of the target point. This mechanism can not only effectively control the AGV to avoid obstacles and move towards the target point, but also the planned path is the optimal path when there are no obstacles. The disadvantages of the artificial potential field method are also obvious, such as being easily trapped in local minima, target unreachability, etc. Therefore, it is necessary to improve the traditional artificial potential field method to make it more adaptable to the operation scenario of AGV. Summary of the Invention

[0003] Aiming at the problems pointed out in the background technique, the present invention provides an AGV path planning method based on an improved artificial potential field method using simulated annealing method. The gravitational potential field function in the traditional artificial potential field method is correspondingly improved to ensure that the AGV can move smoothly towards the target point on the premise of not hitting obstacles. The repulsive force function in the traditional artificial potential field method is correspondingly improved, so that the actual AGV can safely reach the specified point, and the generated path is more practical.

[0004] Technical Solution: The present invention discloses an AGV path planning method based on an improved artificial potential field method using simulated annealing method, including the following steps:

[0005] Step 1: Calculate the gravitational force value using the gravitational function. According to whether the distance between the AGV and the target point exceeds half of the distance between the target point and the starting point, if it exceeds, use the gravitational function of the traditional artificial potential field method to calculate the gravitational force value; if it does not exceed, use the improved gravitational function and calculate the corresponding gravitational force value.

[0006] Step 2: If the AGV enters the influence range of the obstacle, use the repulsive force function to calculate the repulsive force value. According to whether the distance between the AGV and the target point exceeds 1m, if it exceeds, use the repulsive force function of the traditional artificial potential field method to calculate the repulsive force value; if it does not exceed, use the improved repulsive force function and calculate the corresponding repulsive force value. If the AGV does not enter the influence range of the obstacle, there is no need to calculate the repulsive force value.

[0007] Step 3: Calculate the resultant force. Calculate the corresponding resultant force according to the gravitational force value and the repulsive force value calculated in Step 1 and Step 2.

[0008] Step 4: Judgment. According to whether the resultant force calculated in Step 3 is 0, if the resultant force is 0, it means that the AGV has fallen into the dilemma of local optimum. Use the simulated annealing method to calculate the point with lower potential energy near this local optimum point and use this point as the point for this iteration of the AGV. If the resultant force is not 0, calculate the moving point for this iteration based on the calculated resultant force.

[0009] Step 5: Judge whether the target point is reached. If the AGV has not reached the target point, return to Step 1 and perform calculations according to Steps 1 - 4 to obtain the corresponding path points. If the AGV has reached the target point, stop the path planning.

[0010] The main functions of the artificial potential field method include the gravitational potential field function and the repulsive potential field function, and their expressions are respectively:

[0011]

[0012] Among them, q represents the coordinate of the current position, q goal represents the coordinate of the target point, q obs represents the coordinate of the obstacle. k att and k rep are respectively the gravitational potential field coefficient and the repulsive potential field coefficient. ρ(q,q goal ) is the Euclidean distance between the current point and the target point, ρ(q,q obs ) is the Euclidean distance between the current point and the obstacle, and ρ0 is the action range of the repulsive potential field.

[0013] The traditional gravitational function is:

[0014]

[0015] The traditional repulsive force function is:

[0016]

[0017] where ||q - q obs || is the derivative of ρ(q, q obs ).

[0018] In the case where the destination is far from the starting point, the attraction force generated by the target point on the AGV is large, while the repulsive force generated by the obstacle on the AGV is small. At this time, this repulsive force cannot have a great impact on the attraction force, resulting in the AGV being unable to avoid colliding with the obstacle, and thus the planned path is an invalid path. Therefore, too strong an attraction force cannot meet the obstacle avoidance function during the path planning of the trolley. For this reason, it is necessary to make corresponding improvements to the gravitational potential field function in the traditional artificial potential field method to ensure that the AGV can move smoothly towards the target point on the premise of not hitting the obstacle. The improved gravitational potential field function is as follows:

[0019]

[0020] The improved gravitational function is as follows:

[0021]

[0022] where ρ(q start , q goal ) represents the distance between the starting point and the destination point, and r iter represents the ratio of the current update times to the total update times. The meaning of this formula is that when the distance between the AGV and the target point has not reached half of the distance between the target point and the starting point, the improved gravitational function is used for calculation. After exceeding this range, the traditional gravitational function is continued to be used to calculate the magnitude of the gravitational value. By improving the gravitational potential field function and the gravitational function, on the one hand, it can be ensured that when the AGV is far from the target point, the generated gravitational force is reduced compared with the traditional artificial potential field method, and within a certain range, its gravitational force change is not large, which can make the planned path an optimal path, and at the same time can also reduce the probability of the AGV colliding with the obstacle in this case. On the other hand, after exceeding a certain distance, retaining the gravitational function in the traditional artificial potential field method does not affect the subsequent path planning effect.

[0023] During the path planning process, we regard the AGV as a particle, generally between the driving wheels or at the center position of the AGV. Therefore, when using the artificial potential field method, some generated points cannot meet the actual movement requirements of the AGV. Because the AGV needs to move by wheels, and there is a certain distance between the wheels and the particle, which leads to the fact that theoretically it can reach this point, but the actual AGV cannot safely reach the specified point. Therefore, it is necessary to make corresponding improvements to the repulsive force function in the traditional artificial potential field method to make the generated path more practical. The improved repulsive potential field function is as follows:

[0024]

[0025] Improved repulsive force function:

[0026]

[0027] where is the derivative of ρ(q, q obs ). d is the distance from the mass point to the wheel on one side of the AGV. This formula increases the repulsive force value by adding a safety distance. Through such improvement, the repulsive force can be increased, greatly reducing the invalid points planned by the traditional artificial potential field method, enabling the AGV to safely bypass obstacles.

[0028] Regarding the problem of the target being unreachable, the main reason is that the magnitude of the gravitational force function decreases as the distance between the AGV and the target point decreases. At this time, if there are obstacles, the repulsive force generated by them is greater than the gravitational force, making the AGV unable to reach the destination. Therefore, to solve this problem, we further improve equations (7) and (8), and their expressions are respectively:

[0029]

[0030] where and are respectively:

[0031]

[0032] where is the derivative of ρ(q, q goal ). Equation (10) means that when the distance between the AGV and the target point is less than 1, there may be a problem that the repulsive force is too large, resulting in the target point being unreachable. Adding the |(tanh(ρ(q, q goal )) - ρ(q, q goal ))| function to the repulsive force function can gradually reduce the repulsive force value as the AGV gets closer to the target point. Even if the AGV is within the influence range of the obstacle, through such further improvement of the repulsive force function, when approaching the target point and there are obstacles nearby, the repulsive force generated by the obstacles can be reduced, and as it gets closer to the target point, the repulsive force becomes smaller until the AGV reaches the target point, and the repulsive force is 0 at this time. With the magnitude of the gravitational force function remaining unchanged, the reduction of the repulsive force helps to improve the situation where the AGV target is unreachable.

[0033] When trapped in a local optimum, the AGV will stop moving and fall into oscillation. In this paper, the simulated annealing method is introduced to effectively help the AGV get out of this predicament. The core of the simulated annealing method is the Metropolis criterion, which is used to decide whether to accept a worse solution. By introducing a probabilistic acceptance mechanism, the algorithm can jump out of the local optimum and find the global optimum. Its mathematical expression is as follows:

[0034]

[0035] Where ΔE = E(x′) - E(x), E(x′) represents the objective function value of the new solution x′, and E(x) represents the objective function value of the current solution x. T represents the current temperature, and P(ΔE) represents the probability of accepting the new solution x'. The significance of this criterion is that at the high-temperature stage, the algorithm has a high probability of accepting a worse solution, thus jumping out of the local optimum. However, as the temperature drops, the probability of accepting a worse solution becomes smaller and smaller, which means that the algorithm accepts a better solution and finally converges to the vicinity of the global optimum. The algorithm flow of the simulated annealing method is as follows:

[0036] Step 1: Initialize the temperature T0, the local optimum point X, and the annealing rate α.

[0037] Step 2: Generate a random point x′ in the vicinity.

[0038] Step 3: Calculate the potential field at the point x′ and subtract the potential field value of the point X to get ΔU.

[0039] Step 4: If ΔU < 0, it means that the lowest potential energy point has been found, and the local optimum can be escaped. The AGV can move towards the point x′ and exit the simulated annealing method. If ΔU > 0, accept the solution with the probability in Equation (10), otherwise, return to Step 2 after cooling and continue to find a new random point. If T k When no new solution is found after reaching T f it means that the AGV has not jumped out of the local optimum, and it is necessary to return to Step 1 and start over.

[0040] Beneficial effects:

[0041] By making corresponding improvements to the gravitational potential field function in the traditional artificial potential field method, it can ensure that the AGV moves smoothly towards the target point without hitting obstacles.

[0042] When the distance between the AGV and the target point is less than half the distance between the target point and the starting point, the improved gravity function is used for calculation. When it exceeds this range, the traditional gravity function continues to be used to calculate the gravity value. By improving the gravitational potential field function and the gravity function, on the one hand, it can ensure that when the AGV is farther away from the target point, the gravity generated is reduced compared with the traditional artificial potential field method, and within a certain range, the gravity does not change much, so that the planned path can be the optimal path, and at the same time, it can also reduce the probability of the AGV colliding with obstacles in this case. On the other hand, after exceeding a certain distance, retaining the gravity function in the traditional artificial potential field method does not affect the subsequent path planning effect.

[0043] The repulsion function in the traditional artificial potential field method is improved accordingly, so that there is no actual situation where the AGV cannot safely reach the designated point, making the generated path more practical.

[0044] In the repulsion function, a safe distance is added to increase the repulsion value. This improvement can increase the repulsion, greatly reduce the invalid points planned by the traditional artificial potential field method, and enable the AGV to safely bypass obstacles.

[0045] When the AGV falls into the local optimum, it will stop moving and fall into oscillation. The introduction of quasi-annealing method can effectively help the AGV escape from this dilemma. BRIEF DESCRIPTION OF THE DRAWINGS

[0046] Figure 1 Comparison diagram of artificial potential field method before and after improving gravity function;

[0047] Figure 2 For points without actual meaning;

[0048] Figure 3 It is the idea of ​​quasi-annealing method;

[0049] Figure 4 This is the algorithm flow chart of the quasi-annealing method;

[0050] Figure 5 It is an AGV path planning method based on the improved artificial potential field method using the quasi-annealing method.

[0051] Figure 6 A map containing static obstacles;

[0052] Figure 7 A path planning diagram containing static obstacles;

[0053] Figure 8 Path planning with dynamic obstacles Figure 1 ;

[0054] Figure 9 Path planning with dynamic obstacles Figure 2 ;

[0055] Figure 10 For path planning with dynamic obstacles Figure 3 。 Detailed implementation manners

[0056] The present invention will be further illustrated below in conjunction with the accompanying drawings and specific embodiments.

[0057] In the case where the destination is far from the starting point, the attraction generated by the target point on the AGV is large, while the repulsive force generated by the obstacle on the AGV is small. At this time, this repulsive force cannot have a great impact on the attraction, resulting in the AGV being unable to avoid colliding with the obstacle, and thus the planned path is an invalid path. Therefore, too strong an attraction cannot meet the obstacle avoidance function during the path planning of the vehicle. For this reason, it is necessary to make corresponding improvements to the gravitational potential field function in the traditional artificial potential field method to ensure that the AGV can move smoothly towards the target point on the premise of not hitting the obstacle. The improved gravitational potential field function is as follows:

[0058]

[0059] The improved gravitational function is as follows:

[0060]

[0061] Where k att represents the gravitational factor coefficient, and ρ(q, q goal ) represents the distance between the AGV and the target point. ρ(q start , q goal ) represents the distance between the starting point and the destination point, and r iter represents the ratio of the current update count to the total update count. By improving the gravitational potential field function and the gravitational function, on the one hand, it can be ensured that when the AGV is far from the target point, the generated attraction is reduced compared with the traditional artificial potential field method, and within a certain range, its attraction does not change much, which can make the planned path an optimal path, and at the same time can also reduce the probability of the AGV colliding with the obstacle in this case. On the other hand, after exceeding a certain distance, retaining the gravitational function in the traditional artificial potential field method does not affect the subsequent path planning effect.

[0062] Figure 1 It is a comparison diagram before and after improving the gravitational function. As shown in the figure, a) is the force diagram of the traditional artificial potential field method. When the AGV enters the obstacle range, that is, within the repulsive force range, under the action of the repulsive force and the gravitational force of the target point, it moves along the direction of the resultant force. At this time, the AGV will inevitably collide with the obstacle. In b), after improving the gravitational function, the magnitude of the gravitational function is reduced, so that the magnitude and direction of the resultant force are changed, thereby realizing that the planned path can avoid the obstacle and enabling the AGV to work smoothly and safely.

[0063] During the path planning process, we regard the AGV as a particle, generally at the position between the driving wheels or at the center of the AGV. Therefore, when using the artificial potential field method, some generated points cannot meet the actual movement requirements of the AGV. As Figure 2 shown. In the figure, although the resultant force is directed outside the obstacle, in fact, when the AGV moves to the point indicated by the resultant force, it is impossible to avoid colliding with the obstacle. Because the AGV needs to move by wheels, and there is a certain distance between the wheels and the particle, which leads to the situation that theoretically it can reach this point, but in fact the AGV cannot safely reach the specified point. Therefore, it is necessary to make corresponding improvements to the repulsive force function in the traditional artificial potential field method to make the generated path more practical. The improved repulsive potential field function is as follows:

[0064]

[0065] Improved repulsive force function:

[0066]

[0067] Among them, k rep represents the repulsive force factor coefficient, and ρ(q, q obs ) represents the distance between the AGV and the obstacle. is the derivative of ρ(q, q obs ). Among them, d is the distance from the particle to one side of the AGV's wheel. Through such improvement, the repulsive force can be increased, greatly reducing the invalid points planned by using the traditional artificial potential field method, enabling the AGV to safely bypass the obstacle.

[0068] Regarding the problem of the target being unreachable, the main reason is that the magnitude of the gravitational force function decreases as the distance between the AGV and the target point decreases. And at this time, if there is an obstacle, the repulsive force generated by it is greater than the gravitational force, making the AGV unable to reach the destination. Therefore, to solve this problem, we further improve equations (3) and (4), and their expressions are respectively:

[0069]

[0070] Among them and are respectively:

[0071]

[0072]

[0073] By further improving such a repulsive force function, when approaching the target point and there are obstacles nearby, the repulsive force generated by the obstacles can be reduced, and the closer to the target point, the smaller the repulsive force until the AGV reaches the target point, at which time the repulsive force is also 0. With the gravitational force function remaining unchanged, the reduction of the repulsive force helps to improve the situation where the AGV cannot reach the target.

[0074] When trapped in a local optimum, the AGV will stop moving and fall into oscillation. In this paper, the simulated annealing method is introduced to effectively help the AGV get out of this predicament. The specific operation is as Figure 3 shown. When the AGV is trapped in the local optimum point 1, according to the rules of the artificial potential field method, the AGV has reached the target point, but the actual target point is point 3. At this time, the simulated annealing method will randomly generate a new point at point 1. If the potential energy of the generated new point is smaller than that at point 1, the new point will directly replace the previously trapped optimum point. If the potential energy of the new point is greater than that at point 1, then according to the Metropolis criterion, the point will be accepted with a certain probability, and the search will continue with this point until the optimal solution is found. The flow chart of the simulated annealing method is as Figure 4 shown. The algorithm flow of improving the artificial potential field method based on the simulated annealing method is as Figure 5 shown.

[0075] Now place the AGV in the Figure 6 map shown. Use the improved artificial potential field method based on the simulated annealing method proposed by the invention for path planning, so that it can move from the starting point to the target point and avoid all static obstacles on the way. The parameters required are shown in the following table. The planned path is as Figure 7 shown.

[0076]

[0077] It can be seen from Figure 7 that the AGV can safely reach the destination. During this process, the AGV does not collide with obstacles because the gravitational force is too large and the repulsive force is too small when the previous AGV is too far from the target point. At the same time, there is no problem of the target being unreachable due to the presence of obstacles near the target point, and there is no situation of being trapped in a local optimum, indicating that the simulated annealing method has enabled it to get out of the local optimum predicament. Therefore, for a chemical plant with static obstacles, the improved artificial potential field method based on the simulated annealing method proposed by the invention can well complete the path planning work.

[0078] Continue to use the AGV in the Figure 6 map, add dynamic obstacles to the map, and continue to use the improved artificial potential field method based on the simulated annealing method for path planning. The starting coordinates of the dynamic obstacle are (7, 5), and it moves to the left at a speed of 1 m / s. Figures 8 - 10 describes its planning process. In Figure 8 , the dynamic obstacle appears on the map and is captured by the AGV.Figure 9 Among them, the AGV successfully avoided dynamic obstacles through the path algorithm proposed in this paper, Figure 10 and finally showed that the AGV successfully reached the target point like the path of static obstacles. This dynamic obstacle simulation once again proves that the improved artificial potential field method based on the simulated annealing method has good real-time obstacle avoidance function, and the running safety of the AGV is guaranteed.

[0079] As mentioned above, it is not a restriction on the present invention in any form. According to the technical essence of the present invention, any simple modification, equivalent replacement and improvement made to the above embodiments within the spirit and principle of the present invention still belong to the protection scope of the technical solution of the present invention. Specific examples are used in this article to elaborate on the principle and implementation manner of the present invention. The description of the above embodiments is only used to help understand the method and its core idea of the present invention; at the same time, for those of ordinary skill in the art, there will be changes in the specific implementation manner and application scope according to the idea of the present invention. To sum up, the content of this specification should not be construed as a limitation to the present invention.

Claims

1. An AGV path planning method based on the quasi-annealing method to improve the artificial potential field method, characterized in that: The steps include: Step 1: Use the gravity function to calculate the gravity value, depending on whether the distance between the AGV and the target point exceeds half of the distance between the target point and the starting point. If it exceeds, use the gravity function of the traditional artificial potential field method to calculate the gravity value; if it does not exceed, use the improved gravity function and calculate the corresponding gravity value; Step 2: If the AGV enters the influence range of the obstacle, the repulsion value is calculated using the repulsion function, depending on whether the distance between the AGV and the target point exceeds 1m. If so, the repulsion value is calculated using the repulsion function of the traditional artificial potential field method; if not, the improved repulsion function is used and the corresponding repulsion value is calculated. If the AGV does not enter the influence range of the obstacle, there is no need to calculate the repulsion value. Step 3, calculating the resultant force, and calculating the corresponding resultant force according to the attraction value and repulsion value calculated in step 1 and step 2; Step 4: Determine whether the resultant force calculated in step 3 is 0. If the resultant force is 0, it means that the AGV is trapped in the local optimal dilemma. Use the quasi-annealing method to calculate the point with lower potential energy near the local optimal point, and use this point as the point of this iteration of the AGV. If the resultant force is not 0, the calculated resultant force is used to calculate the moving point of this iteration; Step 5, determine whether the target point has been reached. If the AGV has not reached the target point, return to step 1 and calculate according to steps 1-4 to obtain the corresponding path points. If the AGV has reached the target point, stop path planning.

2. The AGV path planning method based on the quasi-annealing method to improve the artificial potential field method according to claim 1 is characterized in that: In step 1, when the destination is far from the starting point, the target point has a greater attraction to the AGV, while the obstacle has a smaller repulsive force on the AGV. At this time, this repulsive force cannot have a greater impact on the attraction, resulting in the AGV being unable to avoid colliding with the obstacle, and the planned path is an invalid path. Therefore, the overly strong attraction cannot meet the obstacle avoidance function during the path planning of the vehicle. The gravitational potential field function in the traditional artificial potential field method is improved accordingly to ensure that the AGV can move smoothly to the target point without colliding with obstacles. The improved gravitational potential field function is: The improved gravity function is: where k att represents the gravitational factor coefficient, ρ(q,q goal ) is the distance between AGV and the target point. start ,q goal ) represents the distance between the starting point and the destination point, r iter It represents the ratio of the current update count to the total update count.

3. The AGV path planning method based on the quasi-annealing method to improve the artificial potential field method according to claim 2 is characterized in that: In the path planning process, we regard the AGV as a mass point. Since the AGV needs to move by wheels, and there is a certain distance between the wheels and the mass point, it can reach the point in theory, but in reality the AGV cannot reach the specified point safely. The repulsive force function in the traditional artificial potential field method is improved accordingly. The improved repulsive force potential field function is as follows: Improved repulsion function: where k rep represents the repulsion factor coefficient, ρ(q,q obs ) indicates the distance between AGV and the obstacle, is ρ(q,q obs ), d is the distance from the particle to the wheel on one side of the AGV.

4. The AGV path planning method based on the quasi-annealing method to improve the artificial potential field method according to claim 3 is characterized in that: The magnitude of the gravitational function decreases as the distance between the AGV and the target point decreases. If there is an obstacle at this time, the repulsive force generated by it is greater than the gravitational force, making it impossible for the AGV to reach the destination. Further improvements to equations (3) and (4) are as follows: in and They are: in is ρ(q,q goal ) is the derivative of .

5. The AGV path planning method based on the quasi-annealing method to improve the artificial potential field method according to claim 1 is characterized in that: Pseudo-annealing method The expression is as follows: Where ΔE=E(x′)-E(x), E(x′) represents the objective function value of the new solution x′, E(x) represents the objective function value of the current solution x, T represents the current temperature, and P(ΔE) represents the probability of accepting the new solution x′.

6. The AGV path planning method based on the quasi-annealing method to improve the artificial potential field method according to claim 5 is characterized in that: The algorithm flow of the quasi-annealing method is: Step 1: Initialize temperature T0, local optimal point X, and annealing rate α; Step 2: Generate a random point x′ nearby; Step 3: Calculate the potential field at point x′ and subtract it from the potential field value at point X to get ΔU; Step 4: If ΔU < 0, it means that the lowest potential energy point has been found and the local optimal solution can be escaped. The AGV can drive to point x′ and exit the quasi-annealing method. If ΔU > 0, the solution is accepted with the probability in formula (10). Otherwise, after cooling down, return to step 2 and continue to look for new random points. If T k Arriving at T f If no new solution is found, it means that the AGV has not jumped out of the local optimum, and it is necessary to return to step 1 and start again.

Images