Robotic Path-Planning Through Dynamic Maps

ByCameron Scott Bowie

A THESIS

submitted to

Oregon State University

University Honors College

in partial fulfillment ofthe requirements for the

degree of

Honors Baccalaureate of Science in Computer Science(Honors Scholar)

Honors Baccalaureate of Science in Mathematics(Honors Scholar)

Presented May 25, 2016Commencement June 2016

AN ABSTRACT OF THE THESIS OF

Cameron Scott Bowie for the degree of Honors Baccalaureate of Science inComputer Science and Honors Baccalaureate of Science in Mathematics presentedon May 25, 2016. Title: Robotic Path-Planning Through Dynamic Maps.

Abstract approved:

William Smart

Robotic global navigation is often based on the assumption that the world is static.

It assumes that obstacles present in maps will be there later and new obstacles will

be dealt with by a local planning algorithm. When robots interact in an environment

inhabited by people, this assumption does not hold. People add dynamism to the

world and, in order for robots to deal with this properly, these changes must be

addressed.

This project explores possible methods for representing dynamism in Robot Oper-

ating System (ROS) navigation software and methods for robots to navigate with this

new information. It presents an adaptation of ROS software to handle the added com-

plexity and compares the results of dynamic map-based navigation with traditional

approaches. The results of this analysis find benefits in plan quality and indicate that

the topic is deserving of further study. As this is a broad topic, many new areas for

future work were uncovered that would allow full integration of this representation

with existing ROS software.

Key Words: robotics, path-planning, ROS, maps

Corresponding e-mail address: bowiec@lifetime.oregonstate.edu

©Copyright by Cameron Scott BowieMay 25, 2016

All Rights Reserved

Robotic Path-Planning Through Dynamic Maps

ByCameron Scott Bowie

A THESIS

submitted to

Oregon State University

University Honors College

in partial fulfillment ofthe requirements for the

degree of

Honors Baccalaureate of Science in Computer Science(Honors Scholar)

Honors Baccalaureate of Science in Mathematics(Honors Scholar)

Presented May 25, 2016Commencement June 2016

Honors Baccalaureate of Science in Computer Science and Honors Baccalaureate ofScience in Mathematics project of Cameron Scott Bowie presented on May 25, 2016.

APPROVED:

William Smart, Mentor, representing MIME

Geoffrey Hollinger, Committee Member, representing MIME

William Curran, Committee Member, representing MIME

Toni Doolen, Dean, University Honors College

I understand that my project will become part of the permanent collection ofOregon State University, University Honors College. My signature below authorizesrelease of my project to any reader upon request.

Cameron Scott Bowie, Author

Contents

1 Introduction 1

2 Background 2

2.1 Map Representations . . . . . . . . . . . . . . . . . . . . . . . . . . . 2

2.1.1 Occupancy Grids . . . . . . . . . . . . . . . . . . . . . . . . . 2

2.1.2 OctoMap . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3

2.1.3 Topological Maps . . . . . . . . . . . . . . . . . . . . . . . . . 4

2.2 Motion Planning Algorithms . . . . . . . . . . . . . . . . . . . . . . . 5

2.2.1 Dijkstra’s . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5

2.2.2 A* . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6

2.3 Robot Operating System . . . . . . . . . . . . . . . . . . . . . . . . . 8

2.3.1 Overview . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8

2.3.2 Navigation Stack . . . . . . . . . . . . . . . . . . . . . . . . . 8

2.3.3 Global Planner . . . . . . . . . . . . . . . . . . . . . . . . . . 9

2.3.4 Costmap . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10

3 Data Collection 11

3.1 Possible Methods . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11

3.2 Stage . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12

3.3 Gazebo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13

4 Dynamic Map Representation 15

4.1 Choice of Representation . . . . . . . . . . . . . . . . . . . . . . . . . 15

4.2 Map Representation . . . . . . . . . . . . . . . . . . . . . . . . . . . 16

4.3 Timemap Server . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17

5 Dynamic Map Planner 17

5.1 Existing Planners . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17

5.2 Dijkstra’s Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . 18

5.3 A* Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18

5.3.1 Overview . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18

5.3.2 Implementation . . . . . . . . . . . . . . . . . . . . . . . . . . 18

6 Results 23

6.1 Plan . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23

6.2 Limitations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26

7 Future Work 26

7.1 Topological Maps . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26

7.2 Rapidly Exploring Random Trees . . . . . . . . . . . . . . . . . . . . 27

7.3 Navigation Stack Integration . . . . . . . . . . . . . . . . . . . . . . . 28

8 Conclusion 29

1 Introduction

Human robot interaction is a challenging problem. Robots struggle to react correctly

to sporadic and unpredictable human behavior. This can cause great difficulty when

robots are required to navigate in areas that are frequently traveled by people such as

schools, hospitals, and offices. Currently, robots use a static map to navigate through

their environments. This is useful for areas that do not experience much change;

however, when people are added to the mix static representations can come up short.

Consider a robot trying to navigate through a school during a passing period.

Rationally, the robot should try to avoid the halls that are the main conduits for

students and travel to its destination via a longer, but less busy route. With the

information robots currently have about their environments, this would not be pos-

sible. The robot would look at its map and use the most direct route which may put

it in a situation where it gets stuck in the midst of people.

Doors are another place where static maps can be woefully insufficient. Again,

consider the example of a school. Suppose a robot is tasked with cleaning a school

after the children leave for the day. In this scenario, the robot might start cleaning

before the faculty have left and conclude its job after the school is empty. While there

are still people in the building, the doors in the school are unlocked and therefore

accessible for a robot. (assuming the robot has the ability to open unlocked doors)

However, once the faculty have left the building, most of the doors are locked by the

faculty in some consistent manner. A robot with a static map would at most get a

representation of one of these snapshots in time. This makes optimal navigation of the

building impossible since the robot cannot accurately determine if a door is unlocked

at the time it will be passing through it. Thus, the robot might make plans based

on the false assumption that certain doors will be open and be forced to re-evaluate

1

when they are not. By collecting data about how the robot’s environment changes

over time, we can enable robotic path-planning to intelligently make decisions about

dynamic environments.

For this project, I looked at methods of representing maps that change over time,

termed dynamic maps, collecting data for dynamic maps, and path-planning through

dynamic maps. To test this representation against existing map representations, I

generated plans through simulated environments that have dynamic elements. The

environments I modeled closely resemble the robotic janitor use case mentioned above.

2 Background

2.1 Map Representations

2.1.1 Occupancy Grids

Another commonly used map representation is occupancy grids. An example of an

occupancy grid is given in figure 1. Occupancy grids divide the world up into a array

of cells. The cells contain probabilistic state information about the corresponding

location in the world which can be done in a number of ways [1]. The simplest way

is for the value of to be a binary representation of if the cell is occupied. In other

words, a value of 0 indicates the space is unoccupied and 1 indicates it is occupied.

Cells can also represent the probability that a cell is occupied as a range of values.

Occupancy cell values can be easily estimated using robotic sensors such as LIDAR

and sonar which give information about both a clear path and obstacle in a single

direction. This data can be used with occupancy grids to decrease the value of the

grid cells that a sensor finds to be clear and increase the value of the grid cells

found to be obstructed. The change in the grid cell value uses Bayes’ theorem and a

2

Figure 1: Occupancy grids represent the world as an array of occupied (black inimage) and unoccupied (white in image) cells. This map was generated using ROS’sSLAM software.

Gaussian sensor model to give a more reliable estimation during map generation [1].

A threshold value on the probability is then used to determine which cells should be

considered occupied.

2.1.2 OctoMap

OctoMaps are a 3D mapping framework that are based on octrees [2]. An example

of an OctoMap is given in figure 2. They are designed to work well with noisy sensor

input by relying on a probabilistic representation of both the occupied and unoccupied

space. This method is based off of occupancy grid sensor integration, as described

in 2.1.1. OctoMaps can also hold additional map information such as environmental,

terrain, and temperature data [2].

OctoMaps have many advantages over other mapping methods due to their use

of an octree as the internal data structure. This improves map lookup times and

3

Figure 2: OctoMaps provide representations of three-dimensional environments basedon octrees. Image from [2].

memory usage. Further, OctoMaps can be pruned so as to represent some areas

of a map using a lower resolution than others [2]. This can be especially useful in

representing large areas of free space.

2.1.3 Topological Maps

Topological maps are a division of the map space into topological regions. An example

of a topological map is given in figure 3. The topological regions are defined by critical

lines on the Voronoi diagram of the map. The Voronoi diagram of a map is the set

of free points on the map that are equidistant from two occupied points. The critical

lines of this diagram are the lines between the two points nearest to points on the

Voronoi diagram where there is a choke-point [3]. The result of these divisions is a

topological map. The map can be represented by the points of intersection between

4

Figure 3: Topological maps divided an environment into topological regions. Imagefrom [3].

the Voronoi diagram and the critical lines and the centroids of the regions.

Topological maps are useful for reducing the size of the path finding problem.

Rather than navigating through many thousands of points, the path planning algo-

rithm would be navigating through a graph many orders of magnitude smaller than

the full map grid [3]. They are less useful in situations where optimality is required.

This is because the paths produced when navigating topological maps are at best

approximations of the optimal path.

2.2 Motion Planning Algorithms

2.2.1 Dijkstra’s

Dijkstra’s algorithm is a classic graph search algorithm from computer science that

finds the shortest path between two nodes in a graph. The Dijkstra’s algorithm is

given in algorithm 1. It returns the shortest path between the two nodes and is

5

guaranteed to be optimal.

Algorithm 1 Dijkstra’s Graph Search based on [4]

1: add start node to B2: while true do3: u← vertex in B with min g(u) {where g(u) is edge distance of u from the start

node}4: remove u from B5: add u to A6: if u = end node then7: return path(u) {where path(u) returns the path from the start to u gener-

ated following the parents of u}8: end if9: for all neighbors, n, of u do10: if n not in A then11: PARENT(n) ← u12: add n to B13: end if14: end for15: end while

2.2.2 A*

In graph exploration, Dijkstra’s algorithm is greedy. It always adds the nearest node

to the start. This greedy behavior does not hold for typical motion planning problems

in robotics. Since robots move in the real world, motion planning algorithms can have

some sense of where the end point is relative to the start point. Dijkstra’s algorithm

is not equipped to deal with this thus its greedy graph search behavior results in a

breadth-first search (BFS) within Euclidean space.

Rather than having to deal with the abstract problem of graphs for which Dijk-

stra’s algorithm was designed, robots can use their additional knowledge about the

world to make better decisions on which nodes to explore next. A* is an algorithm

that is greedy within Euclidean metric spaces. It is a heuristic-based algorithm that

was derived from Dijkstra’s and can share Dijkstra’s optimal path property. A* works

6

in exactly the same manner as Dijkstra’s algorithm except equation 1 is used to select

the next node rather using the shortest distance from the start node.

f(n) = g(n) + h(n) (1)

In equation 1, g(n) represents the shortest distance to node n from the start and

h(n) represents the heuristic function estimating the distance from n to the goal node.

A* retains its optimality for heuristic functions that underestimate the distance from

n to the goal node [5]. These heuristic functions are called admissible heuristics. The

algorithm for A* is given in algorithm 2.

Algorithm 2 A* Graph Search based on [5]

1: add start node to B2: while true do3: u← vertex in B with min f(n) {where f(u) is given in equation 1}4: remove u from B5: add u to A6: if u = end node then7: return path(u) {where path(u) returns the path from the start to u gener-

ated following the parents of u}8: end if9: for all neighbors, n, of u do10: if n not in A then11: PARENT(n) ← u12: add n to B13: end if14: end for15: end while

7

2.3 Robot Operating System

2.3.1 Overview

The Robot Operating System (ROS) is a framework of robotics software and drivers

that provides an easy interface for roboticists. The system was first conceived of as a

means of connecting the world of robotics researchers. Prior to ROS, work would be

duplicated and the software developed for papers would not be distributed due to the

difficulty of broad collaboration. ROS provides access to a variety of packages made

by researchers around the world and provides the tools necessary to build and run

them. It also makes developing new packages simple by relying on an XML document

that that describes the package and its dependencies [6].

Since it was first introduced, ROS has developed into a large set of software

with thousands of packages providing among other things data recording, geometric

transformations, robot descriptions, pose estimation, and navigation. It is now used

by tens of thousands of researchers and hobbyists around the world [7].

2.3.2 Navigation Stack

ROS organizes its motion planning code into the navigation stack. The navigation

stack contains a great deal of code including localization (using adaptive Monte-

Carlo localization), local planners, global planners, costmap generation, simultaneous

localization and mapping (SLAM), motion controllers, and a variety of other software.

It is designed to be modular and therefore easy to adapt and modify [8]. As shown

in figure 4, the ROS navigation stack relies on a large amount of software to provide

robust path-planning, mapping, and obstacle avoidance. This project focuses on the

global planner, costmap, and map server.

8

Figure 4: The packages used in the ROS navigation stack. This project focuses onthe ‘global planner,’ ‘global costmap,’ and ‘map server’ packages. Image from [9].

2.3.3 Global Planner

The global planner is in charge of high level planning for robotic motion. This essen-

tially is the software that creates of plan for getting from position a to b. The global

planner does not focus on unexpected obstacles, but rather deals with a map level

understanding of the world.

The ROS global planner has two main planning algorithms. The first method

uses Dijkstra’s algorithm to navigate. Dijkstra’s algorithm is a breadth-first search

so it will generally explore a large space to find the optimal path to the goal. The

navigation stack implements Dijkstra’s algorithm using a grid of potentials which are

values to represent the distance from the start. Once the goal has been found, a path

is found using an algorithm that follows the gradient of the potential grid from the

start to the goal [10].

The second method is an A* planner. This uses A* to generate a potentials

grid that includes heuristic information. The navigation stack A* planner uses a

Manhattan distance heuristic. Again, this planner uses a gradient ascent algorithm

9

Figure 5: An example of a path planned by ROS navigation software. Note thatthe green line represents the global plan. Planning occurs between the robot at thebottom to the red arrow at the top.

to find the path from start to goal [10]. An example of a plan generated by the ROS

planner is given in figure 5.

2.3.4 Costmap

The costmap layer of the navigation stack is essentially a fancy occupancy grid. It

maintains information about the environment including the cost for each cell in the

map (on a scale of 0-100), the map resolution, and mappings from the map to the real

world. It uses a static map as the base for the costs in the costmap. This is expanded

on with an inflation layer that is derived from the robot footprint and gives a wide

boundary between robots and obstacles [11].

10

As it is used by the global planners, the cost map is an array of char’s whose

value represents the cost for a given cell. The planners received this information and

from it produce potential grids which allow for derivation of a plan.

3 Data Collection

3.1 Possible Methods

We discussed a number of possible methods to collect data for dynamic maps including

collection at Oregon State University, collection using a robotic wheelchair in a long-

term care facility in Boston, and through simulations.

Collecting data at Oregon State University involves navigating robots through an

environment over a long period of time in a space that is commonly frequented by

students. To do this, we would control the robots either through tele-operation or

autonomously using ROS navigation software. This method presented a number of

issues. Primarily, navigation through potentially dense crowds of people is difficult

for robots and can pose serious issues for autonomous control. This indicates that

the robot would likely need to be accompanied by a person for data collection which

is costly in terms of time. A second problem is the logistics of organizing the data

collection. This involves bringing the robot to a space and getting permission to

collect data. While this is certainly feasible, it again would add additional cost in

terms of time and effort.

The second method we considered was data collection from a long-term care facil-

ity in Boston. In this facility, we were testing the use of a robotic wheelchair and thus

data could easily be collected. This was our initial plan for data collection; however,

the data quality returned was quite poor due to the sensors used on our robot. The

11

robotic wheelchair that we used lacks odometry and relies on visual flow and LIDAR

for localization. This gave poor results and made the data difficult to use.

The last option we considered was simulation. Data from simulation is further

separated from actual applications than the other options, but is sufficiently accurate

for purpose of this project. Simulations have several advantages including speed,

consistency, and the lack of a physical robot. Unlike robots, simulations do not need

to be shared among a lab, do not break down, and can be used at any time.

Based on the comparison of the above data collection methods, I decided to collect

data in simulation. For this project, I used two different simulation softwares: Stage

and Gazebo. Each had their advantages and disadvantages which I will discuss in

some detail in 3.2 and 3.3.

3.2 Stage

Stage is a simple two dimensional simulator that is fast to run, stable, and commonly

used in older ROS systems. To collect data, I first created a simple world. The first

world that I used was simply two rooms with an opening between them. I created a

dynamic element for the room in the form of a door. When the door was closed, the

best plan would be for the robot to wait for the door to re-open. This is a simple

test that the dynamic planning works since an ordinary planner would either provide

a plan that does not work (one that assumes the door is open and crashes the robot

into it) or would fail to produce a plan and claim that no possible plan exists.

Once I had successfully made a dynamic planner, I created a map, shown in figure

6, to test its ability to traverse more complicated maps. In this map, the robot starts

at the top left and travels to the bottom left. When the door (in figure 6, the door is

the opening in the center - it is shown open here) was open, this map had two routes

12

Figure 6: A map used to test the dynamic planning algorithm. The opening in thecenter can open and close.

to the goal: one through the door and one through the opening at the far right. With

proper timing of the opening of the door, either the route through the door or the

route around the top could be faster. This world was crafted to demonstrate the

advantages of using a dynamic map over using a traditional static map. The planner

based on a static map would plan through the room while the dynamic planner could

realize that the door opens at a certain time and take advantage of this.

The next map that I made in Stage is shown in figure 7. This map is closer to a

real world example. It has 13 doors and numerous rooms and hallways to explore. I

also wrote software to control the opening and closing of doors so that they can open

and close at set times. The map’s complexity makes the problem both more difficult

and more interesting as it can help determine if dynamic planning is useful. I used

this world extensively for testing of the dynamic planner and for comparisons with

the ROS planner.

3.3 Gazebo

Gazebo is the standard ROS simulation software. It provides realistic physics simula-

tions and can far more accurately represent real-world problems such as sensor noise

13

Figure 7: The Stage world used to test planning algorithms. The doors can open andclose. The red square is the starting location of the robot.

and collisions than Stage can. Gazebo is correspondingly slow and more complicated

to interface with than Stage which led to a number of problems.

First, sensor noise made data collection difficult. While sensor noise simulation

is very useful for comparison to real-world applications, since our data collection

is not a real application noise is simply a nuisance. When combined with drifting

localization estimates, this problem resulted in a complete failure to collect data after

any significant period of time - around 30 minutes or more.

Second, Gazebo is slow. The simulation fails to correctly handle sensor input and

collisions when the simulation is sped up beyond twice speed. This make long term

data collection quite time intensive. Since the collisions and physics being rendered

by Gazebo are not significant to the project, this makes Gazebo a less than optimal

choice of simulator.

Third, Gazebo is complicated. Interfacing with Gazebo is much more difficult

than interfacing with Stage because Gazebo tends to rely on complex robot models

14

rather than the simple abstractions used by Stage. As mentioned above, unlike Stage,

Gazebo was designed to work as a full physics simulator rather than simply a robotics

simulator. As such, development with Gazebo required working with many additional

ROS packages and further increased the computational and development work load.

Despite these problems, I was able to collect data using Gazebo (using a map of

Willow Garage), but it was not of the same quality as the data I collected with Stage

due to the problems listed above. Thus I decided to use Stage’s data instead Gazebo’s

for analysis of this project.

4 Dynamic Map Representation

4.1 Choice of Representation

After reviewing the various options for map representations, I decided to use occu-

pancy grids for a variety of reasons.

First, ROS already uses occupancy grids for its map representations. This allowed

for a good deal of code re-use and gave me a better starting place for the project. I

was able to adapt much of the map representation code directly from ROS code, as

will be discussed shortly.

Second, occupancy grids are simple. They are easy to implement and deal with

and made the programming in the project progress much faster. Further, it is fairly

easy to convert a map image file into a occupancy grid by simply transferring the

color values into occupancy cell values - as is done in existing ROS code. This

contrasts greatly with the other options for map representations such as OctoMaps

and topological maps.

Fourth, occupancy grids can easily be used to find optimal paths. This is not

15

Figure 8: This is a visualization of the dynamic map representation. The maps aresets of occupancy grids which represent snapshots in time. In this visualization, timeis the vertical dimension.

true of the other map representations discussed. Topological maps are approximate

representations of the map and thus give only approximately optimal paths.

4.2 Map Representation

Once I had decided to use an occupancy grid for the map representation, I focused

on the details of implementation. I decided to simply extend the occupancy grid

into three dimensions where time is the third dimension. This allows obstacles and

maps to change overtime. I implemented this by making dynamic maps represent an

array of maps which represent snapshots in time. Each map in the array is tagged

with an associated start time and end time which defines when the map is valid. A

visualization of a dynamic map is given in figure 8.

16

4.3 Timemap Server

The timemap server is an adaptation of the ROS map server. In ROS, the map server

reads in maps from a file on disk and loads them into a ROS map data type. The

map server then supplies the map when it is requested.

To adapt the ROS map server to use dynamic maps, I created a new data type,

timemap, which consists of an array of maps with associated start times and end

times. I next modified the map server code to read in several images with associated

times based on a supplied YAML file and produce a timemap data object. The

timemap is requested in the same manner as the normal ROS map is and functions

in much the same way.

5 Dynamic Map Planner

5.1 Existing Planners

Initially, I hoped to be able to use existing algorithms to perform navigation. This

was, unfortunately, not possible due to the special limitations of the dynamic planner.

By using time as the third dimension in my representation, I am implicitly adding the

constraint that the robot can only move in one direction in that dimension. Further,

the distance moved in the time dimension is directly correlated with the distance

traveled in the space dimensions. Finally, the goal for the dynamic planner is in two

dimensions. This is because we can’t determine how long it will take to reach the

goal before performing the search so we must search in the two space dimensions. For

these reasons, I decided to write my own global planner to handle the dynamic map

representation.

17

5.2 Dijkstra’s Algorithm

The first and simplest planner that I tested was a Dijkstra’s algorithm planner. It

used Dijkstra’s algorithm to perform a breadth-first search for the shortest path.

This provides an optimality guarantee, but is impractical due to the resolution of the

maps. The map resolution by default is set at 5 centimeters which, combined with

the breadth-first search used by Dijkstra’s algorithm, makes for a great number of

nodes to explore. This makes path planning very slow when the endpoint is not near

the start.

After some testing, it became clear that Dijkstra’s algorithm would not be fast

enough to act as a global planner for this project.

5.3 A* Algorithm

5.3.1 Overview

The second algorithm that I tried was A* search. As mentioned in 2.2.2, A* search

is a greedy search algorithm. In addition, it is very similar to Dijkstra’s algorithm

and thus required minimal changes to the planner to implement. Unfortunately, the

planner is still not fast enough to complete in a reasonable amount of time, but it was

fast enough to produce plans that can be used for comparisons with plans produced

by the ROS planner.

5.3.2 Implementation

The most important difference between A* and Dijkstra’s algorithm is the use of the

heuristic function. I tested a number of different heuristic functions and evaluated

each for their planning speed improvements. I performed tests of planning speed

under three typical situations.

18

Figure 9: This map shows the locations of the start and test goals. The robot startsat the star and travels to the red, green, and blue endpoints in the respective testcases.

These situations are labeled blue, red, and green for clarity and are shown in figure

9. First, I tested planning around a corner. (labeled red in the figure and table) This

is a problem that is difficult for greedy path-planning algorithms such as A* since

they try to take the most direct path. In this case, they are forced to explore a

larger number of nodes to find the path to the goal. Second, I tested the performance

planning around a dynamic obstacle. (labeled blue in the figure and table) In this

case, I used a door that started closed and opened after a set period of time. Third, I

tested performance across a room without obstructions. (labeled green in the figure

and table) The results of the performance testing is given in table 1.

The first heuristic function that I tried was a traditional Euclidean distance (2-

norm) metric. I chose this metric first as it gives both a good estimate of the unob-

structed distance and provides an underestimate which is the condition for heuristic

admissibility. These benefits, however, were undermined by the slow speed resulting

from the required square root operation. Note that without the square root opera-

19

Red Blue GreenMean Std. Dev Mean Std. Dev Mean Std. Dev

Euclidean 5.42 1.23 1906.42 91.9475 0.0036 0.000811Manhattan 0.83 0.205 1328.15 29.3256 0.0048 0.001684

Octile 4.29 0.256 1933.26 59.1019 0.0047 0.001517

Table 1: The mean planning times (in seconds of computation) using A* and severalheuristic functions under a variety of planning cases. The cases are given in figure 9.

tion the metric is still valid as a relative distance metric, but the metric is no longer

admissible since the distance is overestimated.

The path generated using the Euclidean heuristic for the red case is given in figure

10. Note that the path is not perfectly optimal in a continuous Euclidean space as

planning is performed in a gridded map with eight directions of travel.

The next heuristic function that I tested was the Manhattan distance (1-norm)

metric. This metric, unlike the Euclidean metric, is not admissible in continuous

Euclidean space since it overestimates the distance to the goal. The code for the

Manhattan heuristic is given in figure 11. Note from table 1 that it is much faster

than the Euclidean heuristic; however, it would be preferable to use an admissible

heuristic.

The path generated using the Manhattan heuristic for the green case is shown in

figure 12.

The third and final heuristic function I tested was the octile heuristic. This heuris-

tic function is a modified version of the Manhattan distance metric that takes into

account diagonal motion. This is generally considered the go-to admissible heuristic

for gridded search problems [12]. The code for the implementation is given in figure

13.

The path generated using the octile heuristic is given in figure 14. Note in table

1 that this heuristic is very similar in speed to the Euclidean heuristic. I chose to use

20

Figure 10: The path generated using the Euclidean heuristic to plan the red case.The green line is the path and the small red dots are the inflation layer. (see 2.3.4)

i n l i n e int heur i s t i c manhattan ( const Node& cur ){int dx = abs ( cur . pt . x − goa l . pt . x ) ;int dy = abs ( cur . pt . y − goa l . pt . y ) ;

return (NORM STEP ∗ ( dx + dy ) ) ;}

Figure 11: The Manhattan metric tested for the A* planner.

21

Figure 12: The path generated using the Manhattan heuristic to plan the green case.The green line is the path and the red dots are the inflation layer. (see 2.3.4)

i n l i n e int h e u r i s t i c d i a g ( const Node& cur ){int dx = abs ( cur . pt . x − goa l . pt . x ) ;int dy = abs ( cur . pt . y − goa l . pt . y ) ;

return (NORM STEP ∗ ( dx + dy ) + (DIAG STEP − 2∗ NORM STEP) ∗ std : : min (dx , dy ) ) ;

}

Figure 13: The Octile metric tested for the A* planner.

22

this heuristic for my A* planner.

Despite choosing a fast heuristic and performing numerous other small optimiza-

tions, A* is still too slow to perform planning in a reasonable amount of time. This

indicates that another algorithm is necessary to use this map representation. A pos-

sible alternative to A* is discussed in 7.2.

6 Results

6.1 Plan

Plans can be evaluated on a number of metrics. For this project, I chose to evaluate

them based on the time required to navigate from the start to the goal. To perform

this evaluation, I recorded the plans produced using a traditional ROS planner and

the path produced using the dynamic planner. The results of these tests are given in

table 2.

Note that the dynamic planner and standard ROS planners both produced roughly

the same plans for around corner and unobstructed cases. This is expected given that

in both these cases the standard static map is does not differ significantly from the

dynamic map. However, note that in the around obstacle case, the dynamic map plan

is roughly twice as fast as the standard map plan. The reason for this discrepancy is

that the dynamic map was able to represent the fact that a door between the start

location and the goal would open after 40 seconds so it waited for this to happen

rather than traveling the long way around the map. Since the standard map could

not represent this information, it was forced to navigate all the way around the map

as shown in figure 15. Compare the path shown in figure 15 to the path from the

dynamic planner in figure 14.

23

Figure 14: The path generated using the Octile heuristic to plan the blue case. Thegreen line is the path and the red dots are the inflation layer. (see 2.3.4)

24

Red Blue GreenStandard 16.5 90.9 8.1Dynamic 16.3 43.0 8.2

Table 2: A comparison of planners using a variety of planning cases. The cases aregiven in figure 9.

Figure 15: The path generated using the ROS planner in the blue case. The greenline is the path.

Waiting is a key difference between the dynamic planner and the standard planner.

It is implemented naıvely and occurs as an emergent property of the dynamic planning

problem. As a result, there are numerous possible methods for waiting. The method

shown in figure 14 is essentially pacing while waiting for the environment to change.

The robot crosses the room and returns just in time for the door to open. Other

methods include erratic motions which end near the dynamic element and stationary

motion through time - in other words, standing still. These are all equivalent using

the current planner, but some could be given preference by modifying the heuristic

function.

25

6.2 Limitations

The dynamic planner, as it is currently implemented, has a number of limitations.

Primarily, the planner is much to slow to be used on robots in real time. As we can

note from table 1, the planner takes over 5000 seconds to produce plans under some

test conditions. This is a problem that could be solved using a faster path planning

algorithm or an alternate map representation. Possible solutions to this problem are

discussed in 7.2.

Not only does the slow computation time make the planning infeasible for use

with real robots, but it also presented problems when using the ROS global planner

plug-in interface. Computation time is limited to 0.5 seconds for ROS global planner

plug-ins so the planner could not be used with the standard ROS interface.

7 Future Work

7.1 Topological Maps

I initially chose the occupancy grid representation for their simplicity, the optimality

of paths planned with them, and for the existing ROS code base using occupancy

grids. When I made this decision, I assumed that A* or another optimal path-

planning algorithm would be fast enough to produce plans using dynamic maps.

Since this has turned out to not be the case, topological maps are now a more viable

alternative. By representing the space with a topological map, the number of points

explored could be reduced by many orders of magnitude [3]. This might be sufficient

to make A* a viable planner.

26

7.2 Rapidly Exploring Random Trees

One area for exploration that I was unable to fully complete is state space exploration

algorithms. Specifically, I considered using rapidly exploring random trees (RRTs) to

find a route between the start and end points. These algorithms are very effective at

quickly finding routes in high dimensional, continuous spaces and thus would likely

resolve the speed issues faced by the other planning algorithms.

RRTs are an efficient method for exploring high dimensional space. They ran-

domly choose points from within the state space and connect these points, if possible,

to the nearest point in the tree. The growth is generally limited by a growth factor

which limits the length of new connections to a set maximum. RRTs can be biased

towards the goal by skewing the probability of choosing the goal as a new point. This

results in a greedy state space exploration and have been shown to find a better path

[13].

One downside to state space exploration algorithms such as RRT is that they

do not seek an optimal path, but simply return a solution. This could be resolved

through either using methods such as T-RRT [14], local path-optimization methods

[15], or hybrid motion planning methods [16]. An example of a non-optimal path

produced by RRT is given in figure 16.

For this project, I first attempted to use the RRT implementation found in the

Open Motion Planning Library (OMPL). This proved difficult to accomplish in spite

of OMPL’s well abstracted planning implementation. It failed for the same reason

that I could not use ROS’s default planning algorithms with the dynamic representa-

tion which is discussed in 5.1. I next started, and did not complete, an implementation

of an RRT planner. As a future project, this implementation could be finished and

the results compared with both my planner and the ROS planner.

27

Figure 16: Paths produced by RRT path planners are not guaranteed to be optimal.Optimality can be approximated using variations on standard RRT planners. Imagefrom [17].

7.3 Navigation Stack Integration

For this project to actually be used in the greater ROS community, it must be fully

integrated with existing ROS navigation code. This only requires a couple additions to

the project. Specifically, a long-term mapping software would have to be developed

and the planning software would have to be modified to fit with the ROS global

planner plug-in system.

Long-term mapping software is the more difficult of the two changes. This is

because it would likely require autonomous long-term navigation. This in turn would

require significant additions or modifications to the existing ROS navigation software.

ROS navigation software currently suffers from issues relating to localization when

performing long-term navigation. I struggled to find a solution to this when I worked

with Gazebo for simulation. Due to poor localization, the robot’s costmap would

slowly become populated with false obstructions which blocked the robot’s movement.

28

This makes long-term mapping difficult to perform automatically.

Another issue that must be addressed for long-term mapping software is the au-

tomatic creation of long-term map files. This requires a separation of the data such

that map snapshots can be created at each time step. Alternately, the maps could be

created as data is collected to better accommodate the data collection process. This

would require an additional modification to the timemap server software.

The other required software modification, global planner plug-in compatibility,

could be achieved with relative ease once the planner runs within the set maxi-

mum time restraints. To fully integrate the planner with the existing framework,

the costmap would also need to be modified as the current planner ignores the global

costmap since it is not compatible with dynamic maps.

8 Conclusion

In this paper, I have presented a new method for representing map data and a global

planner that navigates through the data. The representation provides information

about changes that occur in the environment over time and enables more intelligent

decisions by the global planner. Through testing in simulation, I found that the novel

representation and planner improves plan quality at the cost of computation time.

This gives a motivation for future work to improve the computational speed of the

planner and integrate the planner with the ROS navigation stack. Possible methods

for improving the speed of the planner include using approximate map representations

such as topological maps and/or using approximate search algorithms such as RRT.

29

References

[1] A. Elfes, “Using occupancy grids for mobile robot perception and navigation,”

Computer, vol. 22, no. 6, pp. 46–57, 1989.

[2] K. M. Wurm, A. Hornung, M. Bennewitz, C. Stachniss, and W. Burgard, “Oc-

tomap: A probabilistic, flexible, and compact 3d map representation for robotic

systems,” in Proc. of the ICRA 2010 workshop on best practice in 3D perception

and modeling for mobile manipulation, vol. 2, 2010.

[3] S. Thrun and A. Bucken, “Integrating grid-based and topological maps for mo-

bile robot navigation,” in Proceedings of the National Conference on Artificial

Intelligence, 1996, pp. 944–951.

[4] E. W. Dijkstra, “A note on two problems in connexion with graphs,” Numerische

mathematik, vol. 1, no. 1, pp. 269–271, 1959.

[5] P. E. Hart, N. J. Nilsson, and B. Raphael, “A formal basis for the heuristic

determination of minimum cost paths,” Systems Science and Cybernetics, IEEE

Transactions on, vol. 4, no. 2, pp. 100–107, 1968.

[6] M. Quigley, K. Conley, B. Gerkey, J. Faust, T. Foote, J. Leibs, E. Berger,

R. Wheeler, and A. Ng, “ROS: An open-source robot operating system,” in

Proceedings of the ICRA 2009 Workshop on Open-Source Robotics, 2009.

[7] Open Source Robotics Foundation. (2016) Core components. [Online]. Available:

http://www.ros.org/core-components/

[8] D. Lu and E. Marder-Eppstein. (2016) navigation: Package summary. [Online].

Available: http://wiki.ros.org/navigation

30

[9] Open Source Robotics Foundation. (2009) Robotsetup. [Online]. Available:

http://wiki.ros.org/navigation/Tutorials/RobotSetup

[10] D. Lu. (2016) global planner: Package summary. [Online]. Available:

http://wiki.ros.org/global planner

[11] D. V. Lu, D. Hershberger, and W. D. Smart, “Layered costmaps for context-

sensitive navigation,” in Intelligent Robots and Systems (IROS 2014), 2014

IEEE/RSJ International Conference on. IEEE, 2014, pp. 709–715.

[12] Z. Zhang, N. R. Sturtevant, R. C. Holte, J. Schaeffer, and A. Felner, “A* search

with inconsistent heuristics.” in IJCAI, 2009, pp. 634–639.

[13] C. Urmson and R. G. Simmons, “Approaches for heuristically biasing rrt

growth.” in IROS, vol. 2, 2003, pp. 1178–1183.

[14] L. Jaillet, J. Cortes, and T. Simeon, “Transition-based rrt for path planning in

continuous cost spaces,” in Intelligent Robots and Systems, 2008. IROS 2008.

IEEE/RSJ International Conference on. IEEE, 2008, pp. 2145–2150.

[15] J. Mainprice, E. A. Sisbot, L. Jaillet, J. Cortes, R. Alami, and T. Simeon, “Plan-

ning human-aware motions using a sampling-based costmap planner,” in Robotics

and Automation (ICRA), 2011 IEEE International Conference on. IEEE, 2011,

pp. 5012–5017.

[16] B. Raveh, A. Enosh, and D. Halperin, “A little more, a lot better: Improving

path quality by a path-merging algorithm,” Robotics, IEEE Transactions on,

vol. 27, no. 2, pp. 365–371, 2011.

31

[17] N. Akkaya. (2011) Path finding using rapidly-exploring random tree. [On-

line]. Available: http://nakkaya.com/2011/10/27/path-finding-using-rapidly-

exploring-random-tree/

32