Unmanned aerial vehicle path planning and control integrated optimization method

Abstract

The invention discloses an unmanned aerial vehicle path planning and control integrated optimization method, and the method achieves the simultaneous solving of a path planning problem and an optimal control problem through the direct optimization of control input, then, is different from a conventional quadratic programming method, introduces a particle swarm optimization algorithm to solve a complex nonlinear model prediction problem, and finally, achieves the optimal control of an unmanned aerial vehicle. According to the method, in order to overcome the defects of an artificial potential field method, model prediction control is adopted to predict follow-up waypoints, a particle swarm optimization algorithm is adopted to optimize control input, the influence of a traditional artificial potential field method on path planning and control is avoided, and meanwhile optimization of the unmanned aerial vehicle path and control input is achieved. According to the method, the path and the control input are optimized at the same time, the optimal path can be planned, the control input required by driving is also an optimal solution, and integrated optimization of path planning and control of the unmanned aerial vehicle is achieved.

Description

Technical Field

[0001] The present invention designs an integrated optimization method for UAV path planning and control, which combines the artificial potential field method, model predictive control and particle swarm optimization algorithm, and provides an optimization method for control input that considers the map layout and obstacle distribution in path planning; and considers the magnitude of the control input and the effect of the control input in control. It belongs to the field of automatic control technology. Background Technique

[0002] Nowadays, UAVs are widely used in environmental monitoring, sports photography, commercial fields and military fields. As the tasks become more and more complex and changeable, the current UAVs are restricted by the physical and algorithms of the UAV itself and are difficult to meet the increasingly complex task requirements. Therefore, the demand for effective and efficient path planning and control algorithms for UAVs is becoming increasingly urgent. The UAV system includes a sensing system, a planning system, a control system, etc. Among them, the path planning and optimal control of UAVs are two key aspects of the UAV planning system and the control system. Excellent UAV path planning can help the UAV better improve the path quality, obstacle avoidance ability and the ability to reach the target point. Optimal control can help the UAV complete better flight tasks with less control, improve the flight efficiency of the UAV and reduce the energy consumption of the UAV. Combining the two can obtain a better flight path, lower energy consumption and flight error.

[0003] The artificial potential field method is widely used in UAV path planning, but there is also the problem of falling into local minima. In the traditional artificial potential field method, falling into a local minimum point will cause the gravitational force and repulsive force brought by the artificial potential field acting on the UAV to be equal, and the control input is zero, resulting in the UAV being unable to plan an effective control input for subsequent flight. However, since the planning method designed in this method starts from the control input rather than the artificial potential field force, and predicts the quality of the potential field of subsequent path points through model predictive control, even if it falls into a local minimum point, it will not affect the subsequent planning and control of the UAV.

[0004] The current mainstream UAV path planning and control is a two-layer architecture. That is, the UAV system first conducts path planning and optimization to obtain the optimal path to avoid obstacles and reach the target point. After obtaining the optimal path, the input is optimized to obtain the input with the smallest value and the smallest tracking error. This two-layer architecture can effectively achieve UAV path planning and tracking, but there are also some deficiencies. In the face of emergencies, the two-layer architecture needs to adjust the path from the path planning layer and then adjust the control amount from the input planning layer to achieve tracking of the adjusted path. At the same time, since the tracking difficulty is not considered when planning the path, it may lead to a situation where the path is optimal, but the control input is large or the error is large. Therefore, the adjustment efficiency of the UAV under the two-layer architecture is poor, making it difficult to handle emergencies and not being the optimal solution in global tasks. To solve the delay problem brought by the two-layer path planning-control tracking architecture and the problem of optimal control input, this paper proposes an optimization method that combines path planning and control, taking the path point quality and control input size in the prediction time domain as the optimization objectives to achieve the integrated optimization of UAV path planning and control. Summary of the Invention

[0005] The purpose of the present invention is to provide an integrated optimization method that can take path planning and control as optimization objectives simultaneously. The proposed method can have the characteristics of optimal path and minimum control input.

[0006] The technical solution adopted by the present invention is an integrated optimization method for UAV path planning and control. First, an artificial potential field is constructed according to the task objective and obstacle distribution, and considering the future motion state of the UAV, a multi-objective optimization objective function including path planning and optimal control is constructed; then, the control input sequence within the prediction time domain is initialized, corresponding to the control input quantity corresponding to each time node of the model predictive control; after that, through the control input sequence and the attitude loop model of the UAV, the path points within the prediction time domain of the UAV are predicted, and the potential field value corresponding to each path point is obtained by combining the artificial potential field method, and the control input sequence is optimized by the particle swarm optimization algorithm. After obtaining the optimal control input sequence, the first and last elements of the sequence are used as the control input in the ground coordinate system; finally, through the control input in the ground coordinate system, the lift and desired attitude of the UAV are obtained, and the torque of the attitude loop control input is optimized and solved by the particle swarm optimization algorithm to obtain the optimal control input of the attitude loop. The lift and torque of the UAV are applied to the UAV to achieve attitude adjustment and flight of the UAV.

[0007] The specific steps are as follows:

[0008] Step 1: Since this method optimizes both path planning and control input simultaneously, in terms of path planning, an artificial potential field is constructed according to the environmental information. Referring to the obstacle distribution and the start and end point distributions of the UAV, the traditional artificial potential field method is introduced to construct a partial optimization objective function for path planning.

[0009] In terms of the optimal control in Step 2, considering the magnitude of the control input, an optimization objective function for the optimal control part is constructed. In Step 3, the path planning in Step 1 and the optimal control optimization objective design in Step 2 are combined, and based on the model, the motion state of the UAV at future moments is predicted to obtain the optimization objective function of the integrated optimization.

[0010] In Step 4, a control input sequence of the model predictive control within the prediction horizon is generated to obtain the control input part of the optimization objective function.

[0011] In Step 5, according to the UAV dynamics model, the path points within the prediction horizon of the UAV and the potential field values corresponding to the path points are obtained. The control input sequence of the UAV is optimized by the particle swarm optimization algorithm to obtain the optimal control sequence within the prediction horizon.

[0012] In Step 6, the first element of the control sequence is extracted as the force condition of the UAV in the ground coordinate system under the current state, and the lift force and desired attitude of the UAV are obtained from the force in the ground coordinate system.

[0013] In Step 7, an optimization objective function for the attitude loop control is constructed, and the control quantity of the attitude loop is optimized by the particle swarm optimization algorithm to achieve the tracking of the optimal attitude and the input of the optimal control.

[0014] Among them, in the part of constructing the artificial potential field method described in Step 1, referring to the potential field construction method of the traditional artificial potential field method, p 0 is introduced as the maximum influence range of the obstacle, K gra is used as the gravitational parameter of the artificial potential field method, and K rep is used as the repulsive force parameter of the artificial potential field method. Among them, the gravitational potential field U gra and the repulsive potential field U rep are designed as follows:

[0015]

[0016] Among them, U gra is the gravitational field, and U rep is the repulsive field. D(q, q t ) is the distance between the current position of the UAV and the target point, and d(q, q i ) is the distance between the current position of the UAV and each obstacle. When the distance is greater than p 0 , it is considered that the obstacle has no influence on the UAV. When the distance is less than p 0 , it is considered that the obstacle has an influence on the UAV, and thus the artificial potential field is designed.

[0017] Meanwhile, the total potential field U can be obtained by the following formula:

[0018] U = U gra + Urep (2)

[0019] Among them, for the gravitational field part, when the distance d(q, q t ) between the UAV and the target point decreases, the gravitational field U gra will also decrease; for the repulsive field part, when the distance d(q, q i ) between the UAV and the obstacle increases, the repulsive field U rep will also decrease. Therefore, the decrease of the gravitational field represents a closer distance to the target point, and the decrease of the repulsive field represents a safer driving route. Therefore, by constructing a potential field and flying along the direction of the decreasing potential field, the effects of reaching the target point and avoiding obstacles can be achieved. Different from the traditional artificial potential field, the traditional artificial potential field method constructs an artificial potential field to obtain the potential field force, and uses the potential field force as the control input to achieve the control effect of the controlled object, while this method only draws on the idea of reaching the target point and avoiding obstacles in the artificial potential field method. Thus, the optimized objective function of the path planning part is constructed as follows:

[0020]

[0021] Among them, N c is the prediction horizon of the path planning part, J u is the optimized objective function of the path planning part, Q u is the parameter matrix of the potential field force part, and U i is the potential field force corresponding to each path point within the prediction horizon.

[0022] Among them, in the optimal control part described in step 2, model predictive control is introduced, and the control input should be the control input sequence within the prediction horizon of the model predictive control. At the same time, since the optimal control part hopes that the control input is minimized, the sum of the control inputs within the prediction horizon needs to be minimized in the optimized objective function part. The optimized objective function of the optimal control part is designed as follows:

[0023]

[0024] f is the optimized objective function of the optimal control part, N p is the prediction horizon of the control input part, Q f is the parameter matrix of the optimal control part, and F i is the control input corresponding to each prediction time point.

[0025] Among them, in the optimized objective function part of the integrated optimization method described in step 3, the optimized objective functions of the path planning and optimal control constructed in step 1 and step 2 are combined to obtain the integrated optimized objective function as follows:

[0026]

[0027] Among them, N c is the path point prediction time domain in model predictive control, and N p is the control input prediction time domain in model predictive control, Q u is the potential field parameter matrix, Q f is the control input parameter matrix, F i is the input quantity of control at each moment, U i is the potential field corresponding to each path point.

[0028] In the path planning part, minimizing the optimization objective function can achieve obstacle avoidance and reach the target point; in the optimal control part, it is necessary to minimize the control input within the prediction time domain. Therefore, the objectives of both parts are to minimize the optimization objective function. Minimizing the integrated optimization objective function of path planning and control can achieve the minimum control quantity and avoid obstacles with the shortest path to reach the target point.

[0029] Among them, in the control input sequence generation part within the prediction time domain described in step four, since the UAV needs to fly to the target point and should also perform obstacle avoidance actions towards the position closest to the target point when avoiding obstacles to prevent redundant flight operations and avoid unnecessary searches in global search, the initial direction of the UAV control generation sequence is located in the space containing the target point cut by the plane perpendicular to the line connecting the UAV and the target point and passing through the current position of the UAV. Thus, the control input sequence F = {F 1 , F 2 , F 3 ……F Np} within the prediction time domain is obtained.

[0030] Among them, in the UAV position loop part described in step five, the UAV position loop is designed as follows:

[0031]

[0032] Among them, p is the UAV position, v is the UAV speed, g is the acceleration due to gravity, T is the UAV lift force, and R is the UAV rotation matrix. The predicted control input F in the design of model predictive control is the control input of the UAV in the ground coordinate system, and its relationship with the UAV lift force is as follows:

[0033]

[0034] Thus, the path points of the UAV within the prediction time domain can be obtained by combining the position loop and the control input in the ground coordinate system. Through the path points and combined with formula (1), the potential field values corresponding to the path points are obtained.

[0035] For the particle swarm optimization algorithm part, the particle swarm optimization iteration method is designed as follows:

[0036]

[0037] Among them, v is the particle running speed, I is the particle inertia coefficient, and c 1 is the global optimal learning coefficient, and c 2 is the local optimal learning coefficient, and ξ is the particle state. Through the particle swarm optimization algorithm, the optimal control input sequence of the UAV in the ground coordinate system can be obtained.

[0038] Among them, for the expected attitude of the UAV and the force-bearing part in the UAV body coordinate system described in step six, after obtaining the control input of the UAV in the ground coordinate system, the lift force T received by the UAV can be directly obtained, and the formula is as follows:

[0039]

[0040] At the same time, the expected attitude q of the UAV c can be solved by the following formula:

[0041] q c =[σ c ,q 1 ,q 2 ,q 3 (10)

[0042] Among them, [q 1 ,q 2 ,q 3 T =[-L y ,L x ,0] T / (2||L||σ c ), where, let Through the above formula, the expected attitude of the UAV corresponding to it can be directly obtained based on the known control input situation of the UAV in the ground coordinate system.

[0043] Among them, for the construction of the attitude loop optimization objective function and the solution part described in step seven, the UAV attitude loop model is constructed as follows:

[0044]

[0045] Among them, q is the UAV attitude, and P is calculated through the UAV attitude quaternion. The formula is as follows:

[0046]

[0047] Among them, ω is the UAV angular velocity, and J I ​is the inertia coefficient matrix of the drone, and τ is the input torque of the drone attitude loop control.

[0048] In the process of obtaining the desired attitude and tracking the desired attitude, it is necessary to apply torque τ to the drone to achieve the drone angular velocity tracking the desired angular velocity ω. c . ω c can be calculated by the following formula:

[0049]

[0050] To sum up, the input of the attitude loop control is torque τ. Applying torque to the drone enables the drone angular velocity ω to approach the desired angular velocity ω. c , and finally the drone attitude angle q = q c . Since the above attitude angles are all represented by quaternions, and it is difficult for quaternions to accurately represent the attitude angle error, it is necessary to convert the quaternion attitude angle q to θ represented by Euler angles. To sum up, it is hoped to minimize the drone angular velocity error and attitude angle error, and minimize the control input. The designed optimization objective function is as follows:

[0051]

[0052] Among them, J a is the optimization objective function of the attitude loop, k 1 is the control input parameter, k 2 is the angular velocity error parameter, k 3 is the attitude angle error parameter, τ is the control input, ω e is the angular velocity error, ω e = ω c - ω, θ e is the angular velocity error, θ e = θ c - θ. Finally, the particle swarm optimization algorithm is introduced again. The difference from the position loop particle swarm optimization algorithm is that the particle state initialization constraint is only set to the input range of the control input τ, so as to optimize and solve the attitude loop control input τ. After obtaining the optimal control input τ, the drone lift T and the drone torque τ are applied to the drone to achieve attitude tracking and the action of the drone lift.

[0053] Technical breakthroughs and performance advantages of the present invention:

[0054] In terms of the technical breakthroughs of this method, firstly, this method proposes an integrated optimization method for UAV path planning and control. By directly optimizing the control input, it solves both the path planning problem and the optimal control problem simultaneously. Secondly, different from previous quadratic programming methods, this paper introduces the particle swarm optimization algorithm to solve complex non-linear model prediction problems. Finally, aiming at the deficiencies of the artificial potential field method, this method uses model prediction control to predict subsequent waypoints and the particle swarm optimization algorithm to optimize the control input. While solving the impact of the traditional artificial potential field method on path planning and control, it realizes the optimization of the UAV path and control input.

[0055] In terms of performance advantages, different from the traditional path planning and tracking control with a two-layer architecture of planning - tracking, which may lead to the problem of difficult tracking of the optimal path, this method optimizes both the path and the control input simultaneously. It can achieve the optimal path planning while the required control input for driving is also the optimal solution, realizing the integrated optimization of UAV path planning and control. The symbol explanations are as follows:

[0056] U gra is the gravitational field;

[0057] K gra is the gravitational parameter;

[0058] d(q,q t ) is the distance between the current position and the target point;

[0059] U rep is the repulsive field;

[0060] K rep is the repulsive parameter;

[0061] d(q,q i ) is the distance between the UAV and the obstacle;

[0062] p 0 is the maximum influence range of the obstacle;

[0063] U is the total potential field;

[0064] J is the optimization objective function;

[0065] N c is the prediction time domain of the artificial potential field part;

[0066] N p is the prediction time domain of the control input;

[0067] Q u is the parameter of the potential field part;

[0068] Q f is the parameter of the control input part;

[0069] F is the set within the control input time domain;

[0070] p is the current position of the UAV;

[0071] v is the flight speed of the UAV;

[0072] g is the gravitational acceleration;

[0073] is the matrix of [0, 0, 1] T ;

[0074] T is the lift of the UAV;

[0075] m is the mass of the UAV;

[0076] R is the rotation matrix of the UAV;

[0077] F x is the force in the x-axis direction in the ground coordinate system of the UAV;

[0078] F y is the force in the y-axis direction in the ground coordinate system of the UAV;

[0079] F z is the force in the z-axis direction in the ground coordinate system of the UAV;

[0080] I is the inertia coefficient in the particle swarm optimization algorithm;

[0081] c 1 is the global optimal learning coefficient;

[0082] G best is the global optimal state;

[0083] c 2 is the local optimal learning coefficient;

[0084] P best is the local optimal state;

[0085] ξ is the particle state;

[0086] q is the attitude of the UAV;

[0087] The P formula is

[0088] ω is the flight angular velocity of the UAV;

[0089] J I is the inertia matrix of the UAV;

[0090] τ is the input torque of the attitude loop control;

[0091] σ is the first digit of the attitude quaternion of the UAV;

[0092] q is the last three digits of the UAV attitude quaternion;

[0093] J a is the optimization objective function of the attitude loop;

[0094] k 1 is the control input parameter;

[0095] k 2 is the angular velocity error parameter;

[0096] k 3 is the attitude angle error parameter;

[0097] τ is the control input of the attitude loop;

[0098] ω e is the angular velocity error;

[0099] θ e is the angular velocity error; Description of the Drawings

[0100] Figure 1 is the path planning effect of this method;

[0101] Figure 2 are the position loop control input T and the attitude loop control input τ of the UAV;

[0102] Figure 3 are the expected attitude and the actual attitude diagrams and the attitude error diagrams of the UAV;

[0103] Figure 4 is the block diagram of the present invention. Detailed Implementation Manner

[0104] The design methods of each part in the present invention will be further described below:

[0105] For the "integrated optimization method for UAV path planning and control" of the present invention, the specific steps are as follows:

[0106] Step 1: Construction of the optimization objective function for the path planning part

[0107] The integrated optimization objective function for path planning and control is divided into two parts, namely the path planning part and the control part. The idea of model prediction is introduced, and N c is set as the prediction time domain for path planning, and referring to the idea of the artificial potential field method, the optimization objective function for the path planning part is obtained.

[0108]

[0109] Among them, N c is the prediction time domain for the path planning part, and J uFor the path planning part to optimize the objective function, Q u is the parameter matrix of the potential field force part, U i is the potential field force corresponding to each path point within the prediction horizon.

[0110] To meet the requirements of obstacle avoidance and reaching the target point, it is necessary to minimize the optimization objective function J u .

[0111] Step 2: Construction of the optimization objective function for the optimal control part

[0112] In the control part, the optimal control part needs to minimize the control input. Therefore, the optimization objective function of the optimal control part needs to consider minimizing the sum of the control input sequences within the prediction horizon. Set N p as the prediction horizon of the control input, and obtain the optimization objective function of the control part

[0113]

[0114] where J f is the optimization objective function of the optimal control part, N p is the prediction horizon of the control input part, Q f is the parameter matrix of the optimal control part, F i is the control input corresponding to each prediction time point.

[0115] To minimize the sum of the control input sequences, it is necessary to minimize the optimization objective function J f .

[0116] Step 3: Construction of the integrated optimization objective function

[0117] Combining the optimization objective functions of the path planning and optimal control parts in Step 1 and Step 2, the optimization objective function of the integrated path planning and control optimization method is constructed as follows:

[0118]

[0119] where N c is the prediction horizon of the path points in model predictive control, N p is the prediction horizon of the control input in model predictive control, Q u is the potential field parameter matrix, Q f is the control input parameter matrix, F i is the input quantity of each control moment, U i is the potential field corresponding to each path point.

[0120] Since the path planning part realizes obstacle avoidance and reaches the target point along the shortest path by minimizing the optimization objective function; the optimal control part realizes minimizing the control input by minimizing the optimization objective function. Therefore, in the integrated optimization of path planning and control, it is necessary to minimize the optimization objective function to achieve the task of minimizing the control input, realizing the shortest path, avoiding obstacles and reaching the target point.

[0121] Step 4: Initialize the control input sequence within the prediction horizon

[0122] Initialize the control input sequence within the prediction horizon. Since the UAV needs to avoid obstacles and also reach the target point, the direction of its control input should be within the plane where the included angle between the line connecting the UAV and the target point is less than 90°. Initialize the direction of the control input within the prediction horizon by this method to obtain the control input sequence F = {F 1 , F 2 , F 3 …… F Np}.

[0123] Step 5: Combine the control sequence with the UAV position loop to obtain the path points and the corresponding potential field values

[0124] Combine the predicted position control input in Step 4 with the UAV position loop to obtain the path points of the UAV within the prediction horizon. Combine the path points with the potential field constructed in formula (1) to obtain the potential field of the UAV at the corresponding path points. Thus, the optimization objective function values corresponding to the control sequences of each particle are obtained, and the particle swarm optimization is realized by designing the following particle swarm optimization iteration formula.

[0125]

[0126] Among them, v is the particle running speed, I is the particle inertia coefficient, c 1 is the global optimal learning coefficient, c 2 is the local optimal learning coefficient, and ξ is the particle state. Through the particle swarm optimization algorithm, the optimal control input sequence of the UAV in the ground coordinate system can be obtained.

[0127] Finally, when the particle swarm optimization algorithm reaches the maximum number of iterations or converges, the optimal control input sequence can be obtained. Apply the first element of the optimal control input sequence to the UAV, shift the control sequence one position forward, and randomly generate the last element as the initial value of the particle swarm optimization algorithm at the next time point to ensure the coherence of the optimization algorithm.

[0128] Step 6: Convert the force situation in the ground coordinate system into the desired attitude and the UAV lift

[0129] The optimal control sequence is the control input F of the UAV in the ground coordinate system, while the actual position control input of the UAV is the lift T of the UAV. Therefore, after obtaining the control input F of the UAV in the ground coordinate system, the lift T of the UAV is calculated. At the same time, the expected attitude q of the UAV corresponding to it is obtained c .

[0130] Step 7: Construct the optimization objective function of the attitude loop and solve the control input of the attitude loop

[0131] The attitude loop part realizes the tracking of the angular velocity ω of the UAV to the expected angular velocity θ by applying torque τ to the UAV, and ω c can be obtained by the following formula:

[0132]

[0133] At the same time, since the quaternion cannot accurately represent the attitude error, the quaternion attitude angle q is converted into θ represented by Euler angles, so as to construct an optimization objective function J with the minimum control input, the minimum angular velocity error and the minimum attitude angle error a , and the formula is as follows:

[0134]

[0135] Among them, J a is the optimization objective function of the attitude loop, k 1 is the control input parameter, k 2 is the angular velocity error parameter, k 3 is the attitude angle error parameter, τ is the control input, ω e is the angular velocity error, ω e =ω c -ω, θ e is the angular velocity error, θ e =θ c -θ. Finally, taking formulas (18) and (19) as the particle swarm optimization iteration method, the particle swarm optimization of the control input torque τ of the attitude loop is carried out to find the optimal control input. The position loop control input T and the attitude loop control input τ are applied to the UAV to realize the tracking of the attitude loop and the position update of the UAV

[0136] Experimental charts and related data:

[0137] After the simulation of the present invention, its reliability and feasibility have been verified

[0138] As Figures 1-3 shown, comparisons are made from three aspects: the path planning of the UAV, the position loop control input T and the attitude loop control input τ, and the attitude loop tracking effect Figure 1 The path planning effect of this method; Figure 2The position loop control input T and the attitude loop control input τ of the unmanned aerial vehicle; Figure 3 The desired attitude and the actual attitude diagrams and the attitude error diagrams of the unmanned aerial vehicle;

[0139] As Figure 4 shown is the block diagram of the present invention.

Claims

1. A method for integrated optimization of UAV path planning and control, characterized in that: Firstly, an artificial potential field is constructed according to the mission objectives and obstacle distribution, and the multi-objective optimization objective function including path planning and optimal control is constructed by considering the motion state of the UAV in the future. Then, the control input sequence in the prediction time domain is initialized, corresponding to the control input quantity corresponding to each time node of the model prediction control. After that, the path points in the prediction time domain of the UAV are predicted by the control input sequence and the attitude loop model of the UAV, and the potential field value corresponding to each path point is obtained by combining the artificial potential field method. The control input sequence is optimized by the particle swarm optimization algorithm, and the first position of the sequence is used as the control input in the ground coordinate system after the optimal control input sequence is obtained. Finally, the lift and expected attitude of the UAV are obtained through the control input in the ground coordinate system, and the torque of the attitude loop control input is optimized and solved by the particle swarm optimization algorithm to obtain the optimal control input of the attitude loop. The lift and torque of the UAV are applied to the UAV to realize the attitude adjustment and flight of the UAV.

2. The integrated optimization method for UAV path planning and control according to claim 1 is characterized in that: The specific steps are as follows: Step 1: In terms of path planning, an artificial potential field is constructed based on environmental information. With reference to the obstacle distribution and the starting and ending point distribution of the UAV, the artificial potential field method is introduced to realize the construction of the optimization objective function of the path planning part. Step 2: In terms of optimal control, consider the control input size and construct the optimization objective function of the optimal control part; Step 3: Combine the path planning in Step 1 and Step 2 with the optimal control optimization target design, and predict the UAV motion state at future moments according to the model to obtain the integrated optimization objective function; Step 4: Generate a control input sequence of the model predictive control in the prediction time domain to obtain the control input part of the optimization objective function; Step 5: According to the UAV dynamics model, the path points in the prediction time domain of the UAV and the potential field values ​​corresponding to the path points are obtained; the UAV control input sequence is optimized by the particle swarm optimization algorithm to obtain the optimal control sequence in the prediction time domain; Step 6: Extract the first position in the control sequence as the force condition of the UAV in the ground coordinate system in the current state, and obtain the lift and expected attitude of the UAV through the force condition in the ground coordinate system; Step seven, construct the attitude loop control optimization objective function, optimize the attitude loop control quantity through the particle swarm optimization algorithm, realize the tracking of the optimal attitude and the input of the optimal control.

3. The integrated optimization method for UAV path planning and control according to claim 2 is characterized in that: In the construction part of the artificial potential field method described in step 1, refer to the potential field construction method of the artificial potential field method, introduce p0 as the maximum influence range of the obstacle, K gra As the gravitational parameter of the artificial potential field method, K rep As the repulsive force parameter of the artificial potential field method, the gravitational potential field U gra With the repulsive potential field U rep The design is as follows: Among them, U gra is the gravitational field, U rep is the repulsive field; d(q,q t ) is the distance between the current position of the UAV and the target point, d(q,q i ) is the distance between the current position of the UAV and each obstacle. When the distance is greater than p0, it is considered that the obstacle has no impact on the UAV. When the distance is less than p0, it is considered that the obstacle has an impact on the UAV. The total potential field U is obtained using the following formula: U=U gra +U rep (2) Among them, in the gravitational field part, when the distance between the drone and the target point is d(q,q t ) decreases, the gravitational field U gra will decrease accordingly; in the repulsive field, when the distance between the drone and the obstacle is d(q,q i ) increases, the repulsive field U rep Will decrease accordingly; the optimization objective function of the path planning part is constructed as follows: Among them, N c is the prediction time domain of the path planning part, J u Optimize the objective function for the path planning part, Q u is the parameter matrix of the potential field force part, U i To predict the potential field force corresponding to each path point in the time domain.

4. The integrated optimization method for UAV path planning and control according to claim 3 is characterized in that: In the optimal control part described in step 2, model predictive control is introduced, and the control input is the control input sequence in the prediction time domain of model predictive control; since the optimal control part hopes to minimize the control input, the optimization objective function part needs to minimize the sum of the control input in the prediction time domain, and the optimization objective function of the optimal control part is designed as follows: Among them, J f Optimize the objective function for the optimal control part, N p To control the prediction time domain of the input part, Q f is the parameter matrix of the optimal control part, F i is the control input corresponding to each predicted time point.

5. The integrated optimization method for UAV path planning and control according to claim 4 is characterized in that: In the optimization objective function part of the integrated optimization method described in step 3, the path planning constructed in steps 1 and 2 is combined with the optimal control optimization objective function to obtain the integrated optimization objective function as follows: Among them, N c is the path point prediction time domain in model predictive control, N p is the control input prediction time domain in model predictive control, Q u is the potential field parameter matrix, Q f is the control input parameter matrix, F i is the input quantity controlled at each moment, U i The potential field corresponding to each path point; the objectives of the path planning part and the optimal control part are to minimize the optimization objective function. Minimizing the integrated optimization objective function of path planning and control achieves the minimum control amount and avoids obstacles in the shortest path to reach the target point.

6. The integrated optimization method for UAV path planning and control according to claim 5 is characterized in that: In the control input sequence generation part in the prediction time domain described in step 4, the direction of the control generation sequence of the UAV is initialized to be located in the space containing the target point cut by the plane containing the current position of the UAV perpendicularly cut by the line connecting the UAV and the target point; thus, the control input sequence F in the prediction time domain is obtained = {F1, F2, F3 ... F Np }.

7. The integrated optimization method for UAV path planning and control according to claim 6 is characterized in that: In the drone position loop part described in step 5, the drone position loop is designed as follows: Among them, p is the position of the drone, v is the speed of the drone, g is the acceleration due to gravity, T is the lift of the UAV, R is the rotation matrix of the UAV; the control input F predicted in the design model predictive control is the control input of the UAV in the ground coordinate system, and its relationship with the lift of the UAV is as follows: The path points of the UAV in the prediction time domain are obtained by combining the position loop with the control input in the ground coordinate system; the potential field values ​​corresponding to the path points are obtained by combining the path points with formula (1); In the particle swarm optimization algorithm part, the particle swarm optimization iteration method is designed as follows: Among them, v is the particle running speed, I is the particle inertia coefficient, c1 is the global optimal learning coefficient, c2 is the local optimal learning coefficient, and ξ is the particle state; the optimal control input sequence of the UAV in the ground coordinate system is obtained by the particle swarm optimization algorithm.

8. The integrated optimization method for UAV path planning and control according to claim 7 is characterized in that: The lift T of the drone is obtained by taking the desired posture of the drone and the force part in the drone body coordinate system described in step 6 and obtaining the control input in the drone ground coordinate system. The formula is as follows: At the same time, the drone's desired posture q c Solved by the following formula: q c =[σ c ,q1,q2,q3] (10) in, [q1,q2,q3] T =[-L y ,L x ,0] T / (2||L||σ c ,), where, let Through the known UAV ground coordinate system control input, the corresponding UAV desired posture is directly obtained.

9. The integrated optimization method for UAV path planning and control according to claim 8 is characterized in that: In the construction of attitude loop optimization objective function and solution part described in step 7, the UAV attitude loop model is constructed as follows: Among them, q is the attitude of the drone, and P is calculated by the quaternion of the drone attitude. The formula is as follows: Where ω is the angular velocity of the drone, J I is the UAV inertia coefficient matrix, τ is the UAV attitude loop control input torque; In the process of obtaining the desired posture and tracking the desired posture, the angular velocity of the drone is tracked by applying torque τ to the drone. c ;ω c Calculated by the following formula: The input of the attitude loop control is torque τ, which is applied to the drone to make the drone's angular velocity ω close to the desired angular velocity ω c , and finally achieve the drone attitude angle q = q c , convert the quaternion attitude angle q into θ represented by Euler angles; the optimization objective function to minimize the angular velocity error and attitude angle error of the drone and minimize the control input is as follows: Among them, J a is the attitude loop optimization objective function, k1 is the control input parameter, k2 is the angular velocity error parameter, k3 is the attitude angle error parameter, τ is the control input, ω e is the angular velocity error, ω e =ω c -ω,θ e is the angular velocity error, θ e =θ c -θ; finally, the particle swarm optimization algorithm is introduced again. The difference from the position loop particle swarm optimization algorithm is that the particle state initialization constraint is only set to the input range of the control input τ, and the attitude loop control input τ is optimized and solved; after obtaining the optimal control input τ, the drone lift T and the drone torque τ are applied to the drone to achieve attitude tracking and the effect of drone lift.

Images