Quadrotor trajectory tracking control method with agile disturbance identification and online learning capability

Abstract

This invention discloses a quadrotor trajectory tracking method with agile disturbance identification and online learning capabilities, belonging to the field of quadrotor trajectory tracking control. First, an unknown system dynamics estimator is designed to accurately estimate and compensate for the uncertainties of the quadrotor, establishing a feedforward control law based on disturbance estimation. Second, an adaptive dynamic programming strategy is used to transform the aforementioned trajectory tracking into an error optimal adjustment problem. By constructing a performance index function that includes error and energy consumption, the HJB optimal control equation is derived. An adaptive learning law containing an implicit expression of weighted error is designed to approximate the analytical solution of the HJB equation online, constructing an optimal learning compensation control law. Finally, the optimal learning compensation control law is combined with the feedforward control law to calculate the quadrotor control input, total lift, roll angle, etc., driving the quadrotor kinematics to achieve the desired center-of-mass motion. This invention saves airborne resources, and under disturbance conditions, the neural weights can quickly converge to the true value and control optimality.

Description

Technical Field

[0001] This invention relates to the field of quadrotor trajectory tracking control, specifically a quadrotor trajectory tracking control method with agile interference identification and online learning capabilities. Background Technology

[0002] The quadcopter, a type of multi-rotor aircraft, features vertical takeoff and landing, hovering, and agile maneuvering, making it widely used in military, civilian, and scientific research fields, such as rescue, transportation, and hunting. Its study has attracted considerable interest from researchers. However, the quadcopter is a typical nonlinear multiple-input multiple-output system, susceptible to external disturbances and parameter uncertainties. Therefore, designing a high-performance trajectory tracking controller to achieve stable flight and precise control of the quadcopter is a challenging problem in engineering practice. Furthermore, due to limited onboard energy, the design of a high-performance trajectory tracking control system must simultaneously consider control cost and tracking performance.

[0003] Currently, quadrotor trajectory tracking control faces two major challenges: First, previous research primarily focused on quadrotor trajectory tracking performance under disturbance conditions, neglecting energy consumption, which is unacceptable for quadrotor systems with limited onboard resources. Second, traditional adaptive dynamic programming methods mostly employ an execution-evaluation dual-network structure, which suffers from excessive computational burden in updating neural network weights. Furthermore, the adaptive learning methods commonly utilize gradient descent to achieve online approximation of the optimal control law, making it difficult to ensure convergence of neural weights to the true value and control optimality under disturbance conditions. Therefore, designing an optimal trajectory tracking controller for quadrotors under complex environmental disturbances and limited onboard resources is a pressing and challenging issue. Summary of the Invention

[0004] To address the challenge of designing an optimal trajectory tracking controller for a quadrotor under complex environmental interference and limited airborne resources, and to enable the quadrotor to achieve high-precision tracking according to a given trajectory reference command, this invention provides a quadrotor trajectory tracking control method with agile interference identification and online learning capabilities.

[0005] This invention is achieved through the following technical solution: a quadrotor trajectory tracking control method with agile interference identification and online learning capabilities, comprising the following steps:

[0006] S1: Based on the quadrotor trajectory loop model, and with the help of low-pass filtering and algebraic calculations, an unknown system dynamics estimator (USDEs) is designed to accurately estimate and compensate for the uncertainties of the quadrotor. Based on the estimation results provided by USDEs, a feedforward control law based on disturbance estimation is established to achieve accurate tracking of time-varying trajectory signals, as detailed below:

[0007] First, the quadrotor trajectory loop model is established as follows:

[0008]

[0009] in:

[0010]

[0011] Where x = [p T ,v T ] T Represents the state vector of the quadrotor, p = [p x ,p y ,p z ] T and v = [v x ,v y ,v z ] T These are the position vector and linear velocity vector in the inertial coordinate system, respectively; u = R B U = [u x ,u y ,u z ] T Represents the quadcopter control input, where R B It is a rotation matrix:

[0012]

[0013] In the formula, θ d ,ψ d These represent the roll angle, pitch angle, and yaw angle of the quadrotor around the x-axis, respectively, in the body coordinate system; U = [0, 0, u F / m] T As the input force vector, u F The total lift generated by the four rotors is m, which is the mass of the quadcopter; g is the acceleration due to gravity in the vector G; f(x,t)=d(x) / m+Δ(t) / m represents the lumped disturbance, where d(x) and Δ(t) represent parameter perturbation and unknown wind disturbance, respectively;

[0014] Secondly, to handle unknown lumped interference in the quadrotor system, based on the USDEs principle, a low-pass filter is applied to the measurable state and input of the quadrotor:

[0015]

[0016] In the formula x s and u s Let x and u represent the filtering states respectively; κ represents the filter coefficients to be designed; based on the invariant manifold principle, the lumped interference estimate can be obtained:

[0017]

[0018] in, This represents the estimated value of f; to ensure that the quadcopter has fast and accurate trajectory tracking performance and disturbance resistance under the influence of disturbances, a vector e = xx is defined, which includes the trajectory and speed control deviation vectors. d x d If the desired state vector is given, then the following feedforward control law based on disturbance agile identification is constructed:

[0019]

[0020] In the formula u d It is a constructed feedforward control law. Let k denote the generalized inverse of matrix B, and k = diag(k x ,k y ,k z ) is the control gain matrix;

[0021] S2: To enable the quadrotor to learn online and coordinate tracking performance with control consumption, the trajectory tracking in S1 is transformed into an error-optimal adjustment problem using an adaptive dynamic programming (ADP) strategy. By constructing a performance index function that includes error and energy consumption, and based on the optimal control principle, the Hamilton-Jacobi-Bellman (HJB) optimal control equation is derived. An adaptive learning law containing an implicit expression of weighted error is designed. A single evaluation neural network is used to approximate the analytical solution of the HJB equation online, constructing a learning compensation control law that ensures optimal overall performance, as detailed below:

[0022] First, construct the optimal performance index function and the HJB equation based on the error dynamics equation:

[0023]

[0024] In the formula V * (e) is the optimal performance index function, Ψ represents the admissible set of control inputs, and Q and R represent positive definite matrices. It is the HJB equation. Equation (7) is a typical nonlinear partial differential equation, involving complex dynamics and model nonlinearity, making it difficult to obtain its exact analytical solution; therefore, an analytical solution for evaluating the online approximation of the HJB equation by a neural network is introduced:

[0025]

[0026] Where V(e) is an approximate performance index function, φ represents the estimated value of the weight vector of the evaluation neural network; φ represents the regression vector; definition and Let be the partial derivative of φ; an auxiliary matrix N and a vector S are introduced to implement the filtering operation, as shown below:

[0027]

[0028] In the formula, c > 0 is the forgetting factor. The adaptive learning law, which includes the implicit expression of weight error, is designed as follows:

[0029]

[0030] In the formula, Γ is a positive definite diagonal matrix. Constructing an optimal learning compensation control law u that can improve overall performance e :

[0031]

[0032] S3: Combine the optimal learning compensation control law with the feedforward control law to calculate the quadrotor control input, total lift, roll angle, and roll angle, and drive the quadrotor kinematics to achieve the desired center of mass motion:

[0033]

[0034] Compared with existing technologies, this invention has the following advantages: The quadrotor trajectory tracking control method provided by this invention, with agile interference identification and online learning capabilities, compared with publicly reported disturbance rejection control, achieves intelligent tracking and continuous optimization under the optimal comprehensive performance of error and consumption by introducing an online learning and evaluation mechanism, thus overcoming the shortcomings of existing methods in coordinating accuracy and consumption. The proposed strategy is based on a lightweight shallow neural network structure, resulting in low computational overhead. Furthermore, the proposed online learning mechanism has clear interpretability, fundamentally different from the deep reinforcement learning paradigm of offline training / online application. This invention significantly saves airborne resources while optimizing the trajectory tracking control for a given path; moreover, it only involves a single evaluation neural network to approximate the analytical solution of the HJB equation online, saving a large amount of computational resources compared to the execution-evaluation dual-network structure, reducing the complexity of network design and training, improving system efficiency and performance, and allowing the neural weights to quickly converge to the true value and control optimality under disturbance conditions. Attached Figure Description

[0035] Figure 1 This is a schematic diagram of the quadcopter model of the present invention.

[0036] Figure 2 This is a block diagram of the quadrotor trajectory tracking control structure proposed in this invention.

[0037] Figure 3 This is a schematic diagram of the quadcopter trajectory tracking proposed in this invention.

[0038] Figure 4 This is a schematic diagram comparing the estimated value of lumped disturbance with the actual value proposed in this invention using USDE.

[0039] Figure 5 and Figure 6 This is a schematic diagram comparing the position tracking error and velocity tracking error of the single-network structure proposed in this invention with existing methods.

[0040] Figure 7 and Figure 8 This is a schematic diagram comparing the single-network structure proposed in this invention with the neural network weight vectors of existing methods.

[0041] Figure 9 This is a comparison diagram showing the evolution of the performance index function between the single-network structure proposed in this invention and existing methods. Detailed Implementation

[0042] The present invention will be further described below with reference to specific embodiments.

[0043] A quadrotor trajectory tracking control method with agile interference identification and online learning capabilities, such as... Figure 1 As shown, it includes the following steps:

[0044] S1: Based on the quadrotor trajectory loop model, and with the help of low-pass filtering and algebraic calculations, an unknown system dynamics estimator (USDEs) is designed to accurately estimate and compensate for the uncertainties of the quadrotor. Based on the estimation results provided by USDEs, a feedforward control law based on disturbance estimation is established to achieve accurate tracking of time-varying trajectory signals, as detailed below:

[0045] First, the quadrotor trajectory loop model is established as follows:

[0046]

[0047] in:

[0048]

[0049] Where x = [p T ,v T ] T Represents the state vector of the quadrotor, p = [p x ,p y ,p z ] T and v = [v x ,v y ,v z ] T These are the position vector and linear velocity vector in the inertial coordinate system, respectively; u = R B U = [ux ,u y ,u z ] T Represents the quadcopter control input, where R B It is a rotation matrix:

[0050]

[0051] In the formula, θ d ,ψ d These represent the roll angle, pitch angle, and yaw angle of the quadrotor around the x-axis, respectively, in the body coordinate system; U = [0, 0, u F / m] T As the input force vector, u F The total lift generated by the four rotors is m, which is the mass of the quadcopter; g is the acceleration due to gravity in the vector G; f(x,t)=d(x) / m+Δ(t) / m represents the lumped disturbance, where d(x) and Δ(t) represent parameter perturbation and unknown wind disturbance, respectively;

[0052] Secondly, to handle unknown lumped interference in the quadrotor system, based on the USDEs principle, a low-pass filter is applied to the measurable state and input of the quadrotor:

[0053]

[0054] In the formula x s and u s Let x and u represent the filtering states respectively; κ represents the filter coefficients to be designed; based on the invariant manifold principle, the lumped interference estimate can be obtained:

[0055]

[0056] in, This represents the estimated value of f; to ensure that the quadcopter has fast and accurate trajectory tracking performance and disturbance resistance under the influence of disturbances, a vector e = xx is defined, which includes the trajectory and speed control deviation vectors. d x d If the desired state vector is given, then the following feedforward control law based on disturbance agile identification is constructed:

[0057]

[0058] In the formula u d It is a constructed feedforward control law. Let k denote the generalized inverse of matrix B, and k = diag(k x ,k y ,k z ) is the control gain matrix;

[0059] S2: To enable the quadrotor to learn online and coordinate tracking performance with control consumption, the trajectory tracking in S1 is transformed into an error-optimal adjustment problem using an adaptive dynamic programming (ADP) strategy. By constructing a performance index function that includes error and energy consumption, and based on the optimal control principle, the Hamilton-Jacobi-Bellman (HJB) optimal control equation is derived. An adaptive learning law containing an implicit expression of weighted error is designed. A single evaluation neural network is used to approximate the analytical solution of the HJB equation online, constructing a learning compensation control law that ensures optimal overall performance, as detailed below:

[0060] First, construct the optimal performance index function and the HJB equation based on the error dynamics equation:

[0061]

[0062] In the formula V * (e) is the optimal performance index function, Ψ represents the admissible set of control inputs, and Q and R represent positive definite matrices. It is the HJB equation. Equation (7) is a typical nonlinear partial differential equation, involving complex dynamics and model nonlinearity, making it difficult to obtain its exact analytical solution; therefore, an analytical solution for evaluating the online approximation of the HJB equation by a neural network is introduced:

[0063]

[0064] Where V(e) is an approximate performance index function, φ represents the estimated value of the weight vector of the evaluation neural network; φ represents the regression vector; definition and Let be the partial derivative of φ; an auxiliary matrix N and a vector S are introduced to implement the filtering operation, as shown below:

[0065]

[0066] In the formula, c > 0 is the forgetting factor. The adaptive learning law, which includes the implicit expression of weight error, is designed as follows:

[0067]

[0068] In the formula, Γ is a positive definite diagonal matrix. Constructing an optimal learning compensation control law u that can improve overall performance e :

[0069]

[0070] S3: Combine the optimal learning compensation control law with the feedforward control law to calculate the quadrotor control input, total lift, roll angle, and roll angle, and drive the quadrotor kinematics to achieve the desired center of mass motion:

[0071]

[0072] In the following comparative simulations, the non-optimized structure and the execution-evaluation dual-network structure proposed in the literature Vamvoudakis KG, Lewis FL. Online actor-critic algorithm to solve the continuous-time infinitehorizon optimal control problem[J]. Automatica, 2010, 46(5): 878–888 will be compared.

[0073] Figure 3-9 The results are from the simulation. Figure 3 This is a schematic diagram of the quadcopter trajectory tracking proposed in this invention. It can perfectly track the set trajectory reference command even when the initial position tracking error is non-zero. Figure 4 This diagram illustrates the comparison between the estimated and actual values ​​of lumped interference using the USDE proposed in this invention. As can be seen from the diagram, USDE can accurately estimate lumped interference, which provides an important foundation for establishing a feedforward control law based on interference estimation to achieve accurate tracking of time-varying trajectory signals. Figure 5 and Figure 6 This diagram illustrates the comparison of position tracking error and velocity tracking error between the single-net structure and the dual-net structure proposed in this invention. As can be seen from the diagram, both the single-net structure and the dual-net structure can ensure that the position and velocity tracking errors converge to near the origin, but the optimization effect of the single-net structure is better than that of the dual-net structure. Figure 7 This is a schematic diagram of the optimal learning compensation control law and the evaluation neural network weight vector of the single-network structure proposed in this invention. Figure 8 This is a schematic diagram of the evaluation and execution neural network weight vectors in a dual-network structure. Figure 7 and Figure 8 The comparison shows that the performance index function of the single-network structure is minimized, and the optimal learning compensation control law converges to zero. More importantly, the comparison of neural network weight vectors shows that the transient tracking performance of the single-network structure is faster than that of the dual-network structure. This is because the single-network structure implicitly expresses the weight error, and the proposed adaptive learning law enables the estimated value of the neural network weight vector to converge quickly. In contrast, the weight update law of the dual-network structure uses gradient descent or least squares methods, which exhibit significant lag in weight convergence under uncertainties. Figure 9The graph shows the evolution of the performance index function between the single-network structure proposed in this invention and existing methods. It can be seen that the single-network structure proposed in this invention has online optimization capabilities in terms of tracking performance and control consumption compared with existing methods.

[0074] The scope of protection claimed by this invention is not limited to the specific embodiments described above. Moreover, for those skilled in the art, this invention can have various modifications and alterations. Any modifications, improvements, and equivalent substitutions made within the concept and principles of this invention should be included within the scope of protection of this invention.

Claims

1. A quadrotor trajectory tracking control method with agile interference identification and online learning capabilities, characterized in that: Includes the following steps: S1: Based on the quadrotor trajectory loop model, and with the help of low-pass filtering and algebraic calculations, an unknown system dynamics estimator (USDEs) is designed to accurately estimate and compensate for the uncertainties of the quadrotor. Based on the estimation results provided by USDEs, a feedforward control law based on disturbance estimation is established to achieve accurate tracking of time-varying trajectory signals, as detailed below: First, the quadrotor trajectory loop model is established as follows: （1） in: （2） in, This represents the quadrotor's state vector. and These are the position vector and linear velocity vector in the inertial coordinate system, respectively. Represents the control inputs of the quadcopter, where It is a rotation matrix: （3） In the formula, , , These represent the quadcopter's orbital motion in the body coordinate system. Roll angle of axis rotation, around Pitch angle of axis rotation, rotation around Yaw angle of axis rotation; For input force vector, The total lift generated by the four rotors, Mass of the quadcopter; vector middle It is gravitational acceleration; Indicates lumped interference. and These represent parameter perturbation and unknown wind disturbance, respectively. Secondly, to handle unknown lumped interference in the quadrotor system, based on the USDEs principle, a low-pass filter is applied to the measurable state and input of the quadrotor: （4） In the formula and They represent and The filtering state; Let represent the filter coefficients to be designed; based on the invariant manifold principle, the lumped interference estimate can be obtained: （5） in, express The estimated value; defined to include the trajectory and speed control deviation vector. , If the desired state vector is given, then the following feedforward control law based on disturbance agile identification is constructed: （6） In the formula It is a constructed feedforward control law. Representation matrix The generalized inverse, It is the control gain matrix; S2: To enable quadrotor online learning to coordinate tracking performance and control consumption, the trajectory tracking in S1 is transformed into an error-optimal adjustment problem using an adaptive dynamic programming (ADP) strategy. By constructing a performance index function that includes error and energy consumption, and based on the optimal control principle, the HJB optimal control equation is derived. An adaptive learning law containing implicit weighted error expressions is designed. A single evaluation neural network is used to approximate the analytical solution of the HJB equation online, constructing a learning compensation control law that ensures optimal overall performance, as detailed below: First, construct the optimal performance index function and the HJB equation based on the error dynamics equation: （7） In the formula It is the optimal performance index function. This represents the set of allowed control inputs. Describes a positive definite matrix. It is the HJB equation. , Equation (7) is a typical nonlinear partial differential equation. An analytical solution for evaluating the online approximation of the HJB equation by a neural network is introduced: （8） in This is an approximate performance index function. This represents the estimated value of the weight vector of the evaluation neural network; Represents the regression vector; definition and , for The partial derivatives; introduce an auxiliary matrix. and a vector To perform the filtering operation, see the following: （9） In the formula The forgetting factor is used to design an adaptive learning law that implicitly includes the weight error: （10） In the formula It is a positive definite diagonal matrix. Construct an optimal learning compensation control law that can improve overall performance. : （11） S3: Combine the optimal learning compensation control law with the feedforward control law to calculate the quadrotor control input, total lift, roll angle, and roll angle, and drive the quadrotor kinematics to achieve the desired center of mass motion: （12）。

Images