Unmanned aerial vehicle exploration and fixed-point return flight task relay method and system

Abstract

The invention discloses an unmanned aerial vehicle exploration and fixed-point homeward voyage task relay method and system, and relates to the technical field of robot synchronous positioning and mapping and motion planning, and the method comprises the steps: obtaining unmanned aerial vehicle state estimation information and sensor depth information, and constructing an environment map; clustering the map boundary by using a boundary detector to form a boundary cluster; establishing an adjacent cost matrix according to the clusters, and planning a global path by using a traveling salesman problem method; monitoring the state of the unmanned aerial vehicle, including a return flight trigger event and collision detection; during homeward voyage, planning a homeward voyage trajectory according to the global path, and finally optimizing the homeward voyage trajectory of the unmanned aerial vehicle based on the trajectory optimization model to obtain a smooth trajectory of the unmanned aerial vehicle; the trajectory optimization model comprises a plurality of constraint conditions. According to the invention, the problems of limited endurance and difficult environment information return during large-scale exploration can be effectively solved.

Description

Technical Field

[0001] The present application relates to the technical field of robot synchronous positioning, mapping and motion planning, and in particular to a method and system for relaying unmanned aerial vehicle exploration and fixed-point return missions. Background Art

[0002] In recent years, drones (UAVs) have been widely used in various fields due to their low cost and high maneuverability. Autonomous exploration is a key research direction in UAV technology, particularly in the field of SLAM (Simultaneous Localization and Mapping). UAVs use onboard sensors and computing units to build maps and make decisions in real time to efficiently complete exploration missions. Exploration strategies focus on planning and decision-making to achieve rapid and safe coverage and mapping. In large-scale, unknown environments, UAVs also need to consider autonomous return and task relay. Motion planning methods are categorized as hard-constrained and soft-constrained, each with its own advantages and disadvantages.

[0003] Therefore, in large-scale exploration missions, in order to achieve a higher degree of autonomy for drones, it is necessary to introduce a trigger mechanism for return events during the operation of the drones, consider how to complete the relay between exploration missions and return missions, update the drone flight path in real time during task switching, and provide optimized trajectories for the control module to execute. These are key issues that need to be paid attention to in large-scale scene exploration. Summary of the Invention

[0004] The purpose of this application is to provide a method and system for relaying drone exploration and fixed-point return missions. Taking into account the needs faced by drones in large-scale scenarios, an event-triggered mechanism is introduced to monitor the switching and relay of drone exploration and return missions in real time. At the same time, the drone behaviors corresponding to different events will eventually be output as the global path of the drone operation, which also corresponds to different behaviors during the exploration process, solving the problem of drone exploration and return relay in large-scale scenarios.

[0005] To achieve the above objectives, this application provides the following solutions:

[0006] In the first aspect, the present application provides a method for relaying UAV exploration and fixed-point return missions, including:

[0007] Obtain drone state estimation information and sensor depth information.

[0008] An environment map is constructed based on the drone state estimation information and the sensor depth information; the environment map is composed of explored areas and unexplored areas.

[0009] According to the environment map, a boundary detector is used to perform boundary clustering on the boundaries of the environment map to obtain observation points of a plurality of boundary clusters.

[0010] Based on the information of the observation points, the adjacency cost matrix between the drones and each boundary cluster is constructed.

[0011] Based on the adjacency cost matrix between drones and each boundary cluster, a standard asymmetric traveling salesman problem is used to solve the global open-loop path of each observation point; the global open-loop path is the path from the current observation point to the remaining observation points.

[0012] The state of the drone is monitored based on a return event trigger and a collision detector; the triggering events in the return event trigger include fixed flight time, fixed flight position and fixed target.

[0013] When the drone triggers a return event, the return trajectory of the drone is planned based on the global open-loop path.

[0014] When a UAV collides, the return mission is initiated, and the return trajectory of the UAV is planned based on the global open-loop path.

[0015] Based on a trajectory optimization model, the return trajectory of the UAV is optimized to obtain a smooth trajectory of the UAV; the trajectory optimization model includes multiple constraint conditions.

[0016] Secondly, this application provides a drone exploration and fixed-point return mission relay system, including:

[0017] The information acquisition module is used to obtain the drone state estimation information and sensor depth information.

[0018] The map construction module is used to construct an environment map based on the drone state estimation information and sensor depth information; the environment map is composed of explored areas and unexplored areas.

[0019] The boundary clustering module is used to perform boundary clustering on the boundaries of the environment map using a boundary detector according to the environment map, so as to obtain observation points of a plurality of boundary clusters.

[0020] The adjacency module is used to construct the adjacency cost matrix between drones and each boundary cluster based on the information of the observation points.

[0021] The path solving module is used to solve the global open-loop path of each observation point based on the adjacency cost matrix between the drones and each boundary cluster, using the standard asymmetric traveling salesman problem; the global open-loop path is the path from the current observation point to the remaining observation points.

[0022] The path planning module is used to monitor the status of the drone based on a return event trigger and a collision detector; the triggering events in the return event trigger include a fixed flight time, a fixed flight position, and a fixed target.

[0023] When the drone triggers a return event, the return trajectory of the drone is planned based on the global open-loop path.

[0024] When a UAV collides, the return mission is initiated, and the return trajectory of the UAV is planned based on the global open-loop path.

[0025] The trajectory optimization module is used to optimize the return trajectory of the UAV based on a trajectory optimization model to obtain a smooth trajectory of the UAV; the trajectory optimization model includes multiple constraints.

[0026] According to the specific embodiments provided in this application, this application discloses the following technical effects:

[0027] The present application provides a method and system for relaying unmanned aerial vehicle (UAV) exploration and fixed-point return missions, the method comprising: obtaining state estimation data and sensor depth data of a UAV; constructing an environment map based on the state estimation data and sensor depth data of the UAV; performing cluster analysis on the boundaries of the environment map using a boundary detector to form a set of observation points for a plurality of boundary clusters; establishing an adjacency cost matrix between the UAV and each boundary cluster based on the information of the observation point set; determining a global open-loop path traversing each observation point based on the adjacency cost matrix between the UAV and each boundary cluster using a traveling salesman problem solution method; the global open-loop path being the path from the current observation point to all other observation points; monitoring the state of the UAV based on a return event trigger and a collision detector; the triggering conditions of the return event trigger including a fixed flight time, a specific flight position, and a preset target; planning the return trajectory of the UAV based on the global open-loop path when the UAV performs the return mission; initiating the return mission if the UAV encounters a collision, planning the return trajectory of the UAV based on the global open-loop path; and optimizing the return trajectory of the UAV based on a trajectory optimization model to obtain a smooth trajectory of the UAV; the trajectory optimization model including multiple constraints. In response to the needs of drones in large-scale scenarios, this application introduces an event-triggered mechanism to monitor the switching and collaboration of drones between exploration and return missions in real time, ensuring that drone behaviors triggered by different events are ultimately converted into the global path of drone operation, reflecting the different behavioral patterns in the exploration process, and effectively solving the problem of drone exploration and return collaboration in large-scale scenarios. BRIEF DESCRIPTION OF THE DRAWINGS

[0028] In order to more clearly illustrate the embodiments of the present application or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the embodiments. Obviously, the drawings described below are only some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without creative work.

[0029] Figure 1 A flowchart of a method for relaying drone exploration and fixed-point return missions provided in one embodiment of the present application.

[0030] Figure 2 This is a structural diagram of a drone exploration and fixed-point return mission relay method for large-scale scenarios provided in one embodiment of the present application.

[0031] Figure 3 A schematic diagram of the boundary division and observation point generation process and visualization results provided in one embodiment of the present application.

[0032] Figure 4 A schematic diagram of the structure of a drone exploration and fixed-point return mission relay system provided in one embodiment of the present application. DETAILED DESCRIPTION

[0033] The following will be combined with the drawings in the embodiments of this application to clearly and completely describe the technical solutions in the embodiments of this application. Obviously, the embodiments described are only part of the embodiments of this application, not all of the embodiments. Based on the embodiments in this application, all other embodiments obtained by ordinary technicians in this field without making creative efforts are within the scope of protection of this application.

[0034] In recent years, drones have been widely used in agriculture and forestry, forest fire prevention, traffic monitoring and management, and disaster relief due to their low cost, maneuverability, ease of control, and portability. Drone exploration is a fundamental issue in reconnaissance, inspection, and rescue scenarios. In most of these scenarios, environmental maps are either missing or expensive to collect in advance. Furthermore, in many potential applications, exploration operations within buildings, caves, tunnels, and mines pose safety risks. Using drones to perform these tasks in complex environments significantly reduces the risk to human operators.

[0035] The research field of autonomous exploration has garnered widespread attention over the past few decades. Technically, it falls under the umbrella of active SLAM (simultaneous localization and mapping). The goal of active SLAM research is to efficiently and accurately build a complete model of the environment. Unlike traditional SLAM, active SLAM emphasizes autonomous navigation of the robot toward the direction that maximizes environmental information gain, minimizing reliance on human intervention. Generally speaking, drone exploration requires the drone to rely on its onboard sensors and computing units for closed-loop perception, planning, and decision-making, constructing a map of the scene in real time and coordinating various modules within the system to complete the designated exploration mission. During this process, exploration itself focuses more on the planning and decision-making aspects, emphasizing how to optimize a more effective exploration strategy, achieving a good balance between exploration speed and movement safety, and thus completing coverage and mapping of the entire area in the shortest possible time.

[0036] Considering practical application requirements, in large, unknown environments, drones not only need to complete their exploration missions but also, in many cases, need to autonomously return to home. This can involve resupplying when power is insufficient or transmitting information after detecting a specific target. Furthermore, drones also need to continue their exploration mission after autonomously returning to home, completing a relay mission. A drone's return to home essentially involves executing a motion plan from the current drone's position to the nearest return point.

[0037] Current mainstream methods for UAV motion planning are categorized as hard-constraint methods and soft-constraint methods. Hard-constraint methods, exemplified by Minimum Snap trajectory generation, generate smooth and safe trajectories by representing trajectories as piecewise polynomials and solving quadratic programming problems. These methods typically use geometric shapes such as polyhedrons and spheres to represent free space and generate trajectories within these spaces through convex optimization. However, these methods often rely on simple heuristic strategies for time allocation, which can result in poor trajectory quality. A significant drawback of hard-constraint methods is that they ignore the distance to obstacles, resulting in trajectories that are very close to them. Furthermore, dynamic constraints are often conservative, limiting flight speed. Soft-constraint methods formulate trajectory generation as a nonlinear optimization problem, prioritizing smoothness and safety. These methods typically utilize gradient descent or gradient-free sampling methods to optimize discrete-time or continuous-time polynomial trajectories. Compared to hard-constraint methods, soft-constraint methods can leverage gradient information to keep trajectories away from obstacles. For UAVs, a common approach is to parameterize trajectories using uniform B-splines to reduce the number of constraints and optimization complexity.

[0038] Therefore, in large-scale exploration missions, in order to achieve a higher degree of autonomy for drones, it is necessary to introduce a trigger mechanism for return events during the operation of the drones, consider how to complete the relay between exploration missions and return missions, update the drone flight path in real time during task switching, and provide optimized trajectories for the control module to execute. These are key issues that need to be paid attention to in large-scale scene exploration.

[0039] In order to make the above-mentioned purposes, features and advantages of the present application more obvious and easy to understand, the present application is further described in detail below with reference to the accompanying drawings and specific implementation methods.

[0040] Example 1

[0041] like Figure 1 As shown, this embodiment provides a method for relaying UAV exploration and fixed-point return missions, including:

[0042] Step 101: Obtain drone state estimation information and sensor depth information.

[0043] Step 102: Construct an environment map based on the drone state estimation information and the sensor depth information; the environment map is composed of explored areas and unexplored areas.

[0044] Step 103: Based on the environment map, a boundary detector is used to perform boundary clustering on the boundaries of the environment map to obtain observation points of a plurality of boundary clusters.

[0045] Step 104: Based on the information of the observation points, construct an adjacency cost matrix between the drone and each boundary cluster.

[0046] Step 105: Based on the adjacency cost matrix between the drones and each boundary cluster, a standard asymmetric traveling salesman problem is used to solve the global open-loop path of each observation point; the global open-loop path is the path from the current observation point to the remaining observation points.

[0047] Step 106: Monitor the state of the drone based on a return event trigger and a collision detector; the triggering events in the return event trigger include a fixed flight time, a fixed flight position, and a fixed target.

[0048] When the drone triggers a return event, the return trajectory of the drone is planned based on the global open-loop path.

[0049] When a UAV collides, the return mission is initiated, and the return trajectory of the UAV is planned based on the global open-loop path.

[0050] Step 107: Based on a trajectory optimization model, the return trajectory of the UAV is optimized to obtain a smooth trajectory of the UAV; the trajectory optimization model includes multiple constraint conditions.

[0051] In some embodiments, when executing steps 101-102, the specific steps may be as follows:

[0052] Obtain drone state estimation information and sensor depth information; the drone state estimation information includes position parameters, velocity parameters, acceleration parameters and attitude parameters; the sensor depth information includes the point cloud of the lidar and the depth map of the depth camera.

[0053] The projection method is used to integrate the sensor depth information into the occupancy grid map, and all state grids with changed states are added to the insertion queue and the deletion queue respectively.

[0054] Merge the insert queue and delete queue into an update queue.

[0055] Based on the breadth-first search algorithm, the Euclidean distance field map is updated and the status grid in the update queue is processed.

[0056] The environment map is constructed based on an index data structure that maps the three-dimensional state grid coordinates and a doubly linked list for efficient insertion and deletion operations.

[0057] Specifically, if Figure 2 As shown in the figure, to ensure the safety, stability, and accuracy of drone flight, it is necessary to ensure that the drone can estimate its state in the operating environment in real time. This includes real-time monitoring and calculation of parameters such as position, velocity, acceleration, and attitude. This process is usually carried out simultaneously with the mapping module. The mapping module uses depth information acquired by sensors, such as point cloud data from lidar or depth maps from depth cameras, to obtain distance information about obstacles. Subsequently, this new environmental information is integrated into the occupancy grid map through projection, allowing the drone to perform subsequent exploration and return missions on this map.

[0058] Furthermore, designing efficient data structures is crucial for mapping 3D state grid coordinates, including a doubly linked list for indexing data structures to enable efficient data insertion and deletion. This facilitates the construction of a real-time, high-precision Euclidean distance field map (ESDF), providing fast and accurate map query and update capabilities for drones with minimal computational overhead, ensuring safe and efficient motion planning in complex environments.

[0059] Furthermore, depth measurements are acquired through stereo cameras, RGB-D sensors, or binocular depth estimation devices. Simultaneously, pose measurements are acquired using external devices such as GPS and Vicon, or internal estimation devices such as visual-inertial odometry. A projection method is used to integrate the new depth measurements into the occupancy grid map. All state grids with changed states are added to an insertion queue and a deletion queue, respectively. The insertion and deletion queues are then merged into an update queue, and an initialization step is performed to prepare for the update of all potentially affected state grids. The Euclidean distance field map (ESDF) is then updated using a breadth-first search-based algorithm, processing the state grids in the update queue and updating all necessary state grids with minimal computational overhead. Finally, through an indexed data structure that maps the three-dimensional state grid coordinates and a doubly linked list for efficient insertion and deletion operations, a real-time, high-precision Euclidean distance field map (ESDF) is constructed, ensuring safe and efficient motion planning for drones in complex environments.

[0060] In this embodiment, when executing step 103, the specific steps may be as follows:

[0061] When the map is updated, the boundary detector is triggered to perform a boundary clustering operation to generate a boundary cluster; the boundary cluster consists of several sub-grids and a geometric center.

[0062] Based on the boundary clusters, several observation points from the perspective of the UAV's motion are generated; the observation points are obtained by sampling two layers of information and evaluating the unknown boundary cluster coverage of each observation point.

[0063] Specifically, every time the map is updated, the boundary detector of the drone will trigger a boundary clustering operation, defining F i To represent the boundary cluster formed after clustering, for each F i , which stores all the subgrids c that make up this boundary cluster i And a geometric center p of this cluster in space i , which is obtained by the average position of all subgrids belonging to this cluster in space. At the same time, in order to carry out global path planning for subsequent exploration, a series of observation points VP under the perspective of the drone movement are generated along with the boundary cluster. i , which is obtained by sampling two layers of information and evaluating the unknown boundary cluster coverage of each observation point. Specifically, Figure 3 As shown, the process is as follows:

[0064] Under the unified environmental representation, the boundary is also a series of dynamically changing three-dimensional grids, such as Figure 3As shown in (a), (b), (c) and (d) in the figure, the area that the drone has explored will be marked as free space in the map, while the location of obstacles will be marked as occupied space, and the rest is unknown space. Whenever boundary detection is triggered, the system will mark the boundary grids between free space and unknown space as boundaries, but the boundary cluster at this time usually contains a large number of grids, which makes it difficult to complete more detailed global path planning on this basis. Therefore, for boundary clusters with larger occupied space, the clusters will be separated into two uniform clusters along the first main axis, and this behavior is performed recursively so that all larger boundary clusters can be decomposed into smaller parts.

[0065] Next, for each decomposed boundary cluster, further analyze each cluster F i Sampling of observation points is done in cylindrical coordinates and eventually belongs to cluster F i All sampled observation points are represented in space as (x i ,y i ,z i ,θ i ), where the optimal sampling result is determined by a discretized two-level search around the boundary clusters.

[0066] Sampling consists of two parts. First, the geometric center p of the boundary cluster is used i As the center, each time the step length Δr is expanded outward, the sampling point (x i ,y i ,z i ). Secondly, at the first sampling point (x i ,y i ,z i ) is based on the step size of Δξ, and the sequence {-π,-π+Δξ,…,π-Δξ,π} is executed. At this time, each sampling will update the coverage of the sampling point to the boundary cluster, and the one with the largest coverage will be the ξ i It will be recorded as a yaw angle of the observation point at this location. Regarding the calculation of coverage, it is assumed that the sensor's field of view is a circular sector of a cylinder with a radius of R and a height of The central angle is h fov , where v fov and h fov Represents a field of view of the depth camera in the vertical and horizontal directions respectively. For each observation point, calculate the c of the boundary cluster contained in the field of view i The number of them can be used to estimate the coverage rate.

[0067] In some embodiments, when executing steps 104-105, the specific steps may be as follows:

[0068] For each boundary cluster, the observation point sequence of the boundary cluster is arranged in descending order, and the connection cost between the boundary clusters is calculated based on the first observation point.

[0069] According to the connection costs between each boundary cluster, an adjacency cost matrix for calculating the asymmetric TSP problem is constructed.

[0070] Specifically, based on the information of the observation points, an adjacency cost matrix between the drones and each boundary cluster is constructed, and a traveling salesman problem is constructed to obtain a global path traversing the observation points.

[0071] After the generation of the sampling points is completed, the i Observation point sequence VP i Sort in descending order and extract the observation points with the highest coverage to calculate a connection cost between the boundary clusters, with vp i =(x i ,y i ,z i ,ξ i ) as the boundary cluster F i The observation point with the highest coverage rate is denoted as P in space. i =(x i ,y i ,z i ).

[0072] In order to calculate the cost between different boundary clusters, the lower bound of the time between observation points corresponding to the boundary clusters in the grid map is used to express t lb (vp i ,vp j ), which is calculated as follows:

[0073]

[0074] Where, Path(P i ,P j ) represents the distance from the geometric center P i To the geometric center P j The collision-free path is searched using the A* algorithm; v max and Represent the maximum linear velocity and angular velocity of the UAV respectively, ξ i ,ξ j They represent the observation points with the largest coverage in different boundary clusters.

[0075] To find the optimal global path through these boundary clusters, after accounting for the cost of crossing them, this application formulates the Traveling Salesman Problem (TSP). This method aims to compute a global open-loop path from the current observation point to all remaining observation points. For ease of computation, the problem is simplified to a standard asymmetric Traveling Salesman Problem.

[0076] Assuming that the environment in which the drone is located contains a total of N boundary clusters to be covered, the adjacency cost matrix M for calculating the asymmetric TSP problem is constructed here. atsp The dimension of is N+1, and the main part of the matrix N*N is filled by the connection cost between the boundary clusters calculated previously:

[0077] M atsp (F i ,F j )=M atsp (F j ,F i )=t lb (vp i ,vp j )

[0078] i,j∈{1,2,…,N}

[0079] Then for M atsp The first row of the matrix is ​​filled with a moving cost from the current observation point vp0 to the remaining N observation points, which is calculated by the following formula:

[0080] M atsp (0,F i )=t lb (vp0,vp i )+w c c c (F i )

[0081] i∈{1,2,…,N}

[0082] Where w c Represents the penalty coefficient for the UAV’s round-trip motion from the starting point to the next observation point, t lb Represents the two observation points vp0,vp i The cost of moving between c (F i ) represents the continuity cost of path movement, which is generally calculated by the following formula:

[0083]

[0084] Here v0 represents the current speed. This parameter is introduced here because in some cases, the calculated global path may contain a similar time lower bound, which will cause the drone to swing back and forth, which is not conducive to the continuity of the movement. Therefore, c c (F i ) This is the penalty term introduced for large motion changes.

[0085] Secondly, for M atsp The first column is filled with the following formula:

[0086] M atsp (F i ,0)=0,F i ∈{0,1,2,…,N}.

[0087] By introducing the cost value in this way, we can solve M atsp In this problem, any closed loop returning to the starting observation point will not incur any additional cost, so the solution is to use a closed loop solution to represent the cost of the open loop global path.

[0088] When executing step 106, the specific steps may be as follows:

[0089] When a drone triggers a return event, the return trajectory of the drone is planned based on a path search method according to the drone's current state information and target state information to obtain the drone's return trajectory; the current state information includes the initial posture and environmental information; the target state information includes the terminal posture; the path search method includes the traditional A* search method; the drone's return trajectory is a collision-free path from the drone's starting posture to the terminal posture.

[0090] Specifically, the return event trigger and collision detector monitor the drone's flight status in real time and perform global path planning and correction for the return mission when necessary. Before performing an exploration mission, the return event trigger will preset a series of return trigger conditions, including but not limited to a preset flight time, reaching a specific location, or the sensor detecting a specific target. Once the return mechanism is triggered, the drone will initiate a kinematic path search to generate a collision-free path from the current position to the nearest return point for the downstream trajectory optimization module. At the same time, the collision detector continuously monitors the drone's status in the environment. Once it finds that the global path becomes infeasible, it will perform a path correction, essentially performing a local path search.

[0091] The path search method under dynamics performed in this embodiment is similar to the traditional A* search method. The detailed process is shown in the pseudo code of Algorithm 1. Similar to the classic A* search algorithm, the path search under kinematics also maintains an open set for storing the nodes to be explored. and store the closed set of explored nodes The difference is that the path search under kinematics does not expand nodes in the form of straight lines in the past, but selects potential expansion nodes in a way that conforms to the kinematic characteristics of the drone. The motion primitives formed by these expanded nodes are stored using nodes. The minimum path cost g to move to this node is also stored in this structure. c And consider the cost value f of the heuristic() c In the process of expanding nodes, nodes that collide with obstacles or expand to the same grid will be pruned. The above behavior will be executed until the end point is reached or the drone can reach the end point directly from the current node.

[0092]

[0093]

[0094] In the above process, the expansion of nodes is the key difference from previous path search methods. The differential flatness of the UAV allows the trajectory to be represented by three independent one-dimensional time parameterized polynomials:

[0095]

[0096] Here, μ = {x, y, z}. From the perspective of the UAV system after differential flattening, it represents a linear time-invariant control system, and its system equation can be expressed in the following form:

[0097]

[0098] here is a state variable, and is the control input of the system, and E3 is a unit vector.

[0099] We can further obtain the solution of the trajectory equation when the system's initial state is x(0) and the control input is u(t):

[0100]

[0101] Therefore, the process of node expansion is a discrete output at a given time interval τ. The result is a motion sequence.

[0102] Finally, the design of inspiration is also important for speeding up path search. The cost function of a trajectory is defined as:

[0103]

[0104] The optimal solution of the heuristic function is obtained by solving the optimal control problem under two boundary values:

[0105]

[0106] Here μc ,v μc ,p μc ,v μg Represent the current and end point positions and velocities respectively, and further by μ ,β μ Substitute H * (T), it can be solved to satisfy The minimum cost under the condition is T h Finally, the minimum cost value H * (T h ) is included in f as an inspiration c .

[0107] In some embodiments, when executing step 107, the specific steps may be as follows:

[0108] After obtaining the set of observation points given by the traveling salesman problem and the results of the path search after the return mission is triggered, a third-order B-spline curve is used to describe the operation trajectory of the UAV. The set of observation points given by the previous traveling salesman problem and the set of collision-free path points given by the search algorithm under kinematics are used as the control points of the B-spline curve. Furthermore, considering the constraints such as safety, trajectory smoothness, motion boundaries and total motion time under exploration and return missions, an optimization problem is established to optimize the position of the control points, and finally a feasible trajectory that meets the control operation requirements of the UAV is obtained.

[0109] Here we use the observation point set given by the previous traveling salesman problem and the collision-free path point set given by the kinematic search algorithm as the control points of the third-order B-spline curve, denoted as Q = {Q0, Q1, ..., Q N}, where Q i =(P i ,ξ i ), the order of B-spline is denoted as p b =3, and M=N+1+p b Thus, the B-spline curve can be parameterized by time t, where The uniform B-spline contains the same time span Δt. In order to obtain the parameterized time The position at time t is normalized here by s(t) = (tt m ) / Δt, so the B-spline curve under s(t) has the following form:

[0110]

[0111] Among them, the matrix It is expressed as follows:

[0112]

[0113] Furthermore, considering the constraints of safety, trajectory smoothness, motion boundaries, and total motion time in the exploration and return missions, the following optimization problem is established:

[0114] arg minf s +w t T+λ c f c +λ d (f v +f a ).

[0115] Where, f c 、f v and f a They represent the safety constraints, speed constraints, and acceleration constraints during the operation of the UAV; represents the total exploration time of the UAV; w t ,λ c ,λ d They represent the penalty factors of the constraint function under the soft constraint optimization problem, f s is the trajectory smoothness cost, which is calculated as follows:

[0116]

[0117] Here R s is the penalty matrix:

[0118]

[0119] The total time of the motion is determined by the total number of uniform B-spline segments:

[0120] T=(N+1-p b )·Δt.

[0121] Then f c ,f v and f a These are the constraints on safety, speed, and acceleration during the operation of the drone. For each control point, they have a similar expression:

[0122]

[0123] Therefore, the expression of trajectory safety constraint f is given c as follows:

[0124]

[0125] Here d(P i ) is the distance information from the current control point to the nearest obstacle, which can be obtained from the ESDF map in the mapping module, and the d here thr Represents a safe distance to avoid collision.

[0126] Where, f v and f a are penalty parameters set for exceeding the feasibility limits of the UAV motion, and they are given by:

[0127]

[0128] The derivative form of the control point here is:

[0129]

[0130] Solve for f c 、f v 、f a The formula utilizes the convex hull property of the B-spline curve, so the safety and motion feasibility of the UAV can be restricted by the constraints of the control points.

[0131] Example 2

[0132] like Figure 4 As shown, this embodiment provides a UAV exploration and fixed-point return mission relay system, including:

[0133] The information acquisition module 401 is used to obtain drone state estimation information and sensor depth information.

[0134] The map construction module 402 is used to construct an environment map based on the drone state estimation information and the sensor depth information; the environment map is composed of explored areas and unexplored areas.

[0135] The boundary clustering module 403 is configured to perform boundary clustering on the boundaries of the environment map using a boundary detector according to the environment map, and obtain observation points of a plurality of boundary clusters.

[0136] The adjacency module 404 is used to construct an adjacency cost matrix between the UAV and each boundary cluster based on the information of the observation point.

[0137] The path solving module 405 is used to solve the global open-loop path of each observation point based on the adjacency cost matrix between the drone and each boundary cluster using the standard asymmetric traveling salesman problem; the global open-loop path is the path from the current observation point to the remaining observation points.

[0138] The path planning module 406 is used to monitor the status of the UAV based on a return event trigger and a collision detector; the triggering events in the return event trigger include a fixed flight time, a fixed flight position, and a fixed target.

[0139] When the drone triggers a return event, the return trajectory of the drone is planned based on the global open-loop path.

[0140] When a UAV collides, the return mission is initiated, and the return trajectory of the UAV is planned based on the global open-loop path.

[0141] The trajectory optimization module 407 is used to optimize the return trajectory of the UAV based on a trajectory optimization model to obtain a smooth trajectory of the UAV; the trajectory optimization model includes multiple constraints.

[0142] In summary, this application has the following technical effects:

[0143] (1) This application introduces different event triggers to manage the entire UAV exploration and return mission, fully considering the various situations in the UAV's mission execution process. The design of this mechanism also allows the UAV to pre-set its own operating conditions before executing the mission. For example, for the return event trigger, various situations can be set, such as timed return, return after detecting a specific target, etc., thereby ensuring the freedom of the UAV to execute the mission to the greatest extent. At the same time, the design of this trigger can also ensure that multiple potential tasks are detected in parallel, so that the UAV can achieve smooth switching and task relay during the execution of different tasks.

[0144] (2) This application is designed for drone path planning under different behavioral requirements, so that drones can better adapt to their corresponding tasks. First, for exploration tasks, after the boundary information is updated, the drone will perform two-layer sampling of position and yaw angle according to the location of the boundary, which maximizes the effectiveness of the observation points after sampling. The solution to the traveling salesman problem obtained by the adjacency matrix established by these observation points can also better guide the drone to complete the exploration task. Secondly, for the return mission, the kinematic A* path search method used can find a path that satisfies the kinematic constraints and has the minimum cost, reducing the load of subsequent trajectory optimization.

[0145] (3) This application uses a soft constraint method for back-end trajectory optimization, and flexibly handles complex constraints such as smoothness and obstacle avoidance requirements through nonlinear optimization, so that the generated trajectory has good continuity and safety. In addition, the soft constraint method can use gradient information to effectively push the trajectory away from obstacles, so that it can also have a good effect in local path planning and dynamic environments. It can quickly respond to and adjust to newly emerging obstacles or targets. In addition, soft constraints can also integrate multiple optimization objectives to achieve a trade-off between different requirements, thereby adapting to different trajectory optimization needs.

[0146] The technical features of the above embodiments can be combined arbitrarily. To make the description concise, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0147] This document uses specific examples to illustrate the principles and implementation methods of this application. The description of the above examples is only intended to help understand the method and core concept of this application. At the same time, for those skilled in the art, based on the concept of this application, there may be changes in the specific implementation methods and application scope. In summary, the content of this specification should not be understood as limiting this application.

Claims

1. A method for relaying UAV exploration and fixed-point return missions, characterized in that: include: Obtain drone state estimation information and sensor depth information; Build an environment map based on the drone state estimation information and sensor depth information; The environment map is composed of explored areas and unexplored areas; According to the environment map, a boundary detector is used to perform boundary clustering on the boundary of the environment map to obtain observation points of a plurality of boundary clusters; Based on the information of the observation points, the adjacency cost matrix between the drones and each boundary cluster is constructed; Based on the adjacency cost matrix between drones and each boundary cluster, the standard asymmetric traveling salesman problem is used to solve the global open-loop path of each observation point. The global open-loop path is the path from the current observation point to the remaining observation points; Monitoring the status of the drone based on a return event trigger and a collision detector; the triggering events in the return event trigger include a fixed flight time, a fixed flight position, and a fixed target; When the drone triggers a return event, the return trajectory of the drone is planned based on the global open-loop path; When a UAV collides, a return mission is initiated, and a path planning is performed on the return trajectory of the UAV based on a global open-loop path; Based on a trajectory optimization model, the return trajectory of the UAV is optimized to obtain a smooth trajectory of the UAV; the trajectory optimization model includes multiple constraint conditions.

2. A method for relaying UAV exploration and fixed-point return missions according to claim 1, characterized in that: The drone state estimation information includes position parameters, velocity parameters, acceleration parameters and attitude parameters; the sensor depth information includes the point cloud of the laser radar and the depth map of the depth camera.

3. The method for relaying UAV exploration and fixed-point return missions according to claim 1, characterized in that: Based on the drone state estimation information and sensor depth information, an environment map is constructed, including: The projection method is used to integrate the sensor depth information into the occupancy grid map, and all state grids with changed states are added to the insertion queue and the deletion queue respectively; Merge the insert queue and delete queue into the update queue; Based on the breadth-first search algorithm, the Euclidean distance field map is updated and the status grid in the update queue is processed; The environment map is constructed based on an index data structure that maps the three-dimensional state grid coordinates and a doubly linked list for efficient insertion and deletion operations.

4. The method for relaying UAV exploration and fixed-point return missions according to claim 1, characterized in that: According to the environment map, a boundary detector is used to perform boundary clustering on the boundaries of the environment map to obtain observation points of several boundary clusters, specifically including: When the map is updated, the boundary detector is triggered to perform a boundary clustering operation to generate a boundary cluster; the boundary cluster consists of several sub-grids and a geometric center; Based on the boundary clusters, several observation points from the perspective of the UAV's motion are generated; the observation points are obtained by sampling two layers of information and evaluating the unknown boundary cluster coverage of each observation point.

5. The method for relaying UAV exploration and fixed-point return missions according to claim 1, characterized in that: Based on the information of the observation points, the adjacency cost matrix between the drone and each boundary cluster is constructed, including: For each boundary cluster, the observation point sequence of the boundary cluster is arranged in descending order, and the connection cost between the boundary clusters is calculated based on the first observation point; According to the connection costs between each boundary cluster, an adjacency cost matrix for calculating the asymmetric TSP problem is constructed.

6. A method for relaying UAV exploration and fixed-point return missions according to claim 5, characterized in that: For each boundary cluster, the observation point sequence of the boundary cluster is sorted in descending order, and the connection cost between the boundary clusters is calculated based on the first observation point, specifically including: According to the formula Calculate the connection cost between boundary clusters; Among them, Path(P i ,P j ) represents the distance from the geometric center P i To the geometric center P j The collision-free path, v max and Represent the maximum linear velocity and angular velocity of the UAV respectively, ξ i ,ξ j Represents the observation point with the largest coverage in different boundary clusters, t lb Represents the two observation points vp0,vp i The connection cost between .

7. The method for relaying UAV exploration and fixed-point return missions according to claim 5, characterized in that: According to the connection costs between each boundary cluster, the adjacency cost matrix for calculating the asymmetric TSP problem is constructed, including: According to formula M atsp (0,F i )=t lb (vp0,vp i )+w c c c (F i ), move and fill the first row of the adjacency cost matrix; According to formula M atsp (F i ,0)=0, move and fill the first column of the adjacency cost matrix; Among them, c c (F i ) represents the continuity cost of path movement, i∈{1,2,…,M}, F i ∈{0,1,2,…,N}, v0 represents the current velocity, N is the number of boundary clusters to be covered, w c Represents the penalty coefficient for the UAV’s round-trip motion from the starting point to the next observation point, t lb Represents the two observation points vp0,vp i The cost of movement between .

8. The method for relaying UAV exploration and fixed-point return missions according to claim 7, characterized in that: When a drone triggers a return event, the return trajectory of the drone is planned based on the global open-loop path, specifically including: When a drone triggers a return event, the return trajectory of the drone is planned based on a path search method according to the drone's current state information and target state information to obtain the drone's return trajectory; the current state information includes the initial posture and environmental information; the target state information includes the terminal posture; the path search method includes the traditional A* search method; the drone's return trajectory is a collision-free path from the drone's starting posture to the terminal posture.

9. The method for relaying UAV exploration and fixed-point return missions according to claim 1, characterized in that: The optimization problem of the trajectory optimization model is specifically: arg minf s +w t T+λ c f c +λ d (f v +f a ); Among them, f s represents the trajectory smoothness cost; f c 、f v and f a They represent the safety constraint, speed constraint and acceleration constraint during the operation of the UAV; T represents the total exploration time of the UAV; w t ,λ c ,λ d They represent the penalty factors of the constraint function under the soft constraint optimization problem.

10. A UAV exploration and fixed-point return mission relay system, characterized in that: include: Information acquisition module, used to obtain drone state estimation information and sensor depth information; A map construction module is used to construct an environment map based on the drone state estimation information and sensor depth information; the environment map is composed of explored areas and unexplored areas; A boundary clustering module is used to perform boundary clustering on the boundaries of the environment map using a boundary detector according to the environment map to obtain observation points of a plurality of boundary clusters; The adjacency module is used to construct the adjacency cost matrix between drones and each boundary cluster based on the information of the observation points; The path solving module is used to solve the global open-loop path of each observation point based on the adjacency cost matrix between the drones and each boundary cluster, using the standard asymmetric traveling salesman problem; The global open-loop path is the path from the current observation point to the remaining observation points; A path planning module that monitors the drone's status based on return event triggers and collision detectors; The triggering events in the return event trigger include fixed flight time, fixed flight position and fixed target; When the drone triggers a return event, the return trajectory of the drone is planned based on the global open-loop path; When a UAV collides, a return mission is initiated, and a path planning is performed on the return trajectory of the UAV based on a global open-loop path; The trajectory optimization module is used to optimize the return trajectory of the UAV based on a trajectory optimization model to obtain a smooth trajectory of the UAV; the trajectory optimization model includes multiple constraints.

Images