Optimal Path Planning By Cost Wave Propagation In Metric Configuration Space

In this paper we give a solution to the problem of finding an optimal path for an arbitrary robot among static obstacles, from its present state to the closest of a set of goal states. The path is optimal in the sense of minimizing some criterion (minimum motion, distance, effort, etc.). Our solution is the well-known heuristic search method `A* with h = 0' (no heuristic), applied to a graph representing configuration space, with a metric incorporating the optimization criterion. It is applicable to any robot, unrestricted in the number of degrees of freedom, and capable of handling a broad class of optimization criteria. The method works fairly fast for a few degrees of freedom. Examples in two dimensions are given.