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

Abstract

The invention discloses a risk map search-based heterogeneous agent collaborative underground space exploration method, which comprises the following steps of: acquiring peripheral information of a heterogeneous agent group at a current position to construct a local map, and constructing a first local risk grid map by aerial agents in the heterogeneous agent group according to the local map, calculating a risk value of each cell in the first local risk grid map; searching candidate points, starting from the current position, of the heterogeneous agent group, selecting a next search point from the candidate points by the ground agent according to a search gain combined with a risk value, and selecting a next search point from the candidate points by the aerial agent according to the search gain; and after the aerial intelligent agent and the ground intelligent agent complete the exploration of the local space according to the local planning path, performing global path planning of the global space on the heterogeneous intelligent agent group according to the global exploration gain function until the exploration of the unknown area is completed. And guiding the UGV to avoid potential dangerous areas through a terrain risk map constructed by the UAV to balance heterogeneous cluster search efficiency and safety.

Description

Technical Field

[0001] The present invention relates to heterogeneous unmanned cluster space exploration, and in particular to a heterogeneous intelligent agent collaborative underground space exploration method based on risk graph search. Background Art

[0002] The heterogeneous multi-agent system composed of unmanned vehicles (UGV) and unmanned aerial vehicles (UAV) has the advantages of large-scale rapid reconnaissance and accurate positioning of ground targets. Due to the different dynamic characteristics of UAV and UGV, the multi-agent system composed of the two can combine their respective advantages, such as good payload and task configuration capabilities, as well as positioning capabilities, to provide better performance. As modern unmanned system equipment, UAV and UGV can replace humans to complete many difficult and dangerous tasks, and therefore have been widely used in many fields.

[0003] In order to meet the needs of underground smart integrated pipe corridor facility construction and unmanned attack and defense confrontation in underground projects, it is imperative to focus on underground irregular spaces and carry out multi-heterogeneous robot autonomous collaborative detection technology. Due to the excellent characteristics of heterogeneous multi-agent systems, such as mobility and accessibility, UAV and UGV provide a promising platform, especially in unpredictable and dangerous situations. By leveraging the complementary cooperation between multiple heterogeneous multi-agent systems, such a coordinated network has the potential to cover a larger space and complete search tasks whose computational workload exceeds the capabilities of any single agent. However, as the scene shifts from above ground to underground, the space changes from open to narrow, and the configuration changes from homogeneous to heterogeneous, multi-heterogeneous robots face technical challenges such as satellite signal denial, limited mobility, difficulty in precise control, and harsh detection conditions. Summary of the invention

[0004] Purpose of the invention: The purpose of the present invention is to provide a risk graph search-based heterogeneous agent collaborative underground space exploration method that balances the efficiency and safety of heterogeneous cluster search.

[0005] Technical solution: To solve the above problems, the present invention adopts a risk graph search-based heterogeneous intelligent agent collaborative underground space exploration method, which includes the following steps:

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

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

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

[0009] Step 4: After the aerial agent and the ground agent complete the exploration of the local space according to the local planning path, the global path planning of the global space is performed for the heterogeneous agent group 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, the aerial agent in step 1 is an unmanned aerial vehicle (UAV), and the ground agent is an unmanned vehicle (UGV). Through the 3D lidar-inertial navigation tightly coupled perception system carried by the UAV and UGV, the surrounding obstacle information at the current position of the UAV and UGV is obtained and a local map is constructed. The lidar inertial odometer is realized by using the iterative extended Kalman filtering method with fast lidar-inertial odometer tight coupling.

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

[0012]

[0013] Among them, h d,i is the step height of the ith cell in the first local risk grid map, h crit is the threshold of the step height that the ground agent can bear; s d,i is the slope of the ith cell in the first local risk grid map, s crit is the threshold of the slope that the ground agent can withstand, ω d,h and ω d,s is the weight parameter.

[0014] Furthermore, the latest incremental area centered on the aerial agent is used as the search range, and the RRG algorithm is used to search for the frontier point starting from the current position for the ground agent. The frontier points are clustered by Euclidean distance, and the clustering results are filtered to obtain the current new frontier point set; the frontier points that can be reached by the ground agent in the frontier point set are taken as candidate points, and a reference path is calculated for each candidate point by the Dijkstra algorithm; the path cost in the Dijkstra algorithm is:

[0015]

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

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

[0018]

[0019] Among them, R Nj The selection cost for the ground agent to select the jth candidate point, d j is the moving cost of the ground agent to move to the jth candidate point, r j is the risk cost of the jth candidate point, λ d , r is an adjustable parameter and M is the total number of candidate points.

[0020] Furthermore, the risk cost is:

[0021]

[0022] Among them, r js The reference path of the j-th candidate point passes through the j-th s The risk value of each cell, L j is the length of the reference path of the jth candidate point.

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

[0024]

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

[0026]

[0027] Among them, σ i For a given path, R j is the selection cost of the jth path point on a given path calculated by the multi-factor cost function, m i is the total number of path points on a given path, For a given path σ i The jth path point on To the root path point The cumulative Euclidean distance of and is the adjustable factor, Adds the volume of known space to the exploration process.

[0028] Furthermore, the local planning paths of the aerial agent and the ground agent are smoothed using a B-spline function, the local planning path smoothed by the ground agent is discretized to obtain a series of control points, the control points are used as target points of the local planner, and the target points of the local planner are combined with the risk values ​​calculated by the second local risk grid map to perform path replanning. The second local risk grid map is constructed by the ground agent according to the local map, and 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 is the step height of the ith cell in the second local risk grid map, h crit is the threshold of the step height that the ground agent can bear; s g,i is the slope of the ith cell in the second local risk grid map, s crit is the threshold of the slope that the ground agent can withstand; u g,i is the roughness of the ith cell in the second local risk grid map, u crit is the roughness threshold that the ground agent can withstand, ω d,h ,ω d,s and ω g,u is the weight parameter.

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

[0032]

[0033] Among them, N G,cur is the 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, It is an adjustable parameter.

[0034] The present invention also adopts a computer device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor implements the steps of the above method when executing the computer program.

[0035] The present invention also requires a computer-readable storage medium on which a computer program is stored. When the computer program is executed by a processor, the steps of the above method are implemented.

[0036] Beneficial Effects: Compared with the prior art, the significant advantage of the present invention is that the UAV can search in areas that the UGV cannot directly reach, and guide the UGV to avoid potential dangerous areas through the constructed terrain risk map. This collaborative work improves the search safety of the UGV in unknown underground environments, especially in environments with steep slopes and diverse geometric forms. By utilizing the collaborative work of UAV and UGV, safer environmental perception and path planning can be provided in underground environments, improving the search efficiency in complex, large-scale and multi-branched underground environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0037] Figure 1 It is a flow chart of the space exploration method of the present invention. DETAILED DESCRIPTION

[0038] like Figure 1 As shown, in this embodiment, a method for collaborative space exploration of heterogeneous agents based on risk graph search is as follows:

[0039] Step 1: In an unstructured underground space environment V in a limited three-dimensional space, UGVs and UAVs are divided into I groups. Each group of heterogeneous robots contains at least one UAV and one UGV. Assume that in the time window t k , assign a spatial volume to each heterogeneous robot group i∈I Duration is Through the 3D lidar-inertial navigation tightly coupled perception system carried by UAV and UGV, the surrounding obstacle information of the current position of UAV and UGV is obtained and a local map is constructed. The current motion status of UAV and UGV is obtained by using the lidar-based SLAM (simultaneous localization and mapping) algorithm.

[0040] Through the tightly coupled 3D lidar-inertial navigation perception system carried by UAV and UGV, the surrounding obstacle information of the current position of UAV and UGV is obtained and a local map is constructed. The iterative extended Kalman filter method of the fast lidar-inertial odometer (FAST-LIO2) tightly coupled method is used to realize a highly efficient and robust lidar-inertial odometer. This method reduces the dependence on high-dimensional representation through error state estimation, and uses the concept of IMU continuous and discrete models and lidar frames to handle motion distortion. Through forward propagation and backward propagation correction, combined with residual calculation and iterative state update, the positioning accuracy is improved.

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

[0042] When UAV is measuring underground terrain, it can obtain terrain data of a wider range due to its altitude, and build a UAV local risk grid map. Grid map representing UAVs. To construct the first local risk grid map of UAVs First calculate each cell in the grid map Step height h d,i and slope s d,i , then, cell c d,i The risk value r d,i The design is as follows:

[0043]

[0044] Among them, h crit and crit are the step height and slope thresholds that the UGV can withstand, ω d,h and ω d,s is the weight parameter, and the sum of the weight parameters is equal to 1.

[0045] The UGV local map radius and resolution are set to smaller values ​​to ensure that the UGV can quickly and accurately describe the terrain information around it. Each cell c g,i The roughness u g,i The risk value of the cell r will also be calculated. g,i The design is as follows:

[0046]

[0047] Among them, ω g,h ,ωg,s ,ω g,u is a weight parameter, the sum of which is equal to 1 and is determined according to the underground space passability of the UGV.

[0048] Step 3: By using the RRG algorithm (rapidly exploring random graph) search and Euclidean clustering to extract candidate frontier points, the UAV selects the next search point based on the multi-factor cost function such as the distance factor and the search gain function, and the UGV selects the next search point for the local waypoint based on the multi-factor cost function such as the risk factor and the distance factor and the search gain function, and the local path planner generates a safe travel path.

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

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

[0051] Let Ξ be the set of all collision-free connected entities, To use laser radar The residual volume is expressed as the set of all configurations of voxel m that can be sensed. For a group of UGV and UAV in space Find a collision-free path within This path is from the initial configuration Starting from, free space can be identified and occupied space Until no collision-free configuration can be sensed Right now V free Defined as the space on the map that is not occupied by objects, structures, or natural forms. occ Defined as the space on the map that is already occupied by objects, structures, or natural forms. A feasible path must obey the sensor model, as well as applicable motion constraints, i.e. maximum yaw rate and traversability limits (for ground robots).

[0052] In order to meet the needs of safe search of UGV in complex and large-scale unknown underground environments, a heuristic target point selection method based on risk map is proposed based on the local path point selection in the UAV risk map to effectively reduce the time cost and path risk of UGV and maximize the search gain of UAV and UGV.

[0053] In order to improve the search efficiency, the search range of the local planner frontier point is limited to the latest incremental area centered on the UAV. The frontier point starting from the root node is found by searching using the RRG algorithm (rapidly exploring random graph). The frontier points are then clustered by Euclidean distance and the results are filtered to obtain the current new frontier point set N new .

[0054] After obtaining a new set of frontier points, we further check whether these frontier points are reachable to the UGV, and the reachable frontier points will be used as candidates. At the same time, a multi-factor cost function is designed to evaluate each candidate vertex N. j , and obtain the selection cost of each candidate point.

[0055]

[0056] Among them, λ d ,λ r is an adjustable parameter and M is the total number of frontiers. j is the movement cost of the UGV, defined as follows:

[0057]

[0058] in, is the current position of the unmanned vehicle.

[0059] r j is the risk cost calculated by the UAV risk grid map. In order to ensure that the selected target point is safe enough for the UGV, a reference path is calculated for each candidate point. The risk-based geometric planner is executed, and the risk value of the local map is introduced into the Dijkstra algorithm to calculate the shortest distance between each frontier node and the root node. Each cell in the Dijkstra algorithm The path cost g(i) is replaced by:

[0060]

[0061] Among them, ||c k -c k+1 || 2 is the shortest Euclidean path distance between two cells, i.e., the kth cell c in the reference path k With the k+1th cell c k+1 The Euclidean path between them has the shortest distance. θ is the relative weight parameter between the distance penalty and the risk penalty, which is given a small value to ensure that the path is safe enough. 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:

[0063]

[0064] Among them, r js The reference path of the j-th candidate point passes through the j-th s The risk value of each cell, and L j is the length of the reference path of the jth candidate point.

[0065] For the UAV, the selection range of candidate points is limited to the maximum UAV-UAV collaborative range centered on the next exploration target point of the UAV. By screening the maximum search gain frontier point among the candidate points, a given path σ o ∈Σ L , o = 1…n and the set of vertices on the path Give the UAV local search gain ExplorationGain uav (σ o ) is designed as follows:

[0066]

[0067] in It is defined as the volume of newly known space added during the exploration process. It is defined as follows:

[0068]

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

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

[0071]

[0072] in With adjustable factors The weight function of . is from the path σ i Vertex on To the root The cumulative Euclidean distance is obtained by searching with the conventional Dijkstra algorithm. The UAV selects a relatively short trajectory combination associated with a higher gain, which can achieve a higher search rate. The UGV selects a relatively safe trajectory combination with a higher gain, which can achieve a safer search.

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

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

[0075]

[0076] in, is the B-spline basis function of the ith control point, k is the degree of the B-spline. A B-spline curve is a set of control points P0, P1, …, P n Defined, it can be calculated by the following formula:

[0077]

[0078] In formula (10), C(t) is the B-spline curve, are basis functions, P0, P1, …, P n are control points. Using the B-spline function, a path with good curvature characteristics and smooth changes can be generated to adapt to vehicle kinematics and terrain conditions, so that the vehicle body posture remains safe and stable. 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 the terrain analysis results of the UGV's second local risk grid map, such as the risk value, to replan the trajectory and flexibly avoid risks.

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

[0080] Assume that G,cur is the vertex in the global graph representing the current heterogeneous agent group configuration, 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 is the shortest path length from the current position to the frontier point, and the 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 area V in the remaining unmapped space free Or occupied area V occ , until the entire unknown environment is successfully searched.

Claims

1. A method for collaborative underground space exploration of heterogeneous intelligent agents based on risk graph search, characterized in that: The following steps are involved: Step 1: Perform local planning based on the local space within a preset range around the heterogeneous agent group, obtain the surrounding information of the current position of the heterogeneous agent group to build a local map, and obtain the current motion state of the heterogeneous agent group, wherein the heterogeneous agent group includes 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 the frontier point set of the heterogeneous agent group starting from the current position, and select the frontier points that the ground agent can reach as the candidate points of the ground agent. The ground agent selects the next search point from the candidate points according to the search gain combined with the risk value. In the frontier point set of the aerial agent, select the frontier points within the maximum collaborative search range of the heterogeneous agent group centered on the next search point of the ground agent as the candidate points of the aerial agent. The aerial agent selects the next search point from the candidate points according to the search gain, and obtains the local planning path of the aerial agent and the ground agent in the local planning. Step 4: After the aerial agent and the ground agent complete the exploration of the local space according to the local planning path, the global path planning of the global space is performed for the heterogeneous agent group 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.

2. The method for exploring underground space by collaborative heterogeneous agents according to claim 1, characterized in that: The aerial agent in step 1 is an unmanned aerial vehicle (UAV), and the ground agent is an unmanned vehicle (UGV). The 3D lidar-inertial navigation tightly coupled perception system carried by the UAV and UGV is used to obtain the surrounding obstacle information at the current position of the UAV and UGV and build a local map. The lidar-inertial odometer is realized by using the iterative extended Kalman filtering method with fast lidar-inertial odometer tight coupling.

3. The method for exploring underground space by collaborative heterogeneous agents according to claim 1, characterized in that: The risk value r of each cell d,i The calculation formula is: Among them, h d,i is the step height of the ith cell in the first local risk grid map, h crit is the step height threshold that the ground agent can bear; s d,i is the slope of the ith cell in the first local risk grid map, s crit is the threshold of the slope that the ground agent can withstand, ω d,h and ω d,s is the weight parameter.

4. The method for exploring underground space by collaborative heterogeneous agents according to claim 1, characterized in that: The latest incremental area centered on the aerial agent is used as the search range. The RRG algorithm is used to search for the frontier point starting from the current position for the ground agent. The frontier points are clustered by Euclidean distance, and the clustering results are filtered to obtain the current new frontier point set. The frontier points that can be reached by the ground agent in the frontier point set are taken as candidate points, and a reference path is calculated for each candidate point by the Dijkstra algorithm. The path cost in the Dijkstra algorithm is: Where g(i) is the path cost of the i-th cell in the reference path, R i is the index of the i-th cell in the reference path, n is the total number of cells in the reference path, ||c k -c k+1 || 2 is the shortest Euclidean path distance between two cells in the reference path, θ is the relative weight parameter between distance penalty and risk penalty; The selection cost of each candidate point is calculated by a multi-factor cost function, which is: in, The selection cost for the ground agent to select the jth candidate point, d j is the moving cost of the ground agent to move to the jth candidate point, r j is the risk cost of the jth candidate point, λ d , r is an adjustable parameter and M is the total number of candidate points.

5. The method for exploring underground space in collaboration with heterogeneous agents according to claim 4 is characterized in that: The risk cost is: Among them, r js The reference path of the j-th candidate point passes through the j-th s The risk value of each cell, L j is the length of the reference path of the jth candidate point.

6. The method for exploring underground space in collaboration with heterogeneous agents according to claim 5, characterized in that: The search gain of the aerial agent is: The search gain of the ground agent is: Among them, σ i For a given path, R j is the selection cost of the jth path point on a given path calculated by the multi-factor cost function, m i is the total number of path points on a given path, For a given path σ i The jth path point on To the root path point The cumulative Euclidean distance of and is the adjustable factor, Adds the volume of known space to the exploration process.

7. The method for exploring underground space in collaboration with heterogeneous agents according to claim 6, characterized in that: In the step 3, the local planning paths of the aerial agent and the ground agent are smoothed using a B-spline function, the local planning path smoothed by the ground agent is discretized to obtain a series of control points, the control points are used as target points of the local planner, and the local planner combines the target points with the risk value calculated by the second local risk grid map to perform path replanning, the second local risk grid map is constructed by the ground agent according to the local map, and the risk value r of each cell in the second local risk grid map is g,i The calculation formula is: Among them, h g,i is the step height of the ith cell in the second local risk grid map, h crit is the step height threshold that the ground agent can bear; s g,i is the slope of the ith cell in the second local risk grid map, s crit is the threshold of the slope that the ground agent can withstand; u g,i is the roughness of the ith cell in the second local risk grid map, u crit is the roughness threshold that the ground agent can withstand, ω d,h ,ω d,s and ω g,u is the weight parameter.

8. The method for exploring underground space in collaboration with heterogeneous agents according to claim 7, characterized in that: The search gain of global planning is: Among them, N G,cur is the 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, It is an adjustable parameter.

9. A computer device comprising a memory, a processor and a computer program stored in the memory and executable on the processor, characterized in that: When the processor executes the computer program, the steps of the method according to claim 1 to claim 8 are implemented.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the steps of the method according to claim 1 to claim 8 are implemented.

Images