Multi-unmanned aerial vehicle cooperative autonomous exploration method and system suitable for semi-closed GPS denial environment

Abstract

The invention discloses a multi-unmanned aerial vehicle cooperative autonomous exploration method and system suitable for a semi-closed GPS denial environment. Aiming at the problems of low multi-unmanned aerial vehicle collaborative exploration efficiency, non-uniform task allocation and the like in the prior art, the method realizes efficient collaborative exploration through the following steps: collecting point cloud data by using a laser radar and generating a polyhedron representing a local feasible region of an environment; expressing the explored area as a polyhedron set, generating an exploration boundary and iteratively updating the exploration boundary; a quadratic measurement error is adopted to simplify the polyhedral mesh, and a collision detection box thought is introduced to improve the boundary updating efficiency; constructing a global sparse topological graph, and providing global path guidance for multiple unmanned aerial vehicles based on a Dijkstra algorithm; generating a smooth trajectory through a trajectory optimization method based on gradient constraint; a hybrid communication architecture is adopted, a central unit solves a limited-capacity multi-unmanned aerial vehicle path problem, and the task load of each unmanned aerial vehicle is optimized and balanced. According to the invention, multi-unmanned-aerial-vehicle cooperative autonomous exploration can be continuously and rapidly completed.

Description

Technical Field

[0001] The present invention relates to the technical field of multi-UAV collaborative autonomous exploration, and in particular to a multi-UAV collaborative exploration method and system applicable to a semi-closed GPS-denied environment. Background Art

[0002] With the maturity of multi-UAV cooperation technology, swarm exploration has received increasing attention in many application fields. Compared with a single UAV, multi-UAVs can achieve division of labor and cooperation among UAVs through cooperation, can collect more environmental information in a short time, accelerate the exploration efficiency, and have advantages such as robustness, redundancy, and reliability, with irreplaceable advantages. The research on multi-UAVs has become a trend to solve the problem of exploring unknown environments.

[0003] A semi-closed space refers to a spatial form that is between a completely enclosed space (completely surrounded by four walls or similar structures) and a completely open space (without any obstruction). Physically, at least one side of a semi-closed space is open, and such a space is both independent and has a certain connection with the outside. Current UAV exploration scenarios such as indoor spaces, mine tunnels, pipelines, etc. can be considered semi-closed spaces. Compared with the open outdoor environment, its greatest feature is the absence of GPS signals, that is, GPS denial. In such an environment, exploration requires UAVs to have the ability to represent the environment and extract boundaries, and be able to perform exploration planning. Multi-UAV exploration also requires a collaborative exploration task allocation mechanism.

[0004] Multi-UAV collaborative autonomous exploration is a systematic project that can be divided into three parts: perception, decision-making, and planning methods. The perception part directly generates the original perception results through the on-board sensors of the UAVs, such as point cloud maps, depth images, color images, etc., calculates the positions of the UAVs online and in real time, and processes the nearby environmental information into the format required by the system, which is the premise and guarantee for realizing multi-UAV collaborative exploration. The decision-making part calculates the exploration target points for the next moment based on the current environmental information of the UAVs. In a multi-UAV collaborative system, the task allocation among the UAVs also needs to be considered. The planning part plans a safe and feasible trajectory for the UAVs efficiently based on the current positioning of the UAVs, the obstacle information in the environment, and the target points for the next moment. These three parts complement each other, enabling the UAVs to efficiently complete the exploration of the specified space. Among them, the decision-making part is the most critical part in the multi-UAV unknown environment collaborative exploration system, and its performance will largely affect the exploration efficiency of the entire system. After obtaining multiple observation points, the decision-making part needs to adopt a reasonable task allocation strategy to maximize the collaborative advantages of the multi-UAV system.

[0005] In existing multi-UAV collaborative exploration methods, the multi-UAV exploration system for coordinating multiple UAVs usually relies on a central controller or requires reliable communication between UAVs. Therefore, the centralized architecture makes coordination vulnerable and inefficient, while the distributed architecture can improve the flexibility and robustness of the system and reduce the communication requirements. Secondly, many multi-UAV exploration methods only consider the allocation of boundaries and viewpoints, but do not consider the actual areas explored by each UAV. These strategies can lead to interference between UAVs and mismatches in the workload of the exploration areas of each UAV. These challenges make it difficult for multi-UAVs to collaboratively explore unknown areas. Therefore, it is of great significance to carry out research on the collaborative exploration system of multi-UAVs in a semi-closed GPS-denied environment. Summary of the Invention

[0006] The present invention aims to improve the problems of low efficiency and uneven task allocation in the collaborative autonomous exploration of multi-UAVs in a semi-closed GPS-denied environment. The purpose of the present invention is to use a global sparse topological map to represent the explored areas in the environment, and at the same time use the feasible region represented by polyhedra to obtain the exploration boundary and perform iterative updates of the boundary. A hierarchical planning method is adopted to generate a passable path from the UAV to the target point on the topological map when the exploration target point of the UAV is out of sight, and optimize the obtained initial path to finally obtain a safe, feasible and smooth trajectory. A task allocation is modeled based on the Capacitated Vehicle Routing problem (CVRP) to minimize the total length of the UAV exploration path and balance the task load of each UAV, thereby improving the efficiency of multi-UAV collaborative autonomous exploration.

[0007] To achieve the above object, the present invention proposes a multi-UAV collaborative exploration method applicable to a semi-closed GPS-denied environment, including:

[0008] Step S1: Use the UAV equipped with a lidar sensor to enter an unknown environment, collect point cloud data, and generate a polyhedron that can represent the local feasible region of the environment based on a unit sphere.

[0009] Step S2: Represent the explored area as a set of a series of polyhedra, generate candidate boundaries for exploration according to the polyhedron, and perform boundary updates.

[0010] Step S3: Simplify the polyhedron grid representing the local feasible region by using the Quadric Error Metrics (QEM) method, and introduce the idea of a collision detection box to improve the efficiency of boundary updates.

[0011] Step S4: Construct a global sparse topological map that stores information about the explored areas in the environment, and provide global path guidance for the UAVs based on the Dijkstra method.

[0012] Step S5: The local trajectory optimizer optimizes the trajectory according to the guiding path by using a trajectory optimization method based on a gradient-constrained Euclidean Signed Distance Field (ESDF) map to obtain a smooth, safe, and passable trajectory.

[0013] Step S6: Adopt a hybrid communication architecture. The central unit uses the Lin-Kernighan-Helsgaun (LKH) algorithm to solve the UAV cooperative exploration task assignment result modeled according to CVRP.

[0014] Step S7: After the UAV is assigned a new task point, it jumps from S6 to S4, performs motion planning according to the assigned task point, and simultaneously completes the coverage update of the boundary cluster during flight. S1 to S3 will be continuously performed during the motion process. When there is no boundary in the environment, the cooperative exploration task is completed.

[0015] The polyhedron representing the local feasible region of the environment means that the free space of the environment can be represented by the union of a series of polyhedra. Although the point cloud data of the lidar has high precision, its internal combination is disordered and cannot directly generate the local feasible region space of the UAV's location. Therefore, it is necessary to extract the polyhedron representing the local field of view from the disordered point cloud data of the current frame.

[0016] The set of polyhedra means the set of polyhedra that can be generated when the UAV moves to different positions to sparsely represent the free space of the explored area.

[0017] The boundary refers to the boundary between the known free area and the unknown area in the map. If the UAV is controlled to fly towards the boundary, the unknown area can be explored. If there is no boundary in the space to be explored, it can be considered that the exploration is completed.

[0018] The boundary update means that as the UAV flies, the boundary can gradually expand outwards and perform efficient update iterations.

[0019] The accelerated boundary update is because the grid density of the polyhedron is relatively high. Therefore, when judging whether a boundary point is inside a polyhedron, it is necessary to traverse all the grids for judgment, which will incur a large computational time overhead during the boundary update process.

[0020] The simplified polyhedron grid means reducing the grid density while keeping the local feasible space characteristics represented unchanged, so as to reduce the number of queries when detecting whether a certain boundary voxel is in the known explored area.

[0021] The idea of the collision detection box means that the boundary voxels to be detected are preferentially detected with the axis-aligned bounding boxes of the grid clusters representing obstacles, thus avoiding the problem of consuming a large amount of computing time caused by traversing all grids for judgment.

[0022] The topological map mentioned above is an abstract map representation method that uses the connection relationship between nodes and edges to describe the environment. Nodes represent key positions in the environment, and edges represent the connection relationship between nodes. In UAV navigation, due to its abstract and structured characteristics, the topological map is an efficient and adaptable map representation method.

[0023] The Dijkstra algorithm mentioned above was proposed by the Dutch computer scientist Dijkstra in 1959. This algorithm can solve the problem of calculating the shortest paths from one vertex to the remaining vertices in a weighted graph. The algorithm combines the ideas of greedy, breadth-first search, and dynamic programming algorithms. Starting from the starting point, each time it searches for the nearest expandable adjacent vertex to the starting point. When the end point is found, it means the path-finding process ends.

[0024] The ESDF-free map trajectory optimization method mentioned above means that by comparing the trajectory inside the obstacle and the collision-free path guidance to model the collision cost, then projecting the collision force onto the collision trajectory to obtain the estimated gradient value, and finally obtaining a safe and smooth trajectory through optimization, thus avoiding the time overhead of constructing the ESDF map and preventing the planning from being easily trapped in local minima when the view behind the obstacle is blocked and the information is insufficient.

[0025] The hybrid communication architecture mentioned above means that environmental perception and trajectory planning are realized by decentralized UAV self-units, and the central unit is responsible for integrating environmental information and allocating and scheduling collaborative exploration tasks.

[0026] The CVRP problem mentioned above is an extended form of the multi-UAV path problem (Vehicle Routing problem, VRP). In VRP, the UAV distribution center needs to deliver goods to customers. The demand of each customer is different, and a UAV fleet is sent by the distribution center to be responsible for the delivery task. Each UAV in the fleet has its own route. The ultimate goal is to meet the delivery needs of customers, while minimizing the total distance traveled by the fleet, costs such as energy consumption, and the total time to complete the delivery task. The CVRP problem further introduces a constraint that each customer has its own demand, and each UAV cannot exceed its maximum load. It is required that the sum of the demands of all customers on the path assigned to a UAV does not exceed the maximum load of the UAV.

[0027] A multi - UAV collaborative autonomous exploration system applicable to semi - enclosed GPS - denied environments, using the multi - UAV collaborative autonomous exploration method applicable to semi - enclosed GPS - denied environments described above, includes the following modules:

[0028] Environmental characterization and boundary extraction module: Using the point cloud information of the lidar sensor, quickly extract the polyhedrons representing the feasible regions of the local field of view. A series of polyhedrons represent the explored areas in the environment. At the same time, obtain the exploration boundary using the feasible regions characterized by the polyhedrons and perform iterative updates of the boundary.

[0029] Hierarchical UAV exploration planning module: Construct a global sparse topological map of the environment, generate a passable path for the UAV to the target point on the topological map, without using the global ESDF map, and optimize the distance between the trajectory and obstacles required during the optimization process based on the local occupancy grid map.

[0030] Collaborative exploration task allocation module: Model the task allocation problem using the finite - capacity multi - UAV path problem. The optimization goal is to minimize the total length of the UAV exploration paths, ultimately making the unknown areas explored by each UAV balanced, and at the same time avoiding the UAVs from repeatedly visiting the same areas, improving the collaborative exploration efficiency.

[0031] Compared with the prior art, the beneficial effects of the present invention are:

[0032] Firstly, a polyhedron - based environmental characterization and exploration information extraction method is proposed, and a series of polyhedrons are used to represent the explored areas in the environment. Compared with the existing methods for generating polyhedrons, it can make more full use of the environmental information of the sensor. The time for one update of the boundary update method proposed in this study is less than that of the boundary update method based on the grid map.

[0033] Secondly, for the global map information fusion problem in multi - UAV collaborative exploration, an incremental method is adopted to construct a sparse topological map that stores the polyhedron information representing the explored areas in the environment. Find a feasible path from the current position of the UAV to the target area in this topological map as the global guiding path to the target point to be explored. On this basis, use the trajectory generation method without ESDF to optimize the global path into a smooth and safe trajectory suitable for the UAV controller to execute.

[0034] Thirdly, a multi - UAV unknown environment collaborative exploration system is built. The central unit node is responsible for integrating the global environmental information, generating exploration boundaries and performing task allocation, and sending the exploration target points to the sub - nodes. The UAV sub - nodes are responsible for perceiving the environment and performing trajectory planning tasks. The simulation experiments verify the rationality and effectiveness of the multi - UAV unknown environment collaborative exploration system proposed in this study, and at the same time meet the requirement of UAV workload balance.

[0035] In summary, for the problem of collaborative exploration of multiple UAVs in an unknown environment, a detailed study is carried out from three aspects: environmental representation and boundary extraction methods, hierarchical UAV exploration planning, and task allocation strategies for multi-UAV collaborative exploration. The designed multi-UAV collaborative exploration system can complete the collaborative exploration task in a semi-closed GPS-denied environment. Brief Description of the Drawings

[0036] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the following will briefly introduce the drawings required in the embodiments of the present invention. Obviously, the following described drawings are only some embodiments of the present invention, and those of ordinary skill in the art can obtain other drawings based on these drawings without creative efforts.

[0037] Figure 1 It is a flowchart of the multi-UAV collaborative exploration method applicable to a semi-closed GPS-denied environment in the embodiments of the present invention;

[0038] Figure 2 It is a UAV equipped with a lidar sensor in the embodiments of the present invention;

[0039] Figure 3 It is a schematic diagram of projecting discrete points outside a cylinder onto the cylinder surface in the embodiments of the present invention;

[0040] Figure 4 It is a schematic diagram of generating a local feasible region based on a unit sphere in the embodiments of the present invention;

[0041] Figure 5 It is a schematic diagram of a polyhedron composed of a set of triangular meshes in the embodiments of the present invention;

[0042] Figure 6 It is a schematic diagram of a boundary cluster obtained at the current observation position in the embodiments of the present invention;

[0043] Figure 7 It is a schematic diagram of the boundary update process in the embodiments of the present invention;

[0044] Figure 8 It is a comparison diagram of the mesh before and after simplification in the embodiments of the present invention;

[0045] Figure 9 It is a sparse topological map established by three UAVs for collaborative exploration in the embodiments of the present invention;

[0046] Figure 10 It is a global topological path map in the embodiments of the present invention;

[0047] Figure 11 It is a schematic diagram of collision anchor points and directions in the embodiments of the present invention;

[0048] Figure 12It is a schematic diagram of the multi-UAV collaborative task allocation result obtained by using the CVRP model in the embodiment of the present invention;

[0049] Figure 13 It is a multi-UAV collaborative autonomous exploration flight trajectory map in a semi-closed GPS-denied environment in the embodiment of the present invention. Detailed implementation manners

[0050] In order to make the objectives, technical solutions and advantages of the present invention clearer and more understandable, the present invention will be further described in detail below with reference to embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not used to limit the present invention. The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention.

[0051] As Figure 1 shown, the present invention proposes a multi-UAV collaborative exploration method applicable to a semi-closed GPS-denied environment, including the following steps:

[0052] Step S1: Use a UAV equipped with a lidar sensor to enter an unknown environment, collect point cloud data, and generate a polyhedron that can represent the local feasible region of the environment based on a unit sphere.

[0053] In the embodiment of the present invention, the UAV equipped with a lidar sensor is as Figure 2 shown, and its main components include a lidar sensor, an on-board computer, a flight controller, and a battery.

[0054] An inertial measurement unit chip (including a 3-axis accelerometer and a 3-axis gyroscope) is integrated inside the lidar, and accurate point cloud data can be quickly obtained after processing using the laser SLAM algorithm.

[0055] The specific method for generating a polyhedron based on a unit sphere is as follows: When the UAV is at position Point occ , after obtaining a frame of point cloud data, a cylinder with the current position of the UAV as the center and a radius of R poly is set. When the point cloud of the lidar is inside the cylinder, this point can be considered as the surface point of the obstacle, defined as Point occ . The points P out outside the cylinder are mapped onto the surface of the cylinder using the formula , and the mapped points are named Point Free , as Figure 3 shown. Voxel filtering is performed on the mapped point cloud data to reduce the number of subsequent point clouds to be processed while maintaining the shape of the point cloud. After obtaining the processed point cloud, for Point occPoint clustering is performed to form different point sets, and the set of each cluster of points is the surface of an independent obstacle. To generate the polyhedron at the current position, first use the formula to calculate Point Free and Point occ 's projections on the unit sphere centered at P uav as shown in Figure 4 (a). Where p is the projection point on the unit sphere, and the convex hull mesh formed by the projection points is calculated using the quick hull method at all projection positions. Map the vertices of the convex hull mesh back to Point Free and Point occ to construct the polyhedron, as shown in Figure 4 (b).

[0056] Step S2, represent the explored area as a set of a series of polyhedra, generate candidate boundaries for exploration based on the polyhedra, and perform boundary updates.

[0057] Use polyhedra to sparsely represent the feasible region space of the environment. The polyhedron consists of a set of triangular meshes, as shown in Figure 5 . Part of the voxels constituting the boundary cluster come from the vertices of the polyhedron mesh, and the other part of the voxels come from the edges of the polyhedron mesh. Cluster all the obtained boundary voxels, and the finally obtained boundary cluster is shown in Figure 6 . Each color represents a boundary cluster, and the rectangular boundary box surrounding the boundary cluster is the axis-aligned boundary box of the boundary cluster, which is used for the judgment in the subsequent boundary update process.

[0058] After obtaining the candidate boundaries, delete the voxels in the previously explored area in the candidate boundaries according to the set of polyhedra that can represent the explored area in the global sparse topological map. At the same time, when generating a new polyhedron representing the local feasible region, the voxels in the current frame's local feasible region that appear in the old boundary should be deleted. Generally, there are several frames of lidar point cloud information between the boundary cluster update at the current moment and the boundary cluster update at the previous moment. By maintaining the maximum and minimum values of the voxel positions in the boundary cluster, the axis-aligned bounding box of the boundary cluster to be updated currently can be obtained. Through collision detection between the axis-aligned bounding box and the field of view of the UAV sensor, the range of the boundary cluster to be updated can be reduced, and the update speed of the boundary cluster can be accelerated. If a boundary voxel is inside the feasible region surrounded by the polyhedron, it will be deleted during the boundary update process. The boundary update result is shown in Figure 7 .

[0059] Step S3, accelerate the boundary update. Use the QEM method to simplify the polyhedron mesh representing the local feasible region, and introduce the idea of a collision detection box to improve the efficiency of boundary update.

[0060] The QEM method is used to reduce the number of meshes while maintaining the overall shape of the obstacle surface. The QEM method maintains the appearance and shape of the model based on the geometric error metric on the surface. By identifying and merging smaller meshes on the mesh, QEM can reduce the number of mesh faces, thereby simplifying the model. In QEM, each triangular mesh is represented by a quadratic equation, which can be calculated through the normal vector of the mesh and any point. This quadratic equation can estimate the geometric error between the triangular mesh and other meshes, thereby helping to select the best merging strategy. Through continuous iteration, QEM can gradually reduce the number of model faces while trying to maintain the shape and details of the original model. The result obtained after simplifying the mesh representing the obstacle surface is as follows Figure 8 As shown, it can be seen that compared with the original mesh, the number of simplified meshes is significantly reduced when representing a flat surface.

[0061] When judging whether a certain boundary cluster voxel is inside a polyhedron, first judge whether the line segment connecting the voxel and the center of the polyhedron intersects with the collision detection box of the mesh surrounding the obstacle surface. If it intersects, continue to check whether it intersects with the triangular mesh in the detection box. If it intersects, the voxel is outside the polyhedron. For the drone at the center of the polyhedron, the boundary voxel is invisible and will not be deleted during the update process. If the line segment does not intersect with any triangular mesh inside the collision detection box, the voxel is inside the polyhedron. If the line segment connecting the voxel and the center of the polyhedron does not intersect with the collision detection box of the mesh surrounding the obstacle surface, and the voxel is within the maximum radius of the polyhedron, it can be considered that the voxel is inside the polyhedron and is visible, and will be deleted during the boundary update process.

[0062] Step S4, construct a global sparse topology map that stores the information of explored areas in the environment, and provide global path guidance for the UAV based on the Dijkstra method.

[0063] The topological graph is represented by G = (V, E), where V represents the vertices representing independent free spaces in the topological graph, and E represents the edges showing connectivity in these free spaces. Each edge in E will not collide with the obstacle method. The specific method of constructing a sparse topological graph is as follows: After the drone generates a polyhedron representing the local free space at a given frequency, it is packaged into the vertex format of the topological graph. If the vertex does not exist in the topological graph at this time, the vertex is placed in the topological graph. At the same time, during the exploration process, each drone maintains a vertex v last,i , represents the vertex closest to the drone numbered i in the topological graph G. When the new vertex received by the mapping module is sent by the i-th drone, it first determines whether the vertex is related to v last,i When the distance is less than the threshold t addnewWhen it is, no subsequent operation is performed. When the distance is greater than the threshold t addnew When it is, query all vertex sets SV in the topological graph within a specified radius centered on this vertex v near , if in SV near There already exists a vertex with a very close distance to v, then the new vertex v is not put into the topological graph, and at the same time, v is updated last,i to make it the vertex closest to the drone in the topological graph at the current moment. If in SV near None of the vertices have a vertex that is too close to the new vertex v, then v is added to the topological graph G and named v new . Check whether the connection between v new and the vertices in SV near collides with obstacles. If there is no collision, a new edge is added to the edge set E, and at the same time, v is updated last,i so that v last,i = v new . The establishment effect of the sparse topological graph during the collaborative exploration of three drones is as shown in Figure 9 , where the blue dots represent the actual positions of the three drones in the environment, and the green dots are the vertices closest to the three drones in the topological graph.

[0064] The Dijkstra algorithm first determines the vertex in the topological graph closest to the current position of the drone, denoted as v start , and the vertex in the topological graph closest to the boundary cluster to be reached, denoted as V goal . Find the shortest path from V start to V goal in the topological graph. During the execution of the algorithm, two sets are maintained. One is the close set, and the points in this set do not need to be traversed; the other set is the open set, and the points in this set represent those that have been visited. First, put v start into the close set, and traverse all the vertices connected to the starting point v start in the topological graph. The cost of the vertex is the actual distance to V start . Establish a priority queue to store the traversed vertices and their costs, and put these vertices into the open set, indicating that they have been visited. The priority queue is a data structure that can be automatically sorted, and the dequeue order is according to the priority. In the Dijkstra algorithm, when the cost of the vertex is small, that is, when the path length from the starting point v start to this vertex is the shortest, the priority is higher. In each loop, take out the first vertex in the priority queue, denoted as v cur , and put it into the close set. If v cur = v goal, it means that the path search process has ended, and the shortest path from v start to v goal can be obtained by backtracking through the parent nodes. Otherwise, traverse each vertex v cur connected to the vertex v adj . If v adj is in the close set, it means that the shortest path between this vertex and v start has been obtained, and continue to traverse the next vertex. If v adj is in the open set, it is necessary to check whether the cost value of the vertex with v cur as the parent node is less than the original cost value. If it is less, then change the parent node of v adj to v cur , and at the same time modify the cost of v adj . If v adj is neither in the close set nor in the open set, then: v adj .cost = v cur .cost + |v adj - v cur |, v adj .parent = v cur , and put v adj into the priority queue and at the same time into the open set. In the simulation environment, a global topology map of multi-UAV cooperation is constructed. Given the starting point and the target point, path planning is performed, and the result obtained is the green path in Figure 10 .

[0065] Step S5, the local trajectory optimizer uses the ESDF-map-free trajectory optimization method based on gradient constraints according to the guiding path to perform trajectory optimization, and obtains a smooth, safe and passable trajectory.

[0066] The ESDF-map-free trajectory optimization method based on gradient constraints can be refined into four parts: B-spline-based trajectory representation method, collision thrust estimation method, gradient-based trajectory generation method, and time reallocation and trajectory refinement.

[0067] B-spline based trajectory representation method. Specifically, a spline curve is a widely used mathematical curve in computer graphics. It can accurately describe the motion trajectory of an object in three-dimensional space through control points. A B-spline curve is a piecewise polynomial curve with smooth properties, which is uniquely determined by the order, control points, and knot vector. When moving the control points of a B-spline, only a part of the curve shape related to the control point will be changed. In trajectory optimization, the control points are usually moved to reduce the overall cost of the trajectory and obtain the optimal trajectory curve. A single control point only affects the shape of the local curve related to it. Therefore, this property of the B-spline curve is very suitable for trajectory optimization. The essence of a B-spline curve is a piecewise Bézier curve. Therefore, each segment of the B-spline curve is contained within the convex polygon formed by the control points. When the control points satisfy the constraint conditions, the entire trajectory can be considered to satisfy the constraints. Therefore, in trajectory optimization, only the control points need to be optimized.

[0068] Collision thrust estimation method. Specifically, after obtaining the target point of the UAV, first generate a B-spline curve Φ that satisfies the end constraint conditions from the current position of the UAV to the target point. This curve does not consider whether it collides with obstacles in the environment. Then, according to the locally built occupancy grid map in real time, use the A* algorithm to obtain a path Γ that does not collide with obstacles, as Figure 11 shown. When the control point Q i in the original curve Φ is inside an obstacle, find a positioning point p ij on the surface of the obstacle, where i is the index of the control point and j is the number of the obstacle. Therefore, the distance from the control point Q i to the j-th obstacle is: d ij =(Q i -p ij )×v ij . To avoid repeated {p, v} anchor points in the initial few iterations, only when Q i satisfies d ij >0 for all obstacles, this control point is considered valid. In addition, this criterion only optimizes the obstacles that affect the generation of the final trajectory, thus significantly reducing the calculation time.

[0069] Gradient-based trajectory generation method. Specifically, the overall cost function of trajectory optimization is where J s represents the smoothing penalty term, J c represents the collision penalty term, J d is the dynamic feasibility penalty term, and λ is the weight of each penalty term in the total cost function. The smoothing penalty term J sConsider reducing the speed and acceleration of the B-spline curve, that is, minimizing the speed control points and acceleration control points. The role of the collision penalty term is to keep the trajectory away from obstacles. According to the convex hull property of the B-spline, it is necessary to keep the control points away from obstacles. The dynamic feasibility of the trajectory means that when the trajectory is sent to the UAV for execution, the trajectory needs to meet the dynamic constraints of the UAV, restricting the high-order derivatives of the trajectory in each dimension. According to the convex hull property, it is only necessary to control the control points of the speed, acceleration, and the derivative of the acceleration to meet the constraint conditions, and the entire trajectory will not exceed the maximum dynamic constraint conditions.

[0070] Time reallocation and trajectory refinement. Specifically, usually in the trajectory optimization process, the time allocation of the B-spline trajectory adopted is uniform. However, since the gradient information tends to extend the trajectory and keep it away from obstacles, it may sometimes lead to an infeasible optimized trajectory. At the same time, the dynamic constraints are added to the optimization objective function as soft constraints, which may cause the optimized trajectory to exceed the dynamic constraints that the UAV throttle can provide. Therefore, after obtaining the optimized trajectory, it is necessary to reallocate the time of the trajectory to obtain a non-uniform B-spline trajectory. The least squares method is used to solve the control points to make them have the same shape as the uniform trajectory, and based on the cost function is optimized. J s represents the smoothing penalty term, J d is the dynamic feasibility penalty term, J f is the curve fitting penalty term, representing the integral of the axial and radial displacements corresponding to the uniform trajectory to the non-uniform trajectory. Since the trajectory shapes are the same, the collision penalty is not considered in this optimization.

[0071] Step S6, adopt a hybrid communication architecture, and the central unit uses the LKH algorithm to solve the UAV collaborative exploration task allocation result modeled according to the CVRP.

[0072] Specifically, in the hybrid communication architecture, each drone is equipped with a lidar sensor device to achieve real-time positioning. Based on the point cloud data of the lidar, a polyhedron representing the local feasible space is obtained, and the information including the obstacle surface mesh, the central position of the polyhedron, and the drone number is sent to the central unit. The mapping module of the central unit establishes a sparse topological map that can represent all the explored areas of the environment based on the polyhedron information sent by all drones, and generates and updates the exploration boundary according to this topological map in the central unit. The central unit simultaneously receives the real-time position of each drone. In the task allocation module, the existing exploration boundaries in the environment are allocated to each drone in a reasonable manner, so that the exploration areas of the drones are dispersed and non-overlapping, and at the same time, the exploration task amounts among the drones can be balanced as much as possible. When the own node of the drone receives the exploration target sent by the central unit, a safe and smooth trajectory from the current position to the target point is planned through the trajectory planning module. In the entire multi-drone collaborative exploration system in an unknown environment, environmental perception and trajectory planning are implemented by the decentralized drone own units, and the central unit is responsible for the integration of environmental information and the allocation and scheduling of collaborative exploration tasks.

[0073] The CVRP method is used to perform the task allocation of multi-drone collaborative exploration. The optimization goal is to minimize the total length of the exploration paths of the drones and make the unknown areas explored by each drone balanced. In the modeling of this problem, a virtual warehouse is designed to abstract the multi-drone exploration task into an asymmetric and non-return-to-end CVRP problem, as Figure 12 shown.

[0074] The LKH algorithm is used to solve the task allocation result of the drone collaborative exploration modeled according to the CVRP problem. The LKH solver can convert the VRP problem into an equivalent traveling salesman problem. In the LKH algorithm, a candidate list is constructed from all possible exchange edges. Usually, the edge weight and geometric proximity are considered during the construction. The goal of constructing the candidate list is to limit the path range explored by the search algorithm, thereby reducing the computational complexity. The core of the algorithm is to use a heuristic algorithm to search for a more optimized path by exchanging the edges in the path. The algorithm first determines an initial path, changes the current path by exchanging edges during iteration, and finally outputs the target path that the drone visits in sequence during the exploration process.

[0075] In step S7, when a drone is assigned a new task point, it jumps from S6 to S4, performs motion planning according to the assigned task point, and at the same time completes the coverage update of the boundary cluster during the flight. Steps S1 to S3 will be continuously performed during the motion. When there are no boundaries in the environment, the collaborative exploration task is completed.

[0076] Based on the above steps, the results of the cooperative autonomous exploration flight of multiple unmanned aerial vehicles in the semi-closed GPS-denied environment according to the embodiments of the present invention are as follows Figure 13 shown. Three to four unmanned aerial vehicles successfully completed the exploration of the predetermined area within a short period of time and achieved collision-free and fast autonomous flight.

[0077] The above are only the preferred embodiments of the present invention, and do not limit the patent scope of the present invention. Any equivalent structural or equivalent process transformation made by using the content of the specification and drawings of the present invention, or directly or indirectly applied to other related technical fields, shall be similarly included in the patent protection scope of the present invention.

Claims

1. A multi-UAV cooperative autonomous exploration method applicable to a semi-closed GPS-denied environment, characterized in that The steps include the following: S1: Use a drone equipped with a lidar sensor to enter an unknown environment, collect point cloud data, and generate a polyhedron that can represent the local feasible region of the environment based on a unit sphere. S2: Represent the explored area as a set of a series of polyhedra, generate candidate boundaries for exploration according to the polyhedron, and update the boundaries. S3: Adopt the Quadric Error Metrics (QEM) method to simplify the polyhedron mesh representing the local feasible region, and introduce the idea of a collision detection box to improve the efficiency of boundary update. S4: Construct a global sparse topological map that stores information about the explored areas in the environment, and provide global path guidance for the drone based on the Dijkstra method. S5: Adopt a trajectory optimization method based on a gradient-constrained Euclidean Signed Distance Field (ESDF) map to perform trajectory optimization and obtain a smooth, safe, and passable trajectory. S6: Adopt a hybrid communication architecture. The central unit uses the Lin-Kernighan-Helsgaun algorithm to solve the assignment result of the cooperative exploration task of drones modeled according to the Capacitated Vehicle Routing problem (CVRP). S7: When a drone is assigned a new task point, perform motion planning according to the assigned task point, and at the same time complete the coverage update of the boundary cluster during flight. When there are no boundaries in the environment, the cooperative exploration task is completed.

2. The multi-UAV collaborative autonomous exploration method according to claim 1, wherein, In the step S1, the specific method for generating a polyhedron based on a unit sphere is as follows: Set a cylinder with the current position of the drone as the center and a radius of R, and map the point cloud data of the lidar to the surface of the cylinder. Perform voxel filtering and clustering on the mapped point cloud data to form different point sets. Use the quick hull method to calculate the convex hull mesh surrounded by the projection points and construct a polyhedron.

3. The multi-UAV collaborative autonomous exploration method according to claim 1, characterized in that In the step S2, the boundary update includes: Delete the voxels in the candidate boundary that are in the previously explored area. Delete the voxels in the old boundary that appear in the local feasible region of the current frame. Perform collision detection through the axis-aligned bounding box and the field of view of the drone sensor to accelerate the update speed of the boundary cluster.

4. The multi-UAV collaborative autonomous exploration method according to claim 1, wherein In the step S3, when using the QEM method to simplify the polyhedron mesh, reduce the number of faces of the mesh by identifying and merging smaller meshes on the mesh while maintaining the appearance and shape of the model.

5. The multi-UAV collaborative autonomous exploration method according to claim 1, characterized in that In the step S4, the construction method of the global sparse topological map includes: Pack the polyhedra representing the local free space into the vertex format of the topological map. Check whether the connection line between the new vertex and the existing vertex collides with an obstacle. If there is no collision, add a new edge.

6. The multi-UAV collaborative autonomous exploration method according to claim 1, wherein In the step S5, the gradient-constrained ESDF map-based trajectory optimization method includes: A trajectory representation method based on B-spline; A collision thrust estimation method; A gradient-based trajectory generation method; Time reallocation and trajectory refinement.

7. The multi-UAV collaborative autonomous exploration method according to claim 1, wherein In step S6, the CVRP method is used to model the task allocation problem. The optimization objective is to minimize the total length of the exploration paths of multiple UAVs and balance the unknown areas explored by each UAV.

8. A multi-UAV cooperative autonomous exploration system applicable to a semi-closed GPS-denied environment, characterized in that, The multi-UAV cooperative autonomous exploration method applicable to semi-closed GPS-denied environments described in claim 1 includes the following modules: An environment characterization and boundary extraction module for quickly extracting the polyhedra representing the feasible regions of the local field of view using the point cloud information of the lidar sensor and performing iterative updates of the boundaries. A hierarchical UAV exploration planning module for constructing a global sparse topological map in the environment and generating a passable path from the UAV to the target point on the topological map to optimize the trajectory. A cooperative exploration task allocation module for modeling the task allocation problem using the multi-UAV path problem with limited capacity to optimize the task allocation.

9. The multi-UAV collaborative autonomous exploration system according to claim 8, characterized in that, The environment characterization and boundary extraction module includes: A lidar sensor for collecting point cloud data. An on-board computer for processing the point cloud data and generating polyhedra. A flight controller for controlling the flight path of the UAV.

10. The multi-UAV collaborative autonomous exploration system according to claim 8, wherein, The hierarchical UAV exploration planning module includes: A global path planning unit for providing global path guidance for the UAV based on the Dijkstra algorithm. A local trajectory optimization unit for generating smooth and safe trajectories using a gradient-constrained ESDF-free map trajectory optimization method.

11. The multi-UAV collaborative autonomous exploration system according to claim 8, wherein The cooperative exploration task allocation module includes: A central unit for integrating global environmental information, generating exploration boundaries, and performing task allocation. UAV sub-nodes for perceiving the environment and performing trajectory planning tasks.

Images