cuNRTO: GPU-Accelerated Nonlinear Robust Trajectory Optimization

1. Problem Statement

The paper addresses Robust Trajectory Optimization (RTO) for autonomous systems operating under deterministic uncertainty.

• Context: Real-world robotic systems (e.g., drones, manipulators) face bounded disturbances (e.g., wind, sensor noise, modeling errors) that must be accounted for to ensure safety.
• Challenge: Standard trajectory optimization often fails when constraints (like obstacle avoidance) must be satisfied for all possible realizations of uncertainty within a bounded set.
• Current Limitations: The existing Nonlinear Robust Trajectory Optimization (NRTO) framework [18] successfully handles nonlinear dynamics and constraints by reformulating the problem into Second-Order Conic Programming (SOCP) subproblems. However, solving these SOCPs using traditional Interior Point (IP) methods is computationally expensive, creating a bottleneck that prevents real-time application in high-dimensional systems.

2. Methodology

The authors propose cuNRTO, a GPU-accelerated framework that introduces two novel architectures to solve the NRTO subproblems efficiently. The overall workflow involves an outer loop of Successive Linearization (SL) on the CPU and an inner loop of robust optimization executed on the GPU.

Core Architectures

The paper introduces two distinct methods to accelerate the inner loop:

A. NRTO-DR (Douglas-Rachford Splitting)

• Approach: This method reformulates the SOCP subproblem to be solvable via the Douglas-Rachford (DR) splitting method.
• Mechanism:
  • It decouples the problem into an affine set projection (solved via sparse linear algebra) and a cone projection (solved via parallel operations).
  • GPU Implementation: Uses cuDSS for one-time sparse Cholesky factorization of the KKT matrix and reuses triangular solves. It employs custom CUDA kernels for parallel Second-Order Cone (SOC) projections, which are the most computationally intensive part of the solver.
• Benefit: Significantly reduces matrix operations compared to IP methods by leveraging parallel projections.

B. NRTO-FullADMM (Alternating Direction Method of Multipliers)

• Approach: A novel variant that further exploits the problem structure by reformulating the update blocks within the inner ADMM loop.
• Mechanism:
  • It introduces a slack variable to decouple constraints, allowing the inner ADMM loop to run entirely on the GPU (minimizing CPU-GPU data transfer).
  • Block-1 Update: Performs parallel SOC projections for all constraints simultaneously using batched cuBLAS GEMM calls for affine evaluations and custom kernels for projection.
  • Block-2 Update: Solves a convex Quadratic Program (QP) using Preconditioned Conjugate Gradient (PCG) with a Jacobi preconditioner (computed once per outer iteration) and matrix-free Hessian-vector products.
• Benefit: Eliminates the "nested" structure of NRTO-DR and removes data transfer bottlenecks, offering superior scalability for problems with many constraints.

GPU Implementation Details

• Parallelism: The framework maps each SOCP constraint to a separate GPU warp (a group of 32 threads), enabling massive parallelism.
• Memory: Utilizes shared memory and L1 caches for fast projection and vector updates.
• Kernels: Custom CUDA kernels handle SOC projections (based on SCS logic but optimized for warp-level execution), while cuBLAS handles linear algebra (GEMM chains) for feedback gain updates.

3. Key Contributions

1. NRTO-DR Framework: Introduces a DR splitting method to solve NRTO subproblems, replacing expensive IP methods with parallel SOC projections and sparse direct solves.
2. NRTO-FullADMM Framework: Proposes a fully on-device ADMM variant that integrates parallel SOCP projections directly into the inner loop, eliminating CPU-GPU communication overhead.
3. cuNRTO Implementation: A complete GPU implementation featuring custom CUDA kernels for SOC projections and optimized cuBLAS chains.
4. Empirical Validation: Demonstrates the framework on diverse robotic platforms (Unicycle, Quadcopter, Franka Manipulator) with significant speedups.

4. Results

The authors evaluated cuNRTO on a workstation with an NVIDIA A100 GPU, comparing it against the baseline NRTO (solved via MOSEK on CPU).

• Speedup:
  • Unicycle Model: Achieved up to 139.6× speedup (e.g., reducing solve time from ~30,423s to ~218s with 5 obstacles).
  • Quadcopter Model: Achieved up to 66.89× speedup.
  • Franka Manipulator (7-DOF): Achieved a 25.9× speedup on a high-dimensional system (14 states, 7 controls).
• Constraint Satisfaction:
  • All methods maintained 100% constraint satisfaction when using the NRTO-LE (Linearization Error) extension, verified via 1,000 Monte Carlo rollouts and 1,000 edge-case disturbances.
  • The GPU-accelerated methods produced trajectories indistinguishable in safety from the baseline CPU methods.
• Scalability: Performance gains increased with the number of obstacles (constraints), demonstrating that the parallel architecture scales effectively as problem complexity grows.

5. Significance

• Real-Time Robust Control: By reducing computation time by orders of magnitude, cuNRTO makes robust trajectory optimization feasible for high-dimensional, nonlinear systems in real-time, a task previously limited by computational cost.
• Hardware Utilization: The work effectively bridges the gap between complex robust control theory and modern GPU hardware, moving beyond simple dynamics parallelization to full optimization loop acceleration.
• Safety-Critical Applications: The ability to guarantee safety under worst-case uncertainty without sacrificing computational speed is crucial for deploying autonomous robots in unstructured environments (e.g., autonomous driving, warehouse logistics, and surgical robotics).
• Future Directions: The authors suggest extending this framework to contact-rich manipulation, multi-agent swarms, and integrating learning-to-optimize techniques (deep unfolding) to further enhance performance.