Mechatronics 49 (2018) 187–196

Contents lists available at ScienceDirect

Mechatronics
journal homepage: www.elsevier.com/locate/mechatronics

The development of an autonomous navigation system with optimal control T

of an UAV in partly unknown indoor environment☆
⁎,a,b
Thi Thoa Mac , Cosmin Copotc, Robin De Keysera, Clara M. Ionescua
a
DySC research group on Dynamical Systems and Control, Ghent University, Technologiepark 914, Zwijnaarde 9052, Belgium
b
School of Mechanical Engineering, Hanoi University of Science and Technology, Dai Co Viet street 1, Hanoi, Vietnam
c
Department of Electromechanics, Op3Mech, University of Antwerp, Groenenborgerlaan 171, Antwerp 2020, Belgium

A R T I C L E I N F O A B S T R A C T

Keywords: This paper presents an autonomous methodology for a low-cost commercial AR.Drone 2.0 in partly unknown
Unmanned aerial vehicles (UAVs) indoor flight using only on-board visual and internal sensing. Novelty lies in: (i) the development of a position-
Sensor fusion estimation method using sensor fusion in a structured environment. This localization method presents how to get
Autonomous navigation the UAV localization states (position and orientation), through a sensor fusion scheme, dealing with data pro-
Optimal control
vided by an optical sensor and an inertial measurement unit (IMU). Such a data fusion scheme takes also in to
Multi-objective particle swarm optimization
account the time delay present in the camera signal due to the communication protocols; (ii) improved potential
field method which is capable of performing obstacle avoiding in an unknown environment and solving the non-
reachable goal problem; and (iii) the design and implementation of an optimal proportional - integral - derivative
(PID) controller based on a novel multi-objective particle swarm optimization with an accelerated update
methodology tracking such reference trajectories, thus characterizing a cascade controller. Experimental results
validate the effectiveness of the proposed approach.

1. Introduction An autonomous UAV consists of four essential requirements: (i)

perception, the UAV uses its sensors to extract meaningful information;
In the last few years, Unmanned Aerial Vehicles (UAVs) stir up both (ii) localization, the UAV determines its pose in the working space; (iii)
scholar and commercial interest within the robotics community as the cognition and path planning, the UAV decides how to steer to achieve its
real and potential applications are numerous [1]. To undertake the target; (iv) motion control, the UAV regulates its motion to accomplish
challenging task of autonomous navigation and maneuvering, a versa- the desired trajectory.
tile flight control design is required. The path planning problem can be divided into classical methods
A large number of studies have emerged in the literature on UAVs. and heuristic methods [9]. The most important classical methods con-
Some examples of its application can be found in precision agriculture sist of cell decomposition method (CD), potential field method (PFM),
[2], formation control of Unmanned Ground Vehicles (UGVs) using an subgoal method (SG) and sampling-based methods. Heuristic methods
UAV [3], habitat mapping [4]. Modeling, identification and control of include neural network (NN), fuzzy logic (FL), nature inspired methods
an UAV using on-board sensing are presented in [5]. Catching a falling (NIM) and hybrid algorithms. The potential field method (PFM) is
object using a single UAV, has been accomplished in [6] and for a group particularly attractive since it has a simple structure, low computational
of UAVs in cooperative formation in [7], where high-speed external complexity and easy to implement. In literature, there has been a sig-
cameras were applied to estimate the position of both the objects and nificant amount of work based on this method applied to ground agents
UAVs. Simultaneous localization and mapping (SLAM) was im- path planning [10–13]. An interesting work on implementing and flight
plemented to navigate UAV in working space [8]. Current im- testing of this approach on an UAV is studied in [14]. To operate in real-
plementations in UAV still require collision avoidance, adaptive path- time, a layered approach is developed in uncharted terrain: plan glob-
planing and optimal controller. There exists a need to design meth- ally and react locally. The global planner is based on an implementation
odologies to cope with these requirements to increase the degree of of Laplace equation that generates a potential function with a unique
intelligence and therefore autonomy of UAV. minimum at the target. The local planner uses modification of

☆
This paper was recommended for publication by Associate Editor Dr. Lei Zuo.
⁎
Corresponding author.
E-mail addresses: Thoa.MacThi@ugent.be (T.T. Mac), Cosmin.Copot@uantwerpen.be (C. Copot), Robain.Dekeyser@ugent.be (R.D. Keyser),
ClaraMihaela.Ionescu@ugent.be (C.M. Ionescu).

https://doi.org/10.1016/j.mechatronics.2017.11.014
Received 18 August 2017; Received in revised form 8 November 2017; Accepted 29 November 2017
0957-4158/ © 2017 Elsevier Ltd. All rights reserved.
T.T. Mac et al. Mechatronics 49 (2018) 187–196

conventional potential field method in which not only the position of It is worth mentioning that the coordinate system described above
the UAV (as in the traditional PFM) but also the relative angles between (x; y; z), represents a relative coordinate system used by the internal
the goal and obstacles are taken into account. However, this approach controllers (low layer). Using such a coordinate system instead of ab-
sometimes encounters problems when the repulsion from obstacles solute coordinates (e.g., X; Y; Z) in the high layer will yield errors. For
exceeds the physical constraints of the UAV. It is pointed out that the example, notice that by rotating the quadrotor, the relative coordinates
potential field method has several inherent limitations [15] in which (x; y) will change with respect to the absolute coordinates, as depicted
the non-reachable target problem is the most serious one and is worth in Fig. 1 (Right). In which, the rotation angular of XY coordinate
investigating since it causes an incomplete path in the navigation task. system respect to the absolute xy coordinate system is γ. It is possible to
As an UAV is a complex system in which electromechanical dy- state that the relation between the two-coordinate system depends di-
namics is involved, the robust controller is an essential requirement. In rectly of this angle.
[16], the dynamical characteristics of a quadrotor are analyzed to de- The IMU provides the software with pitch, roll and yaw angle
sign a controller which aims to regulate the posture (position and or- measurements. Communication between Ar.Drone and a command
ientation) of the quadrotor. An autonomous control problem of a station is performed via Wi-Fi connection within a 50 m range.
quadrotor UAV in GPS-denied unknown environments is studied AR.Drone 2.0 is equipped with two cameras in the bottom and in frontal
[17,18]. In order to obtain reasonable dynamical performance, guar- parts with the resolutions of 320 × 240 pixels at 30 frames per second
antee security and sustainable utilization of equipment and plants, (fps) and 640 × 360 pixels at 60 fps, respectively.
controller performance has to be constantly optimal.
In the current study, a real-time implementation for an AR. Drone
2.0 UAV autonomous navigation in indoor environment is proposed to 2.2. Analysis of inputs and outputs and system identification
trigger its identification, able to estimate the UAV pose, detect ob-
stacles, generate the suitable path and to perform the parametric op- The developed Software Development Kit (SDK) mode allows the
timization of its optimal proportional-integral-derivative (PID) con- quadrotor to transmit and receive the information roll angle (rad), pitch
troller. The main contributions are the development of: (i) a position- angle (rad), the altitude (m), yaw angle (rad) and the linear velocities
estimation method based on sensor fusion using only on-board visual on longitudinal/transversal axes (m/s). They are denoted by {θout, ϕout,
and inertial sensing considering the time delay of the camera signal and ζout, ψout, ẋ, ẏ } respectively. The system is executed by four inputs {Vinx,
reducing drift errors; (ii) a solution to solve the non-reachable target Viny, ζ˙in, ψ̇in } which are the linear velocities on longitudinal/ transversal
problem in conventional PFM; (iii) multi-objective optimization PID axes, vertical speed and yaw angular speed references as depicted in
controller based on a proposed multi-objective particle swarm optimi- Fig. 2.
zation (MOPSO) with an accelerated update methodology to execute An Ar. Drone is a multi-variable and naturally unstable system.
navigation task. The motivation behind this research is to illustrate that However, due to the internal low layer control implemented in the
autonomous navigation is feasible on low-cost UAV devices. embedded operative system, it is considered as a Linear Time Invariant
This paper is structured as follows: the next section gives a de- (LTI) System, which is able to be decomposed into multiple single input
scription of AR.Drone 2.0, identification, system setup and localization. single output (SISO) loops. Transfer functions are obtained via para-
Section 3 discusses UAV path planning based on improved potential metric identification using the prediction error method (PEM) and
field method. Multi-objective particle swarm optimization algorithm for Pseudo-Random Binary Signal (PRBS) input signals [19]. A sampling
control parameters optimization and simulation results are described in time of 5 ms for yaw and 66 ms for other degrees of freedom are chosen
detail in Section 4. Next, the effectiveness of the proposed real-time based on the analysis of dynamics characteristic. The identified transfer
collision-free path planning for an AR. Drone 2.0 UAV using only on- functions are given in Eq. (1).
board visual and inertial sensing application in indoor environment is Validation of transfer function of pitch/roll, altitude and yaw are
presented in Section 5. The final section summarizes the main outcome presented in Fig. 3. The validation of the transfer function is made
of this contribution and presents the next challenges. against a different set of data to prove that quadrotor movements are
approximated appropriately.
2. System setup, identification and localization
x (s ) 7.27
Hx (s ) = =
Vinx (s ) s (1.05 s + 1)
A description of the Ar.Drone 2.0 main characteristics, system
identification, sensory equipment, system setup and localization are y (s ) 7.27
Hy (s ) = =
presented in this section. Viny (s ) s (1.0fs + 1)
ζ (s ) 0.72
Haltitude (s ) = out =
2.1. Ar.Drone 2.0 description and coordinates system ζ˙ (s )
in
s (0.23 s + 1)
ψ (s ) 2.94
Hyaw (s ) = out =
There are four basic motions of this UAV: pitch, roll, throttle, yaw ψ˙ in (s ) s (0.031 s + 1) (1)
and translational movements over x, y and z, as shown in Fig. 1 (Left).

Fig. 1. The movements of an AR.Drone 2.0 in absolute and relative planes (Left) and UAV
displacement on (x; y) plane respect to the absolute plane (Right). Fig. 2. Inputs and Outputs of an AR.Drone 2.0.

188
T.T. Mac et al. Mechatronics 49 (2018) 187–196

Fig. 4. Optical sensor and IMU for localization of the Ar.Drone 2.0.

Fig. 3. Validation of pitch/roll (a), altitude (b), yaw (c) transfer function of an AR.Drone
2.0.

2.3. System setup and localization using sensor fusion

In our approach, the localization based on the sensor fusion ap-

Fig. 5. Our navigation approach of an AR.Drone 2.0.
proach which fuses data from ground patterns and IMU data is shown in
Fig. 4a. A time delay is presented in the camera signal due to the
communication protocols. Time delay depends directly on the resolu- cos (yaw ) −sin (yaw ) ⎤
tion of the camera, i.e: 0.33 second approximately for a size of velocityworld = velocitylocal *⎡
⎢ sin (yaw ) cos (yaw ) ⎥
320×240 pixels. Fig. 4b explains the information channels for cameras ⎣ ⎦
and IMU speed sensors. The sensor fusion localization allows Ar. Drone pos world = prevpos world + velocityworld △time (2)
2.0 to determine its location and orientation in a working space.
Fig. 5 presents all components of our navigation approach. The first The bottom camera uses a grid of ground patterns to estimate the
component is sensor fusion and the second one is a cascade control, pose of the drone. Each pattern inside this grid represents (x, y) co-
which guides the quadrotor to follow the designed trajectories. ordinates which are calculated based on the information in the first and
First, the velocity data of the drone in the local coordinate system second rows. Each row includes three bits, the white and black ones are
are transformed into the world coordinate system, then the position is corresponding to ‘0’ and ‘1’. Fig. 6 represents the position of the ground
retrieved by using Euler integration: pattern with ( x = 1, y = 3) coordinates calculated as below:

189
T.T. Mac et al. Mechatronics 49 (2018) 187–196

Fig. 6. A Ground pattern representing position (x = 1, y = 3).

Fig. 8. Image process flowchart of pattern based localization.

The position obtained from the camera represents the offset-free

position n samples before, where n represents the time delay in samples
(i.e., n = 5 with Ts = 66 (ms)). Next, it is possible to integrate the speed
values of the last five samples, in order to obtain the position estimation
from odometry up to time n − 1. Eq. (5) presents the method to elim-
inate the time delay effect on the video signal using a combination with
odometry, assuming the dead time is a multiple of the sample time for y
Fig. 7. (a) Center of the image versus the actual position of the quadrotor; (b) Calculation axis.
of the offset to the center.
0
y = Ts ∑ vy (i) + ycam (Nd )
x = 20x 0 + 21x1 + 22x2 i =−(Nd − 1) (5)
y = 20y0 + 21y1 + 22y2 (3) where y is the final position in meters, Nd = Td/Ts, and Ts is the sample
time: 66 ms; i represents the samples; vy is the speed on the y axis; and
The rectangle at the right bottom of the pattern is used to approx-
ycam represents the position obtained from the camera with a constant
imate the orientation of the quadrotor. The distance between two pat-
time delay Td = 200 ms. The position of the quadrotor in x axis is
terns is 50 cm to ensure that there is always at least one pattern visible
calculated in similar way.
in one image.
Fig. 9 shows an estimation of the position with odometer (IMU),
Due to the quadrotor having different pitch angles during flight, the
optical sensor (camera) and the combination which fuses two sensors. It
center of the image from the bottom camera is not always pointing
is clear that it is not possible to only use the camera for position esti-
perpendicular to the ground (Fig. 7(a)). Therefore, a correction to this
mation as it starts to drift very quickly. The IMU has a robust position
center needs to be made using the information from the on board pitch
estimation but it is not very accurate. The sensor fusion combines the
sensors. This offset is dependent on the pitch/roll angles of the quad-
advantages of both signals. It is a robust estimation without noise.
rotor and the field of view (FOV) of the camera which can be calculated
using trigonometry rules as illustrated in Fig. 7(b). In order to correct
the offset for x and y, the following relationships can be used: 2.4. Obstacle detection

tan (roll) The proposed method in this paper considers both unknown and
offsetx =
x known obstacles, where the known obstacles are predefined from the
image width/2 beginning. This information could be extracted from the provided map
tan (FOV /2) =
x or from previous flights through that environment. Unknown obstacles
2*tan (roll)*tan (FOV /2) become only visible upon detection, which may force the algorithm to
offsetx =
image width
2*tan (pitch)*tan (FOV /2)
offsety =
imageheight (4)
The image processing algorithm is depicted in flowchart as shown in
Fig. 8 in which the input image is converted into gray image, then
applied a suitable threshold to find the contours. This threshold de-
pends on the light condition and therefore has to be appropriately
chosen.
In this work, the proposed solution to achieve a reliable position
estimation consists of combining the information from the two sensors.
In order to reduce the drift effect and noise, IMU is used to read the
variations and the optical sensor is used to find an offset-free mea-
surement. The simplest and functional combination consists of using the
optical sensor only when the standard deviation of the last five samples Fig. 9. Position values in an open loop obtained from the image processing-optical sensor
obtained from odometry is bigger than a tolerance value. The first step (green), the IMU-odometry (blue) and the fused response (red). (For interpretation of the
is to synchronise the two signals (video and speeds) as is shown in the references to colour in this figure legend, the reader is referred to the web version of this
article.)
Fig. 4b.

190
T.T. Mac et al. Mechatronics 49 (2018) 187–196

Fig. 10. Detected Obstacle by ArDrone 2.0.

adjust the previous trajectory. The corners on the obstacle are marked Fig. 12. The problem and the solution when the agent, obstacle and target are aligned in
by colored (blue) patterns in order to distinct them from the rest of the which the obstacle in the middle and the attractive force approximates the repulsive
force. Left: the non-reachable target problem of conventional PFM, Right: Avoid local
room as it can be seen in Fig. 10. The orientations of the triangles also
minima solving the target non-reachable problem of MPFM.
provide information the obstacle’s geometry. The obstacle and its lo-
cation can still be detected when is only partially visible to the frontal
camera. In order to estimate the distance to the obstacle, the size of the goal. As a consequence, it is necessary to introduce the relative distance
triangles in the image can be used. The larger they appear, the closer between the agent and the target (d(q, qgoal)) into the formula of re-
they are and vice-versa. pulsive potential. Furthermore, since the agent is unable to stop sud-
denly at the target position while it is moving at a high speed, the agent
velocity term (q̇ ) is taken into account in the proposed attractive po-
3. Path planning based on improved potential field method tential formula. The total potential U = Uatt + Urep obtains the global
minimum (0) if and only if q = qgoal and q̇ = 0 . The MPFM are for-
Path planning is defined as designing a collision-free path in a mulated as follows:
working environment with obstacles. A path is a set of configuration → q
= {q0, q1, ... , qgoal} ∈ R n of the agent that connects the starting po- U = Uatt + Urep
sition q0 and the final position qgoal. Uatt (q, q˙) = ρp d 2 (q, qgoal) + ρv q˙ 2
Although the conventional PFM generates an effective path, it suf- ⎧1 ⎛ 1 1⎞ β
2

fers from the non-reachable target problem. This problem occurs when ⎪ α⎜ − ⎟ d (q , qgoal ) if d (q , qobs ) ≤ d 0
Urep = 2 ⎝ d (q, qobs ) d0 ⎠
the target is close to obstacles. In that case, when the agent approaches ⎨
⎪0 if d (q, qobs ) > d 0
the target, it approaches the obstacles as well. As a consequence, the ⎩ (6)
attractive force reduces while the repulsive force increases. Therefore, where ρp, ρv, α, β are positive coefficients; d0 is the affected distance of
the agent is trapped in local minima and oscillations might occur. obstacle; d(q, qgoal) is the distance between the agent and the target; d(q,
Fig. 11 (Left) presents the case that there are several obstacles lo- qobs) is the minimum distance between the agent and the obstacles.
cated near the target.The repulsive force is considerably larger than the The attractive force and repulsive force are the negative gradients of
attractive force, therefore the agent is repulsed away rather than attractive potential and repulsive potential as follows:
reaching the target.
F = Fatt + Frep
Fig. 12 (Left) illustrates another case in which the attractive field
and the repulsive field are co-linear in opposite directions and the total Fatt = − 2ρp d (q, qgoal ) − 2ρv q˙
force approximates zero thus the agent is trapped in local minima. Frep1 + Frep2 if d (q, qobs ) ≤ d 0
Frep = ⎧
⎨
⎩ 0 if d (q, qobs ) > d 0
3.1. Proposed attractive and repulsive potential β
1 1 ⎞ d (q, qgoal )
Frep1 = − α ⎛⎜ − ⎟

⎝ d (q, qobs ) d 0 ⎠ d 2 (q, qobs )

The crucial cause of the non-reachable target problem is that the
2
goal position is not a global minimum of the total potential U in Eq. (6). αβ ⎛ 1 1 ⎞ β−1
Frep2 = − ⎜ − ⎟ d (q, qgoal )
When the agent reaches the target, attractive potential Uatt is equal to 2 ⎝ d (q, qobs ) d0 ⎠ (7)
zero; however, the repulsive potential Urep is none-zero if there is at
least one obstacle which satisfies the condition d(q, qobs) < d0. To Applying conventional PFM, the agent is not successful in autono-
overcome that drawback, a proposed attractive and repulsive potential mous navigation tasks when the obstacles are located near the target as
is proposed to ensure that the total potential field force has the unique mentioned before. However, the proposed method can handle such
global minimum at the target position. problems properly because it reduces the repulsive force when the
Obviously, if the repulsive potential approaches zero as the agent agent moves towards the target. Thus, the agent enables to reach the
reaches the target, the total potential attains the global minimum at the target successfully as shown in Fig. 11 (Right) and Fig. 12 (Right). It
indicates that the proposed method effectively solves the non-reachable
target problem when the obstacles are located near the target.
The proposed approach is developed to appropriately work in
known and unknown complex environment. First, the global agent path
planning is generated based on the proposed modified potential field
method (MPFM), then this path is renovated when the agent senses a
new obstacle until reaching the target (local path). The algorithm is
presented in Fig. 13.

3.2. Simulation results in complex environment

Fig. 11. The problem and the solution when the position of target is very close to ob-
stacles. Left: the non-reachable target problem of conventional PFM, Right: Avoid local To validate the proposed algorithm, simulations are executed under
minima solving the target non-reachable problem of MPFM.
various complex environment conditions with known and unknown

191
T.T. Mac et al. Mechatronics 49 (2018) 187–196

Fig. 15. Proposed MPFM under the complex partial known environment, Left: The agent
collides black (unknown) obstacle. Right: The agent re-plans the path according to
perceived environment.

4.1. Proposed accelerated particle updates of MOPSO

In the conventional PSO, the particle’s position is updated based on

both the current global best Gb and the personal best Pbi (or local best)
[20]. The purpose of using the local best is primarily to expand the
diversity of the quality solutions, however, the diversity can be simply
Fig. 13. The flowchart of the agent path planning in known and unknown environment simulated by some randomness. Therefore, to accelerate the con-
based on MPFM. vergence of the algorithm, it is possible to use the global best only.
Based on that statement, the velocity vector and position vector are
obstacles. There are large obstacles with different dimensions and formulated as:
shapes located in the way of the agent. Fig. 14 illustrates two different Vi (t + 1) = Vi (t ) + c1 r + c2 (Gb (t ) − Xi (t )) (8)
cases of complex indoor environment with completely known obstacles
in which the agent arrives at the target without collision with obstacles Xi (t + 1) = Xi (t ) + Vi (t + 1) (9)
using MPFM.
where:
Fig. 15 shows the result of the algorithm in complex scenarios with
c1 ∈ [0.1 0.5]*(UB − LB );
unknown obstacle. The known obstacle is presented in red while the
c2 ∈ [0.1 0.7];
unknown obstacle is presented in black. Since agent has no information
r is a uniform random number in [0,1] that brings the stochastic
about the black obstacle in advance, the trajectory is generated to avoid
state to the algorithm.
only red obstacles as displayed in Fig. 15 (Left). However, it updates
Gb(t) is the global best in iteration t;
the path immediately (local path) as soon as detecting a new obstacle
Vi (t ), Vi (t + 1) are velocities of particles i in iteration t, t+1;
(the obstacle changes its color to orange) as shown in Fig. 15 (Right).
Xi (t ), Xi (t + 1) are positions of particles i in iteration t, t+1;
The results prove the feasibility and effectiveness of the algorithm in
LB, UB are lower bound and upper bound of X. In this study, the
complex environment with known and unknown obstacles.
values of LB and UB are (0, 0, 0) and (50, 50, 50).
To reduce the randomness as iterations are updated, the value of c1
can be designed as:
4. Control parameters optimization based on particle swarm
optimization algorithm c1 = c0 ξ t *(UB − LB ) (10)
where c0 ∈ [0.1 0.5] is the initial value of the randomness parameter
Unlike the existing approaches, in our work the PID controllers of
while t is the number of the iterations and ξ ∈ (0 1) is a control
UAV are designed and implemented based on the proposed multi-ob-
parameter. The pseudo code is presented in Algorithm 1.
jective particle swarm optimization (MOPSO) with an accelerated up-
date methodology. This algorithm aims to facilitate convergence to
optimal set of PID parameters. This section is structured as follows. In 4.2. A proposed MPSO-based PID controller approach
Section 4.1, a proposed accelerated particle updates of MOPSO is pre-
sented. MOPSO-based PID controller approach is described in Starting from the transfer function of a PID controller:
Section 4.2. The final subsection presents Ar.Drone 2.0 control results. Ki
GPID (s ) = Kp + + Kd s
s (11)
The controller parameters Kp, Ki, Kd are chosen to satisfy prescribed
performance criteria regarding the settling times (Ts) and the rise time
(Tr), the overshoot (OS) and the steady-state error (SSE). Since the PID
is a very well-known controller, the definition of Tr, Ts, OS and SSE are
not mentioned in this paper. The three desired objective functions are:

J1 (X ) = SSE
J2 (X ) = OS
J3 (X ) = Ts − Tr (12)
where X is a set of parameters to be optimized, X = (Kp, Ki, Kd).
The block diagram of MPSO-based PID controller approach is pre-
Fig. 14. Proposed MPFM under the complex environment.
sented in Fig. 16. In this procedure, the dimension of the particle is 3.

192
T.T. Mac et al. Mechatronics 49 (2018) 187–196

Initialize parameters: The time (iteration) counter is set to 0, initial values r ∈ (0 1), c0 ∈ [0.1 0.5], c2 ∈ [0.1 0.7]

Fig. 16. The proposed MOPSO-based PID controller approach.

Initially, PSO algorithm assigns arbitrary values of Kp, Ki, Kd and

computes the objectives function and continuously update the con-
troller parameters until the objective functions are optimized. A com-
posite objective optimization for PSO-based PID controller is obtained
Update control parameter: c1 = c0 * ξt with ξ is a control parameter, chosen in (0 1).

by summing values of three mentioned objective functions through the

following weighted-sum method.
Algorithm 1. The proposed MPSO pseudo-code.

J (X ) = β1 J1 (X ) + β2 J2 (X ) + β3 J3 (X ) (13)
where β1, β2 and β3 are positive constants; J1(X), J2(X) and J3(X) are the
while {maximum iterations or minimum error criteria is not attained} do

objective functions defined as in equation (12). In this study, those

values are set as β1 = 0.59, β2 = 0.49 and β3 = 0.88.

4.3. Ar.Drone 2.0 control results

The proposed algorithm MOPSO used a set of parameters as: the

swarm size N = 50, the maximum number of iterations Tmax = 50, c0 =
Initialize particle: X is randomly generated in [LB U B]

0.2; c2 = 0.7; ξ = 0.97 in the following simulations.

Having the dynamic model of the Ar.Drone 2.0 (Section 2.2), con-
trollers parameters is obtained by applying proposed MOPSO-based PID
controller as presented in Section 4.2. PID controllers’ optimal para-
meters are obtained through simulations. It is worth noting that the
models obtained for X and Y position controllers are the same, therefore
(Display results) Output optimal results

their controllers have the same topology and parameters. Fig. 17 show
the results of X(Y) position control using Frtool [21], PID tuner Matlab
toolbox and the proposed MOPSO. The optimal Kp, Ki, Kd parameter sets
Update the iteration: t = t+1

obtained by the proposed MOPSO method together with rise time,

settling time, overshoot, peak and the cost function of each Ar. Drone
Update the objectives

2.0 controller are shown in Table 1. The proposed MOPSO is used to

minimize three cost functions in the term of settling times/ rise time
if J(Xi ) ≤ Jbest

(Ts − Tr ), overshoot (OS) and steady-state error (SSE) of each controller.

Update position

Jbest = J(Xi )

As shown in Table 1, all designed controllers have no overshoot, zero

for i = 1: N

steady state error, extremely short rise time and setting time.
Update Gb:

Gb = Xi ;

The results of the proposed approach (blue solid curves, name PSO-
end while

PID) are compared with the PID using Frtool (green dash-dot curves)
and PID tuner Matlab toolbox (red dot curves). Both PSO-PID and PID-
Frtool have better performances than the third one with no overshoot.
However, the setting time is clearly less for the proposed MPSO-PID
controller than for PID-Frtool and PID tuner.
In order to investigate the robustness and sensitivity of the approach
in changing of the weighted constants, the parameters β1, β2 , β3 are
modified in the range of 20% those values. Therefore, the composite
objective optimization is as following:
J (X ) = (β1 ± Δβ1) J1 (X ) + (β2 ± Δβ2) J2 (X ) + (β3 ± Δβ3) J3 (X ) (14)
where:

193
T.T. Mac et al. Mechatronics 49 (2018) 187–196

Fig. 17. X(Y) position step response with MPSO tuning, Frtool, PID tuner.

Table 1
Optimal control parameter selected by pso algorithm for AR. Drone 2.0 PID controllers.

MOPSO-PID Altitude X controller Y controller YAW

controller controller

Kp 21.9820 43.9582 43.9582 24.0011

Fig. 19. The proposed solution structure with cascade control.
Ki 38.1941 40.9473 40.9473 43.4267
Kd 42.8751 9. 2215 9.2215 29.6480
Rise time (s) 0.0363 0.0072 0.0072 5.3959e-04 5. Experimental validation
Settling time (s) 0.2368 0.0129 0.0129 0.0010
Overshoot 0 0 0 0
Peak 0.9976 0.9999 0.9999 0.9925 The proposed solution structure of this study is depicted in Fig. 19.
Cost function 0.26 0.21 0.21 0.2950 At the first stage, the quadrotor uses its sensor to extract obstacles in-
value formation then generates MPFM path based on the proposed path
planning algorithm. At this phase, the quadrotor should know its pose
based on localization process and decides how to steer to achieve its
β1, β2, β3 are defined in Section 4.2.
goal. After that the drone regulates its motion to accomplish the desired
Δβ1 ≤ 0.2β1; Δβ2 ≤ 0.2β2; Δβ3 ≤ 0.2β3.
trajectory. In real experiments, a cascade control is designed such that
Fig. 18 illustrates different MOPSO-PID controllers for X(Y) position
the Ar.Drone 2.0 accurately performs this task. There are two parts of
on Ar.Drone 2.0 obtained by randomly changing the weights of three
the cascade controller: inner-loop controller and outer-loop controller.
objective functions. It was observed that all MOPSO-PID controllers
The inner-loop controller is performed inside the quadrotor as a black-
react very fast and without overshoot and track the reference input very
box. The model identified in Section 2.2 is used to identify the re-
well. In addition, there are only slightly difference between MPSO-PID
lationships between the inputs and the outputs of this black-box. The
controllers’ outputs. In conclusion, the proposed approach is highly
outer-loop controller or MOPSO-PID is represented by the command
robust and not very sensitive in term of changing of the weighted
station, which defines the references to the internal controllers located
constants.
in the low layer.
In the outer-loop controller, the localization process provides the
current Ar.Drone 2.0 pose in the world coordinate based on the ground
patterns. The obtained free-collision trajectory using MPFM is sent to
the controller by a list of way-points. However, due to missing com-
munication, oscillating and uncertain noise, the Ar.Drone 2.0 may be
unable follow the designed trajectory. Therefore, it is necessary to
⎯⎯⎯⎯→
generate a compensation path, named, Tar that guides the quadrotor to
return to the designed trajectory.
To perform the compensation, several definitions are introduced.
Suppose the drone is currently in the position with the two nearest way-
points, named, Previous way-point and Next way-point.

• Path error: the distance between the drone and the designed path at
a time during flight.
• ⎯Target
⎯⎯⎯⎯⎯⎯⎯⎯⎯→
WP : the vector from Previous way-point to the Next way-point
•
⎯⎯⎯⎯⎯⎯⎯⎯→
Target
pathline as the vector towards the perpendicular path

A schematic representation is depicted as shown in Fig. 20. The

⎯⎯⎯⎯→
compensation Tar is only executed when the Path error (L) is larger
Fig. 18. Simulation results obtained from MOPSO-PID X(Y) control in the variance of β1,
than threshold value L0. The formula of the target vector is:
β2 , β3.

194
T.T. Mac et al. Mechatronics 49 (2018) 187–196

drone detects the real obstacle in actual environment, that one is dis-
played in the virtual environment as black box. The MPFM is generates
the path in order taking account the ArDrone 2.0 dimension. The start
point, the target and the dimensions of the working space are also
shown in the virtual environment.
The results obtained with pattern-based localization, MPFM and
optimal PID control in the real system are presented in Fig. 22. The
bounded rectangle presents the walls of indoor working environment.
The red obstacles are known obstacles and the yellow one is unknown
obstacle. In the beginning, MPFM generate the black trajectory which
collides with unknown (yellow) obstacle since the drone only avoid red
obstacles. However, it updates the path immediately (local path-blue
Fig. 20. Compensation strategy for the quadrotor. path) as soon as its detecting a new (yellow) obstacle as shown in
Fig. 22. The green path is the real path obtained by experiment. It is
possible to observe that the quadrotor has few deviations to the desired
trajectory, with an acceptable overshoot at the moment of performing
sharp bends.

6. Conclusions

This paper proposed an autonomous navigation approach for a low-

cost commercial AR. Drone 2.0 using only on-board visual and internal
sensing. The main achievements for this work are: (i) sensor fusion for
localization in a structured environment taking into account camera
signals time delay; (ii) proposed potential field method for path plan-
ning; and (iii) the design and implementation of an optimal PID con-
troller based on a proposed multi-objective particle swarm optimization
(MOPSO) with an accelerated update methodology that allows ac-
complishing path-following task using a cascade control approach. The
proposed modified potential filed method enables to solve the non-
Fig. 21. Virtual vs. Actual environment in the real-time experiment. reachable target problem and successfully avoids unknown obstacles.
The performed tests using the virtual environment and real-time ex-
periment demonstrate the feasibility of the proposed strategy, which
opens new possibilities of autonomous navigation for the mobile agent
in complex known and unknown environment.
Regarding future work, the approach is currently implemented in a
dynamic working space. An extension to multiple UAVs and a combi-
nation with ground vehicles is also under inspection.

Acknowledgment

All authors from Ghent University are with EEDT group, member of
Flanders Make consortium.

References

[1] Woods AC, La HM. A novel potential field controller for use on aerial robots. IEEE
Trans Syst Man Cybern Syst 2017;99:1–12.
[2] Hernandez A, Murcia H, Copot C, De Keyser R. Towards the development of a smart
Fig. 22. Real-time obstacle avoidance of the AR. Drone 2.0 with pattern-based localiza- flying sensor: illustration in the field of precision agriculture. Sensors (Basel)
tion, MPFM and optimal PID controller. 2015;15(7):16688–709.
[3] Hernandez A, Copot C, Cerquera J, Murcia H, De Keyser R. Formation control of
UGVs using an UAV as remote vision sensor. Proceedings of the 19th IFAC World
⎯⎯⎯⎯→ ⎯⎯⎯⎯⎯⎯⎯⎯⎯→ ⎯⎯⎯⎯⎯⎯⎯⎯⎯→
Tar = λ * TargetWP + (1 − λ )* Targetpathline (15) Congress, Cape Town, South Africa. 2014. p. 618–23.
[4] Dijkshoorn N. Simultaneous localization and mapping with the AR.drone.
University of Amsterdam; 2012. Master’s thesis.
where λ is defined parameter as following: [5] Dullerud G. Modeling, identification and control of a quad-rotor drone using low-
resolution sensing. Champaign, IL, USA: University of Illinois at Urbana-
1 − (L/ L0)n if L < L0
λ=⎧ Champaign; 2012. Master’s thesis.
⎨ [6] Bouffard P. On-board model predictive control of a quadrotor helicopter: Design,
⎩0 if L > L0
implementation, and experiments. Berkeley, CA, USA: University of California;
⎯⎯⎯⎯→ 2012. Master’s thesis.
It is important to notice that the Tar should not significantly change [7] Ritz R, Muller M, Hehn M, D’Andrea R. Cooperative quadrocopter ball throwing and
when the drone is around the trajectory with a small Path error and catching. Proceedings of the IEEE/RSJ International Conference on Intelligent
should quickly react with a large Path error. For our system, n = 4 and Robots and Systems (IROS), Portugal, Algarve. 2012. p. 4972–8.
[8] Alvarez H, Paz LM, Sturm J, Cremers D. Collision avoidance for quadrotors with a
L0 = 0.5 m has been chosen. monocular camera. Exp Rob Springer Trans Adv Rob 2015;109:195–209.
In this work, a virtual environment is developed in order to monitor [9] Mac TT, Copot C, Tran DT, De Keyser R. Heuristic approaches in robot path plan-
on-line and facilitate off-line tests of the quadrotor navigation tasks as ning. A Surv Rob Auton Syst 2016;86:13–28.
[10] Malone N, Chiang H, Lesse K, Oishi M, Tapia L. Hybrid dynamic moving obstacle
presented in Fig. 21. In this figure, the comparison between the virtual avoidance using a stochastic reachable set-based potential field. IEEE Trans Rob
environment (Lelf) and actual environment (Right) is shown. When the

195
T.T. Mac et al. Mechatronics 49 (2018) 187–196

2017;33(5):1124–38. Robin De Keyser received the M.Sc. degree in electro-

[11] Moreau J, Melchior P, Victor S, Anioun F, Guillemard F. Path planning with frac- mechanical engineering and the Ph.D. degree in control
tional potential fields for autonomous vehicles. IFAC-PapersOnLine engineering from GhentUniversity, Ghent, Belgium, in 1974
2017;50(1):14533–8. and 1980, respectively. He is currently a Full Professor of
[12] Rasekhipour Y, Khajepour A, Chen S, Litkouhi B. A potential field-based model control engineering with the Faculty of Engineering, Ghent
predictive path-planning controller for autonomous road vehicles. IEEE Trans Intell University. He is the author or co-author of over 300 pub-
Transp Syst 2017;18(5):1255–67. lications in journals, books, and conference proceedings. He
[13] Valbuena L. Hybrid potential field based control of differential drive mobile robots has acted as an external review expert in several European
h. g. tanner. J Intell Robot Syst 2012;68:307–22. Commission Research Programs and is one of the pioneers
[14] Scherer S, Singh S, Chamberlain L, Elgersma M. Flying fast and low among ob- who produced the original concepts of predictive control
stacles: methodology and experiments. Int J Rob Res 2008;27(5):549–74. during the 1980s. The research is application-driven, with
[15] Golan Y, Edelman S, Shapiro A, Rimon E. On-line robot navigation using con- many pilot implementations in technical and nontechnical
tinuously updated artificial temperature gradients. IEEE Rob Autom Lett systems, amongst others in the chemical, steel, marine,
2017;2(3):1280–7. mechatronic, semiconductor, power electronics, and biomedical spheres. His current re-
[16] Li J, Li Y. Dynamic analysis and PID control for a quadrotor. IEEE international search interests include model predictive control, auto-tuning and adaptive control,
conference on mechatronics and automation, Beijing, China. 2011. p. 573–8. modeling and simulation, and system identification.
[17] Song Y, Xian B, Zhang Y, Jiang X, Zhang X. Towards autonomous control of
quadrotor unmanned aerial vehicles in a GPS-denied urban area via laser ranger
finder. Optik - Int J Light Electron Opt 2015;126:3877–82. Issue 23 Clara-Mihaela Ionescu graduated with a M.Sc. degree in
[18] Engel J, Sturm J, Cremers D. Scale-aware navigation of a low-cost quadrocopter Industrial Informatics and Automation at Dunarea de Jos
with a monocular camera. Rob Auton Syst 2014;62(11):1646–56. University of Galati, Romania, in 2003. She obtained the
[19] Ljung L. System identication: theory for the user. Prentice-Hall; 2007. Ph.D. degree in November 2009 from Ghent University, on
[20] Yang XS, Deb S, Fong S. Accelerated particle swarm optimization and support modelling of the human respiratory system with non-in-
vector machine for business optimization and applications. Networked digital teger order models. She was holder of the prestigious
technologies, communications in computer and information science. 136. Springer; Flander Research Foundation (FWO) post-doctoral grant at
2011. p. 53–66. the same university. She is currently a Professor of control
[21] De Keyser R, Ionescu CM. FRTool: a frequency response tool for CACSD in matlab. engineering with the Faculty of Engineering, Ghent
Proc. of the IEEE Conf. on Computer Aided Control Systems Design, Germany, University. She is involved in several international projects,
Munich. 2006. p. 2275–80. with both industrial and biomedical applications. Her main
research interests revolve around the principle of emerging
tools from mathematics into control engineering practice,
Thi Thoa Mac received the B.Eng. degree in mechatronics i.e. generalized order models and control laws.
engineering from Hanoi University of Science and
Technology, Vietnam, in 2006. She obtained her master
degree in 2009 at National Taiwan University of Science
and Technology in Faculty of Engineering, Taiwan. She is
currently a Ph.D. Researcher with the Department of
Electrical Energy, Systems, and Automation, Ghent
University. She also is a lecturer at Hanoi university of
science and technology. Her interests include fuzzy logic
control, neural networks, path planning and optimization
control for autonomous robot.

Cosmin Copot received his M.Sc. and M.E. degrees in

systems engineering from Gheorghe Asachi Technical
University of Iasi, Romania, in 2007, and 2008, respec-
tively. In 2011 he received Ph.D. degree from the same
university on control techniques for visual servoing sys-
tems. He is currently a postdoctoral research at Ghent
University. His research interests include robotics, visual
servoing systems, identification and control.

196