FM2: A real-time fast marching sensor-based motion planner

Path planning for autonomous robots is an essential capacity for these systems. One class of path planning algorithms use potential fields. However, some problems associated with these algorithms include (1) Trapping due to local minima, (2) No passage between closely spaced obstacles, and (3) Limit cycles. This paper presents a potential-field-based algorithm that does not exhibit the mentioned problems. The algorithm uses a two- part approach. The first part of the approach generates a "velocity map" of the environment that represents the admissible velocity at discrete cells in the workspace. This velocity map of the environment gives a grey scale that is darker near the obstacles and walls (black means zero speed) and more clear far from them (white means a predefined maximum robot speed). This velocity map imitates the repulsive electric potential from walls and obstacles, but with a limited maximum value. In the second step, the application of the Fast Marching method on the velocity map, represents and gives a potential map of the wave propagation with time as last axis. The propagation of this wave uses the previous velocity map. In this potential surface it is calculated the shortest time trajectory, by using the gradient method. The second part of the approach uses concepts of wave propagation to construct a smooth, safe, and time efficient path in the velocity map. This method is very fast, and can be used on line.