Graphs of Trajectory Bundles for Nonlinear Motion Planning
Authors: Joris Verhagen, Jon Arrizabalaga, Jana Tumova, Zachary Manchester
Organizations: Authors are with the Division of Robotics, Perception, and Learning, KTH Royal Institute of Technology, Stockholm, Sweden · Authors are with the Department of Aeronautics and Astronautics, MIT, Cambridge, MA, USA
We present the Graphs of Trajectory Bundles (GTB) method for motion planning of systems with nonlinear and black-box dynamics, combining sampling-based motion planning with trajectory optimization and graph search. Based on a graph-of-convex-sets decomposition of the workspace, GTB iteratively constructs a graph of trajectory bundles that captures the local dynamics of the system at any vertex. By solving a shortest-path problem on this graph, we find maximally dynamically feasible iterates that satisfy the system's dynamics in the limit and any constraints such as collision avoidance or additional safety or specification constraints by construction of the underlying graph. As a result, GTB can handle highly nonlinear, unstable, and even black-box dynamics with complex collision and specification constraints, making it a powerful tool for motion planning of complex systems.
Figures & tables
Fig. 1: The Graph of Trajectory Bundle (GTB) method poses the motion planning problem as a shortest path problem in dynamics space. On each vertex of a graph of convex sets (shaded blue), states are sampled (shaded red) and transported through the dynamics f(x,u) to form a bundle (shaded green), themselves being convex sets. At each iteration, GTB finds the interpolators of the sequence of bundles. (b) a high-dimensional underwater robot through a 3D maze with iterations of GTB taking different paths.
Fig. 2: The workspace graph of convex sets (a) and one iteration of the Graphs of Trajectory Bundles (GTB) algorithm (b,c,d). In (b) we perturb the current vertex solution xˉi,k (red samples) and project onto the region Qk (blue) to form the projected bundle W^x(i,k) . In (c) each projected sample is rolled through the dynamics to form the forward bundle Wf(i,k) (green); the two together are the bundle Wi,k at vertex (i,k) of G^σ . The vertex variables are the weights αi,k , chosen in (d) to minimize dynamic infeasibility (red lines) along a path (black curves) from Source to Target ( 11 ).
Fig. 3: Iterations of GTB for a path planning problem for a car model navigating around an obstacle, showing the projected bundles (shaded red) and forward bundles (shaded green) of the highest-probability path. Early iterations explore modes via the coarse approximation of the dynamics while later iterations focus on refining the trajectory (d) shows the final trajectory and a comparison to (dynamically infeasible) results of GCS initializing an NLP solver, which gets stuck on the shortest-path local minima and the Trajectory Bundle Method which gets stuck on the local minima of the initial guess.
Fig. 4: Comparisons of GTB against the Trajectory Bundle Method and the GCS solve with an NLP refinement. On top the physical trajectories and early iterations of the GTB algorithm highlighting the different paths that are considered. On the bottom the tolerance as a function of the wall clock time. Dotted vertical lines denote the average computation time. The orange and purple x-axis denote the different scales to highlight that GTB converges approximately equally rapid to infeasible solutions, but converges to lower tolerances afterwards. While GTB almost always finds feasible solutions, both TBM and GCS+NLP often fail and are stuck in local minima.
Dynamically feasible trajectory optimization for rigid-body systems is naturally formulated on the special Euclidean group SE(3) but is challenging when dynamics are available only as black-box computations without derivatives. This paper formulates the Trajectory Bundle Method (TBM) for motion planning implicitly on SE(3). Bundles are constructed in the Lie algebra and propagated through nonlinear rigid-body dynamics using exponential and logarithmic maps, enabling derivative-free planning of non-Euclidean trajectories. We show that Euclidean TBM interpolation error is bounded quadratically by bundle diameter and extend this result to SE(3), where the bound additionally depends on a local Lipschitz constant of the Log map. Numerical experiments corroborate these bounds. Finally, we demonstrate SE(3) TBM by optimizing an acrobatic, collision-free fixed-wing maneuver through a rotated aperture without explicit models or derivatives of the vehicle dynamics, aerodynamics, or collision model.
Matthew D. Osburn, Cameron K. Peterson, John L. Salmon
Department of Electrical and Computer Engineering, Brigham Young University, Provo, UT 84602, USA · Department of Mechanical Engineering, Brigham Young University, Provo, UT 84602, USA
Motion planning for autonomous vehicles requires generating collision-free and dynamically feasible trajectories in complex environments under real-time constraints. While nonlinear optimal control formulations provide high-fidelity solutions, they are computationally demanding and sensitive to initialization, whereas geometric planning methods scale well but often decouple path selection from trajectory optimization. This paper studies the extent to which optimization over Graphs of Convex Sets (GCS) can approximate solutions of nonlinear optimal control problems in the context of autonomous driving. The free space is represented as a finite union of convex regions organized as a directed graph, allowing nonconvex geometry to be handled through discrete connectivity decisions while maintaining convex trajectory constraints within each region. Vehicle motion is parameterized using Bezier curves for the spatial path and a polynomial time-scaling function for temporal evolution. Under small-slip and linear tire assumptions, a simplified dynamic bicycle model enables approximate enforcement of dynamic feasibility through convex constraints on trajectory derivatives. The approach is evaluated in CommonRoad scenarios involving static obstacle avoidance and lane-changing maneuvers, and is compared against a nonlinear discrete-time optimal control formulation. The results indicate that the GCS-based method generates collision-free and dynamically consistent trajectories that closely match those obtained from the nonlinear program, while exhibiting improved computational efficiency and reduced sensitivity to initialization. These findings suggest that GCS provides a structured approximation of nonlinear motion planning problems, capturing dominant geometric and dynamic effects while preserving convexity in the continuous relaxation.
Matheus Wagner, Antônio Augusto Fröhlich
Software/Hardware Integration Lab, Federal University of Santa Catarina, Florian´opolis, Brazil
Global Tensor Motion Planning (GTMP) solves motion planning with batched tensor operations over a layered multipartite graph. We generalize GTMP so that adjacent-layer edges are realized by any black-box local planner (e.g., linear interpolation, splines, sampling-based planning, trajectory optimization, or generative sampling). We provide two anytime policies on top of this generalization: Anytime GTMP with random restarts at a fixed budget, which covers every homotopy class almost surely, and AO-GTMP with informed expansion with growing budgets, which converges to the optimal cost. We prove that a single sampled graph covers every endpoint-fixed homotopy class admitting a δ-clear representative of bounded length. We also prove that additional samples per layer reduce the per-layer miss probability exponentially, whereas stronger local planners reduce the required layer count only sublinearly. On manipulation benchmarks the method matches state-of-the-art performance, and on 2D navigation it returns batches of topologically diverse solutions, while the informed baselines concentrate on one or two classes.
Sai Coumar, An T. Le, Zachary Kingston
Department of Computer Science, Purdue University, USA. · College of Engineering and Computer Science, VinUniversity, Hanoi, Vietnam. · Intelligent Autonomous Systems Lab, TU Darmstadt, Germany.