Rolling Horizon Trajectory Optimization

• Rolling horizon trajectory optimization is a framework that divides global path planning into sequential finite-horizon optimal control problems for generating dynamic, collision-free trajectories.
• It employs methods like two-layer convex patch models and topology-driven parallel MPC to handle complex constraints and ensure robust performance in dynamic environments.
• Empirical results demonstrate improved safety, faster trajectory completion, and reliable convergence by integrating global guidance with local real-time replanning.

Rolling horizon trajectory optimization is a computational paradigm for generating collision-free, dynamically feasible trajectories for robotic or autonomous systems in real-time, particularly within complex, cluttered, and dynamic environments. The strategy decomposes the global planning problem into a sequence of interleaved finite-horizon optimal control subproblems, solved at each discrete planning instant over a receding time window. This approach enables efficient local refinement, frequent replanning in the face of environmental uncertainty, and integration of both combinatorial and continuous optimization layers for handling complex constraints—including obstacle avoidance, kinematic bounds, and topology-class decomposition. The method encompasses various instantiations including receding horizon control (RHP), two-layer convex QP patching, and topology-driven parallel MPC architectures, each substantiated by empirical and theoretical evidence for robustness, convergence, and computational tractability (Groot et al., 2024, Stachowicz et al., 2021, Tan et al., 14 Mar 2025, Bergman et al., 2019).

1. Problem Formulation and Mathematical Structure

Rolling horizon trajectory optimization is formulated at each planning cycle as a finite-horizon optimal control problem. Given a system with state $x(t)\in\mathbb{R}^{n_x}$ (or $z(t)$ in specific models) and control $u(t)\in\mathbb{R}^{n_u}$, the core discretized problem over horizon $T$ divided into $N$ steps is: $$\begin{aligned} \min_{\{x_k,\,u_k\}_{k=0}^{N-1}} \quad & J\bigl(\{x_k,\,u_k\}\bigr) \ \text{s.t.} \quad & x_{k+1} = f_d(x_k,\,u_k),\quad x_0 = x_{\rm init} \ & g_i(x_k,\,u_k)\le0 \qquad \forall k,\,i. \end{aligned}$$ Here, $f_d$ is the discrete-time dynamics (e.g., forward Euler of $\dot{x}=f(x,\,u)$) and $g_i$ collect collision, input and state constraints. Typical cost functions balance proximity to reference path $\gamma_k$, control effort, and smoothness: $$J = \sum_{k=0}^{N-1} (x_k-\gamma_k)^\top Q(x_k-\gamma_k) + u_k^\top R u_k + \|\Delta u_k\|_{S}^2$$ with $\Delta u_k=u_k-u_{k-1}$, and weighting matrices $Q,R,S\succeq0$ (Groot et al., 2024, Tan et al., 14 Mar 2025, Bergman et al., 2019).

For nonlinear systems or unknown time-to-go, horizon length $T$ can itself be a decision variable, as in optimal-horizon MPC: $$\min_{T,\,\{u_k\}_{k}} \sum_{t=0}^{T-1}\ell(x_t,u_t) + \Phi(x_T)$$ subject to $x_{t+1}=f(x_t,u_t)$ (Stachowicz et al., 2021).

2. Rolling Horizon Planning Cycle

At each discrete planning instant $t_0$, a finite-horizon problem is solved over $[t_0,\,t_0+T]$, yielding states $x_k$, controls $u_k$ for $k=0,\ldots,N$, with $\Delta t=T/N$. After optimization, only the first control $u_0^*$ (or several steps $\hat{N}$) is applied to the plant; the cycle then advances to $t_1=t_0+\Delta t$ (or $t_0+\hat{N}\Delta t$), where the process repeats. This rolling receding horizon scheme allows the controller to adapt to environment changes, new sensor information, and prediction errors at high replanning frequencies (10–30 Hz demonstrated empirically (Groot et al., 2024, Tan et al., 14 Mar 2025)).

3. Algorithmic Frameworks and Parallel Architectures

3.1 Two-Layer Convex Patch Models

A prominent approach decomposes each cycle into two convex QP subproblems (Tan et al., 14 Mar 2025):

• Outer Layer: Solve an unconstrained QP for dynamics, input bounds, and progress towards goal, neglecting obstacles.
• Inner Layer: Construct convex “funnels” $\mathcal{Z}_i$ around nominal and seed trajectories, enforce collision constraints and apply a safety penalty $\rho$ via a second QP over restricted sets. Incremental seed generation employs CVAPF or CDWA algorithms for fast collision-free initialization. Only a subset $\hat{N}$ of the solution is executed, advancing the system while mitigating “connective infeasibility.”

3.2 Topology-Driven Parallel MPC

Topology-driven rolling-horizon architectures sample multiple guidance trajectories $\tau_i$ in distinct homotopy classes via global planners (Groot et al., 2024). Each guidance path seeds a local MPC problem subject to topology constraints (linear half-space constraints tied to obstacle boundaries). The $P$ MPC instances are optimized in parallel, with the lowest-cost feasible trajectory selected for execution, ensuring both diversity of evasive strategies and consistency (tie-breaking by previous cycle's class). Parallelization (4–8 CPU cores) reduces per-cycle latency to 30–50 ms.

4. Obstacle Avoidance, Constraints, and Topological Considerations

Collision avoidance constraints utilize signed-distance functions $g_i(x_k)\ge0$, linearized to half-space representations for tractable optimization. Dynamic obstacles require predicted states $o_k^{i'}$, constrained via $g_{i'}(x_k,o_k^{i'})$. Topology constraints prevent switching homotopy classes mid-cycle, ensuring that optimized trajectories respect global path structure in time-extended state-space. In practice, homology and H-signature approximations classify distinct path families (Groot et al., 2024).

Convex funnel construction in two-layer models yields per-step domains $\mathcal{Z}_i$, defined as convex hulls over seed and nominal waypoints, tightened via user-tuned slopes $k_i$ for clearance (Tan et al., 14 Mar 2025).

5. Theoretical Guarantees and Performance Metrics

Analyses substantiate several properties across frameworks:

• Recursive Feasibility: If nominal trajectories are feasible, then each rolling-horizon iteration admits a feasible candidate, with cost-to-go represented by splicing on dynamically feasible manifolds (Bergman et al., 2019).
• Monotonic Objective: Full-horizon cost is non-increasing at each iteration, i.e., $$J_{\rm tot}(x_0,\bar{u}_k)\le J_{\rm tot}(x_0,\bar{u}_{k-1})$$ (Bergman et al., 2019).
• Finite-Time Convergence: Number of rolling-horizon iterations is bounded, ensuring guaranteed task completion given progress at each cycle.
• Local/Global Patchwise Optimality: Under sufficient progress and interior-linking controls, concatenated horizon-solutions achieve local optimality; global optimality holds when convex sets avoid all obstacles (Tan et al., 14 Mar 2025).

Empirical results demonstrate 100% safety (no collisions), reduced completion times (15–20% faster trajectories vs. single-horizon MPC), increased safety margins, and significant reductions in indecisive maneuvers for topology-driven parallel optimization (Groot et al., 2024, Stachowicz et al., 2021).

Example Performance Comparison

Approach	Success Rate (%)	Avg. Completion Time (s)	Safety Margin (m)	Planning Latency
Single-horizon MPC	85	3.2	0.15	4.8 ms/cycle
Optimal-horizon MPC (DDP-style)	97	2.7	0.23	5.2 ms/cycle
Topology-driven parallel MPC	100	—	—	30–50 ms/cycle
Two-layer convex patch (CVAPF)	91	16.4	>0.27	35% RCT
Two-layer convex patch (CDWA)	91	15.7	>0.27	37% RCT

6. Parameter Selection, Solver Choices, and Limitations

Key parameters across rolling-horizon frameworks include horizon length ($T=1$–$5$ s), number of discretization steps ($N=20$–$50$), and applied control step ($\hat{N}\approx50\%$ of $N$ for robustness). Weights $(Q,R,S)$ tune trade-offs between aggressiveness and smoothness.

Popular optimization methods: SQP (e.g., FORCES Pro), iLQR for fast local convergence, multiple-shooting CasADi+SQP (WORHP) for general nonlinear systems. Complexity per planning cycle is polynomial in $N$ (typically $O(N^3)$ per QP), manageable for real-time implementation.

Limitations include possible mismatch between discrete-time and continuous-time feasibility (allowing in-between collisions), increased linearization error for excessively large horizons, and connective infeasibility if applying full-horizon control naively (Tan et al., 14 Mar 2025, Bergman et al., 2019).

7. Extensions, Practical Guidelines, and Domains of Application

Rolling horizon trajectory optimization finds application in autonomous ground robot navigation among moving obstacles (e.g., pedestrian avoidance), intelligent vehicles, manipulator motion planning, parking scenarios for multi-unit vehicles, and aerial vehicle control.

Guidelines for robust deployment:

• Choose horizon and step sizes so that linearization remains valid while progress per cycle is significant.
• Employ topology classification (homology/H-signature) for environments with multiple evasive routes.
• Ensure per-step convex sets are tight for cluttered environments and relaxed for open spaces.
• Tune safety weights and convex funnel slopes to maintain obstacle clearance.
• Incremental seed generation should account for anticipated obstacle movement over the prediction horizon.

A plausible implication is that architectures combining global path diversity, parallel optimization, and local convexification outperform single-horizon or purely local MPC, both in safety and speed, especially as complexity and dynamism of the environment increase.

Rolling horizon trajectory optimization provides a theoretically-guaranteed, computationally efficient foundation for real-time safe motion generation in domains with stringent constraints and rapidly changing environments (Groot et al., 2024, Stachowicz et al., 2021, Tan et al., 14 Mar 2025, Bergman et al., 2019).