We present a GPU-based solver for trajectory planning problems using branch model predictive control. Building on iterative LQR methods, we adopt a multiple-shooting formulation for the system dynamics and use an augmented Lagrangian method to handle general stage-wise constraints. This design enables straightforward warm-starting. The constraint-handling capability of our solver is validated on two challenging trajectory planning problems. In addition, we develop two tailored inner LQR solvers that exploit the tree-sparse structure. The solvers offer different levels of parallelism, making them appropriate for different tree sizes. The numerical results demonstrate that, compared to a high-performance CPU-based solver, our approach achieves superior performance on large-scale problems.
Figures & tables
Solver
Problem Type
NC handling
Parallelization
Devices
sLQR (Ours)
NT MPC
Exact
SC
CPU/GPU
stLQR (Ours)
NT MPC
Exact
SC + Time
CPU/GPU
CT [ 34 ]
NT MPC
Inexact
SC
CPU
HPIPM [ 14 ]
LT MPC
Exact
×
CPU
treeQP [ 26 ]
LT MPC
Exact
SC
CPU
NAMA [ 37 ]
LT MPC
Exact
SC
GPU
TABLE I: Comparisons of different tree MPC solvers. NT: Nonlinear Tree. LT: Linear Tree. NC: Nonanticipativity Constraint. SC: scenario
Fig. 1: Parallel scan applied to the reverse cumulative sum problem. We process the array using a balanced binary tree. The tree nodes are highlighted in color. A reduction operation is performed in the up-sweep phase, leading to ∑i=07ai . Then, a down-sweep is conducted, starting from the root of the computation tree. For instance, at the first level, the value of the root is passed to its right and left children, and the value of the root’s right child in the up-sweep ∑i=47ai is added to the root’s left child in the down-sweep.
Fig. 2: (a) Intersection-crossing scenario. The other vehicle (in pink) has two potential driving decisions: “Yield” and “Assert”. The BMPC planner utilizes the trajectory tree to handle both scenarios. (b) Trajectory tree (top) with 12 nodes (N={0,1,…,11}) , two branching nodes (B={1,5}) , and three leaves (L={9,10,11}) . For node 5 , p(5)=3 and ch(5)={7,8} . A chain is a path whose internal nodes each have a single child; the dashed outline highlights the chain 1→2→4→6→9 . The reduced tree (bottom) replaces each such chain with a single edge while retaining the root, branching nodes, and leaves, giving Nred={0,1,5,9,10,11} . For example, the highlighted chain becomes the edge 1→9 after reduction.
Fig. 3: (a) Tree example used in numerical experiments with Nr=2 and nd=2 . (b) Batch of trajectories.
Fig. 4: Solve time versus horizon length. Labels above the bars show the speedup relative to sLQR .
Fig. 5: Solve time versus batch size. (a) State dimension nx=10 with FP32. (b) State dimension nx=10 with FP64. (c) State dimension nx=30 with FP64.
Fig. 6: Solve time versus number of tree paths. The corresponding robust horizon length varies from 1 to 10. The labels ‘4090’ and ‘5060 Ti’ refer to the GPU models (NVIDIA GeForce RTX 4090/5060 Ti).
Fig. 7: Test problems. (a) Quad-pendulum (nx=8,nu=2) . The discretization timestep is Δt=0.02\text{,}\mathrm{s} and the horizon length is $N=200$ . (b) Unicycle $(n_{x}=4,n_{u}=2)$ . The discretization timestep is $\Delta t=$0.1\text{\,}\mathrm{s} and the horizon length is N=50 .
Fig. 8: Proportion of problems solved versus the number of solver iterations. (a) Quad-pendulum. (b) Unicycle.
We present FAME-QP, a GPU-accelerated solver for dense quadratic programs arising in linear model predictive control. FAME-QP combines constraint-dependent operator representations with fused single- and two-kernel iterations replayed through CUDA graphs, automatically selecting the execution configuration through device calibration and problem-specific timing. It probes a cached operator bank to select the initial penalty and updates operators lazily when problem matrices change. Numerical benchmarks and simulated humanoid and quadrotor control experiments demonstrate reduced latency relative to competing GPU solvers while maintaining solution accuracy and control performance.
Model Predictive Control (MPC) delivers constraint-aware control, but its reliance on online optimization limits its use on systems with fast dynamics, high-dimensional models, or long horizons. Existing GPU implementations typically treat the device as a linear-algebra accelerator, leaving the optimization loop dependent on repeated kernel launches and high-latency memory transfers. This paper introduces CUDA MPC, a GPU-native MPC framework that co-designs the optimization algorithm, execution model, and memory architecture for CUDA hardware. CUDA MPC pairs a parallel-in-horizon alternating direction method of multipliers (ADMM) splitting with a fused CUDA kernel that runs the entire iterative solve on the device. Intermediate optimization variables stay in low-latency, on-chip shared memory, and a localized atomic-flag protocol synchronizes only adjacent horizon blocks, minimizing host intervention, kernel-dispatch overhead, and global-memory traffic. Across six nonlinear robotics benchmarks spanning increasing state dimension and constraint density, CUDA MPC sustains real-time rates at horizons one to two orders of magnitude longer than CPU solvers: it solves an optimization-based collision-avoidance parking problem with 100 s of lookahead within a 0.1 s sampling interval, and is the only solver evaluated that achieves both real-time execution and collision-free coordination for a centralized 10-agent swarm, where acados and CasADi return no feasible solution and require 3.5 s and 4.5 s per solve. Against tensor-framework implementations of the same ADMM splitting, the fused kernel is up to 965× faster.
Babak Akbari, Melissa Greeff
Robora Lab, Department of Electrical and Computer Engineering Queen's University, Kingston, ON, Canada
We present GPU-SLS, a GPU-parallelized framework for safe, robust nonlinear model predictive control (MPC) that scales to high-dimensional uncertain robotic systems and long planning horizons. Our method jointly optimizes an inequality-constrained, dynamically-feasible nominal trajectory, a tracking controller, and a closed-loop reachable set under disturbance, all in real-time. To efficiently compute nominal trajectories, we develop a sequential quadratic programming procedure with a novel GPU-accelerated quadratic program (QP) solver that uses parallel associative scans and adaptive caching within an alternating direction method of multipliers (ADMM) framework. The same GPU QP backend is used to optimize robust tracking controllers and closed-loop reachable sets via system level synthesis (SLS), enabling reachability-constrained control in both fixed- and receding-horizon settings. We achieve substantial performance gains, reducing nominal trajectory solve times by 97.7% relative to state-of-the-art CPU solvers and 71.8% compared to GPU solvers, while accelerating SLS-based control and reachability by 237x. Despite large problem scales, our method achieves 100% empirical safety, unlike high-dimensional learning-based reachability baselines. We validate our approach on complex nonlinear systems, including whole-body quadrupeds (61D) and humanoids (75D), synthesizing robust control policies online on the GPU in 20 milliseconds on average and scaling to problems with 2 x 10^5 decision variables and 8 x 10^4 constraints. The implementation of our method is available at https://github.com/Jeff300fang/gpu_sls.
Jeffrey Fang, Glen Chou
Georgia Institute of Technology, Atlanta, GA 30308