Content uploaded by Ahmed Hussein
Author content
All content in this area was uploaded by Ahmed Hussein on Jun 04, 2016
Content may be subject to copyright.
Autonomous Indoor Navigation of Low-Cost Quadcopters
Ahmed Hussein, Abdulla Al-Kaff, Arturo de la Escalera and Jos´
e Mar´
ıa Armingol
Intelligent Systems Lab (LSI) Research Group
Universidad Carlos III de Madrid (UC3M), Legan´
es, Madrid, Spain
Email: {ahussein, akaff, escalera, armingol}@ing.uc3m.es
Abstract— Many researchers from academia and industry
are investigating closely how to control an autonomous mobile
robot, especially Unmanned Aerial Vehicles (UAV). This paper
shows the ability of a low-cost quadcopter, ”Parrot AR-Drone
2.0”, to navigate a predetermined route. The path is obtained
by optimized path planning algorithm. A generic Simulated
Annealing (SA) optimization algorithm is implemented to
generate the obstacle-free path. This path is divided into
several waypoints, which are navigated by the drone in various
experiments. The position and orientation of the quadcopter
are estimated with the incremental motion estimation approach
using Inertial Measurement Unit (IMU), which is mounted on
the drone. The quadcopter is controlled via Simulink model
with PID, which manipulates the drone internal controller
for the pitch, roll, yaw and vertical speed. Four different
experiments were tested to evaluate the performance of the
proposed algorithm and the obtained results indicate the high
performance of the quadcopter and its applicability in various
navigation applications.
I. INTRODUCTION
During the last few years, the field of micro aerial vehicles
has encountered a significant focus among the robotics
research community. This increased interest comes from
the noteworthy advantages of these flying platforms over
standard Unmanned Aerial Vehicles (UAV) in terms of safety
and efficiency at small sizes [1]. The drones or quadcopters
are part of the UAV family, which has the ability of vertical
take-off, one of its main advantages is the maneuverability
[2].
One of the main areas of research in this field is the path
planning problem, where it is required to find the optimal
collision-free path from the start position to the goal position.
Then navigate through it avoiding the static obstacles [3].
Moreover, the indoor navigation for quadcopters has more
attention in the recent researches [2].
For autonomous movement of the quadcopter, and the
ability to move freely in the environment to reach the desired
goal, a path planning algorithm is required. Satisfying the
main advantage of the quadcopters in terms of maneuver-
ability, an efficient and optimized path planning algorithm is
incorporated in the system for near optimal navigation output
[4] [5].
The next step is the localization feedback to navigate the
obtained path with accuracy, there are plenty of methods to
localize the quadcopter. However, this paper focuses on two
methods for available sensors in the AR-Drone. The first
This work is supported by the Spanish Government through the CICYT
Project ADAS ROAD-EYE TRA2013-48314-C3-1-R.
one is the vision-based approach using the camera, many
researchers are using this technique during the last decade [6]
[7] [8] [9]. The second method uses the Inertial Measurement
Unit (IMU) sensor to localize the quadcopter and find its
pose [10] [11].
Therefore, this work tackles the ability of a low-cost
drone ”Parrot AR-Drone 2.0” to navigate a predetermined
route. The path is generated via applying a generic simulated
annealing (SA) approach to any given indoor occupancy grid
map. The quadcopter localization is estimated by the inertial-
based method, using the economical price mounted sensors.
Moreover, the drone is controlled via Simulink model with
PID, which manipulates the internal controller of the drone
to modify the pitch, roll, yaw and vertical speed.
The remainder of this paper is organized as follows:
Section II introduces the proposed path planning algorithm,
followed by Section III which discusses the pose estimation
method. Section IV presents and discusses the experimental
results. Finally, conclusions are summarized in Section V.
II. PATH PLANNING
The path planning algorithm enables the quadcopter to
find collision free path from a given start position to a
goal position, avoiding static obstacles in any well-known
environment. Accordingly for discretized grid-based maps in
a static environment, many solutions are exploited to reach
the best path in terms of the minimum distance [12] [13].
According to previous studies in metaheuristic optimiza-
tion approaches to mobile robots path planning, the SA
algorithm showed the best results [13] [14] [15]. Ergo the
SA is selected to be the optimization technique in this work.
SA is a random search technique commonly used to solve
large-scale problems, particularly the ones with a unique
global optimum hidden between many local optima. It is a
metaheuristic algorithm of the trajectory-based family, which
is the set of approaches that uses a single solution throughout
the algorithm in order to find the optimal solution. Its main
idea is inspired by the physical annealing process; in which
the temperature is used as a control parameter, with the
cooling schedule to search for the global optimum, using
the concept of probabilistically accepting non-improving
solutions to avoid being trapped in local optima [16].
Inputs to the algorithm are the occupancy grid map, start
position and goal position. At each iteration of the algorithm
a solution must be constructed to be evaluated, initially the
algorithm generates a solution that is completely random.
2015 IEEE International Conference on Service Operations And Logistics, And Informatics (SOLI)
978-1-4673-8480-3/15/$31.00 ©2015 IEEE 133
After the construction of the initial candidate solution, the
feasibility of this solution must be checked in terms of
reaching the goal position and do not stuck in deadened path.
Figure 1 is an example grid map, where white cells mean
empty cells, black cells represent the obstacles, the green
cell is the start position and finally the red cell is the goal
position.
Fig. 1. Path planning grid map
As the algorithm progresses, neighboring solutions of the
current solution must be generated, in order to explore the
search space of the problem. There are several proposed
methods in the literature to generate neighbor solutions such
as deletion and insertion, crossover, mutation, swapping and
other methods [17] [13] [18]. These methods depend on the
type of formulation of the problem being solved. In this pro-
posed approach, four methods were implemented to generate
the candidate solutions and a random operator is used for
each iteration. The random choice of the used operator gives
the algorithm both the explorative and exploitative features
that are useful in escaping local minimum and finding a
better solution through searching in the neighbors of elite
solutions respectively. The four methods are elitism, random,
crossover and mutation.
Before the start of the SA algorithm, the cooling schedule
must be defined. The cooling schedule consists of four
variables which are initial temperature, final temperature,
annealing schedule and a number of iterations per temper-
ature. The initial temperature is the starting temperature of
the algorithm, and the final temperature of the algorithm is
the temperature at which the algorithm becomes a greedy
algorithm and stops accepting worse solutions. The annealing
schedule is the variable that decides how the temperature
is decremented from the initial temperature to the final
temperature throughout the algorithm iterations, from the
literature [13] the geometric cooling schedule is used in
the proposed algorithm at which the current temperature is
calculated as shown in equation (1).
T(t)=Toαi,wherei=1,2, ..., if&0<α<1(1)
where Tis the current temperature at each iteration, Tois
the initial temperature that is set to 5000, αis the geometric
cooling coefficient that is set to 85% and iis the number of
iteration.
The number of iterations per temperature is set in order
to allow the system to stabilize at this temperature through
several exploration of the search space at approximately same
acceptance ratio of worst solutions. The acceptance ratio is
calculated as shown in equation (2), at which the condition
to accept is to have the estimated probabilistic coefficient
greater than 85%.
p=exp
−(NL−CL)
T(2)
where pis the probabilistic coefficient, NL is the neighbor
solution length, CL is the current solution length and Tis
the current temperature.
The main purpose of an optimization process is to find the
optimal solution to a certain problem through maximizing
or minimizing a certain function. This function measures the
quality of the provided solution and it is generally called
the objective function of the optimization problem. This
objective was explicitly taken into consideration during the
formulation of this path planning problem as the minimiza-
tion of the total covered distance between all the waypoints,
as shown in equation 3.
L=min di,j (3)
where Lis the total length of the solution and di,j is the
distance between every two way-points in the solution from
the starting point to the goal point.
The SA as any metaheuristic algorithm combines greedy
strategy, to search for better solutions and stochastic strategy
in order to escape getting stuck in a local minimum. There-
fore at each iteration of the SA the neighboring solution
is directly accepted, if and only if, the quality of this
neighboring solution is better than the previous solution.
The algorithm results in a vector of waypoints in terms
of X and Y coordinates, taking into consideration a safety
distance of 1 meter from the occupied cells (obstacles). This
is mainly to avoid the air resistance effect from the walls on
the quadcopter, which disturbs its balance. The SA algorithm
is implemented using Matlab and the algorithm flow chart is
presented Figure 2.
III. INCREMENTAL MOTION POSE ESTIMATION
In this paper, the incremental motion estimation method is
implemented to evaluate the position and orientation of the
quadcopter. This method is based on the IMU sensor readings
interpretation. The IMU sensor is an electronic device that
measures and reports a quadcopter velocity, orientation, and
gravitational forces, using a combination of accelerometers
and gyroscopes [19]. Accordingly the roll and pitch angles
can be measured with an accuracy of 1 degree by applying
integrator to the values obtained from the IMU [20]. The
mathematical formulas governing this method are presented
in equations (4) and (5) for the velocities and position
respectively.
vx
vy=cos(ψ)−sin(ψ)
sin(ψ)cos(ψ)u
v(4)
2015 IEEE International Conference on Service Operations And Logistics, And Informatics (SOLI)
134
Start
Set cooling
schedule
End
Generate random
solution as current
solution [CL]
Completed
all iterations?
Yes
Check
NL < CL?
Generate neighboring
route [NL]
CL ÅNL
& update the
cooling schedule
Yes
Check
probability
coefficient?
Accept
No
No
Reject
Fig. 2. Simulated annealing algorithm flow chart
x=x0+t
t0vx(t)d(t)
y=y0+t
t0vy(t)d(t)
(5)
The implementation of the pose estimation using the IMU
is performed through the Simulink blocks in Figure 3. The
model is originally implemented and presented in [21], then
it is modified to fit the project platform. The modifications
are, but they are not limited to, adjusting the PID controller
gains for the pitch and the roll angles, to generate the output
for horizontal velocities of 11.18 meters per second and the
controller gain for the yaw angle to provide a 4.50 degrees
per second angular velocity.
The system states are monitored by measuring the Euler
angles and altitude every 0.65 seconds. To connect the
path planning algorithm with the pose estimation Simulink
model, an interface program is implemented. The input is an
occupancy 2D grid map, which the path planning algorithm
uses to generate the planar waypoints with an offset of 1
meter between each point. Afterward, the take-off vertical
values are included with a maximum height of 1.50 meters,
same values were chosen for landing vertical values at the
final point.
The user activates the autonomous navigation mode from
the Simulink model to star the process. First, the generated
route from the path planning algorithm is divided into several
waypoints. The difference between each waypoint and the
next one is half a meter, to allow the drone accurate localiza-
tion and re-positioning at every point. Accordingly, the model
reads the segmented path and sends the fly commands to the
drone to reach the selected waypoint. Upon reaching that
point, the IMU feedbacks the localization and the controller
shifts to the next point until it reaches the destination.
IV. EXPERIMENTAL RESULTS
The following subsections present the experiments plat-
form, evaluation metrics, evaluation scenarios and the results
obtained by the proposed approach.
A. Platform
The commercially low-cost Parrot AR-Drone 2.0 quad-
copter is used in the experiments [22]. In order to design
the control system for the drone, it is necessary to know its
dynamic model parameters. However, to avoid the complex-
ity in modeling, the proposed control was applied on the
system’s internal controller, Figure 4. The internal control is
governed by the inputs (pitch, roll, yaw, and vertical speed),
therefore the implemented controller knows the drone actual
position, orientation and velocity.
Fig. 4. AR-Drone internal control structure [23]
B. Evaluation Scenarios
Four different indoors scenarios were tested to evaluate the
performance of the proposed approaches on the quadcopter.
The first scenario was inside a campus building with the
starting point at the building entrance gate and the goal point
at a room’s door. The remaining scenarios were implemented
in an indoor field of 18 meters by 9 meters. The second
scenario was zigzag maneuvers around static obstacles, as
shown in the example of Figure 1. While the third scenario
tested the quadcopter ability to follow an open square shape.
Finally, the last scenario was following a V-shape path.
The occupancy grid maps for the four scenarios are
generated to the scale of the real dimensions, the grid cells
are divided into 1 square meter. All the static obstacles
are considered in the mapping process. Subsequently the
maps are loaded into the program to start the path planning
2015 IEEE International Conference on Service Operations And Logistics, And Informatics (SOLI)
135
Fig. 3. Inertial controller Simulink model
2015 IEEE International Conference on Service Operations And Logistics, And Informatics (SOLI)
136
algorithm and to obtain the final 3D waypoints for the
autonomous navigation.
C. Results
Results from the four experiments are evaluated qualita-
tively and quantitatively. The different proposed scenarios
were used to test the algorithms qualitatively. However, in
order to be able to quantitatively test the quality of the pro-
posed algorithms, two evaluation metrics were introduced.
First evaluation metric is the time taken by the quadcopter
to navigate from the starting point to the goal point, including
the take-off and landing duration. The second one is the error
margin between the theoretical and actual values.
The performance of each scenario is illustrated through
plotting the values of theoretical path planning algorithm
values and the actual path values obtained from the IMU
readings; where the axes refer to the real 3D coordinates of
X, Y and Z-axes in meters.
1) Experiment 1: Figure 5 illustrates the performance
of the quadcopter in navigation from the building entrance
to the room’s door. The results show that the quadcopter
managed to follow the path in straight and diagonal motions.
Fig. 5. From building entrance gate to room’s door results
2) Experiment 2: Figure 6 illustrates the performance of
the quadcopter in navigating a zigzag maneuvers. At which
the quadcopter follows a straight path circumnavigating static
obstacles, maintaining a safe distance of two meters around
them. The results showed the high performance of the
algorithms in following the theoretical path.
Fig. 6. Zigzag maneuvers results
3) Experiment 3: Figure 7 illustrates the performance of
the quadcopter in navigating an open ending square shape.
This experiment shows the accurate performance of the
quadcopter to follow a loop shape where the starting point
and goal point are close to each other. The results show near
optimal navigation.
Fig. 7. Square shape results
4) Experiment 4: Figure 8 illustrates the performance of
the quadcopter in navigating a V-shape to reach a goal point
on the same horizontal level after moving only diagonally.
The results show that quadcopter followed the given path
precisely.
Fig. 8. V-shape results
To sum up, Table I concludes the results of the four exper-
iments. The error represents the deviation of the quadcopter
actual path (red curve) compared to the ground-truth path
(blue curve). The table shows that the error margin is very
small compared to the covered flying distance, which proves
the high performance of the algorithms and their applicability
in various navigation applications. Also proves that a slight
fluctuation during landing as a constant error during all
experiments.
V. C ONCLUSIONS
This paper presented experimental work of a low-cost
quadcopter ”Parrot AR-Drone 2.0” to achieve autonomous
indoor navigation. For any given occupancy grid map, a
generic optimization approach is applied to solve the path
planning problem. The algorithm is optimized by SA meta-
heuristic technique to reach the minimum traveling path from
2015 IEEE International Conference on Service Operations And Logistics, And Informatics (SOLI)
137
TABLE I
QUANTITATIVE ANALYSIS
Exp. No. No. of
waypoints Distance [m] Time [s] Error [%]
1 21 14.89 38.30 3.30
2 43 39.53 72.93 4.03
3 37 32.52 64.29 3.25
4 22 16.88 41.29 5.50
starting position to the goal position. The output path is
divided into several waypoints, which the drone controller
read and send the flying commands accordingly. The drone
is controlled via Simulink model with PID, which manip-
ulates the drone internal controller for the pitch, roll, yaw
and vertical speed. The main contribution is the ability of
the drone to follow the waypoint generated from the path
planning algorithm with minimal error.
Experimental results illustrated the differences in the quad-
copter’s maneuvering abilities through different scenarios.
The pose and the distance errors were minimal, which
indicated the high performance of the proposed algorithms
and their applicability in various navigation applications,
despite the low-cost of the sensors used.
Future aspects of this research include performing fur-
ther test experiments with a visual-based method for the
localization feedback and compare it to the obtained results.
Moreover, handling complicated environments with dynamic
obstacles, which requires extending the problem from path
planning to trajectory planning problem.
REFERENCES
[1] H. Huang, G. Hoffmann, S. Waslander, and C. Tomlin, “Aerodynamics
and control of autonomous quadrotor helicopters in aggressive maneu-
vering,” International Conference on Robotics and Automation 2009.
IEEE., 2009.
[2] S. Gupte, P. I. T. Mohandas, and J. M. Conrad, “A survey of quadrotor
unmanned aerial vehicles,” Proceedings of Southeastcon, IEEE., pp.
1–6, 2012.
[3] M. Achtelika, A. Bachrach, R. He, S. Prentice, and N. Roy, “Au-
tonomous navigation and exploration of a quadrotor helicopter in gps-
denied indoor environments,” First Symposium on Indoor Flight, 2009.
[4] C. Richter, A. Bry, and N. Roy, “Polynomial trajectory planning for
quadrotor flight,” International Conference on Robotics and Automa-
tion (ICRA’2013). IEEE., 2013.
[5] D. Mellinger, N. Michael, and V. Kumar, “Trajectory generation and
control for precise aggressive maneuvers with quadrotors,” Interna-
tional Journal of Robotics Research: 0278364911434236, 2012.
[6] E. Altug, J. P. Ostrowski, and R. Mahony, “Control of a quadrotor he-
licopter using visual feedback,” International Conference on Robotics
and Automation, ICRA’02. IEEE., pp. 72–77, 2002.
[7] E. Altug, J. P. Ostrowski, and C. J. Taylor, “Quadrotor control using
dual camera visual feedback,” International Conference on Robotics
and Automation, ICRA’03. IEEE., pp. 4294–4299, 2003.
[8] T. Krajnik, M. Nitsche, S. Pedre, L. Preucil, and M. E. Mejail, “A
simple visual navigation system for an uav,” International Multi-
Conference on Systems, Signals and Devices (SSD). IEEE., pp. 1–6,
2012.
[9] M. Saska, T. Krajnik, J. Faigl, V. Vonasek, and L. Preucil, “Low cost
mav platform ar-drone in experimental verifications of methods for
vision based autonomous navigation,” International Conference on
Intelligent Robots and Systems (IROS’2012) IEEE., pp. 4808–4809,
2012.
[10] T. Zhang, Y. Kang, M. Achtelik, K. Kuhnlenz, and M. Buss, “Au-
tonomous hovering of a vision/imu guided quadrotor,” International
Conference on Mechatronics and Automation (ICMA). IEEE., pp.
2870–2875, 2009.
[11] V. Grabe, H. Bulthoff, and P. Giordano, “A comparison of scale
estimation schemes for a quadrotor uav based on optical flow and
imu measurements,” International Conference on Intelligent Robots
and Systems (IROS’2013) IEEE., pp. 5193–5200, 2013.
[12] M. Darrah, W. Niland, and B. M. Stolarik, “Multiple uav dynamic task
allocation using mixed integer linear programming in a sead mission,”
American Institute of Aeronautics and Astronautics, 2005.
[13] A. Hussein, H. Mostafa, M. Badreldin, O. Sultan, and A. Khamis,
“Metaheuristic optimization approach to mobile robot path planning,”
in International Conference on Engineering and Technology (ICET),
2012, pp. 1–6.
[14] Q. Zhu, Y. Yan, and Z. Xing, “Robot path planning based on artifi-
cial potential field approach with simulated annealing,” International
Conference on Intelligent Systems Design and Applications (ISDA’06).
IEEE., vol. 2, pp. 622–627, 2006.
[15] Z. Li, J. Jia, M. Cheng, and Z. Cui, “Solving path planning of uav
based on modified multi-population differential evolution algorithm,”
Advances in Neural Networks (ISNN’14). Springer., pp. 602–610,
2014.
[16] J. Stoer and R. Bulirsch, Introduction to Numerical Analysis. Springer
Science and Business Media, 2002, vol. 12, ch. Minimization or
Maximization of Functions, pp. 444–455.
[17] A. Mosteo and L. Montano, “Simulated annealing for multi-robot
hierarchical task allocation with minmax objective,” Workshop on
Network Robot Systems: Toward intelligent robotic system integrated
with environments, IROS’06, 2006.
[18] S. Panov and S. Koceski, “Global path planning algorithm for mobile
robots,” International Scientific Conference on Information, Commu-
nication and Energy Systems and Technologies (ICEST), pp. 355–358,
2013.
[19] S. Sukkarieh, E. M. Nebot, and H. F. Durrant-Whyte, “A high integrity
imu/gps navigation loop for autonomous land vehicle applications,”
Transactions on Robotics and Automation, IEEE., vol. 15, no. 3, pp.
572–578, 1999.
[20] S. Grzonka, G. Grisetti, and W. Burgard, “A fully autonomous indoor
quadrotor,” Transactions on Robotics and Automation, IEEE., vol. 28,
no. 1, pp. 90–100, 2012.
[21] D. Sanabria and P. Mosterman, “Ar-drone simulink development kit,”
MathWorks, 2013.
[22] S. Piskorski, N. Brulez, P. Eline, and F. DHaeyer, “Ar-drone developer
guide,” Parrot, 2012.
[23] T. Krajnik, V. Vonasek, D. Fiser, and J. Faigl, “Ar-drone as a platform
for robotic research and education,” Research and Education in
Robotics-EUROBOT. Springer Berlin Heidelberg, pp. 172–186, 2011.
2015 IEEE International Conference on Service Operations And Logistics, And Informatics (SOLI)
138