Skip to main content
. 2025 Nov 11;25(22):6874. doi: 10.3390/s25226874
Algorithm 1 Real-Time Parallel Trajectory Optimization
  •   1:

    Input: Previous trajectory x(t), control inputs u(t) (warm-start)

  •   2:

    while within time budget and convergence not met do

  •   3:

        Parallel: For each knot along the trajectory:

  •   4:

           Compute gradients for smoothness, obstacle cost, and dynamics

  •   5:

           Compute constraint gradients (e.g., collision or control limits)

  •   6:

       Aggregate gradients

  •   7:

       Update x(t) and u(t) (e.g., via projected gradient descent)

  •   8:

       Project updated trajectory to enforce hard constraints

  •   9:

    end while

  • 10:

    Output: Refined trajectory x*, controls u*