Trajectory robust control of autonomous quadcopters based on model decoupling and disturbance estimation

In this article, a systematic procedure is given for determining a robust motion control law for autonomous quadcopters, starting from an input–output linearizable model. In particular, the suggested technique can be considered as a robust feedback linearization (FL), where the nonlinear state-feedback terms, which contain the aerodynamic forces and moments and other unknown disturbances, are estimated online by means of extended state observers. Therefore, the control system is made robust against unmodelled dynamics and endogenous as well as exogenous disturbances. The desired closed-loop dynamics is obtained by means of pole assignment. To have a feasible control action, that is, the forces produced by the motors belong to an admissible set of forces, suitable reference signals are generated by means of differentiators supplied by the desired trajectory. The proposed control algorithm is tested by means of simulation experiments on a Raspberry-PI board by means of the hardware-in-the-loop method, showing the effectiveness of the proposed approach. Moreover, it is compared with the standard FL control method, where the above nonlinear terms are computed using nominal parameters and the aerodynamical disturbances are neglected. The comparison shows that the control algorithm based on the online estimation of the above nonlinear state-feedback terms gives better static and dynamic behaviour over the standard FL control method.


Introduction
In the last years, the control of unmanned mobile vehicles has received great attention. This interest is motivated by many applications, where the autonomous control is required, such as the tracking of a trajectory for the accomplishment of a particular task. On this subject, a huge number of works propose valid control solutions, especially in the field of aerial vehicles. Most of these solutions have been summarized in several articles and books (see, for example, the lierature 1,2,3,4 and references therein).
In this work, the attention is focused on the control of autonomous quadcopter vehicles. With regard to this field, many problems have been addressed in the literature. The feedback linearization (FL) via dynamic feedback has been shown in the literature, 5 starting from the classical mathematical model of the vehicle with 12 state variables, which is not input-output linearizable. From this model, using the method of the input dynamic feedback and neglecting the exogenous disturbances, a model with 14 state variables is obtained, which results in input-output linearizable. The model linearized by means of FL appears decoupled in four SISO models having the three components of the position vector referred to as an inertial reference frame and the yaw angle as outputs, and an auxiliary input vector with four components. A state-feedback controller is then designed for each of the above four models using the pole assignment technique. Simulation results confirm the feasibility of the controller based on the linearized and decoupled model. In the literature, 6 the input-output linearized and decoupled model is completed by adding to the auxiliary input and the vector of the four exogenous equivalent disturbances projected on this input space. In this way, four linear decoupled and time-invariant models are obtained, each of them differs from the Brunovsky canonical form for the presence of an unknown equivalent disturbance added to a component of the auxiliary input vector. The four components of the equivalent disturbance vector are computed from the state variables estimated by means of a high-order sliding-mode state observer, and then, they are conveniently filtered by means of a filter not indicated in the article, and the filtered values are used to compensate for exogenous disturbances. Consequently, the determination of these components is open-loop type. The remaining attitude variables and the angular velocities in BODY frame, not estimated by the sliding observer, are computed using the estimated variables and the above mentioned linearizable model without exogenous disturbances. The last variables and those estimated are used for the implementation of the control law. In the literature, 7 PID-type classical control techniques and optimal linear quadratic control are employed using linear approximated models only for attitude control. An adaptive version of optimal controller is also discussed. In the literature, 8 Taamallah et al. deal with the online planning of a trajectory for a helicopter with an engine OFF flight condition and robust tracking of the planned trajectory. The planning approach is model-based and feasible and optimal trajectories can be generated. Regarding the controller design, the -synthesis approach is used using a low-order, linear and time-invariant plant with a multiplicative uncertainty to compensate for highfrequency unmodelled dynamics. In the literature, 9 the flight control problem is solved using an inner loop for controlling the attitude and an outer loop for controlling translational motion. Both control loops use adaptive control techniques with the aim of minimizing the effects of modelling errors. The method of pseudocontrol hedging is employed for decoupling the dynamics of the two control loops. For the sake of completeness, it is useful to observe that, in the literature, methods of autonomous flight, which involve video cameras and vision algorithms, are discussed. For example, in the literature, 10 it is described as a vision controlled micro-UAV that is capable to flight autonomously using only a video camera and an IMU on board. In the literature, 11 a control scheme is proposed for attitude control of a quadrotor aimed at the compensation of time delays in the inputs of the quadrotor model. The scheme consists of a nominal controller, which forces the plant to pursue the desired behaviour in absence of delays and a compensator, which gives a signal added to that of the nominal controller for compensating delays. It is shown that robust stability and behaviour can be obtained.
This article deals with the motion control of autonomous quadcopters, which can track a desired trajectory ensuring a high dynamic performance and the robustness of the whole system. The robust motion control is designed so as to satisfy the following requirements: (a) decoupling of the linear motions along the three axes of the inertial frame and the yaw angle; (b) robustness against endogenous and exogenous disturbances; (c) assignment of the desired dynamics to each of the above decoupled motions and (d) the control actions, i.e., the forces to be produced by the motors, have to belong to the admissible set of forces, i.e., the set of forces that the motor propeller is able to generate. To satisfy the above requirements, a combination of FL and active disturbance rejection approaches is utilized. More precisely, starting from the 14-order nonlinear model illustrated in the literature, 6 defining a convenient nonlinear equivalent disturbance vector which contains both the endogenous disturbances and the exogenous ones, assuming known equivalent disturbance, a linear model is obtained consisting of four linear and decoupled models, each of them having the Brunovsky canonical form, forced only by means of a component of the auxiliary input. This goal has been achieved by applying the nonlinear control theory shown in the literature. 12,13 Different from the procedure used in the litreature, 6 each component of the nonlinear equivalent disturbance vector is online estimated by means of an extended state observer (ESO) and this guarantees robustness of the proposed control law. In this way, the requirements of (a) and (b) are satisfied. Note that the applied technique is also different from the -synthesis method used in the literature, 11,8 which is a robust linear control technique, and it is also different with respect to the other techniques shown in the literature. 7,9 To satisfy the requirement (c), pole placement control techniques are used. Finally, to satisfy the requirement (d), the desired trajectory is suitably generated so as the output variables can track their references with bounded forces, or rather with forces that the motor-propeller systems can give.
The validation of the control law is carried out by means of several simulations and the standard mathematical model of the quadrotor. To increase accuracy of the simulations, the controller has been discretized and it has been executed with a fixed sampling time in a Raspberry-PI board by means of the hardware-in-the-loop method. Moreover, the quantization level of the measured signals has been taken into consideration, as well as the fact that the data acquired by the IMU and the GPS, i.e., position and velocity of the quadrotor center of mass, are available with different rates is also considered. This is bargained for using the hybrid nonlinear observer illustrated in the literature. 14 Besides this, the robustness of the designed control law is another aspect, which increases the effectiveness of the controller and the feasibility of its experimental implementation. To the best of the authors' knowledge, the control architecture proposed in this work, for quadcopter control, is not described in the literature.

Preliminaries on the mathematical model of the quadcopter
The mathematical model of the quadcopter considered in this article is the state-space model obtained in the literature 1 and considered also in the literature. 5,6 The state variables are the coordinates of the center of gravity (x, y and z) of the vehicle referred to an inertial frame NED, the speeds (u, v and w) referred to the same frame, the Euler angles (roll , pitch q and yaw ) referred to the body frame and the angular speeds (p, q and r) referred to the same body frame. Since this article is aimed at determining a control law useful for trajectory tracking, the output of the model is chosen as a vector consisting of the coordinate of the center of gravity (x, y and z) and the yaw angle . So, this model is given by the following equations 5,6 : Cinematic model The symbols are defined in Table 1. Models (1) and (2) have four input variables, denoted by u i , i ¼ 1; :::; 4, having a well precise physical meaning: u 1 represents the resultant of the external forces generated, along the z-body axis, by the four propellers; u 2 represents the moment generated around the x-body axis by the above-mentioned external forces scaled by d (cf. Figure 1); u 3 represents the moment generated by the external forces around the y-body axis scaled by d (cf. Figure 1); u 4 represents the moment generated by the external forces around the z-body axis. To determine u 4 , it is assumed that the propellers are in the x-y plane, and the angular speed of a propeller is positive when its rotation is counterclockwise with respect to positive direction of the z-body axis. The propellers T 1 and T 3 rotate counter clockwise at angular speeds ! T 1 and ! T 3 , whereas T 2 and T 4 rotate clockwise at speeds ! T 2 and ! T 4 . Considering, for example, the propeller T 1, its counterclockwise rotation produces a clockwise moment Then, the expressions of u i , i ¼ 1; :::; 4 in terms of the above forces are given by Coordinates of the center of gravity referred to inertial frame NED (m) u; v; w Velocities referred to inertial frame (m/s) ; q; Roll, pitch and yaw angles (rad) p; q; r Angular speeds in the body frame referred to the , q and angles respectively (rad/s) m Mass of the vehicle (kg) g Gravity acceleration (m/s 2 ) a x ; a y ; a z Aerodynamical forces (N) a ; a q ; a Aerodynamical moments (Nm) I x ; I y ; I z Inertia coefficients (Nms 2 ) d Distance from the center of mass to the rotors (m) f i Force generated by the i th rotor (N) Force to moment scaling factor (m) s a , c a , t a sinðaÞ, cosðaÞ, tanðaÞ Figure 1. Scheme of the quadcopter. T1 to T4: the four propellers; f 1 to f 4 : forces generated by the propellers T1 to T4; ! T1 to ! T4 : angular speeds of the propellers.
The above equations allow to obtain the forces f i starting from the inputs u i . Since for assigned u i , i ¼ 1; Á Á Á ; 4, there exist a unique set of forces that produce the inputs themselves, it is convenient to obtain, firstly, the control law in terms of u i and then to obtain the forces f i solving equation (3).
Finally, the output is given by To avoid singularity problems, the following constraint is assumed for models (1) and (2) q 2 ðÀp=2; p=2Þ (4) Note that this constraint is satisfied in a correct operation of a quadcopter vehicle, and unlikely, the quadcopter reaches the boundary of this domain, where singularity problems can occur (q ¼ +p=2, see the fourth equation of (1)).
In the literature, 5 it is shown that the models (1) and (2) have a vector relative degree equal to fr 1 ; r 2 ; r 3 ; r 4 g with Consequently, this model cannot be input-output linearized using a static nonlinear feedback. To cope with this drawback, the input dynamic extension technique 12,13 can be employed. This technique introduces new inputs given by the time derivatives of the actual ones. In this way, the extended state model becomes input-output linearizable. If the sum of the relative degrees (total relative degree) is equal to the order of the extended model, there is no zero dynamics and the stability of the whole system is ensured by a suitable choice of the controller for the linearized model. In the literature, 5 it is shown that the vector relative degree can be increased deriving two times the input u 1 , putting In this way, putting u 2 ¼ t 2 , u 3 ¼ t 3 and u 4 ¼ t 4 , the extended model of the quadcopter is given by the cinematic model (1) and the extended dynamic model is given by The vector relative degree of models (1) and (5) is given by fr 1 ;r 2 ;r 3 ;r 4 g, withr 1 ¼r 2 ¼r 3 ¼ 4 and r 4 ¼ 2. Consequently, the model in question is inputoutput linearizable because the total relative degree is equal to the order of the model itself. To obtain the linearized model, the following procedure is followed. By deriving the outputs as many times as the corresponding relative degree is, the following model is obtained where y ðr i Þ i n u ¼ 4 is the number of the inputs, b i ðxÞ is the equivalent disturbance acting on the output y i and x is the state vector of models (1) and (5) given by These equations, in matrix form, can be written as follows (7) where The terms bðxÞ and ΔðxÞ, named, respectively, disturbance vector and control gain matrix, are given in Appendix 1.
The determinant of matrix ΔðxÞ is given by It follows that, under Assumption 1, det ΔðxÞ ½ 6 ¼ 0 providing that z 6 ¼ 0.

Design of the control algorithm
Now, the motion control law for model (7) has to be designed so as to satisfy the following requirements: a) decoupling of the motions along the three axes of the inertial frame and that of the orientation along the yaw angle; b) robustness against endogenous and exogenous disturbances; c) each of the above decoupled motions has to occur according to the given dynamics; d) the control actions, i.e., the forces to be produced by the motor propellers, belong to the set of forces that the propellers can generate.
As it will be seen later, items (a) and (b) are obtained if good knowledge is gained on the disturbance vector and on the control gain matrix. Item (c) will be satisfied using pole assignment techniques. Item (d) will be gained by choosing suitably the dynamics of the whole closed-loop control system.

Control architecture
The structure of equation (7) is such that if the control gain matrix, ΔðxÞ, and the disturbance vector, bðxÞ, are known, then the control law is given by where n, in an auxiliary control vector, allows to obtain a closed-loop control system given by It is worthwhile to observe that model (10) consists of four chains of integrators of ordersr 1 ,r 2 ,r 3 andr 4 . Moreover, it is possible to assign to each chain the desired closed-loop dynamics, acting on the relative component of the auxiliary control n.
However, to apply the control law (9), a good knowledge of ΔðxÞ and bðxÞ is needed. With reference to the control gain matrix, its elements (see Appendix 1) depend on few mechanical parameters of the system (m, I x , I y , I z and d), which can be assumed known, and from state variables (Euler angles and z), which can be obtained by the IMU on board. Moreover, since z 6 ¼ 0 during flight, the matrix in question is always invertible. The situation is different when we consider the disturbance vector bðxÞ. In fact, as shown in Appendix 1, the elements of this vector are functions of the Euler angle and angular speeds in the body frame, together with the second derivatives of the aerodynamical forces and the aerodynamic moments acting on the vehicle. These forces and moments are obviously unknown. As a result, bðxÞ is unknown even if the parameters of the quadcopter model are known and cannot be computed online and compensated, as previously hypothesized. It follows that the control law based on the FL approach does not give the desired results.
Motivated from these considerations, it proposed a control law based on the disturbance vector estimation, bðxÞ.
Replacing bðxÞ withbðxÞ in equation (9), the control law becomes If the estimation of the disturbance vector is sufficiently accurate (i.e.bðxÞ ! bðxÞ), then the closed-loop control system works according to equation (10).
The control law previously described appears, practically, as a FL control in which the nonlinear term, representing endogenous and exogenous disturbances, is online estimated instead of analytically computed. This technique is called active disturbance rejection control (ADRC) 15 The nonlinear disturbance bðxÞ is estimated by means of ESOs, 16 together with the output variables and their derivatives up to the useful order for implementing the controller. To the best of the authors' knowledge, the ADRC, as proposed in this work, has never been applied for quadcopter control.

Design of the extended state observers
In the present article, we will obtain the estimation of the disturbance vector by constructing four ESOs designed from model (7).
In particular, this model can be divided into four independent submodels as follows With reference to model (12), let us define the following extended state vector Then, it is possible to define the following extended state model . . .
Defining, now, the following error variables (17) and by taking into account equations (15) and (16), the following error dynamics are obtained . . .
Equation (18) can be expressed in matrix form as follows where E ¼ . . . In the literature, 16 it is shown that for a given e > 0, choosing b i;j so that E is a Hurwitz matrix and assuming that j _ b i ðxÞj < M, for some M > 0, then h i converges within a ball around the origin. Moreover, for e ! 0, the ball shrinks to the origin itself. This implies that if the equivalent disturbance is bounded, then the design of the observers requires the choice of b i;j so that E is a Hurwitz matrix, and the final error can be made as small as possible by acting on e. Note that the LESOs (15) and (16) can estimate the equivalent disturbance b i ðxÞ from z i;r i þ1 ¼ b i ðxÞ. To do this, it requires only the knowledge of the input t, of the matrix ΔðxÞ and of the variable x i;1 ðtÞ. No other signals or functions are needed. Moreover, besides the equivalent disturbance, the ESO can estimate the state variable x i;1 ðtÞ and its derivatives up to that of orderr i À 1.

Design of the controller
With reference to the design of the controller, the starting point is the knowledge of a good estimate of the disturbance vector. Then, it is possible to apply the control law (11) to model (7), thus obtaining model (10).
This model can be divided into four submodels as follows The state-space models associated with equation (20) are . . .
which is the Brunovsky canonical form. For this model, the control law is obtained so as to assign the desired dynamics to the corresponding closed-loop system. Note that this fact is important because it shows that it is possible to obtain four independent submodels, and these submodels can be controlled by four independent inputs n i , i ¼ 1; 2; :::4, as shown in equation (20). Therefore, it allows to simplify the controller design, because, instead of considering a MIMO system, it is possible to consider four decoupled SISO systems, independent of each other. The method applied in this article is the pole assignment for all the four submodels. By denoting with y ðkÞ i;d , k ¼ 0; 1; Á Á Á ;r i , the k'th derivative of the i'th desired output, and with y ðkÞ i , k ¼ 0; 1; Á Á Á ;r i À 1, the k'th derivative of the i'th output, then the control law is given by are Hurwitz, then lim t!þ1 e i ¼ 0, that is, the tracking error is led to zero asymptotically.

Differentiators design
This section deals with linear and nonlinear differentiators (LD and ND, respectively). These dynamical systems, starting from the desired motion in the output space fy 1;d ; y 2;d ; y 3;d ; y 4;d g, are aimed at obtaining smooth reference signals so that the desired motion belongs to the space of the feasible motions. Obviously, in the quadcopter case, this space depends on both of its physical characteristics and its range of thrusts generated by the motors. In the following, three five-order LDs are designed for positioning the center of gravity of the quadcopter, and one thirdorder ND for its orientation. The three LDs have the same structure and parameters.
According to the literature, 17 the structure of an ND of order n is given by . . . _ n ¼ Àp 1 f al ð 1 À y d ; a; dÞ À p 2 f al ð 2 ; a; dÞ À Á Á Á À p n f al ð n ; a; dÞ is Hurwitz. Model (25) allows to estimate the derivatives of y d up to that of order n À 1. Equation (26) shows that f al ðr; a; dÞ ¼ r for a ¼ 1, and, with this choice, equation (25) describes an LD, given by with Model (28) is asymptotically stable, and its transfer function, from Y d ðsÞ to 1 ðsÞ, is given by The tracking error EðsÞ It follows that the LD tracks constantly desired signals with null steady-state error and ramp desired signals with a steady-state error equal to p 2 =p 1 . The derivatives of these signals, at the steady-state, are null or constant, respectively, and are reproduced correctly. During transients, these derivatives are smooth signals. To design the LD, it is sufficient to choose the parameters p i ; i ¼ 1; Á Á Á ; n, so as s n þ p n s nÀ1 þ Á Á Á þ p 2 s þ p 1 is Hurwitz. For designing ND, besides the choice of these parameters, it is necessary to choose a 6 ¼ 0 and a suitable value of d.

Testing of the control law
To test the control laws designed in the previous sections, simulations have been carried out by means of hardwarein-the-loop method. In particular, the control law has been implemented in a Raspberry-PI with a sampling frequency of 100 Hz as well as the reference trajectories and the hybrid nonlinear observer described below to perform the data fusion between GPS, working at 10 Hz, and IMU, whose measurements occur at the same frequency of the controller. The quadcopter has been simulated in MATLAB-Simulink environments, implementing equations (1) to (3) and considering the following parameters 18 The aerodynamic forces are chosen as a x ¼ 1, a y ¼ a z ¼ 0:4N , but they act on the system after 10, 20 and 30 s, respectively. The aerodynamic moments are chosen as a p ¼ a q ¼ a r ¼ 0:1 sin ð0:1 tÞ, and act after 10, 20 and 30 s, respectively. At t ¼ 60 s, all the aerodynamical forces and moments are removed. The waveforms of the reference position vector and yaw are shown in Figures 2 and 3.
The ESOs for the positioning that estimate the equivalent disturbances along the axes x, y and z have been designed so that the eigenvalues of the corresponding dynamical matrix E of the observer are eigðEÞ ¼ fÀ28:28 +28:28i; À 169:68; À 226:24; À 282:8g With reference to the ESO for the orientation , it has been designed so as the eigenvalues of the corresponding dynamical matrix E of the observer are eigðEÞ ¼ fÀ14:14+14:14i; À 42:42g With reference to the controllers for positioning and orientation (yaw), they have been designed by means of the pole assignment technique so that the polynomial (24) is equal to ðs þ 2:5Þ 4 for positioning and ðs þ 2Þ 2 for orientation.
The differentiators are designed to constraint the forces generated by the four propellers to the interval ½0; f max in all the operative conditions, where f max ¼ 5 N represents the maximum thrust that a single motor propeller can generate. To satisfy these requirements, the coefficients p j of matrix A in equation (28) are such that eigðAÞ ¼ fÀ1; À 5; À 8; À 15+15ig while, for orientation, an ND has been considered such that eigðAÞ ¼ fÀ1; À 5; À 10g

Data fusion of IMU and GPS measurements
In this subsection, the problem of data fusion between measurements given by IMU (sampled at 100 Hz) and measurements given by GPS (sampled at 10 Hz) is discussed. Since the measurement systems used in this field have a low update rate with respect to the control algorithm, a suitable observer with sampled measurements has to be designed. Moreover, also, the fact that the time between two consecutive measurements is not constant, but it can vary randomly, has to be considered. For these reasons, the data fusion process between data given by the above-mentioned devices is governed by the hybrid nonlinear observer described in the literature, 14 based on the strap-down model of the IMU. In particular, the observer combines a nonlinear Luenberger-type observer (NLTO), which gives the state estimate, and a updating mechanism, which carries out a corrective action on the NLTO when new position and velocity vector measurements arrive from GPS. The NLTO is realized so that its update avoids discontinuities in the estimate. For further details, the reader is addressed to the literature. 14

Simulation results
The control system is tested by means of simulations in MATLAB/Simulink environment. The mathematical model implemented for simulation is the standard one considered in the literature. The implementation of the controller is carried out taking into account the hybrid nonlinear observer briefly described above. The comparison is carried out for nominal parameters and in the presence of aerodynamic disturbances. The results are shown in Figures 4 to 11, and they are referred to two control laws, that is, the FL based on the analytical computation of the disturbance vector and the proposed control law based on the estimation of the disturbance vector, carried out by means of the ESOs. In particular, Figures 4 to 7 show the waveforms of the tracking errors of the coordinates of the center of gravity and the yaw angle with respect to the corresponding reference values. The forces generated by the four propellers corresponding to the considered experiment are shown in Figures 8  to 11. The results show the effectiveness of the proposed approach over the standard FL. In fact, with the proposed robust control law, the quadcopter tracks the trajectory with a high dynamic accuracy, whereas the standard FL control law produces tracking errors greater than those corresponding to the proposed robust control law. The simulation results confirm that the forces required to the propellers are effectively feasible. Finally, the simulation results show that the aerodynamic forces and moments are correctly estimated and

Conclusion
In this article, a systematic procedure is given for determining a robust motion control law for autonomous quadcopter vehicles. This procedure considers advanced control techniques, such as the input-output linearization and the ADRC. These techniques justify the use of the Brunovsky canonical form as the starting point for designing a controller for the motion control of the autonomous quadcopter. Then, it is possible to split the nonlinear augmented model into four decoupled models, three of which are of fourth order and one of second order. Starting from each of these models, it is possible to construct the relative ESO and to impose the dynamics to the model by means of pole assignment technique. As a final result, a robust control law is obtained, which outperforms the standard FL control. While a quantitative expression of this performance improvement is not easy to be provided, the obtained simulation results highlight that a high decrease of the tracking errors during transients has been achieved. The reason for this is that the endogenous and exogenous disturbances have been considered as further state variables estimated by means of ESOs, permitting the nonlinearity of the plant and disturbances to be compensated. It follows that the proposed methodology could be considered as an "adaptive robust version" of the FL control technique since the state-feedback terms are estimated online. In this way, not only the problems due to the uncertainties on the parameters have been addressed but it is also possible to cope with unmodelled dynamics and exogenous disturbances (such as wind and aerodynamic disturbances). Simulation experiments, carried out on a Raspberry-PI board by means of the hardware-in-the-loop method, validate the approach followed in the article and show the better performance achieved with the proposed controller compared with the standard FL. The experimental validation of this methodology represents a future development to show the effectiveness of the proposed approach.

Declaration of conflicting interests
The author(s) declared no potential conflicts of interest with respect to the research, authorship, and/or publication of this article.

Funding
The author(s) received no financial support for the research, authorship, and/or publication of this article.

Supplemental material
Supplemental material for this article is available online.