Multi-rotor unmanned aerial vehicle cooperative trajectory planning method based on sequence convex optimization

Abstract

The invention discloses a multi-rotor unmanned aerial vehicle cooperative trajectory planning method based on sequence convex optimization, and the method comprises the steps: taking a four-rotor unmanned aerial vehicle cluster to cooperatively intercept an enemy unmanned aerial vehicle cluster as a task background, comprehensively considering the obstacle avoidance constraint of unmanned aerial vehicles, the anti-collision constraint between the unmanned aerial vehicles and other constraints, and obtaining the optimization of the time optimal interception of the unmanned aerial vehicle cluster; and the flight path of each unmanned aerial vehicle in the unmanned aerial vehicle cluster is efficiently obtained through a sequence convex optimization algorithm.

Description

Technical Field

[0001] The present invention belongs to the technical field of aircraft trajectory optimization, and relates to collaborative obstacle avoidance trajectory planning of multi-rotor unmanned aerial vehicles, and in particular to a multi-rotor unmanned aerial vehicle collaborative trajectory planning method based on sequential convex optimization. Background Art

[0002] In recent years, with the advantages of flexibility, compactness and low cost, the application of multi-rotor drones in logistics and transportation, industrial inspection, geographic surveying and mapping, military operations and other occasions has been increasing. Autonomy and clustering are the main directions for the future development of the multi-rotor drone industry, which puts forward high demands on the trajectory planning capabilities of drones. At present, due to the lack of autonomous trajectory planning capabilities, drone flight missions often require human participation, and the human-in-the-loop control mode makes it more difficult to achieve large-scale drone cluster flight. How to achieve autonomous, reliable and fast drone cluster trajectory planning is a problem that drone research institutions and companies around the world are working on.

[0003] For the problem of UAV trajectory planning, the key factors to consider are the dynamics of the UAV, performance constraints, and obstacle constraints of the flight field. If the trajectory planning is for a cluster of UAVs, then on this basis, it is also necessary to consider the collaborative constraints of the cluster, such as anti-collision constraints between each UAV, UAV flight time requirements, and process formation constraints. The existing UAV trajectory (path) planning technologies are relatively effective in planning the path of a single UAV, such as grid-based algorithms, sampling-based algorithms, and artificial intelligence-based algorithms. They can consider simple UAV dynamics and give a feasible path connecting the starting point and the end point based on the distribution of obstacles on the field. However, these algorithms have one or more shortcomings, such as incomplete consideration of UAV dynamics and performance constraints, only being able to plan paths (missing the time information of the trajectory), uncertain optimality of the resulting trajectory (path), time-consuming calculation process, and difficulty in meeting the constraints of cluster trajectory planning.

[0004] Constructing and solving the optimal control problem of multi-rotor UAV collaborative trajectory planning is the key to obtaining the optimal trajectory of UAV swarm collaborative flight. However, due to the considerable nonlinearity of the UAV dynamics, related performance constraints, obstacle constraints, and cluster-related constraints, the constructed optimal control problem is often a multi-constrained, highly nonlinear problem. Existing methods for solving optimal control problems, such as sequential second-order cone programming (SQP), interior point method, and direct linearized sequential convex optimization algorithm, cannot meet the requirements of computational efficiency and solution convergence when solving such problems.

[0005] Based on the above problems, in order to comprehensively consider all the requirements given in the cluster trajectory planning problem, ensure the optimality of the obtained trajectory, and reduce the computational cost, it is of great practical significance to study the collaborative trajectory planning method of multi-rotor UAVs based on sequential convex optimization. Summary of the invention

[0006] In order to overcome the above-mentioned problems, the inventors have conducted intensive research and designed a multi-rotor UAV collaborative trajectory planning method based on sequential convex optimization. In this method, the collaborative interception of an enemy UAV cluster by a cluster of quadrotors is taken as the task background, and comprehensive consideration is given to constraints such as obstacle avoidance constraints of UAVs and anti-collision constraints between UAVs to solve the optimization problem of the optimal time interception of a UAV cluster, and a sequential convex optimization algorithm is used to efficiently solve the flight trajectory of each UAV in the UAV cluster; thereby completing the present invention.

[0007] Specifically, the purpose of the present invention is to provide a multi-rotor UAV collaborative trajectory planning method based on sequential convex optimization, which comprises the following steps:

[0008] Step 1: Set the initial state s [0] , set the initial flight time Setting the convergence criteria Obstacle information on the flight path can be retrieved;

[0009] Step 2: Input the obstacle information retrieved in step 1 into the convex optimization model, and then set the initial state s in step 1. [0] and initial flight time Input into the convex optimization model to obtain the first set of state quantities s 1 、The first set of control variables u 1 and the first set of flight times

[0010] Step 3: The first set of state quantities s 1 and the first set of flight times Input into the convex optimization model to obtain the second set of state quantities s 2 , the second set of control variables u 2 and the second set of flight time

[0011] Step 4: Continue to loop through step 3 and input the obtained state quantity and flight time into the convex optimization model.

[0012] Step 5: Each time step 3 is executed, based on the convergence criterion It is determined whether the obtained state quantity and flight time meet the requirements. If they do, step 4 is stopped and the final state quantity, control quantity and flight time are obtained.

[0013] The beneficial effects of the present invention include:

[0014] (1) According to the multi-rotor UAV collaborative trajectory planning method based on sequential convex optimization provided by the present invention, by converting the optimal control problem into a convex optimization problem and establishing a sequential convex optimization algorithm that guarantees convergence, the collaborative obstacle avoidance planning trajectory of the UAV can be efficiently obtained;

[0015] (2) According to the multi-rotor UAV collaborative trajectory planning method based on sequential convex optimization provided by the present invention, by introducing a resistance term related to the square term of the UAV speed into the UAV dynamics, a more accurate optimal trajectory for UAV collaborative obstacle avoidance can be obtained;

[0016] (3) According to the multi-rotor UAV collaborative trajectory planning method based on sequential convex optimization provided by the present invention, the originally difficult-to-solve nonlinear optimal control problem is converted into a sequential convex optimization problem that is easy to solve through convex methods, including variable substitution, relaxation, linearization, etc., thereby efficiently obtaining an accurate optimized trajectory. BRIEF DESCRIPTION OF THE DRAWINGS

[0017] Figure 1 The overall logic diagram of the multi-rotor UAV collaborative trajectory planning method based on sequential convex optimization of the present application is shown;

[0018] Figure 2 A schematic diagram showing the flight trajectory of our UAV and the flight trajectory of the enemy UAV in the embodiment is shown;

[0019] Figure 3 A flight speed variation curve diagram of each UAV in the embodiment is shown;

[0020] Figure 4 A curve diagram showing the thrust change of each drone in the embodiment is shown;

[0021] Figure 5 A thrust deflection angle variation curve diagram of each UAV in the embodiment is shown;

[0022] Figure 6 A trend line diagram of the minimum distance between our drones in the embodiment is shown;

[0023] Figure 7 The difference change of the speed relaxation amount in the x-axis direction during the flight of each UAV in the embodiment is shown;

[0024] Figure 8 The difference change of the speed relaxation amount in the y-axis direction during the flight of each UAV in the embodiment is shown. DETAILED DESCRIPTION

[0025] The present invention will be further described in detail below through the accompanying drawings and embodiments. Through these descriptions, the characteristics and advantages of the present invention will become more clear and distinct.

[0026] The word "exemplary" is used exclusively herein to mean "serving as an example, embodiment, or illustration." Any embodiment described herein as "exemplary" is not necessarily to be construed as preferred or advantageous over other embodiments. Although various aspects of the embodiments are shown in the drawings, the drawings are not necessarily drawn to scale unless otherwise noted.

[0027] When intercepting an enemy drone cluster by using a drone cluster composed of multiple drones, if each drone in the enemy drone cluster is deemed to be moving in a uniform straight line, before the drone takes off, it is necessary to plan the trajectory for each drone in the cluster accordingly, so that each drone can obtain its own planned trajectory. Based on this, the present invention provides a multi-rotor drone collaborative trajectory planning method based on sequential convex optimization, such as Figure 1 As shown in , the method comprises the following steps:

[0028] Step 1: Set the initial state s [0] , set the initial flight time Setting the convergence criteria The initial setting values ​​are selected and set based on actual conditions such as target distance, drone performance, flight environment, etc. Preferably, the initial state quantity includes element information of multiple drones at multiple times, each element information is a vector set composed of 6 data, the 6 data are the speed of the drone in three directions and the position coordinates in three directions, and the initial value can be set to zero, for example, one of the element information is [0,0,0,0,0,0]. The specific setting value of the initial flight time is 120, and the convergence criterion The value of is [1,1,0.5]; the information of obstacles on the flight path is also retrieved according to the actual situation. The information of the obstacle includes the center coordinates and the major and minor axis sizes of the obstacle. In this application, the obstacle size is enlarged and virtually formed into a cylinder or an elliptical cylinder.

[0029] Step 2: Input the obstacle information retrieved in step 1 into the convex optimization model, and then set the initial state s in step 1. [0] and initial flight time Input into the convex optimization model to obtain the first set of state quantities s 1 、The first set of control variables u 1 and the first set of flight times

[0030] Step 3: The first set of state quantities s 1 and the first set of flight times Input into the convex optimization model to obtain the second set of state quantities s 2 , the second set of control variables u 2 and the second set of flight time

[0031] Step 4: Continue to loop through step 3 and input the obtained state quantity and flight time into the convex optimization model.

[0032] Step 5: Each time step 3 is executed, based on the convergence criterion It is determined whether the obtained state quantity and flight time meet the requirements. If they do, step 4 is stopped and the final state quantity, control quantity and flight time are obtained.

[0033] In a preferred embodiment, in step 5, when formula (1) is established, it is considered that the requirement is met, and step 4 is stopped:

[0034]

[0035] Where k represents the number of times step 3 is executed.

[0036] s [k+1] represents the state quantity obtained when step 3 is executed for the kth time,

[0037] s [k] represents the state quantity obtained when step 3 is executed for the k-1th time, that is, the state quantity input when step 3 is executed for the kth time.

[0038] t f [k+1] represents the flight time obtained when step 3 is executed for the kth time,

[0039] t f [k] It represents the flight time obtained when step 3 is executed for the k-1th time, that is, the flight time entered when step 3 is executed for the kth time.

[0040] Preferably, in step 5, the state quantity s obtained when step 3 is executed for the kth time is [k+1] and flight time t f [k +1] When formula (1) is established, the state quantity s obtained when step 3 is executed for the kth time is [k+1] , control quantity u [k+1] and flight time t f [k +1] This is the planned trajectory of the UAV; the state quantity s obtained when executing step 3 for the kth time is [k+1] , control quantity u [k+1] and flight time t f[k+1] The motion state of the drone at each moment is determined, thus forming the trajectory of the drone's motion;

[0041] Preferably, the state quantity s [k+1] Including the state quantity of each drone in the drone cluster, the state quantity in this application is a vector set composed of the speed and position of the drone, and each state quantity contains multiple groups of speed and position information arranged in chronological order; the position is the position coordinate, and the speed includes the speed magnitude and speed direction; the state quantity in this application includes the x-direction speed, y-direction speed, z-direction speed, x-direction coordinate, y-direction coordinate, and z-direction coordinate of each drone at each moment;

[0042] The control quantity u [k+1] This includes the control amount of each drone in the drone cluster, specifically the thrust size and thrust direction of each drone at each moment;

[0043] The flight time t f [k+1] The flight time of each drone in the drone cluster, that is, the time required for each drone to intercept the target, can be made to reach the target point at the same time through the planning method provided by this application. Therefore, the flight time t f [k+1] It is also the time it takes for all drones to reach the target point at the same time.

[0044] In a preferred embodiment, the convex optimization model in the present application is selected from the convex optimization model in Domahidi A, Chu E, Boyd S. ECOS: An SOCP solver for embedded systems [C] / / 2013 European control conference (ECC). IEEE, 2013: 3071-3076. After selecting the convex optimization model, it is necessary to set the trajectory standard, that is, to obtain the minimum value of the standard value J of the drone trajectory, and the standard value J of the drone trajectory is obtained by the following formula (II):

[0045] J=t f (two)

[0046] t f Indicates flight time.

[0047] After selecting the convex optimization model, the following constraints need to be set in the convex optimization model to meet the functional requirements of this application:

[0048] The constraints include the following formulas (III) to (VII):

[0049]

[0050] Among them, T x,q represents the thrust component of the UAV numbered q in the lateral direction,

[0051] T y,q represents the component of the thrust of the UAV numbered q in the longitudinal direction,

[0052] T z,q It represents the vertical component of the thrust of the UAV numbered q.

[0053] T max Indicates the maximum thrust of the drone T max , preferably, T max Take 60N;

[0054]

[0055] Among them, φ max Indicates the maximum thrust deflection angle of the drone, preferably 25°;

[0056]

[0057] Among them, v x,q represents the lateral speed of the UAV numbered q,

[0058] v y,q represents the longitudinal velocity of the UAV numbered q,

[0059] v z,q It represents the vertical speed of the UAV numbered q.

[0060] v max Indicates the maximum flight speed of the drone; preferably 30m / s;

[0061]

[0062] Where r represents the obstacle number,

[0063] m represents the number of obstacles;

[0064] x ob,r represents the horizontal coordinate of the obstacle numbered r,

[0065] y ob,r Represents the ordinate of the obstacle numbered r,

[0066] a ob,r represents the semi-major axis size of the obstacle constraint,

[0067] b ob,rIndicates the size of the minor semi-axis of the obstacle constraint; when the obstacle is cylindrical, the sizes of the major and minor axes are equal;

[0068]

[0069] Among them, q and p represent the number of the drone independently. In order to ensure that each drone does not collide with other drones, it is necessary to impose anti-collision constraints between two drones.

[0070] n represents the total number of drones,

[0071] x q represents the horizontal coordinate of the drone numbered q,

[0072] x p represents the horizontal coordinate of the drone numbered p,

[0073] y q represents the vertical coordinate of the drone numbered q,

[0074] y p represents the vertical coordinate of the drone numbered p,

[0075] r safe Indicates the safe distance of the drone in the horizontal direction, preferably 15m.

[0076] Example

[0077] Set up a mission scenario where a swarm of five UAVs intercepts three enemy UAVs. The initial positions of the swarm and the enemy UAVs are known, as well as the speed, magnitude, and direction of the enemy UAVs. The obstacles in this scenario are considered to be cylindrical in shape. The specific information is as follows:

[0078]

[0079] The initial state of our drone is as follows:

[0080]

[0081] The initial state of the enemy drone is as follows:

[0082]

[0083] The interception strategy is set as follows: our drones 1 and 4 intercept enemy drone 1, our drones 2 and 5 intercept enemy drone 2, and our drone 3 intercepts enemy drone 3; based on this, when setting the terminal speed of our drones, the terminal speed of our drones 1 and 4 is the same as the speed of enemy drone 1 in magnitude and direction, the terminal speed of our drones 2 and 5 is the same as the speed of enemy drone 2 in magnitude and direction, and the terminal speed of our drone 3 is the same as the speed of enemy drone 3 in magnitude and direction.

[0084] Before the interception mission begins, the convex optimization model in Domahidi A, Chu E, Boyd S. ECOS: An SOCP solver for embedded systems [C] / / 2013 European control conference (ECC). IEEE, 2013: 3071-3076 is retrieved. The trajectory standard in the convex optimization model is to minimize the standard value J of the drone trajectory. The standard value J of the drone trajectory is obtained by the following formula (II):

[0085] J=t f (two)

[0086] t f Indicates the flight time,

[0087] The constraints set in this convex optimization model are as follows:

[0088]

[0089] Among them, T x,q represents the thrust component of the UAV numbered q in the lateral direction,

[0090] T y,q represents the component of the thrust of the UAV numbered q in the longitudinal direction,

[0091] T z,q It represents the vertical component of the thrust of the UAV numbered q.

[0092] T max Indicates the maximum thrust of the drone T max , take 60N;

[0093]

[0094] Among them, φ max Indicates the maximum thrust deflection angle of the drone, which is 25°;

[0095]

[0096] Among them, vx,q represents the lateral speed of the UAV numbered q,

[0097] v y,q represents the longitudinal velocity of the UAV numbered q,

[0098] v z,q It represents the vertical speed of the UAV numbered q.

[0099] v max Indicates the maximum flight speed of the drone, which is 30m / s;

[0100]

[0101] Where r represents the obstacle number,

[0102] m represents the number of obstacles;

[0103] x ob,r Represents the horizontal coordinate of the obstacle numbered r

[0104] y ob,r represents the ordinate of the obstacle numbered r,

[0105] a ob,r represents the semi-major axis size of the obstacle constraint,

[0106] b ob,r Represents the minor semi-axis size of the obstacle constraint;

[0107]

[0108] Among them, q and p each independently represent the number of the drone;

[0109] n represents the total number of drones, i.e. 5,

[0110] x q represents the horizontal coordinate of the drone numbered q,

[0111] x p represents the horizontal coordinate of the drone numbered p,

[0112] y q represents the vertical coordinate of the drone numbered q,

[0113] y p represents the vertical coordinate of the drone numbered p,

[0114] r safe Indicates the horizontal safety distance of the drone, and the value is 15m.

[0115] When performing the interception mission, the planned trajectories for the five drones are obtained through the following steps:

[0116] Step 1: Set the initial state s [0] , set the initial flight time Set the convergence criterion to 120 The value of is [1,1,0.5]; all elements in this initial state are 0;

[0117] Step 2: Input the obstacle information into the convex optimization model, and then use the initial state s set in step 1 [0] and initial flight time Input into the convex optimization model to obtain the first set of state quantities s 1 、The first set of control variables u 1 and the first set of flight times

[0118]

[0119] Step 3: The first set of state quantities s 1 and the first set of flight times Input into the convex optimization model to obtain the second set of state quantities s 2 , the second set of control variables u 2 and the second set of flight time

[0120] Step 4: Continue to loop through step 3 and input the obtained state quantity and flight time into the convex optimization model.

[0121] Step 5: Each time step 3 is executed, based on the convergence criterion It is determined whether the obtained state quantity and flight time meet the requirements. If they do, step 4 is stopped and the final state quantity, control quantity and flight time are obtained.

[0122] Among them, when formula (1) is established, it is considered that the requirements are met and step 4 is stopped:

[0123]

[0124] Where k represents the number of times step 3 is executed.

[0125] s [k+1] represents the state quantity obtained when step 3 is executed for the kth time,

[0126] s [k] represents the state quantity obtained when step 3 is executed for the k-1th time, that is, the state quantity input when step 3 is executed for the kth time.

[0127] t f [k+1] represents the flight time obtained when step 3 is executed for the kth time,

[0128] t f [k] It represents the flight time obtained when step 3 is executed for the k-1th time, that is, the flight time entered when step 3 is executed for the kth time.

[0129] When k is finally set to 20, equation (1) holds true, and the state quantity s is obtained. [k+1] , the state quantity s [k+1] The last set of position and velocity information of one of the drones is [1735.8, 1730.6, 1804.5, 1.8, 1.9, 25.9], and the control value u [k+1] = [0,0,-0.3], the obtained flight time t f [k+1] 113.59s;

[0130] Execute steps 1 to 1 to obtain the state quantity s [k+1] , control quantity u [k+1] , flight time t f [k+1] The total time is 1.95s.

[0131] The flight trajectory of our drone obtained from steps 1 to 5 above and the flight trajectory of the enemy drone are as follows: Figure 2 As shown in , at the trajectory terminal, the trajectory of our drone can overlap with the trajectory of the target drone, which can prove that our drone can complete the interception mission;

[0132] In the above steps 1 to 5, the flight speed of each drone in our drone cluster is obtained as follows: Figure 3 As shown in , the thrust is Figure 4 As shown in , the thrust deflection angle is Figure 5 As shown in ; the minimum distance change trend between drones in our drone cluster is obtained as follows Figure 6 As shown in; Comprehensive Figures 2 to 6 It can be seen that the five drones in the cluster can meet the obstacle avoidance and collision avoidance requirements in the early stage of flight.

[0133] In order to further confirm the correctness of the relaxation of the dynamic resistance term by the multi-rotor UAV collaborative trajectory planning method based on sequential convex optimization, we draw and The value at each discrete point, where and represents the projection of the drone's velocity on the x-axis and y-axis before relaxation, and Represents the projection of the drone's velocity on the x-axis and y-axis after relaxation.

[0134] The result is as follows Figure 7 and Figure 8 As shown, it can be seen, and The values ​​are all between 10 -10 The following can be regarded as the numerical error of the computer, which proves that the relaxed constraints are almost everywhere active and the relaxed optimization problem is equivalent to the original optimization problem.

[0135] The present invention has been described above in conjunction with preferred embodiments, but these embodiments are only exemplary and serve only as an illustration. On this basis, the present invention may be subjected to a variety of substitutions and improvements, all of which fall within the scope of protection of the present invention.

Claims

1. A multi-rotor UAV collaborative trajectory planning method based on sequential convex optimization, characterized in that: The method comprises the following steps: Step 1: Set the initial state s [0] , set the initial flight time Setting the convergence criteria Obstacle information on the flight path can be retrieved; Step 2: Input the obstacle information retrieved in step 1 into the convex optimization model, and then set the initial state s [0] and initial flight time Input into the convex optimization model to obtain the first set of state quantities s 1 、The first set of control variables u 1 and the first set of flight times Step 3: The first set of state quantities s 1 and the first set of flight times Input into the convex optimization model to obtain the second set of state quantities s 2 , the second set of control variables u 2 and the second set of flight time Step 4, continuously looping through step 3, inputting the obtained state quantity and flight time into the convex optimization model; Step 5: Each time step 3 is executed, based on the convergence criterion It is determined whether the obtained state quantity and flight time meet the requirements. If they do, step 4 is stopped and the final state quantity, control quantity and flight time are obtained.

2. The method for collaborative trajectory planning of multi-rotor UAVs based on sequential convex optimization according to claim 1 is characterized in that: In step 5, when formula (1) is established, it is considered that the requirement is met, and step 4 is stopped: Where k represents the number of times step 3 is executed. s [k+1] represents the state quantity obtained when step 3 is executed for the kth time, s [k] represents the state quantity obtained when step 3 is executed for the k-1th time, that is, the state quantity input when step 3 is executed for the kth time. t f [k+1] represents the flight time obtained when step 3 is executed for the kth time, t f [k] It represents the flight time obtained when step 3 is executed for the k-1th time, that is, the flight time entered when step 3 is executed for the kth time.

3. The method for collaborative trajectory planning of multi-rotor UAVs based on sequential convex optimization according to claim 2 is characterized in that: In step 5, the state quantity s obtained when step 3 is executed for the kth time is [k+1] and flight time t f [k+1] When formula (1) is established, the state quantity s obtained when step 3 is executed for the kth time is [k+1] , control quantity u [k+1] and flight time t f [k+1] , forming the trajectory of the UAV’s motion; Preferably, the state quantity s [k+1] Including the state of each drone in the drone cluster, The control quantity u [k+1] Including the control volume of each drone in the drone cluster, The flight time t f [k+1] It includes the flight time of each drone in the drone cluster, that is, the time required for each drone to intercept the target.

4. The method for collaborative trajectory planning of multi-rotor UAVs based on sequential convex optimization according to claim 1 is characterized in that: The standard of the trajectory in the convex optimization model is to make the standard value J of the drone trajectory take the minimum value, and the standard value J of the drone trajectory is obtained by the following formula (II): J=t f (two) t f Indicates flight time.

5. The method for collaborative trajectory planning of multi-rotor UAVs based on sequential convex optimization according to claim 1 is characterized in that: Constraints are set in the convex optimization model, and the constraints include the following equations (III) to (VII): Among them, T x,q represents the thrust component of the UAV numbered q in the lateral direction, T y,q represents the component of the thrust of the UAV numbered q in the longitudinal direction, T z,q It represents the vertical component of the thrust of the UAV numbered q. T max Indicates the maximum thrust of the drone T max ; Among them, φ max Indicates the maximum thrust deflection angle of the drone; Among them, v x,q represents the lateral velocity of the UAV numbered q, v y,q represents the longitudinal velocity of the UAV numbered q, v z,q It represents the vertical speed of the UAV numbered q. v max Indicates the maximum flight speed of the drone; Among them, r represents the obstacle number, m represents the number of obstacles; x ob,r The horizontal coordinate y of the obstacle numbered r ob,r Represents the ordinate of the obstacle numbered r, a ob,r represents the semi-major axis size of the obstacle constraint, b ob,r Represents the minor semi-axis size of the obstacle constraint; Among them, q and p represent the number of the drone independently. n represents the total number of drones, x q represents the horizontal coordinate of the drone numbered q, x p represents the horizontal coordinate of the drone numbered p, y q represents the vertical coordinate of the drone numbered q, y p represents the vertical coordinate of the drone numbered p, r safe Indicates the safe distance of the drone in the horizontal direction.

Images