Final Project: Planning in Dynamic Environment


The objective of my final project was to program an NXT mindstorm robot to move in a dynamic environment, an environment where the positions of obstacles are unknown. The algorithm I decided to use in order to achieve my goal is the D* Lite algorithm, a rapid re-planning algorithm coined by Sven Koenig and Maxim Likhachev. The main tasks in the project were as follows:

  • Implementing the D* Lite algorithm
  • Detecting of obstacles
  • Moving from a start to goal location given a planned path


The D* Lite algorithm is implemented using a priority queue. The first step in this project was to implement a priority queue. I implemented the priority queue by transposing an implementation done in Java by Mark Allen Weiss into Robot C. Upon implementing the priority queue, the next step was to start working on the D* Lite algorithm, the main part of project.

The D* Lite algorithm is made up of four main modules: CalculateKeys, UpdateVertex, Initialize, ComputeShortestPath and Main.

State: In my project, a state is used to describe  a cell in the world. It consists of the x and y positions of the cell in the world map, the g value, rhs value and the keys.

Initialize(): this module initializes the search problem before the shortest path can be computed.

CalculateKeys(): calculates the two keys of a state. The first key determines the position of a state in the priority queue. The second key is used to determine the position of a state on the priority queue when the first keys of two states are the same.

UpdateVertex(): this module updates the rhs-values and keys of the vertices potentially affected by the changed edge costs as well as their position in the priority queue

Main(): the module is the main part of the project and puts together all the other modules.


The D* Lite algorithm focuses on identifying cells efficiently to be processed. Consider a robot with a goal location in an unknown terrain.  In moving from cell to cell to arrive at the goal location, the robot uses eight-point connectivity and moves to one of the eight cells with the least cost. The robot calculates the shortest path from its current cell to the goal cell under the assumption that cells with unknown blockage status are traversable. The planned path is then followed to the goal cell if no untraversable cell is observed. If a cell is untraversable, the shortest path is re-computed from the robot’s current location to the goal location.  The goal distances, distance from the cell to the goal, of all cells are important since one can easily determine a shortest path from its current cell of the robot to the goal cell by greedily decreasing the goal distances once the goal distances have been computed. The goal distances of cells change as untraversable cells are detected. The number of cells whose goal distances change are minimal and are sometimes irrelevant in the calculation of the shortest distance. Therefore, in order to efficiently calculate the shortest path, one must efficiently identify the relevant cells. Therefore the D* Lite algorithm solves this challenge.



Implementing the D* Lite algorithm which is the core part of the project was challenging but to successfully complete the project, I implemented each of the modules and ensured that each was working correctly. In piecing together the entire algorithm, I encountered various issues which I did not consider during implementation of the modules. I had to make sure that in getting the numbers of each cell to be processed I had to ensure that they were within the world map and not outside the boundaries. I first implemented the algorithm to work in static environment and made sure that path extraction and movement was working perfectly. The next step was to integrate obstacle detection into the project in order to achieve the objective of the project. To achieve this I used code written by Daniel Botchway 2013 Computer Science, my colleague in the robotics class. These methods detected obstacles and updated the world map working with the sonar sensor. In the world map, one (1) represented obstacle and zero (0) represented a free cell. Upon including these methods and implementing the re-planning section of the D* Lite algorithm, the algorithm worked only if the obstacles were not in close proximity. I therefore had to demo with fewer obstacles than planned and make sure that obstacles were not next to each other. Looking back, I think the problem with the implementation of the algorithm was how I was trying to avoid obstacles. In making the decision on which direction to turn, I never checked the world map to check for obstacles. I believe this omission in addition to the not so perfect implementation of re-planning section of D*Lite algorithm resulted in the robot failing to move and the program unexpectedly closing.


Figure 1 below shows an example of the world map used in implementing the D* Lite algorithm. The robot moves one cell at a time, checking the next cell for obstacles. Figure 2 shows the D* LIte algorithm as written by Sven Koenig and Maxim Likhachev in the reference paper for this project, D* LIte.


Figure 1: example of world map


Figure 2: D* LIte algorithm as written by Sven Koenig and Maxim Likhachev


Figure 3: Ultrasonic sensor used for obstacle detection



Leave a Reply

Fill in your details below or click an icon to log in: Logo

You are commenting using your account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s