A model predictive control-based unmanned aerial vehicle formation trajectory planning method

Abstract

This invention provides a method for UAV formation trajectory planning based on model predictive control. The method includes: determining the centroid motion model of a single UAV using its 3D state vector, a predefined state matrix, a position reference input vector, and a predefined input matrix; obtaining a discrete linear time-invariant system model of the UAV formation based on the centroid motion model of each UAV; designing a trajectory planning target for the UAV formation from its starting point to its target endpoint using the 3D state vector, centroid motion model, and discrete linear time-invariant system model of the UAV formation; and predicting the trajectory of a single UAV that satisfies the trajectory planning target using a distributed model predictive control algorithm, and applying a specified set of elements of the predicted trajectory to the centroid motion model of the single UAV to update its 3D state vector.

Description

Technical Field

[0001] This invention relates to the field of trajectory planning for quadcopter drone formations, and particularly to a model predictive control-based method for planning the trajectory of quadcopter drone formations in complex obstacle environments. Background Technology

[0002] In recent years, due to the ability of multi-UAV collaborative operations to fully utilize the limited onboard resources of a single UAV to complete more complex tasks, UAV swarms have been widely used in search and rescue, agricultural and forestry operations, and target acquisition, and have become an indispensable backbone in key future fields. Therefore, research on online trajectory planning for multi-UAV swarms in complex scenarios has attracted widespread attention, and it is urgent to propose a trajectory planning method for multi-UAV swarms suitable for complex environments.

[0003] The trajectory planning problem for multiple unmanned aerial vehicles (UAVs) is essentially a complex optimal control problem influenced by multiple constraints, variables, and nonlinearities. Currently, this problem is mainly divided into two categories based on the solution method: centralized solution and distributed solution. While existing centralized solution-based UAV formation trajectory planning techniques effectively solve the multi-UAV trajectory planning problem, the computational complexity increases dramatically with the number of UAVs, and the success rate decreases rapidly. Existing distributed solution-based UAV formation trajectory planning techniques, while enabling collaborative work among multiple UAVs and improving overall task execution efficiency and safety, may suffer from reduced planning accuracy in complex dynamic environments and under various obstacle conditions, potentially leading to local optima rather than global optima. Furthermore, distributed planning algorithms require ensuring information exchange and synchronization between UAVs, which can increase communication overhead and complexity for large-scale UAV swarms. Summary of the Invention

[0004] In view of the above problems, the present invention provides a model predictive control-based method for planning the formation trajectory of quadcopter UAVs in complex obstacle environments, in order to at least solve one of the above problems.

[0005] According to an embodiment of the present invention, a method for planning unmanned aerial vehicle (UAV) formation trajectories based on model predictive control is provided, comprising:

[0006] Based on the principle of discrete-time linear invariance, the centroid motion model of a single UAV is determined by using the three-dimensional state vector of a single UAV, the predefined state matrix, the position reference input vector, and the predefined input matrix. Based on the centroid motion model of each UAV, the discrete linear time-invariant system model of the UAV formation is obtained.

[0007] Based on the user's predefined requirements, the trajectory planning objective of the UAV formation from the starting point to the target endpoint is designed using the three-dimensional state vector and centroid motion model of a single UAV and the discrete linear time-invariant system model of the UAV formation. The trajectory planning objective includes a cost function and constraints.

[0008] Using an on-demand collision avoidance algorithm, the inter-drone collision information and obstacle collision information of a single UAV at the current discrete moment are detected. Based on the distributed model predictive control algorithm, the predicted trajectory of a single UAV that meets the trajectory planning objective is predicted using the centroid motion model of a single UAV. The specified group of elements of the predicted trajectory is applied to the centroid motion model of the single UAV to update the three-dimensional state vector of the single UAV.

[0009] According to an embodiment of the present invention, the above-described method for determining the centroid motion model of a single UAV based on the discrete-time linear invariance principle, utilizing the three-dimensional state vector of a single UAV, a predefined state matrix, a position reference input vector, and a predefined input matrix, and obtaining the discrete linear time-invariant system model of the UAV formation based on the centroid motion model of each UAV includes:

[0010] Using the three-dimensional state vector of a single UAV at the k-th time step, a predefined state matrix, the position reference input vector at the k-th time step, and a predefined input matrix, the three-dimensional state vector of the single UAV at the (k+1)-th time step is obtained. The three-dimensional state vector includes a three-dimensional position vector and a three-dimensional velocity vector. The single UAV is equipped with a position tracking controller, where k is a positive integer.

[0011] Based on the discrete linear invariance principle, the centroid motion model of a single UAV is determined by using the three-dimensional state vector of the k-th time step, the predefined state matrix, the position reference input vector of the k-th time step, the predefined input matrix, and the three-dimensional state vector of the (k+1)-th time step.

[0012] The initial state matrix and initial input matrix of the UAV formation are determined based on the state matrix and input matrix of each UAV in the formation. Then, the Kronecker product operation is performed on the initial state matrix and initial input matrix of the UAV formation using a predefined identity matrix to obtain the state matrix and input matrix of the UAV formation.

[0013] By using the centroid motion model of each UAV in the UAV formation, the state matrix of the UAV formation, and the input matrix of the UAV formation, a discrete linear time-invariant system model of the UAV formation is obtained. A directed graph is used to represent the communication topology of the UAV formation, where the directed graph includes nodes representing each UAV and edges representing the communication between UAVs.

[0014] According to an embodiment of the present invention, the cost function includes a trajectory error cost function, a UAV formation error cost function based on a directed graph, a control input cost function, a control input variation cost function, and a slack variable cost function. The constraints include trajectory continuity constraints, physical constraints based on UAV dynamic feasibility, inter-UAV collision avoidance constraints, and obstacle collision avoidance constraints.

[0015] According to an embodiment of the present invention, the above-described cost function design for a UAV formation from its starting point to its target endpoint, based on user-defined requirements and utilizing the three-dimensional state vector and centroid motion model of a single UAV, as well as the discrete linear time-invariant system model of the UAV formation, includes:

[0016] Based on the positive weights of the predicted position, target position, and the difference between the predicted position and the target position of a single UAV at each time step in the prediction time domain, the error term of a single UAV based on the L2 paradigm is obtained at each time step in the prediction time domain.

[0017] Based on the centroid motion model of a single UAV, the predicted position of the single UAV in the prediction time domain is transformed into an affine function representing the input sequence by the predicted trajectory.

[0018] The L2-norm-based error term is transformed into a quadratic cost function associated with the input sequence represented by the predicted trajectory using an affine function to obtain the trajectory error cost function.

[0019] According to an embodiment of the present invention, the above-mentioned design of the cost function for the UAV formation from the starting point to the target endpoint, based on the user's predefined requirements and utilizing the three-dimensional state vector and centroid motion model of a single UAV and the discrete linear time-invariant system model of the UAV formation, further includes:

[0020] Based on the consistency principle, a directed graph is used to identify other drones that communicate with a single drone within a drone formation, and to obtain the expected relative distance between the single drone and other drones that communicate with the single drone.

[0021] Based on the predefined formation error weights, the expected relative distance, the predicted position of a single UAV at each time step in the prediction time domain, and the predicted positions of other UAVs in the UAV formation at each time step in the prediction time domain, the formation error function of a single UAV based on the L2 paradigm is determined.

[0022] By using affine functions, the formation error function of a single UAV based on the L2 paradigm is transformed into a quadratic cost function associated with the input sequence represented by the predicted trajectory, so as to obtain the UAV formation error cost function based on the directed graph.

[0023] According to an embodiment of the present invention, the above-mentioned design of the cost function for the UAV formation from the starting point to the target endpoint, based on the user's predefined requirements and utilizing the three-dimensional state vector and centroid motion model of a single UAV and the discrete linear time-invariant system model of the UAV formation, further includes:

[0024] The position reference input vector of a single UAV is differentiated with respect to time, the square of the derivative is integrated with respect to time, and the result of the integration is weighted and summed with predefined positive weights to obtain the control input cost function.

[0025] Based on the position reference input vector of a single UAV at time point k_t within the k-th time step and the position reference input vector at time point k_t within the (k-1)-th time step, the control input change cost function based on the L1 paradigm is obtained.

[0026] The slack variables used for constraints are restricted according to preset constraints, and multiple different preset positive weights are used to calculate the slack variables used for constraints, resulting in a slack variable cost function based on the L2 paradigm.

[0027] According to an embodiment of the present invention, the constraints for designing the UAV formation from the starting point to the target endpoint based on the user-defined requirements, using the three-dimensional state vector and centroid motion model of a single UAV and the discrete linear time-invariant system model of the UAV formation, include:

[0028] Based on the centroid motion model of a single UAV and the duration of the specified desired discrete time step, the duration of multiple continuous time steps is obtained, and the position reference input vector of the single UAV within the duration is parameterized into multiple Bézier curves.

[0029] Set the initial control points of multiple Bézier curves to a preset constant vector, and set the difference between the control points of multiple Bézier curves and the derivatives of the control points to a preset constant;

[0030] A relational tuple is established to represent trajectory continuity constraints by utilizing the linear relationship between the control points and the derivatives of multiple Bézier curves.

[0031] Based on the physical attribute information of a single UAV, the multi-order derivative of the position reference input vector of the single UAV is calculated, thereby determining the minimum control input vector and the maximum control input vector of the single UAV.

[0032] By utilizing the minimum and maximum control input vectors of a single UAV and its acceleration information, physical constraints based on the dynamic feasibility of the UAV are obtained.

[0033] According to an embodiment of the present invention, the above-mentioned design of constraints for the UAV formation from the starting point to the target endpoint based on the user-defined requirements, using the three-dimensional state vector and centroid motion model of a single UAV and the discrete linear time-invariant system model of the UAV formation, further includes:

[0034] In the prediction time domain, collision avoidance constraints for a single UAV are achieved by using the centroid motion model of the single UAV and constraining the position reference input vector of the single UAV. The collision avoidance constraints include inter-UAV collision avoidance constraints and obstacle collision avoidance constraints.

[0035] Specifically, within the prediction time domain, the collision avoidance constraints for a single UAV are achieved by utilizing the centroid motion model of the UAV and constraining its position reference input vector.

[0036] Based on the minimum flight radius of a single drone, the initial collision avoidance constraints are determined using the predicted position of the single drone, the predicted positions of other drones in the drone formation, and the predicted positions of obstacles.

[0037] The radius around a single drone is modeled based on its minimum flight radius, and the collision boundary of the single drone and other drones and obstacles within the collision boundary are determined using the modeling results.

[0038] By relaxing the initial collision avoidance constraints with the slack variables used for the constraints, intermediate collision avoidance constraints are obtained. The Taylor series expansion of the predicted position of a single UAV at a specific discrete time in the prediction time domain is linearized to obtain the linearized result.

[0039] The relaxation variables used for the constraints are expanded by using other drones and obstacles within the collision boundary, and the linearization result is converted into an affine function of the expanded relaxation variables.

[0040] Based on the other drones and obstacles within the collision boundary, the affine functions of the expanded relaxation variables of all drones are superimposed to obtain the collision avoidance constraint.

[0041] According to an embodiment of the present invention, the above-described method of using an on-demand collision avoidance algorithm to detect inter-drone collision information and obstacle collision information of a single UAV at the current discrete moment, and based on a distributed model predictive control algorithm, using the centroid motion model of a single UAV to predict the predicted trajectory of the single UAV that satisfies the trajectory planning objective, and applying a specified group of elements of the predicted trajectory to the centroid motion model of the single UAV to update the three-dimensional state vector of the single UAV includes:

[0042] By using a communication sharing mechanism with other drones in a preset neighborhood, the collision information of a single drone with other drones at the previous time point in the k-th time step is obtained, where k is a positive integer;

[0043] Under the condition of satisfying collision avoidance constraints, based on the on-demand collision avoidance algorithm, the collision information of other UAVs at the previous time point within the k-th time step is used to detect the inter-UAV collision information and obstacle collision information of a single UAV at the current time point within the k-th time step;

[0044] Based on the trajectory planning objective, and using a distributed model predictive control algorithm, the predicted trajectory of a single UAV in the prediction time domain is obtained by utilizing the centroid motion model and three-dimensional state vector of the single UAV.

[0045] The specified group of elements of the predicted trajectory is applied to the centroid motion model of a single UAV to update the three-dimensional state vector of the single UAV, and the updated three-dimensional state vector of the single UAV is shared in the UAV formation.

[0046] Repeated detection, trajectory acquisition, update, and sharing operations continue until the drone formation reaches the target destination.

[0047] According to an embodiment of the present invention, the above-mentioned method of obtaining the predicted trajectory of a single UAV in the prediction time domain based on the trajectory planning objective, using a distributed model predictive control algorithm and a centroid motion model and three-dimensional state vector of a single UAV, includes:

[0048] Construct a Bernstein polynomial of a predefined order, and use a predefined set of control points and the Bernstein polynomial to determine a Bézier curve in three-dimensional space with respect to duration.

[0049] Based on the preset duration of the desired discrete time step, calculate the duration of a single UAV within a continuous time range over multiple future time steps;

[0050] The position reference input vector of a single UAV over a continuous time range is parameterized as a concatenation of multiple Bézier curves, and the trajectory of a single UAV in multiple future time steps is predicted based on the concatenation of multiple Bézier curves.

[0051] A linear prediction model is determined based on the centroid motion model of a single UAV to represent the state of the single UAV in multiple future time steps, and the affine function of the predicted trajectory of the single UAV is obtained using the linear prediction model.

[0052] This invention provides a model predictive control-based UAV formation trajectory planning method that utilizes a consensus-based formation strategy and a distributed model predictive control algorithm to solve the trajectory planning problem for quadrotor UAV formations. This method enables UAVs to plan trajectories from a starting point to a target point under multiple constraints, while simultaneously achieving UAV formation and maintaining formation shape. Furthermore, considering various constraints, this method transforms the multi-UAV trajectory optimization problem into optimizing a set of Bézier curve sampling points by representing UAV trajectories using Bézier curves, and achieves collision-free UAV position updates through an on-demand collision avoidance algorithm. Moreover, compared to existing technologies, this model predictive control-based UAV formation trajectory planning method offers higher efficiency and better security. Attached Figure Description

[0053] Figure 1 This is a flowchart of a UAV formation trajectory planning method based on model predictive control according to an embodiment of the present invention;

[0054] Figure 2 This is a diagram of the flight trajectory of a drone formation in an obstacle-filled environment according to an embodiment of the present invention;

[0055] Figure 3 This is a schematic diagram of the cost function of each UAV in a UAV formation in an obstacle-filled environment according to an embodiment of the present invention;

[0056] Figure 4 This is a graph showing the error variation of a drone formation in an obstacle-filled environment according to an embodiment of the present invention;

[0057] Figure 5 This is a schematic diagram showing the spacing between drones within a formation during multi-drone formation flight according to an embodiment of the present invention;

[0058] Figure 6 This is a schematic diagram illustrating the distance between multiple unmanned aerial vehicles (UAVs) and obstacles during flight in a formation, according to an embodiment of the present invention.

[0059] Figure 7 This is a graph showing the velocity components of four drones in a drone formation according to an embodiment of the present invention;

[0060] Figure 8 This is a graph showing the acceleration components of four drones in a drone formation according to an embodiment of the present invention.

[0061] Figure 9 This is a flight trajectory diagram of a formation flight test of 16 UAVs for trajectory planning and collision avoidance according to an embodiment of the present invention;

[0062] Figure 10 This is a cost function graph of a formation of 16 drones according to an embodiment of the present invention;

[0063] Figure 11 This is a diagram showing the formation error variation of a 16-drone formation according to an embodiment of the present invention;

[0064] Figure 12 This is a schematic diagram illustrating the distances between drones during multi-drone formation flight according to an embodiment of the present invention;

[0065] Figure 13 This diagram illustrates the formation error of three algorithms during a collision avoidance and obstacle avoidance test of a formation trajectory planning system for four drones. Detailed Implementation

[0066] To make the objectives, technical solutions, and advantages of the present invention clearer, the present invention will be further described in detail below with reference to specific embodiments and accompanying drawings.

[0067] Unmanned aerial vehicle (UAV) formation trajectory planning has become a hot research topic in recent years, with progress mainly focusing on path planning, attitude control, obstacle avoidance strategies, and formation maintenance. Solving the UAV formation trajectory planning problem primarily involves centralized and distributed methods. Centralized methods, such as the sequential convex programming approach based on optimization, enable multiple UAVs to generate collision-free trajectories in three-dimensional space within a fixed time. This is achieved by linearizing non-convex constraints to approximate convex constraints and utilizing an iterative sequential convex programming strategy to generate trajectories for multiple UAVs in open conditions. To address the planning problem for UAVs in non-convex spaces, existing research has proposed a multi-UAV trajectory planning method based on incremental sequential convex programming. This method combines the SCP algorithm with a greedy solution strategy; this decoupled algorithm is faster and more likely to find feasible trajectories compared to the previous SCP algorithm. While the aforementioned centralized methods effectively solve the multi-UAV trajectory planning problem, the computational complexity increases dramatically with the number of UAVs, and the success rate decreases rapidly.

[0068] In recent years, distributed multi-UAV trajectory planning algorithms have been extensively studied. For example, the RRT forest algorithm based on the Rapidly-exploring Random Tree (RRT) algorithm significantly improves the efficiency of collaborative path planning for UAVs in complex environments by randomly selecting root nodes, generating random trees, and connecting and merging random trees. Dynamically-based distributed collision avoidance algorithms realize trajectory planning for UAV swarms in environments with dense multiple obstacles. To further reduce the requirements of distributed planning on communication network capabilities, a multi-agent collision avoidance method based on Buffered Voronoi Cells (BVCs) has been proposed, planning the trajectories of each UAV within its flight space using a rolling optimization approach. This method only requires the interaction of relative position information between agents when solving the multi-agent trajectory problem, solving the problem of online trajectory planning for multiple agents under limited communication bandwidth. To further reduce solution time and improve the success rate, those skilled in the art have proposed the Distributed Model Predictive Control (DMPC) algorithm for real-time trajectory generation of UAV swarms.

[0069] However, the aforementioned research on multiple drones (or drone swarms) mainly involves trajectory planning for single drones or drone clusters, where drones do not form formations and cannot perform cooperative operations. These algorithms are not suitable for trajectory planning and formation control problems of drone swarms. For example, a distributed model predictive control algorithm under multiple constraints based on a virtual navigation strategy was designed to achieve trajectory tracking and formation maintenance for drone swarms; a distributed model predictive control algorithm with referenceless trajectories considering system constraints enables drone swarms to avoid dynamic obstacles. These studies mainly focus on the trajectory tracking problem of drone swarms, failing to adequately address the trajectory planning problem for multiple drone swarms, and the large amount of information exchange in drone swarms results in poor formation robustness.

[0070] To overcome the shortcomings of existing technologies, this invention provides a UAV formation trajectory planning method based on model predictive control. By utilizing a consistent formation strategy and a distributed model predictive planning algorithm, it solves the trajectory planning problem for quadrotor UAV formations in complex environments. The method provided by this invention enables UAVs to plan trajectories from the starting point to the target point under multiple constraints, while simultaneously achieving UAV formation and formation maintenance. To this end, this invention employs a DMPC UAV formation trajectory planning strategy based on a consistent formation strategy. This invention establishes kinematic models of individual quadrotor UAVs and UAV formations, designs a DMPC trajectory planning algorithm based on constraints and planning objectives, and then realizes state transitions and dynamic obstacle avoidance for quadrotor UAV formations.

[0071] Figure 1 This is a flowchart of a UAV formation trajectory planning method based on model predictive control according to an embodiment of the present invention.

[0072] like Figure 1 As shown, the above-mentioned UAV formation trajectory planning method based on model predictive control includes operations S110 to S130.

[0073] In operating S110, based on the principle of discrete-time linear invariance, the centroid motion model of a single UAV is determined using the three-dimensional state vector of a single UAV, the predefined state matrix, the position reference input vector, and the predefined input matrix. Based on the centroid motion model of each UAV, the discrete linear time-invariant system model of the UAV formation is obtained.

[0074] When operating the S120, based on the user's predefined requirements, the trajectory planning objective of the UAV formation from the starting point to the target endpoint is designed using the three-dimensional state vector and centroid motion model of a single UAV and the discrete linear time-invariant system model of the UAV formation. The trajectory planning objective includes a cost function and constraints.

[0075] In operation S130, the on-demand collision avoidance algorithm is used to detect the inter-drone collision information and obstacle collision information of a single UAV at the current discrete moment. Based on the distributed model predictive control algorithm, the predicted trajectory of a single UAV that meets the trajectory planning objective is predicted using the centroid motion model of a single UAV. The specified group of elements of the predicted trajectory is applied to the centroid motion model of the single UAV to update the three-dimensional state vector of the single UAV.

[0076] This invention provides a model predictive control-based UAV formation trajectory planning method that utilizes a consensus-based formation strategy and a distributed model predictive control algorithm to solve the trajectory planning problem for quadrotor UAV formations. This method enables UAVs to plan trajectories from a starting point to a target point under multiple constraints, while simultaneously achieving UAV formation and maintaining formation shape. Furthermore, considering various constraints, this method transforms the multi-UAV trajectory optimization problem into optimizing a set of Bézier curve sampling points by representing UAV trajectories using Bézier curves, and achieves collision-free UAV position updates through an on-demand collision avoidance algorithm. Moreover, compared to existing technologies, this model predictive control-based UAV formation trajectory planning method offers higher efficiency and better security.

[0077] According to an embodiment of the present invention, the above-described method for determining the centroid motion model of a single UAV based on the discrete-time linear invariance principle, using the three-dimensional state vector of a single UAV, a predefined state matrix, a position reference input vector, and a predefined input matrix, and obtaining the discrete linear time-invariant system model of the UAV formation based on the centroid motion model of each UAV includes: using the third centroid motion model of each UAV... The three-dimensional state vector at the nth time step, the predefined state matrix, and the nth time step. Using the position reference input vector at the nth time step and a predefined input matrix, we obtain the nth time step of a single UAV. The three-dimensional state vectors at each time step include a three-dimensional position vector and a three-dimensional velocity vector. Each UAV is equipped with a position tracking controller. For positive integers; based on the discrete linear invariance principle, the first... The three-dimensional state vector at the nth time step, the predefined state matrix, and the nth time step. The position reference input vector at the i-th time step, the predefined input matrix, and the i-th time step. The three-dimensional state vector at each time step determines the centroid motion model of a single UAV. Based on the state matrix and input matrix of each UAV in the UAV formation, the initial state matrix and initial input matrix of the UAV formation are determined respectively. A Kronecker product operation is then performed on the initial state matrix and initial input matrix of the UAV formation using a predefined identity matrix to obtain the state matrix and input matrix of the UAV formation. Using the centroid motion model of each UAV in the UAV formation, the state matrix of the UAV formation, and the input matrix of the UAV formation, a discrete linear time-invariant system model of the UAV formation is obtained. A directed graph is used to represent the communication topology of the UAV formation, where the directed graph includes nodes representing each UAV and edges representing communication between UAVs.

[0078] The above embodiments detail the process of constructing kinematic models of a single UAV (e.g., a quadcopter UAV) and UAV formation motion models in operation S110.

[0079] The following section provides a more detailed explanation of the construction process of the above-mentioned single UAV center of mass motion model and UAV formation motion model, using specific implementation methods as examples.

[0080] This invention focuses on the trajectory planning problem of quadrotor UAV formations, emphasizing their three-dimensional translational motion. During system modeling, the UAV model is reasonably simplified, with a focus on describing the kinematic characteristics of their translational motion. It is assumed that the UAV formation contains... The system consists of drones, with their dynamics decoupled and the effects of wind disturbance ignored. Assume each drone... They are all equipped with controllers for position tracking, and This is the location reference input. The centroid motion model of the UAV can be approximately described as a discrete-time linear time-invariant form as shown in equation (1):

[0081] (1),

[0082] Among them, the subscript on the right Number the drone. For drones In the inertial coordinate system, the first A three-dimensional state vector at each time step; here, It is a three-dimensional position vector. It is a three-dimensional velocity vector; For drones Position reference input; matrix and drones The state matrix (i.e., the predefined state matrix described in operation S110) and the input matrix (i.e., the predefined input matrix described in operation S110).

[0083] Will A collection of drones is called a drone formation system, and the state variables of a drone formation system are defined. Control input quantity The linear time-invariant formation system model can be obtained, and its discrete form is shown in formula (2):

[0084] (2),

[0085] in, , ; for 3D identity matrix, symbol This represents the Kronecker product. It has... The formation communication topology of a UAV can be represented by a directed graph. To represent this, each drone in a drone formation can be viewed as a node in a graph; That is, the set of nodes in the topological graph; Represents the set of boundaries of the topological graph; Indicates drone You can get a drone Status information. The arrow is... point to And called drones It is a drone The neighbor. (Use) Indicates drone The neighbor set.

[0086] The process of determining the trajectory planning target described in operation S120 above will be further explained in detail below with reference to specific implementation methods.

[0087] Before determining the above-mentioned method for standardizing drone formation trajectories, it is first necessary to clarify the planning objectives that the trajectory standardization method should meet.

[0088] This invention makes certain settings regarding the internal conditions and external disturbances of the UAV formation during the process of determining the estimated planning objectives.

[0089] First, all drones in the formation are aware of the positions of their neighbors, as well as the positions of static and dynamic obstacles. Second, the drones in the formation communicate with each other using a connected topology, meaning that within a sampling period, drones in the formation can obtain position and speed information from other drones in real time without delay (where "without delay" is an idealization of the drone formation's communication capabilities during actual flight. That is, when the drones are close together, the communication transmission time delay is assumed to be much smaller than the time it takes for the position sensors to update data, thus idealizing the drone formation's communication process). Finally, the impact of external interference, noise, air resistance, and other disturbances on the drones' dynamics is not considered.

[0090] The trajectory planning objective needs to meet the following conditions: given A drone with a known kinematic model, and a workspace with a finite working range. and the expected target point for each drone This enables the UAV formation system to plan a trajectory from the starting point to the final point that conforms to the dynamic model, while maintaining the predetermined formation and always remaining within the workspace, provided that it meets its own state, input constraints, and inter-UAV collision and obstacle avoidance constraints. Inside.

[0091] The specific mathematical form of the trajectory planning objective can be described by formulas (3) to (6):

[0092] (3),

[0093] (4),

[0094] (5),

[0095] (6),

[0096] Wherein, formula (3) represents the unmanned aerial vehicle (UAV). It can reach the desired target point (i.e., the target endpoint); Formula (4) indicates that multiple UAVs can maintain the desired formation during flight. For drones and drones The expected relative position of the drone For drones The neighbors; Formula (5) represents the inter-machine collision avoidance constraint, The safe distance between drones within the team; Formula (6) represents the collision avoidance constraints between drones and fixed and dynamic obstacles, where the obstacles are simplified to spheres. For obstacles The center of the ball, To maintain a safe distance between the drone and obstacles, It is the set of all obstacles.

[0097] According to an embodiment of the present invention, the above-described method of using an on-demand collision avoidance algorithm to detect inter-drone collision information and obstacle collision information of a single UAV at the current discrete moment, and based on a distributed model predictive control algorithm, using the centroid motion model of a single UAV to predict the predicted trajectory of the single UAV that satisfies the trajectory planning objective, and applying a specified group of elements of the predicted trajectory to the centroid motion model of the single UAV to update the three-dimensional state vector of the single UAV includes: obtaining the state vector of the single UAV at the specified time by means of a communication sharing mechanism with other UAVs in a preset neighborhood. The collision information of other drones at the previous time point within each time step, where... For positive integers; under the condition of satisfying collision avoidance constraints, based on the on-demand collision avoidance algorithm, utilizing the first-order collision avoidance of other UAVs... The collision information from the previous time point within each time step is used to detect the collision information of a single drone in the [number]th time step. Within each time step, the system obtains inter-machine collision information and obstacle collision information at the current time point; based on the trajectory planning objective, and using a distributed model predictive control algorithm, the system obtains the predicted trajectory of a single UAV in the prediction time domain using the centroid motion model and three-dimensional state vector of the single UAV; the system applies a specified group of elements of the predicted trajectory to the centroid motion model of the single UAV to update the three-dimensional state vector of the single UAV, and shares the updated three-dimensional state vector of the single UAV in the UAV formation; the system repeats the detection operation, the predicted trajectory acquisition operation, the update operation, and the sharing operation until the UAV formation reaches the target endpoint.

[0098] According to an embodiment of the present invention, the above-described method for obtaining the predicted trajectory of a single UAV in the prediction time domain based on the trajectory planning objective, using a distributed model predictive control algorithm and the centroid motion model and three-dimensional state vector of a single UAV, includes: constructing a Bernstein polynomial of a preset order, and using a predefined set of control points and the Bernstein polynomial to determine a Bézier curve in three-dimensional space with respect to duration; calculating the duration of a continuous time range of the single UAV within multiple future time steps based on the duration of a preset desired discrete time step; parameterizing the position reference input vector of the single UAV within the duration of the continuous time range into a cascade of multiple Bézier curves, and predicting the trajectory of the single UAV within multiple future time steps based on the cascade of multiple Bézier curves; determining a linear prediction model based on the centroid motion model of the single UAV to represent the state of the single UAV within multiple future time steps, and using the linear prediction model to obtain the affine function of the predicted trajectory of the single UAV.

[0099] The above embodiments detail how operation S130 utilizes the Distributed Model Predictive Control (DMPC) algorithm to plan the trajectory of the UAV formation.

[0100] The following detailed explanation, in conjunction with specific implementation methods, illustrates how this invention designs a UAV formation trajectory planning algorithm based on a distributed model predictive control algorithm.

[0101] For model predictive control of a single UAV, it is required that at each discrete time... This paper addresses the optimization problem of unmanned aerial vehicles (UAVs). The problem is described as follows: within a given prediction time domain, the UAV solves for the optimal input sequence under constraints, applies the first element of this sequence to the actual UAV system to obtain the updated UAV state, and uses this updated UAV state as the starting point for the next optimization problem. This method is performed in a distributed manner, where each UAV performs iterative optimization to generate a trajectory and shares state information with neighboring UAVs.

[0102] The trajectory planning algorithm designed in this invention is based on the synchronous DMPC algorithm. In the synchronous DMPC algorithm, the UAV shares its previous predicted state sequence with neighboring UAVs before simultaneously solving the next optimization problem. At discrete time intervals... In this system, each UAV within the UAV swarm system synchronously calculates its state sequence in the prediction time domain according to the following steps:

[0103] (1) Used at discrete time The predicted states of adjacent drones and obstacles are calculated to detect whether a collision has occurred.

[0104] (2) Construct a trajectory optimization problem by considering state constraints, input constraints, and collision constraints only when needed, so as to obtain the optimal input sequence;

[0105] (3) After obtaining the optimal input sequence, the first set of elements is applied to the UAV motion model so that the UAV updates its state and shares its predicted state in the prediction time domain with neighboring UAVs.

[0106] The basic idea of ​​on-demand collision avoidance is to predict collisions and only include collision constraints when necessary. On-demand collision avoidance algorithms only include constraints relevant to the first predicted collision and repeat this process during drone state updates until all drones have reached their target points. The mathematical model of the optimization problem is given below.

[0107] The DMPC trajectory planning algorithm is an optimization algorithm based on the rolling optimization process, which means that at discrete time... Corresponding to continuous time At this point, it is necessary to calculate the future. The input sequence at time steps. Given the duration of the desired discrete time step. To obtain the duration of a continuous time range .Will Continuous input signal Parameterization A cascade of Bézier curves.

[0108] Bézier curves were chosen as the reference input because they allow for the imposition of smoothness requirements on the input signal, resulting in a smoother trajectory planned by the algorithm. This also satisfies the dynamic constraints of the UAV itself and facilitates the easy acquisition of its input derivative. In order to... Define arbitrary order and duration The Bézier curve is first constructed with order of... The Bernstein polynomial is shown in formula (7):

[0109] (7),

[0110] in Bézier curve is defined as ,in For the first Bézier curve One control point; set This represents the unique characterization of a Bézier curve. A set of control points; the control points are finite parameterizations of a continuous curve and are used as optimization variables to compute the predicted trajectory of the UAV in the prediction time domain. Its derivative can be represented by a linear combination of its control points, which will be used to construct convex optimization problems.

[0111] Based on the discrete UAV model in formula (1), a linear model is designed to represent the fixed time step. Predicted status of drones within range. Introducing symbols. To represent at discrete time The time in the prediction time domain The predicted state of the drone at each time step. ,in Also known as the prediction step size of drones, drones The prediction model can be represented by formula (8):

[0112] (8),

[0113] definition Discrete time drones The initial state; then the drone Predict the predicted location for all times within the time domain. Write as input sequence The affine function is shown in formula (9):

[0114] (9).

[0115] According to an embodiment of the present invention, the cost function includes a trajectory error cost function, a UAV formation error cost function based on a directed graph, a control input cost function, a control input variation cost function, and a slack variable cost function. The constraints include trajectory continuity constraints, physical constraints based on UAV dynamic feasibility, inter-UAV collision avoidance constraints, and obstacle collision avoidance constraints.

[0116] According to an embodiment of the present invention, the above-described cost function for designing the UAV formation from the starting point to the target endpoint based on the user's predefined requirements, utilizing the three-dimensional state vector and centroid motion model of a single UAV and the discrete linear time-invariant system model of the UAV formation, includes: obtaining an L2-paradigm-based error term for each time step of the single UAV in the prediction time domain based on the predicted position, target position, and positive weights of the difference between the predicted and target positions of the single UAV at each time step in the prediction time domain; transforming the predicted position of the single UAV in the prediction time domain into an affine function representing the input sequence of the predicted trajectory based on the centroid motion model of the single UAV; and using the affine function to convert the L2-paradigm-based error term into a quadratic cost function associated with the input sequence represented by the predicted trajectory to obtain the trajectory error cost function.

[0117] According to an embodiment of the present invention, the above-described design of the cost function for a UAV formation from its starting point to its target endpoint, based on user-defined requirements and utilizing the three-dimensional state vector and centroid motion model of a single UAV and the discrete linear time-invariant system model of the UAV formation, further includes: determining other UAVs that communicate with the single UAV within the UAV formation using a directed graph based on the consistency principle, and obtaining the expected relative distance between the single UAV and the other UAVs communicating with the single UAV; determining the L2 paradigm-based formation error function for the single UAV based on predefined formation error weights, the expected relative distance, the predicted position of the single UAV at each time step in the prediction time domain, and the predicted positions of other UAVs in the UAV formation at each time step in the prediction time domain; and converting the L2 paradigm-based formation error function of the single UAV into a quadratic cost function associated with the input sequence represented by the predicted trajectory using an affine function to obtain the directed graph-based UAV formation error cost function.

[0118] According to an embodiment of the present invention, the above-described design of the cost function for the UAV formation from the starting point to the target endpoint, based on the user-defined requirements and utilizing the three-dimensional state vector and centroid motion model of a single UAV and the discrete linear time-invariant system model of the UAV formation, further includes: differentiating the position reference input vector of the single UAV with respect to time, integrating the square of the derivative with respect to time, and weighting the integral result with predefined positive weights to obtain the control input cost function; based on the single UAV's position reference input vector in the first... Within the first time step The location of the time point references the input vector and the position at the twentieth time point. Within the first time step The location of the time point is referenced to the input vector to obtain the control input change cost function based on the L1 paradigm; the slack variables used for the constraint conditions are restricted according to the preset constraints, and multiple different preset positive weights are calculated with the slack variables used for the constraint conditions to obtain the slack variable cost function based on the L2 paradigm.

[0119] The following detailed explanation, in conjunction with specific implementation methods, further illustrates how the present invention determines various cost functions in the trajectory planning objective.

[0120] Under the premise of satisfying the kinematic equations of the UAV, by designing the cost function in the optimal control problem, the multi-UAV formation trajectory planning problem in an unknown multi-obstacle environment can be transformed into an optimal control problem. The cost function used to calculate the optimal input sequence during rolling optimization consists of the following parts: trajectory error cost function, control input cost function, control input change cost function, formation error cost function, and slack variable cost function.

[0121] The trajectory error cost function drives the UAV from its initial position to its target point. By minimizing the UAV's trajectory error cost function... Predicting the time domain Predicted location and target location at each time step The cost function is designed based on the sum of errors between the two sides. The error term is defined as shown in formula (10):

[0122] (10)

[0123] in, This is the positive weight of the difference between the predicted UAV position and the target position at each time step in the prediction time domain. Using formula (9), this error term is converted into a quadratic cost function related to the input sequence, as shown in formula (11):

[0124] (11),

[0125] in, It is a positive definite block diagonal matrix that weights the position error at each time step in the prediction time domain. When hour, ,in It is a diagonal positive definite matrix. The higher the value, the better the drone The stronger the tendency to move toward the target point.

[0126] The control input cost function is a weighted combination that minimizes the sum of squared input derivatives, and the cost function is defined as shown in formula (12):

[0127] (12),

[0128] in, This is the positive weight that controls the input and its derivative. This term can be evaluated in a closed-form equation, thus obtaining... The quadratic form.

[0129] The formation error cost function drives all UAVs in the formation to form and maintain formation based on consistency theory as they reach the target point. The formation error function is defined as shown in formula (13):

[0130] (13)

[0131] in, It is a drone neighboring drones Formation error weights. Based on consistency theory, UAVs... Only drones connected on the communication topology map To transmit information. For drones With drones The expected relative distance between them is used to convert the formation error function term into a quadratic cost function related to the input sequence using formula (9), as shown in formula (14):

[0132] (14).

[0133] The drone formation algorithm designed in this invention is implemented using soft constraints, therefore the drone formation is not a rigid body. Upon encountering an obstacle, the drones will prioritize obstacle avoidance, and after avoiding the obstacle, the drones will quickly form a formation.

[0134] The input variation cost function is used to minimize the positional change, thereby obtaining a smoother input trajectory. The input variation cost function is defined as shown in formula (15):

[0135] (15)

[0136] To transform formula (15) into a quadratic form, first define the matrix. and introduce vectors To include items (The control input at the previous discrete time), as shown in formula (16):

[0137] (16)

[0138] Finally, formula (13) can be written in quadratic form, as shown in formula (17):

[0139] (17)

[0140] in, It is a positive definite diagonal matrix, defined as follows: ,in It is a matrix of weighted control change penalties. The sum of all cost functions yields the value for the UAV. Ignoring the quadratic cost function during collisions As shown in formula (18):

[0141] (18).

[0142] According to an embodiment of the present invention, the above-described design of constraints for a UAV formation from its starting point to its target endpoint, based on user-defined requirements and utilizing the three-dimensional state vector and centroid motion model of a single UAV, as well as the discrete linear time-invariant system model of the UAV formation, includes: obtaining the duration of multiple continuous time steps based on the centroid motion model of the single UAV and the duration of the specified desired discrete time step; parameterizing the position reference input vector of the single UAV within the duration into multiple Bézier curves; setting the initial control points of the multiple Bézier curves as preset constant vectors, and setting the difference between the control points and the derivatives of the control points of the multiple Bézier curves as preset constants; establishing relational tuples to represent trajectory continuity constraints using the linear relationship between the control points and the derivatives of the multiple Bézier curves; calculating multiple derivatives of the position reference input vector of the single UAV based on the physical attribute information of the single UAV, thereby determining the minimum control input vector and the maximum control input vector of the single UAV; and obtaining physical constraints based on the dynamic feasibility of the UAV using the minimum control input vector, the maximum control input vector, and the acceleration information of the single UAV.

[0143] According to an embodiment of the present invention, the above-mentioned design of the constraints for the UAV formation from the starting point to the target endpoint using the three-dimensional state vector and centroid motion model of a single UAV and the discrete linear time-invariant system model of the UAV formation according to the user's predefined requirements further includes: in the prediction time domain, using the centroid motion model of a single UAV and constraining the position reference input vector of the single UAV to realize the collision avoidance constraint of the single UAV, wherein the collision avoidance constraint includes inter-UAV collision avoidance constraint and obstacle collision avoidance constraint.

[0144] Within the prediction time domain, collision avoidance constraints for a single UAV are achieved by using a centroid motion model and constraining its position reference input vector. This includes: determining initial collision avoidance constraints based on the UAV's minimum flight radius, using its predicted position, the predicted positions of other UAVs in the UAV formation, and the predicted positions of obstacles; modeling the UAV's surrounding radius based on its minimum flight radius and using the modeling results to determine the UAV's collision boundary and other UAVs and obstacles within that boundary; obtaining intermediate collision avoidance constraints by relaxing the initial collision avoidance constraints with slack variables used for the constraints, and linearizing the Taylor series expansion of the UAV's predicted position at a specific discrete time in the prediction time domain to obtain a linearized result; expanding the slack variables used for the constraints using other UAVs and obstacles within the collision boundary, and converting the linearized result into an affine function of the expanded slack variables; and superimposing the affine functions of the expanded slack variables of all UAVs based on the other UAVs and obstacles within the collision boundary to obtain the collision avoidance constraints.

[0145] The following detailed explanation, in conjunction with specific implementation methods, further illustrates how the present invention determines various cost functions in the trajectory planning objective.

[0146] During the trajectory planning process of UAV formation, due to its own dynamic limitations and the need for collision and obstacle avoidance, in order to ensure the safety and smoothness of the generated trajectory, the trajectory planning process must meet some constraints, mainly including trajectory continuity constraints, physical constraints based on the dynamic feasibility of UAVs, and collision avoidance constraints (including obstacle avoidance constraints and inter-UAV collision avoidance constraints).

[0147] Trajectory continuity constraints are implemented through equality constraints. First, the initial control points are chosen to be equal to a constant vector; second, to ensure... The continuity between Bézier curves requires that the endpoints of the curves and their derivatives match the starting points of the next curve; that is, the difference between the control points and their derivatives must be zero. A tuple is constructed using the linear relationship between the control points and their derivatives. This indicates input continuity constraints.

[0148] Because the driving capabilities of drones are limited (i.e., the speed and acceleration that drones can achieve), and the workspace of drones... These limitations are also finite, therefore these constraints must be imposed on the optimization problem. For the dynamic feasibility of the UAV, the present invention imposes the following physical constraints, as shown in equation (19):

[0149] (19)

[0150] in, and It is a control input The minimum and maximum values ​​of the first-order derivatives. This invention utilizes a linear combination of control points to obtain specific samples of the control inputs and their derivatives, and restricts the control inputs and their derivatives through linear inequality constraints. This process involves calculating the linear transformation between the control points and the polynomial coefficients in the power basis, and then multiplying it by... get And the accuracy of its derivative. and The value is chosen based on the actual characteristics of the UAV experimental platform used. In the case of a quadcopter UAV as the research object, the acceleration of the UAV... Imposing restrictions can enable the underlying controller to have good tracking performance.

[0151] To avoid collisions between drones within the team and between drones and obstacles, all drones are required to maintain equations (5) and (6) throughout the entire trajectory execution process. This invention utilizes the predictive nature of the DMPC trajectory planning algorithm to detect collision trajectories and applies collision avoidance constraints to avoid the first predicted collision, thereby achieving on-demand collision avoidance. This strategy only addresses the first collision detected in the prediction time domain. Since the input and state of the drone are closely related, the on-demand collision avoidance algorithm has two options: (1) State space: the predicted state of the drone. By imposing constraints, (9) can be used as a linear combination of the optimal input sequence to plan the collision-free prediction location in the prediction time domain; (2) Input space: directly on the input Constraints are imposed to generate collision-free reference trajectories in the prediction time domain. For the kinematic model of system (1), the non-intersecting trajectories generated in the input space may not necessarily achieve collision avoidance.

[0152] When the condition shown in formula (20) below is not met, the drone At discrete time The A collision with a neighboring drone or obstacle was detected at each predicted step:

[0153] (20)

[0154] In solution time Location, drone Only in discrete time The location information of other neighboring drones and obstacles is calculated in real time, which means that collisions are expected in discrete time. It happened there. Indicates drone The first time step in the prediction time domain where a collision with a neighbor occurs. This includes a subset of obstacles with collision constraints. The definition is shown in formula (21):

[0155] (twenty one),

[0156] in, For drones The surrounding radius is modeled, defining the neighbors to be considered obstacles when resolving collision problems. This invention considers all drones and obstacles within a radius three times the collision boundary as neighbors, i.e. .

[0157] If the UAV detects a collision, it must include a collision constraint to compute a new input sequence. Considering the infeasibility of solving the optimization problem, the following relaxed collision avoidance constraint is proposed, as shown in equation (22):

[0158] (twenty two),

[0159] in, New decision variables with relaxed constraints. In discrete time... The goal of collision avoidance algorithms is to optimize... The value satisfies formula (22). This constraint uses a drone. In discrete time The Taylor series expansion at the predicted location is linearized, as shown in formula (23):

[0160] (twenty three),

[0161] in, On the left side of equation (23), the constraint is applied at time. At this position, this is a time step predicted after the collision. This choice was made based on performance evaluations of the algorithm across various transitional scenarios. Simulations revealed that applying collision avoidance constraints one time step after a predicted collision allows the drone to exhibit a stronger tendency to preemptively avoid collisions and complete the transfer on average faster.

[0162] To transform the collision constraints into affine functions of the decision variables, we first extend the previous formula to include slack variables. Consider... ,in Defined as all The superimposed vectors. Equation (23) is transformed into an affine function of the decision variables, as shown in Equation (24):

[0163] (twenty four),

[0164] in, The definition is shown in formula (25):

[0165] (25),

[0166] Through the By superimposing the inequality formulas (24) of the collision neighbors, the complete collision constraint can be obtained, as shown in formula (26):

[0167] (26)

[0168] Apply To constrain slack variables. Because slack variables are imposed, the following cost function is considered to constrain the slack variables, and the on-demand collision avoidance algorithm is implemented in the form of soft constraints, as shown in formula (27):

[0169] (27)

[0170] in, and These are the positive weights of each item. When a collision is detected in the prediction time domain, the drone... The following QP problem needs to be solved to calculate the expected trajectory in the prediction time domain at each time step. This problem includes all the cost functions and constraints mentioned above, as shown in Equation (28):

[0171] (28).

[0172] To verify the effectiveness of the UAV formation trajectory planning method based on model predictive control proposed in this invention, simulations were conducted using 4 UAVs and 16 UAVs respectively. In conjunction with the accompanying drawings, the collision avoidance algorithm (state space and input space) designed in this invention was compared with the BVC collision avoidance algorithm in terms of collision avoidance performance.

[0173] (1) 4 UAVs were used for experimental verification

[0174] This invention creates a simulation environment in MATLAB, models the actual size of drones, and calculates the collision distance between drones. Collision distance between drone and obstacle A UAV model is constructed by fitting a second-order model of the UAV to experimental data on the system's step response using trajectory tracking dynamics. The duration of the discrete time step selected for the simulation is... The time interval is 15 seconds. This means that the trajectory is replanned only at an update frequency of 5Hz. For the input sequence, this invention selects... , =3 and The duration of each curve segment is fixed at 1 second. Furthermore, the maximum single-axis acceleration of the drone is limited to... For the parameters of each cost function, choose... , , , and .

[0175] Table 1: Simulation parameters for the simulation experiment with 4 UAVs

[0176]

[0177] Table 2: Initial and expected positions of the four drones in a formation simulation experiment.

[0178] Expected relative position

[0179]

[0180] The simulation parameters for the trajectory planning, collision avoidance, and obstacle avoidance test of four UAVs in formation are shown in Tables 1 and 2. Table 1 shows the number of UAVs in the formation and the number of static and dynamic obstacles. Table 2 shows the initial and desired positions of the UAVs in the inertial frame, as well as the desired relative positions of each UAV with UAV 1. The desired formation is a quadrilateral, and the unit of position measurement is... The unit of velocity is The simulation time was set to 50 seconds. The performance of UAV formation maintenance was measured as shown in formula (29):

[0181] (29).

[0182] To measure the computational complexity of the DMPC trajectory planning algorithm, the average rolling optimization time is defined as shown in formula (30):

[0183] (30)

[0184] in, For discrete time steps, For drones Discrete time to reach the target point. For drones Solving at each discrete time step The optimization time required for the problem.

[0185] Figure 2 This is a flight trajectory diagram of a drone formation in an obstacle-filled environment according to an embodiment of the present invention.

[0186] Figure 2 The obstacles in the passage can be static obstacles and / or dynamic obstacles, from Figure 2 It can be seen that the four drones maintain the desired formation flight while planning a trajectory from the initial position to the target point, and the drone formation can also maintain a good effect in obstacle environments.

[0187] Figure 3 This is a schematic diagram illustrating the cost functions of each UAV within a UAV formation in an obstacle-filled environment, according to an embodiment of the present invention. From... Figure 3 It can be seen that the cost functions of each UAV and the total system cost function change during formation and obstacle avoidance. After formation and obstacle avoidance, the cost functions of each UAV gradually converge to 0. This demonstrates that the method provided by this invention can still maintain the stability of the system well in obstacle-prone environments.

[0188] Figure 4 This is a graph showing the error variation of a drone formation in an obstacle-prone environment according to an embodiment of the present invention.

[0189] from Figure 4 As can be seen, the formation error quickly becomes 0 in the initial stage, indicating that the algorithm can enable the drones to form a formation rapidly. In the middle stage, the formation error increases slightly, but after passing through obstacles, the formation error changes very little and quickly converges to 0, indicating that multiple drones can still fly in the desired formation in obstacle environments.

[0190] Figure 5 This is a schematic diagram illustrating the spacing between drones within a multi-drone formation during flight according to an embodiment of the present invention. Figure 5 As can be seen, the spacing between the drones within the formation is always maintained above a safe distance, indicating that the algorithm of this invention can ensure that no collision occurs within the formation during formation flight.

[0191] Figure 6 This is a schematic diagram illustrating the distance between multiple unmanned aerial vehicles (UAVs) and obstacles during flight in a formation, according to an embodiment of the present invention. Figure 6 The obstacles in the passage can be static obstacles and / or dynamic obstacles, from Figure 6 As can be seen, the minimum distance between the drone and the obstacle is always maintained above the safe distance, indicating that the algorithm of the present invention can ensure that the drones in the formation will not collide with obstacles during the flight of multiple drones.

[0192] Figure 7 This is a graph showing the velocity components of four drones in a drone formation according to an embodiment of the present invention, along each coordinate axis. Figure 7 It can be seen that each velocity component is always within the constraint conditions and can converge to 0.

[0193] Figure 8 This is a graph showing the acceleration components of four drones in a drone formation according to an embodiment of the present invention, along each coordinate axis. Figure 8 It can be seen that each acceleration component is always within the constraint conditions and can converge to 0.

[0194] (2) Simulation verification of 16 UAVs

[0195] The simulation parameters for the collision avoidance test of 16 UAV formation trajectory planning are shown in Tables 3 and 4. Table 3 shows the number of UAVs in the formation and the number of UAVs within each formation. Table 4 shows the initial and expected positions of the four UAV formations in the inertial frame, as well as their expected relative positions with other UAVs in each formation and with UAV No. 1.

[0196] Table 3: Simulation parameters for the simulation experiment with 16 UAVs

[0197]

[0198] Table 4: Initial and Expected Positions of Formation Drones in a Simulation Experiment with 16 Drones

[0199] Expected relative position

[0200]

[0201] Figure 9 This is a flight trajectory diagram of a 16-drone formation collision avoidance test according to an embodiment of the present invention. Each formation consists of 4 drones, for a total of 4 drone formations. Figure 9 The data shows that the four groups (four drones in each group) of drones can quickly form a formation during flight and can still complete obstacle avoidance tasks well and quickly reach their respective target points even when there are other drone formations as obstacles. At the same time, the formation can also maintain a good effect.

[0202] Figure 10 This is a cost function graph of a formation of 16 drones according to an embodiment of the present invention. Figure 10 As can be seen, the cost of each drone formation converges to near zero as it reaches the target point. The cost increases slightly during obstacle avoidance and then decreases rapidly after obstacle avoidance is completed. This demonstrates that the algorithm of this invention can effectively maintain the stability of the system even in multi-drone formation planning in obstacle-prone environments.

[0203] Figure 11 This is a diagram showing the formation error variation of a 16-drone formation according to an embodiment of the present invention. Figure 11 As can be seen, the formation error of the four drone formations gradually converges to 0 over time. The formation error only increases during obstacle avoidance, but quickly transitions and decreases. This indicates that multiple drone formations can still fly in expected formation and achieve rapid transitions in obstacle environments.

[0204] Figure 12 This is a schematic diagram illustrating the distances between drones during multi-drone formation flight according to an embodiment of the present invention. Figure 12 As can be seen, the distance between each drone is always maintained above the safe distance, indicating that the on-demand collision avoidance algorithm designed in this invention can ensure that no collisions occur between different groups of drones or between drones in the same group during the flight process of multi-drone formation trajectory planning, thus ensuring the safety of the flight process.

[0205] (3) Comparison and verification of anti-collision algorithms

[0206] To verify the effectiveness of the collision avoidance algorithm designed in this invention, the on-demand collision avoidance algorithm was compared with the BVC collision avoidance algorithm. The comparison environment was a test environment for trajectory planning, collision avoidance, and obstacle avoidance of four UAVs in formation. The planned trajectory distance, average speed, average acceleration, average solution time, and flight time of the formation UAVs were compared respectively. Tables 5, 6, and 7 show the performance of on-demand collision avoidance in the input space, on-demand collision avoidance in the state space, and the performance of the BVC collision avoidance algorithm, respectively.

[0207] As shown in Tables 5-7, the trajectory length planned using the on-demand collision avoidance algorithm of this invention is shorter than that planned using the BVC collision avoidance algorithm. The average speed of the UAVs within the formation is greater than the average speed of the UAVs using the BVC collision avoidance algorithm, indicating that the trajectory planned by the on-demand collision avoidance algorithm of this invention is superior to that planned by the BVC collision avoidance algorithm. The average solution time for the UAVs using the on-demand collision avoidance algorithm of this invention is much shorter than the planning time using the BVC collision avoidance algorithm, indicating that the on-demand collision avoidance algorithm of this invention consumes fewer computational resources and is easier to implement in experiments.

[0208] Table 5 Performance of Input Space On-Demand Collision Avoidance Algorithm

[0209]

[0210] Table 6 Performance of State-Space On-Demand Collision Avoidance Algorithm

[0211]

[0212] Table 7 Performance of BVC Collision Avoidance Algorithm

[0213]

[0214] Figure 13 This diagram illustrates the formation error of three algorithms during a collision avoidance and obstacle avoidance test involving four drones in formation. Figure 13As can be seen, the collision avoidance algorithm designed in this invention converges faster during formation collision avoidance, with minimal changes in formation error and rapid convergence to zero during obstacle avoidance. In contrast, the BVC collision avoidance algorithm converges slower than the algorithm designed in this invention, and due to its excessive conservatism, the formation error fluctuates significantly during obstacle avoidance and fails to converge quickly. This demonstrates that the collision avoidance algorithm designed in this invention exhibits superior performance in formation collision avoidance trajectory planning.

[0215] This invention verifies through numerical simulation that the proposed DMPC trajectory planning algorithm, while ensuring the safety of multi-UAV formation (obstacle avoidance and collision avoidance), has a solution time of less than 5 milliseconds for each UAV in the formation. Furthermore, the collision avoidance algorithm designed in this invention is compared with the BVC collision avoidance algorithm under the same environment, demonstrating the superiority of this algorithm.

[0216] In summary, simulation results demonstrate that the DMPC trajectory planning algorithm based on on-demand collision avoidance proposed in this invention can safely achieve formation and maintenance in obstacle-filled environments. Furthermore, it verifies that the on-demand collision avoidance algorithm designed in this invention has advantages in both computational complexity and convergence speed compared to the commonly used BVC collision avoidance algorithm. Moreover, it verifies that in the process of planning the trajectory of multiple UAV formations in complex environments, the algorithm of this invention can still well meet the formation obstacle avoidance requirements and has excellent real-time performance.

[0217] The above specific embodiments further illustrate the purpose, technical solution, and beneficial effects of the present invention. It should be understood that the above are merely specific embodiments of the present invention and are not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A method for planning unmanned aerial vehicle (UAV) formation trajectories based on model predictive control, characterized in that, include: Based on the principle of discrete-time linear invariance, the centroid motion model of a single UAV is determined by using the three-dimensional state vector of a single UAV, a predefined state matrix, a position reference input vector, and a predefined input matrix, and a discrete linear time-invariant system model of the UAV formation is obtained based on the centroid motion model of each UAV. Based on the user's predefined requirements, the trajectory planning objective of the UAV formation from the starting point to the target endpoint is designed using the three-dimensional state vector and centroid motion model of the single UAV and the discrete linear time-invariant system model of the UAV formation. The trajectory planning objective includes a cost function and constraints. Using an on-demand collision avoidance algorithm, the system detects inter-drone collision information and obstacle collision information of a single UAV at the current discrete moment. Based on a distributed model predictive control algorithm, it uses the centroid motion model of the single UAV to predict the predicted trajectory of the single UAV that satisfies the trajectory planning objective. The system then applies a specified set of elements of the predicted trajectory to the centroid motion model of the single UAV to update the three-dimensional state vector of the single UAV. This includes: obtaining the collision information of other UAVs at the previous moment in the k-th time step of the single UAV through a communication sharing mechanism with other UAVs in a preset neighborhood, where k is a positive integer. Under the condition of satisfying collision avoidance constraints, based on the on-demand collision avoidance algorithm, the collision information of the other UAVs at the previous time point in the k-th time step is used to detect the inter-UAV collision information and obstacle collision information of the single UAV at the current time point in the k-th time step; Construct a Bernstein polynomial of a preset order, and use a predefined set of control points and the Bernstein polynomial to determine a Bézier curve in three-dimensional space with respect to duration. Based on the duration of the preset expected discrete time step, calculate the duration of the single UAV within a continuous time range over multiple future time steps; The position reference input vector of the single UAV within the duration of the continuous time range is parameterized into a concatenation of multiple Bézier curves, and the trajectory of the single UAV within the future multiple time steps is predicted based on the concatenation of multiple Bézier curves. Based on the centroid motion model of the single UAV, a linear prediction model is determined to represent the state of the single UAV in the future multiple time steps, and the affine function of the predicted trajectory of the single UAV is obtained using the linear prediction model. The specified group elements of the predicted trajectory are applied to the centroid motion model of the single UAV to update the three-dimensional state vector of the single UAV, and the updated three-dimensional state vector of the single UAV is shared in the UAV formation. Repeated detection, trajectory acquisition, update, and sharing operations continue until the drone formation reaches the target endpoint.

2. The method according to claim 1, characterized in that, Based on the principle of discrete-time linear invariance, the centroid motion model of a single UAV is determined using its three-dimensional state vector, predefined state matrix, position reference input vector, and predefined input matrix. Based on the centroid motion model of each UAV, a discrete-time linear invariant system model of the UAV formation is obtained, including: Using the first of the single drones The three-dimensional state vector at the nth time step, the predefined state matrix, and the nth time step. The position reference input vector at the nth time step and the predefined input matrix are used to obtain the nth time step of the single UAV. A three-dimensional state vector at each time step, wherein the three-dimensional state vector includes a three-dimensional position vector and a three-dimensional velocity vector, and the single UAV is equipped with a position tracking controller, wherein... It is a positive integer; Based on the discrete linear invariance principle, using the first... The three-dimensional state vector at the nth time step, the predefined state matrix, and the nth time step. The position reference input vector at the i-th time step, the predefined input matrix, and the i-th time step. The three-dimensional state vector at each time step determines the center-of-mass motion model of the single UAV; The initial state matrix and initial input matrix of the UAV formation are determined based on the state matrix and input matrix of each UAV in the UAV formation, and the Kronecker product operation is performed on the initial state matrix and initial input matrix of the UAV formation using a predefined identity matrix to obtain the state matrix and input matrix of the UAV formation. Using the centroid motion model of each UAV in the UAV formation, the state matrix of the UAV formation, and the input matrix of the UAV formation, a discrete linear time-invariant system model of the UAV formation is obtained. A directed graph is used to represent the communication topology of the UAV formation, wherein the directed graph includes nodes representing each UAV and edges representing the communication between the UAVs.

3. The method according to claim 2, characterized in that, The cost functions include a trajectory error cost function, a UAV formation error cost function based on a directed graph, a control input cost function, a control input variation cost function, and a relaxation variable cost function. The constraints include trajectory continuity constraints, physical constraints based on UAV dynamic feasibility, inter-UAV collision avoidance constraints, and obstacle collision avoidance constraints.

4. The method according to claim 3, characterized in that, Based on user-defined requirements, the cost function of the UAV formation from the starting point to the target endpoint is designed using the three-dimensional state vector and centroid motion model of the individual UAV, as well as the discrete linear time-invariant system model of the UAV formation. Based on the predicted position, target position, and positive term weight of the difference between the predicted position and the target position of the single UAV at each time step in the prediction time domain, the error term of the single UAV based on the L2 paradigm at each time step in the prediction time domain is obtained. Based on the centroid motion model of the single UAV, the predicted position of the single UAV in the prediction time domain is transformed into an affine function of the input sequence represented by the predicted trajectory; The L2-norm-based error term is transformed into a quadratic cost function associated with the input sequence represented by the predicted trajectory using the affine function to obtain the trajectory error cost function.

5. The method according to claim 4, characterized in that, Also includes: Based on the consistency principle, the directed graph is used to determine other drones that communicate with the single drone within the drone formation, and the expected relative distance between the single drone and the other drones that communicate with the single drone is obtained. Based on the predefined formation error weights, the expected relative distance, the predicted position of the single UAV at each time step in the prediction time domain, and the predicted positions of other UAVs in the UAV formation at each time step in the prediction time domain, the formation error function of the single UAV based on the L2 paradigm is determined. The formation error function of the single UAV based on the L2 paradigm is transformed into a quadratic cost function associated with the input sequence represented by the predicted trajectory using the affine function to obtain the UAV formation error cost function based on the directed graph.

6. The method according to claim 4, characterized in that, Also includes: The position reference input vector of the single UAV is differentiated with respect to time, the square of the derivative is integrated with respect to time, and the integral result is weighted and summed with predefined positive weights to obtain the control input cost function. According to the aforementioned single drone in the first Within the first time step The location of the time point references the input vector and the position at the twentieth time point. Within the first time step Using the location of the input vector at each time point as a reference, we obtain the control input change cost function based on the L1 paradigm. The slack variables used for the constraints are restricted according to preset constraints, and multiple different preset positive weights are used to calculate the slack variables used for the constraints to obtain a slack variable cost function based on the L2 paradigm.

7. The method according to claim 3, characterized in that, Based on user-defined requirements, the constraints for the UAV formation from the starting point to the target endpoint are designed using the three-dimensional state vector and centroid motion model of the single UAV, as well as the discrete linear time-invariant system model of the UAV formation. Based on the centroid motion model of the single UAV and the duration of the specified desired discrete time step, the duration of multiple continuous time steps is obtained, and the position reference input vector of the single UAV within the duration is parameterized into multiple Bézier curves. The initial control points of the multiple Bézier curves are set as a preset constant vector, and the difference between the control points of the multiple Bézier curves and the derivatives of the control points is set as a preset constant. A relational tuple is established using the linear relationship between the control points of multiple Bézier curves and the derivatives of the control points to represent the trajectory continuity constraint; Based on the physical attribute information of the single UAV, the multi-order derivative of the position reference input vector of the single UAV is calculated, thereby determining the minimum control input vector and the maximum control input vector of the single UAV. By utilizing the minimum and maximum control input vectors of the single UAV and the acceleration information of the single UAV, the physical constraints based on the dynamic feasibility of the UAV are obtained.

8. The method according to claim 7, characterized in that, Also includes: In the prediction time domain, the centroid motion model of the single UAV is used and the position reference input vector of the single UAV is constrained to achieve collision avoidance constraints for the single UAV. The collision avoidance constraints include inter-UAV collision avoidance constraints and obstacle collision avoidance constraints. Specifically, within the prediction time domain, the collision avoidance constraints for the single UAV are achieved by utilizing the centroid motion model of the single UAV and constraining the position reference input vector of the single UAV, including: Based on the minimum flight radius of the single UAV, the initial collision avoidance constraints are determined using the predicted position of the single UAV, the predicted positions of other UAVs in the UAV formation, and the predicted positions of obstacles. The radius around the single UAV is modeled based on the minimum flight radius of the single UAV, and the collision boundary of the single UAV and other UAVs and obstacles within the collision boundary are determined using the modeling results. By relaxing the initial collision avoidance constraint with the relaxation variables used for the constraint conditions, an intermediate collision avoidance constraint is obtained, and the Taylor series expansion of the predicted position of the single UAV at a specific discrete time in the prediction time domain is linearized to obtain the linearized result. The relaxation variables used for the constraints are expanded using other drones and obstacles within the collision boundary, and the linearization result is converted into an affine function of the expanded relaxation variables. The collision avoidance constraint is obtained by superimposing the affine functions of the expanded relaxation variables of all drones within the collision boundary, based on the other drones and obstacles.

Images