Sunday, June 6, 2010

Navigation using distance matrix

Now we consider a robot located in a completely known world. All the obstacles are mapped, the goal position is defined and the robot position is known. This kind of scenario is common for example in videogames, in which the agent controlled by the cpu has to reach some location. But another important scenario for this application is the C-Space of any robot in which work space all the obstacles are known and fixed. Applying cell decomposition we can construct a n-dimensional matrix representing the world (where n is the number of degrees of freedom of the robot).
Now the task is very simple. We create a matrix of Cells as follows:

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.


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 );

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.


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

+ easy method
+ correct and complete
+ fast in small worlds
+ suitable for labirinths

- 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

No comments: