[Paper Review] Improved Path Planning by Tightly Combining Lattice-based Path Planning and Numerical Optimal Control

Kristoffer Bergman, Oskar Ljungqvist+1 more|arXiv (Cornell University)|Mar 19, 2019

Robotic Path Planning Algorithms26 references4 citations

TL;DR

This paper proposes a tightly integrated two-stage path planning method that combines lattice-based sampling with numerical optimal control to achieve locally optimal, dynamically feasible trajectories. By using the lattice planner’s combinatorial solution as a warm-start for a numerical optimal control solver—using identical dynamics and objective functions—the approach significantly improves convergence speed, numerical reliability, and solution quality compared to prior initialization strategies.

ABSTRACT

This paper presents a unified optimization-based path planning approach to efficiently compute locally optimal solutions to advanced path planning problems. The approach is motivated by first showing that a lattice-based path planner can be cast and analyzed as a bilevel optimization problem. This information is then used to tightly integrate a lattice-based path planner and numerical optimal control in a novel way. The lattice-based path planner is applied to the problem in a first step using a discretized search space, where system dynamics and objective function are chosen to coincide with those used in a second numerical optimal control step. As a consequence, the lattice planner provides the numerical optimal control step with a resolution optimal solution to the problem, which is highly suitable as a warm-start to the second step. This novel tight combination of a sampling-based path planner and numerical optimal control makes, in a structured way, benefit of the former method's ability to solve combinatorial parts of the problem and the latter method's ability to obtain locally optimal solutions not constrained to a discretized search space. Compared to previously presented combinations of sampling-based path planners and optimization, the proposed approach is shown in several path planning experiments to provide significant improvements in terms of computation time, numerical reliability, and objective function value.

Motivation & Objective

• To address the challenge of computing locally optimal, dynamically feasible paths for nonholonomic vehicles in cluttered environments.
• To overcome limitations of traditional sampling-based planners, which produce suboptimal solutions due to discretized search spaces.
• To improve numerical optimal control convergence by providing high-quality, feasible, and objective-function-aligned warm-starts.
• To unify lattice-based planning and numerical optimal control through a bilevel optimization formulation that reveals the suboptimality of lattice approximations.
• To demonstrate that using the same objective function in both planning and optimization stages enhances convergence and solution quality.

Proposed method

• Formulate the path planning problem as a bilevel optimization problem to analyze the relationship between the original optimal control problem and its lattice-based approximation.
• Use a lattice-based path planner with a discretized search space and a vehicle model that matches the dynamics and objective function used in the subsequent numerical optimal control step.
• Generate a resolution-optimal solution via graph search using motion primitives derived from the same system model and cost function as in the optimal control phase.
• Apply numerical optimal control (NOC) to refine the lattice solution, optimizing continuous trajectory parameters while preserving the combinatorial structure (e.g., path topology).
• Ensure tight coupling by aligning the system dynamics, control constraints, and objective function across both stages to enable reliable and fast convergence of the NOC solver.
• Use the lattice solution as a warm-start in the NOC phase, leveraging its feasibility and near-optimality to accelerate convergence and avoid local minima.

Experimental results

Research questions

• RQ1 Can a bilevel optimization formulation effectively analyze and quantify the suboptimality of lattice-based path planning?
• RQ2 How does using a lattice planner with identical dynamics and objective function as the NOC step improve convergence and solution quality in optimal control?
• RQ3 What is the impact of aligning the objective function in the lattice planner and the NOC phase on computation time and numerical reliability?
• RQ4 How does the proposed method compare to conventional initialization strategies (e.g., geometric or kinematic models) in terms of solution quality and convergence?
• RQ5 Can the tight coupling of sampling-based planning and numerical optimization reduce total planning time while improving objective function values?

Key findings

• The proposed method reduced total computation time by up to 40% compared to using geometric or kinematic initialization, with the fastest convergence observed when the same objective function was used in both stages.
• In the loading site scenario, the method achieved 100% convergence rate across 270 test cases, significantly outperforming other initialization strategies.
• When the lattice planner used the same objective function as the NOC step (γu = γl = γi = 10), the average improvement time was 9.4 seconds, with a 100% success rate and an objective function value of 190.
• Using a different objective function in the lattice phase (e.g., γu = 0) led to a 14.5-second total time and a higher objective value of 212, indicating poor initialization quality and slower convergence.
• The method achieved a 100% improvement rate in the loading site scenario when using dynamic model-based initialization (P_dyn), compared to only 43.7% with geometric initialization.
• The solution cost (J) was reduced from 236 (geometric) to 164 (kinematic) and finally to 164 (dynamic) in the final NOC step, demonstrating consistent improvement with better initialization.