Path planning method and system based on geometric heuristic and dynamic window fusion
By combining the improved A* algorithm and B-spline curve smoothing with the improved dynamic window method, the problems of low path planning efficiency and local optima are solved, generating continuous and smooth paths, and realizing efficient and safe navigation of robots in complex environments.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- ANHUI UNIVERSITY OF TECHNOLOGY
- Filing Date
- 2026-01-20
- Publication Date
- 2026-04-21
AI Technical Summary
Existing path planning algorithms are inefficient in complex environments and the paths are not smooth. Local path planning is prone to getting trapped in local optima, resulting in insufficient robot navigation performance.
An improved A* algorithm combined with B-spline curves is used for global path planning and smoothing, and an improved dynamic window method is used for local real-time obstacle avoidance. The search is optimized through geometric constraints and adaptive neighborhood pruning, and local planning is guided by dynamic sub-target points.
It improves the efficiency and quality of path planning, generates continuous and smooth paths, avoids local optima problems, and enables robots to exhibit efficient, safe, and smooth navigation performance in complex environments.
Smart Images
Figure CN121898463A_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the field of robot path planning technology, specifically to a path planning method and system based on geometric inspiration and dynamic window fusion. Background Technology
[0002] Mobile robots are an important branch of automation and artificial intelligence, and their autonomous navigation capability is a key measure of their intelligence level. Path planning, as the core technology of autonomous navigation, aims to plan a collision-free, optimal, or suboptimal path for the robot from the starting point to the destination in a given environment.
[0003] Existing path planning algorithms are generally divided into global path planning and local path planning. Global path planning, such as the A* algorithm, can find the optimal path on a known static map, but it has the following drawbacks: 1) In complex environments, the number of nodes searched is huge, resulting in low planning efficiency; 2) The planned path is usually composed of a series of polyline segments, containing a large number of inflection points, which does not conform to the kinematic characteristics of the robot, resulting in unsmooth robot movement.
[0004] Local path planning, such as the Dynamic Window (DWA) algorithm, possesses excellent real-time dynamic obstacle avoidance capabilities. It searches directly within the robot's velocity space, making it well-suited for dealing with unknown or dynamic obstacles. However, the DWA algorithm lacks a global perspective. When facing large concave obstacles (such as "U"-shaped traps), it is prone to getting stuck in local optima due to the final target point being obscured, leading to robot oscillations or planning failure.
[0005] To combine the advantages of both, fusing global and local planning algorithms has become a mainstream trend. However, existing fusion methods often suffer from problems such as long global planning time and insufficient guidance of the global path for local obstacle avoidance, resulting in room for improvement in overall navigation performance. Therefore, it is necessary to propose a more efficient and reliable fusion path planning method. Summary of the Invention
[0006] The technical problem to be solved by this invention is: how to solve the problems of low efficiency and non-smooth paths in the existing global path planning algorithm, and the local path planning algorithm is prone to getting trapped in local optima, and to provide a path planning method based on geometric inspiration and dynamic window fusion.
[0007] The present invention solves the above-mentioned technical problems through the following technical solution, and the present invention includes the following steps:
[0008] S1: Global Path Planning
[0009] Based on the environment map, the starting point, and the target point, an improved A* algorithm is used to plan a global path from the starting point to the target point.
[0010] S2: Global Path Smoothing
[0011] B-spline curves are used to smooth the global path, generating a continuous and smooth global reference trajectory;
[0012] S3: Local Real-Time Obstacle Avoidance
[0013] When performing navigation tasks, the robot uses an improved dynamic window method to perform real-time local path planning and dynamic obstacle avoidance based on a global reference trajectory until it reaches the target point.
[0014] Furthermore, in step S1, the improved A* algorithm includes an improvement on the heuristic function of the traditional A* algorithm. The improved heuristic function H'(n) is defined as follows:
[0015] H'(n) = H(n) + w*Area(S,P,G);
[0016] Where n is the current evaluation node, H(n) is the Euclidean distance from the current search node P to the target point, Area(S,P,G) is the area of the triangle formed by the starting point S, the current search node P and the target point G, and w is the weight coefficient of the geometric constraint.
[0017] Furthermore, in step S1, the improved A* algorithm also includes an adaptive neighborhood pruning strategy. The adaptive neighborhood pruning strategy prunes the 8-neighborhood search of the current search node to a 5-neighborhood search based on the relative positional relationship between the current search node and the target point, and eliminates invalid search neighborhoods that are far away from the target point.
[0018] Furthermore, in step S2, before smoothing with B-spline curves, a redundant node removal operation is performed. Starting from the starting point of the global path, subsequent nodes are connected sequentially. If the connected straight line segment does not collide with an obstacle, the intermediate node in the global path is deleted, resulting in a global path after redundant node removal.
[0019] Furthermore, in step S2, the specific process of smoothing using B-spline curves is as follows: each inflection point in the global path after removing redundant nodes is used as a control point, and cubic B-spline curves are applied for interpolation smoothing to generate a continuous and smooth global reference trajectory.
[0020] Furthermore, in step S3, the improved dynamic window method is implemented by modifying the azimuth evaluation function of the traditional dynamic window method. The specific processing procedure of the improved dynamic window method is as follows:
[0021] S31: On the global reference trajectory, based on the robot's current position, search forward a preset look-ahead distance and set the trajectory point corresponding to the look-ahead distance as the dynamic sub-target point of the current cycle;
[0022] S32: Change the target direction of the azimuth evaluation function in the traditional dynamic window method from the final target point to a dynamic sub-target point; the new azimuth evaluation function is used to evaluate the cosine value of the angle between the heading angle at the end of the predicted trajectory and the direction vector from the current position to the dynamic sub-target point; the core of this step is to change the local guidance of the traditional DWA from the global endpoint to the dynamic sub-target point, and use the cosine value to quantify the degree of alignment between the predicted trajectory and the target direction, which is consistent with the actual algorithm logic.
[0023] This invention also provides a path planning system based on the fusion of geometric inspiration and dynamic windows, applied to the above-mentioned method, including:
[0024] The global path planning module is used to plan a global path from the starting point to the target point based on the environment map, the starting point, and the target point, using an improved A* algorithm.
[0025] The global path smoothing module is used to smooth the global path using B-spline curves, generating a continuous and smooth global reference trajectory.
[0026] The local real-time obstacle avoidance module is used to perform real-time local path planning and dynamic obstacle avoidance based on the global reference trajectory when the robot is performing navigation tasks, until the target point is reached.
[0027] The present invention has the following advantages over the prior art:
[0028] 1. Planning efficiency has been significantly improved.
[0029] By employing geometric constraint inspiration and adaptive neighborhood pruning, the improved A* algorithm significantly reduces the number of nodes searched and computation time, thereby increasing the speed of global path generation.
[0030] 2. Improved path quality
[0031] By combining B-spline curve smoothing, the generated global path is not only optimal in length, but also smooth and has continuous curvature, which better meets the kinematic requirements of the robot and is beneficial for energy saving and stable control.
[0032] 3. Strong dynamic obstacle avoidance capability and no local optima problem.
[0033] The improved DWA algorithm uses dynamic sub-target points for guidance, ensuring that local obstacle avoidance always serves the global optimal strategy, effectively solving the problems of target loss and local optima in traditional DWA in complex obstacle environments.
[0034] 4. Excellent fusion effect and strong adaptability
[0035] By closely combining the macro-optimality of global planning with the real-time responsiveness of local planning, the robot can exhibit efficient, safe, and smooth navigation performance in complex environments where static and dynamic obstacles coexist. Attached Figure Description
[0036] Figure 1 This is a schematic diagram of the implementation process of the robot path planning method in an embodiment of the present invention;
[0037] Figure 2 This is a schematic diagram of the geometric constraint heuristic function in an embodiment of the present invention;
[0038] Figure 3 This is a schematic diagram of the adaptive neighborhood pruning strategy in an embodiment of the present invention;
[0039] Figure 4 This is a schematic diagram of the DWA algorithm based on global path guidance in an embodiment of the present invention. Detailed Implementation
[0040] The embodiments of the present invention are described in detail below. These embodiments are implemented based on the technical solution of the present invention, and provide detailed implementation methods and specific operation processes. However, the scope of protection of the present invention is not limited to the following embodiments.
[0041] Example 1
[0042] This invention provides a mobile robot path planning method based on geometric heuristics and dynamic window fusion, comprising the following steps: First, global path planning is performed. This step, based on a known static environment map, starting point, and target point, invokes an improved A* algorithm. The improvement of the A* algorithm is reflected in two aspects: First, the heuristic function is optimized. Based on the traditional Euclidean distance, a geometric constraint term consisting of the current node, starting point, and target point is introduced. This constraint term penalizes nodes that deviate from the straight path between the starting and ending points, thereby enhancing the purposefulness of the search and reducing the expansion of invalid nodes. Second, an adaptive neighborhood pruning strategy is adopted. Based on the orientation of the target point relative to the current node, the traditional 8-direction neighborhood search is dynamically reduced to 5 directions, eliminating directions that are significantly deviated from the target, further improving search efficiency.
[0043] Secondly, global path smoothing is performed. The original path planned in the previous step still has inflection points. This step uses a two-stage post-processing: in the first stage, redundant nodes in the path are removed to simplify the path structure; in the second stage, the simplified key nodes are used as control points, and cubic B-spline curves are used for interpolation to generate a global reference trajectory with continuous curvature suitable for smooth robot movement.
[0044] Finally, local real-time obstacle avoidance is performed. During actual operation, the robot initiates a modified Dynamic Window (DWA) method for real-time decision-making. The core improvement of this DWA algorithm lies in its azimuth evaluation function. Instead of using a distant final target point as the heading guide, it searches for a dynamic sub-target point forward based on the robot's current position on the smooth global reference trajectory generated in the previous step. In each control cycle, the DWA algorithm uses this dynamic sub-target point as the optimal heading, combined with real-time obstacle information perceived by sensors, to solve for the optimal linear and angular velocity commands within a velocity window, and controls the robot to execute them. This process is repeated until the robot reaches its destination.
[0045] This embodiment also provides a system for implementing the above method. The system includes: a global path planning module, a global path smoothing module, and a local real-time obstacle avoidance module. The modules work together to complete the navigation task.
[0046] Example 2
[0047] This invention provides a mobile robot path planning method and system based on geometric inspiration and dynamic window fusion. Its core framework consists of a global path planning layer and a local real-time decision-making layer.
[0048] 1. Improved A* Global Path Planning
[0049] At the start of the task, the system first performs global path planning. The evaluation function of the traditional A* algorithm is F(n) = G(n) + H(n), where G(n) is the actual cost and H(n) is the heuristic cost (heuristic function). The improvements of this invention focus on the heuristic cost H(n) and the search strategy.
[0050] 1.1 Geometric Constraint Heuristic Function
[0051] like Figure 2As shown, the starting point is S, the ending point is G, and the current search node is P. The traditional heuristic function H(n) only considers the distance from P to G. This invention introduces a geometric penalty term. The improved heuristic function H'(n) is defined as: H'(n) = H(n) + w * Area(S,P,G), where H(n) is calculated using Euclidean distance. Area(S,P,G) is the area of the triangle formed by points S, P, and G, reflecting the degree to which the current search node P deviates from the ideal straight path (line segment SG). w is a weighting coefficient used to adjust the influence of this penalty term (the area of the triangle mentioned above). The closer node P is to line segment SG, the smaller Area(S,P,G) is, and the less the corresponding heuristic value increases; therefore, the probability of this node being preferentially expanded is greater. This method effectively guides the search algorithm to focus on the vicinity of the line connecting the starting and ending points, avoiding blind exploration in irrelevant areas.
[0052] 1.2 Adaptive Neighborhood Pruning Strategy
[0053] like Figure 3 As shown, the traditional A* algorithm indiscriminately expands the eight neighborhood directions (up, down, left, right, upper left, lower left, upper right, and lower right) of the current node. This invention dynamically trims the search directions based on the position of the target point G relative to the current search node P. For example, if the target point G is located to the upper right of P (in the first quadrant), the algorithm will primarily retain the "up," "upper right," and "right" directions, and additionally retain the two adjacent directions "upper left" and "lower right" to handle possible concave obstacles. In this way, the original eight-direction search is reduced to five directions, significantly reducing the computational load per node and further improving the planning speed.
[0054] 1.3 Path Post-processing
[0055] The path Path_raw obtained by the improved A* algorithm described above is composed of a series of grid center points. First, redundant nodes are removed. Starting from the path's origin, it is designated as the current key node i. A greedy strategy is used to traverse subsequent nodes j (j=i+2,i+3,…,n). It is determined whether the line connecting node i and node j collides with an obstacle. If the line segment does not collide, the probe continues until the farthest node k that can be directly connected without collision is found. At this point, all intermediate nodes between node i and node k are considered redundant and removed. Then, node k is updated as the new current key node i, and the above steps are repeated until the path ends. After this step, a path Path_key composed of key inflection points is obtained. Then, the key inflection points in Path_key are used as control points, and cubic B-spline curves are applied for interpolation smoothing to generate the final smooth global reference trajectory Path_smooth.
[0056] 2. Improved DWA local obstacle avoidance
[0057] Once the robot begins to move, it enters a real-time local planning loop.
[0058] 2.1 Azimuth Evaluation Based on Global Path Guidance
[0059] like Figure 4 As shown, the traditional DWA algorithm's azimuth evaluation function `heading(v,w)` assesses the quality of the predicted trajectory's orientation towards the final target point G. This can cause problems when point G is occluded by a large obstacle. This invention improves upon this. In each control cycle, the algorithm first determines a dynamic sub-goal. This point is located on the global reference trajectory `Path_smooth` and is at a preset "look-ahead distance L" from the robot's current position. Then, the target direction of the DWA's azimuth evaluation function is changed from the final target point G to this dynamic sub-goal. That is, the new evaluation function `heading'(v,w)` is used to evaluate the cosine of the angle between the heading angle at the end of the predicted trajectory (the robot's facing direction after moving along the predicted trajectory) and the direction vector from the current position to the sub-goal. The smaller the angle, the higher the score.
[0060] 3. Algorithm Fusion Framework
[0061] like Figure 1 As shown, the entire workflow is as follows:
[0062] 1) Initialization: Input the map, robot starting point S and target point G.
[0063] 2) Global planning: Call the improved A* algorithm described in Section 1 to quickly calculate a global path from S to G, and generate a smooth global reference trajectory Path_smooth after post-processing.
[0064] 3) Start the local navigation loop:
[0065] a. Determine sub-goals: Determine the current dynamic sub-goal point on the Path_smooth;
[0066] b. Perception: Acquire real-time obstacle information around the robot using sensors such as LiDAR;
[0067] c. Local decision: Invoke the improved DWA algorithm described in Section 2. This algorithm samples within a velocity window that satisfies the robot's kinematics and safety constraints, and scores each sampled velocity using an evaluation function that includes heading'(v,w);
[0068] d. Execution: Select the linear velocity and angular velocity commands with the highest scores and send them to the robot's underlying controller for execution.
[0069] 4) Loop and Termination: Repeat step 3) until the distance between the robot and the final target point G is less than the preset threshold, and the task is completed.
[0070] Through this framework, global planning provides macro-optimal guidance, while local planning is responsible for real-time dynamic obstacle avoidance. The two are tightly coupled through dynamic sub-target points, jointly achieving efficient, safe, and smooth path planning in complex dynamic environments.
[0071] Although embodiments of the present invention have been shown and described above, it is understood that the above embodiments are exemplary and should not be construed as limiting the present invention. Those skilled in the art can make changes, modifications, substitutions and variations to the above embodiments within the scope of the present invention.
Claims
1. A path planning method based on the fusion of geometric inspiration and dynamic windows, characterized in that, Includes the following steps: S1: Global Path Planning Based on the environment map, the starting point, and the target point, an improved A* algorithm is used to plan a global path from the starting point to the target point. S2: Global Path Smoothing B-spline curves are used to smooth the global path, generating a continuous and smooth global reference trajectory; S3: Local Real-Time Obstacle Avoidance When performing navigation tasks, the robot uses an improved dynamic window method to perform real-time local path planning and dynamic obstacle avoidance based on a global reference trajectory until it reaches the target point.
2. The path planning method based on geometric inspiration and dynamic window fusion according to claim 1, characterized in that, In step S1, the improved A* algorithm includes an improvement on the heuristic function of the traditional A* algorithm. The improved heuristic function H'(n) is defined as follows: H'(n) = H(n) + w*Area(S,P,G); Where n is the current evaluation node, H(n) is the Euclidean distance from the current search node P to the target point, Area(S,P,G) is the area of the triangle formed by the starting point S, the current search node P and the target point G, and w is the weight coefficient of the geometric constraint.
3. The path planning method based on geometric inspiration and dynamic window fusion according to claim 2, characterized in that, In step S1, the improved A* algorithm also includes an adaptive neighborhood pruning strategy. The adaptive neighborhood pruning strategy prunes the 8-neighborhood search of the current search node to a 5-neighborhood search based on the relative positional relationship between the current search node and the target point, and removes invalid search neighborhoods that are far away from the target point.
4. The path planning method based on geometric inspiration and dynamic window fusion according to claim 3, characterized in that, In step S2, before smoothing with B-spline curves, a redundant node removal operation is performed. Starting from the starting point of the global path, subsequent nodes are connected sequentially. If the connected straight line segment does not collide with an obstacle, the intermediate node in the global path is deleted, resulting in the global path after redundant node removal.
5. The path planning method based on geometric inspiration and dynamic window fusion according to claim 4, characterized in that, In step S2, the specific process of smoothing using B-spline curves is as follows: each inflection point in the global path after removing redundant nodes is used as a control point, and cubic B-spline curves are applied for interpolation smoothing to generate a continuous and smooth global reference trajectory.
6. The path planning method based on geometric inspiration and dynamic window fusion according to claim 5, characterized in that, In step S3, the improved dynamic window method is implemented by modifying the azimuth evaluation function of the traditional dynamic window method. The specific processing procedure of the improved dynamic window method is as follows: S31: On the global reference trajectory, based on the robot's current position, search forward a preset look-ahead distance and set the trajectory point corresponding to the look-ahead distance as the dynamic sub-target point of the current cycle; S32: The target direction of the azimuth evaluation function in the traditional dynamic window method is changed from the final target point to the dynamic sub-target point; the new azimuth evaluation function is used to evaluate the cosine value of the angle between the heading angle at the end of the predicted trajectory and the direction vector from the current position to the dynamic sub-target point.
7. A path planning system based on the fusion of geometric inspiration and dynamic windows, characterized in that, The method applied to any one of claims 1 to 6 includes: The global path planning module is used to plan a global path from the starting point to the target point based on the environment map, the starting point, and the target point, using an improved A* algorithm. The global path smoothing module is used to smooth the global path using B-spline curves, generating a continuous and smooth global reference trajectory. The local real-time obstacle avoidance module is used to perform real-time local path planning and dynamic obstacle avoidance based on the global reference trajectory when the robot is performing navigation tasks, until the target point is reached.