- To implement Wavefront algorithm to generate a path.
- Use Wavefront algorithm to plan a path through a given workspace.
- To execute a Wavefront plan.
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.
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.