Extracting optimal paths from roadmaps for motion planning

We present methods for extracting optimal paths from motion planning roadmaps. Our system enables any combination of optimization criteria, such as collision detection, kinematic/dynamic constraints, or minimum clearance, and relaxed definitions of the goal state, to be used when selecting paths from roadmaps. Our algorithm is an augmented version of Dijkstra's shortest path algorithm which allows edge weights to be defined relative to the current path. We present simulation results maximizing minimum path clearance, minimizing localization effort, and enforcing kinematic/dynamic constraints.