An Algorithm for Generating Safe and Execution-Optimized Paths

This paper describes a new scheme for generating paths that are optimized for faster execution while keeping a certain amount of clearance away from obstacles. An optimized path is first extracted from multiple query PRM using a Lazy A* algorithm with a weighted Linfin norm, this latter is post-processed to correct its clearance to over a desired threshold in regions where it is geometrically possible. Finally, the path is further optimized using an iterative oversampling and searching algorithm. This iterative algorithm is monotonically convergent. This scheme is applied to extract optimized motions for robot manipulators with clearance requirements.