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.