Multi-uav cooperative hunting trajectory planning method and system based on three-dimensional vine diagram

Abstract

This invention discloses a multi-UAV cooperative encirclement trajectory planning method and system based on 3D Venographs, including UAV and target motion state information perception, front-end point-to-point trajectory planning based on 3D Venograph calculation, and back-end trajectory optimization. In the point-to-point trajectory planning, GPU-accelerated 3D Venographs are used to model and divide the positions and spatial locations of the UAV and target into cells, and the target point is calculated according to a centroid strategy that guarantees convergence. In the back-end trajectory optimization, a constrained optimization problem that guarantees target visibility is constructed, reparameterized, and the constraints of the optimization problem are transformed into unconstrained optimization terms, and solved using numerical optimization methods. This invention enables flexible and theoretically sound multi-UAV cooperative target encirclement trajectory planning, making encirclement decision-making more efficient, trajectory planning more reliable, and solution speed faster.

Description

Technical Field

[0001] This invention belongs to the field of unmanned aerial vehicle (UAV) technology, specifically relating to a method and system for multi-UAV cooperative encirclement trajectory planning based on three-dimensional Venn diagrams. Background Technology

[0002] Currently, multi-drone collaborative technology has wide applications in various fields such as entertainment, industry, and security. Examples include creating aerial landscapes using multi-drone formations, transporting objects collaboratively, and apprehending illegally intruding drones, suspicious vehicles, and other moving targets. By incorporating advanced perception and navigation technologies, drones can achieve environmental perception, target tracking, and encirclement. During the encirclement process, drones need to plan the optimal distributed encirclement trajectory based on the target's location and their own position distribution. This involves how drones narrow the encirclement and how to ensure the trajectory meets dynamic constraints while keeping the target visible within their field of vision. In general, motion planning plays a decisive role in the collaborative encirclement of dynamic targets by multiple drones, helping them achieve accurate and efficient encirclement cooperation in a distributed manner in the shortest possible time, thus improving the efficiency and success rate of the encirclement. However, current technical solutions suffer from problems such as a lack of 3D implementation, failure to consider visibility in planning, and slow CPU computation. Summary of the Invention

[0003] To address the shortcomings of existing technologies, this invention, based on the theoretical basis of the fastest shrinking target cell in a three-dimensional Venn diagram, achieves the goal of distributed trajectory planning for multi-UAV collaborative target acquisition while maintaining target visibility within the field of view, thus satisfying dynamic constraints. The technical solution adopted by this invention is as follows:

[0004] The multi-UAV cooperative encirclement trajectory planning method based on 3D Vinograph includes the following steps:

[0005] Step S1: Obtain the current motion status of the drone and the target in the environment.

[0006] Step S2: Using the UAV and target positions as seeds and the set Venn diagram boundaries as boundaries, the UAV and target positions and their surrounding space are modeled and divided into cells, and the three-dimensional Venn diagram at the current time t is calculated.

[0007] Step S3: Based on the three-dimensional Venn diagram, calculate the next target point for the UAV to approach the nearest target;

[0008] Step S4: Based on the execution constraints and visibility constraints of the UAV, construct a trajectory optimization model using the UAV's motion state and the next target point to minimize control input and time;

[0009] Step S5: Based on the minimum control theory under geometric constraints, the initial trajectory is reparameterized to obtain the reparameterized trajectory;

[0010] Step S6: Transform the constrained trajectory optimization model into an unconstrained optimization model;

[0011] Step S7: The initial trajectory connecting the target point and the initial point obtained in step S3 is used as the initial value for numerical optimization. The unconstrained optimization model is solved by the quasi-Newton method to obtain the target trajectory. The UAV performs target cooperative encirclement and capture based on the target trajectory.

[0012] Furthermore, in step S1, the motion state information includes position and velocity in each direction. The UAV deploys a distributed decision-making and planning algorithm, and the UAVs exchange their motion states and trajectory plans with each other via wireless communication. i (t) and the observed target motion state z k .

[0013] The target state is acquired by a visual sensor mounted on the drone, and its future T is predicted using a linear acceleration model. p The state of motion over a period of time.

[0014] Furthermore, in step S2, since this stage assumes that there are no obstacles in the current environment, the Venn diagram boundary is set as the circumscribed cube of any recorded drone arrival location, and the safety boundary is expanded by 2m in each of the three dimensions.

[0015] The calculation of 3D Veno maps uses GPU hardware combined with GPU-accelerated Veno map calculation methods. It decomposes the calculation task of Veno map into independent calculations of each cell, and achieves efficient parallel computing by processing multiple cells simultaneously, which significantly speeds up the calculation speed of Veno map.

[0016] Furthermore, in step S3, after calculation and proof based on gradient descent theory, setting the UAV's forward direction to the centroid of the boundary surface between the UAV and the target's Vinocells maximizes the reduction of the enemy's Vinocell spatial volume. Therefore, the next target point of the UAV is set as the centroid C of the boundary surface between the UAV and the target's Vinocells. b .

[0017] Furthermore, the trajectory optimization model in step S4 is as follows:

[0018]

[0019] Where p(t) represents the trajectory of the drone, p (3) (t) denotes the third derivative of the trajectory p(t), ρ denotes the weight, T denotes the trajectory execution time, p(0) denotes the initial state of the trajectory, and P t C represents the motion state of the drone at time t.b Let D represent the target point, and let V represent the dynamic control variables of the UAV (including velocity, angular velocity, and thrust). k ={x∈R| <x-z k ,l k >≤θ k + indicates the drone's activity area when the target is always within the drone's field of view (FoV), x indicates the drone's position constrained by visibility, and z k Indicates the predicted location of the target, l k θ represents the centerline of the field of view (FoV) sector of the UAV. k T represents the included angle of the sector of the drone's field of view (FoV). p Indicates the time range for predicting the target's motion;

[0020] Equation (1) represents the general form of balancing smoothness and aggression, aiming to minimize control quantity and time cost; Equation (2) indicates that time must be greater than 0; Equation (3) indicates that the starting point of the trajectory is the initial state at time t, and the ending point is the target point (centroid of the boundary surface of the Vinograph cell); Equation (4) indicates the actuator constraint, meaning that the speed, angular velocity, and thrust cannot exceed the maximum value set by the user; Equation (5) indicates that the UAV must keep the target within the UAV's field of view (FoV) during the capture process.

[0021] Furthermore, in step S5, based on the minimum control theory under geometric constraints, the overall objective J is optimized by calculating the coefficient c. i and time T i The gradient of the trajectory polynomial (in its mathematical form) is used to optimize the intermediate point q and time T along the trajectory. i (Actual physical space), optimize the trajectory in three-dimensional space, where i represents the segment of the i-th trajectory division, c i Let T represent the coefficient matrix of the i-th segment. i This represents the duration of the i-th segment.

[0022] Furthermore, in step S6, equations (1) to (5) are transformed into an unconstrained optimization problem, wherein the constraints of equation (2) are determined by... Eliminate, e represents a mathematical constant, t i This represents the new optimization variable, which in turn optimizes T. i The problems are all transformed into optimizing t i The problem.

[0023] Furthermore, in step S6, the gradient calculations of equations (1) and (3) are derived from the characteristics of the minimum control theory under geometric constraints; and equation (4) is calculated using mathematical theory.

[0024] Furthermore, in step S6, equation (5) is transformed using the penalty function method to obtain the objective function of the penalty function. in Indicates by V k ={x∈R| <x-z k ,l k >≤θ k Add an adjustable angular gap θ l The resulting constraint equation, where k represents the k-th target location predicted during the perception phase, and M... T Represents the total number of predicted target points; Definition The penalty function and objective function J are obtained according to the chain rule. o For the coefficient matrix c respectively i Time T i gradient Known Where β(t) = (1, t, ..., t) N Let c be the natural basis of time; then, using the penalty function method, the objective function of equation (5) with respect to the number c is calculated. i and time T i The gradient.

[0025] A multi-UAV collaborative encirclement trajectory planning system based on 3D Veno maps includes multiple UAVs that cooperate with each other. Each UAV includes a state acquisition module, a 3D Veno map generation module, a target point calculation module, a trajectory optimization module, a reparameterization module, a transformation module, and a target trajectory generation module.

[0026] The state acquisition module is used to acquire the current motion state of the UAV and the target in the environment.

[0027] The three-dimensional Veno map generation module uses the UAV and target positions as seeds and the set Veno map boundaries as boundaries to model and divide the UAV and target positions and their surrounding space into cells, and calculates the three-dimensional Veno map at the current time t.

[0028] The target point calculation module calculates the next target point for the UAV to approach the nearest target based on the three-dimensional Venn diagram;

[0029] The trajectory optimization module, based on the execution constraints and visibility constraints of the UAV, constructs a trajectory optimization model using the UAV's motion state and the next target point to minimize control input and time.

[0030] The reparameterization module, based on the minimum control theory under geometric constraints, reparameterizes the initial trajectory to obtain the reparameterized trajectory.

[0031] The conversion module converts the constrained trajectory optimization model into an unconstrained optimization model.

[0032] The target trajectory generation module takes the initial trajectory connecting the target point and the initial point obtained in step S3 as the initial value for numerical optimization, solves the unconstrained optimization model using the quasi-Newton method to obtain the target trajectory, and outputs the target trajectory to the UAV controller to perform cooperative encirclement and capture of the target.

[0033] The advantages and beneficial effects of this invention are as follows:

[0034] This invention enables theoretically sound collaborative and efficient encirclement trajectory planning for multiple UAVs, making encirclement decisions more efficient and trajectory planning more reliable, thus ensuring that the target will be captured within a finite time. Furthermore, GPU computing accelerates the solution process. Through dynamic and visibility constraints in the distributed algorithm backend, this invention guarantees the flexible and autonomous reliable trajectory planning properties of individual UAVs in collaborative multi-UAV operations. Finally, by transforming a constrained optimization problem into an unconstrained optimization problem and solving it using numerical methods, this invention improves the solution speed for trajectory optimization. Attached Figure Description

[0035] Figure 1 This is a flowchart of the method in an embodiment of the present invention.

[0036] Figure 2 This is a schematic diagram of modeling and cell partitioning in point-to-point trajectory planning based on three-dimensional Venn diagrams in an embodiment of the present invention.

[0037] Figure 3 This is a schematic diagram of the device in an embodiment of the present invention. Detailed Implementation

[0038] The specific embodiments of the present invention will be described in detail below with reference to the accompanying drawings. It should be understood that the specific embodiments described herein are for illustration and explanation only and are not intended to limit the present invention.

[0039] like Figure 1 As shown, the multi-UAV cooperative target encirclement trajectory planning method based on 3D Veno maps, by activating UAVs and utilizing the position and velocity information of the target and UAVs output by the motion capture system, implements a trajectory planning method for UAVs to intercept high-speed targets. Specifically, it includes the following steps:

[0040] Step S1: Perceive the motion status information of friendly and enemy drones in the current environment, including position and velocity in each direction. Each friendly drone deploys the distributed decision-making and planning algorithm described in this invention, and simultaneously exchanges the motion status and trajectory planning information of the friendly drone via WIFI communication at a frequency of f Hz. i(t) and the observed motion state of the enemy drone z k The enemy drone's status is determined by the visual sensors onboard the friendly drone, and its future T-wave velocity is predicted using a linear acceleration model. p The state of motion over a period of time.

[0041] Step S2: Calculate the 3D Vinograph at time t, using the positions of friendly and enemy drones as seeds and the defined Vinograph boundaries as limits, such as... Figure 2 As shown. Since this stage assumes no obstacles in the current environment, the boundary of the Venn diagram is set as the circumscribed cube of any recorded UAV arrival location, and the safety boundary is expanded by 2m in each of the x, y, and z directions. The 3D Venn diagram calculation is implemented in both GPU and CPU versions. The GPU version uses GPU hardware combined with a GPU-accelerated Venn diagram calculation method. This method decomposes the Venn diagram calculation task into independent calculations of each cell and achieves efficient parallel computation by processing multiple cells simultaneously, significantly accelerating the Venn diagram calculation speed. The CPU version is implemented using the Voro++ library.

[0042] Step S3: Based on the characteristics of the 3D Vinograph, calculate the next target point C that the friendly UAV will approach the nearest enemy UAV. b Specifically, through gradient descent theory calculations and proofs, setting the forward direction of the friendly UAV as the centroid of the boundary surface between the friendly UAV and the target enemy UAV's Vino cell maximizes the reduction of the enemy's Vino cell volume. Therefore, setting the target point of the friendly UAV at this moment as the centroid C of the boundary surface between its own UAV and the target enemy UAV's Vino cell... b .

[0043] Step S4: Construct an optimization problem to solve for the distance from the current point (time t) to the target point C of our own UAV. b The trajectory planning and optimization problem is as follows:

[0044]

[0045] Where, p (3) (t) represents the third derivative of the trajectory, ρ represents the weight, T represents the trajectory execution time, p(0) represents the initial state of the trajectory, and P t C represents the motion state of our drone at time t. b This represents the target point calculated in step S3. D represents the dynamic control variables of the user's UAV (including velocity, angular velocity, and thrust). V k ={x∈R| <x-z k ,l k >≤θ k} represents the area of ​​operation of the friendly drone when the enemy drone is always within the field of view (FoV) of the friendly drone, where x represents the position of the drone subject to visibility constraints, and z k Indicates the predicted motion state of the enemy drone, l k θ represents the centerline of the FoV sector of our own UAV. k T represents the included angle of the FoV sector of our own drone. p The time range for predicting the movement of the target enemy UAV is indicated. Equation (1) represents the general form of balancing smoothness and aggression, aiming to minimize control quantity and time cost; Equation (2) indicates that time must be greater than 0; Equation (3) indicates that the starting point of the trajectory is the initial state at time t, and the ending point is the target point (centroid of the boundary surface of the Vinograph cell) calculated in step S3; Equation (4) indicates the actuator constraint, meaning that the speed, angular velocity, and thrust cannot exceed the maximum value set by the user; Equation (5) indicates that the friendly UAV must keep the target within the field of view of FoV during the encirclement process.

[0046] Step S5: Trajectory Reparameterization. The initial trajectory is reparameterized using Minimum Control Theory (MINCO) under geometric constraints. MINCO can optimize the coefficient matrix c by optimizing the overall objective J. i and time T i (The mathematical form of the trajectory polynomial, where i represents the i-th term of the polynomial, MINCO divides the trajectory into M segments, c) i The coefficient matrix of the i-th segment is represented by Ti, which in turn represents the gradient of the time length of the i-th segment. This allows us to optimize the intermediate point q and time T along the trajectory. i (The actual physical space). Therefore, it is only necessary to calculate the coefficient c of the overall optimization objective. i and time T i By calculating the gradient, the trajectory can be optimized in three-dimensional space. This step takes the initial trajectory as input and outputs the reparameterized MINCO trajectory.

[0047] Step S6: Transformation of Constrained Optimization Problem. The purpose of this step is to transform equation (1-5) into an unconstrained optimization problem. The constraints in equation (2) can be... Eliminate, e is a mathematical constant, t i As new optimization variables, and thus optimize T i All problems can be transformed into optimizing t i The problem is that the gradient calculations of equations (1) and (3) are derived from the characteristics of MINCO. Equation (4) is calculated using mathematical theory. Equation (5) uses the penalty function method for constraint transformation, and the objective function of the penalty function is obtained as follows: in For V k ={x∈R| <x-z k ,lk >≤θ k Add an adjustable angular gap θ l The resulting constraint equation, where k is the k-th target location predicted during the perception phase, and M... T To predict the total number of target points. Definition. According to the chain rule, the objective function J of the penalty function can be obtained. o For the coefficient matrix c respectively i Time T i gradient Known Where β(t) = (1, t, ..., t) N Let c be the natural basis of time. Then, using the penalty function method, the objective function of equation (5) can be calculated with respect to the number c. i and time T i The gradient. Finally, equation (1-5) of the constrained optimization problem is transformed into an unconstrained optimization problem.

[0048] Step S7: Solving the trajectory optimization problem. The transformed unconstrained optimization problem is solved using the quasi-Newton method. The initial trajectory connecting the target point and the initial point calculated in step S3 is used as the initial value for numerical optimization.

[0049] Finally, the target trajectory is output to the drone's controller for execution.

[0050] like Figure 3 As shown, the multi-UAV collaborative encirclement trajectory planning system based on three-dimensional Veno maps includes multiple UAVs that cooperate with each other. Each UAV includes a state acquisition module, a three-dimensional Veno map generation module, a target point calculation module, a trajectory optimization module, a reparameterization module, a transformation module, and a target trajectory generation module.

[0051] The status acquisition module is used to acquire the current motion status of the drone and the target in the environment.

[0052] The 3D Vinograph generation module uses the UAV and target positions as seeds and the set Vinograph boundaries as boundaries to model and divide the UAV and target positions and their surrounding space into cells, and calculates the 3D Vinograph at the current time t.

[0053] The target point calculation module calculates the next target point for the UAV to approach the nearest target based on the three-dimensional Venn diagram;

[0054] The trajectory optimization module, based on the execution constraints and visibility constraints of the UAV, constructs a trajectory optimization model using the UAV's motion state and the next target point to minimize control input and time.

[0055] The reparameterization module, based on the minimum control theory under geometric constraints, reparameterizes the initial trajectory to obtain the reparameterized trajectory.

[0056] The conversion module transforms the constrained trajectory optimization model into an unconstrained optimization model.

[0057] The target trajectory generation module takes the initial trajectory connecting the target point and the initial point obtained in step S3 as the initial value for numerical optimization, solves the unconstrained optimization model using the quasi-Newton method to obtain the target trajectory, and outputs the target trajectory to the UAV controller to perform cooperative encirclement and capture of the target.

[0058] The implementation methods for this part are similar to those for the methods described above, and will not be repeated here.

[0059] The above embodiments are only used to illustrate the technical solutions of the present invention, and are not intended to limit them. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some or all of the technical features therein. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention. For example, they can be used for any motion planning for the purpose of shrinking the target Veno diagram cell.

Claims

1. A multi-UAV cooperative encirclement trajectory planning method based on three-dimensional Venn diagrams, characterized in that... Includes the following steps: Step S1: Obtain the current motion status of the drone and the target in the environment; Step S2: Using the UAV and target positions as seeds and the defined Venn diagram boundaries as limits, the positions of the UAV and target and their surrounding space were modeled and divided into cells. The current... A three-dimensional Vinograph at time; The boundary of the Venn diagram is set as the circumscribed cube of any recorded UAV arrival position, and the safety boundary is expanded in each of the three dimensions. The calculation of the 3D Venn diagram uses GPU hardware combined with a GPU-accelerated Venn diagram calculation method, decomposing the Venn diagram calculation task into independent calculations of each cell, and achieving efficient parallel computation by processing multiple cells simultaneously. The next target point of the UAV is set as the centroid of the boundary surface between its UAV and the target's Venn cell. ; Step S3: Based on the three-dimensional Venn diagram, calculate the next target point for the UAV to approach the nearest target; Step S4: Based on the execution constraints and visibility constraints of the UAV, and using the UAV's motion state and the next target point, construct a trajectory optimization model to minimize the control input and time; the trajectory optimization model is as follows: in, Indicates the trajectory of the drone. Representing the trajectory The third derivative, Indicates weight, Indicates the trajectory execution time. This represents the initial state of the trajectory. Indicates that drones are in The state of motion at any given moment Indicates the target point. Represents the dynamic control variables of the drone. This represents the drone's activity area when the target remains within the drone's field of view, and x represents the drone's position constrained by visibility. Indicates the predicted location of the target. This represents the centerline of the fan-shaped field of view of the drone. This represents the fan-shaped field of view of the drone. included angle Indicates the time range for predicting the target's motion; Equation (1) represents the general form of balancing smoothness and aggression, aiming to minimize control quantity and time cost; Equation (2) indicates that time must be greater than 0; Equation (3) indicates that the starting point of the trajectory is The initial state at time, the endpoint is the target point; Equation (4) represents the actuator constraint; Equation (5) represents that the UAV must keep the target within the UAV's field of view during the capture process; Step S5: Based on the minimum control theory under geometric constraints, the initial trajectory is reparameterized to obtain the reparameterized trajectory; Step S6: Transform the constrained trajectory optimization model into an unconstrained optimization model; Equation (5) is transformed using the penalty function method to obtain the objective function of the penalty function. Indicates by Add an adjustable angle gap The resulting constraint equations, in which The first prediction in the perception stage Target location points, Represents the total number of predicted target points; Definition The penalty function and objective function are obtained according to the chain rule. For the coefficient matrix respectively ,time gradient ; Known ,in Given a natural time basis, the objective function of equation (5) can be calculated using the penalty function method with respect to the coefficient matrix. and duration The gradient; Step S7: Use the initial trajectory connecting the target point and the initial point obtained in step S3 as the initial value for numerical optimization, solve the unconstrained optimization model to obtain the target trajectory, and the UAV performs target cooperative encirclement and capture based on the target trajectory.

2. The multi-UAV cooperative encirclement trajectory planning method based on three-dimensional Venn diagrams according to claim 1, characterized in that: In step S1, the motion state information includes position and velocity in each direction. The UAV deploys a distributed decision planning algorithm, and the UAVs exchange motion state, trajectory planning and observed target motion state with each other through wireless communication. The target state is acquired by the visual sensors on the drone, and its motion state in the future is predicted using a linear acceleration model.

3. The multi-UAV cooperative encirclement trajectory planning method based on three-dimensional Venn diagrams according to claim 1, characterized in that: In step S5, based on the minimum control theory under geometric constraints, the overall objective is optimized by calculation. For coefficients and duration The gradient is used to optimize the intermediate points along the trajectory. and duration The trajectory is optimized in three-dimensional space, where i represents the segment of the i-th trajectory. This represents the coefficient matrix of the i-th segment. This represents the duration of the i-th segment.

4. The multi-UAV cooperative encirclement trajectory planning method based on three-dimensional Venn diagrams according to claim 3, characterized in that: In step S6, equations (1) to (5) are transformed into an unconstrained optimization problem, wherein the constraints of equation (2) are resolved through... Eliminate, e represents a mathematical constant, t i This represents the new optimization variable, which in turn optimizes... The problems were all transformed into optimizations. The problem.

5. The multi-UAV cooperative encirclement trajectory planning method based on three-dimensional Venn diagrams according to claim 3, characterized in that: In step S6, the gradients of equations (1) and (3) are calculated using the characteristics of the minimum control theory under geometric constraints; equation (4) is calculated using mathematical theory.

6. A multi-UAV cooperative encirclement trajectory planning system based on three-dimensional Venn diagrams, comprising multiple cooperating UAVs, characterized in that: The UAV includes a state acquisition module, a three-dimensional Veno map generation module, a target point calculation module, a trajectory optimization module, a reparameterization module, a transformation module, and a target trajectory generation module. It adopts the multi-UAV cooperative encirclement trajectory planning method based on three-dimensional Veno map as described in claim 1 to perform multi-UAV cooperative encirclement. The state acquisition module is used to acquire the current motion state of the UAV and the target in the environment. The 3D Vinograph generation module, using the UAV and target positions as seeds and the set Vinograph boundaries as limits, models and divides the UAV and target positions and their surrounding space into cells, and calculates the current... A three-dimensional Vinograph at time; The target point calculation module calculates the next target point for the UAV to approach the nearest target based on the three-dimensional Venn diagram; The trajectory optimization module, based on the execution constraints and visibility constraints of the UAV, constructs a trajectory optimization model using the UAV's motion state and the next target point to minimize control input and time. The reparameterization module, based on the minimum control theory under geometric constraints, reparameterizes the initial trajectory to obtain the reparameterized trajectory. The conversion module converts the constrained trajectory optimization model into an unconstrained optimization model. The target trajectory generation module uses the initial trajectory connecting the next target point and the initial point as the initial value for numerical optimization, solves the unconstrained optimization model to obtain the target trajectory, and outputs the target trajectory to the UAV controller to perform cooperative encirclement and capture of the target.

Images