An obstacle avoidance path planning method based on particle swarm and improved dynamic window approach

Abstract

The present invention discloses an obstacle avoidance path planning method based on particle swarm and improved dynamic window method, belonging to the technical field of path planning, which includes the following steps: S1: Use the particle swarm optimization algorithm to perform path planning to obtain the global path; S2: Optimize the path by using the key point selection strategy to obtain the key nodes generated by the global path; S3: Use the key nodes as the local target points of the improved dynamic window method; S4: Set the initial velocity space (v, ω) and heading angle θ0 of the AGV according to environmental requirements; S5: Use the improved dynamic window method to sample the velocity of the AGV to obtain the simulated movement trajectories of each velocity combination; S6: Select the optimal simulated movement trajectory according to the evaluation function, and control the AGV to move towards the target point with this velocity combination; S7: Inherit the information such as the position and running speed of the AGV after the completion of the local path planning of this section, and repeat steps S3-S4 until the AGV reaches the final target point.

Description

Technical Field

[0001] The present invention belongs to the technical field of path planning, and particularly relates to an obstacle avoidance path planning method based on particle swarm and improved dynamic window method. Background Art

[0002] According to the different working environments of AGVs, the path planning problem of AGVs can be divided into two categories: global path planning and local path planning. Global path planning refers to pre-planning under the condition of knowing all external environment information, and the particle swarm optimization algorithm is often used to solve it. However, when the AGV operating environment information is incorrect or the noise is too large, the robustness of the generated path is poor. Local path planning refers to real-time planning under the condition of knowing part of the external environment information, which has high robustness to environmental errors and noise, and the dynamic window method is often used to solve it. However, it selects the local optimal path in the current situation at each iteration, resulting in the final generated path often unable to reach the global optimal goal.

[0003] Therefore, aiming at the defects of a single algorithm, the particle swarm optimization algorithm is fused with the dynamic window algorithm. Through the complementary advantages of the algorithms, both the obstacle avoidance problem during the AGV driving process is considered, and the completeness of path planning is ensured, and a path that simultaneously meets the motion constraints of the AGV and the actual application requirements is planned. There are also problems such as path redundancy and ineffective obstacle avoidance in the path generated by the dynamic window algorithm, which cannot guarantee the safe operation of the AGV in the intelligent warehousing environment and improve its efficiency of handling packages. Summary of the Invention

[0004] In view of this, the purpose of the present invention is to provide an obstacle avoidance path planning algorithm, which uses the particle swarm optimization algorithm to generate a global path, and proposes a key node selection strategy to delete redundant nodes and retain key nodes for the global path; then improves the evaluation function on the basis of the dynamic window method to improve the problems of path redundancy or unreachable target in the path generated by the dynamic window method; finally, uses the key nodes as sub-goal nodes of the improved dynamic window method algorithm to guide the AGV to perform real-time path planning along the direction of the global path, and finally realizes that the AGV can generate a safe optimal or sub-optimal path in a complex intelligent warehousing environment with both static obstacles and dynamic obstacles.

[0005] To achieve the above object, the present invention provides the following technical solutions:

[0006] An obstacle avoidance path planning method based on particle swarm and improved dynamic window method, comprising the following steps:

[0007] S1: Use the particle swarm optimization algorithm to perform path planning to obtain a global path;

[0008] S2: Optimize the path using the key point selection strategy to obtain the key nodes generated by the global path;

[0009] S3: Take the key nodes as the local target points for improving the dynamic window method;

[0010] S4: Set the initial velocity space (v, ω) and the heading angle θ0 of the AGV according to environmental requirements;

[0011] S5: Use the improved dynamic window method to sample the velocities of the AGV and obtain the simulated movement trajectories of each velocity combination;

[0012] S6: Select the optimal simulated movement trajectory according to the evaluation function and control the AGV to move towards the target point with this velocity combination;

[0013] S7: Inherit the information such as the position and running speed of the AGV after the completion of the local path planning for this section, and repeat steps S3 - S4 until the AGV reaches the final target point.

[0014] Furthermore, in step S1, design the fitness function for the particle swarm optimization algorithm as shown in Equation (5):

[0015]

[0016] In Equation (1), f L is used to calculate the path length between the starting point and the target point of the AGV. (x i , y i ) and (x i+1 , y i+1 ) are the position coordinates of the path points P i and P i+1 respectively;

[0017]

[0018]

[0019]

[0020] In Equations (2) - (4), (Ox j, Oy j) is the coordinate of the center of the j - th obstacle, v ij is the penalty term between the i - th node in the path and the j - th obstacle, and f V is used to calculate the risk level of the generated path;

[0021]

[0022] In Equation (5), (x0, y0) and (x n , y n ) are the coordinates of the starting point P0 and the target point P n respectively.

[0023] Further, in step S2, a key node selection strategy is used to retain key nodes, specifically including:

[0024] Starting from the second node P1 in the global path Sol generated by the particle swarm optimization algorithm, if the distance between the current node and the previous node is less than a certain threshold, it is considered that the distance between the above two nodes is too close, the current node is a redundant point, and this node is deleted and the path is updated; traverse in turn to the penultimate node P n-1 , delete all redundant points therein to obtain a new path {P0, P1,..., P m}, m ≤ n.

[0025] Further, the improved dynamic window method has the following evaluation function:

[0026]

[0027] In formula (6), heading(v, ω) is the direction angle evaluation sub-function, which evaluates the consistency between the AGV and the target direction; dist(v, ω) is the distance evaluation sub-function, which evaluates the distance between the AGV and the obstacle; velocity(v, ω) is the speed evaluation sub-function, which evaluates the motion performance of the AGV; α, β, and γ are the weight factors of the evaluation sub-functions; σ is the normalization factor; obs min is the average value of the minimum distance between the end of the predicted trajectory and the obstacle when the AGV is at the current position, and its value range is obs min ∈(0, 2r2); distance is the distance from the starting point to the target point; r2 is the radius after the static obstacle is inflated; η is a coefficient.

[0028] Further, in the improved dynamic window method, the reference position for calculating the value of the heading(v, ω) evaluation function is the end of the simulated trajectory, and the position where the AGV travels forward for 1 s from the current position is used as the reference position for calculating the value of the heading(v, ω) sub-evaluation function.

[0029] The beneficial effects of the present invention are as follows: The particle swarm optimization algorithm is used as the global path planning algorithm to generate the optimal path in the static environment, and the key node selection strategy is used to delete redundant nodes and retain key nodes; the evaluation function is improved on the basis of the dynamic window method to improve the problems of path redundancy and target unreachability generated by using the dynamic window method; finally, the key nodes are used as the sub-target nodes of the improved dynamic window method to guide the global path direction of the AGV for real-time path planning, so as to generate the optimal or sub-optimal path while realizing smooth obstacle avoidance.

[0030] Other advantages, objectives, and features of the present invention will be described in the subsequent specification, and to some extent, will be obvious to those skilled in the art or can be taught from the practice of the present invention. The objectives and other advantages of the present invention can be achieved and obtained through the following specification. Brief Description of the Drawings

[0031] To make the objectives, technical solutions, and beneficial effects of the present invention clearer, the present invention provides the following drawings for illustration:

[0032] Figure 1 Schematic diagram of the angle after the AGV travels forward for different times;

[0033] Figure 2 Flowchart of the obstacle avoidance path planning method based on particle swarm and improved dynamic window method;

[0034] Figure 3 Schematic diagram of the AGV operating environment;

[0035] Figure 4 In (a) is the global path map, and in (b) is the key path node map;

[0036] Figure 5 Schematic diagram of the generated path. Detailed Embodiment

[0037] The particle swarm optimization algorithm based on global path planning generates a global path represented by the node set Sol = {P0, P1,..., P n}, and uses the key node selection strategy to retain key nodes.

[0038] The fitness function for the particle swarm optimization algorithm is designed as shown in Equation (5).

[0039]

[0040] In Equation (1), f L is used to calculate the path length between the starting point and the target point of the AGV. (x i , y i ) and (x i+1 , y i+1 ) are the position coordinates of the path points P i and P i+1 respectively.

[0041]

[0042]

[0043]

[0044] In formulas (2)-(4), (Oxj, Oyj) are the coordinates of the center of the j-th obstacle, and v ij is the penalty term between the i-th node and the j-th obstacle in the path, and f V is used to calculate the risk degree of the generated path.

[0045]

[0046] In formula (5), (x0, y0) and (x n , y n ) are the coordinates of the starting point P0 and the target point P n respectively.

[0047] Use the key node selection strategy to retain key nodes. Specifically, starting from the second node P1 in Sol, if the distance between the current node and the previous node is less than a certain threshold, it is considered that the distance between the above two nodes is too close, and the current node is a redundant point, then delete this node and update the path; traverse in turn to the second-to-last node P n-1 , delete all redundant points among them, and obtain a new path {P0, P1,..., P m}, where m ≤ n.

[0048] Based on the improved dynamic window method of local path planning, two improvements are made to the dynamic window method.

[0049] (1) Improve the weight of the evaluation sub-function of the dynamic window method, and propose a new evaluation function formula as shown in formula (6).

[0050]

[0051] In formula (6), heading(v, ω) is the direction angle evaluation sub-function, which evaluates the consistency between the AGV and the target direction; dist(v, ω) is the distance evaluation sub-function, which evaluates the distance between the AGV and the obstacle; velocity(v, ω) is the speed evaluation sub-function, which evaluates the motion performance of the AGV; α, β, and γ are the weight factors of the evaluation sub-functions; σ is the normalization factor; obs min is the average value of the minimum distance between the end of the predicted trajectory and the obstacle when the AGV is at the current position, and its value range is obs min ∈(0, 2r2); distance is the distance from the starting point to the target point; r2 is the radius after the static obstacle is inflated; η is a coefficient.

[0052] (2) In the dynamic window method, the reference position for calculating the sub-evaluation function value of heading(v, ω) is the end of the simulated trajectory. Usually, the position where the AGV continues to travel for 3 s from the current position is selected. However, in fact, the speed of the AGV will change after 0.1 s. That is, only a small section in front of the simulated trajectory is the position where the AGV will actually travel, as Figure 1 where θ1 is the angle between the current traveling direction of the AGV and the target direction, and θ2 and θ3 are the angles between the traveling direction of the predicted AGV after traveling forward for 1 s and 3 s and the target direction, respectively. Obviously, there is a large gap between the predicted position of the AGV after traveling forward for 3 s and the direction of the AGV's current position relative to the target point, while the gap of the predicted position after 1 s relative to the target point is smaller.

[0053] To avoid the reference position deviating too much from the original position and losing its reference value, the positions of the AGV after traveling forward for different times are selected as the reference positions for calculating the sub-evaluation function value of heading(v, ω). A large number of obstacle avoidance path planning simulation experiments are carried out, and it is found that when the position of the AGV after traveling forward for 1 s is used as the reference position for calculating the sub-evaluation function value of heading(v, ω), both the algorithm running time and the generated path length are better.

[0054] Therefore, the reference position for calculating the direction angle evaluation sub-function value is changed from the commonly used position of the AGV after traveling forward for 3 s to the position of the AGV after traveling forward for 1 s.

[0055] The specific algorithm flow of the obstacle avoidance path planning algorithm based on the particle swarm optimization algorithm and the improved dynamic window method is as Figure 2 shown. The method of the present invention will be explained below in conjunction with specific embodiments: The operating environment of the AGV in the intelligent warehouse is established as a 10 m × 10 m square site. 12 shelves and 1 packing table are set as static obstacles in the environment, which are represented by large circles, and 3 AGVs performing other tasks are set as dynamic obstacles in the environment, which are represented by small circles. Among them, 4 shelves are combined shelves composed of 2 shelves, and the size of the packing table is the same as that of 8 combined shelves, and they are respectively subjected to dilation processing. In this embodiment, r2 is The radius of the AGV is 0.15 m, as Figure 3 shown.

[0056] (1) Use the particle swarm optimization algorithm to perform path planning to obtain the global path. Set the parameters of the particle swarm optimization algorithm ω min = 0.4, ω max = 0.9, c1 = c2 = 1.5, the size of the particle population is 50, the maximum number of iterations is 100, the starting point position of the AGV is (1, 1), and the target point position is (9, 9). The global path is obtained by iterative calculation as Figure 4 (a) shown.

[0057] (2) Optimize the path using the key point selection strategy, that is, starting from the second initial node P1 in the global path, if the distance between the current node and the previous node is less than 2m, it is considered that the distance between the above two nodes is too close, the current node is a redundant point, delete this node, and update the path; traverse in turn to the second-to-last initial node P n-1 , delete all redundant points among them, and obtain the key path nodes {P0, P1,..., P6} as Figure 4 (b);

[0058] (3) Set the initial speed of the AGV, where the linear speed is 0m / s, the initial angular speed is 0rad / s, the maximum linear speed is 1m / s, the maximum angular speed is 20 / 180*pi rad / s, and the maximum linear acceleration is 0.2m / s 2 , the maximum angular acceleration is 50 / 180*pi rad / s 2 , the heading angle θ0 is where P i x and are the abscissa and ordinate of P i respectively, and P i y and are the abscissa and ordinate of P i-1 respectively.

[0059] (4) Use the key nodes selected from the global path as the local target points of the improved dynamic window method, sample the speed of the AGV, and obtain the simulated movement trajectories of each speed combination. The specific steps are as follows:

[0060] Taking Figure 4 section {P0, P1} in (b) as an example, the coordinates of point P0 are (1, 1), the coordinates of point P1 are (2.439, 2.446), the heading angle θ0 is 0.785rad, within the speed range of the AGV, the linear speed and the angular speed are sampled at intervals of 0.01m / s and respectively.

[0061] (5) Set α = 0.1, β = 0.3, γ = 0.2, η = 60, calculate the scores of each pair of speed combinations according to the evaluation function shown in Equation (6), select the trajectory corresponding to the speed combination with the largest evaluation function value as the optimal simulated movement trajectory, and control the AGV to move towards the target point with this speed combination. The specific steps are as follows:

[0062] Taking Figure 4Taking the segment {P0, P1} in (b) as an example, after the first speed sampling, the speed combination (0, -0.087) is obtained. The corresponding value of the heading(v, ω) sub-evaluation function is the angle difference between the orientation when reaching the end of the simulated trajectory and the target point, which is 174.348. The value of the dist(v, ω) sub-evaluation function is the distance between the AGV and the nearest obstacle on the current simulated trajectory, which is 0.639 (if there is no obstacle on this trajectory, it is set to ). The value of the velocity(v, ω) sub-evaluation function is its current linear velocity value, which is 0. After obtaining the values of each sub-evaluation function for other speed combinations in the same way, to prevent the influence of different dimensions on the results, regularization processing is performed on each value. Using the processed data and the evaluation function shown in Equation (6), the scores of each speed combination are calculated, and the maximum value is selected. The AGV is controlled to move towards the target point with this speed combination. In this embodiment, it is 0.066, and the corresponding speed combination is (0.02, 0).

[0063] (6) Inherit the position, running speed and other information of the AGV after the completion of the local path planning of this segment, and repeat steps (3)-(4) until the AGV reaches the final target point, as Figure 5 shown.

[0064] Finally, it should be noted that the above preferred embodiments are only used to illustrate the technical solutions of the present invention and are not intended to limit. Although the present invention has been described in detail through the above preferred embodiments, those skilled in the art should understand that various changes can be made in form and details without departing from the scope defined by the claims of the present invention.

Claims

1. A collision avoidance path planning method based on particle swarm and improved dynamic window method, characterized in that: It includes the following steps: S1: Use the particle swarm optimization algorithm to perform path planning to obtain the global path; S2: Optimize the path using the key point selection strategy to obtain the key nodes generated by the global path; S3: Take the key nodes as the local target points of the improved dynamic window method; S4: Set the initial velocity space (v, ω) and heading angle θ0 of the AGV according to environmental requirements; S5: Use the improved dynamic window method to sample the velocity of the AGV to obtain the simulated movement trajectories of each velocity combination; the improved dynamic window method has the following evaluation function: In formula (6), heading(v, ω) is the direction angle evaluation sub-function, which evaluates the consistency between the AGV and the target direction; dist(v, ω) is the distance evaluation sub-function, which evaluates the distance between the AGV and the obstacle; velocity(v, ω) is the speed evaluation sub-function, which evaluates the motion performance of the AGV; α, β, and γ are the weight factors of the evaluation sub-functions; σ is the normalization factor; obs min is the average value of the minimum distance between the end of the predicted trajectory and the obstacle when the AGV is at the current position, and its value range is obs min ∈(0, 2r2); distance is the distance from the starting point to the target point; r2 is the radius after the static obstacle is inflated; η is the coefficient; S6: Select the optimal simulated movement trajectory according to the evaluation function, and control the AGV to move towards the target point with this velocity combination; S7: Inherit the position and running speed information of the AGV after the completion of the local path planning of this section, and repeat steps S3 - S4 until the AGV reaches the final target point.

2. The obstacle avoidance path planning method based on particle swarm and improved dynamic window method according to claim 1, wherein: In step S1, a fitness function for the particle swarm optimization algorithm is designed as shown in Equation (5): In formula (1), f L is used to calculate the path length between the starting point and the target point of the AGV. (x i , y i ) and (x i+1 , y i+1 ) are the position coordinates of path points P i and P i+1 respectively; In formulas (2)-(4), are the coordinates of the center of the j-th obstacle, and v ij is the penalty term between the i-th node and the j-th obstacle in the path, and f V is used to calculate the risk level of the generated path; In formula (5), (x0, y0) and (x n , y n ) are the coordinates of the starting point P0 and the target point P n , respectively.

3. The obstacle avoidance path planning method based on particle swarm and improved dynamic window approach according to claim 1, wherein: In step S2, the key point selection strategy is used to retain the key nodes, specifically including: Starting from the second node P1 in the global path Sol generated by the particle swarm optimization algorithm, if the distance between the current node and the previous node is less than a certain threshold, it is considered that the distance between the above two nodes is too close, the current node is a redundant point, delete this node, and update the path; traverse to the penultimate node P in turn n-1 , delete all redundant points among them, and obtain a new path {P0, P1, …, P m}}, m ≤ n.

4. The obstacle avoidance path planning method based on particle swarm and improved dynamic window method according to claim 1, characterized in that: In the improved dynamic window method, the reference position for calculating the evaluation function value of heading(v, ω) is the end of the simulated trajectory, and the position of the AGV 1 s after moving forward from the current position is used as the reference position for calculating the sub-evaluation function value of heading(v, ω).

Images