Receding Fixed-Horizon Optimization for Near-Time-Optimal Trajectory Planning and Control

Abstract

Time-optimal trajectory planning and control is central for autonomous vehicles, yet its application and real-time deployment confronts two fundamental challenges: the non-convexity of optimal control problems and the unpredictable computation time inherent to nonlinear programming. To address these challenges, we propose a hierarchical convex optimization framework that addresses both issues by decomposing the original problem into short, fixed-horizon planning cycles. Each cycle solves a convex subproblem within a collision-free region identified by a customized search algorithm; the complete trajectory and control is assembled by concatenating state-input sequences across cycles. Under mild assumptions, we establish finite-time convergence of the decomposition procedure and show that the concatenated solution satisfies the necessary conditions for local optimality. Numerical experiments on randomly generated maps with static and dynamic obstacles demonstrate that the proposed algorithm achieves a higher success rate and substantially lower computation time than sequential convex programming, while maintaining comparable control time. These results show that decomposition-based convex optimization provides a practical pathway to reliable, real-time near-time-optimal trajectory planning.