Improved Path Planning by Tightly Combining Lattice-Based Path Planning and Optimal Control

This paper presents a unified optimization-based path planning approach to efficiently compute locally optimal solutions to optimal path planning problems in unstructured environments. The approach is motivated by showing that a lattice-based planner can be cast and analyzed as a bilevel optimization problem. This insight is used to integrate a lattice-based planner and an optimal control-based method in a novel way. The lattice-based planner is applied to the problem in a first step using a discretized search space. In a second step, an optimal control-based method is applied using the lattice-based solution as an initial iterate. In contrast to prior work, the system dynamics and objective function used in the first step are chosen to coincide with those used in the second step. As an important consequence, the lattice planner provides a solution which is highly suitable as a warm-start to the optimal control step. This proposed combination makes, in a structured way, benefit of sampling-based methods ability to solve combinatorial parts of the problem and optimal control-based methods ability to obtain locally optimal solutions. Compared to previous work, the proposed approach is shown in simulations to provide significant improvements in terms of computation time, numerical reliability and objective function value.