A Novel Trajectory Tracking Control Strategy for Underactuated Quadrotor UAV with Uncertainties and Disturbances

- In this research, a novel position trajectory tracking control architecture has been constructed for an underactuated quadrotor unmanned aerial vehicle (UAV) with uncertainties and disturbances. Primarily, we divide the whole dynamic system into an underactuated position subsystem and a fully-actuated attitude subsystem. For the position subsystem, we have transformed it into a fully-actuated system by con-structing a virtual PD controller, and this controller can render the position tracking error asymptotically stable. Besides, based on the position controller designed for quadrotor UAV, the desired attitudes, i.e. roll, pitch and yaw angles, will be derived. Next, as for the attitude subsystem which is sensitive to uncertainties and external disturbances, a novel robust attitude constraint-following controller is proposed for this aircraft, this attitude controller can not only guarantee the uniform boundedness and uniform ultimate boundedness of constraint deviation, but also does not requiring more information of uncertainties and disturbances except their bounds. Eventually, the simulations have demonstrated a sound tracking performance of our proposed control strategy for quadrotor UAV even in the presence of uncertainties and disturbances.


Introduction
Compared with the conventional fixed-wing unmanned aircraft, the quadrotor UAV possesses the advantages of simplified mechanical structure and spe-cial flying capabilities, for example, vertical take-off and landing (VTOL), hovering over a target and flying through a narrow space [1], therefore, since the first full-scale quadrotor unmanned aerial vehicle in true sense was invented by De Bothezat in 1921 [2], the quadrotor UAV has been applied broadly in terrain mapping, resource exploration and archaeology etc [3]. However, due to the special mechanical structure of this aircraft, the underactuation, high coupling, nonlinearity as well as unmodeled dynamics will arise in the dynamic system of quadrotor UAV inevitably. Moreover, the uncertainties and external disturbances will also exist in the system. As such, the set-point regulation and trajectory tracking control problems of quadrotor UAV have become a challenge in front of researchers, and during last two decades, various advanced control methodologies have been developed to address these problems either theoretically or experimentally [4].
For instance, in [5], the dynamic system of quadrotor UAV was separated into three parts and a full state backstepping technique proposed can not only realize the trajectory tracking control of position and yaw angle but also stabilize the roll and pitch angles to their desired values, however, for the uncertainties and disturbances which will emerge in real circumstances were not considered. Based on the position dynamic system of quadrotor UAV with Lagrangian form, [6] has built an outer loop for position control and an inner loop for attitude control respectively, in addition, neural network method was also introduced to estimate the unknown aerodynamic forces and moments. Similar to the strategy taken in [6], for the sake of achiev-ing a precise tracking control of desired position and yaw angle, a backstepping technique combined with adaptive control has been put forward for quadrotor UAV with mass uncertainties [7], although the inertial uncertainties, external disturbances and aerodynamic forces were neglected during the controller design. In [8], an attitude tracking control objective for quadrotor UAV with external disturbances was realized by adopting quaternion feedback and integral backstepping control technique, in the end, the simulation results indicated that, the angular velocities were sensitive to external disturbances. Different with all the control schemes mentioned above, a novel L 1 adaptive backstepping control technique which was based upon the inner-outer loop architecture has been presented in [9], and in which, the position control loop was mainly designed to deal with position subsystem with uncertainties and disturbances, while the attitude control loop was responsible for stabilizing the attitude angles.
Apart from the literatures aforementioned for quadrotor UAV, aiming at the drawbacks of various robust control methods, such as backstepping control method, adaptive control and even sliding mode control, several synthetic control methodologies have been proposed recently by combining the merits of each single robust control method to further improve system control performance. Specifically, in order to enhance the robustness against uncertainties and disturbances, various advanced sliding mode control methods [10][11][12][13] connected with backstepping technique [14], adaptive control technique [15,16] and even passive control scheme [17] are believed to be a principle trend for the tracking control problems of quadrotor UAV. This is attributed to the sound robustness of sliding mode control against uncertainties and disturbances. For instance, based on the inner-outer loop architecture, to control the position and attitudes of quadrotor UAV accurately, a second order sliding mode control method was put forward in [10] to reject uncertainties and disturbances, [11] proposed an integral backstepping sliding mode control method, but in [12,13,15] and [16], the sliding mode control integrated with the adaptive control was employ to control the position and attitude subsystems of quadrotor UAV. For the same purpose, [14] and [17] have developed the backstepping sliding mode control and passive control method for quadrotor UAV, respectively. Furthermore, in recent years, several other robust or intelligent control approaches, such as LQR [18], feedback linearization [19], neural network [20] and fuzzy logic control [21], were also introduced to the tracking control investigations of quadrotor UAV with uncertainties and disturbances.
Despite extensive remarkable researches have been done for quadrotor UAV, it's not difficult to find that, most of which preferred to put their focus on the inner-outer loop architecture. That is, the position outer loop provides the desired roll and pitch angles for the attitude inner loop, then the controllers devised for these two subsystems will stabilize the tracking errors of all coordinates, and this is a common practice researchers would like to adopt frequently when controlling quadrotor UAV. Therefore, inspired by these previous studies, in this paper, our proposed control is also based on this inner-outer architecture. However, the difference is that, we have advanced a novel servo constraint-following controller for attitude subsystem, instead of taking the aforementioned approaches. The advantages of our proposed control lie in, the constraint motion problems of mechanical systems can be well addressed no matter the constraints are holonomic or nonholonomic [22,23]. Moreover, the sound robustness (i.e. uniform boundedness and uniform ultimate boundedness [24,25]) can be also guaranteed, and this is seldom mentioned in previous studies like illustrated. The principle contributions of our current study are twofold, first, to make the tracking error of position subsystem asymptotically stable, a virtual PD controller is constructed and will be used to derive the desired attitudes. Second, by transforming the trajectory tracking control problem into servo constraintfollowing control problem, we have proposed a novel attitude constraint-following controller for quadrotor UAV to assure a sound robustness against uncertainties and disturbances, and this attitude controller does not require more information of uncertainties except their bounds.
The remaining sections of this paper is arranged as follows. The dynamic modelling of quadrotor UAV is presented in Section 2, Section 3 will show the position controller design. In Section 4, we will illustrate the design of attitude controller which includes servo constraint and robust controller design. In Section 5, we will analyse the simulation results meticulously, and ultimately, the conclusions are summarized in Section 6.

The dynamic modelling
As a classic underactuated mechanical system, the quadrotor UAV has four independent control inputs and six coordinate outputs, for the convenience of describing its motion in 3D space, two coordinate frames are introduced to determine the dynamic system of this aircraft. Like shown in Fig.1, the origin of body frame B{x b , y b , z b } is assumed to be at the gravity center of UAV with z b -axis pointed upwards, and this frame is connected with the earth frame E{x, y, z} by a coordinate transformation matrix. θ, ψ and φ are corresponding to roll, pitch and yaw attitudes of quadrotor UAV in space, respectively.
The motion of this aircraft can be achieved via the cooperation of four rotors mounted on the bracket of quadrotor UAV symmetrically. Owing to the special structure of rotors, the thrusts F i (i = 1, . . . , 4) produced by four motors are always parallel to z b axis, while the counteractive moments M i (i = 1, . . . , 4) produced are always opposite to the orientation of rotors' rotations, both the magnitudes of thrusts and moments can be changed by varying the speeds of rotors. If we expect that the quadrotor UAV moves along the positive direction of x b -axis, it's necessary to increase the speeds of rotor 3 and 4 wile decrease the speeds of rotor 1 and 2 in the meantime, the motion along y b -axis is similar to this mechanism. In addition, to prevent the self-rotation of quadrotor UAV in space, rotor 1 and 3 in one diagonal line are assigned to rotate clockwise, but rotor 2 and 4 will rotate anticlockwise. Before conducting the dynamic modelling for quadrotor UAV in space, we prescribe the rotation order of Euler angles as " In accordance with this arrangement, we can certainly decide the formulation of rotation matrix R from frame B to frame E, which is given as follows R =   cos ψ cos φ sin θ sin ψ cos φ − cos θ sin φ cos θ sin ψ cos φ + sin θ sin φ cos ψ sin φ sin θ sin ψ sin φ + cos θ cos φ cos θ sin ψ sin φ − sin θ cos φ − sin ψ sin θ cos ψ cos θ cos ψ   .
(2.1) Therefore, the thrust components F x , F y and F z in frame E can be obtained from the rotation matrix R and the total thrusts produced by four rotors 4 i=1 F i , that is, According to the principle of force and moment balance, a complete dynamic model of quadrotor UAV in Fig.1 can be written as I : and II : 4) where I and II represent the position and attitude subsystems respectively, m denotes the total mass of quadrotor UAV, J x , J y and J z are the inertial moments with respect to their respective axes in frame B, J r is the inertial moment of each rotor, d stands for the distance from the center of rotor to x b and y b axis, g is the gravitational acceleration, c is a scaling factor from force to torque, K i (i = 1, . . . , 6) denote the drag coefficients, Ω r = −Ω 1 +Ω 2 −Ω 3 +Ω 4 and Ω i (i = 1, . . . , 4) denote the rotational speeds of four rotors. J rψ Ω r and −J rθ Ω r denote the gyroscopic torques and which can be regarded as disturbances because their impacts on dynamic system are very tiny.

Position controller design
and II : Similar to [15], we provide the following PD controller for position subsystem I and Where K px , K py , K pz and K dx , K dy , K dz denote the proportional and derivative parameters respectively, x r ,y r ,z r and φ r are the commanded signals of position and yaw angle given priori for position controller (3.3), the acquired θ d and ψ d are the desired roll and pitch angles for attitude controller to be stabilized. From (3.5), it can be inferred that, the position controller (3.3) will make the tracking errors of position subsystem asymptotically stable.
Since the attitude controller put forward requires the first and second order derivatives of desired attitude angles (i.e. θ d , ψ d ) should be known priori, we calculate the first order derivatives of θ d and ψ d as belowθ (3.7) Regarding the second derivatives of θ d and ψ d , a tracking differentiator which has been frequently exploited in aeronautical engineering [26] is adopted here to compute the second order derivatives of θ d and ψ d numerically,χ attitude subsystem II, taking J x , J y , J z and c as uncertain parameters, then J x =J x + ∆J x , J y =J y + ∆J y , J z =J z +∆J z , c =c+∆c. Rewriting the attitude subsystem II into a generalized mechanical system with the following form where t ∈ R denotes the time variable, σ ∈ Σ ⊂ R ξ denotes an uncertain vector and which may be fast timevarying, the compact set Σ indicates a possible bound of σ with ξ as its dimension, q,q ∈ R n×1 represent the generalized coordinate and velocity, B(q,q, σ, t) ∈ R n×s represents a control input matrix while τ (t) ∈ R s×1 denotes an actual control input, and s ≤ n. If s = n, then (3.9) will degenerate into a fully actuated mechanical system. J(q,q, σ, t) ∈ R n×n , C(q,q, σ, t) ∈ R n×n and G(q,q, σ, t) ∈ R n×1 represent the inertial matrix, the Coriolis force matrix and the gravitational vector, respectively. For the uncertain system parameter matrices J(q,q, σ, t), C(q,q, σ, t), G(q,q, σ, t), and B(q,q, σ, t), we decompose them into the nominal parts and uncer-tain parts, e.g. J(q,q, σ, t) =J(q,q, t) + ∆J(q,q, σ, t), C(q,q, σ, t) =C(q,q, t) + ∆C(q,q, σ, t), G(q,q, σ, t) =Ḡ(q,q, t) + ∆G(q,q, σ, t), (3.10) J(q,q, t),C(q,q, t),Ḡ(q,q, t) andB(q,q, t) in (3.10) represent the nominal parts of system parameters, while ∆J(q,q, σ, t), ∆C(q,q, σ, t), ∆G(q,q, σ, t) and ∆B(q,q, σ, t) are the uncertain parts. Let D(q,q, t) = , then we can rewrite ∆D(q,q, σ, t) as ∆D(q,q, σ, t) = D(q,q, t)E(q,q, σ, t).
Recalling the attitude subsystem II in (3.2), the associated system parameter matrices for (3.10) are given as where τ = [U 2 , U 3 , U 4 ] T denotes system input, q = [θ, ψ, φ] T is the state variable. The gyroscopic torques J rψ Ω r and J rθ Ω r have been considered as external disturbances (i.e. d 1 , d 2 , d 3 ) with known bounds and In analytical dynamics, the constraint can be classified into two categories, i.e. natural constraint and servo constraint, the natural constraint will be met via the constraint force provided by natural environment, but for the servo constraint, extra constraint force should be contributed artificially. Our current research falls into the second class and the trajectory tracking control problem of quadrotor UAV has been transformed into servo constraint-following control problem.
First of all, consider the first order constraints expressed in following form A li (q, t)q i = a l (q, t), l = 1, 2, . . . , s, whereq i is ith element ofq, A li (q, t) and a l (q, t) are both C 1 continuous in q and t. s can be explained as a maximum quantity of constraints that can be imposed on the attitude subsystem (3.9). The expression (4.1) declares clearly that the constraints are put in form of displacements or velocities. Rewrite the constraint (4.1) into matrix form as follows i.e. A(q, t) = [A li (q, t)] s×n , a(q, t) = [a 1 , a 2 , . . . , a s ] T . In order to facilitate stability analysis and robust control design, usually we need to transform the constraint in first order form (4.1) into that in second order By using equations (4.1) and (4.3), the constraint in second order form can be acquired as and which can be further rewritten in matrix form with

Definition 1
The constraint (4.7) with second order form is consistent if there exists at least a solutionq such that (4.7) can be met when A(q, t) and b(q,q, t) are known priori.

Remark 1
The purpose of Assumption 1 is to ensure the desired trajectory reachable. In this paper, to track the desired attitude trajectories accurately, first of all, we need to determine the corresponding constraint in first order (4.2), then by differentiating it with respect to time once again, the second order constraint (4.7) is derived and will be employed to construct a legitimate Lyapunov function for the stabilization and trajectory tracking control problems [27,28].

Robust controller design
Preceding to conduct the robust attitude controller design, we intend to present several assumptions which will be exploited in subsequent derivative process.
Remark 2 Consider a special case when there are no uncertainties in dynamic system, namely σ = 0, thus ω(q,q, σ, t) = 0, the inequality (4.9) will evolve into −1 < ρ ω ≤ 0, then we can definitely choose a proper ρ ω such that (4.9) is satisfied. On the other hand, for the uncertainties in the dynamic system of quadrotor UAV, by continuity we stress that, the effect of uncertainties on the possible offset of J(q,q, σ, t) from J(q,q, t) and B(q,q, σ, t) fromB(q,q, t) will be sustained within a certain threshold and this threshold is unidirectional.

Remark 3
In Assumption 4, all the uncertainties and disturbances have been lumped into the parameter α, and then we bound the uncertain terms by a positive real function Π(α, q,q, t). The function Π(α, q,q, t) can be parametrized by α and α will be attained according to the bounds of uncertainties and disturbances in later developments.
Aim to realize a sound tracking control target for underactuated quadrotor UAV, we put forward a complete robust attitude controller as follows τ (t) =p 1 (q(t),q(t), t) + p 2 (q(t),q(t), t) + p 3 (α, q(t),q(t), t). (4.14) As for the deterministic robust performance given by the proposed control (4.14), we have the following theorem.
Proof : For the sake of simplifying the stability proof, the arguments of functions will be omitted if there are no confusions arising, select a legitimate Lyapunov function as follows taking the derivative of V (β) along the trajectory of (3.9), thusV = 2β T Pβ. Recalling the constraint (4.7) and uncertainty decomposition (3.10), we havė As shown in (4.18),V has been separated into four parts, and the first part disappears due to the p 1 given by Udwadia and Kalaba approach. From now on, we will analyse the remaining parts sequentially, with p 2 in (4.10), the second part ofV will become next, with p 3 in (4.12), Assumption 3 and Rayleigh's principle, then we can obtain and As for the fourth part ofV , with (4.11) in Assumption 4, yield ≤2(1 + ρ ω )||β||Π(α, q,q, t) = 2(1 + ρ ω )||ρ||.
To clarify the design process of robust attitude controller and an overall control architecture for quadrotor UAV, two flowcharts that summarizing the procedures are displayed in Fig. 2 TD: Tracking Differentiator Fig. 3. The control architecture for quadrotor UAV

Numerical simulations
In this section, we will present the simulation results under our proposed control architecture for quadrotor UAV, three classical position tracking trajectories, i.e. the cylindrical helix, elliptic curve and 8-shaped curve, are taken for examples to demonstrate the effectiveness of our proposed algorithm. The commanded signals of position and yaw angle for these three cases are set as follows (5.1) By taking the first and second order derivatives of (5.1) with respect to time, the first order and second order derivatives of commanded signals can be obtained directly, the second order derivativesθ d andψ d will be calculated through the tracking differentiator. Regarding the attitude controller for quadrotor UAV, it's necessary to determine the constraints with first order and second order formats, and to this end, first of all, we want to elaborate several nomenclatures. The desired attitude angle is defined as q d = [θ d ψ d φ r ] T , then the tracking error with desired attitude is e = q − q d . In particular, e θ ,e ψ and e φ corresponds to the tracking errors of three attitude angles. And identically, e x , e y and e z are on behalf of the tracking errors of three translational movements. Aiming to render the initial tracking errors of attitude angles asymptotically stable, we arrange the attitude constraints in following formė = −ζe, where ζ = diag(ζ 1 , ζ 2 , ζ 3 ) is a diagonal positive definite matrix. Rewrite the constraint (5.2) into the formats of (4.2) and (4.7) in Section 4.1, therefore, As a result of several assumptions (Assumption 1-4) have played an important role in controller development, here it's necessary to make further analysis about their correctness. Taking Assumption 1 into account, since A(q, t) in (5.3) is invertible, thus A(q, t)A + (q, t)b(q,q, t) = b(q,q, t) and Assumption 1 will be met. As for Assumption 2, because the matrices J(q,q, σ, t), B(q,q, σ, t) and A(q, t)A T (q, t) are all positive definite for any (q,q, σ, t) ∈ R n × R n × Σ × R, the inverses of these matrices always exist and then Assumption 2 is satisfied. For Assumption 3, as P , A, D, B and E are all diagonal matrices, then ω(q,q, σ, t) in (4.8) will further turn into obviously, there exists a proper ρ ω such that −1 < ρ ω ≤ λ min (ω) σ∈Σ . In this paper, we select ρ ω = λ min (ω)   it's not difficult to find that, the constraint deviation norm decreased fast and then entered into a small region around zero, this phenomenon indicated the uniform boundedness and uniform ultimate boundedness were assured under our proposed control. The tracking errors of each position or attitude coordinate were presented in Fig.5-6, the variation of tracking errors in these figures has validated our declaration in former sections, that is, the tracking errors of attitudes are approximately asymptotically stable. Similar to the results reflected by Fig.5-6, the tracking histories of every position or attitude coordinate were presented in Fig.7-13, and to take on the tracking state in 3D space clearly, Fig.10 has been plotted. The results displayed by all these figures have drawn the conclusion that a sound robustness and tracking precision can be achieved under our proposed algorithm. To evaluate the effectiveness of tracking differentiator (3.8) on calculating second order derivatives of θ d and ψ d , the time histories ofθ d andψ d were also drawn in Fig.14-15, as indicated by these two figures, the variation of these two parameters (i.e.θ d ,ψ d ) was always at a reasonable range, this has further verified the feasibility of tracking differentiator in acquiring derivatives indirectly. With regard to the control efforts devoted by our proposed algorithm, the evolutions of four intermediate control variables (i.e. U 1 -U 4 ) with respect to time have been presented in Fig.16-19, according to U 1 -U 4 , the original control inputs F 1 -F 4 can be obtained straightforward. In the end, for the tracking control of ellipse and 8-shaped trajectories, only the tracking histories in 3D space were shown (see Fig.20-21) due to the similarities with cylindrical helix, and a sound tracking performance under our proposed strategy was also self-evident.

Conclusion
In this paper, based on the inner-outer loop control architecture, a novel position control strategy has been put forward to address the position control problem of quadrotor UAV. The aircraft considered in this research is underactuated and suffering from uncertainties and external disturbances. To realize a sound tracking control performance, first, a virtual PD position controller was designed to drive this aircraft toward its desired position trajectories. Second, with the desired attitudes acquired from this position controller, we have constructed a novel attitude constraint-following controller for attitude subsystem of quadrotor UAV, as a deterministic robust controller, no further information about the uncertainties is required except their bounds. In addition, the uniform boundedness and uniform ultimate boundedness of attitude constraint deviation were also guaranteed under this controller. In the end, the simulation results presented have demonstrated the effectiveness of our proposed control architecture on the trajectory tracking control of underactuated quadrotor UAV.