Path-planning in Discretized Environments with Optimized Waypoints Computation

This paper considers the path-planning problem in discretized environments, obtained for example by a cell decomposition approach. The specification for the mobile robot can be the classical navigation problem (reach a given region by avoiding the obstacles) or a high-level specification as a Boolean and/or temporal logic formula. We propose a general methodology to compute piecewise linear trajectories consisting in a sequence of intermediate points (waypoints). The waypoints are computed by solving optimization problems whose solutions permit to optimally select the intermediate points on the common facets of traversed cells from the decomposition. The proposed solution is similar to a Model Predictive Control (MPC) strategy, in each step an optimization problem is solved over a finite horizon, the first action is considered and the problem is iterated. The method developed in this paper has been implemented and integrated in Robot Motion Toolbox allowing a comparison with other methods by simulation.