Method and system for autonomous search path planning for rescue robots

Abstract

This invention relates to path planning, and more particularly to a method and system for autonomous search path planning for rescue robots. Specifically, it involves acquiring a grid map of the rescue environment and calculating a weighted extended distance for adjacent nodes, which integrates geometric distance, terrain accessibility, and environmental risk cost. A forward open list starting from the starting point and a reverse open list starting from the target are established. In the bidirectional search, the forward evaluation function incorporates environmental risk, while the reverse evaluation function does not. When a node already visited in another direction is found in one direction, it is recorded as the initial intersection point. Using the current total path extension distance as the upper bound, the search continues bidirectionally to find a better intersection point. Among all intersection points, the point that minimizes the total extension distance after splicing the forward and reverse paths is selected as the optimal intersection point, and an initial path is generated. The initial path is smoothed to ensure that the terrain accessibility and environmental risk values ​​of each point on the smoothed path are within safe thresholds, thus obtaining a feasible path.

Description

Technical Field

[0001] This application belongs to the field of path planning, and in particular relates to a method and system for autonomous search path planning for rescue robots. Background Technology

[0002] Algorithms used for robot path planning include A*, D*, and their variants. In a gridded environment, a cost function is constructed to evaluate path costs, and heuristic information is used to guide the search direction, thus finding a collision-free path. However, unidirectional search algorithms, such as A*, suffer from low search efficiency when dealing with large-scale rescue environment maps, as the search space expands with the map size. Furthermore, path cost models often rely on geometric distances or simple obstacle information; rugged terrain, slippery surfaces, potential collapse zones, or toxic gas leaks can also affect path planning. Bidirectional search strategies, by simultaneously initiating searches from both the starting and target points and converging in the middle, can reduce unnecessary node expansion. However, bidirectional search algorithms, such as the bidirectional A* algorithm, employ symmetric evaluation functions and expansion strategies—meaning forward and backward searches follow the same logic—making them inflexible in handling asymmetric environments with uneven risk distribution and unable to avoid high-risk areas. The initial path obtained from a grid map search consists of a series of discrete straight line segments, which does not conform to the robot's motion characteristics. Therefore, smoothing of the initial path is necessary. However, when optimizing the smoothness and length of a path, the path smoothing method ignores the constraints of the original environmental information. This may result in the smoothed path crossing the terrain or high-risk areas that were previously deliberately avoided. Although the generated path is geometrically continuous, it sacrifices safety and reduces the success rate of the task. Summary of the Invention

[0003] This invention proposes an autonomous search path planning method for rescue robots, addressing the shortcomings of existing methods in handling asymmetric environments with uneven risk distribution and the difficulty in avoiding high-risk areas. The method includes:

[0004] A grid map of the rescue environment is obtained, and an extended distance is calculated as the comprehensive path cost for any adjacent node in the map. The extended distance is based on geometric distance and is weighted by a preset value of terrain accessibility and value of environmental risk.

[0005] Establish a forward open list starting from the starting node and a reverse open list starting from the target node. In the iterative search, define a forward evaluation function that includes the environmental risk cost value and a reverse evaluation function that does not include the environmental risk cost value, and prioritize selecting nodes from the open list with smaller evaluation function values ​​for expansion.

[0006] When a search in one direction encounters a node that has already been visited by a search in another direction, that node is determined as the initial intersection point. The total expansion distance of the current path is recorded as the upper bound, and the search continues to expand in both directions until either open list is empty or a predetermined number of steps is reached to find a better intersection point. Among all the generated intersection points, the intersection point that minimizes the global total expansion distance calculated by concatenating the forward and reverse paths and recalculating the environmental risk cost of the reverse path is selected as the optimal intersection point.

[0007] An initial path is generated based on the optimal intersection point, and the initial path is smoothed to ensure that the terrain accessibility value and environmental risk value corresponding to any point on the smoothed path are within a preset safety threshold, thereby obtaining the path.

[0008] Furthermore, the present invention also relates to an autonomous search path planning system for rescue robots, comprising the following modules:

[0009] The calculation module is used to acquire a grid map of the rescue environment and calculate the extended distance as the comprehensive path cost for any adjacent node in the map. The extended distance is based on geometric distance and is weighted by integrating the preset terrain accessibility value and environmental risk value.

[0010] The extension module is used to establish a forward open list starting from the starting node and a reverse open list starting from the target node. In the iterative search, a forward evaluation function that includes the environmental risk cost value and a reverse evaluation function that does not include the environmental risk cost value are defined, and nodes are preferentially selected from the open list with smaller evaluation function values ​​for extension.

[0011] The selection module is used to determine the node as the initial intersection point when the search in one direction encounters a node that has been visited by the search in another direction. It records the total expansion distance of the current path as the upper bound and continues to expand in both directions until either open list is empty or a predetermined number of steps are reached, in order to find a better intersection point. Among all the generated intersection points, the intersection point that minimizes the global total expansion distance calculated by concatenating the forward and reverse paths and recalculating the environmental risk cost of the reverse path is selected as the optimal intersection point.

[0012] The processing module is used to generate an initial path based on the optimal intersection point and to smooth the initial path to ensure that the terrain accessibility value and environmental risk value corresponding to any point on the smoothed path are within a preset safety threshold, thereby obtaining the path.

[0013] This invention constructs a comprehensive path cost that integrates terrain accessibility and environmental risk, enabling the assessment of the complexity of the rescue environment. This ensures that the planned path considers not only geometric distance but also safety and feasibility. An asymmetric bidirectional search strategy is employed, using different forward and reverse evaluation functions to accelerate search convergence and shorten path planning time while ensuring safe exploration. Furthermore, a mechanism that continues searching for better intersection points after finding the initial intersection point avoids the problem of bidirectional search getting trapped in local optima. The initial path is smoothed with safety constraints, ensuring that the path not only meets the robot's motion characteristics, possessing good smoothness and executability, but also that every point on the path is within a safe zone, making the planning results reliable and practical. Attached Figure Description

[0014] Figure 1 A flowchart of the first embodiment;

[0015] Figure 2 This is a framework diagram of the calculation process of the present invention. Detailed Implementation

[0016] The features and exemplary embodiments of various aspects of this application will be described in detail below. To make the objectives, technical solutions, and advantages of this application clearer, the application will be further described in detail below with reference to the accompanying drawings and specific embodiments. It should be understood that the specific embodiments described herein are only intended to explain this application and not to limit it. For those skilled in the art, this application can be implemented without some of these specific details. The following description of the embodiments is merely to provide a better understanding of this application by illustrating examples.

[0017] In the first embodiment, the present invention proposes a method for autonomous search path planning for rescue robots, such as... Figure 1 ,include:

[0018] S1, Obtain a grid map of the rescue environment, and calculate the extended distance as the comprehensive path cost for any adjacent node in the map. The extended distance is based on geometric distance and is weighted by integrating the preset terrain accessibility value and environmental risk value.

[0019] A two-dimensional grid map of the environment is generated using LiDAR or depth camera sensors through real-time localization and mapping technology. Each grid cell in the map is marked as a passable area or obstacle. For any passable node A and its adjacent node B in the map, the extended distance is calculated using the following formula: Extended Distance = Weighting Coefficient 1 × Geometric Distance + Weighting Coefficient 2 × Terrain Accessibility Cost of Node B + Weighting Coefficient 3 × Environmental Risk Cost of Node B. Wherein, the geometric distance is set according to the direction of movement; if B is A's horizontal or vertical nearest neighbor, the distance is 1; if it is a diagonal nearest neighbor, the distance is 1.414. The terrain accessibility cost is set according to surface conditions, for example, 1 for flat ground, 5 for gravel piles, and 10 for steep slopes. The environmental risk cost is set according to the hazard source, for example, 0 for safe areas, 20 for potential collapse areas, and 50 for open flame areas. Weighting coefficients 1, 2, and 3 are preset values ​​used to adjust the impact of each cost item.

[0020] In some embodiments, the extended distance is based on geometric distance and is weighted by incorporating a preset value for terrain accessibility and a value for environmental risk, including:

[0021] From node to adjacent nodes Extended distance The calculation formula is:

[0022] ;

[0023] in, For nodes arrive geometric distance, and They are nodes The value of terrain accessibility and the value of environmental risks, and These are the preset terrain weight coefficient and risk weight coefficient, respectively;

[0024] The geometric distance is the Manhattan distance.

[0025] In a multi-layered raster map that models environmental information, each raster cell is a node, storing various attribute values. Retrieving the current node... With neighboring nodes The coordinates, for example Located at coordinates (3,4), Located at coordinates (3,5). Calculate the Manhattan distance between the two points as the geometric distance. The value is 1. Query the target node from the map data. Environmental attributes, assuming the value of terrain accessibility A value of 0.8 indicates a certain slope and a corresponding environmental risk value. A value of 0.5 indicates proximity to a danger zone. Weights should be set according to mission requirements, such as terrain weighting. Set to 1.0, risk weight Set it to 1.5.

[0026] Substitute all the acquired data into the formula for calculation. Extended distance. The value is 2.55. The extended distance includes not only the geometric location of the nodes but also the difficulty of the terrain and the degree of danger of the environment, serving as the basis for evaluating path costs in path search algorithms. It not only considers the shortest distance but also proactively avoids rugged terrain and high-risk areas, generating safe paths.

[0027] S2, establish a forward open list starting from the starting node and a reverse open list starting from the target node. In the iterative search, define a forward evaluation function that includes the environmental risk cost value and a reverse evaluation function that does not include the environmental risk cost value, and prioritize selecting nodes from the open list with smaller evaluation function values ​​for expansion.

[0028] Two priority queues are created as a forward open list and a reverse open list to store nodes to be expanded. The evaluation function for the forward search is the actual cumulative expansion distance from the starting node to the current node, plus the estimated heuristic distance from the current node to the target node. The actual cumulative expansion distance includes geometric, terrain, and environmental risks. The evaluation function for the reverse search is the actual cumulative expansion distance from the target node to the current node, plus the estimated heuristic distance from the current node to the starting node. The actual cumulative expansion distance only includes geometric and terrain costs, excluding environmental risks. At each iteration, the evaluation function values ​​of the first node in both the forward and reverse open lists are obtained, compared, and the node with the smaller value is selected for expansion. This node is then removed from its corresponding open list, and its neighboring nodes are added to that list.

[0029] Suppose the robot needs to rescue someone from a safe "entrance" to a high-risk, smoke-filled "room": During initialization, a forward list is placed at the entrance, and a reverse list is placed at the room. Since the forward evaluation takes into account the smoke risk, its evaluation value is 100, while the reverse evaluation ignores the risk and only calculates the distance, its evaluation value may only be 20. Therefore, the robot will first continuously expand the reverse open list until the reverse path's evaluation value exceeds 100 due to the increased distance, at which point it will switch to processing the forward list, thus achieving the effect of quickly advancing the search progress in low-cost areas.

[0030] In some embodiments, defining a positive evaluation function that includes the environmental risk cost value and a negative evaluation function that does not include the environmental risk cost value in the iterative search includes:

[0031] The positive evaluation function The calculation formula is:

[0032] ;

[0033] Where n is the current node, This represents the actual cumulative extension distance from the starting node to the current node n. This is a heuristic distance estimate from the current node n to the target node;

[0034] The Use the Euclidean distance between the current node and the target node;

[0035] The reverse evaluation function The calculation formula is:

[0036] ;

[0037] Where n is the current node, This represents the cumulative cost in reverse from the target node to the current node n. This is a heuristic distance estimate from the current node n to the starting node;

[0038] The reverse cumulative cost When accumulating, the cost of each step is calculated only based on the weighted value of geometric distance and terrain accessibility, without taking into account the value of environmental risk.

[0039] The The Euclidean distance between the current node and the starting node is used.

[0040] Determine the starting node S and the target node T, for example, S is located at coordinates (1,1) and T is located at coordinates (10,10). When the search reaches a current node n, for example, n is located at coordinates (5,6), the evaluation function value needs to be calculated. Calculate the actual cumulative expansion distance. This value is the sum of all expansion distances from the starting node S to node n along the found path. This value is maintained by the algorithm during the expansion process, and is assumed to be 15.2 at present.

[0041] Calculate heuristic distance estimation , which is the Euclidean straight-line distance from the current node n to the target node T. According to the formula... Substituting the data, the value is approximately 6.4. Adding the two parts together yields the evaluation function value. This evaluation value is used to sort nodes in the open list, prioritizing those selected. The node with the smallest value is expanded, thus guiding the search toward the target node. The model structure of this process is based on a priority queue as an open list, where each element stores the coordinates of the node. Value and total value.

[0042] Set the starting node S at coordinates (1,1) and the target node T at coordinates (10,10). The reverse search starts from T, assuming it has now expanded to node n at coordinates (8,7). Calculate the cumulative cost of the reverse search. Unlike forward search, the environmental risk cost is ignored when accumulating costs at each step. For example, the single-step cost from parent node p at coordinates (9,7) to node n is... Assuming a geometric distance of 1, the terrain cost... The terrain weight is 0.6. If the cost is 1.0, then the single-step cost is 1.6. If the cumulative cost from T to p is 4.5, then... The result is 4.5 + 1.6 = 6.1.

[0043] Calculate heuristic distance estimation , which is the Euclidean distance from the current node n to the starting node S, is approximately 9.22. Adding the two parts yields the value of the inverse evaluation function. The asymmetric cost structure allows the reverse search to tend to explore flat terrain areas to construct part of the return path, while leaving the avoidance of high-risk areas entirely to the forward search. This enables path planning to find paths that meet specific travel preferences while ensuring safety. Furthermore, it prevents computational timeouts or unsolvable problems due to excessive caution in complex, high-risk environments.

[0044] To schedule the progress of both forward and reverse searches using a single master loop, in some embodiments, the step of prioritizing the selection of nodes from the open list with smaller evaluation function values ​​for expansion includes:

[0045] In each iteration, the minimum evaluation function value in the forward open list is compared with the minimum evaluation function value in the reverse open list after risk weight pre-compensation, and nodes are selected from the open list with smaller equivalent evaluation values ​​for expansion.

[0046] Two priority queue data structures, one as a forward open list and the other as a forward open list. and reverse open list At the beginning of each iteration, examine the first element of each of the two open lists to obtain the current minimum evaluation function value, denoted as . and .

[0047] because Since environmental risk costs were not factored in, to ensure consistency in the evaluation criteria, the two values ​​need to be pre-compensated and compared using risk weights. For example, at a certain moment, The minimum evaluation function value is 21.6, while The minimum evaluation function value is 15.32. The system evaluates the value based on preset rules, such as the global average risk coefficient. Equivalent compensation is performed, assuming the equivalent assessment value after compensation calculation is 19.92, which is 15.32 plus the estimated compensation. Since the equivalent assessment value of 19.92 is still less than... In step 21.6, a reverse search is performed in this iteration. Therefore, from... The node with the original evaluation function value of 15.32 is extracted and expanded, and the evaluation function values ​​of its neighboring nodes are calculated and added. Forward search is paused in this iteration. Conversely, if... Smaller, then from Nodes are extracted from the data and expanded. The asymmetric expansion, through the introduction of an equivalent evaluation mechanism, retains the advantages of lightweight computation in reverse search while avoiding one-way over-expansion caused by inconsistent numerical scales. It always prioritizes the search direction that seems more promising for quickly reaching the destination, thereby improving the overall search efficiency.

[0048] S3, When the search in one direction encounters a node that has been visited by the search in another direction, the node is determined as the initial intersection point. The total expansion distance of the current path is recorded as the upper bound, and the bidirectional expansion continues until either open list is empty or a predetermined number of steps are reached to find a better intersection point. Among all the generated intersection points, the intersection point that minimizes the global total expansion distance calculated by splicing the forward path and the reverse path and re-incorporating the environmental risk cost of the reverse path is selected as the optimal intersection point.

[0049] When a node reached through the forward search is added to the visited list of the reverse search, that node becomes a junction. At this point, the total expansion distance from the starting node through this junction to the target node is calculated; this is the sum of the actual cumulative forward expansion distance of the junction and the actual cumulative reverse expansion distance after re-incorporating environmental risk costs. This global distance is used as the initial upper bound of the path cost. The search does not stop but continues to expand in both directions. Whenever a new junction is found, the total global expansion distance corresponding to that junction is calculated. If this distance is less than the currently recorded upper bound of cost, the upper bound is updated to this smaller value, and the new, better junction is recorded. This process continues until one of the open lists becomes empty, or the total expansion steps reach the set upper limit. After the search ends, the junction with the minimum total global expansion distance is the optimal junction.

[0050] For example, suppose that when the robot is searching for a path, the forward and reverse search waves first meet in the middle of a muddy path that is short but full of obstacles, which is the initial intersection point. At this time, after the risk value of the missing reverse path is made up in the background, the total cost of the entire road segment is calculated to be 100, and 100 is set as the upper bound to continue expanding the search. The two search waves meet again on a slightly longer but flat asphalt road. After the risk value is made up, the total cost of this road is calculated to be only 80, and the upper bound is updated to 80. After the search is completed, the earliest discovered high-cost muddy road is discarded, and the asphalt road node with the smallest global total cost after splicing and recalculating the environmental risk value of the reverse path is selected as the optimal intersection point, thereby planning a path with the highest overall efficiency.

[0051] To continue optimizing the path after the initial encounter in the bidirectional search, in some embodiments, determining the node as the initial rendezvous point, recording the total extended distance of the current path as an upper bound, and continuing bidirectional expansion until either open list is empty or a predetermined number of steps is reached, and searching for a better rendezvous point, includes:

[0052] Set the predetermined number of steps as a preset proportion of the total number of nodes on the map;

[0053] After finding the initial intersection point, the total global extension distance of the intersection point is used as the initial upper bound, and the search continues;

[0054] When a new intersection point is found whose total global extension distance is less than the current upper bound, the upper bound is updated and the new intersection point is recorded.

[0055] Set an additional number of search steps; for example, for a map with 10,000 nodes, the default percentage is 5%, meaning an additional 500 search steps. This applies when the forward and reverse searches intersect at an initial point. When they meet, calculate the total global path cost through this point. ,in The replenishment cost, calculated by re-incorporating the environmental risk value into the reverse path, is assumed to be 50. This value of 50 is set as the initial upper bound of the cost, U.

[0056] The search continues for the preset 500 steps. During this time, if the forward search expands to a node... Since it has already been accessed via reverse search, then This becomes a new potential intersection point. Calculations are performed through... The total cost of the new path after completion, for example The new cost is compared with the current upper bound U. Since 48 < 50, this path is better under global risk. The upper bound U is then updated to 48, and the success rate is recorded. The path found is taken as the current optimal path. This process is repeated until all extra steps are used up, ensuring that the found path is of higher quality. By utilizing upper bounds and a continuous search mechanism, and strictly using the global cost after risk mitigation as the evaluation criterion, the problem of being satisfied with a local optimum due to errors in the heuristic function and missing costs in the reverse part is avoided.

[0057] S4. Generate an initial path based on the optimal intersection point, and smooth the initial path to ensure that the terrain accessibility value and environmental risk value corresponding to any point on the smoothed path are within a preset safety threshold, thereby obtaining the path.

[0058] Starting from the optimal intersection point, backtrack along the parent node pointers of the forward and reverse search records until the starting and target nodes are reached. These two path segments are then joined to form an initial polyline path composed of a series of grid center points. The initial path is then optimized by sequentially connecting the current point with subsequent non-adjacent nodes on the path, starting from the path's origin, to form a straight line segment. This straight line segment is then densely sampled, and the grid cell containing each sampled point is checked to verify that the cell is neither an obstacle, nor does it have a terrain accessibility cost below a preset terrain threshold, nor an environmental risk cost below a preset risk threshold. If all sampled points on the entire straight line segment meet the safety conditions, this straight line segment replaces the original intermediate nodes on the path. This process is repeated until the path cannot be shortened further; the resulting smooth path is the navigation path.

[0059] In order to transform the discrete grid path generated by the search algorithm into a continuous trajectory suitable for physical entities, in some embodiments, the smoothing process employs a cubic B-spline interpolation algorithm, using the inflection points in the initial path as control points to generate a smooth curve path with continuous second derivative.

[0060] Extract key node sequences from the initial path, for example, a path with node sequences (1,1), (2,1), (3,1), (3,2), (3,3), (4,3). Identify inflection points where the path direction changes. In this example, the inflection points are (1,1), (3,1), (3,3), (4,3). These inflection points will be used as the control point set for the B-spline curve.

[0061] The control points are input into a cubic B-spline interpolation algorithm to generate a parameterized mathematical curve. The control points constrain the curve's shape but do not cross any intermediate control points except for the beginning and end. The generated curve exhibits the characteristic of continuous second derivative; the curvature at any point on the curve changes continuously without sharp angles. This ensures smooth transitions in velocity and acceleration during motion, conforming to the laws of physical motion. Here, the model structure transforms from a discrete node graph to a continuous curve model defined by control points and basis functions.

[0062] In some embodiments, ensuring that the terrain accessibility cost and environmental risk cost corresponding to any point on the smoothed path are both within a preset safety threshold includes:

[0063] Sample the smooth path with a preset step size;

[0064] Check the cost value of each sampling point location. If the terrain accessibility cost value or environmental risk cost value of any sampling point exceeds the corresponding safety threshold, add control points to the corresponding section of the initial path and re-smooth the path until all sampling points on the path meet the safety threshold.

[0065] The generated cubic B-spline curve path is densely sampled with a fixed step size, for example, taking a sampling point every 0.1 units. For each sampling point, its coordinates in continuous space are obtained, for example, coordinates (3.4, 2.7), and these coordinates are mapped back to a discrete raster map to determine the raster cell as 3,2. The pre-stored terrain cost and risk cost of this raster cell are then queried, for example, the terrain cost is 0.9.

[0066] The retrieved cost is compared with a preset safety threshold, assuming the terrain safety threshold is 0.85. Since 0.9 > 0.85, the location of this sampling point is deemed unsafe. The original path segment causing the unsafe section is located, for example, the portion between the initial inflection points (3,1) and (3,3). A new control point is added to the initial path of this segment, for example, inserting a node at (3,2). The B-spline curve is regenerated using the new control point set, as shown below. Figure 2 As shown. The cycle of sampling, checking, adding control points, and re-smoothing will continue until the environmental cost of all sampling points along the entire path is below the safety threshold, thus obtaining a path that combines smoothness and safety.

[0067] In the second embodiment, the present invention also proposes an autonomous search path planning system for rescue robots, comprising the following modules:

[0068] The calculation module is used to acquire a grid map of the rescue environment and calculate the extended distance as the comprehensive path cost for any adjacent node in the map. The extended distance is based on geometric distance and is weighted by integrating the preset terrain accessibility value and environmental risk value.

[0069] The extension module is used to establish a forward open list starting from the starting node and a reverse open list starting from the target node. In the iterative search, a forward evaluation function that includes the environmental risk cost value and a reverse evaluation function that does not include the environmental risk cost value are defined, and nodes are preferentially selected from the open list with smaller evaluation function values ​​for extension.

[0070] The selection module is used to determine the node as the initial intersection point when the search in one direction encounters a node that has been visited by the search in another direction. It records the total expansion distance of the current path as the upper bound and continues to expand in both directions until either open list is empty or a predetermined number of steps are reached, in order to find a better intersection point. Among all the generated intersection points, the intersection point that minimizes the global total expansion distance calculated by concatenating the forward and reverse paths and recalculating the environmental risk cost of the reverse path is selected as the optimal intersection point.

[0071] The processing module is used to generate an initial path based on the optimal intersection point and to smooth the initial path to ensure that the terrain accessibility value and environmental risk value corresponding to any point on the smoothed path are within a preset safety threshold, thereby obtaining the path.

[0072] In some embodiments, the extended distance is based on geometric distance and is weighted by incorporating a preset value for terrain accessibility and a value for environmental risk, including:

[0073] From node to adjacent nodes Extended distance The calculation formula is:

[0074] ;

[0075] in, For nodes arrive geometric distance, and They are nodes The value of terrain accessibility and the value of environmental risks, and These are the preset terrain weight coefficient and risk weight coefficient, respectively;

[0076] The geometric distance is the Manhattan distance.

[0077] In some embodiments, defining a positive evaluation function that includes the environmental risk cost value and a negative evaluation function that does not include the environmental risk cost value in the iterative search includes:

[0078] The positive evaluation function The calculation formula is:

[0079] ;

[0080] Where n is the current node, This represents the actual cumulative extension distance from the starting node to the current node n. This is a heuristic distance estimate from the current node n to the target node;

[0081] The Use the Euclidean distance between the current node and the target node;

[0082] The reverse evaluation function The calculation formula is:

[0083] ;

[0084] Where n is the current node, This represents the cumulative cost in reverse from the target node to the current node n. This is a heuristic distance estimate from the current node n to the starting node;

[0085] The reverse cumulative cost When accumulating, the cost of each step is calculated only based on the weighted value of geometric distance and terrain accessibility, without taking into account the value of environmental risk.

[0086] The The Euclidean distance between the current node and the starting node is used.

[0087] In some embodiments, the step of preferentially selecting nodes from the open list with smaller evaluation function values ​​for expansion includes:

[0088] In each iteration, the minimum evaluation function value in the forward open list is compared with the minimum evaluation function value in the reverse open list after risk weight pre-compensation, and nodes are selected from the open list with smaller equivalent evaluation values ​​for expansion.

[0089] In some embodiments, determining the node as the initial rendezvous point, recording the total extended distance of the current path as an upper bound, and continuing bidirectional expansion until either open list is empty or a predetermined number of steps is reached, and searching for a better rendezvous point, includes:

[0090] Set the predetermined number of steps as a preset proportion of the total number of nodes on the map;

[0091] After finding the initial intersection point, the total global extension distance of the intersection point is used as the initial upper bound, and the search continues;

[0092] When a new intersection point is found whose total global extension distance is less than the current upper bound, the upper bound is updated and the new intersection point is recorded.

[0093] In some embodiments, the smoothing process employs a cubic B-spline interpolation algorithm, using inflection points in the initial path as control points to generate a smooth curve path with continuous second derivative.

[0094] In some embodiments, ensuring that the terrain accessibility cost and environmental risk cost corresponding to any point on the smoothed path are both within a preset safety threshold includes:

[0095] Sample the smooth path with a preset step size;

[0096] Check the cost value of each sampling point location. If the terrain accessibility cost value or environmental risk cost value of any sampling point exceeds the corresponding safety threshold, add control points to the corresponding section of the initial path and re-smooth the path until all sampling points on the path meet the safety threshold.

[0097] The above description is merely a specific implementation of this application. Those skilled in the art will clearly understand that, for the sake of convenience and brevity, the specific working processes of the systems, modules, and units described above can be referred to the corresponding processes in the foregoing method embodiments, and will not be repeated here. It should be understood that the protection scope of this application is not limited thereto. Any person skilled in the art can easily conceive of various equivalent modifications or substitutions within the technical scope disclosed in this application, and these modifications or substitutions should all be covered within the protection scope of this application.

Claims

1. A method for autonomous search path planning for rescue robots, characterized in that, Includes the following steps: A grid map of the rescue environment is obtained, and an extended distance is calculated as the comprehensive path cost for any adjacent node in the map. The extended distance is based on geometric distance and is weighted by a preset value of terrain accessibility and value of environmental risk. Establish a forward open list starting from the starting node and a reverse open list starting from the target node. In the iterative search, define a forward evaluation function that includes the environmental risk cost value and a reverse evaluation function that does not include the environmental risk cost value, and prioritize selecting nodes from the open list with smaller evaluation function values ​​for expansion. When a search in one direction encounters a node that has already been visited by a search in another direction, that node is determined as the initial intersection point. The total expansion distance of the current path is recorded as the upper bound, and the search continues to expand in both directions until either open list is empty or a predetermined number of steps is reached to find a better intersection point. Among all the generated intersection points, the intersection point that minimizes the global total expansion distance calculated by concatenating the forward and reverse paths and recalculating the environmental risk cost of the reverse path is selected as the optimal intersection point. An initial path is generated based on the optimal intersection point, and the initial path is smoothed to ensure that the terrain accessibility value and environmental risk value corresponding to any point on the smoothed path are within a preset safety threshold, thereby obtaining the path. The extended distance is based on geometric distance and is calculated by weighting a preset value for terrain accessibility and environmental risk, including: From node to adjacent nodes Extended distance The calculation formula is: ； in, For nodes arrive geometric distance, and They are nodes The value of terrain accessibility and the value of environmental risks, and These are the preset terrain weight coefficient and risk weight coefficient, respectively; The geometric distance is the Manhattan distance; In the iterative search, a positive evaluation function that includes the environmental risk cost value and a negative evaluation function that does not include the environmental risk cost value are defined, including: The positive evaluation function The calculation formula is: ； Where n is the current node, This represents the actual cumulative extension distance from the starting node to the current node n. This is a heuristic distance estimate from the current node n to the target node; Use the Euclidean distance between the current node and the target node; The reverse evaluation function The calculation formula is: ； Where n is the current node, This represents the cumulative cost in reverse from the target node to the current node n. This is a heuristic distance estimate from the current node n to the starting node; Reverse cumulative cost When accumulating, the cost of each step is calculated only based on the weighted value of geometric distance and terrain accessibility, without taking into account the value of environmental risk. The The Euclidean distance between the current node and the starting node is used.

2. The method according to claim 1, characterized in that, The step of prioritizing the selection of nodes from the open list with smaller evaluation function values ​​for expansion includes: In each iteration, the minimum evaluation function value in the forward open list is compared with the minimum evaluation function value in the reverse open list after risk weight pre-compensation, and nodes are selected from the open list with smaller equivalent evaluation values ​​for expansion.

3. The method according to claim 1, characterized in that, The process involves determining the node as the initial rendezvous point, recording the total extended distance of the current path as an upper bound, and continuing bidirectional expansion until either open list is empty or a predetermined number of steps are reached, in order to find a better rendezvous point, including: Set the predetermined number of steps as a preset proportion of the total number of nodes on the map; After finding the initial intersection point, the total global extension distance of the intersection point is used as the initial upper bound, and the search continues; When a new intersection point is found whose total global extension distance is less than the current upper bound, the upper bound is updated and the new intersection point is recorded.

4. The method according to claim 1, characterized in that, The smoothing process employs a cubic B-spline interpolation algorithm, using inflection points in the initial path as control points to generate a smooth curve path with continuous second derivative.

5. The method according to claim 1, characterized in that, Ensuring that the terrain accessibility value and environmental risk value corresponding to any point on the smoothed path are both within a preset safety threshold includes: Sample the smooth path with a preset step size; Check the cost value of each sampling point location. If the terrain accessibility cost value or environmental risk cost value of any sampling point exceeds the corresponding safety threshold, add control points to the corresponding section of the initial path and re-smooth the path until all sampling points on the path meet the safety threshold.

6. A system for autonomous search path planning of a rescue robot, used to implement the method as described in any one of claims 1 to 5, characterized in that, Includes the following modules: The calculation module is used to acquire a grid map of the rescue environment and calculate the extended distance as the comprehensive path cost for any adjacent node in the map. The extended distance is based on geometric distance and is weighted by integrating the preset terrain accessibility value and environmental risk value. The extension module is used to establish a forward open list starting from the starting node and a reverse open list starting from the target node. In the iterative search, a forward evaluation function that includes the environmental risk cost value and a reverse evaluation function that does not include the environmental risk cost value are defined, and nodes are preferentially selected from the open list with smaller evaluation function values ​​for extension. The selection module is used to determine the node as the initial intersection point when the search in one direction encounters a node that has been visited by the search in another direction. It records the total expansion distance of the current path as the upper bound and continues to expand in both directions until either open list is empty or a predetermined number of steps are reached, in order to find a better intersection point. Among all the generated intersection points, the intersection point that minimizes the global total expansion distance calculated by concatenating the forward and reverse paths and recalculating the environmental risk cost of the reverse path is selected as the optimal intersection point. The processing module is used to generate an initial path based on the optimal intersection point and to smooth the initial path to ensure that the terrain accessibility value and environmental risk value corresponding to any point on the smoothed path are within a preset safety threshold, thereby obtaining the path.

Images