Path Planning – Wavefront algorithm



One of the main achievements for functioning robots is to perform interesting tasks on its own. Some robots have been programmed to fold napkins; others have been able to help assemble various car parts, and recently, NASA launched their newest rover, Curiosity, to Mars on an exciting exploration mission, to tell whether there could be life on Mars and so on. The world of robotics seems to get bigger and interesting each day and this is as a result of the different components and studies that come together to make robotics what it is today. One of the studies is motion planning, and in this report we will discuss one of the various motion planning algorithms – Wavefront algorithm – and relate it to a particular task that was given to us in class.

Task 4: Application of Wavefront Algorithm:

In the task that was given, our goal was to program a robot to find its way from a starting point to a goal area given a map. In this map, obstacles are placed in various points and the robot must prevent itself from crossing any path that had an obstacle in the way. In other words, the robot must find an optimal path from the starting point to the goal point without moving into the path of obstacles.    The map is represented in a 2D array with a starting point (x and y coordinates), which has been defined already for us. Below is a simple file demonstrating the map


#define X_SIZE 5

#define Y_SIZE 4

#define MAP_AREA 20

#define FREE 0

#define OBST 1

#define GOAL 2


int world_map[] = {{0, 0, 0, 0, 0},

{0, 0, 1, 1, 0},

{0, 0, 1, 0, 0},

{0, 1, 0, 0, 0}};

int start_x = 1;

int start_y = 2;

int goal_x = 4;

int goal_y = 3;


The world_map[] is  a representation of a given area in which the robot can traverse and below is the declaration of the coordinates of the start position and goal position in the map.

Implementation and Observation:

Path Following:

We divided the task into two major models. The first part was to plan a path for the robot(path following) to follow in a given map. Before we were able to complete this model, we created different methods for specific tasks. Some of the methods are spinLeft(), spinRight(), moveForward(), moveBackward(). As the names of the methods suggest, they perform exactly the task given it. Also, we implemented another method known as orientation() and direction(). The orientation method defines the robot’s position at a given time in a particular cell. So the robot could be facing north, south, east or west depending on the direction it is heading. Now this takes us to the direction method. The direction method only helps the robot to move into the next cell after processing all its neighbouring cells. So we had four different direction points labeled 1, 2, 3, and 4 and at every orientation the robot must determine which direction it is heading with respect to the cell it must move to.

Wavefront algorithm:

The next model was the application of the wavefront algorithm. This time the robot wasn’t given a path to follow but was given a map, a starting point, a goal point and obstacle cells. The work of the robot was to apply the wavefront algorithm to the map, extract the path from the map and follow the extracted map.


We were required to repeatedly process cells and this was done by implementing queues to store the cells. Now we had to add elements to the queue and so we added the goal cell to the queue. As long as the queue is not empty and the start cell has not been processed, and this is done over and over so far as a cell hasn’t been processed. Then we take the next cell from the queue, set their values by adding one (1) of the current cell.


We faced series of challenges when implementing the wavefront algorithm. One major challenge was that whenever the robot made a right turn or a left turn, it couldn’t determine its direction as defined earlier in the report. Another was how to extract the path after the robot has gone through the map. Also, we experienced oscillation between two cells, in other words, when the robot was going to make a left turn or right turn, the robot keeps going back and forth between its current cell and initial cell.


This task gave us a real experience in motion planning, and made us understand the various principles that come together to complete this task with respect to wavefront planning.


Keith and Frank


Wavefront Planning-Task 4

Task: Path Planning

Group Members:

Flynn Darko

Selase Attah Kwamuar

Lecturer: Dr. Ayorkor Korsah

Task 4 Report

Introduction & Objective

The task required us to implement the wavefront planning algorithm to enable our robot plan its way through a specified map from a start point to a goal area.

Design Concept

The task required us to implement the wavefront algorithm in an enclosed map that was divided into grids and with “x” and “y” coordinates/boundary. The figure below shows the whole map divided into grids. The numbered grids each have an x and y coordinate and contain the numbers 0 and 1 which specify free spaces and obstacles respectively.

Figure 1: Our World Map

Figure 1: Our World Map

As can be seen above figure 1, gives a summary of the task our robot is to perform. The robot is supposed to start at the “start cell” and follow along the red line until it gets to the goal cell or area. In the implementation section of the report, the program is explained further.

The concept we decided to follow is as follows;

  • Get specified map
  • Populate map from goal area
  • Calculate and find shortest path
  • Using the shortest, navigate to the goal area

Implementation in RobotC Language

Before going ahead to write the program to implement the wavefront algorithm the pseudo code that follows explains the structure of the our program;

Cell Population

Set value of goal cell to 2

Enqueue goal cell

         While  (start cell has not been processed)

                currentcell = Enqueue

                Expand currentcell

                for each neighbor of currentcell, set neighbor’s value to currentcell value

                Enqueue neighbor

Path Extraction

Start at the start cell

While not at goal cell

                Move to the neighbor cell with the smallest value which is an not obstacle

Navigate to goal Cell


Implementation Stages

Movement through the map (tiles)

To do this we had to calculate the distance between the center points of two cells

And write a turnLeft() and turnRight() function for our robot.

Writing a function for the movement of the robot was the first part of the task we implemented. After using and observing how the encoders work and using the distance calculated for the tile distances we implemented forward, left, and right movement of our robot. The respective functions used in our program were moveForward(), ninty_turnLeft() and the ninty_turnRight(). We also had to make sure that for the left and right turns our robot turned 90 degress for both turns. We were able to do this by adjusting the magnitude of the power assigned to the both wheels for the respective turns.

Displaying the map on NXT screen

To implement this we used a printWaveMap( ) method to display the map specified in the planning_map.h source code. Using the nxtDisplayString( ) method we printed the various map values in the various cells, specifying the free cells with 0, obstacles as 1 and the goal cell as 2 as can be seen in figure 2.

figure 2

figure 2

Populate the cells from the goal area to start area

After implementing the display of the given map on the screen of the robot, we went ahead to populate the cells within the map starting from the goal cell.  That is, the wavefront plan. We used unique functions namely addQueue( ) and removeQueue( ) to queue all the processed cells until we got to the start area. These functions also made use of variables which were passed as parameters in the function to queue up the values of the neighboring cells as they were populated. From figure 3, it can be seen that from the goal area 2, we populate the neighboring cells which are enqueued till we get to 12 which is the start area.


figure 3

figure 3

Planning and Navigating through planned path to goal

Finally, we made use of the movement function and the wavefront plan to implement finding the shortest path and navigation to the goal area. The program algorithm we wrote for this implementation is as follows;

  1. Given the start cell of robot
  2. Get value of cell
  3. Compare value of that cell to its neighboring cells and find the least cell value
  4. Repeat steps 1,2 and 3 till it gets to the goal cell which is 2
  5. Save shortest path in an Array

After finding the shortest path to the goal we encountered the challenge of factoring in the orientation of the robot. Relative to the bearing of the world map we were using, the orientation for the robot changed after a turn.

We therefore had to use a variable called heading which was assigned variables representing the current bearing of the robot. In our program this enabled us to inform the robot to know the position of the minimum cell to itself and know which direction to turn in order to get to the right neighboring cell with the minimum number.

figure 4

figure 4


On major challenge we faced was how to factor in the orientation for the robot. After getting an understanding of the concept of how to use it, we still had a lot of errors when implementing it in our program. We had to write about sixteen conditions for all possible positions of the minimum cells the robot was supposed to move to and the current orientation for the robot as well. We were however finally able to have our program ran by writing a condition for all four possible orientations for the robot. This included a condition for when the robot was facing north, east, south and west. Each condition included the action the robot was to take depending on where the minimum value cell was to the robot.

During our testing, we also realized that our robot did not exactly turn 90 degrees for both left and right turns. We therefore had to adjust the assigned motor powers as well as the wait times.


After debugging our program following each unit task we realized that our approach to the task cost us a lot of time on the whole task. What we had to do first was implement the movements function and make sure our turns worked before writing our program for the wavefront plan. This would have saved us some time since we had to go back and correct the movement functions.

In summary this task was challenging in terms of determining the orientation of the robot upon every turn but it taught us the value of breaking a problem or a solution algorithm down to the very minute detail since this really helps when writing the program.

Task 4: Path planning: Wavefront Algorithm


Edem Diaba

Abdelrahman Barakat

Task Objectives:

  1. To implement Wavefront algorithm to generate a path.
  2. Use Wavefront algorithm to plan a path through a given workspace.
  3. To execute a Wavefront plan.

Task description

In this task, we will use the NXT mindstorrm robot kit to implement a path planning algorithm called the Wavefront algorithm. The workspace for this task is a 5 by 4 square grid with obstacles identified by numbers. The RobotC program which we will load onto the NXT  brick will be able to determine the path it will take using map which will be preloaded in its memory. Later we will create a program which will be able to find its way using the wavefront algorithm.

What is the Wavefront Algorithm?

It is a cell-decomposition path planning method. This means that it is a way to plan the best path across a workspace which has been divided into equal polygons(e.g. a grid of squares). The squares occupied by obstacles are marked and therefore considered to be obstacles themselves.


Figure 1 Initial map


Figure 2 Wavefront planning

Every obstacle is marked by a specific number(e.g. 1) while the goal and start point are also give two other members (e.g. 0 and 2 respectively). Now, all the other boxes are still empty. To find the best path using this method, we will have to mark all the other empty squares with numbers greater than that go the goal (>2). This will be done by systematically labelling the surrounding squares with increments of the numbers starting from a particular point (the goal) as seen in the figure 2. Now, ne notice that e numbers increase as they approach the start point.

To find the path to be followed, we follow the numbers as they decrease towards the goal. Notice that we took one step at a time till the goal is reached.


Our approach

For this task, we didn’t use any additional sensors because the map for the workspace was already provided to the robot. We also used the tiles on the ground as our grid.

Explain Implementation in RobotC

The program is divided into several sections. First, there is a map which is written in a file of its own and consists of a two dimensional array and two points to mark the start and goal points. The array is already filled with values; 0 to indicate the absence of an obstacle and 1 to indicate its presence. The map file is included at the beginning of the main program(#include “map.h”). Secondly, there are functions in the program which are separated from the main task to perform every action separately. The actions are moving forward, moving clockwise, moving counter clockwise and stopping. We therefore,  used a 4-point connectivity motion to navigate the workspace. Functions which were written to perform basic thinking tasks included getNeighbours(), validateNeighbours(), findMin(), extract(),  enqueue() and dequeue(). Thirdly, there are functions which combine basic thinking and basic moves to decide which actions to take. Finally, the main task, which calls the relevant task; mapping the area, extracting and then traversing towards the goal.

Planning the workspace involved loading the map and using a queue structure to assign numbers to the empty cells and ordering them in increasing order towards the start point. In order not to exceed the boundaries of the workspace, the squares outside the grid are given very high values to avoid being picked for the next move. The outcome is a plan of the entire area populated in an array.

Thinking(extraction) uses the array generated in the planning stage to decide which path to take. Starting from the start point, the function generates a series of commands which follow the decreasing numbers towards the goal. These decisions are stored for the next function to use them.

Acting(traversing) uses the points generated by the extraction function which to move from one cell to the other according to planned.


Figure 3 final path followed


The program was successful in planning, thinking and acting and successfully performed the task even when the map was changed.

Task 4 – Wavefront Planning

Emmanuel Nkansah

Daniel Botchway


The task is to have a robot plan and navigate through a known map by implementing the wavefront planning algorithm. The robot will be given a map representing a cell decomposition of the workspace. The map will be presented by a 2-dimensional array. Furthermore, the coordinates of the start and goal cells will be provided. The robot will have to continuously calculate the values of neighboring cells (starting from the goal cell which will be given a value of 2) and determine the next cell to traverse by examining neighboring cells to see which one has a lower non-obstacle value. Combining these tasks, the robot should be able to plan and navigate its way to the goal cell.


Preparation for Wave front Planning:

Driving (Forward)

We started by trying to determine how many rotations it would take the robot to move from the center of one cell or tile to another. We measured the distance using the tape measure and concluded the distance between the two centers was 45 cm. With that as our basis, we calculated the number of rotations required to cover that distance and obtained 920 degrees, which is equivalent to the 2.5 rotations we obtained after a series of adjustments.

Turning 90 degrees

We then tried to get our robot to accurately turn 45 degrees and 90 degrees. To do this, we first employed a mathematical calculation from an online source. The calculation was based on the concept that a circle is made spins in place. The idea was to determine the circumference of one 90-degree sector (with the radius being half the distance between the tires) and use that in computing the number of rotations required to achieve an accurate turn. The value we obtained was not too far off and after a few adjustments we arrived at 207 degrees.

Finding bearing of destination cell

Subsequently, we worked towards getting the robot to move in the direction of the goal cell or desired direction. We started by initializing integer variables named north, south, east and west (in the four point connectivity version) and northeast, northwest, southeast and southwest (in the eight point connectivity version). We used these variables to make the robot aware of the direction it was facing. We did this by assigning a variable called current bearing a value each time the robot executed a turn. For instance, if the robot was facing north and it turned 90 degrees to the right then current bearing was assigned east, telling the robot that it’s facing east. This allowed us to create a method that ensured the robot moved towards the desired direction.

Moving using given points

After creating a method to alter and determine the direction in which the robot was facing, we tried combining driving one cell, turning 90 degrees and finding the bearing into a method that would make the robot move from one point to another given a start cell and a destination or goal cell. This was done successfully.

Extracting a path from a map

We then gave the robot a completed wavefront-planning map to extract a path and follow in an effort to test our implementation thus far.  The method we implemented involved starting from a given start cell and proceeding to identify the neighbors of the cell with the lowest value. The identified cell is then marked as the next cell to be traversed. Figure 1. Shows a completed map. During the implementation of this phase, we had difficulty reading values from an array of points created to store the neighbors and the next cell to be processed. To solve this problem, we defined a structure called vector. Vector, had an x, y and a value. This aided us in keeping track of values in cells without having to access the cells.


 Figure 1 – Completed Map

Performing the wave front algorithm

Afterwards, we wrote a method for performing wavefront planning. The method implemented a queue data structure to keep track of the cells waiting processing. The following is a step-by-step breakdown of our method:

  1. Enqueue the goal
  2. Get value at goal
  3. Dequeue
  4. While the queue is not empty
  5. Find the free neighbors
  6. For each free neighbor, the value of neighbor becomes the value of Dequeued cell + 1
  7. Enqueue the neighbor
  8. Go to step 4

Subsequently, the robot extracts the path to follow by implementing the methodology discussed prior to the performing the wave front algorithm section. The path is stored node by node in an array structure named EXTRACTED_path[ ] and the points are traversed consecutively.


We put together the methods we had written. To ensure our methodology was fail proof we tested it using 1 major scenario. We modified the map to include obstacles at more places than usual, so doing, our method will display “no path found” on the screen of the NXT brick. It worked. Figure 2 shows a map with obstacles cutting of the path to the goal area marked with value 2.


 Figure 2 – Modified World Map


From the carefully outlined paragraphs above, task 4 involved executing and implementing the wavefront algorithm. Although the task was not the easiest one, we were able to complete the task in time and extended it by adding a version that implements 8-point connectivity.

Task#4: Path Planning

Robotics, 2nd Semester (Jan-May) 2012
Nii Adjetey Sowah, Kwame Owusu Afram

– Understand the wavefront planning algorithm
– Implement a wavefront plan using 4-point connectivity in RobotC programming language
– Demo implementation using the NXT-Robot kit

Summary of the wavefront planning algorithm:
The wavefront planning algorithm resembles the concept of the ripple-effect, much like the way one tile causes the next to topple over when it is falling (the
Domino effect). The wavefront planning algorithm involves transforming your work-space or environment into a uniform grid. This work-space can then be
represented with a two-dimensional array. The obstacles in the environment are represented in the array by the number 1. The free cells are marked with 0.

Figure 1.1 An array representation of an environment

Continue reading