A Fast Semidefinite Convex Relaxation for Optimal Control Problems With Spatio-Temporal Constraints

TL;DR

This work tackles the nonconvex global OCPs that couple trajectory and gate-crossing times under spatio-temporal constraints. It combines a time-scaling direct multiple shooting formulation with a fast, structure-preserving SDP relaxation that exploits sparsity to produce tight approximations and high-quality initial guesses for NLP refinement. Key contributions include the segment-wise time-scaling variables $\theta_i$, a lifted SDP framework using sparse $\tilde{Y}_{k,i}$ matrices, and a rigorous intra-/inter-segment continuity structure, all demonstrated on eco-driving-like benchmarks and a quadrotor waypoint task under time windows. The approach enables efficient, near-global solutions with practical applicability to CAV and UAV planning in complex environments, including real-world quadrotor experiments.

Abstract

Solving optimal control problems (OCPs) of autonomous agents operating under spatial and temporal constraints fast and accurately is essential in applications ranging from eco-driving of autonomous vehicles to quadrotor navigation. However, the nonlinear programs approximating the OCPs are inherently nonconvex due to the coupling between the dynamics and the event timing, and therefore, they are challenging to solve. Most approaches address this challenge by predefining waypoint times or just using nonconvex trajectory optimization, which simplifies the problem but often yields suboptimal solutions. To significantly improve the numerical properties, we propose a formulation with a time-scaling direct multiple shooting scheme that partitions the prediction horizon into segments aligned with characteristic time constraints. Moreover, we develop a fast semidefinite-programming-based convex relaxation that exploits the sparsity pattern of the lifted formulation. Comprehensive simulation studies demonstrate the solution optimality and computational efficiency. Furthermore, real-world experiments on a quadrotor waypoint flight task with constrained open time windows validate the practical applicability of the approach in complex environments.

Figures (6)

• Figure 1: Time-scaling illustration.
• Figure 2: Representative trajectories with active upper (top) and lower (bottom) characteristic time constraints.
• Figure 3: Comparison of computation time across Standard SDR, Fast SDR, and Refined SDR.
• Figure 4: Time-lapse of an experiment showing the trajectory executed by the quadrotor. To better visualize the evolution of the time windows during the experiments, an indicator is placed beneath each waypoint: a green light $\boldsymbol{\bullet}$ signified that the waypoint was within its open time window and could be safely crossed, whereas a red light $\boldsymbol{\bullet}$ indicated that passage through the waypoint was temporarily prohibited.
• Figure 5: Trajectories obtained from real-world experiments in two scenarios. On the left, the position with the velocity color scheme is shown. On the right, the projected horizontal position is plotted over time on the z-axis. The orange bars visualize the open or closed temporal constraints.