2015 IEEE/SICE International Symposium on System Integration (SII)
December 11-13, 2015. Meijo University, Nagoya, Japan

Control Architecture for Service Drone
in Informationally Structured Environment
Farouk Ghallabi, Akihiro Kawamura, Yoonseok Pyo, Tokuo Tsuji, and Ryo Kurazume
Abstract— In this paper, we present a control architecturethat enables a service drone to navigate in an informationallystructured environment (ISE) and to accomplish a specificservice task autonomously based on the ROS-TMS framework. The ROS-TMS is a ROS-based distributed information
management system for ISE. It manages a variety of subsystems ranging from sensing by distributed sensors to motion
planning and behavior control for service robots. The proposedarchitecture is designed as a component of the ROS-TMS, andconsists of a navigation system that solves a path planningin ISE and a flight control system by a behavior-based finite
state machine. The navigation of the drone is allowed by an
optical motion tracking system consisting of distributed infrared
cameras managed by the ROS-TMS. A graphical user interface
is designed to provide simple manipulation of a service task bya drone. Service experiments have been conducted to validate
the performance of the architecture.

and robots are sensed and stored structurally in a database.
We have been developing the ROS-TMS framework [7] in
ISE, which is a ROS-based distributed information management system. The ROS-TMS manages a variety of subsystems ranging from sensing by distributed sensors tomotion planning, and behavior control for service robots.
The control architecture for a service drone proposed in thispaper is designed as a component of the ROS-TMS. The
architecture consists of a navigation system that solves apath planning problem in ISE, a non-linear flight control
system for a drone, and a behavior control system by abehavior-based finite state machine. In addition, a graphicaluser interface is designed to provide simple manipulation of
the drone.
The rest of the paper is organized as follows. After
introducing the concept of ISE and ROS-TMS in Section II,
we will explain the drone platform in Section III. In Sections
IV and V, the control system for a drone consisting of a path
planning system and non-linear flight control system will
be introduced. In Section VI, we will show the graphicalinterface for making a service task by a drone. Finally,
experimental results in a room will be shown in Section VII.

I. INTRODUCTION
Unmanned aerial vehicle (UAV), generally known as adrone, is used in many situations where manned flight isconsidered to be difficult, too risky, or in a few cases
impossible. In recent years, the use of these unmanned
aircrafts, especially quadcopters has become widespread inmany fields as they possess great locomotive abilities andhave proved very efficient in performing tasks. Such tasks
include mainly civil missions, for example the traffic surveillance [1], [2], the monitoring of oil and gas pipelines andpower lines [3], missions of civil security such as the people
detection [4]. Many other uses are being developed in various
fields like agricultural spraying [5], vegetation monitoring[6], region mapping, promotional films for tourism andsports, ecological surveys, fire extinguishing, etc. Hence,these flying robots are the subject of continuous labor andresearch in order to provide drones that can carry out their
missions with the least fail probability as possible.
The aim of this research is to develop an application
using a parrot platform for human daily life assistance in
an informationally structured environment (ISE). The ISE is
an intelligent environment where a number of sensors areembedded. Various information such as events occurred in
the environment, positions and situation of objects, humans,
FaroukBoulevard

Ghallabi
is
with
ENSTA
ParisTech,des Maréchaux, 91762 Palaiseau Cedex,farouk.ghallabi@ensta-paristech.fr

II. I NFORMATIONALLY S TRUCTURED E NVIRONMENT
AND ROS-TMS
A. Informationally Structured Environment
Many researches to design autonomous systems have been
conducted in robotic field. The basic idea is to develop robots
that take decisions without human assistance. Doing sucha work is not trivial and requires some conditions to besatisfied. In fact, robots cannot execute tasks autonomously
without interacting with the physical world. Therefore, theinformation about the environment is of key importance fordesigning autonomous systems. Various sensor devices havebeen designed to sense some characteristics of the physicalworld. However, until today, no single sensor is able toprovide all of the information about the environment. Thus,
robot platforms are equipped with many sensors to achievetasks. For instance, global positioning system (GPS) is used
to localize a robot in the 3D world, inertial measurement
units are used to sense accelerations and angular velocities,
wheel encoders are used to measure the speed or distance ofa wheel travels, etc. Collecting all kinds of sensors on robots
is obviously not achievable due to the hardware capacity andresource limitation. An informationally structured environment (ISE) is a concept that supports robot’s knowledge byoffering different sensor modalities distributed in the world.
In the ISE, a number of sensors are embedded not only

1024,
FranceA. Kawamura, T. Tsuji and R. Kurazume are with Faculty ofInformation Science and Electrical Engineering, Kyushu University,744, Motooka, Nishi-ku, Fukuoka, Japan {kawamura,tsuji,kurazume}@ait.kyushu-u.ac.jp

Y. Pyo is with Graduate School of Information Science and ElectricalEngineering, Kyushu University, 744, Motooka, Nishi-ku, Fukuoka, Japanpyo@irvs.ait.kyushu-u.ac.jp

978-1-4673-7241-1/15/$31.00 ©2015 IEEE611

on a robot but also in an environment. Additionally, sensed
information is stored structurally in a database. If a robot
requests an information to the database, the information issent back to the robot immediately.
As an example of the ISE, we have been developing an
intelligent room named Big Sensor Box (B-sen) as shown in
Fig. 1. In B-sen, various sensors are embedded such as laser
scanners, RGB-D cameras, intelligent cabinets, intelligent
refrigerator, floor sensing system, etc. For tracking positionsof human, robots, and objects, Vicon motion capture system
is installed. This sensor detects positions of human, robots,
and objects in B-sen accurately. Vicon system detects objects
through a passive optical motion capture technique, whichuses retroreflective markers tracked by infrared cameras as
illustrated in Fig. 2. Therefore, in this study, many markers
are placed on a drone, as illustrated in Fig. 3, so that we
can get the orientation and position information of the dronein B-sen. The Vicon system has been used for team motion
control of multiple quadcopters [8], [9], [10]. However, inthese systems, the frame rate of the position data is quite
high. It is 100∼375 [Hz]. In our system, since the viconsystem is one of the embedded sensors in ISE, such a highbandwidth cannot be assigned to the vicon system due tothe limitation of the available bandwidth. In our system, theframe rate of the vicon data is about 10∼100 [Hz] and it
causes unstable behaviors for the drone in some cases.

all available robots. The management system uses ROS tomanage data and to publish it. The software developed iscalled ROS TMS (Town Management System). In the next
subsection, ROS TMS architecture is briefly described.
B. ROS TMS architecture
ROS TMS [7] is a software designed to manage information received from sensors distributed in ISE. The datais stored in a local database, processed according to eachtask, and delivered to all robot services. Data management,
processing, and scheduling are executed using more than 90
ROS modules, as listed below and illustrated in Fig. 4:• The user request (TMS UR ) is for communicating withthe user by using a remote controllers, smartphones,
tablets, and other portable devices, interpreting thecommands sent by the user and delivering it to the taskscheduler.
• The task scheduler (TMS TS) receives the commands
from the task request and manages the executions ofthese commands.
• Sensor and robot controller modules (TMS SD and
TMS RC) are responsible for interpreting the data fromsensors and computing the commands to execute eachtask.
Tracker (Vicon)

Intelligent refrigerator
SmartPal IVIntelligent cabinet
SmartPal V
Laser range finder

KXP robot

Wheeled chair robot

Fig. 1. An informationally structured environment named Big Sensor Box(B-sen)Fig. 2. Vicon motion capture system (Bonita) and a retroreflective marker

User

TabletTMS_UR : User Request
TMS_TS : Task Scheduler

ServerBoard PC

TMS_SA
State Analyzer

TMS_SD
Sensor Driver

TMS_RC
Robot Controller

SensorRobot

Sensor & Robot

Fig. 4.
Fig. 3.

Drone covered by retroreflective markers

TMS_RP
TMS_SS
Sensor System Robot Planning

TMS_DB : Data Base

Service Target

DB
ServerFile
ServerROS TMS architecture

As ROS TMS is based on ROS, data broadcasting is done
by ROS publisher nodes. For instance, in order to get Vicon
data, we need to subscribe to a specific ROS node. This nodereads data from the database and publishes it through a local
network.

The information gathered by the sensors including Vicon
is processed and sent to a centralized management system[11] which, in turn, broadcasts it via a local network to612

III. PARROT A R . DRONE P LATFORM

4) Compute a safe path (avoiding existing obstacles).
To do that, we need to represent the environment. Startingfrom a 3D model, as shown in Fig. 6, we created a 2D
occupancy grid map in which only xy plane is considered tosimplify the planning problem. Ideally, since the drone lives
in a 3D world, a 3D map should be considered in the future.

We have been working on Parrot Ar.drone 2.0 platformshown in Fig. 5. This platform has 3-axis gyroscope andaccelerometer, ultrasound sensor, two cameras: the front
camera with high resolution used for recording images andvideos, the bottom camera with lower resolution and used toestimate the altitude of the drone.

Fig. 5.

Parrot Ardrone 2.0 and front camera

Parrot platform is also equipped with a cortex A8 microprocessor, and this processing unit uses the onboard sensors
to stabilize the system, to maintain a fixed altitude, tocontrol the pitch, roll and yaw angles. Parrot is controlled
by sending four commands u = (ux , uy , uz , uψ ), and all ofthem should be in [−1, 1]. The (ux , uy ) commands control
the horizontal speeds in the x and y directions respectively.
The uz command controls the vertical speed, and the uψcontrols the angular velocity around the vertical axes z. As
we mentioned above, the microprocessor executes an inner
loop that controls the pitch, roll, and yaw angles. Thus
the control commands are simply percentages of maximum
values of these parameters, positive and negative values
determine the direction of the movement. To control thedrone, Wifi connection must be established and specificcommands must be sent. As we use ROS, we have beenusing ardrone autonomy [12].

Fig. 6.

3D model of the environment

Since the indoor hull of the drone is about 517mm ×451mm [13] and the dimension of the environment is
12.5m × 4.5m, we selected 0.5m as the resolution of thegrid map. Therefore, the occupancy grid map consists of
25 × 9 grids. In practice, to insure safe flights, some virtual
obstacles are added, for instance, this is convenient to fill
narrow places and unreachable regions for the drone and toinsure a safe distance from true obstacles.
Finally, for completion of the work, the drone position isread from Vicon sensors and marked on the map. Figure 7
shows a graphical display of the map using Matlab. Whitesquares refer to obstacle-free spaces, black ones refer toobstacle spaces, and the gray one is the drone position.

IV. M AP R EPRESENTATION AND PATH P LANNING
As we mentioned in the introduction, the system has tosolve a path planning problem in ISE and control a drone
motion to track the generated path. Thus, a dynamic model
is of particular value to understand the drone behavior and todesign a robust controller. The following sections will deal
with these problems and will demonstrate the methods used
to solve them.

B. Dijkstra’s algorithm

A. Occupancy grid map

Now that we defined a 2D map, a Dijkstra’s algorithm isimplemented to find a shortest path between two positions.
Dijkstra’s algorithm uses a weighted graph G = (E, V ). The
set V represents the set of vertices of the graph G, and theset E is the set of couples (u, v) ∈ V 2 . For each couple(u, v), we define a cost function w(u, v), which must be
nonnegative, i.e: w(u, v)  0.
In our case, the map elements are labeled from 1 to 255(25 × 9). The set V is constructed as

The goal of this part is to solve a path planning problem
in ISE. In ISE, all of the necessary information including thestructure and positions of obstacles are obtained beforehand.
Thus this is of key importance for designing autonomous
systems and for finding shortest, or optimal, path between
two points. In this study, we plan to develop a path planningalgorithm as follows:
1) Localize the drone in the environment2) Input a goal position by GUI device3) Compute the shortest path between initial and goal
positions

V = {i ∈ 1, 2, .., 255, such that element i of the map isfree(no obstacle)},
613

(1)

For simplicity, we denote the terms cos θ and sin θ by Cθ
and Sθ , respectively.
1) Dynamics: Acceleration of the center of mass can bederived from Newton’s lawyẍ = (RRP Y f − fg )/mx
Fig. 7.

Let f = (0, 0, f )T and fg = (0, 0, −g)T , the equations of
motion are:Occupancy grid map

where V is the set of obstacle-free spaces in the occupancy
grid map. Now, we need to construct the set E. Our approachconsists in reducing the movements taken by the drone into
four elementary movements: {forward (F), backward (B), left(L), right (R)}, that means, from a given vertex, maximum
four vertices are possible to be selected.
The last thing to do is defining the cost function w. Let u
and v be two adjacent vertices. Suppose that we are currently
in vertex u then
1, if v is reachablew(u, v) =(2)∞, if v is not reachable

C ψ CθRRP Y = ⎝ Sψ Cθ
−SθQuadcopter parameters

C ψ Sθ Sφ − Sψ C φ
Sψ Sθ Sφ + C ψ C φ
C θ Sφ

=

ÿ

=

z̈

=

Cφ Sθ Cψ + Sφ Sψ
fm
C φ Sθ Sψ − S φ C ψfm
C φ Cθ
f −g
m

(5)
(6)
(7)

B. Behavior based control approach

Let’s denote by f the total thrust generated by the four
propellers, RRP Y the rotation matrix, and m the drone mass.
According to roll-pitch-yaw convention one representation of
the matrix RRP Y is⎛ẍLet’s make a simple interpretation of these equations andpoint out some remarks. The generated thrust is the sumof the forces produced by the system {motor+propeller}.
In fluid mechanics, the force generated by a propeller is
proportional to the square of the rotational speed of each
motor. Therefore, the values of motor speeds are required toknow f . However, in some platforms, like parrot ar.drone, we
do not have access to this information. As a consequence, thevalue of f is considered as an unknown function. This said,using the equations of motion may not be simple and requires
knowledge about f or about its estimate. Our approach isplanned to solve this problem and to simplify the 3D dynamic
model so that we can design a suitable feedback controller.

V. B EHAVIOR CONTROL BY F INITE S TATE M ACHINE
A. Quadcopter dynamics
The drone is a rigid body that lives in a six degrees of
freedom parametrized by a translation and orientation vectors. The translation vector can be identified as the positionof its center of mass x = (x, y, z) and the orientation vector
can be parametrized according to roll-pitch-yaw convention(θ, φ, ψ). Forces applied on the system are generated byfour motors and propellers, these forces induce three torques(τx , τy , τz ), around three body axes as shown in Fig. 8.

Fig. 8.

(4)⎞C ψ Sθ C φ + Sψ SφSψ Sθ C φ − Sψ Sφ ⎠
Cθ Cφ
(3)
614

In order to simplify the dynamic model expressed in theprevious subsection, behaviors have been implemented toreduce the degrees of freedom and to make the control
design easy. Before starting demonstrating the method, it isconvenient to start by defining the meaning of behaviors inrobotics. Ronald C.Arkin defines in his book [14] a behavior
as a reaction to a stimulus. Stimulus has different forms,
for instance if we consider a mobile robot navigating in an
unknown environment one behavior can be ’avoid-obstacle’.
This behavior is not active until an obstacle is detected,
hence ”obstacle-detected?” can be seen as a stimulus to”avoid-obstacle” behavior. Many approaches have been used
to model behaviors. Ronald mentioned in the same bookthree approaches to express behaviors.
1) Stimulus-response (SR) diagram2) Functional notation
3) Finite state acceptor (FSA) diagram
Our case study uses the third approach to express the
behaviors implemented for parrot platform. “FSAs are bestused to specify complex behavioral control systems where
entire sets of primitive behaviors are swapped in and outof execution during the accomplishment of some high-levelgoal” [14]. It is represented by a set of behavior states andtransitions between them. A transition can be expressed by{condition+action}. Figure 9 shows a simple FSA with twostates and one transition.

Condition / Action

State Athe drone to maintain a fixed position (x, y, z), this isconvenient to insure safety when the previous behaviors areswapped in and out during execution.

State B

q2
Fig. 9.

Finite state acceptor diagram

q3

yawControl

HOVER

pathTracking

q2
q11) Model Simplification: In this part, we will describe
steps which have been taken to simplify the model. Indeed,in the Path Planning section, the generated path is a simple
set of discrete 2D points (xref , yref ). This does not include
the altitude of the drone z and suppose that it moves only in a
2D world. Moreover, orientation is not expressed so that fora fixed (xref , yref ) we have infinite values of ψ ∈ [0, 2π].
This suppose that during trajectory tracking, ψ and z have
fixed values which is not always satisfied. That’s why to keep
validating these assumptions. The behaviors are implemented
to control orientation and altitude of the drone as illustrated
in the following points:
1) Controlling the orientation ( yawControl )Consider the room model showed in Fig. 7, two major
directions can be identified.
• The drone is moving in the x-positive direction sothat ψ = 0• The drone is moving in the x-negative direction sothat ψ = π
The yawControl behavior uses a proportional controller
to control the orientation. Thus, the control command
sent to the drone is uψ = K1 (ψ −ψref ) where ψref ={0, π}
2) Controlling the altitude ( altitudeControl )This behavior is for controlling the altitude at a fixedheight z = zref . This behavior also uses a proportional
controller to control the altitude. The command sent is
uz = K2 (z − zref )3) 2D path tracking ( pathTracking ) This behavior isfor tracking the reference points (xref , yref ) generated
by the path planning algorithm. The control techniqueused in this behavior will be discussed in the next part.
If we consider that during the 2D path tracking, the altitude
remains constant, i.e z = zref , vertical acceleration can be
set to 0 and then we can compute the value f . Conforming to(7) and to the result z̈ = 0, the expression of f is deduced as. Using the new expression of f and the possible
f = Cmg
θ Cφvalues of ψ we retrieve the following systems:
ẍ

=

ÿ

=

z̈

=

±g tan(θ)
tan(φ)∓gcos(θ)0(ψ = 0 or π)(8)(ψ = 0 or π)

(9)

q4
q1

altitudeControl

Fig. 10.

Finite State Acceptor diagram

Figure 10 illustrates behaviors and transitions implemented as described in this section. As it was mentioned,
transition can be viewed as simple condition q, the notationq̄ refers simply to the negative of condition q. Transitions
are described as follows:• q1 = z − zref  > 1
• q2 = ψ − ψref  > 2 and z − zref  < 1
• q3 = q̄1 , q̄2 and (x, y) − (xref , yref ) > 3
• q4 = (x, y) − (xref , yref ) < 3 ,where 1 , 2 and 3 are positive values fixed throughout
experiments. We used 1 = 0.2 [m], 2 = 0.2 [deg] and3 = 0.4 [m] in the experiments, respectively. Note that as
the step value for the grid map described in the previous
section is 0.5 m, it is convenient to have 3 < 0.5, otherwise
we will not have satisfying results. Indeed, take the exampleof 3 = 1, the pathTracking behavior is inactive when thecondition q4 is satisfied, or 3 = 1 is like allowing the
tracking error to be represented by two squares in the map,
hence the goal position might not be tracked in the desired
square. Moreover, transitions are defined in a manner that
there is a predefined execution order. For instance, condition
q2 cannot be true until condition q̄1 is true. The execution
orders are1) altitudeControl2) yawControl3) pathTracking
The implementation of this diagram has been computed
using Stateflow simulink toolbox.
C. 2D position control
Now that we simplified the dynamic model using behaviorbased design, let’s get down to the details of the pathTracking
behavior. We saw that the pathTracking behavior is intended
to provide a control approach for tracking 2D paths generated
by the path planning algorithm. In this part, (8) and (9) incase (ψ = 0) are considered, note that the same work is done
for the case (ψ = π).
We want the drone to track a reference points (xref , yref ).
As we mentioned in Section 2, the input commands for(10)

One extra behavior can be added for the parrot platform
which is hover mode. In fact, hover command enables615

parrot platform are percentages of certain maximum values
for these angles.Thus, the input commands are (θdes , φdes ),where θdes is the desired angle for roll, and φdes is thedesired angle for pitch. Equations (8) and (9) can be writtenasẍ

= g tan(θdes ),

(11)

ÿ

des )
= −g tan(φcos(θdes ) .

(12)1) Outer loop/Inner loop: The onboard software uses
these commands (20) and (21) to control roll and pitch. As
we do not know exactly what algorithm is running inside thesystem, many experiments have been conducted to estimate
the control parameters running behind the scenes. In [15],the influence of control commands as a first order linear
model is described so that the real pitch and roll angles as
the following equations:Suppose that we want to design θdes and φdes in a waythat we transform the 2D dynamics equations ((11) and (12))
to a second order system defined byẍ = α1 ẋ + β1 x,ÿ = α2 ẏ + β2 y.

θ̇ + λ1 θφ̇ + λ2 θThis system can be written in the state-space form ẋ = Ax ,where x = (x, ẋ, y, ẏ)T and
⎞
⎛
0
1
0
0
⎜β1 α1 0
0⎟
⎟.
(15)
A=⎜
⎝0
0
0
1⎠
0
0 β2 α2The system is asymptotically stable only if the eigenvalues ofA have strict negative real parts. Equations (11), (12), (13),and (14) yield the following system:(16)

tan(φdes )= α2 ẏ + β2 ycos(θdes )(17)−g tan(φdes ) = cos(θdes )(α2 ẏ + β2 y)

(18)

−g

−g tan(φdes ) = cos arctan

α1 ẋ + β1 xg

(α2 ẏ + β2 y).

φdes = − arctan

1xcos(arctan( α1 ẋ+β))(α2 ẏ + β2 y)
g

g

(23)

θ(s)

=

φ(s)

=

c1
θdes ,
s + λ1c2
φdes .
s + λ2(24)(25)

Step input
[deg]

The control inputs sent to the drone are
α1 ẋ + β1 xg

= c2 φdes .

Due to the symmetric geometry of the drone we can assume
that roll and pitch follow the same response so that we canassume that λ1 = λ2 = λ and c1 = c2 = c.

(19)θdes = arctan(22)

In order to estimate λ1 and λ2 , step commands are sent to
the drone, i.e: a set of fixed values of θdes and φdes , angularmeasurements are collected from the onboard sensors forfew seconds. Figure 11 shows an example of an angularresponse for θdes = αθmax , where α = 0.4 and θmax = 0.21
[rad]. The shape of the curve validates approximating the
response as a first order system. Let’s write (22) and (23) inthe Laplace domain:

(13)
(14)

g tan(θdes ) = α1 ẋ + β1 x= c1 θdes ,

(20)
.

(21)
˜ ẏ = ẏref − ẏ,˜ x = xref − x̃,
Note that we set ẋ = ẋref − ẋ,˜ ẏ˜ ,x̃, and ỹ are current
and y = yref − ỹ in (20) and (21). ẋ,velocities and positions obtained by Kalman filter in the next
section, and ẋref , ẏref , xref , and yref are target velocities(ẋref = ẏref = 0) and next reference positions.
Let’s point out some remarks. As it has been mentioned in
section 2, control commands must be in [−1, 1] or (20) and(21) do not guarantee that we can satisfy such a condition.
Three steps are taken to get rid of this problem. First, in the
choice of the coefficients α1 , α2 and β1 , β2 , for that reason,
simulations are running to pick out acceptable coefficients.
Second, as the control commands are percentages of φmaxand θmax , desired angles θdes and φdes are divided by θmax
and φmax . Finally, saturation functions are added to eliminate
unexpected overshoots.

Time [s]Fig. 11.

Examples of roll responses

Using these data in System Identification toolbox in
Matlab enables as to have an estimation of (c, λ). These
parameters are of particular value for the state estimation
and Kalman filtering process in the next part.
Now that we determined (c, λ), two independent loops
are running. The outer loop is going to compute the desired
angles according to the desired position as described above.
The onboard loop, the inner loop, is going to stabilize
the system around these commands. Figure 12 resumes thearchitecture of Outer/Inner loops.
616

xref

des
Outer loop

yref

.
(x,x)

.
(y,y)

Fig. 12.

executed once the user clicks on the corresponding button.
Inner loop

desInner/outer loops

D. State estimation and Kalman filter
In the previous part, (20) and (21) are computed using(x, ẋ, y, ẏ). This implicitly supposes that information on
position and velocity is available and with no errors. In
practice, none of these two assumptions is true. Vicon data
only provides position and orientation. Moreover, there is
no guarantee to have a noiseless data. That’s said, a state
estimation and filtering techniques are required for goodcomputation of feedback control. A Kalman filter is a good
estimator for our case, and thus we adopted the Kalmanfilter for state estimation. Control System toolbox in Matlab
provides a simple Simulink block to implement it. This
block requires measurement data and input commands andoutputs the estimate of x which we will be used for thecontrol techniques discussed in the previous parts. Figure 13
illustrates a completion of Fig. 12 integrating Kalman filter
and Vicon data.

xref

.


(x,x)

Inner loop

.


(y,y)

des

VII. E XPERIMENTS
We conducted a service experiment in B-sen. The service
scenario of the experiment is as follows. Firstly, someone
stands in front of the entrance door and tries to enter into theroom. Then the user requests to ROS-TMS by a GUI device
to send a drone to the entrance so that the user can see what
is happening . The captured video by the drone is displayedon a screen in a room. Figures 15 and 16 show the map
and the planned trajectory by the proposed system. Figure 17
shows the experimental results. Left images show the motionof a drone and right images show the GUI interface. Aftersuggesting the destination by pointing on the GUI, the drone
took off, moved along the desired path, and reached to thedestination automatically.

(x,y,z)Kalman filter

Vicon data

(,)Fig. 13.

Graphical user interface

Let’s explain the button functions shown in Fig. 14.
Basically, 5 major blocks are illustrated in the GUI: Map,
Generate path, Extra info, Takeoff/Land, Move the drone.
The Map block enables the user to input the goal position
by clicking on one of the white squares of the map image
and provides a real time localization of the drone. Generate
path block uses the goal position as input and generate
a possible path from the drone initial. Takeoff/Land block
affords manual takeoff and land commands, it also offers an
emergency button ’reset’ that stops the drone motors during
the flight and reset the software running onboard, and a’hover’ button in case when the user wants to keep the drone
in fixed position. Extra info block shows information about
battery level and drone state. The drone battery level is of key
importance for insuring safe flights and avoiding accidental
power off.

desOuter loop

yref

Fig. 14.

General control schema

VI. G RAPHICAL USER INTERFACE
Now that we already solved the two major problems (Pathplanning and feedback control), we want to offer a simple
way to manipulate drone system. Consequently, any user
can easily get benefit from the application that we havedeveloped. A very simple way to do that is using Graphical
User Interface Editor (GUIDE) in Matlab. The GUI must
provide the following options• Manual takeoff and Land buttons• Emergency land
• Extra information including battery level and the drone
state• Real time localization of the system in the map• Input the goal position and generate the 2D path
• Follow the generated path
The graphical user interface design is based on callback
functions to execute desired tasks. Callback functions are

VIII. C ONCLUSIONSIn this paper, we have been solving path planning andfeedback control problems in an Informationally Structured
Environment based on ROS-TMS framework. The major
problems solved are a path planning using Dijkstra’s algorithm and occupancy grid map, trajectory tracking problem
using dynamic model of the drone and some control theory
results. We conducted some service experiments by a drone
using a developed GUI interface. Some assumptions havebeen done to design the path planning algorithm can causesome problems in specific cases. For instance, the assumption
617

Sofa
TargetTable

Bed
InitialFig. 15.

Room map and initial and target positionsof a static environment is not valid if obstacle positions
are modified or new unexpected obstacles are added to thestructure. Hence, a dynamic map of the environment, updated
each time the algorithm is executed, is of particular value andsolves this problem.

Target position
Entrance door

Initial position

Fig. 16.

Planned trajectory by the developed system

ACKNOWLEDGMENTS
This research is supported by The Japan Science andTechnology Agency (JST) through its “Center of Innovation Science and Technology based Radical Innovation and
Entrepreneurship Program (COI Program).”Fig. 17.

Experiments

R EFERENCES[1] Anuj Puri. A survey of unmanned aerial vehicles (uav) for traffic
surveillance. Department of computer science and engineering,
University of South Florida, 2005.
[2] Vladimir Reilly, Haroon Idrees, and Mubarak Shah. Detection andtracking of large number of targets in wide area surveillance. In
Computer Vision–ECCV 2010, pages 186–199. Springer, 2010.
[3] Cristina Gomez and David R. Green. Small-scale airborne platforms
for oil and gas pipeline monitoring and mapping. University ofAberdeen report, 2015.
[4] Y. Iwashita, A. Stoica, and R. Kurazume. Finding people by theirshadows: Aerial surveillance using body biometrics extracted fromground video. In Emerging Security Technologies (EST), 2012 Third
International Conference on, pages 43–48, Sept 2012.
[5] Parth N Patel, Malav A Patel, Rahul M Faldu, and Yash R Dave.
Quadcopter for agricultural surveillance. Advance in Electronic andElectric Engineering, 3(4):427–432, 2013.
[6] J. Berni, P.J. Zarco-Tejada, L. Suarez, and E. Fereres. Thermal andnarrowband multispectral remote sensing for vegetation monitoringfrom an unmanned aerial vehicle. Geoscience and Remote Sensing,IEEE Transactions on, 47(3):722–738, March 2009.
[7] Yoonseok Pyo, Kouhei Nakashima, Shunya Kuwahata, Ryo Kurazume,
Tokuo Tsuji, Ken’ichi Morooka, and Tsutomu Hasegawa. Service
robot system with an informationally structured environment. Robotics
and Autonomous Systems, 2015.
[8] Nathan Michael, Daniel Mellinger, Quentin Lindsey, and Vijay Kumar. The grasp multiple micro-uav testbed. Robotics & Automation
Magazine, IEEE, 17(3):56–65, 2010.

[9] Lionel Heng, Lorenz Meier, Petri Tanskanen, Friedrich Fraundorfer,
and Marc Pollefeys. Autonomous obstacle avoidance and maneuvering
on a vision-guided mav using on-board processing. In Robotics andautomation (ICRA), 2011 IEEE international conference on, pages
2472–2477. IEEE, 2011.
[10] Daniel Gurdan, Jan Stumpf, Michael Achtelik, Klaus-Michael Doth,Gerd Hirzinger, and Daniela Rus. Energy-efficient autonomous fourrotor flying robot controlled at 1 khz. In Robotics and Automation,2007 IEEE International Conference on, pages 361–366. IEEE, 2007.
[11] Tokuo Tsuji, Oscar Martinez Mozos, Hyunuk Chae, Yoonseok Pyo,
Kazuya Kusaka, Tsutomu Hasegawa, Ken’ichi Morooka, and Ryo
Kurazume. An informationally structured room for robotic assistance.
Sensors, pages 1–28, 2015.
[12] ardrone autonomy. http://ardrone-autonomy.readthedocs.org/en/latest/.
[13] Parrot. http://ardrone2.parrot.com/ardrone-2/specifications/.
[14] Ronald A RKIN. Behavior-Based Robotics. The MIT Press, 1998.
[15] Engel Jakob, Sturm Jürgen, and Cremers Daniel. Scale-aware navigation of a low-cost quadrocopter with a monocular camera. Robotics
and Autonomous Systems (RAS), pages 1–14, 2014.

618