 # Trajectory Tracking for Quadrotors with Attitude Control on S^2 ×S^1

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 S^2×S^1. We propose a non-linear tracking control law on S^2×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.

## Authors

##### This week in AI

Get the week's most popular data science and artificial intelligence research sent straight to your inbox every Saturday.

## 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.,  and . 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  and  are applied to quadrotors in [7, 8, 9]; control strategies on the special orthogonal group, described, e.g., in , are applied in  and ; and a quaternion-based hybrid control law for attitude tracking in  is applied in . Note that, as stated in , 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., ) and hybrid control laws (see, e.g., ). Fig. 1: Control scheme with an outer-loop position controller generating reference accelerations, and an inner-loop attitude controller on S2×S1; p,v,R, represent position, velocity and attitude, and ¯p,¯v,¯R corresponding references; aref is a desired acceleration reference, ω is the angular velocity and T is the total thrust. A yaw reference ¯ψ, along with aref, can determine ¯R , see Section II-A.

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

and

 (S2∖{−e3})×S1,  e3:=⊤,

where is the special orthogonal group of order three, is the 2-sphere and is the 1-sphere, formally , for . Where can be interpreted as the set of orientations considering all rotations around the vector , 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 , take the third column , which belongs to , and the representation of the first column in the vectors of the orthogonal space to defined by the parallel transport of and along the geodesic on between and (see Figure 2). Note that, for , with , corresponds to the normalized thrust vector, and is equal to when hovering, and determines the angle about the thrust vector. Fig. 2: Illustration of the parallel transport of vectors ¯r1, ¯r2 and ¯γ from the tangent space to ¯v=¯r3 to the tangent space to v=r3≠−¯r3 along the curve coinciding with the geodesic on S2 from v=r3 to ¯v=¯r3. This results in vectors ~r1, ~r2 and ~γ in the tangent space to r3; ¯γ is a general vector. Suppose that ¯v=¯r3=e3 and ¯r1=e1, ¯r2=e2. If ~γ=r1, its local coordinates in basis {~r1,~r2} belong to S1 and together with v fully characterize the rotation matrix from {r1,r2,r2} to {¯r1,¯r2,¯r3} (homeomorphism between SO(3)∖Rc and (S2∖{−e3})×S1). This figure is also useful to visualize the feedforward signal (21), which results from considering ¯γ=˙¯r3 and ~γ=Θ˙¯r3.

We then show that by applying a suitable input transformation, and can be controlled independently. We propose tracking control laws for the tracking subproblems in and , 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.,  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 denote a world fixed frame, and a body fixed frame centered at the vehicle’s center of mass. The third axis of , denoted by , is assumed to be aligned with the gravity vector, as is the third axis of , denoted by , when the quadrotor is hovering. Moreover, let and denote the position and velocity, respectively, of the center of mass of the quadrotor expressed in . Let denote the attitude of relative to .

The forces acting on the quadrotor and expressed in the world fixed frame are assumed to be the gravity force, , where and denote the mass and gravitational acceleration; the thrust force , where denotes the total thrust generated by the blades. Therefore,

 ˙p =v (1) m˙v =mge3−TRe3 ˙R =RS(ω),

where is the angular velocity expressed in , and for any , we let

 S(z):=⎡⎢⎣0−z3z2z30−z1−z2z10⎤⎥⎦.

The thrust vector, defined as , 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., is the control input of the model. Alternatively, one can consider that the thrust generated by each propeller generates torques, denoted by , which are related to angular velocities by

 I˙ω=−S(ω)(Iω)+τ, (2)

where denotes the inertia matrix.

### Ii-a Reference trajectory

Consider a reference continuous in time 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

 ¯T=¯r⊤3(−m˙¯v+mge3)>0, ∀t∈R≥0. (3)

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 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., , and the gravity vector does not exceed , i.e.,

 0

for every . We can parameterize this class with the position reference and an auxiliary angle , which plays the role of the yaw reference angle. The position reference , 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 , and the thrust , since, from the second equation in (1) we can write

 ¯r3=b∥b∥,  ¯T=m∥b∥,  b:=−¨¯p+ge3. (5)

The reference angle , 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 and the projection of on . Therefore, it defines through

 ¯r1=S(¯r⊥ψ)¯r3s(¯r⊥ψ,¯r3),  ¯r⊥ψ:=[−sin(¯ψ)cos(¯ψ)0]⊤,

where, for , . Given and , and from we obtain

 ¯ω1=−¯r⊤2˙¯r3,  ¯ω2=¯r⊤1˙¯r3,  ¯ω3=¯r⊤2˙¯r1. (6)

If (1), (2) are considered, then

### 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 to control the model

 ˙p=v,˙v=aref, (7)

such that the tracking errors and converge to zero, and (ii) find either and if the considered quadrotor model is (1) or and if (1), (2) are considered, such that the forces applied to the quadrotor (normalized by mass) track the desired virtual acceleration , i.e., the acceleration error

 ~a:=[00g]⊤+R[00−Tm]⊤−aref, (8)

converges to zero, and 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 to coincide with the reference vector and once such a condition is met, one can replace the goal by simply which is equivalent to stating that the angle of rotation about the normalized thrust vector (i.e., when ) 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 .

###### Remark 1

The inner-loop control problem for quadrotors is typically tackled directly in , or quaternion or Euler angles representations. For instance, letting represent a reference attitude and , the feedback controller (see [24, 11])

 ω(~R)=K3∑i=1kiei×(~Rei), (9)

where , and are distinct (e.g., ) is such that converges to for every initial condition . 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 to a desired value and controls independently the angle about the third column of so that 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 S2×S1

We start by introducing, in Section III-A, a convenient parametrization of , by defining a homeomorphism between and . In Section III-B we define an input transformation that allows us to decouple the control of the normalized thrust vector on from the control of the angle of rotation about the thrust vector, specified by a vector on . In Section III-C, we consider the normalized thrust vector control problem in , and in Section III-D, we consider the control of the angle about the thrust vector in . Section III-E combines the two strategies to provide the overall control law.

### Iii-a Parameterization of SO(3)∖Rc

We can parameterize as follows. Let and . For define the following two perpendicular vectors in the tangent space to at ,

 a⊥1(v):=−(I−vv⊤)s(e3,v)e3,  a⊥2(v):=S(e3)s(e3,v)v,

where denotes the identity matrix. Note that is also in the tangent space to at . Let

 b⊥1(v):=−(I−e3e3⊤)s(e3,v)v

be an extra vector in the tangent space to at , which is perpendicular to . The first component of these tangent vectors at are denoted by

 d1(v)=e⊤1b⊥1(v),  d2(v)=e⊤1a⊥2(v).

Moreover, define the following rotation matrices

 U(v) :=[a⊥1(v)a⊥2(v)v] V(v) :=⎡⎢⎣d1(v)−d2(v)0d2(v)d1(v)0001⎤⎥⎦ W(w) :=⎡⎢⎣w1−w20w2w10001⎤⎥⎦,

for . Then, the following map is the desired homeomorphism

 ϕ(v,w)={U(v)V(v)W(w) if e3≠vW(w) if e3=v,

where . The inverse is given by

In fact, it is clear that and are one-to-one and continuous; therefore, defines an homeomorphism between and .

### Iii-B Input transformation

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

 ˙R=RS(ω), (10)

and consider the standard stabilization problem of finding a control law for that drives to the identity matrix for a given initial condition . The tracking problem can either be converted into a stabilization problem by defining and noticing that , for or tackled as we do in the sequel.

For this problem, we define an input transformation where , such that, for ,

 ˙v =fA(v,~ω1,~ω2) (11) ˙w =fB(w,~ω3). (12)

Note that with this transformation we can control independently and .

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

 ω1=−r⊤2uv,ω2=r⊤1uv,

for some control input , and replace these expressions in the third column of (10), i.e., , we obtain

 ˙r3=r1r⊤1uv+r2r⊤2uv=(I−r3r⊤3)uv,

or, equivalently, substituting ,

 ˙v=(I−vv⊤)uv. (13)

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

 uv=~ω1z1+~ω2z2, (14)

for some vectors and in the tangent space of at , we conclude that (13) and (14) take the form (11).

Let and define

 Re :=[S(¯v)s(¯v,v)v−(I−¯v¯v⊤)s(¯v,v)v¯v] (15) Rr :=[S(¯v)s(¯v,v)v(I−vv⊤)s(¯v,v)¯vv].

The parallel transport from the space orthogonal to to the space orthogonal to along the geodesic curve in connecting the two is given by for a vector that belongs to the tangent space to at . We can then use the parallel transport for and and obtain a local frame in the tangent space of , the angle between this frame and is the error angle. Note that , where

 w1 =r⊤1RrR⊤e¯r1 (16) w2 =r⊤2RrR⊤e¯r1,

Moreover, , and the error angle is . We wish to drive this angle to zero, or equivalently, drive to and to . Let and be obtained in a similar fashion to (6) and be such that

 ˙Re=ReS(ωe),˙Rr=RrS(ωr).

Using these expressions, and the fact that, from (10), and , we can now write

 ˙w1 =ω3w2−(ω2r⊤3RrR⊤e¯r1−r⊤1RrS(ωr−ωe)R⊤e¯r1):=β1 (17) ˙w2 =−ω3w1+(ω1r⊤3RrR⊤e¯r1+r⊤2RrS(ωr−ωe)R⊤e¯r1):=β2.

Since we have , from which we conclude that , or, assuming that and , . Since we might have or but not simultaneously, the following is bounded

 ωr=⎧⎪ ⎪ ⎪⎨⎪ ⎪ ⎪⎩β2w1 % if |w1|>|w2|β1w2 otherwise.

Then we can write

 ˙w1=(ω3−ωr:=uw)w2,˙w2=−(ω3−ωr:=uw)w1, (18)

which, by considering , takes the form (12).

### Iii-C Controlling v∈S2

Let be the reference for . We propose the following control law to control (13),

 uv=uv,FB+uv,FF∈R3, (19)

where and can be seen as feedback and feedforward laws as detailed next.

For feedback, we consider

 uv,FB=⎧⎪ ⎪⎨⎪ ⎪⎩k1¯vif v⊤¯v≥0k1r1if v=−¯vk11s(v,¯v)¯votherwise, (20)

with . Intuitively, this control law is such that when used in (13), becomes a vector of the tangent space at proportional to the derivative at of the geodesic curve from to (the curve connecting and in with shortest distance). The norm of this tangent vector is unitary in and decreases as approaches in . Note that when the geodesic is not unique. Therefore the control law corresponds to a rotation in the direction of , while in fact, any unitary vector in the plane can be used.

For the feedforward term, we consider

 uv,FF=⎧⎨⎩˙¯vif v=¯v−˙¯vif v=−¯vΘ˙¯votherwise, (21)

with , which maps to the tangent space of at (see Fig. 2). The intuition behind this feedforward control law is that the angle between and 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 for define a given differentiable curve in and consider (13) with control law (19), (20) and (21). Then for every initial condition .

Proving that converges to as converges to infinity is equivalent to proving that converges to since . We shall prove that for every ,

 ˙ξ=⎧⎨⎩k1(1−ξ2), if 0≤ξ≤1,k1√(1−ξ2) if −1<ξ<0. (22)

Then, we can compute a closed-form expression for : if ,

 ξ(t)=κe2k1t−1κe2k1t+1, for every t≥0,

where and if , letting and

 ξ(t)=⎧⎪⎨⎪⎩e2k1t−1e2k1t+1, if t≥tssin(k1t+c1),if 0≤t

By taking the limit as converges to infinity, we conclude that converges to 1.

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

 ˙ξ =(˙¯v⊤v+¯v⊤˙v)=(˙¯v⊤v+¯v⊤(I−vv⊤)(uv,FB+uv,FF)).

Note that and therefore which can be rewritten as

 ˙ξ=⎧⎪ ⎪⎨⎪ ⎪⎩¯v⊤(I−vv⊤)k1¯vif v⊤¯v≥0¯v⊤(I−vv⊤)k1r1=0if v=−¯v¯v⊤(I−vv⊤)k1¯vs(v,¯v)otherwise. (23)

Since , then , and since monotonically increases, , for every . Then (23) implies (22), concluding the proof.

### Iii-D Controlling w∈S1

In this section, we consider an arbitrary function of time (not necessarily the identity). It is convenient therefore to define , where , which is still described by (16) but now for time-varying . Following the same steps as in Section III-B, we arrive at (17) with and replaced by where , . As a result, instead of (18), we obtain

 ˙w1=(uw−ζ)w2,˙w2=−(uw−ζ)w1,

where

 ζ:=⎧⎪ ⎪ ⎪⎨⎪ ⎪ ⎪⎩θ2w1 if % |w1|>|w2|θ1w2 otherwise.

We propose the following control law

 uw=uw,FB+uw,FF∈R, (24)

where and can be seen as feedback and feedforward laws. For feedforward, we consider and for feedback we consider

 uw,FB=⎧⎪⎨⎪⎩k2w2 if w1≥0−k2 if w1<0 and w2<0k2 if w1<0 and w2≥0, (25)

for some positive constant .

The next result establishes that this control drives to 1 and to , as desired.

###### Theorem 2

Let for define a given differentiable curve in . Consider the system (18) with control law (24), (25). Then

 limt→∞w1=1 and limt→∞w2=0, (26)

for every initial condition .

Replacing the control law , we obtain

 ˙w1=uw,FBw2,˙w2=−uw,FBw1.

Consider the Lyapunov function

 V(w1,w2)=(w1−1)2+w22=2(1−w1),

which is always positive definite. The derivative is given by and can be rewritten as

 ˙V(w1,w2)=⎧⎪⎨⎪⎩−k2w22if w1≥0k2w2if w1<0 and w2<0−k2w2if w1<0 and w2≥0. (27)

Therefore, the Lyapunov function derivative is negative semi-definite, and in the set , i.e., when , we have if , if . Therefore, the only solution that can stay in is , and thus according to LaSalle’s Theorem  we have that is globally asymptotically stable.

### Iii-E Inner-loop control law

The combined control law in allows for (10) to track a given reference as stated in the next result, which results from the arguments given in the previous sections.

###### Theorem 3

Let for define a given differentiable curve in and consider (10) with control law

 ω=[−r⊤2uvr⊤1uvuw+ωr]⊤, (28)

where are are obtained by replacing by and by in (19), (20) and (21), and in (15), (16), (17) respectively. Moreover, let, . Then

 limt→∞~R=I (29)

for every initial condition such that .

Since asymptotic convergence of to 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 .

## 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 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 where

 A:={[ 0  0  g ]⊤−r3Tm∣∣∣0

is a ball in and is the maximum total thrust that can be applied. Since, for a vector , , where

 A∞:={[ 0  0  g ]⊤−r3Tm∣∣∣0