An unmanned aerial vehicle path planning and control integrated optimization method

Abstract

The application discloses a kind of unmanned plane path planning and control integration optimization method, this method is directly optimized control input, simultaneously solve path planning problem and optimal control problem, secondly, unlike the secondary planning method of the past, the particle swarm algorithm is introduced in this paper to solve complex nonlinear model prediction problem, finally, in view of the deficiency of artificial potential field method, the subsequent navigation point is predicted using model predictive control, and the control input is optimized using particle swarm optimization algorithm, which solves the influence of traditional artificial potential field method on path planning and control, and realizes the optimization of unmanned plane path and control input. This method optimizes the path and control input simultaneously, can realize the optimal path planning while the control input required for driving is also the optimal solution, realizes the integration optimization of unmanned plane path planning and control.

Description

TECHNICAL FIELD

[0001] The application designs an unmanned aerial vehicle path planning and control integrated optimization method, which combines artificial potential field method, model predictive control and particle swarm optimization algorithm, and provides an optimization method for considering map layout and obstacle distribution in path planning, and considering control input size and control input effect in control. BACKGROUND

[0002] Nowadays, unmanned aerial vehicles are widely used in environmental monitoring, sports photography, commercial fields and military fields. With the increasing complexity of tasks, the current unmanned aerial vehicles are difficult to meet the increasingly complex task requirements due to the limitations of the physical and algorithm of the unmanned aerial vehicles. Therefore, the demand for effective and efficient path planning and control algorithm of unmanned aerial vehicles is increasingly urgent. The unmanned aerial vehicle system includes a perception system, a planning system, a control system and the like. Among them, the path planning and optimal control of the unmanned aerial vehicle are two key aspects of the planning system and the control system of the unmanned aerial vehicle. Excellent unmanned aerial vehicle path planning can help the unmanned aerial vehicle to better improve the path quality, obstacle avoidance ability and the ability to reach the target point. Optimal control can help the unmanned aerial vehicle to complete better flight tasks with less control, improve the flight efficiency of the unmanned aerial vehicle and reduce the energy consumption of the unmanned aerial vehicle. 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 unmanned aerial vehicle path planning, but it also has the problem of falling into local minimum value. In the traditional artificial potential field method, falling into the local minimum point will cause the attraction and repulsion brought by the artificial potential field of the unmanned aerial vehicle to be equal, and the control input is zero, which leads to that the unmanned aerial vehicle cannot plan effective control input for subsequent flight. However, since the planning method designed by the present application starts from the control input instead of the artificial potential field force, the subsequent path point potential field is predicted by the model predictive control, and although falling into the local minimum point, it will not affect the subsequent planning control of the unmanned aerial vehicle.

[0004] The current mainstream unmanned aerial vehicle path planning and control is a double-layer architecture, that is, the unmanned aerial vehicle system first carries out path planning and optimization to obtain an optimal path for obstacle avoidance and reaching a target point. After obtaining the optimal path, the input is optimized to obtain the minimum input and the minimum tracking error. This double-layer architecture can effectively realize the path planning and tracking of the unmanned aerial vehicle, but also has some shortcomings. In the face of emergencies, the double-layer architecture needs to adjust the path from the path planning layer and then adjust the control quantity from the input planning layer to realize the tracking of the adjusted path. At the same time, since the tracking difficulty is not considered when planning the path, the optimal path may result in a large control input or a large error. Therefore, the adjustment efficiency of the unmanned aerial vehicle under the double-layer architecture is poor, it is difficult to respond to emergencies, and it is not the optimal solution in the global task. In order to solve the delay problem caused by the double-layer path planning-control tracking architecture and the optimal control input problem, an optimization method combining path planning and control is proposed, which takes the path point quality and control input size in the prediction time domain as the optimization target to realize the integrated optimization of unmanned aerial vehicle path planning and control. SUMMARY

[0005] The purpose of the present application is to provide an integrated optimization method that can take path planning and control as optimization targets at the same time. The proposed method can have the characteristics of optimal path and minimum control input.

[0006] The technical solution adopted by the present application is an integrated optimization method for path planning and control of an unmanned aerial vehicle. First, an artificial potential field is constructed according to the task target and obstacle distribution, and the motion state of the unmanned aerial vehicle at future time is considered to construct a multi-objective optimization objective function including path planning and optimal control. Then, the control input sequence in the prediction time domain is initialized, corresponding to the control input quantity of each time node of the model predictive control. After that, the path points in the prediction time domain of the unmanned aerial vehicle are predicted through the control input sequence and the attitude loop model of the unmanned aerial vehicle, 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 sequence is taken as the control input in the ground coordinate system after obtaining the optimal control input sequence. Finally, the lift and expected attitude of the unmanned aerial vehicle 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 unmanned aerial vehicle are applied to the unmanned aerial vehicle to realize the attitude adjustment and flight of the unmanned aerial vehicle.

[0007] The specific steps are as follows:

[0008] Step one Since this method optimizes path planning and control input at the same time, in the aspect of path planning, an artificial potential field is constructed according to the environmental information, the obstacle distribution and the starting point and ending point distribution of the unmanned aerial vehicle are referred to, and the traditional artificial potential field method is introduced to realize the construction of the path planning part of the optimization objective function.

[0009] In step two, the optimization objective function of the optimal control part is constructed considering the size of the control input. In step three, the path planning in step one and the optimal control optimization objective design in step two are combined, and the future state of the UAV is predicted according to the model to obtain an integrated optimization optimization objective function.

[0010] In step four, the control input sequence of the model predictive control in the prediction time domain is generated to obtain the control input part of the optimization objective function.

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

[0012] In step six, the first control sequence is extracted as the force on the UAV in the ground coordinate system under the current state, and the lift and the expected attitude of the UAV are obtained by the force in the ground coordinate system.

[0013] In step seven, the attitude loop control optimization objective function is constructed, and the attitude loop control quantity is optimized by the particle swarm optimization algorithm to realize the tracking of the optimal attitude and the input of the optimal control.

[0014] In step one, the artificial potential field method construction part, the potential field construction method of the traditional artificial potential field method is referred to, p0 is introduced as the maximum influence range of the obstacle, K gra is the artificial potential field attraction parameter, K rep is the artificial potential field repulsion parameter, wherein the attraction potential field U gra and the repulsion potential field U rep are designed as follows:

[0015]

[0016] wherein U gra is the attraction field, U rep is the repulsion 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, the obstacle is considered to have no influence on the UAV, and when the distance is less than p0, the obstacle is considered to have an influence on the UAV, and thus the artificial potential field is designed.

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

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

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

[0020]

[0021] where, N c is the prediction time domain of the path planning part, J u is the optimization 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 in the prediction time domain.

[0022] where, the optimal control part in step two introduces model predictive control, and the control input should be the control input sequence in the prediction time domain of the model predictive control. At the same time, since the optimal control part hopes that the control input is minimum, therefore, in the optimization objective function part, the sum of the control input in the prediction time domain should be minimum, and the optimization objective function of the optimal control part is designed as follows:

[0023]

[0024] where, J f is the optimization objective function of the optimal control part, N p is the prediction time domain 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] where, in the optimization objective function part of the integrated optimization method in step three, the path planning and optimal control optimization objective functions constructed in steps one and two are combined to obtain the integrated optimization objective function as follows:

[0026]

[0027] where, Nc N is the prediction horizon of path points in model predictive control p Q is the prediction horizon of control input in model predictive control u Q is the potential field parameter matrix f F is the control input parameter matrix i U is the control input at each time i Q is the potential field corresponding to each path point

[0028] Due to the path planning part, the optimization objective function can be minimized to achieve obstacle avoidance and reach the target point; the optimal control part needs to minimize the control input in the prediction horizon. Therefore, the goal of the two parts is to minimize the optimization objective function, and the integrated optimization objective function of path planning and control minimization can achieve the minimum control and avoid obstacles on the shortest path to reach the target point.

[0029] In the control input sequence generation part in step four, since the UAV needs to fly to the target point, and the obstacle avoidance should also be performed towards the position closest to the target point to prevent redundant flight operations and unnecessary search in global search, the UAV control generation sequence direction is initialized to be located in the space containing the target point which is vertically cut by the line connecting the UAV and the target point and containing the current position of the UAV. Thus, the control input sequence F = {F1, F2, F3…FQ} in the prediction horizon is obtained. Np}

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

[0031]

[0032] where p is the UAV position, v is the UAV speed, g is the gravity acceleration, T is the UAV lift, and R is the UAV rotation matrix. The designed prediction control input F in the model predictive control is the control input of the UAV in the ground coordinate system, and its relationship with the UAV lift is as follows:

[0033]

[0034] Thus, the path points of the UAV in the prediction horizon can be obtained by combining the position loop and the control input in the ground coordinate system. The potential field value corresponding to the path point is obtained by the path point and formula (1).

[0035] The particle swarm optimization algorithm part is designed as follows:

[0036]

[0037] Where v is the particle velocity, 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 for the UAV in the ground coordinate system can be obtained using the particle swarm optimization algorithm.

[0038] In step six, regarding the desired attitude of the UAV and the forces acting on the UAV in the UAV body coordinate system, after obtaining the control input in the UAV ground coordinate system, the lift force T acting on the UAV can be directly calculated, as shown in the following formula:

[0039]

[0040] Meanwhile, the drone's desired attitude q c The solution can be obtained using the following formula:

[0041] q c =[σ c [ ,q1,q2,q3] (10)

[0042] in, [q1,q2,q3] T =[-L y ,L x ,0] T / (2||L||σ c ,), where, let Using the above formula, the desired attitude of the UAV can be directly obtained from the known UAV ground coordinate system control input.

[0043] In step seven, the objective function for constructing and solving the attitude loop is described, and the UAV attitude loop model is constructed as follows:

[0044]

[0045] Where q represents the UAV attitude, and P is calculated using the UAV attitude quaternion, as shown in the following formula:

[0046]

[0047] Where ω is the angular velocity of the UAV, J I Let τ be the inertial coefficient matrix of the UAV, and τ be the input torque for the UAV attitude loop control.

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

[0049]

[0050] In summary, the attitude loop control input is torque τ, which is applied to the UAV to realize the angular velocity ω of the UAV close to the expected angular velocity ω c Finally, the attitude angle q of the UAV is realized q c Since the above attitude angles are represented by quaternions, it is difficult to accurately represent the attitude angle error, so it is necessary to convert the quaternion attitude angle q to the Euler angle representation θ. In summary, it is desirable to minimize the UAV angular velocity error and the attitude angle error, and the control input is minimized, and the optimization objective function is designed as follows:

[0051]

[0052] Wherein, 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, and the difference between the position loop particle swarm optimization algorithm is that the particle state initialization constraint is only set as 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 UAV lift T and the UAV torque τ are applied to the UAV to realize the tracking of the attitude and the action of the UAV lift.

[0053] The technical breakthrough and performance advantages of the present application are:

[0054] The technical breakthrough of the present method is that firstly, the present method proposes an integrated optimization method for UAV path planning and control, which solves the path planning problem and the optimal control problem by directly optimizing the control input, secondly, unlike the previous quadratic programming method, the particle swarm algorithm is introduced to solve the complex nonlinear model prediction problem, and finally, the present method predicts the subsequent waypoints using model predictive control in view of the shortcomings of the artificial potential field method, and optimizes the control input using the particle swarm optimization algorithm, which solves the influence of the traditional artificial potential field method on path planning and control, and realizes the optimization of the UAV path and the control input.

[0055] In terms of performance advantages, unlike the optimal path difficult to track problem caused by the path planning and tracking control layering of the traditional planning-tracking double-layer architecture, the method simultaneously optimizes the path and control input, can achieve the optimal path planning while the required control input is also the optimal solution, and realizes the integrated optimization of unmanned aerial vehicle path planning and control. The symbols are as follows:

[0056] U gra is the attractive field;

[0057] K gra is the attractive 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 unmanned aerial vehicle and the obstacle;

[0062] p0 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 control input prediction time domain;

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

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

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

[0070] p is the current position of the unmanned aerial vehicle;

[0071] v is the flight speed of the unmanned aerial vehicle;

[0072] g is the gravitational acceleration;

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

[0074] T is the lift of the unmanned aerial vehicle;

[0075] m is a UAV mass;

[0076] R is a UAV rotation matrix;

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

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

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

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

[0081] c1 is a global optimal learning coefficient;

[0082] G best is a global optimal state;

[0083] c2 is a local optimal learning coefficient;

[0084] P best is a local optimal state;

[0085] ξ is a particle state;

[0086] q is a UAV attitude;

[0087] P formula is

[0088] ω is a UAV flight angular velocity;

[0089] J I is a UAV inertia matrix;

[0090] τ is an attitude loop control input torque;

[0091] σ is a first position of a UAV attitude quaternion;

[0092] q is a last three positions of a UAV attitude quaternion;

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

[0094] k1 is a control input parameter;

[0095] k2 is an angular velocity error parameter;

[0096] k3 is an attitude angle error parameter;

[0097] τ is an attitude loop control input;

[0098] ω e is an angular velocity error;

[0099] theta e is an angular velocity error; BRIEF DESCRIPTION OF DRAWINGS

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

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

[0102] Figure 3 is the expected attitude and actual attitude graph and attitude error graph of the UAV;

[0103] Figure 4 is the block diagram of the application. DETAILED DESCRIPTION

[0104] The design method of each part in the application is further described as follows:

[0105] The specific steps of the "unmanned aerial vehicle path planning and control integrated optimization method" of the application are as follows:

[0106] Step one: constructing the optimization objective function of the path planning part

[0107] The path planning and control integrated optimization objective function is divided into two parts, namely the path planning part and the control part, both of which introduce the idea of model prediction, and set N c is the path planning prediction time domain, and the idea of artificial potential field method is referred to to obtain the optimization objective function of the path planning part.

[0108]

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

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

[0111] Step two: constructing the optimization objective function of the optimal control part

[0112] In the control part, the optimal control part needs to realize the minimum control input, so the optimal control part optimization objective function needs to consider the minimum sum of the control input sequence in the prediction time domain. Set N p is the control input prediction time domain, and the control part optimization objective function

[0113]

[0114] where, J f is the optimal control part optimization objective function, N p is the prediction time domain 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] In order to achieve the minimum sum of the control input sequence, it is necessary to minimize the optimization objective function J f .

[0116] Step three: integrated optimization objective function construction

[0117] The optimization objective function of the path planning and control integrated optimization method is constructed by integrating the path planning and the optimal control part optimization objective function in step one and step two as follows:

[0118]

[0119] where, 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 control input at each time, 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 by minimizing the optimization objective function; the optimal control part realizes the minimization of the control input by minimizing the optimization objective function. Therefore, in the path planning and control integrated optimization, it is necessary to minimize the optimization objective function to achieve the task of the shortest path, obstacle avoidance and reaching the target point with the minimum control input.

[0121] Step four: initialization of control input sequence in prediction time domain

[0122] The control input sequence in the prediction time domain is initialized. Since the unmanned aerial vehicle needs to avoid obstacles and also needs to reach the target point, the control input direction should be in the plane with the angle between the unmanned aerial vehicle and the target point less than 90°. By this method, the control input direction in the prediction time domain is initialized, and the control input sequence in the prediction time domain F={F1,F2,F3……F Np} is obtained.

[0123] Step five: obtain path point and corresponding potential field value by combining control sequence with unmanned aerial vehicle position ring

[0124] The predicted position control input in step four is combined with the UAV position loop to obtain the path point in the prediction time domain of the UAV. The path point is combined with the potential field constructed in formula (1) to obtain the potential field of the UAV at the path point. The optimized objective function value of the control sequence corresponding to each particle is obtained, and the particle swarm optimization is realized by designing the following particle swarm optimization iteration formula.

[0125]

[0126] where 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. Through the particle swarm optimization algorithm, the optimal control input sequence of the UAV in the ground coordinate system can be obtained.

[0127] Finally, the optimal control input sequence can be obtained when the particle swarm optimization algorithm reaches the maximum number of iterations or converges. The first position of the optimal control input sequence is applied to the UAV, and the control sequence is moved one position forward, and the last position is randomly generated as the initial value of the particle swarm optimization algorithm at the next time point to ensure the continuity of the optimization algorithm.

[0128] Step six: converting the force in the ground coordinate system into the desired attitude and UAV lift

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

[0130] Step seven: constructing the attitude loop optimization objective function and solving the attitude loop control input

[0131] The attitude loop part realizes the tracking of the desired angular velocity θ by applying torque τ to the UAV, and the angular velocity ω of the UAV 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 to the Euler angle representation θ, so as to construct the optimization objective function J a with the minimum control input and the minimum angular velocity error and attitude angle error, which is shown as follows:

[0134]

[0135] where J aFor the attitude loop optimization objective function, k1 is a control input parameter, k2 is an angular velocity error parameter, k3 is an attitude angle error parameter, tau is a control input, omega e For the angular velocity error, omega e = omega c - omega, theta e For the angular velocity error, theta e = theta c - theta. Finally, as formulas (18) and (19), the particle swarm optimization iteration method is used to perform particle swarm optimization on the attitude loop control input torque tau to find the optimal control input. The position loop control input T and the attitude loop control input tau are applied to the unmanned aerial vehicle to realize the tracking of the attitude loop and the position update of the unmanned aerial vehicle.

[0136] Experimental charts and related data:

[0137] After simulation, the present application has been verified to have reliability and feasibility.

[0138] As shown in Figures 1-3 , the unmanned aerial vehicle path planning, the position loop control input T and the attitude loop control input tau, and the attitude loop tracking effect are compared. Figure 1 The path planning effect of the present method; Figure 2 The position loop control input T and the attitude loop control input tau of the unmanned aerial vehicle; Figure 3 The expected attitude and actual attitude chart and attitude error chart of the unmanned aerial vehicle;

[0139] As shown in Figure 4 , the block diagram of the present application is shown.

Claims

1. An integrated optimization method for UAV path planning and control, characterized in that, First, an artificial potential field is constructed based on the mission objective and obstacle distribution. 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 in the prediction time domain is initialized, corresponding to the control input quantity for each time node predicted by the model. Next, using the control input sequence and the UAV's attitude loop model, path points in the prediction time domain are predicted. The potential field value corresponding to each path point is obtained using the artificial potential field method. The control input sequence is then optimized using a particle swarm optimization algorithm to obtain the optimal control input sequence. The first element of the sequence is used as the control input in the ground coordinate system. Finally, using the control input in the ground coordinate system, the UAV lift and desired attitude are obtained. The torque of the attitude loop control input is optimized using a particle swarm optimization algorithm to obtain the optimal attitude loop control input. The UAV lift and torque are then applied to the UAV to achieve attitude adjustment and flight. Step 1: In terms of path planning, an artificial potential field is constructed based on environmental information. Taking into account the distribution of obstacles and the starting and ending points of the UAV, the artificial potential field method is introduced to construct the objective function for optimizing the path planning part. Step two, regarding optimal control, considering the size of the control input, construct the optimization objective function for the optimal control part; Step 3: Combine the path planning and optimal control optimization objective design from Step 1 and Step 2, and obtain the integrated optimization objective function based on the model's prediction of the UAV's motion state at future moments. Step 4: Generate the control input sequence of the model predictive control in the prediction time domain to obtain the control input part of the optimized objective function; Step 5: Based on the UAV dynamics model, obtain the path points and corresponding potential field values ​​of the UAV in the prediction time domain; optimize the UAV control input sequence using the particle swarm optimization algorithm to obtain the optimal control sequence in the prediction time domain. Step 6: Extract the first character of the control sequence as the force situation of the UAV in the ground coordinate system under the current state, and obtain the lift and desired attitude of the UAV through the force situation in the ground coordinate system. Step 7: Construct the attitude loop control optimization objective function, and optimize the attitude loop control quantity through the particle swarm optimization algorithm to achieve optimal attitude tracking and optimal control input; The artificial potential field construction section described in step one refers to the potential field construction method of the artificial potential field method and introduces... As the maximum area of ​​influence of the obstacle As a gravitational parameter in the artificial potential field method As a repulsive parameter in the artificial potential field method, the gravitational potential field is... With repulsive potential field The design is as follows: （1）； in, For gravitational field, It is a repulsive field; The distance between the drone's current position and the target point. The distance between the drone's current position and each obstacle; when the distance is greater than... When the distance is less than 100 km, it is considered an obstacle and does not affect the drone. At that time, it is considered an obstacle that affects the drone; The total potential field U is obtained using the following formula: （2）； In the gravitational field part, when the distance between the drone and the target point... When the gravitational field decreases, The force will decrease accordingly; regarding the repulsive field, when the distance between the drone and the obstacle... When increased, the repulsive field This will decrease accordingly; the optimization objective function for the path planning part is constructed as follows: （3）； in, For the prediction time domain of the path planning part, Optimize the objective function for the path planning part. Let be the parameter matrix of the potential force component. To predict the potential force corresponding to each path point in the time domain.

2. The integrated optimization method for UAV path planning and control according to claim 1, characterized in that, In the optimal control section described in step two, model predictive control is introduced, and the control input is the control input sequence within the prediction time domain of the model predictive control. Since the optimal control section aims to minimize the control input, the objective function optimization part needs to minimize the sum of the control inputs within the prediction time domain. The objective function optimization part is designed as follows: （4）； in, Optimize the objective function for the optimal control part. To control the prediction time domain of the input part, This is the parameter matrix for the optimal control part. The control input corresponding to each predicted time point.

3. The integrated optimization method for UAV path planning and control according to claim 2, characterized in that, The integrated optimization method described in step three optimizes the objective function by combining the path planning and optimal control optimization objective functions constructed in steps one and two, resulting in the integrated optimization objective function shown below: （5）； in, For path point prediction in model predictive control, in the time domain, For model predictive control, the control input is predicted in the time domain. Here is the potential field parameter matrix. To control the input parameter matrix, The input quantity controlled at each moment. To predict the potential force corresponding to each path point in the time domain, the objectives of both the path planning and optimal control parts are to minimize the optimization objective function. Minimizing the integrated optimization objective function of path planning and control achieves the minimum control quantity and avoids obstacles along the shortest path to reach the target point.

4. The integrated optimization method for UAV path planning and control according to claim 3, characterized in that, In the control input sequence generation part in the prediction time domain described in step four, the direction of the UAV control generation sequence is initialized. This sequence direction lies within the space containing the target point, which is tangent to a plane perpendicular to the line connecting the UAV and the target point and including the UAV's current position. Thus, the control input sequence in the prediction time domain is obtained. .

5. The integrated optimization method for UAV path planning and control according to claim 4, characterized in that, The UAV described in step five is based on a position loop component, which is designed as follows: （6）； Where p is the drone's position, v is the drone's velocity, and g is the acceleration due to gravity. T represents the lift of the UAV, and R represents the rotation matrix of the UAV. The predicted control input F in the 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: （7）； The path points of the UAV in the prediction time domain are obtained by combining the position loop and the control input in the ground coordinate system; the potential field value corresponding to the path points is obtained by combining the path points with formula (1); The particle swarm optimization algorithm section, specifically the iterative method, is designed as follows: （8）； （9）； Where v is the particle's velocity. The particle inertia coefficient, The globally optimal learning coefficients. The learning coefficients are locally optimal. The state is represented by particles; the optimal control input sequence of the UAV in the ground coordinate system is obtained by particle swarm optimization algorithm.

6. The integrated optimization method for UAV path planning and control according to claim 5, characterized in that, The desired attitude of the UAV and the forces acting on the UAV in the UAV body coordinate system, as described in step six, are used to calculate the lift force T on the UAV after obtaining the control input in the UAV ground coordinate system. The formula is as follows: ； Meanwhile, the drone's desired attitude Solve using the following formula: ； in, , Among them, let By using the known UAV ground coordinate system control input, the corresponding desired UAV attitude can be directly obtained.

7. The integrated optimization method for UAV path planning and control according to claim 6, characterized in that, The UAV attitude loop model is constructed as follows, based on the objective function for attitude loop control optimization described in step seven: ； Where q represents the UAV attitude, and P is calculated using the UAV attitude quaternion, as shown in the following formula: ； in, For the angular velocity of the drone, The inertial coefficient matrix of the UAV. Input torque for the attitude loop control of the UAV; During the process of obtaining and tracking the desired attitude, torque is applied to the UAV. Achieving UAV angular velocity tracking of desired angular velocity ; Calculated using the following formula: ； The attitude loop control input is torque. Applying torque to the drone to achieve its angular velocity Approaching the desired angular velocity Ultimately, the drone's attitude angle q = Convert the quaternion attitude angle q to Euler angles. The optimization objective function for minimizing the angular velocity error and attitude angle error of the UAV while minimizing the control input is shown below: in, Optimize the objective function for the attitude loop. To control the input parameters, For angular velocity error parameters, For attitude angle error parameters, To control the input, For angular velocity error, , For angular velocity error, Finally, the particle swarm optimization algorithm is introduced again. The difference between this algorithm and the position loop particle swarm optimization algorithm is that the particle state initialization constraint is only set as the control input. The input range for attitude loop control input Perform optimization to obtain the optimal control input. Then, lift the drone With drone torque It is applied to drones to achieve attitude tracking and provide lift for the drone.

Images