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:
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:
- Enqueue the goal
- Get value at goal
- While the queue is not empty
- Find the free neighbors
- For each free neighbor, the value of neighbor becomes the value of Dequeued cell + 1
- Enqueue the neighbor
- 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.