Method and system for uav flight trajectory optimization and conflict prevention

Abstract

The application discloses a method and system for unmanned aerial vehicle flight trajectory optimization and conflict prevention, and belongs to the technical field of unmanned aerial vehicle flight control; the method establishes a six-degree-of-freedom dynamic model, adopts an extended state observer to estimate and compensate external disturbance, constructs a performance optimization objective function for multi-aircraft coordination, solves an optimal control problem by using a variational method, designs a real-time rolling optimization algorithm, and constructs a safety constraint verification mechanism to realize conflict prevention.The application can effectively solve the cooperative flight problem of multiple unmanned aerial vehicles in a complex environment, is particularly suitable for trajectory optimization and collision avoidance control of a plant protection unmanned aerial vehicle cluster operation homeward journey, significantly improves flight safety and control accuracy, and has good practical value.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of unmanned aerial vehicle flight control, in particular to an unmanned aerial vehicle flight trajectory optimization and conflict prevention method and system. BACKGROUND

[0002] Multi-unmanned aerial vehicle cooperative operation has been widely applied in agricultural plant protection, disaster rescue, environmental monitoring and other fields; for the field of agricultural plant protection, in a complex farmland environment, plant protection unmanned aerial vehicles need to face various external disturbances, including wind disturbance, ground effect, load change and the like, which will cause the unmanned aerial vehicles to deviate from the predetermined trajectory, affecting the operation accuracy and flight safety; when multiple unmanned aerial vehicles operate in the same area, how to avoid inter-aircraft conflict becomes a key problem, especially in the concentrated return phase after operation is completed, multiple unmanned aerial vehicles may produce trajectory intersection in the limited airspace, and there is a risk of collision.

[0003] The multi-unmanned aerial vehicle conflict avoidance method mainly includes a collision avoidance method based on a geometric algorithm, a method based on an artificial potential field and a method based on model predictive control; these methods have the following disadvantages: first, there is a lack of effective estimation and compensation of external disturbances, resulting in low control accuracy; second, the dynamics characteristics of the system are not fully considered in multi-aircraft coordination optimization; third, the safety constraint verification mechanism is not perfect, which may cause collision avoidance reaction lag.

[0004] Therefore, there is an urgent need for an unmanned aerial vehicle flight trajectory optimization and conflict prevention method which can effectively handle external disturbances, realize multi-aircraft coordination optimization and has a perfect safety constraint verification mechanism. SUMMARY

[0005] The purpose of the present application is to provide an unmanned aerial vehicle flight trajectory optimization and conflict prevention method and system, by establishing an accurate six-degree-of-freedom dynamics model, using an extended state observer to estimate and compensate external disturbances in real time, combining a variational method to solve the optimal control problem, designing a real-time rolling optimization algorithm and a safety constraint verification mechanism, realizing cooperative flight trajectory optimization and effective conflict prevention of multiple unmanned aerial vehicles in a complex environment, and being particularly suitable for plant protection unmanned aerial vehicle cluster operation scenarios.

[0006] To achieve the above purpose, in a first aspect, the present application provides an unmanned aerial vehicle flight trajectory optimization and conflict prevention method for realizing trajectory optimization and conflict prevention of multiple unmanned aerial vehicles during return, the method comprising the following steps:

[0007] Step S1, establishing a six-degree-of-freedom dynamics model of each unmanned aerial vehicle, defining a state vector and a control input;

[0008] Step S2, based on the disturbance term in the dynamics model, establishing an external disturbance estimation and compensation mechanism, using the dynamics model and the disturbance compensation result to construct a performance optimization objective function for multi-aircraft coordination;

[0009] Step S3: Establish the optimal control problem based on the performance objective function, derive the analytical solution of the control law, discretize the optimal control law, design a real-time rolling optimization algorithm, and construct a safety constraint verification mechanism based on the discretized control law to achieve conflict prevention.

[0010] Furthermore, the six-degree-of-freedom dynamics model of the UAV is specifically as follows: for each UAV in the UAV swarm... Its state vector is defined as a six-dimensional vector containing position and velocity information: The disturbance term of the control input is represented in state-space form, taking into account both the physical characteristics of the UAV and the influence of the external environment.

[0011] ,in, express The three-dimensional thrust vector that acts on the UAV at all times. , These are three thrust components in three dimensions, subject to physical constraints: ; For the maximum thrust of the drone, ; express External disturbance term at any given time; Indicates drone exist Dynamic state at time t, dynamic function The specific form is:

[0012] ;

[0013] in, Assuming the drone's weight is 2.5 kg (empty weight), and These are the drag coefficients in the horizontal x and y directions, respectively. , This is the vertical drag coefficient. ; This is the acceleration due to gravity.

[0014] Furthermore, based on the disturbance term Establish an external disturbance estimation and compensation mechanism, disturbance term Including wind disturbances and model uncertainties, an extended state observer is used for real-time estimation: ,in, The disturbance is wind, which includes three-dimensional wind speed components. The uncertainty in the model represents the deviation between the dynamic model and the actual system.

[0015] Furthermore, the extended state observer is designed as follows:

[0016] ,in, and drones The state estimates and disturbance estimates, , The observer gain matrix is ​​based on the quality parameter. design:

[0017] ;in, , where is the observer bandwidth, and diag represents the diagonal matrix.

[0018] Furthermore, based on the six-degree-of-freedom dynamic model and perturbation estimation Construct the compensated system dynamics:

[0019] ; The residual disturbance after compensation. ;

[0020] The total control input is: , This is the total control input, including the nominal control input. and disturbance compensation items Nominal control input Based on the desired trajectory design, the disturbance compensation term Using perturbation estimation: ;in, for Acceleration components in the three coordinate axes.

[0021] Furthermore, based on the compensated system, a performance objective function is constructed. :

[0022] ,in, To control the weighting coefficients, For the number of drones, For drones The reference trajectory state vector, To control the time period, it should not exceed 240 seconds.

[0023] Furthermore, based on the performance objective function Using compensation dynamics, the optimal control problem is solved using the variational method:

[0024] Introducing costate variables Its dimension is the same as that of the state vector, and the Hamiltonian function is constructed as follows:

[0025] , For the first Hamiltonian function of the UAV, the optimality condition is:

[0026] , combined with the specific form of the dynamic function: , is a 3x3 identity matrix, is a 3x3 zero matrix, and the optimal control law :

[0027] ; where is the co-state variable corresponding to the component of the speed state.

[0028] Further, the optimal control law is discretized, and a rolling optimization algorithm is designed:

[0029] At each sampling time , linearization is performed based on the system matrix to obtain the discretized system matrix of the first UAV at time :

[0030] , where ;

[0031] The rolling optimization problem is:

[0032] ; is a six-dimensional vector of the first UAV at time k+j; is the reference trajectory state vector of the first UAV at time k+j, is the nominal control input of the first UAV at time k+j, and the constraint condition is: , where , the prediction horizon , and the control horizon .

[0033] Further, based on the discretized control law, a safety constraint verification mechanism is constructed: using the position component , the inter-UAV safety distance function is defined as:

[0034] ; where is the UAV and The safety distance function at time k; representing a UAV and The position component difference of the state vector at time k, ;

[0035] Based on the predicted trajectory, the safety constraint at future time k is constructed:

[0036] ; When the predicted , the control input is corrected:

[0037] , wherein, is the control input after avoidance correction, and the avoidance control term ; wherein is the avoidance gain.

[0038] Based on the same inventive concept, in a second aspect, the present application provides a UAV flight trajectory optimization and conflict prevention system, which comprises a dynamic modeling module, an interference estimation and compensation optimization module, and a conflict control prevention module.

[0039] The dynamic modeling module is used to establish a six-degree-of-freedom dynamic model of each UAV, and define a state vector and a control input.

[0040] The interference estimation and compensation optimization module is used to establish an external interference estimation and compensation mechanism based on the disturbance term in the dynamic model, and to construct a performance optimization objective function for multi-UAV coordination using the dynamic model and the interference compensation result.

[0041] The conflict control prevention module is used to establish an optimal control problem based on the performance objective function, derive an analytical solution of the control law, discretize the optimal control law, design a real-time rolling optimization algorithm, construct a safety constraint verification mechanism based on the discretized control law, and realize conflict prevention.

[0042] Compared with the prior art, the present application has the following beneficial effects:

[0043] The present application uses an extended state observer to estimate wind disturbance and model uncertainty in real time, significantly improves control accuracy through feedforward compensation, and is particularly suitable for plant protection operations in complex farmland environments; the analytical solution of the optimal control law is derived based on the variational method, and real-time control is realized by combining a rolling optimization algorithm, which reduces the computational complexity while ensuring control performance. BRIEF DESCRIPTION OF DRAWINGS

[0044] Figure 1 is a flowchart of the UAV flight trajectory optimization and conflict prevention method of the present application; ​

[0045] Figure 2 A schematic diagram of the unmanned aerial vehicle flight trajectory optimization and conflict prevention system according to the present application. DETAILED DESCRIPTION

[0046] In order to make the objectives, technical solutions and advantages of the present application clearer, the technical solutions in the present application are described below clearly and completely. Obviously, the described embodiments are part of the embodiments of the present application, rather than all the embodiments. Based on the embodiments in the present application, all other embodiments obtained by those of ordinary skill in the art without creative work fall within the protection scope of the present application.

[0047] Embodiment 1

[0048] As shown in FIG. 1, a flowchart of the unmanned aerial vehicle flight trajectory optimization and conflict prevention method according to the present application is shown. The method is used to realize trajectory optimization and conflict prevention when multiple unmanned aerial vehicles return, and includes the following steps: Figure 1

[0049] Step S1, a six-degree-of-freedom dynamic model of each unmanned aerial vehicle is established, and a state vector and a control input are defined.

[0050] The six-degree-of-freedom dynamic model of the unmanned aerial vehicle is specifically: for each unmanned aerial vehicle in the unmanned aerial vehicle group , the state vector is defined as a six-dimensional vector containing position and velocity information: ; the disturbance term of the control input is expressed in a state space form, and the physical characteristics of the unmanned aerial vehicle and the external environmental influence are considered:

[0051] , wherein, represents a three-dimensional thrust vector acting on the unmanned aerial vehicle at time t, , , are three-dimensional thrust components, which are subject to physical constraints: ; is the maximum thrust of the unmanned aerial vehicle, ; represents an external disturbance term at time t; represents the dynamic state of the unmanned aerial vehicle at time t, and the specific form of the dynamic function is: ;

[0052] ;

[0053] , wherein, is the mass of the unmanned aerial vehicle, and the unloaded mass of the unmanned aerial vehicle is 2.5 kg, and ​​​The drag coefficients in horizontal directions x and y, respectively, , The vertical drag coefficient, ; The acceleration of gravity.

[0054] Taking a certain type of plant protection unmanned aerial vehicle as an example, the empty weight of the unmanned aerial vehicle is 2.5 kg, the maximum load is 10 L of pesticide, and the total mass is about 12.5 kg when fully loaded. In the farmland operation environment, the unmanned aerial vehicle is mainly subjected to the following forces: thrust, gravity, air resistance and wind disturbance.

[0055] For example: in a farmland with an area of 100 mu, 3 plant protection unmanned aerial vehicles are set up for cooperative operation, and after the operation is completed, the 3 unmanned aerial vehicles need to return to the take-off point located in the center of the farmland from different positions; taking the cooperative return of the 3 plant protection unmanned aerial vehicles as an example, assuming that the initial return position of unmanned aerial vehicle 1 is (100, 50, 30) m, unmanned aerial vehicle 2 is located at (150, 100, 30) m, and unmanned aerial vehicle 3 is located at (80, 120, 30) m, and the target return point is (120, 80, 0) m; the system first obtains the real-time position and speed information of each unmanned aerial vehicle. Through the GPS positioning system and the inertial measurement unit (IMU), the three-dimensional position coordinates and speed components of the unmanned aerial vehicle are collected in real time, and the sampling frequency is 10 Hz.

[0056] When the plant protection unmanned aerial vehicle flies at a horizontal speed of 5 m / s, the resistance is: The unmanned aerial vehicle is subjected to various external disturbances, mainly including: wind disturbance, the common wind speed in the farmland environment is 2-8 m / s, and the gust can reach 12 m / s; ground effect, when the unmanned aerial vehicle flies below 3 m, it will be obviously affected by the ground effect; mass change caused by pesticide consumption during the plant protection process.

[0057] Step S2, based on the disturbance term in the dynamic model, an external disturbance estimation and compensation mechanism is established, and a performance optimization objective function for multi-vehicle coordination is constructed by using the dynamic model and the disturbance compensation result.

[0058] Based on the disturbance term , an external disturbance estimation and compensation mechanism is established, and the disturbance term contains wind disturbance and model uncertainty, and an extended state observer is used for real-time estimation: wherein, is the wind disturbance, containing three-dimensional wind speed components, is the model uncertainty, representing the deviation between the dynamic model and the actual system.

[0059] The extended state observer is designed as:

[0060] where, and are the state estimation and the disturbance estimation of the UAV, , , is the observer gain matrix, designed based on the mass parameter :

[0061] ; where, is the observer bandwidth, diag denotes a diagonal matrix.

[0062] Based on the six-degree-of-freedom dynamics model and the disturbance estimation , the compensated system dynamics is constructed as:

[0063] ; is the residual disturbance after compensation, ;

[0064] where, the total control input is: , is the total control input, including the nominal control input and the disturbance compensation term , the nominal control input is designed based on the desired trajectory, and the disturbance compensation term is constructed using the disturbance estimation: ; where, is the acceleration component in the three coordinate axis directions.

[0065] For a plant protection UAV with a mass of 2.5 kg, the observer gain matrix is: ; ; when encountering a lateral wind disturbance of 3 m / s, the disturbance compensation term is:

[0066] ;

[0067] Based on the compensated system, the performance objective function is constructed as:

[0068] where, is the control weight coefficient, is the number of UAVs, is the reference trajectory state vector of the UAV , and is the control time period, not greater than 240 s.

[0069] Step S3, based on the performance objective function to establish the optimal control problem, to derive the control law analytical solution, the optimal control law discretization, design real-time rolling optimization algorithm, based on the discrete control law to build safety constraint verification mechanism, to achieve conflict prevention.

[0070] Based on the performance objective function and compensation dynamics, the optimal control problem is solved by using the variational method:

[0071] Introducing the adjoint variable , which has the same dimension as the state vector, the Hamilton function is constructed:

[0072] , is the Hamilton function of the first unmanned aerial vehicle, and the optimality condition is:

[0073] , combined with the specific form of the dynamic function: , is a 3x3 unit matrix, is a 3x3 zero matrix, and the optimal control law :

[0074] ; wherein, is the adjoint variable corresponding to the component of the speed state.

[0075] Discretize the optimal control law , and design a rolling optimization algorithm:

[0076] At each sampling time , linearize based on the system matrix to obtain the discretized system matrix of the first unmanned aerial vehicle at time :

[0077] , wherein ;

[0078] The rolling optimization problem is:

[0079] ; is a six-dimensional vector of the first unmanned aerial vehicle at time k+j; is the reference trajectory state vector of the first unmanned aerial vehicle at time k+j, is the nominal control input of the first unmanned aerial vehicle at time k+j, and the constraint condition is: , wherein , the prediction horizon , control horizon .

[0080] For a quality of 2.5kg of plant protection UAV, when the coordination variable , ; at each sampling time , solve the following optimization problem: ; the prediction horizon is 20 steps (2 seconds), and the control horizon is 10 steps (1 second).

[0081] Based on the discretized control law, a safety constraint verification mechanism is constructed: using the position component , define the inter-UAV safety distance function:

[0082] ; where is the safety distance function of UAV and at time k; denotes the position component difference of the state vector of UAV and at time k, ;

[0083] Based on the predicted trajectory, construct the safety constraint at future time :

[0084] ; when it is predicted that , correct the control input:

[0085] , where is the control input after avoidance correction, and the avoidance control term ; where is the avoidance gain.

[0086] Suppose at a certain time, UAV 1 is located at (110, 70, 25) m, and UAV 2 is located at (115, 75, 25) m, the distance between the two UAVs is: ; since = 7.07<15m, the avoidance mechanism is triggered: ; the corrected control input is: .

[0087] When the application is applied in a 100-mu rice field of an agricultural cooperative, 5 plant protection unmanned aerial vehicles are deployed for pest control operation; the operation area is rectangular, 500 m long and 400 m wide; after the operation is completed, the 5 unmanned aerial vehicles need to return to the charging pile located in the center of the field from different positions.

[0088] The initial conditions are as follows:

[0089] Unmanned aerial vehicle 1: position (50, 100, 30) m, remaining power 15%; unmanned aerial vehicle 2: position (450, 300, 30) m, remaining power 18%; unmanned aerial vehicle 3: position (200, 50, 30) m, remaining power 12%; unmanned aerial vehicle 4: position (350, 200, 30) m, remaining power 20%; unmanned aerial vehicle 5: position (100, 350, 30) m, remaining power 16%; target point: (250, 200, 0) m (position of the charging pile); average wind speed: 3-5 m / s (southeast wind), gust: maximum 8 m / s; temperature: 28°C, relative humidity 85%.

[0090] The total return time of the simulation of the traditional geometric collision avoidance method is 156 seconds, the close approach (<10 m) occurs 3 times, the minimum safety distance is 8.2 m, the total trajectory energy consumption is 2.8 kWh, and the trajectory deviation (RMS) is 4.2 m; the total return time of the method of the application is 142 seconds (reduced by 9.0%); the close approach (<10 m) occurs 0 times; the minimum safety distance is 15.3 m; the total trajectory energy consumption is 2.4 kWh (reduced by 14.3%); and the trajectory deviation (RMS) is 1.8 m (reduced by 57.1%).

[0091] Example 2

[0092] As shown in Figure 2 FIG. 1 is a schematic diagram of the unmanned aerial vehicle flight trajectory optimization and conflict prevention system of the application, which comprises a dynamics modeling module, an interference estimation and compensation optimization module, and a conflict control prevention module.

[0093] The dynamics modeling module is used to establish a six-degree-of-freedom dynamics model of each unmanned aerial vehicle, and define a state vector and a control input.

[0094] The interference estimation and compensation optimization module is used to establish an external interference estimation and compensation mechanism based on the disturbance term in the dynamics model, and construct a performance optimization objective function of multi-aircraft coordination using the dynamics model and the interference compensation result.

[0095] The conflict control prevention module is used to establish an optimal control problem based on the performance objective function, derive an analytical solution of the control law, discretize the optimal control law, design a real-time rolling optimization algorithm, construct a safety constraint verification mechanism based on the discretized control law, and realize conflict prevention.

[0096] The above detailed description has further explained the purpose, technical solutions and beneficial effects of the present application. It should be understood that the above description is only a specific embodiment of the present application and is not intended to limit the protection scope of the present application. Any modification, equivalent replacement, improvement, etc. within the spirit and principle of the present application shall be included in the protection scope of the present application.

Claims

1. A method for trajectory optimization and conflict prevention of multiple unmanned aerial vehicles (UAVs) during their return flight, comprising the following steps: Step S1, establishing a six-degree-of-freedom dynamic model for each UAV, and defining a state vector and a control input; Step S2, based on a disturbance term in the dynamic model, establishing an external disturbance estimation and compensation mechanism, using the dynamic model and the disturbance compensation result, and constructing a performance optimization objective function for multi-UAV coordination; Based on six degrees of freedom dynamics model and disturbance estimation , build the compensated system dynamics: ; for the compensated residual disturbance, ; wherein, is a six-dimensional vector, is a nominal control input and a dynamics function; the total control input is: , is the total control input, comprising the nominal control input and a disturbance compensation term , the nominal control input is designed based on a desired trajectory, the disturbance compensation term utilizes a disturbance estimation: ; wherein, is an acceleration component in three coordinate axis directions, is the mass of the UAV; Based on the compensated system, a performance objective function is constructed : ,in, To control the weighting coefficients, For the number of drones, For drones The reference trajectory state vector, To control the time period, it should not exceed 240 seconds; Step S3, based on the performance objective function, establishing an optimal control problem, deriving an analytical solution of the control law, discretizing the optimal control law, designing a real-time rolling optimization algorithm, constructing a safety constraint verification mechanism based on the discretized control law, and achieving conflict prevention. Based on the performance objective function and compensating dynamics, the optimal control problem is solved using the calculus of variations: Introducing the co-state variable of the same dimension as the state vector, the Hamiltonian function is constructed: , For the first Hamiltonian function of the UAV, the optimality condition is: , depending on the specific form of the kinetic function: , is a 3x3 identity matrix, is a 3x3 zero matrix, resulting in the optimal control law : ; wherein, is a covariate component corresponding to the velocity state; Based on the discretized control law, a safety constraint verification mechanism is constructed: the inter-machine safety distance function is defined by using the position component . ; wherein, is a drone and a safety distance function at time k; denotes a drone and a position component difference of the state vector at time k, ; Based on the predicted trajectory, a safety constraint is constructed for a future time instant ​ ; when it is predicted that the control input is corrected: wherein, is the jth nominal control input of the jth is the jth ; wherein is an avoidance gain.

2. The method of claim 1, wherein, The six-degree-of-freedom dynamics model of the UAV is specifically: for each UAV in the UAV group , a state vector defined as a six-dimensional vector containing position and velocity information: ; the disturbance term of the control input is expressed in a state space form, while considering the physical characteristics of the UAV and the external environmental influence: ,in, express The three-dimensional thrust vector that acts on the UAV at all times. , These are three thrust components in three dimensions, subject to physical constraints: ; For the maximum thrust of the drone, ; express External disturbance term at any given time; Indicates drone exist Dynamic state at time t, dynamic function The specific form is as follows: ； wherein the drone mass is taken as the empty mass of the drone 2.5 kg, and are the drag coefficients in horizontal directions x and y, respectively, , is the vertical drag coefficient, ; is the acceleration of gravity.

3. The method of claim 2, wherein, Based on the disturbance term , an external disturbance estimation and compensation mechanism is established, the disturbance term contains wind disturbance and model uncertainty, and an extended state observer is used for real-time estimation: , wherein, is the wind disturbance, containing three-dimensional wind speed components, is the model uncertainty, representing the deviation of the dynamic model from the actual system.

4. The method of claim 3, wherein, The extended state observer is designed as: wherein, and are state estimation values and estimation values of disturbances of the UAV, , , is an observer gain matrix, designed based on mass parameters : ; wherein, is the observer bandwidth, diag denotes a diagonal matrix.

5. The method of claim 4, wherein, The optimal control law Discretization is performed, and a rolling optimization algorithm is designed: At each sampling time , based on the system matrix, linearization is performed to obtain the first discretization system matrix of the UAV at time :​ wherein ; The rolling optimization problem is: ; for the jth six-dimensional vector of the jth for the jth reference trajectory state vector of the jth constraint condition is: wherein, prediction horizon control horizon .

6. An unmanned aerial vehicle flight trajectory optimization and conflict prevention system for performing the method of any one of claims 1-5, characterized by, The system comprises a dynamic modeling module, a disturbance estimation and compensation optimization module, and a conflict control prevention module. The dynamic modeling module is configured to establish a six-degree-of-freedom dynamic model for each UAV, and define a state vector and a control input. The disturbance estimation and compensation optimization module is configured to, based on a disturbance term in the dynamic model, establish an external disturbance estimation and compensation mechanism, use the dynamic model and the disturbance compensation result, and construct a performance optimization objective function for multi-UAV coordination. The conflict control prevention module is configured to, based on the performance objective function, establish an optimal control problem, derive an analytical solution of the control law, discretize the optimal control law, design a real-time rolling optimization algorithm, construct a safety constraint verification mechanism based on the discretized control law, and achieve conflict prevention.

Images