Introduction to Fast Marching

The FM algorithm was introduced by J. Sethian in 1996 and is a numerical algorithm that approximates the viscosity solution of the Eikonal equation which represents, among other,
The FM method is used to solve the Eikonal equation and is very similar to the Dijkstra algorithm that finds the shortest paths on graphs, though it is applied to continuous media.

Fast Marching and Motion Planning

To get a Motion Planner for mobile robots with desirable
properties, such as smoothness and safety, we can think of
attractive potentials. In Nature, there are phenomena with a
similar behaviour, e.g., the electromagnetic waves. If there
is an antenna in the goal point that emits an electromagnetic
wave, then the robot can drive to the destination by tracing
the waves back to the source. In general, the concept of
electromagnetic waves is especially interesting, since the potential and its associated vector field have the good properties
desired for the trajectory, such as smoothness and the absence
of local minima.

This attractive potential still has some problems. The most
important one that typically arises in mobile robotics, is
that optimal motion plans may bring robots too close to
obstacles or people, which is not safe. To obtain a safe
path, it is necessary to add a component that repels the robot
from obstacles. In addition, this repulsive potential and its
associated vector field should have good properties such as
those of electrical fields. If we consider that the robot has
an electrical charge of the same sign as obstacles, then the
robot would be pushed away from obstacles. The properties
of this electric field are very good because it is smooth and
there are no singular points in the interest space.

To help understand the Fast Marching Path Planning basis method, let us suppose a two dimensional wave propagating in a homogeneous medium. The front wave is then a circle propagating outwards the initial point. If an additional axis is added to represent the time, the results is as shown in the next figure:

Now, if the initial point of the wave propagation are all those points which represents obstacles in a binary occupancy map, we obtain a map in which the value for each cell is proportional to the distance to the nearest obstacle, as shown in the next figures:

An the path obtained over this new “distances” map, applying the gradient method is:

FM2: Fast Marching Square

The path obtained applying Fast Marching directly is non-smooth and runs too close to obstacles, being not safe at all. The solution we propose is to use the “distances” map obtained applying Fast Marching as a slowness map. This means that the lower is the value for a given cell the closer it is to an obstacle (or wall) thus the velocity has to be slower.

Then, a wave is propagating from the goal point until it reaches the current position of the robot. For this propagation, the velocity of the wave for each cell is proportional to the value of the slowness map for that cell. Then it is obtained a map in which each cell has a value for the time the wave lasts to reach that cell. This map will never have local minima, since the velocity of the wave is always non negative.

The map with the time values applying Fast Marching over the previous slowness map is:

And applying the gradient method from the goal point to the initial point the path obtained is:

The result is a path much more smooth, safer and optimal in time.

We already proposed other alternatives such as Voronoi Fast Marching. Please, see the publications list below to find more information.

FM Applications

This proposed path planning has been applied successfully to:

– 2D and 3D path planning.
– Exploration and SLAM.
– Robot formations.
– Outdoor path planning.