Dynamic path planning method based on JPS-DWA fusion

Abstract

The invention belongs to the technical field of mobile robot path planning, discloses a dynamic path planning method based on JPS-DWA fusion, and solves the problem that the dynamic path planning is easy to fail in complex scenes such as a U-shaped obstacle and the like when the JPS and the DWA are combined to carry out dynamic path planning in the prior art. Comprising the following steps: (1) constructing a grid map; (2) performing global path planning based on a JPS algorithm: designing a comprehensive cost function, dynamically adjusting weights of a heuristic item and a path cost item according to a distance between a current node and a target node, and guiding a path to be away from an obstacle dense area; detecting whether obstacles pass through the path nodes and deleting redundant nodes; and (3) fusing a DWA algorithm, taking the global path planned in the step S2 as a basis, taking path nodes as local target points in sequence, and carrying out local path planning. According to the invention, path planning in a complex environment can be efficiently and accurately realized, and quick response and decision can be made for real-time obstacles.

Description

Technical Field

[0001] This invention belongs to the field of mobile robot path planning technology, and particularly relates to a dynamic path planning method based on JPS-DWA fusion. Background Technology

[0002] Path planning is a core problem in autonomous navigation of mobile robots, and can generally be divided into two categories: global path planning and local path planning. Global path planning is based on a known environment map and searches for the optimal or near-optimal path from the starting point to the destination for the robot; local path planning, on the other hand, relies on real-time sensor data to dynamically perceive the surrounding environment during movement, autonomously avoid obstacles, and generate a continuous, smooth, and safe motion trajectory.

[0003] A typical global path planning algorithm is the Jump Point Search (JPS) algorithm, which is an improvement on the Astar algorithm. Although the JPS algorithm reduces node expansion through the jump point mechanism, it still suffers from problems such as high search blindness, a large number of expanded nodes, large memory consumption, high path redundancy, and low decision-making efficiency in complex environments. In addition, the JPS algorithm is based on discrete grid maps, making it difficult to adapt to the real-time obstacle avoidance requirements in dynamic environments.

[0004] A typical local path planning method is the Dynamic Window Approach (DWA). Although the DWA algorithm can dynamically generate motion trajectories based on real-time sensor data, its comprehensive evaluation function is prone to causing the robot to get stuck in local minima in complex obstacle environments, resulting in stagnation or detours.

[0005] Although there have been attempts to combine JPS with DWA in the existing technology, the following problems still exist: (1) The JPS algorithm does not fully consider the impact of obstacle density on path safety; (2) The DWA algorithm lacks global path guidance and is prone to failure in complex scenarios such as U-shaped obstacles; (3) The path smoothness is insufficient, with redundant nodes and jagged trajectories; (4) The overall system's response speed and robustness in dynamic environments need to be improved. Summary of the Invention

[0006] The purpose of this invention is to provide a dynamic path planning method based on JPS-DWA fusion, which effectively solves the problem that existing methods of combining JPS and DWA for dynamic path planning are prone to failure in complex scenarios such as U-shaped obstacles.

[0007] To solve the above technical problems, the technical solution adopted by this invention is: a dynamic path planning method based on JPS-DWA fusion, including the following steps: S1, constructing a grid map and discretizing the environment into passable areas and obstacle areas; S2, performing global path planning based on the JPS algorithm; S21, designing a comprehensive cost function, dynamically adjusting the weights of the heuristic term and the path cost term according to the distance between the current node and the target node, while guiding the path away from dense obstacle areas; S22, detecting whether the path nodes cross obstacles to ensure that the path does not cross obstacles and deleting redundant nodes; S3, fusing the DWA algorithm, using the global path planned in step S2 as the basis, and sequentially using the path nodes as local target points for local path planning; S31, constructing a dynamic window, considering the robot's kinematic constraints and safety braking constraints; S32, discretizing the linear velocity and angular velocity to generate multiple sets of predicted trajectories; S33, evaluating the predicted trajectories generated in step S32 using a comprehensive evaluation function, and selecting the trajectory with the highest comprehensive evaluation score.

[0008] Comprehensive evaluation function : ; in, Represents the objective evaluation function. This represents the obstacle distance evaluation function. This represents the velocity tendency evaluation function. This represents the robot's heading angle evaluation function. This represents the function for restoring a stagnant state. , , , and These represent the weight parameters of the target evaluation function, obstacle distance evaluation function, velocity tendency evaluation function, robot heading angle evaluation function, and stationary state recovery function, respectively.

[0009] Furthermore, in step S21, the comprehensive cost function... : ; ; ; ; in, This represents the distance ratio between the current node and the target node. This represents the straight-line distance between the current node and the target node. The straight-line distance between the starting point and the target node. It is a vector consisting of the starting point and the target node. The vector formed by the starting point and the current node cross product, Representing vectors with vector The angle between them; Indicates obstacle density. and These represent the number of obstacles and the total number of squares within the rectangular area formed between the current node and the target node, respectively. Indicates the distance from the starting point to the current node. The actual cost; Indicates the current node Heuristic cost estimation to the target node.

[0010] Furthermore, in step S22, the optimized DDA algorithm is used to detect whether obstacles are crossed between path nodes. The optimized DDA algorithm includes the following steps: S221, assuming the detection path is line segment AB, calculate the difference between line segment AB in the X-axis direction of the grid. The difference in the Y-axis direction of the grid ; ; ; in, It is the x-axis coordinate of the midpoint B of line segment AB. It is the X-axis coordinate of the midpoint A of line segment AB. It is the Y-axis coordinate of the midpoint B of line segment AB. It is the Y-axis coordinate of the midpoint A of line segment AB.

[0011] S222. Determine the step size and number of steps.

[0012] ; in, Indicates the step size.

[0013] S223. Calculate the increment in the X-axis direction of the grid at each step. and the increment in the Y-axis direction of the grid .

[0014] ; ; S224, From the starting point Initially, each move yields new coordinates. The new coordinates are rounded down to obtain the coordinates. The coordinates obtained by rounding down Add to the detection queue.

[0015] when and At that time, for each point After processing the coordinates and adding them to the detection queue, then make a judgment. Is it in the detection queue? If If it is not in the detection queue, then Add to the detection queue; when and At that time, for each point After processing the coordinates and adding them to the detection queue, then make a judgment. Is it in the detection queue? If If it is not in the detection queue, then Add to the detection queue; when and At that time, for each point After processing the coordinates and adding them to the detection queue, then make a judgment. Is it in the detection queue? If If it is not in the detection queue, then Add to the detection queue; when and At that time, for each point After processing the coordinates and adding them to the detection queue, then make a judgment. Is it in the detection queue? If If it is not in the detection queue, then Add to the detection queue.

[0016] Furthermore, in step S31, the dynamic window is: ; in, ; ; In the formula, and These refer to the linear velocity and angular velocity that the robot needs to achieve in the next step, as obtained from the final dynamic window. and These refer to the minimum and maximum linear velocities that the robot can achieve, respectively. and These refer to the robot's minimum and maximum linear velocities, respectively. This refers to the robot's maximum linear acceleration. and These refer to the minimum and maximum angular velocities that the robot can achieve, respectively. and These refer to the robot's minimum and maximum angular velocities, respectively. This refers to the robot's maximum angular acceleration. and They refer to robots in linear velocity and angular velocity at time t. Refers to the robot's safe speed. This refers to the robot's minimum safe linear velocity. This refers to the robot's maximum safe linear velocity. To predict the minimum distance between the trajectory point and the obstacle, This is the robot's maximum deceleration. Refers to the time step.

[0017] Furthermore, in step S33, the robot heading angle evaluation function... : ; ; Stasis recovery function : ; in, For the weights of the heading angle evaluation function, This is the angle difference between the robot trajectory direction and the direction from the robot trajectory to the target node.

[0018] Furthermore, in step S33, the objective evaluation function... : ; in, The weights for the target evaluation values, The coordinates of the target node. These are the coordinates of the robot's current state.

[0019] Obstacle distance evaluation function : ; in, The distance between the predicted trajectory point and the nearest obstacle in the robot's current state. To set a safe distance for the robot, The weight of the obstacle distance assessment value.

[0020] Speed ​​tendency evaluation function : ; in, As the weight for the speed evaluation value, This represents the linear velocity of the robot in its current state.

[0021] Furthermore, in step S21, the obstacle-dense area refers to the rectangular area formed by the current node coordinates and the target node coordinates, where the ratio of the number of obstacles in the rectangular area to the total number of squares in the rectangular area exceeds 20%.

[0022] Compared with the prior art, the beneficial technical effects of the present invention are: (1) significantly improved search efficiency: by designing a comprehensive cost function and an adaptive weight mechanism, the comprehensive cost function integrates distance ratio, deviation, obstacle density and vector angle to achieve adaptive adjustment of the search direction. The improved JPS algorithm of the present invention reduces the number of extended nodes by about 56% and the planning time by about 60%.

[0023] (2) Path quality optimization: By adopting the optimized DDA traversal detection algorithm, redundant nodes are deleted to improve the naturalness and safety of the path; after pruning redundant nodes, the number of path nodes is reduced by up to 74%, the path length is shortened, and the smoothness is improved.

[0024] (3) Strong dynamic obstacle avoidance capability: By improving the comprehensive evaluation function of the DWA algorithm, introducing local target guidance terms and stagnation recovery mechanism, the robot's target tendency and motion continuity are improved, enabling the robot to effectively avoid getting trapped in local minima and still plan the path smoothly in complex scenarios such as U-shaped obstacles.

[0025] (4) Fast system response speed: By integrating the improved JPS algorithm with the improved DWA algorithm, the efficient and seamless connection between the global path of JPS and the local planning of DWA is achieved, which enhances the overall performance of the system in dynamic environments and shortens the overall planning time by about 14%.

[0026] (5) Strong applicability: This invention is applicable to maps of different sizes and obstacle densities, and has good generalization ability and robustness. Attached Figure Description

[0027] Figure 1 This is a schematic diagram of a two-dimensional grid map in Example 1.

[0028] Figure 2 This is a schematic diagram of path planning using the traditional Astar algorithm in an environment with a map size of 20m×20m and an obstacle density of 25%.

[0029] Figure 3 This is a schematic diagram of path planning using the traditional JPS algorithm in an environment with a map size of 20m×20m and an obstacle density of 25%.

[0030] Figure 4 This is a schematic diagram of path planning using the JPS algorithm for optimizing the comprehensive cost function in Example 1, in an environment with a map size of 20m×20m and an obstacle density of 25%.

[0031] Figure 5 This is a schematic diagram of the redundant node pruning strategy in Example 1.

[0032] Figure 6 This is a schematic diagram of the path crossing detection using the DDA algorithm in Example 1.

[0033] Figure 7 This is a schematic diagram of the path planning of the traditional DWA algorithm under a U-shaped obstacle. The green dot is the starting point, the blue dot is the robot's current position, the purple dot is the target point, and the blue line is the robot's trajectory.

[0034] Figure 8 This is a schematic diagram of the path planning of the JPS-DWA algorithm under a U-shaped obstacle in Example 1. In the diagram, the green dot is the starting point, the blue dot is the current position of the robot, the purple dot is the target point, the blue line is the robot's trajectory, and the red line is the global path planned by the JPS-DWA algorithm.

[0035] Figure 9 This is a schematic diagram of the path planning of the traditional DWA algorithm under L-shaped obstacles. The green dot is the starting point, the blue dot is the robot's current position, the purple dot is the target point, and the blue line is the robot's trajectory.

[0036] Figure 10 This is a schematic diagram of the path planning of the JPS-DWA algorithm under an L-shaped obstacle in Example 1. In the diagram, the green dot is the starting point, the blue dot is the robot's current position, the red dot is the target point, the blue line is the robot's trajectory, and the red line is the global path planned by the JPS-DWA algorithm.

[0037] Figure 11 This is a diagram of the robot's overall motion trajectory in an environment with a map size of 50m×50m and an obstacle density of 25% using the traditional DWA algorithm. The green dot represents the starting point, the red dot represents the ending point, the blue dot represents the target point that the robot finally reaches, the solid blue line represents the robot's trajectory, and the dashed red line represents the global path planned by the JPS-DWA algorithm in Example 1.

[0038] Figure 12 This is a graph showing the changes in the robot's speed and angular velocity using the traditional DWA algorithm in an environment with a map size of 50m×50m and an obstacle density of 25%. The blue line represents the change in the robot's speed (m / s) during movement, and the green line represents the change in the robot's angular velocity (rad / s) during movement.

[0039] Figure 13This is a diagram showing the overall motion trajectory of the robot using the JPS-DWA algorithm in Example 1, in an environment with a map size of 50m×50m and an obstacle density of 25%. The green dot represents the starting point, the red dot represents the ending point, the blue dot represents the target point finally reached by the robot, the solid blue line represents the robot's trajectory, and the dashed red line represents the global path planned by the JPS-DWA algorithm in Example 1.

[0040] Figure 14 This is a graph showing the changes in robot speed and angular velocity using the JPS-DWA algorithm in Example 1, under an environment with a map size of 50m×50m and an obstacle density of 25%. The blue line represents the change in robot speed (m / s) during movement, and the green line represents the change in robot angular velocity (rad / s) during movement. Detailed Implementation

[0041] Example 1: This example provides a dynamic path planning method based on JPS-DWA fusion. By fusing the improved JPS algorithm with the improved DWA algorithm, path planning in complex environments can be achieved efficiently and accurately, and rapid response and decision-making can be made to real-time obstacles to ensure that the robot continues to move towards the target.

[0042] The dynamic path planning method based on JPS-DWA fusion includes the following steps: S1, constructing a grid map and discretizing the environment into passable areas and obstacle areas.

[0043] This embodiment uses a two-dimensional raster map for environmental modeling, discretizing a continuous space into regular grid cells. For example... Figure 1 As shown, the two-dimensional grid map is the map mode used by the entire pathfinding system. When planning a path, the environment to be planned is regarded as a two-dimensional image on a plane, and this image is converted into coordinates. The map is divided into equal-length unit squares, and the coordinates of the lower left corner of each square are used as the square coordinates required by the actual system calculation. The white area in the map is the passable area, and the black area is a simplified obstacle, that is, an impassable area.

[0044] S2. Global Path Planning Based on the JPS Algorithm. This embodiment addresses the problems of high blindness, large number of expanded nodes, large memory consumption, and low decision efficiency in path planning in complex environments by the traditional JPS algorithm, as well as the limitations of its discretized map representation and simple heuristic function in adapting to complex environments. An improved JPS algorithm is proposed. By introducing an improved comprehensive cost function, the adaptability of the search direction is enhanced, and the blindness of node expansion is reduced. Furthermore, the simplicity and naturalness of the path are improved through redundant node deletion and path smoothing.

[0045] Specifically, this includes: S21, designing a comprehensive cost function, introducing an adaptive weighting mechanism, and dynamically adjusting the heuristic term based on the distance between the current node and the target node. With path cost term The weights of the heuristic terms are increased when the path is far from the target node to accelerate the search, and the weights of the path cost terms are increased when the path is close to the target node. At the same time, obstacle density information is incorporated to guide the path away from the obstacle-dense area (the ratio of the number of obstacles in the rectangular area formed by the coordinates of the current node and the coordinates of the target node to the total number of squares in this rectangular area; if this ratio exceeds 20%, it is considered an obstacle-dense area), thereby improving the safety and quality of the path.

[0046] Comprehensive cost function : ; ; ; ; in, This represents the distance ratio between the current node and the target node. This represents the straight-line distance between the current node and the target node. This is the straight-line distance between the starting point and the target node; It is a vector consisting of the starting point and the target node. The vector formed by the starting point and the current node cross product, The value represents the area of ​​the parallelogram formed by the two vectors. The size of the area represents the quality of the search direction. The larger the cross product, the worse the current search direction is and the higher the deviation from the target point. Otherwise, the better the search direction is and the lower the deviation from the target point. That is and The angle between them; Indicates obstacle density. This represents the number of obstacles within the rectangular area formed between the current node and the target node. The total number of squares in this rectangular area; Indicates the distance from the starting point to the current node. The actual cost, that is, the real distance traveled from the previous node to the current node in the actual map (using Euclidean distance). Indicates the current node Heuristic cost estimation to the target node.

[0047] Figure 2 , Figure 3 and Figure 4The results of path planning for the traditional Astar algorithm, the traditional JPS algorithm, and the JPS algorithm with optimized comprehensive cost function in this embodiment are shown in an environment with a map size of 20m×20m and an obstacle density of 25%. Figure 2 , Figure 3 and Figure 4 The grid starts at the dark green grid (0,0) and ends at the red grid (19,19). The green squares represent areas yet to be explored, while the red squares represent areas that have already been explored.

[0048] Figure 2 The predecessor of the JPS algorithm, the Astar algorithm, explored 136 regions before finding the final path, and there were still 51 regions to be explored. That is, the total number of regions to be explored was 187. The final planned path length was 39m, and the time taken was 0.212735 seconds.

[0049] Figure 3 The traditional JPS algorithm explored 78 regions during path planning, found the final path, and still has 36 regions to be explored, meaning a total of 114 regions need to be explored. The final planned path length is 34.97m, and the time taken is 0.010000 seconds.

[0050] Figure 4 In this embodiment, the JPS algorithm with optimized comprehensive cost function explored 34 regions during path planning, found the final path, and there are still 31 regions to be explored. That is, the total number of regions to be explored is 65. The final planned path length is 34.97m, and the time taken is 0.003996 seconds.

[0051] Depend on Figure 2 , Figure 3 and Figure 4 The results show that the JPS algorithm with optimized comprehensive cost function in this embodiment is 98% faster than the traditional Astar algorithm and 60.04% faster than the traditional JPS algorithm in terms of computation time; the number of accessed nodes is reduced by 75% and 56.41% respectively compared with the traditional Astar algorithm and the traditional JPS algorithm, which significantly improves the search efficiency.

[0052] S22. Adopt a redundant node pruning strategy: Use the optimized DDA algorithm to detect whether the path nodes cross obstacles (i.e., whether they intersect with obstacles). If they do not intersect with obstacles and there are redundant nodes, delete the redundant nodes.

[0053] like Figure 5 As shown, the path (n1,n2,n3,n4) is pruned by removing the redundant node n2. Through redundant node removal and path smoothing, the simplicity and naturalness of the path are further improved.

[0054] The commonly used traversal detection algorithm for 2D grids is the Digital Differential Analyzer (DDA) algorithm. Its core idea is to treat a line segment as a parametric equation and "walk" it in a step of one grid unit, recording all the grids traversed along the way. For example... Figure 6 As shown, assume the line segment is AB, the coordinates of point A are (1,3) and the coordinates of point B are (8,7), and the intersection points of line segment AB with the grid Y-axis are A, n1, n3, n4, n6, n7, n9, and B, respectively.

[0055] The DDA algorithm includes the following steps: S221, Calculate the difference between line segment AB and the X-axis of the grid. The difference in the Y-axis direction of the grid .

[0056] ; ; in, It is the x-axis coordinate of point B on line segment AB. It is the x-axis coordinate of point A on line segment AB. It is the y-axis coordinate of point B on line segment AB. It is the Y-axis coordinate of point A on line segment AB.

[0057] S222. Determine the step size and number of steps.

[0058] ; in, Indicates the step size.

[0059] S223. Calculate the increment in the X-axis direction of the grid at each step. and the increment in the Y-axis direction of the grid ; ; ; S224, From the starting point Initially, each move yields new coordinates. The new coordinates are rounded down to obtain the coordinates. The coordinates obtained by rounding down Add to the detection queue. For example... Figure 6 Starting from point A, move one step to point n1, with coordinates (2, 3.57). Rounding down, the actual grid coordinates should be (2, 3). Add coordinates (2, 3) to the detection queue. Total repetitions: Next, the new grid coordinates are all rounded down to obtain the actual grid coordinates and added to the detection queue (to avoid adding adjacent identical grids repeatedly).

[0060] However, conventional DDA algorithms are limited in detecting, for example... Figure 6 When line segment AB traverses a grid, according to the detection rules, only points n1, n3, n4, n6, n7, and n9 need to be judged. Following the rounding down rule, their coordinates are (2,3), (3,4), (4,4), (5,5), (6,5), and (7,6). All grids with these coordinates are passable. Figure 6 The computer clearly passed through two obstacles, indicating that the conventional DDA algorithm missed some during detection.

[0061] In this embodiment, the DDA algorithm is optimized as follows: (1) When and At that time, for each point After processing the coordinates and adding them to the detection queue, then make a judgment. Is it in the detection queue? If If it is not in the detection queue, then Add to the detection queue; (2) when and At that time, for each point After processing the coordinates and adding them to the detection queue, then make a judgment. Is it in the detection queue? If If it is not in the detection queue, then Add to the detection queue; (3) when and At that time, for each point After processing the coordinates and adding them to the detection queue, then make a judgment. Is it in the detection queue? If If it is not in the detection queue, then Add to the detection queue; (4) when and At that time, for each point After processing the coordinates and adding them to the detection queue, then make a judgment. Is it in the detection queue? If If it is not in the detection queue, then Add to the detection queue. The optimized DDA algorithm in this embodiment ensures that no grid is missed when detecting grid crossings.

[0062] Simulation experiments were conducted on the JPS algorithm with optimized comprehensive cost function in this embodiment before and after pruning. The simulation results are shown in Table 1. The pruning strategy significantly reduces the number of path nodes and path length. When the obstacle ratio is 25%, for a map with a size of 20m×20m, the number of path nodes before pruning is 26, and after pruning it is 8, with an improvement rate of up to 69.23% and a path length reduction of 8.67%. For a map with a size of 50m×50m, the number of nodes before and after pruning is 70 and 18 respectively, with an improvement rate of up to 74.28% and a path length reduction of 24.95%. The improvement rate of nodes for the 50m×50m map is 5.05% higher than that for the 20m×20m map, and the path shortening rate is 16.28% higher. When the obstacle ratio is 10% for both maps, for a 20m×20m map, the number of nodes decreases by 27.78% and the path length decreases by 2.1% after pruning. For a 50m×50m map, the number of nodes decreases by 46.67% and the path length decreases by 1.11%. These results show that the larger the map size and the more obstacles, the more significant the optimization effect. This also verifies that the JPS algorithm optimized in this embodiment is more applicable to various maps and more efficient.

[0063] Table 1. Comparison of pruning results of the JPS algorithm for optimizing the comprehensive cost function in this embodiment before and after pruning. S3. Local Path Planning: Although pruning reduces redundant nodes, the path still has jagged edges and may intersect with obstacle corners, posing a risk to robot movement. Therefore, this embodiment integrates the DWA algorithm. Based on the pruned global path planned in step S2, all path nodes except the start and end points are sequentially used as local target points. The robot moves towards the local targets sequentially, performing local path planning until the final target point is reached, reducing the risk of collision.

[0064] S31. Construct a dynamic window, taking into account the robot's kinematic constraints and safety braking constraints.

[0065] Physical constraints are satisfied: ; Safety braking constraints must meet: ; The final dynamic window is: ; In the formula, and These refer to the linear velocity and angular velocity that the robot needs to achieve in the next step, as obtained from the final dynamic window. and These refer to the minimum and maximum linear velocities that the robot can achieve, respectively. and These refer to the robot's minimum and maximum linear velocities, respectively. This refers to the robot's maximum linear acceleration. and These refer to the robot's minimum and maximum angular velocities, respectively. and These refer to the robot's minimum and maximum angular velocities, respectively. This refers to the robot's maximum angular acceleration. and They refer to robots in linear velocity and angular velocity at time t. Refers to the robot's safe speed. This refers to the robot's minimum safe linear velocity. This refers to the robot's maximum safe linear velocity. To predict the minimum distance between the trajectory point and the obstacle, This is the robot's maximum deceleration. Refers to the time step.

[0066] S32. Discretely sample the linear velocity and angular velocity to generate multiple sets of predicted trajectories.

[0067] Range of values ​​for the linear velocity in the robot's dynamic window The range of angular velocity values ​​in the robot's dynamic window Discrete sampling was performed to generate multiple velocity pairs, as shown in Table 2.

[0068] Table 2. Velocity pairs generated from discrete sampling of linear velocity and angular velocity. in, and These represent the changes in linear velocity and angular velocity in the next step during trajectory prediction.

[0069] For the sake of experimental convenience, in this embodiment, it is assumed that the robot is a two-wheeled differential speed robot, and its kinematic equations are as follows: ; in, The linear velocity of the robot in its current state; The heading angle of the robot's current state is the angle formed with the map's X-axis coordinates, and it also represents the direction the robot is facing.

[0070] After discretization, it becomes: ; in, Let the coordinates of the robot be distributed on a discrete map. For robots in The time corresponds to the coordinates distributed on the discrete map. For robots in The time corresponds to the coordinates distributed on the discrete map. The direction the robot is facing. For robots in The direction you are always facing. For robots in The direction the object is facing at any given moment. All possible linear velocities generated based on the aforementioned velocity sampling space. With angular velocity Trajectory simulation must be performed, in order to For the time step, every Each time, the trajectory needs to be simulated once, and the specific length of the predicted trajectory needs to be determined based on the designed prediction time.

[0071] S33. The predicted trajectory generated in step S32 is evaluated using a comprehensive evaluation function, and the trajectory with the highest comprehensive evaluation score is selected.

[0072] The comprehensive evaluation function provided in this embodiment : ; In the formula, , , , and These represent the weight parameters for the target evaluation function, obstacle distance evaluation function, velocity tendency evaluation function, robot heading angle evaluation function, and stationary state recovery function, respectively. By adding weight parameters for each function, the robot's weight parameters can be dynamically changed according to its current state, thereby further reducing the pathfinding error rate.

[0073] Wherein, the objective evaluation function : ; In the formula, The weights for the target evaluation values, The coordinates of the target node. These are the coordinates of the robot's current state.

[0074] Obstacle distance evaluation function : ; In the formula, The distance between the predicted trajectory point and the nearest obstacle in the robot's current state. To set a safe distance for the robot, The weight of the obstacle distance assessment value.

[0075] Speed ​​tendency evaluation function : ; In the formula, Weights for speed evaluation values.

[0076] Comprehensive evaluation function of traditional DWA algorithm : ; Traditional DWA algorithms, when dealing with complex maps with high obstacle density, exhibit overly sensitive obstacle detection, leading to various detours and erratic maneuvers. Their speed evaluation is too simplistic, preventing the robot from maintaining high speeds during pathfinding when combined with the JPS algorithm. This results in excessively long pathfinding times and inconsistent speeds. Furthermore, the lack of heading angle evaluation causes the robot to deviate from the optimal route when traversing gaps between closely spaced obstacles due to the high cost of obstacle assessment. Therefore, this embodiment adds a stagnation recovery function and a robot heading angle evaluation function to the comprehensive evaluation function.

[0077] Stasis recovery function Allow the robot to change different heading angles while stationary, encouraging it to try multiple directions. Reward the robot if its current direction differs from the previous best direction. ; Robot heading angle evaluation function : ; ; in, For the weights of the heading angle evaluation function, This represents the angle difference between the robot's trajectory direction and the direction from the robot's trajectory to the target node. By adding a heading angle evaluation to the target node's coordinates, the robot is made to move more towards the target rather than making excessive angular deviations.

[0078] Experiment Example 1: To verify the effectiveness and generalization performance of the dynamic path planning method based on JPS-DWA fusion proposed in Example 1, this experiment example built a complete two-dimensional grid map simulation experimental platform in the PyCharm integrated development environment. This platform is built using Python 3.12 and comprehensively utilizes scientific computing and visualization libraries such as NumPy, SciPy, and Matplotlib.

[0079] (1) Comparison of path planning of JPS-DWA algorithm in Example 1 and traditional DWA algorithm under L-shaped and U-shaped obstacles.

[0080] For U-shaped obstacles, such as Figure 7 As shown, under the traditional DWA algorithm, the robot clearly gets stuck in a U-shaped obstacle during path planning and cannot escape, thus entering a dead end; Figure 8 As shown, under the guidance path planned by the JPS-DWA algorithm in Example 1, the robot successfully avoided getting stuck in a U-shaped obstacle dead end.

[0081] For L-shaped obstacles, such as Figure 9 As shown, under the traditional DWA algorithm, the robot clearly gets stuck in an L-shaped obstacle during path planning and cannot escape, thus entering a dead end; Figure 10 As shown, under the guidance path planned by the JPS-DWA algorithm in Example 1, the robot successfully avoided getting stuck in an L-shaped obstacle dead end.

[0082] (2) In an environment with a map size of 50m×50m and an obstacle density of 25%, the JPS-DWA algorithm of Example 1 is compared with the traditional DWA algorithm in terms of path planning, robot travel speed and angular velocity.

[0083] Figure 11 and Figure 12 The figures shown are the robot's overall trajectory and the changes in the robot's speed and angular velocity within this map size using the traditional DWA algorithm. Figure 13 and Figure 14 The figures show the overall trajectory of the robot and the changes in its speed and angular velocity within the map size using the JPS-DWA algorithm of Example 1. The traditional DWA algorithm took 871.16 seconds (the time in this example includes the time for each drawing step and does not represent the actual algorithm response time), with a path length of 74.94 m and an average speed of 0.086 m / s. The JPS-DWA algorithm of Example 1 took 681.27 seconds, with a path length of 70.54 m and an average speed of 0.103 m / s. Therefore, the JPS-DWA algorithm of Example 1 produces a smoother trajectory, takes less time, and has a shorter path length during path planning.

[0084] Of course, the above description is not intended to limit the present invention, and the present invention is not limited to the examples given above. Any changes, modifications, additions or substitutions made by those skilled in the art within the scope of the present invention should also fall within the protection scope of the present invention.

Claims

1. A dynamic path planning method based on JPS-DWA fusion, characterized in that, Includes the following steps: S1. Construct a grid map, discretizing the environment into passable areas and obstacle areas; S2. Global path planning based on the JPS algorithm; S21. Design a comprehensive cost function that dynamically adjusts the weights of the heuristic and path cost terms based on the distance between the current node and the target node, while guiding the path away from areas with dense obstacles. S22. Detect whether the path nodes cross obstacles, ensure that the path does not cross obstacles, and delete redundant nodes; S3. Integrate the DWA algorithm, and based on the global path planned in step S2, use the path nodes as local target points in sequence to perform local path planning. S31. Construct a dynamic window, considering the robot's kinematic constraints and safety braking constraints; S32. Discretely sample the linear velocity and angular velocity to generate multiple sets of predicted trajectories; S33. The predicted trajectory generated in step S32 is evaluated using a comprehensive evaluation function, and the trajectory with the highest comprehensive evaluation score is selected. Comprehensive evaluation function : ； in, Represents the objective evaluation function. This represents the obstacle distance evaluation function. This represents the velocity tendency evaluation function. This represents the robot's heading angle evaluation function. This represents the function for restoring a stagnant state. , , , and These represent the weight parameters of the target evaluation function, obstacle distance evaluation function, velocity tendency evaluation function, robot heading angle evaluation function, and stationary state recovery function, respectively.

2. The dynamic path planning method based on JPS-DWA fusion according to claim 1, characterized in that, In step S21, the comprehensive cost function is... : ； ； ； ； in, This represents the distance ratio between the current node and the target node. This represents the straight-line distance between the current node and the target node. The straight-line distance between the starting point and the target node. It is a vector consisting of the starting point and the target node. The vector formed by the starting point and the current node cross product, Representing vectors with vector The angle between them; Indicates obstacle density. and These represent the number of obstacles and the total number of squares within the rectangular area formed between the current node and the target node, respectively. Indicates the distance from the starting point to the current node. The actual cost; Indicates the current node Heuristic cost estimation to the target node.

3. The dynamic path planning method based on JPS-DWA fusion according to claim 1, characterized in that, In step S22, the optimized DDA algorithm is used to detect whether obstacles are crossed between path nodes. The optimized DDA algorithm includes the following steps: S221. Assuming the detection path is line segment AB, calculate the difference between line segment AB and the grid X-axis. The difference in the Y-axis direction of the grid ; ； ； in, It is the x-axis coordinate of the midpoint B of line segment AB. It is the X-axis coordinate of the midpoint A of line segment AB. It is the Y-axis coordinate of the midpoint B of line segment AB. It is the Y-axis coordinate of the midpoint A of line segment AB; S222. Determine the step size and number of steps; ； in, Indicates the step size; S223. Calculate the increment in the X-axis direction of the grid at each step. and the increment in the Y-axis direction of the grid ; ； ； S224, From the starting point Initially, each move yields new coordinates. The new coordinates are rounded down to obtain the coordinates. The coordinates obtained by rounding down Add to the detection queue; when and At that time, for each point After processing the coordinates and adding them to the detection queue, then make a judgment. Is it in the detection queue? If If it is not in the detection queue, then Add to the detection queue; when and At that time, for each point After processing the coordinates and adding them to the detection queue, then make a judgment. Is it in the detection queue? If If it is not in the detection queue, then Add to the detection queue; when and At that time, for each point After processing the coordinates and adding them to the detection queue, then make a judgment. Is it in the detection queue? If If it is not in the detection queue, then Add to the detection queue; when and At that time, for each point After processing the coordinates and adding them to the detection queue, then make a judgment. Is it in the detection queue? If If it is not in the detection queue, then Add to the detection queue.

4. The dynamic path planning method based on JPS-DWA fusion according to claim 1, characterized in that, In step S31, the dynamic window is: ； in, ; ； In the formula, and These refer to the linear velocity and angular velocity that the robot needs to achieve in the next step, as obtained from the final dynamic window. and These refer to the minimum and maximum linear velocities that the robot can achieve, respectively. and These refer to the robot's minimum and maximum linear velocities, respectively. This refers to the robot's maximum linear acceleration. and These refer to the minimum and maximum angular velocities that the robot can achieve, respectively. and These refer to the robot's minimum and maximum angular velocities, respectively. This refers to the robot's maximum angular acceleration. and They refer to robots in linear velocity and angular velocity at time t. Refers to the robot's safe speed. This refers to the robot's minimum safe linear velocity. This refers to the robot's maximum safe linear velocity. To predict the minimum distance between the trajectory point and the obstacle, This is the robot's maximum deceleration. Refers to the time step.

5. The dynamic path planning method based on JPS-DWA fusion according to claim 4, characterized in that, In step S33, the robot heading angle evaluation function is... : ； ； Stasis recovery function : ； in, For the weights of the heading angle evaluation function, This is the angle difference between the robot trajectory direction and the direction from the robot trajectory to the target node.

6. The dynamic path planning method based on JPS-DWA fusion according to claim 5, characterized in that, In step S33, the objective evaluation function : ； in, The weights for the target evaluation values, The coordinates of the target node. The coordinates represent the robot's current state. Obstacle distance evaluation function : ； in, The distance between the predicted trajectory point and the nearest obstacle in the robot's current state. To set a safe distance for the robot, Weights for obstacle distance assessment values; Speed ​​tendency evaluation function : ； in, As the weight of the speed evaluation value, This represents the linear velocity of the robot in its current state.

7. The dynamic path planning method based on JPS-DWA fusion according to claim 1, characterized in that, In step S21, the obstacle-dense area refers to the rectangular area formed by the current node coordinates and the target node coordinates, where the ratio of the number of obstacles in the rectangular area to the total number of squares in the rectangular area exceeds 20%.

Images