1
Pose estimation, path planning and object avoidance
for autonomous racing drones
N. Rozkvas
n.rozkvas@student.tue.nl, ID 0924842
Eindhoven University of Technology
AbstractThis paper concerns the pose estimation of a drone
relative to a predefined object using a captured image by a
camera on the drone. Furthermore, a path planning algorithm
for drones is proposed, which is capable of satisfying input and
output kinematic constraints with an extension of its function-
ality by trajectory correction for object avoidance. The paper
describes the implementation of the perspective correction using
a template matching method and minimum jerk polynomial
trajectory planning on an ODROID single board computer and
introduces an algorithm for collision avoidance contributing to
the autonomy of drone navigation.
I. I NTRODUCTION
T HE high versality of Unmanned Aerial Vehicles (UAV),
together with the improvement of various associated
technologies have eased the execution of multiple civil and
military tasks. The active research on this topic has started
at the beginning of the 21st century. A stimulus to the
development of the field was the rapid advancements in
sensing technologies, microactuators and microelectronics in-
dustries resulting in increased computational capabilities of
the electronic components and sensors while reducing their
dimensions and price. As a result, drones of various sizes are
now widely used for surveillance missions, communications,
traffic monitoring, agriculture, delivery, etc. However, all of the
mentioned activities require a human operator for the drone.
The latest trend in the research of the UAVs is concerned
with the automation of the vehicles. With development of the
industry, drones are now capable of tracking and following
remote objects [1] [2] or coordinating for complex tasks with
other drones [3] [4] [5] and moving objects [6] [7] in both
indoor and outdoor conditions. This introduces requirements
for accurate sensing, efficient trajectory planning and excellent
control structures. While the latter two require a purely math-
ematical approach, the new applications include additional
sensors as cameras [8] [9] [10], ultrasonic sensors [11] and
laser scanners [12] [13] for more accurate localization of the
drone. However, there has been no consensus reached on the
optimal sensor combination regarding performance, cost and
space requirements, where the latter is rather limited in most
drone designs. In this paper, it is attempted to use an on-board
monocular vision system as the main source of information for
localization of predefined objects in a limited area for the high
speed applications. More specifically, the goal is using a single
on-board camera, recognize a hoop with LED markers on it
and estimate its relative orientation and distance at a frame rate
of at least 30 FPS. The output information has to be suitable
for navigation system of the drone to ensure safe movement.
Regarding the navigation, many researchers focus on the
optimization of the trajectory planning and selection of the
controller to provide the best performance at higher speeds,
while keeping high maneuverability of a robotic system.
Currently, most of the achievements in this area are related
to applications that have a fixed ground (robot manipulators)
or can move only on the ground (robotic cars), while UAVs
are generally under-actuated and have nonlinear dynamics
with large uncertainties due to aerodynamic effects [14]. The
second part of this paper is focused on creating a trajectory
planning algorithm for drones used in autonomous drone
racing, where the objective is to go through hoops with LED
markers at the shortest time. The path planner developed in
this paper assumes a perfect controller implemented on the
flight controller, which is able to robustly follow the proposed
trajectory at high speeds of the drone. The case is further
extended by considering an obstacle on the planned path.
This research is conducted in collaboration with Peter
Nagy [15], when the following is presented in this paper.
First, a pose estimation algorithm based on template matching
method using visual sensing is described in section II. Next, a
polynomial trajectory planning method is proposed for vehicle
navigation and its implementation is explained in section III.
The resulting components are combined with the limitations
of the system, defined as constraints [15]. Furthermore, the
experiments regarding the accuracy of the developed hoop
detection and pose estimation algorithms are presented and
the trajectory generated by path planner in MATLAB is
demonstrated. Finally, the obstacle avoidance in section IV is
shown as an extension to the research on path planning with
the obstacle being defined either as a cube or a sphere.
For the implementation of the mentioned algorithms, a
Hardkernel ODROID C2 quad-core single board computer
with 1.5 GHz CPU was used. It has a Mali GPU which allows
for fast image processing required for this application. The
chosen hardware has powerful specifications for its small size
and low price, which is suitable for developments as described
in this paper. Implementation of the methods and simulations
investigated in this paper has been done in C++ language on
the ODROID and in MATLAB. The detection of the hoop
and simulation of the pose estimation are constructed with
the widely used and well documented open source OpenCV
v3.2 library [16] for graphics processing. The visual sensing
system consists of the Logitech webcamera with 1080p HD
and 90-degree field of view (FOV). The choice of this piece
of hardware is conditioned by absence of distortion on the
edges of the captured image frames (compared to wide FOV
cameras), digitally encoded signal, which eliminates a need fo
any converters in the system, and relatively high resolution.
The described hardware is to be attached to a customly made
quadrotor for racing purposes with a PixHawk flight controller.
The communication between the modules is established via
a MAVLink protocol, through which the information about
the state of the drone (required for the path planning and
2
accurate pose estimation) is delivered from the PixHawk to the
ODROID and trajectory setpoints are fed in opposite direction.
II. P OSE ESTIMATION
This section describes implementation of the pose estima-
tion given a filtered image of a perceived hoop with LED
attached to it that the drone has an objective to fly through.
A. Related work
There are multiple approaches to tackle the problem of
the relative position and orientation estimation of an object
with respect to the camera. In general, the methods can be
subdivided into two main groups: creation of a map around
the observer and template matching.
Mapping of the location can be done by Simultaneous
Localization and Mapping (SLAM) technique using various
sensors: RGB-D [8] [9] [17], lasers [13] or a camera [10].
The essence of the environment mapping lies in comparing the
pose of the drone relative to the surfaces of the objects in its
environment using the mentioned sensors and reconstructing a
3D map from this data. There are great results achieved, par-
ticularly in [8] and [10], however, the visual SLAM method is
not efficient for the current application since it puts limitations
on the maximum speed at which a drone can move due to the
actual construction of the map around it and various artifacts
that cause inaccuracy (e.g. inability of rotation tracking after
turns, scale variations and scale ambiguity [17]).
Another approach to perform pose estimation is template
matching. The essence of template matching lies in searching
for predetermined shapes that can be fitted onto the contour
of a target object in a captured image and then based on the
perception of the shape, determining the relative orientation
and distance. This method in its simplistic form (a more
advanced algorithm by [18] was able to track an object without
any prior calibration or knowledge of focal length of the
camera) is only possible under conditions of knowing the size
and visual perception of the objects by the camera that are to
be distinguished beforehand, so possessing a template.
Template matching can be implemented using multi- or
mono-view methods, depending on the amount of vision
sensors and their locations. However, in the current application
it is desired to have all computations and sensors on-board,
which limits the possible options to one camera on the drone.
In [19], predefined contour and shape primitives have been
used to detect and estimate the exact pose of an object using
a monocular camera system. Here, the non-primitive objects
are decomposed in an array of point clouds describing the
primitive 2D and 3D forms like circles, cylinders, lines, cubes.
By fusing the ones having common edges, the algorithm
is able to reconstruct the shape of the object (shape-graph
matching). In turn, [18], [20] and [21] use Hough, Latent-Class
Hough and Cascaded template forest methods and generate the
template by a training algorithm that collects picture samples
of an object at different conditions. Later, this template again
serves as a reference for matching. A similar idea is described
in [22], where the pose estimation algorithm is optimized for
detection of objects under various conditions given their planar
photo representation as a template.
For the current application, it has been decided to use an
approach similar to [19] where all contours in captured frames
from continuous video stream are compared with a set of
primitive shapes. This is an optimal solution to determine the
relative pose of a drone from the target for the considered
application, since there is only one predefined object that is to
be searched for - the hoop that the drone has to fly through,
which can be represented by a primitive geometric 2D shape
as an ellipse from any point of observation, so no automated
template generation as in [18], [20] and [21] is needed.
B. Assumptions
In order to simplify the task of pose estimation and optimize
the solution for a specific given application, there are few
assumptions to make:
The hoop has shape of a perfect circle and is recognized
by the camera as a circle or an ellipse depending on the
relative pose of the visual system [15].
The input to the pose estimation algorithm are the pa-
rameters of an ellipse that represents the perceived shape
of the hoop from an observation point, including length
of the semi-major and semi-minor axes, tilt of the major
axis with respect to the vertical axis on a 2D plane.
The altitude of the camera and the hoop relative to the
physical ground are known.
A camera is always facing the hoop and can be located
only in one half space formed by the plane of the opening
of the hoop.
Rotation of the camera around the direction towards the
center of the hoop is known.
C. Camera calibration
The calibration of the camera relates to the calculation
of the focal length of the camera. This is crucial for the
distance estimation since the focal point, where the light rays
converge and form image, of each camera is different causing
objects being perceived differently at the same distance. For
the current application it can be computed as follows:
Pradius D
F = , (1)
R
where F is the focal length, R is radius of the hoop in meters,
D is the distance at which the reference shot is taken and
Pradius is the representation of the radius of the hoop in
pixels in a captured frame. For the webcamera used for the
experiments in this paper, this parameter is equal to 730 with
Pradius = 256 pixels, D = 1 m and R = 0.35 cm.
D. Distance estimation
First of all, it can be proven (Appendix A) that the mag-
nitude of the major axis of the detected ellipse is constant at
any angle of observation, considering the same distance from
the camera to the center of the hoop under assumption that the
hoop is a perfect circle. Therefore, the distance for the current
application is decoupled from the orientation. Furthermore,
distance estimation is also independent from the orientation of
the drone. The only requirement for accuracy of the estimation
is the presence of sufficient area of the hoop on the captured
frame so that the detected and appropriately rotated ellipse
corresponds to the one detected if the center of the hoop is
aligned with the center of the frame [15].
In order to determine the distance to the object (hoop), the
semi-major axis of the ellipse in the captured photo frame
is compared to the ellipse (circle) in the reference frame.

Knowing the size of the ellipse in pixels from [15] and
focal length from section II-C the distance can be found by
rearranging the equation (1):
pmajor
d= , (2)
FR
where pmajor is the semi-major axis of the perceived ellipse.
E. Rotation estimation
Since the distance from the camera to the center of the
hoop is known from section II-D, the problem of precise pose
estimation can be addressed as a rotation transformation of a
point at a fixed distance d in cameras coordinate frame. In
a right-handed coordinate system, defining y as vertical axis
of the camera and x as horizontal axis and, z always pointing
towards the hoop with origin of the coordinate frame in the
p0 , the rotation is described by:
1 0 0 cos 0 sin
p0 v = M p1 = 0 cos sin 0 1 0 p1 ,
0 sin cos sin 0 cos
(3)
where p1 is the center point of the hoop, v is the vector of
length d from p0 to p1 and the matrix M is composed of Euler
rotation matrices with angles of rotation around the cameras
horizontal and vertical axes. The rotation in this case can
be described only with 2 elementary rotation matrices due to
the fact that the pose estimation is relative to the reference
frame of the camera instead of the global inertial frame and
rotation of the circle around the center does not change its
perception.
Furthermore, the perceived circle that represents the hoop
if observed perpendicularly to the opening, is mapped onto a
2-dimensional image frame as
 T  T
i j 0 =M x y 0 , (4)
where i and j are values describing the hoop on 2D image
plane, x and y are axes of the plane with the hoop.
In turn, any ellipse on a plane is defined as
   
u k sin(t) cos() + cos(t) sin()
= , (5)
v k sin(t) sin() + cos(t) cos()
where k is the ratio between the semi-minor and semi-major
axes of the ellipse, is the rotation of the ellipse around the
origin and t is the varying parameter to describe the ellipse.
Equating the two representations [i j]> in (4) and [u v]>
in (5) gives a relation between the rotation angles of the center
of the hoop around the vertical and horizontal axes of the
camera and the description of the perceived ellipse in terms
of the rotation angle and ratio k:
cos + sin2 sin2 k cos2 + sin2
 2   2 
= 2 2 . (6)
cos2 k sin + cos2
Using this and distance found in section II-D, it is possible
to determine the position of the camera relative to the drone.
F. Correction for roll of the camera.
Another artifact that is important for the accuracy of the
pose estimation is the roll angle of the camera which corre-
sponds to the roll action of the drone. This, in turn, affects
the rotation estimation described above, since includes also
the roll of the drone. The parameter describing this rotation
3
(a) Camera (b) Simulation
Fig. 1: Perception of the hoop from different positions.
is available in the flight controller as a part of the state data
(section II-B) and can be used to adjust the value of the at
the current instant of time by subtracting or adding the value.
G. Problem of exact position
The result of the previous steps is the representation of the
position of the hoop as a point on a half sphere where the
center is the camera at a radius d estimated using the procedure
in section II-D. However, it is impossible to obtain the exact
position of the camera relative to the hoop possessing only
this data since the object is perceived to be the same while
looking on it from opposite quadrants. Namely, the picture is
the same if, for example, the camera is located at the same
relative rotation angles to the right and above the hoop or to the
left and below the hoop (Fig. 1a). Therefore, as a rule, there is
always one position of the drone in the top-left quadrant, where
the perceived ellipse is identical to the perceived ellipse at a
unique point in bottom-right quadrant under assumption that
the camera is always on one side of the opening of the hoop
(section II-B), and where quadrants are assigned depending on
the position of the drone as top-left, top-right, bottom-left and
bottom-right from the hoop.
In order to eliminate this artifact, additional data is required.
The following options are considered in the context of the
application:
1) Comparison of the cameras and hoops altitude to
choose the correct quadrant.
2) Comparison of the orientation of the hoop relative to
the magnetic fields of the Earth using the magnetometer
measurement of the flight controller.
3) Tracking of the displacement of the center of the per-
ceived hoop on the consecutive captured frames knowing
the velocity of the drone in each of the 3 directions.
4) Detection and pose estimation of the additional features
on the target (e.g. rectangles attached to the hoops would
be perceived as 4 different parallelograms in each of
these positions).
The most computationally efficient options are 1 and 2,
where a parameter describing the current state of the drone
is compared to a predetermined constant. Options 3 and 4, in
turn, may be incorporated for more complicated tasks but in
the cost of extensive computations. Furthermore, measurement
of the hoops position above the ground is more accurate and
4
Distance Offset [m] Variance [m] Angle Offset [ ] Variance [ ]
2m -0.0323 0.0341 0 4.86 3.15
3m 0.00392 0.0782 15 4 5.61
4m -0.0194 0.132 30 0.823 27.3
5m -0.0645 0.334 45 -3.67 23.4
60 -3.6 28.2
TABLE I: Offset and variance of the estimation error at
different target positions.
practical than its orientation relative to the magnetic field
of the Earth. However, the altitude of the drone captured
by a barometer, ultrasonic sensor or a GPS contains larger
error than the orientation from the integrated value from 3
magnetometers on the PixHawk flight controller. Nevertheless,
the choice was made to use the option 1 due to practical
considerations when setting up the racing track.
H. Simulation
The implementation of the method described above has
been done on the ODROID C2 using the C++ language and
OpenCV v3.2 library. The simulation recreated the perception
fo the hoop dependent on the specified distance and angle to
the object. The results are presented in Fig. 1b, where the blue
circle is a representation of the template and the white ellipse
is the perceived object. The results are comparable with the
captured video stream in Fig. 1a.
I. Experiment
Furthermore, an experiment was conducted in order to
determine the accuracy of the hoop detection described in
[15] and pose estimation in this paper. The reference videos
were taken from various distances and orientations of the hoop.
The experiment focused on the discrepancy in detection of the
rotation angle around the vertical axis and distance by locating
the camera on the same height as the center of the hoop. The
results are presented in the Table I.
Here, the mean of the error in distance and angle over all
videos taken from various angles and distances respectively,
is shown together with the variance of the values. The mean
of the average discrepancy for the distance remains below
6 cm and 5 degrees for the angle. Taking into account the
measurement errors, it is possible to state that both values have
sufficient accuracy for consideration of the visual sensing data
for navigation purposes.
The variance, in turn, shows that more filtering is to be
applied since when the drone is located more than 5 meters
away from the hoop and oriented more than 30 degrees from
the opening of the hoop, the pose estimation for each frame
obtained from the camera cannot be trusted and has to be
averaged over few samples. An alternative option would be
to create a filter to disregard data that is not reasonable when
checking with past samples and velocity over this period.
III. T RAJECTORY PLANNING
This section describes the approach for the trajectory plan-
ning of a drone that is to be used as a navigation system in
autonomous drone races. The required output of the trajectory
planning system is the position and velocity in Cartesian
coordinate frame at each time instant till the destination (1
meter behind the hoop), given the state of the drone and the
relative position to the hoop.
A. Related work
There are many methods to design a trajectory for a body
given specifications on initial and final conditions. The main
difference between them is the way to describe the position
throughout the path. In [23], the expressions for the motion
profile using polynomials of various orders, trigonometric
functions, exponentials and Fourier expansion are explained.
Each of these have their own advantages for specific applica-
tions:
Polynomial description of the trajectory is the simplest to
derive and it produces a continuous and smooth motion.
Trigonometric trajectories are better for harmonic mo-
tions, but introduce discontinuities in position derivatives.
Exponential path description introduces better control of
smoothness while requiring high acceleration magnitudes.
Fourier series is the best for finding the balance between
the latter two in the cost of more complex computations.
For such a highly dynamic vehicle as a quadcopter with
limited computational capabilities, the choice of polynomial
description of the path is natural, since the trajectories are
obtained efficiently as a solution to a quadratic programming
(QP) problem to minimize the cost function for the path
derivatives [24], [23] [25]. This approach allows the motion
parameters at the endpoints of path segments to be optionally
fixed to desired values or left free, and the polynomials
can be jointly optimized while maintaining continuity of the
derivatives up to an arbitrary order. The continuity of the
derivatives, therefore, ensures the smoothness of the trajectory
and can be used to achieve a path optimized for minimum time
[23], minimum snap [25] or shortest distance trajectories [26].
B. Description of the path
The application in the context of which the research is done
requires the drone to go through a series of hoops possessing
only information about the relative pose of only the next hoop
(determined in section II) on the track and the current state
description of the drone.
For the current application, it is desired to achieve contin-
uous acceleration profile for smooth motion. In practice, this
also means that the snap over the whole path is expressed as
a constant so the jerk throughout the trajectory is minimized
with the following optimization problem for 1 dimension,
given the endpoints parameters x(0) = p1 and x(tf ) = p2 :
Z tf
...
x (t) = argmin L( x, x, x, x, t)dt, (7)
x(t) 0
...
where x, x, x and x are position, velocity, acceleration
and jerk. The optimization problem results in a 5th order
polynomial that describes the position (equation 8) of the
controlled object at each instant of time t on a segment [0; tf ]
requiring 6 boundary conditions for unique solution.
x = a5 t5 + a4 t4 + a3 t3 + a2 t2 + a1 t + a0 (8)
Nevertheless, the L in (7) can also be described by any
higher order polynomial, since the only limitation is that
the solution presented in (8) has to be non-zero for t > 0
(otherwise, there is no displacement). On the other hand, in
case the order of the polynomial is higher than the amount
of numeric conditions for the path, the resulting optimization
problem would still have a solution, but the position is not
 5
Path through the hoop with 3 waypoints

symmetric through the time period. This leads to higher

maximum acceleration and larger peaks in velocity over the 3

segment of the path. 2

Z position
For the drone navigation system it was chosen to use a 5th 1

order polynomial with 6 endpoint conditions for the descrip- 0

tion of position throughout the path for smooth movement of -1 3

the drone, which is desirable for the application. In addition, 3

2
2
1
1
as described later in section III-D, the path planning algorithm 0 0

Y position -1 -1 X position
estimates the thrusts that the motors produce over the whole
path. However, as mentioned in [15], the dynamic model of Fig. 2: Generated trajectory with waypoints w
~ 1, w
~ 2 and w
~ 3.
the drone is highly non-linear and a higher order polynomial
the next hoop. The state of the drone at this point also serves
would reduce the performance by increasing the required
as initial conditions (position, velocity and acceleration) for
computational time for trajectory generation.
T the segment w23 with the w ~ 3 located 1 meter perpendicularly
The coefficients ~a = a5 . . . a0 to describe the move-
behind the hoop.
ment from point to point can be found by solving a system of
In order to satisfy the requirement on constant velocity when
linear equations (9) while specifying velocity and acceleration
passing through a hoop, the endpoint conditions of w12 are
in addition to position as path conditions. This ensures that
set to have the same customly specified value for the velocity
the drone comes to the desired location with the predefined
(perpendicular to the opening of the hoop) and acceleration,
velocity and acceleration required by the application.
while the latter additionally has to be equal to 0.
0 0 0 0 0 1
0 As the drone has passed the hoop the process repeats, and
0 0 0 1 0

a5 w
~ 3 becomes the initial condition for the next part of the track.
0 0 0 2 0 0 .
~p12 = t5 t 4
t3
t2
t 1
.. , (9) D. Feasible trajectory
f

f4 f f f Due to the fact that the quadrotor is a real physical system,
4tf 3tf 2tf 1 0 a0
3 2

5tf
it has limitations on acceleration caused by the specifications
20t3f 12t2f 6tf 2 0 0 of its motors (Emax RS2205 2300KV drone motors with 5045
 T
where ~p12 = p~1 p~2 is a vector describing the initial propellers are able to produce thrust to lift a maximum mass of
and final position, velocity and acceleration on a segment of 938 g each, while DYS BE2208-13 1400KV motors with 6050
a path at t = 0 and t = tf . propellers can pull only 640 g each - both kinds of motors were
As a result, the description of the path in each dimension at used for the prototypes of the customly made drone for the
each time instant t is independent and has the following form: application). Therefore, checking if the suggested trajectory
5
t t4 t3 t2 t 1 a5
can be executed by the system is of crucial importance.
First, the dynamic model is derived and the thrusts produced
~p = 5t4 4t3 3t2 2t 1 0 ... , (10)

by each rotor are evaluated for each instant of time in [15]
20t3 12t2 6t 2 0 0 a0
using the coefficients for a segment of path (described in
where ~p is the vector with position, velocity and acceleration section III-B). In case the thrust is exceeding the predefined
at a given time instant t. limit, the path coefficient vector ~a is recalculated by increasing
C. Trajectory generation to pass through the hoops the total time tf for its completion. This is performed with
The path produced as a result of the optimization problem bisection search in 2 steps. First, the time is continuously
in (7) ensures only continuous point-to-point movement, while multiplied by a factor of 2 until the algorithm produces a
for the racing application it is also desirable for the drone to dynamically feasible solution. As the feasible trajectory is
always face the hoop in a certain range from it (5 meters) and found, the bisection search method is implemented in order to
pass the hoop perpendicularly to its opening with a constant find a more optimal solution by iterating on the range between
speed. Therefore, additional waypoints on the trajectory have the last non-feasible and feasible values for t.
to be separately defined and the yaw angle of the drone has It is clear that this method may run infinitely long searching
to be computed throughout the path. While the yaw planner for the best solution, therefore, a limitation on the amount of
is described in more detail in [15], this paper focuses on the performed iterations has to be set. This introduces at least one
solution for the trajectory with intermediate waypoints. feasible path by the end of the computation which is sub-
As it has been already mentioned, the drone in the racing optimal for the drone.
application has to go through hoops, therefore, the destination E. Simulation
endpoint position cannot be defined by the center of a hoop. The described algorithm is implemented in MATLAB en-
The approach taken in this paper lies in subdividing the path vironment. Fig. 2 illustrates the generated by the navigation
between the hoops into 2 segments with 3 waypoints defined system trajectory for the center of the drone passing through
by a 5th order polynomial: points w ~ 1, w~ 2 and w~ 3 forming the 3 mentioned waypoints.
segments w12 and w23 with w ~ 2 being the destination of w12 IV. O BJECT AVOIDANCE
and departure point of w23 . More specifically, it is chosen This section describes the extended case for the trajectory
to locate w ~ 2 1 meter (the distance at which the detection planning which includes path readjustment for collision avoid-
algorithm developed in [15] is still able to perceive the whole ance and is an extension to the research. The classification for
contour of the hoop) perpendicularly in front of the opening of collision is preformed by [15] and this paper discusses the
complications of the optimization problem in section III-B.
6
A. Related work
Collision avoidance is a well researched topic and many
solutions for the problem can be found. The focus in the
research can be subdivided into two groups. The first set of the
algorithms define the optimal path knowing the environment
(occupancy grid, D*, A*, Rapidly-exploring Random Tree
(RRT) methods [27]), while other concern operations in an
unknown surrounding (simple automata [27], temporary close
range map navigation methods [28]).
The map-based obstacle avoidance has a huge variety of
approaches and [27] mentions a few of them. One approach is
to describe the obstacles as an array of polygons with their own
vertices and edges, and determine the trajectory by analyzing
all possible collisions. Another is to represent a map as an
occupancy grid for more efficient analysis. Here, each cell of
the grid can be either occupied by the obstacle or not, while
the free cells additionally have a distance to the destination
specified. The algorithm runs through the neighboring cells
and directs the robot on a path with the smallest distance.
Similarly, the D* method is evaluating the map with the travel
parameters as time, distance or margin from the obstacles,
assigning cost to each specification and optimizing the path
accordingly. Finally, a widely researched RRT algorithm [29]
searches for the possible configurations for each time period
starting from the current position of a robot by expanding on
a fixed distance from the feasible point in a random direction.
It should be noted that the obstacle avoidance methods using
the available map, are generally computationally expensive due
to the map analysis and interpretation, however, generally pro-
duce better results for optimizations over various parameters.
In the simple automata method described in [27], a con-
trolled system follows a straight line between the endpoints
and when the obstacle is detected, the path is changed to move
around the obstacle (always in a clock-wise direction) until the
robot reaches a point along the original line. This method is
not suitable for the algorithm described in section III since
in order to move around the obstacle, multiple intermediate
waypoints are needed, which is computationally expensive
with the polynomial trajectory planning approach. Another
approach under conditions of the absence of the map reaches
the destination by trial and error [28]. Namely, using ultrasonic
and infrared sensors a robot detects an obstacle in front of it
and attempts to move around the obstacle, creating a temporary
map of the nearby environment. The disadvantage of this
method is obvious for high speed racing applications.
In this paper, the only information available to the path
planner is the location and the size of the obstacle together
with the final destination. The approach suggested in this
paper, therefore, considers a map where all the locations are
feasible except of the space taken by a convex obstacle.
B. Obstacle avoidance point
The obstacle avoidance in this paper comes as an extension
to the polynomial path planning following specified waypoints.
Therefore, a logical method for obstacle avoidance would be
finding an intermediate point to omit an object while minimiz-
ing the total distance traveled. For this algorithm, an object is
represented by a sphere or a cube due to the fact that the space
taken by most objects can be efficiently approximated by these
shapes. [15] describes obstacle detection and pose estimation
for a cardboard box when the suggested in this paper algorithm
may be used for a drone to avoid it.
The methodology to determine the coordinates of the in-
termediate point for avoidance applied in this algorithm is
explained in Appendix B.
It is important to note, that the described approach identifies
only one of the endpoint parameters position. It is also
possible to specify velocity that the drone has to have while
passing through the intermediate point for obstacle avoidance
in order to have more control throughout the path. The speed
can be chosen arbitrarily and for this application it is set to
1 cell per time instant. The velocity vector at the avoidance
waypoint has to point in the direction towards the next point on
the path 1 meter before the hoop for the current application.
C. Waypoints parameters specification
The new waypoint on the path does not have enough
conditions (there is no specification for acceleration at the
avoidance point) to solve the optimization problem for 5th
order polynomial in a fast and efficient fashion as described in
section III. Therefore, the implemented approach identifies the
optimization problem with maximum available constraints and
switches between the 4th and 5th order polynomial definition
for the position of the drone when avoiding an obstacle or
following a straight path, respectively. For the avoidance case,
the known information still includes the 3 parameters to define
the initial condition (position, velocity and acceleration), but
only 2 parameters for the final condition (position and velocity,
where velocity is directed towards the next specified waypoint
and is manually described as a constant).
Avoidance of two objects is explained in Appendix C.
D. Simulation
The described above algorithm was built in MATLAB
environment for both sphere and cube obstacles. The results of
the trajectory are presented on Fig. 3 and 4 with a drone guided
around the obstacles through the extra avoidance waypoint.
V. C ONCLUSIONS
In this paper methods for pose estimation, path plan-
ning and object avoidance are researched. Furthermore, an
implementation of these functionalities is described in an
autonomous drone racing context. The algorithm for pose
estimation is based on template matching with the template
being a circle that represents the hoop from a perpendicular
view. The accuracy of the resultant algorithm is up to 6
cm when estimating the distance and up to 5 degrees for
rotation angle around horizontal axis. In turn, the trajectory
planner used an approach of 5th order polynomial description
of the position throughout the path accounting on endpoints
conditions on position, velocity and acceleration and included
evaluation of the dynamic limitations of the motors together
with bisection search for optimization of the time needed to
create a suboptimal trajectory. Finally, the case was extended
by the object avoidance algorithm that creates an additional
intermediate point on the path to omit the obstacle with 4th
degree polynomial description of the position disregarding the
exact specification on acceleration at that point. The mentioned
algorithms are implemented in C++ and MATLAB, where the
experiments showed sufficient results for further integration of
the visual navigation system in the racing drones.
 7

Avoidance of a sphere

6
1) Identify the intersection points of the original trajectory
5 with the surface of the obstacle.
4
2) Find a point in the middle between the intersections.
3
3) Find a vector describing the closest distance from the
middle point of the intersections to the surface of the
Z position

2
obstacle (nearest edge for the cube-shaped obstacle).
1
4) Locate an intermediate waypoint in the direction of the
0 found vector with a specified offset.
-1 5) Check if the trajectory connecting the initial position,
-2
intermediate waypoint and the destination does not in-
-2
0
2
4 2 4 6
tersect the obstacle. In case it does, increase the offset.
6 -2 0
X position Y position
A PPENDIX C
Fig. 3: Avoidance of a sphere.
Avoidance of a cube AVOIDANCE OF 2 OBJECTS
6
The avoidance of one object that is described in the main
5
part of the paper has been further extended to the case with
4
two objects on the path.
3
When avoiding two objects, the generation of the trajectory
is similar to a task of avoiding one obstacle. For the two
Z position

1
objects, it is still chosen to generate an intermediate point with
0
position and velocity constraints for each obstacle, however,
the difference is that one intermediate point might already suf-
-1
fice the condition for avoidance. Therefore, the implemented
-2
-2
0
2 6
method first derives an avoidance point for the first obstacle
4
4 2

X position
6 -2
0
as described in Appendix B and then checks again if a path
Y position
from that point to the end destination intersects the second
Fig. 4: Avoidance of a cube. obstacle. In case it does, an extra intermediate point is added.
The results are shown on Fig. 6, 7 and 8. Fig. 6 depicts
A PPENDIX A
the condition where one intermediate point is enough for
P ROOF OF DECOUPLED DISTANCE AND ORIENTATION
avoidance of two obstacles, while Fig. 7 illustrates the path
ANGLES FOR POSE ESTIMATION
with 2 intermediate avoidance points. Fig. 8, in turn, proves
Consider a hoop to be a cross section of an empty sphere A
that the algorithm is capable of generating a safe path in
and a point of observation P = P1 , which lie on the surface of
between two objects.
another sphere B. The center of B is aligned with the center
of sphere A. The cross section of A forms a circle C on the
plane. By connecting P with all the points on circle C, a cone
6
is formed, which represents the light coming to the camera
5
from the contour of the hoop. When projecting the view of C
4
from P onto a plane, an ellipse is formed with the semimajor
3
axis equal to the radius of the cone (Fig. 5a), where the radius
Z position

2
is defined as half of the distance between the 2 unique points
1
on the circle, to which the distance from P is the same.
0
If the position of P = P2 on sphere B is changed, the major
Y position

-1
6
axis of the ellipse obtained from the projection of the circle 2
4

-2 0
is the same except for the rotation of the ellipse around its -2 -1 0 1 2 3 4
X position
5 6 7
-2

center (Fig. 5b).

A PPENDIX B Fig. 6: Avoidance of two objects with 1 intermediate point.
D ERIVATION OF THE AVOIDANCE POINT
In order to obtain the coordinates of the avoidance point,
the following steps were taken assuming a sphere or a cube
as an obstacle:
6

4
Z position

6
2

0
2

0
-2

(a) (b) -2 -1 0 1 2 3 4 5
-2 Y position
6 7
X position

Fig. 5: Perception of the circle from P1 (a) and P2 (b)

Fig. 7: Avoidance of two objects with 2 intermediate points.
8
6
4
Z position
2
0
-2
6 tude control of an unmanned quadrotor helicopter considering actuator
4
2
0 autonomous drones, BSc thesis, Eidhoven University of Technology,
Y position -2
4 5
3
-2 -1 0 1 2
X position
Fig. 8: Avoidance of two objects with 2 intermediate points.
R EFERENCES
[1] M. Coskun and S. Unal, Implementation of Tracking of a Moving
Object Based on Camshift Approach with a UAV, Procedia Technology,
vol. 22, pp. 556561, 2016.
[2] S. A. P. Quintero and J. P. Hespanha, Vision-based target tracking with a
small UAV: Optimization-based control strategies, Control Engineering
Practice, vol. 32, pp. 2842, 2014.
[3] I. Jawhar, N. Mohamed, J. Al-Jaroodi, D. P. Agrawal, and S. Zhang,
Communication and networking of UAV-based systems: Classification
and associated architectures, Journal of Network and Computer Appli-
cations, vol. 84, pp. 93108, 2017.
[4] A. Mahmood and Y. Kim, Leader-following formation control of
quadcopters with heading synchronization, Aerospace Science and
Technology, vol. 47, pp. 6874, 2015.
[5] A. Aguiar, I. Kaminer, R. Ghabcheloo, A. Pascoal, E. Xargay, N. Ho-
vakimyan, C. Cao, and V. Dobrokhodov, Coordinated Path Follow-
ing of Multiple UAVs for Time-Critical Missions in the Presence of
Time-Varying Communication Topologies, IFAC Proceedings Volumes,
vol. 41, no. 2, pp. 16 01516 020, 2008.
[6] M. W. Mueller, M. Hehn, and R. DAndrea, A computationally ef-
ficient algorithm for state-to-state quadrocopter trajectory generation
and feasibility verification, in Proc. of the International Conference
on Intelligent Robots and Systems, 2013, pp. 34803486.
[7] , A computationally efficient motion primitive for quadrocopter
trajectory generation, IEEE Transactions on Robotics, vol. 31, no. 6,
pp. 12941310, 2015.
[8] E. Bylow, J. Sturm, C. Kerl, F. Kahl, and D. Cremers, Real-Time Cam-
era Tracking and 3D Reconstruction Using Signed Distance Functions,
Robotics: Science and Systems, vol. 9, pp. 238246, 2013.
[9] P. Henry, M. Krainin, E. Herbst, X. Ren, and D. Fox, RGB-D Mapping:
Using Depth Cameras for Dense 3D Modeling of Indoor Environments,
Proc. of the 12th International Symposium on Experimental Robotics,
pp. 477491, 2014.
[10] F. Fraundorfer, L. Heng, D. Honegger, G. H. Lee, L. Meier, P. Tanskanen,
and M. Pollefeys, Vision-based autonomous mapping and exploration
using a quadrotor MAV, in Proc. of the International Conference on
Intelligent Robots and Systems, Oct 2012, pp. 45574564.
[11] M. C. Santos, L. V. Santana, A. S. Brandao, M. Sarcinelli-Filho, and
R. Carelli, Indoor low-cost localization system for controlling aerial
robots, Control Engineering Practice, vol. 61, pp. 93111, 2017.
[12] M. J. Landau and P. A. Beling, Optimal Model-Based 6-D Object Pose
Estimation With Structured-Light Depth Sensors, IEEE Transactions on
Computational Imaging, vol. 3, pp. 5873, 2017.
[13] S. Grzonka, G. Grisetti, and W. Burgard, Towards a navigation system
for autonomous indoor flying, in Proc. of the IEEE International
Conference on Robotics and Automation, May 2009, pp. 28782883.
[14] B. Wang, K. A. Ghamry, and Y. Zhang, Trajectory tracking and atti-
dynamics, in Proc. of the 35th Chinese Control Conference, 2016, pp.
10 79510 800.
[15] P. Nagy, Visual object recognition, tracking and path planning for
6 7 Jun 2017.
[16] OpenCV 3.2. Accessed: 2017-04-13. [Online]. Available:
http://opencv.org/opencv-3-2.html
[17] S. Garcia, M. E. Lopez, R. Barea, L. M. Bergasa, A. Gomez, and
E. J. Molinos, Indoor SLAM for Micro Aerial Vehicles Control Using
Monocular Camera and Sensor Fusion, in Proc. of the International
Conference on Autonomous Robot Systems and Competitions, 2016, pp.
205210.
[18] S. Chen, L. Liang, W. Liang, and H. Foroosh, 3D Pose Tracking With
Multitemplate Warping and SIFT Correspondences, IEEE Transactions
on Circuits and Systems for Video Technology, vol. 26, no. 11, pp. 2043
2055, 2016.
[19] A. Berner, J. Li, D. Holz, J. Stckler, S. Behnke, and R. Klein, Combin-
ing contour and shape primitives for object detection and pose estimation
of prefabricated parts, in Proc. of the IEEE International Conference
on Image Processing, 2013, pp. 33263330.
[20] R. Kouskouridas, A. Tejani, A. Doumanoglou, D. Tang, and T. K. Kim,
Latent-Class Hough Forests for 6 DoF Object Pose Estimation, IEEE
Transactions on Pattern Analysis and Machine Intelligence, in print,
2017.
[21] E. Munoz, Y. Konishi, C. Beltran, V. Murino, and A. D. Bue, Fast 6D
pose from a single RGB image using Cascaded Forests Templates, in
Proc. of the International Conference on Intelligent Robots and Systems,
2016, pp. 40624069.
[22] H. Y. Tseng, P. C. Wu, M. H. Yang, and S. Y. Chien, Direct 3D pose
estimation of a planar target, in Proc. of the IEEE Winter Conference
on Applications of Computer Vision, 2016, pp. 19.
[23] L. Biagiotti and C. Melchiorri, Trajectory Planning for Automatic
Machines and Robots. Springer, 2008.
[24] C. Richter, A. Bry, and N. Roy, Polynomial trajectory planning for
aggressive quadrotor flight in dense indoor environments, in Proc. of
the International Symposium of Robotics Research, 2013, pp. 649666.
[25] D. Mellinger and V. Kumar, Minimum snap trajectory generation and
control for quadrotors, in Proc. of the IEEE International Conference
on Robotics and Automation, 2011, pp. 25202525.
[26] M. Hehn and R. DAndrea, Real-time trajectory generation for quadro-
copters, IEEE Transactions on Robotics, vol. 31, no. 4, pp. 877892,
2015.
[27] P. Corke, Robotics, Vision and Control - Fundamental Algorithms in
MATLAB. Springer, 2011, vol. 73.
[28] A. M. Alajlan, M. M. Almasri, and K. M. Elleithy, Multi-sensor based
collision avoidance algorithm for mobile robot, in 2015 Long Island
Systems, Applications and Technology, 2015, pp. 16.
[29] O. Salzman and D. Halperin, Asymptotically near-optimal rrt for fast,
high-quality motion planning, IEEE Transactions on Robotics, vol. 32,
no. 3, pp. 473483, 2016.