Saturated Trajectory Tracking Controller in the Body-Frame for Quadrotors

: This paper introduces a quadrotor trajectory tracking controller comprising a steady-state optimal position controller with a normed input saturation and modular integrative action coupled with a backstepping attitude controller. First, the translational and rotational dynamical models are designed in the body-fixed frame to avoid external rotations and are partitioned into an underactuated position system and a quaternion-based attitude system. Secondly, a controller is designed separately for each subsystem, namely, (i) the position controller synthesis is derived from the Maximum Principle, Lyapunov, and linear quadratic regulator (LQR) theory, ensuring the global exponential stability and steady-state optimality of the controller within the linear region, and global asymptotic stability is guaranteed for the saturation region when coupled with any local exponential stable attitude controller, and (ii) the attitude system, with the quaternion angles and the angular velocity as the controlled variables, is designed in the error space through the backstepping technique, which renders the overall system, position, and attitude, with desirable closed-loop properties that are almost global. The overall stability of the system is achieved through the propagation of the position interconnection term to the attitude system. To enhance the robustness of the tracking system, integrative action is devised for both position and attitude, with emphasis on the modular approach for the integrative action on the position controller. The proposed method is experimentally validated on board an off-the-shelf quadrotor to assess the resulting performance.


Introduction
In recent years, quadrotors have emerged as versatile aerial platforms, with applications spanning from surveillance and inspection to aerial photography and autonomous exploration.The inherent agility and maneuverability of these vehicles have rendered them indispensable in various fields [1,2].To fully exploit the capabilities of quadrotors in tracking tasks, precise and robust control strategies are paramount.The trajectory tracking problem for quadrotors is challenging due to their nonlinear dynamics and underactuated nature, and many control strategies have been devised [2,3].
With the purpose of taking the maximum benefit of the capabilities of these vehicles for tracking purposes, numerous controllers have been proposed [4][5][6].In [7], successful implementation of linear control techniques, including the linear quadratic regulator (LQR), was achieved for low-speed indoor flights.However, these linear techniques often rely on the linearization of quadcopter dynamics, which can limit their ability to fully explore the vehicle's flight envelope and establish a clear region of attraction for stability.In the literature, various nonlinear approaches are available, including feedback linearization [8], backstepping [9], differential flatness [10], and sliding mode control.
Nowadays, state-of-the-art research in quadrotor control blends hierarchical control with geometric feedback.This method entails the creation of a virtual feedback law at the acceleration level, which is subsequently extended to the inner attitude controller.A geometric approach for trajectory tracking with thrust vectoring capabilities is proposed in [11], where position tracking can be achieved by easing attitude tracking requirements.Asymptotic stability is proved for the entire system.In [12], the stability of the system is tackled through integral input-to-state stability.This method yields a quasi-time-optimal control law, which achieves global asymptotic stabilization for any initial position.As a result, it enhances transient performance compared with existing control designs.Recent significant contributions to quadrotor control, which combine hierarchical control with geometric feedback, can be found in [13][14][15][16].
The backstepping method has been extensively applied in various studies to control underactuated quadrotors, as documented in [17][18][19].In these approaches, the attitude error is skillfully derived either from the error in the thrust direction or through traditional backstepping techniques.This results in the interconnection term being backstepped into the attitude control law, effectively canceling its influence.
In the field of quadrotor control, effectively handling system dynamics within the constraints of actuator limits is of paramount importance.The intrinsic underactuated nature of quadrotors necessitates sophisticated control strategies that can handle the nonlinear constraints imposed by actuator saturation.The formulation of control laws incorporating saturation for quadrotors typically aims to avert the singularity linked with a null thrust value, thereby ensuring well-posed attitude references [14,16,20].This approach is critical, as it ensures that the system remains robustly stable and bounded, regardless of the given trajectory.Moreover, such control laws can provide large regions of attraction with disturbances rejection capability in a closed-form solution [21].A recently emerging alternative strategy is the Explicit Reference Governor (ERG), which has gained traction in the field.This Lyapunov-based scheme acts as an add-on to prestabilized systems, from which the reference commands are modified to ensure the handling of constraints [22].The ERG closed-form solutions are particularly advantageous for constraint applications with limited computational resources but may lead to conservative solutions [23].This method has been successfully applied to underactuated Unnamed Air Vehicles (UAVs), demonstrating its efficacy in real-time constraint management [24,25].
This work presents a quadrotor trajectory tracking controller that integrates a steadystate optimal position controller with normed saturated input and a backstepping attitude controller.This approach is characterized by a blend of inner-outer loop theory and backstepping, enhancing transient performance.The dynamical models for translation and rotation are designed in the body-fixed frame to prevent external rotations (algebraic transformations to convert sensor data into inertial coordinates) and are split into underactuated position and quaternion-based attitude systems.Separate controllers are crafted for each subsystem.The position controller is synthesized with a modular integrative action using the Maximum Principle, Lyapunov, and linear quadratic regulator theory, ensuring global exponential stability and steady-state optimality in the linear region, with global asymptotic stability (GAS) guaranteed in the saturation region.On the other hand, the attitude system, with quaternion angles and angular velocity as controlled variables, is developed using the backstepping technique, ensuring desirable almost global closed-loop properties as a consequence of the double cover quaternion representation [26].The position controllers are designed with a normed input saturation with the shown GAS property when coupled with any attitude controller with local exponential stability.This strategic approach ensures stability throughout the operation, preserving both GAS and local exponential stability for the overall system.
The proposed methodology incorporates rigid-body translational dynamics expressed in body-fixed coordinates, which is rooted in several compelling benefits.One of the notable advantages is the avoidance of algebraic transformations to convert sensor data into inertial coordinates.This avoidance is crucial as it helps minimize the amplifica-tion of noise and biases on attitude estimates, a viewpoint highlighted in [27].Moreover, the present approach aligns well with sensor-based strategies and image-based algorithms often utilized in aerial vehicles, as they are designed in a similar frame.This synergy with existing strategies is advantageous, as it allows for seamless integration into various applications, such as visual servoing tasks [28].Additionally, positioning dynamics in the body-fixed frame enable the direct utilization of accelerometer measurements for body acceleration estimation.This capability is vital in helping the system effectively deal with specific unmodeled dynamics, a feature that proves advantageous in real-world scenarios [29].Furthermore, when addressing the position dynamics in the body-fixed frame, the interconnection term primarily depends on gravity and attitude error, thereby making it independent of how the inertial frame is defined [30].This distinctive characteristic offers a fresh perspective on solving the thrust and desired attitude extraction problem, setting it apart from the majority of approaches in the literature.In essence, the decision to employ the body-fixed frame in constructing the controller is grounded in its multifaceted advantages, including noise reduction in attitude estimates, compatibility with existing sensor-based strategies, direct utilization of accelerometer data for robustness, and a unique approach to handling position and attitude dynamic interconnections.
Summarizing, the key contributions of this work encompass several aspects: 1.
A position control law developed in the body-fixed frame, incorporating an input normed saturation with a modular integral control component.This innovative approach ensures global asymptotic stability and presents a novel way of addressing the thrust and desired attitude extraction problem, distinguishing it from the majority of existing methods in the literature.

2.
The proposal of a novel trajectory tracking approach for quadrotors, characterized by almost global asymptotic stability and local exponential stability, exhibiting optimal steady-state properties within the linear region.

3.
The design of the controller is fine-tuned using the linear quadratic regulator framework, which optimizes for energy efficiency over time.
This paper is organized as follows.Section 2 introduces the dynamics and kinematics of the quadrotor.The problem statement and the control objective are also provided.Section 3 provides the design of an optimal steady-state position control law to guarantee asymptotic convergence of the closed-loop error to zero.A saturated control law is devised to overcome singularities during the thrust and reference attitude extraction.The attitude controller design is also given with stability proof and explicitly takes into consideration the coupling between both systems.Section 4 details the implementation of the controllers, as well as some insight into the gain-tuning procedure.Section 5 provides simulation results to assess the proposed tracking law.In Section 6, the experimental results of the proposed control structure onboard an AR.Drone 2.0 are shown and its performance is analyzed.Concluding remarks are pointed out in Section 7.

Notation
Throughout this paper, bold lowercase letters (e.g., x, b) denote column vectors, bold uppercase letters (e.g., X, K) denote matrices, the symbol 0 denotes a matrix of zeros, and I is an identity matrix, both with appropriate dimensions.The vectors e 1 , e 2 , and e 3 denote the unit vectors codirectional with the x, y, and z axes, respectively.The i-th element of the vector x is denoted by x i .The time derivative of s is denoted by ṡ.The Euclidean norm of vectors is denoted as ∥x∥, x ∈ R n .In R 3 , the skew-symmetric matrix of a generic vector a ∈ R 3 is defined as [a×] and given by The vector s expressed in the coordinate frame {E} is denoted by E s , and B I R is a rotation matrix from the frame {I} to {B}.Finally, the transpose operator is denoted by the superscript (•) T .

Quadrotor Dynamics and Kinematics
Consider the quadrotor vehicle model presented in Figure 1.The quadrotor equations are represented in the body-fixed frame since that is usually the frame of the available sensory data.Thus, unnecessary rotations are avoided.The equations of motion that govern the quadrotor [1], used throughout the paper, are given by ṗ where p = p x p y p z T ∈ R 3 is the inertial position, v ∈ R 3 is the linear body velocity, ω ∈ R 3 is the body angular velocity, T ∈ R ≥0 is the total thrust generated by the rotors, m is the total mass of the quadrotor, assumed constant, g is the gravitational acceleration, and R = I B R ∈ R 3 is the rotation matrix from {B} to {I}.Let q = q T v q 4 T = q 1 q 2 q 3 q 4 T ∈ R 4 represent the orientation vector, in terms of quaternions, and λ = ψ θ ϕ T ∈ R 3 denote the equivalent orientation vector in terms of Euler angles.The rotation matrix follows the Euler sequence of rotation Z-Y-X and is given by where c and s are the shorthand forms for the cosine and sine trigonometric functions and are equivalent to 1 − 2 q 2 2 + q 2 3 2(q 1 q 2 − q 0 q 3 ) 2(q 0 q 2 + q 1 q 3 ) 2(q 1 q 2 + q 0 q 3 ) 1 − 2 q 2 1 + q 2 3 2(q 2 q 3 − q 0 q 1 ) 2(q 1 q 3 − q 0 q 2 ) 2(q 0 q 1 + q 2 q 3 ) 1 − 2 q 2 1 + q 2 2 . (4) Similarly, the exploited equations for the angular motion of the quadrotor are defined as follows: where Ω(ω) = −[ω×] ω −ω T 0 , J ∈ R 3×3 corresponds to the quadrotor inertia tensor described in {B}, assumed to be constant, and τ = τ ψ τ θ τ ϕ T denotes the moments applied to the vehicle airframe by the aerodynamics of the rotors, described in {B}, which affect the rotation of the vehicle about the B z, B y, and B x axes, respectively.It is assumed that p, v, q, and ω are available from the sensor set.The rigid-body equations of motion of the quadcopter (2) and (5) can be written, in compact state-space form, as and q ω = 0 where and Ω = [0 ω T ] T .The aerodynamic forces are disregarded as a consequence of the low speed of operation.

Problem Formulation
Consider a UAV with kinematics and dynamics given by ( 6) and (7), respectively.Let the desired position p d (t) ∈ R 3 be a curve of class C 4 and the desired yaw ψ d (t) ∈ R be a curve of class C 2 , both with bounded time derivatives.The problem considered herein is that of designing a sensor-based integrated tracking control law for the thrust T and torque τ, which enforces a bound on the thrust force with respect to the position, velocity errors, and an integrative term, ensuring global convergence of the quadrotor position p(t) to the desired position p d (t) and the yaw angle to ψ d (t), minimizing the energy expenditure over time.

Controller Design
In this section, a nonlinear tracking and control law is derived for the tracking problem stated in Section 2.2.The schematic of the proposed quadrotor controller is presented in Figure 2. By virtue of the underactuated nature of the quadrotor system, a hierarchical strategy is employed to achieve the control development.It consists of an inner loop, the attitude control, combined with an outer loop, the position control, which when bounded provides references to the inner loop.Pontryagin's principle and the LQR theory aligned with Lyapunov's theory are at the basis of the controller's design and the stability proof.

Position Control
Suppose that the desired position of the quadrotor represented in the inertial frame is Furthermore, p d must verify some conditions, namely, it is assumed to be continuous and three-times differentiable.The position error is defined as with ṗd = Rv d , where v d ∈ R 3 is the desired velocity in the body-fixed frame.Define the body velocity error as In order to achieve zero steady-state error and to add robustness to model uncertainties, a modular integral feedback is taken into account η = w 1 e p + w 2 Re v , (10) where w 1 ∈ R >0 and w 2 ∈ R ≥0 are user-defined.Hence, by taking the derivative of ( 8)-( 9), considering the position system (6) and the integral dynamics (10), the position tracking system is defined as where and pd is the desired inertial acceleration.The proposed feedback control law is where u * represents the optimal LQR controller for the system (11), K p , K v , and K η are the steady-state LQR gain matrices ∈ R 3×3 corresponding to the fully controllable LTI system Theorem 1.Consider the dynamics of position error described by (11).The closed-loop system that results from applying the feedback law (13) is globally exponentially stable.Moreover, the feedback law is steady-state optimal in the sense that it minimizes the cost function where Q p , Q v , and Q η ∈ R 3×3 are positive semidefinite matrices, R p ∈ R 3×3 is a positive definite matrix, and Proof.The proof follows a similar approach as in the LQR derivation.Applying the Maximum Principle theorem [31] to minimize the cost function (15) subject to the constraint (11) renders the Hamiltonian where λ is the costate vector of variables, and A and B are defined in (11).Applying the Maximum Principle theorem [31], the following necessary conditions are obtained: Hence, the optimal control law is given by To find a solution for u * , λ must be determined.Let λ = Px, where P is a square symmetric matrix, and substitute ( 17) into the necessary conditions to obtain the equation which is the Riccati matrix differential equation.Since system ( 11) is time-varying, P will always vary for any t.Consider the following well-defined Lyapunov transformation Then, as detailed in Appendix A, it is possible to write which is the Riccati equation for the LTI system (14), and Q l is defined in Appendix A. Consequently, at the infinity, Ṗ∞ = 0, and the optimal control law u * (13) can be rewritten as where P = Thereby, u * is the optimal control law that minimizes the cost function ( 15) for the position system (11).The proof of global exponential stability is now immediate.Consider the following well-defined Lyapunov function with the time derivative given by with ≥ 0 and V1 (x) < 0 for an x different from zero, and, by [32] [Theorem 4.9], the origin of the closed-loop error system is globally asymptotically stable.To prove exponential stability, observe the following inequalities where Λ min (Q l ) and Λ max (P ) are the minimum eigenvalue of Q l and the maximum eigenvalue of P, respectively, which are positive.Define α = Λ min (Q l ) Λ max (P ) > 0 and apply ( 25) to (24), which yields V1 (x) ≤ −αV 1 (x), Hence, the origin is globally exponentially stable.
The block diagram of the position controller is depicted in Figure 3.

Position Dynamics
(2) The position dynamics are an underactuated system with one control input and three output variables.The outer loop is responsible for generating not only the desired thrust but also angular references.Noting that u = − T m e 3 + R T (ge 3 − pd ), although T is a system input, the rotation matrix R T is not and cannot be set arbitrarily.Nevertheless, the attitude variable R can be controlled by means of the attitude system (7), and the input moments can be exploited to drive the thrust force in some intended direction.The input u can be rewritten as where the term ∆ ∈ R 3 is a disturbance due to the attitude error control, R d is the desired rotation matrix, and R = R T R d describes the discrepancy between the vehicle attitude and the attitude command.The rotation matrix R verifies [33] R(e q ) = q 2 4e − q T ve q ve I + 2q ve q T ve − 2q 4e [q ve ×] , where e q = q T ve q 4e T ∈ S 3 denotes the unit quaternion error parameterization that is further defined in Section 3.3, which ensures that R(e q ) = R through the mapping R : S 3 → SO(3).The mapping (28) allows to write the perturbation term ∆ as where ϑ = R T (ge 3 − pd ), and provide an upper bound resorting to the spectral norm of ∆ given by which is a strictly increasing class-K function [34].Keep in mind that similar constraints on ∆ can be derived using alternative attitude parameterizations.The desired quaternion q d is obtained by equating u * in (27) with the output of the feedback law u * (13), which gives where R(q ψ d ) is the rotation matrix defined in quaternions that represents the chosen yaw angle, i.e., q ψ d , of the quadrotor, and R(q θϕd ) denotes the desired rotation matrix defined by the equivalent quaternion for a roll rotation followed by a pitch rotation, i.e., q θϕd .The thrust is computed as and the desired quaternion command q θϕd is given by where The time derivative of the desired quaternion (33) given by where The guidance law (33) only specifies the desired roll and pitch, but it can be extended to include the desired yaw.Let q ψ d represent the desired yaw quaternion, then the desired quaternion can be defined as follows: where the operator ⊗ defines the quaternion product, i.e., and Remark 1.The stability proof presented in this section does not take into consideration the coupling term ∆, which is a requisite when position and attitude are coupled together.In Sections 3.2 and 3.3, the coupling term ∆ is addressed while the attitude controller is deduced and the stability analysis for the overall system is discussed.
The reference angular velocity ω d and its time derivatives along the body's x and y axes can be readily expressed as functions of u t , t, and their respective time derivatives.In fact, this can be deduced from the kinematic relationship q = Ξ(q)Ω, which implies that where It is clear that the extracted attitude is time-varying, and that ω d and ωd can be derived using the expressions of ut and üt , which are functions of available signals.

Saturated Control Law
The control law derived in Section 3.1 requires certain conditions to be met.First, due to the nature of the aircraft, the thrust T must be positive, which leads to the following control law requirement ∥u∥ < ∥ge 3 − pd ∥ = ∥t∥.
Secondly, the rotors and fans can only manage a maximum thrust, i.e., T < T max ∈ R >0 , which is equivalent to the next requirement In the context of a well-defined thrust magnitude, the inequalities ∥t∥ 2 − t 2 2 ≥ u t1 2 and 2 can be considered equivalent.Consequently, the last necessary bound for u comes from the quaternion extraction (33) and is given by −u t1 2 + t 2 1 + t 2 3 ≥ 0, which is equivalent to the following inequality In Figure 4, the allowed space for input u considering u * 2 = 0 is depicted in light blue and gray.Due to the complexity of finding a smooth saturation function that guarantees these bounds, one could saturate u within the gray space; see Figure 4 or even to choose the maximum possible radius sphere around the origin.To accomplish the requirements in (43), one may employ the following smooth saturation function: where k > 0 is the saturation limit, and p > 0 adjusts the slope transition between linear and saturated regions; see Figure 5.Given (43) and (44), the saturated control law is designed as follows: where where a ∈ R and c ∈ R can be defined by the user such that c > aK 2 p K −2 v , and ε 1 ∈ R >0 , ε 2 ∈ R >0 define the allowed saturation limits between each saturation function and satisfy Theorem 2. Let the dynamics of the position error be described by (11).Suppose that the equilibrium q ve = 0 of qve = f (q ve ) is GAS and LES and that the gain matrices The closed-loop system that results from applying the saturated feedback law (45) achieves global asymptotic stabilization, driving both position and velocity errors to zero for any c = cI and a = aI chosen such that c > aK 2 p K −2 v .
Proof.The proof can be found in Appendix B. □ Remark 2. The inclusion of the plant integrator η s allows the control system to manage constant uncertainties within the dynamic model.For this particular scenario, the Lyapunov function from the preceding theorem can be adapted to account for a constant bias in the position acceleration dynamics.This adaptation can be achieved by employing the methodologies outlined in [20,21] and adjusting them for the application of normed saturation functions.This implies that the proposed system is capable of handling a disturbance b ∈ R 3 provided that ∥b∥ < ϵ 2 k min .
The preceding theorem guarantees that s(u 1 ) asymptotically converges to zero.This implies that there exists a time t > 0 at which s(u 1 ) tends to u 1 , entering the linear region.Consequently, and assuming that ∆ also tends asymptotically to zero, the closed-loop system (A4) can be reformulated as where Lemma 1.If one of the following inequalities are met: where s = w 2 w 1 k v and, assuming that ∆ tends to zero asymptotically and exponentially locally, then the Lyapunov function V s1 (A8) is positive definite and its derivative negative semidefinite, and the closed-loop system (A4) converges to (49).

Proof. The inequalities come to light by setting
and Then, The graphical illustration in Figure 6 showcases the range of gains concerning the s ratio, from which Theorem 2 and Lemma 1 are satisfied.Theorem 3. Let the dynamics of the position error be described by (11) and assume that ∆ → 0 asymptotically and exhibits exponential local convergence and that the gain matrices K p = k p I, K v = k v I, and K η = k η I, where k p ∈ R >0 , k v ∈ R >0 , and k η ∈ R >0 .Additionally, let R p = I (22), which implies that K p = P T 2 , K v = P 4 , and K η = P T 5 .Choose w 1 = k p and w 2 = k v , such that s = k 2 v k p in Theorem 1, and let where q 1 ∈ R >0 , q 2 ∈ R >0 , and q 3 ∈ R >0 .Then, the origin of the closed-loop system that results from applying the saturated feedback law (45) is globally asymptotically stable and locally exponentially stable as soon as ∥s(u 1 )∥ = ∥u 1 ∥.Moreover, the feedback law is steady-state optimal in the sense that it minimizes the cost function in the linear region.
Proof.The proof can be found in Appendix C. □ Remark 3. The saturation law (45), as stipulated in Theorem 3, ensures global asymptotic stability and local exponential stability for the system (11), with the assumption that ∆ converges to zero asymptotically and locally exponentially.Moreover, it ensures steady-state optimal control within the linear region of the saturation function and enables the computation of the thrust T and the desired quaternion q d for every instance.

Attitude Control-Quaternion
The attitude controller is designed via backstepping to cope with the coupling term ∆.Suppose the desired orientation of the quadrotor is q d = q T vd q 4d T = q 1d q 2d q 3d q 4d T .
The quaternion error is a four-dimensional vector, defined as follows: where q d −1 = [−q T vd q 4d ] T represents the inverse of the estimated quaternion rotation, and the error (57) verifies (28).
Define the angular velocity error vector in the body-fixed frame as where ω d is the desired angular velocity.Take the derivative of ( 57) and (58), and define the attitude tracking system by the kinematic differential equation e ω (59) and by the dynamic equation Apply the following input-to-state linearization control law and rewrite (59)-(60) as follows: Define the vectors of variables and The objective is to drive x 1 and x 2 to zero.Note that, due to the dual quaternion representation, two equilibria exist, i.e., e q = [0 ± 1].To cope with model imbalances, the control is augmented with an integrator state qi = q ve .(64) Remark 4. Incorporating plant integrators endows the control system to effectively handle unchanging uncertainties inherent in the dynamical model.As long as these uncertainties are constant, the control system can sustain its properties of stability in both position and attitude error.Nevertheless, when confronted with nonlinear uncertainties, such as those stemming from biased mass or moment of inertia, additional measures are required.The introduction of nonlinear integrative or adaptive terms into the control system emerges as a valuable recourse, effectively reinstating the stability properties and addressing the effects of these nonlinear uncertainties [21].
The quaternion parameter set is redundant due to the fact that a particular attitude of a rigid body will always have two mathematical representations, involving a 2π rotation about an axis relative to each other [35].Only one of the equilibrium points is considered, i.e., e q = [0 ± 1], as formalized with the assumption that follows.
In order to stabilize the equilibrium points of e q , the signum function, denoted as sgn(x), is introduced sgn Remark 5.In practical scenarios, control continuity is maintained as it hinges on the initial condition q 4e (t 0 ).However, it is important to exercise caution if the initial quaternion error q 4e (t 0 ) is zero and exposed to noise, as this may trigger unwinding behavior.This potential issue can be relaxed through hybrid switching techniques, as detailed in [35,36].Nonetheless, in a well-defined trajectory, it is typically unrealistic to encounter an initial tracking error q 4e that is nearly zero.
A candidate Lyapunov function is devised as where V s2 is defined in (A19), k 1 is a positive gain to tune the effects of the coupling term, and K i ∈ R 3×3 is a positive definite diagonal matrix.
Accounting for the coupling term ∆, the Lyapunov function V s1 within the saturated region, i.e., t < t1, is given by where K ∆ = (x T s P s 0 I T + 1 k c s(u 1 l ) T K v ).Exploiting the property in ( 28) and ( 29), the previous term can be rewritten as follows: where W(x 1 ) = −k 1 x T s Q m x s .The time derivative of V 2 is given by V2 =W(x 1 ) + 2k 1 K ∆ ([q ve ×] + q 4e I)[ϑ×]q ve + sgn(q 4e (t 0 ))q T ve e ω + q T i K i q ve = W(x 1 ) + q T ve sgn(q 4e (t 0 ))e ω + K i q i Applying the backstepping procedure, a new error is defined where K q ∈ R 3×3 >0 is a positive definite gain matrix, and a new Lyapunov function is defined as with time derivative V3 = W(x 1 ) − q T ve K q q ve + x T 3 sgn(q 4e (t 0 ))τ n The actuation τ n can now be set to cancel the coupling terms and to render the Lyapunov function V3 negative semidefinite.Define the actuation as follows: where K ω ∈ R 3×3 >0 is a positive definite gain matrix.
Theorem 4. Let the quadrotor kinematics and dynamics be described by ( 6)-( 7), let p d (t) ∈ C 4 and ψ d (t) ∈ C 2 be the reference trajectory, and consider the transformation of error coordinates x s , x 2 , x 3 , given by (63a), (63b) and (70), respectively.The closed-loop system that results from applying the inputs (45) and (61) achieves trajectory tracking by guaranteeing that the errors x 1 , x 2 , and x 3 converge to zero.Additionally, almost uniform global asymptotic stability of (x s , u 2 l , q ve , q i , e ω ) = 0 is established.
To show uniform asymptotic stability of the equilibrium (x s , u 2 l , q ve , q i , x 3 ) = 0, one can resort to the Nested Matrosov Theorem [37] [Theorem 1].Define the following auxiliary function V 4 = 1 2 e T ω e ω , with time derivative V4 = e T ω τ n .By inspection, τ n and its time derivative are bounded, and within the set Consider the function Finally, consider the last auxiliary function V 6 = sgn(q 4e (t 0 )q T i e ω , which satisfies By setting Y i = 0 for i = 1, 2, 3, 4 and applying the Nested Matrosov Theorem [37] [Theorem 1], almost uniform global asymptotic stability of (x s , u 2 l , q ve , q i , e ω ) = 0 is established, since the closed-loop system exhibits a zero measure point for q T d q = 0 [26].Therefore, by leveraging the Lyapunov coordinate transformation T, it can be asserted that the equilibrium point of the closed-loop system (A4) is also almost globally asymptotically stable.
The preceding theorem establishes global asymptotic stability for e q = [0 sgn(q 4e (t 0 ))] in the context of classical solutions.Nonetheless, it is crucial to exercise caution as this solution is susceptible to arbitrarily small measurement noise near q 4e = 0.This vulnerability is underscored in [36], where it is asserted that initial conditions in close proximity to the discontinuity maintain the state near the discontinuity, implying that the condition q 4e = 0 represents a set of zero measure, rendering the system almost global.This issue can potentially be addressed through hybrid switching techniques, as demonstrated in [38].However, in practical scenarios where the generated path is well-defined, it is unrealistic for the initial tracking error q 4e to be close to 0, and it has been disregarded in this context.
Keep in mind that constructing the Lyapunov function V s1 from scratch is possible by utilizing the Lyapunov transformation T alongside the position system errors e p and e v .This would yield a result akin to applying the transformation at the end of Theorems 2 and 4.However, such an extension of the proof is unnecessary, as the Lyapunov transformation T inherently preserves the system's properties.Remark 6.The current approach utilized the principles of backstepping theory to eliminate the interconnection term between position and attitude.Nevertheless, according to Theorem 2, any attitude controller relying exclusively on an attitude parameterization error with asymptotic and locally exponential stability would render the overall system, i.e., position and attitude, asymptotically stable.

Implementation
As detailed in Section 3.1, the matrix gains K p , K v , and K η are derived using the LQR method applied to the LTI system (14), and they must adhere to the constraints depicted by the shaded region in Figure 6.An alternative approach involves selecting a specific line, such as 1  s , and computing the LQR solution in a manner consistent with Theorem 3. In order to have some similar sensibility on the gains for the attitude system, the following similar LTI system is considered to compute K i , K q , and By performing the closed-loop system on the attitude system consisting of q ve , e ω , and q i , and considering only small angle variations, the following system is devised: where K 1 , K 2 , and K 3 are the LQR gains for the system (78).With some algebraic computations, K i , K q , and K ω are retrieved as follows: where K 1 and K 3 + K 2 should be invertible, and K i , K q , K ω should check positive definiteness.By considering diagonal input and state matrices while performing the LQR, this is most likely the case, but one must always check.The desired angular velocity ω d and its derivative are computed through (39).

Control Application-Steps
A complete step-by-step approach is outlined below to simplify the applicability of the proposed control law: 1.
Begin by computing the controller gains, as outlined in the initial part of Section 4; 2.
Set the maximum thrust T max and choose p and Define the values of ε 1 and ε 2 and calculate the position saturation law, u sat , using the Equation (45).From this, extract the values of T using (32), q d following (36), and also determine ω d and ωd based on (39); 4.

Simulation Results
In this section, simulation results are presented to illustrate the performance of the presented control law.The model was simulated in Simulink at a rate of 100 Hz using the equations of motion ( 6)- (7).The vehicle starts with the states at zero, while the desired position starts at p d (0) = −1 −1 −0.8 T .At 4.5 s, the vehicle takes off, and the position controller is activated at 6 s.Then, the vehicle hovers for around 6 s, and the following lemniscate of Gerono is generated: and it finishes after two laps.
To ensure smooth transitions, a second-order low-pass filter with a natural frequency of 2 Hz is applied to the trajectory.The vehicle mass is m = 0.55 kg, the inertia matrix is J = diag(2.22.9 5.3)10 −3 , the thrust is chosen to be bound by T max = 6.5 N, which implies a k min ≈ 2, the parameter p of the saturation function is set to p = 30, and ε 1 = 0.8.The transition point from the linear to saturated region is ε = 0.8, where | d dx s(x, 0.8k min )| ≈ 0.999.The controller parameters are presented in Table 1, which are used to compute the controller gains, as described in Section 4. The gains for the position controller are selected in accordance with Theorem 3.
Table 1.Controller parameters: Input and state matrices to compute the controller gains, as described in Section 4 and Theorem 3.
1)10 −2 diag(5.5 5.5 5.5) diag(1 1 1) diag(1 1 8)10 −4 0.02 The simulation results are presented in Figures 7-9.In Figure 8, the tracking errors converge exponentially to zero, which shows that the proposed solution is capable of following paths.The transient convergence to the pass is less than 3 s.The controller provides zero position and velocity steady-state errors.The rotor, thrust, and moment actuations can be seen in Figure 9, respectively.As illustrated in the bottom left plot of Figure 9, the position controller saturates during the take-off and stays in the imminence of saturation at the time when the trajectory is given.In comparison, the newly proposed position saturation law exhibits a higher convergence rate when contrasted with a conventional nested saturation law [39], as demonstrated in Figure 10.The notable discrepancy in convergence behavior within the saturated region arises from the unique configuration where the position error term is placed on the outer saturation function, as also performed in [12].This is a departure from the typical nested approach, where the velocity error is positioned on the outer part of the saturation function.Furthermore, the controller is evaluated against the position saturation law described in [40]; see Figure 10.To ensure an equitable assessment, a comparison is made excluding the integrative element of the controller, since the controller proposed in [40] does not have integrative action.The key distinction lies in the saturation approach, where the proposed controller saturates the norm of the input, whereas the controller from [40] applies saturation to each input component individually.The proposed controller exhibits improved convergence rates in the step response, particularly when the step input occurs in one or two directions, as depicted in Figure 10.This enhanced performance can be attributed to the controller's ability to allocate a larger control action when fewer than three directions are saturated, resulting in faster convergence rates.
UAV Reference is positioned on the outer part of the saturation function.Furthermore, the controller is 49 evaluated against the position saturation law described in [42], see Figure 10.To ensure 49 an equitable assessment, the comparison is made excluding the integrative element of the 49 controller, since the controller proposed on [42] does not have integrative action.The key 49 distinction lies in the saturation approach where the proposed controller saturates the norm 49 of the input, whereas the controller from [42] applies saturation to each input component 49

Robustness Comparisons
To rigorously evaluate the proposed controller's effectiveness in counteracting constant biases within the position acceleration dynamics, two distinct scenarios were analyzed.For both scenarios, the system's maximum tolerable bias threshold was set to b < 1, meaning that ϵ 2 k min = 1, with k min = 3.The system is initialized at a zero position and a step p d = 1 1 −0.8 is applied.
In the first scenario, the disturbance is characterized by b = 1 √ 3 [0.95 0.95 − 0.95] T .Comparative results with the robust position controller from [14] are depicted in Figure 11.This figure illustrates the progression of the integrative saturation action, demonstrating that both controllers successfully converge and neutralize the disturbance.While the position error diminishes to zero for both controllers, the one proposed in this work exhibits superior transient performance.One should notice that the controller in [14] does not account for the bounded normed saturation function, which is the reason why each saturation function is applied individually to each vector element and is bounded by such that the bound is preserved for all time.
It is important to observe that the controller described in [14] does not incorporate a bounded normed saturation function.Consequently, the saturation function must be applied separately to each element of the vector and bounded by ϵ 2 k min √ 3 or ϵ 1 k min √ 3 to ensure that the overall bound is maintained at all times.This requirement may be seen as a drawback, as evidenced in the second scenario that follows.The second scenario considers a disturbance b = [0.65 0 − 0.65] T , with ∥b∥ = 0.92.The outcomes of this test are illustrated in Figure 12.The results demonstrate that the proposed controller successfully manages to cope with the disturbance, driving the position error zero.In contrast, the position controller from [14] fails to achieve this, as its saturation function is unable to handle biases in individual elements exceeding 1 √ 3 . Essentially, the utilization of a normed bound function allows the system to more effectively distribute the integrative capacity across each variable.Comparison of the proposed controller with the controller presented in [14].The upper plot demonstrates the integration action compensating for a constant system bias b = [0.65 0 − 0.65] T , with the lower plot illustrating the norm of the position error.

Experimental Results
The experimental results attained with the coupled position ( 32), (45) and attitude controllers (61), on board an AR.Drone 2.0, are presented in Figures 13-15.This particular quadrotor offers the capability to implement custom software that can override the factory control system through a Simulink package [41].This custom software grants direct access to the Pulse Width Modulation (PWM) of each motor.For the experiment, a position tracking system (Qualisys) alongside the onboard IMU of the vehicle was exploited to provide real-time accurate measurements at 100 Hz of position, velocity, quaternion angles, and angular velocity.To allow a fair comparison, the controller parameters and the path generated in the simulation are also used for the real-time experiment.Similarly to the simulation results, the quadrotor starts at 4.5 s from the ground floor and aims to follow a lemniscate path for position and a ramp for the yaw angle.The vehicle starts far off from the initial position in order to trigger saturation of the position control law.In Figure 13, it is shown that the controller was successfully able to track the given path displayed in red.The actual trajectories are displayed in black.The transient convergence to the path occurs within 3 s, and the steady-state error is fewer than 10 cm.The system errors, depicted in Figure 14, converge to the vicinity of zero and remain bounded.The actuations are plotted in Figure 15.The experimental results exhibit an analogous resemblance to the simulation results; see for comparison   Table 2 details the root mean square errors (RMSEs) obtained during the steady state of the trajectory.Furthermore, the entire experiment was recorded, and the result can be seen at https://youtu.be/lg_5mCDDU8s (accessed on 17 April 2024).

Conclusions
This paper presented a tracking controller for a quadrotor with steady-state optimal properties, developed on the body-fixed frame and quaternion space.The controller stabilizes the position and attitude of a rigid body robustly and almost globally asymptotically.Through a specific set of transformation matrices, the positioning system is rendered LTI, allowing the exploitation of linear control techniques that optimally stabilize it.To avoid the physical limitations of the vehicle, during the thrust and attitude references extraction, a saturation law with modular integrative action that bounds the norm of the position input is devised, ensuring GAS and LES for the position system when paired with any controller that achieves LES for the attitude system.At first, the position and attitude are addressed separately.The origin of the saturation position controller exhibits global asymptotic stability in the saturated region and exponential stability in the linear region.The attitude controller, derived from the backstepping technique, tackles the effect of the interconnection term ∆, which results in almost uniform global asymptotic stability of the overall closed-loop system.Experimental and simulation results were shown to assess the performance of the proposed controller.

Appendix B
Proof of Theorem 2. Let position and velocity error dynamics subjected to the input saturation u * = u sat be given by and consider for now there is no inner-loop tracking error, i.e., ∆ = 0.By applying the Lyapunov transformation of coordinates to the closed-loop system (A4) results in the following LTI system with saturation input Accounting for the interconnection term ∆, the system (A6) can be written as where ξ = x p x v , and e represents any parameterization of the attitude error.Notice that ∆(e) is bounded by a class-K function (30) and does not depend on ξ, and that Vs1 (x s , ∆) can be bounded by Λ min (P s ) ∥q ve ∥V s1 . (A13) for some d 1 and d 2 ∈ R.Then, by following the proof in [34] [Theorem 4.7], the fact that the system (A12) is forward complete [42] and due to the assumption that the equilibrium e = q ve = 0 of ė = a(e) is to be GAS and LES, it can be deduced that there exists a time t > 0 for which ∥e∥ exponentially converges to zero.Consequently, there exists a constant α ∈ R >0 and a function γ(•) ∈ K, such that Vs1 (x s , ∆) ≤ max(d 1 , d 2 ) Λ min (P s ) γ(∥q ve (0)∥)e −αt V s1 , (A14) for ∥x s ∥ > 1.This analysis establishes the boundedness of V s1 .Given that V s1 is radially unbounded, the boundedness of V s1 implies the boundedness of ∥ξ∥.Hence, the GAS of the equilibrium point (x s , u 2 l , q ve ) = (0, 0, 0) follows.Therefore, by leveraging the Lyapunov coordinate transformation (A5), it can be asserted that the equilibrium point of the closed-loop system (A4) is globally asymptotically stable.

Appendix C
Proof of Theorem 3. By choosing w 1 = k p and w 2 = k v , then s = k 2 v k p , which satisfies the initial inequality in Theorem 1.As a result, ∥u 1 ∥ tends to zero asymptotically, and there exists a time t > 0 at which the original system (A4) converges to the system (49).
Following the same logic as in the beginning of Theorem 2, if one applies the transformation of coordinates  where the solutions for P 2 , P 4 , and P 5 are given by P 2 =K p = 2q 1 2 I, √ q 3 + 4q 2 + q 3 + 4 2q 1 I, which is a subtly distinct solution compared with the conventional LQR solution, as in this instance the state matrix A ls in (A16) is dependent on matrix P. Hence, if u s l achieves asymptotic stabilization for the system (A16), the stability characteristics are then extended, through the Lyapunov transformation T, to the closed-loop system (49).
A candidate unbounded Lyapunov function for system (A16) is where k t ∈ R >0 .The time derivative of (A19) is given by where M 2 = d du 2l s(u 2l , ε 2 k min ) with eig(M ′ 2 M 2 ) ≤ 1 as in (47).According to Theorem 2 and Lemma 1, s(u 1 l ) enters the linear region in finite time.Meaning that after a t > t 1 , s(u 1 l ) = u 1 l and u2 l = K η u 1 l .Consequently, Vs2 can be written as It follows that the origin of the closed-loop system (A16) is globally exponentially stable.Therefore, leveraging the Lyapunov coordinate transformation T, it can be asserted that for w 1 = k p , w 2 = k v , and K p , K v , and K η given by (55), the equilibrium point of the closed-loop system (49) is globally exponentially stable, as there is time t > t 2 such that s(u 2 , ε 2 k min ) = u 2 , meaning that the closed-loop system will converge to which falls under the systems outlined by Theorem 1.

Figure 6 .
Figure 6.Graphical representation of the inequalities from Lemma 1.The shaded region indicates the allowable region for the modular integral feedback concerning the ratio k p k 2 v .

Figure 7 .
Figure 7. Simulated tridimensional response obtained with the quadrotor during trajectory tracking with the proposed nonlinear control approach, from top to bottom: a) Tridimensional; b) Yaw angle.

Figure 7 .
Figure 7. Simulated tridimensional response obtained with the quadrotor during trajectory tracking with the proposed nonlinear control approach, from top to bottom: tridimensional; yaw angle.

Figure 11 . 1 √ 3 [ 0 .
Figure 11.Comparison of the proposed position controller with the position controller presented in [14].The upper plot demonstrates the integration action compensating for a constant system bias b = 1 √ 3 [0.950.95 − 0.95] T , with the lower plot illustrating the norm of the position error.

Figure 12 .
Figure 12.Comparison of the proposed controller with the controller presented in[14].The upper plot demonstrates the integration action compensating for a constant system bias b = [0.65 0 − 0.65] T , with the lower plot illustrating the norm of the position error.

Figure 13 .
Figure 13.Tridimensional response (upper) and yaw angle response (lower) obtained with the quadrotor during trajectory tracking.

Figure 14 .
Figure 14.Error state variables during the experiment.
Figures 7 and 13 and Figures 9 and 15 .

Figure 15 .
Figure 15.Thrust, moments, and norm of the position saturation control law resulting from the control law implemented during the experimental validation.

P 4
-loop system (49) results in the following LTI system with saturation input , and P 5 represent the LQR gains that minimize the cost function (56).These gains correspond to the matrices derived from the positive definite solution of the following Riccati equation P A ls + A T ls P − 2P B ls B T ls P = −Q s , (A17)

Table 2 .
Root mean square error obtained in the experimental test.