A safety-enhanced trajectory planning method based on Astar algorithm

Abstract

The application discloses a kind of safety enhancement trajectory planning methods based on Astar algorithm, belong to intelligent robot navigation and path planning technical field, the method includes the following steps: 1, constructs grid map and marks obstacle region;2, establish safety evaluation mechanism, the environment around path node is evaluated, and the safety index is quantified;3, in the process of node expansion, the heuristic term in cost function is combined with safety index, and the priority search of safety area is realized when path selection;4, based on Douglas-Poke algorithm, the key node of the initial safety path obtained is extracted to generate rectangular safety corridor, and based on segmented Bezier curve, the initial path is smoothed, the application can guarantee the basis of path optimality, and in the process of path search, safety is used as auxiliary guide, so that the path length and environmental risk are considered, and the navigation path with more execution safety is generated.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of intelligent robot path planning, in particular to a safety-enhanced path planning method based on Astar algorithm, which is suitable for tasks with high requirements for path safety in indoor and outdoor autonomous navigation, mobile robots, unmanned vehicles and the like. BACKGROUND

[0002] As a heuristic search algorithm, Astar algorithm is widely used in the field of path planning, and has the advantages of high calculation efficiency and path close to optimality. In a two-dimensional or three-dimensional discrete grid map, Astar algorithm can find the shortest path from the starting point to the ending point based on the cost function, and has been widely used in unmanned vehicles, robot navigation and intelligent logistics systems.

[0003] However, the traditional Astar algorithm mainly optimizes the path length or total cost, and does not consider the environmental safety of the area through which the path passes. In a complex environment, the path tends to stick to obstacles when passing through obstacle-dense areas or narrow passages, although the optimality of the path length is guaranteed, there is a high risk of collision, which affects the robustness and execution effect of the system.

[0004] In addition, existing improved methods mainly focus on the structural adjustment of the cost function, such as introducing a weighted heuristic function, a cost adjustment coefficient, etc. Although the path quality is improved to some extent, there is a lack of clear and interpretable safety evaluation mechanism, which cannot reasonably quantify and guide the environmental risk of candidate path nodes, limiting the practical application ability of path planning in high safety tasks.

[0005] Therefore, there is an urgent need for a path planning method that can consider environmental safety factors on the basis of ensuring path accessibility and optimality, in order to improve the executability and navigation safety of the path. SUMMARY

[0006] To solve the shortcomings of existing Astar path planning methods in safety guidance, the present application proposes a safety-enhanced path planning method based on Astar algorithm, which introduces a node safety evaluation mechanism and a tie-breaking strategy in the traditional Astar search framework, and uses safety as an auxiliary guidance basis in the path search process, thereby achieving a balance between path length and environmental risk, and generating a navigation path with higher execution safety.

[0007] The above technical purpose of the present application is achieved by the following technical scheme:

[0008] Step 1, constructing a two-dimensional grid map, discretizing the map space into regular grids, and marking the obstacle state of each grid unit;

[0009] Step 2: Establish a node security assessment mechanism, conduct environmental detection on a fixed neighborhood range around each candidate path node, and design a tie-breaking strategy. When nodes have the same cost and a tie occurs, prioritize expanding nodes with higher security based on security indicators.

[0010] Step 3: Smooth the searched path to generate the final path used for navigation execution.

[0011] Further, step 1 includes the following:

[0012] The map workspace is modeled as a two-dimensional grid of size M×N. Each cell represents the discrete occupancy status of the corresponding location in the actual environment. For each grid cell... Define its occupancy value When a grid cell 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 prior maps, simulation models, or sensor scan data, obstacle boundary areas in the environment are identified, and all grid cells within these areas are marked as obstacles, forming an obstacle occupancy matrix map. The starting point and the target point in the map are denoted as follows: , Its corresponding grid cell should satisfy That is, the starting and ending points must both be located within a passable area, and these locations are used as the starting and ending points for path search input to subsequent algorithm modules.

[0013] Furthermore, step 2 includes the following:

[0014] During the node expansion phase, for candidate path nodes Define its corresponding cumulative security cost. This is used to quantify the overall environmental risk of the path from the starting point to the node. The specific calculation method is as follows:

[0015]

[0016] in, This represents the distance from the starting point to the current node. The set of all intermediate nodes traversed along the path. Represents a node The local security cost is used to reflect the obstacle density in the region where the node is located. The computation is based on nodes The neighborhood region constructed around the center is defined as having a side length of Square grid window Within this area, count each grid cell. To the central node Euclidean distance And calculate its weighted barrier density, expressed as follows:

[0017]

[0018] in, It is an exponentially decaying distance weighting function. The attenuation coefficient set by the user is used to reduce the impact of obstacles far from the center area. For obstacle indication functions, when the grid The value is 1 when the object is occupied by an obstacle, and 0 otherwise.

[0019] Through the above mechanism, fine-grained environmental security assessments can be performed on candidate nodes of a path, establishing a correlation between path cost and risk, and providing security guidance for subsequent search strategies.

[0020] During the Astar search process, the node with the lowest total cost is selected from the open list each time as the expansion target. If multiple candidate nodes have the same total cost, then the following condition is met:

[0021]

[0022] in Represents a node The total cost, From the starting point to the node The actual path cost For the node To improve the safety of the search path when the costs mentioned above are equal, a tie-break strategy based on a safety metric is designed, which estimates the heuristic cost to the target node. This strategy is based on the cumulative safety cost of each candidate node. Perform priority judgment and prioritize expansion that satisfies:

[0023]

[0024] The nodes guide the search to avoid areas with dense obstacles or high risks. Furthermore, during the path expansion phase, if a path is found leading from the starting point through intermediate nodes to a visited node... The new path, its path cost Cost of the currently recorded optimal path The condition is satisfied if the cumulative security cost is lower than the condition if they are equal:

[0025] and

[0026] Then, the parent node information, path cost and cumulative safety cost of the node are updated to ensure the safety redundancy of the path is improved without sacrificing the shortest path.

[0027] Through the above tie-breaking strategy, the higher safety of the node with the same total cost is selected preferentially, and the stability and executability of the search result in the high-density obstacle environment are enhanced.

[0028] Further, the step 3 includes the following contents:

[0029] For the improved path obtained by performing the above steps The discrete path point sequence obtained by the algorithm search is firstly subjected to key point extraction by applying the Douglas-Peucker (DP) algorithm to the original path, and the original path is set as a point set The algorithm removes the intermediate points between the line segments which have less influence on the overall path curvature through recursion, and retains a number of key points representing the main geometric shape of the path to generate a key point set , wherein .

[0030] Based on the above key point sequence, a rectangular safety corridor is constructed between each two adjacent key points to limit the feasible region of the control point in the path smoothing process, and the corridor between any two points and is represented as a rectangular constraint domain , which is obtained by inflating the above key points.

[0031] Further, the step 4 includes the following contents:

[0032] In each corridor, the path segment is reconstructed by using a segmented Bezier curve, and each Bezier curve is determined by eight control points, and the mathematical form is:

[0033]

[0034] , wherein is a seventh Bezier base function, is the control point coordinate of the segment trajectory.

[0035] In each corridor, linear inequality constraints are applied to the control points to ensure that the generated path is completely located inside the corridor, and by applying the saturation constraints of acceleration and acceleration and the first-order and second-order derivative continuity constraints at the curve splicing points, the executability and smoothness of the entire path in the dynamics level are ensured.

[0036] Finally, the smoothed trajectory satisfies the space obstacle constraint, the boundary continuity constraint and the dynamic feasibility constraint, and can be used as an execution path in an actual navigation system. BRIEF DESCRIPTION OF DRAWINGS

[0037] Figure 1 A flowchart of a safety-enhanced trajectory planning method based on an improved Astar algorithm according to an embodiment of the present application;

[0038] Figure 2 A comparison diagram of safety-enhanced path planning results according to an embodiment of the present application;

[0039] Figure 3 A safety-enhanced trajectory planning result diagram according to an embodiment of the present application. DETAILED DESCRIPTION

[0040] The present application provides a safety-enhanced path planning method based on an Astar algorithm, and the technical solutions of the present application will be further described in detail below in combination with the drawings and embodiments:

[0041] In one specific embodiment of the present application, the task scenario is to perform path planning for a robot in a two-dimensional grid map, and it is required to improve the safety distance from obstacles on the basis of as short a path as possible, to ensure the feasibility and robustness of the path, and to be suitable for path navigation of robots or unmanned systems in complex environments.

[0042] As shown in Figure 1 , the trajectory planning method of the present application mainly includes the following steps:

[0043] Step 1: Establish a search space based on a grid map.

[0044] Model the map workspace as a two-dimensional grid map with a size of MxN , where each cell represents the discrete occupancy state of the corresponding position in the actual environment. For each grid cell , define its occupancy value , which is assigned a value of 1 when the grid position is occupied by an obstacle, indicating an impassable area, or a value of 0 when it is not, indicating a passable area. By loading prior map, simulation model or sensor scan data, the obstacle boundary area in the environment is identified, and all grid cells therein are marked as obstacles to form an obstacle occupancy matrix map. The start point and the target point in the map are denoted as ,

[0045] Each grid cell can expand to its 8 adjacent cells (including 4 orthogonal directions and 4 diagonal directions), and define the corresponding cost for each expandable direction:

[0046] (1) The cost of orthogonal direction is 1;

[0047] (2) The cost of diagonal direction is .

[0048] Step two: node expansion based on safety evaluation mechanism.

[0049] In the node expansion stage, for the candidate path node , define its corresponding cumulative safety cost , which is used to quantify the overall environmental risk of the path from the starting point to the node, and the specific calculation method is:

[0050]

[0051] wherein, represents the set of all intermediate nodes on the path from the starting point to the current node , and represents the local safety cost of node , which is used to reflect the obstacle density of the area where the node is located. The calculation of the local safety cost is based on the neighborhood area centered on node , which is defined as a square grid window with a side length of . In this area, the Euclidean distance from each grid cell to the center node is calculated, and its weighted obstacle density is calculated as follows:

[0052]

[0053] wherein, is an exponential decay type distance weight function, is a user set decay coefficient, which is used to reduce the impact of obstacles far from the center area, is an obstacle indicator function, which takes the value of 1 when the grid is occupied by obstacles, and 0 otherwise.

[0054] Through the above mechanism, the environmental safety of the path candidate node can be evaluated in detail, and the correlation between the path cost and the risk is established, which provides safety guidance for the subsequent search strategy.

[0055] During the Astar search process, the node with the lowest total cost is selected from the open list each time as the expansion target. If multiple candidate nodes have the same total cost, then the following condition is met:

[0056]

[0057] in Represents a node The total cost, From the starting point to the node The actual path cost For the node The heuristically estimated cost to the target node is expressed as:

[0058]

[0059] To improve the security of the search path when the aforementioned costs are equal, a tie-breaking strategy based on security metrics is designed. This strategy is based on the cumulative security cost of each candidate node. Perform priority judgment and prioritize expansion that satisfies:

[0060]

[0061] The nodes guide the search to avoid areas with dense obstacles or high risks. Furthermore, during the path expansion phase, if a path is found leading from the starting point through intermediate nodes to a visited node... The new path, its path cost Cost of the currently recorded optimal path The values ​​are equal, but the cumulative security cost is lower, i.e., the following condition is met:

[0062] and

[0063] Then update the parent node information, path cost, and cumulative security cost of the node to ensure that the security of the path is improved without sacrificing the shortest path property.

[0064] By employing the aforementioned tie-breaking strategy, priority is given to nodes with higher security among those with the same total cost, thereby enhancing the stability and executability of search results in high-density obstacle environments. The pseudocode for this method is as follows: Figure 2 As shown.

[0065] Step 3: Extracting key nodes and constructing a rectangular security corridor.

[0066] After the preliminary path sequence is obtained by searching the path based on the improved Astar algorithm, in order to simplify the number of path nodes and ensure the safety of the path execution in a complex environment, the application proposes a key node extraction method based on the Douglas-Peucker (DP) algorithm, and on this basis, the application further proposes a method of dynamically constructing a rectangular safety corridor based on obstacle distance in combination with the distribution information of environmental obstacles, which specifically includes the following steps:

[0067] 1. Key node extraction

[0068] The purpose of key node extraction is to eliminate redundant intermediate nodes in the path and extract the geometric skeleton of the path without destroying the overall contour and obstacle avoidance characteristics of the path. The application adopts the DP simplification algorithm, and the specific steps include:

[0069] (1) The path searched based on the improved Astar algorithm is taken as the input path sequence, where is the starting point, is the ending point.

[0070] (2) All intermediate nodes between the starting point and the ending point of the current path segment are regarded as candidate nodes, and the shortest distance of each candidate node to the straight line of the segment is calculated. .

[0071] (3) In all candidate nodes, the maximum distance and the corresponding node are found.

[0072] (4) If , the node is retained as a key node, and the path segment is split into and , and steps (2) to (4) are recursively performed on the two path segments; if , it is considered that the path segment is approximately a straight line, and all intermediate nodes can be deleted, and only the starting point and the ending point are retained.

[0073] (5) The above recursive process is performed until the errors of all path segments do not exceed the simplification threshold , and the key node sequence of the path is finally output. .

[0074] Through the above steps, redundant nodes in the path can be effectively eliminated, the complexity of the path is reduced, and the main geometric contour and turning features of the path are retained.

[0075] 2. Rectangular safety corridor construction ​

[0076] In order to improve the safety margin and dynamic feasibility of the path in the actual execution process, the application constructs a rectangular safety corridor with the key node as the seed and extending along the positive and negative directions of the coordinate axis based on the extracted key node, specifically including the following steps:

[0077] (1) For each key node , on the two-dimensional grid map where it is located, extend outward along the X-axis positive direction (+X), X-axis negative direction (-X), Y-axis positive direction (+Y) and Y-axis negative direction (-Y) respectively, and gradually search for the nearest obstacle cell from the node.

[0078] (2) In each direction, record the grid distance from the node to the nearest obstacle cell, respectively , , , , if there is no obstacle in a certain direction, the maximum expansion limit constraint needs to be met.

[0079] (3) In order to improve the safety margin of the path, a safety buffer distance is reserved on the basis of calculation, and the final expansion distance of the safety corridor in each direction is:

[0080]

[0081]

[0082]

[0083]

[0084] wherein is the preset maximum expansion limit to avoid unlimited expansion of the corridor in the empty area.

[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 turn to form a rectangular safety corridor with the key node as the seed and dynamically determine the boundary according to the actual distribution of environmental obstacles​​ .

[0091] (6) Repeat the above steps for all key nodes in the path, and finally obtain the safety corridor set of the entire path .

[0092] (7) When there is an overlapping area between the safety corridors of adjacent key nodes, select the intersection area as the effective safety passing area of the path segment to ensure the continuity and safety of the path segment.

[0093] Through the above method, the expansion boundary of the safety corridor in each direction can be adaptively determined according to the actual distribution of obstacles around the key node, ensuring that the path has sufficient obstacle avoidance margin and feasible space in a complex environment, and providing a reliable geometric constraint basis for subsequent path smoothing and trajectory optimization.

[0094] Step four: path smoothing and trajectory optimization.

[0095] After completing the extraction of path key nodes and the construction of safety corridors, the initial path is further smoothed and optimized to improve the feasibility and safety of the path in the actual navigation execution process. The present application uses a seven-order segmented Bezier curve to model the path, and based on the Minimum Snap trajectory optimization method, the overall smoothing and optimization of the path is realized on the premise that the trajectory is within the safety corridor range.

[0096] First, according to the sequence of path key nodes and the corresponding safety corridor information, the trajectory segment is divided. Unlike the traditional method of taking key nodes as the path segment boundary, the present application determines the end point position of the trajectory segment according to the overlapping area of adjacent safety corridors. The starting point and the end point of the path are fixed at the starting point and the 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 continuity and safety of each trajectory segment in space. Through the above method, the entire path is divided into several trajectory segments, and each trajectory segment is modeled using a seven-order Bezier curve.

[0097] Each Bezier curve is determined by eight control points, and its mathematical form is:

[0098]

[0099] where is the seven-order Bezier base function, is the control point coordinate of the segment trajectory.

[0100] For the control point position constraint, the application imposes a spatial range constraint on each Bezier curve control point based on the aforementioned constructed rectangular safety corridor data, requiring it to be located inside the safety corridor associated with the corresponding trajectory segment, assuming that the path segment The corresponding safety corridor boundary range is and Therefore, the control point position needs to satisfy the following inequalities:

[0101]

[0102] In addition, to ensure the continuity and smoothness between trajectory segments, the application imposes continuity constraints on position, velocity and acceleration at the path segment connection, specifically including:

[0103] Position continuity:

[0104]

[0105] Velocity continuity:

[0106]

[0107] Acceleration continuity:

[0108]

[0109] In terms of trajectory dynamic feasibility constraints, the application uses the characteristic that the Bezier curve remains in the form of a Bezier curve after differentiation to strictly constrain the velocity and acceleration range of the trajectory, by expressing the first and second derivatives of the trajectory as six-order and five-order Bezier curves, respectively, to impose velocity and acceleration envelope constraints, satisfying and to ensure that the trajectory meets the dynamic feasibility requirements in the entire parameter domain, avoiding the uncertainty of trajectory discrete sampling detection.

[0110] In terms of trajectory optimization objectives, the application minimizes the square sum of the fourth derivative (Snap) of the trajectory, defining the optimization objective function as:

[0111]

[0112] This objective function can be converted into a quadratic form representation of the control points as:

[0113]

[0114] where is the joint vector of all control points, is a sparse symmetric matrix calculated in advance through the characteristics of Bezier curves.

[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, Includes the following steps: 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, perform environmental detection on a fixed neighborhood around each candidate path node, and calculate a security index based on obstacle density and distance weighting to quantify the security level of the node; Step 3: Establish a node expansion mechanism based on the cost function of the traditional Astar algorithm. Expanding nodes, where From the starting point to the node The actual path cost For the node Heuristically estimate the cost to the target node, design a tie-breaking strategy, and when the node costs are the same and a tie occurs, prioritize expanding the node with higher security based on the security index. Step 4: Smooth the searched path to generate the final path used 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 the impassable attribute to the grid cell where the obstacle is located.

3. The method according to claim 1, characterized in that, The node security assessment mechanism described in step 2 quantifies the security level of a path by calculating a security index, where the security index is the distance from the origin to the node. The cumulative security cost is expressed as: ； in, Is to The set of all nodes along the path of a node. It is at the node The local security cost is assessed by node-based evaluation. Centered on the square area, the side length is obtained from the obstacle density within the square area specified by the user, and is expressed as: ； in Represented by node Centered on, The grid cells are within a square region with sides of length 1. Refers to nodes With grid cells Euclidean distance, It is an exponentially decaying weighting function. It is an indicator function, if the cell If it contains obstacles, it equals 1; otherwise, it equals 0.

4. The method according to claim 1, characterized in that, The node expansion mechanism described in step 3 is based on ensuring optimal path length. Therefore, the cost function F(n) is consistent with the traditional Astar algorithm, obtained by adding the path cost G(n) and the heuristic estimate H(n). The designed tie-breaking strategy can prioritize expanding nodes with higher safety indices when multiple nodes have the same total cost. Specifically, during 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 node discovers a new path from its origin to a visited node, if the new path has the same path cost as the current path, then... Moreover, the cumulative security cost is smaller, that is... If so, update the parent node information and the cumulative security value of the node.

5. The method according to claim 1, characterized in that, 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 a rectangular safe corridor based on the key nodes, and finally smooths the initial path based on piecewise Bézier curves to ensure the continuity of the path in terms of position, velocity and acceleration, thereby improving the executability and dynamic feasibility of the path.

Images