Heterogeneous agent collaborative underground space exploration method based on risk map search

Abstract

The application discloses a heterogeneous agent cooperative underground space exploration method based on a risk map search, acquires surrounding information under current positions of a heterogeneous agent group to construct a local map, and an aerial agent in the heterogeneous agent group constructs a first local risk grid map according to the local map and calculates risk values of each cell in the first local risk grid map; candidate points of the heterogeneous agent group starting from current positions are searched, a ground agent selects a next search point in the candidate points according to a search gain combined with the risk values, the aerial agent selects the next search point in the candidate points according to the search gain, after the aerial agent and the ground agent complete local space exploration according to a local planning path, global path planning of a global space is performed on the heterogeneous agent group according to a global exploration gain function, and the unknown region exploration is completed. The topographic risk map constructed by the UAV guides the UGV to avoid potential dangerous regions, and balances the search efficiency and safety of the heterogeneous cluster.

Description

Technical Field

[0001] This invention relates to heterogeneous unmanned swarm space exploration, specifically to a method for heterogeneous intelligent agents to collaboratively explore underground space based on risk graph search. Background Technology

[0002] Heterogeneous multi-agent systems composed of unmanned vehicles (UGVs) and unmanned aerial vehicles (UAVs) offer advantages such as rapid, wide-area reconnaissance and precise ground target localization. Due to the different dynamic characteristics of UAVs and UGVs, multi-agent systems combining them can leverage their respective strengths, such as good payload and mission configuration capabilities, and positioning capabilities, resulting in superior performance. As modern unmanned systems, UAVs and UGVs can replace humans in many difficult and dangerous tasks, and therefore have found widespread application in numerous fields.

[0003] To meet the needs of underground intelligent integrated utility tunnel construction and unmanned attack and defense in underground engineering, it is imperative to focus on developing autonomous collaborative exploration technology using multiple heterogeneous robots in irregular underground spaces. Due to the excellent characteristics of heterogeneous multi-agent systems, such as mobility and accessibility, UAVs and UGVs provide a promising platform, especially in unpredictable and hazardous situations. By leveraging the complementary cooperation among multiple heterogeneous multi-agent systems, such a coordinated network has the potential to cover larger spaces and accomplish search tasks with computational workloads exceeding the capabilities of any single agent. However, as the scenario shifts from above ground to underground, the space from open to narrow, and the configuration from homogeneous to heterogeneous, multi-heterogeneous robots face technical challenges such as satellite signal rejection, limited mobility, difficulties in precise control, and harsh detection conditions. Summary of the Invention

[0004] Purpose of the invention: The purpose of this invention is to provide a method for collaborative exploration of underground space by heterogeneous intelligent agents based on risk graph search, which balances the search efficiency and security of heterogeneous clusters.

[0005] Technical Solution: To solve the above problems, this invention adopts a heterogeneous intelligent agent collaborative underground space exploration method based on risk graph search, including the following steps:

[0006] Step 1: Perform local planning based on the local space within a preset range around the heterogeneous intelligent agent group, obtain the surrounding information of the current position of the heterogeneous intelligent agent group to construct a local map, and obtain the current motion state of the heterogeneous intelligent agent group. The heterogeneous intelligent agent group includes at least one aerial intelligent agent and at least one ground intelligent agent.

[0007] Step 2: The aerial agent in the heterogeneous intelligent agent group constructs a first local risk grid map based on the local map, and calculates the risk value of each cell in the first local risk grid map;

[0008] Step 3: Search the set of leading points of the heterogeneous intelligent agent group starting from the current position, and select the leading points that the ground intelligent agent can reach as candidate points of the ground intelligent agent. The ground intelligent agent selects the next search point from the candidate points according to the search gain combined with the risk value. In the set of leading points of the air intelligent agent, select the leading points within the maximum cooperative search range of the heterogeneous intelligent agent group centered on the next search point of the ground intelligent agent as candidate points of the air intelligent agent. The air intelligent agent selects the next search point from the candidate points according to the search gain, and obtains the local planning path of the air intelligent agent and the ground intelligent agent in the local planning.

[0009] Step 4: After the aerial and ground agents have completed their exploration of the local space according to the local planning path, the heterogeneous agent group performs global path planning for the global space according to the global exploration gain function. The global space includes several local spaces. The heterogeneous agent group performs local exploration of the next local space according to the global path planning until the exploration of the unknown area is completed.

[0010] Furthermore, in step 1, the aerial intelligent agent is a UAV (Unmanned Aerial Vehicle), and the ground intelligent agent is a UGV (Unmanned Vehicle). The 3D lidar-inertial navigation tightly coupled perception system on the UAV and UGV is used to acquire information about surrounding obstacles at the current position of the UAV and UGV and construct a local map. The lidar inertial odometry is realized by using a fast lidar-inertial odometry tightly coupled iterative extended Kalman filter method.

[0011] Furthermore, the risk value r of each cell d,i The calculation formula is:

[0012]

[0013] Among them, h d,i h represents the step height of the i-th cell in the first local risk grid map. crit The threshold of step height that a ground-based intelligent agent can withstand; s d,i Let s be the slope of the i-th cell in the first local risk grid map. crit ω is the threshold of the slope that a ground-based intelligent agent can withstand. d,h and ω d,s These are the weight parameters.

[0014] Furthermore, using the latest incremental region centered on the airborne agent as the search range, the RRG algorithm is used to search for leading points from the current position for the ground agent. These leading points are clustered using Euclidean distance, and the clustering results are filtered to obtain a new set of leading points. Leading points reachable by the ground agent from this set are selected as candidate points, and a reference path is calculated for each candidate point using Dijkstra's algorithm. The path cost in Dijkstra's algorithm is:

[0015]

[0016] Where g(i) is the path cost of the i-th cell in the reference path, R i Let |c| be the index of the i-th cell in the reference path, and n be the total number of cells in the reference path. k -c k+1 || 2 θ represents the shortest Euclidean path distance between two cells in the reference path, and θ is the relative weighting parameter between the distance penalty and the risk penalty.

[0017] The selection cost of each candidate point is calculated using a multi-factor cost function, which is as follows:

[0018]

[0019] Among them, R Nj The selection cost for choosing the j-th candidate point for the ground agent, d j Let r be the movement cost of a ground agent moving to the j-th candidate point. j λ is the risk cost of the j-th candidate point. d , λ r It is an adjustable parameter, and M is the total number of candidate points.

[0020] Furthermore, the risk cost is:

[0021]

[0022] Where, r js This indicates that the reference path to the j-th candidate point passes through the j-th... s The risk value of each cell, L j It is the length of the reference path for the j-th candidate point.

[0023] Furthermore, the search gain of the aerial agent is:

[0024]

[0025] The search gain of the ground agent is:

[0026]

[0027] Where, σ i For a given path, R j It is the choice cost calculated using a multi-factor cost function for the j-th path point on a given path, m. i The total number of path points on a given path. For a given path σ i The j-th path point to root path The cumulative Euclidean distance, and It is an adjustable factor. This refers to the volume of newly known spaces added during the exploration process.

[0028] Furthermore, B-spline functions are used to smooth the local planning paths of the aerial agent and the ground agent. The smoothed local planning path of the ground agent is discretized to obtain a series of control points. These control points are used as target points for the local planner. The local planner combines the target points with the risk values ​​calculated by the second local risk grid map for path replanning. The second local risk grid map is constructed by the ground agent based on the local map. The risk value r of each cell in the second local risk grid map is... g,i The calculation formula is:

[0029]

[0030] Among them, h g,i h represents the step height of the i-th cell in the second local risk grid map. crit The threshold of step height that a ground-based intelligent agent can withstand; s g,i Let s be the slope of the i-th cell in the second local risk grid map. crit The threshold of slope that a ground-based intelligent agent can withstand; u g,i Let u be the roughness of the i-th cell in the second local risk grid map. crit ω is the threshold of roughness that a ground-based intelligent agent can withstand. d,h ω d,s and ω g,u These are the weight parameters.

[0031] Furthermore, the search gain of global planning is:

[0032]

[0033] Where, N G,cur It is the vertex representing the current robot group configuration in the global graph. It is a set of potential frontiers that are updated in the global graph. It is the shortest path length from the current position to the frontier. This is an adjustable parameter.

[0034] The present invention also employs a computer device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the steps of the above method.

[0035] The present invention also requires a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the steps of the above method.

[0036] Beneficial Effects: Compared to existing technologies, the significant advantage of this invention is that UAVs can search areas inaccessible to UGVs and guide UGVs to avoid potentially hazardous areas through a constructed terrain risk map. This collaborative work improves the safety of UGVs searching in unknown underground environments, especially those with steep slopes and diverse geometries. By utilizing the collaborative work of UAVs and UGVs, safer environmental awareness and path planning can be provided in underground environments, improving search efficiency in complex, large-scale, and multi-branched underground environments. Attached Figure Description

[0037] Figure 1 This is a flowchart illustrating the space exploration method of the present invention. Detailed Implementation

[0038] like Figure 1 As shown in this embodiment, a heterogeneous agent cooperative space exploration method based on risk graph search is presented:

[0039] Step 1: In an unstructured underground space environment V within a finite three-dimensional space, divide the UGV and UAV into group I. Each group of heterogeneous robots contains at least one UAV and one UGV. Assume that within the time window t... k Allocate a spatial volume for each heterogeneous robot group i∈I Duration is The UAV and UGV are equipped with a 3D LiDAR-INS tightly coupled sensing system to obtain information about surrounding obstacles at their current positions and construct a local map. The LiDAR-based SLAM (Simultaneous Localization and Mapping) algorithm is then used to obtain the current motion state of the UAV and UGV.

[0040] By employing a tightly coupled 3D LiDAR-INS sensing system onboard UAVs and UGVs, information about surrounding obstacles at the current location of the UAVs and UGVs is acquired to construct a local map. A tightly coupled iterative extended Kalman filter method using Fast LiDAR-Inertial Odometry (FAST-LIO2) is employed to achieve high-efficiency and robust LiDAR-INS odometry. This method reduces reliance on high-dimensional representations through error state estimation and handles motion distortion using continuous and discrete IMU models and the concept of LiDAR frames. Positioning accuracy is improved through forward and backward propagation corrections, combined with residual calculation and iterative state updates.

[0041] Step 2: Combine map data with terrain risk factors such as elevation difference and slope angle to construct a risk grid map. Input laser point cloud data into the UAV to create a first local risk grid map and transmit it to the UGV. The UGV creates a second local risk grid map based on the received map data and the laser point cloud data it has collected.

[0042] During underground environmental topographic surveys, UAVs, due to their altitude, can obtain topographic data over a wider area, enabling the construction of a local risk grid map for the UAV. Let... A grid map representing UAVs. This is used to construct a first local risk grid map for UAVs. First, calculate each cell in the grid map. step height h d,i and slope s d,i Then, cell c d,i Risk value r d,i The design is as follows:

[0043]

[0044] Among them, h crit and s crit These are the threshold values ​​for step height and slope that UGV can withstand, ω d,h and ω d,s These are the weight parameters, and the sum of the weight parameters equals 1.

[0045] The radius and resolution of the UGV local map were set to smaller values ​​to ensure that the UGV could quickly and accurately describe the terrain information around it. This was done to construct a second local risk grid map for the autonomous vehicle. Each cell c g,i roughness u g,i The risk value r of the cell will also be calculated. g,i The design is as follows:

[0046]

[0047] Where, ω g,h ωg,s ω g,u The weight parameters are summed to 1 and determined based on the accessibility of the underground space of the UGV.

[0048] Step 3: Candidate frontier points are extracted by using the RRG (rapidly exploring random graph) algorithm and Euclidean clustering. The UAV selects the next search point based on the cost function of multiple factors such as distance and the search gain function. The UGV selects the next search point for local waypoints based on the cost function of multiple factors such as risk factors and distance and the search gain function. A safe travel path is generated by the local path planner.

[0049] (1) Frontier selection and path planning based on risk graph search

[0050] To ensure safe UGV search, the heterogeneous multi-agent cooperative search problem is defined as follows:

[0051] Let Ξ be the set of all collision-free connected sets. To use lidar The residual volume is represented by the set of all configurations of voxel m that can be perceived. For a group of UGVs and UAVs in space Find a collision-free path inside This path starts from the initial configuration. Starting from there, one can identify free space. and space occupied Until no collision-free configuration can be detected. Right now V free Defined as space on a map that is not occupied by objects, structures, or natural forms. V occ Defined as space on a map already occupied by objects, structures, or natural forms. A feasible path must follow the sensor model and applicable motion constraints, namely maximum yaw rate and drivability limits (for ground robots).

[0052] To meet the needs of safe search by UGVs in complex and large-scale unknown underground environments, a heuristic target point selection method based on the risk map is proposed to effectively reduce the time cost and path risk of UGVs while maximizing the search gain of both UAVs and UGVs.

[0053] To improve search efficiency, the search range of the local planner's forefront points is limited to the latest incremental region centered on the UAV. The forefront points originating from the root node are found using the RRG (rapidly exploring random graph) algorithm. Then, the forefront points are clustered using Euclidean distance, and the results are filtered to obtain the current new set N of forefront points. new .

[0054] Once a new set of frontier points is obtained, it is further examined whether these frontier points are reachable from the UGV. Reachable frontier points are then designated as candidates. A multi-factor cost function is designed to evaluate each candidate vertex N. j The selection cost for each candidate point is obtained.

[0055]

[0056] Where, λ d ,λ r It is an adjustable parameter, and M is the total number of leading edges. d j The movement cost of UGV is defined as follows:

[0057]

[0058] in, This shows the current location of the driverless car.

[0059] r j The risk cost is calculated using a UAV risk grid map. To ensure the selected target points are sufficiently safe for the UGV, a reference path is calculated for each candidate point. A risk-based geometry planner is executed, and the risk values ​​from the local map are incorporated into Dijkstra's algorithm to calculate the shortest distance between each front node and the root node. Each cell in Dijkstra's algorithm... The path cost g(i) is replaced with:

[0060]

[0061] Among them, ||c k -c k+1 || 2 It is the shortest Euclidean path distance between two cells, that is, the k-th cell c in the reference path. k With the (k+1)th cell c k+1 The shortest Euclidean path distance is between the two. θ is a relative weighting parameter between the distance penalty and the risk penalty; it is assigned a small value to ensure the path is sufficiently safe. Cells with a risk value greater than 0.5 are considered obstacles and are not passable during the planning process.

[0062] Therefore, the risk cost of each candidate point can be calculated as follows:

[0063]

[0064] Where, r js This indicates that the reference path to the j-th candidate point passes through the j-th... s The risk value of each cell, and L j It is the length of the reference path for the j-th candidate point.

[0065] For drones, the selection range of candidate points is limited to the maximum drone-drone cooperative range centered on the next exploration target point of the drone. This is achieved by filtering the candidate points to find the leading edge of the search gain, given the path σ. o ∈Σ L ,o=1…n and the set of vertices on the path Give the UAV local search gain ExplorationGain uav (σ o The design is as follows:

[0066]

[0067] in Defined as the volume of newly added known space during the exploration process. The definition is as follows:

[0068]

[0069] Where V new V is the volume of the known space at the current moment. old It is the known spatial volume at the previous moment.

[0070] Give the UGV local search gain ExplorationGain ugv (σ o The design is as follows:

[0071]

[0072] in It has an adjustable factor The weight function. From path σ i The top of the vertex To the root The cumulative Euclidean distance is obtained by searching using the conventional Dijkstra algorithm. The UAV selects relatively short trajectory combinations associated with higher gain, achieving a higher search rate. The UGV selects relatively safe trajectory combinations with higher gain, achieving a safer search.

[0073] (2) Reference path optimization based on local planner

[0074] Use B-spline functions to smooth the reference path. The general form of a B-spline function is as follows:

[0075]

[0076] in, Let k be the B-spline basis function of the i-th control point, and k be the degree of the B-spline. The B-spline curve is formed by a set of control points P0, P1, ..., P... n Defined, it can be calculated using the following formula:

[0077]

[0078] In formula (10), C(t) is a B-spline curve. These are basis functions, P0, P1, ..., P n These are control points. Using B-spline functions, paths with good curvature characteristics and smooth transitions can be generated to adapt to vehicle kinematics and terrain conditions, thus maintaining the vehicle's posture safely and stably. The smoothed reference path is then discretized to generate a series of control points. These control points are used as target points for the local planner on the UGV. The local planner combines the target points with terrain analysis results from the UGV's second local risk grid map, such as risk values, to perform trajectory replanning, flexibly avoiding risks.

[0079] Step 4: After completing Step 3, such as the current space volume There is no If it can be detected by UAV or UGV, then global path planning is performed according to the global search gain function to allocate the next spatial volume to the heterogeneous robot group;

[0080] Assume ν G,cur These are the vertices in the global graph that represent the current configuration of the heterogeneous agent group. It is a set of potential frontiers updated in the global graph. The search gain of the global planner is shown in Equation (11).

[0081]

[0082] in It is the shortest path length from the current position to the leading edge, and it is an adjustable parameter. Used to balance the effects of path length and volume gain on search gain.

[0083] Step 5: Repeat steps 1-4 to globally identify the free regions V in the remaining unmapped space. free Or occupying area V occ Until the entire unknown environment is successfully searched.

Claims

1. A method for collaborative underground space exploration of heterogeneous agents based on risk map search, characterized in that, Comprising the following steps: Step 1: local planning based on the local space within the preset range around the heterogeneous agent group, obtaining the surrounding information under the current position of the heterogeneous agent group to construct a local map, and obtaining the current motion state of the heterogeneous agent group, the heterogeneous agent group comprising at least one aerial agent and at least one ground agent; Step 2: the aerial agent in the heterogeneous agent group constructs a first local risk grid map according to the local map, and calculates the risk value of each cell in the first local risk grid map; Step 3: search for a set of front points of the heterogeneous agent group starting from the current position, and filter out the front points reachable by the ground agent as candidate points of the ground agent, the ground agent selects the next search point in the candidate points according to the search gain combined with the risk value, in the set of front points of the aerial agent, the front points within the maximum cooperative search range of the heterogeneous agent group centered on the next search point of the ground agent are filtered out as candidate points of the aerial agent, the aerial agent selects the next search point in the candidate points according to the search gain, and the local planning path of the aerial agent and the ground agent in the local planning is obtained; The search gain of the aerial agent is: The search gain of the ground agent is: in, For a given path, It is the first on the given path The selection cost of each path point is calculated using a multi-factor cost function. The total number of path points on a given path. For a given path The first Path points to root path The cumulative Euclidean distance, and It is an adjustable factor. The volume of newly known space added during the exploration process; Step 4: after the aerial agent and the ground agent explore the local space according to the local planning path, the heterogeneous agent group is globally path planned in the global space according to a global exploration gain function, the global space comprises a plurality of local spaces, and the heterogeneous agent group performs local exploration in the next local space according to the global path planning until the unknown area exploration is completed; The global exploration gain function is: wherein, is a vertex in the global graph representing the current robot group configuration, is a set of potential frontiers updated in the global graph, is the shortest path length from the current position to the frontier, is a tunable parameter.

2. The heterogeneous agent collaborative underground space exploration method according to claim 1, characterized in that, The aerial agent in step 1 is a UAV, and the ground agent is a UGV, the 3D laser radar-inertial navigation tightly coupled perception system carried by the UAV and the UGV is used to obtain the surrounding obstacle information under the current position of the UAV and the UGV and construct a local map, and a fast laser radar-inertial odometer tightly coupled iterative extended Kalman filter method is used to realize laser radar inertial odometer.

3. The heterogeneous agent collaborative underground space exploration method according to claim 1, characterized in that, a risk value for each cell The formula for calculating the risk value is: wherein, is a step height of the i-th cell in the first local risk grid map, is a step height of the i-th cell in the first local risk grid map, is a threshold of a step height that can be tolerated by the ground agent; is a slope of the i-th cell in the first local risk grid map, is a slope of the i-th cell in the first local risk grid map, is a threshold of a slope that can be tolerated by the ground agent, and is a weight parameter.

4. The heterogeneous agent collaborative underground space exploration method according to claim 1, characterized in that, The newly increased area centered on the aerial agent is used as the search range, the RRG algorithm is used to search for the front points starting from the current position of the ground agent, the front points are clustered through Euclidean distance, and the clustering results are filtered to obtain a new set of front points; the front points reachable by the ground agent in the front point set are used as candidate points, and a reference path is calculated for each candidate point through the Dijkstra algorithm; the path cost in the Dijkstra algorithm is: in, For the reference path The path cost of each cell. For the reference path The index of each cell. This represents the total number of cells in the reference path. This refers to the shortest Euclidean path distance between two cells in the reference path. It is the relative weighting parameter between distance penalty and risk penalty; The selection cost of each candidate point is calculated through a multi-factor cost function, and the multi-factor cost function is: in, Selecting the first for ground-based intelligent agents The cost of selecting each candidate point For the ground-based intelligent agent to move to the first The cost of moving each candidate point It is the first The risk cost of each candidate point It is an adjustable parameter. It represents the total number of candidate points.

5. The heterogeneous agent collaborative underground space exploration method according to claim 4, characterized in that, The risk cost is: in, Indicates the first The reference path of the candidate point passes through the first... Risk value of each cell It is the first The length of the reference path for each candidate point.

6. The heterogeneous agent collaborative underground space exploration method according to claim 5, characterized in that, In step 3, the local planning path of the aerial agent and the ground agent is smoothed using a B-spline function, the smoothed local planning path of the ground agent is discretized to obtain a series of control points, the control points are taken as target points of the local planner, and the local planner re-plans the path by combining the target points with risk values calculated by a second local risk grid map of the ground agent according to a local map, wherein the risk value of each cell in the second local risk grid map is calculated according to the following formula: Risk value of each cell in the second local risk grid map = (1 - (distance between the target point and the cell) / (maximum distance between the target point and the cell)) * (1 - (risk value of the cell in the first local risk grid map) / (maximum risk value of the cell in the first local risk grid map)) wherein, is a step height of the i-th cell in the second local risk grid map, is a slope of the i-th cell in the second local risk grid map, is a threshold of the slope that the ground agent can withstand, is a roughness of the i-th cell in the second local risk grid map, is a threshold of the roughness that the ground agent can withstand, is a step height of the i-th cell in the second local risk grid map, is a slope of the i-th cell in the second local risk grid map, is a threshold of the slope that the ground agent can withstand, is a roughness of the i-th cell in the second local risk grid map, , and are weight parameters.

7. A computer device comprising a memory, a processor, and a computer program stored on the memory and executable on the processor, characterized in that, The processor executes the computer program to realize the steps of the method of any one of claims 1 to 6.

8. A computer readable storage medium having stored thereon a computer program, characterized in that, The computer program is executed by the processor to realize the steps of the method of any one of claims 1 to 6.

Images