CN114967718B - 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
- CN114967718B CN114967718B CN202210486067.5A CN202210486067A CN114967718B CN 114967718 B CN114967718 B CN 114967718B CN 202210486067 A CN202210486067 A CN 202210486067A CN 114967718 B CN114967718 B CN 114967718B
- Authority
- CN
- China
- Prior art keywords
- rotor
- control
- 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.)
- Active
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-wing track tracking optimal control method under control limitation, wherein a four-rotor-wing 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 of a four-rotor-wing position and attitude system based on a full-drive system is established, and auto-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 the 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:
mi Peiliang control and implementation of a quad-rotor aircraft [ D ]. Large connection: university of graduate, 2015.
Yu Xiaoyan, sun Xiankun, xiong Yujie, hu Qingli and Chen Shanpeng. Design of anti-interference attitude control system for quad-rotor unmanned aerial vehicle based on improved ADRC [ J ] electro-optical 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 dynamics model based on total disturbance and virtual control quantity;
s20, converting a position and attitude dynamic model of the quad-rotor unmanned aerial vehicle into a quad-rotor attitude error system and a quad-rotor position error system based on a full-drive system by adopting a frame for decoupling control of a position ring and an attitude ring;
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 an equivalent closed-loop first-order linear steady error system to enable a four-rotor unmanned aerial vehicle position and attitude dynamics model 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 that control input limitation is considered;
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.
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 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 diagram illustrating the input of the controller after optimization according to 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 position and attitude dynamics model of the quad-rotor unmanned aerial vehicle into a quad-rotor attitude error system and a quad-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 through an expanded state observer, and observing and estimating to realize the observation and estimation of unknown time-varying parameter items 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 an equivalent closed-loop first-order linear steady error system to enable a four-rotor unmanned aerial vehicle position and attitude dynamics model 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 that control input limitation is considered;
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 I = x, y, z, phi, theta, psi is total disturbance, U, of each state model caused by inaccurate modeling of each state quantity dynamic equation of the quad-rotor unmanned aerial vehicle and external disturbance i I =1,2,3,4 is a virtual control amount defined as follows
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 I =1,2,3,4 is the speed of the four propellers of a quad-rotor drone.
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, position and attitude reference signals, and the error forms of the position and attitude systems of the quad-rotor unmanned aerial vehicle are 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 each 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 a steepest synthesis function which enables a time-optimum convergence, the value is based on>
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 in the same way.
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 I = x, y, z, phi, theta, psi, introducing a state-extended observer, expanding the total disturbance term into a state quantity to observe the estimate.
By disturbance amount D in 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, ∈ φ Epsilon (0,1) is the 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):
the design of the active disturbance rejection parameterized tracking controller is given 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 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 formula control law is applied to the attitude error system formula (13), there is an equivalent system as follows
Similarly, the parameterized control law of the four-rotor position error system is
When the control law of the above equation is applied to the positional error system equation (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) epsilon 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 sectionally continuous coefficient matrixes of the system. 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-wheel-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 above-mentioned theorem 1 in two respective parameterization control law formulas (25) and (23), will position and two systems of attitude 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, 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 same way can optimize the auto-disturbance rejection parameterized tracking controller parameters of the position loop.
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 Not equal to 0, and the requirement that the matrix F is a Hurwitz matrix is met, so we choose to simplify the matrix optimization problem into a parameter vector optimization problem, namely, the Hurwitz matrix F is chosen as a fixed diagonal standard form, and the matrix Z is set as 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 is a diagonal element of the matrix F, i.e., a closed-loop eigenvalue of a first order steady error system of the quad-rotor drone; 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 is naturally satisfied. Thus, the optimization problem for the selection of the parameter matrices F and Z is switched to the 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, so that only the parameter vector λ = [ λ ] in the matrix F is needed 1 ,λ 2 ,...,λ 6 ] T And (6) optimizing.
For improving the attitude tracking performance, we consider an optimized 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 the motor speed of the quad-rotor unmanned aerial vehicle is limited, the virtual control input u that can be provided 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 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 relative 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 obtained by solving the following differential equation
Theorem 3: each constraint G i I =1,2, 6 has a gradient of parameter vector λ
In the formula, the following differential equation is solved
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 the 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 I =0,1, N, having a gradient with respect to the parameter vector k of
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 Hamilton function, ζ 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 with respect to state quantity X r Partial derivatives of sum parameter vector lambda
And (5) substituting the expressions (56) - (58) into the expressions (53) - (55) to obtain the gradient of the objective function relative to the parameter vector in theorem 2, thus completing the certification. Theorem 3 proves that the process is similar to the theorem.
The invention also carries out simulation verification on the track tracking all-wheel-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 Table 1 below.
| 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. Position ring and postureThe parameters of the tracking differentiator of the state loop are set as: r =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 relevant 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 all control law parameters in the auto-disturbance-rejection controller are optimized by the provided gradient method. The simulation effect is shown in fig. 3, and it can be seen that compared with the conventional active disturbance rejection control method, the control method provided by the present invention has higher tracking control precision, the obtained tracking curve is almost overlapped with the reference curve, while the conventional active disturbance rejection control has obvious tracking error in the x-axis direction, and the tracking overshoot at 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-described 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 position and attitude dynamics model of the quad-rotor unmanned aerial vehicle into a quad-rotor attitude error system and a quad-rotor position error system based on a full-drive system by adopting a position ring and attitude ring decoupling control frame;
s30, acquiring reference signals of each order of the quad-rotor unmanned aerial vehicle by adopting a tracking differentiator with anti-noise and differential smoothing functions, expanding a total disturbance item into a state quantity through an expanded state observer, and observing and estimating to realize the observation and estimation of unknown time-varying parameter items 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 an equivalent closed-loop first-order linear steady error system to enable a four-rotor unmanned aerial vehicle position and attitude dynamics model 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 that control input limitation is considered;
s60, processing the nonlinear state inequality constraint into a single-dimensional state integral inequality through a constraint transcription methodConstraint, converting the optimal parameter setting optimization problem established in step S50 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 optimal control method for trajectory tracking of quadrotors under the control limitation of claim 1, wherein the dynamic model of the position and attitude of the quadrotor unmanned aerial vehicle in the step S10 is as follows:
wherein, (x, y, z) is the position coordinate of the four-rotor ground coordinate system, and (phi, theta, psi) is the Euler angle of the four-rotor, which is the rolling angle, the pitch angle and the yaw angle respectively, and 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 I = x, y, z, phi, theta, psi is the total disturbance, U, of the quad-rotor drone i I =1,2,3,4 is a virtual control amount defined as follows
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 I =1,2,3,4 is the speed of the four propellers of a quad-rotor drone.
3. The optimal control method for trajectory tracking of four rotors under control limitation according to claim 2, wherein the step S20 of obtaining the position error system and the attitude error system of the four rotors by using the framework of decoupling control of the position ring and the attitude ring is 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 optimal control method for trajectory tracking of four rotors under the control limitation according to claim 3, wherein in the step S20, the attitude error system of the four rotors based on the full-drive system is as follows:
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 ,σ 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, a state vector, a parameter vector, a 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 as follows:
wherein s = [ phi ]) c ,θ c ,ψ c ] T As a time-varying parameter term xi r Of each 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 respectively 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 that, φ epsilon (0,1) is the 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 observations of i = x, y, z, θ, ψ are 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 active 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:
applying the control law of the above formula to the formula (15) results in an equivalent closed-loop first-order linear steady position error system
From equations (24) and (26), the active 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 is the diagonal element of the matrix F, p 1 ＞0,p 2 Is greater than 0; only the parameter vector λ = [ λ ] in the matrix F is needed 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:
in the formula, K 1 And K 2 A state feedback matrix in the form of a proportional differential;
the same rationale for modeling the auto-disturbance rejection parameterized tracking controller for the position loop is available.
8. The optimal control method for trajectory tracking of quadrotors under control limitation according to claim 7, wherein in step S60, the process of processing the non-linear state inequality constraint into the one-dimensional state integral inequality constraint through the constraint transcription method comprises:
rewrite the control input saturation constraint to the form:
wherein χ > 0, γ > 0,
optimization problem P of transformation r Comprises the following steps:
9. the method for controlling the trajectory tracking of the quadrotor under the control limitation according to claim 8, wherein the optimization problem P is solved by a gradient method in the 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 parameter vector λ
Wherein the equation is obtained by solving the following differential equation
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 CN114967718A (en) | 2022-08-30 |
| CN114967718B true 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) |
Family Cites Families (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US8180464B2 (en) * | 2002-04-18 | 2012-05-15 | Cleveland State University | Extended active disturbance rejection controller |
| CN104865968B (en) * | 2015-04-22 | 2017-06-23 | 浙江工业大学 | A kind of quadrotor Hovering control method of use tandem Auto Disturbances Rejection Control Technique |
| CN108829123B (en) * | 2018-07-18 | 2021-06-01 | 广西师范大学 | Control method, system and device for four-rotor aircraft |
| CN109676634B (en) * | 2018-08-28 | 2021-02-19 | 杭州电子科技大学 | Active disturbance rejection controller and industrial robot |
| CN109884895B (en) * | 2019-03-11 | 2022-03-11 | 南京邮电大学 | Unmanned aerial vehicle self-adaptive tracking control algorithm based on saturation limited condition |
| CN110221539B (en) * | 2019-05-17 | 2022-04-08 | 江苏理工学院 | Four-rotor nonsingular terminal sliding mode control method based on linear extended observer |
| CN113419565B (en) * | 2021-06-08 | 2022-05-31 | 宁波城市职业技术学院 | Four-rotor aircraft preset performance trajectory tracking inversion control method and system |
-
2022
- 2022-05-06 CN CN202210486067.5A patent/CN114967718B/en active Active
Also Published As
| Publication number | Publication date |
|---|---|
| CN114967718A (en) | 2022-08-30 |
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 | |
| Jiang et al. | Finite-time backstepping control for quadrotors with disturbances and input constraints | |
| Gadewadikar et al. | H-infinity static output-feedback control for rotorcraft | |
| Chen et al. | Fuzzy adaptive nonsingular fixed-time attitude tracking control of quadrotor UAVs | |
| Mehndiratta et al. | Automated tuning of nonlinear model predictive controller by reinforcement learning | |
| Huang et al. | Wind-tunnel tests for active flutter control and closed-loop flutter identification | |
| CN111258216B (en) | Sliding mode repetitive controller suitable for four-rotor aircraft | |
| Allahverdy et al. | Back-stepping integral sliding mode control with iterative learning control algorithm for quadrotor UAVs | |
| 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 | |
| CN111399473B (en) | Self-adaptive fault-tolerant control method and system and aerial robot | |
| Li et al. | Multivariable finite-time composite control strategy based on immersion and invariance for quadrotor under mismatched disturbances | |
| Basri et al. | Optimal backstepping control of quadrotor UAV using gravitational search optimization algorithm | |
| Akbar et al. | Adaptive modified super-twisting control for a quadrotor helicopter with a nonlinear sliding surface | |
| Basri et al. | Nonlinear control of an autonomous quadrotor unmanned aerial vehicle using backstepping controller optimized by particle swarm optimization | |
| Labbadi et al. | Robust tracking control for a quadrotor subjected to disturbances using new hyperplane-based fast terminal sliding mode | |
| Krishna et al. | Robust geometric trajectory tracking control of a variable-pitch quadrotor | |
| Liang et al. | Attitude control of quadrotor UAV based on LADRC method | |
| CN114967718B (en) | Four-rotor-wing trajectory tracking optimal control method under control limitation | |
| Ding et al. | Optimal Joint Space Control of a Cable-Driven Aerial Manipulator. | |
| CN112068594A (en) | JAYA algorithm optimization-based course control method for small unmanned helicopter | |
| Xu et al. | Control of a tilting hexacopter under wind disturbance | |
| Meradi et al. | A predictive sliding mode control for quadrotor’s tracking trajectory subject to wind gusts and uncertainties |
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 |