CN114967718A - Four-rotor-wing trajectory tracking optimal control method under control limitation - Google Patents
Four-rotor-wing trajectory tracking optimal control method under control limitation Download PDFInfo
- Publication number
- CN114967718A CN114967718A CN202210486067.5A CN202210486067A CN114967718A CN 114967718 A CN114967718 A CN 114967718A CN 202210486067 A CN202210486067 A CN 202210486067A CN 114967718 A CN114967718 A CN 114967718A
- Authority
- CN
- China
- Prior art keywords
- control
- rotor
- attitude
- tracking
- aerial vehicle
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- G05D1/08—Control of attitude, i.e. control of roll, pitch, or yaw
- G05D1/0808—Control of attitude, i.e. control of roll, pitch, or yaw specially adapted for aircraft
- G05D1/0816—Control of attitude, i.e. control of roll, pitch, or yaw specially adapted for aircraft to ensure stability
- G05D1/0825—Control of attitude, i.e. control of roll, pitch, or yaw specially adapted for aircraft to ensure stability using mathematical models
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- G05D1/10—Simultaneous control of position or course in three dimensions
- G05D1/101—Simultaneous control of position or course in three dimensions specially adapted for aircraft
- G05D1/106—Change initiated in response to external conditions, e.g. avoidance of elevated terrain or of no-fly zones
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Abstract
The invention discloses a four-rotor track tracking optimal control method under control limitation, wherein a four-rotor unmanned aerial vehicle position and attitude dynamic model based on total disturbance and virtual control quantity is decoupled by adopting a position ring and an attitude ring, an error system model based on a four-rotor position and attitude system of a full-drive system is established, and active disturbance rejection parametric tracking controllers of the position ring and the attitude ring are respectively designed through a full-drive system parametric control method, so that the positions and attitudes of two error systems gradually converge to a reference instruction. On the premise of considering control input limitation, the constraint transcription method is adopted to process control input constraint, the problem of parameter selection optimization of the controller is established, relevant gradients are deduced, the gradient method is adopted to optimize and set parameters, and the transient performance of trajectory tracking of the quad-rotor unmanned aerial vehicle is further improved.
Description
Technical Field
The invention relates to the technical field of unmanned aerial vehicle control, in particular to a four-rotor-wing trajectory tracking optimal control method under the control limitation.
Background
In recent years, a quad-rotor Unmanned Aerial Vehicle (UAV) has advantages of low cost, high flexibility, vertical take-off and landing, small size and the like, and is widely applied to military, agriculture and life aspects such as aerial photography, search and rescue, reconnaissance and the like. And because the quad-rotor unmanned aerial vehicle is an under-actuated nonlinear system with strong coupling of position and attitude, the design of the controller is difficult to a certain degree, and the influences of limited input of the actuator, system coupling and the like need to be fully considered, so that the design of the controller capable of accurately controlling the attitude and the track of the quad-rotor unmanned aerial vehicle is very critical in consideration of the limited control input.
The prior art has some more or less shortcomings, and the technical problems are as follows: although the existing four-rotor trajectory tracking control method is relatively complete, most of the existing nonlinear control methods cause that a four-rotor closed-loop system is also a strong nonlinear system, and the saturation constraint of control input is difficult to be considered at the same time.
Reference documents:
mileage four-rotor aircraft control and implementation [ D ]. Dalian university of graduate 2015.
The four-rotor unmanned aerial vehicle anti-interference attitude control system based on the improved ADRC is designed [ J ] electro-optic and control, 2020, 27(12):78-83.
G Duan.High-Order Fully Actuated System Approaches:Part I.Models and Basic Procedure[J].International Journal of Systems Science,2021,52(2):422–435.
G Duan.Quasi-Linear System Approaches for Aerocraft Control–Part1:An Overview and Problems[J].Journal of Astronautics,2020,41(6):633–646.
G Duan.Quasi-Linear System Approaches for Aerocraft Control–Part2:Methods and Prospects[J].Journal of Astronautics,2020,41(7):839–849.
K.L.Teo,B.Li,C.Yu,and V.Rehbock.Applied And Computational Optimal Control[M].Springer Optimization and Its Applications,2021。
Disclosure of Invention
Aiming at the problems in the prior art, the invention provides a four-rotor-wing track tracking optimal control method under the control limitation, which adopts a constraint transcription method to process the control input constraint under the limited state, constructs a controller parameter selection optimization problem for an error model of a four-rotor-wing position ring and an attitude ring based on full-drive system parametric control, and adopts a gradient method to optimize and set the parameters, thereby improving the tracking performance.
In order to achieve the purpose, the technical scheme adopted by the invention is as follows:
a four-rotor trajectory tracking optimal control method under control limitation comprises the following steps:
s10, establishing a four-rotor unmanned aerial vehicle position and attitude dynamic model based on total disturbance and virtual control quantity;
s20, converting a four-rotor unmanned aerial vehicle position and attitude dynamics model into a four-rotor attitude error system and a four-rotor position error system based on a full-drive system by adopting a position ring and attitude ring decoupling control frame;
s30, acquiring each order reference signal of the quad-rotor unmanned aerial vehicle by adopting a tracking differentiator with certain anti-noise and differential smoothing functions, expanding a total disturbance item into a state quantity by an expanded state observer, and observing and estimating to realize the observation and estimation of an unknown time-varying parameter item in a quad-rotor attitude error system and a quad-rotor position error system;
s40, designing corresponding active disturbance rejection parameterized tracking controllers aiming at a four-rotor attitude error system and a four-rotor position error system respectively, and converting the controllers into equivalent closed-loop first-order linear steady error systems to ensure that a four-rotor unmanned aerial vehicle position and attitude dynamic model is globally and gradually stabilized based on a reference signal;
s50, establishing an optimal parameter setting optimization problem and nonlinear state inequality constraints thereof based on the active disturbance rejection parameterized tracking controller under the condition of considering control input limitation;
s60, approximating the non-linear state inequality by constrained transcriptionThe beam processing is a single-dimensional state integral inequality constraint, so that the optimal parameter setting optimization problem established in the step S50 is converted into a solvable optimization problem P r ；
S70, solving the optimization problem P by adopting a gradient method r And optimizing and setting the active disturbance rejection parameterized tracking controller to realize the optimal control of the trajectory tracking of the quad-rotor unmanned aerial vehicle.
Compared with the prior art, the invention has the following beneficial effects:
in the invention, for a four-rotor unmanned aerial vehicle control system with system internal and external disturbance, error systems are respectively constructed for a decoupled position ring and an attitude ring, and a full-drive parameterized optimal trajectory tracking controller is designed based on a full-drive system theory, so that the two systems are respectively equivalent to a closed-loop first-order linear constant system, and the trajectory and the attitude of the four rotors are stably converged to a reference instruction; and in consideration of the problem of limited control input in practice, the optimization and setting problems of the controller parameters in the position and attitude system are further optimized and solved by combining a gradient method, so that the transient performance of the four-rotor tracking is further improved. The invention has the advantages of ingenious design, novel conception, convenient realization, good control stability and excellent track tracking performance, and is suitable for being applied to the control of a four-rotor unmanned aerial vehicle.
Drawings
Fig. 1 is a schematic view of an overall control structure of a quad-rotor unmanned aerial vehicle according to an embodiment of the present invention.
Fig. 2 is a schematic diagram of a control structure of an attitude ring system according to an embodiment of the present invention.
Fig. 3 is a comparison of the four-rotor trajectory tracking effect under active disturbance rejection control in an embodiment of the present invention.
FIG. 4 is a schematic diagram of the optimized controller inputs in an embodiment of the present invention.
Detailed Description
The present invention is further illustrated by the following figures and examples, which include, but are not limited to, the following examples.
Examples
As shown in fig. 1 to 2, the four-rotor trajectory tracking optimal control method under the control limitation includes the following steps:
s10, establishing a four-rotor unmanned aerial vehicle position and attitude dynamics model based on total disturbance and virtual control quantity;
s20, converting a four-rotor unmanned aerial vehicle position and attitude dynamics model into a four-rotor attitude error system and a four-rotor position error system based on a full-drive system by adopting a position ring and attitude ring decoupling control frame;
s30, acquiring each order reference signal of the quad-rotor unmanned aerial vehicle by adopting a tracking differentiator with certain anti-noise and differential smoothing functions, expanding a total disturbance item into a state quantity by an expanded state observer, and observing and estimating to realize the observation and estimation of an unknown time-varying parameter item in a quad-rotor attitude error system and a quad-rotor position error system;
s40, designing corresponding active disturbance rejection parametric tracking controllers aiming at a four-rotor attitude error system and a four-rotor position error system respectively, and converting the active disturbance rejection parametric tracking controllers into equivalent closed-loop first-order linear steady error systems to enable the position and attitude dynamics model of the four-rotor unmanned aerial vehicle to be globally and gradually stabilized based on a reference signal;
s50, establishing an optimal parameter setting optimization problem and nonlinear state inequality constraints thereof based on the active disturbance rejection parameterized tracking controller under the condition of considering control input limitation;
s60, processing the nonlinear state inequality constraint into a one-dimensional state integral inequality constraint through a constraint transcription method, so that the optimal parameter setting optimization problem established in the step S50 is converted into a solvable optimization problem P r ；
S70, solving the optimization problem P by adopting a gradient method r And optimizing and setting the active disturbance rejection parameterized tracking controller to realize the optimal control of the trajectory tracking of the quad-rotor unmanned aerial vehicle.
Specifically, the invention is based on the following reasonable settings to quickly establish a four-rotor aircraft dynamics model without any irregularity: the four rotors are rigid bodies and the quality is kept unchanged; the gravity acceleration does not change along with the change of the height; the four-rotor unmanned aerial vehicle flies at a low angle and a low speed, and air friction is ignored; the pulling force generated by each motor propeller of the aircraft is approximately in direct proportion to the rotating speed of each blade.
The established four-rotor unmanned plane position and attitude dynamics model is as follows:
wherein, (x, y, z) is the position coordinate of the four-rotor ground coordinate system, (phi, theta, psi) is the Euler angle of the four-rotor, which is the rolling angle, the pitch angle and the yaw angle respectively, (J) x ,J y ,J z ) For four rotor unmanned aerial vehicle around each axial inertia of organism, m is four rotor unmanned aerial vehicle's quality, g is the acceleration of gravity constant, D i And i is x, y, z, phi, theta and psi, and is the total disturbance of each state model caused by inaccurate modeling of each state quantity dynamic equation of the quad-rotor unmanned aerial vehicle and external disturbance, U i And i is a virtual control variable defined as follows, 1,2,3,4
In the formula, k l Lift coefficient of quadrotor unmanned aerial vehicle, arm length between body center of quadrotor unmanned aerial vehicle and motor, and k ψ Motor reaction torque coefficient, omega, for rotor yaw direction i And i is the rotating speed of four propellers of the quadrotor unmanned plane 1,2,3 and 4.
The quad-rotor unmanned aerial vehicle in the formula (2) is a typical under-actuated and strong pose coupling system, so a position ring and attitude ring decoupling control frame is adopted to further process a position kinetic model in an unmanned aerial vehicle kinetic model into a position kinetic model
In the formula, F x ,F y ,F z A virtual control quantity is assumed for an equivalent position loop,
through the further processing of the position kinetic equation of the quad-rotor unmanned aerial vehicle, the controllers are respectively designed for the position system and the attitude system of the quad-rotor unmanned aerial vehicle.
Designing a tracking controller for the model formula (3) of the processed position loop system, and obtaining a control output and a virtual control quantity U 1 And a reference attitude angle phi to be tracked by a subsequent attitude ring controller c 、θ c 、ψ c The relationship between is
For position and attitude variables of quad-rotor unmanned aerial vehicle, order
(x c ,y c ,z c ),(φ c ,θ c ,ψ c ) Respectively, the position and attitude reference signals, the error form of the position and attitude system of the quad-rotor unmanned aerial vehicle is respectively
For a four-rotor attitude error system, a state vector x is defined r Parameter vector xi r Control vector u r Are respectively as
And defining the following coefficient matrixes of the system
A 2r ＝I 3 ,A 0r ＝0 3 ,σ r (ξ r )＝σ 1r (ξ r )-D r ……(10)
The quad-rotor attitude error system can be expressed as a second order pseudo-linear system as follows
For the above system, the control quantity and the state quantity have the same dimension and satisfy
The system for obtaining the position error of the four rotors is a full-drive system
In the formula, the state vector x t Parameter vector xi t Control vector u t The system coefficient matrix is as follows:
unknown time-varying term xi in error system for position and attitude t 、ξ r The first half part is a first-order differential signal and a second-order differential signal of the reference signal, and in order to process the unknown part, a tracking differentiator with certain noise resistance and differential smoothing functions is adopted to obtain the reference signal of each order.
Taking the attitude loop as an example, the following form of nonlinear tracking differentiator is selected
Wherein s ═ phi c ,θ c ,ψ c ] T As a time-varying parameter item xi r Of the respective reference signal, s 1 ＝[φ d ,θ d ,ψ d ] T ,
Tracking signals, f, being reference signal s and its derivative, respectively han (x 1 ,x 2 ,r,h 0 ) The fastest synthesis function that enables time-optimal convergence is as follows,
in the formula, x 1 、x 2 Are function input variables, r, h 0 Respectively the speed tracked by the control signal and the parameters of the smoothing.
The tracking differentiator of the position loop can be obtained by the same method.
In this way, the first and second derivatives of the reference signals of the position loop and the attitude loop can be smoothly approximated by the tracking differentiator.
Next, the total disturbance term D is calculated for the remaining positions in the time-varying parameter term i And i is estimated as x, y, z, phi, theta, psi, and an extended state observer is introduced to expand the total disturbance term into a state quantity to observe the estimation.
By disturbance D in the attitude ring φ For example, the corresponding extended state observer adopts a multi-input linear extended state observer expression as
In the formula, symbol
Represents an estimate of an arbitrary variable a, ∈ φ E (0,1) is an observer parameter,
for input into the system equation, k, for other attitude angle observations φ1 ,k φ2 ,k φ3 Is greater than 0 and satisfies the following matrix Hurwitz matrix
For other disturbance quantities D of the total disturbance term i Perturbation observations of i ═ x, y, z, θ, ψ are similarly available.
By the extended state observer and the tracking differentiator corresponding to each disturbance quantity, the time-varying parameter term can be approximated by the following equation (22):
based on the established four-rotor attitude error system and position error system, and the observation estimation of unknown time-varying parameter items in the system through a tracking differentiator and an extended state observer, the design of the active disturbance rejection parameterized tracking controller is provided in the embodiment. The design of the active disturbance rejection parameterized tracking controller taking the attitude loop as an example is given, and the active disturbance rejection parameterized tracking controller of the position loop can be obtained by the same way.
Theorem 1: parameterized control law of four-rotor attitude error system
When the above control law is applied to the attitude error system equation (13), there is an equivalent system as follows
Similarly, the parameterized control law of the four-rotor position error system is
When the above formula control law is applied to the position error system formula (15), there is an equivalent system as follows
The auto-disturbance-rejection parameterized tracking controller is designed by the equations (24) and (26)
The proof process of theorem 1 above is as follows:
first, a definition of a full drive system is given.
Consider a system in pseudo-linear form with a second order matrix as follows:
wherein x ∈ R n ,u∈R n Respectively, the state vector and the control vector of the system, xi (t) is belonged to R p For being in a certain tight set omega ∈ R p A system parameter vector of 2 ,A 1 ,A 0 ,B∈R n×n ,σ∈R n The relative distance between the two electrodes is relative to xi, x,
at least the continuous coefficient matrixes of the system are segmented. For the system equation (28), if satisfied
The system is a full drive system.
Based on the full-drive system, the parameterization control method of the full-drive system is given by the following processes:
for the second-order all-drive system (29), the following control law is designed:
u＝u σ +u c ……(30)
in the formula (I), the compound is shown in the specification,
compensation term for the system control law, u c Law of state feedback control in the form of proportional derivative
In the formula (I), the compound is shown in the specification,
two time-varying state feedback matrices:
wherein Z ∈ R n×2n ,F∈R 2n×2n Is a constant parameter matrix. If and only if Z, F ∈ H, H is defined as set
When the control law of the second-order all-wheel-drive system is designed as an equation (30), the global asymptotic stability of the second-order all-wheel-drive system (29) can be realized, and the equivalent first-order closed-loop system is a linear steady system as follows:
it is easy to know that the above-mentioned four rotor position error system formula (15) and four rotor attitude error system formula (13) of establishing are the second order all-wheel-drive system, directly according to the respective parameterization control law formula (25) and formula (23) of two systems in above-mentioned theorem 1, will position and two systems of gesture all be equivalent to linear constant system formula (26) and formula (24), namely two systems are the global gradual stabilization respectively, namely formula (27) holds, and four rotor unmanned aerial vehicle's position error and attitude error will all converge to zero. And (5) finishing the certification.
Further consider the position orbit of four rotor unmanned aerial vehicle and the tracking dynamic performance and the controller saturation of attitude system. Taking an auto-disturbance rejection parameterized tracking controller of an attitude ring as an example, an optimal parameter optimization problem of the controller is established, nonlinear state inequality constraints which are difficult to process are processed by a constraint transcription method, and parameter matrixes F and Z are optimized based on a gradient method. The auto-disturbance rejection parameterized tracking controller parameters of the position loop can be optimized in the same way.
Because the control law parameters in the auto-disturbance rejection parameterized tracking controller are the matrix F and the matrix Z, the matrix optimization problem is difficult to process and the two matrices need to meet det [ Z ZF] T Constraint not equal to 0 and it is necessary to satisfy the matrix F as Hurwithe tz matrix is selected, so that the matrix optimization problem is simplified into a parameter vector optimization problem, namely, the Hurwitz matrix F is selected to be in a fixed diagonal standard form, and the matrix Z is set to be in a form of a sub-unit matrix
F＝diag(λ 1 ,λ 2 ,λ 3 ,λ 4 ,λ 5 ,λ 6 ),Z＝[p 1 I 3 p 2 I 3 ]……(35)
In the formula, λ i Less than 0, i is 1,2, 6 is a diagonal element of the matrix F, namely a closed-loop characteristic value corresponding to an equivalent first-order constant error system of the quad-rotor unmanned aerial vehicle; p is a radical of 1 ＞0,p 2 ＞0。
By setting the matrix F and the matrix Z to the form of equation (35), the constraint det [ Z ZF ] is known easily] T Not equal to 0 naturally satisfies. Thus, the optimization problem for the selection of the parameter matrices F and Z is transformed to a vector λ ═ λ 1 ,λ 2 ,...,λ 6 ] T Sum vector p ═ p 1 ,p 2 ] T Is an optimization problem of decision variables.
By substituting the formula (35) into the control law of the formula (23), the control law can be further obtained
In the formula, the state feedback matrices in the form of proportional differentiation are respectively:
it can be seen that by setting the matrix F and the matrix Z to a particular form, the control law will be independent of the matrix Z, and therefore only the parameter vector λ in the matrix F needs to be ═ λ 1 ,λ 2 ,...,λ 6 ] T And (6) optimizing.
For improving the attitude tracking performance, we consider the optimization parameter vector λ ═ λ 1 ,λ 2 ,...,λ 6 ] T To reduce accumulated attitude tracking errors, and to select an optimization problem objective function as follows:
after the parameter optimization target of the active disturbance rejection parameterized tracking controller is determined, the problem of limited control input is further considered. Because four rotor unmanned aerial vehicle's motor rotational speed is limited to virtual control input u that can provide r Is also limited due to u r Being a non-linear expression on parameters and states, the controller parameter tuning problem can therefore be modeled as a parameter vector optimization problem with inequality state constraints as follows (40).
As described above, the state nonlinear constraint in the controller parameter tuning optimization problem is difficult to handle, which results in difficulty in gradient derivation on one hand, and on the other hand, when the numerical method iterative optimization is performed, an inequality constraint exists at each time point t, which results in a nonlinear inequality constraint of almost infinite dimension. Therefore, the control input constraint (actually, the state nonlinear inequality constraint) in the above problem is processed by a constraint transcription method, and is processed into a single-dimensional state integral inequality constraint.
Rewrite the control input saturation constraint to the form:
wherein χ > 0, γ > 0,
g 1 ＝U 2 (t)-U 2min ≥0,g 2 ＝-U 2 (t)+U 2max ≥0
g 3 ＝U 3 (t)-U 3min ≥0,g 4 ＝-U 3 (t)+U 3max ≥0……(43)。
g 5 ＝U 4 (t)-U 4min ≥0,g 6 ＝-U 4 (t)+U 4max ≥0
for the transformed constraint and the original state inequality constraint, it is easy to know that χ > 0 and γ > 0 are equivalent, and when the two parameters are not equal to zero, it is equivalent to performing a certain degree of relaxation on the original constraint, and the gradient derivation of the transformed constraint with respect to the parameter vector is available. The two parameters can be initially set to be smaller positive numbers so as to increase the constraint feasibility of the optimization algorithm iteration initiation, then the two constraint parameters are gradually reduced for re-iteration, the approximation of the original constraint is gradually realized, and the optimal solution is converged to the optimal solution of the original problem.
After the state inequality constraint is processed, the original controller parameter setting optimization problem is converted into a problem P as shown in the following formula (44) r ：
The controller parameter optimization of a general four-rotor attitude system is considered, so that the optimization problem P can be set by the controller parameter r The intermediate objective function and the gradient formula of the nonlinear state constraint relative to the parameter vector lambda are deduced, so that the optimization problem P is solved based on the gradient method r 。
Theorem 2: for optimization problem P r The gradient of the objective function with respect to the parameter vector λ is
In the formula (I), the compound is shown in the specification,
λ 2 ＝[λ 4 λ 5 λ 6 ] T
ζ 0 is solved by the following differential equation
Theorem 3: each constraint G i 1,2, 6 has a gradient of parameter vector λ of
Wherein the equation is obtained by solving the following differential equation
The proof process of theorem 2 is as follows:
the following system is given:
in the formula (I), the compound is shown in the specification,
are state variables and parameter vectors of the system.
Parameter optimization problem of the system to find the optimal parameter vector k minimizes the following objective function
And satisfies inequality constraints and equality constraints as follows
The gradient of the above objective function and constraint on the parameter vector k can be obtained by the following procedure:
for G i N, whose gradient with respect to the parameter vector k is 0,1
In the formula, H i (t,x(t),k,ζ i (t))＝L i (t,x(t),k)+ζ i (t) T f (t, x (t), k) … … (54) is the corresponding Hamiltonian, ζ i (t) is a covariate obtained by solving the following covariate differential equation
For the optimization problem P in theorem 2 r Based on the above process, a Hamiltonian of the objective function relative to the parameter vector λ is defined as
The terminal item in the objective function is zero, and the system initial value state is independent of the parameters, so that
Further solving for H 0 Respectively relative to state quantity X r Partial derivatives of sum parameter vector lambda
And substituting the expressions (56) to (58) into the expressions (53) to (55) to obtain the gradient of the objective function relative to the parameter vector in theorem 2, thus finishing the certification. Theorem 3 proves that the process is similar to the theorem.
The invention also carries out simulation verification on the track tracking full-drive parameterization optimal control method under the control limitation. The simulation environment is Matlab R2019a/Simulink, the discrete step size is 0.001s, and the settings of all parameters of the quad-rotor unmanned aerial vehicle are shown in the following table 1.
| Parameter(s) | Value taking |
| m | 3.245kg |
| l | 0.21m |
| J x | 0.17337kg·m 2 |
| J y | 0.16105kg·m 2 |
| J z | 0.34320kg·m 2 |
| k l | 9.138e -6 N/(rad/s) 2 |
| k ψ | 1.368e -7 N/(rad/s) 2 |
Table 1 quad-rotor unmanned aerial vehicle parameter settings
The simulation scene sets the initial position and the attitude of the quad-rotor unmanned aerial vehicle to be (x) 0 ,y 0 ,z 0 )＝(0,0,0)m，(φ 0 ,θ 0 ,ψ 0 ) (0,0,0) rad, initial velocity and angular velocity set to
The upper limit of the virtual control quantity is set as U 1 ∈[0,50.75]N，U 2,3 ∈[-5.75,5.75]Nm，U 3 ∈[-0.75,0.75]And N is added. The parameters of the tracking differentiators of the position ring and the attitude ring are set as follows: r is 500, h 0 ＝0.02,
The parameters of the extended state observer of the attitude ring are set as follows:
∈ i ＝0.01,(k i,1 ,k i,2 ,k i,3 )＝(15,3,1.3),i＝φ,θ,ψ
the parameters of the extended state observer of the position loop are set as:
∈ j ＝0.01,(k j,1 ,k j,2 ,k j,3 )＝(10,2,1),j＝x,y,z
setting the control input range to U 1 ∈[0,50.75]N，U 2,3 ∈[-5.75,5.75]Nm，U 3 ∈[-0.75,0.75]N，
The total disturbance of the four rotor positions and the four attitude channels is set as follows:
D x ＝sin(0.2t)+sin(0.3t),D y ＝sin(0.3t),D z ＝2sin(0.2t)
set up cylinder spiral curve and refer as four rotor unmanned aerial vehicle's orbit, the position instruction is with the driftage instruction:
x d (t)＝0.5cos(0.5t),y d (t)＝0.5sin(0.5t),z d (t)＝2+0.1t,ψ d ＝30°。
the auto-disturbance rejection parameterized tracking controller provided by the invention is compared with the conventional auto-disturbance rejection control, the settings of a related tracking differentiator and an extended state observer in the auto-disturbance rejection control are the same as those of the controller provided by the invention, and each control law parameter in the auto-disturbance rejection controller is optimized by the proposed gradient method. The simulation effect is shown in fig. 3, and it can be seen that the control method provided by the invention has higher tracking control precision compared with the conventional active disturbance rejection control method, the obtained tracking curve is almost overlapped with the reference curve, the conventional active disturbance rejection control has obvious tracking error in the x-axis direction, and the tracking overshoot amount in the initial tracking stage is larger. As shown in fig. 4, the controller parameter optimization method proposed by the present invention can make the control input satisfy the set constraint range. In conclusion, the optimal trajectory tracking controller provided by the invention has a better effect, and can realize high-performance trajectory tracking control of the quad-rotor unmanned aerial vehicle.
The above embodiments are only preferred embodiments of the present invention, and are not intended to limit the scope of the present invention, but all changes that can be made by applying the principles of the present invention and performing non-inventive work on the basis of the principles shall fall within the scope of the present invention.
Claims (10)
1. A four-rotor trajectory tracking optimal control method under control limitation is characterized by comprising the following steps:
s10, establishing a four-rotor unmanned aerial vehicle position and attitude dynamics model based on total disturbance and virtual control quantity;
s20, converting a four-rotor unmanned aerial vehicle position and attitude dynamics model into a four-rotor attitude error system and a four-rotor position error system based on a full-drive system by adopting a position ring and attitude ring decoupling control frame;
s30, acquiring each order reference signal of the quad-rotor unmanned aerial vehicle by adopting a tracking differentiator with certain anti-noise and differential smoothing functions, expanding a total disturbance item into a state quantity by an expanded state observer, and observing and estimating to realize the observation and estimation of an unknown time-varying parameter item in a quad-rotor attitude error system and a quad-rotor position error system;
s40, designing corresponding active disturbance rejection parametric tracking controllers aiming at a four-rotor attitude error system and a four-rotor position error system respectively, and converting the active disturbance rejection parametric tracking controllers into equivalent closed-loop first-order linear steady error systems to enable the position and attitude dynamics model of the four-rotor unmanned aerial vehicle to be globally and gradually stabilized based on a reference signal;
s50, establishing an optimal parameter setting optimization problem and nonlinear state inequality constraints thereof based on the active disturbance rejection parameterized tracking controller under the condition of considering control input limitation;
s60, processing the nonlinear state inequality constraint into a one-dimensional state integral inequality constraint through a constraint transcription method, so that the optimal parameter setting optimization problem established in the step S50 is converted into a solvable optimization problem P r ；
S70, solving the optimization problem P by adopting a gradient method r And optimizing and setting the active disturbance rejection parameterized tracking controller to realize the optimal control of the trajectory tracking of the quad-rotor unmanned aerial vehicle.
2. The method for controlling the optimal trajectory tracking of the quadrotors under the limited control according to claim 1, wherein the dynamics model of the position and the attitude of the quadrotor drone in the step S10 is as follows:
wherein, (x, y, z) is the position coordinate of the four-rotor ground coordinate system, (phi, theta, psi) is the Euler angle of the four-rotor, which is the rolling angle, the pitch angle and the yaw angle respectively, (J) x ,J y ,J z ) For four rotor unmanned aerial vehicle around each axial inertia of organism, m is four rotor unmanned aerial vehicle's quality, g is the acceleration of gravity constant, D i X, y, z, phi, theta, psi is the total disturbance of the quad-rotor drone, U i And i is a virtual control variable defined as follows, 1,2,3,4
In the formula, k l For four rotor unmanned aerial vehicle's lift coefficient, l is the arm length between four rotor unmanned aerial vehicle organism center and motor, k ψ Motor reaction torque coefficient, omega, for rotor yaw direction i And i is the rotating speed of four propellers of the quad-rotor unmanned aerial vehicle 1,2,3 and 4.
3. The method for controlling the trajectory tracking of the quadrotors under the control limitation according to claim 2, wherein the step S20 is performed by using a frame for controlling the decoupling of the position ring and the attitude ring to obtain a position error system and an attitude error system of the quadrotors respectively
Wherein, F x ,F y ,F z Is an equivalent position ring virtual control quantity, (x) c ,y c ,z c ),(φ c ,θ c ,ψ c ) Respectively a position and an attitude reference signal,
4. the method for controlling the trajectory tracking of the quadrotors under the control limitation according to claim 3, wherein in the step S20, the quadrotor attitude error system based on the all-wheel-drive system is:
for the above system, the control quantity and the state quantity have the same dimension and satisfy
In the formula, x r ＝[φ e ,θ e ,ψ e ] T ,
u r ＝[U 2 ,U 3 ,U 4 ] T Respectively a state vector, a parameter vector, a control vector, A 2r ＝I 3 ,A 0r ＝0 3 ,σ r (ξ r )＝σ 1r (ξ r )-D r ,
σ 1r (ξ r ),D r ,B r Is a system coefficient matrix;
the four-rotor position error system based on the full-drive system comprises:
in the formula, x t ＝[x e ,y e ,z e ] T ,
u t ＝[F x ,F y ,F z ] T Respectively, state vector, parameter directionQuantity, control vector, σ t (ξ t )＝σ 1t (ξ t )-D t ，σ 1t (ξ t ),D t ,B t Is a system coefficient matrix.
5. The method for controlling the trajectory tracking of the quadrotor under the control limitation according to claim 4, wherein in the step S30, the tracking differentiator of the attitude ring is:
wherein s is [ [ phi ] ] c ,θ c ,ψ c ] T As a time-varying parameter term xi r Of the respective reference signal, s 1 ＝[φ d ,θ d ,ψ d ] T ,
Tracking signals, f, being reference signal s and its derivative, respectively han (x 1 ,x 2 ,r,h 0 ) For the fastest synthesis function, x, enabling time-optimal convergence 1 、x 2 Are function input variables, r, h 0 Speed and smooth parameters tracked by the control signal respectively;
the tracking differentiator of the position loop can be obtained by the same method;
disturbance quantity D φ The corresponding expression of the extended state observer is:
in the formula, symbol
Represents an estimate of an arbitrary variable a, ∈ φ Epsilon (0,1) is an observer parameter,
for input into the system equation, k, for other attitude angle observations φ1 ,k φ2 ,k φ3 Greater than 0 and satisfying the Hurwitz matrix;
other disturbance quantities D i Perturbation observation of i ═ x, y, z, θ, ψ is similarly available;
through the extended state observer and the tracking differentiator corresponding to each disturbance quantity, the time-varying parameter term is approximated by the following formula (22):
6. the method for controlling the trajectory tracking of the quadrotor under the control limitation according to claim 5, wherein in the step S40, the parameterized control law of the auto-disturbance rejection parameterized tracking controller on the quadrotor attitude error system is as follows:
in the formula, Z r 、F r For the parameter matrix to be optimized,
applying the above formula control law to formula (13) results in the following equivalent closed-loop first-order linear steady attitude error system
The parameterized control law of the active disturbance rejection parameterized tracking controller on the four-rotor position error system is as follows:
by applying the control law of the above equation to equation (15), there is an equivalent closed-loop first-order linear steady-state position error system
From equations (24) and (26), the auto-disturbance rejection parameterized tracking controller causes
7. The control-limited quadrotor trajectory tracking optimal control method according to claim 6, wherein in the step S50, the matrix F is set to a fixed diagonal standard form, and the matrix Z is set to a form of a sub-formula of an identity matrix:
F＝diag(λ 1 ,λ 2 ,λ 3 ,λ 4 ,λ 5 ,λ 6 ),Z＝[p 1 I 3 p 2 I 3 ]……(35)
in the formula, λ i < 0, i ═ 1, 2., 6 are diagonal elements of the matrix F, p 1 ＞0,p 2 Is greater than 0; then only the parameter vector λ in the matrix F needs to be made ═ λ 1 ,λ 2 ,...,λ 6 ] T Optimizing;
modeling an active disturbance rejection parameterized tracking controller of an attitude ring as a parameter vector setting optimization problem with inequality state constraints:
where K1 and K2 are state feedback matrices in proportional derivative form;
the same rationale for modeling the auto-disturbance rejection parameterized tracking controller for the position loop is available.
8. The method for controlling the trajectory tracking optimization of the quadrotors under the control limitation according to claim 7, wherein the step S60 is implemented by processing the nonlinear state inequality constraint into a one-dimensional state integral inequality constraint through a constraint transcription method by:
rewrite the control input saturation constraint to the form:
wherein χ > 0, γ > 0,
optimization problem P of transformation r Comprises the following steps:
9. the control-limited quadrotor trajectory tracking optimal control method according to claim 8, wherein the optimization problem P is solved by a gradient method in step S70 r The gradient of the objective function with respect to the parameter vector λ is
In the formula (I), the compound is shown in the specification,
ζ 0 is solved by the following differential equation
10. The method for controlling the trajectory tracking of the quadrotor under the control limitation according to claim 9, wherein the optimization problem P is solved by a gradient method in step S70 r Each constraint G i I-1, 2.., 6 has a gradient of a parameter vector λ
In the formula, the following differential equation is solved
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210486067.5A CN114967718B (en) | 2022-05-06 | 2022-05-06 | Four-rotor-wing trajectory tracking optimal control method under control limitation |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210486067.5A CN114967718B (en) | 2022-05-06 | 2022-05-06 | Four-rotor-wing trajectory tracking optimal control method under control limitation |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114967718A true CN114967718A (en) | 2022-08-30 |
| CN114967718B CN114967718B (en) | 2023-03-28 |
Family
ID=82982126
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210486067.5A Active CN114967718B (en) | 2022-05-06 | 2022-05-06 | Four-rotor-wing trajectory tracking optimal control method under control limitation |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114967718B (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117192982A (en) * | 2023-08-28 | 2023-12-08 | 四川大学 | Control parameterization-based short-distance air combat maneuver decision optimization method |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20120283850A1 (en) * | 2002-04-18 | 2012-11-08 | Cleveland State University | Extended active disturbance rejection controller |
| CN104865968A (en) * | 2015-04-22 | 2015-08-26 | 浙江工业大学 | Quad-rotor aircraft hovering control method employing cascade auto disturbances rejection control technology |
| CN108829123A (en) * | 2018-07-18 | 2018-11-16 | 广西师范大学 | A kind of quadrotor control method, system and device |
| CN109676634A (en) * | 2018-08-28 | 2019-04-26 | 杭州电子科技大学 | Automatic disturbance rejection controller and industrial robot |
| CN109884895A (en) * | 2019-03-11 | 2019-06-14 | 南京邮电大学 | Based on the unmanned plane adaptive Gaussian filtering algorithm under saturation limited situation |
| CN110221539A (en) * | 2019-05-17 | 2019-09-10 | 江苏理工学院 | Quadrotor non-singular terminal sliding-mode control based on linear expansion observer |
| CN113419565A (en) * | 2021-06-08 | 2021-09-21 | 宁波城市职业技术学院 | Four-rotor aircraft preset performance trajectory tracking inversion control method and system |
-
2022
- 2022-05-06 CN CN202210486067.5A patent/CN114967718B/en active Active
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20120283850A1 (en) * | 2002-04-18 | 2012-11-08 | Cleveland State University | Extended active disturbance rejection controller |
| CN104865968A (en) * | 2015-04-22 | 2015-08-26 | 浙江工业大学 | Quad-rotor aircraft hovering control method employing cascade auto disturbances rejection control technology |
| CN108829123A (en) * | 2018-07-18 | 2018-11-16 | 广西师范大学 | A kind of quadrotor control method, system and device |
| CN109676634A (en) * | 2018-08-28 | 2019-04-26 | 杭州电子科技大学 | Automatic disturbance rejection controller and industrial robot |
| CN109884895A (en) * | 2019-03-11 | 2019-06-14 | 南京邮电大学 | Based on the unmanned plane adaptive Gaussian filtering algorithm under saturation limited situation |
| CN110221539A (en) * | 2019-05-17 | 2019-09-10 | 江苏理工学院 | Quadrotor non-singular terminal sliding-mode control based on linear expansion observer |
| CN113419565A (en) * | 2021-06-08 | 2021-09-21 | 宁波城市职业技术学院 | Four-rotor aircraft preset performance trajectory tracking inversion control method and system |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117192982A (en) * | 2023-08-28 | 2023-12-08 | 四川大学 | Control parameterization-based short-distance air combat maneuver decision optimization method |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114967718B (en) | 2023-03-28 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Sun et al. | Fixed-time sliding mode disturbance observer-based nonsmooth backstepping control for hypersonic vehicles | |
| Hua et al. | Tracking differentiator and extended state observer-based nonsingular fast terminal sliding mode attitude control for a quadrotor | |
| Wang et al. | Model-free–based terminal SMC of quadrotor attitude and position | |
| Jiang et al. | Finite-time backstepping control for quadrotors with disturbances and input constraints | |
| Seigler et al. | Modeling and flight control of large-scale morphing aircraft | |
| Yan et al. | Extended state observer‐based sliding mode fault‐tolerant control for unmanned autonomous helicopter with wind gusts | |
| Labbadi et al. | Novel robust super twisting integral sliding mode controller for a quadrotor under external disturbances | |
| Mehndiratta et al. | Automated tuning of nonlinear model predictive controller by reinforcement learning | |
| CN111258216B (en) | Sliding mode repetitive controller suitable for four-rotor aircraft | |
| Huang et al. | Wind-tunnel tests for active flutter control and closed-loop flutter identification | |
| Cai et al. | Quadrotor waypoint-tracking control under exogenous disturbances based on equivalent-input-disturbance approach | |
| Invernizzi et al. | Comparison of control methods for trajectory tracking in fully actuated unmanned aerial vehicles | |
| Razzaghian et al. | Adaptive fuzzy sliding mode control for a model-scaled unmanned helicopter | |
| CN114967718B (en) | Four-rotor-wing trajectory tracking optimal control method under control limitation | |
| CN115576341A (en) | Unmanned aerial vehicle trajectory tracking control method based on function differentiation and adaptive variable gain | |
| Li et al. | Multivariable finite-time composite control strategy based on immersion and invariance for quadrotor under mismatched disturbances | |
| Akbar et al. | Adaptive modified super-twisting control for a quadrotor helicopter with a nonlinear sliding surface | |
| Labbadi et al. | Robust tracking control for a quadrotor subjected to disturbances using new hyperplane-based fast terminal sliding mode | |
| Liang et al. | Attitude control of quadrotor UAV based on LADRC method | |
| Krishna et al. | Robust geometric trajectory tracking control of a variable-pitch quadrotor | |
| CN115202213B (en) | Four-rotor aircraft control method based on active disturbance rejection control | |
| Liu et al. | Active disturbance rejection control of quadrotor uav based on whale optimization algorithm | |
| Song et al. | Research on attitude control of quadrotor uav based on active disturbance rejection control | |
| Ding et al. | Optimal Joint Space Control of a Cable-Driven Aerial Manipulator. | |
| Ermeydan et al. | Feedback linearization control of a quadrotor |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |