Security-enhanced trajectory planning method based on Astar algorithm
By introducing node safety assessment and tie-breaking strategy into the Astar algorithm, combining key node extraction and rectangular safety corridor construction, and using Bezier curves for path smoothing, the problem of insufficient path safety of the traditional Astar algorithm in complex environments is solved, and the safety and executability of the path are improved.
Patent Information
- Application Number
- CN202511173317.X
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2025-08-21
- Publication Date
- 2025-10-14
- Estimated Expiration
- 2045-08-21
AI Technical Summary
The traditional Astar algorithm lacks a clear environmental safety assessment mechanism in path planning, resulting in a high collision risk in complex environments, affecting the robustness and execution performance of the system.
A node security assessment mechanism and tie-breaking strategy are introduced into the Astar algorithm. High-security nodes are expanded preferentially based on security indicators. The Douglas-Peucker algorithm is combined to extract key nodes, construct rectangular safety corridors, and use the seventh-order Bezier curve for path smoothing to ensure the safety and executability of the path in complex environments.
The generated path enhances the environmental safety and execution stability of the path while ensuring the optimality of the path, and improves the application capability of path planning in high-security tasks.
Smart Images

Figure CN120779970A_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the technical field of intelligent robot path planning, and specifically to a safety-enhanced path planning method based on the Astar algorithm, which is suitable for tasks with high requirements on path safety in scenarios such as indoor and outdoor autonomous navigation, mobile robots, and unmanned vehicles. Background Art
[0002] As a heuristic search algorithm, the Astar algorithm is widely used in the field of path planning. It has the advantages of high computational efficiency and near-optimal paths. In two-dimensional or three-dimensional discrete grid maps, the Astar algorithm can find the shortest path from the starting point to the end point based on a cost function. It has been widely used in systems such as unmanned vehicles, robot navigation, and intelligent logistics.
[0003] However, the traditional Astar algorithm mainly optimizes path length or total cost, and does not give sufficient consideration to the environmental safety of the area through which the path passes. In complex environments, the path tends to stick closely to obstacles when passing through areas with dense obstacles or narrow passages. Although the optimal path length is guaranteed, there is a high collision risk, which affects the robustness and execution performance of the system.
[0004] In addition, existing improvement methods mostly focus on the structural adjustment of the cost function, such as introducing weighted heuristic functions and cost adjustment coefficients. Although these methods have improved the path quality to a certain extent, they lack a clear and explainable safety assessment mechanism and are unable to reasonably quantify and guide the environmental risks of candidate path nodes, which limits the practical application of path planning in high-security tasks.
[0005] Therefore, there is an urgent need for a path planning method that can comprehensively consider environmental safety factors while ensuring path accessibility and optimality, so as to improve the path feasibility and navigation safety. Summary of the Invention
[0006] To address the shortcomings of existing Astar path planning methods in terms of safety guidance, this paper proposes a safety-enhanced path planning method based on the Astar algorithm. It introduces a node safety assessment mechanism and a tie-breaking strategy into the traditional Astar search framework, and uses safety as an auxiliary guidance basis during the path search process, thereby achieving a balance between path length and environmental risks, and generating a navigation path with greater execution safety.
[0007] The above technical objectives of the present invention are achieved through the following technical solutions:
[0008] Step 1: Construct a two-dimensional grid map, discretize the map space into a regular grid, and mark the obstacle status of each grid cell;
[0009] Step 2: Establish a node security assessment mechanism to perform environmental monitoring on a fixed neighborhood around each candidate path node and design a tie-breaking strategy. When a tie occurs with equal node costs, nodes with higher security are prioritized for expansion based on security indicators.
[0010] Step 3: Smooth the searched path to generate the final path for navigation execution.
[0011] Furthermore, the step 1 includes the following contents:
[0012] Model the map workspace as a two-dimensional grid of size M×N , where each cell represents the discrete occupancy state of the corresponding position in the actual environment. For each grid cell , define its occupancy value When a grid position is occupied by an obstacle, it is assigned a value of 1, indicating that it is impassable. Otherwise, it is assigned a value of 0, indicating that it is a passable area. By loading the prior map, simulation model or sensor scanning data, the obstacle boundary area in the environment is identified, and all grid cells in it are marked as obstacles to form an obstacle occupancy matrix map. The starting point and target point in the map are respectively recorded as 、 , the corresponding grid cell should satisfy , that is, the starting point and the end point must be located in the passable area and input into the subsequent algorithm module as the starting point and end point of the path search.
[0013] Furthermore, the step 2 includes the following:
[0014] In the node expansion phase, for candidate path nodes , define its corresponding cumulative security cost , which is used to quantify the overall environmental risk of the path from the starting point to the node. The specific calculation method is:
[0015]
[0016] in, Represents the distance from the starting point to the current node The set of all intermediate nodes on the path, Representation node The local safety cost is used to reflect the obstacle density in the area where the node is located. The calculation is based on the node The neighborhood area constructed as the center is defined as a side length of Square grid window , in this area, count each grid cell To the central node Euclidean distance , and calculate its weighted obstacle density, which is expressed as follows:
[0017]
[0018] in, is an exponentially decaying distance weight function, The attenuation coefficient set by the user is used to reduce the impact of obstacles far away from the center area. is the obstacle indication function, when the grid The value is 1 if it is occupied by an obstacle, and 0 otherwise.
[0019] Through the above mechanism, a fine-grained environmental safety assessment can be performed on the candidate nodes of the path, and the relationship between the path cost and risk can be established, providing safety guidance for subsequent search strategies.
[0020] During the Astar search process, each time the node with the smallest total cost is selected from the open list as the expansion object, if there are multiple candidate nodes with the same total cost, it satisfies:
[0021]
[0022] in Representation node The total cost, From the starting point to the node The actual path cost, For slave nodes To improve the security of the search path when the above costs are equal, a tie-break strategy based on the security index is designed. This strategy is based on the cumulative security cost of each candidate node. Make priority judgments and expand first to meet the following requirements:
[0023]
[0024] Nodes, thereby guiding the search to avoid areas with dense obstacles or high-risk areas. In addition, in the path expansion phase, if a path is found that starts from the starting point and passes through the intermediate nodes to a visited node, The new path, its path cost The optimal path cost with the current record Equal, but its cumulative security cost is lower, which satisfies:
[0025] and
[0026] The parent node information, path cost and accumulated security cost of the node are updated to ensure that the security redundancy of the path is improved without sacrificing the shortest path.
[0027] Through the tie-breaking strategy, nodes with the same total cost and higher security are given priority, which enhances the stability and feasibility of search results in high-density obstacle environments.
[0028] Furthermore, step 3 includes the following:
[0029] Improvements made by following the above steps The discrete path point sequence obtained by the algorithm search is firstly applied to extract the key points of the original path using the Douglas-Peucker (DP) algorithm. The original path is set as the point set The algorithm recursively deletes the intermediate points between line segments that have little impact on the overall path curvature, retains several key points that represent the main geometric shape of the path, and generates a key point set ,in .
[0030] Based on the above key point sequence, a rectangular safety corridor is constructed between every two adjacent key points to limit the feasible area of the control point in the path smoothing process. and The corridor between them is represented as a rectangular constraint domain , the constraint domain is obtained by expanding the above key points.
[0031] Furthermore, the step 4 includes the following contents:
[0032] In each corridor, piecewise Bezier curves are used to smoothly reconstruct the path segments. Each Bezier curve is determined by eight control points, and its mathematical form is:
[0033]
[0034] in is the seventh-order Bessel basis function, For the The first segment of the trajectory The coordinates of the control points.
[0035] Linear inequality constraints are imposed on the control points within each corridor to ensure that the generated path is completely located inside the corridor. At the same time, saturation constraints on velocity and acceleration and continuity constraints on first-order and second-order derivatives at curve splicing points are imposed to ensure the executable and smoothness of the entire path at the dynamic level.
[0036] Finally, the smoothed trajectory satisfies the spatial obstacle constraints, boundary continuity constraints and dynamic feasibility constraints, and can be used as an execution path in actual navigation systems. BRIEF DESCRIPTION OF THE DRAWINGS
[0037] Figure 1 This is an overall flow chart of a safety-enhanced trajectory planning method based on an improved Astar algorithm according to an embodiment of the present invention;
[0038] Figure 2 A comparison diagram of safety-enhanced path planning results according to an embodiment of the present invention;
[0039] Figure 3 This is a diagram of the safety-enhanced trajectory planning result according to an embodiment of the present invention. DETAILED DESCRIPTION
[0040] The present invention provides a safety-enhanced path planning method based on the Astar algorithm. The technical solution of the present invention is further described in detail below with reference to the accompanying drawings and embodiments:
[0041] In a specific embodiment of the present invention, the task scenario is: to plan the path of a robot in a two-dimensional grid map, requiring the path to be as short as possible while increasing the safety distance from obstacles, ensuring the feasibility and robustness of the path, and is suitable for path navigation of robots or unmanned systems in complex environments.
[0042] like Figure 1 As shown, the trajectory planning method of the present invention mainly includes the following steps:
[0043] Step 1: Establish a search space based on the grid map.
[0044] Model the map workspace as a two-dimensional grid of size M×N , where each cell represents the discrete occupancy state of the corresponding position in the actual environment. For each grid cell , define its occupancy value When a grid position is occupied by an obstacle, it is assigned a value of 1, indicating that it is impassable. Otherwise, it is assigned a value of 0, indicating that it is a passable area. By loading the prior map, simulation model or sensor scanning data, the obstacle boundary area in the environment is identified, and all grid cells in it are marked as obstacles to form an obstacle occupancy matrix map. The starting point and target point in the map are respectively recorded as 、 , the corresponding grid cell should satisfy , that is, the starting point and the end point must be located in the passable area and input into the subsequent algorithm module as the starting point and end point of the path search.
[0045] Each grid cell can be expanded to its 8 adjacent cells (including 4 orthogonal directions and 4 diagonal directions), and the corresponding cost is defined for each expandable direction:
[0046] (1) The cost in the orthogonal direction is 1;
[0047] (2) The cost in the diagonal direction is .
[0048] Step 2: Node expansion based on security assessment mechanism.
[0049] In the node expansion phase, for candidate path nodes , define its corresponding cumulative security cost , which is used to quantify the overall environmental risk of the path from the starting point to the node. The specific calculation method is:
[0050]
[0051] in, Represents the distance from the starting point to the current node The set of all intermediate nodes on the path, Representation node The local safety cost is used to reflect the obstacle density in the area where the node is located. The calculation is based on the node The neighborhood area constructed as the center is defined as a side length of Square grid window , in this area, count each grid cell To the central node Euclidean distance , and calculate its weighted obstacle density, which is expressed as follows:
[0052]
[0053] in, is an exponentially decaying distance weight function, The attenuation coefficient set by the user is used to reduce the impact of obstacles far away from the center area. is the obstacle indication function, when the grid The value is 1 if it is occupied by an obstacle, and 0 otherwise.
[0054] Through the above mechanism, a fine-grained environmental safety assessment can be performed on the candidate nodes of the path, and the relationship between the path cost and risk can be established, providing safety guidance for subsequent search strategies.
[0055] During the Astar search process, each time the node with the smallest total cost is selected from the open list as the expansion object, if there are multiple candidate nodes with the same total cost, it satisfies:
[0056]
[0057] in Representation node The total cost, From the starting point to the node The actual path cost, For slave nodes The heuristic estimated cost to the target node is expressed as:
[0058]
[0059] In order to improve the security of the search path under the above-mentioned equal costs, a tie-breaking strategy based on security indicators is designed. This strategy is based on the cumulative security cost of each candidate node. Make priority judgments and expand first to meet the following requirements:
[0060]
[0061] Nodes, thereby guiding the search to avoid areas with dense obstacles or high-risk areas. In addition, in the path expansion phase, if a path is found that starts from the starting point and passes through the intermediate nodes to a visited node, The new path, its path cost The optimal path cost with the current record Equal, but its cumulative security cost is lower, that is, it satisfies:
[0062] and
[0063] The parent node information, path cost and accumulated security cost of the node are updated to ensure that the security of the path is improved without sacrificing the shortest path.
[0064] Through the above-mentioned tie node balance breaking strategy, the nodes with higher security among the nodes with the same total cost are preferentially selected, and the stability and executability of the search results in a high-density obstacle environment are enhanced. The pseudo code of the method is as follows: Figure 2 shown.
[0065] Step 3: Extract key nodes and construct rectangular safety corridors.
[0066] After obtaining a preliminary path sequence by searching for a path based on the improved Astar algorithm, this paper proposes a key node extraction method based on the Douglas-Peucker (DP) algorithm to simplify the number of path nodes and ensure path execution safety in complex environments. Furthermore, this paper proposes a method for dynamically constructing rectangular safety corridors based on obstacle distances and incorporating environmental obstacle distribution information. The method specifically includes the following steps:
[0067] 1. Key node extraction
[0068] The purpose of key node extraction is to remove redundant intermediate nodes in the path and extract the geometric skeleton of the path without destroying the overall outline and obstacle avoidance characteristics of the path. The present invention adopts the DP simplification algorithm. The specific steps include:
[0069] (1) The result path of the improved Astar algorithm path search As input path sequence, where As a starting point, For the end point.
[0070] (2) Set the starting point of the current path segment and the end point All intermediate nodes between them are considered candidate nodes, and the shortest distance from each candidate node to the straight line is calculated. .
[0071] (3) Find the maximum distance among all candidate nodes and its corresponding nodes .
[0072] (4) If , then keep the node As the key node, the path segment is split into and , recursively execute steps (2) to (4) for the two paths respectively; if , then the path is considered to be approximately a straight line, and all intermediate nodes can be deleted, leaving only the starting point and the end point.
[0073] (5) The above recursive process continues until the error of all path segments does not exceed the simplified threshold. , and finally output the key node sequence of the path .
[0074] Through the above steps, redundant nodes in the path can be effectively eliminated, the path complexity can be reduced, and the main geometric contours and turning features of the path can be retained.
[0075] 2. Construction of rectangular safety corridor
[0076] In order to improve the safety margin and dynamic feasibility of the path during actual execution, the present invention constructs a rectangular safety corridor based on the extracted key nodes, with the key nodes as seeds and extending along the positive and negative directions of the coordinate axis. The specific steps include:
[0077] (1) For each key node , on the two-dimensional grid map where it is located, along the positive direction of the X axis (+X), the negative direction of the X axis ( ), the positive direction of the Y axis (+Y) and the negative direction of the Y axis ( ) expand outwards respectively, gradually searching for the obstacle cells closest to the node.
[0078] (2) In each direction, record the slave nodes The grid distance from the start to the nearest obstacle cell is recorded as 、 、 、 ,If there is no obstacle in a certain direction, the maximum expansion limit constraint must be met.
[0079] (3) To improve the safety margin of the path, a safety buffer distance is reserved based on the calculation. , the final expansion distances of the safety corridor in each direction are:
[0080]
[0081]
[0082]
[0083]
[0084] in, It is the preset maximum expansion limit to prevent the corridor from expanding infinitely in open areas.
[0085] (4) According to the expansion distance, determine the coordinates of the four vertices of the rectangular corridor:
[0086]
[0087]
[0088]
[0089]
[0090] (5) Connect the four vertices in sequence to form a key node A rectangular safety corridor with a seed and dynamically determined boundaries based on the actual distribution of environmental obstacles .
[0091] (6) Repeat the above steps for all key nodes in the path, and finally obtain the set of safe corridors along the entire path. .
[0092] (7) When there are overlapping areas in the safety corridors of adjacent key nodes, the intersection area is selected as the effective safe passage area of the path segment to ensure the continuity and safety of the path segment.
[0093] Through this method, the expansion boundaries of the safety corridor in all directions can be adaptively determined according to the actual distribution of obstacles around key nodes, ensuring that the path has sufficient obstacle avoidance margin and feasible space in complex environments, providing a reliable geometric constraint basis for subsequent path smoothing and trajectory optimization.
[0094] Step 4: Path smoothing and trajectory optimization.
[0095] After completing the extraction of key nodes on the path and the construction of the safe corridor, the present invention further smoothes and optimizes the initial path to improve the feasibility and safety of the path during actual navigation execution. The present invention uses a seventh-order piecewise Bezier curve to model the path and, based on the Minimum Snap trajectory optimization method, achieves overall smooth optimization of the path, provided that the trajectory is within the safe corridor.
[0096] First, the path is divided into trajectory segments based on the sequence of key nodes and the corresponding safety corridor information. Unlike the traditional method that uses key nodes as path segment boundaries, the present invention determines the end point position of the trajectory segment based on the overlapping area of adjacent safety corridors. The starting point and end point of the path are fixed at the starting point and end point of the path, respectively. The end point position of the intermediate trajectory segment is selected within the overlapping area of adjacent corridors to ensure the spatial continuity and safety of each trajectory segment. In this way, the entire path is divided into several trajectory segments, and each path segment is modeled using a seventh-order Bezier curve.
[0097] Each Bezier curve is determined by eight control points, and its mathematical form is:
[0098]
[0099] in is the seventh-order Bessel basis function, For the The first segment of the trajectory The coordinates of the control points.
[0100] Regarding the position constraint of the control point, the present invention imposes a spatial range constraint on each Bezier curve control point based on the rectangular safety corridor data constructed above, requiring it to be located inside the safety corridor associated with the corresponding trajectory segment. The corresponding safety corridor boundary range is and , then the control point position must satisfy the following inequality:
[0101]
[0102] In addition, to ensure the continuity and smoothness between trajectory segments, the present invention imposes continuity constraints on position, velocity, and acceleration at the connection points of the path segments, specifically including:
[0103] Position continuity:
[0104]
[0105] Speed Continuity:
[0106]
[0107] Acceleration continuity:
[0108]
[0109] In terms of trajectory dynamic feasibility constraints, the present invention uses the characteristic of the Bezier curve that it still maintains the form of the Bezier curve after derivation to strictly constrain the velocity and acceleration range of the trajectory. By expressing the first-order and second-order derivatives of the trajectory as sixth-order and fifth-order Bezier curves respectively, velocity and acceleration envelope constraints are applied respectively to meet and , ensuring that the trajectory meets the dynamic feasibility requirements in the full parameter domain and avoiding the uncertainty of discrete sampling detection of the trajectory.
[0110] In terms of trajectory optimization objectives, the present invention minimizes the sum of squares of the fourth-order derivatives (Snap) of the trajectory, and defines the optimization objective function as:
[0111]
[0112] The objective function can be transformed into the quadratic form of the control point as follows:
[0113]
[0114] in represents the joint vector of all control points, is a sparse symmetric matrix precomputed using the Bezier curve feature.
[0115] Finally, the trajectory optimization problem is modeled as a quadratic programming problem with linear equality and inequality constraints, whose mathematical model is:
[0116]
[0117] Wherein:
[0118] · Equality constraints Including position, velocity and acceleration continuity constraints at the connection of path segments;
[0119] · Inequality constraints Including control point safety corridor range constraints and trajectory dynamic performance constraints.
[0120] By solving the above quadratic programming problem, the optimal trajectory control point sequence meeting the requirements of path safety, smoothness and dynamic feasibility can be obtained, ensuring the path to have good execution performance in complex environments.
Claims
1. A safety-enhanced trajectory planning method based on the Astar algorithm, characterized in that: The following steps are involved: Step 1: Construct a two-dimensional grid map, discretize the map space into a regular grid, and mark the obstacle status of each grid cell; Step 2: Establish a node security assessment mechanism to perform environmental detection on a fixed neighborhood around each candidate path node and calculate its security index; Step 3: Cost function based on traditional Astar algorithm Extension node, where From the starting point to the node The actual path cost, For slave nodes Heuristically estimate the cost to the target node and design a tie-breaking strategy. When a tie occurs with equal node costs, prioritize expanding to more secure nodes based on security metrics. Step 4: Smooth the searched path to generate the final path for navigation execution.
2. The method according to claim 1, characterized in that The marking of the obstacle status in step 1 is achieved by assigning an impassable attribute to the grid cell where the obstacle is located.
3. The method according to claim 1, characterized in that The node evaluation mechanism in step 2 is used to quantify the safety of the path by calculating the safety index, which is the safety index from the starting point to the node The cumulative security cost is expressed as: in, Yes The set of all nodes on the path of the node, It is at the node The local security cost is calculated by evaluating the node The center is the obstacle density in the square area specified by the user, and the side length is obtained by: in Represents a node As the center, The grid cells in the square area with side length Refers to the node With grid cells The Euclidean distance of is an exponentially decaying weight function, is an indicator function, if the cell If there is an obstacle, it is equal to 1, otherwise it is equal to 0.
4. The method according to claim 1, wherein The safety node expansion mechanism described in step 3 is based on the premise of ensuring the optimality of the path length. Therefore, the cost function F(n) is consistent with the traditional Astar algorithm, which is obtained by adding the path cost G(n) and the heuristic estimate H(n). The designed tie-breaking strategy can give priority to expanding nodes with higher safety indicators when multiple nodes have the same total cost. Specifically, in the node expansion phase, when multiple nodes share the same minimum F value, the node with the minimum cumulative safety cost is given priority. When a new path from the starting point to a visited node is found, if the new path has the same path cost as the current path, that is, , and the cumulative security cost is smaller, that is, , then update the parent node information and the accumulated security cost value of the node.
5. The method according to claim 1, wherein The path smoothing process described in step 4 first extracts the key nodes of the obtained initial safe path based on the Douglas-Peucker (DP) algorithm, generates rectangular safe corridors based on the key nodes, and finally smoothes the initial path based on piecewise Bezier curves to ensure the continuity of the path at the level of position, velocity, and acceleration, thereby improving the executable and dynamic feasibility of the path.
Citation Information
Patent Citations
Fusion path planning method based on improved A* algorithm and Bezier curve
CN118034299A
Mobile robot path planning method based on improved A-star algorithm
CN118550299A
Unmanned aerial vehicle autonomous route planning and collision avoidance decision method and system
CN118915804A
Unmanned logistics vehicle path planning method based on fusion of improved A* and improved DWA
CN119043359A
Local path planning method, system and device integrating improved speed obstacle and TEB algorithm and medium
CN119085651A
Cited By
Road marking repairing and marking method based on image recognition
CN121527732A
A road marking repair and drawing method based on image recognition
CN121527732B
Path planning method for electric power inspection mobile robot
CN121632194A
Orthoimage navigation-oriented elastic security constraint trajectory generation method and system
CN122062698A
