Efficient motion planning for problems lacking optimal substructure

TL;DR

This paper introduces a novel motion planning algorithm for risk-aware paths that do not satisfy optimal substructure, extending Dijkstra's algorithm to handle super-linear risk penalties in robotic navigation.

Contribution

The paper proposes a new path-finding algorithm for non-optimal substructure problems, applicable to risk-aware motion planning with a specific cost function, and demonstrates its efficiency and effectiveness.

Findings

Algorithm runs in near-linear time with respect to graph size.

Produced paths effectively balance length and risk exposure.

Simulations validate both path quality and computational performance.

Abstract

We consider the motion-planning problem of planning a collision-free path of a robot in the presence of risk zones. The robot is allowed to travel in these zones but is penalized in a super-linear fashion for consecutive accumulative time spent there. We suggest a natural cost function that balances path length and risk-exposure time. Specifically, we consider the discrete setting where we are given a graph, or a roadmap, and we wish to compute the minimal-cost path under this cost function. Interestingly, paths defined using our cost function do not have an optimal substructure. Namely, subpaths of an optimal path are not necessarily optimal. Thus, the Bellman condition is not satisfied and standard graph-search algorithms such as Dijkstra cannot be used. We present a path-finding algorithm, which can be seen as a natural generalization of Dijkstra's algorithm. Our algorithm runs in $O\left((n_B\cdot n) \log( n_B\cdot n) + n_B\cdot m\right)$ time, where~$n$ and $m$ are the number of vertices and edges of the graph, respectively, and $n_B$ is the number of intersections between edges and the boundary of the risk zone. We present simulations on robotic platforms demonstrating both the natural paths produced by our cost function and the computational efficiency of our algorithm.