**3. Quadrotor mathematical model**

The quadrotor dynamics, also called the equations of motion, are a set of 6 s order differential equations. The quadrotor being a 6 DoF plant, a total of 12 states are required to describe its motion completely. These 12 states are described using the 6 equations of motion. These play a vital role in controller design and would be extensively used in the subsequent sections of this chapter.

The kinematic and dynamic models of a quadrotor will be derived based on a Newton-Euler formalism with the following assumptions [10]:


In this work, the vision-based position and altitude tracking control of a quadrotor UAV is considered. This would be then on used to align the drone to the center of a pre-defined landing pad marker on which the quadrotor would autonomously land. In practical missions, the stability of the quadrotor is easily affected by abrupt changes in the input commands. The flight controller that is designed must be capable in offering an accurate and robust control to the quadrotor. The controller demonstrated in this chapter is the sliding mode controller (SMC). The sliding mode control (SMC) technique, being a non-linear control technique, has found great applications in offering robust control solutions for handling quadrotors [2–7]. This chapter will briefly describe the process of implementing a vision algorithm alongside a classical SMC for autonomous landing of the quadrotor on a stationary

The quadrotor UAV is a highly non-linear, 6 DoF, Multi-Input-Multi-Output (MIMO) and under-actuated system [8]. One can describe the vehicle as having four propellers in cross configuration as shown in **Figure 1**. Quadrotor motion is controlled by varying the speed of the four rotors. A quadrotor has two sets of clockwise and two sets of counter-clockwise rotating propellers to neutralize the effective aerodynamic drag. Vertical movement of the quadrotor system is controlled by simultaneously increasing or decreasing the thrust of all rotors. Yawing motion is created by proportionally varying the speeds of counter-clockwise rotating propellers and the rolling and pitching motions are created by applying differ-

The quadrotor dynamics, also called the equations of motion, are a set of 6 s order differential equations. The quadrotor being a 6 DoF plant, a total of 12 states are required to describe its motion completely. These 12 states are described using the 6 equations of motion. These play a vital role in controller design and would be

ential thrust forces on opposite rotors of the quadrotor [9].

**3. Quadrotor mathematical model**

extensively used in the subsequent sections of this chapter.

platform.

**Figure 1.**

**30**

*Quadrotor UAV configuration.*

**2. Quadrotor configuration**

*Unmanned Robotic Systems and Applications*

• Thrust and drag are proportional to the square of propeller's speed.

The first step in developing the quadrotor kinematic model is to describe the different frames of references associated with the system. It is necessary to use these coordinate systems for the following reasons:


In this case, we describe a total of frames, namely: inertial frame (*F<sup>i</sup>* ), the vehicle frame (*F<sup>v</sup>* ), the vehicle frame-1 (*F<sup>v</sup>*<sup>1</sup> ), the vehicle frame-2 (*F<sup>v</sup>*<sup>2</sup> ), and the body frame (*F<sup>b</sup>*). The inertial frame is fixed at a point at ground level and uses the N-E-D notation, where N points towards north direction, E points towards east direction and D points towards earth. On the other hand, the body frame is at the center of quadrotor body, with its *x* axis pointing towards the front of the quadrotor, *y* axis pointing towards the left of the quadrotor and the z axis pointing towards the ground. The vehicle frame has an axis parallel to the inertial frame but has the origin shifted to the quadrotor's center of gravity. Vehicle frame's yaw is adjusted to match the quadrotor's yaw to get the vehicle frame-1 frame which is then pitch adjusted to get the vehicle frame-2. Finally the body frame is obtained by adjusting the roll of the vehicle frame-2.

The transformation from inertial to vehicle frame is just a simple translation. On the other hand, the transformation from vehicle to body frame is given by a rotation matrix *Rb <sup>v</sup>* ð Þ *ϕ; θ; ψ* , given by:

$$
\begin{aligned}
\begin{bmatrix}
R\_{\nu}^{b}(\boldsymbol{\theta},\boldsymbol{\theta},\boldsymbol{\psi}) &= R\_{\nu 2}^{b}(\boldsymbol{\theta})R\_{\nu 1}^{\varphi}(\boldsymbol{\theta})R\_{\nu}^{\varphi}(\boldsymbol{\psi}) &= \\
\begin{bmatrix}
1 & 0 & 0 \\
0 & \cos\left(\boldsymbol{\phi}\right) & \sin\left(\boldsymbol{\phi}\right) \\
0 & -\sin\left(\boldsymbol{\phi}\right) & \cos\left(\boldsymbol{\phi}\right)
\end{bmatrix}
\begin{bmatrix}
\cos\left(\boldsymbol{\theta}\right) & 0 & -\sin\left(\boldsymbol{\theta}\right) \\
0 & 1 & 0 \\
\sin\left(\boldsymbol{\theta}\right) & 0 & \cos\left(\boldsymbol{\theta}\right)
\end{bmatrix}
\begin{bmatrix}
\cos\left(\boldsymbol{\psi}\right) & \sin\left(\boldsymbol{\psi}\right) & 0 \\
0 & 0 & 1
\end{bmatrix} \\
&= \begin{bmatrix}
c\_{\theta}c\_{\psi} & s\_{\phi}s\_{\theta}c\_{\psi} - c\_{\phi}c\_{\psi} & c\_{\phi}s\_{\theta}c\_{\psi} + s\_{\phi}s\_{\psi} \\
c\_{\theta}s\_{\psi} & s\_{\phi}s\_{\theta}s\_{\psi} + c\_{\phi}c\_{\psi} & c\_{\phi}s\_{\theta}s\_{\psi} - s\_{\phi}c\_{\psi} \\
\end{bmatrix}
\end{aligned} \tag{1}$$

where, *ϕ*, *θ* and *ψ* represent the roll, pitch and yaw angles of the quadrotor measured in the vehicle frame-2, vehicle frame-1 and vehicle frame respectively. In addition to these Euler angles, the quadrotor is associated with several other state variables that describe its position, linear velocity and angular velocities. These are described as:

We need to relate p, q, and r to *ϕ*\_ , \_

*DOI: http://dx.doi.org/10.5772/intechopen.86057*

*ϕ*\_ 0 0

*<sup>v</sup>*<sup>2</sup> \_ *θ* � � and *Rv*<sup>2</sup>

1 0 � sin ð Þ*θ* 0 cosð Þ *ϕ* sin ð Þ *ϕ* cosð Þ*θ* 0 � sin ð Þ *ϕ* cosð Þ *ϕ* cosð Þ*θ*

*<sup>b</sup> <sup>ϕ</sup>*\_ � �*Rv*<sup>1</sup> *<sup>v</sup>*<sup>2</sup> \_ *θ* � �

*<sup>v</sup>*1ð Þ *<sup>ψ</sup>*\_ , *Rv*<sup>1</sup>

*<sup>b</sup> <sup>ϕ</sup>*\_ � �

noting that *R<sup>v</sup>*

3 7 <sup>5</sup> <sup>¼</sup> *Rv*<sup>2</sup>

¼

**3.2 Dynamic model**

where *f <sup>x</sup> f <sup>y</sup> f <sup>z</sup>*

**33**

h i*<sup>T</sup>*

equation of Coriolis we have:

2 6 4

Inverting this, we get:

*ϕ*\_ \_ *θ ψ*\_ 3 5 ¼ 2 4

> *m d***v** *dti* ¼ *m*

equation in body coordinates, where **<sup>v</sup>***<sup>b</sup>* <sup>¼</sup> ð Þ *<sup>u</sup>; <sup>v</sup>; <sup>w</sup>*

fore, in body coordinates the above equation becomes:

2 4

¼ *f*.

*u*\_ *v*\_ *w*\_ 3 5 ¼

For rotational motion, Newton's second law states that:

2 4

2 4

the equation modifies as follows:

*p q r*

2 6 4 *θ*, and *ψ*\_ . Since *ϕ*\_ , \_

0 \_ *θ* 0

3 7 5

1 sin ð Þ *ϕ* tan ð Þ*θ* cosð Þ *ϕ* tan ð Þ*θ* 0 cosðÞ � *ϕ* sin ð Þ *ϕ* 0 sin ð Þ *ϕ* sec ð Þ*θ* cosð Þ *ϕ* sec ð Þ*θ*

Let **v** be the velocity vector of the quadrotor. Newton's laws of motion hold good for inertial frames of references only. On applying these to a transnational frame,

where *m* is the mass of the quadrotor, *f* is the total applied to the quadrotor, and *<sup>d</sup>*

þ *ω<sup>b</sup>=<sup>i</sup>* � **v** � �

> 3 5 þ 1 *m*

*<sup>T</sup>*, and *ω<sup>b</sup>*

*f x f y f z*

3

¼ **m** (8)

2 4

is the time derivative in the inertial frame. From the equation of Coriolis, we have:

where *ω<sup>b</sup>=<sup>i</sup>* is the angular velocity of the air-frame with respect to the inertial frame. Since the control force is computed and applied in the body coordinate system, and since *ω* is measured in body coordinates, we will express the above

> *rv* � *qw pw* � *ru qu* � *pv*

*d***h***<sup>b</sup> dti*

where **h** is the angular momentum and **m** is the applied torque. Using the

*d***v** *dtb*

*m d***v** *dti*

3 7 <sup>5</sup> <sup>þ</sup> *<sup>R</sup>v*<sup>2</sup>

> *ϕ*\_ \_ *θ ψ*\_

2 6 4

*Vision-Based Autonomous Control Schemes for Quadrotor Unmanned Aerial Vehicle*

*<sup>b</sup> <sup>ϕ</sup>*\_ � � are all identity matrices, we get:

*<sup>b</sup> <sup>ϕ</sup>*\_ � �*Rv*<sup>1</sup> *<sup>v</sup>*<sup>2</sup> \_ *θ* � �*R<sup>v</sup> <sup>v</sup>*1ð Þ *ψ*\_

*θ* and *ψ*\_ are small and

0 0 *ψ*\_

3 7 5 9

>>>>>>>>>>>=

(3)

*dti*

*<sup>T</sup>*. There-

>>>>>>>>>>>;

5 (4)

2 6 4

3 5

¼ *f* (5)

¼ *f* (6)

*<sup>b</sup>=<sup>i</sup>* ¼ ð Þ *p; q;r*

5 (7)

2 4 *p q r*

3

1. *x*—The inertial (north) position of the quadrotor.

2. *y*—The inertial (east) position of the quadrotor.

3. *z*—The altitude of the aircraft.

4.*u*—The body frame velocity in *x* direction in body frame.

5. *v*—The body frame velocity in *y* direction in body frame.

6.*w*—The body frame velocity in *z* direction in body frame.

7. *p*—The roll rate measured in body frame.

8.*q*—The pitch rate measured in body frame.

9.*r*—The yaw rate measured in body frame.

Hence a total of 12 states are used to describe the motion of the quadrotor in the 3D space.

#### **3.1 Kinematic model**

The position derivatives ð Þ *x*\_*; y*\_*; z*\_ are inertial frame quantities and velocities ð Þ *u; v; w* are in the body frame. They can be related through the transformation matrix as follows [11]:

$$\begin{aligned} \frac{d}{dt} \begin{bmatrix} x \\ y \\ z \end{bmatrix} &= R\_b^v \begin{bmatrix} u \\ v \\ w \\ w \end{bmatrix} \\ &= \begin{pmatrix} R\_v^b \end{pmatrix}^T \begin{bmatrix} u \\ v \\ v \\ w \end{bmatrix} \\ &= \begin{bmatrix} \mathcal{L}\_{\theta} \mathcal{L}\_{\psi} & \mathcal{S}\_{\phi} \mathcal{S}\_{\theta} \mathcal{L}\_{\psi} - \mathcal{L}\_{\phi} \mathcal{L}\_{\psi} \mathcal{L}\_{\theta} \mathcal{S}\_{\psi} + \mathcal{S}\_{\phi} \mathcal{S}\_{\psi} \\ \mathcal{S}\_{\theta} \end{bmatrix} \\ &= \begin{bmatrix} \mathcal{L}\_{\theta} \mathcal{L}\_{\psi} & \mathcal{S}\_{\phi} \mathcal{S}\_{\theta} \mathcal{S}\_{\psi} + \mathcal{L}\_{\theta} \mathcal{L}\_{\psi} \mathcal{L}\_{\theta} \mathcal{S}\_{\psi} - \mathcal{S}\_{\phi} \mathcal{L}\_{\psi} \\ -\mathcal{S}\_{\theta} & \mathcal{S}\_{\phi} \mathcal{C}\_{\theta} \mathcal{L}\_{\theta} \mathcal{O} \end{bmatrix} \begin{bmatrix} u \\ v \\ w \\ w \end{bmatrix} \end{aligned} \tag{2}$$

The relationship between absolute angles *ϕ*, *θ*, and *ψ*, and the angular rates p, q, and r is also complicated by the fact that these quantities are defined in different coordinate frames. The angular rates are defined in the body frame *F<sup>b</sup>*, whereas the roll angle *ϕ* is defined in *F<sup>v</sup>*<sup>2</sup> , the pitch angle *θ* is defined in *F<sup>v</sup>*<sup>1</sup> , and the yaw angle *ψ* is defined in the vehicle frame *F<sup>v</sup>* .

*Vision-Based Autonomous Control Schemes for Quadrotor Unmanned Aerial Vehicle DOI: http://dx.doi.org/10.5772/intechopen.86057*

We need to relate p, q, and r to *ϕ*\_ , \_ *θ*, and *ψ*\_ . Since *ϕ*\_ , \_ *θ* and *ψ*\_ are small and noting that *R<sup>v</sup> <sup>v</sup>*1ð Þ *<sup>ψ</sup>*\_ , *Rv*<sup>1</sup> *<sup>v</sup>*<sup>2</sup> \_ *θ* � � and *Rv*<sup>2</sup> *<sup>b</sup> <sup>ϕ</sup>*\_ � � are all identity matrices, we get:

$$\begin{aligned} \begin{bmatrix} P \\ q \\ r \end{bmatrix} &= R\_b^{v2} \left( \dot{\phi} \right) \begin{bmatrix} \dot{\phi} \\ 0 \\ 0 \\ 0 \end{bmatrix} + R\_b^{v2} \left( \dot{\phi} \right) R\_{v2}^{v1} \left( \dot{\theta} \right) \begin{bmatrix} 0 \\ \dot{\theta} \\ 0 \\ 0 \end{bmatrix} + R\_b^{v2} \left( \dot{\phi} \right) R\_{v2}^{v1} \left( \dot{\theta} \right) R\_{v1}^{v} \left( \dot{\psi} \right) \begin{bmatrix} 0 \\ 0 \\ \dot{\psi} \end{bmatrix} \end{aligned} \tag{3}$$
 
$$\begin{aligned} &= \begin{bmatrix} 1 & 0 & -\sin\left(\theta\right) \\ 0 & \cos\left(\phi\right) \sin\left(\phi\right) \cos\left(\theta\right) \\ 0 & -\sin\left(\phi\right) \cos\left(\phi\right) \cos\left(\theta\right) \end{bmatrix} \begin{bmatrix} \dot{\phi} \\ \dot{\theta} \\ \dot{\psi} \end{bmatrix} \end{aligned} \tag{3}$$

Inverting this, we get:

$$
\begin{bmatrix}
\dot{\phi} \\
\dot{\theta} \\
\dot{\psi}
\end{bmatrix} = \begin{bmatrix}
1 & \sin\left(\phi\right)\tan\left(\theta\right) & \cos\left(\phi\right)\tan\left(\theta\right) \\
0 & \cos\left(\phi\right) & -\sin\left(\phi\right) \\
0 & \sin\left(\phi\right)\sec\left(\theta\right) & \cos\left(\phi\right)\sec\left(\theta\right)
\end{bmatrix} \begin{bmatrix} p \\ q \\ r \end{bmatrix} \tag{4}$$

### **3.2 Dynamic model**

where, *ϕ*, *θ* and *ψ* represent the roll, pitch and yaw angles of the quadrotor measured in the vehicle frame-2, vehicle frame-1 and vehicle frame respectively. In addition to these Euler angles, the quadrotor is associated with several other state variables that describe its position, linear velocity and angular velocities. These are

1. *x*—The inertial (north) position of the quadrotor.

2. *y*—The inertial (east) position of the quadrotor.

7. *p*—The roll rate measured in body frame.

8.*q*—The pitch rate measured in body frame.

9.*r*—The yaw rate measured in body frame.

4.*u*—The body frame velocity in *x* direction in body frame.

5. *v*—The body frame velocity in *y* direction in body frame.

6.*w*—The body frame velocity in *z* direction in body frame.

Hence a total of 12 states are used to describe the motion of the quadrotor in the

The position derivatives ð Þ *x*\_*; y*\_*; z*\_ are inertial frame quantities and velocities ð Þ *u; v; w* are in the body frame. They can be related through the transformation

> *cθc<sup>ψ</sup> sϕsθc<sup>ψ</sup>* � *cϕc<sup>ψ</sup> cϕsθc<sup>ψ</sup>* þ *sϕs<sup>ψ</sup> cθs<sup>ψ</sup> sϕsθs<sup>ψ</sup>* þ *cϕc<sup>ψ</sup> cϕsθs<sup>ψ</sup>* � *sϕc<sup>ψ</sup>*

The relationship between absolute angles *ϕ*, *θ*, and *ψ*, and the angular rates p, q, and r is also complicated by the fact that these quantities are defined in different coordinate frames. The angular rates are defined in the body frame *F<sup>b</sup>*, whereas the

, the pitch angle *θ* is defined in *F<sup>v</sup>*<sup>1</sup>

�*s<sup>θ</sup> sϕcθcϕc<sup>θ</sup>*

.

*d dt* *x y z* 3 7 7 <sup>5</sup> <sup>¼</sup> *<sup>R</sup><sup>v</sup> b*

<sup>¼</sup> *<sup>R</sup><sup>b</sup> v* � �*<sup>T</sup>*

*u v w*

*u v w*

*u v w*

>>>>>>>>>>>>>>>>>>>>>;

9

>>>>>>>>>>>>>>>>>>>>>=

(2)

, and the yaw angle *ψ*

3. *z*—The altitude of the aircraft.

*Unmanned Robotic Systems and Applications*

described as:

3D space.

**3.1 Kinematic model**

matrix as follows [11]:

¼

roll angle *ϕ* is defined in *F<sup>v</sup>*<sup>2</sup>

**32**

is defined in the vehicle frame *F<sup>v</sup>*

Let **v** be the velocity vector of the quadrotor. Newton's laws of motion hold good for inertial frames of references only. On applying these to a transnational frame, the equation modifies as follows:

$$m\frac{d\mathbf{v}}{dt\_i} = f \tag{5}$$

where *m* is the mass of the quadrotor, *f* is the total applied to the quadrotor, and *<sup>d</sup> dti* is the time derivative in the inertial frame. From the equation of Coriolis, we have:

$$m\frac{d\mathbf{v}}{dt\_i} = m\left(\frac{d\mathbf{v}}{dt\_b} + \alpha\_{b/i} \times \mathbf{v}\right) = f \tag{6}$$

where *ω<sup>b</sup>=<sup>i</sup>* is the angular velocity of the air-frame with respect to the inertial frame. Since the control force is computed and applied in the body coordinate system, and since *ω* is measured in body coordinates, we will express the above equation in body coordinates, where **<sup>v</sup>***<sup>b</sup>* <sup>¼</sup> ð Þ *<sup>u</sup>; <sup>v</sup>; <sup>w</sup> <sup>T</sup>*, and *ω<sup>b</sup> <sup>b</sup>=<sup>i</sup>* ¼ ð Þ *p; q;r <sup>T</sup>*. Therefore, in body coordinates the above equation becomes:

$$
\begin{bmatrix}
\dot{u} \\
\dot{v} \\
\dot{w}
\end{bmatrix} = \begin{bmatrix}rv - qu \\ pw - ru \\ qu - pv
\end{bmatrix} + \frac{1}{m} \begin{bmatrix}f\_x \\ f\_y \\ f\_z \end{bmatrix} \tag{7}
$$

where *f <sup>x</sup> f <sup>y</sup> f <sup>z</sup>* h i*<sup>T</sup>* ¼ *f*.

For rotational motion, Newton's second law states that:

$$\frac{d\mathbf{h}^b}{dt\_i} = \mathbf{m} \tag{8}$$

where **h** is the angular momentum and **m** is the applied torque. Using the equation of Coriolis we have:

*Unmanned Robotic Systems and Applications*

$$\frac{d\mathbf{h}}{dt\_i} = \left(\frac{d\mathbf{h}}{dt\_b} + a\_{b/i} \times \mathbf{h}\right) = \mathbf{m} \tag{9}$$

**3.3 Forces and moments**

primarily due to gravity and the four propellers.

acting on the quadrotor is given by:

*DOI: http://dx.doi.org/10.5772/intechopen.86057*

motor pair 1–3 and 2–4 and is given as:

torque of each motor can be expressed as:

*F τϕ τθ τψ*

zero and one.

**35**

torques on the quadrotor can be written in matrix form as:

torques. The actual motors commands can be found as:

*K*<sup>1</sup> *K*<sup>1</sup> *K*<sup>1</sup> *K*<sup>1</sup> *LK*<sup>1</sup> �*LK*<sup>1</sup> �*LK*<sup>1</sup> *LK*<sup>1</sup> *LK*<sup>1</sup> �*LK*<sup>1</sup> *LK*<sup>1</sup> �*LK*<sup>1</sup> *K*<sup>2</sup> *K*<sup>2</sup> �*K*<sup>2</sup> �*K*<sup>2</sup>

> *δ*1 *δ*2 *δ*3 *δ*4

1–4 and 2–3 and is given as:

The objective of this section is to describe the forces and torques that act on the quadrotor. Since there are no aerodynamic lifting surfaces, we will assume that the aerodynamic forces and moments are negligible. The forces and moments are

*Vision-Based Autonomous Control Schemes for Quadrotor Unmanned Aerial Vehicle*

As seen in **Figure 1**, each motor produces a force F and a torque *τ*. The total force

The rolling torque is produced by the force difference between the motor pair

Similarly, the pitching torque is produced by the force difference between the

Due to Newton's third law, the drag of the propellers produces a yawing torque on the body of the quadrotor. The direction of the torque will be in the opposite direction

The lift and drag produced by the propellers is proportional to the square of the angular velocity. We will assume that the angular velocity is directly proportional to the pulse width modulation command sent to the motor. Therefore, the force and

�

*F τϕ τθ τψ*

*δ*1 *δ*2 *δ*3 *δ*4

*δ*1 *δ*2 *δ*3 *δ*4

*F* <sup>∗</sup> ¼ *K*1*δ* <sup>∗</sup> *τ* <sup>∗</sup> ¼ *K*2*δ* <sup>∗</sup>

where *K*<sup>1</sup> and *K*<sup>2</sup> are constants that are determined experimentally, *δ* <sup>∗</sup> is the motor command signal, and \*—represents 1, 2, 3, and 4. Therefore, the forces and

The control strategies derived in subsequent sections will specify forces and

Note that the pulse width modulation commands are required to be between

of the motion of the propeller. Therefore the total yawing torque is given by:

*F* ¼ *F*<sup>1</sup> þ *F*<sup>2</sup> þ *F*<sup>3</sup> þ *F*<sup>4</sup> (15)

*τϕ* ¼ *L F*ð Þ� <sup>1</sup> þ *F*<sup>4</sup> *L F*ð Þ <sup>2</sup> þ *F*<sup>3</sup> (16)

*τθ* ¼ *L F*ð Þ� <sup>1</sup> þ *F*<sup>3</sup> *L F*ð Þ <sup>2</sup> þ *F*<sup>4</sup> (17)

*τψ* ¼ *τ*<sup>1</sup> þ *τ*<sup>2</sup> � *τ*<sup>3</sup> � *τ*<sup>4</sup> (18)

(19)

(20)

(21)

Again, the above equation is most easily resolved in body coordinates where **<sup>h</sup>***<sup>b</sup>* <sup>¼</sup> **<sup>J</sup>***ω<sup>b</sup> b=i* , where **J** is the constant inertia matrix given by:

$$\mathbf{J} = \begin{bmatrix} J\_{\mathbf{x}} & -J\_{\mathbf{xy}} & -J\_{\mathbf{x}x} \\ -J\_{\mathbf{xy}} & J\_{\mathbf{y}} & -J\_{\mathbf{yx}} \\ -J\_{\mathbf{x}x} & -J\_{\mathbf{yx}} & J\_{x} \end{bmatrix} \tag{10}$$

As we use a quadrotor with a symmetric frame about all three axes, *Jxy* ¼ *Jyz* ¼ *Jxz* ¼ 0. Hence, **J** becomes:

$$\mathbf{J} = \begin{bmatrix} J\_x & \mathbf{0} & \mathbf{0} \\ \mathbf{0} & J\_y & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & J\_z \end{bmatrix} \tag{11}$$

Defining **<sup>m</sup>***<sup>b</sup>* <sup>¼</sup> *τϕτθτψ* � �*<sup>T</sup>* we can write Eq. (9) in the body coordinates as:

$$\mathbf{J} \begin{bmatrix} \dot{p} \\ \dot{q} \\ \dot{r} \end{bmatrix} = \begin{bmatrix} 0 & r & -q \\ -r & 0 & p \\ p & -q & 0 \end{bmatrix} \begin{bmatrix} J\_x & 0 & 0 \\ 0 & J\_y & 0 \\ 0 & 0 & J\_z \end{bmatrix} \begin{bmatrix} p \\ q \\ r \end{bmatrix} + \begin{bmatrix} \tau\_{\phi} \\ \tau\_{\theta} \\ \tau\_{\psi} \end{bmatrix} \tag{12}$$

Hence:

$$
\begin{bmatrix}
\dot{p} \\
\dot{q} \\
\dot{r}
\end{bmatrix} = \begin{bmatrix}
\frac{J\_y - J\_x}{J\_x} qr \\
\frac{J\_x - J\_x}{J\_y} pr \\
\frac{J\_y - J\_y}{J\_x} qp \\
\frac{J\_x - J\_y}{J\_x} qp
\end{bmatrix} + \begin{bmatrix}
\frac{1}{J\_x} \tau\_\phi \\
\frac{1}{J\_y} \tau\_\theta \\
\frac{1}{J\_x} \tau\_\psi \\
\end{bmatrix} \tag{13}
$$

To summarize, from Eqs. (2)–(13), we obtain the 6 DoF equation of a quadrotor and is given as follows:

*x*\_ *y*\_ *z*\_ 2 4 3 5 ¼ *cθc<sup>ψ</sup> sϕsθc<sup>ψ</sup>* � *cϕc<sup>ψ</sup> cϕsθc<sup>ψ</sup>* þ *sϕs<sup>ψ</sup> cθs<sup>ψ</sup> sϕsθs<sup>ψ</sup>* þ *cϕc<sup>ψ</sup> cϕsθs<sup>ψ</sup>* � *sϕc<sup>ψ</sup>* �*s<sup>θ</sup> sϕc<sup>θ</sup> cϕc<sup>θ</sup>* 2 4 3 5 *u v w* 2 4 3 5 *ϕ*\_ \_ *θ ψ*\_ 2 6 4 3 7 5 ¼ 1 sin ð Þ *ϕ* tan ð Þ*θ* cosð Þ *ϕ* tan ð Þ*θ* 0 cosðÞ � *ϕ* sin ð Þ *ϕ* 0 sin ð Þ *ϕ* sec ð Þ*θ* cosð Þ *ϕ* sec ð Þ*θ* 2 4 3 5 *p q r* 2 4 3 5 *u*\_ *v*\_ *w*\_ 2 4 3 5 ¼ *rv* � *qw pw* � *ru qu* � *pv* 2 4 3 5 þ 1 *m f x f y f z* 2 6 4 3 7 5 *p*\_ *q*\_ *r*\_ 2 4 3 5 ¼ *Jy* � *Jz Jx qr Jz* � *Jx Jy pr Jx* � *Jy Jz qp* 2 6 6 6 6 6 6 6 4 3 7 7 7 7 7 7 7 5 þ 1 *Jx τϕ* 1 *Jy τθ* 1 *Jz τψ* 2 6 6 6 6 6 6 6 4 3 7 7 7 7 7 7 7 5 9 >>>>>>>>>>>>>>>>>>>>>>>>>>>>>= >>>>>>>>>>>>>>>>>>>>>>>>>>>>>; (14)

*Vision-Based Autonomous Control Schemes for Quadrotor Unmanned Aerial Vehicle DOI: http://dx.doi.org/10.5772/intechopen.86057*

### **3.3 Forces and moments**

*d***h** *dti*

**<sup>h</sup>***<sup>b</sup>* <sup>¼</sup> **<sup>J</sup>***ω<sup>b</sup> b=i* <sup>¼</sup> *<sup>d</sup>***<sup>h</sup>** *dtb*

, where **J** is the constant inertia matrix given by:

2 4

As we use a quadrotor with a symmetric frame about all three axes,

2 4

> 3 5

*Jy* � *Jz Jx*

*Jz* � *Jx Jy*

*Jx* � *Jy Jz*

*qr*

þ

*pr*

*qp*

To summarize, from Eqs. (2)–(13), we obtain the 6 DoF equation of a quadrotor

*cθc<sup>ψ</sup> sϕsθc<sup>ψ</sup>* � *cϕc<sup>ψ</sup> cϕsθc<sup>ψ</sup>* þ *sϕs<sup>ψ</sup> cθs<sup>ψ</sup> sϕsθs<sup>ψ</sup>* þ *cϕc<sup>ψ</sup> cϕsθs<sup>ψ</sup>* � *sϕc<sup>ψ</sup>* �*s<sup>θ</sup> sϕc<sup>θ</sup> cϕc<sup>θ</sup>*

1 sin ð Þ *ϕ* tan ð Þ*θ* cosð Þ *ϕ* tan ð Þ*θ* 0 cosðÞ � *ϕ* sin ð Þ *ϕ* 0 sin ð Þ *ϕ* sec ð Þ*θ* cosð Þ *ϕ* sec ð Þ*θ*

> *f x f y f z*

3 7 5

2 6 4

> 1 *Jx τϕ*

1 *Jy τθ*

1 *Jz τψ*

2 4

**J** ¼

0 *r* �*q* �*r* 0 *p p* �*q* 0

**J** ¼

*Jxy* ¼ *Jyz* ¼ *Jxz* ¼ 0. Hence, **J** becomes:

*Unmanned Robotic Systems and Applications*

Defining **<sup>m</sup>***<sup>b</sup>* <sup>¼</sup> *τϕτθτψ*

**J** *p*\_ *q*\_ *r*\_

Hence:

and is given as follows:

*x*\_ *y*\_ *z*\_

*ϕ*\_ \_ *θ ψ*\_

*u*\_ *v*\_ *w*\_

*p*\_ *q*\_ *r*\_

3 5 ¼

2 4

**34**

2 4 3 5 ¼

3 5 ¼ 2 4

> 2 4

2 4

*rv* � *qw pw* � *ru qu* � *pv*

*Jy* � *Jz Jx*

*Jz* � *Jx Jy*

*Jx* � *Jy Jz*

3 5 þ 1 *m*

þ

*qr*

*pr*

*qp*

2 4

2 6 4

2 4 3 5 ¼ 2 4

> *p*\_ *q*\_ *r*\_

3 5 ¼

2 4

þ *ωb=<sup>i</sup>* � **h** � �

*Jx* �*Jxy* �*Jxz* �*Jxy Jy* �*Jyz* �*Jxz* �*Jyz Jz*

> *Jx* 0 0 0 *Jy* 0 0 0 *Jz*

3

� �*<sup>T</sup>* we can write Eq. (9) in the body coordinates as:

*Jx* 0 0 0 *Jy* 0 0 0 *Jz* 3 5

1 *Jx τϕ*

1 *Jy τθ*

1 *Jz τψ*

2 4

*p q r*

> 3 5

2 4

3 5

2 4

*u v w*

> *p q r*

3 5

3 5 9

>>>>>>>>>>>>>>>>>>>>>>>>>>>>>=

>>>>>>>>>>>>>>>>>>>>>>>>>>>>>;

3 5 þ

3

Again, the above equation is most easily resolved in body coordinates where

¼ **m** (9)

5 (10)

5 (11)

*τϕ τθ τψ* 3

5 (12)

(13)

(14)

2 4

The objective of this section is to describe the forces and torques that act on the quadrotor. Since there are no aerodynamic lifting surfaces, we will assume that the aerodynamic forces and moments are negligible. The forces and moments are primarily due to gravity and the four propellers.

As seen in **Figure 1**, each motor produces a force F and a torque *τ*. The total force acting on the quadrotor is given by:

$$F = F\_1 + F\_2 + F\_3 + F\_4 \tag{15}$$

The rolling torque is produced by the force difference between the motor pair 1–4 and 2–3 and is given as:

$$
\pi\_{\phi} = L(F\_1 + F\_4) - L(F\_2 + F\_3) \tag{16}
$$

Similarly, the pitching torque is produced by the force difference between the motor pair 1–3 and 2–4 and is given as:

$$
\pi\_{\theta} = L(F\_1 + F\_3) - L(F\_2 + F\_4) \tag{17}
$$

Due to Newton's third law, the drag of the propellers produces a yawing torque on the body of the quadrotor. The direction of the torque will be in the opposite direction of the motion of the propeller. Therefore the total yawing torque is given by:

$$
\pi\_{\Psi} = \pi\_1 + \pi\_2 - \pi\_3 - \pi\_4 \tag{18}
$$

The lift and drag produced by the propellers is proportional to the square of the angular velocity. We will assume that the angular velocity is directly proportional to the pulse width modulation command sent to the motor. Therefore, the force and torque of each motor can be expressed as:

$$\begin{aligned} F\_\* &= K\_1 \delta\_\* \\ \pi\_\* &= K\_2 \delta\_\* \end{aligned} \tag{19}$$

where *K*<sup>1</sup> and *K*<sup>2</sup> are constants that are determined experimentally, *δ* <sup>∗</sup> is the motor command signal, and \*—represents 1, 2, 3, and 4. Therefore, the forces and torques on the quadrotor can be written in matrix form as:

$$
\begin{bmatrix} F \\ \boldsymbol{\pi}\_{\phi} \\ \boldsymbol{\pi}\_{\theta} \\ \boldsymbol{\pi}\_{\theta} \end{bmatrix} = \begin{bmatrix} K\_1 & K\_1 & K\_1 & K\_1 \\ LK\_1 & -LK\_1 & -LK\_1 & LK\_1 \\ LK\_1 & -LK\_1 & LK\_1 & -LK\_1 \\ K\_2 & K\_2 & -K\_2 & -K\_2 \end{bmatrix} \begin{bmatrix} \delta\_1 \\ \delta\_2 \\ \delta\_3 \\ \delta\_4 \end{bmatrix} = M \begin{bmatrix} \delta\_1 \\ \delta\_2 \\ \delta\_3 \\ \delta\_4 \end{bmatrix} \tag{20}
$$

The control strategies derived in subsequent sections will specify forces and torques. The actual motors commands can be found as:

$$
\begin{bmatrix}
\delta\_1\\\delta\_2\\\delta\_3\\\delta\_4
\end{bmatrix} = \mathcal{M}^{-1} \begin{bmatrix}
F\\\tau\_{\phi}\\\tau\_{\theta}\\\tau\_{\psi}
\end{bmatrix} \tag{21}
$$

Note that the pulse width modulation commands are required to be between zero and one.

In addition to the force exerted by the motor, gravity also exerts a force on the quadrotor. In the vehicle frame *F<sup>v</sup>* , the gravity force acting on the center of mass is given by:

$$\mathbf{f}\_g^b = \begin{bmatrix} \mathbf{0} \\ \mathbf{0} \\ mg \end{bmatrix} \tag{22}$$

*x*€ ¼ ð Þ cosð Þ *ϕ* sin ð Þ*θ* cosð Þþ *ψ* sin ð Þ *ϕ* sin ð Þ *ψ*

*Vision-Based Autonomous Control Schemes for Quadrotor Unmanned Aerial Vehicle*

*y*€ ¼ ð Þ cosð Þ *ϕ* sin ð Þ*θ* sin ð Þ� *ψ* sin ð Þ *ϕ* cosð Þ *ψ*

*F <sup>m</sup>* � *<sup>g</sup>*

Equation set (28) would be used henceforth for developing control strategies.

In order to control a system like the quadrotor, very reliable sensors are needed that can provide a good estimate of the system states. Sensors like the IMU and GPS are subjected to noise which can make them quite undesirable for control applications. Hence, an efficient method of developing control strategies for autonomous

Using computer vision algorithms, the on-board camera of the quadrotor can be used to confer full autonomy on the system, thereby allowing it to operate in almost any environment. This also eradicates the necessity of setting up an additional set of cameras or to calibrate the environment lighting. As long as the on-board camera is previously calibrated (just needed once) and the target to be tracked is perfectly known (marker size and ID), this system is ready to operate. The usage of ArUco markers as targets allows an easy and fast computation enabling its use in real time

In this chapter, let us consider the application of autonomous landing of the quadrotor on a stationary platform like a car roof-top. To enable the quadrotor to identify the landing pad, an ArUco markers board must be attached to the roof of a car. The vision algorithm must be designed to detect a specific ArUco marker ID, and provide the quadrotor's pose relative to the marker. The algorithms used for detection and identification of the marker board are reviewed in the succeeding sub-section.

To detect the marker with a regular camera (RGB camera) a library called ArUco

is used that was developed by Aplicaciones de la Visión Artificial (AVA) from the Universidad de Córdoba (UCO) [12]. This library is "a minimal library for Augmented Reality applications based on open source computer vision (OpenCV)" [13] and has an API for developing markers in C++ which is very useful in this

A generic ArUco marker is a 2D bar-code that can be considered as a 7 � 7 Boolean matrix, with the outer cells filled with black (which makes a perfect square, easy to find with image processing). The remaining 5 � 5 matrix is a 10-bits coded ID (up to 1024 different IDs), where each line represents a couple of bits. Each line has only 2 bits of information out of the 5 bits, with the other 3 being used for error

work. A 100 mm Code 7 ArUco marker is shown in **Figure 2**.

*z*€ ¼ ð Þ cosð Þ *ϕ* cosð Þ*θ*

\_ *θψ*\_ <sup>þ</sup> *τϕ Jx*

\_ *θϕ*\_ <sup>þ</sup> *τψ Jz*

*<sup>ϕ</sup>*\_ *<sup>ψ</sup>*\_ <sup>þ</sup> *τθ Jy*

quadrotor operations is to utilize the concept of computer-vision.

applications like autonomous take-off and landing.

**4.1 The ArUco library**

detection.

**37**

*<sup>ϕ</sup>*€ <sup>¼</sup> *Jy* � *Jz Jx*

*DOI: http://dx.doi.org/10.5772/intechopen.86057*

€ <sup>¼</sup> *Jz* � *Jx Jy*

*<sup>ψ</sup>*€ <sup>¼</sup> *Jx* � *Jy Jz*

*θ*

**4. Vision algorithm development**

*F m* 9

>>>>>>>>>>>>>>>>>>=

(28)

>>>>>>>>>>>>>>>>>>;

*F m*

Hence, transforming **f** *b <sup>v</sup>* , we get:

$$\begin{aligned} \mathbf{f}\_v^b &= R\_b^v \begin{bmatrix} \mathbf{0} \\ \mathbf{0} \\ \mathbf{0} \\ mg \end{bmatrix} \\ &= \begin{bmatrix} -mg\sin\left(\theta\right) \\ mg\cos\left(\theta\right)\sin\left(\phi\right) \\ mg\cos\left(\theta\right)\cos\left(\phi\right) \end{bmatrix} \end{aligned} \tag{23}$$

Therefore, transforming equation set (14), we get:

$$
\begin{bmatrix}
\dot{\boldsymbol{x}} \\
\dot{\boldsymbol{y}} \\
\dot{\boldsymbol{z}}
\end{bmatrix} = \begin{bmatrix}
\boldsymbol{c}\_{\theta}\boldsymbol{c}\_{\psi} & \boldsymbol{s}\_{\phi}\boldsymbol{s}\_{\theta}\boldsymbol{c}\_{\psi} - \boldsymbol{c}\_{\phi}\boldsymbol{c}\_{\psi} & \boldsymbol{c}\_{\phi}\boldsymbol{s}\_{\theta}\boldsymbol{c}\_{\psi} + \boldsymbol{s}\_{\phi}\boldsymbol{s}\_{\psi} \\
\boldsymbol{c}\_{\theta}\boldsymbol{s}\_{\psi} & \boldsymbol{s}\_{\phi}\boldsymbol{s}\_{\theta}\boldsymbol{s}\_{\psi} + \boldsymbol{c}\_{\phi}\boldsymbol{c}\_{\psi} & \boldsymbol{c}\_{\phi}\boldsymbol{s}\_{\theta}\boldsymbol{s}\_{\psi} - \boldsymbol{s}\_{\phi}\boldsymbol{c}\_{\psi} \\
\end{bmatrix} \begin{bmatrix}
u \\ \boldsymbol{v} \\ \boldsymbol{w} \\ \boldsymbol{w}
\end{bmatrix} \tag{24}
$$

$$
\begin{bmatrix}
\dot{\phi} \\
\dot{\theta} \\
\dot{\psi}
\end{bmatrix} = \begin{bmatrix}
1 & \sin\left(\phi\right)\tan\left(\theta\right) & \cos\left(\phi\right)\tan\left(\theta\right) \\
0 & \cos\left(\phi\right) & -\sin\left(\phi\right) \\
0 & \sin\left(\phi\right)\sec\left(\theta\right) & \cos\left(\phi\right)\sec\left(\theta\right)
\end{bmatrix} \begin{bmatrix} p \\ q \\ r \end{bmatrix} \tag{25}
$$

$$
\begin{bmatrix}
\dot{u} \\
\dot{v} \\
\dot{w}
\end{bmatrix} = \begin{bmatrix}
rv - qw \\
pw - ru \\
qu - pv
\end{bmatrix} + \begin{bmatrix}
g\cos\left(\theta\right)\sin\left(\phi\right) \\
g\cos\left(\theta\right)\cos\left(\phi\right)
\end{bmatrix} + \frac{1}{m} \begin{bmatrix}
0 \\ 0 \\ -F
\end{bmatrix} \tag{26}
$$

$$
\begin{bmatrix}
\dot{p} \\
\dot{q} \\
\dot{r}
\end{bmatrix} = \begin{bmatrix}
\frac{J\_y - J\_x}{J\_x} qr \\
\frac{J\_x - J\_x}{J\_y} pr \\
\frac{J\_y - J\_y}{J\_x} qp \\
\hline
J\_x - J\_y \\
\end{bmatrix} + \begin{bmatrix}
\frac{1}{J\_x} \tau\_\phi \\
\frac{1}{J\_y} \tau\_\theta \\
\frac{1}{J\_x} \tau\_\psi \\
\end{bmatrix} \tag{27}
$$

Eqs. (24)–(27) represent the complete non-linear model of the quadrotor. However, they are not appropriate for control design for several reasons. The first reason is that they are too complicated to gain significant insight into the motion of the quadrotor. The second reason is that the position and orientation are relative to the inertial world fixed frame, whereas camera measurements will measure position and orientation of the target with respect to the camera frame. Hence, the above set of equations are further simplified using small angle approximation. We obtain:

*Vision-Based Autonomous Control Schemes for Quadrotor Unmanned Aerial Vehicle DOI: http://dx.doi.org/10.5772/intechopen.86057*

$$\begin{aligned} \ddot{x} &= \left(\cos\left(\phi\right)\sin\left(\theta\right)\cos\left(\psi\right) + \sin\left(\phi\right)\sin\left(\psi\right)\right) \frac{F}{m} \\ \ddot{y} &= \left(\cos\left(\phi\right)\sin\left(\theta\right)\sin\left(\psi\right) - \sin\left(\phi\right)\cos\left(\psi\right)\right) \frac{F}{m} \\ \ddot{z} &= \left(\cos\left(\phi\right)\cos\left(\theta\right)\right) \frac{F}{m} - g \\ \ddot{\theta} &= \frac{J\_y - J\_z}{J\_x}\dot{\theta}\dot{\psi} + \frac{\tau\_\theta}{J\_x} \\ \ddot{\theta} &= \frac{J\_z - J\_x}{J\_y}\dot{\phi}\dot{\psi} + \frac{\tau\_\theta}{J\_y} \\ \ddot{\psi} &= \frac{J\_x - J\_y}{J\_x}\dot{\theta}\dot{\phi} + \frac{\tau\_w}{J\_z} \end{aligned} \tag{28}$$

Equation set (28) would be used henceforth for developing control strategies.
