Trajectory Tracking for Quadrotors with Attitude Control on $\mathcal{S}^2 \times \mathcal{S}^1$

TL;DR

This paper introduces a novel attitude control method for quadrotors using a representation on 2222 2222 2222 that decouples thrust and yaw control, improving stability and accuracy.

Contribution

It proposes a non-linear control law on 2222 2222 that achieves almost global stability and decouples thrust and yaw control, unlike traditional methods.

Findings

Simulation results demonstrate improved control performance.

Decoupling reduces yaw tracking errors.

Almost global asymptotic stability achieved.

Abstract

The control of a quadrotor is typically split into two subsequent problems: finding desired accelerations to control its position, and controlling its attitude and the total thrust to track these accelerations and to track a yaw angle reference. While the thrust vector, generating accelerations, and the angle of rotation about the thrust vector, determining the yaw angle, can be controlled independently, most attitude control strategies in the literature, relying on representations in terms of quaternions, rotation matrices or Euler angles, result in an unnecessary coupling between the control of the thrust vector and of the angle about this vector. This leads, for instance, to undesired position tracking errors due to yaw tracking errors. In this paper we propose to tackle the attitude control problem using an attitude representation in the Cartesian product of the 2-sphere and the 1-sphere, denoted by $\mathcal{S}^2\times \mathcal{S}^1$. We propose a non-linear tracking control law on $\mathcal{S}^2\times \mathcal{S}^1$ that decouples the control of the thrust vector and of the angle of rotation about the thrust vector, and guarantees almost global asymptotic stability. Simulation results highlight the advantages of the proposed approach over previous approaches.

Figures

Equations

$$\begin{gathered} \mathcal{S}\mathcal{O}(3)\backslash {\mathcal{R}}_c,{\mathcal{R}}_c:=\left\{\left[\begin{array}{ccc}-\alpha & \beta & 0\\ \beta & \alpha & 0\\ 0& 0& -1\end{array}\right]\mid \\ {\left[\alpha \beta \right]}^{\top }\in {\mathcal{S}}^1\right\}, \end{gathered}$$

$$\left({\mathcal{S}}^2\backslash \{-e_3\}\right)\times {\mathcal{S}}^1,e_3:={\left[\begin{array}{ccc}0& 0& 1\end{array}\right]}^{\top },$$

$$\dot{p}$$

$$m\dot{v}$$

$$\dot{R}$$

$${S(z)}:=\left[{\begin{array}[]{*{20}{c}}0&-z_{3}&z_{2}\\ z_{3}&0&-z_{1}\\ -z_{2}&z_{1}&0\end{array}}\right]\,.$$

$$\mathcal{I}\dot{\omega }=-S(\omega )(\mathcal{I}\omega )+\tau ,$$

$$\bar{T}={\bar{r}}_3^{\top }(-m\dot{\bar{v}}+mg{e}_3)>0,\forall t\in {\mathbb{R}}_{\ge 0}.$$

$$0<e_3^{\top }{\bar{r}}_3\le 1,$$

$${\bar{r}}_3=\frac{b}{\Vert b\Vert },\bar{T}=m\Vert b\Vert ,b:=-\ddot{\bar{p}}+g{e}_3.$$

$${\bar{r}}_1=\frac{S({\bar{r}}_{\psi }^{\perp }){\bar{r}}_3}{s({\bar{r}}_{\psi }^{\perp },{\bar{r}}_3)},{\bar{r}}_{\psi }^{\perp }:={\left[\begin{array}{ccc}-\sin (\bar{\psi })& \cos (\bar{\psi })& 0\end{array}\right]}^{\top },$$

$${\bar{\omega }}_1=-{\bar{r}}_2^{\top }{\dot{\bar{r}}}_3,{\bar{\omega }}_2={\bar{r}}_1^{\top }{\dot{\bar{r}}}_3,{\bar{\omega }}_3={\bar{r}}_2^{\top }{\dot{\bar{r}}}_1.$$

$$\dot{p}=v,\dot{v}=a_{ref},$$

$$\tilde{a}:=\left[{\begin{array}[]{*{20}{c}}0&0&g\end{array}}\right]^{\top}+R\left[{\begin{array}[]{*{20}{c}}0&0&\frac{-T}{m}\end{array}}\right]^{\top}-{a}_{ref},$$

$$\omega (\tilde{R})=K\sum _{i=1}^3{k}_i{e}_i\times (\tilde{R}{e}_i),$$

$$a_1^{\perp }(v):=-\frac{(I-v{v}^{\top })}{s(e_3,v)}{e}_3,a_2^{\perp }(v):=\frac{S(e_3)}{s(e_3,v)}v,$$

$$b_1^{\perp }(v):=-\frac{(I-e_3{e_3}^{\top })}{s(e_3,v)}v$$

$$d_1(v)=e_1^{\top }{b}_1^{\perp }(v),d_2(v)=e_1^{\top }{a}_2^{\perp }(v).$$

$$U(v)$$

$$V(v)$$

$$W(w)$$

$$\varphi (v,w)=\{\begin{array}{rl}& U(v)V(v)W(w)\text{ if }{e}_3\ne v\\ & W(w)\text{ if }{e}_3=v,\end{array}$$

$${\varphi }^{-1}(R)=\{\begin{array}{rl}& (r_3,\left[\begin{array}{cc}{d}_1(r_3)& d_2(r_3)\\ -d_2(r_3)& d_1(r_3)\end{array}\right]\left[\begin{array}{c}{r}_1^{\top }{a}_1^{\perp }(r_3)\\ r_1^{\top }{a}_2^{\perp }(r_3)\end{array}\right]).\text{ if }{r}_3\ne e_3,\\ & (e_3,\left[\begin{array}{c}{e}_1^{⊺}{r}_1\\ e_2^{⊺}{r}_1\end{array}\right])\text{ if }{r}_3=e_3.\end{array}$$

$$\dot{R}=RS(\omega ),$$

$$\dot{v}$$

$$\dot{w}$$

$${\omega }_1=-r_2^{\top }{u}_v,{\omega }_2=r_1^{\top }{u}_v,$$

$${\dot{r}}_3=r_1{r}_1^{\top }{u}_v+r_2{r}_2^{\top }{u}_v=(I-r_3{r}_3^{\top })u_v,$$

$$\dot{v}=(I-v{v}^{\top })u_v.$$

$$u_v={\tilde{\omega }}_1{z}_1+{\tilde{\omega }}_2{z}_2,$$

$$R_e$$

$$R_r$$

$$w_1$$

Full text

I Introduction

Quadrotors have been the focus of much research in the past decade [1, 2, 3, 4]. Typically, the controller has a cascade architecture consisting of an inner- and an outer-loop controller for attitude and position control, respectively, see, e.g., [5] and [6]. The outer-loop controller is designed for a simple double integrator model, providing virtual accelerations to control the quadrotor’s positions. The actual accelerations result from the inner-loop controller, which controls the attitude of the quadrotor and the total thrust in order to track these accelerations and to track a yaw angle reference. An overview of this approach is given in Figure 1.

The strategies to control the attitude of the quadrotor, see e.g. [7, 8, 9, 10, 11, 12], i.e., to design the inner loop, often resort to general methods to control the attitude of a rigid body [13, 14, 15, 16]. For instance, the quaternion-based attitude control strategies in [13] and [14] are applied to quadrotors in [7, 8, 9]; control strategies on the special orthogonal group, described, e.g., in [17], are applied in [10] and [11]; and a quaternion-based hybrid control law for attitude tracking in [15] is applied in [12]. Note that, as stated in [18], any continuous state-feedback control law, using local coordinates, is not globally well defined. This leads to unwinding, where the controller unnecessarily rotates the attitude through large angles, instead of global asymptotic stability. Therefore, it is impossible to stabilize any equilibrium point in the 3D rotations manifold, motivating discontinuous control laws (see, e.g., [10]) and hybrid control laws (see, e.g., [15]).

However, these general attitude control strategies are not necessarily the most suitable to address the inner-loop quadrotor control problem. In fact, while the thrust vector of the quadrotor can be controlled independently of the angle of rotation about the thrust vector, which defines the yaw angle, these attitude control laws consider controlling the full attitude. This results in an unnecessary coupling between the control of the thrust vector and of the rotation about this vector. This is undesirable since a yaw error can result in a position error, and this is particularly noticeable for “slow” quadrotors where the ratio between the maximum torque that can be applied about the thrust vector and the moment of inertia about this axis is small.

In this paper, we propose to tackle the attitude tracking problem with a different attitude parameterization intended to decouple the control of the thrust vector from the control of the angle about this vector. To this end, we provide a convenient homeomorphism between

[TABLE]

and

[TABLE]

where $\mathcal{SO}(3):=\left\{R\in\mathbb{R}^{3\times 3}|R^{\top}R=\mkern-3.0muRR^{\top}\mkern-3.0mu=I,\det R=1\right\}$ is the special orthogonal group of order three, $\mathcal{S}^{2}$ is the 2-sphere and $\mathcal{S}^{1}$ is the 1-sphere, formally $\mathcal{S}^{n}:=\{x\in\mathbb{R}^{n+1}|x^{\top}x=1\}$, for $n\in\{1,2\}$. Where $\mathcal{R}_{c}$ can be interpreted as the set of orientations considering all rotations around the vector $-e_{3}$, these orientations are excluded to be able to define a homeomorphism. This homeomorphism can be intuitively explained as follows. Given a rotation matrix in the special orthogonal group, which is not in $\mathcal{R}_{c}$, take the third column $r_{3}$, which belongs to $\mathcal{S}^{2}$, and the representation of the first column in the vectors of the orthogonal space to ${r}_{3}$ defined by the parallel transport of ${e}_{1}:=\begin{bmatrix}1&0&0\end{bmatrix}^{\top}$ and ${e}_{2}:=\begin{bmatrix}0&1&0\end{bmatrix}^{\top}$ along the geodesic on $\mathcal{S}^{2}$ between ${e}_{3}$ and ${r}_{3}$ (see Figure 2). Note that, for $({v},w)\in\mathcal{S}^{2}\times\mathcal{S}^{1}$, with ${v}\neq-{e}_{3}$, ${v}$ corresponds to the normalized thrust vector, and is equal to $e_{3}$ when hovering, and $w$ determines the angle about the thrust vector.

We then show that by applying a suitable input transformation, $v$ and $w$ can be controlled independently. We propose tracking control laws for the tracking subproblems in $\mathcal{S}^{2}$ and $\mathcal{S}^{1}$, considering the angular velocity as an input to the quadrotor. The proposed control law avoids coupling by construction. For differentiable attitude references, asymptotic convergence of the attitude tracking errors to zero as time converges to infinity is established, for every initial condition except at a single point, which has zero Lebesgue measure (following, e.g., [19] we call this almost global stability).

In order to show the advantages of the new inner-loop control approach for quadrotor control, we introduce a simple trajectory tracking control law for the outer-loop generating acceleration references. The advantages of the proposed control law are then highlighted through simulations.

At the time of writing we found similar decoupling approaches for the attitude control of quadcopters in the literature (see [20, 21, 22, 23]). Our approach differs from previous work in the parametrization we propose, which we believe provides a more natural way to define the rotation about the thrust vector.

This paper is organized as follows. Section II introduces the problem. The proposed approach is presented in Section III. Section IV discusses an outer-loop trajectory tracking strategy to control the overall model. Section V provides simulation results that show the advantages of the proposed solution compared to previous attitude control strategies for quadcopters, and conclusions are provided in Section VI.

II Problem formulation

Let $\{A\}$ denote a world fixed frame, and $\{B\}$ a body fixed frame centered at the vehicle’s center of mass. The third axis of $\{A\}$, denoted by $e_{3}$, is assumed to be aligned with the gravity vector, as is the third axis of $\{B\}$, denoted by $r_{3}$, when the quadrotor is hovering. Moreover, let ${p}\in\mathbb{R}^{3}$ and ${v}\in\mathbb{R}^{3}$ denote the position and velocity, respectively, of the center of mass of the quadrotor expressed in $\{A\}$. Let R=\left[{\begin{array}[]{*{20}{c}}{r}_{1}&{r}_{2}&{r}_{3}\end{array}}\right]\in\mathcal{SO}(3) denote the attitude of $\{B\}$ relative to $\{A\}$.

The forces acting on the quadrotor and expressed in the world fixed frame are assumed to be the gravity force, $mg{e}_{3}$, where $m\in\mathbb{R}$ and $g\in\mathbb{R}$ denote the mass and gravitational acceleration; the thrust force $-TR{e}_{3}$, where $T\in\mathbb{R}_{>0}$ denotes the total thrust generated by the blades. Therefore,

[TABLE]

where $\omega=\begin{bmatrix}\omega_{1}&\omega_{2}&\omega_{3}\end{bmatrix}^{\top}\in\mathbb{R}^{3}$ is the angular velocity expressed in $\{B\}$, and for any $z\in\mathbb{R}^{3}$, we let

[TABLE]

The thrust vector, defined as $-TR{e}_{3}=-T{r}_{3}$, can then be controlled by controlling the last column of the rotation matrix. Often in the literature, and also here one assumes that the angular velocity can be directly controlled by controlling the motor speeds, i.e., $\omega$ is the control input of the model. Alternatively, one can consider that the thrust generated by each propeller generates torques, denoted by $\tau\in\mathbb{R}^{3}$, which are related to angular velocities by

[TABLE]

where $\mathcal{I}\in\mathbb{R}^{3\times 3}$ denotes the inertia matrix.

II-A Reference trajectory

Consider a reference $(\bar{p}(t),\bar{v}(t),\bar{R}(t),\bar{\omega}(t))$ continuous in time $t\in\mathbb{R}_{\geq 0}$ that satisfies (1) for a given initial condition, i.e., a reference that the quadrotor can track exactly and such that the corresponding total thrust satisfies

[TABLE]

This defines a broad class of references, which we can consider with the tools provided in this paper, allowing acrobatic maneuvers where the angle between the gravity vector and $r_{3}$ is arbitrary. However, it will also be convenient to define the following subclass of references which we can easily parameterize and for which we can easily define a yaw angle relying on its orientation with respect to the gravity vector. These are references for which the angle between the normalized reference thrust vector, i.e., $\bar{r}_{3}$, and the gravity vector does not exceed $\frac{\pi}{2}$, i.e.,

[TABLE]

for every $t\in\mathbb{R}_{\geq 0}$. We can parameterize this class with the position reference $\bar{p}$ and an auxiliary angle $\bar{\psi}\in(-\pi,\pi]$, which plays the role of the yaw reference angle. The position reference $\bar{p}(t)$, for which the first three derivatives are assumed to exist if (1) is considered or the first four derivatives are assumed to exist if (1), (2) are considered, characterizes the third column of the rotation matrix $\bar{r}_{3}$, and the thrust $\bar{T}$, since, from the second equation in (1) we can write

[TABLE]

The reference angle $\bar{\psi}$, for which the first derivative is assumed to exist if (1) is considered or the first two derivatives are assumed to exist if (1), (2) are considered, is defined as the angle between ${e}_{1}$ and the projection of $\bar{r}_{1}$ on $({e}_{1},{e}_{2})$. Therefore, it defines ${\bar{r}}_{1}$ through

[TABLE]

where, for $a,b\in\mathbb{R}^{3}$, $s({a},{b}):=\sqrt{1-({a}^{\top}{b})^{2}}$. Given ${\bar{r}}_{1}$ and ${\bar{r}}_{3}$, ${\bar{r}}_{2}={S(\bar{r}}_{3}){\bar{r}}_{1}$ and from $\dot{\bar{R}}=\bar{R}S(\bar{\omega})$ we obtain

[TABLE]

If (1), (2) are considered, then $\bar{\tau}=\mathcal{I}\dot{\bar{\omega}}+S(\bar{\omega})(\mathcal{I}\bar{\omega}).$

II-B *Cascade trajectory tracking problem *

Following a common cascaded control design approach, we divide the tracking control problem into two subproblems: (i) find a virtual acceleration $a_{ref}\in\mathbb{R}^{3}$ to control the model

[TABLE]

such that the tracking errors $\tilde{p}=\bar{p}-{p}$ and $\tilde{v}=\bar{v}-{v}$ converge to zero, and (ii) find either $\omega$ and $T$ if the considered quadrotor model is (1) or $\tau$ and $T$ if (1), (2) are considered, such that the forces applied to the quadrotor (normalized by mass) track the desired virtual acceleration ${a}_{ref}$, i.e., the acceleration error

[TABLE]

converges to zero, and $\bar{R}^{\top}R$ converges to the identity as time converges to infinity. We will denote the outer-loop control problem by (i), and the inner-loop control problem by (ii).

Note that driving (8) to zero constraints $r_{3}$ to coincide with the reference vector $\bar{r}_{3}$ and once such a condition is met, one can replace the goal $\bar{R}^{\top}R\rightarrow I$ by simply $\bar{r}_{1}^{\top}r_{1}=1\text{ and }\bar{r}_{2}^{\top}r_{2}=1,$ which is equivalent to stating that the angle of rotation about the normalized thrust vector (i.e., $r_{3}=\bar{r}_{3}$ when $\tilde{a}=0$) must coincide with the desired value. When (4) is met, this means that the yaw angle of the quadrotor coincides with the desired yaw reference angle $\bar{\psi}$.

Remark 1

The inner-loop control problem for quadrotors is typically tackled directly in $\mathcal{SO}(3)$, or quaternion or Euler angles representations. For instance, letting $\tilde{R}\in\mathcal{SO}(3)$ represent a reference attitude and $\tilde{R}:=\bar{R}^{\top}R$, the feedback controller (see [24, 11])

[TABLE]

where $K=K^{\top}>0\in\mathbb{R}^{3\times 3}$, and $k_{i}>0\in\mathbb{R}$ are distinct (e.g., $0<k_{1}<k_{2}<k_{3}$) is such that $\tilde{R}(0)$ converges to $I$ for every initial condition $\tilde{R}(0)\in\mathcal{SO}(3)\backslash\{\text{diag}(1,-1,-1),\text{diag}(-1,1,-1),\text{diag}(-1,-1,1)\}$. Moreover, a globally stabilizing discontinuous feedback quaternion-based law can be found in [15, 16]. However, the position control of a quadrotor is only dependent on the third column of the rotation matrix (collinear with the thrust vector), as one attempts to drive the error (8) to zero. While the method we propose drives the third column of $R$ to a desired value and controls independently the angle about the third column of $R$ so that $R$ converges to a desired reference, both of these methods do not control these two quantities independently leading to undesired behaviors (see Section V).

III Inner loop reference tracking in $\mathcal{S}^{2}\times\mathcal{S}^{1}$

We start by introducing, in Section III-A, a convenient parametrization of $\mathcal{S}\mathcal{O}(3)\backslash\mathcal{R}_{c}$, by defining a homeomorphism between $\mathcal{S}\mathcal{O}(3)\backslash\mathcal{R}_{c}$ and $\left(\mathcal{S}^{2}\backslash\{-e_{3}\}\right)\times\mathcal{S}^{1}$. In Section III-B we define an input transformation that allows us to decouple the control of the normalized thrust vector on $\mathcal{S}^{2}\backslash\{-e_{3}\}$ from the control of the angle of rotation about the thrust vector, specified by a vector on $\mathcal{S}^{1}$. In Section III-C, we consider the normalized thrust vector control problem in $\mathcal{S}^{2}$, and in Section III-D, we consider the control of the angle about the thrust vector in $\mathcal{S}^{1}$. Section III-E combines the two strategies to provide the overall control law.

III-A Parameterization of $\mathcal{SO}(3)\backslash\mathcal{R}_{c}$

We can parameterize $\mathcal{S}\mathcal{O}(3)\backslash\mathcal{R}_{c}$ as follows. Let $v\in\mathcal{S}^{2}\backslash\{-e_{3}\}$ and $w\in\mathcal{S}^{1}$. For $v\neq{e_{3}}$ define the following two perpendicular vectors in the tangent space to $\mathcal{S}^{2}$ at $v$,

[TABLE]

where $I$ denotes the $3\times 3$ identity matrix. Note that $a_{2}^{\perp}(v)$ is also in the tangent space to $\mathcal{S}^{2}$ at ${e_{3}}$. Let

[TABLE]

be an extra vector in the tangent space to $\mathcal{S}^{2}$ at $e_{3}$, which is perpendicular to $a_{2}^{\perp}(v)$. The first component of these tangent vectors at ${e_{3}}$ are denoted by

[TABLE]

Moreover, define the following rotation matrices

[TABLE]

for $w=(w_{1},w_{2})$. Then, the following map is the desired homeomorphism $\phi:\left(\mathcal{S}^{2}\backslash\{-e_{3}\}\right)\times\mathcal{S}^{1}\mapsto\mathcal{S}\mathcal{O}(3)\backslash\mathcal{R}_{c}$

[TABLE]

where $w=(w_{1},w_{2})$. The inverse $\phi^{-1}:\mathcal{S}\mathcal{O}(3)\backslash\mathcal{R}_{c}\mapsto\left(\mathcal{S}^{2}\backslash\{-e_{3}\}\right)\times\mathcal{S}^{1}$ is given by

[TABLE]

In fact, it is clear that $\phi$ and $\phi^{-1}$ are one-to-one and continuous; therefore, $\phi$ defines an homeomorphism between $\left(\mathcal{S}^{2}\backslash\{-e_{3}\}\right)\times\mathcal{S}^{1}$ and $\mathcal{S}\mathcal{O}(3)\backslash\mathcal{R}_{c}$.

III-B Input transformation

In this section, we focus on the control of the attitude described by the equation

[TABLE]

and consider the standard stabilization problem of finding a control law for $\omega$ that drives $R$ to the identity matrix $\bar{R}=\begin{bmatrix}\bar{r}_{1}&\bar{r}_{2}&\bar{r}_{3}\end{bmatrix}=I$ for a given initial condition $R(0)=R_{0}\in\mathcal{S}\mathcal{O}(3)$. The tracking problem can either be converted into a stabilization problem by defining $\tilde{R}=\bar{R}^{\top}R$ and noticing that $\dot{\tilde{R}}=\tilde{R}S(\tilde{\omega})$, for $\tilde{\omega}=\omega-R^{\top}\bar{R}\bar{\omega}$ or tackled as we do in the sequel.

For this problem, we define an input transformation $\tilde{\omega}=g(R,\omega),$ where $\tilde{\omega}=(\tilde{\omega}_{1},\tilde{\omega}_{2},\tilde{\omega}_{3})$, such that, for $(v,w)=\phi^{-1}(R)$,

[TABLE]

Note that with this transformation we can control independently $v\in\mathcal{S}^{2}$ and $w\in\mathcal{S}^{1}$.

To this effect, we start by noticing that if we make

[TABLE]

for some control input $u_{v}\in\mathbb{R}^{3}$, and replace these expressions in the third column of (10), i.e., $\dot{r}_{3}=\omega_{2}r_{1}-\omega_{1}r_{2}$, we obtain

[TABLE]

or, equivalently, substituting $r_{3}=v$,

[TABLE]

Note that the component of $u_{v}$ parallel to $v$ plays no role in (13). Therefore, considering that ${u_{v}}$ belongs to the tangent space to $v$, that is,

[TABLE]

for some vectors $z_{1}$ and $z_{2}$ in the tangent space of $\mathcal{S}^{2}$ at $v$, we conclude that (13) and (14) take the form (11).

Let $\bar{v}=\bar{r}_{3}$ and define

[TABLE]

The parallel transport from the space orthogonal to $\bar{v}$ to the space orthogonal to $v$ along the geodesic curve in $\mathcal{S}^{2}$ connecting the two is given by $R_{r}R_{e}^{\top}z,$ for a vector $z$ that belongs to the tangent space to $\mathcal{S}^{2}$ at $\bar{v}$. We can then use the parallel transport for ${\bar{r}}_{1}$ and ${\bar{r}}_{2}$ and obtain a local frame in the tangent space of $v$, $\left\{R_{r}R_{e}^{\top}{\bar{r}}_{1},R_{r}R_{e}^{\top}{\bar{r}}_{2}\right\},$ the angle between this frame and $\{{r}_{1},{r}_{2}\}$ is the error angle. Note that $w=(w_{1},w_{2})$, where

[TABLE]

Moreover, $w_{1}^{2}+w_{2}^{2}=1$, and the error angle is $\text{atan2}(w_{2},w_{1})$. We wish to drive this angle to zero, or equivalently, drive $w_{1}$ to $1$ and $w_{2}$ to [math]. Let $\omega_{e}$ and $\omega_{r}$ be obtained in a similar fashion to (6) and be such that

[TABLE]

Using these expressions, and the fact that, from (10), $\dot{r}_{1}=\omega_{3}r_{2}-\omega_{2}r_{3}$ and $\dot{r}_{2}=-\omega_{3}r_{1}+\omega_{1}r_{3}$, we can now write

[TABLE]

Since $w_{1}^{2}+w_{2}^{2}=1$ we have $w_{1}\dot{w}_{1}+w_{2}\dot{w}_{2}=0$, from which we conclude that $w_{2}\beta_{2}-w_{1}\beta_{1}=0$, or, assuming that $w_{1}\neq 0$ and $w_{2}\neq 0$, $\frac{\beta_{2}}{w_{1}}=\frac{\beta_{1}}{w_{2}}$. Since we might have $w_{1}=0$ or $w_{2}=0$ but not simultaneously, the following is bounded

[TABLE]

Then we can write

[TABLE]

which, by considering $\tilde{\omega}_{3}=u_{w}$, takes the form (12).

III-C Controlling $v\in\mathcal{S}^{2}$

Let $\bar{v}={\bar{r}}_{3}$ be the reference for $v$. We propose the following control law to control (13),

[TABLE]

where ${u}_{v,\text{FB}}$ and ${u}_{v,\text{FF}}$ can be seen as feedback and feedforward laws as detailed next.

For feedback, we consider

[TABLE]

with $k_{1}>0$. Intuitively, this control law is such that when used in (13), $(I-vv^{\intercal}){u}_{\text{FB}}$ becomes a vector of the tangent space at $v$ proportional to the derivative at $v$ of the geodesic curve from $v$ to $\bar{v}$ (the curve connecting $v$ and $\bar{v}$ in $\mathcal{S}^{2}$ with shortest distance). The norm of this tangent vector is unitary in $\{v\in\mathcal{S}^{2}|v^{\top}\bar{v}\leq 0\}$ and decreases as $v$ approaches $\bar{v}$ in $\{v\in\mathcal{S}^{2}|v^{\top}\bar{v}>0\}$. Note that when $v=-\bar{v}$ the geodesic is not unique. Therefore the control law corresponds to a rotation in the direction of ${r}_{1}$, while in fact, any unitary vector in the plane $({r}_{1},{r}_{2})$ can be used.

For the feedforward term, we consider

[TABLE]

with $\Theta=\frac{1}{s(v,\bar{v})^{2}}((v\times\bar{v})(v\times\bar{v})^{\top}-(I-vv^{\top})\bar{v}v^{\top})$, which maps $\dot{\bar{v}}$ to the tangent space of $\mathcal{S}^{2}$ at $v$ (see Fig. 2). The intuition behind this feedforward control law is that the angle between $v$ and $\bar{v}$ remains constant when only the feedforward input is applied.

Now we will look at the stability of this control law, which is summarized in the next result.

Theorem 1

Let $\bar{v}(t)$ for $t\in\mathbb{R}_{\geq 0}$ define a given differentiable curve in $\mathcal{S}^{2}$ and consider (13) with control law (19), (20) and (21). Then $\lim_{t\rightarrow\infty}v=\bar{v}$ for every initial condition $v(0)\in\mathcal{S}^{2}$.

Proof:

Proving that $v$ converges to $\bar{v}$ as $t$ converges to infinity is equivalent to proving that $\xi:=\bar{v}^{\intercal}v$ converges to $1$ since $\frac{1}{2}(\bar{v}-v)^{\intercal}(\bar{v}-v)=1-\bar{v}^{\top}v$. We shall prove that for every $v(0)\neq\bar{v}(0)$,

[TABLE]

Then, we can compute a closed-form expression for $\xi$: if $0\leq\xi(0)\leq 1$,

[TABLE]

where $\kappa:=\frac{1+\xi(0)}{1-\xi(0)}$ and if $-1<\xi(0)<0$, letting $c_{1}:=\text{asin}(\xi(0))$ and $t_{s}:=-\frac{c_{1}}{k_{1}}$

[TABLE]

By taking the limit as $t$ converges to infinity, we conclude that $\xi$ converges to 1.

It rests to prove (22). The time derivative of $\xi$ is

[TABLE]

Note that $\bar{v}^{\top}(I-vv^{\top}){u}_{v,\text{FF}}=-\dot{\bar{v}}^{\top}v,$ and therefore $\dot{\xi}=\bar{v}^{\top}(I-vv^{\top}){u}_{v,\text{FB}},$ which can be rewritten as

[TABLE]

Since $v(0)\neq-\bar{v}(0)$, then $\xi(0)\neq-1$, and since $\xi(t)$ monotonically increases, $v(t)\neq-\bar{v}(t)$, for every $t\geq 0$. Then (23) implies (22), concluding the proof.

∎

III-D Controlling $w\in S^{1}$

In this section, we consider an arbitrary function of time $\bar{R}(t)$ (not necessarily the identity). It is convenient therefore to define $(.,w)=\phi^{-1}(\tilde{R})$, where $\tilde{R}=\bar{R}^{\top}R$, which is still described by (16) but now for time-varying $\bar{R}$. Following the same steps as in Section III-B, we arrive at (17) with $\beta_{1}$ and $\beta_{2}$ replaced by $\bar{\beta}_{i}=\beta_{i}+\theta_{i},\quad i\in\{1,2\},$ where $\theta_{1}:=-r_{1}^{\top}R_{r}R_{e}^{\top}{\dot{\bar{r}}}_{1}$, $\theta_{2}:=r_{2}^{\top}R_{r}R_{e}^{\top}{\dot{\bar{r}}}_{1}$. As a result, instead of (18), we obtain

[TABLE]

where

[TABLE]

We propose the following control law

[TABLE]

where ${u}_{w,\text{FB}}$ and ${u}_{w,\text{FF}}$ can be seen as feedback and feedforward laws. For feedforward, we consider $u_{w,\text{FF}}=\zeta$ and for feedback we consider

[TABLE]

for some positive constant $k_{2}$.

The next result establishes that this control drives $w_{1}$ to 1 and $w_{2}$ to [math], as desired.

Theorem 2

Let $\bar{R}(t)$ for $t\in\mathbb{R}_{\geq 0}$ define a given differentiable curve in $\mathcal{S}\mathcal{O}(3)$. Consider the system (18) with control law (24), (25). Then

[TABLE]

for every initial condition $w(0)\in\mathcal{S}^{1}$.

Proof:

Replacing the control law $u_{w}$, we obtain

[TABLE]

Consider the Lyapunov function

[TABLE]

which is always positive definite. The derivative is given by $\dot{V}(w_{1},w_{2})=-\dot{w}_{1}$ and can be rewritten as

[TABLE]

Therefore, the Lyapunov function derivative is negative semi-definite, and in the set $\Omega:=\{(w_{1},w_{2})\in\mathbb{R}^{2}|V(w_{1},w_{2})=0\}$, i.e., when $w_{2}=0$, we have $\dot{w}_{2}=k_{2}$ if $w_{1}=-1$, $\dot{w}_{2}=0$ if $w_{1}=1$. Therefore, the only solution that can stay in $\Omega$ is $w_{1}=1$, $w_{2}=0$ and thus according to LaSalle’s Theorem [25] we have that $(1,0)$ is globally asymptotically stable. ∎

III-E Inner-loop control law

The combined control law in $\mathcal{S}^{2}\times\mathcal{S}^{1}$ allows for (10) to track a given reference $\bar{R}$ as stated in the next result, which results from the arguments given in the previous sections.

Theorem 3

Let $\bar{R}(t)$ for $t\in\mathbb{R}_{\geq 0}$ define a given differentiable curve in $\mathcal{S}\mathcal{O}(3)$ and consider (10) with control law

[TABLE]

where $u_{v}$ are $u_{w}$ are obtained by replacing $v$ by $r_{3}$ and $\bar{v}$ by $\bar{r}_{3}$ in (19), (20) and (21), and in (15), (16), (17) respectively. Moreover, let, $\tilde{R}:=\bar{R}^{\top}R$. Then

[TABLE]

for every initial condition $R(0)$ such that $\tilde{R}\in\mathcal{S}\mathcal{O}(3)\backslash\mathcal{R}_{c}$.

Since asymptotic convergence of $R$ to $\bar{R}$ is obtained for every initial condition except in a set of measure zero, this result assures almost global convergence, following the standard terminology in the literature [19].

IV Outer loop and overall control law

Following the control approach of Section II-B, in this section we consider the position tracking problem of finding virtual acceleration input references ${a}_{ref}$ for (7) that can track a certain position and velocity trajectory. The inner loop must make the error (8) zero. Note that when (8) is zero, then ${a}_{ref}\in\mathcal{A}$ where

[TABLE]

is a ball in $\mathbb{R}^{3}$ and $T_{M}$ is the maximum total thrust that can be applied. Since, for a vector $z\in\mathbb{R}^{3}$, $\|z\|_{2}\leq\sqrt{3}\|z\|_{\infty}$, $\mathcal{A}_{\infty}\subseteq\mathcal{A}$ where

[TABLE]

is a box in $\mathbb{R}^{3}$. It is more convenient to restrict $a_{ref}$ to belong to $\mathcal{A}_{\infty}$ because this imposes an independent restriction on each of the three components of $a_{ref}$. We focus on the following single-input single-output (SISO) system

[TABLE]

where $x_{1}$ and $x_{2}$ represent the position and velocity, respectively, and the control law for $u$ must satisfy $u\in[\underline{L},\overline{L}]$ for given constants $\underline{L}$ and $\overline{L}$. We let $p=\begin{bmatrix}p_{x}&p_{y}&p_{z}\end{bmatrix}^{\top}$, where $p_{x},p_{y},p_{z}$ are the positions in the direction of $e_{1},e_{2},$ and $e_{3}$, respectively. Then, for the system pertaining to $p_{x}$ and $p_{y}$, $\underline{L}=-\frac{\bar{T}}{m\sqrt{3}}$, $\overline{L}=\frac{\bar{T}}{m\sqrt{3}}$ and for the system pertaining to $p_{z}$, $\underline{L}=g-\frac{\bar{T}}{m\sqrt{3}}$, $\overline{L}=g+\frac{\bar{T}}{m\sqrt{3}}$. If we further restrict $\overline{L}\leq g$ for the system pertaining to $p_{z}$, we assure that  (4) holds, which is convenient as explained at the end of Section II-B, since then the angle about $\bar{r}_{3}$ can be specified by a predefined function $\bar{\psi}$, playing the role of a yaw angle reference. In fact, setting (8) to zero, multiplying by $e_{3}^{\intercal}$, noticing that $T$ must be non-negative and using (4), we conclude that $e_{3}^{\top}a_{ref}\leq g,$ which simply states that the required acceleration downwards cannot be larger than the gravitational acceleration when (4) is met.

In addition to the saturations on the control input $a_{\text{ref}}$ for (7), we must assume that it is differentiable, if the considered quadrotor model is (1) or twice differentiable if (1), (2) are considered. Although we consider simply (1), we provide a twice-differentiable control input to cope also with cases where (1), (2) are considered.

In Section IV-A, we tackle the problem of finding a saturated twice-differentiable reference tracking control law for (30) and, in Section IV-B, we provide the proposed overall control law.

IV-A Outer loop

We are interested in tracking a reference trajectory $(\bar{x}_{1},\bar{x}_{2})$ and write the error system with states $e_{1}=\bar{x}_{1}-x_{1}$ and $e_{2}=\bar{x}_{2}-x_{2}$ as

[TABLE]

Note that since we have constraints on the input $u$, this also imposes constraints on the reference, namely,

[TABLE]

Consider the control law

[TABLE]

where $k_{1},k_{2}\in\mathbb{R}_{>0}$ and

[TABLE]

with $\epsilon\in[0,\frac{b-a}{2}]$. Since $\underline{L}<\bar{u}<\overline{L}$, we can rewrite (33) to

[TABLE]

The error dynamics for the closed-loop system become

[TABLE]

where $a=\underline{L}-\bar{u}\in(\underline{L}-\overline{L},0)$ and $b=\overline{L}-\bar{u}\in(0,\overline{L}-\underline{L})$.

IV-B Overall control law

The control law for the outer-loop (33) provides a virtual acceleration reference vector, denoted by ${a}_{ref}=\begin{bmatrix}u_{x}&u_{y}&u_{z}\end{bmatrix}^{\top}$,where $u_{x}$, $u_{y}$, $u_{z}$ are given by (33) for the corresponding error variables corresponding to $p_{x}$, $p_{y}$ and $p_{z}$. In this section, we use $a_{ref}$ to find a control input $\omega$ and $T$ to reduce the error (8) considering the dynamics given in (1). We define \bar{a}_{ref}:=\left[{\begin{array}[]{*{20}{c}}0&0&g\end{array}}\right]^{\top}-{a}_{ref}. The error (8) can be rewritten as

[TABLE]

where $\bar{r}_{3}=\frac{\bar{a}_{ref}}{\left\lVert\bar{a}_{ref}\right\rVert}$ represents the direction, and $\left\lVert\bar{a}_{ref}\right\rVert$ the magnitude of $\bar{a}_{ref}$. Note that $\left\lVert\bar{a}_{ref}\right\rVert>0$. A natural choice for the thrust is $T=m\left\lVert\bar{a}_{ref}\right\rVert$. Moreover, $\bar{r}_{3}$ provides the reference for the inner-loop. Note that $\bar{r}_{3}$ must be now computed online and depends on the position and velocity errors in (33). By picking $\underline{L},\overline{L}$, the saturation constants in Section IV-A, sufficiently small we can guarantee that (4) holds. Therefore, $\bar{r}_{1}$ and $\bar{r}_{2}$ are completely characterized by a desired yaw angle $\bar{\psi}$ as explained in Section II-A.

The overall control law is then

[TABLE]

where $u_{x}$, $u_{y}$, $u_{z}$ are given by (33) for the corresponding error variables corresponding to $p_{x}$, $p_{y}$ and $p_{z}$, $u_{v}$ and $u_{w}$ are obtained by replacing $v$ by $r_{3}$ and $\bar{v}$ by $\bar{r}_{3}$ in (19), (20) and (21) and in (15), (16), (24), (25) respectively.

V Simulations

In this section, we present simulation results where we use the proposed saturated PD control law in the outer loop, and in the inner loop, we compare the proposed decoupled controller in $\mathcal{S}^{2}\times\mathcal{S}^{1}$ with the discontinuous quaternion-based control proposed in [15, 16] and the continuous controller in $\mathcal{S}\mathcal{O}(3)$ described in Remark 1. We consider the dynamics (1) with $m=1[\text{kg}]$, and g=9.81[\text{m/s{}^{2}}]. The outer-loop controller given by (33) has the following parameters: $k_{p}=1$ and $k_{v}=2$. For the inner loop we consider the $\mathcal{S}^{2}\times\mathcal{S}^{1}$ controller proposed in Section III-C and III-D with $k_{1}=2.5$ and $k_{2}=4$, the quaternion controller with $k=5$, and the $\mathcal{S}\mathcal{O}(3)$ controller (9) with $K=5I$, $k_{1}=0.9$, $k_{2}=1$, $k_{3}=1.1$. Please note that the aim of this simulation example to show the behavior of the different controllers and not the performance.

We compare these controllers in two scenarios that illustrate how the quaternion-based control and control in $\mathcal{SO}(3)$ described in Remark 1 can cause undesired behavior. In both scenarios, we let $p_{x}(0),p_{y}(0)=0$ and $\bar{p}_{x}(t)=1,\bar{p}_{y}(t)=0,\bar{\psi}(t)=0,\operatorname{\forall}t>0$. Similarly to $\bar{\psi}$, we define $\psi$ to be the angle between the projection of $r_{1}=\begin{bmatrix}r_{1,1}&r_{1,2}&r_{1,3}\end{bmatrix}$ on the $xy$-plane and $e_{1}$, i.e., $\psi=\text{atan2}(r_{1,2},r_{1,1})^{\top}$. In the simulation $r_{1}$ is never perpendicular to the $xy$-plane so that $\psi$ can always be computed. We denote $\psi$ by the yaw angle. The results with $\psi(0)=0$ are shown in Fig. 3 and with $\psi(0)=\pi-0.01$ are shown in Fig. 4.

We can observe that the yaw error results in a position error in the $y$ direction when only movement in the $x$ direction is required for both the discontinuous quaternion control and continuous $\mathcal{SO}(3)$ control. In the proposed control law in $\mathcal{S}^{2}\times\mathcal{S}^{1}$, the thrust vector is decoupled from the yaw and, therefore, a yaw error does not result in a position error.

VI Conclusion

We presented an attitude controller that achieves almost global asymptotic stability for the tracking error dynamics of a quadrotor, where the thrust vector is controlled in $S^{2}$ and the heading in $S^{1}$. By considering the decoupled control of the thrust vector and of the angle about this vector, we avoid undesired coupling of previous approaches. These advantages were illustrated in simulation. In future work, the stability of the overall closed-loop system, i.e., the quadrotor with the inner- and outer-loop controllers should be addressed.

VII Acknowledgements

The authors are thankful to A. A. J. (Erjen) Lefeber for several comments which have helped us improving the paper.