**5. Control design procedure**

The equations of motion (2) of quadrotor defined in the previous section can be rewritten as

$$\begin{cases} \Sigma \mathbf{l} \begin{cases} \dot{\eta} = \nu \\ \dot{\nu} = \frac{1}{m} \operatorname{Re}\_3 T - \operatorname{ge}\_z \end{cases} \tag{6} $$

$$\begin{cases} \dot{R}e\_3 = RS(\Omega)e\_3\\ \dot{\Theta} = K(\Theta)\Omega\\ \dot{\Omega} = -\Omega \times (I\Omega) - G\_a + \tau\_a \end{cases} \tag{7}$$

The dynamic model above is separated into two dynamic systems; translational dynamic system Σ1 and rotational dynamic system Σ2. These two systems are chosen completely coupled by the rotation vector *Re*<sup>3</sup> and total thrust vector *T* as shown in **Figure 2**. In our methodology, we adopt a hierarchical controller design for the entire system dividing it into translational and rotational dynamics subsytems as it is shown in **Figure 2**. In this design, we assign the outer loop for the position controller and the inner loop for the attitude controller. The control design procedure can be summarized in the following two points:

**1.** In the design of the position controller, we propose a nonlinear filter to estimate the linear velocity tracking error rather than having its feedback measurement and design the intermediate position control from which we can calculate the desired value of the total thrust and also extract the desired orientation that will be considered as a reference for the rotational dynamics loop.

**2.** For the attitude controller, backstepping technique and control design with a barrier Lyapunov function have been applied due to the fact that the dynamic of Σ<sup>2</sup> is in the form of "strict feedback" system, which is appropriate for such a controller. The virtual control laws for angular velocities resulting from applying backstepping technique are obtained *α*Ω = [*αr*, *αp*, *αq*] to ensure the asymptotic convergence of followed rotation angles errors to zero, from the torques. Then, the four rotational speed signals can be calculated by the transformation matrix *R*.

**Figure 2.** Control architecture.

We will introduce barrier Lyapunov function (BLF) to design the attitude tracking controller. This function is designed to avoid the constraints (singularity points) that may occur in the rotation angles, roll, pitch and yaw and to produce a feasible initial condition for torques control.

**Definition**. A barrier Lyapunov function is a scalar function *V*(*x*), defined with respect to the system *x*˙ = *f* (*x*) on an open region D containing the origin, that is continuous, positive definite, has continuous first-order partial derivative at every point of D, has the property *V* (*x*)→*∞* as *x* approaches the boundary of D and satisfies *V* (*x*(*t*))≤*b* ∀ *t* ≥0 along the solution of *x*˙ = *f* (*x*) for *x*(0)∈D and some positive constant b [13].

**Lemma 1**. For any positive constant *kb*<sup>1</sup> , let Z<sup>1</sup> <sup>=</sup> *<sup>z</sup>*1∈R: *<sup>z</sup>*<sup>1</sup> <sup>&</sup>lt; <sup>|</sup>*k*<sup>1</sup> <sup>|</sup> <sup>⊂</sup>R and N: =R*<sup>l</sup>* <sup>×</sup>Z1⊂R*<sup>l</sup>*+1 be open sets [13].

Consider the system

intermediate position control from which we can calculate the desired value of the total thrust and also extract the desired orientation that will be considered as a reference for

**2.** For the attitude controller, backstepping technique and control design with a barrier Lyapunov function have been applied due to the fact that the dynamic of Σ<sup>2</sup> is in the form of "strict feedback" system, which is appropriate for such a controller. The virtual control laws for angular velocities resulting from applying backstepping technique are obtained *α*Ω = [*αr*, *αp*, *αq*] to ensure the asymptotic convergence of followed rotation angles errors to zero, from the torques. Then, the four rotational speed signals can be calculated by the

We will introduce barrier Lyapunov function (BLF) to design the attitude tracking controller. This function is designed to avoid the constraints (singularity points) that may occur in the rotation angles, roll, pitch and yaw and to produce a feasible initial condition for torques

**Definition**. A barrier Lyapunov function is a scalar function *V*(*x*), defined with respect to the system *x*˙ = *f* (*x*) on an open region D containing the origin, that is continuous, positive definite, has continuous first-order partial derivative at every point of D, has the property *V* (*x*)→*∞* as *x* approaches the boundary of D and satisfies *V* (*x*(*t*))≤*b* ∀ *t* ≥0 along the solution of *x*˙ = *f* (*x*) for

the rotational dynamics loop.

142 Recent Progress in Some Aircraft Technologies

transformation matrix *R*.

**Figure 2.** Control architecture.

*x*(0)∈D and some positive constant b [13].

control.

$$
\dot{\zeta} = h(t, \zeta) \tag{8}
$$

where *ζ* : = *ω*,*z*<sup>1</sup> *<sup>T</sup>* <sup>∈</sup>N, and *<sup>h</sup>* :R<sup>+</sup> <sup>×</sup>N→R*l*+1 is piecewise continuous in *t* and locally Lipschitz in *z*, uniformly in *t*, on R<sup>+</sup> <sup>×</sup>N. Suppose that there exist functions *<sup>U</sup>* :N*<sup>l</sup>* →R+ and *V*<sup>1</sup> :Z1→R+, continuously differentiable and positive definite in their respective domains, such that

$$V\_1(z\_1) \to \lnot\text{as} z\_1 \to z\_1 \to \pm k\_{h\_{\lnot}} \tag{9}$$

$$
\sigma\_1(\|\omega\|\|) \le \mathcal{U}(\omega) \le \sigma\_2(\|\omega\|\|) \tag{10}
$$

where *σ*1 and *σ*2 are class *k*∞ functions. Let *V* (*ζ*):=*V*1(*z*1) + *U* (*ω*) and *z*1(0) belong to the set *z*1∈(−*kb*<sup>1</sup> ,*kb*1 ). If the inequality holds:

$$
\dot{V} = \frac{\partial V}{\partial \zeta} h \le 0 \tag{11}
$$

then *z*1(*t*) remains in the open set *z*1∈(−*kb*<sup>1</sup> ,*kb*1 ) ∀ *t* ∈ 0, *∞*).

**Proof**. The conditions on *h* ensure the existence and uniqueness of a maximal solution *ζ*(*t*) on the time interval 0,*τmax*), according to [14]. This implies that *V*(*ζ*(*t*)) exist for all *t* ∈ 0, *τmax*).

Since *V*(*ζ*) is positive definite and *V*˙ <sup>≤</sup>0, we know that *<sup>V</sup>* (*ζ*(*t*))≤*<sup>V</sup>* (*ζ*(0)) for all *<sup>t</sup>* <sup>∈</sup> 0, *<sup>τ</sup>max*). From *V* (*ζ*):=*V*1(*z*1) + *U* (*ω*) and the fact that *V*1(*z*1) and *U* (*ω*) are positive functions, it is obvious that *V*1(*z*1(*t*)) is also bounded for all *t* ∈ 0, *τmax*). As a result, it is known from Eq. (9) that | *z*<sup>1</sup> | ≠ ± *kb*<sup>1</sup> . Based on the *z*1(0)< <sup>|</sup>*kb*<sup>1</sup> |, We can figure out that *z*1(*t*) remains in the set *z*<sup>1</sup> <sup>&</sup>lt; <sup>|</sup>*kb*<sup>1</sup> <sup>|</sup> for all *t* ∈ 0, *τmax*).

Accordingly, there exist a compact set *K* ⊆N such that the maximal solution of Eq. (8) satisfies *ζ*(*t*)∈ *K* for all *t* ∈ 0, *τmax*). Based on [14], we conclude that *ζ*(*t*) is defined for all *t* ∈ 0, *∞*) and hence *z*1(*t*)∈(−*kb*<sup>1</sup> ,*kb*1 ) ∀ *t* ∈ 0, *∞*).

#### **5.1. Position control design**

To design the position control for the dynamics of translational subsystem Eq. (6) that accomplish our control goal when no linear velocity measurements are available, we have considered a nonlinear first-order filter to estimate the tracking error of the linear velocity. The classification of the designed filter is output feedback controller [15]. It is designed with hyperbolic tangent function to ensure boundedness of thrust vector. The form of the filter can be written as

$$\begin{aligned} \dot{\nu}\_{\nu} &= q + B \eta\_{\mu} \\ \dot{q}\_{\mu} &= -A \tanh(\nu) \end{aligned} \tag{12}$$

where *A* and *B* are two diagonal matrices.

Next section presents an explanation of operation of selected filter and simple simulation example.

#### *5.1.1. Filter principal design*

The main idea of our designed filter is inspired from a simple first-order linear high-pass filter [16].

$$\frac{q(j\alpha)}{\mathbf{x}\_{\rm l}(j\alpha)} = \frac{\beta}{j\alpha - \beta} \tag{13}$$

To interpret the filter in frequency domain, we introduce the variable *s* = *jω* and replace this variable in Eq. (13), which yields

$$\text{sq} = \beta(\mathbf{x}\_1 + q) \tag{14}$$

where

$$q = \frac{\beta x\_1}{s - \beta}$$

extracting the term (*x*<sup>1</sup> + *q*), we get

$$\mathbf{x}\_1(\mathbf{x}\_1 + q) = \mathbf{x}\_1 + \frac{\beta \mathbf{x}\_1}{\mathbf{s} - \beta} = \frac{s \mathbf{x}\_1}{\mathbf{s} - \beta} \tag{15}$$

#### *5.1.2. Intermediate position control design*

The goal of this part is to design an intermediate control that ensures the convergence of position tracking and velocity errors to zero.

A Hierarchical Tracking Controller for Quadrotor Without Linear Velocity Measurements http://dx.doi.org/10.5772/62442 145

$$\lim\_{t \to 0} \|\eta - \eta\_d\| = 0,\\ \lim\_{t \to 0} \|v - v\_d\| = 0 \tag{16}$$

The choice of vector input *U* = *ux*,*uy*,*uz <sup>T</sup>* <sup>=</sup> <sup>1</sup> *<sup>m</sup> Re*3*T* leads easily to extract the desired orientation since it constitutes the link between the position dynamics and attitude dynamics. This can be formulated as

$$U = \frac{1}{m} \operatorname{Re}\_3 T = -k\_\rho \tanh(\eta\_e) - k\_d \tanh(\nu) + \ddot{\eta}\_d + \operatorname{ge}\_z \tag{17}$$

where *kp* and *kd* are two column vectors that contain three elements which will be defined by the proof of stability of position controller. The boundedness of Eq. (17) must be taken into consideration.

$$\|\mathcal{U}\| = \|\frac{T}{m}\| = \|-k\_p \tanh(\eta\_\varepsilon) - k\_d \tanh(\upsilon) + \eta\_d^\circ + g e\_\mathbb{Z}\|\tag{18}$$

considering

considered a nonlinear first-order filter to estimate the tracking error of the linear velocity. The classification of the designed filter is output feedback controller [15]. It is designed with hyperbolic tangent function to ensure boundedness of thrust vector. The form of the filter can

=

& *<sup>e</sup> v qB*

1 ( ) <sup>=</sup> ( ) w

*q j*

w

= tanh( ) + h

Next section presents an explanation of operation of selected filter and simple simulation

The main idea of our designed filter is inspired from a simple first-order linear high-pass filter

 b

To interpret the filter in frequency domain, we introduce the variable *s* = *jω* and replace this

<sup>1</sup> = b

*<sup>x</sup> <sup>q</sup> <sup>s</sup>*

b

+ + - -

b

The goal of this part is to design an intermediate control that ensures the convergence of

1 1 ( )= =

 b

1 1

 b

*x sx xq x s s* (15)

*sq x q* =( ) b

 wb-

*q Av* (12)

*xj j* (13)

<sup>1</sup> + (14)

be written as

example.

[16].

where

*5.1.1. Filter principal design*

variable in Eq. (13), which yields

extracting the term (*x*<sup>1</sup> + *q*), we get

*5.1.2. Intermediate position control design*

position tracking and velocity errors to zero.

where *A* and *B* are two diagonal matrices.

144 Recent Progress in Some Aircraft Technologies

$$\|\|\mathbf{U}\|\| = \|\frac{T}{m}\| \le k\_p + k\_\mathfrak{g} + k\_d \tag{19}$$

such that: *kg* ≤ ∥*η*¨ *<sup>d</sup>* ∥ + *g*. So, the selected positive variables of *kp*,*kd*, *kg* guaranteeing the vector thrust to be bounded.

To show the stability of closed loop position system with the controller Eq. (19), we will introduce it later in the Appendix A.

#### **5.2. Attitude control design**

The control of the attitude system is implemented to ensure the asymptotic convergence of desired orientation. we exploited the two interconnected blocks: block of virtual control *α*<sup>Ω</sup> = *αr*,*αp*,*α<sup>q</sup> <sup>T</sup>* and block of torque control *τ<sup>a</sup>* <sup>=</sup> *τφ*,*τθ*,*τψ* . Consider the subsystem (6, Σ2)

$$
\dot{\Theta} = k(\Theta)\Omega \tag{20}
$$

which describes the rotation system, the angular velocity vector Ω and torque vector *τa* that can be restated as

$$\Sigma\_2 = \begin{cases} \dot{\Theta} = f\_1(\Omega) \\ \dot{\Omega} = f\_1(\Omega, \tau) \end{cases} \tag{21}$$

Thus, the virtual angular velocity control laws were introduced to produce a zero error of the derivative of angular position Θ˙ <sup>=</sup> *<sup>e</sup>*˙*φ*,*e*˙*θ*,*e*˙*ψ* and to obtain the appropriate torque for the control system. From Eq. (20), we define the angular velocity tracking error vector *e*<sup>Ω</sup> = *ep*,*eq*,*er <sup>T</sup>* . where

$$e\_{\alpha} = \Omega - a\_{\alpha} \tag{22}$$

By substituting the Eq. (22) into Eq. (20) yields

$$
\begin{bmatrix}
\dot{\phi} \\
\dot{\theta} \\
\dot{\nu}
\end{bmatrix} = \begin{bmatrix}
1 & \sin(\phi)\tan(\theta) & \cos(\phi)\tanh(\theta) \\
0 & \cos(\phi) & -\sin(\phi) \\
0 & \frac{\sin(\phi)}{\cos(\theta)} & \frac{\cos(\phi)}{\cos(\theta)} \\
\end{bmatrix} \begin{bmatrix}
e\_{\rho} + \alpha\_{\rho} \\
e\_{q} + \alpha\_{q} \\
e\_{r} + \alpha\_{r}
\end{bmatrix} \tag{23}
$$

similarly,

$$\begin{cases} \dot{\phi} = e\_\rho + \alpha\_\rho + \sin(\phi)\tan(\theta)q + \cos(\phi)\tan(\theta)r \\ \dot{\theta} = \cos(\phi)(e\_q + \alpha\_q) - \sin(\phi)r \\ \dot{\nu} = \frac{\cos(\phi)}{\cos(\theta)}(e\_r + \alpha\_r) + \frac{\sin(\phi)}{\cos(\theta)}q \end{cases} \tag{24}$$

the derivative of angular position error can be derived below

$$\begin{aligned} \dot{e}\_{\phi} &= \dot{\phi} - \dot{\phi}\_{d} \\ &= e\_{\rho} + \alpha\_{\rho} + \sin(\phi)\tan(\theta)q + \cos(\phi)\tan(\theta)r - \dot{\phi}\_{d} \\ \dot{e}\_{\theta} &= \dot{\theta} - \dot{\theta}\_{d} \\ &= \cos(\phi)(e\_{q} + \alpha\_{q}) - \sin(\phi)r\dot{\theta}\_{d} \\ \dot{e}\_{\nu} &= \dot{\nu} - \dot{\nu}\_{d} \\ &= \frac{\cos(\phi)}{\cos(\theta)}(e\_{r} + \alpha\_{r}) + \frac{\sin(\phi)}{\cos(\theta)}q - \dot{\nu}\_{d} \end{aligned} \tag{25}$$

We choose an appropriate barrier Lyapunov function for roll and pith angles. And this is not needed for the yaw angle since it has singularity-free condition and we can use a single quadratic function to derive its control law.

The augmented Lyapunov function can be stated as:

$$V\_1 = \frac{1}{2}e\_\nu^{\ \ 2} + \frac{1}{2}\log\frac{k\_\phi^{\ \ 2}}{k\_\phi^{\ \ 2} - e\_\phi^{\ \ 2}} + \frac{1}{2}\log\frac{k\_\theta^{\ \ 2}}{k\_\theta^2 - e\_\theta^{\ \ 2}}\tag{26}$$

and its time derivative is

Thus, the virtual angular velocity control laws were introduced to produce a zero error of the derivative of angular position Θ˙ <sup>=</sup> *<sup>e</sup>*˙*φ*,*e*˙*θ*,*e*˙*ψ* and to obtain the appropriate torque for the control

*<sup>T</sup>* . where

(23)

(24)

(25)

(22)

system. From Eq. (20), we define the angular velocity tracking error vector *e*<sup>Ω</sup> = *ep*,*eq*,*er*

*e*W W = W a

1 sin( ) tan( ) cos( ) tanh( ) = 0 cos( ) sin( ) sin( ) cos( ) <sup>0</sup> cos( ) cos( )

ê ú é ù é ù <sup>+</sup> ê ú ê ú - + ê ú ê ú ê ú ê ú ë û + ë û ë û

= sin( ) tan( ) cos( ) tan( )

*e qr*

 f

> f

> > q

é ù

 f  q

> fq

 f

 y

&

 qf

&

fa

*e e e*

fa

 q  a

*p p q q r r*

 fq

> f

 f

q

= cos( )( ) sin( )

*q q*

<sup>ï</sup> ++ + <sup>ï</sup>

 fq

*e r*

*e q*

= sin( ) tan( ) cos( ) tan( )

*q q d*

*e q*

 q

*e qr*

++ + -

*p p d*

 fq

&

 f

++ -

*r r d*

 q

We choose an appropriate barrier Lyapunov function for roll and pith angles. And this is not needed for the yaw angle since it has singularity-free condition and we can use a single

 a

*p p*

 fa

f

í + -

q

the derivative of angular position error can be derived below

a

*d*

f

*d*

f

*d*

q

<sup>ï</sup> + + ïî

= cos( )( ) sin( )

*ee r*

 a

 f

+ -

cos( ) sin( ) = () cos( ) cos( )

a

cos( ) sin( ) = () cos( ) cos( )

*r r*

 a

By substituting the Eq. (22) into Eq. (20) yields

146 Recent Progress in Some Aircraft Technologies

f

& & &

q

y

f

& &

ì

ï

ï

q

y

&

=

& & &

f f




q q

y y

f

*e*

q

y

quadratic function to derive its control law.

*e*

=

& & &

=

& & &

similarly,

$$\dot{V}\_1 = e\_\nu \dot{e}\_\nu + \frac{e\_\phi \dot{e}\_\phi}{k\_\phi^{\,^2} - e\_\phi^{\,^2}} + \frac{e\_\theta \dot{e}\_\theta}{k\_\theta^{\,^2} - e\_\theta^{\,^2}}\tag{27}$$

So, the virtual control law for the angular velocity is derived as follows:

$$\begin{aligned} \alpha\_{\rho} &= -\sin(\phi)\tan(\theta)q - \cos(\phi)\tan(\theta)r + \dot{\phi}\_d - k\_{\phi}e\_{\phi} \\ \alpha\_{\epsilon} &= -k\_{\theta}\cos(\phi)e\_{\theta} + \tan(\theta)r + \frac{\dot{\partial}\_d}{\cos(\phi)} \\ \alpha\_{r} &= \frac{\cos(\theta)}{\cos(\phi)}(\dot{\nu}\_d - \frac{\sin(\phi)}{\cos(\theta)}q - k\_{\nu}e\_{\nu}) \end{aligned} \tag{28}$$

Substituting Eq. (28) into Eq. (25) and then substituting the resulting equations into (27) yields

$$\begin{split} \dot{V}\_{1} &= -k\_{\psi}e\_{\psi}^{2} - \frac{k\_{\phi}e\_{\phi}^{2}}{k\_{\phi}^{2} - e\_{\phi}^{2}} \\ &- \frac{k\_{\theta}\cos^{2}(\phi)e\_{\phi}^{2}}{k\_{\theta}^{2} - e\_{\theta}^{2}} + \frac{\cos(\phi)}{\cos(\theta)}e\_{r}e\_{\psi} \\ &+ \frac{e\_{\theta}\cos(\phi)e\_{q}}{k\_{\theta}^{2} - e\_{\theta}^{2}} + \frac{e\_{\phi}e\_{\rho}}{k\_{\phi}^{2} - e\_{\phi}^{2}} \end{split} \tag{29}$$

It is obvious that Eq. (29) has negative terms and these terms can stabilize the system only if the conditions |*e<sup>φ</sup>* | <*kφ*, |*e<sup>θ</sup>* | <*kθ*∀*k<sup>φ</sup>* >0*and K<sup>θ</sup>* >0 are met. These conditions will be met using Lemma (1). The other positive terms have to be cancelled and this will be done in the next. Now, backstepping control algorithm was applied to the partial of rotation system to control the input torque *τa*. The stability of rotation closed loop system is proved. We define the angular velocity error and its derivative:

$$
\Omega\_{\iota} = \Omega - \alpha\_{\alpha} \tag{30}
$$

$$
\dot{\Omega}\_e = \dot{\Omega} - \dot{a}\_\Omega \tag{31}
$$

Consider the following positive definite Lyapunov function candidate:

$$V\_{RF} = V\_1 + \frac{1}{2} \Omega\_\nu^\mathrm{T} I \Omega\_\nu \tag{32}$$

and its time derivative is

$$
\dot{V}\_{\text{BF}} = \dot{V}\_1 + \Omega\_\varepsilon^r I \dot{\Omega}\_\varepsilon \tag{33}
$$

Substituting Eq. (30) into Eq. (32) yields

$$
\dot{V}\_{R^{r}} = \dot{V}\_{1} + \boldsymbol{\Omega}\_{e}^{r} [I \dot{\boldsymbol{\Omega}} - I \dot{\boldsymbol{\alpha}}\_{\boldsymbol{\Omega}}] \tag{34}
$$

Replacing the third equation of system (6, Σ2) in Eq. (33) yields

$$\dot{V}\_{R^{\varepsilon}} = \dot{V}\_{\text{l}} + \boldsymbol{\Omega}\_{\text{e}}^{\text{T}} [-\boldsymbol{\Omega} \times I \boldsymbol{\Omega} - G\_{a} + \boldsymbol{\tau}\_{a} - I \dot{\boldsymbol{\alpha}}\_{\text{\alpha}}] \tag{35}$$

The time derivative of *Vbf* ,using Eqs. (29) and (34), is given by

$$\dot{V}\_{\text{BF}} = \dot{V}\_1 + \Omega\_e^T [- (\Omega\_e + \alpha\_\alpha) \times I \Omega - G\_a + \tau\_a - I d\dot{\alpha}\_\alpha] \tag{36}$$

In view of last equation, we propose the following torque input for the rotational dynamics

$$\begin{aligned} \tau\_a &= k\_\varepsilon \Omega\_\varepsilon - \frac{e\_\phi}{k\_\phi^2 - e\_\phi^2} \begin{pmatrix} 0 \\ 1 \\ 0 \end{pmatrix} - \frac{\cos(\phi) e\_\phi}{k\_\theta^2 - e\_\theta^2} \begin{pmatrix} 0 \\ 0 \\ 1 \end{pmatrix} \\\ &- \frac{\cos(\phi)}{\cos(\theta)} e\_r e\_\psi + G\_a + I \dot{\phi}\_{\text{tr}} + \phi\_{\text{tr}} I \Omega \end{aligned} \tag{37}$$

and hence

$$\begin{aligned} \dot{W}\_1 &= -k\_\psi e\_\nu^2 - \frac{k\_\phi e\_\phi^2}{k\_\phi^2 - e\_\phi^2} \\ &- \frac{k\_\phi \cos(\phi) e\_\rho^2}{k\_\theta^2 - e\_\theta^2} \\ &- \Omega\_\epsilon^T k\_\epsilon \Omega\_\epsilon - \Omega\_\epsilon^T \Omega\_\epsilon I \Omega \le 0 \end{aligned} \tag{38}$$
