Now the task is very simple. We create a matrix of Cells as follows:
PSEUDO CODE:
public class Cell {
private Cell [] nearCell; // contains the 4 near cells: north, south, west, east
private int distance; // represents the distance from the goal. Could also represent an obstacle.
public void propagate();
}
Every cell is linked with the nearest cells and contains information about the distance to walk from that cell to reach the goal.
1st STEP: BUILD THE DISTANCE MATRIX
We construct a matrix using these cells and we assign for example 99999 to distance by default and -2 where the cell represents an obstacle. The cell representing the goal is initialized to 0.
When the matrix is ready we propagate the distance from goal, starting from the goal cell. The propagate method is recoursive and will terminate when all the cells will be explored.
A possible implementation of propagate method is this:
public void propagate(){
for(int i=0;i<4;i++){
if( nearCell[i].getDistance() > this.distance + 1){ //if not obstructed and not optimal path
nearCell[i].setDistance( this.distance + 1 );
nearCell[i].propagate();
}
}
}
If the nearCell is an obstacle, its distance is -1, so we won't propagate it.
If the nearCell has been propagated yet, its distance will be update if we come from a shortest path from the goal, else it won't be explored again.
In this way we construct a matrix in which each cell contains its minimum distance from the goal. So the solution it's optimal, except for the fact that we approximated world with squared cells.
If the robot is allowed to navigate also in oblique way, nearCell would be sized 8.
2nd STEP: NAVIGATE THE ROBOT TO THE GOAL
Last step is very simple. For every iteration the robot will move to the cell with a lower distance from the goal, choosing from the nearCells of the cell in which the robot is located. In this way the robot monotonically decreases his distance from the goal, reaching it following the shortest path. This method also satisfies Cauchy theorem
If you don't see the applet click here
Pros:
+ easy method
+ correct and complete
+ fast in small worlds
+ suitable for labirinths
Cons:
- the path found isn't smooth
- expands too many useless nodes (not suitable for large worlds)
Possible applications:
- maze resolution
- path planning in small dimensioned C-Spaces
- high level path planner for rovers, automated wheelchairs, container transport