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.Comment: Submitted for IFAC World Congress 202