A dynamic environment path planning method and system based on improved A-Star algorithm and DWA algorithm

Abstract

The application provides a dynamic environment path planning method and system based on improved A-Star algorithm and DWA algorithm, and belongs to the field of dynamic path planning. In order to solve the problems that A-Star algorithm cannot avoid dynamic obstacles, DWA algorithm is easy to fall into local minimum value, cannot find a path, and lacks effective fusion of the two, the application optimizes the robot driving path length and speed, introduces the minimum distance between the path point and the path planned by the improved A-Star algorithm into the evaluation function, and selects the coefficient of the speed in the evaluation function by the minimum distance between the end of the predicted trajectory and the obstacle, so as to improve the driving speed of the robot far from the obstacle; the robot runs at a faster speed far from the obstacle, and runs at a slower speed near the obstacle to ensure driving safety, so that the overall driving speed is improved, and the walking safety of the robot is ensured.

Description

Technical Field

[0001] This invention relates to the field of dynamic path planning technology, and more specifically, to a dynamic environment path planning method and system based on an improved A-Star algorithm and a DWA algorithm. Background Technology

[0002] Dynamic path planning refers to the real-time adjustment and optimization of paths during operation to cope with changes in the environment and task requirements. In industries such as warehousing and logistics, dynamic path planning is crucial because it helps robots intelligently adapt to rapidly changing work environments and tasks, improving their efficiency and flexibility. In practice, the environment in warehouses or production lines frequently changes, such as the placement of goods or road congestion, requiring adjustments and optimizations to paths based on real-time conditions to avoid obstacles and shorten routes as much as possible.

[0003] When dynamic obstacles exist on the map, the obstacle information is not fully known. The A-Star algorithm, being a global path planning algorithm, relies on known map information during path planning, thus it cannot avoid dynamic obstacles. The DWA algorithm, being a local path planning algorithm, continuously updates the map model during path planning to avoid collisions with dynamic obstacles. However, the DWA algorithm is prone to getting trapped in local minima, which can lead to situations where a path cannot be found. Summary of the Invention

[0004] The technical problem to be solved by this invention is:

[0005] To address the issues of the A-Star algorithm's inability to avoid dynamic obstacles, the DWA algorithm's tendency to get stuck in local minima and encounter situations where it cannot find a path, and the lack of an effective fusion of the two algorithms.

[0006] The technical solution adopted by the present invention to solve the above-mentioned technical problems is as follows:

[0007] This invention provides a dynamic environment path planning method based on an improved A-Star algorithm and a DWA algorithm, comprising the following steps:

[0008] Step 1: Use the improved A-Star algorithm to plan the complete path considering only static obstacles;

[0009] Step 2: Determine the robot's initial state C, including horizontal axis coordinates, vertical axis coordinates, heading angle, velocity, and angular velocity;

[0010] Step 3: Determine if the end of the current trajectory is the path endpoint. If the end of the current trajectory has not reached the path endpoint, repeat the following steps until the end of the current trajectory reaches the endpoint and then proceed to Step 4.

[0011] ① The speed V that the robot itself can achieve s The speed V that can be reached within a certain time with the current speed and maximum acceleration. d Determine the window for velocity sampling;

[0012] ② Determine the achievable velocity by using the maximum velocity, minimum velocity, maximum angular velocity, minimum angular velocity, velocity resolution, and angular velocity resolution within the window;

[0013] ③ Generate multiple predicted trajectories based on the sampled speeds, and calculate the heading angle deviation, minimum distance to obstacles, braking distance, speed magnitude, and shortest distance to the path planned by the improved A-Star algorithm based on the state at the end of the trajectory;

[0014] ④ Determine the speed coefficient used for evaluation based on the minimum distance between the end of the trajectory and the obstacle;

[0015] ⑤ Select the optimal trajectory. Substitute the heading angle deviation, minimum distance to the obstacle, braking distance, speed, and shortest distance to the path planned by the improved A-Star algorithm obtained in ③, and the coefficient used for speed evaluation in ④ into the evaluation function. Calculate the path with the largest evaluation function value in the evaluation function as the optimal trajectory, record its corresponding speed and angular velocity, and update the robot state.

[0016] Step 4: Combine the segments of the trajectory into a complete path.

[0017] Furthermore, step one specifically includes the following steps:

[0018] ① Obtain and input the starting point, target node, and map information, and add the starting point to the open list;

[0019] ② Obtain the path, including:

[0020] Starting from the initial point, i.e., the first current node, determine whether the current node's neighboring nodes are in the open list: if they are, record the node with the smallest g value among the neighboring nodes, and make the current node the parent node of the neighboring node with the smallest g value; if they are not, add the neighboring node to the open list and determine whether the neighboring node is the target node. If it is the target node, end the current loop; if it is not the target node, make the current node the parent node of the target node.

[0021] (2) Move the current node obtained in (1) into the closed list;

[0022] (3) The f-values ​​of adjacent nodes are calculated using an improved evaluation function;

[0023] (4) Select the node with the smallest f value in the open list as the next current node;

[0024] Repeat the above steps if the open list is not empty; otherwise, skip to step ③.

[0025] ③ If the target node is in the closed list, then start from the target node and backtrack along the parent node of each node to the starting node, and save the path; if the target node is not in the closed list, the path cannot be found.

[0026] ④ Analyze the path points. If the minimum distance between a path point and an obstacle is less than the minimum safe distance, the path is invalid. Otherwise, save the path and use the saved path as the path obtained in the initial planning. The minimum safe distance is defined as the distance from the robot's center to its farthest edge.

[0027] ⑤: Reprocess the path obtained from the initial planning. While ensuring that the distance between the path and obstacles is greater than the minimum safe distance, remove the first and second type nodes in the path to obtain the final path.

[0028] Furthermore, in step ②(3), the improved evaluation function is shown in equation (1).

[0029] f(n)=g(n)+h(n)+5.1 / o(n) (1)

[0030] The formulas for calculating o(n) are shown in equations (2) and (3):

[0031] o(n) = min(dist) (2)

[0032]

[0033] Where f(n) represents the evaluation function; g(n) is the observed evaluation value from the starting point to the current node n; h(n) is the estimated evaluation value from the current node n to the end point; o(n) is the minimum distance between the current node n and the obstacle; and dist is the distance between the current node and the obstacle. n ,y n (x) represents the coordinates of the current node n; o (i),y o (i) represents the coordinates of the i-th obstacle; i represents the number of obstacles.

[0034] Furthermore, in step three④, the expression for the improved evaluation function is shown in equation (9):

[0035]

[0036] Where α, β, and γ are weighting coefficients; σ is a normalization parameter; heading(v,ω) is the angle between the current travel direction and the target direction; dist(v,ω) is the minimum distance between the robot trajectory and the obstacle; velocity(v,ω) is the magnitude of the current velocity; λ is a parameter; A is a constant representing the farthest distance from the path point to the path planned by the improved A-Star algorithm; distAstar(v,ω) is the minimum value between the current node and the sampling point at a specified velocity and angular velocity; γ_fast and γ_slow are both velocity coefficients in the evaluation function.

[0037] Furthermore, the expression for distAstar(v,ω) is given in equations (10) and (11).

[0038]

[0039] distAstar(v,ω)=min(dis) (11)

[0040] Wherein, the coordinates of the current path point N are (N x N y The coordinates of the sampling point (Sample) are (Samp... ix Samp iy ).

[0041] Furthermore, the expression for dist(v,ω) is shown in equation (12).

[0042]

[0043] The coordinates of the i-th obstacle are (ob) ix ,ob iy ).

[0044] Furthermore, in step one ④, the minimum safe distance is as shown in equation (4).

[0045] o(p) ≥ Distance from robot center to farthest edge (4)

[0046] Where p is a path point.

[0047] Further, in step one (⑤), the method for determining whether a path point is a first-type node is to calculate whether the slopes of the preceding and following paths of that node are the same. Let the coordinates of the path point C to be determined be (x... C ,y C ), the coordinates of its previous path point P1 are (x p1 ,y p1 The coordinates of the next path point P2 are (x... p2 ,y p2If the slopes K1 and K2 of these two paths are as shown in equations (5) and (6), respectively:

[0048]

[0049]

[0050] If K1 and K2 are the same, the path point is determined to be a first-class node and is removed. If K1 and K2 are different, the node is retained.

[0051] Furthermore, in step one (⑤), the method for determining whether the path points retained after removing the first type of path points are second type nodes is as follows: first, connect the starting point and the target node to obtain a straight line l. 1n If the line does not pass through any obstacle and the line l 1n If the distance between the path and the obstacle is greater than or equal to the minimum safe distance, then the path points between the starting point and the target node are classified as second-type nodes, and the straight line l is retained. 1n As the final path; if the straight line passes through an obstacle, then connect the starting point with the second-to-last path point to obtain the straight line l. 1n-1 If the straight line l 1n-1 If a path point does not pass through an obstacle and its distance from an obstacle is greater than or equal to the minimum safe distance, then the path point between the starting point and the second-to-last path point is classified as a second-type node, removed, and then connected to the target point to determine the final path; if the straight line l 1n-1 If an obstacle is encountered, connect the starting point to the third-to-last path point, and so on, until the starting point and the target node are connected without obstacles. The path obtained after removing the second type of node is the final path.

[0052] A dynamic environment path planning system based on the improved A-Star algorithm and DWA algorithm is provided. The system has program modules corresponding to the above steps and executes the steps in the dynamic environment path planning method based on the improved A-Star algorithm and DWA algorithm when running.

[0053] Compared with the prior art, the beneficial effects of the present invention are:

[0054] This invention discloses a dynamic environment path planning method and system based on an improved A-Star algorithm and a DWA algorithm. Aiming to improve robot efficiency, it proposes optimizations to the robot's path length and speed. Two improvements are made to the DWA algorithm: First, the minimum distance between the path point and the path planned by the improved A-Star algorithm is incorporated into the evaluation function, making the DWA-planned path closer to a straight line and shortening the path length. Second, the speed coefficient in the evaluation function is selected based on the minimum distance from the predicted trajectory end to the obstacle, increasing the robot's speed when far from the obstacle. This allows the robot to operate at a faster speed when far from the obstacle and at a slower speed when approaching the obstacle to ensure safety. This approach improves both overall speed and robot safety.

[0055] The performance of the improved DWA algorithm proposed in this invention was compared with that of other existing algorithms under single-task and multi-task simulations. The simulation results show that its performance is improved in terms of path length, running time and travel speed, making it more suitable for high-speed intelligent warehousing. Attached Figure Description

[0056] Figure 1 This is a framework diagram of a dynamic environment path planning method based on an improved A-Star algorithm and a DWA algorithm in an embodiment of the present invention.

[0057] Figure 2 This is a flowchart of a dynamic environment path planning method based on an improved A-Star algorithm and a DWA algorithm in an embodiment of the present invention;

[0058] Figure 3 This is a schematic diagram of the path reprocessing method in an embodiment of the present invention;

[0059] Figure 4 This is a comparison diagram of the improved A-Star algorithm under different environments in an embodiment of the present invention;

[0060] Figure 5 This is a comparison diagram of the paths planned by the improved A-Star algorithm, algorithm 3, and the improved A-Star algorithm and DWA algorithm under a single task model in the embodiments of the present invention;

[0061] Figure 6 This is a comparison chart of the path lengths planned by Algorithm 3, the improved A-Star algorithm, and the DWA algorithm in 50 repeated experiments in this embodiment of the invention;

[0062] Figure 7 This is a comparison chart of robot travel speeds under Algorithm 3, the improved A-Star algorithm, and the DWA algorithm in 50 repeated experiments in this embodiment of the invention;

[0063] Figure 8 This is a comparison chart of robot running times under Algorithm 3, the improved A-Star algorithm, and the DWA algorithm in 50 repeated experiments in this embodiment of the invention;

[0064] Figure 9 This is a comparison diagram of the improved A-Star algorithm, algorithm 3, and the improved A-Star algorithm with the DWA algorithm under the multi-task model in the embodiments of the present invention;

[0065] Figure 10 This is a performance comparison chart of different algorithms, including Algorithm 3, the improved A-Star algorithm, and the DWA algorithm, in 50 repeated experiments under the multi-task model in this embodiment of the invention. Detailed Implementation

[0066] In the description of this invention, it should be noted that the terms "first," "second," and "third" mentioned in the embodiments of this invention are for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the number of indicated technical features. Therefore, a feature defined with "first," "second," and "third" may explicitly or implicitly include one or more of that feature.

[0067] To make the above-mentioned objects, features and advantages of the present invention more apparent and understandable, specific embodiments of the present invention will be described in detail below with reference to the accompanying drawings.

[0068] Specific Implementation Plan 1: Combining Figures 1 to 4 As shown, this invention provides a dynamic environment path planning method based on an improved A-Star algorithm and a DWA algorithm, comprising the following steps:

[0069] Step 1: Use the improved A-Star algorithm to plan the complete path considering only static obstacles. Specifically,

[0070] ① Obtain and input the starting point, target node, and map information, and add the starting point to the open list;

[0071] ② Obtain the path, including:

[0072] (1) Starting from the starting point, i.e. the first current node, judge the adjacent nodes of the current node. If the adjacent node has the following three conditions: it is occupied by an obstacle, it is in the closed list, or it is inconsistent with the direction of the target node, then ignore the node.

[0073] If none of the above three conditions exist, then determine whether the adjacent node is in the open list: if it is, record the node with the smallest g value among the adjacent nodes, and make the current node the parent node of the adjacent node with the smallest g value; if it is not, add the adjacent node to the open list and determine whether the adjacent node is the target node. If it is the target node, end the current loop; if it is not the target node, make the current node the parent node of the node.

[0074] (2) Move the current node obtained in (1) into the closed list;

[0075] (3) The f-values ​​of adjacent nodes are calculated using an improved evaluation function;

[0076] (4) Select the node with the smallest f value in the open list as the next current node;

[0077] Repeat the above steps if the open list is not empty; otherwise, skip to step ③.

[0078] The improved evaluation function is shown in equation (1).

[0079] f(n)=g(n)+h(n)+5.1 / o(n) (1)

[0080] The formulas for calculating o(n) are shown in equations (2) and (3):

[0081] o(n) = min(dist) (2)

[0082]

[0083] Where f(n) represents the evaluation function; g(n) is the observed evaluation value from the starting point to the current node n; h(n) is the estimated evaluation value from the current node n to the end point; o(n) is the minimum distance between the current node n and the obstacle; and dist is the distance between the current node and the obstacle. n ,y n (x) represents the coordinates of the current node n; o (i),y o (i) represents the coordinates of the i-th obstacle; i represents the number of obstacles.

[0084] ③ If the target node is in the closed list, then start from the target node and backtrack along the parent node of each node to the starting node, and save the path; if the target node is not in the closed list, the path cannot be found.

[0085] ④. The path points are judged. If the minimum distance between a path point and an obstacle is less than the minimum safe distance, the path is invalid; otherwise, the path is saved and used as the initial planning path. The minimum safe distance is defined as the distance from the robot's center to its farthest edge. After planning the complete path, the path is judged according to the constraint of the minimum safe distance. If there are path points that do not satisfy condition (4), the path is invalid. Condition (4) is:

[0086] o(p) ≥ Distance from robot center to farthest edge (4)

[0087] Where p is a path point;

[0088] ⑤: Reprocess the path obtained from the initial planning. While ensuring that the distance between the path and obstacles is greater than the minimum safe distance, remove the first and second type nodes in the path to obtain the final path.

[0089] The path planned by the A-Star algorithm contains two types of unnecessary nodes. The first type of node has no impact on the path whether it is removed or retained. The second type of node, after being removed, will prevent the robot from colliding with obstacles, and the path length will be shortened and the number of turns will be reduced.

[0090] This invention proposes a method for determining whether a path point is a first-type node: calculating whether the slopes of the preceding and following paths of the node are the same. Let the coordinates of the path point C to be determined be (x...). C ,y C ), the coordinates of its previous path point P1 are (x p1 ,y p1 The coordinates of the next path point P2 are (x... p2 ,y p2 If the slopes K1 and K2 of these two paths are as shown in equations (5) and (6), respectively:

[0091]

[0092]

[0093] If K1 and K2 are the same, the path point is determined to be a first-class node and is removed. If K1 and K2 are different, the node is retained.

[0094] After removing the first type of path points, this invention determines whether the remaining path points are second type nodes by first connecting the starting point and the target node to obtain a straight line l. 1n If the line does not pass through any obstacle and the line l 1n If the distance between the path and the obstacle is greater than or equal to the minimum safe distance, then the path points between the starting point and the target node are classified as second-type nodes, and the straight line l is retained.1n As the final path; if the straight line passes through an obstacle, then connect the starting point with the second-to-last path point to obtain the straight line l. 1n-1 If the straight line l 1n-1 If a path point does not pass through an obstacle and its distance from an obstacle is greater than or equal to the minimum safe distance, then the path point between the starting point and the second-to-last path point is classified as a second-type node, removed, and then connected to the target point to determine the final path; if the straight line l 1n-1 If an obstacle is encountered, connect the starting point to the third-to-last path point, and so on, until the starting point and the target node are connected without any obstacles.

[0095] After removing the second type of nodes, the reprocessing of the path in this paper is completed, which effectively reduces the number of turns in the path, thereby shortening the path length and increasing the safety of the robot path.

[0096] Examples of path reprocessing methods are as follows: Figure 3 As shown in the diagram, point A is the starting point, point B is the target node, and the path [ACFDEB] is the path obtained from the initial planning. Point C's preceding path [AC] and following path [CF] have different slopes, so path point C is retained. Similarly, path points D and E are retained. Point F's preceding path [CF] and following path [FD] have the same slope, therefore path point F is a first-class node and is removed, leaving the path [ACDEB].

[0097] Based on this path, we continue to determine the second type of node for the path points. First, we connect path point A to point B. The line passes through an obstacle. Then, we connect point A to point E. The line still passes through an obstacle. Next, we connect point A to point D. The line does not pass through an obstacle, and it can be seen that the straight line [AD] is safer than the path [ACF]. It can be determined that the minimum distance between the straight line and the obstacle is greater than the minimum safe distance. Therefore, point C is determined to be a second type of node and is removed. Similarly, we connect point D to the target point B. It does not pass through an obstacle, and the minimum distance between the straight line and the obstacle is greater than the minimum safe distance. Therefore, point E is determined to be a second type of node and is removed. The final path is [ADB].

[0098] Step 2: Determine the robot's initial state C, including horizontal axis coordinates, vertical axis coordinates, heading angle, velocity, and angular velocity;

[0099] Step 3: Determine if the end of the current trajectory is the path endpoint, i.e., the target node. If the end of the current trajectory has not reached the path endpoint, repeat the following steps until the end of the current trajectory reaches the endpoint and then proceed to Step 4.

[0100] ① The speed V that the robot itself can achieve sThe speed V that can be reached within a certain time with the current speed and maximum acceleration. d Determine the window for velocity sampling;

[0101] Among them, V s The expression is shown in equation (7):

[0102] V s ={v∈[v min ,v max ],ω∈[ω min ,ω max ]} (7)

[0103] Where v min and v max These are the minimum speed and the maximum speed, respectively; ω min and ω max These are the minimum angular velocity and the maximum angular velocity, respectively.

[0104] V d Its expression is shown in equation (8):

[0105]

[0106] Where v n and ω n These are the current velocity and angular velocity, respectively; vt max and ωt max For maximum acceleration and maximum angular acceleration; vt min and ωt min These are the minimum acceleration and the minimum angular acceleration, respectively.

[0107] ② Determine the achievable velocity by using the maximum velocity, minimum velocity, maximum angular velocity, minimum angular velocity, velocity resolution, and angular velocity resolution within the window;

[0108] ③ Generate multiple predicted trajectories based on the sampled velocities, and calculate the heading angle deviation (v, ω), minimum distance to the obstacle (dist), braking distance (Stopdist), velocity magnitude (velocity), and shortest distance to the path planned by the improved A-Star algorithm (distAstar) based on the state at the end of the trajectory.

[0109] The expression for heading(v,ω) is shown in equation (15):

[0110]

[0111] ④ Determine the speed coefficient used for evaluation based on the minimum distance between the end of the trajectory and the obstacle; the minimum distance is the minimum safe distance, which is also the distance from the center of the robot to the outermost edge;

[0112] In intelligent warehousing, robots typically operate at high speeds, but when close to obstacles, their speed needs to be appropriately reduced to ensure safe operation. This invention proposes to determine the speed coefficient when evaluating a trajectory using an evaluation function based on the minimum distance between the predicted trajectory's end and the obstacle. When the minimum distance between the predicted trajectory's end and the obstacle is greater than the minimum safe distance, the speed coefficient in the evaluation function is increased, meaning that a faster trajectory is preferred while ensuring safety. Conversely, the speed coefficient in the evaluation function is decreased, favoring a safer driving trajectory. The improved evaluation function is expressed as equation (9).

[0113]

[0114] Where α, β, and γ are weighting coefficients; σ is a normalization parameter; heading(v,ω) is the angle between the current travel direction and the target direction; dist(v,ω) is the minimum distance between the robot trajectory and the obstacle; velocity(v,ω) is the magnitude of the current velocity; λ is a parameter; A is a constant representing the farthest distance from the path point to the path planned by the improved A-Star algorithm; distAstar(v,ω) is the minimum value between the current node and the sampling point at a specified velocity and angular velocity, and its expression is given in equations (10) and (11).

[0115]

[0116] distAstar(v,ω)=min(dis) (11)

[0117] Wherein, the coordinates of the current path point N are (N x N y The coordinates of the sampling point (Sample) are (Samp... ix Samp iy );

[0118] The expression for dist(v,ω) is shown in equation (12): γ_fast and γ_slow are both coefficients of velocity in the evaluation function;

[0119]

[0120] The coordinates of the i-th obstacle are (ob) ix ,ob iy );

[0121] ⑤ Select the optimal trajectory by substituting the heading angle deviation, minimum distance to the obstacle, braking distance, speed, and shortest distance to the path planned by the improved A-Star algorithm obtained in ③, and the coefficient used for speed evaluation in ④ into the formula, calculate the path with the largest evaluation value in the evaluation function as the optimal trajectory, record its corresponding speed and angular velocity, and update the robot state.

[0122] Step 4: Combine the segments of the trajectory into a complete path.

[0123] Specific Implementation Scheme Two: A dynamic environment path planning system based on the improved A-Star algorithm and DWA algorithm. This system has program modules corresponding to the steps described above, and executes the steps in the dynamic environment path planning method based on the improved A-Star algorithm and DWA algorithm during runtime. Other combinations and connections in this implementation scheme are the same as in Specific Implementation Scheme One.

[0124] Example

[0125] Experimental environment: CPU: 12th Gen Intel(R) Core(TM) i5-1240P 1.70GHz, 16G RAM, Windows 11.

[0126] Experimental Comparison Algorithm: The present invention is compared with the proposed DWA improvement method, which is as follows: The distance between the end position of the robot trajectory and the target point is introduced into the evaluation function, and the improved evaluation function is shown in formula (13):

[0127]

[0128] The definition of goal_dist is shown in equation (14):

[0129]

[0130] As can be seen from equation (14), the smaller the distance between the end of the trajectory and the target point, the larger the value of goal_dist.

[0131] To compare the algorithm proposed in this invention with the algorithms mentioned above, this invention reproduces the algorithm. For ease of description, the improved DWA algorithm proposed above is named Algorithm 3. In this comparative experiment, let k be the number of loops required to form a complete path from the starting point to the ending point in the local path, and n be the number of acceleration and velocity sampling values. Then, the number of trajectories obtained by combining the velocity sampling values ​​with the robot motion model is n. 2 The final algorithm has a time complexity of O(kn). 2 ).

[0132] Experimental Scenario: In this experiment, the A-Star fusion algorithm based on the improved evaluation function and DWA, along with Algorithm 3, were compared. A map of 30m × 30m was used. Considering practical application scenarios, the performance of the algorithms was evaluated under two robot task models. In the first task model, the robot starts from the picking station (3, 1) and stops after reaching the task point, whose coordinates are (17, 22). In the second task model, the robot starts from the picking station (3, 1), passes through multiple task points, and then returns to the picking station (12, 1). The task points are randomly determined on the map; in this experiment, the selected task points were (7, 15), (17, 22), (7, 8), and (27, 15).

[0133] Algorithm performance comparison and evaluation in real-world environments:

[0134] (1) Comparison and evaluation of algorithm performance under single-task model

[0135] In a model where a robot needs to complete a single task, a path is planned using three algorithms based on the result of a selected experiment. The path is as follows: Figure 5 As shown, the three algorithms include the improved A-Star algorithm, the improved A-Star algorithm of this invention, the DWA algorithm, and Algorithm 3.

[0136] The present invention records the statistical results of dynamic path length, robot speed, and algorithm running time in repeated experiments in Tables 1 and 2, and their changing trends are recorded in Tables 1 and 2. Figures 6 to 8 middle.

[0137] 1) Static path length

[0138] This invention utilizes a path planned by an improved A-Star algorithm to guide dynamic paths. The A-Star algorithm is a global path planning algorithm that only considers static obstacles during path planning, ignoring dynamic obstacles; therefore, the length of the static path remains unchanged in repeated experiments. In the experiment, the path length planned by the improved A-Star algorithm and the DWA algorithm was 25.0669m.

[0139] 2) Dynamic path length

[0140] As shown in Table 1 and Figure 6 As shown, the minimum and average path lengths planned by Algorithm 3 are both longer than those planned by the improved A-Star algorithm and the DWA algorithm proposed in this invention. This is because the static path length planned by the improved A-Star algorithm of this invention is shorter, and the improved DWA algorithm further shortens the path length by approaching a straight path. The maximum value is greatly affected by dynamic obstacles, and in this experiment, it is slightly longer than that of Algorithm 3, but the increase in length is within 0.1m, which has a small impact.

[0141] Table 1 Comparison of dynamic path lengths planned by different algorithms

[0142]

[0143] 3) Robot travel speed

[0144] As shown in Table 2 and Figure 7 As shown, the improved A-Star algorithm and DWA algorithm proposed in this invention achieve the fastest travel speed, with an average speed of 0.7653 m / s in 50 repeated experiments. This is because this invention divides the speed coefficient in the evaluation function according to the state at the end of the predicted trajectory, i.e., whether it is greater than the minimum safe distance. If it is greater than the minimum safe distance, the robot can move forward at a faster speed when there are no obstacles nearby. Compared with Algorithm 3, which evaluates the predicted trajectory with a fixed coefficient, the overall speed of the robot is improved.

[0145] Table 2 Comparison of robot speeds under different algorithms

[0146]

[0147] 4) Algorithm running time

[0148] As shown in Table 3 and Figure 8 As shown, since the improved A-Star algorithm and DWA algorithm proposed in this invention have achieved better performance in terms of path length and robot speed, the running time of this invention is also greatly reduced compared with the other two algorithms. The average time is 51.1364s in 50 repeated experiments, while the time of Algorithm 3 is 84.7084s.

[0149] In summary, this method can significantly improve work efficiency under a single-task model.

[0150] Table 3 Comparison of running times for different algorithms

[0151]

[0152] (2) Algorithm performance comparison and evaluation under multi-task model

[0153] In a model where a robot needs to complete multiple tasks, a path is planned using two algorithms based on the result of a single experiment. The path is as follows: Figure 9 As shown.

[0154] This application records the statistical results of dynamic path length, robot speed, and algorithm running time obtained from repeated experiments in Tables 4 and 5, and their changing trends are recorded in Tables 4 and 5. Figure 10 middle.

[0155] Table 4 Comparison of dynamic path lengths planned by different algorithms

[0156]

[0157] Table 5 Comparison of robot speeds under different algorithms

[0158]

[0159] Table 6 Comparison of running times for different algorithms

[0160]

[0161] The results show that the improved A-Star algorithm and DWA algorithm proposed in this invention have the best performance in terms of dynamic path length, travel speed, and algorithm running time under the multi-task model. Specifically, the average path length is reduced by about 0.4m; the average travel speed is increased by about 0.02m / s; and the average running time is reduced by about 71s, with the most significant reduction in running time. This indicates that the robot using this method achieves the highest work efficiency and is suitable for applications in multi-task scenarios.

[0162] While the present invention has been disclosed above, its scope of protection is not limited thereto. Those skilled in the art can make various changes and modifications without departing from the spirit and scope of the present invention, and all such changes and modifications will fall within the scope of protection of the present invention.

Claims

1. A dynamic environment path planning method based on an improved A-Star algorithm and a DWA algorithm, characterized in that, Includes the following steps: Step 1: Use the improved A-Star algorithm to plan the complete path considering only static obstacles; Specifically, including, ① Obtain and input the starting point, target node, and map information, and add the starting point to the open list; ② Obtain the path, including: (1) Starting from the starting point, i.e. the first current node, determine whether the adjacent nodes of the current node are in the open list: if they are, record the node with the smallest g value among the adjacent nodes, and make the current node the parent node of the adjacent node with the smallest g value; if they are not, add the adjacent node to the open list and determine whether the adjacent node is the target node. If it is the target node, end the current loop; if it is not the target node, make the current node the parent node of the node. (2) Move the current node obtained in (1) into the closed list; (3) The f-values ​​of adjacent nodes are calculated using the improved evaluation function; The improved evaluation function is shown in equation (1). (1) The calculation formulas are shown in equations (2) and (3): (2) (3) Where f(n) represents the evaluation function; g(n) is the observed evaluation value from the starting point to the current node n; and h(n) is the estimated evaluation value from the current node n to the end point. This represents the minimum distance between the current node n and the obstacle. This represents the distance between the current node and the obstacle. Let n be the coordinates of the current node n; Let be the coordinates of the i-th obstacle; i is the number of obstacles. (4) Select the node with the smallest f value in the open list as the next current node; Repeat the above steps if the open list is not empty; otherwise, skip to step ③. ③ If the target node is in the closed list, then start from the target node and backtrack along the parent node of each node to the starting node, and save the path; if the target node is not in the closed list, the path cannot be found. ④ Analyze the path points. If the minimum distance between a path point and an obstacle is less than the minimum safe distance, the path is invalid. Otherwise, save the path and use the saved path as the path obtained in the initial planning. The minimum safe distance is defined as the distance from the robot's center to its farthest edge. ⑤: Reprocess the path obtained from the initial planning. While ensuring that the distance between the path and obstacles is greater than the minimum safe distance, remove the first and second type nodes in the path to obtain the final path. Step 2: Determine the robot's initial state C, including horizontal axis coordinates, vertical axis coordinates, heading angle, velocity, and angular velocity; Step 3: Determine if the end of the current trajectory is the path endpoint. If the end of the current trajectory has not reached the path endpoint, repeat the following steps until the end of the current trajectory reaches the endpoint and then proceed to Step 4. ①Speed ​​achievable by the robot itself The speed that can be reached within a certain time with the current speed and maximum acceleration. Determine the window for velocity sampling; ② Determine the achievable velocity by using the maximum velocity, minimum velocity, maximum angular velocity, minimum angular velocity, velocity resolution, and angular velocity resolution within the window; ③ Generate multiple predicted trajectories based on the sampled speeds, and calculate the heading angle deviation, minimum distance to obstacles, braking distance, speed magnitude, and shortest distance to the path planned by the improved A-Star algorithm based on the state at the end of the trajectory; ④ Determine the speed coefficient used for evaluation based on the minimum distance between the end of the trajectory and the obstacle; The expression for the improved evaluation function is shown in equation (9): in, , and These are weighting coefficients; These are normalization parameters; The angle between the current driving direction and the target direction; This represents the minimum distance between the robot's trajectory and the obstacle. The magnitude of the current speed; is a parameter; A is a constant representing the farthest distance from the path point to the path already planned by the improved A-Star algorithm; γ_fast and γ_slow are the minimum values ​​between the current node and the sampling point at specified velocities and angular velocities; γ_fast and γ_slow are both coefficients of velocity in the evaluation function. ⑤ Select the optimal trajectory. Substitute the heading angle deviation, minimum distance to the obstacle, braking distance, speed, and shortest distance to the path planned by the improved A-Star algorithm obtained in ③, and the coefficient used for speed evaluation in ④ into the evaluation function. Calculate the path with the largest evaluation function value in the evaluation function as the optimal trajectory, record its corresponding speed and angular velocity, and update the robot state. Step 4: Combine the segments of the trajectory into a complete path.

2. The dynamic environment path planning method based on the improved A-Star algorithm and DWA algorithm according to claim 1, characterized in that: The expressions for are given in equations (10) and (11). (10) (11) Wherein, the coordinates of the current path point N are The coordinates of the sampling point are: .

3. The dynamic environment path planning method based on the improved A-Star algorithm and DWA algorithm according to claim 2, characterized in that: The expression for is shown in equation (12). (12) The coordinates of the i-th obstacle are: .

4. The dynamic environment path planning method based on the improved A-Star algorithm and DWA algorithm according to claim 3, characterized in that: In step 1, ④, the minimum safe distance is as shown in equation (4). (4) Where p is a path point.

5. The dynamic environment path planning method based on the improved A-Star algorithm and DWA algorithm according to claim 4, characterized in that: In step 1.5, the method for determining whether a path point is a first-type node is to calculate whether the slopes of the preceding and following paths of that node are the same. Let the coordinates of the path point C to be determined be... its previous path point The coordinates are The next path point The coordinates are Then the slopes of these two path segments and As shown in equations (5) and (6) respectively: (5) (6) like and If they are the same, the path point is determined to be a first-class node and is removed. and If they are different, then the node is retained.

6. The dynamic environment path planning method based on the improved A-Star algorithm and DWA algorithm according to claim 5, characterized in that: In step 1.5, the method for determining whether the path points retained after removing the first type of path points are second type nodes is as follows: first, connect the starting point and the target node to obtain a straight line. If the straight line does not pass through any obstacle and the straight line If the distance between the path and the obstacle is greater than or equal to the minimum safe distance, then the path points between the starting point and the target node are classified as second-type nodes, and the straight line is retained. As the final path; if the straight line passes through an obstacle, then connect the starting point with the second-to-last path point to obtain the straight line. If the straight line If the path point between the starting point and the second-to-last path point is not obstructed and the distance to the obstruction is greater than or equal to the minimum safe distance, then the path point between the starting point and the second-to-last path point is classified as a second-type node, removed, and then connected to the target point to determine the final path; if it is a straight line... If an obstacle is encountered, connect the starting point to the third-to-last path point, and so on, until the starting point and the target node are connected without obstacles. The path obtained after removing the second type of node is the final path.

Images