Computing a set of local optimal paths through cluttered environments and over open terrain

This paper describes an efficient algorithm to generate a set of local optimal paths between two given end points in cluttered environments or over open terrain. The local optimal paths are selected from the set of shortest constrained paths through every node (one for each path) in the graph, generated by running twice a "single-source" search. The initial set of the shortest constrained paths spans the entire search space and includes local optimal paths with costs equal or better than the longest constrained path in the set. The search for the optimal path is transformed to a search for the best path in each homotopy class generated by this search. The initial search is of complexity O(nlogn), and the pruning procedure is O(nmlogm), where n is the number of nodes and m is the number of homotopy classes generated by this search. The algorithm is demonstrated for motion planning on rough terrain.