**2. Quadrotor model**

The equations are described in the rigid body model of the quadrotor, and its displacement is related to an inertial *frame*, fixed on the Earth surface [18]. Three main *frames* are considered for a quadrotor model: Inertial (**I**), Vehicle (**V**) and Rigid Body (**B**), as illustrated in **Figure 1**.

The quadrotor owns 4 propellers in cross configuration. Each pair of propellers (1,3) and (2,4) rotates in opposite directions (**Figure 1**). Setting different rotor speeds to each pair (*ω*<sup>2</sup> 6¼ *ω*4) or (*ω*<sup>1</sup> 6¼ *ω*3) produces *roll* or *pitch* rotations with corresponding lateral motion. *Yaw* rotation results from rolling moments difference (*M*<sup>1</sup> þ *M*<sup>3</sup> � *M*<sup>2</sup> � *M*4) between propellers [9]. Maximum and minimum motor speeds are some of the parameters to be set in the model.

Newton-Euler equations describe quadrotor dynamics and kinematics [9, 18]. The Rotation Matrix represents the rotation around the three axes in the sequence *Z* � *X* � *Y* (1), and is described in (2).

$$\mathbf{R}\_{\mathbf{B}\_{\mathbf{Z}-\mathbf{X}-\mathbf{Y}}}^{\mathbf{I}} = \mathbf{R}\_{\mathbf{A}2}^{\mathbf{I}}(\boldsymbol{\varphi})\mathbf{R}\_{\mathbf{A}1}^{\mathbf{A}2}(\boldsymbol{\phi})\mathbf{R}\_{\mathbf{B}}^{\mathbf{A}1}(\boldsymbol{\theta})\tag{1}$$

The orientation vector is defined by *η* and the position of quadrotor's center of mass is defined in the reference frame **I** by **r**. Angular velocities in the body frame **B**

> *x y z*

The derivative of the angles *ϕ*, *θ* and *ψ* and the angular velocities measured by a

*cθ* 0 �*cϕsθ* 0 1 *sϕ sθ* 0 *cϕcθ*

3 7 5

*ν* ¼

*p q r*

3 7

<sup>5</sup> (3)

<sup>5</sup> (4)

2 6 4

*θ* and *ψ*\_ in the same *frame* **B** is in (4).

3 7 5

2 6 4

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

*Ax* 0 0 0 *Ay* 0 0 0 *Az* 3 7 5

9 >=

B

(5)

>;

3 7

2 6 4

**r** ¼

*Quadrotor Unmanned Aerial Vehicles: Visual Interface for Simulation and Control…*

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

sensor fixed to the *Frame* of the Rigid Body are not the same. *p*, *q* and *r* are the angular velocities around the *x*, *y* and *z* axes of the Rigid Body *Frame*. The

> 2 6 4

Considering that the body is symmetrical with respect to the *x*-*z* and *y*-*z* planes of the *frame* **B**,and that the only forces acting on it are the weight and the four thrusts, its resulting linear acceleration with respect to the inertial *Frame* can be described using Newton's Second Law. Air drag forces are represented by a matrix

> 0 0 P*Fi*

2 6 4

2 6 4

8 ><

>:

(*p*, *q* and *r*) are defined by *ν* (3).

*Main frames in a quadrotor.*

**Figure 1.**

relationship with the angular rates *ϕ*\_ , \_

*m*f g€**r** I ¼

**105**

*p q r*

3 7 <sup>5</sup> <sup>¼</sup> **<sup>T</sup>**

0 0 �*mg* 3 7 5

9 >=

I

>;

2 6 4

8 ><

>:

2 6 4 *η* ¼

*ϕ θ ψ*

3 7 5

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

in (5), and the values of *Ax*, *Ay* and *Az*, are inputs on the parameters set.

<sup>þ</sup> **<sup>R</sup>**<sup>I</sup> **B**

2 6 4

2 6 4

$$\mathbf{R}\_{\mathbf{B}\_{\mathbf{Z}\rightarrow X-Y}}^{\mathbf{I}} \stackrel{\Delta}{=} \begin{bmatrix} c\psi c\theta - s\phi s\psi s\theta & -c\phi s\psi & c\psi s\theta + c\theta s\phi s\psi \\ c\theta s\psi + c\psi s\phi s\theta & c\phi c\psi & s\psi s\theta - c\theta s\phi c\psi \\ -c\phi s\theta & s\phi & c\phi c\theta \end{bmatrix} \tag{2}$$

The angle *ϕ* around the *x* axis is the *roll angle*, the angle *θ* around the *y* axis is *pitch angle*, and the *ψ* angle around the *z* axis is *yaw angle*.

*Quadrotor Unmanned Aerial Vehicles: Visual Interface for Simulation and Control… DOI: http://dx.doi.org/10.5772/intechopen.97435*

**Figure 1.** *Main frames in a quadrotor.*

linear techniques such as Lyapunov [9] and *Backstepping* [5, 9] and intelligent and adaptive control techniques as Fuzzy200 [10] and Reinforcement Learning [11]. Research in quadrotor demand sophisticated equipment and costly laboratory. However, it can be optimized employing low cost virtual development tools. In autonomous control the path planning, path tracking and joint operation with other UAVs, can be supported by optimization techniques with support of software tools [1, 2, 12]. Published works relate the use of software such as Visual Basic, MatLab, Panda3D, Gazebo, or more open applications developed in languages as Python and C+ + [2]. In [1], a UAV 3D flight environment programmed in Python and developed in Panda3D is presented. A GUI developed in LabVIEW was published in [2], and other developed on MatLab-Simulink employs quadratic linear regulator (LQR) control [13]. Gazebo is an important virtual environment for robotics. Its integration with ROS provides a powerful testbed to analyze control algorithms. In [12] works that employ Gazebo and ROS for developing simulation of UAVs are described.

Most of the cited simulation tools have a difficult start for users with little programming experience. Even open-source models can be tricky. An interesting tool depicted in [14, 15] gives support for quadrotor control development, analyzing and comparing various control strategies on challenging trajectories. It may be used by beginning users with not much knowledge in ROS, Gazebo or in programming languages such as Python or C ++. The tool easies the understanding of quadrotor dynamics and related equations, as well as the development of control strategies. The present Chapter describes this user-friendly framework, the employed techniques are described in [14–17].

Section 2 presents a description of the employed model. Section 3 explains the optimal trajectory planning method. Section 4 describes the controllers available in the tool. Section 5 presents the graphical user interface developed by the tool. Section 6 presents the graphical and numerical results. In the end of the Chapter,

The equations are described in the rigid body model of the quadrotor, and its displacement is related to an inertial *frame*, fixed on the Earth surface [18]. Three main *frames* are considered for a quadrotor model: Inertial (**I**), Vehicle (**V**) and

The quadrotor owns 4 propellers in cross configuration. Each pair of propellers (1,3) and (2,4) rotates in opposite directions (**Figure 1**). Setting different rotor speeds to each pair (*ω*<sup>2</sup> 6¼ *ω*4) or (*ω*<sup>1</sup> 6¼ *ω*3) produces *roll* or *pitch* rotations with corresponding lateral motion. *Yaw* rotation results from rolling moments difference (*M*<sup>1</sup> þ *M*<sup>3</sup> � *M*<sup>2</sup> � *M*4) between propellers [9]. Maximum and minimum motor

Newton-Euler equations describe quadrotor dynamics and kinematics [9, 18]. The Rotation Matrix represents the rotation around the three axes in the sequence

**A2**ð Þ *<sup>ψ</sup>* **<sup>R</sup>A2**

The angle *ϕ* around the *x* axis is the *roll angle*, the angle *θ* around the *y* axis is

**A1** ð Þ *<sup>ϕ</sup>* **<sup>R</sup>A1**

*cψcθ* � *sϕsψsθ* �*cϕsψ cψsθ* þ *cθsϕsψ cθsψ* þ *cψsϕsθ cϕcψ sψsθ* � *cθsϕcψ* �*cϕsθ sϕ cϕcθ*

**<sup>B</sup>** ð Þ*θ* (1)

3 7

<sup>5</sup> (2)

the Section 7 emphasizes the main aspects and critical issues related.

**2. Quadrotor model**

Rigid Body (**B**), as illustrated in **Figure 1**.

*Robotics Software Design and Engineering*

*Z* � *X* � *Y* (1), and is described in (2).

**RI**

**104**

**BZ**�**X**�**<sup>Y</sup>** ¼ Δ

speeds are some of the parameters to be set in the model.

**RI**

2 6 4

*pitch angle*, and the *ψ* angle around the *z* axis is *yaw angle*.

**BZ**�**X**�**<sup>Y</sup>** <sup>¼</sup> **<sup>R</sup><sup>I</sup>**

The orientation vector is defined by *η* and the position of quadrotor's center of mass is defined in the reference frame **I** by **r**. Angular velocities in the body frame **B** (*p*, *q* and *r*) are defined by *ν* (3).

$$\boldsymbol{\eta} = \begin{bmatrix} \phi \\ \theta \\ \boldsymbol{\nu} \end{bmatrix} \qquad \mathbf{r} = \begin{bmatrix} x \\ y \\ z \end{bmatrix} \qquad \boldsymbol{\nu} = \begin{bmatrix} p \\ q \\ r \end{bmatrix} \tag{3}$$

The derivative of the angles *ϕ*, *θ* and *ψ* and the angular velocities measured by a sensor fixed to the *Frame* of the Rigid Body are not the same. *p*, *q* and *r* are the angular velocities around the *x*, *y* and *z* axes of the Rigid Body *Frame*. The relationship with the angular rates *ϕ*\_ , \_ *θ* and *ψ*\_ in the same *frame* **B** is in (4).

$$
\begin{bmatrix} p \\ q \\ r \end{bmatrix} = \mathbf{T} \begin{bmatrix} \dot{\phi} \\ \dot{\theta} \\ \dot{\psi} \end{bmatrix} = \begin{bmatrix} c\theta & 0 & -c\phi s\theta \\ 0 & 1 & s\phi \\ s\theta & 0 & c\phi c\theta \end{bmatrix} \begin{bmatrix} \dot{\phi} \\ \dot{\theta} \\ \dot{\psi} \end{bmatrix} \tag{4}$$

Considering that the body is symmetrical with respect to the *x*-*z* and *y*-*z* planes of the *frame* **B**,and that the only forces acting on it are the weight and the four thrusts, its resulting linear acceleration with respect to the inertial *Frame* can be described using Newton's Second Law. Air drag forces are represented by a matrix in (5), and the values of *Ax*, *Ay* and *Az*, are inputs on the parameters set.

$$m\{\ddot{\mathbf{r}}\}^1 = \left\{ \begin{bmatrix} \mathbf{0} \\ \mathbf{0} \\ -m\mathbf{g} \end{bmatrix} \right\}^1 + \mathbf{R}\_\mathbf{B}^1 \left\{ \begin{bmatrix} \mathbf{0} \\ \mathbf{0} \\ \sum F\_i \end{bmatrix} - \begin{bmatrix} A\_x & \mathbf{0} & \mathbf{0} \\ \mathbf{0} & A\_y & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & A\_x \end{bmatrix} \right\}^\mathbf{B} \tag{5}$$

Besides the force each rotor produces a moment in the rigid body perpendicular to the plane of rotation of the propeller (*Mi*), contrary to the direction of rotation of the propellers. *L* is the size of the quadrotor arm (**Figure 1**) and **J** the inertia matrix presented in (6).

$$\mathbf{J} = \begin{bmatrix} J\_{xx} & \mathbf{0} & \mathbf{0} \\ \mathbf{0} & J\_{yy} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & J\_{xx} \end{bmatrix} \tag{6}$$

Desired angles for *roll* (*ϕdes*) and *pitch* (*θdes*) are calculated from *ψdes*, the

*Quadrotor Unmanned Aerial Vehicles: Visual Interface for Simulation and Control…*

This procedure yields a minimum *snap* trajectory for two points. For additional waypoints it is necessary to consider more equations and intermediary restrictions [15]. The tool calculates the complete optimized trajectory for any set of waypoints. Desired trajectory **r***des*, orientation *ψdes*, and their derivatives are calculated for being

Some of the challenges to be overcome in quadrotor operation are: attitude stability for large angles, trajectory following, collision avoidance through aggres-

ative structure (FF-PD). The tool is open to easily add more control strategies.

The control architecture employed in the following control strategies uses two cascaded loops. The inner loop (attitude control) runs in a fast time-scale and is assumed exponentially stable. The outer loop (position control) runs in a slow time-scale, with a higher bandwidth [4]. All of them employ a feed-forward with proportional plus deriv-

Small Angle control assumes an operation not too far from the hovering condition. A simple heuristic method with FF-PD control calculates the required acceler-

It is assumed small deviations from zero in *roll* and *pitch* angles, small deviations in the *yaw* angle from the desired value, and angular velocities close to zero. The algorithm assumes all upward-pointing thrust vectors (control signal *u*<sup>1</sup> in (10)).

After linear simplifications the equations for attitude control are defined in (11)

*<sup>x</sup>*€*<sup>c</sup>* sin *<sup>ψ</sup>des* ð Þ� €*yc* cos *<sup>ψ</sup>des* ð Þ � �

*<sup>x</sup>*€*<sup>c</sup>* cos *<sup>ψ</sup>des* ð Þþ €*yc* sin *<sup>ψ</sup>des* ð Þ � � *ψdes*

**u**<sup>2</sup> ¼ *KR*ð Þþ *η<sup>c</sup>* � *η Kν*ð Þ *ν<sup>c</sup>* � *ν* (13)

<sup>5</sup> <sup>¼</sup> **<sup>r</sup>***des* <sup>þ</sup> *Kp* **<sup>e</sup>***<sup>r</sup>* <sup>þ</sup> *Kd* **<sup>e</sup>***<sup>r</sup>* (9)

*u*<sup>1</sup> ¼ *m g*ð Þ þ €*zc* (10)

<sup>5</sup> <sup>¼</sup> **<sup>T</sup>***η<sup>c</sup>* (12)

(11)

equations depend on which control strategy is employed.

**4. Attitude and trajectory following control strategies**

*x*€*c* €*yc* €*zc*

3 7

2 6 4

sive maneuvers, monitoring, and others [2, 4].

used in the control algorithms.

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

**4.1 Small angle control**

and (12) [16].

**107**

ations for the desired trajectory (9).

*η<sup>c</sup>* ¼

*ϕc θc ψc*

1 *g*

1 *g*

*ν<sup>c</sup>* ¼

*pc qc rc*

A PD control law is used for attitude control and to calculate **u**<sup>2</sup> (13) [16].

3 7

2 6 4

2 6 4

Due to the forces produced by the rotors, moments are produced in the rigid body (*L* � *Fi*), causing the system to rotate around the *x* and *y* axes. The rotation around the *z* axis is due to the torque created by the rotation of the motors, which are fixed to the plant. Based on the Coriolis Equation, the equation that describes the angular acceleration in the *Frame* **B** is in (7).

$$\mathbf{J} \left\{ \begin{bmatrix} \dot{p} \\ \dot{q} \\ \dot{r} \end{bmatrix} \right\}^{\mathrm{B}} = \left\{ \begin{bmatrix} L(F\_2 - F\_4) \\ L(F\_1 - F\_3) \\ M\_1 + M\_2 + M\_3 + M\_4 \end{bmatrix} \right\}^{\mathrm{B}} - \left\{ \begin{bmatrix} p \\ q \\ r \end{bmatrix} \right\}^{\mathrm{B}} \times \mathbf{J} \left\{ \begin{bmatrix} p \\ q \\ r \end{bmatrix} \right\}^{\mathrm{B}} \tag{7}$$

For simulating the measurement noise, the tool allows to add a random signal to position and orientation variables and their derivatives. A first order delay between the comands *u*<sup>1</sup> and **u2** and rotor speed variations, with a time constant *τ* seconds, was included in the tool to simulate the rotor dynamics.
