An Iterative Optimization and Control Method for UAV Perching Maneuvers

Abstract

The present invention provides an iterative optimization and control method for the perching maneuver of an unmanned aerial vehicle. First, a longitudinal nonlinear dynamic model of the fixed-wing unmanned aerial vehicle's perching maneuver is established. Secondly, in order to obtain the initial feasible trajectory of the ILMPC strategy with the terminal state variables meeting the expected requirements, the constrained iterative linear quadratic regulator (CILQR) algorithm is used to optimize the perching maneuver trajectory, and the optimal trajectory is used as the initial feasible trajectory of the ILMPC strategy. Then, the perching maneuver process of the unmanned aerial vehicle is defined as an iterative task, a sampling safety set and a terminal cost function related to the perching maneuver trajectory are designed, and the perching maneuver ILMPC strategy is designed by using the sampling safety set and the terminal cost function. Finally, the numerical simulation results show that as the number of iterations increases, the iterative cost gradually decreases, and the perching maneuver trajectory gradually converges to the optimal trajectory, thus proving the effectiveness of the designed controller.

Description

Technical Field

[0001] The present invention relates to an iterative optimization and control method for the perching maneuver of an unmanned aerial vehicle (UAV). Background Art

[0002] The landing method of birds has advantages that current artificial aircraft cannot achieve. By observing the landing process of birds, it can be found that when approaching the target, they will spread their wings and tails and adjust the whole body to a large angle of attack, using the air resistance after stall to quickly decelerate. Researchers have proposed the concept of perching maneuver inspired by the maneuvering flight of birds. If a fixed-wing UAV can also achieve rapid landing through the perching maneuver like birds, without the need for a taxiway or other auxiliary landing devices, its application scenarios will be greatly broadened.

[0003] Currently, in the research on perching maneuver control, the control objective is basically to minimize the tracking error, that is, assuming that the reference trajectory is known in advance, a feedback controller is designed to track the reference trajectory. When the reference trajectory is unknown, how to carry out the control design for the perching maneuver system is a problem that needs to be further solved. And the existing research on perching maneuver trajectory optimization does not make full use of historical data to optimize the control performance through iteration. Summary of the Invention

[0004] Object of the Invention: To overcome the deficiencies in the prior art, the present invention provides an iterative optimization and control method for the perching maneuver of a fixed-wing UAV. Through this method, the UAV can perch at a specified position and obtain an optimal perching trajectory that minimizes the cost function under the premise of satisfying state and input constraints. Technical Solution: To achieve the above object, the present invention adopts the following technical solutions:

[0005] Step 1, establish a longitudinal nonlinear dynamics model for the perching maneuver of the UAV;

[0006] Step 2, use a Constrained Iterative Linear Quadratic Regulator (CILQR) to plan state variables such as position, velocity, and pitch angle at the end of the perching maneuver to obtain an optimal trajectory;

[0007] Step 3, use the optimal trajectory obtained in Step 2 as the initial feasible trajectory of the Iterative Learning Model Predictive Control (ILMPC) strategy, and design an iterative learning model predictive controller.

[0008] Preferably, the longitudinal nonlinear dynamics model for the perching maneuver described in Step 1 is:

[0009]

[0010] In the formula, V represents the flight speed of the UAV, θ is the pitch angle, α is the angle of attack, q is the pitch angular velocity, x is the horizontal displacement, h is the altitude, Denotes the derivative of V. m is the mass of the aircraft, g is the local acceleration of gravity, T is the engine thrust, and I y is the pitch moment of inertia of the UAV. M is the aerodynamic moment, and L and D represent the lift and drag forces acting on the UAV, respectively. The expression equations are as follows:

[0011]

[0012] where ρ is the air density, S is the aerodynamic area of the UAV, and C L , C D and C M represent the lift, drag, and moment coefficients of the fixed-wing UAV, respectively. The expression equations are as follows:

[0013]

[0014] where S e represents the surface area of the elevator, l e represents the distance from the aerodynamic center of the elevator to the center of mass of the aircraft, and δ e is the elevator deflection angle.

[0015] Preferably, step 2 includes the following steps:

[0016] Step 2-1, formulate the constrained landing maneuver trajectory planning problem into the following form and denote it as Problem 1:

[0017]

[0018] s.t. x k+1 = f(x k , u k ), k = 0, 1, …, N-1 (4b)

[0019] x0 = x start (4c)

[0020] g uk (u k ) < 0, k = 0, 1, …, N-1 (4d)

[0021] g xk (x k ) < 0, k = 0, 1, …, N (4e)

[0022] where x k and u k in Equation (4a) are the landing maneuver state variables and control input variables at time step k, respectively. N is the number of time steps required for the UAV to complete one landing maneuver, x N is the terminal state quantity, and φ(x N ) is the terminal state stage cost, Lk (x k , u k ) is the stage cost at time step k. argmin represents the independent variable value when the function takes the minimum value. x * and u * are the state quantity sequence and control input sequence when the cost function reaches the minimum value.

[0023] s.t. is an abbreviation for subject to, indicating being subject to.... Equation

[0024] (4b) the f(·, ·) represents the perching maneuver dynamics equation;

[0025] x0 in equation (4c) is the initial state quantity, and x start is the set initial state quantity;

[0026] g in equation (4d) uk (u k ) represents the control input constraint;

[0027] g in equation (4e) xk (x k ) represents the constraint on the state quantity;

[0028] Step 2-2: To handle the constraints in Problem 1, introduce the logarithmic barrier function b(x, u):

[0029]

[0030] where q > 0 is a parameter, and g(x, u) represents the state quantity and control input constraints.

[0031] Step 2-3 specifically includes the following steps:

[0032] Step 2-3-1: Given the initial control sequence of the perching maneuver The initial value of the parameter q is q0 > 0, the proportionality coefficient μ > 1, 0 < α < 1, and forward simulation is used to obtain the initial feasible trajectory

[0033] Step 2-3-2: Use the logarithmic barrier function to transform the inequality constraints (4d) and (4e) into a cost function and add it to the objective function in (4a) to obtain an unconstrained optimization problem;

[0034] Step 2-3-3: Use the iterative LQR algorithm to solve the unconstrained optimization problem obtained in Step 2-3-2, and obtain the control sequence u for the next iteration. The control sequence is a sequence composed of control input variables at all times in one iteration, that is, u = {u1,..., u k ,..., u N-1};

[0035] Step 2-3-4: Use a line search to determine whether the current control sequence \(u\) can make the perching trajectory satisfy the constraint conditions. If it is satisfied, go to Step 2-3-5. If not, let \(u = \alpha u\) until the perching trajectory satisfies the constraint conditions.

[0036] Step 2-3-5: Perform forward simulation to obtain the current state sequence and determine whether the iterative cost converges. If it converges, end the iteration. If not, let \(q=\mu q\), and return to Step 2-3-2.

[0037] Preferably, Step 3 includes the following steps:

[0038] Step 3-1, the goal is to make the UAV perching maneuver trajectory reach the end point \(x\) S from the starting point \(x\) F , and solve the finite-horizon optimal control problem described as follows, denoted as Problem 2:

[0039]

[0040]

[0041]

[0042] \(x_0 = x\) S (6d)

[0043] where, in Equation (6a), represents the optimal cost, \(x\) t and \(u\) t represent the perching maneuver state variable and control input variable at time \(t\) respectively, \(h(x\) t , \(u\) t ) is the stage cost; \(u\) t and \(u\) T do not mean the same thing. Here, \(t\) takes a certain value in the set \(\{0, \ldots, T\}\). When \(t\) takes the value of \(T\), \(u\) t = \(u\) T .

[0044] In Equation (6b), \(f(\cdot, \cdot)\) represents the perching maneuver dynamics equation;

[0045] In Equation (6c), represent the state and control input constraints respectively;

[0046] In Equation (6d), \(x_0\) is the initial state quantity, and \(x\) S is the set initial state quantity.

[0047] Define one iteration as the UAV successfully completing one perching maneuver and storing the closed-loop trajectory of the perching maneuver; in the j-th iteration, the vector x j and u j store the state variables and the corresponding control inputs of the perching maneuver in the j-th iteration respectively:

[0048]

[0049]

[0050] where, and represent the state variables and control inputs of the perching maneuver at time t in the j-th iteration respectively, and t ranges from 0 to T j ; T j represents the time when the closed-loop trajectory of the perching maneuver in the j-th iteration reaches the end point, that is represent the state variables and control inputs at time 0 in the j-th iteration respectively.

[0051] Set that the closed-loop trajectory of the perching maneuver in each iteration starts from the same initial state, that is:

[0052]

[0053] Set that there is a local optimal solution to Problem 2, denoted as:

[0054]

[0055]

[0056] where, x * , u * represent the optimal state variables and the series of control inputs respectively, represent the optimal state variables and control inputs at time t respectively, and the value range of t is 0 to T j . represent the optimal state variables and control inputs at time 0 respectively.

[0057] Step 3-2, design an iterative learning model predictive controller.

[0058] Furthermore, Step 3-2 includes:

[0059] Step 3-2-1, construct a sampling safety set for the perching maneuver trajectory: Define the perching maneuver closed-loop system of the UAV as follows:

[0060]

[0061] where, respectively represent the state variables at the (t + 1)-th moment and the t-th moment of the j-th iteration of the perching maneuver, represents the closed-loop control strategy;

[0062] In the j-th iteration, the sampled safety set is defined as:

[0063]

[0064] where, i ∈ M j , t ∈ [0, T j , M j is the index set that has successfully completed the iteration task in the previous j iterations, and is defined as:

[0065]

[0066] Then the sampled safety set represents the set of perching maneuver state trajectories that have successfully completed the iteration task in the previous j iterations.

[0067] Step 3-2-2, establish the iterative cost function of the perching maneuver trajectory: At the t-th moment of the j-th iteration, the cost function related to the perching maneuver closed-loop trajectory (17) and the input sequence (18) is defined as:

[0068]

[0069] where, and respectively represent the true state variables and input variables of the j-th iteration of the perching maneuver, is the stage cost, and the value of k ranges from t to T j .

[0070] Using the cost function, the value function Q j (x) with respect to the sampled safety set is defined as:

[0071]

[0072] Step 3-2-3, obtain the iterative learning model predictive controller ILMPC:

[0073] At the t-th moment of the j-th iteration, the iterative learning model predictive controller ILMPC strategy will solve the following receding horizon optimal control problem, denoted as Problem 3:

[0074]

[0075]

[0076]

[0077]

[0078]

[0079] Among them, in formula (17a), is the optimal cost, h(x k|t , u k|t ) is the stage cost, and Q j-1 (x t+N|t ) is the value function described in formula (16); formula (17d) means that the terminal state of the perching maneuver in the time domain [t, t + N] is constrained to the perching maneuver sampling safety set .

[0080] Assume that there is an optimal solution to Problem 3 at the t-th moment of the j-th iteration, denoted as:

[0081]

[0082] Among them, respectively represent the optimal state quantity and the series of control inputs on the time period [t, t + N] solved at the t-th moment;

[0083] represents the optimal control input at the k-th moment, where k ranges from t to t + N - 1;

[0084] represents the optimal state quantity at the k-th moment, where k ranges from t to t + N;

[0085] Then, at the t-th moment of the j-th iteration, apply the first element of to the perching maneuver dynamics equation, that is:

[0086]

[0087] Among them, [[ID=5!]] as described in formula (12), represents the closed-loop control strategy. At this time, the state quantity of the perching maneuver at the t + 1 moment can be obtained Then, based on the new state , solve Problem 3 at the t + 1 moment until reaching the end point of this iteration, that is

[0088] Beneficial effects: The present invention regards the perching maneuver of the unmanned aerial vehicle (UAV) as a repeatedly executed task, and uses the Constrained Iterative Linear Quadratic Regulator (CILQR) to plan the state variables such as the position, velocity, and pitch angle at the end of the perching maneuver, providing an initial feasible trajectory for the LMPC control strategy. By utilizing the data-driven ILMPC strategy and using the optimal trajectory obtained by the CILQR algorithm as the initial feasible trajectory of the ILMPC strategy, the problem of trajectory optimization and control of the perching maneuver is studied under the premise of satisfying the state and input constraints. Through this method, the UAV can perch at a specified position and obtain a perching trajectory that optimizes the cost function under the premise of satisfying the state and input constraints. Description of the Drawings

[0089] The following further describes the present invention in detail with reference to the drawings and specific embodiments, and the above and / or other advantages of the present invention will become clearer.

[0090] Figure 1 It is a schematic diagram of the iterative cost of the CILQR algorithm.

[0091] Figure 2a and Figure 2b and Figure 2c and Figure 2d and Figure 2e and Figure 2f are schematic diagrams of the speed change curves of different iteration times of the CILQR algorithm.

[0092] Figure 3a and Figure 3b are schematic diagrams of the control variable change curves of different iteration times of the CILQR algorithm.

[0093] Figure 4a is a schematic diagram of the LMPC prediction trajectory at t = 0 of the second iteration, Figure 4b is a schematic diagram of the LMPC prediction trajectory at t = 1 of the second iteration.

[0094] Figure 5 is a schematic diagram of the change of the iterative cost of the ILMPC strategy with the number of iterations.

[0095] Figure 6a and Figure 6b and Figure 6c and Figure 6d and Figure 6e and Figure 6f are schematic diagrams of the state variable change curves of different iteration times of the LMPC strategy.

[0096] Figure 7a and Figure 7b are schematic diagrams of the control variable change curves of different iteration times of the LMPC strategy.

[0097] Figure 8 This is a schematic diagram of the optimal trajectory obtained by the method of the present invention. DETAILED DESCRIPTION

[0098] The present invention provides an iterative optimization and control method for a UAV landing maneuver, comprising:

[0099] 1. UAV landing maneuver dynamics modeling

[0100] The research object of this invention is a conventional fixed-wing UAV. To simplify the model of the research object, this invention only performs dynamic modeling for the longitudinal motion of the UAV and assumes that the lateral motion, lateral force, and torque of the UAV have no effect on the longitudinal motion of the UAV. The state variables and control inputs are set as follows:

[0101] x=[V,θ,α,q,x,h] T ,u=[T,δ e ] T

[0102] Where: V, θ, α, q represent the flight speed, pitch angle, angle of attack, and pitch velocity of the UAV respectively; x and h represent the horizontal position and vertical height of the UAV respectively; T and δ e They are respectively the thrust generated by the UAV engine and the elevator deflection angle. T represents the transpose of the matrix. The longitudinal motion equation of the fixed-wing UAV is as follows:

[0103]

[0104] Where, represents the derivative of V, m is the mass of the aircraft, g is the local acceleration of gravity, I y is the pitch moment of inertia of the drone; M is the aerodynamic torque, L and D represent the lift and drag on the drone respectively. The expression equations are:

[0105]

[0106] Where: ρ is the air density, S is the aerodynamic area of the drone, C L 、C D and C M They represent the lift, drag and torque coefficients of the fixed-wing UAV respectively, and the expression equations are:

[0107]

[0108] Where S e represents the surface area of the elevator, l e It represents the distance from the aerodynamic center of gravity of the elevator to the center of mass of the aircraft.

[0109] 2. Constrained Iterative LQR Trajectory Optimization Algorithm

[0110] 2.1 Constrained Iterative LQR Algorithm

[0111] The Iterative Linear Quadratic Regulator (ILQR) is an LQR-based algorithm that can solve the unconstrained optimal control problem of nonlinear system dynamics. Its greatest advantage is high time efficiency, but its disadvantage is that it cannot handle constraints. The Constrained Iterative Linear Quadratic Regulator (CILQR) effectively solves the constraint problem in ILQR by adding a barrier function to transform the constraints into additional cost function terms and adding them to the original cost function. Therefore, in this section, for the perching maneuver system subject to state and control input constraints, the CILQR algorithm is used to pre-plan the state variables such as the end position, velocity, and pitch angle of the perching maneuver, and use them as the initial feasible trajectory of the LMPC control strategy. The constrained perching maneuver trajectory planning problem is formulated in the following form and denoted as Problem 1:

[0112] Problem 1 (Constrained Perching Maneuver Trajectory Planning Problem):

[0113]

[0114] s.t. x k+1 = f(x k , u k ), k = 0, 1, …, N - 1 (4b)

[0115] x0 = x start (4c)

[0116] g uk (u k ) < 0, k = 0, 1, …, N - 1 (4d)

[0117] g xk (x k ) < 0, k = 0, 1, …, N (4e)

[0118] Among them, x k and u k in Equation (4a) are the perching maneuver state variables and control input variables at time step k, respectively, N is the number of time steps required for the UAV to complete a perching maneuver, x N is the end state variable, φ(x N ) is the end state stage cost, L k (x k , uk ) is the stage cost at time step k. argmin represents the argument value when the function takes the minimum value. x * and u * are the state quantity sequence and control input sequence when the cost function reaches the minimum value. s.t. is the abbreviation of subject to, indicating being constrained by.... f(·,·) in Equation (4b) represents the perching maneuver dynamics equation. x0 in Equation (4c) is the initial state quantity, and x start is the set initial state quantity. g uk (u k ) represents the control input constraint, and g xk (x k ) represents the state quantity constraint.

[0119] Due to the state constraints and control input constraints in Problem 1, the ILQR algorithm cannot be directly used. The control-constrained differential dynamic programming algorithm adds constraints in (4a), but only for the control input and cannot act on the state constraints. Another way to handle the constraints is to introduce penalties. The basic idea is to transform the constraint functions (4d) and (4e) into barrier functions and add them to the original objective function. The present invention will use the logarithmic barrier function:

[0120]

[0121] In the formula: q>0 is a parameter, and g(x,u) represents the state quantity and control input constraints. The logarithmic function has several good properties. First, it ensures the hard constraints by definition. Second, by increasing q, it will asymptotically converge to the indicator function. And the logarithmic barrier function is twice differentiable, and its derivative is easy to calculate.

[0122] Based on the logarithmic barrier function, this section constructs the main structure of the constrained iterative linear quadratic regulator CILQR algorithm for perching maneuvers. The basic idea is to construct outer loop and inner loop iterations. The outer loop uses the barrier function to transform the constraints into penalties, and the inner loop uses the ILQR algorithm to solve the transformed unconstrained problem. The pseudocode of the CILQR algorithm is shown in Table 1.

[0123] Table 1

[0124]

[0125] The three core components of this algorithm are:

[0126] Outer loop: This algorithm starts from the initial feasible trajectory of the perching maneuver and the initial parameter q (0)Start from \(\mu>0\). In each iteration, first use the barrier function in Equation (5) to transform the constraints (4d) and (4e) into penalties and add them to the objective function in (4a), then pass the transformed problem to the inner loop for solution. After each iteration, the parameter \(q\) is incremented by a scalar \(\mu>0\).

[0127] Inner loop: The inner loop solves the transformed problem introduced by the outer loop. Since it is now an unconstrained problem, the unconstrained ILQR algorithm can be used to solve this problem.

[0128] Line search: During the ILQR calculation process, line search is usually used to ensure convergence. Although the barrier function can guarantee the hard constraints, the constraint conditions may still be violated during the ILQR trajectory update process. To solve this problem, line search is performed iteratively to check whether the updated landing trajectory violates the constraint conditions until it fully satisfies the constraint conditions.

[0129] 2.2 Simulation and Analysis

[0130] First, use the open-loop controller to calculate an initial feasible trajectory for the landing maneuver, define and to represent the state quantity and control input at time step \(k\) in the \(j\)-th iteration of the landing maneuver respectively. Then the initial state of the initial trajectory is The end state is Since during the landing process, it is desired that the speed, pitch angle, and altitude at the end are as small as possible. However, considering some characteristics of the landing maneuver model itself, it may not be possible to reduce the speed, pitch angle, and altitude simultaneously. And the pitch angle of the end state of the initial trajectory is already relatively small. Therefore, the end target is set as The end state stage cost and the stage cost at time step \(k\) are respectively expressed as:

[0131]

[0132]

[0133] Among them, represents the state quantity at time step \(N\) in the \(j\)-th iteration of the landing maneuver, is the set end target, \(Q\) f is the end weight matrix, and \(Q\) and \(R\) represent the weight matrices of the state quantity and the control quantity respectively.

[0134] Since this simulation mainly plans the end - state variables of the perching maneuver, that is, it is hoped that the error between the end point of the perching trajectory and the set target end point in each iteration is as small as possible. Therefore, the optimization index does not limit the state variables, and the weight of the control variables does not need to be too large. Thus, set \(Q = diag[0,0,0,0,0,0]\) and \(R = diag[0.01,0.01]\); during the perching process, more attention is paid to the speed, pitch angle, and altitude at the end point. The weights of the corresponding state variables in the end - point weight matrix should be set relatively large. Set \(Q\) f \(= diag[100,1,0.01,0.01,0.01,100]\) (the \(diag\) function is used to construct a diagonal matrix in FreeMat and Matlab). The constraint conditions of the state variables and control inputs are shown in Table 2.

[0135] Table 2

[0136]

[0137] The condition for the iteration algorithm to end is:

[0138]

[0139] That is, the maximum deviation between the state variables \( in the \(i\) - th iteration and the state variables \( in the \((i - 1)\) - th iteration is less than the tolerance \(\gamma=10\) -8 . At this time, the trajectory in the \(i\) - th iteration is called the optimal trajectory. As Figure 8 shown.

[0140] In the simulation, the sampling time for discretizing the UAV continuous system is set as \(\Delta t = 0.01s\), and the number of time steps \(N = 150\) required for the UAV to complete one perching maneuver. Therefore, the time required to complete one perching maneuver is \(T = 1.5s\).

[0141] The simulation results are as Figure 1 、 Figure 2a 、 Figure 2b 、 Figure 2c 、 Figure 2d 、 Figure 2e 、 Figure 2f 、 Figure 3a 、 Figure 3b shown. Figure 1 The blue curve in it represents the cost of each iteration, and the red dashed line represents the optimal cost. It can be seen that as the number of iterations increases, the cost gradually decreases until it converges to a stable value. The cost function converges to the optimal value at the iteration number \(j = 51\).

[0142] Figure 2a 、 Figure 2b 、 Figure 2c 、 Figure 2d 、Figure 2e 、 Figure 2f The curves representing the state variables for different iteration numbers are shown. The blue dashed line represents the initial feasible trajectory for the perching maneuver, and the red solid line represents the optimal trajectory. As can be seen, under the CILQR controller, as the number of iterations increases, the endpoint values of speed and altitude gradually approach the set target endpoint values, and all state variables satisfy the set constraints. Figure 3a 、 Figure 3b Curves showing the control variables for different iteration numbers. The blue dashed line represents the control variables corresponding to the initial feasible trajectory of the perching maneuver, while the red solid line represents the control variables corresponding to the optimal trajectory. The figure shows that both the thrust and rudder angle satisfy the constraints, demonstrating the effectiveness of the CILQR algorithm.

[0143] 3. Design an Iterative Learning Model Predictive Controller

[0144] In this section, we use the data-driven ILMPC strategy to study the trajectory optimization and control problem of the perching maneuver when the reference trajectory is unknown, while satisfying the state and input constraints. The optimal trajectory obtained by the CILQR algorithm in the previous section is used as the initial feasible trajectory of the ILMPC strategy.

[0145] 3.1 Problem Formulation

[0146] The dynamic model (1) is discretized and simplified into the following form:

[0147] x t+1 =f(x t ,u t ) (9)

[0148] The state constraints and input constraints are:

[0149]

[0150] The goal of this section is to make the UAV landing maneuver trajectory from the starting point x S Arrive at the destination x F , and solve the finite-horizon optimal control problem described below, denoted as Problem 2, as follows:

[0151] Problem 2 (Global Optimal Control Problem in Finite Horizon):

[0152]

[0153]

[0154]

[0155] x0=x S (11d)

[0156] Among them, in formula (11a), represents the optimal cost, x t and u t respectively represent the perching maneuver state variable and the control input variable at time t, h(x t , u t ) is the stage cost. In formula (11b), f(·,·) represents the perching maneuver dynamics equation. In formula (11c), respectively represent the state and control input constraints. In formula (11d), x0 is the initial state quantity, and x S is the set initial state quantity.

[0157] It is set that the stage cost h(·,·) in equation (11a) is continuous and satisfies:

[0158]

[0159] One iteration is defined as the UAV successfully completing one perching maneuver and storing the perching maneuver closed-loop trajectory. In the j-th iteration, the vectors x j and u j respectively store the state quantity and the corresponding control input of the perching maneuver in the j-th iteration:

[0160]

[0161]

[0162] Where: and respectively represent the state quantity and the control input of the perching maneuver at time t in the j-th iteration, and the value range of t is 0 to T j ; T j represents the time when the perching maneuver closed-loop trajectory reaches the end point in the j-th iteration, that is,

[0163] It is set that the perching maneuver closed-loop trajectory starts from the same initial state every time, that is:

[0164]

[0165] It is set that there is a local optimal solution to problem 2, denoted as:

[0166]

[0167]

[0168] Among them, x * , u * respectively represent the optimal state quantity and the series of control inputs, respectively represent the optimal state quantity and control input at time t, where t ranges from 0 to T j .

[0169] 3.2 Controller Design

[0170] This section first introduces how to design the sampling safety set and terminal cost function using historical data; secondly, it presents the design method of the ILMPC controller.

[0171] 3.2.1 Sampling Safety Set

[0172] In this subsection, the iterative property of the control problem will be used to construct the sampling safety set of the perching maneuver trajectory and apply it to the design of the perching maneuver ILMPC controller. Define the closed-loop system of the UAV perching maneuver:

[0173]

[0174] where respectively represent the state quantities at time t + 1 and time t in the j-th iteration of the perching maneuver, represents the closed-loop control strategy, and f(·,·) represents the perching maneuver dynamics equation.

[0175] In the j-th iteration, the sampling safety set is defined as:

[0176]

[0177] In the formula: represents the state quantity at time t in the i-th iteration of the perching maneuver, where i ∈ M j , t ∈ [0, T j , M j is the index set that successfully completed the iterative task in the previous j iterations, and is defined as:

[0178]

[0179] Therefore, the sampling safety set represents the set of perching maneuver state trajectories that successfully completed the iterative task in the previous j iterations.

[0180] 3.2.2 Iterative Cost Function

[0181] This subsection introduces the sampled value function of the perching maneuver and uses it as the terminal cost of the iterative learning model predictive control, so that the iterative performance of the perching maneuver closed-loop system can be gradually improved.

[0182] At time t in the j-th iteration, the cost function associated with the perching maneuver closed-loop trajectory (16) and input sequence (17) is defined as:

[0183]

[0184] In the formula: and respectively represent the true state quantity and input quantity of the j-th iteration of the perching maneuver.,, is the stage cost, and the value of k is from t to T j . Using the cost function, the value function Q j (x) is defined as:

[0185]

[0186] The value function assigns the minimum cost function of the data stored in the perching maneuver sampling safety set to each point in it, that is:

[0187]

[0188] In the formula: is a function of x, taking the minimum value in formula (22), that is:

[0189]

[0190] 3.2.3 ILMPC Controller Design

[0191] In this subsection, an iterative learning model predictive controller (ILMPC) will be designed for the UAV perching maneuver system. The closed-loop trajectory of the perching maneuver system will be stored in each iteration to construct the sampling safety set and the value function, and the sampling safety set and the value function will be used to design the ILMPC controller for the next iteration of the perching maneuver.

[0192] At time t of the j-th iteration, the perching maneuver ILMPC strategy will solve the following receding horizon optimal control problem, denoted as Problem 3.

[0193] Problem 3 (Receding Horizon Local Optimal Control Problem):

[0194]

[0195]

[0196]

[0197]

[0198]

[0199] Among them, in formula (25a), is the optimal cost, h(x k|t , u k|t) is the stage cost, Q j-1 (x t+N|t ) is the value function described by Equation (22); f(·,·) in Equation (25b) represents the perching maneuver dynamics equation; and respectively represent the state and control input constraints; Equation (25d) constrains the terminal state of the perching maneuver in the time domain [t, t+N] to the perching maneuver sampling safety set defined by Equation (19) .

[0200] Assume that there is an optimal solution to Problem 3 at the t-th moment of the j-th iteration, denoted as:

[0201]

[0202] where, respectively represent the optimal state quantity and control input series on the solution time period [t, t+N] solved at the t-th moment, respectively represent the optimal control input at the k-th moment, where k ranges from t to t+N-1, respectively represent the optimal state quantity at the k-th moment, where k ranges from t to t+N.

[0203] Then, at the t-th moment of the j-th iteration, apply the first element of to the perching maneuver dynamics equation, that is:

[0204]

[0205] where, as described by Equation (12), represents the closed-loop control strategy. At this time, the state quantity of the perching maneuver at the t+1 moment can be obtained Then, based on the new state , solve Problem 3 at the t+1 moment until reaching the end point of this iteration, that is

[0206] At the first iteration, set SS j-1 = SS 0 is a non-empty set, the initial perching maneuver trajectory is x 0 ∈ SS 0 , and it can reach the end point x F . Figure 4a , Figure 4b Taking the second iteration as an example, the working principle of the perching maneuver LMPC controller is illustrated. The blue dots in the figure represent the perching maneuver sampling safety set shown in Equation (19), the green stars represent the optimal predicted trajectories of the perching maneuver within N time domains given by the optimal solution of Problem 3. Finally, the red squares represent the perching maneuver closed-loop trajectories. Note that at the moment t = 0, the state of the system is equal to the starting state x S, at the moment \(t = 1\), the state of the system is equal to the first state variable in the predicted trajectory of the perching maneuver at \(t = 0\).

[0207] 4. Design a numerical solution method for the perching maneuver ILMPC

[0208] 4.1 Preprocessing of the perching maneuver model

[0209] Since the perching maneuver nonlinear model (1) is relatively complex, directly using the original model in the solution of Problem 3 will increase the difficulty of the solution, and even the situation where a feasible solution cannot be obtained may occur. To reduce the computational difficulty, the perching maneuver nonlinear model (1) is simplified as follows:

[0210]

[0211] The input of the simplified four-dimensional model is the thrust \(T\) and the pitch angular acceleration \(\eta\), and the state variables are the flight speed \(V\), the track angle \(\mu\), the angle of attack \(\alpha\), and the pitch angular velocity \(q\) of the fixed-wing UAV. Compared with the original six-dimensional model, the horizontal position \(x\) and the vertical height \(h\) are missing. Since these two state variables are only related to the speed \(V\) and the track angle \(\mu\) and have no direct connection with the control input, these two state variables can be made not to participate in the optimization in each iteration. After the optimization is completed, the horizontal position \(x\) and the vertical height \(h\) are directly obtained using the speed \(V\) and the track angle \(\mu\) of the last iteration. Since these two state variables do not participate in the optimization, it may not be possible to ensure that the end position of the perching maneuver trajectory in the last iteration is the same as the end position of the initial trajectory. However, it is possible to make the end position consistent with the end position of the initial trajectory by translating the starting position of the perching maneuver trajectory in the last iteration.

[0212] In addition, for the real UAV perching maneuver system, the control input should also be the thrust \(T\) and the rudder deflection angle \(\delta\) e , and \(\eta\) is only a virtual control input. To make it conform to the physical laws of the actual six-dimensional perching maneuver system, it is necessary to calculate its constraint range. The constraint of \(\eta\) is not a fixed value but changes at each moment, and the value depends on the speed \(V\), the value of the angle of attack \(\alpha\), and the constraint range of the rudder deflection angle \(\delta\) e as shown in Equation (29):

[0213]

[0214] Therefore, the specific algorithm for calculating the \(\eta\) constraint is: let the constraint range of the rudder deflection angle \(\delta\) e be \([-\pi / 3,\pi / 3]\), and at each moment, one thousand points \(X1\) (usually taken as 1000) are taken in the value range \([-\pi / 3,\pi / 3]\) of \(\delta\) e , and then the value of the speed \(V\), the value of the angle of attack \(\alpha\), and the \(X1\) \(\delta\) eSubstitute the value into Equation (29) to calculate X1 values, and take the maximum and minimum values among the X1 values as the upper and lower limits of η respectively, which is the constraint range of η at this moment.

[0215] 4.2 Solution to the optimization problem of perching maneuver

[0216] The solution to Optimization Problem 3 mainly utilizes the Yalmip toolbox. The Yalmip toolbox is a free optimization-solving toolbox based on the Matlab platform. It has its own set of modeling and solving languages for solving optimization problems and integrates many external optimization solvers, including Gurobi, Cplex, Ipopt, etc. When users use it, they do not need to separately learn the usage methods of these external solvers. They only need to learn the modeling and solving statements of Yalmip. As for which external optimization solver to use, it only needs to be specified in the syntax of Yalmip. Yalmip truly realizes the separation of modeling and algorithms, effectively reducing the learning cost of learners. If users do not know which solver is suitable, Yalmip can also automatically detect the type of problem the user is solving and select a suitable solver. The modeling syntax of Yalmip is also very simple, with only four most basic commands:

[0217] (1) Create decision variables

[0218] Yalmip provides three types of decision variables that can be created, namely: real-valued decision variables, integer-valued decision variables, and 0 / 1-valued decision variables. The corresponding statements are sdpvar, intbar, and binvar respectively.

[0219] In this paper, the solution to Optimization Problem 3 requires creating 2 real-valued decision variables: the state variable x and the input variable u. The modeling statements are: x = sdpvar(6,N); u = binvar(2,N). Among them, N is the prediction horizon.

[0220] (2) Add constraint conditions

[0221] The constraint conditions of Yalmip are very intuitive to write: for example, for such a constraint, it can be directly written as: constraints = [x(1)+x(2)+x(3)<=3]. If a new constraint is to be added, such as x2 = 1, it can be written as: constraints = [constraints,x(2)==1].

[0222] In the solution to Optimization Problem 3 of the present invention, 5 constraint conditions need to be created, namely the perching maneuver system dynamics model of Equation (25b), the state quantity and control quantity constraints represented by Equation (25c), the terminal state constraints represented by Equation (25d), and the initial condition constraints represented by Equation (25e).

[0223] (3) Parameter Configuration

[0224] The parameter configuration statement of Yalmip is: opstions = sdpsettings(option1, value1, option2, value2,...). Most of the uses of parameter settings are to set the solver. For example, in the present invention, the solver used is the interior-point nonlinear solver (Ipopt), then the configuration statement is sdpsettings('solver', 'ipopt').

[0225] (4) Solving

[0226] The solving statement of Yalmip is: optimize(constraints, cost, opstions). Among them, constraints are the constraint conditions in step (2), opstions are the parameter configurations in step (3), and cost is the solving objective. The solving objective of optimization problem 3 in this article is shown in Equation (33). After solving, the optimal solutions of the corresponding variables can be extracted through the statements value(x) and value(u).

[0227] 5. Simulation and Analysis

[0228] In order to construct the initial sampling safety set SS 0 and the initial cost function Q 0 , first, it is necessary to use an open-loop controller to calculate an initial feasible trajectory x 0 ∈SS 0 that satisfies the constraints of the state variables and input variables. The specific constraint conditions are shown in Table 1.

[0229] The stage cost in optimization problem 3 is set as shown in Equation (30):

[0230]

[0231] Among them, Q is the weight of the control input. The larger this item is, the smaller the expected control energy is; R represents the weight of the state variables. The larger this item is, the more stable the state variables are expected to be. Since the landing maneuver needs to achieve rapid deceleration through large changes in the state variables, the stage cost does not limit the state variables, and Q = diag[0, 0, 0, 0] is set; in order to minimize the input energy of the whole process, R = diag[1, 1] is set.

[0232] In the simulation, the initial time t0 = 0s of each landing maneuver iteration is set, the initial state is x0 = [13; 0; 0.1770; 0], and the end state is equal to the end point of the initial feasible trajectory. The sampling time for the discretization of the UAV continuous system is set to Δt = 0.01 s, the prediction horizon N = 7, and the number of data points taken in each iteration of the convex local safety set is K = 21. The time T to reach the end point in each iteration j is not fixed and is defined as: The condition for the end of the iterative algorithm is:

[0233]

[0234] that is, the maximum deviation between the state quantity in the i-th iteration and the state quantity in the (i - 1)-th iteration is less than the tolerance γ = 10 -8 At this time, the trajectory of the i-th iteration is called the optimal trajectory.

[0235] The simulation results are as shown in Figure 5 、 Figure 6a 、 Figure 6b 、 Figure 6c 、 Figure 6d 、 Figure 6e 、 Figure 6f 、 Figure 7a 、 Figure 7b . Figure 5 The blue curve represents the cost of each iteration, and the red dashed line represents the optimal cost. It can be seen that as the number of iterations increases, the cost gradually decreases until it converges to a stable value, and the cost function converges to the optimal value at the iteration number j = 35.

[0236] Figure 6a 、 Figure 6b 、 Figure 6c 、 Figure 6d 、 Figure 6e 、 Figure 6f represent the curves of the change of state quantities for different numbers of iterations. Among them, the blue dashed line is the initial feasible trajectory of the perching maneuver, and the red solid line is the optimal trajectory of the perching maneuver at the iteration number j = 35. It can be seen that under the action of LMPC, as the number of iterations increases, the trajectory of the perching maneuver gradually deviates from the initial feasible trajectory and approaches the optimal trajectory. In addition, from Figure 6a 、 Figure 6b 、 Figure 6c 、 Figure 6d 、 Figure 6e 、 Figure 6f it can also be seen that the state quantities of each iteration can satisfy the constraint conditions, and the trajectory of each iteration can reach the end point of the initial trajectory.

[0237] Figure 7a 、 Figure 7bThe control quantity variation curves representing different numbers of iterations are shown. The blue dashed line is the control quantity corresponding to the initial feasible trajectory of the perching maneuver, and the red solid line is the control quantity corresponding to the optimal trajectory. As can be seen from the figure, as the number of iterations increases, the control quantity generally decreases gradually, which also intuitively shows that as the number of iterations increases, the cost gradually decreases (Since the attached drawings in the specification can only be grayscale images, the colors in the figure cannot be seen, hereby explained).

[0238] For the longitudinal motion of the perching maneuver of a fixed-wing unmanned aerial vehicle, the present invention establishes an aerodynamic model and a nonlinear dynamic model of the perching maneuver system. In order to obtain the initial feasible trajectory of the ILMPC strategy with the end state quantity meeting the expected requirements, the CILQR algorithm is used to optimize the perching maneuver trajectory. The simulation results show that as the number of iterations increases, the end values of the speed and altitude gradually approach the set target end values, and all state quantities and control quantities meet the set constraints, indicating the effectiveness of the CILQR algorithm. The present invention designs a data-driven ILMPC strategy for the trajectory optimization and control problem of the perching maneuver of an unmanned aerial vehicle with an unknown reference trajectory. This controller can learn from previous iterations and improve performance. The simulation results show that as the number of iterations increases, the iterative cost gradually decreases, and the perching maneuver trajectory gradually converges to the optimal trajectory, indicating the effectiveness of the designed controller.

[0239] The present invention provides an iterative optimization and control method for the perching maneuver of an unmanned aerial vehicle. There are many methods and ways to specifically implement this technical solution. The above description is only the preferred embodiment of the present invention. It should be noted that for those of ordinary skill in the art, without departing from the principle of the present invention, several improvements and refinements can be made, and these improvements and refinements should also be regarded as the protection scope of the present invention. Each component not clearly defined in this embodiment can be implemented by using existing technologies.

Claims

1. An iterative optimization and control method for the perching maneuver of an unmanned aerial vehicle, characterized in that It includes the following steps: Step 1, establish the longitudinal nonlinear dynamics modeling of the drone landing maneuver; Step 2, use the Constrained Iterative Linear Quadratic Regulator (CILQR) to plan the position, velocity, and pitch angle state variables at the end of the landing maneuver to obtain the optimal trajectory; Step 2 includes the following steps: Step 2-1, formulate the constrained landing maneuver trajectory planning problem into the following form and denote it as Problem 1: s.t.x k+1 = f(x k , u k ), k = 0, 1, …, N-1 (4b) x0 = x start (4c) g uk (u k )<0, k = 0, 1, …, N-1(4d) g xk (x k )<0, k = 0, 1, …, N(4e) Among them, x in formula (4a) k and u k are respectively the perching maneuver state variable and the control input variable at time step k. N is the number of time steps required for the UAV to complete one perching maneuver. x N is the terminal state quantity, φ(x N ) is the terminal state stage cost, L k (x k , u k ) is the stage cost at time step k. argmin represents the value of the independent variable when the function takes the minimum value. x * and u * are the state quantity sequence and the control input sequence when the cost function takes the minimum value; The f(·,·) in Equation (4b) represents the landing maneuver dynamics equation; In formula (4c), x0 is the initial state quantity, and x start is the set initial state quantity; g in Equation (4d) uk (u k ) represents the control input constraint; The g in Equation (4e) xk (x k ) represents the constraint on the state quantity; Step 2-2, in order to handle the constraints in Problem 1, introduce the logarithmic barrier function; Step 2-3, based on the logarithmic barrier function, construct the Constrained Iterative Quadratic Regulator (CILQR) algorithm for the landing maneuver; Step 3, use the optimal trajectory obtained in Step 2 as the initial feasible trajectory of the Iterative Learning Model Predictive Control (ILMPC) strategy and design the iterative learning model predictive controller.

2. The method according to claim 1, wherein The landing maneuver dynamics model described in Step 1 is: where V represents the flight speed of the UAV, θ is the pitch angle, α is the angle of attack, q is the pitch angular velocity, x is the horizontal displacement, h is the altitude, represents the derivative of V; m is the mass of the aircraft, g is the acceleration due to gravity, T is the engine thrust, and I y is the pitch moment of inertia of the UAV; M is the aerodynamic moment, and L and D respectively represent the lift and drag forces acting on the UAV.

3. The method according to claim 2, characterized in that, The expression equations of M, L, and D are respectively: where ρ is the air density, S is the aerodynamic area of the UAV, and C L , C D and C M represent the lift, drag, and moment coefficients of the fixed-wing UAV, respectively.

4. The method according to claim 3, characterized in that, C L 、C D and C M The expression equations are respectively: where S e represents the surface area of the elevator, l e represents the distance from the aerodynamic center of gravity of the elevator to the center of gravity of the aircraft, and δ e is the elevator deflection angle.

5. The method according to claim 4, characterized in that, In Step 2-2, the logarithmic barrier function b(x,u) is: Where q>0 is a parameter, and g(x,u) represents the state variable and control input constraints.

6. The method according to claim 5, wherein Step 2-3 specifically includes the following steps: Step 2-3-1: Given the initial control sequence for the perching maneuver The initial value of the parameter q is q0>0, the proportionality coefficient μ>1, 0<α<1, and the initial feasible trajectory is obtained through forward simulation Step 2-3-2: Use the logarithmic barrier function to transform the inequality constraints (4d) and (4e) into cost functions and add them to the objective function in (4a) to obtain an unconstrained optimization problem; Step 2-3-3: Use the iterative LQR algorithm to solve the unconstrained optimization problem obtained in Step 2-3-2 and obtain the control sequence u for the next iteration; Step 2-3-4: Use line search to determine whether the current control sequence u can make the landing trajectory satisfy the constraint conditions. If it satisfies, proceed to Step 2-3-5. If not, let u = αu until the landing trajectory satisfies the constraint conditions; Step 2-3-5: Forward simulation to obtain the current state sequence And determine whether the iteration cost converges. If it converges, end the iteration. If it does not converge, then let q = μq, and return to Step 2-3-2.

Images