Multi-unmanned vehicle collaborative exploration system in unknown environment

Abstract

A multi-unmanned-vehicle collaborative exploration system in an unknown environment comprises a plurality of unmanned vehicles carrying radio stations, laser radars and inertial measurement units, each unmanned vehicle generates a three-dimensional occupied grid map of a surrounding environment through synchronous positioning and map construction, grids scanned by the laser radars are marked as known voxels, and the known voxels are marked as known voxels. The voxels which are not scanned are marked as unknown voxels; a hierarchical H grid is constructed based on a three-dimensional occupied grid map, a preset maximum exploration area is divided into multiple layers of grids, and the cell size of each layer of grids is decreased progressively according to the hierarchy. The invention aims to provide the method which can quickly plan actions to respond to environmental changes, so that the system can flexibly navigate and collect information, and meanwhile, collision with unknown obstacles and other agents in the system is avoided; the multi-unmanned-vehicle collaborative exploration system under the unknown environment can explore the complete unknown environment in a task area in a faster, more robust, more coordinated and higher error-tolerant rate mode.

Description

Technical Field

[0001] The present invention relates to a multi-unmanned vehicle collaborative exploration system in an unknown environment. Background Art

[0002] Autonomous exploration using unmanned platforms to obtain map information of unknown environments is a technical challenge faced by various robots such as reconnaissance, rescue, and search robots. The autonomous exploration based on unmanned vehicles aims to plan a continuous, smooth, and vehicle-dynamics-constrained motion trajectory to achieve a more efficient and complete perception coverage of the unknown environment. Compared with using a single unmanned vehicle for exploration, using multiple unmanned vehicles can not only complete the exploration faster but also has a higher fault tolerance than a single unmanned vehicle, that is, using multiple unmanned vehicles for exploration is faster and more robust (the ability of a system or algorithm to maintain its performance and stability in the face of uncertainties and anomalies such as input errors, environmental changes, noise interference, and parameter changes). Robustness, derived from the English word Robust, means robust and strong. In computer science and related fields, robustness specifically refers to the ability of a system or algorithm to maintain its performance and stability in the face of uncertainties and anomalies such as input errors, environmental changes, noise interference, and parameter changes. In short, it means that the system or algorithm has the ability to "resist strikes". It has the advantage of a higher fault tolerance rate.

[0003] Methods for coordinating multiple unmanned vehicles usually rely on a central controller or require reliable communication between each unmanned vehicle. Unfortunately, unreliable and limited-range communication is common in practice, which makes the coordination between each unmanned vehicle fragile and inefficient. To improve the flexibility and robustness of the system, a decentralized coordination method with fewer communication requirements is needed.

[0004] Existing multi-unmanned vehicle exploration methods only consider the allocation of boundaries or viewpoints. Since the actual areas explored by each unmanned vehicle are not considered, these strategies usually lead to mutual interference between each unmanned vehicle and unfair workload distribution. In addition, since the global coverage route of the unknown space is not considered, each unmanned vehicle may repeatedly visit the same area, thus significantly reducing the overall efficiency. To fully utilize the potential of the system, more complex cooperation strategies are needed. Summary of the Invention

[0005] The purpose of the present invention is to provide a decentralized coordination method with fewer communication requirements that can quickly plan actions to respond to environmental changes, enabling it to flexibly navigate and collect information while avoiding collisions with previously unknown obstacles and other agents in the system, and realizing a multi-unmanned vehicle collaborative exploration system in an unknown environment that can explore the entire unknown environment faster, more robustly, more coordinately, and with a higher fault tolerance rate in the task area of the multi-unmanned vehicle system.

[0006] The multi-unmanned vehicle collaborative exploration system in an unknown environment of the present invention comprises the following steps:

[0007] A. It includes multiple unmanned vehicles equipped with radio stations, lidar, and inertial measurement units (IMUs). Each unmanned vehicle generates a three-dimensional occupancy grid map of the surrounding environment through simultaneous localization and mapping (SLAM). Among them, the grid cells scanned by the lidar are marked as known voxels, and those not scanned are marked as unknown voxels;

[0008] B. Based on the three-dimensional occupancy grid map, a hierarchical H grid is constructed. The preset maximum exploration area is divided into multiple layers of grids, and the size of each grid cell decreases hierarchically; record the number of unknown voxels, central coordinates, and index values in each grid cell;

[0009] C. Dynamically update the H grid. When the proportion of unknown voxels in a grid cell exceeds 70% and does not reach the highest level, it is subdivided into the next level; if the proportion of unknown voxels in the grid cell of the highest level is less than 10% or the path is inaccessible, then delete the grid cell;

[0010] D. The unmanned vehicles interact pairwise to overcome limited communication and coordinate their movements. Each unmanned vehicle broadcasts a communication data packet containing the grid cell index, interaction timestamp, current position, and partition request through the radio station; the receiving party verifies the timeliness of the timestamp. If there is a conflict, it rejects the interaction and triggers re-pairing;

[0011] E. Reallocate grid cells based on the vehicle routing problem with capacity constraints. Create virtual warehouse nodes for two unmanned vehicles that have successfully interacted, and generate a global coverage path for the two vehicles using an asymmetric cost matrix and capacity constraints;

[0012] F. Solve the optimal local trajectory of each unmanned vehicle based on the time elastic band algorithm (TEB). Discretize the global coverage path into a pose sequence with timestamps, initialize the time elastic band nodes, and solve the trajectory that satisfies the constraints and minimizes the total time consumption through the g2o framework;

[0013] G. Convert the optimized trajectory into motor speed and front wheel steering angle commands through a PID controller. Each unmanned vehicle independently executes the trajectory and shares position and H grid update information through the radio station to achieve collaborative obstacle avoidance.

[0014] In the multi-unmanned vehicle collaborative exploration system in an unknown environment of the present invention, in step A, each unmanned vehicle respectively generates a three-dimensional occupancy grid map of the surrounding environment of each unmanned vehicle through simultaneous localization and mapping (SLAM);

[0015] In step B, the H grid divides the pre-set maximum exploration area into multiple levels. Each level consists of multiple grid cells, and the size of the smallest grid cell is an integer multiple of the lidar resolution. For each grid cell, record the number of unknown voxels it contains, the center coordinates of all unknown voxels within the grid cell, and the grid cell index value;

[0016] In step C, it also includes storing the grid cells of the H grid using a linearized array and accelerating the retrieval and division of grid cells through an axis-aligned bounding box (AABB);

[0017] In step D, each unmanned vehicle packs the grid cell index value, interaction timestamp, current position information, and the latest partition request data into a communication data packet through a radio station and then broadcasts it; each unmanned vehicle maintains an interaction timestamp list. When each unmanned vehicle initiates an interaction request through radio station communication, it attaches a timestamp encryption verification. The receiving party confirms the timeliness of the request through a double-check mechanism. If a task conflict is detected, the interaction is rejected, and the unmanned vehicle with the task conflict is triggered to re-pair;

[0018] In step E, reallocate the grid cells based on the capacitated vehicle routing problem (CVRP). Create virtual warehouse nodes for each pair of interacting unmanned vehicles, use an asymmetric cost matrix to minimize the total length of the covering path, add capacity constraint conditions for each pair of interacting unmanned vehicles, and finally construct a capacitated vehicle routing problem. Solve the capacitated vehicle routing problem to obtain the optimal global covering path;

[0019] Embed a sparse graph in the hierarchical H grid to perform path search. The nodes of the sparse graph are the current positions of each unmanned vehicle and the center coordinates of each updated grid cell. The cost between the nodes of the sparse graph is the length of the collision-free path searched by the standard path search algorithm between the nodes in the three-dimensional occupancy grid map;

[0020] In step F, the global covering path is discretized into a pose sequence with timestamps, and the time elastic band (TEB) nodes are initialized;

[0021] Fuse the constraints through a hypergraph optimization model. Set the variables to be optimized, such as the discrete vehicle poses, the time differences between discrete vehicle poses, and the possible colliding obstacles, as the nodes of the hypergraph. Set the constraint conditions, such as vehicle kinematic constraints, maximum curvature constraints, maximum linear velocity constraints, maximum angular velocity constraints, maximum acceleration constraints, and distance constraints from obstacles, as the edges of the hypergraph;

[0022] The vehicle kinematic constraint is constructed based on the Ackerman front-wheel drive and rear-wheel drive model. The vehicle kinematic constraint is:

[0023] (1)

[0024] In formula (1) represents the vehicle poses at the (k + 1)-th and k-th moments, represents the vehicle heading angles at the (k + 1)-th and k-th moments, represents the vehicle pose change vector between the (k + 1)-th and k-th moments;

[0025] The maximum curvature constraint is:

[0026] (2)

[0027] In formula (2) represents the maximum curvature limit of the vehicle, which is determined according to the maximum steering angle of the vehicle, ;

[0028] The maximum speed (including the linear speed and the angular speed ) constraint is:

[0029] (3)

[0030] In formula (3) and represent the maximum linear speed and maximum angular speed when the vehicle is traveling, represents the time difference between the (k + 1)-th and k-th moments, , represents the direction of the linear speed, positive for forward and negative for backward, and the angular speed ;

[0031] The maximum acceleration constraint is:

[0032] (4)

[0033] In formula (4) represents the maximum acceleration when the vehicle is traveling, represents the vehicle pose at the (k + 2)-th moment, represents the time difference between the (k + 2)-th and (k + 1)-th moments, and the acceleration ;

[0034] The distance constraint from the obstacle is: Use the KDTree algorithm to find the trajectory points within a certain range of the obstacle in the entire global coverage path and the corresponding obstacles , and use the scalar to constrain their distances. The constraint inequality is as follows:

[0035] (5)

[0036] In formula (5), represents the minimum distance from the pose to the obstacle The minimum distance represents the minimum distance from the obstacle that the vehicle can safely travel;

[0037] The hypergraph optimization model is solved through the g2o framework, and the optimization goal is to minimize the total time consumption of the trajectory under all constraint conditions.

[0038] For the multi-unmanned vehicle collaborative exploration system in an unknown environment of the present invention, in the H-grid update information in step C, when the grid cell is subdivided, the central coordinates of the unknown voxels of the sub-grid cells are calculated by the grid clustering algorithm, and the index of each grid cell is determined by the following formula:

[0039] ;

[0040] where respectively represent the integer coordinates of the grid cell at the level in the direction, represents the number of grid cells in the three directions of the space, represents the sum of the number of cells from the 0th level to the level, so as to determine the unique index of each level of cell;

[0041] After the grid cell is deleted in step C, its parent grid cell needs to re-evaluate the proportion of unknown voxels, triggering a hierarchical fallback mechanism;

[0042] The axis-aligned bounding box (AABB) in step C is composed of the maximum rectangular bounding box aligned axially with the area explored by the unmanned vehicle itself and the shared area of nearby unmanned vehicles.

[0043] For the multi-unmanned vehicle collaborative exploration system in an unknown environment of the present invention, in step C, the protocol of the radio communication adopts the TDMA time division multiple access mechanism. Each unmanned vehicle is assigned a fixed time slot to broadcast data. The communication data packet contains a CRC check code and a sequence number. When the packet loss rate exceeds 20%, it switches to the redundant frequency band.

[0044] For the multi-unmanned vehicle collaborative exploration system in an unknown environment of the present invention, in step E, the asymmetric vehicle path problem includes nodes, where there are nodes in the grid cell, 2 nodes for the interacting unmanned vehicles, and 1 node for the virtual warehouse. The relevant cost matrix The form design is as follows:

[0045]

[0046] In the formula is a matrix block of, corresponding to the connection cost between all grid cells, the connection cost between the unmanned vehicle and the grid cell is , and the connection cost between the virtual warehouse node and the unmanned vehicle is , which is actually set to a very large value;

[0047] The capacity constraint condition is that the number of unknown voxels assigned to each unmanned vehicle does not exceed 40% of the global total;

[0048] The LKH algorithm is used to transform the asymmetric capacitated vehicle routing problem into a standard traveling salesman problem for real-time solution, and the optimal global coverage path of the two vehicles is generated.

[0049] In the multi-unmanned vehicle collaborative exploration system in an unknown environment of the present invention, in the construction of the sparse graph in step E, the collision-free path length between sparse graph nodes is approximately calculated through a pre-stored Dijkstra algorithm path library;

[0050] For grid cells far from the unmanned vehicle, the cost is the path length from the unmanned vehicle to the nearest grid cell plus the path length between the two grid cells on the sparse graph.

[0051] In the multi-unmanned vehicle collaborative exploration system in an unknown environment of the present invention, the distance constraint from the obstacle in step F is calculated in real time through a signed distance field (SDF).

[0052] In the multi-unmanned vehicle collaborative exploration system in an unknown environment of the present invention, the radio station is a dual-band radio station of 2.4GHz / 5.8GHz, the radio station supports the TDMA protocol, the communication distance of the radio station is greater than 3km, and the lidar is a 16-line mechanical lidar.

[0053] Compared with the prior art, the multi-unmanned vehicle collaborative exploration system in an unknown environment of the present invention has the following beneficial effects:

[0054] 1. The present invention uses multiple unmanned vehicles for exploration, so that the exploration work of an area can be completed faster, and using multiple unmanned vehicles for exploration has higher fault tolerance than a single unmanned vehicle.

[0055] 2. The present invention can coordinate the work of multiple unmanned vehicles through communication with limited scope and less information, and can coordinate to ensure that multiple unmanned vehicles complete the exploration work efficiently. Moreover, the entire system has extremely high flexibility and robustness.

[0056] 3. The technical solution of the present invention fully considers the actual areas explored by each unmanned vehicle, avoids mutual interference between robots and unfair workload distribution, and considers the global coverage route of the unknown space. Therefore, the unmanned vehicle will not repeatedly visit the same area, thus significantly improving the overall efficiency and giving full play to the great potential of multiple unmanned vehicles for exploration.

[0057] 4. The technical solution of the present invention enables each unmanned vehicle to quickly plan actions to respond to environmental changes, enabling it to flexibly navigate and collect information while avoiding collisions with previously unknown obstacles and other agents in the system.

[0058] 5. The technical solution of the present invention can perform collaborative planning and does not require all robots to communicate with the server. Thus, when the robots explore a large-scale environment, they can stay away from the server, or in a cluttered environment, the signal can still enable the unmanned vehicle to continue exploring. Even if there is occlusion, the unmanned vehicle can still continue to explore. Therefore, the multi-unmanned vehicle collaborative exploration system in the unknown environment of the present invention has the characteristics of being able to quickly plan actions to respond to environmental changes, enabling it to flexibly navigate and collect information while avoiding collisions with previously unknown obstacles and other agents in the system, having a decentralized coordination method with fewer communication requirements, and achieving faster, more robust, more coordinated, and higher fault tolerance exploration of the entire unknown environment in the multi-unmanned vehicle system within the task area.

[0059] The following further describes the multi-unmanned vehicle collaborative exploration system in the unknown environment of the present invention with reference to the accompanying drawings. Description of the Drawings

[0060] Figure 1 Schematic diagram of the online H-grid construction of the present invention;

[0061] Figure 2 Flowchart of the online H-grid decomposition of the unknown environment of the present invention;

[0062] Figure 3 Flowchart of the request interaction of the unmanned vehicle in the multi-unmanned vehicle collaborative exploration system in the unknown environment of the present invention;

[0063] Figure 4 For the unmanned vehicle and the unmanned vehicle Flowchart of the response interaction in the double-check;

[0064] Figure 5Sequence diagram of discrete poses on the time elastic band (TEB) of the present invention;

[0065] Figure 6 Schematic diagram for generating a curved path between discrete poses of the present invention;

[0066] Figure 7 Schematic diagram of the hypergraph optimization model structure of the present invention. Detailed implementation manners

[0067] The multi-unmanned vehicle collaborative exploration system under unknown environment of the present invention includes the following steps:

[0068] A. It includes multiple unmanned vehicles equipped with radio stations, lidars, and inertial measurement units (IMUs). Each unmanned vehicle generates a three-dimensional occupancy grid map of the surrounding environment through simultaneous localization and mapping (SLAM). Among them, the grid cells scanned by the lidar are marked as known voxels, and those not scanned are marked as unknown voxels;

[0069] B. Based on the three-dimensional occupancy grid map, a hierarchical H grid is constructed. The preset maximum exploration area is divided into multiple layers of grids, and the size of each grid cell decreases hierarchically; record the number of unknown voxels, central coordinates, and index values in each grid cell;

[0070] C. Dynamically update the H grid. When the proportion of unknown voxels in a grid cell exceeds 70% and does not reach the highest level, it is subdivided into the next level; if the proportion of unknown voxels in the grid cell of the highest level is less than 10% or the path is inaccessible, then this grid cell is deleted;

[0071] D. The unmanned vehicles interact pairwise to overcome limited communication and coordinate their movements. Each unmanned vehicle broadcasts a communication data packet containing the grid cell index, interaction timestamp, current position, and partition request through the radio station; the receiving party verifies the timeliness of the timestamp. If there is a conflict, the interaction is rejected and re-pairing is triggered;

[0072] E. Reallocate grid cells based on the vehicle routing problem with capacity constraints. Create virtual warehouse nodes for two unmanned vehicles with successful interactions, and generate the global coverage paths of the two vehicles using an asymmetric cost matrix and capacity constraints;

[0073] F. Solve the optimal local trajectory of each unmanned vehicle based on the time elastic band algorithm (TEB). Discretize the global coverage path into a sequence of timestamped poses, initialize the time elastic band nodes, and solve the trajectory that satisfies the constraints and minimizes the total time consumption through the g2o framework;

[0074] G. Convert the optimized trajectory into motor speed and front wheel steering angle commands through a PID controller. Each unmanned vehicle independently executes the trajectory and shares position and H grid update information through the radio station to achieve cooperative obstacle avoidance.

[0075] As a further improvement of the present invention, in the above step A, each driverless vehicle generates a three-dimensional occupancy grid map of the surrounding environment of each driverless vehicle through Simultaneous Localization and Mapping (SLAM);

[0076] In the above step B, the H grid divides the preset maximum exploration area into multiple levels. Each level consists of multiple grid cells, and the size of the smallest grid cell is an integer multiple of the lidar resolution. For each grid cell, the number of unknown voxels it contains, the central coordinates of all unknown voxels within the grid cell, and the grid cell index value are recorded;

[0077] In the above step C, it also includes storing the grid cells of the H grid using a linearized array and accelerating the retrieval and division of grid cells through an Axis-Aligned Bounding Box (AABB);

[0078] In the above step D, each driverless vehicle packs the grid cell index value, interaction timestamp, current position information, and the latest partition request data into a communication data packet through a radio station and then broadcasts and sends it; each driverless vehicle maintains an interaction timestamp list. When each driverless vehicle initiates an interaction request through radio station communication, it attaches timestamp encryption verification. The receiving party confirms the timeliness of the request through a double-check mechanism. If a task conflict is detected, the interaction is rejected, and the driverless vehicle with the task conflict is triggered to re-pair;

[0079] In the above step E, the grid cells are reallocated based on the Capacitated Vehicle Routing Problem (CVRP). A virtual warehouse node is created for each pair of interacting driverless vehicles, and an asymmetric cost matrix is used to minimize the total length of the covering path. Capacity constraint conditions are added for each pair of interacting driverless vehicles, and finally, the Capacitated Vehicle Routing Problem is constructed, and the Capacitated Vehicle Routing Problem is solved to obtain the optimal global covering path;

[0080] A sparse graph is embedded in the hierarchical H grid to perform path search. The nodes of the sparse graph are the current positions of each driverless vehicle and the central coordinates of each updated grid cell. The cost between the nodes of the sparse graph is the length of the collision-free path searched by the standard path search algorithm between the nodes in the three-dimensional occupancy grid map;

[0081] In the above step F, the global covering path is discretized into a pose sequence with timestamps, and the nodes of the Time Elastic Band (TEB) are initialized;

[0082] Constraints are fused through a hypergraph optimization model. The variables to be optimized, such as the discrete vehicle poses, the time differences between discrete vehicle poses, and the potentially colliding obstacles, are set as the nodes of the hypergraph, and the constraint conditions such as vehicle kinematic constraints, maximum curvature constraints, maximum linear velocity constraints, maximum angular velocity constraints, maximum acceleration constraints, and distance constraints from obstacles are set as the edges of the hypergraph;

[0083] The vehicle kinematic constraints are constructed based on the Ackermann front-steering and rear-wheel-drive model. The vehicle kinematic constraints are as follows:

[0084] (1)

[0085] In Equation (1), represents the vehicle poses at the (k + 1)-th and k-th moments, represents the vehicle heading angles at the (k + 1)-th and k-th moments, represents the vehicle pose change vector between the (k + 1)-th and k-th moments;

[0086] The maximum curvature constraint is:

[0087] (2)

[0088] In Equation (2), represents the maximum curvature limit of the vehicle, which is determined according to the maximum steering angle of the vehicle, ;

[0089] The maximum speed (including the linear speed and the angular speed ) constraint is:

[0090] (3)

[0091] In Equation (3), and represent the maximum linear speed and the maximum angular speed when the vehicle is traveling, represents the time difference between the (k + 1)-th and k-th moments, , represents the direction of the linear speed, positive for forward and negative for backward, and the angular speed ;

[0092] The maximum acceleration constraint is:

[0093] (4)

[0094] In Equation (4), represents the maximum acceleration when the vehicle is traveling, represents the vehicle pose at the (k + 2)-th moment, represents the time difference between the (k + 2)-th and (k + 1)-th moments, and the acceleration ;

[0095] The distance constraint from the obstacle is: Use the KDTree algorithm to find the trajectory points within a certain range from the obstacle in the entire global coverage path and corresponding obstacles , using a scalar to constrain its distance, and the constraint inequality is as follows:

[0096] (5)

[0097] In formula (5) represents the minimum distance from the pose to the obstacle , represents the minimum distance from the vehicle to the obstacle that can be safely traveled;

[0098] The hypergraph optimization model is solved through the g2o framework, and the optimization goal is to minimize the total trajectory time under all constraint conditions.

[0099] As a further improvement of the present invention, in the H grid update information in step C above, when the grid cell is subdivided, the central coordinates of the unknown voxels in the sub-grid cell are calculated by the grid clustering algorithm, and the index of each grid cell is determined by the following formula:

[0100] ;

[0101] where respectively represent the integer coordinates of the grid cell at the th level in the direction, represents the number of grid cells in the three directions of the space, represents the sum of the number of cells from the 0th level to the th level, so as to determine the unique index of each level cell;

[0102] After the grid cell is deleted in step C, its parent grid cell needs to re-evaluate the proportion of unknown voxels and trigger a hierarchical fallback mechanism;

[0103] The axis-aligned bounding box (AABB) in step C is composed of the maximum rectangular bounding box that is axially aligned with the area explored by the unmanned vehicle itself and the shared area of nearby unmanned vehicles.

[0104] As a further improvement of the present invention, in step C above, the protocol of the radio communication adopts the TDMA time division multiple access mechanism. Each unmanned vehicle is assigned a fixed time slot to broadcast data, and the communication data packet contains a CRC check code and a sequence number. When the packet loss rate exceeds 20%, it switches to the redundant frequency band.

[0105] As a further improvement of the present invention, in step E above, the asymmetric vehicle path problem includes nodes, where the grid cells have nodes, 2 nodes of the interacting autonomous vehicles, and 1 node of the virtual warehouse. The relevant cost matrix is designed in the following form:

[0106]

[0107] In the formula is matrix block of, corresponding to the connection cost between all grid cells, the connection cost between the autonomous vehicle and the grid cell is and the connection cost between the virtual warehouse node and the autonomous vehicle is , which is actually set to a very large value;

[0108] The capacity constraint condition is that the number of unknown voxels allocated to each autonomous vehicle does not exceed 40% of the global total;

[0109] Using the LKH algorithm, the asymmetric capacitated vehicle routing problem is transformed into a standard traveling salesman problem for real-time solution to generate the optimal global coverage paths of the two vehicles.

[0110] As a further improvement of the present invention, in the sparse graph construction in step E above, the collision-free path length between sparse graph nodes is approximately calculated through a pre-stored Dijkstra algorithm path library;

[0111] For grid cells far from the autonomous vehicle, the cost is the path length from the autonomous vehicle to the nearest grid cell plus the path length between the two grid cells on the sparse graph.

[0112] As a further improvement of the present invention, the distance constraint from the obstacle in step F above is calculated in real time through a signed distance field (SDF).

[0113] As a further improvement of the present invention, the above radio station is a 2.4GHz / 5.8GHz dual-band radio station, the radio station supports the TDMA protocol, the communication distance of the radio station is greater than 3km, and the lidar is a 16-line mechanical lidar.

[0114] Embodiment 1

[0115] The multi-autonomous vehicle collaborative exploration system in an unknown environment of the present invention includes the following steps:

[0116] a. Include multiple unmanned vehicles respectively equipped with a radio station, lidar, and inertial measurement unit (IMU). Each unmanned vehicle generates a three-dimensional occupancy grid map of the surrounding environment of each unmanned vehicle through simultaneous localization and mapping (SLAM). The grids in the three-dimensional occupancy grid map include known voxels and unknown voxels. The grids covered by the lidar on the unmanned vehicle are marked as known voxels, and the grids not covered by the lidar on the unmanned vehicle are marked as unknown voxels;

[0117] b. Construct a three-dimensional H-grid based on the three-dimensional occupancy grid map in step A. The H-grid divides the pre-set maximum exploration area into multiple levels. Each level consists of multiple grid cells. The sizes of the grid cells in each level are arranged in decreasing order by level. The size of the smallest grid cell is an integer multiple of the lidar resolution. For each grid cell, record the number of unknown voxels it contains, the central coordinates of all unknown voxels within the grid cell, and the grid cell index value;

[0118] c. Update the H-grid according to the real-time three-dimensional occupancy grid map data, and perform dynamic subdivision and merging of the unknown areas;

[0119] d. When the ratio of the number of unknown voxels to the total number of voxels in a grid cell exceeds 70% and does not reach the highest level, divide the grid cell into sub-grid cells of the next level;

[0120] e. Dynamically maintain a list of grid cells. If the ratio of the number of unknown voxels to the total number of voxels in the highest-level grid cell is less than 10% or the unmanned vehicle cannot reach the central coordinates of all unknown voxels within the grid cell through path search, delete the grid cell;

[0121] f. Store the grid cells of the H-grid using a linearized array, and accelerate the retrieval and division of grid cells through an axis-aligned bounding box (AABB);

[0122] g. Each unmanned vehicle broadcasts and sends the grid cell index value, interaction timestamp, current position information, and the latest partition request of its own H-grid through the radio station;

[0123] In step g, each unmanned vehicle packs the grid cell index value, interaction timestamp, current position information, and the latest partition request data into a communication data packet through the radio station and then broadcasts and sends it;

[0124] h. Each unmanned vehicle maintains a list of interaction timestamps. When each unmanned vehicle initiates an interaction request through radio communication, it attaches timestamp encryption verification. The receiving party confirms the timeliness of the request through a double-check mechanism. If a task conflict is detected, the interaction is rejected, and the unmanned vehicle with the task conflict is triggered to re-pair;

[0125] The task conflict refers to the repeated allocation of the same grid cell by two unmanned vehicles;

[0126] i. Reallocate grid cells based on the capacitated vehicle routing problem (CVRP), create virtual warehouse nodes for each pair of interacting unmanned vehicles, use an asymmetric cost matrix to minimize the total length of the covering path, add capacity constraint conditions for each pair of interacting unmanned vehicles, and finally construct the capacitated vehicle routing problem, solve the capacitated vehicle routing problem to obtain the optimal global covering path, and balance the workload of each pair of interacting unmanned vehicles;

[0127] In the setting of the present invention, there are two vehicles corresponding to a pair of interacting unmanned vehicles and . Different from the standard VRP, in the standard VRP, there is a central warehouse and the vehicle routes form a closed loop, while the present invention requires an open path starting from the position of the unmanned vehicle and passing through the H grid cells. By introducing a virtual warehouse and appropriately designing the connection cost, this variant is simplified to an asymmetric VRP.

[0128] Suppose there are grid cells. The asymmetric VRP involves nodes, where the grid cells have nodes, and are 2 nodes, and the virtual warehouse is 1 node. The relevant cost matrix is in the following form:

[0129]

[0130] In the formula is the matrix block, corresponding to the connection cost between all H grid cells. Since the goal of the present invention is to find the shortest CPs, the path length between paired cells is used as the connection cost:

[0131]

[0132] where here , represent the centers of the unknown voxels in the th and th cells, and is a collision-free path, which can be found by the standard A* path search algorithm.

[0133] The connection cost between the unmanned vehicle and the H grid cell is calculated by ; this is a matrix block. The calculation method is:

[0134]

[0135] Sometimes there are multiple solutions with the same length but different coverage patterns. Therefore, is introduced to prevent the path from changing frequently between different patterns, which may lead to inconsistent movement and slow down the exploration speed.

[0136] To simplify the problem to an asymmetric VRP, the connection cost from the virtual warehouse to the two unmanned vehicles is allocated using blocks , where is a large value. A huge negative value will directly connect the node of the virtual warehouse to the two unmanned vehicles because it greatly reduces the overall cost of the output path. In this way, the output of the asymmetric VRP consists of the required shortest path and four additional edges, where two edges connect the warehouse to the unmanned vehicles, and the other two edges connect the two units to the warehouse.

[0137] If the actual number of unknown areas explored by each unmanned vehicle is not considered, it will still lead to an unbalanced workload allocation. Therefore, the present invention introduces a vehicle capacity limit to further balance the workload allocated to the unmanned vehicles. For each node in the VRP , if it is associated with an H-grid cell (assumed to be -th cell), a demand equal to the number of unknown voxels will be allocated . Otherwise, the demand is set to zero. The present invention limits the capacity of each vehicle to a percentage of the total number of unknown voxels. Represent the set of nodes in the route of the unmanned vehicle as , then the capacity constraint is:

[0138] .

[0139] Due to the setting of the capacity constraint, the vehicle routing problem becomes a capacitated vehicle routing problem (CVRP). To solve this problem, the open-source LKH (Lin Kernighan Helsgaund) solver can be used to calculate the optimal path for each unmanned vehicle. Based on LKH, the vehicle routing problem can be transformed into an equivalent standard traveling salesman problem (TSP), and the capacity constraint can be handled using a penalty function to obtain the optimal solution.

[0140] j. Embed a sparse graph in the hierarchical H-grid to perform path search. The nodes of the sparse graph are the current positions of each unmanned vehicle and the center coordinates of each updated grid cell. The cost between the nodes of the sparse graph is the length of the collision-free path searched by the standard path search algorithm between the nodes in the three-dimensional occupancy grid map;

[0141] To obtain the costs between nodes in the CVRP more quickly, the present invention embeds a sparse graph in the H grid to perform global path search to reduce the computational cost. Specifically, for each grid cell, if there is a collision-free path within the range of two grid cells, a graph node is attached, and the nodes of each adjacent cell are connected to it by weighted edges. Whenever the H grid cell is updated, the paths of each adjacent grid cell are recalculated on the occupancy map. If a solution is found, the weight of the edge is updated according to the path length, and the longer the path, the greater the weight of the edge. Otherwise, the edge is deleted from the graph. Using this sparse graph, it is easier to calculate the connection cost between the grid cell and the unmanned vehicle. For each pair of grid cells, a path is searched on the sparse graph to approximate the shortest path on the occupancy map. For grid cells far from the unmanned vehicle, the cost is the path length from the unmanned vehicle to the nearest grid cell plus the path length between the two grid cells on the sparse graph.

[0142] Previously, the global reference paths of each vehicle were obtained by solving the CVRP. The present invention solves the real-time local trajectory of the vehicle based on the improved TEB algorithm (Time elastic-band, TEB).

[0143] k. Discretize the global coverage path obtained in step i into a pose sequence with timestamps, and initialize the Time Elastic Band (TEB) nodes;

[0144] Regarding step k, the traditional Elastic Band (EB) algorithm uses the pose of the vehicle to optimize the path, and defines the entire optimization variable as follows:

[0145] (6)

[0146] TEB adds the corresponding time interval on the basis of EB , and its formula is as follows:

[0147] (7)

[0148] represents the pose converted to the pose The required time. Combining equations (6) and (7), the optimization variable of TEB is obtained as follows:

[0149] (8)

[0150] As Figure 5 shown, , and are respectively three poses on the Time Elastic Band, and and respectively and as well as and The time interval. The principle is to discretize the entire path, assuming the entire path as an elastic band, with the path points as the respective nodes of the elastic band, and the path constraints and the objective function as the external forces of the elastic band. Based on the external forces, a suitable path is found by modifying the nodes. This algorithm combines local planning with control quantities, takes the global path as the input, and takes the motor speed and wheel angle as the output. On the global path perform interpolation to obtain an initial path . Among them and both represent the pose of the vehicle.

[0151] In the initial stage, the TEB will be initialized using the global planning path, and as the vehicle moves, the optimized state quantities will be dynamically deleted and added. The adjustment interval is as follows:

[0152]

[0153] In the formula represents the time interval that should be satisfied, is used as a lag parameter to avoid the oscillation of the optimization result. To ensure the density of the entire optimization result.

[0154] l. By fusing constraints through the hypergraph optimization model, the variables to be optimized, such as the discrete vehicle pose (including position and attitude), the time difference between discrete vehicle poses, and the obstacles that may collide, are set as the nodes of the hypergraph, and the vehicle kinematic constraints, maximum curvature constraints, maximum linear velocity constraints, maximum angular velocity constraints, maximum acceleration constraints, and distance constraints from obstacles are set as the edges of the hypergraph;

[0155] The vehicle kinematic constraint is constructed based on the Ackermann front-wheel drive and rear-wheel drive model. The vehicle kinematic constraint is:

[0156] (1)

[0157] In formula (1) represents the vehicle poses at the (k + 1)-th and k-th moments, represents the vehicle heading angles at the (k + 1)-th and k-th moments, represents the vehicle pose change vector between the (k + 1)-th and k-th moments;

[0158] The maximum curvature constraint is:

[0159] (2)

[0160] In formula (2) represents the maximum curvature limit of the vehicle, which is determined according to the maximum steering angle of the vehicle ;

[0161] The maximum speed (including the linear speed and the angular speed ) constraint is:[[]]

[0162] (3)

[0163] In formula (3) and represent the maximum linear speed and the maximum angular speed when the vehicle is traveling represents the time difference between the (k + 1)-th moment and the k-th moment , represents the direction of the linear speed, positive for forward and negative for backward, and the angular speed ;

[0164] The maximum acceleration constraint is:[[]]

[0165] (4)

[0166] In formula (4) represents the maximum acceleration when the vehicle is traveling represents the vehicle pose at the (k + 2)-th moment represents the time difference between the (k + 2)-th moment and the (k + 1)-th moment, and the acceleration ;

[0167] The distance constraint from the obstacle is: Use the KDTree algorithm to find the trajectory points within a certain range of the obstacle in the entire global coverage path and the corresponding obstacle , and use the scalar to constrain its distance. The constraint inequality is as follows

[0168] (5)

[0169] In formula (5) represents the minimum distance from the pose to the obstacle , represents the minimum distance from the vehicle to the obstacle at which the vehicle can safely travel;

[0170] The present invention transforms the optimization problem into a hyper - graph. Different from the basic two - dimensional graph, the nodes connected by an edge in this graph are not restricted, and its constraint conditions are the edges of the hyper - graph. The optimization variables related to it are the connected nodes of the edges. Such a transformation makes the constraints only act on local optimization variables. For example, Figure 7 the speed constraint in , and their time interval are related. Therefore, this hyper - graph has three nodes and one edge. Thus, the entire optimization problem can be represented as a sparse matrix, which is more convenient to solve.

[0171] m. The hyper - graph optimization model is solved through the g2o framework, and the optimization goal is to minimize the total time consumption of the trajectory under all constraint conditions;

[0172] Specifically, different from directly selecting the minimum motion cost, the present invention takes the total time of the trajectory as the objective function to improve the exploration efficiency:

[0173] (10)

[0174] In formula (10), n represents n discrete pose points. Combining the above constraints and the objective function, the present invention transforms the non - linear programming problem with hard constraints into a quadratic optimization problem to improve the solving efficiency. At the same time, using the sparsity of the problem, the constraints are transformed into penalty terms. For the equality constraint in formula (1), it is transformed into a quadratic constraint as follows:

[0175] (11)

[0176] In formula (11), is the identity matrix, is the penalty value of this constraint.

[0177] For the inequality constraint such as (2), (3), (4), (5), it can be transformed into a unilateral quadratic constraint with a penalty value of :

[0178] (12)

[0179] The present invention transforms the final objective function into the following formula:

[0180] (13)

[0181] Among them, is the time interval between two poses, is its kinematic constraint, is the curvature constraint, and are the velocity and acceleration constraints respectively, represents the distance constraint from the obstacle, and and and and are their penalty matrices respectively. Such a structure facilitates the addition of new constraint conditions in the future.

[0182] The flow of the overall algorithm is as follows. First, an initial path is obtained by interpolation based on the globally referenced path solved previously. The optimized state variables are dynamically inserted or deleted as the vehicle moves. Then, the trajectory nodes are corresponded to the obstacles and waypoints near the trajectory, and the corresponding obstacles for each trajectory point are found. A hypergraph is established using the objective function in Equation (13) and optimized to obtain the optimal trajectory . When the trajectory is feasible, the control quantity of the vehicle is calculated and sent to the underlying controller to drive each vehicle to move. The control quantity at time is calculated as follows:

[0183] (9)

[0184] In Equation (9), and are respectively the velocity control quantity and the front-wheel steering angle control quantity at time is the wheelbase of the vehicle, and are respectively the horizontal and vertical coordinates of the vehicle in the rectangular coordinate system at time represents the first derivative of is the yaw angle of the vehicle at time is the speed of the vehicle at time represents the direction of the linear velocity, positive for forward and negative for backward.

[0185] The velocity control quantity and the front-wheel steering angle control quantity are converted into motor speed and front-wheel steering angle commands through a PID controller. The control period is 20 ms. Each unmanned vehicle independently executes the trajectory. Each unmanned vehicle periodically shares the position information of other unmanned vehicles and the H-grid update information through a radio station to ensure cooperative obstacle avoidance.

[0186] As a further improvement of the present invention, in the H-grid update information in step m above, when the grid cell is subdivided, the central coordinates of the unknown voxels in the sub-grid cells are calculated by the grid clustering algorithm, and the index of each grid cell is determined by the following formula:

[0187] ,

[0188] Among them, respectively represent the integer coordinates of the grid cells at the th level in the direction, represents the number of grid cells in the three directions of the space, then represents the total number of cells from the 0th level to the th level, so as to determine the unique index of each level of cells;

[0189] After the grid cell is deleted in step e, its parent grid cell needs to re-evaluate the proportion of unknown voxels, triggering a hierarchical fallback mechanism;

[0190] The axis-aligned bounding box (AABB) in step f consists of the maximum rectangular bounding box axially aligned with the area explored by the unmanned vehicle itself and the shared area of nearby unmanned vehicles. The update period considers the average movement speed of the unmanned vehicle and the sensing frequency of the lidar, and is set to 100 ms as the best.

[0191] As a further improvement of the present invention, the protocol of the radio communication in step h above is:

[0192] (1) Adopt the TDMA time division multiple access mechanism, and each unmanned vehicle is assigned a fixed time slot to broadcast data;

[0193] (2) The communication data packet contains a CRC check code and a sequence number. When the packet loss rate exceeds 20%, switch to the redundant frequency band, and the communication frequency is 10 Hz.

[0194] As a further improvement of the present invention, in step i above, the asymmetric vehicle path problem includes nodes, among which there are nodes in the grid cells, 2 nodes of the interacting unmanned vehicles, and 1 node of the virtual warehouse. The relevant cost matrix is designed in the following form:

[0195]

[0196] In the formula is the matrix block of, corresponding to the connection cost between all grid cells, the connection cost between the unmanned vehicle and the grid cell is , the connection cost between the virtual warehouse node and the unmanned vehicle is , and is actually set to a very large value;

[0197] The capacity constraint condition is that the number of unknown voxels assigned to each unmanned vehicle does not exceed 40% of the global total;

[0198] Using the LKH algorithm, the asymmetric CVRP (Capacitated Vehicle Routing Problem) is transformed into a standard Traveling Salesman Problem (TSP) for real-time solution, generating the optimal global coverage paths for two vehicles.

[0199] As a further improvement of the present invention, in the construction of the sparse graph in step j above, the collision-free path length between sparse graph nodes is approximately calculated through a pre-stored Dijkstra algorithm path library;

[0200] For grid cells far from the unmanned vehicle, the cost is the path length from the unmanned vehicle to the nearest grid cell plus the path length between the two grid cells on the sparse graph.

[0201] As a further improvement of the present invention, the distance constraint from obstacles in step n above includes: the distance constraint from obstacles is calculated in real time through Signed Distance Field (SDF), and the update frequency is 20Hz.

[0202] As a further improvement of the present invention, the above radio station is a dual-band radio station of 2.4GHz / 5.8GHz, the radio station supports the TDMA protocol, the communication distance of the radio station is greater than 3km, and the lidar is a 16-line mechanical lidar.

[0203] The biggest problem with traditional uniform grids is their inability to cope gracefully with objects of very different sizes. When an object is much larger than the size of a grid cell and eventually overlaps with many grid cells, the cost of updating the position of a dynamically changing object becomes very expensive. If the cell size is adjusted to accommodate larger objects, the spatial resolution of the grid will decrease as more small objects appear in the same cell, resulting in performance degradation. Using a hierarchical grid, or H-grid, can effectively solve this size variation problem, and this grid structure is particularly suitable for accommodating dynamically changing objects.

[0204] A hierarchical grid, or H-grid, consists of multiple grids with different cell sizes that overlap each other and cover the same space. Depending on different purposes, these grids can be two-dimensional or three-dimensional. Given a hierarchical structure with n layers of grids, let represent the size of the grid cells in the k-th layer of the hierarchical structure, and the grids are arranged in ascending order of cell size, i.e., < <... < , the first layer is the lowest layer, and the n-th layer is the highest layer. For the convenience of fast insertion and deletion operations, an object is only inserted into a cell of one of the levels.

[0205] Geometrically, the H-grid is similar to an octree-based decomposition, but it is implemented using an array, allowing for O(1) time complexity to retrieve any cell. Additionally, a linearized data storage structure is adopted in the actual implementation of the H-grid to increase the level of data locality, improving the CPU data cache hit rate and thus enhancing the overall performance.

[0206] The H-grid of the present invention contains a total number L of levels , where the grid cells of are the coarsest, and the grid cells of are the finest (i.e., the highest level of the hierarchical grid introduced above). For each grid cell, it records the number of unknown voxels it contains, the central coordinates of all unknown voxels within the grid cell, and the grid cell index value. To store the entire H-grid cells using a single array, the present invention uses a mapping similar to a standard 3D grid, dividing space into cells. For a cell with integer coordinates , its index is determined by the following formula: , where respectively represent the integer coordinates of the cell at the th level in the direction, represents the number of cells in the space in the three directions, represents the sum of the number of cells from the 0th level to the th level, and in this way, the unique index of each level of grid cell can be uniquely determined. An array-like list Figure 1 is actually used to store the decomposition of the unknown region, as shown in . At the same time, to facilitate the update of the H-grid, the present invention records , where represents the rectangular bounding box of the Mth region,

[0207] is their set, that is, the axis-aligned bounding box (Axis-Aligned Bounding Box, AABB) of the updated map region, which consists of the region explored by the unmanned vehicle itself and the region shared by nearby unmanned vehicles, that is, the largest rectangular bounding box of the sum of these irregular regions.

[0208] As Figure 2 shown, Figure 2Online H-grid decomposition flowchart for an unknown environment, corresponding to steps C, D, E, and F. Initially, the task area is completely unknown and is decomposed into the coarsest grid cells, i.e., a linearized array containing only grid cells in the hierarchy. As the unmanned vehicle constructs and shares a 3D occupancy grid map, some areas become partially unknown, and the relevant cells are further subdivided to partition the unknown areas with a finer resolution grid. The update of the H-grid is carried out by iterating through each hierarchy in the linearized array and screening the grid cells that overlap with any axis-aligned bounding box . Recalculate the number of unknown voxels , the number of known voxels , and the central coordinates of all unknown voxels within the grid cell. Then there are three parallel decision conditions:

[0209] (1) If the ratio of the known quantity contained in the grid cell in the hierarchy to the total voxel quantity exceeds a certain ratio (this ratio is obtained through experimental comparison and is set to 70% as the optimal value), and 's level is not the finest level , then search for 's sub-division , and remove from , and add the next sub-divided hierarchy to .

[0210] (2) If 's level is the finest level , and the ratio of the known quantity it contains in its grid cell to the total voxel quantity is less than , then it will be deleted from .

[0211] (3) The cells that cannot reach their central coordinates from the current position of the unmanned vehicle are also removed, which eliminates areas that cannot be covered by the sensor, such as the hollow space in a thick wall, etc.

[0212] Finally, the level label is incremented by one and the higher-level hierarchies are further subdivided, and this iteration continues until the linearized array is traversed.

[0213] As shown Figure 3 in the figure Figure 3 is a flowchart for considering the interaction request of an unmanned vehicle in the H grid, corresponding to steps G and H. Taking the unmanned vehicle as an example, it broadcasts its number , the index of the H grid cell where it is located and the timestamp of the most recent attempt to perform paired interaction . In addition, the unmanned vehicle records , indicating the timestamp of the most recent successful interaction with other unmanned vehicles

[0214] The process of the unmanned vehicle proposing an interaction method is as follows: The interaction starts with searching for unmanned vehicles that can communicate with the unmanned vehicle . For the set of these unmanned vehicles found , for the k-th unmanned vehicle in the set, if the following conditions are met: (1) The difference between the current time and the timestamp of the most recent attempt to perform paired interaction with the unmanned vehicle is greater than the interaction threshold ; (2) The difference between the current time and the timestamp of the most recent successful interaction with the unmanned vehicle is greater than or equal to ; then update , . Condition (1) ensures that those unmanned vehicles that have attempted another interaction within the recent time will not be considered, which can prevent an unmanned vehicle from interacting with several unmanned vehicles simultaneously. Interacting with several unmanned vehicles simultaneously usually results in conflicting interaction results. Condition (2) ensures that the unmanned vehicle can select the unmanned vehicle that has not had a successful interaction with the unmanned vehicle for the longest time, which gives other unmanned vehicles the opportunity to interact with the unmanned vehicle . Then, a CVRP model is constructed and solved for the unmanned vehicle and to find a suitable global coverage path for the two unmanned vehicles. The grid cells traversed by the global coverage path are the new partition results of the two vehicles . The request message containing the result is sent to the unmanned vehicle . Then, the unmanned vehicle updates the timestamp of the most recent attempt to perform paired interaction , and waits within the duration of " " , and waits within the duration of " " The response will only be made when the response is received, and then the partition results will be updated using the driverless vehicle and the driverless vehicle Otherwise, the interaction will be ignored and a new interaction will be attempted later. It is necessary to double-check the most recent interaction attempt. Sometimes the driverless vehicle and may just request to interact with another driverless vehicle and broadcast the updated timestamp. Due to communication delays, when the driverless vehicle attempts to interact with the driverless vehicle this timestamp may not be available. In this case, double-checking ensures that the driverless vehicle does not interact with the driverless vehicle and the driverless vehicle simultaneously, thus avoiding task assignment conflicts. The process of responding to the interaction during double-checking is as shown. When the driverless vehicle Figure 4 receives an interaction request and interaction information, it first determines whether the difference between the current time and the timestamp of the most recent attempted paired interaction with the driverless vehicle is greater than the interaction threshold . If so, it responds to the driverless vehicle that the interaction was successful and updates the timestamp of the most recent attempted paired interaction and the timestamp of the most recent successful interaction for the driverless vehicle to the current time , and updates the partition published by the driverless vehicle for the driverless vehicle . .

[0215] Task area allocation and global path generation based on the Capacitated Vehicle Routing Problem (CVRP).

[0216] The Traveling Salesman Problem (TSP) is a classic combinatorial optimization problem. The classic TSP can be described as follows: A salesman needs to promote products in several cities. Starting from one city, the salesman has to pass through all the cities and then return to the starting point. How should the traveling route be selected to minimize the total travel distance? From the perspective of graph theory, this problem essentially involves finding a Hamiltonian cycle with the minimum weight in a weighted complete undirected graph. Since the feasible solutions to this problem are all the permutations of all vertices, as the number of vertices increases, a combinatorial explosion will occur, and it is an NP-complete problem (Non-deterministic Polynomial). The Vehicle Routing Problem (VRP) generalizes the TSP to multiple vehicles and can find the optimal coverage paths (CPs) of grid cells.

[0217] The embodiments described above are only descriptions of the preferred embodiments of the present invention and do not limit the scope of the present invention. Without departing from the spirit of the present invention's design, all various deformations and improvements made by those of ordinary engineering and technical personnel in the art to the technical solution of the present invention shall fall within the protection scope determined by the claims of the present invention.

Claims

1. Multi-robot collaborative exploration system in unknown environment, characterized by: Including the following steps: A. There are multiple unmanned vehicles equipped with radio stations, lidar, and inertial measurement units. Each unmanned vehicle generates a three-dimensional occupancy grid map of the surrounding environment through simultaneous localization and mapping. Among them, the grid cells scanned by the lidar are marked as known voxels, and those not scanned are marked as unknown voxels; B. Based on the three-dimensional occupancy grid map, a hierarchical H-grid is constructed. The preset maximum exploration area is divided into multiple layers of grids, and the size of each grid cell decreases hierarchically; record the number of unknown voxels, the central coordinates, and the index value in each grid cell; C. Dynamically update the H-grid. When the proportion of unknown voxels in a grid cell exceeds 70% and does not reach the highest level, it is subdivided into the next level; if the proportion of unknown voxels in the highest-level grid cell is less than 10% or the path is inaccessible, then delete the grid cell; D. The unmanned vehicles interact pairwise to overcome limited communication and coordinate their movements. Each unmanned vehicle broadcasts a communication data packet containing the grid cell index, interaction timestamp, current position, and partitioning request through the radio station; The receiving party verifies the timeliness of the timestamp. If there is a conflict, it rejects the interaction and triggers re-pairing; E. Reallocate grid cells based on the vehicle routing problem with capacity constraints. Create virtual warehouse nodes for two unmanned vehicles with successful interactions, and use an asymmetric cost matrix and capacity constraints to generate the global coverage paths of the two vehicles; F. Solve the optimal local trajectory of each unmanned vehicle based on the time elastic band algorithm. Discretize the global coverage path into a pose sequence with timestamps, initialize the time elastic band nodes, and solve the trajectory that satisfies the constraints and minimizes the total time consumption through the g2o framework; G. Convert the optimized trajectory into motor speed and front-wheel steering angle commands through a PID controller. Each unmanned vehicle independently executes the trajectory and shares position and H-grid update information through the radio station to achieve cooperative obstacle avoidance.

2. The multi-robot collaborative exploration system in an unknown environment according to claim 1, characterized in that: In step A, each unmanned vehicle generates a three-dimensional occupancy grid map of the surrounding environment of each unmanned vehicle through simultaneous localization and mapping respectively; In step B, the H-grid divides the preset maximum exploration area into multiple levels. Each level consists of multiple grid cells. The size of the smallest grid cell is an integer multiple of the lidar resolution. For each grid cell, record the number of unknown voxels it contains, the central coordinates of all unknown voxels in the grid cell, and the grid cell index value; In step C, it also includes storing the grid cells of the H-grid using a linearized array and accelerating the retrieval and division of grid cells through an axis-aligned bounding box; In step D, each unmanned vehicle packs the grid cell index value, interaction timestamp, current position information, and the latest partitioning request data into a communication data packet through the radio station and then broadcasts and sends it; each unmanned vehicle maintains a list of interaction timestamps. When each unmanned vehicle initiates an interaction request through radio communication, it attaches timestamp encryption verification. The receiving party confirms the timeliness of the request through a double-check mechanism. If a task conflict is detected, it rejects the interaction and triggers the unmanned vehicle with the task conflict to re-pair; In step E, the grid cells are reallocated based on the vehicle routing problem with capacity constraints. Virtual warehouse nodes are created for each pair of interacting unmanned vehicles, and an asymmetric cost matrix is used to minimize the total length of the coverage path. Capacity constraint conditions are added for each pair of interacting unmanned vehicles, and finally, the vehicle routing problem with capacity constraints is constructed. The vehicle routing problem with capacity constraints is solved to obtain the optimal global coverage path; A sparse graph is embedded in the hierarchical H grid to perform path search. The nodes of the sparse graph are the current positions of each unmanned vehicle and the center coordinates of each updated grid cell. The cost between the nodes of the sparse graph is the length of the collision-free path searched by the standard path search algorithm between the nodes in the three-dimensional occupancy grid map; In step F, the global coverage path is discretized into a sequence of poses with timestamps, and the nodes of the time elastic band (TEB) are initialized; By fusing constraints through the hypergraph optimization model, the variables to be optimized, such as the discrete vehicle poses, the time differences between discrete vehicle poses, and the potentially colliding obstacles, are set as the nodes of the hypergraph, and the constraint conditions, such as vehicle kinematic constraints, maximum curvature constraints, maximum linear velocity constraints, maximum angular velocity constraints, maximum acceleration constraints, and distance constraints from obstacles, are set as the edges of the hypergraph; The vehicle kinematic constraints are constructed based on the Ackermann front-steering and rear-drive model, and the vehicle kinematic constraints are as follows: （1） In formula (1) represents the vehicle poses at the (k + 1)-th and k-th moments, represents the vehicle heading angles at the (k + 1)-th and k-th moments, represents the vehicle pose change vector between the (k + 1)-th and k-th moments; The maximum curvature is constrained to: （2） In formula (2) represents the maximum curvature limit of the vehicle, which is determined according to the maximum steering angle of the vehicle ; Maximum speed (including linear speed and angular speed ) constraint is as follows: （3） In formula (3) and represent the maximum linear velocity and the maximum angular velocity when the vehicle is moving, represents the time difference between the (k + 1)-th moment and the k-th moment, , represents the direction of the linear velocity, positive for forward and negative for backward, and the angular velocity ; Maximum acceleration constraint is as follows: （4） In formula (4) represents the maximum acceleration when the vehicle is moving represents the vehicle pose at the (k + 2)-th moment represents the time difference between the (k + 2)-th moment and the (k + 1)-th moment, and the acceleration ; The distance constraint with respect to the obstacle is as follows: Using the KDTree algorithm, trajectory points within a certain range of the obstacle are found in the entire global coverage path and the corresponding obstacle , and the distance is constrained using a scalar . The constraint inequality is as follows: （5） In formula (5) represents the minimum distance from the pose to the obstacle represents the minimum distance from the obstacle at which the vehicle can safely travel; The hypergraph optimization model is solved through the g2o framework, and the optimization objective is to minimize the total time consumption of the trajectory under all constraint conditions.

3. The multi-robot collaborative exploration system in an unknown environment according to claim 2, characterized in that: In the H-grid update information in step C, when the grid cell is subdivided, the central coordinates of the unknown voxels of the sub-grid cells are calculated by the grid clustering algorithm, and the index of each grid cell is determined by the following formula: ； wherein respectively represent the integer coordinates of the grid cells at the direction, represents the number of grid cells in the three directions of the space, then represents the total number of cells from the 0th level to the level, so as to determine the unique index of each level of cells; In step C, after the grid cell is deleted, its parent grid cell needs to re-evaluate the proportion of unknown voxels, triggering the hierarchical fallback mechanism; The axis-aligned bounding box in step C consists of the region explored by the unmanned vehicle itself and the axially aligned maximum rectangular bounding box of the shared region of nearby unmanned vehicles.

4. The multi-robot collaborative exploration system in an unknown environment according to claim 3, wherein: In step D, the protocol of the radio communication adopts the TDMA (Time Division Multiple Access) mechanism. Each unmanned vehicle is assigned a fixed time slot to broadcast data. The communication data packet contains a CRC check code and a sequence number. When the packet loss rate exceeds 20%, it switches to the redundant frequency band.

5. The multi-robot collaborative exploration system in an unknown environment according to claim 4, characterized in that: In step E, the asymmetric vehicle routing problem includes nodes, where the grid cells have nodes, 2 nodes for the interacting autonomous vehicles, and 1 node for the virtual warehouse. The relevant cost matrix is designed in the following form: In the formula is matrix block, corresponding to the connection cost between all grid cells. The connection cost between the unmanned vehicle and the grid cell is , and the connection cost between the virtual warehouse node and the unmanned vehicle is , which is actually set to a very large value; The capacity constraint condition is that the number of unknown voxels assigned to each unmanned vehicle does not exceed 40% of the global total; The LKH algorithm is used to transform the asymmetric capacitated vehicle routing problem into a standard traveling salesman problem for real-time solution, generating the optimal global coverage path for the two vehicles.

6. The multi-robot collaborative exploration system in an unknown environment according to claim 5, wherein: In the construction of the sparse graph in step E, the collision-free path length between the sparse graph nodes is approximately calculated through the pre-stored Dijkstra algorithm path library; For the grid cells far from the unmanned vehicle, the cost is the path length from the unmanned vehicle to the nearest grid cell plus the path length between the two grid cells on the sparse graph.

7. The multi-robot collaborative exploration system in an unknown environment according to claim 6, wherein: In step F, the distance constraint from obstacles is calculated in real time through the Signed Distance Field (SDF).

8. The multi-robot collaborative exploration system in an unknown environment according to claim 7, wherein: The radio is a dual-band radio of 2.4 GHz / 5.8 GHz. The radio supports the TDMA protocol, and the communication distance of the radio is greater than 3 km. The lidar is a 16-line mechanical lidar.

Images