aerospace

Article

Stochastic Trajectory Generation Using ParticleSwarm Optimization for Quadrotor UnmannedAerial Vehicles (UAVs)

Babak Salamat * and Andrea M. Tonello

EcoSys Lab, Alpen-Adria-Universität Klagenfurt, Universitätsstraße 65-67, 9020 Klagenfurt, Austria;andrea.tonello@aau.at* Correspondence: babaksa@edu.aau.at; Tel.: +43-(0)-463-2700-3661

Academic Editors: David Anderson, Javaan Chahl and Michael WingReceived: 22 March 2017; Accepted: 4 May 2017; Published: 8 May 2017

Abstract: The aim of this paper is to provide a realistic stochastic trajectory generation methodfor unmanned aerial vehicles that offers a tool for the emulation of trajectories in typical flightscenarios. Three scenarios are defined in this paper. The trajectories for these scenarios areimplemented with quintic B-splines that grant smoothness in the second-order derivatives of Eulerangles and accelerations. In order to tune the parameters of the quintic B-spline in the searchspace, a multi-objective optimization method called particle swarm optimization (PSO) is used.The proposed technique satisfies the constraints imposed by the configuration of the unmanned aerialvehicle (UAV). Further particular constraints can be introduced such as: obstacle avoidance, speedlimitation, and actuator torque limitations due to the practical feasibility of the trajectories. Finally,the standard rapidly-exploring random tree (RRT*) algorithm, the standard (A*) algorithm and thegenetic algorithm (GA) are simulated to make a comparison with the proposed algorithm in terms ofexecution time and effectiveness in finding the minimum length trajectory.

Keywords: stochastic trajectory; unmanned aerial vehicle (UAV); multi-objective optimization;obstacle avoidance; particle swarm optimization (PSO)

1. Introduction

In the last decade, unmanned aerial vehicles (UAVs), mostly known as an autonomous aerialvehicles, have been used in numerous military, aerial photography, agricultural and surveillanceapplications. UAVs can be classified into three significant groups: fixed-wing UAVs, rotary-wing UAVsand hybrid-layout UAVs [1].

The advantages of fixed-wing UAVs are the high-speed and the ability to fly for long distances.However, mechanical systems for landing and take-off, for example the landing gear, have to beinstalled. Furthermore, spacious structures, e.g., landing strips, have to be built. In small areaswith obstacles, vertical take-off and landing (VTOL) and the ability to hover in a motionless spotare crucial. Therefore, rotary-wing UAVs have more applications. In addition, VTOL UAVs havegreater maneuverability in indoor flight. Hybrid-layout UAVs have both long distance flight andVTOL properties. On the other hand, they have complex mechanisms for changing from rotary wingto fixed wing during the flight.

Considering harsh scenarios characterized by limited area [2], the presence of obstacles andpossible dynamic constraints, rotary-wing UAVs have been shown to give the best performance [3].This is why we consider these specific types in this paper. In particular, a quadrotor UAV is assumed.Its model structure is depicted in Figure 1. The aim is to derive a model to represent realistic trajectoriesthat such a UAV can follow. In particular, there exist several methods to generate trajectories, including

Aerospace 2017, 4, 27; doi:10.3390/aerospace4020027 www.mdpi.com/journal/aerospace

Aerospace 2017, 4, 27 2 of 19

splines [4], rational functions [5], polynomial [6] and Bézier curves [7,8]. Likewise, another techniquefor trajectory generation is to use analytical functions such as higher-order piecewise polynomialsto represent the evolution of the position over time [9]. Also, there are remarkable works in whicha collision-free path can be obtained using the standard rapidly-exploring random tree (RRT*) andsampling-based algorithms [10,11]. In order to provide a method that can represent the variability ofthe ensemble of flight trajectories followed by a UAV, it is of interest to define a stochastic approach.In this respect, the Confined Area Random Aerial Trajectory Emulator (CARATE) proposed in [12]stochastically generates a 3D path obtained from a variable length previous history of the trajectory anda tunable set of random variables. CARATE is specifically designed to emulate stochastic trajectorieswith a limited flight area. However, it does not consider the dynamics of the UAV and it is notoptimized to cope with harsh environmental constraints. In this paper, we tackle the limitationsabove and we propose a novel stochastic trajectory generation method that: (1) takes into accountthe dynamics of a quadrotor UAV, and (2) copes with environmental limitations. To do this, thedynamic model of a quadrotor [13,14] is considered. Moreover, an analytic function (quintic B-spline)is used to generate candidate trajectories. Nonetheless, the disadvantage of the deterministic functionsis that they are not able to satisfy all the constraints at the same time when planing the trajectoryin the search space. Therefore, the genetic algorithm (GA) may be used to obtain an optimizedtrajectory [15]. Another approach uses swarm intelligence. In particular, particle swarm optimization(PSO) is based on a simple mathematical model, extended by Kennedy and Eberhart in 1995 [16], todescribe the social behavior of birds and fish. PSO follows the basic principle of self-organizationwhich is used to characterize the dynamics of complex systems. PSO uses a model of social behaviorto unravel the optimization problems, in a cooperative and sagacious skeleton [17]. PSO is easy toapply and is powerful in solving a diverse range of multi-objective optimization problems where GAcan not be used. Some other related papers based on particle swarm optimization for path planninginclude [18,19]. Our approach, on the other hand, enjoys a specific method for solving the optimizationproblem with constraints. We change the problem into one that considers a cost metric that includes aviolation function. The constraints, namely position, velocity, acceleration, Euler angles, torques andobstacle avoidance are added in the violation function.

Φ ϴ

ψ

M gT

Figure 1. Reference frames of the unmanned aerial vehicle (UAV).

To validate the proposed method, three scenarios are defined:

1. Flight level (FL) trajectory;2. Take-off→Mission→ Landing (TML);3. Complex maneuvering in 3D space (CMS).

In each scenario, random trajectories are generated under certain constraints that can be imposedwith the given initial point (IP) and target point (TP) in three-dimensional coordinates.

Aerospace 2017, 4, 27 3 of 19

We organize the contents of this paper as follows. Section 2 describes the world frame, bodyframe, dynamic model and the kinematics of the quadrotor. The trajectory modeling approach andthe description of the three considered scenarios is given in Section 3. In Section 4, we define the costfunction and the violation function related to the constraints and obstacle avoidance. Finally, examplesof realized trajectories are reported in Section 5. The conclusions follow.

2. Dynamic Model

In order to represent the dynamic model of a quadrotor, two coordinate systems will be introduced.They describe the orientation and the motion of a quadrotor in the world frame and in the body frame.The world frame is referred to as the inertial earth coordinate o (north east down, NED) system. As canbe observed in Figure 1, the earth is assumed to be fixed. Therefore, the inertial earth frame can bedefined. The body coordinate o system is installed at the quadrotor’s center of gravity (CoG). Eachframe follows the right hand rule [20]. The notations used in this paper are reported in Table 1.

Table 1. List of notations.

Symbol Definition

o(xe, ye, ze) Earth axesNED North east downo(xb, yb, zb) Body axesφ Roll angleθ Pitch angleψ Yaw angleEuler angles [φ, θ, ψ][φ, θ, ψ]T Rate of change of Euler angles[p, q, r]T Body angular ratesMT = M + Mr Overall mass of the UAVIxx, Iyy, Izz Moment of inertiaR Radius of central sphereg Gravitational accelerationFi Force generated by ith rotorTi Torque generated by ith rotorΩi Angular velocity of ith rotorU1, U2, U3, U4 Control inputs

The Euler angles are used to represent the orientation of the quadrotor which is considered as apoint mass, with respect to a fixed reference frame (body frame). The euler angle rates [φ, θ, ψ]T can berepresented in matrix form as follows [21]:φ

θ

ψ

=

1 sin(φ) tan(θ) cos(φ) tan(θ)0 cos(φ) − sin(φ)0 sin(φ) sec(θ) cos(φ) sec(θ)

p

qr

. (1)

The direction cosine matrix that allows to obtain a transform from the body frame to the worldframe is denoted with Cbn. Hence, Cbn consists of three sequential single-axis rotations through thebasic Euler angles R(x, φ), R(y, θ) and R(z, ψ) that represent the roll, pitch and yaw, respectively.The direction cosine matrix Cbn can be obtained according to the relations below [22]:

R(x, φ) =

1 0 00 cos(φ) − sin(φ)0 sin(φ) cos(φ)

(2)

Aerospace 2017, 4, 27 4 of 19

R(y, θ) =

cos(θ) 0 sin(θ)0 1 0

− sin(θ) 0 cos(θ)

(3)

R(z, ψ) =

cos(ψ) − sin(ψ) 0sin(ψ) cos(ψ) 0

1 0 1

(4)

Cbn = R(x, φ)R(y, θ)R(z, ψ) . (5)

2.1. The Moment of Inertia

In this paper, the quadrotor is assumed to be a symmetric body. To define the moment of inertia,we assume the quadrotor to consist of four rotors and a main body. The main body is a sphere withradius R and a mass M. The four rotors are assumed to be a point mass Mr. Then, the point massesare connected to the center of the sphere with radius l. Therefore, the inertia matrix is a diagonalsymmetric matrix:

I =

Ixx 0 00 Iyy 00 0 Izz

(6)

with,

Ixx = Iyy =2MR2

5+ 2Mrl2 (7)

Izz =2MR2

5+ 4Mrl2 . (8)

2.2. Thrust and Torques

The translational position, velocity and acceleration of the point mass o represented in the inertialframe o, are:

ξ = [x, y, z]T (9)

ξ = [x, y, z]T (10)

ξ = [x, y, z]T . (11)

According to Figure 1 the speed of each propeller can affect the motion of the quadrotor, so therelation between the thrust and torque generated by each rotor can be expressed as follows [13]:

Fi = bΩ2i , f or i = 1, 2, 3, 4 (12)

Ti = dΩ2i , f or i = 1, 2, 3, 4 , (13)

while b and d are coefficients related to the Mach number, the Reynolds number and the angle of attack.Also, the input torques ςi can be defined by:

ςi = (−1)i+1dΩ2i , f or i = 1, 2, 3, 4 (14)

Aerospace 2017, 4, 27 5 of 19

2.3. Dynamic Model

The dynamic model of the quadrotor can be defined [13,14] according to the following relations:

x = U1(cos(φ) sin(θ) cos(ψ) + sin(φ) sin(ψ))

y = U1(sin(φ) sin(θ) cos(ψ)− cos(φ) sin(ψ))

z = U1(cos(θ) cos(ψ))− g

ψ = U2 + φθ I1

θ = U3 + φψI2

φ = U4 + ψθ I3

(15)

where, U1 = F1 + F2 + F3 + F4

U2 = F4 − F2

U3 = F3 − F1

U4 = T2 + T4 − T1 − T3

(16)

In Equation (15), I1, I2 and I3 are equal to:I1 = (Iyy − Izz)/Ixx

I2 = (Izz − Ixx)/Iyy

I3 = (Ixx − Iyy)/Izz

(17)

The inverse kinematics of our model that are referred to as control inputs of the quadrotoraccording to Equation (15) are:

U1 =√

x2 + y2 + (g + z)2

U2 = ψ− φθ I1

U3 = θ − φψI2

U4 = φ− ψθ I3

(18)

Indeed, the quadrotor has four motors actuating the four rotors while our system has six degreesof freedom. It can be deduced that our system is underactuated. In order to resolve the issue, we haveto identify the dependency in Equation (15), as proposed by [23]:

x cos(φ)− (z + g) tan(θ) = 0

x sin(φ)− y cos(φ)− (√

x2 + y2 + (g + z)2 ) sin(ψ) = 0 .(19)

Equation (19) shows two nonholonomic constraints existing in the kinematics of the quadrotor.Therefore, the angles θ and ψ can be derived from these nonlinear equations:θ = atan2(x cos(φ) + y sin(φ), z + g)

ψ = arcsin( x sin(φ)−y cos(φ)√x2+y2+(g+z)2

)(20)

For calculating the torques, Equation (17) can be rearranged in matrix form so that:

U = Ξ Ω (21)

Aerospace 2017, 4, 27 6 of 19

where Ξ is:

Ξ =

b b b b0 −bl 0 bl−bl 0 bl 0−bl bl −bl bl

. (22)

The input torques Ui can be obtained by the actuators of the quadrotor. ExploitingEquation (20), we can define the fitness function and boundary conditions in our model byevaluating Γ(t) = (x(t), y(t), z(t), φ(t)), for t ∈ [0 T] and its time derivatives. We refer to Γ(t) asthe trajectory evolution.

3. Modeling of Three Representative Scenarios

Our goal is to develop a stochastic model for the trajectory evolution. Indeed, at a first glancethis can appear a difficult task since the UAV can follow a number of different trajectories and be invarious flying situations. Therefore, we propose to consider certain scenarios that can be representativeof certain flying operations. In each scenario, the trajectory evolution Γ(t) is represented with aB-spline drawn from a certain random model. In more detail (Figure 2), the B-spline has order kand it is defined by n + 1 control points [4]. In our model, the first and the last control point arefixed and they correspond to the start and the end of the mission, respectively. In the search spacebetween these two points, additional control points are placed in order to generate the segments ofthe B-spline curve. Each one of the control points is defined with coordinates in the search space,which are x(t), y(t), z(t), φ(t) and each of them is generated stochastically as a uniform randomvariable ranging between a lower and an upper limit that are distributed over time by the user. Thisconfiguration enables a stochastic behaviour for Γ(t). Indeed, by changing the location of the controlpoints in the search space, a new path can be realized.

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1Time [s]

0

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

1

Po

siti

on

[m

]

B-spline with 8 control points of order 5

Upper Bound

Lower Bound

Start Point

End Point

Search Space

Figure 2. Evolution of the B-spline.

The mathematical model for a B-spline curve of degree k is defined by:

S(t) =n

∑i=0

Ni,k(t)Pi (23)

Aerospace 2017, 4, 27 7 of 19

where (P0, P1, ...Pn) are control points and Ni,k(t) are the basis functions defined using the Cox-de Boorrecursion formula [4]:

Ni,0(t) =

1, if ti ≤ t < ti+1

0, otherwise

(24)

Ni,j(t) =t− ti

ti+1 − tiNi,j−1(t) +

ti+j+1 − tti+j+1 − ti+1

Ni+1,j−1(t) .

The reason for using the B-spline is that unlike the global property of the Bezier curve, it hasthe local property [24]. Furthermore, the order of the B-spline is very important to generate feasibletrajectories that fulfill constraints and obtain trajectory continuity and smoothness. To do so, we usefourth-order B-splines (quadratic) and fifth-order B-splines (quintic) in the considered scenarios.

3.1. Scenario One (FL)

According to the aeronautical literature [25], the flight level (FL) is expressed as a vertical altitudewith zero roll, pitch and yaw at sea level pressure. For this scenario a fourth-order B-spline with fourcontrol points is considered. More details on the FL can be found in Section 4.

3.2. Scenario Two (TML)

In all kind of UAVs, take-off, mission and landing (TML) are crucial. The challenge in modelingthis scenario is the tuning of the parameters of the B-spline in a way that: (1) the velocity profiles ofthe trajectories during take-off and landing are zero (rest-to-rest manoeuvre); (2) the absolute valueof the velocity should be less than 1.5 m/s; (3) the Euler angles during take-off and landing are zero(rest-to-rest manoeuvre); and (4) the quadrotor reaches the specific altitude (10 m) during the flighttime. Clearly, other constraints can be defined. For simplicity, the surface of the ground is assumed tobe horizontal. The length and duration of the trajectory between take off and landing can be madevariable. Further details about modeling this scenario are reported in Section 4 and numerical examplesin Section 5.

3.3. Scenario Three (CMS)

The third considered scenario highlights the ability of our approach to model trajectories thatcomprise obstacle avoidance procedures. Static obstacles with different size can be defined in theflight space. In particular, we consider three obstacles. Finding a safe and short trajectory in the searchspace with obstacles becomes a challenging problem in path planning. On the other hand, accordingto the configuration of our quadrotor, the feasibility of these trajectories is also significant. Boundaryconstraints for complex maneuver in 3D space are defined in Equation (25). More details are reportedin Section 5.

4. Trajectory Realization under Practical Constraints

As discussed previously, the trajectory evolution is Γ(t) = (x(t), y(t), z(t), φ(t)) with inputcontrols Λ(t) = (ς1(t), ς2(t), ς3(t), ς4(t)). Also, the dynamics of a quadrotor and environmentalconstraints add mathematical constraints to the generation of feasible quadrotor trajectories. In thefollowing discussion, the constraints will be defined. Therefore, our problem is to determine the

Aerospace 2017, 4, 27 8 of 19

optimal coefficients of the B-spline under particular constraints. The boundary constraints in ourscenarios are:

ξ(0) = Γ(start) and ξ(T) = ξ(end)

ξ(0) = 0 and ξ(T) = 0

ξ(0) = 0 and ξ(T) = 0

Λmini ≤ Λt

i ≤ Λmaxi

dmin = min(d)

(25)

where dmin is the minimum Euclidian distance between the UAV and a given a generic obstacle.It means that for dmin ≥ 0, the UAV will not crash during the flight. We solve this problem with thePSO algorithm in Section 4.2.

4.1. Design of Trajectory with Minimum Length

The aim of this paper is generating stochastic trajectories. However, in path planning whenthe search space is populated with generic obstacles, finding the short and safe path among thesetrajectories becomes significant. We can therefore consider the problem of designing the shortestpath trajectory. This translates into the definition of a cost function that equals the length of the pathas follows:

Ji =√(xi+1 − xi)2 + (yi+1 − yi)2 + (zi+1 − zi)2 (26)

where we have discretized the time evolution so that the coordinates at time instant i are denotedwith xi = x(iT), yi = y(iT), zi = z(iT) and the sampling period is T. Our problem is a multi-objectiveoptimization problem that has a cost function with boundary constraints. To proceed, we write itas a problem without constraints, however it includes a violation function P(Υ) to obtain the newcost function.

J =1N(

N

∑i=1

Ji +N

∑i=1

M

∑Υ=1

Pi(Υ)) (27)

where N denotes the total number of samples considered in the overall trajectory. i represents thetime index. J is the cost function that is equal to the length of the path. Pi(Υ) is the violation at time ifor constraint Υ. The constraints Υ are the position, velocity, acceleration, Euler angles, torques andobstacle avoidance. Two important properties for the violation function are P(0) = 0 and P(Υ) ≥ 0.In order to satisfy the boundary constraints in Equation (25), the violation function for less, more andequal constraints can be defined respectively as follows:

Pi(Υ) =

max( f (Υ)

f0(Υ)− 1, 0) for f (Υ) ≤ f0(Υ)

max(1− f (Υ)f0(Υ)

, 0) for f (Υ) ≥ f0(Υ)

| ( f (Υ)f0(Υ)

− 1) | for f (Υ) = f0(Υ)

(28)

where Υ refers to a certain constraint as already explained. In addition, f (Υ) and f0(Υ) represent thephysical quantity and target constraint respectively. Now with the violation functions definition inEquation (28), the boundary conditions in Equation (25) can be satisfied. For obstacle avoidance, theEuclidian distance between the edge of the object and the quadrotor is used [26]. The whole algorithmis sketched in Figure 3.

Aerospace 2017, 4, 27 9 of 19

Start

Create Model (Configuration)

Parse Solution Discretize the

trajectory in timestep

Connect to PSO

If violation is zero

The dynamic of the trajectory is

feasible

End

The dynamic of the trajectory is

not feasibleYesNo

Sum with cost function as a penalty value

Figure 3. Flowchart of trajectory generation algorithm. PSO: particle swarm optimization.

4.2. Basics of PSO

Particle swarm optimization (PSO) is an intelligent optimization algorithm. It belongs to theclass of algorithms called metaheuristics. PSO is based on the paradigm of swarm intelligence andit is inspired by social behavior of animals like fish and birds [16]. PSO is a simple yet powerfuloptimization algorithm and it is successfully applied to multiple applications in various fields ofscience and engineering like machine learning, mesh processing, data mining and many other fields.

PSO contains a population of candidate solutions called a swarm. According to Figure 4, everyparticle p(n) is a candidate solution to the optimization problem. Any particle has a position in thesearch space of the optimization problem. The search space is the set of all possible solutions to theoptimization problem. PSO allows for efficiently finding an optimal solution under a target functionwith multiple constraints in the problem at hand.

g (n)-x (n)

X (n)

i i

i

g(n)

V (n)i

P (n)-x (n)

P (n)i

ii

v (n+1)i

x (n+1)i

Figure 4. Mathematical model of PSO.

Aerospace 2017, 4, 27 10 of 19

To proceed, we consider particles for the position vector x and the velocity vector v. Therefore,the updating realization for the particles includes [16]:

xi+1(n) = xi(n) + vi(n + 1) (29)

vi+1(n) = ωvi(n) + r1c1(pi(n)− xi(n)) + r2c2(gi − xi(n)) (30)

where i denotes the time index. n = 1, ..., L is the particle index, and L is the number of consideredparticles. ω, c1 and c2 are real valued coefficients. c1 is the personal learning coefficient and c2 isthe global learning coefficient. w is an inertial coefficient. c1 and c2 are acceleration coefficients.On the other hand, r1, r2 are random numbers uniformly distributed in the range from zero to one(r1, r2 ∼ U(0, 1)). pi(n) is the best personal position of the particle of index n at time i, while gi is thebest collective position of all particles at time i in the swarm. It should be noted that in the originalversion of PSO proposed in 1995, there was no inertia term. However, in 1998 Shi and Eberhart addedthe inertia term to the standard PSO [27].

5. Numerical Generator and Results

In this section, first we report a summary of example parameters to be used to generate trajectoriesin the three considered scenarios. The proposed algorithm can be used for any configuration of UAVs,but herein, we focus on a quadrotor. A number of parameters is initially fixed as shown in Table 2 [13].Furthermore, in each flight scenario the start point, end point and a number of control points aredefined by the user. We also report the number of particles and the number of iterations run by the PSOfor the generation of trajectories with a certain number of control points. The details for each scenarioare given in Table 3. The number of iterations listed in Table 3 to obtain the minimum length path areneeded for the convergence. The average cost metric as a function of the iterations will be shown inSections 5.1–5.3 for Scenarios 1, 2 and 3, respectively. The execution time has been computed for asimulation setup with an Intel i5 CPU with 3.3 GHz, and RAM of 4GB using MATLAB 2016b. Finally,we demonstrate the performance of our method in comparison with the standard (A*) algorithm, thestandard rapidly-exploring random tree (RRT*) algorithm and the genetic algorithm (GA) for obtainingthe minimum length path in 2D.

Table 2. Physical parameters of the quadrotor.

Symbol Unit

MT 0.65 Kgl 0.232 mg 9.806 m/s2

R 0.15 mIxx 0.07582 Kg m2

Iyy 0.07582 Kg m2

Izz 0.1457924 Kg m2

b 3.13× 10−5 Ns2

d 7.5× 10−7 Nms2

Λmax = −Λmin 0.04 Nmφmax = −φmin 0.1 Radθmax = −θmin 0.1 Radψmax = −ψmin 0.1 Rad

Inertia Weight (ω) 1c1 2c2 2

Aerospace 2017, 4, 27 11 of 19

Table 3. Solution details.

Scenarios Control Points Swarm Size Iterations Execution Time

Scenario 1 4 50 250 5.090549 (s)Scenario 2 5 100 400 6.572763 (s)Scenario 3 5 100 450 7.464875 (s)

5.1. Scenario 1 (FL)

Figure 5 shows a number of realizations of possible trajectories generated by the algorithm.Among these trajectories one is chosen based on the constraints set in Equation (25) in order to isolatethe one with least distance. The initial and final position can be chosen by the user. In this specificexample the initial position of our quadrotor is Γinitial = (0, 0, 25, 0) in meters. The final position isΓ f inal = (20, 0, 25, 0). The fourth-order B-spline is considered for this scenario. Figure 6 shows theminimum length trajectory that is obtained from the PSO. Furthermore, the details about positions,actuators torques, control efforts, translational velocities and the acceleration obtained with the PSOare given in Figures 7–10. Figure 11 shows the average cost metric (Equation (27)) as a function ofiterations. The convergence is achieved in about 250 iterations. PSO algorithm converges quickly tothe optimal solution. The minimum length of the trajectory for this scenario obtained via simulation isJ = 20 m and the total flight time is 20 s.

X [m]

Y [m]

25

200

152101

0 5-1 0

-2

Stochastic realization for (FL)

20

Z [

m]

40

End mission

Start mission

Figure 5. Stochastic realization for flight level (FL).

Y [m]

X [m]

30

2001

10

100.5

20Z [

m]

0

30

-0.5

40

0-1

50

Flight levelStart MissionEnd MissionControl point

Characteristics

Flight level

Figure 6. Three-dimensional flight level (FL) trajectory.

Aerospace 2017, 4, 27 12 of 19

0 10 20Time [s]

0

2

4

6

8

10

12

14

16

18

20

X [

m]

North

0 10 20Time [s]

-1

-0.8

-0.6

-0.4

-0.2

0

0.2

0.4

0.6

0.8

1

Y [

m]

East

0 10 20Time [s]

0

5

10

15

20

25

30

35

40

45

50

Z [

m]

Down

NEDStart MissionEnd Mission

Characteristics

Figure 7. North east down (NED) for FL.

0 10 20Time [s]

-0.04

-0.035

-0.03

-0.025

-0.02

-0.015

-0.01

-0.005

0

To

rqu

e 1

[N.m

]

0 10 20Time [s]

0

0.005

0.01

0.015

0.02

0.025

0.03

0.035

0.04

To

rqu

e 2

[N.m

]

0 10 20Time [s]

-0.04

-0.035

-0.03

-0.025

-0.02

-0.015

-0.01

-0.005

0

To

rqu

e 3

[N.m

]

0 10 20Time [s]

0

0.005

0.01

0.015

0.02

0.025

0.03

0.035

0.04

To

rqu

e 4

[N.m

]

Figure 8. Evolution of actuators torques for FL.

0 10 20Time [s]

9.81

9.8105

9.811

9.8115

9.812

9.8125

9.813

9.8135

9.814

9.8145

9.815

U1

[N]

0 10 20Time [s]

-1

-0.5

0

0.5

1

1.5

U2

[N]

10-14

0 10 20Time [s]

-6

-4

-2

0

2

4

6

U3

[N]

10-7

0 10 20Time [s]

-1

-0.8

-0.6

-0.4

-0.2

0

0.2

0.4

0.6

0.8

1

U4

[N]

Figure 9. Control efforts for FL.

Aerospace 2017, 4, 27 13 of 19

0 5 10 15 20Time [s]

0

0.5

1

1.5

2

Vx

[m/s

]0 5 10 15 20

Time [s]

-0.5

0

0.5

Vy

[m/s

]

Velocity and Acceleration profilesStart MissionEnd Mission

0 5 10 15 20Time [s]

-0.5

0

0.5

Vz

[m/s

]

0 5 10 15 20Time [s]

-0.5

0

0.5

ax [

m/s

2 ]Figure 10. Translation of velocities and acceleration in FL.

0 1000 2000 3000 4000 5000 6000Iteration

0

100

200

300

400

500

600

700

Ave

rag

e co

st

Figure 11. Average cost function vs. iteration for FL.

5.2. Scenario 2 (TML)

In Figures 12–16, we report an example of trajectory belonging to the Scenario 2 (TML).In particular, the starting point is Γinitial = (0, 0, 0, 0) and the landing point is Γlanding = (8, 6, 0, 0).The maximum altitude during the flight time is 10 m. Also, Figure 17 shows the minimum lengthtrajectory (J = 33.9608 m) which is obtained after 100 iterations of the PSO algorithm. Figure 15represents the inertial frame for the velocity components, which are less than 1.5 m/s according to ourconstraints definition in scenario two and the total flight time is 23 s.

08

2

4

6

Z [

m] 6

8

4 10

10

862

420 0

TMLStart MissionEnd Mission

Characteristics

X [m]

Landing Region

Y [m]

Figure 12. Three-dimensional take-off, mission and landing (TML) trajectory.

Aerospace 2017, 4, 27 14 of 19

0 5 10 15 20 25

Time [s]

-0.01

-0.005

0

0.005

0.01

Rad

PhiThetaPsi

Figure 13. Roll, pitch and yaw angle for TML.

0 10 20 30Time [s]

0

1

2

3

4

5

6

7

8

9

X [

m]

North

0 10 20 30Time [s]

0

1

2

3

4

5

6

Y [

m]

East

0 10 20 30Time [s]

0

1

2

3

4

5

6

7

8

9

10

Z [

m]

Down

NEDStart MissionEnd Mission

Figure 14. North east down NED for TML.

0 5 10 15 20 25

Time [s]

-0.2

0

0.2

0.4

0.6

0.8

Vx

[m/s

]

0 5 10 15 20 25

Time [s]

0

0.1

0.2

0.3

0.4

0.5

Vy

[m/s

]

0 5 10 15 20 25

Time [s]

-2

-1

0

1

2

Vz

[m/s

]

0 5 10 15 20 25

Time [s]

-0.1

-0.05

0

0.05

0.1

ax [

m/s

2]

Velocity and acceleration profilesStart MissionEnd Mission

Figure 15. Translation of velocities and acceleration for TML.

Aerospace 2017, 4, 27 15 of 19

0 10 20 30

Time [s]

-6.4

-6.3

-6.2

-6.1

-6

-5.9

-5.810-3Torque 1 [N.m]

0 10 20 30

Time [s]

5.8

5.9

6

6.1

6.2

6.3

6.410-3 Torque 2 [N.m]

0 10 20 30

Time [s]

-6.4

-6.3

-6.2

-6.1

-6

-5.9

-5.810-3 Torque 3 [N.m]

0 10 20 30

Time [s]

5.8

5.9

6

6.1

6.2

6.3

6.410-3Torque 4 [N.m]

Figure 16. Evolution of actuators torques for TML.

0 1000 2000 3000 4000 5000 6000Iteration

0

100

200

300

400

Ave

rag

e co

st

Figure 17. Average cost function vs. iteration for TML.

5.3. Scenario 3 (CMS)

In Figure 18, the search space is populated with three generic spherical objects that act as obstaclesfor the quadrotor and thus hinder their path. These obstacles can be either positioned arbitrarilyby the user or randomly generated by the algorithm. In this specific case they were generated bythe user. For CMS, a stochastic realization as a set of trajectories is shown in Figure 18. The startpoint is Γinitial = (0, 0, 0, 0) and the end point is Γ f inal = (8, 6, 0, 0). Simulation results for the costfunction and the actuator torques are shown in Figures 19 and 20. Also, the minimum path after150 iterations and the total flight time are J = 23.495, and 23 s, respectively. Furthermore, the minimumlength path obtained with PSO and time histories of the attitude angle of the quadrotor are shown inFigures 21 and 22, respectively.

Aerospace 2017, 4, 27 16 of 19

-28

0

2

4

6

6

8

4 9

10

872 6540 321-2 0

Z [m]

Y [m] X [m]

Stochastic CMS

Start Mission

End Mission

Obstacle 2

Obstacle 3

Obstacle 1

Figure 18. Stochastic complex maneuvering in 3D space (CMS).

0 2000 4000 6000 8000 10000 12000 14000 16000Iteration

0

200

400

600

800

1000

1200

Ave

rag

e co

st

Figure 19. Average cost function vs. iteration for CMS.

0 10 20 30Time [s]

-6.4

-6.3

-6.2

-6.1

-6

-5.9

-5.8

To

rqu

e 1

[N.m

]

10-3

0 10 20 30Time [s]

5.8

5.9

6

6.1

6.2

6.3

6.4

To

rqu

e 2

[N.m

]

10-3

0 10 20 30Time [s]

-6.4

-6.3

-6.2

-6.1

-6

-5.9

-5.8

To

rqu

e 3

[N.m

]

10-3

0 10 20 30Time [s]

5.8

5.9

6

6.1

6.2

6.3

6.4

To

rqu

e 4

[N.m

]

10-3

Figure 20. Evolution of actuator torques for CMS.

Aerospace 2017, 4, 27 17 of 19

8067

2

65

4

44

Z [

m]

3

6

2 2

8

10

10

0-1

CMSStart missionEnd missionObstacle 1Obstacle 2Obstacle 3

Characteristics

Y [m]X [m]

Figure 21. Complex maneuvering in 3D space (CMS).

0 5 10 15 20 25Time [s]

-0.01

-0.005

0

0.005

0.01

0.015

Rad

PhiThetaPsi

Figure 22. Roll, pitch and yaw angle for CMS.

5.4. PSO vs. A*, RRT* and GA

As already mentioned before, to illustrate the effectiveness of the proposed method, we considerthree different methods named the RRT* algorithm, the A* algorithm and the GA to obtain theminimum path in 2D that obeys several constraints. The search space is also surrounded by fourobstacles. As shown in Table 4 and Figure 23, the results for the proposed method show that it has afaster convergence time and it finds a path with smaller length and collision avoidance. The optimumlength for our method is 14.2095 m. Also, it is noticeable that the execution-time for the A* algorithmis 0.02315 s. The execution time does not consider the part for the construction of the B-spline curves.

0 2 4 6 8 10X [m]

0

2

4

6

8

10

Y [

m]

Obstacle 1Obstacle 2Obstacle 3Obstacle 4Our methodControl-PointStart missionEnd missionGA methodA star methodRRT star

Figure 23. Optimal path by rapidly-exploring random tree (RRT*), standard (A*) algorithm, geneticalgorithm (GA) and the PSO.

Aerospace 2017, 4, 27 18 of 19

Table 4. Comparision Results.

Parameters RRT* A* GA PSO + B-Spline

Length (m) 18.1265 16.5563 17.2111 14.2095Execution time (s) 17.343599 0.02315 9.230695 1.544244

6. Conclusions

In this paper, a new methodology has been proposed for stochastic modeling and generatingtrajectories followed by a UAV. The approach is based, firstly, on modeling a certain flight scenario.Then, the evolution of the trajectory is obtained using a B-spline with a certain order and setting somecontrol points. A PSO multi-objective optimization algorithm is used to not only tune the parameters ofthe B-spline function, but also to guarantee that the trajectories are dynamically feasible. The trajectoriescan also model an obstacle avoidance situation. This has been realized by defining a violationfunction to avoid collision during the flight time with obstacles at certain positions. A numericalsimulator has been implemented to emulate the reference scenarios. The simulator can be configuredto allow variability and representativity of the flight scenario. Several trajectory examples have beenshown to verify the trajectory smoothness and fulfillment of practical constraints introduced by theUAV dynamics.

Author Contributions: Babak Salamat developed the UAV trajectory optimization procedure under feasibilityconstraints, obtained the numerical results and prepared the paper. Andrea M. Tonello conceived the overall UAVrandom trajectory generation concept, supervised the development of the model, results and paper.

Conflicts of Interest: The authors declare no conflict of interest.

References

1. Austin, R. Unmanned Aircraft Systems: UAVS Design, Development and Deployment, 1st ed.; Wiley: Chichester,UK, 2010.

2. Majka, A. Trajectory Management of the Unmanned Aircraft System (UAS) in Emergency Situation. Aerospace2015, 2, 222–234.

3. Suicmez, E.C.; Kutay, A.T. Optimal path tracking control of a quadrotor UAV. In Proceedings of the 2014International Conference on Unmanned Aircraft Systems (ICUAS), Orlando, FL, USA, 27–30 May 2014;pp. 115–125.

4. Sprunk, C. Planning Motion Trajectories for Mobile Robots Using Splines; Student Project; University of Freiburg:Freiburg, Germany, 2008.

5. Montés, N.; Mora, M.C.; Tornero, J. Trajectory Generation based on Rational Bezier Curves as Clothoids.In Proceedings of the IEEE Intelligent Vehicles Symposium, Istanbul, Turkey, 13–15 June 2007; pp. 505–510.

6. Babel, L. Three-dimensional Route Planning for Unmanned Aerial Vehicles in a Risk Environment. J. Intell.Robot. Syst. 2013, 71, 255–269.

7. Jayasinghe, J.A.S.; Athauda, A.M.B.G.D.A. Smooth trajectory generation algorithm for an unmannedaerial vehicle (UAV) under dynamic constraints: Using a quadratic Bezier curve for collision avoidance.In Proceedings of the 2016 Manufacturing Industrial Engineering Symposium (MIES), Colombo, Sri Lanka,22 October 2016; pp. 1–6.

8. Tsai, Y.J.; Lee, C.S.; Lin, C.L.; Huang, C.H. Development of Flight Path Planning for Multirotor AerialVehicles. Aerospace 2015, 2, 171–188.

9. Cruz, A.B.; Montaño, J.C.; Mier, P.G. TG2M: Trajectory Generator and Guidance Module for the AerialVehicle Control Language AVCL. In Proceedings of the 40th International Symposium on Robotics, Barcelona,Spain, 10–14 March 2009.

10. Karaman, S.; Frazzoli, E. Sampling-based algorithms for optimal motion planning. Int. J. Robot. Res. (IJRR)2011, 30, 846–894.

11. Barbehenn, M. A note on the complexity of Dijkstra’s algorithm for graphs with weighted vertices.IEEE Trans. Comput. 1998, 47, 263.

Aerospace 2017, 4, 27 19 of 19

12. Papaiz, A.; Tonello, A. Azimuth and Elevation Dynamic Tracking of UAVs via 3-Axial ULA and ParticleFiltering. Int. J. Aerosp. Eng. 2016, 2016, 1–9.

13. Sanca, A.S.; Alsina, P.J.; Cerqueira, J.J.F. Dynamic Modelling of a Quadrotor Aerial Vehicle with NonlinearInputs. In Proceedings of the 2008 IEEE Latin American Robotic Symposium, Salvador, Bahia, Brazil,29–30 October 2008 ; pp. 143–148.

14. Mohammadi, M.; Shahri, A.M. Modelling and decentralized adaptive tracking control of a quadrotor UAV.In Proceedings of 2013 First RSI/ISM International Conference on Robotics and Mechatronics (ICRoM),Tehran, Iran, 13–15 Febuary 2013.

15. Galvez, R.L.; Dadios, E.P.; Bandala, A.A. Path planning for quadrotor UAV using genetic algorithm.In Proceedings of the 2014 International Conference on Humanoid, Nanotechnology, Information Technology,Communication and Control, Environment and Management (HNICEM), Puerto Prinsesa, Palawan,Philippines, 12–16 November 2014; pp. 1–6.

16. Eberhart, R.; Kennedy, J. A new optimizer using particle swarm theory. In Proceedings of theSixth International Symposium on Micro Machine and Human Science, 1995 (MHS ’95), Nagoya, Japan,4–6 October 1995.

17. Kennedy, J. The particle swarm: Social adaptation of knowledge. In Proceedings of the IEEE InternationalConference on Evolutionary Computation, Indianapolis, IN, USA, 13–16 April 1997.

18. Kang, H.I.; Lee, B.; Kim, K. Path Planning Algorithm Using the Particle Swarm Optimization and theImproved Dijkstra Algorithm. In Proceedings of the 2008 IEEE Pacific-Asia Workshop on ComputationalIntelligence and Industrial Application, Wuhan, China, 19–20 December 2008; Volume 2, pp. 1002–1004.

19. Wang, Q.; Zhang, A.; Qi, L. Three-dimensional path planning for UAV based on improved PSO algorithm.In Proceedings of the 26th Chinese Control and Decision Conference (2014 CCDC), Changsha, China,31 May–2 June 2014; pp. 3981–3985.

20. Cook, M.V. Flight Dynamics Principles; Elsevier: Amsterdam, The Netherlands, 2007.21. Tang, Y.R.; Li, Y. Dynamic modeling for high-performance controller design of a UAV quadrotor.

In Proceedings of the 2015 IEEE International Conference on Information and Automation, Lijiang, China,8–10 August 2015; pp. 3112–3117.

22. Nemra, A.; Aouf, N. Robust INS/GPS Sensor Fusion for UAV Localization Using SDRE Nonlinear Filtering.IEEE Sens. J. 2010, 10, 789–798.

23. Jolly, A.C. Trajectory Generation by Piecewise Spline Interpolation; Techinical Report; U.S. Army MissileCommand: Redstone Arsenal, AL, USA, 1976.

24. Prautzsch, H.; Boehm, W.; Paluszny, M. Bézier and B-Spline Techniques; Springer: Berlin/Heidelberg, Germany;New York, NY, USA, 2002.

25. Eshelby, M.E. Aircraft Performance; Elsevier: Oxford, UK, 2000.26. Jamieson, J.; Biggs, J. Path Planning Using Concatenated Analytically-Defined Trajectories for Quadrotor

UAVs. Aerospace 2015, 2, 155.27. Shi, Y.; Eberhart, R. A modified particle swarm optimizer. In Proceedings of the 1998 IEEE International

Conference on Evolutionary Computation, IEEE World Congress on Computational Intelligence (Cat.No. 98TH8360), Anchorage, AK, USA, 4–9 May 1998; pp. 69–73.

c© 2017 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open accessarticle distributed under the terms and conditions of the Creative Commons Attribution(CC BY) license (http://creativecommons.org/licenses/by/4.0/).