Robust Optimal Adaptive Trajectory Tracking Control of Quadrotor Helicopter 1

This paper focuses on robust optimal adaptive control strategy to deal with tracking problem of a quadrotor unmanned aerial vehicle (UAV) in presence of parametric uncertainties, actuator amplitude constraints, and unknown time-varying external disturbances. First, Lyapunov-based indirect adaptive controller optimized by particle swarm optimization (PSO) is developed for multi-input multi-output (MIMO) nonlinear quadrotor to prevent input constraints violation, and then disturbance observer-based control (DOBC) technique is aggregated with the control system to attenuate the effects of disturbance generated by an exogenous system. The performance of synthesis control method is evaluated by a new performance index function in time-domain, and the stability analysis is carried out using Lyapunov theory. Finally, illustrative numerical simulations are conducted to demonstrate the effectiveness of the presented approach in altitude and attitude tracking under several conditions, including large time-varying uncertainty, exogenous disturbance, and control input constraints.


Latin American Journal of Solids and
during flight, which provides safer maneuver with lower risk of damaging the environment; nevertheless, quadrotors have a nonlinear, coupled, and underactuated dynamics, which poses serious challenges in control system design.
Myriad advanced methods, considered quadrotor attitude and altitude control design, have been proposed in literature such as backstepping (Kobilarov (2013(Kobilarov ( ), huo et al. (2014)), Jasim and Gu (2015)), feedback linearization (Voos (2009), Choi and Ahn (2015)), optimal control (Navabi and Mirzaei (2016)), Suicmez and Kutay (2014)), and robust control (Xiong and Zheng (2014)).This list is, of course, far from being exhaustive.Also attitude control design is need for some other applications such as spacecraft control (Navabi et al. (2012), Navabi and Radaei (2013), Navabi and Rangraz (2013), Navabi and Meshkinfam (2013), Navabi and Barati (2017) All these control methods have been successfully controlled systems with certain and fully known dynamics; however, in the presence of uncertainties, the closed-loop control system may be unstable and lose performance.Owing to specific flight mission, quadrotors may be equipped with diverse devices such as camera and measurement instruments and carry an unknown payload, so these reconfigurations lead to change in center of gravity (CoG), mass, and inertia properties of quadrotor's system.In order to enhance robustness against uncertainty, neural network control (Dierks and Jagannathan (2010)) and sliding mode control (Mokhtari and Cherki (2015), Zhu and Huo (2013)) are utilized for quadrotor flight, but the performance of these control methods severely depends on prior information related to upper bound of uncertainty (Slotine (1991)).In this case, to overcome this obstacle, adaptive control can be designated as eminent candidate in dealing with quadrotor's uncertain system.Adaptive controller is an invaluable and efficient approach to handle a wide variety of systems with uncertainties and nonlinearities in a broad range of operating conditions.The aforementioned method affords desired level of control system performance without requiring a priori knowledge of quadrotor's parameters (Zhu and Wen (2008)).
In Morel and Leonessa (2006), to handle uncertainties in the model, direct adaptive control based on backstepping method is applied for trajectory tracking problem.In Dydek et al. (2010), model reference adaptive controller is presented to tackle parametric uncertainties which arise from actuator failure.The effect of displacement of CoG in quadrotor is investigated in Palunko and Fierro (2011), and in order to cope with this uncertainty, an adaptive feedback linearization controller is employed.In Sun and Zuo (2014), unified noncertainty equivalent adaptive control method is suggested to handle the parametric uncertainties in mass properties of quadrotor.By using passivity theory in Ha et al. (2014), adaptive backstepping control framework for quadrotor helicopter is proposed.Backstepping method is utilized to cope with quadrotor's under-actuation dynamic, and compensation of parameter uncertainties is handled through online parameter estimation.Unfortunately, in all of the existing research results, the uncertain external disturbances are not taken into account.This flying robot, due to its small size and light weight, is extremely sensitive to external disturbances such as a wind gust.Numerous researches have been addressed both adaptive control and disturbance rejection simultaneously (Bialy et al. (2013), Selfridge and Tao (2014), Zhao et al. (2015), Basri et al. (2015), Yang and Yan (2016), Chen et al. (2016)); nonetheless, the effect of input saturation has not been considered with these approaches.
In spite of comprehensive investigations on robust adaptive controller design for quadrotor, there is poor literature on control input constraints and actuators restrictions.It is worth mentioning that Latin American Journal of Solids and Structures 14 (2017) 1040-1063 in an actuated dynamic system, uncertainties and disturbances are not the only sources of performance degradation and instability.Sometimes, the best tracking and regulation performance are achievable by means of large control input magnitude which may exceed the actuators' bounds.Disregard of actuator limits probably brings about system's collapse; in addition, undesired transient response and even damaging the actuators can be the other effects (Izadbakhsh et al. (2011)).In Kendoul et al. (2007), global asymptotic stability of quadrotor and boundedness of control inputs are guaranteed via nested-saturation based nonlinear controller.Constrained finite time optimal control of quadcopter in the presence of mechanical constraints, such as maximum thrust in the rotors, is proposed in Alexis et al. (2010).Optimal control theory and optimization is applied to various missions of satellite, airplane and rotorcraft (Khamseh, H.B., Navabi, M. (2010), Khamseh, H.B., Navabi, M. (2011)).In Guerrero-Castellanose et al. (2011), by using nested saturation functions, a quaternion-based bounded control method is presented.In Cutler and How (2012) and Bry et al. (2015), to incorporate physical constraints on actuators of variable-pitch quadrotor, minimum-time path between any two waypoints in space is generated.This technique provides smooth reference inputs, capability of aerobatic maneuvers, and aggressive flight.In Zuo et al. (2015), cascaded control scheme based on modified Rodrigues parameters representation is used to design control design an autopilot system which satisfies the input constraints.Unfortunately, in all mentioned methods, sensitivity of control systems to parameter variation is not completely investigated, or distinct mechanism for disturbance rejection is not introduced.In Izadi et al. (2011), Model Predictive Control (MPC) is applied to calculate a stabilizing control signal and satisfy saturation constraint during abrupt changes in the quadrotor dynamics; however, gyroscopic effects resulting from the rigid body rotation are neglected, and simplified dynamics of quadrotor is utilized.To the best of authors' knowledge, the problem of disturbance rejection and input saturation for full nonlinear uncertain model of quadrotor is not investigated simultaneously.
In this paper, adaptive control law is used to tackle attitude and altitude tracking control problem when mass and inertia matrix of quadrotor are unknown.The PSO algorithm tries to regulate controller parameters offline so that the input constraints will be satisfied.In order to enhance robustness of system, a nonlinear disturbance observer is combined with the presented control method.Compared with other studies in literature, the main contribution of this paper contains: (i) an adaptive control, which is qualified to stabilize fully nonlinear and uncertain dynamic of the quadrotor, is combined with the optimization algorithm, PSO, so as to modify controller's attribute, which results in optimal adaptive controller.(ii) a new performance index, precisely compatible with control objective, in time domain is introduced to consider constraints of control inputs and optimal problem.(iii) a nonlinear disturbance observer is added to optimal adaptive controller to elevate robustness of system when the quadrotor confronts unknown external disturbances.As far as we know, there is almost no optimal adaptive control paper studies the attitude and altitude tracking problem of MIMO nonlinear model of quadrotor in presence of parametric uncertainties, exogenous disturbance, and input constraints.To validate the efficiency of the contributions, simulations on a quadrotor in the presence of parametric uncertainties and exogenous disturbance are performed.The structure of the paper is arranged as follows.The quadrotor dynamics are presented in section 2. The adaptive control system, the details of the PSO algorithm, and DOBC are outlined in section 3.In section 4, a simulation examples Latin American Journal of Solids and Structures 14 (2017) 1040-1063 are presented to illustrate the overall validity and effectiveness of the proposed approach.Finally, the conclusions are discussed in section 5.

QUADROTOR MODEL
In this section, the six-degree-of-freedom mathematical model of a quadcopter is presented.Model derivation is based on the following assumptions (Bouabdallah and Siegwart (2007)):  Quadcopter has a symmetrical structure. Quadcopter is a rigid body. The center of gravity and the body's frame origin are coincided. Variation of thrust and drag are proportional to the square of propeller's speed.
As depicted in Figure 1, the quadrotor includes four fixed-pitch-angle blades.Each rotor produces both thrust and antitorque.Two pairs of rotors, i.e. rotors (1,3) rotate clockwise and rotors (2,4) rotate counterclockwise.Accordingly, the produced antitorque can be neutralized by each other.Diverse maneuvers in space will be accomplished based on relative angular velocity of four rotors.The equations of motion are derived by using of two coordinate frames shown in Figure 1.Let 1 2 3 ( ) O e e e   denotes the inertial frame and O e e e   stands for the body-fixed frame.
Attitude of quadrotor helicopter with respect to inertia frame is introduced by  and  are roll, pitch, and yaw angles respectively.
is the position of the center of gravity of the quadrotor in inertial frame.R is the transformation matrix for vector from body frame to inertia frame as follows.

c c c s s s c c s c s s R s c s s s c c s s c c s s cs cc
y q y q f y f y q f y f y q y q f y f y q f y f Latin American Journal of Solids and Structures 14 (2017) 1040-1063 Where x c means cos( ) x and x s means sin( ) x .Based on the Newton-Euler equation, translational dynamics of rigid body is described by the following equation Where m is the total mass of quadrotor, and g denotes acceleration due to gravity. 1 u represents total trust, and 1 d is external disturbance.The external disturbance only possess z component because, in this paper, altitude and attitude control of quadrotor are only considered.The rotational equations in the inertia frame can be expressed as Where and r J stands for the inertia of propeller.In addition, stand for angular speed of the th i rotor).In this problem, r  is assumed to be the measurable disturbance.
is the control torque input, and notes an external disturbance with unknown bound generated by an exogenous system. 1 u and u are defined as Where denote the thrust produced by rotors, and horizontal distance between propeller's center to center of mass of the quadrotor.In order to simplify the calculation of angular velocity of rotors, equation ( 4) can be written as Latin American Journal of Solids and Structures 14 (2017) 1040-1063 The dynamical equations of quadrotor helicopter derived from equation ( 2) and ( 3) can be written in following forms: It is worthwhile to note that control inputs appear in altitude ( z ), attitude (  ,  and  ), and their time derivatives; therefore, equation ( 6) can be divided into two subsystems.The first, 1  (underactuated sub-system), comprises linear translations in x and y directions, and the second one, 2  (fully actuated sub-system), contains the dynamics of altitude and attitude.The subsystems 1  and 2  are listed as follows 1 1 1 (cos sin cos sin sin ) : (cos sin sin sin cos ) f qy q q yf f y fq Since the outputs of the subsystem 1  depend on the angles, it suffices to extract appropriate outputs from subsystem 2  ; accordingly, design of controller for subsystem 2  is the focal point of this paper.See Table 1 for all the values of these model parameters.(Bouabdallah and Siegwart (2006)).

CONTROL DESIGN
The control objective is forcing the states of subsystem 2  to track desired altitude and attitude despite the presence of parametric uncertainties, input constraints and external disturbance.Because the control design is separated from disturbance observer design, in the first step, optimal adaptive control is introduced without external disturbance, and in the next subsection, nonlinear disturbance observer is presented.By integrating the disturbance observer with optimal adaptive control method, the disturbance produced by exogenous system can be estimated and compensated in finite time.Finally, tracking performance is guaranteed by means of the Lyapunov theory.

Adaptive Trajectory Tracking Control Design
The dynamic model associated with fully actuated subsystem 2  of quadrotor can be written as following form: denotes state vector of system, is the control input vector, and Latin American Journal of Solids and Structures 14 (2017) 1040-1063 Where x is achievable by calculating qualified control laws; however, it gets too complicated due to unknown mass properties of system.Consider following Lyapunov function candidate: Where a , â , D , and D are unknown and estimated parameters, and disturbances respectively.a  and D  are estimation errors. 1  and 2  represent symmetric positive definite matrices, and s denotes velocity error Where is tracking error, to obtain control law and adaption law, we can define new variable  , and  is a symmetric positive definite matrix.The time derivative of V is given by Substituting s  by r  x x   into Eq.( 15) yields Using Eq. ( 9), the following expression is calculated: Substituting x  with r  s x  in Eq. ( 17) gives: Latin American Journal of Solids and Structures 14 (2017) 1040-1063 Since the quadratic function associated with a skew-symmetric matrix is zero, Eq. ( 18) can be written as:   , and ( ) g x linearly depend on unknown parameters of system, left side of Eq. ( 9) can be expressed as: The controller is designed as: Here D K is a constant symmetric positive definite matrix.Using Eq. ( 19), Eq. ( 20), and Eq. ( 21) Adaption law and disturbance estimation can be selected as follows.
Then, the derivative of the Lyapunov function becomes: Convergence of tracking errors to zero is proved using Lyapunov theory and Barbalat's lemma (Slotine (1991)).Simulation results show that an amplitude of control law and performance characteristics of control system depend on 1  ,  , and D K ; moreover, it is possible to ignore the effects of 2  .
In this paper, the controller design, i.e. the determining of  , 1  , and D K , is achieved by minimizing a novel performance index.In the literature, for tuning PID controller parameters, some performance indices in frequency domain such as Integrated Absolute Error (IAE), Integral of Squared-Error (ISE), and the Integrated of Time-Weighted-Squared-Error (ITSE) are introduced (Westcott (1954), Krohling (2001)).The proposed integral performance indices have their own advantages and disadvantages (Gaing (2004)).Here, new performance index in time domain is presented for evaluating the adaptive controller.This performance index is constituted by three terms.The first term deals with control input constraints, and the second term considers time response of closed-loop system; in addition, the third one exhibits total energy consumption.To satisfy control input constraints, Summation of Integral of Violated Control Law (SIVCL) should be minimized for each control input.
is the number of actuators and T is simulation time span.i u denotes control law which is sent to the th i actuator, also max i u and min i u are upper and lower bounds of the th i actuator.
i a is weighting factor which depends on unit and order of magnitude of different terms involved in SIVCL.There are many control laws ( i u ) that make SIVCL equal to zero; however, the fastest and optimal response is desired.In order to reach this goal, Summation of Settling Time (SST) of all outputs is considered as second term in performance index according to Eq. ( 29).
i c denotes weighting factor.To have an optimal tradeoff between actuators limits, fast and optimal transient responses, J is defined as: Minimization of this performance index, 1 ( , , ) , by the PSO algorithm satisfies the control system objectives.Importance of each term and magnitude of weighting coefficients in performance index are determined by designer requirements.

Adaptive Control Parameters Optimization
In order to reach the control objectives, adaptive control parameters should be determined by the Particle Swarm Optimization (PSO) algorithm.PSO is a computational technique which was introduced by Kenndy and Eberhart (1995) based on social interaction activities.The PSO algorithm begins with selection of random candidate solution known as particle.Particles shift toward the best solution by randomized velocity iteratively.Each particle's movement depends on its local best and global best known positions in the search space, which are updated as better positions found by other particles.This is believed as the explanations how the swarm move toward the best solutions.
In this problem, the final aim is determination of the best settling time under constraints on control law.In fact, constrained problem is changed with optimization problem which will be solved by the PSO algorithm.Basic elements of PSO in solving constrained optimization problems can be presented as follows.
 Particle, ( ) X t : It is a solution represented by m-dimensional vector where m denotes t numbers of optimized parameters.The th j particle at the th t iteration can be expressed as: x t is the position of the  Population, ( )  Velocity, ( ) V t : The velocity of the th j particle at the th t iteration is determined by an m- dimensional vector as:  where ( ) k j v t is the velocity of the th k optimized parameter for the th j particle.Hence the vector of velocity is according the following expression: Latin American Journal of Solids and Structures 14 (2017) 1040-1063  Inertia weight, ( ) w t : It is a parameter that controls the impact of previous velocities in current velocities. Individual best, ( ) best X t : The value of performance index changes due to particles' movement through the search space.The best position which is related with the best value faced so far is known as individual best.Therefore if ( ( )) ( ( )) The vector of individual best can be expressed as:  Stopping criteria: There are different methods to stop the search process in the PSO algorithm.
In this investigation, it's terminated after specified iterations.The computational flow of algorithm can be described in the following steps Step 1: Set the iteration counter equal to zero, and generate (0) j X randomly.The Velocity of each particle at 0 t  must be equal to zero.Evaluate all particles with defined performance index, and set each particle as its individual best, and determine global best.
Step 2: Update the iteration counter.
Step 3: Update the inertia weight: ( ) Where  is inertia weight damping ratio.
Step 4: Update the velocity of the th j particle according the following equation: Where 1  and 2  are personal and global learning coefficients respectively.1 r and 2 r are uniformly distributed random numbers in   0 1 .
Step 5: Change each particle's position with updated velocities from previous step as follows.
Step 7: Determine individual best of each particle, global best position.

Nonlinear Disturbance Observer Design
This subsection discusses the derivation of nonlinear disturbance observer (NDO) (Chen and Chen (2010), Li et al. (2014), Chen and Mei (2011)) which constitutes one of the most important subjects of this paper.For this, the mathematical model of a quadrotor (fully actuated subsystem 2  ) is represented in the following matrix form: x , u and d denote respectively state, input, and external disturbance vector as follows 1 2 3 4 5 6 7 8 Using equations ( 8) and ( 41), one can obtain the nonlinear function It is supposed that the disturbance d is generated by a linear exogenous system Where is the state vector of the exogenous system.
The matrix A and C are as follows.

and ( )
L x stands for the nonlinear gain function of observer which should be designed.By using Eq. ( 40) and Eq. ( 45) becomes Generically, quadrotors' platform encompasses diverse sensors such as accelerometers, gyroscopes, magnetometers, global position system (GPS) module, and so forth; nevertheless, none of these sensors is capable to feedback the derivative of states ( x  ) to observer; hence, the disturbance observer is not feasible for practical usages.In order to tackle this problem, an auxiliary variable is defined as  x is the observer function needed to be designed.The observer gain is then determined by x. Using Eq. ( 46) and Eq. ( 47) the derivative of  can be written as Using the equations ( 43), (47), and (48), we can obtain observation error dynamics as follows.
Latin American Journal of Solids and Structures 14 (2017) 1040-1063 Where 1  is a small positive scalar.observer V for observer error dynamic Eq. ( 49) is defined as P is positive definite matrix.Taking the time derivative of Eq. (57) along augmented system Eq.( 56), and using Eq. ( 59), it follows that From Eq. ( 58), we know that ; furthermore, based on the proof presented in Li et al. (2014), where  is small positive scalar.Conse- quently, the time derivative of Lyapunov candidate yields According to Eq. ( 61) and a method described in Isidori (1995), all state and observer error converge to origin as t   .

SIMULATION RESULTS
In this section, to verify effectiveness of the investigated method in presence of parametric uncertainties, external disturbance, and control input constraints, three simulations are presented.In the first simulation, the effects of time-varying parameters on tracking performance is detected.In the second, the robustness issue of nonlinear observer against external disturbance is studied.Last simulation focuses on hovering maneuver; also, in this subsection, the effects of parametric uncertainties and disturbance are simultaneously studied in presence of input constraints.

Simulation 1
In order to demonstrate robustness of presented control methods, time-varying parameters are utilized in this simulation test.The mass and moments of inertia are increased every 10 seconds.The initial altitude and attitude of quadrotor for this simulation are 0 meter and [10 10 10]  T degrees respectively.
100% uncertainty in the mass and inertia matrix is considered.Since r J for this quadrotor has small value, the term r r J  is negligible compared with body gyroscopic effects.Simulation of altitude and attitude tracking is performed with sinusoidal reference signals ( 10 sin(0.5 ) ).The responses of system are illustrated in Figure 2. It can be seen that there is no problem with controller to make the Latin American Journal of Solids and Structures 14 (2017) 1040-1063 outputs track the desired reference trajectories fast and precisely.Figure 3 displays estimation of the quadrotor parameters.As can be seen from the results, estimator precisely estimates unknown parameters and fixes them bounded.

Simulation 2
In this case, the capability of nonlinear disturbance observer is verified through a numerical simulation of the quadcopter in hovering flight when external disturbance imposed on system for 7.5 seconds.It is clear that the solution obtained for 1 It can be seen that despite starting from initial random particles far from the best final values, the PSO algorithm is able to make particles reach to the best global solutions.These results show that the PSO algorithm can search optimal adaptive controller parameter quickly and efficiently.Figures 8 and 9 show that the outputs properly reach desired setpoints, and the control inputs are continuous and limited as desired, and reasonable for the purpose of practical implementation.The real-time implementation of the proposed controller will be investigated in the future works.

CONCLUSION
In this work, the altitude and attitude stabilization and tracking problem of quadrotor helicopter are addressed.The adaptive control method is utilized to overcome the lack of exact knowledge about the robot parameters.Using the PSO algorithm, optimal adaptive controller parameters are achieved.
The optimal adaptive controller is augmented with nonlinear disturbance observer to attenuate external exogenous disturbance effects.Based on numerical results, robust optimal adaptive controller has a prominent ability to stabilize nonlinear dynamic system of quadrotor, force the states to follow desired reference signals, and find optimal solution for the tracking problem without control input saturation.Dynamic convergence behavior of all particles in population demonstrates that the aforementioned method can perform an efficient search for the optimal adaptive controller parameters; in addition, compatibility of the proposed performance index with the problem results in fast global convergence to optimal solution.
denotes inertia matrix ( xx I , yy I , and zz I are the inertias of the quadrotor), of the th i rotor.b and d are thrust and drag factors respectively, and L stands for factor, and i St stands for the settling time of the th i outputs.Summation of Lower Riemann Sum (SLRS) of control laws is selected as third part of performance index function.10 given known matrices.For this problem, four different disturbance sources are assumed.

Figure 2 :
Figure 2: Altitude and attitude control with a sinusoidal desired output in presence of uncertainty but without external disturbance.

Figure 3 :
Figure 3: Estimation of time-varying unknown parameters of quadrotor.

0[
2sin1 4 cos1 0.25sin1 0.75cos1 0.05sin1 0.1sin1 00 .0 5 ] T   is initialized value for exogenous system.Simulation results are presented in Figures4, 5, and 6.Figure4illustrates that the observer effectively estimates unknown disturbance.It is worthwhile to note that the convergence rate severely depends on magnitude of observer gains.The large gains lead to faster convergence and no excessive control effort is required for quick estimation of disturbance.The height and attitude responses for stabilizing the flying robot before and after of disturbance effects are shown in Figure5.It is axiomatic that the external disturbances considerably decline the stabilizing performance, which results in oscillatory response in altitude and attitude.Implying NDO, however, in conjunction with adaptive control diminishes the effects of external disturbance and parametric uncertainties; in addition, it guarantees stable flight without large deviation from starting conditions.The effects of external disturbance on quadrotor position are depicted in Figure6.It is obvious that the applied composite controller prevents quadrotor from large position deviation from origin.Small deviation from set point, shown with blue line in XY graph in figure6, is because of large disturbance possess attitude.Since dynamic of x and y components of position are intertwined with the dynamic of attitude dynamic, one can expect such small deviation in position.

Figure 7 :
Figure 7: PSO algorithm's Outputs for 1 convergence of particles to the best answers is represented in Figure7 (b, c, and d).

Figure 8 :
Figure 8: Simulation results of the setpoint tracking.

Figure 9 :
Figure 9: Angular speed of rotors which are confined in aforementioned bounds.

Table 1 :
Parameters of quadrotor Latin American Journal of Solids andStructures 14 (2017) 1040-1063 represents measurable disturbance originated from r  with unknown bound.( ) H x is the system inertia matrix, ( , ) C x x  contains centrifugal and Coriolis torques, and ( ) g x indicates the vector of gravitational torques.( ) H x , ( , ) C x x  , and ( ) g x are presented as follows.
Since objective function of this problem is highly nonlinear and probably non-convex, the PSO algorithm may be trapped in local optima.To tackle this problem, feasible region should be divided to subregions in which the PSO algorithm independently searches the best answer, and then among a set of detected controller parameters, control designer selects the answer, which results in minimum cost function, as global optima.It is not problematic issue in offline application although this approach increases the complexities of solution.
Remark 1: For  , 1  and 0 D K  , Lyapunov theory and Barbalat's lemma guarantee stability and convergence of tracking error to zero respectively.Since PSO algorithm searches adaptive controller Latin American Journal of Solids and Structures 14 (2017) 1040-1063parameters in positive real domain, closed-loop system will remain stable, and outputs track references signals.Remark 2: )]

Table 2 :
Best solution obtained using the PSO algorithm with different weighting factor values.
i LRC i  is Lower Riemann of Control Input for rotors.Latin American Journal of Solids and Structures 14 (2017) 1040-1063