Graphs of Trajectory Bundles for Nonlinear Motion Planning

Joris Verhagen; Jon Arrizabalaga; Jana Tumova; Zachary Manchester

cs.ROOct 23, 2026

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

Abstract

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

\bar{x}_{i,k}

(red samples) and project onto the region

\mathcal{Q}_{k}

(blue) to form the projected bundle

\hat{W}_{x}^{(i,k)}

. In (c) each projected sample is rolled through the dynamics to form the forward bundle

W_{f}^{(i,k)}

(green); the two together are the bundle

\mathcal{W}_{i,k}

at vertex

(i,k)

\hat{\mathcal{G}}_{\sigma}

. The vertex variables are the weights

\alpha_{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.

	Car-ring	Car-maze	Drone	Cart-pole LTL	AUV
$N$	10	30	40	40	40
$M_{s}$	60	60	100	40	70
max_iter	100	80	50	200	250
$dt$	0.75	0.25	0.1	0.2	1.0
$u_{\text{order}}$	1	1	1	1	2
$n_{\text{regions}}$	6	35	8	5	17

TABLE I: Per-scenario hyperparameters.

Constrained Motion Planning

Constrained motion planning finds robot paths and controls that satisfy geometric, dynamic, safety, and task-specific requirements.

See all 188 papers

Trajectory Bundle Method in SE(3) for Black-Box Fixed-Wing Aircraft Trajectory Optimization
Sep 10, 2026Matthew D. Osburn, Cameron K. Peterson, John L. SalmonTrajectory Optimization Trajectory Generation
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 using Optimization over Graphs of Convex Sets
May 13, 2026Matheus Wagner, Antônio Augusto FröhlichTrajectory Optimization Autonomous Driving
Software/Hardware Integration Lab, Federal University of Santa Catarina, Florian´opolis, Brazil
Anytime Global Tensor Motion Planning
Aug 26, 2026Sai Coumar, An T. Le, Zachary KingstonMotion Planning Sampling-Based Motion Planning
Department of Computer Science, Purdue University, USA. · College of Engineering and Computer Science, VinUniversity, Hanoi, Vietnam. · Intelligent Autonomous Systems Lab, TU Darmstadt, Germany.

Sep 10, 2026J/K move · Enter open · S save

Trajectory Bundle Method in SE(3) for Black-Box Fixed-Wing Aircraft Trajectory Optimization

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

Trajectory Optimization Trajectory Generation Black-Box Optimization Lie Group Methods Motion Planning Derivative-Free Optimization

Open paper AskPDF arXiv

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.

Abstract

Constrained Motion Planning

Explore similar work

Trajectory Bundle Method in SE(3) for Black-Box Fixed-Wing Aircraft Trajectory Optimization

Motion Planning for Autonomous Vehicles using Optimization over Graphs of Convex Sets

Anytime Global Tensor Motion Planning