CN111026160B - Trajectory tracking control method for quad-rotor unmanned aerial vehicle - Google Patents

Trajectory tracking control method for quad-rotor unmanned aerial vehicle Download PDF

Info

Publication number
CN111026160B
CN111026160B CN201911367910.2A CN201911367910A CN111026160B CN 111026160 B CN111026160 B CN 111026160B CN 201911367910 A CN201911367910 A CN 201911367910A CN 111026160 B CN111026160 B CN 111026160B
Authority
CN
China
Prior art keywords
angle
loop
control
aerial vehicle
unmanned aerial
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
Application number
CN201911367910.2A
Other languages
Chinese (zh)
Other versions
CN111026160A (en
Inventor
刘智伟
程星
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Huazhong University of Science and Technology
Original Assignee
Huazhong University of Science and Technology
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Huazhong University of Science and Technology filed Critical Huazhong University of Science and Technology
Priority to CN201911367910.2A priority Critical patent/CN111026160B/en
Publication of CN111026160A publication Critical patent/CN111026160A/en
Application granted granted Critical
Publication of CN111026160B publication Critical patent/CN111026160B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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/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
    • 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

Abstract

The invention belongs to the field of robot control, and particularly relates to a trajectory tracking control method for a quad-rotor unmanned aerial vehicle, which comprises the following steps: estimating the disturbance of the loop in real time by adopting a position and speed control loop, and calculating the resultant force control quantity of the unmanned aerial vehicle, a first angle and the corresponding angular speed and angular acceleration of the first angle based on the disturbance estimation quantity, wherein the first angle is a roll angle and a pitch angle, and the disturbance estimation error of the loop is converged to zero within fixed time; estimating the loop disturbance in real time by adopting an attitude angle control loop, and respectively calculating moment control quantities of the unmanned aerial vehicle rotating around x, y and z axes of a coordinate system of the body based on the disturbance estimation quantity, wherein the second angle is a yaw angle, and the loop disturbance estimation error converges to zero within fixed time; and controlling the unmanned aerial vehicle trajectory tracking based on the resultant force control quantity and the moment control quantity, wherein the angle, the angular speed error, the position and the speed error of the unmanned aerial vehicle are converged to zero in fixed time. The invention realizes the high-precision track tracking control of the unmanned aerial vehicle at fixed time.

Description

Trajectory tracking control method for quad-rotor unmanned aerial vehicle
Technical Field
The invention belongs to the field of robot control, and particularly relates to a trajectory tracking control method for a quad-rotor unmanned aerial vehicle.
Background
Along with the development of modern control theory and rotor unmanned aerial vehicle technical research, rotor unmanned aerial vehicle can realize stable hovering, and good characteristics such as fixed point take-off and descending, rotor unmanned aerial vehicle has encouraged a large amount of scientific research personnel to engage in the research work of rotor unmanned aerial vehicle control system relevant topic as the mainstream development direction of realizing future air logistics transportation.
However, the system for the single-rotor unmanned aerial vehicle often has poor control difficulty and high stability in practical application, and researchers start to turn research objects to a four-rotor unmanned aerial vehicle system aiming at the stability simplicity of the application field. On the other hand, because of there are more input variables, four rotors often more nimble, easily control and stability better than single rotor unmanned aerial vehicle, can be when flying, accomplish some high-demand tasks simultaneously, like taking photo by plane. Therefore, four rotor unmanned aerial vehicle more accord with practical application demand.
In practical application, a quad-rotor unmanned aerial vehicle system may be required to complete a trajectory tracking task at a fixed time, and the current mainstream control method cannot realize fixed time trajectory tracking, so that the fixed time tracking method is particularly important for meeting the requirements of practical application. In addition, like most mechanical rigid structures, the dynamic characteristics of a quad-rotor drone can be expressed by a mathematical model represented by its mechanical parameters, but provided that the structure of the drone is known and the mechanical parameters are known. In fact, during the working process of the unmanned aerial vehicle, part of mechanical parameters of the unmanned aerial vehicle are often unable to be accurately measured under the influence of working conditions and external interference. So parameter uncertainty usually needs to be considered when mathematically modeling the drone.
Therefore, in combination with the above, the finite time trajectory tracking control of the quad-rotor unmanned aerial vehicle system considering the disturbance and the uncertainty of the model parameters in the task space has great significance.
Disclosure of Invention
The invention provides a trajectory tracking control method for a quad-rotor unmanned aerial vehicle, which is used for solving the technical problem that high-precision tracking control of the unmanned aerial vehicle on time and trajectory cannot be realized in practical application due to the fact that mechanical parameters cannot be accurately obtained and the trajectory tracking time cannot be fixed during modeling in the existing trajectory tracking control of the quad-rotor unmanned aerial vehicle.
The technical scheme for solving the technical problems is as follows: a trajectory tracking control method for a quad-rotor unmanned aerial vehicle comprises the following steps:
based on the expected position, the speed and the acceleration corresponding to the expected position, the real-time acquired actual position, the speed and the angle corresponding to the actual position, a position and speed control loop is adopted to estimate the disturbance of the loop in real time, and based on the estimation quantity of the disturbance, the resultant force control quantity of the unmanned aerial vehicle is calculated, and a first reference angle, the angular speed and the angular acceleration corresponding to the first reference angle are calculated, wherein the first angle is a roll angle and a pitch angle, and the disturbance estimation error of the loop converges to zero within a fixed time;
estimating the disturbance of the loop in real time by adopting an attitude angle control loop based on the reference first angle and the angular velocity and the angular acceleration corresponding to the reference first angle, the angular velocity and the angular acceleration corresponding to the expected second angle, and the actual angle and the angular velocity corresponding to the actual angle, and respectively calculating the moment control quantity of the unmanned aerial vehicle rotating around x, y and z axes of a body coordinate system based on the estimation quantity of the disturbance, wherein the second angle is a yaw angle, and the disturbance estimation error of the loop converges to zero within a fixed time;
and controlling the unmanned aerial vehicle to track based on the resultant force control quantity and the moment control quantity, wherein the angle, the angular speed error, the position and the speed error of the unmanned aerial vehicle are converged to zero in fixed time.
The invention has the beneficial effects that: the method is a fixed time trajectory tracking control method considering disturbance and model parameter uncertainty, and achieves the purposes that the disturbance estimation error in a position speed ring and the disturbance estimation error in an attitude angle control ring are both converged to zero within fixed time, so that the position speed control ring enables the angle and the angular speed error of the unmanned aerial vehicle to be converged to zero within fixed time, and the attitude angle control ring enables the position and the speed error of the unmanned aerial vehicle to be converged to zero within fixed time. In the concrete implementation, the uncertainty of dynamic parameters and external disturbance in a four-rotor unmanned aerial vehicle model can be considered, and appropriate control parameters are selected to meet the requirements on the working speed and the working precision of the robot, so that the related control method can realize that a four-rotor unmanned aerial vehicle system completes a track tracking task in fixed time in practical application, the timeliness is good, the practicability is higher, and the high-precision tracking control of the unmanned aerial vehicle on time and track in practical application is realized.
On the basis of the technical scheme, the invention can be further improved as follows.
Further, the position and speed control loop comprises a controller for making the disturbance estimation error of the loop at a first fixed time T1A state estimator that internally converges to zero, represented as:
Figure GDA0002724168920000031
wherein the content of the first and second substances,
Figure GDA0002724168920000032
is the actual disturbance D of the loop1At an initial time t0Actual disturbance quantity D of the loop1Is zero, L1An upper bound for the actual disturbance component rate of change of the loop; k is a radical ofiI is 1,2,3 is a control gain constant;
Figure GDA0002724168920000033
k2＞0,k3＞4L1,0＜μ1＜1,λ1is greater than 1, v is the actual velocity vector of the unmanned plane, vdIs the desired velocity vector of the drone, g is the acceleration of gravity, u1For the total force control quantity, ezExpressing a unit vector of a z axis under a geodetic coordinate system, wherein m is the quality of the unmanned aerial vehicle, and R belongs to R3×3Is a rotation matrix sequentially rotated in the z, y, x order of the body coordinate system, adIs the desired acceleration vector of the drone;
said first fixed time T1Expressed as:
Figure GDA0002724168920000034
further, the resultant force control amount u1Expressed as:
Figure GDA0002724168920000035
Figure GDA0002724168920000041
Figure GDA0002724168920000042
wherein alpha isi＞0,βi＞0,0＜ni＜mi,0＜pi＜qi,i＝1,2，mi,ni,pi,qiAre all positive odd numbers; p is the actual position of the drone, pdIs the desired position of the drone, evFor speed error of the drone, S1The slip form surface is preset for the loop.
Further, the calculation expression of the reference first angle is:
Figure GDA0002724168920000043
where ψ is a yaw angle in the geodetic coordinate system.
Further, the position and speed error of the position and speed control loop control is at a fourth fixed time T4Inner convergence is at zero, and the expression is:
Figure GDA0002724168920000044
the invention has the further beneficial effects that: the invention adopts the state estimator and the control law in the position and speed control loop to carry out the synergistic action, and the formula of the fourth fixed time can know that the position and speed errors can be effectively converged to zero in the fixed time only by effectively setting parameters according to actual needs, so that the practicability is strong.
Further, the attitude angle control element comprises a control unit for enabling the disturbance estimation error of the loop to be at a second fixed time T2The inter-state estimator that internally converges to zero is represented as:
Figure GDA0002724168920000045
wherein the content of the first and second substances,
Figure GDA0002724168920000046
is WD2Estimate of (2), D2For actual disturbance of the loop, L2For an upper bound on the rate of change of the actual disturbance components of the loop, at an initial time t0Actual disturbance quantity D of the loop2Zero, W is a coordinate system transformation matrix, I is an identity matrix, omega is an actual angular velocity vector under a body coordinate system,
Figure GDA0002724168920000051
the expected angular velocity vector of the unmanned aerial vehicle under the geodetic coordinate system is shown, and tau' is a sliding mode control law of the ring; k is a radical ofiI is 4,5 and 6 are control gain constants,
Figure GDA0002724168920000052
k5＞0,k6＞4L2,0＜μ2＜1,λ2＞1；
said second fixed time T2Expressed as:
Figure GDA0002724168920000053
further, the moment control quantities of the unmanned aerial vehicle rotating around the x, y and z axes of the body coordinate system are u respectively2、u3、u4Expressed as: [ u ] of2,u3,u4]T＝τ；τ＝I-1Wτ'；
Figure GDA0002724168920000054
Figure GDA0002724168920000055
Wherein alpha isi＞0,βi＞0,0＜ni＜mi,0＜pi＜qi,i＝3,4，mi,ni,pi,qiAre all positive odd numbers; theta is the actual angle vector of the unmanned aerial vehicle under the geodetic coordinate system, and thetadFor the desired angular vector of the drone in the geodetic coordinate system,
Figure GDA0002724168920000056
for a desired angular acceleration vector of the drone in the geodetic coordinate system,
Figure GDA0002724168920000057
is the derivative of W and is,
Figure GDA0002724168920000058
S2the slip form surface is preset for the loop.
Further, the error of the angle and the angular speed controlled by the attitude angle control loop is in a third fixed time T3Inner convergence to zero, T3The expression of (a) is:
Figure GDA0002724168920000059
the invention has the further beneficial effects that: the state estimator and the control law in the attitude angle control loop are adopted to carry out the synergistic action, the formula of the third fixed time can know that the convergence of the angle and the angular speed error to zero in the fixed time can be effectively realized only by effectively setting parameters according to actual needs, and the practicability is strong.
Further, the total convergence time T of the track tracking is less than or equal to max (T)1,T2)+T3+T4。
The invention has the further beneficial effects that: the errors of angle and angular velocity converge first, then the errors of position and velocity converge again, and the disturbance convergence time T1、T2And the method is irrelevant to the control input quantity, and based on the control input quantity, the determined total convergence time of the trajectory tracking is high in accuracy and strong in practicability.
The invention also provides a storage medium, wherein the storage medium stores instructions, and when the instructions are read by a computer, the instructions cause the computer to execute any one of the trajectory tracking control methods of the quad-rotor unmanned aerial vehicle.
Drawings
Fig. 1 is a flow chart of a trajectory tracking control method for a quad-rotor unmanned aerial vehicle according to an embodiment of the present invention;
fig. 2 is a schematic diagram of trajectory tracking dual-loop control of a quad-rotor unmanned aerial vehicle according to an embodiment of the present invention;
fig. 3 is a diagram of a model of a quad-rotor drone provided in accordance with an embodiment of the present invention;
FIG. 4 is a block diagram of a position and velocity control loop provided by an embodiment of the present invention;
fig. 5 is a structural diagram of an attitude angle control ring according to an embodiment of the present invention;
FIG. 6 is a diagram illustrating a comparison between disturbance estimators and actual disturbances in position velocity control loops and attitude angle control loops, according to an embodiment of the present invention;
FIG. 7 is a diagram of tracking trajectories of position and attitude angles in a task space provided by an embodiment of the invention;
fig. 8 is a schematic diagram of control input of the unmanned aerial vehicle according to the embodiment of the present invention;
FIG. 9 is a diagram of position and attitude angular trajectory tracking in task space provided by an embodiment of the present invention.
Detailed Description
In order to make the objects, technical solutions and advantages of the present invention more apparent, the present invention is described in further detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative of the invention and are not intended to limit the invention. In addition, the technical features involved in the embodiments of the present invention described below may be combined with each other as long as they do not conflict with each other.
Example one
A method 100 of trajectory tracking control of a quad-rotor drone, as shown in fig. 1, comprising:
step 110, estimating disturbance of the loop in real time by adopting a position and speed control loop based on the expected position, the speed and the acceleration corresponding to the expected position, the actual position acquired in real time, and the speed and the angle corresponding to the actual position, and calculating a resultant force control quantity of the unmanned aerial vehicle and a reference first angle and an angular speed and an angular acceleration corresponding to the reference first angle based on the estimation quantity of the disturbance, wherein the first angle is a roll angle and a pitch angle, and the disturbance estimation error of the loop converges to zero within a fixed time;
step 120, estimating disturbance of the loop in real time by adopting an attitude angle control loop based on a reference first angle and an angular velocity and an angular acceleration corresponding to the reference first angle, an expected second angle and an angular velocity and an actual angle and an angular velocity corresponding to the expected second angle, and respectively calculating moment control quantities of the unmanned aerial vehicle rotating around x, y and z axes of a coordinate system of the body based on the estimation quantity of the disturbance, wherein the second angle is a yaw angle, and the disturbance estimation error of the loop is converged to zero within a fixed time;
and step 130, controlling the unmanned aerial vehicle to track based on the resultant force control quantity and the moment control quantity, wherein the angle, the angular speed error, the position and the speed error of the unmanned aerial vehicle are converged to zero in fixed time.
It should be noted that, the method firstly considers disturbance and carries out dynamics and kinematics modeling on the unmanned aerial vehicle; then the track tracking control of the unmanned aerial vehicle is converted into double-loop control, the outer loop is a position and speed control loop, and the inner loop is attitude angle loop control; as shown in fig. 2, the position speed control loop inputs a desired position and speed and an actual position and speed, part of control input amounts output to the motor and reference desired angles and angular speeds to the attitude angle control loop; the attitude angle control loop inputs a desired angle and angular speed and an actual angle and angular speed, and outputs the angle and angular speed to the other part of the motor to control input quantity. Next, designing an inner-outer ring controller, specifically including: the position and speed controller design comprises a fixed time state estimator and a fixed time sliding mode controller design, wherein the state estimator can accurately estimate the disturbance of a position and speed control loop after fixed time, and can track an expected position and speed track under the control of the proposed sliding mode control law after accurately compensating the disturbance of the position and speed control loop and after another fixed time; the attitude angle controller design comprises a fixed time state estimator and a fixed time sliding mode controller design, wherein the state estimator can accurately estimate the disturbance of an attitude angle control loop after fixed time, and can track an expected angle and an expected angular speed under the control of the proposed sliding mode control law after accurately compensating the disturbance of the attitude angle control loop and after another fixed time. Wherein the time at which the estimation error of the state estimator converges to 0 is uniquely determined by its parameters; after compensating for the disturbance, the sliding mode controller controls the time for the position velocity to converge to the desired position velocity value to be uniquely determined by its parameters. Based on the system design, the trajectory tracking control of the quad-rotor unmanned aerial vehicle is carried out. And the roll angle, the pitch angle and the yaw angle are all angles in a geodetic coordinate system.
The method is a fixed time trajectory tracking control method considering disturbance and model parameter uncertainty, and achieves the purposes that the disturbance estimation error in a position speed ring and the disturbance estimation error in an attitude angle control ring are both converged to zero within fixed time, so that the position speed control ring enables the angle and the angular speed error of the unmanned aerial vehicle to be converged to zero within fixed time, and the attitude angle control ring enables the position and the speed error of the unmanned aerial vehicle to be converged to zero within fixed time. In the concrete implementation, the uncertainty of dynamic parameters and external disturbance in a four-rotor unmanned aerial vehicle model can be considered, and appropriate control parameters are selected to meet the requirements on the working speed and the working precision of the robot, so that the related control method can realize that a four-rotor unmanned aerial vehicle system completes a track tracking task in fixed time in practical application, and the control method is good in timeliness and higher in practicability.
In order to ensure the correctness of the results of the method, the method is based on the following three conditions:
(1) the disturbance of the position and speed control loop satisfies the following conditions:
Figure GDA0002724168920000081
namely, the disturbance initial value of the position and speed control loop is 0, and the disturbance initial value exists once and is bounded;
(2) the disturbance of the attitude angle control loop meets the following requirements:
Figure GDA0002724168920000082
namely, the initial value of the disturbance of the attitude angle control loop is 0, and the disturbance exists once and is bounded;
(3) rotation angle of quad-rotor unmanned aerial vehicle satisfies
Figure GDA0002724168920000083
The dynamics and kinematics modeling model was:
Figure GDA0002724168920000091
a model diagram of a quad-rotor drone as shown in fig. 3, where E ═ Ex,ey,ezDenotes the geodetic coordinate system, B ═ Bx,by,bzDenotes the unmanned plane body coordinate system, p ═ x, y, z]T∈R3Representing the position of the drone in the world coordinate system, v ═ vx,vy,vz]T∈R3For the velocity of the drone in the world coordinate system, Θ ═ phi, θ, ψ]T∈R3For the Euler angle of the unmanned plane in the world coordinate system, omega ═ omega [ omega ]x,ωy,ωz]T∈R3For the angular velocity of the unmanned aerial vehicle in the coordinate system of the fuselage, g, m are the acceleration of gravity and the mass of the unmanned aerial vehicle, respectively, ezIs a unit vector in the z direction, D1＝[dx,dy,dz]T∈R3,D2＝[dφ,dθ,dψ]T∈R3Is a disturbance; i ═ diag (I)xx,Iyy,Izz)∈R3×3Representing an inertia matrix; τ ═ u2,u3,u4]T∈R3Representing the system input torque, u1Representing the system input unmanned plane total lift; r is formed by R3×3,W∈R3×3Representing a rotation matrix, C·＝cos(·),S·Sin (·), additionally:
Figure GDA0002724168920000092
Figure GDA0002724168920000093
the mathematical expression of the tracking target is as follows:
Figure GDA0002724168920000094
the expression is the current tracking target, pd＝[xd,yd,zd]T∈R3，vd＝[vx_d,vy_d,vz_d]T∈R3，ad＝[ax_d,ay_d,az_d]T∈R3Respectively representing the position state, velocity and acceleration of the tracking target.
The physical parameters of a quad-rotor drone are shown in table 1 below:
TABLE 1 physical parameters of quad-rotor unmanned aerial vehicle
Figure GDA0002724168920000095
Figure GDA0002724168920000101
Combining the position, the speed and the acceleration of the target, and obtaining a position and speed control equation according to Newton's second law:
Figure GDA0002724168920000102
wherein, T4Is an upper bound of convergence time, a constant determined by design parameters,
Figure GDA0002724168920000103
is an estimate of the disturbance. Thereby can obtain
Figure GDA0002724168920000104
Order to
Figure GDA0002724168920000105
Then u is1,
Figure GDA0002724168920000106
It can be determined that,
Figure GDA0002724168920000107
angle of target thetad＝[φd,θd,ψd]T∈R3It is possible to obtain:
Figure GDA0002724168920000108
ψdcan be designed by self and is generally set as 0; to thetadsObtaining the target angular velocity by calculating the derivative
Figure GDA0002724168920000109
aΩ_d＝[aωx_d,aωy_d,aωz_d]T∈R3Target angular acceleration, aΩ_dIs determined by the design parameters of the controller, theta is the roll angle, phi is the pitch angle, and psi is the yaw angle.
And (3) combining the angle, the angular velocity and the acceleration of the target to obtain a control equation of the attitude angle control loop:
Figure GDA0002724168920000111
wherein, T3Is an upper bound of convergence time, a constant determined by design parameters,
Figure GDA0002724168920000112
is an estimate of the disturbance. From this it is possible to obtain τ, then u2,u3,u4Can be determined.
Preferably, the position and velocity control loop comprises means for causing a disturbance estimation error of the loop to occur at a first fixed time T1A state estimator that internally converges to zero, represented as:
Figure GDA0002724168920000113
wherein the content of the first and second substances,
Figure GDA0002724168920000114
is the actual disturbance D of the loop1At an initial time t0Actual disturbance quantity D of the loop1Is zero, L1An upper bound for the actual disturbance component rate of change of the loop; k is a radical ofiI is 1,2,3 is a control gain constant;
Figure GDA0002724168920000115
k2＞0,k3＞4L1,0＜μ1＜1,λ1is greater than 1, v is the actual velocity vector of the unmanned plane, vdIs the desired velocity vector of the drone, g is the acceleration of gravity, u1For the total force control quantity, ezExpressing a unit vector of a z axis under a geodetic coordinate system, wherein m is the quality of the unmanned aerial vehicle, and R belongs to R3×3Is a rotation matrix sequentially rotated in the z, y, x order of the body coordinate system, adIs the desired acceleration vector of the drone; the first fixed time T1Expressed as:
Figure GDA0002724168920000116
preferably, the total force control amount u is set to be smaller than the total force control amount u1Expressed as:
Figure GDA0002724168920000117
Figure GDA0002724168920000121
Figure GDA0002724168920000122
wherein alpha isi＞0,βi＞0,0＜ni＜mi,0＜pi＜qi,i＝1,2，mi,ni,pi,qiAre all positive odd numbers; p is the actual position of the drone, pdIs the desired position of the drone, evFor speed error of the drone, S1The slip form surface is preset for the loop.
Preferably, the position and speed error controlled by the position and speed control loop is at a fourth fixed time T4Inner convergence is at zero, and the expression is:
Figure GDA0002724168920000123
as shown in fig. 4, the structure of the position and velocity control loop includes a state estimator and a control law, in which:
Figure GDA0002724168920000124
Figure GDA0002724168920000125
Figure GDA0002724168920000126
preferably, the attitude angle control unit includes a controller for controlling the disturbance estimation error of the loop at a second fixed time T2The inter-state estimator that internally converges to zero is represented as:
Figure GDA0002724168920000127
wherein the content of the first and second substances,
Figure GDA0002724168920000131
is WD2Estimate of (2), D2For actual disturbance of the loop, L2For an upper bound on the rate of change of the actual disturbance components of the loop, at an initial time t0Actual disturbance quantity D of the loop2Zero, W is a coordinate system transformation matrix, I is an identity matrix, omega is an actual angular velocity vector under a body coordinate system,
Figure GDA0002724168920000132
the expected angular velocity vector of the unmanned aerial vehicle under the geodetic coordinate system is shown, and tau' is a sliding mode control law of the ring; k is a radical ofiI is 4,5 and 6 are control gain constants,
Figure GDA0002724168920000133
k5＞0,k6＞4L2,0＜μ2＜1,λ2＞1；
the second fixed time T2Expressed as:
Figure GDA0002724168920000134
the parameters of each estimator are specifically selected as shown in table 2 below, and the perturbation is set as follows:
di＝0.2sin(5t),i＝x,y,z,φ,θ,ψ；
TABLE 2 parameters of the estimator
Parameter name Value of parameter Parameter name Value of parameter
k1 3 k4 3
k2 0.5 k5 0.5
k3 4 k 6 4
μ1 0.5 μ2 0.5
λ 1 2 λ 2 2
Preferably, the torque control amount of the unmanned aerial vehicle rotating around the x, y and z axes of the body coordinate system is u2、u3、u4Expressed as: [ u ] of2,u3,u4]T＝τ；τ＝I-1Wτ'；
Figure GDA0002724168920000135
Figure GDA0002724168920000136
Wherein alpha isi＞0,βi＞0,0＜ni＜mi,0＜pi＜qi,i＝3,4，mi,ni,pi,qiAre all positive odd numbers; theta is the actual angle vector of the unmanned aerial vehicle under the geodetic coordinate system, and thetadFor the desired angular vector of the drone in the geodetic coordinate system,
Figure GDA00027241689200001412
for a desired angular acceleration vector of the drone in the geodetic coordinate system,
Figure GDA0002724168920000142
is the derivative of W and is,
Figure GDA0002724168920000143
S2the slip form surface is preset for the loop.
Preferably, the attitude angle control loop controls the angle and the angular velocity error at a third fixed time T3Inner convergence to zero, T3The expression of (a) is:
Figure GDA0002724168920000144
preferably, the total convergence time T of the trajectory tracking is less than or equal to max (T)1,T2)+T3+T4。
As shown in fig. 5, the structure diagram of the attitude angle control loop includes a state estimator and a control law, in which:
Figure GDA0002724168920000145
Figure GDA0002724168920000146
Figure GDA0002724168920000147
Figure GDA0002724168920000148
the selection of the control parameters in each control law can be specifically seen in the following table 3, and the target trajectory is set as:
Figure GDA0002724168920000149
Figure GDA00027241689200001410
zd(t)＝0.25,ψd(t)＝0；
TABLE 3 control parameters
Figure GDA00027241689200001411
Figure GDA0002724168920000151
To better illustrate the invention, it has now been demonstrated that the state estimator is fixed time converged, as follows:
defining state observer estimation error
Figure GDA0002724168920000152
The position and speed control loop state estimator is derived to obtain:
Figure GDA0002724168920000153
analyzing the state estimator to obtain the following analysis results:
(1) when | | | Δ1When | | ≠ 0, both sides simultaneously multiply left
Figure GDA0002724168920000154
Obtaining:
Figure GDA0002724168920000155
the mathematical expression of the state estimator satisfies the following conditions:
Figure GDA0002724168920000156
let s | | | Δ1||，
Figure GDA0002724168920000157
Constructing the Lyapunov function
Figure GDA0002724168920000158
The derivation can be:
Figure GDA0002724168920000159
so it is progressively stabilized.
When the V is greater than 1, the reaction mixture is,
Figure GDA0002724168920000161
v will be at
Figure GDA0002724168920000162
Then converge to 1;
when V is less than or equal to 1,
Figure GDA0002724168920000163
v will be at
Figure GDA0002724168920000164
It converges to 0.
So elapsed time
Figure GDA0002724168920000165
Then, V, s, Δ1Will converge to 0 because
Figure GDA0002724168920000166
Passing through t1+t2After a period of time
Figure GDA0002724168920000167
(2) When | | | Δ1When the value of | is 0,
Figure GDA0002724168920000168
time to converge to 0
Figure GDA0002724168920000169
Wherein the content of the first and second substances,
Figure GDA00027241689200001619
elapsed time T1，
Figure GDA00027241689200001611
Error of estimated value of state observer
Figure GDA00027241689200001612
Converging to 0.
Total elapsed time T for the same reason2Status of stateObserver estimation error
Figure GDA00027241689200001613
Converging to 0.
Figure GDA00027241689200001614
As shown in fig. 6, the left plot corresponds to the position velocity control loop, the right plot corresponds to the attitude angle control loop, the position velocity control loop and the attitude angle control loop disturbance estimates and the actual disturbance values, and each component of the position and attitude angle disturbance estimates converges to its corresponding disturbance value at a fixed time under the influence of the state estimator designed.
The designed control rate is demonstrated below
Figure GDA00027241689200001615
The position velocity can be guaranteed to be fixed time converged:
to slip form surface S1Taking the derivative, we can get:
Figure GDA00027241689200001616
modeling dynamics and kinematics
Figure GDA00027241689200001617
Of (2) and designed
Figure GDA00027241689200001618
Substituting the expression of (a) into the above formula, one can obtain:
Figure GDA0002724168920000171
wherein
Figure GDA0002724168920000172
Is the estimation error.
According to the above, the time T passes1，
Figure GDA0002724168920000173
Obtaining:
Figure GDA0002724168920000174
constructing the Lyapunov function
Figure GDA0002724168920000175
The derivation can be:
Figure GDA0002724168920000176
V1when the pressure is higher than 1,
Figure GDA0002724168920000177
V1will be at
Figure GDA0002724168920000178
And then converges to 1.
Figure GDA0002724168920000179
V1Will be at
Figure GDA00027241689200001710
And then converges to 0.
So elapsed time
Figure GDA00027241689200001711
Rear, V1,S1Will converge to 0.
From S 10, get
Figure GDA00027241689200001712
Let ep＝p-pd，ev＝v-vdThen, then
Figure GDA00027241689200001713
Elapsed time of the same reason
Figure GDA00027241689200001714
After, epAnd evWill converge to 0.
The position and speed control loop convergence time is as follows:
Figure GDA00027241689200001715
the convergence time of the attitude angle control loop is as follows:
Figure GDA00027241689200001716
because the errors of angle and angular velocity are converged firstly, then the errors of position and velocity are converged, and the disturbance convergence time T1、T2Independent of the control input, so the total time of trajectory tracking convergence T ≦ max (T)1,2T)+3T+4T。
As shown in fig. 7, under the action of the designed estimator-based fixed time controller, the tracking trajectory diagram of the position and attitude angle in the task space converges to its corresponding tracking target trajectory in a fixed time (within 8 seconds). Based on fig. 6 and 7, the control input of the unmanned aerial vehicle is obtained, including the resultant force control quantity and all the moment control quantities, as shown in fig. 8, and finally, a position and attitude angle trajectory tracking diagram in the task space is obtained, as shown in fig. 9, the result of the simulation of fig. 9 is very close to that of fig. 7, only because the accuracy problem of the sensor causes a slight error in the position and attitude angle tracking, and the error caused by the factor is removed, so that the position and attitude angle can be converged to the corresponding tracking target trajectory within a fixed time (within 8 seconds), and the result can support the control effect of the method of the embodiment.
Example two
A storage medium having instructions stored therein, which when read by a computer, cause the computer to execute a method for trajectory tracking control of a quad-rotor drone according to the first embodiment.
The related technical solution is the same as the first embodiment, and is not described herein again.
It will be understood by those skilled in the art that the foregoing is only a preferred embodiment of the present invention, and is not intended to limit the invention, and that any modification, equivalent replacement, or improvement made within the spirit and principle of the present invention should be included in the scope of the present invention.

Claims (6)

1. A trajectory tracking control method for a quad-rotor unmanned aerial vehicle is characterized by comprising the following steps:
based on the expected position, the speed and the acceleration corresponding to the expected position, the real-time acquired actual position, the speed and the angle corresponding to the actual position, a position and speed control loop is adopted to estimate the disturbance of the loop in real time, and based on the estimation quantity of the disturbance, the resultant force control quantity of the unmanned aerial vehicle is calculated, and a first reference angle, the angular speed and the angular acceleration corresponding to the first reference angle are calculated, wherein the first angle is a roll angle and a pitch angle, and the disturbance estimation error of the loop converges to zero within a fixed time;
estimating the disturbance of the loop in real time by adopting an attitude angle control loop based on the reference first angle and the angular velocity and the angular acceleration corresponding to the reference first angle, the angular velocity and the angular acceleration corresponding to the expected second angle, and the actual angle and the angular velocity corresponding to the actual angle, and respectively calculating the moment control quantity of the unmanned aerial vehicle rotating around x, y and z axes of a body coordinate system based on the estimation quantity of the disturbance, wherein the second angle is a yaw angle, and the disturbance estimation error of the loop converges to zero within a fixed time;
controlling the unmanned aerial vehicle to track on the basis of the resultant force control quantity and the moment control quantity, wherein the angle, the angular speed error, the position and the speed error of the unmanned aerial vehicle converge to zero within fixed time;
the position and speed control loop comprises a first fixed time T for enabling the disturbance estimation error of the loop1A state estimator that internally converges to zero, represented as:
Figure FDA0002724168910000011
wherein the content of the first and second substances,
Figure FDA0002724168910000012
is the actual disturbance D of the loop1At an initial time t0Actual disturbance quantity D of the loop1Is zero, L1An upper bound for the actual disturbance component rate of change of the loop; k is a radical ofiI is 1,2,3 is a control gain constant;
Figure FDA0002724168910000013
k2＞0,k3＞4L1,0＜μ1＜1,λ1is greater than 1, v is the actual velocity vector of the unmanned plane, vdIs the desired velocity vector of the drone, g is the acceleration of gravity, u1For the total force control quantity, ezExpressing a unit vector of a z axis under a geodetic coordinate system, wherein m is the quality of the unmanned aerial vehicle, and R belongs to R3×3Is a rotation matrix sequentially rotated in the z, y, x order of the body coordinate system, adIs the desired acceleration vector of the drone;
said first fixed time T1Expressed as:
Figure FDA0002724168910000021
the total force control amount u1Expressed as:
Figure FDA0002724168910000022
Figure FDA0002724168910000023
Figure FDA0002724168910000024
wherein alpha isi＞0,βi＞0,0＜ni＜mi,0＜pi＜qi,i＝1,2，mi,ni,pi,qiAre all positive odd numbers; p is the actual position of the drone, pdIs the desired position of the drone, evFor speed error of the drone, S1Presetting a sliding mode surface for the loop;
the attitude angle control unit comprises a control unit for controlling the disturbance estimation error of the loop at a second fixed time T2The inter-state estimator that internally converges to zero is represented as:
Figure FDA0002724168910000025
wherein the content of the first and second substances,
Figure FDA0002724168910000026
is WD2Estimate of (2), D2For actual disturbance of the loop, L2For an upper bound on the rate of change of the actual disturbance components of the loop, at an initial time t0Actual disturbance quantity D of the loop2Zero, W is a coordinate system transformation matrix, I is an identity matrix, omega is an actual angular velocity vector under a body coordinate system,
Figure FDA0002724168910000027
the expected angular velocity vector of the unmanned aerial vehicle under the geodetic coordinate system is shown, and tau' is a sliding mode control law of the ring; k is a radical ofiI is 4,5 and 6 are control gain constants,
Figure FDA0002724168910000031
k5＞0,k6＞4L2,0＜μ2＜1,λ2＞1；
said second fixed time T2Expressed as:
Figure FDA0002724168910000032
the moment control quantity of the unmanned aerial vehicle rotating around the x, y and z axes of the coordinate system of the body is u2、u3、u4Expressed as: [ u ] of2,u3,u4]T＝τ；τ＝I-1Wτ'；
Figure FDA0002724168910000033
Figure FDA0002724168910000034
Wherein alpha isi＞0,βi＞0,0＜ni＜mi,0＜pi＜qi,i＝3,4，mi,ni,pi,qiAre all positive odd numbers; theta is the actual angle vector of the unmanned aerial vehicle under the geodetic coordinate system, and thetadFor the desired angular vector of the drone in the geodetic coordinate system,
Figure FDA0002724168910000035
for a desired angular acceleration vector of the drone in the geodetic coordinate system,
Figure FDA0002724168910000036
is the derivative of W and is,
Figure FDA0002724168910000037
S2the slip form surface is preset for the loop.
2. The method of claim 1, wherein the computational expression for the reference first angle is:
Figure FDA0002724168910000038
where ψ is a yaw angle in the geodetic coordinate system.
3. The method of claim 2, wherein the position and speed error of the position and speed control loop control is at a fourth fixed time T4Inner convergence is at zero, and the expression is:
Figure FDA0002724168910000039
4. the trajectory tracking control method for quad-rotor unmanned aerial vehicle according to claim 1, wherein the angle and angular speed errors controlled by the attitude angle control loop are within a third fixed time T3Inner convergence to zero, T3The expression of (a) is:
Figure FDA0002724168910000041
5. the trajectory tracking control method for quad-rotor unmanned aerial vehicle according to claim 4, wherein total convergence time T of trajectory tracking is less than or equal to max (T ≦ max)1,T2)+T3+T4。
6. A storage medium having stored thereon instructions which, when read by a computer, cause the computer to perform a method of trajectory tracking control of a quad-rotor drone according to any one of claims 1 to 5.
CN201911367910.2A 2019-12-26 2019-12-26 Trajectory tracking control method for quad-rotor unmanned aerial vehicle Active CN111026160B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201911367910.2A CN111026160B (en) 2019-12-26 2019-12-26 Trajectory tracking control method for quad-rotor unmanned aerial vehicle

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911367910.2A CN111026160B (en) 2019-12-26 2019-12-26 Trajectory tracking control method for quad-rotor unmanned aerial vehicle

Publications (2)

Publication Number Publication Date
CN111026160A CN111026160A (en) 2020-04-17
CN111026160B true CN111026160B (en) 2020-12-08

Family

ID=70214743

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911367910.2A Active CN111026160B (en) 2019-12-26 2019-12-26 Trajectory tracking control method for quad-rotor unmanned aerial vehicle

Country Status (1)

Country Link
CN (1) CN111026160B (en)

Families Citing this family (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111650951B (en) * 2020-05-22 2021-09-17 南京航空航天大学 Full-loop composite dynamic inverse tracking control method for complex track of quad-rotor unmanned aerial vehicle
CN111665870A (en) * 2020-06-24 2020-09-15 深圳市道通智能航空技术有限公司 Trajectory tracking method and unmanned aerial vehicle
CN111948935A (en) * 2020-08-03 2020-11-17 曾喆昭 Self-coupling PD control theory method of under-actuated VTOL aircraft
CN112269401B (en) * 2020-09-04 2021-07-20 河南大学 Self-adaptive active sensor tracking method based on tracking precision and risk control
CN112650303B (en) * 2021-01-20 2022-01-11 南京航空航天大学 Unmanned aerial vehicle formation fault-tolerant control method based on fixed time technology
CN114564047B (en) * 2022-04-28 2022-08-16 北京航空航天大学 Unmanned aerial vehicle constant-speed flight control method considering meteorological conditions

Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2007018572A2 (en) * 2004-11-08 2007-02-15 Bell Helicopter Textron Inc. Flight system with three feedback control loops
CN104865968A (en) * 2015-04-22 2015-08-26 浙江工业大学 Quad-rotor aircraft hovering control method employing cascade auto disturbances rejection control technology
CN106933104A (en) * 2017-04-21 2017-07-07 苏州工业职业技术学院 A kind of quadrotor attitude based on DIC PID and the mixing control method of position
CN106970646A (en) * 2017-05-23 2017-07-21 四川理工学院 Quadrotor control method based on Adaptive Integral contragradience
CN107608367A (en) * 2017-09-06 2018-01-19 天津大学 The rotor wing unmanned aerial vehicle track of multivariable interference compensation four and posture cooperative control method
CN108594837A (en) * 2018-02-12 2018-09-28 山东大学 Model-free quadrotor drone contrail tracker and method based on PD-SMC and RISE
WO2019055025A1 (en) * 2017-09-15 2019-03-21 Sanyal Amit K Integrated guidance and feedback control for autonomous vehicle
CN109597426A (en) * 2018-11-13 2019-04-09 上海无线电设备研究所 Based on L1The four-rotor helicopter Trajectory Tracking Control method of self adaptive control
CN109901606A (en) * 2019-04-11 2019-06-18 大连海事大学 A kind of mixing finite time control method for quadrotor Exact trajectory tracking
CN110456816A (en) * 2019-07-05 2019-11-15 哈尔滨工程大学 A kind of quadrotor Trajectory Tracking Control method based on continuous terminal sliding mode

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9715234B2 (en) * 2015-11-30 2017-07-25 Metal Industries Research & Development Centre Multiple rotors aircraft and control method
CN107922056B (en) * 2016-02-26 2021-03-23 深圳市大疆灵眸科技有限公司 Method and system for stabilizing a load
US10564650B2 (en) * 2017-07-27 2020-02-18 Intel Corporation Trajectory tracking controllers for rotorcraft unmanned aerial vehicles (UAVS)
KR102118952B1 (en) * 2018-03-12 2020-06-05 광주과학기술원 A device for positioning and tracking control based RF of multi unnamed aerial vehicle
CN110275542B (en) * 2019-06-14 2022-04-08 合肥工业大学 Four-rotor aircraft control method based on self-adaptive finite time control

Patent Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2007018572A2 (en) * 2004-11-08 2007-02-15 Bell Helicopter Textron Inc. Flight system with three feedback control loops
CN104865968A (en) * 2015-04-22 2015-08-26 浙江工业大学 Quad-rotor aircraft hovering control method employing cascade auto disturbances rejection control technology
CN106933104A (en) * 2017-04-21 2017-07-07 苏州工业职业技术学院 A kind of quadrotor attitude based on DIC PID and the mixing control method of position
CN106970646A (en) * 2017-05-23 2017-07-21 四川理工学院 Quadrotor control method based on Adaptive Integral contragradience
CN107608367A (en) * 2017-09-06 2018-01-19 天津大学 The rotor wing unmanned aerial vehicle track of multivariable interference compensation four and posture cooperative control method
WO2019055025A1 (en) * 2017-09-15 2019-03-21 Sanyal Amit K Integrated guidance and feedback control for autonomous vehicle
CN108594837A (en) * 2018-02-12 2018-09-28 山东大学 Model-free quadrotor drone contrail tracker and method based on PD-SMC and RISE
CN109597426A (en) * 2018-11-13 2019-04-09 上海无线电设备研究所 Based on L1The four-rotor helicopter Trajectory Tracking Control method of self adaptive control
CN109901606A (en) * 2019-04-11 2019-06-18 大连海事大学 A kind of mixing finite time control method for quadrotor Exact trajectory tracking
CN110456816A (en) * 2019-07-05 2019-11-15 哈尔滨工程大学 A kind of quadrotor Trajectory Tracking Control method based on continuous terminal sliding mode

Non-Patent Citations (5)

* Cited by examiner, † Cited by third party
Title
Fixed-time trajectory tracking for a quadrotor with external disturbances：A flatness-based sliding mode control approach;Xiaolin Ai 等;《Aerospace Science and Technology》;20190403;第89卷;58-76 *
Multivariable sliding mode backstepping controller design for quadrotor UAV based on disturbance observer;Zheng ZHANG 等;《SCIENCE CHINA Information Sciences》;20181130;第61卷;1-16 *
四旋翼无人机轨迹跟踪控制系统设计;姜明瑞;《中国优秀硕士学位论文全文数据库工程科技Ⅱ辑》;20190115(第01期);C031-310 *
四旋翼无人机轨迹跟踪控制系统设计;王春阳 等;《电光与控制》;20190331;第26卷(第3期);103-107 *
四旋翼飞行器轨迹跟踪控制器的设计与验证;范云生 等;《仪器仪表学报》;20170331;第38卷(第3期);741-749 *

Also Published As

Publication number Publication date
CN111026160A (en) 2020-04-17

Similar Documents

Publication Publication Date Title
CN111026160B (en) Trajectory tracking control method for quad-rotor unmanned aerial vehicle
CN109062042B (en) Limited time track tracking control method of rotor craft
Colorado et al. Mini-quadrotor attitude control based on Hybrid Backstepping & Frenet-Serret theory
CN107608367B (en) Multivariable interference compensation quadrotor unmanned aerial vehicle trajectory and attitude cooperative control method
CN106325291B (en) Sliding mode control law and ESO (electronic stability program) based four-rotor aircraft attitude control method and system
CN109947126B (en) Control method, device and equipment of quad-rotor unmanned aerial vehicle and readable medium
Tang et al. Nonlinear dynamic modeling and hybrid control design with dynamic compensator for a small-scale UAV quadrotor
CN109614633A (en) A kind of composite rotor craft non-linear modeling method and Calculate Ways
CN111258216B (en) Sliding mode repetitive controller suitable for four-rotor aircraft
CN108037662A (en) A kind of limited backstepping control method of quadrotor output based on Integral Sliding Mode obstacle liapunov function
CN109507890A (en) A kind of unmanned plane dynamic inverse generalized predictive controller based on ESO
CN106874617B (en) Efficient helicopter maneuvering flight quality grade evaluation method
CN109885074B (en) Finite time convergence attitude control method for quad-rotor unmanned aerial vehicle
CN112578805B (en) Attitude control method of rotor craft
CN106802570B (en) Method and device for tracking position of unmanned helicopter
CN111813140A (en) High-precision trajectory tracking control method for quad-rotor unmanned aerial vehicle
CN110377044B (en) Finite time height and attitude tracking control method of unmanned helicopter
CN111459188B (en) Quaternion-based multi-rotor nonlinear flight control method
CN111198570B (en) Anti-delay high-precision active disturbance rejection attitude control method based on fixed time differentiator prediction
CN110703603B (en) Control method of multi-layer recursive convergence neural network controller of unmanned aerial vehicle
CN109976364B (en) Attitude decoupling control method for six-rotor aircraft
CN106292297A (en) Based on PID controller and the attitude control method of L1 adaptive controller
CN116088549B (en) Tailstock type vertical take-off and landing unmanned aerial vehicle attitude control method
CN108845508B (en) CMAC-sliding mode integrated control-based semi-physical simulation control method for unmanned aerial vehicle
González-Hernández et al. Super-twisting control scheme for a miniature quadrotor aircraft: Application to trajectory-tracking problem

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