The Problem

We address the problem of robot path planning in uncertain worlds, that is, when the positions of the map features are not known exactly. This is a typical situation when the map of the world is obtained by a moving robot equipped with sensors. The acquired data contains noise due to measurement noise on the sensors and odometry errors from robot motions. Unlike most previous approaches which pick the most likely position and fix it, we model the obtained map stochastically, giving a distribution over possible maps. Next, we design probabilistic methods that appropriately handle an uncertain world model. We utilize these methods within the framework of the Probabilistic Roadmap.

Motivation

Randomized planning technique such as the Probabilistic Roadmap (PRM) [1] is responsible for much of the recent success in robot motion planning. Unfortunately, it assumes perfect and complete map, and can fail arbitrarily badly if the map has errors. In contrast, robot systems strongly depend on localization and mapping algorithms that maintain models of perceptual uncertainty to compensate for sensor error. For example, a point-feature-based SLAM algorithm [2] such as the Kalman filter represents point feature distributions as multivariate Gaussians. Similarly, we represent the uncertainty in the obstacle position as probability distributions over obstacle vertices (see figure 1).

Figure 1: An example uncertain world. (a) The maximum-likelihood map, shown with distributions on the vertices, along with start (lower left) and goal locations. Ellipses visualize one Σ deviation from the mean. (b) Example maps sampled from distributions.

Background

The power of the original PRM resides in the sampling stage, which exploits the fact that even if the C-free (space of all collision-free poses of a robot) cannot be computed and represented tractably, it is relatively efficient to determine if an arbitrary pose lies in the C-free. Therefore, the PRM approximates the C-free by randomly sampling poses according to some strategy, and then rejecting poses that do not lie within the C-free. It is common to combine different sampling approaches such as uniform [3], Bridge [4], and Gaussian [5] in order to focus on expressing different features of the map (eg. narrow corridors etc). Using the samples, and including the start and goal nodes, PRM builds a k-connected graph (assuming a user-specified k number of neighbors for each graph node) using a local planner. Finally, a graph search is performed on the nodes in order to find the lowest cost path between the start and goal node. Since the trajectory search process may be quadratic (Dijkstra search algorithm [6]) in the sample number or exponential (A*) in the length of the path, minimizing the set of samples helps to keep the planning problem tractable.

Figure 2: The minimum cost paths for a fixed set of samples using A* planner with a conventional, minimum distance cost function (solid) and a minimum expected cost function based on collision likelihood (dashed).

Approach

First, we model the uncertainty of the world with distritbutions of its point features, in figure 1 the vertices of the polygonal obstacles. We represent the uncertainty of the location of each vertex via a multivariate Gaussian distribution.

Next, we adapt PRM's sampling to handle uncertainty. We designed probabilistic techniques that evaluate the validity of a sample in order to decide whether to keep it or not. We put more samples (biasing the trajectories) in the regions where the robot has more chance to pass through without collision. Our results show that the adapted sampling strategies substantially reduce the number of samples (see figure 3) required to express good (less collision-prone) trajectories in uncertain worlds.

Figure 3: The performance of PRM algorithms as a function of the sample number, using original and adapted sampling. The planner is probabilistic in both cases. Notice that the planner performance, measured by the percentage of collisions for small sample numbers, is improved by using a more appropriate sampling technique.

Finally, in addition to sampling, we adapt the PRM planning stage to utilize our stochastic methods by extending the basic A* planner to incorporate the expected collision costs. The resulting trajectories respect the uncertainty in the world model.

We have results showing success of our algorithm for 2D world maps, generating trajectories that are robust to errors in the world model thus substantially less collision-prone (figure 3).

Future Work

We plan on extending probabilistic models to higher dimensions, including robots with multiple degrees of freedom. The challenges here are both in selecting a good data model, as well as determining new ways of incorporating uncertainty into sampling in higher dimensions, into graph costs, and trajectory selection.

Research Support

The support of Draper Laboratories under the URPP grant "Robust Distributed Sensor Networks" is gratefully acknowledged.

References:

[1] L. E. Kavraki, P. Svestka, J.-C. Latombe, and M. Overmars, ``Probabilistic roadmaps for path planning in high dimensional configuration spaces,'' Transactions on Robotics and Automation, vol. 12, no. 4, pp. 566--580, 1996.

[2] J. Leonard and H. Durrant-Whyte, ``Mobile robot localization by tracking geometric beacons,'' IEEE Transactions on Robotics and Automation, vol. 7, no. 3, pp. 376--382, June 1991.

[3] Y. K. Hwang and N. Ahuja, ``Gross motion planning - a survey,'' ACM Comput. Surv., vol. 24, no. 3, pp. 219--291, 1992.

[4] D. Hsu, T. Jiang, J. Reif, and Z. Sun, ``The bridge test for sampling narrow passages with probabilistic roadmap planners,'' in Proceedings of the IEEE International Conference on Robotics and Automation, 2003, pp. 4420--4426.

[5] V. Boor, M. H. Overmars, and A. F. van der Stappen, ``The gaussian sampling strategy for probabilistic roadmap planners,'' in Proceedings of the 1999 IEEE International Conference on Robotics and Automation, vol. 1, 1999, pp. 1018--1023.

[6] E. W. Dijkstra, ``A note on two problems in connexion with graphs,'' in Numerische Mathematik. Mathematisch Centrum, Amsterdam, The Netherlands, 1959, vol. 1, pp. 269--271. Available online.