Saturated Trajectory Tracking Controller in the Body-Frame for Quadrotors

Abstract

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.

1. 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 steady-state 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 amplification 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:

• 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.
• 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.
• 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., $\mathit{x}$, $\mathit{b}$) denote column vectors, bold uppercase letters (e.g., $\mathit{X}$, $\mathit{K}$) denote matrices, the symbol $\mathit{0}$ denotes a matrix of zeros, and $\mathit{I}$ is an identity matrix, both with appropriate dimensions. The vectors ${\mathit{e}}_1$, ${\mathit{e}}_2$, and ${\mathit{e}}_3$ denote the unit vectors codirectional with the x, y, and z axes, respectively. The i-th element of the vector $\mathit{x}$ is denoted by ${\mathit{x}}_i$. The time derivative of $\mathit{s}$ is denoted by $\dot{\mathit{s}}$. The Euclidean norm of vectors is denoted as $\Vert \mathit{x}\Vert$, $\mathit{x}\in {\mathbb{R}}^n$. In ${\mathbb{R}}^3$, the skew-symmetric matrix of a generic vector $\mathit{a}\in {\mathbb{R}}^3$ is defined as $[\mathit{a}\times ]$ and given by

$$[\mathit{a}\times ]=\left[\begin{array}{ccc}0& -a_z& a_y\\ a_z& 0& -a_x\\ -a_y& a_x& 0\end{array}\right].$$

(1)

The vector $\mathit{s}$ expressed in the coordinate frame $\left\{E\right\}$ is denoted by ${}^E\mathit{s}$, and ${ }_I^B\mathit{R}$ is a rotation matrix from the frame $\left\{I\right\}$ to $\left\{B\right\}$. Finally, the transpose operator is denoted by the superscript ${(·)}^T$.

2. Problem Statement

2.1. 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

$$\left\{\begin{array}{c}\dot{\mathit{p}}=\mathit{R}\mathit{v}\hfill \\ \dot{\mathit{v}}=-[\mathit{\omega }\times ]\mathit{v}-\frac{T}{m}{\mathit{e}}_3+{\mathit{R}}^{T}g{\mathit{e}}_3\hfill \end{array}\right.,$$

(2)

where $\mathit{p}={\left[\begin{array}{ccc}{p}_x& p_y& p_z\end{array}\right]}^T\in {\mathbb{R}}^3$ is the inertial position, $\mathit{v}\in {\mathbb{R}}^3$ is the linear body velocity, $\mathit{\omega }\in {\mathbb{R}}^3$ is the body angular velocity, $T\in {\mathbb{R}}_{\ge 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 $\mathit{R}={ }_B^I\mathit{R}\in {\mathbb{R}}^3$ is the rotation matrix from $\left\{B\right\}$ to $\left\{I\right\}$. Let $\mathit{q}={\left[\begin{array}{cc}{{\mathit{q}}_{\mathit{v}}}^T& q_4\end{array}\right]}^T={\left[\begin{array}{cccc}{q}_1& q_2& q_3& q_4\end{array}\right]}^T\in {\mathbb{R}}^4$ represent the orientation vector, in terms of quaternions, and $\mathit{\lambda }={\left[\begin{array}{ccc}\psi & \theta & \varphi \end{array}\right]}^T\in {\mathbb{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

$$\mathit{R}\left(\mathit{\lambda }\right)=\left[\begin{array}{ccc}{c}_{\theta }{c}_{\psi }& s_{\varphi }{s}_{\theta }{c}_{\psi }-c_{\varphi }{s}_{\psi }& c_{\varphi }{s}_{\theta }{c}_{\psi }+s_{\varphi }{s}_{\psi }\\ c_{\theta }{s}_{\psi }& s_{\varphi }{s}_{\theta }{s}_{\psi }+c_{\varphi }{c}_{\psi }& c_{\varphi }{s}_{\theta }{s}_{\psi }-s_{\varphi }{c}_{\psi }\\ -s_{\theta }& s_{\varphi }{c}_{\theta }& c_{\varphi }{c}_{\theta }\end{array}\right],$$

(3)

where c and s are the shorthand forms for the cosine and sine trigonometric functions and are equivalent to

$$\mathit{R}\left(\mathit{q}\right)=\left[\begin{array}{ccc}1-2\left(q_2^2+q_3^2\right)& 2\left(q_1{q}_2-q_0{q}_3\right)& 2\left(q_0{q}_2+q_1{q}_3\right)\\ 2\left(q_1{q}_2+q_0{q}_3\right)& 1-2\left(q_1^2+q_3^2\right)& 2\left(q_2{q}_3-q_0{q}_1\right)\\ 2\left(q_1{q}_3-q_0{q}_2\right)& 2\left(q_0{q}_1+q_2{q}_3\right)& 1-2\left(q_1^2+q_2^2\right)\end{array}\right].$$

(4)

Similarly, the exploited equations for the angular motion of the quadrotor are defined as follows:

$$\left\{\begin{array}{c}\dot{\mathit{q}}=\frac{1}{2}\mathsf{\Omega }\left(\mathit{\omega }\right)\mathit{q}\hfill \\ \dot{\mathit{\omega }}=-{\mathit{J}}^{-1}[\mathit{\omega }\times ]\mathit{J}\mathit{\omega }+{\mathit{J}}^{-1}\mathit{\tau }\hfill \end{array}\right.,$$

(5)

where $\mathsf{\Omega }\left(\mathit{\omega }\right)=\left[\begin{array}{cc}-[\mathit{\omega }\times ]& \mathit{\omega }\\ -{\mathit{\omega }}^{\mathrm{T}}& 0\end{array}\right]$, $\mathit{J}\in {\mathbb{R}}^{3\times 3}$ corresponds to the quadrotor inertia tensor described in $\left\{B\right\}$, assumed to be constant, and $\mathit{\tau }={\left[\begin{array}{ccc}{\tau }_{\psi }& {\tau }_{\theta }& {\tau }_{\varphi }\end{array}\right]}^T$ denotes the moments applied to the vehicle airframe by the aerodynamics of the rotors, described in $\left\{B\right\}$, which affect the rotation of the vehicle about the ${}^B\mathit{z}$, ${}^B\mathit{y}$, and ${}^B\mathit{x}$ axes, respectively. It is assumed that $\mathit{p}$, $\mathit{v}$, $\mathit{q}$, and $\mathit{\omega }$ 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

$$\left[\begin{array}{c}\dot{\mathit{p}}\\ \dot{\mathit{v}}\end{array}\right]=\left[\begin{array}{cc}\mathit{0}& \mathit{R}\\ \mathit{0}& -[\mathit{\omega }\times ]\end{array}\right]\left[\begin{array}{c}\mathit{p}\\ \mathit{v}\end{array}\right]+\left[\begin{array}{cc}\mathit{0}& \mathit{0}\\ -\frac{T}{m}\mathit{I}& +g{\mathit{R}}^T\end{array}\right]{\mathit{e}}_3$$

(6)

and

$$\left[\begin{array}{c}\dot{\mathit{q}}\\ \dot{\mathit{\omega }}\end{array}\right]=\left[\begin{array}{cc}\mathit{0}& \frac{1}{2}\mathbf{\Xi }\left(\mathit{q}\right)\\ \mathit{0}& -{\mathit{J}}^{-1}[\mathit{\omega }\times ]\mathit{J}\end{array}\right]\left[\begin{array}{c}\mathit{q}\\ \mathsf{\Omega }\end{array}\right]+\left[\begin{array}{c}\mathit{0}\\ {\mathit{J}}^{-1}\end{array}\right]\mathit{\tau },$$

(7)

where $\mathbf{\Xi }\left(\mathit{q}\right)=\left[\begin{array}{c}{q}_4{I}_{3\times 3}+\left[{\mathit{q}}_v\times \right]\\ -{\mathit{q}}_v^{\mathrm{T}}\end{array}\right]$ and $\mathsf{\Omega }={\left[0{\mathit{\omega }}^T\right]}^T$. The aerodynamic forces are disregarded as a consequence of the low speed of operation.

2.2. Problem Formulation

Consider a UAV with kinematics and dynamics given by (6) and (7), respectively. Let the desired position ${\mathit{p}}_d\left(t\right)\in {\mathbb{R}}^3$ be a curve of class ${\mathcal{C}}^4$ and the desired yaw ${\psi }_d\left(t\right)\in \mathbb{R}$ be a curve of class ${\mathcal{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 $\mathit{\tau }$, 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 $\mathit{p}\left(t\right)$ to the desired position ${\mathit{p}}_d\left(t\right)$ and the yaw angle to ${\psi }_d\left(t\right)$, minimizing the energy expenditure over time.

3. 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.

3.1. Position Control

Suppose that the desired position of the quadrotor represented in the inertial frame is ${\mathit{p}}_d={\left[\begin{array}{ccc}{x}_d& y_d& z_d\end{array}\right]}^T$. Furthermore, ${\mathit{p}}_d$ must verify some conditions, namely, it is assumed to be continuous and three-times differentiable. The position error is defined as

$${\mathit{e}}_{\mathit{p}}=\mathit{p}-{\mathit{p}}_d,$$

(8)

with ${\dot{\mathit{p}}}_d=\mathit{R}{\mathit{v}}_d$, where ${\mathit{v}}_d\in {\mathbb{R}}^3$ is the desired velocity in the body-fixed frame. Define the body velocity error as

$${\mathit{e}}_{\mathit{v}}=\mathit{v}-{\mathit{v}}_d.$$

(9)

In order to achieve zero steady-state error and to add robustness to model uncertainties, a modular integral feedback is taken into account

$$\dot{\mathit{\eta }}=w_1{\mathit{e}}_{\mathit{p}}+w_2\mathit{R}{\mathit{e}}_{\mathit{v}},$$

(10)

where $w_1\in {\mathbb{R}}_{>0}$ and $w_2\in {\mathbb{R}}_{\ge 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

$$\left[\begin{array}{c}{\dot{\mathit{e}}}_{\mathit{p}}\\ {\dot{\mathit{e}}}_{\mathit{v}}\\ \dot{\mathit{\eta }}\end{array}\right]=\underset{\mathit{A}}{\underbrace{\left[\begin{array}{ccc}\mathit{0}& \mathit{R}& \mathit{0}\\ \mathit{0}& -[\mathit{\omega }\times ]& \mathit{0}\\ w_1\mathit{I}& w_2\mathit{R}& \mathit{0}\end{array}\right]}}\left[\begin{array}{c}{\mathit{e}}_{\mathit{p}}\\ {\mathit{e}}_{\mathit{v}}\\ \mathit{\eta }\end{array}\right]+\underset{\mathit{B}}{\underbrace{\left[\begin{array}{c}\mathit{0}\\ \mathit{I}\\ \mathit{0}\end{array}\right]}}\mathit{u},$$

(11)

where

$$\mathit{u}=-\frac{T}{m}{\mathit{e}}_3+{\mathit{R}}^T\left(g{\mathit{e}}_3-{\ddot{\mathit{p}}}_d\right)$$

(12)

and ${\ddot{\mathit{p}}}_d$ is the desired inertial acceleration.

The proposed feedback control law is

$$\mathit{u}:={\mathit{u}}^{*}=-\left[\begin{array}{ccc}{\mathit{R}}^T{\mathit{K}}_{\mathit{p}}& {\mathit{R}}^T{\mathit{K}}_{\mathit{v}}\mathit{R}& {\mathit{R}}^T{\mathit{K}}_{\mathit{\eta }}\end{array}\right]\left[\begin{array}{c}{\mathit{e}}_{\mathit{p}}\\ {\mathit{e}}_{\mathit{v}}\\ \mathit{\eta }\end{array}\right],$$

(13)

where ${\mathit{u}}^{*}$ represents the optimal LQR controller for the system (11), ${\mathit{K}}_{\mathit{p}}$, ${\mathit{K}}_{\mathit{v}}$, and ${\mathit{K}}_{\mathit{\eta }}$ are the steady-state LQR gain matrices $\in {\mathbb{R}}^{3\times 3}$ corresponding to the fully controllable LTI system

$$\left[\begin{array}{c}{\dot{\mathit{x}}}_{\mathit{p}}\\ {\dot{\mathit{x}}}_{\mathit{v}}\\ {\dot{\mathit{x}}}_{\mathit{\eta }}\end{array}\right]=\underset{{\mathit{A}}_l}{\underbrace{\left[\begin{array}{ccc}\mathit{0}& \mathit{I}& \mathit{0}\\ \mathit{0}& \mathit{0}& \mathit{0}\\ w_1\mathit{I}& w_2\mathit{I}& \mathit{0}\end{array}\right]}}\left[\begin{array}{c}{\mathit{x}}_{\mathit{p}}\\ {\mathit{x}}_{\mathit{v}}\\ {\mathit{x}}_{\mathit{\eta }}\end{array}\right]+\underset{{\mathit{B}}_l}{\underbrace{\left[\begin{array}{c}\mathit{0}\\ \mathit{I}\\ \mathit{0}\end{array}\right]}}\mathit{u}.$$

(14)

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

$$J={\int }_0^{\infty }\frac{1}{2}\left({\mathit{x}}_1^T\underset{\mathit{Q}}{\underbrace{\left[\begin{array}{ccc}{\mathit{Q}}_{\mathit{p}}& \mathit{0}& \mathit{0}\\ \mathit{0}& {\mathit{R}}^T{\mathit{Q}}_{\mathit{v}}\mathit{R}& \mathit{0}\\ \mathit{0}& \mathit{0}& {\mathit{Q}}_{\mathit{\eta }}\end{array}\right]}}{\mathit{x}}_1+{\mathit{u}}^T\underset{{\mathit{R}}_{\mathit{u}}}{\underbrace{{\mathit{R}}^T{\mathit{R}}_{\mathit{p}}\mathit{R}}}\mathit{u}\right)dt,$$

(15)

where ${\mathit{Q}}_{\mathit{p}}$, ${\mathit{Q}}_{\mathit{v}}$, and ${\mathit{Q}}_{\mathit{\eta }}\in {\mathbb{R}}^{3\times 3}$ are positive semidefinite matrices, ${\mathit{R}}_{\mathit{p}}\in {\mathbb{R}}^{3\times 3}$ is a positive definite matrix, and ${\mathit{x}}_1={\left[{{\mathit{e}}_{\mathit{p}}}^T{{\mathit{e}}_{\mathit{v}}}^T{\mathit{\eta }}^T\right]}^T$.

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 H

$$H=\frac{1}{2}{\mathit{x}}^T\mathit{Q}\mathit{x}+\frac{1}{2}{\mathit{u}}^T{\mathit{R}}_{\mathit{u}}\mathit{u}+{\mathit{\lambda }}^T(\mathit{A}\mathit{x}+\mathit{B}\mathit{u}),$$

(16)

where $\mathit{\lambda }$ is the costate vector of variables, and $\mathit{A}$ and $\mathit{B}$ are defined in (11). Applying the Maximum Principle theorem [31], the following necessary conditions are obtained:

$$\left\{\begin{array}{cc}\hfill \dot{\mathit{x}}& ={\left(\frac{𝜕H}{𝜕\mathit{\lambda }}\right)}^T=\mathit{A}\mathit{x}+\mathit{B}\mathit{u}\hfill \\ \hfill -\dot{\mathit{\lambda }}& ={\left(\frac{𝜕H}{𝜕\mathit{x}}\right)}^T=\mathit{Q}\mathit{x}+{\mathit{A}}^T\mathit{\lambda }\hfill \\ \hfill 0& =\frac{𝜕H}{𝜕\mathit{u}}={\mathit{R}}_{\mathit{u}}\mathit{u}+{\mathit{B}}^T\mathit{\lambda }\hfill \end{array}.\right.$$

(17)

Hence, the optimal control law is given by

$$0=\frac{𝜕H}{𝜕\mathit{u}}={\mathit{R}}_{\mathit{u}}{\mathit{u}}^{*}+{\mathit{B}}^T\mathit{\lambda }\equiv {\mathit{u}}^{*}=-{{\mathit{R}}_{\mathit{u}}}^{-1}{\mathit{B}}^T\mathit{\lambda }.$$

(18)

To find a solution for ${\mathit{u}}^{*}$, $\mathit{\lambda }$ must be determined. Let $\mathit{\lambda }=\mathit{P}\mathit{x}$, where $\mathit{P}$ is a square symmetric matrix, and substitute (17) into the necessary conditions to obtain the equation

$$-\dot{\mathit{P}}=\mathit{P}\mathit{A}+{\mathit{A}}^T\mathit{P}-\mathit{P}\mathit{B}{{\mathit{R}}_{\mathit{u}}}^{-1}{\mathit{B}}^T\mathit{P}+\mathit{Q},$$

(19)

which is the Riccati matrix differential equation. Since system (11) is time-varying, $\mathit{P}$ will always vary for any t. Consider the following well-defined Lyapunov transformation $\mathit{T}$

$$\mathit{P}=\mathit{T}\mathcal{P}{\mathit{T}}^T, \mathit{T}=\left[\begin{array}{ccc}\mathit{I}& \mathit{0}& \mathit{0}\\ \mathit{0}& {\mathit{R}}^T& \mathit{0}\\ \mathit{0}& \mathit{0}& \mathit{I}\end{array}\right].$$

(20)

Then, as detailed in Appendix A, it is possible to write

$$-\dot{\mathcal{P}}=\mathcal{P}{\mathit{A}}_l+{\mathit{A}}_l^T\mathcal{P}-\mathcal{P}{\mathit{B}}_l{{\mathit{R}}_{\mathit{p}}}^{-1}{\mathit{B}}_l^T\mathcal{P}+{\mathit{Q}}_l,$$

(21)

which is the Riccati equation for the LTI system (14), and ${\mathit{Q}}_l$ is defined in Appendix A. Consequently, at the infinity, ${\dot{\mathcal{P}}}_{\infty }=\mathit{0}$, and the optimal control law ${\mathit{u}}^{*}$ (13) can be rewritten as

$$\begin{array}{cc}\hfill {\mathit{u}}^{*}=& -{{\mathit{R}}_{\mathit{u}}}^{-1}{\mathit{B}}^T\mathit{P}\mathit{x}=-{{\mathit{R}}_{\mathit{u}}}^{-1}{\mathit{B}}^T\mathit{T}\mathcal{P}{\mathit{T}}^T\mathit{x}\hfill \\ \hfill =& -\left[{\mathit{R}}^T\underset{{\mathit{K}}_{\mathit{p}}}{\underbrace{{\mathit{R}}_{\mathit{p}}{{\mathcal{P}}_{\mathit{2}}}^T}}{\mathit{R}}^T\underset{{\mathit{K}}_{\mathit{v}}}{\underbrace{{\mathit{R}}_{\mathit{p}}{\mathcal{P}}_{\mathit{4}}}}\mathit{R}{\mathit{R}}^T\underset{{\mathit{K}}_{\mathit{\eta }}}{\underbrace{{\mathit{R}}_{\mathit{p}}{\mathcal{P}}_{\mathit{5}}}}\right]\left[\begin{array}{c}{\mathit{e}}_{\mathit{p}}\\ {\mathit{e}}_{\mathit{v}}\\ \mathit{\eta }\end{array}\right]\hfill \\ \hfill =& -{\mathit{K}}_{nl}{\mathit{x}}_1,\hfill \end{array}$$

(22)

where $\mathcal{P}=\left[\begin{array}{ccc}{\mathcal{P}}_{\mathit{1}}& {\mathcal{P}}_{\mathit{2}}& {\mathcal{P}}_{\mathit{3}}\\ {{\mathcal{P}}_{\mathit{2}}}^T& {\mathcal{P}}_{\mathit{4}}& {\mathcal{P}}_{\mathit{5}}\\ {{\mathcal{P}}_{\mathit{3}}}^T& {{\mathcal{P}}_{\mathit{5}}}^T& {\mathcal{P}}_{\mathit{6}}\end{array}\right]$. Thereby, ${\mathit{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

$$V_1\left(\mathit{x}\right)={\mathit{x}}^T\mathit{T}\mathcal{P}{\mathit{T}}^T\mathit{x},$$

(23)

with the time derivative given by

$$\begin{array}{cc}\hfill {\dot{V}}_1\left(\mathit{x}\right)=& \frac{d}{dt}\left({\mathit{x}}^T\mathit{T}\right)\mathcal{P}{\mathit{T}}^T\mathit{x}+{\mathit{x}}^T\mathit{T}\mathcal{P}\frac{d}{dt}\left({\mathit{T}}^T\mathit{x}\right)\hfill \\ \hfill =& {\mathit{x}}^T\left({\mathit{A}}_{cl}^T\mathit{T}+\dot{\mathit{T}}\right)\mathcal{P}{\mathit{T}}^T\mathit{x}+{\mathit{x}}^T\mathit{T}\mathcal{P}\left({\mathit{T}}^T{\mathit{A}}_{cl}+{\dot{\mathit{T}}}^T\right)\mathit{x}\hfill \\ \hfill =& {\mathit{x}}^T\mathit{T}{\left({\mathit{A}}_l-\mathit{B}{\mathit{K}}_l\right)}^T\mathcal{P}{\mathit{T}}^T\mathit{x}+{\mathit{x}}^T\mathit{T}\mathcal{P}\left({\mathit{A}}_l-\mathit{B}{\mathit{K}}_l\right){\mathit{T}}^T\mathit{x}\hfill \\ \hfill =& -{\mathit{x}}^T\mathit{T}{\mathit{Q}}_l{\mathit{T}}^T\mathit{x},\hfill \end{array}$$

(24)

with ${\mathit{A}}_{cl}=\mathit{A}-\mathit{B}{\mathit{K}}_{nl}$ and ${\mathit{K}}_l=\left[\begin{array}{ccc}{\mathit{K}}_{\mathit{p}}& \mathit{0}& \mathit{0}\\ \mathit{0}& {\mathit{K}}_{\mathit{v}}& \mathit{0}\\ \mathit{0}& \mathit{0}& {\mathit{K}}_{\mathit{\eta }}\end{array}\right]$. Thus, $V_1\left(\mathit{x}\right)\ge 0$ and ${\dot{V}}_1\left(\mathit{x}\right)<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 [32]

$$\left\{\begin{array}{cc}\hfill -{\mathit{x}}^T\mathit{T}{\mathit{Q}}_l{\mathit{T}}^T\mathit{x}& \le -{\Lambda }_{min}\left({\mathit{Q}}_l\right){\mathit{x}}^T\mathit{x}\hfill \\ \hfill {\mathit{x}}^T\mathit{T}\mathcal{P}{\mathit{T}}^T\mathit{x}& \le {\Lambda }_{max}\left(\mathcal{P}\right){\mathit{x}}^T\mathit{x}\hfill \end{array}\right.,$$

(25)

where ${\Lambda }_{min}\left({\mathit{Q}}_l\right)$ and ${\Lambda }_{max}\left(\mathcal{P}\right)$ are the minimum eigenvalue of ${\mathit{Q}}_l$ and the maximum eigenvalue of $\mathcal{P}$, respectively, which are positive. Define $\alpha =\frac{{\Lambda }_{min}\left({\mathit{Q}}_l\right)}{{\Lambda }_{max}\left(\mathcal{P}\right)}>0$ and apply (25) to (24), which yields

$${\dot{V}}_1\left(\mathit{x}\right)\le -\alpha V_1\left(\mathit{x}\right),$$

(26)

Hence, the origin is globally exponentially stable. □

The block diagram of the position controller is depicted in Figure 3.

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 $\mathit{u}=-\frac{T}{m}{\mathit{e}}_3+{\mathit{R}}^T\left(g{\mathit{e}}_3-{\ddot{\mathit{p}}}_d\right)$, although T is a system input, the rotation matrix ${\mathit{R}}^T$ is not and cannot be set arbitrarily. Nevertheless, the attitude variable $\mathit{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 $\mathit{u}$ can be rewritten as

$$\mathit{u}=\underset{{\mathit{u}}^{*}}{\underbrace{-\frac{T}{m}{\mathit{e}}_3+{\mathit{R}}_d^T\left(g{\mathit{e}}_3-{\ddot{\mathit{p}}}_d\right)}}-\underset{\mathbf{\Delta }}{\underbrace{\left({\tilde{\mathit{R}}}^T-\mathit{I}\right){\mathit{R}}^T\left(g{\mathit{e}}_3-{\ddot{\mathit{p}}}_d\right)}},$$

(27)

where the term $\mathbf{\Delta }\in {\mathbb{R}}^3$ is a disturbance due to the attitude error control, ${\mathit{R}}_d$ is the desired rotation matrix, and $\tilde{\mathit{R}}={\mathit{R}}^T{\mathit{R}}_d$ describes the discrepancy between the vehicle attitude and the attitude command. The rotation matrix $\tilde{\mathit{R}}$ verifies [33]

$$\mathcal{R}\left({\mathit{e}}_{\mathit{q}}\right)=\left[\left(q_{4e}^2-{\mathit{q}}_{ve}^{\mathrm{T}}{\mathit{q}}_{ve}\right)\mathit{I}+2{\mathit{q}}_{ve}{\mathit{q}}_{ve}^{\mathrm{T}}-2q_{4e}\left[{\mathit{q}}_{ve}\times \right]\right],$$

(28)

where ${\mathit{e}}_{\mathit{q}}={\left[\begin{array}{cc}{\mathit{q}}_{ve}^T& q_{4e}\end{array}\right]}^T\in {\mathbb{S}}^3$ denotes the unit quaternion error parameterization that is further defined in Section 3.3, which ensures that $\mathcal{R}\left({\mathit{e}}_{\mathit{q}}\right)=\tilde{\mathit{R}}$ through the mapping $\mathcal{R}:{\mathbb{S}}^3\to \mathrm{SO}\left(3\right)$. The mapping (28) allows to write the perturbation term $\mathbf{\Delta }$ as

$$\mathbf{\Delta }=2(\left[{\mathit{q}}_{ve}\times \right]+q_{4e}\mathit{I})\left[{\mathit{q}}_{ve}\times \right]\mathit{\vartheta }$$

(29)

where $\mathit{\vartheta }={\mathit{R}}^T\left(g{\mathit{e}}_3-{\ddot{\mathit{p}}}_d\right)$, and provide an upper bound resorting to the spectral norm of $\mathbf{\Delta }$ given by

$$\Vert \mathbf{\Delta }\Vert \le 2\Vert (\left[{\mathit{q}}_{ve}\times \right]+q_{4e}\mathit{I})\left[{\mathit{q}}_{ve}\times \right]\Vert \Vert \mathit{\vartheta }\Vert =2\Vert \vartheta \Vert \Vert {\mathit{q}}_{ve}\Vert \le 2\max (\Vert \vartheta \Vert )\Vert {\mathit{q}}_{ve}\Vert ,$$

(30)

which is a strictly increasing class-$\mathcal{K}$ function [34]. Keep in mind that similar constraints on $\mathbf{\Delta }$ can be derived using alternative attitude parameterizations.

The desired quaternion ${\mathit{q}}_d$ is obtained by equating ${\mathit{u}}^{*}$ in (27) with the output of the feedback law ${\mathit{u}}^{*}$ (13), which gives

$$\begin{array}{cc}\hfill & {\mathit{u}}^{*}=-\frac{T}{m}{\mathit{e}}_3+{\mathit{R}}_d^T\left(g{\mathit{e}}_3-{\ddot{\mathit{p}}}_d\right)\equiv \hfill \\ \hfill & \underset{{\mathit{u}}_t}{\underbrace{{\mathit{u}}^{*}+\frac{T}{m}{\mathit{e}}_3}}=\mathit{R}{\left({\mathit{q}}_{\theta {\varphi }_d}\right)}^T\underset{\mathit{t}}{\underbrace{\mathit{R}{\left({\mathit{q}}_{{\psi }_d}\right)}^T\left(g{\mathit{e}}_3-{\ddot{\mathit{p}}}_d\right)}}\equiv \hfill \\ \hfill & {\mathit{u}}_t=\mathit{R}{\left({\mathit{q}}_{\theta {\varphi }_d}\right)}^T\mathit{t},\hfill \end{array}$$

(31)

where $\mathit{R}\left({\mathit{q}}_{{\psi }_d}\right)$ is the rotation matrix defined in quaternions that represents the chosen yaw angle, i.e., ${\mathit{q}}_{{\psi }_d}$, of the quadrotor, and $\mathit{R}\left({\mathit{q}}_{\theta \varphi d}\right)$ denotes the desired rotation matrix defined by the equivalent quaternion for a roll rotation followed by a pitch rotation, i.e., ${\mathit{q}}_{\theta \varphi d}$. The thrust is computed as

$$T=m\left[\sqrt{{\mathit{t}}^2-{u_1^{*}}^2-{u_2^{*}}^2}-u_3^{*}\right],$$

(32)

and the desired quaternion command ${\mathit{q}}_{\theta \varphi d}$ is given by

$${\mathit{q}}_{\theta {\varphi }_d}=\left[\begin{array}{c}{c}_2\\ c_1\\ -c_1{c}_2\\ 1\end{array}\right]\frac{1}{\sqrt{c_1^2+1}\sqrt{c_2^2+1}},$$

(33)

where

$$\left\{\begin{array}{cc}{c}_1=\frac{-t_3+\sqrt{-{u_{t1}}^2+t_1^2+t_3^2}}{u_{t1}+t_1}, \hfill & {\Vert \mathit{t}\Vert }^2-{t_2}^2\ge {u_{t1}}^2, g+{\ddot{p}}_{d3}>0\hfill \\ c_2=\frac{-u_{t3}+\sqrt{-{t_2}^2+u_{t2}^2+u_{t3}^2}}{u_{t2}+t_2}, \hfill & {\Vert {\mathit{u}}_t\Vert }^2-{t_2}^2\ge {u_{t1}}^2, {\Vert t\Vert }^2>{u_1^{*}}^2+{u_2^{*}}^2\hfill \end{array}\right.,$$

The time derivative of the desired quaternion (33) given by

$${\dot{\mathit{q}}}_{\theta {\varphi }_d}=\left[\begin{array}{c}{\dot{c}}_2{q}_{\theta {\varphi }_{d4}}+c_2{\dot{q}}_{\theta {\varphi }_{d4}}\\ {\dot{c}}_1{q}_{\theta {\varphi }_{d4}}+c_1{\dot{q}}_{\theta {\varphi }_{d4}}\\ -\left({\dot{c}}_1{c}_2+c_1{\dot{c}}_2\right)q_{\theta {\varphi }_{d4}}-c_1{c}_2{\dot{q}}_{\theta {\varphi }_{d4}}\\ {\dot{q}}_{\theta {\varphi }_{d4}}\end{array}\right],$$

(34)

where

$${\dot{q}}_{\theta {\varphi }_{d4}}=-q_{\theta {\varphi }_{d2}}\frac{{\dot{c}}_1}{c_1^2+1}-q_{\theta {\varphi }_{d1}}\frac{{\dot{c}}_2}{c_2^2+1}.$$

(35)

The guidance law (33) only specifies the desired roll and pitch, but it can be extended to include the desired yaw. Let ${\mathit{q}}_{{\psi }_d}$ represent the desired yaw quaternion, then the desired quaternion can be defined as follows:

$${\mathit{q}}_d={\mathit{q}}_{{\psi }_d}\otimes {\mathit{q}}_{\theta {\varphi }_d},$$

(36)

where the operator ⊗ defines the quaternion product, i.e.,

$${\mathit{q}}_1\otimes {\mathit{q}}_2\triangleq \left[\begin{array}{c}{q}_{2_4}{\mathit{q}}_{\mathit{1}\mathit{v}}+q_{1_4}{\mathit{q}}_{\mathit{2}\mathit{v}}-[{\mathit{q}}_{\mathit{2}\mathit{v}}\times ]{\mathit{q}}_{\mathit{1}\mathit{v}}\\ q_{2_4}{q}_{1_4}-{{\mathit{q}}_{\mathit{2}\mathit{v}}}^T{\mathit{q}}_{\mathit{1}\mathit{v}}\end{array}\right],$$

(37)

and

$${\mathit{q}}_{{\psi }_d}={\left[\begin{array}{cccc}0& 0& sin\left(\frac{{\psi }_d}{2}\right)& cos\left(\frac{{\psi }_d}{2}\right)\end{array}\right]}^T.$$

(38)

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 Section 3.2 and Section 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 ${\mathit{\omega }}_d$ and its time derivatives along the body’s x and y axes can be readily expressed as functions of ${\mathit{u}}_t$, $\mathit{t}$, and their respective time derivatives. In fact, this can be deduced from the kinematic relationship $\dot{\mathit{q}}=\mathbf{\Xi }\left(\mathit{q}\right)\mathsf{\Omega }$, which implies that

$$\begin{array}{cc}\hfill {\mathit{\omega }}_d=& \mathit{W}\mathbf{\Xi }{\left({\mathit{q}}_d\right)}^T{\dot{\mathit{q}}}_d,\hfill \\ \hfill {\dot{\mathit{\omega }}}_d=& \mathit{W}\left(\mathbf{\Xi }{\left({\dot{\mathit{q}}}_d\right)}^T{\dot{\mathit{q}}}_d+\mathbf{\Xi }{\left({\mathit{q}}_d\right)}^T{\ddot{\mathit{q}}}_d\right)=\mathit{W}\mathbf{\Xi }{\left({\mathit{q}}_d\right)}^T{\ddot{\mathit{q}}}_d,\hfill \end{array}$$

(39)

where

$$\mathit{W}=\left[\begin{array}{cccc}0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right],$$

$${\dot{\mathit{q}}}_d={\dot{\mathit{q}}}_{{\psi }_d}\otimes {\mathit{q}}_{\theta {\varphi }_d}+{\mathit{q}}_{{\psi }_d}\otimes {\dot{\mathit{q}}}_{\theta {\varphi }_d},$$

and

$${\ddot{\mathit{q}}}_d=2{\dot{\mathit{q}}}_{{\psi }_d}\otimes {\dot{\mathit{q}}}_{\theta {\varphi }_d}+{\ddot{\mathit{q}}}_{{\psi }_d}\otimes {\mathit{q}}_{\theta {\varphi }_d}+{\mathit{q}}_{{\psi }_d}\otimes {\ddot{\mathit{q}}}_{\theta {\varphi }_d}.$$

It is clear that the extracted attitude is time-varying, and that ${\mathit{\omega }}_d$ and ${\dot{\mathit{\omega }}}_d$ can be derived using the expressions of ${\dot{\mathit{u}}}_t$ and ${\ddot{\mathit{u}}}_t$, which are functions of available signals.

3.2. 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

$$\Vert \mathit{u}\Vert <\Vert g{\mathit{e}}_3-{\ddot{\mathit{p}}}_d\Vert =\Vert \mathit{t}\Vert .$$

(40)

Secondly, the rotors and fans can only manage a maximum thrust, i.e., $T<T_{max}\in {\mathbb{R}}_{>0}$, which is equivalent to the next requirement

$$u_1^{*2}+u_2^{*2}+{\left(u_3^{*}+\frac{T_{max}}{m}\right)}^2>{\Vert g{\mathit{e}}_3-{\ddot{\mathit{p}}}_d\Vert }^2.$$

(41)

In the context of a well-defined thrust magnitude, the inequalities ${\Vert \mathit{t}\Vert }^2-{t_2}^2\ge {u_{t1}}^2$ and ${\Vert {\mathit{u}}_t\Vert }^2-{t_2}^2\ge {u_{t1}}^2$ can be considered equivalent. Consequently, the last necessary bound for $\mathit{u}$ comes from the quaternion extraction (33) and is given by $-{u_{t1}}^2+t_1^2+t_3^2\ge 0$, which is equivalent to the following inequality

$$u_1^{*2}\le {\Vert g{\mathit{e}}_3-{\ddot{\mathit{p}}}_d\Vert }^2-t_2^2.$$

(42)

In Figure 4, the allowed space for input $\mathit{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 $\mathit{u}$ within the gray space; see Figure 4, i.e.,

$$\left\{\begin{array}{c}\Vert \mathit{u}\Vert <\Vert g{\mathit{e}}_3-{\ddot{\mathit{p}}}_d\Vert \hfill \\ u_1^{*2}\le {\Vert g{\mathit{e}}_3-{\ddot{\mathit{p}}}_d\Vert }^2-t_2^2\hfill \\ u_3^{*}>\frac{T_{max}}{m}-\Vert g{\mathit{e}}_3-{\ddot{\mathit{p}}}_d\Vert \hfill \end{array}\right.,$$

(43)

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:

$$\mathit{s}(\mathit{x},k)=\mathit{x}\underset{\kappa }{\underbrace{{\left(\frac{{\Vert \mathit{x}\Vert }^p}{k^p}+1\right)}^{-\frac{1}{p}}}}=\mathit{x}\kappa \left(\mathit{x}\right),$$

(44)

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:

$$\begin{array}{cc}\hfill {\mathit{u}}_{sat}& =-\mathit{s}\left({\mathit{R}}^T{\mathit{K}}_{\mathit{p}}{\mathit{e}}_{\mathit{p}}+{\mathit{R}}^T{\mathit{K}}_{\mathit{v}}\mathit{R}{\mathit{e}}_{\mathit{v}},{\epsilon }_1{k}_{min}\right)-\mathit{s}\left({\mathit{R}}^T{\mathit{K}}_{\mathit{\eta }}{\mathit{\eta }}_{\mathit{s}},{\epsilon }_2{k}_{min}\right)\hfill \\ \hfill & =-\mathit{s}\left({\mathit{u}}_1,{\epsilon }_1{k}_{min}\right)-s\left({\mathit{u}}_2,{\epsilon }_2{k}_{min}\right)\hfill \end{array},$$

(45)

where

$${\dot{\mathit{\eta }}}_{\mathit{s}}=\left[\begin{array}{cc}{{\mathit{K}}_{\mathit{v}}}^T{\mathit{M}}^T& \mathit{I}\end{array}\right]{{\mathit{P}}_{\mathit{s}}}^T\left[\begin{array}{cc}\mathit{R}& \mathit{0}\\ \mathit{0}& \mathit{R}\end{array}\right]{\left[\begin{array}{cc}\mathit{s}{\left({\mathit{u}}_1,{\epsilon }_1{k}_{min}\right)}^T& {{\mathit{e}}_{\mathit{v}}}^T\end{array}\right]}^T+\mathit{c}{\mathit{K}}_{\mathit{v}}{{\mathit{K}}_{\mathit{p}}}^{-1}\mathit{R}\mathit{s}\left({\mathit{u}}_1,{\epsilon }_1{k}_{min}\right),$$

(46)

$$\mathit{M}=\frac{d}{d{\mathit{u}}_1}\mathit{s}({\mathit{u}}_1,k)=\kappa \left({\mathit{u}}_1\right)\left(\mathit{I}-\frac{{\Vert {\mathit{u}}_{\mathit{1}}\Vert }^{\mathit{p}-\mathit{2}}}{{\Vert {\mathit{u}}_{\mathit{1}}\Vert }^{\mathit{p}}+{\mathit{k}}^{\mathit{p}}}{\mathit{u}}_{\mathit{1}}{\mathit{u}}_{\mathit{1}}^{\mathit{T}}\right),$$

(47)

with $0<\mathrm{eig}\left(\mathit{M}\right)\le 1$, ${\mathit{P}}_{\mathit{s}}=\left[\begin{array}{cc}\mathit{a}& -\mathit{b}\\ -\mathit{b}& \mathit{c}\end{array}\right]$, $\mathit{a}=a\mathit{I}$, $\mathit{b}=a{{\mathit{K}}_{\mathit{v}}}^{-1}{\mathit{K}}_{\mathit{p}}$, $\mathit{c}=c\mathit{I}$, where $a\in \mathbb{R}$ and $c\in \mathbb{R}$ can be defined by the user such that $\mathit{c}>\mathit{a}{{\mathit{K}}_{\mathit{p}}}^2{{\mathit{K}}_{\mathit{v}}}^{-2}$,

$$k_{min}=\min \left(\Vert g{\mathit{e}}_3-{\ddot{\mathit{p}}}_d\Vert ,\sqrt{{\Vert g{\mathit{e}}_3-{\ddot{\mathit{p}}}_d\Vert }^2-t_2^2},\frac{T_{max}}{m}-\Vert g{\mathit{e}}_3-{\ddot{\mathit{p}}}_d\Vert \right),$$

(48)

and ${\epsilon }_1\in {\mathbb{R}}_{>0}$, ${\epsilon }_2\in {\mathbb{R}}_{>0}$ define the allowed saturation limits between each saturation function and satisfy ${\epsilon }_1+{\epsilon }_2=1$.

Theorem 2. 

Let the dynamics of the position error be described by (11). Suppose that the equilibrium ${\mathit{q}}_{ve}=\mathit{0}$ of ${\dot{\mathit{q}}}_{ve}=\mathit{f}\left({\mathit{q}}_{ve}\right)$ is GAS and LES and that the gain matrices ${\mathit{K}}_{\mathit{p}}=k_p\mathit{I}$, ${\mathit{K}}_{\mathit{v}}=k_v\mathit{I}$, and ${\mathit{K}}_{\mathit{\eta }}=k_{\eta }\mathit{I}$, where $k_p\in \mathbb{R}$, $k_v\in \mathbb{R}$, and $k_{\eta }\in \mathbb{R}$. 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 $\mathit{c}=c\mathit{I}$ and $\mathit{a}=a\mathit{I}$ chosen such that $\mathit{c}>\mathit{a}{{\mathit{K}}_{\mathit{p}}}^2{{\mathit{K}}_{\mathit{v}}}^{-2}$.

Proof. 

The proof can be found in Appendix B. □

Remark 2. 

The inclusion of the plant integrator ${\mathit{\eta }}_{\mathit{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 $\mathit{b}\in {\mathbb{R}}^3$ provided that $\Vert \mathit{b}\Vert <{ϵ}_2{k}_{min}$.

The preceding theorem guarantees that $\mathit{s}\left({\mathit{u}}_1\right)$ asymptotically converges to zero. This implies that there exists a time $t>0$ at which $\mathit{s}\left({\mathit{u}}_1\right)$ tends to ${\mathit{u}}_1$, entering the linear region. Consequently, and assuming that $\mathbf{\Delta }$ also tends asymptotically to zero, the closed-loop system (A4) can be reformulated as

$$\left[\begin{array}{c}{\dot{\mathit{e}}}_{\mathit{p}}\\ {\dot{\mathit{e}}}_{\mathit{v}}\\ {\dot{\mathit{\eta }}}_{\mathit{s}}\end{array}\right]=\left[\begin{array}{ccc}\mathit{0}& \mathit{R}& \mathit{0}\\ -{\mathit{R}}^T{\mathit{K}}_{\mathit{p}}& -[\mathit{\omega }\times ]-{\mathit{R}}^T{\mathit{K}}_{\mathit{v}}\mathit{R}& \mathit{0}\\ w_1\mathit{I}& w_2\mathit{R}& \mathit{0}\end{array}\right]\left[\begin{array}{c}{\mathit{e}}_{\mathit{p}}\\ {\mathit{e}}_{\mathit{v}}\\ {\mathit{\eta }}_{\mathit{s}}\end{array}\right]-\left[\begin{array}{c}\mathit{0}\\ \mathit{I}\\ \mathit{0}\end{array}\right]\mathit{s}\left({\mathit{R}}^T{\mathit{K}}_{\mathit{\eta }}{\mathit{\eta }}_{\mathit{s}},{\epsilon }_2{k}_{min}\right),$$

(49)

where $w_1=a(k_p{k}_v-k_v^2{k}_p^{-1})+c{k}_v$ and $w_2=a(k_v^2-2k_p)+c(k_p^{-1}{k}_v^2+1)$.

Lemma 1. 

If one of the following inequalities are met:

$$\left\{\begin{array}{c}\frac{1}{2}(1+s-\sqrt{(s-1)(s+3)})<k_p{k}_v^{-2}<\frac{1}{2}(1+s+\sqrt{(s-1)(s+3))}\hfill \\ k_p{k}_v^{-2}<\frac{1}{s-1}\hfill \end{array}\right.$$

(50)

or

$$\left\{\begin{array}{c}{k}_p{k}_v^{-2}<\frac{1}{2}(1+s+\sqrt{(s-1)(s+3)})\hfill \\ k_p{k}_v^{-2}>2\hfill \end{array}\right.$$

(51)

where $s=\frac{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

$$c=k_v^{-1}(a(k_p^2{k}_v^{-1}-k_p{k}_v)+w_1)>a{k}_p^2{k}_v^{-2}\equiv a<\frac{w_1}{k_p{k}_v},$$

(52)

and

$$a=\frac{w_1(k_v^3+k_p{k}_v)-w_2{k}_p{k}_v^2}{k_p^2(2k_v^2-k_p)}>0\equiv \left\{\begin{array}{c}\frac{k_p}{k_v^2}<\frac{1}{s-1},\mathrm{for}\frac{k_p}{k_v^2}<2\hfill \\ \frac{k_p}{k_v^2}>\frac{1}{s-1},\mathrm{for}\frac{k_p}{k_v^2}>2\hfill \end{array}\right..$$

(53)

Then,

$$a<\frac{w_1}{k_p{k}_v}\equiv \frac{1}{2}(1+s-\sqrt{(s-1)(s+3)})<k_p{k}_v^{-2}<\frac{1}{2}(1+s+\sqrt{(s-1)(s+3))}.$$

(54)

□

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 $\mathbf{\Delta }\to \mathit{0}$ asymptotically and exhibits exponential local convergence and that the gain matrices ${\mathit{K}}_{\mathit{p}}=k_p\mathit{I}$, ${\mathit{K}}_{\mathit{v}}=k_v\mathit{I}$, and ${\mathit{K}}_{\mathit{\eta }}=k_{\eta }\mathit{I}$, where $k_p\in {\mathbb{R}}_{>0}$, $k_v\in {\mathbb{R}}_{>0}$, and $k_{\eta }\in {\mathbb{R}}_{>0}$. Additionally, let ${\mathit{R}}_{\mathit{p}}=\mathit{I}$ (22), which implies that ${\mathit{K}}_{\mathit{p}}={{\mathcal{P}}_{\mathit{2}}}^T$, ${\mathit{K}}_{\mathit{v}}={\mathcal{P}}_{\mathit{4}}$, and ${\mathit{K}}_{\mathit{\eta }}={{\mathcal{P}}_{\mathit{5}}}^T$. Choose $w_1=k_p$ and $w_2=k_v$, such that $s=\frac{k_v^2}{k_p}$ in Theorem 1, and let

$$\begin{array}{cccc}\hfill {\mathit{K}}_{\mathit{p}}=& \frac{\sqrt{2q_1}}{2}\mathit{I}, {\mathit{K}}_{\mathit{v}}=\hfill & \hfill \frac{\sqrt{2}}{4}\left(\sqrt{q_3}+\sqrt{4q_2+q_3+4\sqrt{2q_1}}\right)\mathit{I}, {\mathit{K}}_{\mathit{\eta }}=& \frac{\sqrt{2q_3}}{2}\mathit{I},\hfill \end{array}$$

(55)

where $q_1\in {\mathbb{R}}_{>0}$, $q_2\in {\mathbb{R}}_{>0}$, and $q_3\in {\mathbb{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 $\Vert \mathit{s}\left({\mathit{u}}_1\right)\Vert =\Vert {\mathit{u}}_1\Vert$. Moreover, the feedback law is steady-state optimal in the sense that it minimizes the cost function

$$J={\int }_0^{\infty }\frac{1}{2}\left({\mathit{x}}_1^T\underset{{\mathit{Q}}_{\mathit{s}}}{\underbrace{\left[\begin{array}{ccc}{q}_1\mathit{I}& \mathit{0}& \mathit{0}\\ \mathit{0}& q_2\mathit{I}& \mathit{0}\\ \mathit{0}& \mathit{0}& q_3\mathit{I}\end{array}\right]}}{\mathit{x}}_1+{\mathit{u}}^T\mathit{u}\right)dt,$$

(56)

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 ${\mathit{q}}_d$ for every instance.

3.3. Attitude Control—Quaternion

The attitude controller is designed via backstepping to cope with the coupling term $\mathbf{\Delta }$. Suppose the desired orientation of the quadrotor is ${\mathit{q}}_d={\left[\begin{array}{cc}{\mathit{q}}_{\mathit{v}d}^T& q_{4d}\end{array}\right]}^T={\left[\begin{array}{cccc}{q}_{1d}& q_{2d}& q_{3d}& q_{4d}\end{array}\right]}^T$. The quaternion error is a four-dimensional vector, defined as follows:

$${\mathit{e}}_{\mathit{q}}=\mathit{q}\otimes {{\mathit{q}}_d}^{-1}=\left[\begin{array}{c}{\mathit{q}}_{ve}\\ q_{4e}\end{array}\right]\in {\mathbb{S}}^3,$$

(57)

where ${{\mathit{q}}_d}^{-1}={[-{\mathit{q}}_{\mathit{v}d}^T{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

$${\mathit{e}}_{\mathit{\omega }}:=\mathit{\omega }-\tilde{\mathit{R}}{\mathit{\omega }}_d\in {\mathbb{R}}^3,$$

(58)

where ${\mathit{\omega }}_d$ is the desired angular velocity. Take the derivative of (57) and (58), and define the attitude tracking system by the kinematic differential equation

$${\dot{\mathit{e}}}_{\mathit{q}}=\frac{1}{2}\underset{\mathbf{\Xi }\left({\mathit{e}}_{\mathit{q}}\right)}{\underbrace{\left[\begin{array}{c}{q}_{4e}{I}_{3\times 3}+\left[{\mathit{q}}_{ve}\times \right]\\ -{\mathit{q}}_{ve}^{\mathrm{T}}\end{array}\right]}}{\mathit{e}}_{\mathit{\omega }}$$

(59)

and by the dynamic equation

$${\dot{\mathit{e}}}_{\mathit{\omega }}=-{\mathit{J}}^{-1}[\mathit{\omega }\times ]\mathit{J}+{\mathit{J}}^{-1}\mathit{\tau }+[{\mathit{e}}_{\mathit{\omega }}\times ]\tilde{\mathit{R}}{\mathit{\omega }}_d-\tilde{\mathit{R}}{\dot{\mathit{\omega }}}_d.$$

(60)

Apply the following input-to-state linearization control law

$$\mathit{\tau }=\mathit{J}\left({\mathit{J}}^{-1}[{\mathit{\tau }}_{\mathit{n}}+\mathit{\omega }\times ]\mathit{J}-[{\mathit{e}}_{\mathit{\omega }}\times ]\tilde{\mathit{R}}{\mathit{\omega }}_d+\tilde{\mathit{R}}{\dot{\mathit{\omega }}}_d\right)$$

(61)

and rewrite (59)–(60) as follows:

$$\left\{\begin{array}{c}{\dot{\mathit{e}}}_{\mathit{q}}=\frac{1}{2}\mathbf{\Xi }\left({\mathit{e}}_{\mathit{q}}\right){\mathit{e}}_{\mathit{\omega }}\hfill \\ {\dot{\mathit{e}}}_{\mathit{\omega }}={\mathit{\tau }}_{\mathit{n}}\hfill \end{array}\right..$$

(62)

Define the vectors of variables

$${\mathit{x}}_1={\left[{{\mathit{e}}_{\mathit{p}}}^T{{\mathit{e}}_{\mathit{v}}}^T{{\mathit{\eta }}_{\mathit{s}}}^T\right]}^T,$$

(63a)

and

$${\mathit{x}}_2={\left[{\mathit{q}}_{ve}^{T}1-\left|q_{4e}\right|\right]}^T.$$

(63b)

The objective is to drive ${\mathit{x}}_1$ and ${\mathit{x}}_2$ to zero. Note that, due to the dual quaternion representation, two equilibria exist, i.e., ${\mathit{e}}_{\mathit{q}}=[\mathit{0}\pm 1]$. To cope with model imbalances, the control is augmented with an integrator state

$${\dot{\mathit{q}}}_{\mathit{i}}={\mathit{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\pi$ rotation about an axis relative to each other [35]. Only one of the equilibrium points is considered, i.e., ${\mathit{e}}_{\mathit{q}}=[\mathit{0}\pm 1]$, as formalized with the assumption that follows.

Assumption 1. 

The scalar part of the quaternion error $q_{4e}$ maintains its signal for all times, i.e., $sgn\left(q_{4e}\left(t\right)\right)=sgn\left(q_{4e}\left(t_0\right)\right)\forall t$.

In order to stabilize the equilibrium points of ${\mathit{e}}_{\mathit{q}}$, the signum function, denoted as $sgn\left(\mathrm{x}\right)$, is introduced

$$sgn\left(\mathrm{x}\right)=\left\{\begin{array}{cc}\hfill -1,& x<0\hfill \\ \hfill 1,& x\ge 0\hfill \end{array}.\right.$$

(65)

Remark 5. 

In practical scenarios, control continuity is maintained as it hinges on the initial condition $q_{4e}\left(t_0\right)$. However, it is important to exercise caution if the initial quaternion error $q_{4e}\left(t_0\right)$ 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

$$V_2=k_1{V}_{s1}+{\mathit{x}}_2^T{\mathit{x}}_2+\frac{1}{2}{{\mathit{q}}_{\mathit{i}}}^T{\mathit{K}}_i{\mathit{q}}_{\mathit{i}},$$

(66)

where $V_{s2}$ is defined in (A19), $k_1$ is a positive gain to tune the effects of the coupling term, and ${\mathit{K}}_i\in {\mathbb{R}}^{3\times 3}$ is a positive definite diagonal matrix.

Accounting for the coupling term $\mathbf{\Delta }$, the Lyapunov function $V_{s1}$ within the saturated region, i.e., $t<t1$, is given by

$${\dot{V}}_{s1}=-k_1{{\mathit{x}}_{\mathit{s}}}^T{\mathit{Q}}_{\mathit{m}}{\mathit{x}}_{\mathit{s}}-k_1{\mathit{K}}_{\mathbf{\Delta }}\mathbf{\Delta }$$

(67)

where ${\mathit{K}}_{\mathbf{\Delta }}=({{\mathit{x}}_{\mathit{s}}}^T{\mathit{P}}_{\mathit{s}}{\left[\begin{array}{cc}\mathit{0}& \mathit{I}\end{array}\right]}^T+\frac{1}{k_c}\mathit{s}{\left({\mathit{u}}_{1_l}\right)}^T{\mathit{K}}_{\mathit{v}})$. Exploiting the property in (28) and (29), the previous term can be rewritten as follows:

$${\dot{V}}_{1s}=W\left({\mathit{x}}_1\right)+2k_1{\mathit{K}}_{\mathbf{\Delta }}\left([{\mathit{q}}_{ve}\times ]+q_{4e}\mathit{I}\right)[\mathit{\vartheta }\times ]{\mathit{q}}_{ve},$$

(68)

where $W\left({\mathit{x}}_1\right)=-k_1{{\mathit{x}}_{\mathit{s}}}^T{\mathit{Q}}_{\mathit{m}}{\mathit{x}}_{\mathit{s}}$. The time derivative of $V_2$ is given by

$$\begin{array}{cc}\hfill {\dot{V}}_2=& W\left({\mathit{x}}_1\right)+2k_1{\mathit{K}}_{\mathbf{\Delta }}\left([{\mathit{q}}_{ve}\times ]+q_{4e}\mathit{I}\right)[\mathit{\vartheta }\times ]{\mathit{q}}_{ve}\hfill \\ & +sgn\left(q_{4e}\left(t_0\right)\right){\mathit{q}}_{ve}^T{\mathit{e}}_{\mathit{\omega }}+{{\mathit{q}}_{\mathit{i}}}^T{\mathit{K}}_i{\mathit{q}}_{ve}\hfill \\ \hfill & =W\left({\mathit{x}}_1\right)+{\mathit{q}}_{ve}^T(sgn\left(q_{4e}\left(t_0\right)\right){\mathit{e}}_{\mathit{\omega }}+{\mathit{K}}_i{\mathit{q}}_{\mathit{i}}\hfill \\ & +2k_1[\mathit{\vartheta }\times ]\left([{\mathit{q}}_{ve}\times ]-q_{4e}\mathit{I}\right){{\mathit{K}}_{\mathbf{\Delta }}}^T).\hfill \end{array}$$

(69)

Applying the backstepping procedure, a new error is defined

$$\begin{array}{cc}\hfill {\mathit{x}}_{\mathit{3}}=& sgn\left(q_{4e}\left(t_0\right)\right){\mathit{e}}_{\mathit{\omega }}+2k_1[\mathit{\vartheta }\times ]\left([{\mathit{q}}_{ve}\times ]-q_{4e}\mathit{I}\right){{\mathit{K}}_{\mathbf{\Delta }}}^T\hfill \\ & +{\mathit{K}}_{\mathit{q}}{\mathit{q}}_{ve}+{\mathit{K}}_i{\mathit{q}}_{\mathit{i}}\hfill \end{array},$$

(70)

where ${\mathit{K}}_{\mathit{q}}\in {\mathbb{R}}_{>0}^{3\times 3}$ is a positive definite gain matrix, and a new Lyapunov function is defined as

$$V_3=V_2+\frac{1}{2}{{\mathit{x}}_{\mathit{3}}}^T{\mathit{x}}_{\mathit{3}},$$

(71)

with time derivative

$$\begin{array}{cc}\hfill {\dot{V}}_3=W\left({\mathit{x}}_1\right)-{\mathit{q}}_{ve}^T{\mathit{K}}_{\mathit{q}}{\mathit{q}}_{ve}+{{\mathit{x}}_{\mathit{3}}}^T(sgn\left(q_{4e}\left(t_0\right)\right){\mathit{\tau }}_{\mathit{n}}& \\ \hfill +(\mathit{I}+{\mathit{K}}_i){\mathit{q}}_{ve}+2k_1[\dot{\mathit{\vartheta }}\times ]\left([{\mathit{q}}_{ve}\times ]-q_{4e}\mathit{I}\right){{\mathit{K}}_{\mathbf{\Delta }}}^T& \\ \hfill +2k_1[\mathit{\vartheta }\times ]\left([{\dot{\mathit{q}}}_{ve}\times ]-{\dot{q}}_{4e}\mathit{I}\right){{\mathit{K}}_{\mathbf{\Delta }}}^T& \\ \hfill +2k_1[\mathit{\vartheta }\times ]\left([{\mathit{q}}_{ve}\times ]-q_{4e}\mathit{I}\right){{\dot{\mathit{K}}}_{\mathbf{\Delta }}}^T& +{\mathit{K}}_{\mathit{q}}{\dot{\mathit{q}}}_{ve})\hfill \end{array}.$$

(72)

The actuation ${\mathit{\tau }}_{\mathit{n}}$ can now be set to cancel the coupling terms and to render the Lyapunov function ${\dot{V}}_3$ negative semidefinite. Define the actuation as follows:

$$\begin{array}{c}\hfill {\mathit{\tau }}_{\mathit{n}}=sgn\left(q_{4e}\left(t_0\right)\right)(-2k_1[{\stackrel{⃛}{\mathit{p}}}_d\times ]\left([{\mathit{q}}_{ve}\times ]-q_{4e}\mathit{I}\right){{\mathit{K}}_{\mathbf{\Delta }}}^T\\ \hfill -2k_1[\mathit{\vartheta }\times ]\left([{\dot{\mathit{q}}}_{ve}\times ]-{\dot{q}}_{4e}\mathit{I}\right){{\mathit{K}}_{\mathbf{\Delta }}}^T\\ \hfill -2k_1[\mathit{\vartheta }\times ]\left([{\mathit{q}}_{ve}\times ]-q_{4e}\mathit{I}\right){{\dot{\mathit{K}}}_{\mathbf{\Delta }}}^T\\ \hfill -(\mathit{I}+{\mathit{K}}_i){\mathit{q}}_{ve}-{\mathit{K}}_{\mathit{q}}{\dot{\mathit{q}}}_{ve}-{\mathit{K}}_{\mathit{\omega }}{\mathit{x}}_{\mathit{3}}),\end{array}$$

(73)

where ${\mathit{K}}_{\mathit{\omega }}\in {\mathbb{R}}_{>0}^{3\times 3}$ is a positive definite gain matrix.

Theorem 4. 

Let the quadrotor kinematics and dynamics be described by (6)–(7), let ${\mathit{p}}_d\left(t\right)\in C^4$ and ${\psi }_d\left(t\right)\in C^2$ be the reference trajectory, and consider the transformation of error coordinates ${\mathit{x}}_{\mathit{s}}$, ${\mathit{x}}_2$, ${\mathit{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 ${\mathit{x}}_{\mathit{1}}$, ${\mathit{x}}_2$, and ${\mathit{x}}_3$ converge to zero. Additionally, almost uniform global asymptotic stability of $({\mathit{x}}_{\mathit{s}},{\mathit{u}}_{2_l},{\mathit{q}}_{ve},{\mathit{q}}_{\mathit{i}},{\mathit{e}}_{\mathit{\omega }})=\mathit{0}$ is established.

Proof. 

The closed-loop quadrotor error dynamic system, with state variables ${\mathit{x}}_{\mathit{s}}$, ${\mathit{u}}_{{\mathit{2}}_{\mathit{l}}}$, ${\mathit{x}}_{\mathit{\eta }\mathit{s}}$, ${\mathit{x}}_{\mathit{2}}$, and ${\mathit{x}}_{\mathit{3}}$, defined as (63a), (63b), and (70), respectively, with $V_{s1}$ devised according to Theorem 2, admits the Lyapunov function $V_3$ (71) with time derivative

$$\begin{array}{cc}\hfill {\dot{V}}_3=& -k_1{{\mathit{x}}_{\mathit{s}}}^T{\mathit{Q}}_{\mathit{m}}{\mathit{x}}_{\mathit{s}}-{\mathit{q}}_{ve}^T{\mathit{K}}_{\mathit{q}}{\mathit{q}}_{ve}-{{\mathit{x}}_{\mathit{3}}}^T{\mathit{K}}_{\mathit{\omega }}{\mathit{x}}_{\mathit{3}}=Y_1\le 0\hfill \end{array}.$$

(74)

Since ${\dot{V}}_3\le 0$, which implies that $V_3\le V_3\left(0\right)$ is compact, and according to [32] [Theorem 4.8], the set $({\mathit{x}}_{\mathit{s}},{\mathit{u}}_{2_l},{\mathit{q}}_{ve},{\mathit{q}}_{\mathit{i}},{\mathit{x}}_{\mathit{3}})=\mathit{0}$ is uniformly stable. Resorting to the LaSalle–Yoshizawa Theorem [32] [Theorem 8.4], it follows from (74) that ${\displaystyle \underset{t\to \infty }{lim}{\mathit{x}}_{\mathit{s}}\left(t\right)=\underset{t\to \infty }{lim}{\mathit{q}}_{ve}\left(t\right)=\underset{t\to \infty }{lim}{\mathit{x}}_{\mathit{3}}\left(t\right)=\mathit{0}}$ and ${\displaystyle \underset{t\to \infty }{lim}{q}_{4e}\left(t\right)=sgn\left(q_{4e}\left(t_0\right)\right)}$.

To show uniform asymptotic stability of the equilibrium $({\mathit{x}}_{\mathit{s}},{\mathit{u}}_{2_l},{\mathit{q}}_{ve},{\mathit{q}}_{\mathit{i}},{\mathit{x}}_{\mathit{3}})=\mathit{0}$, one can resort to the Nested Matrosov Theorem [37] [Theorem 1]. Define the following auxiliary function $V_4=\frac{1}{2}{{\mathit{e}}_{\mathit{\omega }}}^T{\mathit{e}}_{\mathit{\omega }}$, with time derivative ${\dot{V}}_4={{\mathit{e}}_{\mathit{\omega }}}^T{\mathit{\tau }}_{\mathit{n}}$. By inspection, ${\mathit{\tau }}_{\mathit{n}}$ and its time derivative are bounded, and within the set $Y_1=0$, ${\dot{V}}_4$, yield

$${\dot{V}}_4=-{{\mathit{e}}_{\mathit{\omega }}}^T{\mathit{K}}_{\mathit{q}}{\mathit{e}}_{\mathit{\omega }}=Y_2\le 0\forall \{({\mathit{x}}_{\mathit{s}},{\mathit{u}}_{2_l},{\mathit{q}}_{ve},{\mathit{q}}_{\mathit{i}},{\mathit{x}}_{\mathit{3}})\in {\mathbb{R}}^{18}:Y_1=0\}.$$

(75)

Consider the function $V_5={\mathit{u}}_{2_l}^T{\mathit{x}}_{\mathit{v}}$, with time derivative

$${\dot{V}}_5=-{\mathit{u}}_{2_l}^T\mathit{s}({\mathit{u}}_{2_l},{ϵ}_2{k}_{min})=Y_3\le 0\forall \{({\mathit{x}}_{\mathit{s}},{\mathit{u}}_{2_l},{\mathit{q}}_{ve},{\mathit{q}}_{\mathit{i}},{\mathit{x}}_{\mathit{3}})\in {\mathbb{R}}^{18}:Y_1=0,Y_2=0\}.$$

(76)

Finally, consider the last auxiliary function $V_6=sgn(q_{4e}\left(t_0\right){{\mathit{q}}_{\mathit{i}}}^T{\mathit{e}}_{\mathit{\omega }}$, which satisfies

$$\begin{array}{cc}\hfill {\dot{V}}_6=& sgn(q_{4e}\left(t_0\right){\mathit{q}}_{ve}^T{\mathit{e}}_{\mathit{\omega }}+{{\mathit{q}}_{\mathit{i}}}^T(-\frac{d}{dt}\left(2k_1[\mathit{\vartheta }\times ]\left([{\mathit{q}}_{ve}\times ]-q_{4e}\mathit{I}\right){{\mathit{K}}_{\mathbf{\Delta }}}^T\right)\hfill \\ \hfill & -(\mathit{I}+{\mathit{K}}_i+{\mathit{K}}_{\mathit{\omega }}{\mathit{K}}_{\mathit{q}}){\mathit{q}}_{ve}-({\mathit{K}}_{\mathit{q}}(q_{4e}{I}_{3\times 3}+\left[{\mathit{q}}_{ve}\times \right])+sgn\left(q_{4e}\left(t_0\right){\mathit{K}}_{\mathit{\omega }}\right){\mathit{e}}_{\mathit{\omega }}\hfill \\ & -{\mathit{K}}_{\mathit{\omega }}2k_1[\mathit{\vartheta }\times ]\left([{\mathit{q}}_{ve}\times ]-q_{4e}\mathit{I}\right){{\mathit{K}}_{\mathbf{\Delta }}}^T-{\mathit{K}}_{\mathit{\omega }}{\mathit{K}}_i{\mathit{q}}_{\mathit{i}})\hfill \\ \hfill =& -{{\mathit{q}}_{\mathit{i}}}^T{\mathit{K}}_{\mathit{\omega }}{\mathit{K}}_i{\mathit{q}}_{\mathit{i}}=Y_4\le 0\forall \{({\mathit{x}}_{\mathit{s}},{\mathit{u}}_{2_l},{\mathit{q}}_{ve},{\mathit{q}}_{\mathit{i}},{\mathit{x}}_{\mathit{3}})\in {\mathbb{R}}^{18}:Y_1=0,Y_2=0,Y_3=0\}\hfill \end{array}.$$

(77)

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 $({\mathit{x}}_{\mathit{s}},{\mathit{u}}_{2_l},{\mathit{q}}_{ve},{\mathit{q}}_{\mathit{i}},{\mathit{e}}_{\mathit{\omega }})=\mathit{0}$ is established, since the closed-loop system exhibits a zero measure point for ${{\mathit{q}}_{\mathit{d}}}^T\mathit{q}=0$ [26].

Therefore, by leveraging the Lyapunov coordinate transformation $\mathit{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 ${\mathit{e}}_{\mathit{q}}=[\mathit{0}sgn\left(q_{4e}\left(t_0\right)\right)]$ 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 $\mathit{T}$ alongside the position system errors ${\mathit{e}}_{\mathit{p}}$ and ${\mathit{e}}_{\mathit{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 $\mathit{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.

4. Implementation

As detailed in Section 3.1, the matrix gains ${\mathit{K}}_{\mathit{p}}$, ${\mathit{K}}_{\mathit{v}}$, and ${\mathit{K}}_{\mathit{\eta }}$ 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 $\frac{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 ${\mathit{K}}_{\mathit{i}}$, ${\mathit{K}}_{\mathit{q}}$, and ${\mathit{K}}_{\mathit{\omega }}$

$$\left[\begin{array}{c}{\dot{\mathit{x}}}_{\mathit{q}}\\ {\dot{\mathit{x}}}_{\mathit{\omega }}\\ {\dot{\mathit{x}}}_{{\mathit{q}}_{\mathit{i}}}\end{array}\right]=\left[\begin{array}{ccc}\mathit{0}& \frac{1}{2}\mathit{I}& \mathit{0}\\ \mathit{0}& \mathit{0}& \mathit{0}\\ \mathit{I}& \mathit{0}& \mathit{0}\end{array}\right]\left[\begin{array}{c}{\mathit{x}}_{\mathit{q}}\\ {\mathit{x}}_{\mathit{\omega }}\\ {\mathit{x}}_{\mathit{\eta }}\end{array}\right]+\left[\begin{array}{c}\mathit{0}\\ \mathit{I}\\ \mathit{0}\end{array}\right]\mathit{u}.$$

(78)

By performing the closed-loop system on the attitude system consisting of ${\mathit{q}}_{ve}$, ${\mathit{e}}_{\mathit{\omega }}$, and ${\mathit{q}}_i$, and considering only small angle variations, the following system is devised:

$$\left[\begin{array}{c}{\dot{\mathit{x}}}_{\mathit{q}}\\ {\dot{\mathit{x}}}_{\mathit{\omega }}\\ {\dot{\mathit{x}}}_{{\mathit{q}}_{\mathit{i}}}\end{array}\right]=\left[\begin{array}{ccc}\mathit{0}& \frac{1}{2}\mathit{I}& \mathit{0}\\ -\underset{{\mathit{K}}_{\mathit{1}}}{\underbrace{\left(\mathit{I}+{\mathit{K}}_{\mathit{i}}+{\mathit{K}}_{\mathit{\omega }}{\mathit{K}}_{\mathit{q}}\right)}}& -\underset{{\mathit{K}}_{\mathit{2}}}{\underbrace{\left({\mathit{K}}_{\mathit{\omega }}+{\mathit{K}}_{\mathit{q}}\right)}}& -\underset{{\mathit{K}}_{\mathit{3}}}{\underbrace{{\mathit{K}}_{\mathit{\omega }}{\mathit{K}}_{\mathit{i}}}}\\ \mathit{I}& \mathit{0}& \mathit{0}\end{array}\right]\left[\begin{array}{c}{\mathit{x}}_{\mathit{q}}\\ {\mathit{x}}_{\mathit{\omega }}\\ {\mathit{x}}_{\mathit{\eta }}\end{array}\right],$$

(79)

where ${\mathit{K}}_{\mathit{1}}$, ${\mathit{K}}_{\mathit{2}}$, and ${\mathit{K}}_{\mathit{3}}$ are the LQR gains for the system (78). With some algebraic computations, ${\mathit{K}}_{\mathit{i}}$, ${\mathit{K}}_{\mathit{q}}$, and ${\mathit{K}}_{\mathit{\omega }}$ are retrieved as follows:

$$\left\{\begin{array}{c}{\mathit{K}}_{\mathit{\omega }}=\left({\mathit{K}}_{\mathit{3}}+{\mathit{K}}_{\mathit{2}}\right){{\mathit{K}}_{\mathit{1}}}^{-1}\hfill \\ {\mathit{K}}_{\mathit{i}}={\mathit{K}}_{\mathit{1}}{\left({\mathit{K}}_{\mathit{3}}+{\mathit{K}}_{\mathit{2}}\right)}^{-1}{\mathit{K}}_{\mathit{3}}\hfill \\ {\mathit{K}}_{\mathit{q}}={\mathit{K}}_{\mathit{2}}-\left({\mathit{K}}_{\mathit{3}}+{\mathit{K}}_{\mathit{2}}\right){{\mathit{K}}_{\mathit{1}}}^{-1}\hfill \end{array}\right.,$$

(80)

where ${\mathit{K}}_{\mathit{1}}$ and ${\mathit{K}}_{\mathit{3}}+{\mathit{K}}_{\mathit{2}}$ should be invertible, and ${\mathit{K}}_{\mathit{i}}$, ${\mathit{K}}_{\mathit{q}}$, ${\mathit{K}}_{\mathit{\omega }}$ 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 ${\mathit{\omega }}_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:

• Begin by computing the controller gains, as outlined in the initial part of Section 4;
• Set the maximum thrust $T_{max}$ and choose p and $\epsilon \in ]0,1[$ such that $|\frac{d}{dx}s(x,\epsilon k_{min})|\lesssim 1$;
• Define the values of ${\epsilon }_1$ and ${\epsilon }_2$ and calculate the position saturation law, ${\mathit{u}}_{sat}$, using the Equation (45). From this, extract the values of T using (32), ${\mathit{q}}_d$ following (36), and also determine ${\mathit{\omega }}_d$ and ${\dot{\mathit{\omega }}}_d$ based on (39);
• Finally, compute ${\mathit{\tau }}_n$ using (73), and calculate the actuation $\mathit{\tau }$ using (61).

5. 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 ${\mathit{p}}_{\mathit{d}}\left(0\right)={\left[\begin{array}{ccc}-1& -1& -0.8\end{array}\right]}^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:

$${\mathit{p}}_d\left(t\right)=2\left[\begin{array}{c}cos\left({\psi }_d\left(t\right)\right)\\ \frac{sin\left(2{\psi }_d\left(t\right)\right)}{2}\\ \frac{cos\left({\psi }_d\left(t\right)\right)}{3}\end{array}\right]-\left[\begin{array}{c}2.7\\ 1.2\\ 1.75\end{array}\right]\mathrm{with}{\psi }_d\left(t\right)=\frac{3\pi }{32}t,$$

(81)

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\mathrm{kg}$, the inertia matrix is $\mathit{J}=\mathrm{diag}\left(2.22.95.3\right){10}^{-3}$, the thrust is chosen to be bound by $T_{max}=6.5 \mathrm{N}$, which implies a $k_{min}\approx 2$, the parameter p of the saturation function is set to $p=30$, and ${\epsilon }_1=0.8$. The transition point from the linear to saturated region is $\epsilon =0.8$, where $|\frac{d}{dx}s(x,0.8k_{min})|\approx 0.999$. The controller parameters are presented in

Table 1. Controller parameters: Input and state matrices to compute the controller gains, as described in Section 4 and Theorem 3.

${\mathit{q}}_1$ (55)	${\mathit{q}}_2$ (55)	${\mathit{q}}_3$ (55)	${\mathit{Q}}_{\mathit{q}}$	${\mathit{Q}}_{\mathit{\omega }}$	${\mathit{Q}}_{{\mathit{q}}_{\mathit{i}}}$	${\mathit{R}}_{\mathit{q}}$	${\mathit{k}}_1$
93	20	0.1	$\mathrm{diag}(111){10}^{-2}$	$\mathrm{diag}(5.55.55.5)$	$\mathrm{diag}(111)$	$\mathrm{diag}(118){10}^{-4}$	0.02

, 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.

The simulation results are presented in Figure 7, Figure 8 and Figure 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.

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 $\mathit{b}<1$, meaning that ${ϵ}_2{k}_{min}=1$, with $k_{min}=3$. The system is initialized at a zero position and a step ${\mathit{p}}_{\mathit{d}}=\left[\begin{array}{ccc}1& 1& -0.8\end{array}\right]$ is applied.

In the first scenario, the disturbance is characterized by $\mathit{b}=\frac{1}{\sqrt{3}}{[0.950.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 $\frac{{ϵ}_2{k}_{min}}{\sqrt{3}}$ or $\frac{{ϵ}_1{k}_{min}}{\sqrt{3}}$ 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 $\frac{{ϵ}_2{k}_{min}}{\sqrt{3}}$ or $\frac{{ϵ}_1{k}_{min}}{\sqrt{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 $\mathit{b}={[0.650-0.65]}^T$, with $\Vert \mathit{b}\Vert =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 $\frac{1}{\sqrt{3}}$. Essentially, the utilization of a normed bound function allows the system to more effectively distribute the integrative capacity across each variable.

6. 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 Figure 13, Figure 14 and Figure 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 \mathrm{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 Figure 7 and Figure 13 and Figure 9 and Figure 15.

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

	${\mathit{p}}_{\mathit{x}}$	${\mathit{p}}_{\mathit{y}}$	${\mathit{p}}_{\mathit{z}}$
$\mathrm{RMSE} \left(\mathrm{m}\right)$	0.0373	0.0580	0.0189

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).

7. 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 $\mathbf{\Delta }$, 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.