Unmanned aerial vehicle real-time trajectory planning method based on support hyperplane

Abstract

The invention discloses an unmanned aerial vehicle real-time trajectory planning method based on a support hyperplane, and belongs to the technical field of unmanned aerial vehicle flight control. The implementation method comprises the following steps: comprehensively considering the dynamics and necessary constraint conditions of the unmanned aerial vehicle, and establishing a trajectory planning problem by taking the minimization of flight time as an optimization target; a problem is equivalently converted into a trajectory planning problem containing a non-convex equality constraint through a proper method, and a relaxation problem of an original problem is obtained by relaxing the non-convex equality constraint into a convex inequality constraint. The method comprises the following steps of: noticing that a relaxation problem under an original objective function is not certainly equivalent to an original problem, proposing to replace the original objective function with a parameterized objective function based on a support hyperplane theory, designing a parameter updating strategy, and iteratively solving the parameterized relaxation problem to gradually approach a solution of the original problem. For non-convex obstacle avoidance constraints in the parameterized relaxation problem, the parameterized relaxation problem is convexed by adopting a sequence linearization method, and an iterative algorithm sequence is designed to solve a series of convex optimization problems to obtain a solution of the parameterized relaxation problem. According to the contents, a double-circulation trajectory planning algorithm for planning the minimum-time obstacle avoidance flight trajectory of the unmanned aerial vehicle in real time is provided. The method disclosed by the invention is good in convergence and high in calculation efficiency, can reliably and quickly calculate the optimal obstacle avoidance flight trajectory meeting complex constraint conditions, and has the capability of realizing real-time online trajectory planning.

Description

Technical Field

[0001] The present invention belongs to the technical field of unmanned aerial vehicle (UAV) flight control, and relates to a real-time trajectory planning method for an UAV, and in particular to a real-time trajectory planning method for an UAV based on a supporting hyperplane. Background Art

[0002] As a new type of unmanned combat force, drones are widely used in various missions, including intelligence gathering, communications relay, fire strikes, and combat assessment, and are gradually becoming an indispensable component of information-based combat systems. Real-time trajectory planning is a key technology for autonomous drone flight. The goal is to plan a flight trajectory with optimal performance indicators from a given initial state to a desired terminal state in real time, while meeting the necessary constraints imposed by the mission environment and flight performance limitations.

[0003] Over the past few decades, numerous methods have been applied to solving the trajectory planning problem for UAVs in complex obstacle environments. Typical path planning methods include graph search, artificial potential field methods, and swarm intelligence. These methods, each with its own unique characteristics, have played a significant role in solving UAV path planning problems. However, they rarely accurately consider the UAV's dynamics and constraints, resulting in the generated paths not always being trackable by the UAV. In recent years, numerical optimization methods have attracted widespread attention due to their excellent performance in handling nonlinear dynamics and complex constraints. A typical approach is to directly solve the problem using nonlinear optimization methods. However, this approach often requires long computational time and cannot guarantee convergence. An alternative approach is convex optimization. Specifically, the nonlinear programming problem is first transformed into a convex optimization problem using appropriate convexification methods. Then, an algorithm is designed to solve one or a series of convex optimization problems to obtain a solution to the original problem. Because convex optimization problems can be quickly solved using interior point algorithms, these methods can effectively improve the efficiency of trajectory planning. Nevertheless, how to transform trajectory planning problems into convex optimization problems and design trajectory planning algorithms with good convergence and high computational efficiency remain issues that deserve attention and research. Summary of the Invention

[0004] The technical problem addressed by the supporting hyperplane-based real-time trajectory planning method for unmanned aerial vehicles (UAVs) disclosed in this paper is to achieve real-time trajectory planning for UAVs while comprehensively considering the UAV's dynamics and necessary constraints. This method effectively improves the convergence and computational efficiency of the trajectory planning algorithm, enabling real-time online trajectory planning.

[0005] The purpose of the present invention is achieved through the following technical solutions.

[0006] The real-time trajectory planning method for a UAV based on a supporting hyperplane disclosed in the present invention is described as follows: Taking into account the UAV dynamics and necessary constraints, a trajectory planning problem is established with the optimization objective of minimizing flight time. The problem is equivalently transformed into a trajectory planning problem with non-convex equality constraints through appropriate methods, and the relaxation of the original problem is obtained by relaxing the non-convex equality constraints into convex inequality constraints. Noting that the relaxation problem and the original problem under the original objective function are not necessarily equivalent, based on the supporting hyperplane theory, it is proposed to replace the original objective function with a parameterized objective function, and a parameter update strategy is designed to iteratively solve the parameterized relaxation problem to gradually approximate the solution to the original problem. For the non-convex obstacle avoidance constraints in the parameterized relaxation problem, a sequential linearization method is used to convexify them, and an iterative algorithm is designed to sequentially solve a series of convex optimization problems to obtain the solution to the parameterized relaxation problem. In summary, a double-loop trajectory planning algorithm for real-time planning of the minimum time obstacle avoidance flight trajectory of a UAV is given.

[0007] The method for real-time trajectory planning of a UAV based on a supporting hyperplane disclosed in the present invention comprises the following steps:

[0008] Step 1: Considering the UAV dynamics and necessary constraints, establish a trajectory planning problem with the goal of minimizing flight time.

[0009] The implementation method of step one is:

[0010] The UAV dynamics is expressed as

[0011]

[0012] Where r = [x, y, z] T is the position coordinate of the UAV, v=[v x ,v y ,v z ] T is the speed of the drone, g=[0,0,-g0] T is the acceleration due to gravity, a=[a x ,a y ,a z ] T is the acceleration of the drone, defined as follows:

[0013]

[0014] Where, T=[T x ,T y ,T z ] T is the thrust of the UAV and m is its mass.

[0015] The following are the necessary constraints that need to be considered in the real-time trajectory planning problem of UAVs:

[0016] (1) Initial and terminal state constraints: The initial and terminal state constraints of the UAV can be expressed as

[0017]

[0018] Where t0 and t f are the initial and terminal moments, r0 and r f are the initial and final positions, v0 and v f are the initial and terminal velocities, respectively.

[0019] (2) Speed ​​constraints: To ensure flight safety, the following speed constraints are introduced to limit flight speed:

[0020] ‖v‖2≤v max (4)

[0021] Where, v max is the maximum speed.

[0022] (3) Control constraints: The acceleration of the UAV is generated by thrust. Since thrust has a maximum value and a safety angle limit, the acceleration should satisfy the maximum acceleration constraint and the acceleration direction constraint:

[0023] ‖a‖2≤a max (5)

[0024]

[0025] Where a max =T max / m is the maximum thrust T max The associated maximum acceleration, is the maximum angle between the acceleration vector and the plumb bob direction.

[0026] (4) Obstacle avoidance constraints: UAVs often face complex obstacle environments during mission execution. Considering the presence of several elliptical obstacle areas in the mission environment, the obstacle avoidance constraints are as follows:

[0027]

[0028] Where i=1,…,m is the number of the obstacle area, (x o,i ,y o,i ) is the coordinate of the center point of the i-th obstacle area, a o,i and b o,i are the semi-major axis and semi-minor axis of the i-th obstacle area respectively.

[0029] In order to improve the efficiency of mission completion, the optimization goal is to minimize the flight time, so the objective function is set to

[0030] J=t f (8)

[0031] In summary, the trajectory planning problem is established

[0032]

[0033] subject to(1),(3)-(7)

[0034] Trajectory planning problem In the example, the objective function is a linear function and the constraints are convex constraints, but because the flight time t f It is unknown, and the dynamics discretization is actually a nonlinear equality constraint, so it is still a nonlinear programming problem.

[0035] Step 2: Select appropriate free variables to solve the problem Equivalent transformation into problem P ε .

[0036] The implementation method of step 2 is:

[0037] First, define a new free variable to map time to the fixed interval [0,1]:

[0038]

[0039] Taking t0 = 0, according to formula (9), we can get Then the dynamics of equation (1) can be rewritten as

[0040]

[0041] Where (·)′ represents the differentiation of τ.

[0042] Considering that the differential equation of r in formula (10) is nonlinear, a new state variable is defined:

[0043]

[0044] According to Equation (11), the dynamics of Equation (10) can be rewritten as

[0045]

[0046] In formula (12) The differential equation is still nonlinear. In order to deal with nonlinear terms Introducing constraints:

[0047]

[0048] At the same time, define new state variables:

[0049]

[0050] Using Equations (13) and (14), the dynamics of Equation (12) is equivalently transformed into linear dynamics:

[0051]

[0052] Because variable substitution is used in the transformation process, the constraints of equations (3)-(6) need to be transformed into constraints on new variables:

[0053]

[0054] Ultimately, the problem Is equivalently transformed into problem Px:

[0055] P ε :minimum J=t f

[0056] subject to(7),(13),(15)-(19)

[0057] Step 3: Relax the non-convex constraints of (13) and replace the original objective function with a parameterized objective function based on the support hyperplane theory to obtain the parameterized relaxation problem

[0058] The implementation method of step three is:

[0059] Question P ε Part of the non-convexity of comes from the non-convex equality constraint of (13), but it can be relaxed to a convex inequality constraint:

[0060]

[0061] Substituting equation (20) for equation (13), we get the problem P ε The relaxation problem

[0062]

[0063] subject to(7),(15)-(20)

[0064] Note that relaxing Equation (13) to Equation (20) actually expands the feasible domain of the problem, and the problem is solved if and only if Equation (20) is active, that is, when the exact relaxation is achieved. and Problem P εNote that the validity of the relaxation cannot be guaranteed under the original objective function, so the objective function is modified to obtain the following parameterized relaxation problem

[0065]

[0066] subject to(7),(15)-(20)

[0067] like Figure 1 As shown, when When Defines a vector The associated supporting hyperplane supports the feasible set According to the support hyperplane theory, the support point It's a problem The optimal solution of That is the problem It is a question of P ε Precise relaxation.

[0068] Step 4: Design a parameter update strategy and iteratively solve the parameterized relaxation problem Come get the problem The solution.

[0069] The implementation method of step four is:

[0070] question It is a question of P ε The exact relaxation of The objective function and Related, and In solving the problem In view of this, the iterative algorithm is designed to start from the initial value σ [0] = 0, iteratively update the support hyperplane parameter σ [k] , until convergence. Specifically, let σ [k] is the value of parameter σ in the kth iteration, and there are the following problems

[0071]

[0072] subject to(7),(15)-(20)

[0073] Then, in each iteration, according to the problem The solution to update the parameter σ [k] ,Right now

[0074]

[0075] To summarize, the design iteration algorithm is as follows:

[0076] Step 4.1: Set k = 0 and give the initial value of parameter σ [0] =0;

[0077] Step 4.2: At the k+1th iteration, solve the problem get and corresponding flight trajectories;

[0078] Step 4.3: Check convergence conditions:

[0079]

[0080] Where,∈ t is the given convergence criterion. If equation (22) satisfies, execute step 4.4, otherwise let k = k + 1 and update the parameter σ according to equation (21) [k] , then proceed to step 4.2;

[0081] Step 4.4: The iterative algorithm terminates and the trajectory planning result is output.

[0082] like Figure 2 As shown, as the iteration proceeds, σ [k] Approaching question The solution to the problem is close to The solution.

[0083] Step 5: Convexify the non-convex obstacle avoidance constraint of formula (7) and design an iterative algorithm to obtain the problem The solution.

[0084] The implementation method of step five is:

[0085] question It is still a non-convex optimization problem, and its only non-convexity comes from the non-convex obstacle avoidance constraint of formula (7). Let x [j] and y [j] is the solution of the jth iteration, and the non-convex obstacle avoidance constraint of formula (7) is convexed by the sequential linearization method to obtain

[0086] A0+A1x+A2y≥0 (23) where

[0087]

[0088] Then we get the convex optimization problem

[0089]

[0090] subjectto(15)-(20),(23)

[0091] Since Equation (7) is convexed using the sequential linearization method, an iterative algorithm is designed to solve the problem sequentially. Come get the problem The algorithm flow is as follows:

[0092] Step 5.1: Set j = 0 and give the initial reference trajectory x [0] and y [0] ;

[0093] Step 5.2: At the j+1th iteration, solve the problem Get x [j+1] and y [j+1] ;

[0094] Step 5.3: Check convergence conditions:

[0095] max|x [j+1] -x [j] |≤∈ x ,max|y [j+1] -y [j] |≤∈ y (twenty four)

[0096] Where,∈ x and ∈ y Is a given convergence criterion. If equation (24) is satisfied, execute step 5.4, otherwise let j = j + 1 and update the reference trajectory x [j] and y [j] , then proceed to step 5.2;

[0097] Step 5.4: The iterative algorithm terminates and the trajectory planning result is output.

[0098] Step 6: Design a double-loop trajectory planning algorithm for real-time planning of the UAV's minimum time obstacle avoidance flight trajectory, which is applied to solve the UAV's real-time trajectory planning problem.

[0099] The implementation method of step six is:

[0100] like Figure 3 As shown, the solution to the problem described in step 5 is The iterative algorithm (inner loop) is embedded into the solution problem described in step 4 In the iterative algorithm (outer loop), the UAV trajectory planning algorithm is obtained as follows:

[0101] Step 6.1: Set k = 0 and give the initial value of parameter σ [0] =0;

[0102] Step 6.2: At the k+1th iteration, set j = 0 and give the initial reference trajectory x [0] and y [0] ;

[0103] Step 6.3: At the j+1th iteration, solve the problem Get x [j+1] and y [j+1] ;

[0104] Step 6.4: Check convergence conditions:

[0105] max|x [j+1] -x [j] |≤∈ x ,max|y [j+1] -y [j] |≤∈ y (25)

[0106] Where,∈ x and ∈ y Is a given convergence criterion. If equation (25) is satisfied, execute step 6.5, otherwise let j = j + 1 and update the reference trajectory x [j] and y [j] , then proceed to step 6.3;

[0107] Step 6.5: The iterative algorithm terminates and outputs and corresponding flight trajectories;

[0108] Step 6.6: Check convergence conditions:

[0109]

[0110] Where,∈ t is the given convergence criterion. If equation (26) satisfies, execute step 6.7, otherwise let k = k + 1 and update the parameter σ according to equation (21) [k] , then proceed to step 6.2;

[0111] Step 6.7: The iterative algorithm terminates and the trajectory planning result is output.

[0112] Beneficial effects

[0113] 1. The real-time trajectory planning method for a UAV based on a supporting hyperplane disclosed in the present invention can accurately consider the UAV dynamics, speed constraints, control constraints, and obstacle avoidance constraints, and the resulting flight trajectory can be stably tracked by the UAV.

[0114] 2. The real-time trajectory planning method for UAVs based on supported hyperplanes disclosed in the present invention transforms the nonlinear programming problem into a convex optimization problem based on the supported hyperplane theory, thereby designing a double-loop trajectory planning algorithm with good convergence and high computational efficiency. It can reliably and quickly calculate the optimal obstacle avoidance flight trajectory that meets complex constraints, and has the ability to realize real-time online trajectory planning. BRIEF DESCRIPTION OF THE DRAWINGS

[0115] Figure 1 Schematic diagram of the supporting hyperplane in step three of the present invention;

[0116] Figure 2 Schematic diagram of iteratively updating the supporting hyperplane in step 4 of the present invention;

[0117] Figure 3 This is a flowchart of the algorithm for the double-loop trajectory planning algorithm in step six of the present invention;

[0118] Figure 4 It is a flight trajectory diagram in an example of the present invention;

[0119] Figure 5 It is a diagram showing changes in state variables and control variables in an example of the present invention. DETAILED DESCRIPTION

[0120] In order to better illustrate the purpose and advantages of the present invention, the invention is further described below with reference to the accompanying drawings and examples.

[0121] This section uses a UAV flying through a multi-obstacle environment as an example to verify the UAV real-time trajectory planning method based on supporting hyperplane disclosed in the present invention.

[0122] The detailed steps are as follows:

[0123] Step 1: Considering the UAV dynamics and necessary constraints, establish a trajectory planning problem with the goal of minimizing flight time.

[0124] The UAV dynamics is expressed as

[0125]

[0126] Where r = [x, y, z] T is the position coordinate of the UAV, v=[v x ,v y ,v z ] T is the speed of the drone, g=[0,0,-g0] T is the acceleration due to gravity, g0 = 9.81 m / s 2 ,a=[a x ,a y ,a z ] T is the acceleration of the drone, defined as follows:

[0127]

[0128] Where, T=[T x ,T y ,T z ]T is the thrust of the UAV and m is its mass.

[0129] The following are the necessary constraints that need to be considered in the real-time trajectory planning problem of UAVs:

[0130] (1) Initial and terminal state constraints: The initial and terminal state constraints of the UAV can be expressed as

[0131]

[0132] Where t0 and t f are the initial and terminal moments, r0 and r f are the initial and final positions, v0 and v f are the initial and terminal velocities, respectively. The initial and terminal states of the UAV for a given crossing mission are shown in Table 1.

[0133] Table 1 UAV initial state and terminal state

[0134]

[0135] (2) Speed ​​constraints: To ensure flight safety, the following speed constraints are introduced to limit flight speed:

[0136] ‖v‖2≤v max (30) where v max =10m / s is the maximum speed.

[0137] (3) Control constraints: The acceleration of the UAV is generated by thrust. Since thrust has a maximum value and a safety angle limit, the acceleration should satisfy the maximum acceleration constraint and the acceleration direction constraint:

[0138] ‖a‖2≤a max (31)

[0139]

[0140] Where a max =15m / s is the maximum acceleration, is the maximum angle between the acceleration vector and the plumb bob direction.

[0141] (4) Obstacle avoidance constraints: UAVs often face complex obstacle environments during mission execution. Considering the presence of several elliptical obstacle areas in the mission environment, the obstacle avoidance constraints are as follows:

[0142]

[0143] Where i=1,…,m is the number of the obstacle area, (x o,i ,y o,i) is the coordinate of the center point of the i-th obstacle area, a o,i and b o,i are the semi-major axis and semi-minor axis of the ith obstacle region, respectively. In the given crossing mission case, there are three obstacle regions in the considered space, and their parameters are shown in Table 2.

[0144] Table 2 Obstacle area parameters

[0145]

[0146] In order to improve the efficiency of mission completion, the optimization goal is to minimize the flight time, so the objective function is set to

[0147] J=t f (34)

[0148] In summary, the trajectory planning problem is established

[0149]

[0150] subject to(27),(29)-(33)

[0151] Step 2: Select appropriate free variables to solve the problem Equivalent transformation into problem P ε .

[0152] First, define a new free variable to map time to the fixed interval [0,1]:

[0153]

[0154] Taking t0 = 0, according to formula (35), we can get Then the dynamics of equation (1) can be rewritten as

[0155]

[0156] Where (·)′ represents the differentiation of τ.

[0157] Considering that the differential equation of r in Equation (36) is nonlinear, a new state variable is defined:

[0158]

[0159] According to Equation (37), the dynamics of Equation (36) can be rewritten as

[0160]

[0161] In formula (38) The differential equation is still nonlinear. In order to deal with nonlinear terms Introducing constraints:

[0162]

[0163] At the same time, define new state variables:

[0164]

[0165] Using Equations (39) and (40), the dynamics of Equation (38) is equivalently transformed into linear dynamics:

[0166]

[0167] Because variable substitution is used in the transformation process, the constraints of equations (29)-(32) need to be transformed into constraints on new variables:

[0168]

[0169] Ultimately, the problem is equivalently transformed into problem P ε :

[0170] P ε :minimum J=t f

[0171] subject to(33),(39),(41)-(45)

[0172] Step 3: Relax the non-convex constraints of (39) and replace the original objective function with a parameterized objective function based on the support hyperplane theory to obtain the parameterized relaxation problem

[0173] Question P ε Part of the non-convexity of comes from the non-convex equality constraint of (39), but it can be relaxed to a convex inequality constraint:

[0174]

[0175] Substituting equation (46) for equation (39), we get the problem P ε The relaxation problem

[0176]

[0177] subject to(33),(41)-(46)

[0178] Note that relaxing Equation (39) to Equation (46) actually expands the feasible domain of the problem, and the problem is correct if and only if Equation (46) is active, that is, if the exact relaxation is achieved. and Problem Pε Note that the validity of the relaxation cannot be guaranteed under the original objective function, so the objective function is modified to obtain the following parameterized relaxation problem

[0179]

[0180] subject to(33),(41)-(46)

[0181] like Figure 1 As shown, when When Defines a vector The associated supporting hyperplane supports the feasible set According to the support hyperplane theory, the support point It's a problem The optimal solution of That is the problem It is a question of P ε Precise relaxation.

[0182] Step 4: Design a parameter update strategy and iteratively solve the parameterized relaxation problem Come get the problem The solution.

[0183] question It is a question of P ε The exact relaxation of The objective function and Related, and In solving the problem In view of this, the iterative algorithm is designed to start from the initial value σ [0] = 0, iteratively update the support hyperplane parameter σ [k] , until convergence. Specifically, let σ [k] is the value of parameter σ in the kth iteration, and there are the following problems

[0184]

[0185] subject to(7),(15)-(20)

[0186] Then, in each iteration, according to the problem The solution to update the parameter σ [k] ,Right now

[0187]

[0188] To summarize, the design iteration algorithm is as follows:

[0189] Step 4.1: Set k = 0 and give the initial value of parameter σ [0] =0;

[0190] Step 4.2: At the k+1th iteration, solve the problem get and corresponding flight trajectories;

[0191] Step 4.3: Check convergence conditions:

[0192]

[0193] Where,∈ t = 0.001 is the given convergence criterion. If equation (48) is satisfied, execute step 4.4, otherwise let k = k + 1 and update the parameter σ according to equation (47) [k] , then proceed to step 4.2;

[0194] Step 4.4: The iterative algorithm terminates and the trajectory planning result is output.

[0195] like Figure 2 As shown, as the iteration proceeds, σ [k] Approaching question The solution to the problem is close to The solution.

[0196] Step 5: Convexify the non-convex obstacle avoidance constraint of formula (7) and design an iterative algorithm to obtain the problem The solution.

[0197] question It is still a non-convex optimization problem, and its only non-convexity comes from the non-convex obstacle avoidance constraint of formula (7). Let x [j] and y [j] is the solution of the jth iteration, and the non-convex obstacle avoidance constraint of formula (7) is convexed by the sequential linearization method to obtain

[0198] A0+A1x+A2y≥0 (49)

[0199] Where,

[0200]

[0201] Then we get the convex optimization problem

[0202]

[0203] subject to(15)-(20),(23)

[0204] Since Equation (7) is convexed using the sequential linearization method, an iterative algorithm is designed to solve the problem sequentially. Come get the problem The algorithm flow is as follows:

[0205] Step 5.1: Set j = 0 and give the initial reference trajectory x [0] and y [0] ;

[0206] Step 5.2: At the j+1th iteration, solve the problem Get x [j+1] and y [j+1] ;

[0207] Step 5.3: Check convergence conditions:

[0208] max|x [j+1] -x [j] |≤∈ x ,max|y [j+1] -y [j] |≤∈ y (50)

[0209] Where,∈ x =0.1m and∈ y = 0.1m is the given convergence criterion. If equation (50) is satisfied, execute step 5.4, otherwise let j = j + 1 and update the reference trajectory x [j] and y [j] , then proceed to step 5.2;

[0210] Step 5.4: The iterative algorithm terminates and the trajectory planning result is output.

[0211] Step 6: Design a double-loop trajectory planning algorithm for real-time planning of the UAV's minimum time obstacle avoidance flight trajectory, which is applied to solve the UAV's real-time trajectory planning problem.

[0212] like Figure 3 As shown, the solution to the problem described in step 5 is The iterative algorithm (inner loop) is embedded into the solution problem described in step 4 In the iterative algorithm (outer loop), the UAV trajectory planning algorithm is obtained as follows:

[0213] Step 6.1: Set k = 0 and give the initial value of parameter σ [0] =0;

[0214] Step 6.2: At the k+1th iteration, set j = 0 and give the initial reference trajectory x [0] and y [0] ;

[0215] Step 6.3: At the j+1th iteration, solve the problem Get x [j+1]and y [j+1] ;

[0216] Step 6.4: Check convergence conditions:

[0217] max|x [j+1] -x [j] |≤∈ x ,max|y [j+1] -y [j] |≤∈ y (51)

[0218] Where,∈ x =0.1m and∈ y = 0.1m is the given convergence criterion. If equation (51) is satisfied, execute step 6.5, otherwise let j = j + 1 and update the reference trajectory x [j] and y [j] , then proceed to step 6.3;

[0219] Step 6.5: The iterative algorithm terminates and outputs and corresponding flight trajectories;

[0220] Step 6.6: Check convergence conditions:

[0221]

[0222] Where,∈ t = 0.001 is the given convergence criterion. If equation (52) is satisfied, execute step 6.7, otherwise let k = k + 1 and update the parameter σ according to equation (47) [k] , then proceed to step 6.2;

[0223] Step 6.7: The iterative algorithm terminates and the trajectory planning result is output.

[0224] The trajectory planning result obtained by the method disclosed in the present invention is as follows Figure 4-5 As shown. It can be seen that the drone successfully avoided all obstacle areas and reached the desired terminal position, and the speed, acceleration magnitude, and the angle between the acceleration vector and the plumb bob direction strictly met the constraints. It is worth noting that the method disclosed in the present invention has good convergence and computational efficiency. The outer loop converged after 2 iterations, and the inner loop converged after 5 and 2 iterations respectively. On an ordinary computer equipped with an AMD Ryzen 76800H CPU 3.20GHz, the calculation time of the method disclosed in the present invention is only 144ms, which has good computational efficiency and the ability to realize real-time online trajectory planning.

[0225] The above specific description further illustrates the purpose, technical solutions and beneficial effects of the invention in detail. It should be understood that the above description is only a specific embodiment of the present invention and is not intended to limit the scope of protection of the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention should be included in the scope of protection of the present invention.

Claims

1. A real-time trajectory planning method for UAV based on supporting hyperplane, characterized in that: The following steps are included: Step 1: Consider the UAV dynamics and necessary constraints, and establish a trajectory planning problem with the goal of minimizing flight time. The implementation method of step one is: The UAV dynamics is expressed as Where r = [x, y, z] T is the position coordinate of the UAV, v=[v x ,v y ,v z ] T is the speed of the drone, g=[0,0,-g0] T is the acceleration due to gravity, a=[a x ,a y ,a z ] T is the acceleration of the drone, defined as follows: Where, T=[T x ,T y ,T z ] T is the thrust of the UAV, and m is the mass; The following are the necessary constraints that need to be considered in the UAV trajectory planning problem: (1) Initial and terminal state constraints: The initial and terminal state constraints of the UAV can be expressed as Where t0 and t f are the initial and terminal moments, r0 and r f are the initial and final positions, v0 and v f are the initial and terminal velocities, respectively; (2) Speed ​​constraints: To ensure flight safety, the following speed constraints are introduced to limit flight speed: ‖v‖2≤v max (4) Where, v max is the maximum speed; (3) Control constraints: The acceleration of the UAV is generated by thrust. Since thrust has a maximum value and a safety angle limit, the acceleration should satisfy the maximum acceleration constraint and the acceleration direction constraint: ‖a‖2≤a max (5) Where a max =T max / m is the maximum thrust T max The associated maximum acceleration, is the maximum angle between the acceleration vector and the plumb bob direction; (4) Obstacle avoidance constraints: UAVs often face complex obstacle environments during mission execution. Considering the presence of several elliptical obstacle areas in the mission environment, the obstacle avoidance constraints are as follows: Where i=1,…,m is the number of the obstacle area, (x o,i ,y o,i ) is the coordinate of the center point of the i-th obstacle area, a o,i and b o,i are the semi-major axis and semi-minor axis of the i-th obstacle area respectively; In order to improve the efficiency of mission completion, the optimization goal is to minimize the flight time, so the objective function is set to J=t f (8) In summary, the trajectory planning problem is established minimum J=t f subject to(1),(3)-(7) Step 2: Select appropriate free variables to solve the problem Equivalent transformation into problem P ε ; The implementation method of step 2 is: First, define a new free variable to map time to the fixed interval [0,1]: Taking t0 = 0, according to formula (9), we can get Then the dynamics of equation (1) can be rewritten as Where (·)′ represents the differentiation of τ; Considering that the differential equation of r in formula (10) is nonlinear, a new state variable is defined: According to Equation (11), the dynamics of Equation (10) can be rewritten as In formula (12) The differential equation is still nonlinear. In order to deal with nonlinear terms Introducing constraints: At the same time, define new state variables: Using Equations (13) and (14), the dynamics of Equation (12) is equivalently transformed into linear dynamics: Because variable substitution is used in the transformation process, the constraints of equations (3)-(6) need to be transformed into constraints on new variables: Ultimately, the problem is equivalently transformed into problem P ε : P ε :minimum J=t f subject to(7),(13),(15)-(19) Step 3: Relax the non-convex constraints of (13) and replace the original objective function with a parameterized objective function based on the support hyperplane theory to obtain the parameterized relaxation problem The implementation method of step three is: Question P ε Part of the non-convexity of comes from the non-convex equality constraint of (13), but it can be relaxed to a convex inequality constraint: Substituting equation (20) for equation (13), we get the problem P ε The relaxation problem minimum J=t f subject to(7),(15)-(20) Note that relaxing Equation (13) to Equation (20) actually expands the feasible domain of the problem, and the problem is solved if and only if Equation (20) is active, that is, when the exact relaxation is achieved. and Problem P ε are equivalent; note that under the original objective function, the effectiveness of the relaxation cannot be guaranteed, so the objective function is modified to obtain the following parameterized relaxation problem minimum J=t s -σt f subject to(7),(15)-(20) As shown in Figure 1, when When Defines a vector The associated supporting hyperplane supports the feasible set According to the support hyperplane theory, the support point It's a problem The optimal solution of That is the problem It is a question of P ε Precise relaxation of Step 4: Design a parameter update strategy and iteratively solve the parameterized relaxation problem Come get the problem The solution; The implementation method of step four is: question It is a question of P ε The exact relaxation of The objective function and Related, and In solving the problem is unknown before; in view of this, the iterative algorithm is designed to start from the initial value σ [0] = 0, iteratively update the support hyperplane parameter σ [k] , until convergence; specifically, let σ [k] is the value of parameter σ in the kth iteration, and there are the following problems minimum J=t s -σ [k] t f subject to(7),(15)-(20) Then, in each iteration, according to the problem The solution to update the parameter σ [k] ,Right now To summarize, the design iteration algorithm is as follows: Step 4.1: Set k = 0 and give the initial value of parameter σ [0] =0; Step 4.2: At the k+1th iteration, solve the problem get and corresponding flight trajectories; Step 4.3: Check convergence conditions: Where,∈ t is the given convergence criterion; if equation (22) satisfies, execute step 4.4, otherwise let k = k + 1 and update the parameter σ according to equation (21) [k] , then proceed to step 4.2; Step 4.4: The iterative algorithm terminates and the trajectory planning result is output; As shown in Figure 2, as the iteration proceeds, σ [k] Approaching question The solution to the problem is close to The solution; Step 5: Convexify the non-convex obstacle avoidance constraint of formula (7) and design an iterative algorithm to obtain the problem The solution; The implementation method of step five is: question It is still a non-convex optimization problem, and its only non-convexity comes from the non-convex obstacle avoidance constraint of formula (7); let x [j] and y [j] is the solution of the jth iteration, and the non-convex obstacle avoidance constraint of formula (7) is convexed by the sequential linearization method to obtain A0+A1x+A2y≥0 (23) where Then we get the convex optimization problem minimum J=t s -σ [k] t f subject to(15)-(20),(23) Since Equation (7) is convexed using the sequential linearization method, an iterative algorithm is designed to solve the problem sequentially. Come get the problem The algorithm flow is as follows: Step 5.1: Set j = 0 and give the initial reference trajectory x [0] and y [0] ; Step 5.2: At the j+1th iteration, solve the problem Get x [j+1] and y [j+1] ; Step 5.3: Check convergence conditions: max|x [j+1] -x [j] |≤∈ x ,max|y [j+1] -and [j] |≤∈ y (24) Where,∈ x and ∈ y is a given convergence criterion; if equation (24) is satisfied, execute step 5.4, otherwise let j = j + 1 and update the reference trajectory x [j] and y [j] , then proceed to step 5.2; Step 5.4: The iterative algorithm terminates and the trajectory planning result is output; Step 6: Design a double-loop trajectory planning algorithm for real-time planning of the UAV's minimum time obstacle avoidance flight trajectory, which is applied to solve the UAV's real-time trajectory planning problem; The implementation method of step six is: As shown in Figure 3, the solution to the problem described in step 5 is The iterative algorithm (inner loop) is embedded into the solution problem described in step 4 In the iterative algorithm (outer loop), the UAV trajectory planning algorithm is obtained as follows: Step 6.1: Set k = 0 and give the initial value of parameter σ [0] =0; Step 6.2: At the k+1th iteration, set j = 0 and give the initial reference trajectory x [0] and y [0] ; Step 6.3: At the j+1th iteration, solve the problem Get x [j+1] and y [j+1] ; Step 6.4: Check convergence conditions: max|x [j+1] -x [j] |≤∈ x ,max|y [j+1] -and [j] |≤∈ y (25) Where,∈ x and ∈ y is a given convergence criterion; If equation (25) satisfies, execute step 6.5, otherwise set j = j + 1 and update the reference trajectory x [j] and y [j] , then proceed to step 6.3; Step 6.5: The iterative algorithm terminates and outputs and corresponding flight trajectories; Step 6.6: Check convergence conditions: Where,∈ t is the given convergence criterion; if equation (26) satisfies, execute step 6.7, otherwise let k = k + 1 and update the parameter σ according to equation (21) [k] , then proceed to step 6.2; Step 6.7: The iterative algorithm terminates and the trajectory planning result is output.

Images