An Optimization-Based Receding Horizon Trajectory Planning Algorithm

TL;DR

This paper introduces an optimization-based receding horizon trajectory planning algorithm that combines initial feasible path finding with local trajectory refinement, providing theoretical guarantees and tested on complex vehicle scenarios.

Contribution

It proposes a novel two-step trajectory planning method that integrates motion planning with optimization-based refinement, offering theoretical guarantees in complex environments.

Findings

Successfully plans trajectories in cluttered environments

Provides recursive feasibility and convergence guarantees

Validated on truck and trailer system scenarios

Abstract

This paper presents an optimization-based receding horizon trajectory planning algorithm for dynamical systems operating in unstructured and cluttered environments. The proposed approach is a two-step procedure that uses a motion planning algorithm in a first step to efficiently find a feasible, but possibly suboptimal, nominal solution to the trajectory planning problem where in particular the combinatorial aspects of the problem are solved. The resulting nominal trajectory is then improved in a second optimization-based receding horizon planning step which performs local trajectory refinement over a sliding time window. In the second step, the nominal trajectory is used in a novel way to both represent a terminal manifold and obtain an upper bound on the cost-to-go online. This enables the possibility to provide theoretical guarantees in terms of recursive feasibility, objective function value, and convergence to the desired terminal state. The established theoretical guarantees and the performance of the proposed algorithm are verified in a set of challenging trajectory planning scenarios for a truck and trailer system.