Documente Academic
Documente Profesional
Documente Cultură
accurate pose estimation) is delivered from the PixHawk to the from continuous video stream are compared with a set of
ODROID and trajectory setpoints are fed in opposite direction. primitive shapes. This is an optimal solution to determine the
II. P OSE ESTIMATION relative pose of a drone from the target for the considered
This section describes implementation of the pose estima- application, since there is only one predefined object that is to
tion given a filtered image of a perceived hoop with LED be searched for - the hoop that the drone has to fly through,
attached to it that the drone has an objective to fly through. which can be represented by a primitive geometric 2D shape
A. Related work as an ellipse from any point of observation, so no automated
There are multiple approaches to tackle the problem of template generation as in [18], [20] and [21] is needed.
the relative position and orientation estimation of an object B. Assumptions
with respect to the camera. In general, the methods can be In order to simplify the task of pose estimation and optimize
subdivided into two main groups: creation of a map around the solution for a specific given application, there are few
the observer and template matching. assumptions to make:
Mapping of the location can be done by Simultaneous The hoop has shape of a perfect circle and is recognized
Localization and Mapping (SLAM) technique using various by the camera as a circle or an ellipse depending on the
sensors: RGB-D [8] [9] [17], lasers [13] or a camera [10]. relative pose of the visual system [15].
The essence of the environment mapping lies in comparing the The input to the pose estimation algorithm are the pa-
pose of the drone relative to the surfaces of the objects in its rameters of an ellipse that represents the perceived shape
environment using the mentioned sensors and reconstructing a of the hoop from an observation point, including length
3D map from this data. There are great results achieved, par- of the semi-major and semi-minor axes, tilt of the major
ticularly in [8] and [10], however, the visual SLAM method is axis with respect to the vertical axis on a 2D plane.
not efficient for the current application since it puts limitations The altitude of the camera and the hoop relative to the
on the maximum speed at which a drone can move due to the physical ground are known.
actual construction of the map around it and various artifacts A camera is always facing the hoop and can be located
that cause inaccuracy (e.g. inability of rotation tracking after only in one half space formed by the plane of the opening
turns, scale variations and scale ambiguity [17]). of the hoop.
Another approach to perform pose estimation is template Rotation of the camera around the direction towards the
matching. The essence of template matching lies in searching center of the hoop is known.
for predetermined shapes that can be fitted onto the contour C. Camera calibration
of a target object in a captured image and then based on the The calibration of the camera relates to the calculation
perception of the shape, determining the relative orientation of the focal length of the camera. This is crucial for the
and distance. This method in its simplistic form (a more distance estimation since the focal point, where the light rays
advanced algorithm by [18] was able to track an object without converge and form image, of each camera is different causing
any prior calibration or knowledge of focal length of the objects being perceived differently at the same distance. For
camera) is only possible under conditions of knowing the size the current application it can be computed as follows:
and visual perception of the objects by the camera that are to Pradius D
F = , (1)
be distinguished beforehand, so possessing a template. R
Template matching can be implemented using multi- or where F is the focal length, R is radius of the hoop in meters,
mono-view methods, depending on the amount of vision D is the distance at which the reference shot is taken and
sensors and their locations. However, in the current application Pradius is the representation of the radius of the hoop in
it is desired to have all computations and sensors on-board, pixels in a captured frame. For the webcamera used for the
which limits the possible options to one camera on the drone. experiments in this paper, this parameter is equal to 730 with
In [19], predefined contour and shape primitives have been Pradius = 256 pixels, D = 1 m and R = 0.35 cm.
used to detect and estimate the exact pose of an object using D. Distance estimation
a monocular camera system. Here, the non-primitive objects First of all, it can be proven (Appendix A) that the mag-
are decomposed in an array of point clouds describing the nitude of the major axis of the detected ellipse is constant at
primitive 2D and 3D forms like circles, cylinders, lines, cubes. any angle of observation, considering the same distance from
By fusing the ones having common edges, the algorithm the camera to the center of the hoop under assumption that the
is able to reconstruct the shape of the object (shape-graph hoop is a perfect circle. Therefore, the distance for the current
matching). In turn, [18], [20] and [21] use Hough, Latent-Class application is decoupled from the orientation. Furthermore,
Hough and Cascaded template forest methods and generate the distance estimation is also independent from the orientation of
template by a training algorithm that collects picture samples the drone. The only requirement for accuracy of the estimation
of an object at different conditions. Later, this template again is the presence of sufficient area of the hoop on the captured
serves as a reference for matching. A similar idea is described frame so that the detected and appropriately rotated ellipse
in [22], where the pose estimation algorithm is optimized for corresponds to the one detected if the center of the hoop is
detection of objects under various conditions given their planar aligned with the center of the frame [15].
photo representation as a template. In order to determine the distance to the object (hoop), the
For the current application, it has been decided to use an semi-major axis of the ellipse in the captured photo frame
approach similar to [19] where all contours in captured frames is compared to the ellipse (circle) in the reference frame.
3
Z position
For the drone navigation system it was chosen to use a 5th 1
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 shapes. [15] describes obstacle detection and pose estimation
Collision avoidance is a well researched topic and many for a cardboard box when the suggested in this paper algorithm
solutions for the problem can be found. The focus in the may be used for a drone to avoid it.
research can be subdivided into two groups. The first set of the The methodology to determine the coordinates of the in-
algorithms define the optimal path knowing the environment termediate point for avoidance applied in this algorithm is
(occupancy grid, D*, A*, Rapidly-exploring Random Tree explained in Appendix B.
(RRT) methods [27]), while other concern operations in an It is important to note, that the described approach identifies
unknown surrounding (simple automata [27], temporary close only one of the endpoint parameters position. It is also
range map navigation methods [28]). possible to specify velocity that the drone has to have while
The map-based obstacle avoidance has a huge variety of passing through the intermediate point for obstacle avoidance
approaches and [27] mentions a few of them. One approach is in order to have more control throughout the path. The speed
to describe the obstacles as an array of polygons with their own can be chosen arbitrarily and for this application it is set to
vertices and edges, and determine the trajectory by analyzing 1 cell per time instant. The velocity vector at the avoidance
all possible collisions. Another is to represent a map as an waypoint has to point in the direction towards the next point on
occupancy grid for more efficient analysis. Here, each cell of the path 1 meter before the hoop for the current application.
the grid can be either occupied by the obstacle or not, while C. Waypoints parameters specification
the free cells additionally have a distance to the destination The new waypoint on the path does not have enough
specified. The algorithm runs through the neighboring cells conditions (there is no specification for acceleration at the
and directs the robot on a path with the smallest distance. avoidance point) to solve the optimization problem for 5th
Similarly, the D* method is evaluating the map with the travel order polynomial in a fast and efficient fashion as described in
parameters as time, distance or margin from the obstacles, section III. Therefore, the implemented approach identifies the
assigning cost to each specification and optimizing the path optimization problem with maximum available constraints and
accordingly. Finally, a widely researched RRT algorithm [29] switches between the 4th and 5th order polynomial definition
searches for the possible configurations for each time period for the position of the drone when avoiding an obstacle or
starting from the current position of a robot by expanding on following a straight path, respectively. For the avoidance case,
a fixed distance from the feasible point in a random direction. the known information still includes the 3 parameters to define
It should be noted that the obstacle avoidance methods using the initial condition (position, velocity and acceleration), but
the available map, are generally computationally expensive due only 2 parameters for the final condition (position and velocity,
to the map analysis and interpretation, however, generally pro- where velocity is directed towards the next specified waypoint
duce better results for optimizations over various parameters. and is manually described as a constant).
In the simple automata method described in [27], a con- Avoidance of two objects is explained in Appendix C.
trolled system follows a straight line between the endpoints D. Simulation
and when the obstacle is detected, the path is changed to move The described above algorithm was built in MATLAB
around the obstacle (always in a clock-wise direction) until the environment for both sphere and cube obstacles. The results of
robot reaches a point along the original line. This method is the trajectory are presented on Fig. 3 and 4 with a drone guided
not suitable for the algorithm described in section III since around the obstacles through the extra avoidance waypoint.
in order to move around the obstacle, multiple intermediate V. C ONCLUSIONS
waypoints are needed, which is computationally expensive In this paper methods for pose estimation, path plan-
with the polynomial trajectory planning approach. Another ning and object avoidance are researched. Furthermore, an
approach under conditions of the absence of the map reaches implementation of these functionalities is described in an
the destination by trial and error [28]. Namely, using ultrasonic autonomous drone racing context. The algorithm for pose
and infrared sensors a robot detects an obstacle in front of it estimation is based on template matching with the template
and attempts to move around the obstacle, creating a temporary being a circle that represents the hoop from a perpendicular
map of the nearby environment. The disadvantage of this view. The accuracy of the resultant algorithm is up to 6
method is obvious for high speed racing applications. cm when estimating the distance and up to 5 degrees for
In this paper, the only information available to the path rotation angle around horizontal axis. In turn, the trajectory
planner is the location and the size of the obstacle together planner used an approach of 5th order polynomial description
with the final destination. The approach suggested in this of the position throughout the path accounting on endpoints
paper, therefore, considers a map where all the locations are conditions on position, velocity and acceleration and included
feasible except of the space taken by a convex obstacle. evaluation of the dynamic limitations of the motors together
B. Obstacle avoidance point with bisection search for optimization of the time needed to
The obstacle avoidance in this paper comes as an extension create a suboptimal trajectory. Finally, the case was extended
to the polynomial path planning following specified waypoints. by the object avoidance algorithm that creates an additional
Therefore, a logical method for obstacle avoidance would be intermediate point on the path to omit the obstacle with 4th
finding an intermediate point to omit an object while minimiz- degree polynomial description of the position disregarding the
ing the total distance traveled. For this algorithm, an object is exact specification on acceleration at that point. The mentioned
represented by a sphere or a cube due to the fact that the space algorithms are implemented in C++ and MATLAB, where the
taken by most objects can be efficiently approximated by these 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
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
6
[12] M. J. Landau and P. A. Beling, Optimal Model-Based 6-D Object Pose
Estimation With Structured-Light Depth Sensors, IEEE Transactions on
4
Computational Imaging, vol. 3, pp. 5873, 2017.
[13] S. Grzonka, G. Grisetti, and W. Burgard, Towards a navigation system
Z position
2
for autonomous indoor flying, in Proc. of the IEEE International
0
Conference on Robotics and Automation, May 2009, pp. 28782883.
-2
[14] B. Wang, K. A. Ghamry, and Y. Zhang, Trajectory tracking and atti-
6 tude control of an unmanned quadrotor helicopter considering actuator
4
dynamics, in Proc. of the 35th Chinese Control Conference, 2016, pp.
10 79510 800.
2
[15] P. Nagy, Visual object recognition, tracking and path planning for
0 autonomous drones, BSc thesis, Eidhoven University of Technology,
Y position -2
4 5 6 7 Jun 2017.
3
-2 -1 0 1 2
X position
[16] OpenCV 3.2. Accessed: 2017-04-13. [Online]. Available:
http://opencv.org/opencv-3-2.html
Fig. 8: Avoidance of two objects with 2 intermediate points. [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
R EFERENCES Monocular Camera and Sensor Fusion, in Proc. of the International
Conference on Autonomous Robot Systems and Competitions, 2016, pp.
[1] M. Coskun and S. Unal, Implementation of Tracking of a Moving 205210.
Object Based on Camshift Approach with a UAV, Procedia Technology, [18] S. Chen, L. Liang, W. Liang, and H. Foroosh, 3D Pose Tracking With
vol. 22, pp. 556561, 2016. Multitemplate Warping and SIFT Correspondences, IEEE Transactions
[2] S. A. P. Quintero and J. P. Hespanha, Vision-based target tracking with a on Circuits and Systems for Video Technology, vol. 26, no. 11, pp. 2043
small UAV: Optimization-based control strategies, Control Engineering 2055, 2016.
Practice, vol. 32, pp. 2842, 2014. [19] A. Berner, J. Li, D. Holz, J. Stckler, S. Behnke, and R. Klein, Combin-
[3] I. Jawhar, N. Mohamed, J. Al-Jaroodi, D. P. Agrawal, and S. Zhang, ing contour and shape primitives for object detection and pose estimation
Communication and networking of UAV-based systems: Classification of prefabricated parts, in Proc. of the IEEE International Conference
and associated architectures, Journal of Network and Computer Appli- on Image Processing, 2013, pp. 33263330.
cations, vol. 84, pp. 93108, 2017. [20] R. Kouskouridas, A. Tejani, A. Doumanoglou, D. Tang, and T. K. Kim,
[4] A. Mahmood and Y. Kim, Leader-following formation control of Latent-Class Hough Forests for 6 DoF Object Pose Estimation, IEEE
quadcopters with heading synchronization, Aerospace Science and Transactions on Pattern Analysis and Machine Intelligence, in print,
Technology, vol. 47, pp. 6874, 2015. 2017.
[5] A. Aguiar, I. Kaminer, R. Ghabcheloo, A. Pascoal, E. Xargay, N. Ho- [21] E. Munoz, Y. Konishi, C. Beltran, V. Murino, and A. D. Bue, Fast 6D
vakimyan, C. Cao, and V. Dobrokhodov, Coordinated Path Follow- pose from a single RGB image using Cascaded Forests Templates, in
ing of Multiple UAVs for Time-Critical Missions in the Presence of Proc. of the International Conference on Intelligent Robots and Systems,
Time-Varying Communication Topologies, IFAC Proceedings Volumes, 2016, pp. 40624069.
vol. 41, no. 2, pp. 16 01516 020, 2008. [22] H. Y. Tseng, P. C. Wu, M. H. Yang, and S. Y. Chien, Direct 3D pose
[6] M. W. Mueller, M. Hehn, and R. DAndrea, A computationally ef- estimation of a planar target, in Proc. of the IEEE Winter Conference
ficient algorithm for state-to-state quadrocopter trajectory generation on Applications of Computer Vision, 2016, pp. 19.
and feasibility verification, in Proc. of the International Conference [23] L. Biagiotti and C. Melchiorri, Trajectory Planning for Automatic
on Intelligent Robots and Systems, 2013, pp. 34803486. Machines and Robots. Springer, 2008.
[7] , A computationally efficient motion primitive for quadrocopter [24] C. Richter, A. Bry, and N. Roy, Polynomial trajectory planning for
trajectory generation, IEEE Transactions on Robotics, vol. 31, no. 6, aggressive quadrotor flight in dense indoor environments, in Proc. of
pp. 12941310, 2015. the International Symposium of Robotics Research, 2013, pp. 649666.
[8] E. Bylow, J. Sturm, C. Kerl, F. Kahl, and D. Cremers, Real-Time Cam- [25] D. Mellinger and V. Kumar, Minimum snap trajectory generation and
era Tracking and 3D Reconstruction Using Signed Distance Functions, control for quadrotors, in Proc. of the IEEE International Conference
Robotics: Science and Systems, vol. 9, pp. 238246, 2013. on Robotics and Automation, 2011, pp. 25202525.
[9] P. Henry, M. Krainin, E. Herbst, X. Ren, and D. Fox, RGB-D Mapping: [26] M. Hehn and R. DAndrea, Real-time trajectory generation for quadro-
Using Depth Cameras for Dense 3D Modeling of Indoor Environments, copters, IEEE Transactions on Robotics, vol. 31, no. 4, pp. 877892,
Proc. of the 12th International Symposium on Experimental Robotics, 2015.
pp. 477491, 2014. [27] P. Corke, Robotics, Vision and Control - Fundamental Algorithms in
[10] F. Fraundorfer, L. Heng, D. Honegger, G. H. Lee, L. Meier, P. Tanskanen, MATLAB. Springer, 2011, vol. 73.
and M. Pollefeys, Vision-based autonomous mapping and exploration [28] A. M. Alajlan, M. M. Almasri, and K. M. Elleithy, Multi-sensor based
using a quadrotor MAV, in Proc. of the International Conference on collision avoidance algorithm for mobile robot, in 2015 Long Island
Intelligent Robots and Systems, Oct 2012, pp. 45574564. Systems, Applications and Technology, 2015, pp. 16.
[11] M. C. Santos, L. V. Santana, A. S. Brandao, M. Sarcinelli-Filho, and [29] O. Salzman and D. Halperin, Asymptotically near-optimal rrt for fast,
R. Carelli, Indoor low-cost localization system for controlling aerial high-quality motion planning, IEEE Transactions on Robotics, vol. 32,
robots, Control Engineering Practice, vol. 61, pp. 93111, 2017. no. 3, pp. 473483, 2016.