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

Given the start point and the goal point (in this case (0,5) (7,5) respectively), the wavefront algorithm is implemented as follows:

1. The goal point is initilized with 2
2. The immediate neighbouring cells that are not obstacles are assigned the value of the goal point +1
3. Each free cell after that is assigned the value of the cell on the edge of the wavefront +1 until the whole array is filled

Figure 1.2 An array propagated with the wavefront algorithm

With the wavefront plan (Figure 1.2), the robot can navigate the shortest path to the goal cell.

Implementation in RobotC programming language:
In implementing the wavefront algorithm in RobotC, we had to first basic movement routines for our robot to use. Our movement routines contained move_forward()
and move_back; these two control the forward and backward motions of the robot. The move_forward() and move_back routines moved the robot for the length of a
single cell or tile. We also coded left and right turm routines. Our turn routines moved the robot 90 degrees left or right (we called them turn90L() and
turn90R()). In addition to this, we had a routine that moved the robot 180 degrees and another one that caused itt to pause for a second. We called those two
about_turn() and pause() respectively.

After the movement routines, we wrote the implementation of the wavefront plan. We made use of critical functions namely, getNeighbors(int x, int y), enQ(int
x, int y), deQ() and getWaveFront(). The function getNeighbors(x,y) is to take a point(x,y) and store the location of the neighbouring cells of that point.
With the help of enQ(x,y) and deQ(), cells on the grid waiting to be processed were queued and processed one cell at a time. The main function, getWaveFront()
utilized the other functions to get the plan.

Finally, we used the movement routines and the wavefront plan to build the last piece of the implementation: following the plan. We wrote the logic which will
make the robot follow the wavefront plan. The logic was as follows: given a start cell, get the value of that cell, compare the value obtained to that of its
neighboring cell and then move to the cell with the least value. One of the major challenges we faced in implementing this was the fact that, when the robot
changes its orientation, its sense of LEFT, RIGHT, UP, DOWN also changes. For example, cell (3,3) is left of cell(3,5) when the robot is facing UP (in the
direction of decreasing Y-axis). However, when the robot is facing RIGHT (in the direction of increasing X-axis), cell (3,3) is below cell (3,5).

Figure 1.3 Depiction of our direction challenge

We solved this challenge by using a simple equation: Real_robot_direction = map_direction – direction_robot_is_facing. We were able to achieve this by
representing the directions UP, RIGHT, DOWN, LEFT with integers 0, 1, 2, 3 respectively. For instance, in figure 1.3, where the cell (3,5) is on the right of
the robot, its real direction would be (according to our equation) RIGHT(2) – UP(0), which is RIGHT(2). In the second scenario, cell (3,5) is RIGHT(2) – RIGHT
(2) = UP(0) of the robot.

For this task Adjetey’s robot was used and thus all measurements and adjustments were made using his robot. Adjetey’s robot had a much wider distance between
its front two wheels and was lower to the ground (in comparison to Afram’s robot). While testing our program with Adjetey’s robot, the battery died. The
immediate suggestion was to download the program onto Afram’s robot and test the program. So we did just that and also changed my port mapping just to suit
that of Adjetey’s robot. However, due to the various structural differences between the two completely different robots, the same code failed to work on both.
To continue with our task, we charged the battery of Adjetey’s robot and
focused on getting the program to work on it.

Figure 1.4 From left to right: Adjetey's robot, Afram's robot

During development and testing we used a particular ‘world map’. After we were finally convinced that the entire build was ready, we then begun to test our
program with different ‘world maps’ and kept changing the start and goal positions. Before we run our program on each map, we would manually predict the route
we expected the robot to use, and then run it to see what actually happens. We were correct for all except one particular map we tried. For that particular
map, we anticipated that the robot would take a particular route to the goal, only it to take a much shorter legal route to the goal. When the robot first took
to its more efficient route, we almost cried, and then we realized it landed safely in the goal position. After watching the robot, we went back to our
wavefront planner program to examine the shortest path to our goal, and truly, the robot was right!

In our algorithm, we made the assumption that all valid non-obstacle cells would have values greater than the goal value, which was 2. Running the wavefront
planner program was successful, until we run our planner on a map that no valid route to the goal (the obstable cells form a barrier between the start and goal
cells). Thus, all the cells after the obstacle cell line still had the value of 0. For a very long time, we were unable to determine that the proble was from
the map, we spend majority of the time inspecting our program.

We both agree that, this task was the most fulfilling and interesting one yet. Given more time, we would want to try to tackle this problem using 8-point
connectivity instead of just 4-point connectivity.


Leave a Reply

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

WordPress.com Logo

You are commenting using your WordPress.com 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