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:

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

Navigate a point robot to a target using force fields

Suppose we have a punctiform omnidirectional robot and we want to navigate it toward a target. There are many techniques to achieve this goal, but for this first tutorial I want to propose you the the method that I consider the easiest one: the use of force fields. In this approach the target attracts the robot like a gravitational field and obstacles have an opposite force that keeps the robot away.

In this way the robot goes directed toward the targets, keeping distant from the obstacles.

Let's analyze the details of this model.

The obstacles:

Every obstacle has a force field that keeps the robot away. The formula of the force of this field is built by using the Newton gravitation formula, but assigning a negative mass to the obstacle. This because we don't want the robot to be actracted by obstacles. The principle is that the force must be inversely proportional to the square of the distance:

F(o) = o.m / ( o.dist^2 )

where o is the obstacle and m is the mass.

To allow the robot to pass near the obstacles at low speed we can make the field proportional to the speed of the robot:

F(r,o) = k*r.speed*o.m / ( o.dist^2 )

where r is the robot and k is a proportional parameter.

In order to make computation easiest for the computer we can say that the field is activated only when the robot is near the obstacle:

if(dist

F(r,o) = k*r.speed*o.m / ( o.dist^2 );

}

else{

F(r,o) = 0;

}

The goal:

The formula for the goal field should be a little bit different. In fact if we use a force that is inversely proportional to the square of the distance we would have a slow start when the robot is far from the target and infinite acceleration when the robot is near the goal. Actually we want the inverse behavior.

So we define the force as directly proportional to the distance:

F(g) = g.m * g.dist

The robot:

As we said, our robot can go in any direction so we simply use the sum of all forces to find the robot acceleration. Then we use acceleration to find the robot speed. We always talked about a robot that navigate a 2-dimensional space, but immagine this space 3-dimensional or n-dimensional. Actually this space could be the C-Space of a n dof manipulator.

Pros:

+ easy method

+ smooth path if parameters are well setted

+ relative fast computation
+ good even in a dynamic world

Cons:

- necessity to trim parameters

- suffers of local optimals

- not suitable for even small complex scenarios (eg. labyrinths)

Possible applications:

- Homing missiles or torpedos

- Space ships

- Robot arms in simple environment

- Support for higher level path planners

Here's a simple applet that uses field forces to navigate a ball-robot to a ball-goal, avoiding ball-obstacles:

http://robotapps.appspot.com/forcenav