Construction and use of roadmaps that incorporate workspace modeling errors

Probabilistic Roadmap Methods (PRMs) have been shown to work well at solving high Degree of Freedom (DoF) motion planning problems. They work by constructing a roadmap that approximates the topology of collision-free configuration space. However, this requires an accurate model of the robot's workspace in order to test if a sampled configuration is in collision or not. In this paper, we present a method for roadmap construction that can be used in workspaces with uncertainties in the model. For example, these can be inaccuracies that are caused by sensor error when an environment model was constructed. The uncertainty is encoded into the roadmap directly through the incorporation of non-binary collision detection values, e.g., a probability of collision. We refer to this new roadmap as a Safety-PRM because it allows tunability between the expected safety of the robot and the distance along a path. We compare the computational cost of Safety-PRM against two planning methods for environments without modeling errors, basic PRM and Medial Axis PRM (MAPRM), known for low computational cost and maximizing clearance, respectively. We demonstrate that in most cases, Safety-PRM produces high quality paths maximized for clearance and safety with the least amount of computational cost. We show that these paths are tunable for both robot safety and clearance. Finally, we demonstrate the applicability of Safety-PRM on an experimental system, a Barrett Whole Arm Manipulator (WAM). On the WAM, we demonstrate the mapping of expected collision to robot speeds to enable the robot to physically test the safety of the roadmap and use torque estimation to make roadmap modifications.

I. Introduction

The motion planning problem consists of finding a valid (collision-free) path from a start state to a goal state. One solution to this problem is to define a roadmap that captures the topology of the collision-free portion of configuration space. However, the complexity of the workspace and robot can make this process challenging. Probabilistic Roadmap Methods (PRMs) have addressed this challenge by constructing a roadmap of randomly sampled robot configurations and testing each configuration for collision [20]. Connections are made between two samples when a collision-free transition can be made. These samples (vertices) and connections (edges) define a roadmap that the robot can safely traverse. Recently, PRMs have been extended to be adaptable [26] [4]. These new methods can deform paths [18], update roadmaps due to moving obstacles [15] [27], map both collision and collision-free states [9], and deal with uncertainty in the motion model [6] [2] [1] [25]. However, despite all these advances, PRMs require that the model of the problem must be accurate, e.g., there must be a clear delineation between collision and collision-free states. Error-prone collision detection can lead to erroneous roadmaps which produce feasible paths in the modeled environment but lead to collisions in the actual world. Fig. 1:

Whole arm manipulator (WAM) feeling an obstacle boundary.