Mapping, Localization and Path Planning of
a UAV (Quad-Copter)
Sarmad Naveed, Hassan Hafeez Butt, Syed Talha Ali, Dr. Omer Ahmed
Department of Electrical Engineering
COMSATS Institute of Information Technology
Islamabad, Pakistan
[email protected], [email protected]
Abstract— Our project is about localization mapping propellers. These quad-copters are usually controlled
and path planning of Unmanned Air Vehicle (UAV). In remotely but are capable of autonomous flight. These
this project the hardware platform of this project is UAVs carry an on-board computer and a collection
quad-copter and we programmed its functionalities
of sensors to perceive their environment and interact
such as localization mapping and path planning. The
with it. The project’s goal is to utilize the UAVs for
quad-copter used video feedback from the two on-board
cameras and the GPS to first localize its self in the given
commercial purposes such as for courier services,
map of the environment. If the environment is unknown surveillance, media coverage etc. These are very
it gets switch to Simultaneous Localization and complex tasks even for a remotely controlled robotic
Mapping (SLAM) algorithm to navigate. We performed system, let alone an autonomous one. To implement
several tasks such as vision guidance and pick and place such systems the UAV must be able to localize itself
maneuvers. We integrated our hardware with Robot in a given map, be able to plan its path and avoid
Operating System (ROS) libraries to perform high level obstacles (such as trees, buildings and other UAVs).
tasks.
Researchers have proposed various techniques and
algorithms for each of these tasks. As in our project
I. INTRODUCTION
we aim to make our UAV capable of autonomous
“Robotics is the science of perceiving and navigation we will be implementing these following
manipulating the environment and physical techniques and algorithms.
surrounding through computer-controlled
mechanical devices” by Sebastian Thrun. [1] The software platform of this project is Robot
Operating System (ROS) which allows integrating
Autonomous robotics is one of the most key topics of the data of the sensors and applying different
this generation in research. It has a wide range of algorithms to perform high level task such as
applications, such as surveillance, pick and place localization, mapping and path planning. Robot
services, manufacturing, waste management, space Operating System (ROS) is the combination of
exploration and rescue services. Autonomous robots software libraries, functions and tools which are
can simply defined as intelligent machines that are helpful in building the robot applications. It allows
capable of performing certain tasks in their interacting with hardware devices and provides
environment without explicit human interaction, libraries, package management, message-passing and
lowering the operation costs and complexity. One device drivers.
of the examples of autonomous robot is Quad-Copter.
The hardware platform of this project is AR. Drone II. LOCALIZATION
2.0 which is quad-copter capable of autonomous “Localization is the problem of finding out a robot’s
flight. coordinates relative to its environment, assuming
that one is provided with a map of the environment”
Quad-copters are the major element of research in
by Sebastian Thrun. [2]
robotics now-a-days. Quad-Copters are kind of
Unmanned Aerial Vehicles (UAVs) with four
Imagine a robot is lost in space. There should be a Motion model describes how the world state changes
way to help it realize its position in an arbitrary when an action is executed by a Quad-copter. Quad-
world, for this purpose we employ the procedure of copter executes an action “u” and updates the belief
Localization. In a remote controlled robot there is no state according to motion model which is given
need of localization because we are giving below;
instructions to it but an autonomous robot has to take ̇ ) (3)
its own decisions. Therefore localization becomes a
major requirement before a robot can plan its path. ̇ Is the current state is the previous world sate,
Now the question arises how a robot localizes itself? “g” is the transition function and “u” is the executed
A GPS can also be used for this purpose but GPS action. These are the deterministic models but
provides location in form of coordinates with a sensors cannot estimate the world perfectly because
degree of error ranging till 10 meters which in mobile sensors values are noisy and potentially missing. So
robotics is a very huge error. The acceptable range is we replace the deterministic model with probabilistic
in centimeters and to decrease probability of this model and the Figure (2) shows the probabilistic
error we apply several techniques and algorithms of model;
Localization. The robot which is Quad-copter in our
case cannot observe the environment directly. It
estimates the world through probabilistic belief. The
belief of a robot is actually the probability of a robot
pose in a given environment. Belief is updated using
sensor values and the motion is executed. Figure (1)
shows the sensors and action model
Figure (2) Probabilistic Model
Bayes’s Filter is the base of Kalman Filter. It is a
Bayes’s filter with continuous states. The reason
behind using Kalman filter is, it represents state as a
normal distribution .it is very efficient require few
matrix operation per time step. Figure (3) shows the
representation of Kalman Filter.
Figure (1) sensors and action model
The sensor model describes how the real world is
linked to the sensor observations. Robot perceives the
environment through sensors. The sensor model is
given as;
(1)
“Z” is the sensor reading, “h” represents the Figure (3) representation of Kalman Filter
observation function and “x” is the physical world
state. In order to obtain the information of the world Where µ is the mean and σ is the covariance .The
state the mathematical equation becomes; Histogram filter is the discrete type of Bayes’s filter
that can be used for localization of a robot but a robot
(z) (2) has to store a very large grid in memory and update
of belief in every cell which is not very efficient
technique. This is the main reason why Kalman filter
is used for localization. Consider a Markov chain and the fact that robot mapping is characterized by
represent the estimated state (xt) with Gaussian and uncertainty and sensor noise. Almost every
assume that system is linear over time (depends successful mapping algorithm is based on Bayes’s
linearly on control u) has zero mean normally rule and is written as;
distributed process noise. Sensor measurements (z)
depend linearly on state and zero mean noise. | ) | ) ) (6)
(4) In robotic mapping, data arrives over time. And as
discussed above there are two different types of data,
(5) Sensor measurements and controls. If we denote
sensor measurements (e.g., camera images) by the
Where (0.R) and ϵt (0, N) are the zero mean noises variable z, and the control motion (e.g., motion
whereas A, B and C are the linear transformation commands) by u. Suppose the data arrives in
matrices. alternate fashion;
III. MAPPING (7)
“Mapping is the problem of generating maps from
sensor measurements” by Sebastian Thrun [3] . Here subscripts are used as time index. In particular,
is the sensor measurement taken at time t, and
Mapping is the task of building the maps of an
specifies the robot motion command asserted in the
unknown environment that a robot perceives. By time interval [ ); t)]. Sometimes, odometry
using this technique, the autonomous robot is able to measurements or measurements from inertial
construct a map and localize itself. Every navigation units (INUs) are used for u instead of
autonomous robot needs a map to reach its controls, since they may more accurately reflect the
destination. The robot usually has the map of an actual robot motion.
environment that is known to it but in order to move As the data arrives over time so we applied Bayes
filter. It is a recursive estimator for computing a
in an unknown environment robot must have to do
sequence of posterior probability distributions over
mapping so that it can manage its path. At any given quantities that cannot be observed directly—such as a
time, the robot has quite a bit of data. This data is map.
known as state of a robot. One of the major part of
robot’s state is its position and orientation. IV. PATH PLANNING
Navigation, localization and mapping are connected “Path planning generates a geometric path from an
with each other. They are usually done in an iterative initial to final point, passing through pre-defined
way. In order to operate in an unknown environment via points, either in the joint space points or in an
robot maintains the knowledge of its current state and operating space of the robot” by Sebastian Thrun
will do the following steps; [4]. The objective of path planning is to devise a plan
through which robot can find its path to the goal
1. Do the control action (i.e., Move some
location. The factors which are highly considerable in
distance to introduce some error in our
path planning to find an optimal path are;
known pose.)
2. Read the sensor data. 1. Execution speed/path length
3. On the base of sensor readings calculate the
2. Energy consumption
distance from its current state and localize
3. Planning speed
itself on the given map.
4. Safety
4. Update the map using its sensor readings.
5. Repeat the following steps. 5. Robustness against disturbance
6. Probability of success
There are several algorithms that are used for
mapping. All algorithms have one common feature Planning takes place in configuration space. It
and that is they all are probabilistic. The reason for represents the work space of the robot which specify
the popularity of probabilistic techniques stem from the degree of freedom of planning it means if robot
work in a 3-d space the degree of freedom is 3.the
Degree of freedom (DOF) can be increased according
to the planning problem e.g. in Quadcopter full pose
require 6 DOF and if velocity is included the DOF
will be 12. Configuration space(C-space) is the
configuration of all possible configurations and it is
not cartesian due to rotations. Configuration Space is
denoted by C it splits into two parts Cfree and obstacle
space Cobs .Cfree is the path accessible for robot and
Cobs is the path which is not accessible.
Figure (6) Rapidly exploring random trees (RRT)
(8)
B. A* Algorithm
(9)
It combines path cost with estimated goal distance.A*
is optimal and complete which mean in the number of
The question arises how to find the free space in
expanded nodes it finds the solution if there is one
configuration space. Free space is obtained by sliding
and fails otherwise.
the robot along the edge of the obstacle region
“blowing them up” by the robot radius which is also ) ) ) (10)
called Minowski sum. Figure (5) shows the
representation of work space and configuration Where g(s) is the path cost,f(s) is evaluation function
space. and h(s) is the Heuristic function. Heuristic function
should never over estimate true cost it should be
smaller or equal to the final cost and it has to satisfy
the triangular inequalities to be consistent.A* works
in a following way; and Figure (7) shows the working
of A* algorithm.
Initialize
Figure (5) work space and configuration space
1. OPEN={start}, CLOSED={}
Planning tachniques provide graph consisting of 2. F(s)=inf
vertices and edges with associated cost to find the
shortest path between two vertices search algorithms While goal not in CLOSED
are applied [5].The search algorithms which are often
used are following; 3. Remove vertex s from OPEN with smallest
estimated cost f(s) and insert s into
A. Rapidly Exploring Random Trees (RRT) CLOSED
Most efficient algorithm useful when search space is
large or dimension search environment is high. The For every successor s` of s not yet in CLOSED
algorithm work in a following way;
4. Update g(s`)=min(g(s`),g(s) + c(s,s`))
5. Insert s` into OPEN
Initialize tree with first node qI
Pick a random target location (every 100th Fig (7) shows the representation of A* Algorithm.
iteration, choose qg)
Find closest vertex in road map
Extend this vertex toward target location
Repeat steps until goal is reached
Figure (6) shows the representation of Rapidly
exploring random trees (RRT).
Figure (7) shows the A* algorithm
V. PROPOSED MODEL (11)
Simultaneous Localization and Mapping (SLAM) is
used for localization of a robot in a 3D world while ( ) ) (12)
simultaneously generating a 3D map of its
G function predicts the location of a feature point in a
environment based on a real time data gathered from
camera image through 6D pose of a camera and 3D
various robot sensors. Parallel Tracking and Mapping
feature point after prediction of a feature point in the
(PTAM) is a technique refers to visual SLAM, new
local frame, convert the local 3D coordinates into the
points in world are calculated and localized through
image through hessian function h. Hessian function
mapping on the other hand tracking defines the
transforms coordinate system of the front camera to
location of a point in each of the frames and location
the coordinate system of the quadcopter.
and orientation of camera in each of the images.
Mapping and tracking are two separate threads
( ) ( ) ) (13)
functioning parallel in the dual core processor [6].
PTAM is based on bundle adjustment. In quadcopter hessian function is 6D and state space
of a quad copter is consist of 10 variables which
A. Bundle Adjustment
include x, y and z positions, velocity of x, y, z, roll,
Quadcopter has monocular camera and camera
pitch, yaw and yaw velocity.
observe 3D points, system has camera poses and
position of these points in 3D space.3D location of Thread 1 track camera and compute camera pose
the points and 3D poses of the cameras are through camera pose it gets the location of a feature
triangulated at the same time in this technique. point and insert this point into the map then 2 thread
Triangulation is the process calculating point location optimize the map which means it solves the linearize
by forming triangles to it from known points. Camera system again and again. The hessian matrix increases
poses c1, c2, c3 are independent of each other and after every optimization so in the beginning map
points are also independent of each other but one optimization work in the real time after little duration
camera observation is linked with different points [7]. it becomes slow. Thread 2 becomes slow and takes
Figure (8) shows relation between camera poses and few seconds to optimize the map in the mean while
3D points. system can track the camera with respect to the
model and optimize in the background. Figure (9)
shows the PTAM’s Threads.
Figure (9) shows the representation PTAM’s
Threads.
VI. CONCLUSION
Figure (8) relation between camera poses and 3D In this work, we implemented the tele-operation
points using keyboard, tele-operation using joystick, visual
navigation, autonomous flight and object tracking
Bundle adjustment optimizes 6DOF camera poses and following by using the algorithm.
and 3D points where m are the 3D points and n are
camera poses.
VII. REFERENCE
[1] G. Trehard, Intelligent vehicles: integration and
issues, Paris-Rocquencourt, 2006.
[2] Sebastian Thrun, Probabilistic Algorithms in
Robotics, Al Magazine Volume 21 Number 4, 2000.
[3] Sebastian Thrun, Probabilistic Algorithms in
Robotics, Al Magazine Volume 21 Number 4, 2000.
[4] Sebastian Thrun, Probabilistic Algorithms in
Robotics, Al Magazine Volume 21 Number 4, 2000.
[5] Mike Brady, “Aiming the realistic goals”, in
Intelligent Robotics, 2013.
[6] Clarke, Roger, "Robotics: Implications for
Information Technology", Part 1 and Part 2, in Robot
Institute of America, August 19, 1996.
[7] Merriam-Webster, “Sensor Technologies”, in
Artificial Human Sensors: Science and Application,
2012.
