Trajectory planning method and device and unmanned aerial vehicle cluster

Abstract

The application provides a trajectory planning method and device and a UAV cluster, and relates to the field of UAVs. The centralized trajectory planning problem of the UAV cluster based on MPC is decomposed by using the alternating direction multiplier method to obtain a first cost function and a second cost function, so that each trajectory point between the starting point and the ending point is solved based on the first cost function and the second cost function in the process of trajectory planning, and data exchange is performed with each neighboring UAV in the process of trajectory point solving to ensure information cooperation between the UAVs to avoid collision. In this way, the high computational complexity and the insufficient reliability caused by the centralized method can be effectively solved, and the problem is decomposed by combining the distributed optimization idea of the alternating direction multiplier method, data exchange is performed with the neighboring UAV to avoid mutual collision, and the continuous and smooth trajectory meeting the requirements can be more efficiently generated.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of unmanned aerial vehicles, and in particular, to a trajectory planning method and device and an unmanned aerial vehicle cluster. BACKGROUND

[0002] In recent years, with the rapid development of new generation information technologies such as artificial intelligence and network communication, intelligent unmanned aerial vehicle cluster systems inspired by biological cluster behavior have attracted widespread attention. Intelligent unmanned aerial vehicle cluster systems refer to a whole system in which a certain number of unmanned aerial vehicles can complete complex tasks through mutual cooperation and information sharing. Compared with single unmanned aerial vehicle systems, unmanned aerial vehicle cluster systems have incomparable advantages in timeliness, economy and functionality. Unmanned aerial vehicle cluster technology has become an important development direction of unmanned aerial vehicle system applications, and is widely used in civil fields such as search and rescue, agricultural plant protection, power line patrol and environmental monitoring, and also plays an important role in special tasks such as firepower attack, electronic countermeasure and radar decoy. Therefore, the development of unmanned aerial vehicle cluster related technologies is urgently needed for the industry, and has important significance for promoting social development and promoting scientific and technological progress.

[0003] As a key component of unmanned aerial vehicle cluster in task execution, trajectory planning is a bridge between perception and control, determines the efficiency of unmanned aerial vehicle cluster in cooperative task execution, and is a bottleneck technology for realizing the autonomy and intelligence of the cluster. Cooperative trajectory planning of unmanned aerial vehicle cluster is not simply superimposing the trajectories of multiple unmanned aerial vehicles planned separately. In the process of cooperative trajectory planning, many factors are involved, such as the complexity of the flight environment, the explosive growth of problem dimensions caused by the expansion of the number of unmanned aerial vehicles, and the conflict between unmanned aerial vehicle tasks. These factors are intertwined, making the problem extremely complex.

[0004] For the cluster trajectory planning problem, most existing research works adopt centralized architecture system, and combine pure mathematical optimization, heuristic intelligence and biological population evolution to realize cooperative trajectory planning of unmanned aerial vehicle cluster. These centralized methods need a central computing unit in the cluster system, the central unit can obtain the state information of the whole system, and the planning results are transmitted to each unmanned aerial vehicle. Although these centralized methods can plan globally optimal trajectories, the biggest disadvantage is that the calculation is large and the calculation time is long, which seriously depends on the communication system, and once the central node fails, the whole system will collapse, and it is difficult to directly extend to a larger cluster system. SUMMARY

[0005] The application aims to provide a trajectory planning method, device and unmanned aerial vehicle cluster, so as to effectively solve the problems of high calculation complexity and insufficient reliability caused by the centralized method in the prior art, and combine the distributed optimization idea such as the alternating direction multiplier method to decompose the centralized trajectory planning problem of the unmanned aerial vehicle cluster based on MPC (Model Predictive Control), exchange data with neighboring unmanned aerial vehicles to avoid mutual collision, and more efficiently generate continuous and smooth trajectories meeting the requirements.

[0006] Embodiments of the application can be implemented as follows:

[0007] In a first aspect, embodiments of the application provide a trajectory planning method applied to any target unmanned aerial vehicle in an unmanned aerial vehicle cluster, wherein the target unmanned aerial vehicle corresponds to a set of neighboring unmanned aerial vehicles each of which needs to avoid collision, and the method comprises the following steps:

[0008] Obtaining a starting point and an ending point, and initializing to obtain an input parameter set;

[0009] Taking the starting point as a current trajectory point;

[0010] According to the input parameter set, a first cost function and a second cost function, exchanging data with each neighboring unmanned aerial vehicle to determine an input parameter set and a target flat input of a next trajectory point; the first cost function and the second cost function are obtained by decomposing the centralized trajectory planning problem of the unmanned aerial vehicle cluster based on MPC by using the alternating direction multiplier method;

[0011] Based on state information of the current trajectory point and the target flat input, determining state information and output information of the next trajectory point;

[0012] Judging whether the next trajectory point is the ending point;

[0013] If yes, obtaining state information and output information of K trajectory points from the starting point to the ending point to generate a smooth flight trajectory;

[0014] If no, taking the next trajectory point as the current trajectory point, and returning to execute the step of exchanging data with each neighboring unmanned aerial vehicle to determine an input parameter set and a target flat input of a next trajectory point according to the input parameter set, the first cost function and the second cost function, until the next trajectory point is the ending point, and obtaining state information and output information of K trajectory points from the starting point to the ending point to generate a smooth flight trajectory.

[0015] In a second aspect, the embodiments of the present application further provide a trajectory planning device, which is applied to any target unmanned aerial vehicle in an unmanned aerial vehicle cluster, and the target unmanned aerial vehicle corresponds to a neighborhood unmanned aerial vehicle set composed of at least one neighborhood unmanned aerial vehicle which needs to avoid collision, and the device comprises:

[0016] a data acquisition module, configured to acquire a starting point and an ending point, and initialize an input parameter set;

[0017] a trajectory planning module, configured to:

[0018] take the starting point as a current trajectory point;

[0019] exchange data with each neighborhood unmanned aerial vehicle according to the input parameter set, a first cost function and a second cost function to determine an input parameter set and a target flat input of a next trajectory point; the first cost function and the second cost function are obtained by decomposing an unmanned aerial vehicle cluster centralized trajectory planning problem based on MPC by using an alternating direction multiplier method;

[0020] determine state information and output information of the next trajectory point based on state information of the current trajectory point and the target flat input;

[0021] determine whether the next trajectory point is the ending point;

[0022] if yes, obtain state information and output information of K trajectory points from the starting point to the ending point to generate a smooth flight trajectory;

[0023] if no, take the next trajectory point as the current trajectory point, and return to execute the step of exchanging data with each neighborhood unmanned aerial vehicle according to the input parameter set, the first cost function and the second cost function to determine an input parameter set and a target flat input of a next trajectory point until the next trajectory point is the ending point, and obtain state information and output information of K trajectory points from the starting point to the ending point to generate a smooth flight trajectory.

[0024] In a third aspect, the embodiments of the present application further provide an unmanned aerial vehicle cluster, which comprises N unmanned aerial vehicles, and each unmanned aerial vehicle is configured to perform trajectory planning in a preset flight area according to the trajectory planning method in the first aspect, so as to realize a flight task of the unmanned aerial vehicle cluster.

[0025] Compared with the prior art, the embodiment of the present application provides a trajectory planning method, device and unmanned aerial vehicle cluster, the centralized trajectory planning problem of the unmanned aerial vehicle cluster based on MPC is decomposed by using the alternating direction multiplier method, the first cost function and the second cost function are obtained, so that each trajectory point between the starting point and the ending point is solved based on the first cost function and the second cost function in the process of trajectory planning, and data exchange is carried out with each neighboring unmanned aerial vehicle in the process of trajectory point solving to ensure information cooperation between the unmanned aerial vehicles to avoid collision. In this way, the shortcomings of high computational complexity and insufficient reliability caused by the centralized method can be effectively solved, and the problem decomposition is carried out in combination with the distributed optimization idea of the alternating direction multiplier method, data exchange is carried out with the neighboring unmanned aerial vehicles to avoid mutual collision, and the continuous smooth trajectory meeting the requirements can be more efficiently generated. BRIEF DESCRIPTION OF DRAWINGS

[0026] In order to more clearly illustrate the technical solutions of the embodiments of the present application, the drawings needed to be used in the embodiments will be briefly introduced below, and it should be understood that the following drawings only show some embodiments of the present application, and therefore should not be regarded as a limitation on the scope, and for those skilled in the art, other related drawings can also be obtained without creative labor on the basis of these drawings.

[0027] Figure 1 One of the flowcharts of the trajectory planning method provided by the embodiment of the present application.

[0028] Figure 2 The second flowchart of the trajectory planning method provided by the embodiment of the present application.

[0029] Figure 3 The third flowchart of the trajectory planning method provided by the embodiment of the present application.

[0030] Figure 4 The trajectory planning effect diagram of case one.

[0031] Figure 5 The distance diagram between unmanned aerial vehicles of case one.

[0032] Figure 6 The statistical result diagram of the distance between the unmanned aerial vehicles and the obstacles of case one.

[0033] Figure 7 The trajectory planning effect diagram of case two.

[0034] Figure 8 The distance diagram between unmanned aerial vehicles of case two.

[0035] Figure 9 The statistical result diagram of the distance between the unmanned aerial vehicles and the obstacles of case two.

[0036] Figure 10 A structural schematic diagram of a trajectory planning device provided by an embodiment of the present application. DETAILED DESCRIPTION

[0037] In order to make the objects, technical solutions and advantages of the embodiments of the present application clearer, the following will clearly and completely describe the technical solutions in the embodiments of the present application with reference to the drawings in the embodiments of the present application. Obviously, the described embodiments are only some of the embodiments of the present application but not all the embodiments. The components of the embodiments of the present application described and shown in the drawings can be arranged and designed in various different configurations.

[0038] Therefore, the following detailed description of the embodiments of the present application provided in the drawings is not intended to limit the scope of the claimed present application, but only represents selected embodiments of the present application. Based on the embodiments in the present application, all other embodiments obtained by those of ordinary skill in the art without creative work are within the scope of protection of the present application.

[0039] It should be noted that: similar reference numbers and letters represent similar items in the following drawings, therefore, once an item is defined in one drawing, it does not need to be further defined and explained in subsequent drawings.

[0040] In addition, if the terms "first", "second" and the like are used, they are only used to distinguish descriptions, and cannot be understood as indicating or implying relative importance.

[0041] It should be noted that: the features in the embodiments of the present application can be combined with each other without conflict.

[0042] For the cluster trajectory planning problem, most of the existing research works adopt centralized architecture system, and combine pure mathematical optimization, heuristic intelligence and biological population evolution to realize the cooperative trajectory planning of the unmanned aerial vehicle cluster. These centralized methods need a central computing unit in the cluster system, the central unit can obtain the state information of the whole system, and the planning results are transmitted to each unmanned aerial vehicle. Although these centralized methods can plan the globally optimal trajectory, the biggest disadvantage is that the calculation amount is large, the calculation time is long, it is seriously dependent on the communication system, and once the central node fails, the whole system will collapse, and it is difficult to directly extend to a larger cluster system.

[0043] Based on the discovery of the above technical problems, the inventors propose the following technical solutions to solve or improve the above problems after creative labor. It should be noted that the defects in the above prior art solutions are the result of the inventors' careful research after practice, therefore, the discovery process of the above problems and the solutions proposed by the embodiments of the present application to solve the above problems should be the contribution of the inventors to the present application during the invention process, and should not be understood as technical content known to those skilled in the art.

[0044] The inventors found through research that, in essence, the cooperative trajectory planning of a UAV swarm is a problem of simultaneously optimizing multiple targets, which requires not only to ensure that the individual trajectory of each UAV meets the flight requirements, but also to ensure that the trajectories of multiple UAVs can cooperate to achieve the overall goal of the swarm. In recent years, distributed optimization theory and application have received more and more attention and have gradually penetrated into various aspects of scientific research, engineering application and social life. Distributed optimization is to effectively achieve the optimization task through the cooperation and coordination between multiple agents, which can be used to solve large-scale complex optimization problems that centralized algorithms cannot handle, which is consistent with the characteristics of the UAV swarm system. Specifically, in the distributed optimization of a multi-agent system, the goal of distributed optimization is to minimize a global objective function through local computation and communication in a distributed manner, and this function is the sum of the sub-objective functions of all agents. Although distributed optimization has been a long-term research topic in the field of optimization, due to the fact that trajectory planning is mostly a constrained non-convex optimization problem, plus the communication overhead brought by cooperation, its application in UAV swarm technology is still in its infancy. Currently, the alternating direction multiplier method (ADMM), as one of the distributed optimization algorithms, has been proven to have excellent convergence speed, computational efficiency and communication efficiency in the field of robotics, making it an attractive choice for solving the trajectory planning problem of a UAV swarm.

[0045] Therefore, to solve the series of problems caused by the centralized architecture system used to solve the swarm trajectory planning problem, the inventors propose a way to solve the trajectory planning problem of a UAV swarm based on a distributed architecture. In the distributed architecture, there is no central computing unit, and each UAV calculates and generates its own trajectory, which has adaptive, self-organizing and good coordination performance, and is more suitable for online real-time trajectory planning than the centralized architecture.

[0046] Therefore, the embodiment of the present application provides a trajectory planning method, which aims to generate a smooth flight trajectory for each unmanned aerial vehicle in a cluster system in real time, so as to reach a target position from a starting position, while considering the kinematic constraints of the unmanned aerial vehicle, ensuring mutual collision avoidance between unmanned aerial vehicles, and avoiding collision with external environmental obstacles, so as to ensure the lowest or lower total cost. Moreover, according to the trajectory planning characteristics, the idea of a distributed optimization algorithm is effectively combined, so as to further reduce the problem complexity and improve the efficiency of the trajectory planning of the unmanned aerial vehicle cluster. The following will be described in detail through the embodiment and in combination with the accompanying drawings.

[0047] It should be noted that the trajectory planning method provided by the embodiment of the present application can be applied to a target unmanned aerial vehicle, which is any one of N unmanned aerial vehicles in a cluster of unmanned aerial vehicles, and there is at least one neighborhood unmanned aerial vehicle set corresponding to the target unmanned aerial vehicle, which needs to avoid collision. Among them, the neighborhood unmanned aerial vehicle is only a relative concept. For example, assuming that the cluster of unmanned aerial vehicles includes 20 unmanned aerial vehicles (unmanned aerial vehicle 1-unmanned aerial vehicle 20), for unmanned aerial vehicle 5, the neighborhood unmanned aerial vehicle set of unmanned aerial vehicle 5 can include unmanned aerial vehicle 4 and 6; and for unmanned aerial vehicle 13, the neighborhood unmanned aerial vehicle set of unmanned aerial vehicle 13 can include unmanned aerial vehicle 11, unmanned aerial vehicle 12 and unmanned aerial vehicle 14. The example is only an example, which is not limited here.

[0048] Reference is made to Figure 1 , Figure 1 A flowchart of a trajectory planning method provided by the embodiment of the present application is shown, the execution subject of the trajectory planning method is a target unmanned aerial vehicle in a cluster of unmanned aerial vehicles, and the trajectory planning method includes the following steps S101-S107:

[0049] S101, obtaining a starting point and an ending point, and initializing to obtain an input parameter set.

[0050] It can be understood that for a flight task, all unmanned aerial vehicles in a cluster of unmanned aerial vehicles need to be cooperated to complete, and the starting point and the ending point of different unmanned aerial vehicles can be different or the same, and the starting point and the ending point of the target unmanned aerial vehicle are set according to actual task requirements.

[0051] In the embodiment, the target unmanned aerial vehicle needs to be initialized at the starting point position of itself, and the initialization can obtain the starting point, the ending point and the input parameter set at the initialization time.

[0052] S102, taking the starting point as a current trajectory point.

[0053] S103, exchanging data with each neighborhood unmanned aerial vehicle to determine an input parameter set and a target flat input of a next trajectory point according to the input parameter set, a first cost function and a second cost function.

[0054] In the embodiment, the first cost function and the second cost function are obtained by decomposing the centralized trajectory planning problem based on discrete MPC using the distributed optimization idea of ADMM.

[0055] S104, determine the state information and output information of the next trajectory point based on the state information of the current trajectory point, target flat input.

[0056] The state information can represent the flat state of the target UAV, including position, velocity, acceleration and yaw angle information. The output information can represent the flat output of the target UAV, including position and yaw angle. The target flat input can represent the third derivative of the position of the target UAV and the yaw angle rate.

[0057] S105, determine whether the next trajectory point is the end point.

[0058] If the next trajectory point is the end point, the trajectory planning of the target UAV itself is completed, that is, step S107 is executed to obtain the state information and output information of the K trajectory points from the starting point to the end point to generate a smooth flight trajectory; if the next trajectory point is not the end point, the following step S106 is executed and then the above step S103 is executed until the next trajectory point is the end point, that is, step S107 is executed to obtain the state information and output information of the K trajectory points from the starting point to the end point to generate a smooth flight trajectory.

[0059] S106, take the next trajectory point as the current trajectory point.

[0060] S107, obtain the state information and output information of the K trajectory points from the starting point to the end point to generate a smooth flight trajectory.

[0061] The trajectory planning method provided by the embodiment can effectively solve the problems of high computational complexity and insufficient reliability caused by centralized methods, and combine the distributed optimization idea of the alternating direction multiplier method to decompose the problem and exchange data with neighboring UAVs to avoid mutual collision, so that a continuous and smooth trajectory meeting the requirements can be generated more efficiently.

[0062] Here, the process of decomposing the centralized trajectory planning problem based on discrete MPC using the distributed optimization idea of ADMM is introduced. The process is roughly divided into the following three steps:

[0063] Step 1, obtain the UAV dynamics model, trajectory performance index and trajectory constraint information of the UAV;

[0064] Step 2, construct the centralized trajectory planning problem of the UAV cluster based on MPC;

[0065] Step 3, based on the ADMM decomposition of the original problem, a distributed trajectory planning method for UAV swarm is designed.

[0066] First, the data in step 1 is introduced:

[0067] First, each UAV in the UAV swarm can be a quadrotor UAV. Due to its typical differential flatness characteristics, the nonlinear model of the UAV is converted into an equivalent linear flat model using feedforward linearization, which reduces the dimension of the planning space and the difficulty of solving the problem. At the same time, the planned trajectory is feasible for the UAV. Therefore, the UAV dynamics model of the quadrotor UAV can be represented by the dynamic equation:

[0068] v k+t+1|k =Av k+t|k +Bu k+t|k

[0069] ζ k+t+1|k =Cv k+t+1|k

[0070] where t = 1, 2, …, H, H represents the prediction domain length, A, B, C are coefficient matrices; v k+t|k , v k+t+1|k represent the flat state of the UAV at the kth trajectory point in the prediction domain: the tth prediction point, the t+1th prediction point; u k+t|k represents the flat input of the UAV at the kth trajectory point in the prediction domain at the tth prediction point; ζ k+t+1|k represents the flat output of the UAV at the kth trajectory point in the prediction domain at the t+1th prediction point.

[0071] The flat state of the UAV includes position, velocity, acceleration and yaw angle information. The flat input of the UAV includes the third derivative of the position and the yaw angle rate. The flat output of the UAV includes position and yaw angle.

[0072] where x, y, z together represent the position coordinates of the UAV; together represent the velocity of the UAV; together represent the acceleration of the UAV. represents the third derivative of the position coordinates of the UAV; represents the yaw angle of the UAV, represents the yaw angle rate of the UAV.

[0073] Second, the trajectory performance indicators can include the smoothness of the trajectory and the proximity to the target position (i.e. the end point) as much as possible.

[0074] Third, the trajectory constraint information of the UAV includes the initial state, target state, kinematic constraints, obstacle collision avoidance constraints, and inter-UAV collision avoidance constraints. Among these, the obstacle collision avoidance and inter-UAV collision avoidance constraints can be addressed using the Control Obstacle Function (CBF) method to ensure the safety of the planned trajectory. Let i represent the control obstacle function between UAV i and its neighboring UAV j, and let j represent the control obstacle function between UAV i and obstacle o, respectively.

[0075] Therefore, under normal circumstances, the inter-machine collision avoidance constraint can be expressed as the following formula (1):

[0076]

[0077] Typically, obstacle collision avoidance constraints can be expressed as follows: (2)

[0078]

[0079]

[0080] Where, p i p j p o These represent the position information of UAV i, neighboring UAV j, and obstacle o, respectively; γ ij γ io All are scaling factors, d safe1 d safe2 Let N be the minimum safe distance between UAV i and its neighboring UAV j, and the minimum safe distance between UAV i and obstacle o. i Let i be the set of neighboring drones.

[0081] In actual flight, drones may collide with each other due to the aerodynamic effects of surrounding drones. Therefore, A scaling matrix Θ was added to model the collision boundary of the UAV as an ellipsoid, reducing the influence of aerodynamic effects. Θ is a diagonal matrix (i.e., the elements on the diagonal are a, b, and c).

[0082] Next, step 2 will be introduced:

[0083] By introducing the rolling time-domain optimization concept of MPC, the UAV swarm trajectory planning problem is described as a multi-objective optimization problem, taking into account the constraints of dynamic performance, obstacle avoidance and collision avoidance, and trajectory performance indicators of the UAV. Thus, a centralized trajectory planning problem based on discrete MPC can be constructed. The objective cost function of this centralized trajectory planning problem is shown in formula (3):

[0084]

[0085] Three trajectory performance indicators are included in the target cost function, which are the deviation of the current state and the target state, the size of the flat input and the size of the flat input change, that is, the purpose of optimization is to make the UAV approach the target state as soon as possible, and at the same time, to ensure the continuity and smoothness of the trajectory, so as to facilitate the effective tracking of the UAV. Q, R and S are all weight coefficient matrices.

[0086] The three constraint conditions of the target cost function are:

[0087] 1. Kinematic constraint of the UAV:

[0088]

[0089]

[0090]

[0091]

[0092] Wherein, Ω is a state constraint set.

[0093] 2. Inter-UAV collision avoidance constraint between UAV i and UAV j:

[0094]

[0095] 3. Obstacle collision avoidance constraint of UAV i:

[0096]

[0097] In order to facilitate subsequent representation, the inter-UAV collision avoidance constraint between UAV i and UAV j is represented by , and the obstacle collision avoidance constraint of the UAV is represented by

[0098] Then, step 3 is introduced:

[0099] The traditional centralized method will result in huge calculation amount, and the system reliability is low, once the central calculation unit fails, the whole cluster system may stop working, and the scale and configuration of the cluster system are not flexible enough, and the system scalability is poor. Based on the idea of distributed optimization algorithm ADMM "divide and conquer", the high-dimensional UAV cluster trajectory planning problem is divided into multiple low-dimensional optimization problems, and each UAV plans its own optimal trajectory, so as to parallel computing to speed up the online solution speed of the trajectory planning problem, and form a distributed UAV cluster cooperative trajectory planning framework, wherein the key is that the UAV and its neighborhood UAVs interact with each other to avoid inter-UAV collision.

[0100] ​For the trajectory planning problem of UAV cluster, the purpose of interaction between UAV and its neighborhood UAV is to tell each other the position information of itself, so as to avoid collision. Based on the idea of ADMM, the invention introduces a flat output copy, that is, the copy w i of ζ i , and uses w i→j to represent the expected output trajectory from UAV i to neighborhood UAV j, then the inter-UAV collision avoidance constraint and obstacle collision avoidance constraint of UAV i can be re-expressed as:

[0101]

[0102]

[0103]

[0104] It can be seen that after introducing the copy of the flat output, the inter-UAV collision avoidance constraint of the UAV can no longer depend on the original trajectory ζ i , but the copies w i and w i→j are used to realize the cluster collaborative collision avoidance.

[0105] Thus, the target cost function can be converted into a new cost function: min f(Z k , U k ) + g(W k ), wherein f(Z k , U k ) and g(W k ) are defined as follows:

[0106]

[0107]

[0108] Wherein, Respectively represent the flat output discrete sequence, the flat input discrete sequence, the flat output discrete sequence copy of UAV i in the prediction domain. I is an indicator function, and Φ represents the obstacle collision avoidance constraint and the inter-UAV collision avoidance constraint of the UAV i. Therefore, the new cost function expression is as follows formula (4):

[0109]

[0110] S.t. means the constraint condition.

[0111] Formula (4) is a general optimization problem with equality constraints, and its augmented Lagrangian form is shown in formula (5):

[0112]

[0113] Where, λ i With λ i→j Let ρ be the dual variable, T be the penalty coefficient, and T be the transpose of the matrix. Then, the ADMM algorithm can be used to solve this optimization problem, iterating sequentially. and λ i With λ i→j The iteration continues until the stopping condition is met.

[0114] The following describes the specific process of solving the trajectory points in this invention.

[0115] In the optional implementation, solving for the next trajectory point from the current trajectory point requires multiple iterations. Figure 1 Based on this, please refer to Figure 2 The sub-steps of step S103 above may include S1031-S1034:

[0116] S1031. Based on the input parameter set, the first cost function and the second cost function, perform two data exchanges with each neighboring UAV to determine a new input parameter set and a flat input discrete sequence.

[0117] S1032. Determine whether the new set of input parameters satisfies the stopping iteration condition;

[0118] S1033. Use the new set of input parameters as the set of input parameters for the next trajectory point, and determine the target flat input for the next trajectory point from the flat input discrete sequence.

[0119] If the new set of input parameters satisfies the stopping iteration condition, then step S1033 is executed to obtain the input parameter set of the next trajectory point; if the new set of input parameters does not satisfy the stopping iteration condition, the iteration continues, that is, the execution of step S1031 is returned until the new set of input parameters satisfies the stopping iteration condition and the input parameter set of the next trajectory point is obtained.

[0120] Optionally, assuming the target drone is drone i in a drone swarm, at the k-th trajectory point, the input parameter set may include: the flat output discrete sequence of the target drone, a copy of the flat output discrete sequence and its local dual variable, the expected dual variable of the target drone with respect to each neighboring drone, and the expected dual variable and expected output sequence of each neighboring drone with respect to the target drone. That is, the input parameter set includes: λ i , Each execution of step S1031 constitutes one iteration, which can be roughly divided into the following 5 steps:

[0121] 1. The target UAV first updates

[0122] 2. The target UAV exchanges data with each neighbor UAV for the first time: sends to neighbor UAV j (j ∈ N i ), and gets new

[0123] 3. The target UAV then updates

[0124] 4. The target UAV then updates λ i ,

[0125] 5. The target UAV exchanges data with each neighbor UAV for the second time: sends λ i→j and to neighbor UAV j (j ∈ N i ), and gets new and new

[0126] After the above-mentioned five steps, the new input parameter set includes: new new new λ i , new new and new

[0127] The above-mentioned five steps are described below.

[0128] In an optional implementation, on the basis of Figure 2 , please refer to Figure 3 , the sub-steps of the step S1031 can include S001-S006.

[0129] S001, input the local dual variable of the target UAV and the copy of the flat output discrete sequence, the expected dual variable of each neighbor UAV to the target UAV and the expected output sequence into the first cost function, to get the updated flat output discrete sequence of the target UAV and the updated flat input discrete sequence of the target UAV.

[0130] In this embodiment, the expression of the first cost function can be as shown in formula (6):

[0131]

[0132]

[0133]

[0134] In formula (6), i and k represent the target UAV and the current trajectory point respectively, H represents the prediction domain length, and H can be set according to the requirements of the flight task, and can generally be 10-20. j e N i represents the jth neighbor UAV in the neighbor UAV set N of the target UAV i. i

[0135] respectively represent the flat output discrete sequence, the flat input discrete sequence, and the flat output discrete sequence copy of the target UAV i at the current trajectory point k; represents the flat output of the Hth prediction point of the current trajectory point k. represents the flat input of the H-1th prediction point of the current trajectory point k. represents the flat output copy of the Hth prediction point of the current trajectory point k.

[0136] respectively represent the transpose of the local dual variable of the target UAV i and the transpose of the expected dual variable of the neighbor UAV j to the target UAV i; represents the expected output sequence of the neighbor UAV j to the target UAV i; p is a penalty coefficient.

[0137] is an output cost function, represents the terminal point; Q, R, and S are weight coefficient matrices; and ‖·‖2 represents an L2 norm.

[0138] s.t. represents that the constraint condition of the first cost function is a state constraint set F, and F includes kinematic constraints and obstacle avoidance constraints of the UAV.

[0139]

[0140]

[0141]

[0142]

[0143] (2) The obstacle avoidance constraint in the state constraint set F is which is expanded as

[0144]

[0145]

[0146] ​Therefore, in formula (6), is needed to solve the update under the premise of minimizing the value of the first cost function Correspondingly, the required input data of the first cost function includes: λi, Then solve the update of the new New

[0147] S002, send the updated flat output discrete sequence of the target UAV to each neighbor UAV, and receive the flat output discrete sequence of each neighbor UAV.

[0148] In this embodiment, step S002 represents the first data exchange of the target UAV: sending the new to the neighbor UAV j (j ∈ N i ), receiving the new

[0149] S003, input the local dual variable, the updated flat output discrete sequence of the target UAV, and the expected dual variable of the target UAV to each neighbor UAV into the second cost function, and cooperatively calculate the updated flat output discrete sequence copy of the target UAV and the expected output sequence of the target UAV to each neighbor UAV.

[0150] In this embodiment, the expression of the second cost function can be as shown in formula (7):

[0151]

[0152]

[0153] In formula (7), denotes the transpose of the expected dual variable of the target UAV i to the neighbor UAV j, the expected output sequence of the target UAV i to the neighbor UAV j,

[0154] s.t. denotes the constraint condition of the second cost function, which includes the inter-vehicle collision avoidance constraint between the target UAV i and the neighbor UAV j.

[0155] represents the inter-vehicle collision avoidance constraint between the target UAV i and the neighbor UAV j, which is expanded as:

[0156]

[0157]

[0158] Θ = diag(a, b, c), a = b = 1, c = 2

[0159] Therefore, in formula (7), is required to minimize the value of the second cost function under the premise of solving the update Correspondingly, the required input data of the second cost function includes: λ i 、 and the new obtained after step S001 is executed The new

[0160] S004, based on the local dual variables, the updated flat output discrete sequence of the target UAV and the copy of the updated flat output discrete sequence of the target UAV, determine the updated local dual variables of the target UAV.

[0161] In this embodiment, the calculation formula of the updated local dual variables of the target UAV i is: In this calculation formula, λ i is the local dual variable in the input parameter set corresponding to this iteration, is the new obtained after executing the above step S001

[0162] S005, for each neighbor UAV, based on the expected dual variables of the target UAV to the neighbor UAV, the flat output discrete sequence of the neighbor UAV, and the expected output sequence of the target UAV to the neighbor UAV, determine the updated expected dual variables of the target UAV to the neighbor UAV.

[0163] In this embodiment, for the neighbor UAV j, the calculation formula of the updated expected dual variables of the target UAV i to the neighbor UAV j is: In this calculation formula, λ i→j is the expected dual variable of the target UAV i to the neighbor UAV j in the input parameter set corresponding to this iteration, is the new of the neighbor UAV j obtained after executing the above step S002 is the new obtained after executing the above step S003

[0164] S006, for each neighbor UAV, send the expected output sequence of the target UAV to the neighbor UAV and the updated expected dual variables of the target UAV to the neighbor UAV to the neighbor UAV, and receive the expected dual variables and the expected output sequence of the neighbor UAV to the target UAV.

[0165] In this embodiment, step S006 represents the second data exchange of the target UAV: sending the new new i→j to the neighbor UAV j (j ∈ N i ), obtaining the new new

[0166] The above steps S001-S006 are the process of one iteration, and after completing the iteration in turn, the new input parameter set is obtained, which can include:

[0167] (1) new

[0168] (2) new

[0169] (3) new i ;

[0170] (4) new

[0171] (5) new new

[0172] The above steps S001-S006 and their detailed descriptions are a complete iteration process, based on which the stopping iteration condition in S1032 can be:

[0173]

[0174]

[0175] wherein ε1 and ε2 both represent a preset threshold. That is, in the stopping iteration condition, only the new new new

[0176] When the three items meet the stopping iteration condition, the iteration is stopped. And the new input parameter set obtained by the last iteration can be used as the input parameter set of the next trajectory point, and the target flat input for the next trajectory point is determined from the new obtained by the last iteration. Then the state information and output information of the next trajectory point are calculated, which is expressed as:

[0177]

[0178]

[0179] wherein, represents state information of the target UAV i at the current trajectory point, is in represents a target flat input, is state information of the next trajectory point, is output information of the next trajectory point.

[0180] For the target UAV i, when the next trajectory point is the terminal point thereof, the target UAV i completes the trajectory planning thereof, and K trajectory points from the starting point to the terminal point can be fitted as a smooth flight trajectory.

[0181] For the UAV cluster, when the N UAVs all reach the respective terminal points, the trajectory planning of the entire cluster is completed, and the algorithm stops.

[0182] It should be noted that the execution sequence of each step in the above method embodiment is not limited to the drawing shown, and the execution sequence of each step is subject to the actual application situation.

[0183] Two cases are given below to verify the effect brought by the trajectory planning method provided by the application in actual application.

[0184] Case 1:

[0185] Suppose that the UAV cluster includes 10 UAVs (No. 1-10), and the flight task is: therefore, the UAVs fly through the environment filled with cylindrical obstacles in the same direction, and the initial positions (unit: m) of No. 1-10, i.e., the starting points, are respectively: T [0, 1, 2] T [0, 3, 2] T [0, 5, 2] T [0, 7, 2] T [0, 9, 2] T [0, 11, 2] T [0, 13, 2] T [0, 15, 2] T [0, 17, 2] T .

[0186] The target positions, i.e., the terminal points, of No. 1-10 are respectively: T [40, 1, 2] T [40, 3, 2] T [40, 5, 2] T [40, 7, 2] T [40, 9, 2][40, 11, 2] T [40, 13, 2] T [40, 15, 2] T [40, 17, 2] T [40, 19, 2] T .

[0187] Here, it is assumed that the yaw angle is always 0, and the speed, acceleration, and third-order derivative of the speed of the UAV are all initially 0, the minimum safety distance d safe1 = 0.4 m, d safe2 = 0.2 m, the number of prediction points in the prediction domain H = 10, the sampling time T s = 0.15 s, the scale factor γ ij = γ io = 0.8, and the penalty coefficient ρ = 1. The three weight coefficient matrices are Q = diag[50, 50, 50, 50], R = diag[5, 5, 5, 5], and Q = diag[10, 10, 10, 10]. The flight area limit of the flight task (in units of m) is as shown in equation (8), the speed limit of the UAV flight (in units of m / s) is as shown in equation (9), and the acceleration limit of the UAV flight (in units of m / s 2 ) is as shown in equation (10):

[0188] p min = [0, -3, 0] T p max = [44, 24, 6] T (8)

[0189] v min = [-3, -3, -3] T v max = [3, 3, 3] T (9)

[0190] a min = [-1, -1, -1] T a max = [1, 1, 1] T (10)

[0191] Based on the above data of Case 1, No. 1-10 each complete its own trajectory planning based on the above trajectory planning method, and the flight trajectory of each can be as shown in Figure 4 .

[0192] When No. 1-10 each flies according to its own trajectory point, the distance between the UAVs is as shown in Figure 5 , and the statistical results of the distance between the UAVs and the obstacles are as shown in Figure 6 .

[0193] Case two:

[0194] Suppose the UAV cluster includes 12 UAVs (1-12), and the flight task is: 12 UAVs (6 on each side) fly through the environment full of cylindrical obstacles. The initial positions (m) of 1-12 are respectively: [0, 3, 3] T , [0, 6, 3] T , [0, 9, 3] T , [0, 12, 3] T , [0, 15, 3] T , [0, 18, 3] T , [40, 3, 3] T , [40, 6, 3] T , [40, 9, 3] T , [40, 12, 3] T , [40, 15, 3] T , [40, 18, 3] T ; The target positions of 1-12 are respectively the initial positions of the opposite UAVs, and other parameters are consistent with those of case one.

[0195] Based on the above data of case two, 1-12 respectively complete their own trajectory planning based on the above trajectory planning method, and the flight trajectories of 1-12 can be shown as Figure 7 When 1-12 fly according to their own trajectory points, the distance between UAVs is shown in Figure 8 , and the statistical results of the distance between UAVs and obstacles are shown in Figure 9 .

[0196] From the effect diagram of case one and case two, it can be seen that the trajectory planning method based on distributed architecture proposed by the application can effectively solve the trajectory planning problem of the UAV cluster, and the generated trajectory is safe and continuous and smooth.

[0197] In order to further verify the efficiency of the trajectory planning, on the basis of the parameters proposed in case one, the performance of the trajectory planning method of the application and the performance of the centralized trajectory planning method (MPC-based UAV cluster trajectory planning method) are compared under different number of UAVs (2, 5, 10, 15), and the statistical table is shown in the following table:

[0198]

[0199] It can be concluded from the table that the length of the trajectory obtained by the trajectory planning method based on the distributed MPC-ADMM of the application for realizing the trajectory planning of the swarm is not much different from the length of the trajectory obtained by the trajectory planning method based on the centralized MPC for realizing the trajectory planning of the swarm. However, the application is shorter in calculation time (that is, the time-consuming of the trajectory planning is shorter), and the advantage of shorter time-consuming is more obvious when the number of unmanned aerial vehicles is larger. Therefore, the expansion of the number of unmanned aerial vehicles of the unmanned aerial vehicle swarm does not affect the time-consuming of the trajectory planning, and even the time-consuming is shorter and stronger, so the trajectory planning method of the application is particularly suitable for large-scale unmanned aerial vehicle trajectory planning tasks.

[0200] In order to perform the above-mentioned method embodiments and the corresponding steps in each possible implementation, the following respectively gives an implementation of a trajectory planning device.

[0201] Please refer to Figure 10 , Figure 10 A structure schematic diagram of a trajectory planning device provided by an embodiment of the application is shown. The trajectory planning device 200 is applied to any target unmanned aerial vehicle in an unmanned aerial vehicle swarm, and the target unmanned aerial vehicle corresponds to at least one neighborhood unmanned aerial vehicle set formed by neighborhood unmanned aerial vehicles that need to avoid collision. The trajectory planning device 200 comprises a data acquisition module 210 and a trajectory planning module 220.

[0202] The data acquisition module 210 is configured to acquire a starting point and an ending point, and initialize an input parameter set;

[0203] The trajectory planning module 220 is configured to: take the starting point as a current trajectory point; exchange data with each neighborhood unmanned aerial vehicle to determine an input parameter set and a target flat input of a next trajectory point according to the input parameter set, a first cost function and a second cost function; the first cost function and the second cost function are obtained by decomposing a centralized trajectory planning problem of the unmanned aerial vehicle swarm based on MPC by using an alternating direction multiplier method; determine state information and output information of the next trajectory point based on state information of the current trajectory point and the target flat input; determine whether the next trajectory point is the ending point; if yes, obtain the state information and the output information of K trajectory points from the starting point to the ending point to generate a smooth flight trajectory; if not, take the next trajectory point as the current trajectory point, and return to execute the step of exchanging data with each neighborhood unmanned aerial vehicle to determine the input parameter set and the target flat input of the next trajectory point according to the input parameter set, the first cost function and the second cost function, until the next trajectory point is the ending point, and the state information and the output information of K trajectory points from the starting point to the ending point are obtained to generate the smooth flight trajectory.

[0204] It should be noted that the trajectory planning module 220 can be specifically configured to implement the above steps S102-S107 and their respective sub-steps. It can be clearly understood by those skilled in the art that, for the convenience and brevity of description, the specific working process of the trajectory planning device 200 described above can refer to the corresponding process in the foregoing method embodiments, and will not be described here.

[0205] The embodiment of the present application also provides a UAV cluster, which comprises N UAVs, each of which is configured to perform trajectory planning in a preset flight area according to the above trajectory planning method, so as to realize the flight task of the UAV cluster.

[0206] In summary, the embodiment of the present application provides a trajectory planning method, device and UAV cluster, which decomposes the centralized trajectory planning problem of the UAV cluster based on MPC by using the alternating direction multiplier method, obtains a first cost function and a second cost function, so as to solve each trajectory point between the starting point and the ending point based on the first cost function and the second cost function in the process of trajectory planning, and exchanges data with each neighboring UAV in the process of trajectory point solving to ensure information coordination between the UAVs to avoid collision. In this way, the disadvantages of high computational complexity and insufficient reliability caused by the centralized method can be effectively solved, and the problem decomposition is combined with the distributed optimization idea of the alternating direction multiplier method to avoid mutual collision by exchanging data with the neighboring UAVs, so that the continuous and smooth trajectory meeting the requirements can be more efficiently generated.

[0207] The above is only a specific embodiment of the present application, but the protection scope of the present application is not limited thereto, and any person skilled in the art can easily think of changes or replacements within the technical range disclosed by the present application, which should be covered within the protection scope of the present application. Therefore, the protection scope of the present application should be subject to the protection scope of the claims.

Claims

1. A trajectory planning method characterized by, The method is applied to any target unmanned aerial vehicle in an unmanned aerial vehicle cluster, the target unmanned aerial vehicle corresponds to a neighborhood unmanned aerial vehicle set composed of at least one neighborhood unmanned aerial vehicle needing to avoid collision, and the method comprises the following steps: Obtaining a starting point and an ending point, and initializing an input parameter set; Taking the starting point as a current trajectory point; According to the input parameter set, a first cost function and a second cost function, data exchange is performed with each neighborhood unmanned aerial vehicle to determine an input parameter set and a target flat input of a next trajectory point; the first cost function and the second cost function are obtained by decomposing an unmanned aerial vehicle cluster centralized trajectory planning problem based on MPC by using an alternating direction multiplier method; Based on state information of the current trajectory point and the target flat input, state information and output information of the next trajectory point are determined; It is judged whether the next trajectory point is the ending point; If yes, state information and output information of K trajectory points from the starting point to the ending point are obtained to generate a smooth flight trajectory; If no, the next trajectory point is taken as the current trajectory point, and the step of performing data exchange with each neighborhood unmanned aerial vehicle according to the input parameter set, the first cost function and the second cost function to determine an input parameter set and a target flat input of a next trajectory point is returned to be executed until the next trajectory point is the ending point, and state information and output information of K trajectory points from the starting point to the ending point are obtained to generate a smooth flight trajectory; The step of performing data exchange with each neighborhood unmanned aerial vehicle according to the input parameter set, the first cost function and the second cost function to determine an input parameter set and a target flat input of a next trajectory point comprises the following steps: Based on the input parameter set, the first cost function and the second cost function, twice data exchange is performed with each neighborhood unmanned aerial vehicle to determine a new input parameter set and a flat input discrete sequence; It is judged whether the new input parameter set meets a stop iteration condition; If yes, the new input parameter set is taken as an input parameter set of the next trajectory point, and a target flat input of the next trajectory point is determined from the flat input discrete sequence; If no, the step of performing twice data exchange with each neighborhood unmanned aerial vehicle based on the input parameter set, the first cost function and the second cost function to determine a new input parameter set and a flat input discrete sequence is returned to be executed until the new input parameter set meets the stop iteration condition, and an input parameter set of the next trajectory point is obtained.

2. The method of claim 1, wherein, The input parameter set comprises a flat output discrete sequence of the target unmanned aerial vehicle, a flat output discrete sequence copy and a local dual variable, an expected dual variable of each neighborhood unmanned aerial vehicle to the target unmanned aerial vehicle, and an expected dual variable and an expected output sequence of each neighborhood unmanned aerial vehicle to the target unmanned aerial vehicle. The step of performing twice data exchange with each of the neighbor unmanned vehicles based on the input parameter set, the first cost function and the second cost function to determine a new input parameter set and a flat input discrete sequence, comprises: inputting the local dual variable and the copy of the flat output discrete sequence of the target unmanned vehicle, the expected dual variable and the expected output sequence of each of the neighbor unmanned vehicles to the target unmanned vehicle into the first cost function to obtain an updated flat output discrete sequence of the target unmanned vehicle and the flat input discrete sequence of the target unmanned vehicle; sending the updated flat output discrete sequence of the target unmanned vehicle to each of the neighbor unmanned vehicles and receiving the flat output discrete sequence of each of the neighbor unmanned vehicles; inputting the local dual variable, the updated flat output discrete sequence of the target unmanned vehicle and the expected dual variable of the target unmanned vehicle to each of the neighbor unmanned vehicles into the second cost function to cooperatively calculate the copy of the updated flat output discrete sequence of the target unmanned vehicle and the expected output sequence of the target unmanned vehicle to each of the neighbor unmanned vehicles; determining an updated local dual variable based on the local dual variable, the updated flat output discrete sequence of the target unmanned vehicle and the copy of the updated flat output discrete sequence of the target unmanned vehicle; for each of the neighbor unmanned vehicles, determining an updated expected dual variable of the target unmanned vehicle to the neighbor unmanned vehicle based on the expected dual variable of the target unmanned vehicle to the neighbor unmanned vehicle, the flat output discrete sequence of the neighbor unmanned vehicle and the expected output sequence of the target unmanned vehicle to the neighbor unmanned vehicle; for each of the neighbor unmanned vehicles, sending the expected output sequence of the target unmanned vehicle to the neighbor unmanned vehicle and the updated expected dual variable of the target unmanned vehicle to the neighbor unmanned vehicle to the neighbor unmanned vehicle and receiving the expected dual variable and the expected output sequence of the target unmanned vehicle to the neighbor unmanned vehicle; wherein the new input parameter set comprises the updated flat output discrete sequence of the target unmanned vehicle, the copy of the updated flat output discrete sequence of the target unmanned vehicle, the updated local dual variable of the target unmanned vehicle, the updated expected dual variable of the target unmanned vehicle to each of the neighbor unmanned vehicles, the newly received expected dual variable and the expected output sequence of each of the neighbor unmanned vehicles to the target unmanned vehicle.

3. The method of claim 2, wherein, The expression of the first cost function is: wherein, , respectively represent the target UAV, the current trajectory point, , represents a prediction domain length, represents a kth prediction point within a prediction domain of a current trajectory point , represents a kth prediction point within a prediction domain of a kth trajectory point; represents a kth prediction point within a prediction domain of a kth trajectory point; represents a kth neighbor UAV within a neighbor UAV set of a target UAV , represents a kth neighbor UAV within a neighbor UAV set of a target UAV , represents a kth neighbor UAV within a neighbor UAV set of a target UAV , , respectively represent a flat output discrete sequence, a flat input discrete sequence, a flat output discrete sequence copy of a target UAV , represents a flat output of a Hth prediction point of a current trajectory point , represents a flat input of a H-1th prediction point of a current trajectory point , represents a flat output copy of a Hth prediction point of a current trajectory point . , denote the transpose of the local dual variable of the target UAV , the neighbor UAV , the desired dual variable of the target UAV ; denote the desired output sequence of the target UAV , the neighbor UAV ; is a penalty coefficient; to output a cost function, denotes the end point; Q, R, S are all weight coefficient matrices; denotes the L2 norm; The constraint conditions representing the first cost function are a set of state constraints , including kinematic constraints of the UAV and obstacle avoidance constraints.

4. The method of claim 3, wherein, The expression of the second cost function is: wherein, denotes the target UAV the neighboring UAVs the transpose of the desired dual variables, the target UAV the neighboring UAVs the desired output sequence, the constraints of the second cost function include inter-vehicle collision avoidance constraints between the target UAV and the neighboring UAVs .

5. The method of claim 4, wherein, Updated target drone The local dual variable of the target drone is: ; The updated target drone To neighborhood drones The desired dual variable for the neighborhood drone is: .

6. The method of claim 5, wherein, The stop iteration condition is: wherein, , each represent a preset threshold value.

7. The method of claim 5, wherein, The state information and the output information of the next trajectory point are: representing a target drone state information of the current trajectory point, for the target flat input, state information of the next trajectory point, output information of the next trajectory point; A, B, C are all coefficient matrices.

8. A trajectory planning apparatus for implementing the method of any one of claims 1 to 7, characterized by The device is applied to any target unmanned vehicle in a cluster of unmanned vehicles, the target unmanned vehicle corresponding to a neighbor unmanned vehicle set comprising at least one neighbor unmanned vehicle which needs to avoid collision, and the device comprises: a data acquisition module configured to acquire a starting point and an ending point and initialize to obtain an input parameter set; a trajectory planning module configured to: take the starting point as a current trajectory point; According to the input parameter set, the first cost function and the second cost function, data is exchanged with each of the neighbor UAVs to determine an input parameter set and a flat input of a next trajectory point; the first cost function and the second cost function are obtained by decomposing a centralized trajectory planning problem of the UAV cluster based on MPC using an alternating direction method of multipliers; Based on the state information of the current trajectory point and the flat input, state information and output information of the next trajectory point are determined; It is judged whether the next trajectory point is the terminal point; If yes, state information and output information of K trajectory points from the starting point to the terminal point are obtained to generate a smooth flight trajectory; If no, the next trajectory point is taken as the current trajectory point, and the method of claim 1 is executed again until the next trajectory point is the terminal point, and state information and output information of K trajectory points from the starting point to the terminal point are obtained to generate a smooth flight trajectory.

9. A swarm of drones, characterized in that, The UAV cluster comprises N UAVs, and each of the UAVs is used to perform trajectory planning in a preset flight area according to the trajectory planning method of any one of claims 1-7 to achieve a flight task of the UAV cluster.

Images