Autonomous search path planning method and system for rescue robot

Abstract

The invention relates to path planning, in particular to an autonomous search path planning method and system for a rescue robot, and specifically, the method comprises the following steps: obtaining a grid map of a rescue environment, and calculating a weighted extension distance fusing a geometric distance, terrain trafficability and environment risk cost for adjacent nodes; a forward open list starting from a starting point and a reverse open list starting from a target are established, and in bidirectional search, environmental risks are included in a forward evaluation function, and the environmental risks are not included in a reverse evaluation function; when a node accessed in the other direction is searched in a certain direction, recording the node as an initial intersection, and continuing bidirectional expansion by taking the total expansion distance of the current path as an upper bound to search a better intersection; in all the intersection points, selecting a point which enables the total extension distance to be minimum after the forward path and the reverse path are spliced as an optimal intersection point, and generating an initial path; and smoothing the initial path to ensure that the terrain trafficability and the environmental risk value of each point on the smoothed path are within a safety threshold, and obtaining a feasible path.

Description

Methods and systems for autonomous search path planning of rescue robots 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 difficulty in avoiding high-risk areas. The method includes: acquiring a grid map of the rescue environment; calculating an extended distance as a comprehensive path cost for any adjacent nodes on the map, where the extended distance is based on geometric distance and weighted by a preset terrain accessibility cost and environmental risk cost; establishing a forward open list starting from the starting node and a reverse open list starting from the target node; defining a forward evaluation function that includes the environmental risk cost and a reverse evaluation function that does not, and prioritizing open lists with smaller evaluation function values. Nodes are selected for expansion. When a search in one direction encounters a node already 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 an upper bound, and bidirectional expansion continues until any open list is empty or a predetermined number of steps are reached to find a better intersection point. Among all 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 cost and environmental risk cost corresponding to any point on the smoothed path are within a preset safety threshold, thus obtaining the path.

[0004] Furthermore, this invention also relates to an autonomous search path planning system for rescue robots, comprising the following modules: a calculation module, used to acquire a grid map of the rescue environment, calculate the extended distance as a comprehensive path cost for any adjacent nodes in the map, the extended distance being based on geometric distance and weighted by a preset terrain accessibility cost and environmental risk cost; an extension module, used to establish a forward open list starting from the starting node and a reverse open list starting from the target node, defining a forward evaluation function that includes the environmental risk cost and a reverse evaluation function that does not include the environmental risk cost in the iterative search, and prioritizing the selection of nodes from the open list with smaller evaluation function values ​​for extension; and a selection module, used for... When a search in one direction encounters a node already visited by a search in another direction, that node is determined as the initial intersection point. The total extension distance of the current path is recorded as an upper bound, and bidirectional expansion continues until either open list is empty or a predetermined number of steps are reached, searching for a better intersection point. Among all generated intersection points, the intersection point that minimizes the global total extension 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 generates an initial path based on the optimal intersection point and smooths the initial path to ensure that the terrain accessibility cost and environmental risk cost corresponding to any point on the smoothed path are within a preset safety threshold, thereby obtaining the path.

[0005] 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

[0006] Figure 1 is a flowchart of the first embodiment; Figure 2 is a framework diagram of the calculation process of the present invention. Detailed Implementation

[0007] 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.

[0008] In the first embodiment, the present invention proposes a method for autonomous search path planning for rescue robots, as shown in Figure 1, including: S1, acquiring a grid map of the rescue environment, calculating the extended distance as a comprehensive path cost for any adjacent node in the map, the extended distance being based on geometric distance and weighted by incorporating preset terrain accessibility value and environmental risk value; generating a two-dimensional grid map of the environment using LiDAR or depth camera sensors through real-time localization and mapping technology, where each grid 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 by the following formula: Extended distance = Weight coefficient 1 × Geometric distance + Weight coefficient 2 × Terrain accessibility value of node B + Weight coefficient 3 × Environmental risk value of node B. Wherein, the geometric distance is set according to the direction of movement; if B is a horizontal or vertical nearest neighbor of A, the distance is 1; if it is a diagonal nearest neighbor, the distance is 1.414. The terrain accessibility value is set according to the 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, a safe area is 0, a potential collapse area is 20, and an open flame area is 50. Weighting coefficients one, two, and three are preset values ​​used to adjust the degree of impact of each cost item.

[0009] 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: 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 preset terrain weight coefficients and risk weight coefficients, respectively; the geometric distance is the Manhattan distance.

[0010] 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 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.

[0011] 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.

[0012] 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.

[0013] 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.

[0014] 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.

[0015] 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: 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 estimation from the current node n to the target node; The Euclidean distance between the current node and the target node is used; the back 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. The heuristically estimated distance from the current node n to the starting node; the reverse cumulative cost During the accumulation process, the cost of each step is calculated only based on the weighted average of geometric distance and terrain accessibility, without taking into account the environmental risk cost; The Euclidean distance between the current node and the starting node is used.

[0016] 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.

[0017] 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.

[0018] 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.

[0019] 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.

[0020] In order to schedule the progress of the forward and reverse searches through a master control loop, in some embodiments, the step of preferentially selecting nodes from the open list with smaller evaluation function values ​​for expansion includes: in each iteration, comparing the minimum evaluation function value in the forward open list with the minimum evaluation function value in the reverse open list after risk weight pre-compensation, and selecting nodes from the open list with smaller equivalent evaluation values ​​for expansion.

[0021] 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 .

[0022] 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.

[0023] S3, when the search in one direction encounters a node already visited by the search in another direction, that node is determined as the initial intersection point. The total expansion distance of the current path is recorded as an upper bound, and bidirectional expansion continues until either open list is empty or a predetermined number of steps are reached, searching for a better intersection point. Among all 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 for the reverse path is selected as the optimal intersection point. When a node expanded by the forward search is added to the visited list of the reverse search, that node becomes an intersection point. At this time, the total expansion distance from the starting node through this intersection point to the target node is calculated, which is the sum of the actual cumulative forward expansion distance of this intersection point and the actual cumulative reverse expansion distance after recalculating the environmental risk cost. This global distance is used as the upper bound of the initial path cost. The search does not stop but continues to expand in both directions. Whenever a new intersection is found, the total global expansion distance corresponding to that intersection 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 intersection 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 is complete, the intersection with the minimum total global expansion distance is the optimal intersection.

[0024] 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.

[0025] To continue optimizing the path after the initial encounter in the bidirectional search, in some embodiments, determining the node as the initial intersection point, recording the total expansion 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 to find a better intersection point includes: setting the predetermined number of steps as a preset proportion of the total number of nodes on the map; after finding the initial intersection point, using the total expansion distance of the intersection point as the initial upper bound, and continuing the search; when a new intersection point with a total expansion distance of less than the current upper bound is found, updating the upper bound and recording the new intersection point.

[0026] 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.

[0027] 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.

[0028] 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.

[0029] 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.

[0030] 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.

[0031] 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.

[0032] 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.

[0033] 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: sampling the smoothed path with a preset step size; checking the cost of each sampling point; if the terrain accessibility cost or environmental risk cost of any sampling point exceeds the corresponding safety threshold, then adding a control point to the corresponding segment of the initial path and re-smoothing the path until all sampling points on the path meet the safety threshold.

[0034] 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.

[0035] The environmental cost value 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 determined to be unsafe. The original path segment causing the unsafe section is located, for example, the part 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 set of control points, as shown in Figure 2. The cycle of sampling, checking, adding control points, and re-smoothing will continue until the environmental cost value of all sampling points along the entire path is below the safety threshold, thus obtaining a path that is both smooth and safe.

[0036] In the second embodiment, the present invention also proposes an autonomous search path planning system for rescue robots, comprising the following modules: a calculation module, used to acquire a grid map of the rescue environment, calculate the extended distance as a comprehensive path cost for any adjacent nodes in the map, the extended distance being based on geometric distance and weighted by a preset terrain accessibility cost and environmental risk cost; an extension module, used to establish a forward open list starting from the starting node and a reverse open list starting from the target node, defining a forward evaluation function that includes the environmental risk cost and a reverse evaluation function that does not include the environmental risk cost in the iterative search, and prioritizing the selection of nodes from the open list with smaller evaluation function values ​​for extension; and a selection module. When a search in one direction encounters a node already visited by a search in another direction, the node is identified as the initial intersection point. The total extension distance of the current path is recorded as an upper bound, and bidirectional expansion continues until either open list is empty or a predetermined number of steps are reached, searching for a better intersection point. Among all generated intersection points, the intersection point that minimizes the global total extension 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 generates an initial path based on the optimal intersection point and smooths the initial path to ensure that the terrain accessibility cost and environmental risk cost corresponding to any point on the smoothed path are within a preset safety threshold, thereby obtaining the path.

[0037] 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: 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;

[0038] The geometric distance is the Manhattan distance.

[0039] 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: 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 estimation from the current node n to the target node; The Euclidean distance between the current node and the target node is used; the back 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. The heuristically estimated distance from the current node n to the starting node; the reverse cumulative cost During the accumulation process, the cost of each step is calculated only based on the weighted average of geometric distance and terrain accessibility, without taking into account the environmental risk cost; The Euclidean distance between the current node and the starting node is used.

[0040] In some embodiments, the step of preferentially selecting nodes from the open list with smaller evaluation function values ​​for expansion includes: in each iteration, comparing the minimum evaluation function value in the forward open list with the minimum evaluation function value in the reverse open list after risk weight pre-compensation, and selecting nodes from the open list with smaller equivalent evaluation values ​​for expansion.

[0041] In some embodiments, determining the node as the initial junction point, recording the total expansion 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 to find a better junction point includes: setting the predetermined number of steps as a preset proportion of the total number of nodes on the map; after finding the initial junction point, using the total expansion distance of the junction point as the initial upper bound, and continuing the search; when a new junction point with a total expansion distance of the junction point less than the current upper bound is found, updating the upper bound and recording the new junction point.

[0042] 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.

[0043] 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: sampling the smoothed path with a preset step size; checking the cost of each sampling point; if the terrain accessibility cost or environmental risk cost of any sampling point exceeds the corresponding safety threshold, then adding a control point to the corresponding segment of the initial path and re-smoothing the path until all sampling points on the path meet the safety threshold.

[0044] 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 the search in one direction encounters a node that has been visited by the search in another direction, determine the node as the initial intersection point, record the total expansion distance of the current path as the upper bound, and continue to expand in both directions until either open list is empty or a predetermined number of steps are reached, and find a better intersection point. Among all generated intersection points, select the intersection point that minimizes the global total expansion distance calculated by concatenating the forward and reverse paths and re-including the environmental risk cost value of the reverse path as the optimal intersection point. Generate an initial path based on the optimal intersection point, and smooth the initial path to ensure that the terrain accessibility cost value and environmental risk cost value corresponding to any point on the smoothed path are within a preset safety threshold, thereby obtaining the path.

2. The method according to claim 1, characterized in that, The extended distance is based on geometric distance and is weighted by incorporating preset terrain accessibility value and environmental risk value, 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 preset terrain weight coefficients and risk weight coefficients, respectively; the geometric distance is the Manhattan distance.

3. The method according to claim 1, characterized in that, 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; The Euclidean distance between the current node and the target node is used; the back 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. Heuristic distance estimation from the current node n to the starting node; reverse cumulative cost During the accumulation process, the cost of each step is calculated only based on the weighted average of geometric distance and terrain accessibility, without taking into account the environmental risk cost; The Euclidean distance between the current node and the starting node is used.

4. 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, comparing the minimum evaluation function value in the forward open list with the minimum evaluation function value in the reverse open list after risk weight pre-compensation, and selecting nodes from the open list with smaller equivalent evaluation values ​​for expansion.

5. The method according to claim 1, characterized in that, The process of determining the node as the initial intersection point, recording the total expansion distance of the current path as the upper bound, and continuing to expand in both directions until any open list is empty or a predetermined number of steps is reached, and searching for a better intersection point, includes: setting the predetermined number of steps as a preset proportion of the total number of nodes on the map; after finding the initial intersection point, using the total expansion distance of the intersection point as the initial upper bound, and continuing the search; when a new intersection point with a total expansion distance of the intersection point less than the current upper bound is found, updating the upper bound and recording the new intersection point.

6. 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.

7. The method according to claim 1, characterized in that, Ensuring that the terrain accessibility cost and environmental risk cost of any point on the smoothed path are within a preset safety threshold includes: sampling the smoothed path with a preset step size; checking the cost of each sampling point; if the terrain accessibility cost or environmental risk cost of any sampling point exceeds the corresponding safety threshold, then adding control points to the corresponding section of the initial path and re-smoothing the path until all sampling points on the path meet the safety threshold.

8. A system for autonomous search path planning for rescue robots, characterized in that, It includes the following modules: a calculation module, 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 cost and environmental risk cost corresponding to any point on the smoothed path are within a preset safety threshold, thereby obtaining the path.

9. The system according to claim 8, characterized in that, The extended distance is based on geometric distance and is weighted by incorporating preset terrain accessibility value and environmental risk value, 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 preset terrain weight coefficients and risk weight coefficients, respectively; the geometric distance is the Manhattan distance.

10. The system according to claim 8, characterized in that, 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 estimation from the current node n to the target node; The Euclidean distance between the current node and the target node is used; the back 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. The heuristically estimated distance from the current node n to the starting node; the reverse cumulative cost During the accumulation process, the cost of each step is calculated only based on the weighted average of geometric distance and terrain accessibility, without taking into account the environmental risk cost; The Euclidean distance between the current node and the starting node is used.

Images