Simultaneous Planning Localization And Mapping For Unmanned Aerial Vehicles

Sameer Ansari, Billy Gallagher, Kyel Ok, William Sica

Project Abstract:

This is a course project & report for Georgia Tech course Robotics Intelligence & Planning 2011; the paper is an academic final report.

The goal of the project is to implement a planning technique into the SLAM framework, using a local planner to direct the path towards the global exploration goal. This work is motivated to improve SLAM performance in uncertain or rapidly changing environments, such as search and rescue missions in a forest using a UAV. Images & Videos



A GIT repository of the simulator code is freely available: Source Code

Related Work:

  • Y. Huang and K. Gupta "RRT-SLAM for motion planning with motion and map uncertainty for robot exploration" IEEE/RSJ International Conference on Intelligent Robots and Systems, 1077 - 1082 (2008). - This paper details an approach for solving the motion planning problem of SLAM by using RRT. RRT is adapted by adding additional dimension to the navigation problem representing collision of the robot with objects in the environment and the uncertainty of the desired area. This approach motivates our work by showing ways to include additional metrics in the planning process to utilize a known planning algorithm. This approach could be expanded by further considering robot velocity or robotic platforms which can move in three-dimensions.
  • Kyoungmin Lee and Wan Kyun Chung "Navigable voronoi diagram : a local path planner for mobile robots using sonar sensors" IEEE/RSJ International Conference on Intelligent Robots and Systems, 2813 - 2818 (2007). - This paper modifies Voronoi diagrams to solve the motion planning problem of SLAM. This is done by adding a sonar sensor ring to the robot, such that as the global path is being explored local collision detection is handled through generating a "navigable Voronoi diagram" at each step.
  • Cindy Leung, Shoudong Huang, and Gamini Dissanayake Active SLAM using Model Predictive Control and Attractor based Exploration IEEE/RSJ International Conference on Intelligent Robots and Systems, 5026-5031 (2006). - In this paper the problem of map coverage in SLAM is addressed. Attractors are placed in areas of uncertainty to guide the robots movement choices and provide faster coverage of an area as compared to greedy search of a location. This approach does not account for certain limitations of of an aerial platform, such as inability to stop mid-flight.
  • Shem, A.G.; Mazzuchi, T.A.; Sarkani, S.; , "Addressing uncertainty in UAV navigation decision-making," Aerospace and Electronic Systems, IEEE Transactions on , vol.44, no.1, pp.295-313, January 2008 doi: 10.1109/TAES.2008.4517005 - The UAVs make navigation decisions based on response to potential fields generated by the probabilistic maps.
  • Garrido, S.; Moreno, L.; Blanco, D.; Martin, F.; , "Smooth path planning for non-holonomic robots using fast marching," Mechatronics, 2009. ICM 2009. IEEE International Conference on , vol., no., pp.1-6, 14-17 April 2009 doi: 10.1109/ICMECH.2009.4957121 - The VFM and FM2 methods use the propagation of a wave (Fast Marching) operating on the world model, to determine a motion plan over a slowness map (similar to the refraction index in Optics) extracted from the updated map model.
  • Mostafavi, M.A.; Beni, L.H.; Hins-Mallet, K.; , "Representing Dynamic Spatial Processes Using Voronoi Diagrams: Recent Developements," Voronoi Diagrams, 2009. ISVD '09. Sixth International Symposium on , vol., no., pp.109-117, 23-26 June 2009 doi: 10.1109/ISVD.2009.34 - The paper presents how different types of Voronoi diagrams for points in two and three dimensional spaces as well as Voronoi diagrams for line segments and polygons could be effectively used in different contexts to represent and simulate different dynamic spatial fields and processes.

Proposed Work:



General Task Sam Billy Kyel Will
Week 1 Perform a literature review on the previous work in SPLAM. Literature review on SLAM Literature review on SPLAM/SLAM. Literature review on SPLAM/SLAM. Developing code to autonomously control an AR.Drone UAV. Building and tuning PID controllers on UAV roll/yaw/pitch/altitude commands. Literature review on SPLAM
Week 2 Literature review on Voronoi Diagrams & SLAM Literature review on Voronoi Diagrams & SLAM Literature review on Voronoi Diagrams & SLAM Literature review on Voronoi Diagrams & SLAM. MATLAB simulation of following Voronoi edges as the path to the global goal using simple control laws. Literature review on Voronoi Diagrams & SLAM
Week 3 Brainstorming & discussing algorithm details Brainstorming & discussing algorithm details Brainstorming & discussing algorithm details Brainstorming & discussing algorithm details Brainstorming & discussing algorithm details
Week 4 Dividing work for final project Voronoi decomposition Forming Gaussian/potential field Simulator A-Star path planner
Week 5~6 Continue working on individual parts Voronoi decomposition Forming Gaussian/potential field Simulator A-Star path planner
Week 7 Finish Simulator Simulator Engine Local planner (Potential Field) & A* Simulator Engine Global Planner (Voronoi) & A*
Week 8 Finish Simulator & Final Report/Presentation Simulator Engine Local planner (Potential Field) & A* Draft & Simulator Engine Global Planner (Voronoi) & A* & Draft

Week 1

Group Meeting (October 14, 2011)
The project "blog" was created and a synopsis was added to the Project Ideas page. In addition, previous work in SPLAM was discussed.

Plans for next week
1. Populate the related works section (Author, Title, Description, Areas of improvement)
2. Discuss whether to use SLAM to build a path away from uncertainties or to build a path towards reducing uncertainties and building a better map.
3. Narrow down the topic to a well defined problem
4. Continue working on SLAM simulator and UAV platform.

Week 2

Group Meeting(October 24, 2011)
Discussed the details of RRT-SLAM paper.
Discussed different path planning algorithms and ways to fuse with the SLAM framework.
Agreed that Voronoi Diagram would work well for our 2D problem with point-like obstacles (trees).
Brainstormed ideas to fuse Voronoi diagram with uncertainty from SLAM
Mark the points on the voronoi diagram as vertices and the lines at edges.
At each step when a vertex is reached;

  • Update the Voronoi diagram based on new objects discovered to prevent collisions.
  • Propagate uncertainty information from the global model to modify edge weights.
  • Select the local optimal edge and follow it to reach the next vertex.

Plans for next week
More brainstorming on VD and SLAM fusion. Next meeting in a few days.

Week 3

Group Meeting(November 2,2011)
Decided on the implementation details of the project.
Discussed different ways to fuse uncertainty with Voronoi decomposition.

Plans for next week
Compile list of methods for generating Voronoi diagrams
Generate ideas on how to introduce uncertainty into these methods

Week 3-2

Group Meeting(November 4,2011)
Discussion on the details of the algorithm:

  • Two part planner (Global & Local)
  • Voronoi Diagram as the Global planner
    • Voronoi edges biased as a function of uncertainty
    • Voronoi vertices as way-points for the potential field to follow. The vertices are local minima in the Gaussian Potential Field.
  • Gaussian Potential Field as the local planner
    • Sum of all the Gaussian uncertainty around each object + distance to goal to represent the 3D surface (size of Gaussian i.r.t the world is very important)
    • Gradient descent on the 3D surface (simulated annealing?)
    • Always heading towards the local minima (Voronoi vertex) - no chance of "getting stuck in the local minima"
  • Some notes
    • Can vary the complexity of the voronoi diagram to see what performs better (generic voronoi vs uncertainty weighted voronoi)

Week 4

Group Meeting (November 7,2011)
Divided work among group members.
1. Voronoi decomposition (Sam)
2. Forming Gaussian / potential field surface and gradient descent. (Billy)
3. Using Voronoi edges in A-Star for forming a path (Will)
-Removing nodes to re-plan
4. Simulator (Kyel)
-Gradually reveal points and generate uncertainty
5. Integration (Group)

Internal deadline is November 18.

Discussed requirements for the final project.
1. Show completeness of the planner.
2. Show optimality (or lack thereof) of the final planner
3. Simulate the planner in difficult environments

Week 5~6

Continue working on individual parts.
To meet on Dec 2nd to put things together
Basic Voronoi & Simulator:

2D Simulation of a quadrotor going from the starting position to the goal using Voronoi Edges as its path.
Using the following control laws:

  • Roll to minimize the perpendicular distance to the closest edge
  • Yaw to stay parallel to the closest edge
  • Constant forward velocity and altitude

Gaussian noise added to all control signals.

Week 7

Group Meeting (December 12, 2011)
Compiling individual parts together
Running experiments
Start on making the presentation slides and the final report

Current Simulator status:

  • Local Planner using Potential Field
  • Global Planner using Voronoi Diagram
  • Uncertainty is accounted for as potential for local planner, and last known estimated position for global planner

Local Planner
Potential Field, Local Goal


  • Magenta Diamond - UAV position
  • Blue Star - Local Goal
    • Contour lines of varying color by intensity
    • Black line - gradient descent path to goal

Global Planner
Voronoi Diagram, Global Goal, DFS/A* choose node as local goal [Helps Local Planner deal with big picture and avoid local mininma]


  • Magenta Diamond - UAV position
  • Blue Star - Local Goal
  • Red Star - Global Goal
    • Black dot - Ground truth positions of obstacles (unknown to UAV)
    • Yellow Circle - UAV Field of View (360 degree view)
    • Green dot w/ green dotted line - Last known observed position of obstacle
    • Blue dotted lines - Voronoi diagram

1000 obstacles

Week 8

Simulator updates

  • noticed that with a lot of trees (I set it to 500!), voronoi would very often choose a node that sent the robot right between two very close trees that it couldn't fit between, so it would get stuck in place. Looking through Voronoi to find out why, I noticed that while it was attempting to trim edges that were too close to a tree, there were quite a few cases where the edge would not be trimmed, especially if it was a small edge, nearly horizontal, or nearly vertical. I fixed this, so edges are trimmed whenever there's a tree within a certain tolerance of them.
  • updated the plot show it shows only the edges that aren't trimmed in blue. Edges that were trimmed are shown in a feint grey color for reference.
  • made this tolerance a parameter set in the main simulator called TREE_SIZE. It's passed to both the voronoi planner (for choosing which edges to trim) and the local planner (for adding the linear component to the cost when the robot is close to a tree)
  • I also made it so the number of points added around the robot and goal is adjustable, because I wanted to play with this value to see if it gave better results. Seems increasing it can give the robot more options and it will avoid zigzagging if it can just go straight, so I currently have it set to 8, but I found 12 & 16 worked fairly well, too.
  • Finally, with the trimming edges working better, I found it actually could trim out enough edges to orphan a vertex, so I added an algorithm that scans through and removes orphaned verticies so they're not used when searching for the goal.


  • Potential field force of obstacle by distance is increased if stuck (and degrades to normal after)
  • If visible obstacle uncertainties less than threshold, continues to use previous voronoi, still updates local planner (planners don't always run in same timestep :D )
  • contour plot shows local area overlayed onto global to really show what local means, looks great and uploading video soon
  • bunch of tweak variables added, separated force effects for local planner and global for easier tweaking (this has to do with varying force variable also)
  • Added path smoothing to output of local_plan, gives smoother lines…
  • Added compression method 'FFDS', which needs ffdshow installed on os,


  • A* is implemented, and can be used by modifying the line

Local Planner

Global Planner

Dec 14: 10,000 Obstacles

Subscription expired — please renew

Pro account upgrade has expired for this site and the site is now locked. If you are the master administrator for this site, please renew your subscription or delete your outstanding sites or stored files, so that your account fits in the free plan.