Paper deep dive
Search-Based Spatiotemporal and Multi-Robot Motion Planning on Graphs of Space-Time Convex Sets
Jingtao Tang, Zining Mao, Lufan Yang, Hang Ma
Intelligence
Status: succeeded | Model: Gemma-4-26B-A4B | Prompt: intel-v1 | Confidence: 96%
Last extracted: 7/5/2026, 9:09:28 AM
Summary
The paper introduces a novel algorithmic framework for spatiotemporal and multi-robot motion planning using Graphs of Space-Time Convex Sets (ST-GCSs). Unlike traditional sampling-based or optimization-based methods, the ST-GCS approach represents collision-free regions as convex sets in space-time, allowing for a unified treatment of geometric and temporal constraints. The authors develop a best-first search solver that operates on path-indexed states to achieve time-optimal planning. Key contributions include an Exact Convex Decomposition (ECD) scheme for reserving trajectory occupancies to handle dynamic obstacles and multi-robot interactions, and a windowed coordination scheme for efficient multi-robot coordination. Experimental results demonstrate high scalability, successfully solving dense multi-robot instances with up to 100 robots.
Entities (7)
Relation Signals (4)
Exact Convex Decomposition → enables → Dynamic Obstacle Handling
confidence 100% · enabling unified handling of dynamic obstacles and multi-robot interactions
Windowed Coordination Scheme → improves → Multi-Robot Coordination
confidence 100% · introduce a windowed coordination scheme to improve efficiency in multi-robot coordination
ST-GCS → isbasedon → Graphs of Convex Sets
confidence 100% · An ST-GCS extends a spatial GCS into the time dimension
Best-First Search Solver → solves → ST-GCS Planning
confidence 100% · We formulate time-optimal planning on ST-GCSs as a graph-search problem... and develop a best-first search solver
Cypher Suggestions (0)
No Cypher suggestions yet.
Abstract
Abstract:Spatiotemporal motion planning, especially in multi-robot settings, requires robots to reason about collision-free regions that change over time, which is challenging in continuous spaces when feasible regions are transient and geometrically constrained. We present an algorithmic framework based on graphs of space-time convex sets (ST-GCSs), where collision-free regions are represented as convex sets in space-time and trajectories correspond to paths on the graph together with continuous motions within the selected sets. We formulate time-optimal planning on ST-GCSs as a graph-search problem over path-indexed states and develop a best-first search solver that evaluates partial paths via continuous trajectory optimization, guided by admissible heuristics and dominance checks. We further present an Exact Convex Decomposition (ECD) scheme to reserve trajectory occupancies in space-time, enabling unified handling of dynamic obstacles and multi-robot interactions. For multi-robot motion planning, we integrate ST-GCS planning and ECD into prioritized planning methods and introduce a windowed coordination scheme to improve efficiency. Extensive experiments on single-robot and multi-robot problems demonstrate substantial speedups over various planners while maintaining high solution quality, particularly in environments with narrow and transient feasible regions. Large-scale demonstrations further show that the proposed multi-robot motion planner can solve instances with up to $100$ robots within only a few minutes. Project homepage: this https URL
Tags
Links
- Source: https://arxiv.org/abs/2607.00444v1
- Canonical: https://arxiv.org/abs/2607.00444v1
Trouble viewing inline? Open PDF directly →
Full Text
166,578 characters extracted from source content.
Expand or collapse full text
Search-Based Spatiotemporal and Multi-Robot Motion Planning on Graphs of Space-Time Convex Sets Jingtao Tang, Zining Mao, Lufan Yang, and Hang Ma Simon Fraser University, Canada Corresponding author email: jingtao_tang@sfu.ca Abstract Spatiotemporal motion planning, especially in multi-robot settings, requires robots to reason about collision-free regions that change over time, which is challenging in continuous spaces when feasible regions are transient and geometrically constrained. We present an algorithmic framework based on graphs of space-time convex sets (ST-GCSs), where collision-free regions are represented as convex sets in space-time and trajectories correspond to paths on the graph together with continuous motions within the selected sets. We formulate time-optimal planning on ST-GCSs as a graph-search problem over path-indexed states and develop a best-first search solver that evaluates partial paths via continuous trajectory optimization, guided by admissible heuristics and dominance checks. We further present an Exact Convex Decomposition (ECD) scheme to reserve trajectory occupancies in space-time, enabling unified handling of dynamic obstacles and multi-robot interactions. For multi-robot motion planning, we integrate ST-GCS planning and ECD into prioritized planning methods and introduce a windowed coordination scheme to improve efficiency. Extensive experiments on single-robot and multi-robot problems demonstrate substantial speedups over various planners while maintaining high solution quality, particularly in environments with narrow and transient feasible regions. Large-scale demonstrations further show that the proposed multi-robot motion planner can solve instances with up to 100100 robots within only a few minutes. Project homepage: https://sites.google.com/view/stgcs. Keywords: Motion Planning, Multi-Robot Coordination, Heuristic Search, Graphs of Convex Sets 1 Introduction Spatiotemporal motion planning is a core problem in robotics. A robot must move from a start state to a goal while avoiding both static obstacles and time-varying constraints induced by dynamic environments or other robots. This problem becomes particularly challenging in continuous domains when feasible regions are transient, geometrically constrained, and tightly coupled with time. Such conditions arise naturally in Multi-Robot Motion Planning (MRMP), where each robot must treat the trajectories of others as dynamic obstacles, as well as in single-robot planning tasks with moving obstacles or temporal constraints. Despite extensive progress, existing approaches still struggle to provide efficient and reliable solutions in these settings. Sampling-based planners, such as PRM (Kavraki et al., 1996) and RRT (LaValle, 1998), and their spatiotemporal variants (Hüppi et al., 2022; Grothe et al., 2022), offer modeling flexibility but rely on random exploration, which can be ineffective in capturing narrow or short-lived feasible regions in space-time. Their performance further degrades due to repeated collision checking in a time-augmented state space. Optimization-based approaches based on the Graph of Convex Sets (GCS) replace random exploration with deterministic reasoning over convex decompositions (Marcucci et al., 2023, 2024b), but extending them to dynamic environments requires a unified treatment of time, velocity constraints, and dynamic obstacle avoidance. When formulated as a single large optimization, this leads to significant computational challenges. Figure 1: Demonstration of the proposed MRMP planner on ST-GCS, where robots 11 and 33 exchange positions with robots 22 and 44, respectively. Left: Solution trajectories visualized in 2D space, with higher transparency indicating states at later time stamps. Right: Solution trajectories τ1 _1 and τ2 _2 visualized in 3D space-time, where τ2 _2 treats τ1 _1 as a space-time obstacle and plans a trajectory through space-time collision-free convex sets (colored polyhedra) that exclude the τ1 _1 occupancy. In this work, we present a general framework for time-optimal spatiotemporal motion planning in continuous spaces with dynamic obstacles based on a novel graph representation, namely a Graph of Space-Time Convex Sets (ST-GCS), where vertices correspond to collision-free convex sets in space-time and edges encode nonempty intersections between sets. An ST-GCS extends a spatial GCS into the time dimension, allowing both geometric and temporal constraints to be captured within a single graph structure. A feasible trajectory then corresponds to a path on an ST-GCS together with a continuous trajectory constructed within the selected convex sets. This representation also provides a natural basis for spatiotemporal planning in dynamic environments and for multi-robot coordination, where planned robot trajectories can be incorporated back into the graph as reserved occupancies. A key challenge in solving time-optimal planning on an ST-GCS by graph search is that the cost and feasibility of reaching a vertex cannot be determined solely by the vertex itself. Instead, they depend on the entire prefix path used to reach that vertex, since different paths induce different feasible arrival states and different continuation costs. As a result, standard shortest-path formulations that identify search states only by vertices are generally insufficient. To address this, we formulate planning on an ST-GCS as a graph search over path-indexed states, where each search node represents a partial path together with the corresponding optimal trajectory and its cost. Building on this formulation, we develop a best-first search solver for planning on ST-GCSs and introduce several algorithmic components that together make it practical and scalable. The search repeatedly evaluates partial paths by solving the corresponding continuous trajectory optimization over the convex sets along that path. To improve efficiency, we design admissible heuristics over partial paths and develop reachability-based dominance checks, including a safe set-containment check that preserves optimality and two lightweight heuristic checks that are substantially faster while maintaining high solution quality in practice. In addition, we present the Exact Convex Decomposition (ECD) scheme to reserve the spatiotemporal occupancy of trajectories by updating an ST-GCS, thereby enabling a unified treatment of dynamic obstacles and inter-robot interactions. On top of this, we integrate ECD into prioritized planning methods for MRMP, and further introduce a windowed coordination scheme to improve efficiency in multi-robot coordination by focusing planning effort on the relevant time intervals. We demonstrate the proposed Windowed-PBS + BFS planner on large-scale and highly congested MRMP instances, solving a bottlenecked 5050-robot instance in 1.641.64m and a dense 100100-robot instance in 1.601.60m, which shows its scalability beyond the main benchmark range. We summarize our contributions as follows: 1. We formulate time-optimal spatiotemporal motion planning as a graph-search problem on ST-GCSs. 2. We develop a search-based solver that operates on path-indexed states, together with admissible heuristics and dominance checks tailored to searching ST-GCSs. 3. We present the ECD scheme to reserve trajectory occupancies in arbitrary-dimensional space-time, enabling unified handling of dynamic obstacles and multi-robot interactions. 4. We integrate ST-GCS planning and ECD into prioritized planning methods for MRMP, and further introduce a windowed coordination scheme to improve efficiency in multi-robot coordination. 5. We provide extensive empirical evaluation demonstrating substantial speedups over optimization-based approaches and strong performance in challenging spatiotemporal scenarios. This work substantially extends our prior conference version (Tang et al., 2025) as follows: 1. The prior version formulates time-optimal motion planning on an ST-GCS as a unified optimization problem, whereas this work develops a search-based formulation on ST-GCSs with path-indexed states, pruning strategies, and heuristic guidance, resulting in orders of magnitude faster in practice. 2. The prior version presents ECD in the original 3D setting arising from 2D spatial motion plus time, whereas this work generalizes ECD to arbitrary-dimensional space-time. 3. The prior version integrates ST-GCS within prioritized multi-robot planning, whereas this work additionally introduces windowed coordination to improve efficiency in multi-robot coordination. 4. This work also provides substantially more extensive empirical evaluation, including additional search ablations and real-robot experiments. 2 Related Work This section positions our work relative to spatiotemporal planning, MRMP, and planning on GCSs. 2.1 Spatiotemporal Motion Planning Spatiotemporal motion planning treats time as part of the planning domain to handle moving obstacles with known trajectories (Erdmann and Lozano-Perez, 1987). Grid-based methods reserve discrete space-time states (Silver, 2005); SIPP reasons over safe time intervals for discrete spatial occupancies (Phillips and Likhachev, 2011); any-angle variants such as Zeta∗-SIPP combine safe intervals with any-angle search for time-optimal planning in dynamic environments (Zou and Borst, 2024); and state-lattice methods plan over spatiotemporal lattices built from continuous space and fixed motion primitives (McNaughton et al., 2011). Sampling-based planners lift RRT-, PRM-, and RRT∗-style methods into configuration–time space, validating sampled time-parameterized states or edges against moving obstacles over their execution intervals (Sintov and Shapiro, 2014; Hüppi et al., 2022; Grothe et al., 2022); they are flexible, but can struggle when feasible regions are narrow, short-lived, or repeatedly changing, and their runtime can be dominated by collision checking in the time-augmented state space. Recent work mitigates this issue by bringing safe-interval ideas from MAPF (Phillips and Likhachev, 2011) into sampling-based planning (Sim et al., 2024; Kerimov et al., 2025). In contrast, we plan on ST-GCSs: collision-free space-time is represented by convex sets connected through nonempty intersections, so reserved occupancies can be removed from an updatable free-space decomposition while continuous trajectory optimization remains available over remaining regions. 2.2 MRMP MRMP methods differ mainly in how they represent and resolve inter-robot conflicts. Coupled methods reason in composite configuration spaces. Coordinated PRMs explicitly construct products of individual roadmaps (Švestka and Overmars, 1998), dRRT-style methods explore implicit tensor-product roadmaps without materializing the full joint graph (Solovey et al., 2016; Shome et al., 2020), and partially coupled methods such as M∗ plan independently when possible while expanding into higher-dimensional coupled search near conflicts (Wagner and Choset, 2015). Decoupled and conflict-based methods improve scalability by using priorities, reservations, or constraints. Prioritized planning treats planned robots as moving obstacles in configuration–time space (Erdmann and Lozano-Perez, 1987; Van Den Berg and Overmars, 2005), cooperative pathfinding uses discrete space-time reservations (Silver, 2005), CBS separates high-level conflict resolution from low-level planning (Sharon et al., 2015), and PBS searches over priority constraints rather than a fixed ordering (Ma et al., 2019). These ideas underlie MAPF variants including continuous-time MAPF (Andreychuk et al., 2022), safe-interval planning on continuous-time roadmaps (Kasaura et al., 2022), and representation-optimal MRMP for heterogeneous robots in continuous spaces (Solis et al., 2021). Recent MRMP work connects conflict-based coordination with sampling-based, kinodynamic, optimization, and control-based planning. Kinodynamic CBS incorporates motion primitives into CBS (Kottinger et al., 2022), while adaptive methods vary coupling online through adaptive robot coordination (Solis et al., 2024), kinodynamic adaptive coordination (Qin et al., 2025), or guidance-informed grouping (McBeth et al., 2026). Other approaches use decentralized trajectory optimization (Tordesillas and How, 2021), path retiming (Mao et al., 2024), CBS-guided MPC (Tajbakhsh et al., 2024), or mixed-integer continuous formulations (Zhao et al., 2025a; Ren et al., 2025). Our MRMP planner is closest to prioritized planning, but each low-level query plans on an ST-GCS; after a robot is planned, ECD updates the ST-GCS to reserve its swept occupancy, so later robots plan in the remaining collision-free space-time and jointly reason about route choice, timing, and dynamic collision avoidance. 2.3 Planning on GCSs GCSs combine graph structure with convex optimization for mixed discrete–continuous planning (Marcucci et al., 2023, 2024b). They have been applied beyond cluttered Euclidean motion planning to non-Euclidean configuration spaces (Cohn et al., 2023), temporal-logic and precedence-constrained planning (Kurtz and Lin, 2023; You et al., 2025), contact-rich manipulation (Graesdal et al., 2024), and guidance for downstream nonconvex trajectory optimization (von Wrangel and Tedrake, 2024). These works demonstrate the breadth of GCS modeling, but mainly consider static, task-augmented, or contact-mode domains rather than a free-space representation repeatedly updated by moving obstacles and planned robot trajectories. GCS performance depends strongly on the convex decomposition of free space. IRIS grows large obstacle-free convex regions (Deits and Tedrake, 2015); visibility-graph clique covers and certified polyhedral decompositions improve coverage, certification, and scalability (Werner et al., 2024; Dai et al., 2024); and GPU-accelerated methods compute collision-free configuration-space convex sets online in changing environments (Werner et al., 2025). Our ECD scheme is complementary. It updates an ST-GCS by removing swept occupancies while maintaining a convex decomposition of the remaining collision-free space-time. Efficient GCS solving has also received substantial attention. Large safe-box planners exploit offline preprocessing with fast runtime shortest-path and convex-control subproblems (Marcucci et al., 2024a), while search-based solvers such as GCS∗ (Chia et al., 2024), implicit graph search (Natarajan et al., 2024), and A∗-GCS (Sundar and Rathinam, 2024) reduce reliance on one monolithic mixed-integer convex program over the full graph. Other variants address multi-query planning (Morozov et al., 2024), shortest walks with repeated vertices (Morozov et al., 2025), nonconvex-cost (Clark and Xie, 2025) or parametrized-space objectives (Garg et al., 2025), fixed-sequence minimum-time motion through convex sets (Marcucci et al., 2025), and routing problems that couple discrete visitation order with continuous trajectories (Philip et al., 2024; Bhat et al., 2025; Tang and Ma, 2026). Closest to our setting, Osburn et al. (2025) studies ST-GCS planning in dynamic environments and constructs GCS-compatible constraints, while Zhao et al. (2025b) combines CBS with time-augmented GCS under a fixed time-step representation. Recent GCS work has expanded modeling scope and solver efficiency, but has not jointly addressed path-indexed search on ST-GCSs, occupancy reservation by convex-decomposition updates, and windowed multi-robot coordination. 3 Preliminaries This section introduces the notation and basic GCS formulation used in this paper. We first review the standard definition of (spatial) GCSs, where vertices correspond to collision-free convex sets and edges indicate nonempty intersections. We then describe the standard motion-planning problem on GCSs and the path-conditioned convex subproblem used later by our search-based solver for planning on ST-GCSs. 3.1 GCS Definition We consider a robot with an m-dimensional state space, whose collision-free region is decomposed into a collection of convex sets. These spatial convex sets need not be disjoint. A GCS, denoted as G=(V,E,)G=(V,E,X), is a connected graph representing such a convex decomposition, where V is the vertex set, E is the edge set, and =Xvv∈VX=\X_v\_v∈ V is the collection of convex sets. Each vertex v∈Vv∈ V corresponds to a convex set Xv=|v⪯v⊆ℝmX_v=\x\,|\,A_vx _v\ ^m of states. Each edge e=(u,v)∈Ee=(u,v)∈ E indicates that Xu∩Xv≠∅X_u∩ X_v≠ . Our use of GCS is slightly more restrictive than the original formulation of Marcucci et al. (2024b), where constraints may be attached more generally to vertices and edges. A path on G is an ordered sequence of vertices π=⟨v1,v2,…,vl⟩π= v_1,v_2,…,v_l where (vi−1,vi)∈E(v_i-1,v_i)∈ E for all i=2,…,li=2,…,l. Unless otherwise stated, we consider simple paths, i.e., paths without repeated vertices. A trajectory associated with π is a continuous curve obtained by concatenating local trajectory segments inside the convex sets along π. 3.2 Motion Planning on GCSs Given a start state sx_s and a goal state gx_g, motion planning on a GCS G=(V,E,)G=(V,E,X) asks for a trajectory from sx_s to gx_g. Since the start and goal states may lie in multiple convex sets, we introduce auxiliary source and target vertices vsv_s and vgv_g, with Xvs=sX_v_s=\x_s\ and Xvg=gX_v_g=\x_g\, connected to all start- and goal-containing vertices, respectively, and use these auxiliary vertices as the unique endpoints of the graph path. We introduce two sets of variables. The binary edge-selection variables Φ=ϕee∈E =\ _e\_e∈ E parameterize a path πΦ _ , where ϕe=1 _e=1 if and only if edge e is selected by the path πΦ _ . The continuous variables v,v∈Xvx_v,y_v∈ X_v represent the entry and exit states of the local trajectory segment inside XvX_v. For each vertex v∈Vv∈ V, let ℓv(v,v) _v(x_v,y_v) denote a given local cost associated with vertex v. The motion planning problem on GCS can be written as: minΦ,, _ ,x,y\; ∑v∈πΦℓv(v,v) _v∈ _ _v(x_v,y_v) (1a) s.t. πΦ=⟨vs,…,vg⟩∈(vs,vg;G), _ = v_s,…,v_g (v_s,v_g;G), (1b) v,v∈Xv, _v,y_v∈ X_v, ∀v∈πΦ,∀ v∈ _ , (1c) v=u, _v=y_u, ∀(u,v)∈E(πΦ),∀(u,v)∈ E( _ ), (1d) vs=s and vg=g, _v_s=x_s and y_v_g=x_g, (1e) where (vs,vg;G)P(v_s,v_g;G) denotes the set of simple paths in G that start from vsv_s and end at vgv_g. Figure 2: A motion planning problem on a GCS, where each vertex denotes a collision-free convex set and a feasible solution reconstructs a continuous piecewise-linear trajectory from sx_s to gx_g along a selected graph path. The objective in Eqn. (1a) sums the local costs of the trajectory segments v→vx_v _v along the selected path. Eqn. (1b) enforces that the selected edges form a valid simple path from the auxiliary source vertex to the auxiliary target vertex. This path constraint can be encoded using standard flow-conservation and degree constraints for routing problems (Miller et al., 1960; Bertsimas and Tsitsiklis, 1997), yielding a Mixed-Integer Convex Program (MICP). Eqn. (1c) enforces that each local segment lies inside the corresponding convex set. Since XvX_v is convex, the straight-line segment (v,v)(x_v,y_v) is collision-free. Eqn. (1d) enforces continuity between consecutive local segments for every selected edge. Eqn. (1e) enforces the trajectory starts from and ends at the given start state and goal state, respectively. As illustrated in Fig. 2, a feasible solution reconstructs a continuous, collision-free, piecewise-linear trajectory from sx_s to gx_g by chaining the local segments v→vx_v _v along v∈πΦv∈ _ . The above formulation focuses on kinematic feasibility, which aligns with the standard convention in the literature. Differential or kinodynamic constraints can be incorporated by augmenting the state space and adding suitable constraints, such as in Marcucci et al. (2023). The formulation can also be adapted to a convex start set XsX_s or a convex goal set XgX_g by replacing the point constraints in Eqn. (1e) with set-membership constraints. 3.3 Path-Conditioned Optimization Solving Eqns. (1a–1e) directly as an MICP can be computationally expensive (Marcucci et al., 2024b). A useful subproblem is obtained by fixing the graph path and optimizing only the continuous variables along that path. This operation is commonly referred to as convex restriction (Diamond et al., 2018) and is used as a subroutine in several search-based solvers for graph optimization problems on GCSs (Marcucci, 2024; Natarajan et al., 2024; Tang and Ma, 2026). Given a path π=⟨v1,v2,…,vl⟩π= v_1,v_2,…,v_l on G, we say that a trajectory τ is conditioned on π if it is represented by an ordered sequence τ=⟨(vi,vi)⟩i=1lτ= (x_v_i,y_v_i) _i=1^l of entry and exit states, where vi,vi∈Xvix_v_i,y_v_i∈ X_v_i with continuity enforced between consecutive sets. Conditioning on a path π fixes the binary variables Φ so that πΦ=π _ =π. The remaining optimization is the convex program over the continuous entry and exit states, given as follows: min, _x,y ∑i=1lℓvi(vi,vi) _i=1^l _v_i(x_v_i,y_v_i) (2a) s.t. vi,vi∈Xvi, _v_i,y_v_i∈ X_v_i, i=1,…,l, i=1,…,l, (2b) vi=vi−1, _v_i=y_v_i-1, i=2,…,l, i=2,…,l, (2c) v1=s and vl=g. _v_1=x_s and y_v_l=x_g. (2d) This is a convex program that off-the-shelf optimizers can efficiently solve whenever the local costs and set constraints are convex. If feasible, its solution defines the optimal trajectory conditioned on the fixed path π. For example, in Fig. 2, fixing π=⟨v1,v2,v3,v4,v5⟩π= v_1,v_2,v_3,v_4,v_5 removes the discrete path-selection variables and optimizes only the entry and exit state variables along that path. 4 Spatiotemporal Planning on ST-GCSs This section extends the GCS formulation in Sec. 3 to spatiotemporal motion planning. The key concept is a Graph of Space-Time Convex Sets (ST-GCS), where each convex set lies in the joint space of position and time. Planning on an ST-GCS allows dynamic-obstacle avoidance, variable arrival times, velocity limits, and time optimality to be handled within a unified graph-optimization problem. 4.1 Problem Formulation Let G=(V,E,)G=(V,E,X) denote an input ST-GCS, where =Xvv∈VX=\X_v\_v∈ V is a collection of space-time convex sets. Each vertex v∈Vv∈ V corresponds to a space-time convex set Xv⊂ℝm+1X_v ^m+1 that is collision-free from both static and dynamic obstacles. Each edge (u,v)∈E(u,v)∈ E indicates that Xu∩Xv≠∅X_u∩ X_v≠ . As shown in Fig. 3, an ST-GCS can be initialized by extruding each given spatial collision-free convex set from time 0 to an arbitrarily large but finite time limit tmaxt_max. In addition, the ECD scheme introduced in Sec. 6 can further remove the space-time occupancies of dynamic obstacles or previously planned robots, potentially subdividing the extruded sets. Figure 3: A 3D ST-GCS (right) constructed by extruding each set of a 2D spatial GCS (left) along the time dimension t∈[0,tmax]t∈[0,t_ ]. A feasible trajectory is planned from the start state sx_s to the goal position gp_g, with the dashed vertical line indicating the possible goal states g=(g,t)x_g=(p_g,t) across time. Let =(,t)∈ℝm+1x=(p,t) ^m+1 denote a space-time state, where .∈ℝmx.p ^m is the spatial position and .t∈ℝx.t is the time. Within each space-time convex set XvX_v, the local trajectory segment v→vx_v _v is linear in space-time, that is, traversed at a uniform speed but may vary across different sets. For each v, we let Δtv=v.t−v.t t_v=y_v.t-x_v.t and Δv=v.−v. _v=y_v.p-x_v.p denote the elapsed time and spatial displacement of the segment within XvX_v, respectively. For each query, we consider a start space-time state sx_s and a goal spatial position gp_g, with an unconstrained arrival time. To avoid treating multiple start- or goal-containing vertices specially, we augment G with a single auxiliary start vertex vsv_s and a single auxiliary goal vertex vgv_g (Bertsimas and Tsitsiklis, 1997). In this work, we consider a single start state point sx_s and define: Xvs=s.X_v_s=\x_s\. More generally, XvsX_v_s can be any convex set of admissible start states. The auxiliary goal set contains all admissible goal states induced by gp_g. In this work, by convention, a solution must also allow the robot to remain at gp_g after arrival until the time limit tmaxt_max. We therefore define the auxiliary goal set by Xvg= X_v_g= \ (g,t)|0≤t≤tmax and (p_g,t) |0≤ t≤ t_ and (g,t′)∈⋃v∈VXvfor all t′∈[t,tmax]. (p_g,t )∈ _v∈ VX_v\ for all t ∈[t,t_ ] \. Thus, membership in XvgX_v_g certifies that the robot reaches gp_g at a time from which it can safely remain there until tmaxt_ through the space-time collision-free region ⋃v∈VXv _v∈ VX_v represented by G. If the query instead specifies a full goal state, or only requires reaching gp_g without remaining there, the definition of XvgX_v_g can be modified directly while the rest of the formulation remains unchanged. We define the query-augmented ST-GCS as G♯=(V♯,E♯,♯)G =(V ,E ,X ), where V♯=V∪vs,vgV =V∪\v_s,v_g\, ♯=∪Xvs,XvgX =X∪\X_v_s,X_v_g\, and E♯=(u,v)∣u,v∈V♯,u≠v,Xu∩Xv≠∅E =\(u,v) u,v∈ V ,u≠ v,X_u∩ X_v≠ \. We denote this construction by (G♯,vs,vg)=QueryAugment(G,s,g)(G ,v_s,v_g)= QueryAugment(G,x_s,p_g). Fig. 4 illustrates a query-augmented ST-GCS. Figure 4: A query-augmented 2D ST-GCS with auxiliary start vsv_s and goal vgv_g. Input-graph edges are solid while augmented edges are dashed. Vertices v1v_1 and v8v_8 are adjacent to vsv_s because their sets contain sx_s. Vertices v5v_5 and v6v_6 are adjacent to vgv_g because their sets intersect XvgX_v_g, the set of goal states from which the robot can remain at gp_g until tmaxt_max. If the robot is only required to reach gp_g, then v2v_2 is also adjacent to vgv_g. Given a component-wise velocity bound lim∈ℝ>0mv_lim ^m_>0, the time-optimal spatiotemporal planning problem on the query-augmented ST-GCS G♯G is written as: minΦ,, _ ,x,y c(τ)=∑v∈πΦΔtv c(τ)= _v∈ _ t_v (3a) s.t. πΦ∈(vs,vg;G♯), _ (v_s,v_g;G ), (3b) v,v∈Xv, _v,y_v∈ X_v, ∀v∈πΦ, ∀ v∈ _ , (3c) v=u, _v=y_u, ∀(u,v)∈E♯(πΦ),∀(u,v)∈ E ( _ ), (3d) Δtv≥0, t_v≥ 0, ∀v∈πΦ, ∀ v∈ _ , (3e) −limΔtv⪯Δv⪯limΔtv, -v_lim t_v _v _lim t_v, ∀v∈πΦ. ∀ v∈ _ . (3f) Eqn. (3a) minimizes the time cost c(τ)c(τ), defined as the total elapsed time of trajectory τ reconstructed from the selected path πΦ _ and continuous variables. Eqns. (3b–3d) folllow the graph topology, set membership, and continuity constraints from Eqns. (1b–1d), specialized to the query-augmented ST-GCS G♯G . Eqn. (3e) enforces time monotonicity within each selected convex set. Eqn. (3f) imposes component-wise velocity limits by bounding spatial displacement over elapsed time within each selected convex set. The auxiliary start and goal vertices encode the query through set membership and graph connectivity. Since Xvs=sX_v_s=\x_s\, the trajectory starts at sx_s. Since XvgX_v_g contains only states with spatial position gp_g, any feasible path ending at vgv_g reaches the goal position. Under the goal-staying convention, membership in XvgX_v_g further guarantees that the robot can remain at gp_g until tmaxt_ . As illustrated in Fig. 3, a feasible solution reconstructs a continuous, space-time collision-free, piecewise-linear trajectory τ from sx_s to some goal state g=(g,⋅)x_g=(p_g,·). 4.2 Optimization-Based Solving The formulation above can be solved as an MICP. The binary variables select a sequence of space-time convex sets, while the continuous variables optimize the entry and exit states within the selected sets. Time monotonicity, velocity limits, and continuity between consecutive local trajectory segments constrain these continuous states. Following Tang et al. (2025), we use two optimization-based variants as baselines in our experiments. The first, denoted by MICP, solves the mixed-integer program directly. The second, denoted by MICP(g), solves the continuous relaxation and then applies stochastic path rounding multiple times. Once a graph path is fixed, the remaining trajectory optimization is path-conditioned, as described in Sec. 3.3. 5 Graph Search on ST-GCSs This section presents the search-based solver for spatiotemporal planning on ST-GCSs. We first define the path-indexed search space and explain why vertex-indexed shortest-path search is insufficient. We then justify excluding set revisitation under the piecewise-linear trajectory representation. Finally, we describe the best-first search solver, which repeatedly solves path-conditioned convex programs, uses admissible heuristics for guidance, and applies upper-bound pruning and dominance checks for pruning the search tree. 5.1 Search Space In standard cost-minimal-path search on a discrete graph, such as A∗ search (Russell and Norvig, 2020), a search state is typically identified by the current vertex. This is valid because the graph has a vertex-level optimal substructure. Once the minimum cost-to-come to a vertex v is known, any more expensive prefix ending at v can be discarded, since all future costs depend only on v and the remaining path. This vertex-level optimal substructure does not generally hold for motion planning on GCSs or ST-GCSs. The cost of a path is not the sum of fixed edge costs. Instead, once a vertex sequence is chosen, the continuous trajectory variables along the entire sequence are optimized jointly. Consequently, when a prefix path π=⟨vs,…,v⟩π= v_s,…,v is extended to π′=⟨vs,…,v,w⟩π = v_s,…,v,w , the optimal continuous states along the earlier part of the path, including the state at v, may change. Thus, the optimal trajectory conditioned on π′π is not obtained by simply appending a locally optimal transition from v to w to the optimal trajectory conditioned on π. A prefix that is more expensive when considered only up to v may still result in a lower-cost full trajectory after extension. Thus, search nodes for planning on ST-GCSs must be indexed by the full prefix path, rather than only by the arrival vertex. Let Π denote the set of partial vertex paths on the query-augmented G♯G that start from the auxiliary start vertex vsv_s. As justified in Sec. 5.2, we restrict Π to paths that do not revisit a vertex. Definition 1 (Path-Indexed Search Node). A search node, identified by its prefix path, is a record N=(π,v,τ,g,f),N=(π,v,τ,g,f), where N.π∈ΠN.π∈ is the prefix path, N.vN.v is the last vertex of N.πN.π, N.τN.τ is the optimal trajectory conditioned on N.πN.π, N.g=c(N.τ)N.g=c(N.τ) is the cost-to-come, and N.f=N.g+ϵ⋅h(N.π)N.f=N.g+ε· h(N.π) is the search key. For convenience, let N.N.x and N.N.y denote the entry and exit states of the terminal segment of N.τN.τ in XN.vX_N.v, respectively. The fields N.τN.τ, N.gN.g, N.N.x, and N.N.y are computed by solving the path-conditioned convex program for N.πN.π, as defined in Sec. 3.3 and specialized to the ST-GCS formulation in Sec. 4.1. The states N.N.x and N.N.y are not part of the search-node identity; they are stored only as witnesses from the optimized trajectory conditioned on N.πN.π. Since the terminal segment has no successor-continuity constraint, time-minimization implies N.v=N.vx_N.v=y_N.v in an optimal solution. Therefore, N.N.x can be viewed as the stored witness state at N.vN.v. If N.v=vgN.v=v_g, membership in XvgX_v_g ensures that N.τN.τ is a solution trajectory. If the path-conditioned program is infeasible, no search node is created for N.πN.π. Here ϵ≥1ε≥ 1 is a heuristic inflation factor, and h:Π→ℝ≥0h: _≥ 0 is an admissible heuristic as defined in Definition 3. Definition 2 (Prefix-Consistency and Prefix-Conditioned Solution Cost). For a generated search node N, a solution trajectory τ¯ τ is N.πN.π-consistent if its vertex path has N.πN.π as a prefix. Let sol(N.π)T_sol(N.π) denote the set of all N.πN.π-consistent solution trajectories. The prefix-conditioned optimal solution cost is J∗(N.π)=infτ¯∈sol(N.π)c(τ¯)J^*(N.π)= _ τ _sol(N.π)c( τ), with J∗(N.π)=+∞J^*(N.π)=+∞ if sol(N.π)=∅T_sol(N.π)= . Definition 3 (Admissible Heuristic). A heuristic h:Π→ℝ≥0h: _≥ 0 is admissible if, for every generated search node N, N.g+h(N.π)≤J∗(N.π)N.g+h(N.π)≤ J^*(N.π), and h(N.π)=0h(N.π)=0 when N.v=vgN.v=v_g. Equivalently, h(N.π)h(N.π) lower-bounds the optimal remaining solution cost J∗(N.π)−N.gJ^*(N.π)-N.g from its prefix N.πN.π, and this remaining cost is zero if N.v=vgN.v=v_g. 5.2 Set Transition and Revisitation Exclusion The candidate graph transition follows the topology of the query-augmented ST-GCS G♯G . For a vertex v, let Adj(v)=w∈V♯∣(v,w)∈E♯ (v)=\w∈ V (v,w)∈ E \ denote its adjacent vertices. By construction of G♯G , w∈Adj(v)w (v) implies Xv∩Xw≠∅X_v∩ X_w≠ . The temporal direction and velocity feasibility of an extended path are not imposed at the graph-transition level, but are instead enforced by the path-conditioned convex program. Unlike a monolithic GCS optimization, which needs additional machinery to explicitly allow repeated visits to the same convex set, graph search can naturally represent such paths by allowing the same vertex to appear multiple times in a prefix path. However, for the piecewise-linear trajectory representation and time-cost objective as described in Sec. 4.1, such set revisitation is unnecessary. Intuitively, if a trajectory enters the same convex set more than once, then the portion between the first entry and the later exit can be replaced by a single line segment inside that set with no higher cost. Lemma 1. Consider a feasible trajectory conditioned on a vertex path π=⟨v1,…,vi,…,vj,…,vl⟩π= v_1,…,v_i,…,v_j,…,v_l on an ST-GCS, where vi=vj=v_i=v_j=v for some i<ji<j. Under the piecewise-linear trajectory representation and the time-cost objective in Eqn. (3a), there exists a feasible trajectory conditioned on the shortened path π′=⟨v1,…,vi,vj+1,…,vl⟩π = v_1,…,v_i,v_j+1,…,v_l , where the suffix ⟨vj+1,…,vl⟩ v_j+1,…,v_l is omitted when j=lj=l, with no higher cost. Proof 1. Let the feasible trajectory conditioned on π have entry and exit states (k,k)(x_k,y_k) for the k-th vertex occurrence, where k,k∈Xvkx_k,y_k∈ X_v_k, and continuity gives k=k+1y_k=x_k+1 for k=1,…,l−1k=1,…,l-1. Since vi=vjv_i=v_j, both ix_i and jy_j lie in the same convex set XviX_v_i. Hence the straight shortcut segment i→jx_i _j lies entirely in XviX_v_i. It remains to check time monotonicity and velocity feasibility for this shortcut segment. For each original segment k=i,…,jk=i,…,j, let Δtk=k.t−k.t t_k=y_k.t-x_k.t and Δk=k.−k. _k=y_k.p-x_k.p. Feasibility of the original trajectory gives Δtk≥0 t_k≥ 0 and −limΔtk⪯Δk⪯limΔtk-v_lim t_k _k _lim t_k for all k=i,…,jk=i,…,j. Summing these inequalities gives −lim∑k=ijΔtk⪯∑k=ijΔk⪯lim∑k=ijΔtk.-v_lim _k=i^j t_k _k=i^j _k _lim _k=i^j t_k. By continuity, the sums are ∑k=ijΔtk=j.t−i.t,∑k=ijΔk=j.−i.. _k=i^j t_k=y_j.t-x_i.t, _k=i^j _k=y_j.p-x_i.p. Thus, the shortcut segment i→jx_i _j satisfies the same time-monotonicity and velocity constraints. Replacing the portion of the trajectory τ from ix_i to jy_j by this shortcut yields a feasible trajectory on π′π , while preserving continuity with the preceding segment and, if j<lj<l, the following segment. Its time cost j.t−i.t=∑k=ijΔtky_j.t-x_i.t= _k=i^j t_k is equal to the time cost of the removed portion. Thus, the shortened path has a feasible trajectory with no larger cost. Repeating this shortcut operation removes all repeated vertices. Lemma 1 shows that set revisitation is not needed for time optimality under this trajectory representation and cost objective. Thus, it is sufficient to search over simple partial vertex paths without cycles. In Alg. 1, candidate successors are generated from Adj(N.v)Adj(N.v), and successors already appearing in N.πN.π are skipped. 5.3 Best-First Search (BFS) Solver We now present the BFS solver for spatiotemporal planning on ST-GCSs. Given an input ST-GCS G=(V,E,)G=(V,E,X) and a query (s,g)(x_s,p_g), the solver first applies QueryAugment to construct the query-augmented ST-GCS G♯=(V♯,E♯,♯)G =(V ,E ,X ) from Sec. 4.1. It then searches over path-indexed nodes and returns a solution trajectory if one is found. Optionally, it can take a feasible incumbent trajectory τub _ub with cost cubc_ub; if the search exhausts OPEN without finding a better trajectory, this incumbent is returned. The optimality guarantee depends on the heuristic inflation factor, the validity of the incumbent upper bound, and whether the selected dominance check is safe. Input : ST-GCS G=(V,E,)G=(V,E,X), query (s,g)(x_s,p_g) Param : admissible heuristic h, inflation factor ϵ≥1ε≥ 1, valid incumbent pair (cub,τub)←(+∞,∅)(c_ub, _ub)←(+∞, ), dominance check δ←δ∅δ← _ Output : solution trajectory if one exists 1 (G♯,vs,vg)←QueryAugment(G,s,g)(G ,v_s,v_g)← QueryAugment(G,x_s,p_g) 2 τr← _r← zero length trajectory at sx_s 3 Nr=(⟨vs⟩,vs,τr,0,ϵ⋅h(⟨vs⟩))N_r= ( v_s ,v_s, _r,0,ε· h ( v_s ) ) 4 OPEN ←[Nr]←[N_r] ⊳ min-heap prioritized by N.fN.f 5 initialize S(v)←∅S(v)← for each vertex v∈V♯v∈ V 6 S(vs)←NrS(v_s)←\N_r\ 7 while OPEN ≠∅≠ do 8 N←N← OPEN.pop() 9 if N.v=vgN.v=v_g then 10 return N.τN.τ 11 12 foreach w∈Adj(N.v)w (N.v) with w∉N.πw∉ N.π do 13 π←N.π⊕⟨w⟩π← N.π w 14 (τ,g)←PathOptimize(G♯,π)(τ,g)← PathOptimize(G ,π) 15 if τ is feasible then 16 Nchild←(π,w,τ,g,g+ϵ⋅h(π))N_child←(π,w,τ,g,g+ε· h(π)) 17 if Nchild.f<cubN_child.f<c_ub and not δ(Nchild,S(w))δ(N_child,S(w)) then 18 Update S(w)S(w) with NchildN_child 19 OPEN.add(Nchild)N_child) 20 21if τub≠∅ _ub≠ then 22 return τub _ub and “no better solution found” 23 24else 25 return “no solution found”; 26 27 Function PathOptimize(G♯G , π): 28 construct the path-conditioned convex program by fixing πΦ=π _ =π in Eqns. (3a–3f) 29 solve the resulting convex program 30 return the optimal trajectory τ conditioned on π and its cost g=c(τ)g=c(τ), or report infeasibility Algorithm 1 Best-First Search on an ST-GCS Alg. 1 summarizes the procedure. The solver creates the root node NrN_r corresponding to the trivial path ⟨vs⟩ v_s , inserts into OPEN, and stores it in S(vs)S(v_s) (Lines 1–1). The map S defined by S(v)=N|N.v=vS(v)=\N\,|\,N.v=v\ records the accepted search nodes that arrive at each vertex v. These stored nodes are used by dominance checks. At each iteration, the solver expands the node N with the smallest f-value in OPEN (Lines 1–1). If N.v=vgN.v=v_g, the stored trajectory N.τN.τ is returned as a solution (Lines 1–1). Otherwise, the solver expands N by considering each adjacent vertex w of N.vN.v that does not already appear in N.πN.π (Lines 1–1). For each extended path π=N.π⊕⟨w⟩π=N.π w , the solver calls PathOptimize to solve the corresponding path-conditioned convex program. If the program is feasible, a child node is created (Lines 1–1). The child node is discarded if its key is no smaller than the input cost upper bound cubc_ub, or if the dominance check δ certifies that it is dominated by accepted nodes in S(w)S(w). Otherwise, the child node is inserted into both S(w)S(w) and OPEN, and previously accepted nodes in S(w)S(w) that are dominated by the child node are removed (Lines 1–1). If OPEN becomes empty, the solver returns the incumbent trajectory τub _ub if one was provided; otherwise, it reports failure (Lines 1–1). 5.3.1 Upper-Bound Pruning with cubc_ub Alg. 1 uses an optional input cost upper bound cubc_ub to avoid exploring nodes that cannot improve the current incumbent, inspired by the spatial GCS search (Natarajan et al., 2024). If no incumbent is available, it uses cub=+∞c_ub=+∞ and τub=∅ _ub= . If cub<+∞c_ub<+∞, it must be paired with a corresponding feasible incumbent trajectory τub _ub for the same query, with c(τub)=cubc( _ub)=c_ub. This ensures that, if all remaining nodes are pruned by the upper-bound test, the solver can still return a valid trajectory. We obtain τub _ub using a fast vertex-indexed best-first search on the same query-augmented ST-GCS G♯G . This auxiliary search still generates graph paths and calls PathOptimize to validate each path and compute its trajectory, but it applies the standard duplicate-pruning rule from graph search. For each arrival vertex, it keeps only the lowest-cost node found so far and discards later nodes arriving at the same vertex with no smaller cost. As discussed in Sec. 5.1, this vertex-indexed pruning is not sound for optimal planning on ST-GCSs, as future costs depend on the entire prefix path. We therefore use this auxiliary search only to obtain a feasible incumbent. If it reaches vgv_g, the returned trajectory is feasible because it is produced by PathOptimize, and its cost provides a valid cubc_ub for Alg. 1. 5.3.2 Pruning with Dominance Checks δ We now define the dominance relation used by the optional check δ on Line 1 of Alg. 1. Since search nodes are path-indexed, dominance is defined through the path-conditioned optimal solution cost J∗(⋅)J^*(·) (Definition 2). Consider two search nodes N and N′N with the same arrival vertex v. We say that N′N dominates N if J∗(N′.π)≤J∗(N.π)J^*(N .π)≤ J^*(N.π). A dominance check δ(N,S(N.v))δ(N,S(N.v)) is safe if it returns true only when there exists a retained node N′∈S(N.v)N ∈ S(N.v) that dominates N. Safe checks are sufficient but may be conservative. They need not detect all dominated nodes, and they only compare nodes pairwise rather than detecting whether a set of nodes jointly dominate another node. In contrast, heuristic dominance checks may prune without certifying this relation and therefore do not by themselves preserve the theoretical guarantee. 5.3.3 Optimality with Safe Pruning The following theorem states the optimality guarantee for Alg. 1 when the pruning operations used on Line 1 are safe. Theorem 2 (ϵε-Optimality). For any ϵ≥1ε≥ 1, with an admissible heuristic h, a valid incumbent pair (cub,τub)(c_ub, _ub), and a safe dominance check, Alg. 1 returns an ϵε-optimal trajectory for the query on the input ST-GCS whenever a feasible trajectory exists. In particular, when ϵ=1ε=1, the returned trajectory is cost-optimal. Proof 2. Let c∗c^* be the optimal trajectory cost. Because the graph is finite and Lemma 1 lets us restrict attention to simple prefixes, the search space is finite. If τub≠∅ _ub≠ and cub≤ϵc∗c_ub≤ε c^*, returning τub _ub is already ϵε-optimal. Any goal returned before the incumbent has passed the upper-bound test, so its key, and hence its cost, is smaller than cub≤ϵc∗c_ub≤ε c^*. Thus, it remains to consider the complementary case. Either no incumbent is available or cub>ϵc∗c_ub>ε c^*. In both cases, upper-bound pruning keeps every node with key at most ϵc∗ε c^*. We maintain the following invariant: until termination, OPEN contains a representative node N with J∗(N.π)=c∗J^*(N.π)=c^* and N.f≤ϵc∗N.f≤ε c^*. Initially this holds for the root because ⟨vs⟩ v_s prefixes every feasible trajectory and admissibility gives Nr.f=ϵh(⟨vs⟩)≤ϵc∗N_r.f=ε h( v_s )≤ε c^*. If OPEN expands a non-representative node, the representative remains in OPEN unless the expanded node is a goal; in that case the minimum-key rule and h=0h=0 imply that the returned trajectory has cost at most ϵc∗ε c^*. It remains to show that the invariant is preserved when a representative node N is expanded. If N.v=vgN.v=v_g, then N.τN.τ is an optimal solution. Otherwise, take an optimal N.πN.π-consistent solution of cost c∗c^*, and let w be its next vertex, giving the feasible prefix π′=N.π⊕⟨w⟩π =N.π w . Then PathOptimize creates a child M with M.π=π′M.π=π and J∗(M.π)=c∗J^*(M.π)=c^*. By admissibility, we have M.f M.f =M.g+ϵh(M.π) =M.g+ε h(M.π) ≤M.g+ϵ(c∗−M.g)=ϵc∗−(ϵ−1)M.g≤ϵc∗. ≤ M.g+ε(c^*-M.g)=ε c^*-(ε-1)M.g≤ε c^*. By the case assumption above, upper-bound pruning keeps M. If M is inserted into OPEN, the invariant follows. If M is pruned by a safe dominance check, some M′∈S(w)M ∈ S(w) satisfies J∗(M′.π)≤J∗(M.π)=c∗J^*(M .π)≤ J^*(M.π)=c^*. Optimality of c∗c^* gives J∗(M′.π)=c∗J^*(M .π)=c^*, and the same admissibility argument gives M′.f≤ϵc∗M .f≤ε c^*. Thus M′M is a valid representative if it is in OPEN; if it has already been expanded, the same argument applied at its expansion transfers the representative to an OPEN descendant or to a retained node safely dominating such a descendant. Likewise, if a retained representative is later removed from S(w)S(w), safety implies that the newly accepted node has J∗=c∗J^*=c^* and key at most ϵc∗ε c^*. Safe dominance pruning therefore transfers, but never eliminates, all representatives before termination. The incumbent-return case was handled above. In the complementary case, the invariant prevents OPEN from becoming empty before termination, so Alg. 1 must return a goal node NgN_g expanded from OPEN. The invariant and the minimum-key rule give Ng.f≤ϵc∗N_g.f≤ε c^*; since h(Ng.π)=0h(N_g.π)=0, c(Ng.τ)=Ng.g=Ng.f≤ϵc∗c(N_g.τ)=N_g.g=N_g.f≤ε c^*. Setting ϵ=1ε=1 gives cost optimality. 5.4 Pairwise Dominance Checks We now present three pairwise dominance checks as the optional check δ used in Alg. 1. Throughout this subsection, let N be a newly generated node and N′∈S(N.v)N ∈ S(N.v) be a retained node with the same arrival vertex v=N.v=N′.v=N.v=N .v. The trivial case is v=vgv=v_g, where both nodes already represent solution trajectories, and exact dominance reduces to directly comparing their trajectory costs. The checks below are therefore described for non-goal vertices v≠vgv≠ v_g. Each check compares N only with each retained node N′N , rather than testing whether several retained nodes jointly dominate N. This pairwise restriction affects pruning power but not correctness when the check is safe. Recall from Sec. 5.3.2 that N′N dominates N if J∗(N′.π)≤J∗(N.π)J^*(N .π)≤ J^*(N.π). A dominance check is safe if it returns true only when this dominance relation is guaranteed to hold for some N′∈S(N.v)N ∈ S(N.v). Among the three checks below, δset _set is safe, while δstate _state and δpos _pos are heuristic checks that trade optimality guarantees for stronger empirical pruning. 5.4.1 Safe Set-Containment Check δset _set The set-containment check δset _set certifies dominance by showing that N′N can reach every state that an N.πN.π-consistent trajectory may reach after entering the shared vertex v. Let N.π=⟨vs,…,u,v⟩N.π= v_s,…,u,v and define the arrival set RN=Xu∩Xv∩∣.t≥N..t. R_N=X_u∩ X_v∩\x .t≥ N.x.t\. (4) The set RNR_N contains all states that any N.πN.π-consistent solution trajectory may use to enter v. Continuity (Eqn. (1d)) requires membership in the predecessor interface Xu∩XvX_u∩ X_v, and N..tN.x.t is the earliest arrival time achieved by the path-conditioned optimum for N.πN.π. For a space-time state x, define its forward reachable generalized cone under the velocity limits by C()= C(x)= ∈ℝm+1∣.t≥.t, \z ^m+1 .t .t, −lim(.t−.t)⪯.−.⪯lim(.t−.t). -v_lim(z.t-x.t) .p-x.p _lim(z.t-x.t)\. Thus, C()C(x) contains all states reachable from x by a single time-forward segment satisfying the velocity bounds. Then δset _set is defined by δset(N,S)≡ _set(N,S)≡ ∃N′∈S(N.v), ∃\,N ∈ S(N.v), (5) s.t. ∀∈RN,C()⊆C(N′.). .t. ∀\,x∈ R_N,\;C(x) C(N .x). In implementation, since RNR_N is convex, it is sufficient to check whether every corner of RNR_N lies in C(N′.)C(N .x) by transitivity of the cone reachability relation under the same velocity limits. Figure 5: Comparison of dominance checks on a 2D ST-GCS. A node N reaches XvX_v from XuX_u, while a retained node N′∈S(v)N ∈ S(v) reaches XvX_v from XzX_z. In the left panel, δstate _state prunes N because C(N.)⊆C(N′.)C(N.x) C(N .x). In the middle panel, δset _set tests all states in RNR_N (hatched region; Eqn. (4)) and keeps N because ∃∗∈RN ^*∈ R_N such that C(∗)⊈C(N′.)C(x^*) C(N .x). In the right panel, δpos _pos compares arrival times at sampled spatial positions 1p_1 and 2p_2; it prunes N if ∗.x^*.p is sampled since 1.t>∗.tx_1.t>x^*.t, but keeps N if N′..N .x.p is sampled since 2.t>N′..tx_2.t>N .x.t. Lemma 3 (Safety of δset _set). If δset(N,S(N.v)) _set(N,S(N.v)) is true, then N is dominated by a retained node N′∈S(N.v)N ∈ S(N.v). Proof 3. Let N′∈S(N.v)N ∈ S(N.v) satisfy Eqn. (5). Consider any N.πN.π-consistent solution trajectory τ¯ τ. Let x and y be the states where τ¯ τ enters and exits XN.vX_N.v, respectively. By construction, ∈RNx∈ R_N. Moreover, the segment →x is part of τ¯ τ within XN.vX_N.v, so ∈C()y∈ C(x). Eqn. (5) gives C()⊆C(N′.)C(x) C(N .x), and therefore ∈C(N′.)y∈ C(N .x). Since N′.,∈XN.vN .x,y∈ X_N.v, the straight segment N′.→N .x remains in XN.vX_N.v and satisfies the time-monotonicity and velocity constraints. Replacing the zero-duration terminal segment of N′.τN .τ with this segment, and then concatenating the suffix of τ¯ τ after y, yields a solution trajectory with the same time cost as τ¯ τ. Since this holds for every N.πN.π-consistent solution trajectory, we have J∗(N′.π)≤J∗(N.π)J^*(N .π)≤ J^*(N.π). Thus, N′N dominates N. 5.4.2 Heuristic Arrival-State-Containment Check δstate _state The arrival-state-containment check δstate _state approximates δset _set by testing only the stored witness state N.N.x, rather than all states in RNR_N. Then δstate _state is defined by δstate(N,S)≡ _state(N,S)≡ ∃N′∈S(N.v), \,∃\,N ∈ S(N.v), s.t. C(N.)⊆C(N′.). .t. C(N.x) C(N .x). This check is cheaper than δset _set because it only compares two cones. It is heuristic since N.N.x is only the stored witness state of the prefix-optimal trajectory for N.πN.π, while an optimal N.πN.π-consistent solution trajectory may use a different state to enter XN.vX_N.v. 5.4.3 Heuristic Position-Based Dominance Check δpos _pos The set-containment check δset _set can be expensive because it reasons over all states in RNR_N. A cheaper alternative is to compare two prefixes only at a sampled spatial position ∈.|∈XN.vp∈\x.p\,|\,x∈ X_N.v\. For a generated search node N, let J(N.π)J_p(N.π) denote the cost of the optimal trajectory conditioned on N.πN.π whose terminal state in XvX_v has spatial position p, with J(π)=+∞J_p(π)=+∞ if no such trajectory exists. Then δpos _pos is defined by δpos(N,S)≡ _pos(N,S)≡ ∃N′∈S(N.v), \,∃\,N ∈ S(N.v), s.t. J(N′.π)≤J(N.π). s.t. J_p(N .π)≤ J_p(N.π). This check can be strengthened by using multiple sampled positions. In the limiting case of covering all relevant positions in XN.vX_N.v, it can certify a safe pairwise dominance relation between N and a retained node N′N . One can further extend the idea to certify joint dominance by allowing different retained nodes in S(N.v)S(N.v) to dominate N at different positions. These extensions are more expensive. Experimentally, we use only one sampled position, so δpos _pos remains heuristic. 5.4.4 Comparison and Use with Upper-Bound Pruning Fig. 5 illustrates the difference between the three pairwise dominance checks. The δ=δsetδ= _set check reasons over all relevant states in RNR_N, δstate _state only compares the stored witness states, and δpos _pos compares the prefixes at sampled spatial positions. In our implementation, upper-bound pruning is applied before one of these dominance checks. When the input δ=δ∅δ= _ or δ=δsetδ= _set, the guarantee in Theorem 2 applies. When δstate _state or δpos _pos is used, the solver becomes heuristic and can run substantially faster, but the pruning no longer preserves the optimality guarantee. Combining upper-bound pruning with heuristic dominance checks is empirically effective but can make the search more likely to return the incumbent trajectory. The heuristic checks may remove nodes that would otherwise lead to a feasible solution, while the upper-bound test further restricts the search to trajectories with cost strictly below cubc_ub. Thus, a solution that the heuristic search might find without upper-bound pruning can be discarded if it is no better than the incumbent. In that case, Alg. 1 falls back to the stored incumbent trajectory τub _ub. Among the two heuristic checks, δpos _pos is usually more aggressive because it compares prefixes only through sampled spatial positions and may miss useful unsampled states. The check δstate _state is also heuristic, but remains tied to the actual stored witness states through cone containment. This makes δpos _pos generally more sensitive to upper-bound pruning, while δstate _state tends to be more conservative in practice. 5.5 Admissible Heuristics We now describe admissible heuristics for Alg. 1. All heuristics below are defined to be zero when N.v=vgN.v=v_g, since N.τN.τ is already a solution trajectory. Thus, the definitions in this subsection focus on non-goal nodes N.v≠vgN.v≠ v_g. By Definition 3, an admissible heuristic must lower-bound the optimal remaining solution cost from a prefix path N.πN.π, rather than the cost-to-go from the stored witness state N.N.x alone. This distinction is important because N.N.x is only the stored witness state of the trajectory optimized for N.πN.π; after extending the path, the optimal trajectory may pass through a different state in XN.vX_N.v. Thus, a heuristic evaluated from N.N.x can overestimate the optimal remaining solution cost for the path-indexed node, as illustrated in Fig. 6. Figure 6: Inadmissibility of a point-to-go heuristic for a search node N. In the left panel, the search tree contains N and its child N′N . In the right panel, although N stores the stored witness state N.∈Xv1N.x∈ X_v_1, expanding to N′N re-optimizes the full path and instead passes through ′∈Xv1x ∈ X_v_1 before reaching the goal. Thus, even a valid lower bound (red dashed) from N.N.x to the goal can overestimate the optimal remaining solution cost from N.πN.π. For a non-goal node N, we define the prefix interface IN=s,if N.π=⟨vs⟩,Xu∩Xv,if N.π=⟨vs,…,u,v⟩,I_N= cases\x_s\,&if N.π= v_s ,\\ X_u∩ X_v,&if N.π= v_s,…,u,v , cases (6) which is a useful source set for computing heuristics, as any N.πN.π-consistent trajectory must pass through some state in INI_N before continuing beyond the prefix. A general way to construct admissible heuristics is to relax different subsets of constraints in the path-conditioned ST-GCS problem. The original problem enforces graph topology, set membership, continuity between adjacent sets, motion constraints, and the query-specific goal-position constraint. The three heuristics below represent different choices. The hmoth_mot heuristic keeps only the motion constraints, htrih_tri keeps local set-membership and motion constraints but relaxes continuity across local transitions, and htabh_tab uses precomputed true interface-to-set costs with an online correction to the query goal position. Offline precomputation can use the static environment and any dynamic obstacles known before the query. Query-specific information, such as sx_s, gp_g, and vgv_g, is incorporated online after query augmentation. In MRMP, trajectories of previously planned robots are not known during offline preprocessing; the resulting ECD-updated ST-GCSs only remove feasible space, so lower bounds computed on the initial ST-GCS remain admissible relaxations, as discussed later in Sec. 7. For the ST-GCS trajectory representation in Sec. 4.1, we define the following useful quantity for query-specific online computation. For a set Y of space-time states, define the velocity-only travel-time lower bound to the query goal position by ρ(Y,g)=min∈Y∥(g−.)⊘lim∥∞ ρ(Y,p_g)= _x∈ Y \|(p_g-x.p) _lim \|_∞ with value of +∞+∞ if Y=∅Y= , where ⊘ denotes element-wise division. Since the query specifies a goal position with free arrival time, all our heuristics estimate the cost to gp_g rather than to a fixed goal state. 5.5.1 Motion-Only Heuristic hmoth_mot This heuristic relaxes all the graph-topology and set-membership constraints after the prefix and keeps only the motion constraints needed to reach goal gp_g. In a more general setting, hmoth_mot is the minimum travel cost from some state in INI_N to any state with spatial position gp_g, subject only to the chosen motion constraints. For our ST-GCS trajectory representation, the retained motion constraints are time monotonicity and component-wise velocity limits. Therefore, hmoth_mot is defined by hmot(N.π)=ρ(IN,g). h_mot(N.π)=ρ(I_N,p_g). This is a lightweight online heuristic. Compared with estimating from any state in XN.vX_N.v (Chia et al., 2024), using INI_N is stronger because it respects the predecessor–current-set interface induced by the prefix path. Lemma 4. The heuristic hmoth_mot is admissible. Proof 4. Consider any N.πN.π-consistent trajectory τ¯ τ. Let ∈INx∈ I_N be the state through which τ¯ τ passes before continuing beyond the prefix. The prefix portion of τ¯ τ has cost at least N.gN.g as N.gN.g is the optimal cost conditioned on N.πN.π. The remaining portion from x to gp_g satisfies the the velocity limits, so its cost is at least ρ(,g)ρ(\x\,p_g), and therefore at least ρ(IN,g)ρ(I_N,p_g). Thus, c(τ¯)≥N.g+hmotc( τ)≥ N.g+h_mot holds for any N.πN.π-consistent solution trajectory τ¯ τ. 5.5.2 Triplet-Relaxation Heuristic htrih_tri This heuristic keeps the graph topology and local constraints within each convex set, but relaxes continuity between consecutive local transitions. A similar idea has been used by spatial GCS solvers (Natarajan et al., 2024; Tang and Ma, 2026). For each input-graph triplet (u,v,w)(u,v,w) with u,v,w∈Vu,v,w∈ V and u,wu,w adjacent to v in G, a local lower-bound cost is precomputed for moving through XvX_v from the interface Xu∩XvX_u∩ X_v to the interface Xv∩XwX_v∩ X_w, subject to the same local set-membership and motion constraints. In our piecewise-linear instantiation, this cost is computed offline by a local convex program over one segment inside XvX_v with time monotonicity and velocity limits, denoted by q(u,v,w)q(u,v,w). If no such segment exists, q(u,v,w)=+∞q(u,v,w)=+∞. At query time, costs of query-specific triplets induced by the auxiliary goal vertex vgv_g of the form q(u,v,vg)q(u,v,v_g) are computed online by a local convex program similarly. For N.π=⟨…,u,v⟩N.π= …,u,v , let ΠNsuf ^suf_N denote the set of all the suffixes with its predecessor u, where each π∈ΠNsufπ∈ ^suf_N is in the form of π=⟨u,v=w0,…,wl=vg⟩π= u,v=w_0,…,w_l=v_g ending at vgv_g. We define htrih_tri as the minimum accumulated relaxed triplet cost over ΠNsuf ^suf_N by htri(N.π)= h_tri(N.π)= minπ∈ΠNsuf[q(u,w0,w1)+∑i=1l−1q(wi−1,wi,wi+1)], _π∈ ^suf_N [q(u,w_0,w_1)+ _i=1^l-1q(w_i-1,w_i,w_i+1) ], where the first term is omitted if u=vsu=v_s, and the summation is omitted if l=1l=1. For the root node N.π=⟨vs⟩N.π= v_s , it omits the first two triplets and minimizes over the first input-graph vertex w1w_1 after vsv_s. The minimization is computed online as a shortest-path problem on the induced triplet graph, e.g., by Dijkstra’s algorithm (Dijkstra, 2022). Lemma 5. The heuristic htrih_tri is admissible. Proof 5. Fix any N.πN.π-consistent trajectory. Its suffix induces a sequence of triplets. For each triplet, the actual segment through the middle convex set is feasible for the corresponding triplet subproblem, so the triplet cost is at most the actual segment cost. Omitting query-specific start triplets only further relaxes the cost. Summing these inequalities over the suffix lower bounds the optimal remaining solution cost. Since htrih_tri takes the minimum over all suffixes in the relaxed triplet graph, it is at most the remaining cost of any N.πN.π-consistent trajectory beyond the prefix represented by N.πN.π. Figure 7: Heuristic comparison for a prefix path ⟨…,v0,v1⟩ …,v_0,v_1 and a query goal position gp_g on a 2D ST-GCS. Green indicates query-specific online computation, while blue indicates offline precomputed costs. In the left panel, hmoth_mot relaxes the graph and set constraints after the prefix and computes the online motion-only cost from Xv0∩Xv1X_v_0∩ X_v_1 to gp_g. In the middle panel, htrih_tri concatenates offline relaxed triplet costs and the online query-specific cost q(v5,v7,vg)q(v_5,v_7,v_g) along the online-computed minimum-cost suffix ⟨v1,v4,v5,v7,vg⟩ v_1,v_4,v_5,v_7,v_g . In the right panel, htabh_tab combines the offline interface-to-set cost from Xv0∩Xv1X_v_0∩ X_v_1 to v7v_7 with an online correction to gp_g. 5.5.3 Interface-to-Set Cost Table Heuristic htabh_tab This heuristic precomputes stronger lower bounds by solving interface-to-set problems offline and storing their solution costs in a table. For any input-graph interface I=Xu∩XvI=X_u∩ X_v with u,v∈Vu,v∈ V and any input-graph vertex w∈Vw∈ V, let d(I,w)d(I,w) denote the minimum trajectory cost of reaching any state in XwX_w from any state in I, over all graph paths, subject to the same graph, set-membership, continuity, and motion constraints. These values can be precomputed offline for all interfaces and all vertices by running Alg. 1 to solve the corresponding interface-to-set problems. They are query independent because they start from input-graph interfaces and end at input-graph sets. For a non-goal search node N, define the relaxed table-start value by d^N(vp)= d_N(v_p)= minv∈Adj(vs)∩Vw∈Adj(v)∩Vd(Xv∩Xw,vp),if N.v=vs,minw∈Adj(N.v)∩Vd(XN.v∩Xw,vp),if N.π=⟨vs,N.v⟩,d(IN,vp),otherwise. dcases _v (v_s)∩ V w (v)∩ Vd(X_v∩ X_w,v_p),&if N.v=v_s,\\ _w (N.v)∩ Vd(X_N.v∩ X_w,v_p),&if N.π= v_s,N.v ,\\ d(I_N,v_p),&otherwise. dcases The first two cases handle query-specific start interfaces by omitting the nonnegative cost from sx_s to the first input-graph interface used by the cost table. For a non-goal N with N.v∉Adj(vg)N.v (v_g), we define the heuristic by htab(N.π)=minvp∈Adj(vg)∩Vu∈Adj(vp)∩V[d^N(vp)+ρ(Xu∩Xvp,g)].h_tab(N.π)= _ subarraycv_p (v_g)∩ V\\ u (v_p)∩ V subarray [ d_N(v_p)+ρ(X_u∩ X_v_p,p_g) ]. If N.v∈Adj(vg)N.v (v_g), we instead set htab(N.π)=ρ(IN,g)h_tab(N.π)=ρ(I_N,p_g). Lemma 6. The heuristic htabh_tab is admissible. Proof 6. If N.v∈Adj(vg)N.v (v_g), then htab(N.π)=ρ(IN,g)h_tab(N.π)=ρ(I_N,p_g) is admissible by the same argument as Lemma 4. Now consider N.v∉Adj(vg)⋃vgN.v (v_g) \v_g\ and any N.πN.π-consistent solution trajectory τ¯ τ. Let vp∈Adj(vg)∩Vv_p (v_g)∩ V be the input-graph vertex through which the vertex path of τ¯ τ reaches vgv_g, and let u∈Adj(vp)∩Vu (v_p)∩ V be the input-graph vertex preceding vpv_p on that path. If INI_N is an input-graph interface, the portion from INI_N to XvpX_v_p has cost at least d(IN,vp)d(I_N,v_p). If INI_N is query-specific because N.v=vsN.v=v_s or N.π=⟨vs,N.v⟩N.π= v_s,N.v , then d^N(vp) d_N(v_p) starts from a later input-graph interface and omits only nonnegative cost needed to reach that interface, so it remains a lower bound. The final portion from Xu∩XvpX_u∩ X_v_p to gp_g satisfies the velocity limits and is lower-bounded by ρ(Xu∩Xvp,g)ρ(X_u∩ X_v_p,p_g). Thus, the corresponding term in the minimization defining htabh_tab is no larger than the remaining cost of τ¯ τ. Since htabh_tab minimizes over all such pairs (u,vp)(u,v_p), it is also no larger than the remaining cost of τ¯ τ, for any N.πN.π-consistent solution trajectory. 5.5.4 Comparison and Maximum Heuristic hmaxh_max Fig. 7 illustrates the three heuristics above on an example query. They capture complementary relaxations of the optimal remaining solution cost from a prefix path. The maximum heuristic hmaxh_max combines them by taking their pointwise maximum: hmax(N.π)=maxhmot(N.π),htri(N.π),htab(N.π). h_max(N.π)= \h_mot(N.π),h_tri(N.π),h_tab(N.π) \. This preserves the strongest available lower bound at each search node. Lemma 7. The heuristic hmaxh_max is admissible. Proof 7. Each component heuristic is no larger than the optimal remaining solution cost from N.πN.π by Lemmas 4, 5, and 6. The maximum of several lower bounds is still a lower bound, so hmaxh_max is admissible. 6 Trajectory Occupancy Reservation This section presents the Exact Convex Decomposition (ECD) scheme for reserving piecewise-linear trajectory occupancies in an ST-GCS. Given the spatiotemporal occupancy of a moving object, such as a robot or a dynamic obstacle, ECD removes the occupied region from the relevant space-time convex sets and decomposes the remaining free space into convex subsets. The resulting ST-GCS can then be used for subsequent planning queries that must avoid the reserved trajectory in space-time. 6.1 Spatiotemporal Trajectory Occupancy We assume that each moving object, including a robot or a dynamic obstacle, is represented by a piecewise-linear center trajectory τ=⟨1,…,l⟩τ= _1,…,x_l in (m+1)(m+1)-dimensional space-time with radius r>0r>0. At each time, the object is centered at the position .x.p of state x along τ and occupies a spatial collision body of hypercube ⊡(.,r)=∣||−.||∞≤r⊆ℝm. (x.p,r)=\p ||p-x.p||_∞≤ r\ ^m. (7) Let Ψ(i,i+1,r) (x_i,x_i+1,r) be the parallelotope obtained by sweeping the spatial ⊡(.,r) (x.p,r) along each segment i→i+1x_i _i+1 of τ. A piecewise-convex spatiotemporal occupancy for trajectory τ with object radius r can be defined by Ω(τ,r)=⋃i=1l−1Ψ(i,i+1,r)⊆ℝm+1. (τ,r)= _i=1^l-1 (x_i,x_i+1,r) ^m+1. (8) Consider a robot with radius r and another moving object with trajectory τ and radius r′r . An ST-GCS without any intersections with Ω(τ,r+r′) (τ,r+r ) naturally represents the collection of the space-time collision-free space for the robot. In the next section, we introduce the ECD scheme, which aims to reserve (i.e., remove) any occupancy represented as in Eqn. (8) from an ST-GCS. Fig. 8 illustrates two piecewise-linear trajectories and their reserved occupancies in 1D and 2D spaces. 6.2 Exact Convex Decomposition (ECD) ECD reserves trajectory occupancy on an arbitrary ST-GCS G=(V,E,)G=(V,E,X) using a piecewise-linear trajectory τ=⟨1,…,l⟩τ= _1,…,x_l and a safe clearance parameter R as input. As aforementioned, R is typically the sum of the radii of two objects, with a positive offset if needed. For each parallelotope piece Ψ(i,i+1,R)∈Ω(τ,R) (x_i,x_i+1,R)∈ (τ,R) of τ, it may intersects multiple convex sets of G. On the other hand, a convex set of G might also contain multiple parallelotope occupancy pieces. To account for above cases, ECD subdivides τ and constructs an ordered sequence of vertex–segments tuples Lv=⟨(j,j)⟩j=1ηvL_v= (x_j,y_j) _j=1 _v for each v∈Vv∈ V, such that for every j=1,…,ηvj=1,…, _v, it satisfies that Ψ(j,j,R)∩Xv≠∅ (x_j,y_j,R)∩ X_v≠ and that j.t≤j+1.ty_j.t _j+1.t if j≠ηvj≠ _v. Intuitively speaking, each sequence LvL_v collects the time-sorted subdivided segments of τ whose occupancies intersect with Xv∈X_v . Writing ℒL for all collected vertex–segment tuples, we have ⋃(v,,)∈ℒΨ(,,R)=Ω(τ,R) _(v,x,y) (x,y,R)= (τ,R) Input: ST-GCS G=(V,E,)G=(V,E,X), piecewise-linear trajectory τ, safe clearance R Output: updated ST-GCS with Ω(τ,R) (τ,R) removed 1 G′=(V′,E′,′)←G =(V ,E ,X )← a copy of G=(V,E,)G=(V,E,X) 2 foreach v∈Vv∈ V do 3 Lv←L_v← vertex–segments tuple sequence of τ for v 4 if LvL_v is empty then 5 continue 6 7 foreach (j,j)∈Lv(x_j,y_j)∈ L_v do 8 Y←Xv∩∣j.t≤.t≤j.tY← X_v∩\x _j.t .t _j.t\ 9 foreach (,)∈the facets of Ψ(j,j,R)(A,b) facets of (x_j,y_j,R) do 10 X←Y∩∣⪰X← Y∩\x \ 11 Y←Y∩∣⪯Y← Y∩\x \ 12 add vertex v′v to V′V and set Xv′=X_v =X to ′X 13 14 add the XvX_v residuals defined in Eqn. (9) to G′G 15 remove v and XvX_v from G′G 16 17E′←(u,w)∣u,w∈V′,u≠w,Xu∩Xw≠∅E ←\(u,w) u,w∈ V ,\;u≠ w,\;X_u∩ X_w≠ \ 18 return updated ST-GCS G′=(V′,E′,′)G =(V ,E ,X ) Algorithm 2 Exact Convex Decomposition Figure 8: Reserved trajectory occupancy Ω(τ,r) (τ,r) in Eqn. (8) and the ECD scheme. (a) A piecewise-linear trajectory τ=⟨1,2,3,4⟩τ= _1,x_2,x_3,x_4 in 2D space-time and its occupancy Ω(τ,r) (τ,r), represented as a union of parallelotopes. (b) The input 2D ST-GCS, together with the first separating hyperplanes induced by the occupancy parallelotopes. (c)–(d) Progressive ECD slicing of XvX_v. Previously used separating hyperplanes are shown in light dashed lines, and newly introduced hyperplanes are shown in black dashed lines. (e) The updated ST-GCS after ECD, whose convex sets partition Xv∖Ω(τ,r)X_v (τ,r), overlaid with the trajectory and the outline of Ω(τ,r) (τ,r). (f) A 3D ST-GCS after ECD reservation, with a trajectory in red and its reserved occupancy overlaid. Alg. 2 summarizes the ECD scheme. It starts by iterating each vertex v∈Vv∈ V and collects the corresponding vertex–segments tuple sequence LvL_v from τ (Lines 2-2). If LvL_v is not empty, ECD replaces XvX_v by a convex decomposition of Xv∖Ω(τ,R)X_v (τ,R) (Lines 2–2) and removes the original v and XvX_v from G′G (Line 2). For each segment j→jx_j _j from LvL_v, it slices only the time-bounded subset Y⊆XvY X_v (Line 2). Using each facet of the parallelotope occupancy Ψ(j,j,R) (x_j,y_j,R) as the separating hyperplane (Line 2), ECD creates outside convex pieces disjoint from the parallelotope (Line 2) and updates the residual on the occupancy side (Line 2). By convention, each facet (,)(A,b) is formed such that the convex set ∣⪯\x \ contains parallelotope Ψ(j,j,R) (x_j,y_j,R). After the subdivision by each time-bounded Y, there remain three types of XvX_v residuals that are outside the interiors of the time-bounded subsets Y and thus need to be added back to G′G if nonempty (Line 2). We define the three types of XvX_v residuals as follows: Xv∩∣.t≤1.t, X_v∩\x .t _1.t\, (9a) Xv∩∣j−1.t≤.t≤j.t, X_v∩\x _j-1.t .t _j.t\, ∀j=2,…,ηv, ∀ j=2,…, _v, (9b) Xv∩∣.t≥ηv.t X_v∩\x .t _ _v.t\ (9c) Finally, ECD rebuilds the adjacency of G′G by checking set intersections (Line 2), and returns the updated ST-GCS G′G (Line 2). The ECD scheme is quite general and applies to various other motions that can be represented by a piecewise-linear space-time trajectory. For example, waiting or in-place turning can be encoded as a segment (,t)→(,t′)(p,t)→(p,t ) along τ and being reserved in the input ST-GCS. More importantly, as we will see in Sec. 7, pre-start and post-arrival waiting segments should be inserted into the beginning and end of every single-robot trajectory τ, respectively. As a result, when planning on the ST-GCS with τ reserved by ECD, the other robots must avoid the robot that stays at the start and goal positions before and after executing τ. 7 Multi-Robot Motion Planning This section integrates spatiotemporal planning on ST-GCSs and ECD reservation for Multi-Robot Motion Planning (MRMP). The low-level solver solves single-robot queries one at a time with the BFS solver in Sec. 5, while the high-level coordinator resolves inter-robot conflicts by deciding which robot trajectories are reserved in each low-level call. 7.1 Problem Definition We consider n robots with the same radius r sharing a given base ST-GCS G0=(V0,E0,0)G_0=(V_0,E_0,X_0), which represents the collision-free space-time region before inter-robot reservations and may be constructed, for example, by extruding a spatial GCS through time and then applying the ECD scheme to remove occupancies of known dynamic obstacles. Each robot i=1,2,…,ni=1,2,…,n has a query (s,i,g,i)(x_s,i,p_g,i) with s,i=(s,i,ts,i)∈ℝm+1x_s,i=(p_s,i,t_s,i) ^m+1, where the arrival time at g,ip_g,i is unconstrained. A feasible solution of any query (s,i,g,i)(x_s,i,p_g,i) on ST-GCS is a piecewise-linear trajectory τi=⟨1,…,l⟩ _i= _1,…,x_l , where 1=s,ix_1=x_s,i and l.=g,ix_l.p=p_g,i. When ts,i≠0t_s,i≠ 0 or l.t≠tmaxx_l.t≠ t_ , the pre-start or post-arrival staying motions are absent in τi _i. Planning on an ECD-reserved ST-GCS with such τi _i could lead to collisions on s,ip_s,i during t∈[0,ts,i]t∈[0,t_s,i] or on g,ip_g,i during t∈[l.t,tmax]t∈[x_l.t,t_ ]. Therefore, we let τ~i τ_i denote its endpoint-augmented variant, defined as follows: τ~i=⟨(1.,0),1,…,l,(l.,tmax)⟩, τ_i= (x_1.p,0),x_1,…,x_l,(x_l.p,t_ ) , A trajectory set =τii=1nT=\ _i\_i=1^n is feasible if and only if each τi _i is a feasible single-robot trajectory for query (s,i,g,i)(x_s,i,p_g,i) on G0G_0, and every robot pair is collision-free under the robot occupancy model defined in Eqn. (8): Ω(τ~j,r)∩Ω(τ~i,r)=∅,∀i≠j. ( τ_j,r)∩ ( τ_i,r)= , ∀\,i≠ j. The MRMP problem seeks to find such a feasible trajectory set described above. We evaluate the MRMP solution quality by two common aggregate metrics, namely the sum-of-costs (SOC) and the makespan: SOC()=∑i=1nc(τi),Makespan()=maxi=1,…,nc(τi), (T)= _i=1^nc( _i),\,Makespan(T)= _i=1,…,nc( _i), where makespan is the maximum individual trajectory duration, excluding the pre-start waiting time. For notation, let ECD(G,τ,R)ECD(G,τ,R) denote the ST-GCS returned by Alg. 2. For an ordered list of trajectories, ECD(G,⟨τ1,…,τl⟩,2r)ECD(G, _1,…, _l ,2r) denotes the ST-GCS obtained by applying Alg. 2 successively to τ1,…,τl _1,…, _l in that order, with ECD(G,⟨⟩,2r)=GECD(G, ,2r)=G. It is worth noting that, for MRMP where the robots have different radii, we only need to construct an individual base G0G_0 for each robot and change the ECD clearance parameter accordingly, while the low-level single-robot solver and the following high-level coordination algorithms remain unchanged. 7.2 Prioritized Planning with ST-GCS and ECD We instantiate Prioritized Planning (P) (Erdmann and Lozano-Perez, 1987) in the ST-GCS setting using the BFS solver in Alg. 1 for low-level single-robot planning and ECD for trajectory reservation. We consider a total priority order =⟨σ1,…,σn⟩ σ= _1,…, _n , where σ is a permutation of the robot indices 1,2,…,n1,2,…,n. Robot σi _i has higher priority than robot σj _j if i<ji<j. P plans robots sequentially on progressively updated ST-GCSs. When planning for robot σk _k, the trajectories of robots σ1,σ2,…,σk−1 _1, _2,…, _k-1 have already been planned and reserved. Set G(1)=G0G^(1)=G_0. At iteration k, robot σk _k is planned on G(k)G^(k). The low-level solver solves the query (s,σk,g,σk)(x_s, _k,p_g, _k) on G(k)G^(k), so the returned trajectory avoids all higher-priority trajectories, including their endpoint-staying portions. If this call fails, P reports failure for the priority order σ. Otherwise, after obtaining τσk _ _k, P updates the graph for the next iteration by G(k+1)=ECD(G(k),τ~σk,2r). G^(k+1)=ECD(G^(k), τ_ _k,2r). (10) After all robots have been processed, P returns the solution set of trajectories. We consider the base ST-GCS G0G_0 constructed by extruding a spatial GCS along the time dimension from 0 to tmaxt_max. Without loss of generality, we assume no dynamic obstacles are present to analyze the MRMP solvability on ST-GCS. Let t0=maxi=1,…,nts,it_0= _i=1,…,nt_s,i defines the time when all robots are ready to move. In case there are dynamic obstacles, we shift t0t_0 to the time when all dynamic obstacles have reached their terminal positions and all robots are ready to go, and the rest analysis remains identical. Let τstay(,t)=⟨(,t),(,tmax)⟩ _stay(p,t)= (p,t),(p,t_max) denote the trajectory segment staying at position p from some time t≤tmaxt≤ t_max to tmaxt_max. We further define a sequence i(t)=⟨ _i(t)= τstay(s,j1,t),…,τstay(s,jn−1,t), _stay(p_s,j_1,t),…, _stay(p_s,j_n-1,t), τstay(g,j1,t),…,τstay(g,jn−1,t)⟩ _stay(p_g,j_1,t),…, _stay(p_g,j_n-1,t) collecting the start and goal staying trajectories for all robots j=j1,…,jn−1j=j_1,…,j_n-1 and j≠ij≠ i. We now define well-formedness conditions for MRMP instances that are sufficient for the completeness of P on ST-GCS. Definition 4 (Well-Formedness). The MRMP instance is well-formed if it satisfies the following conditions: (i) The query starts and goals are pairwise separated, that is, for every pair i≠ji≠ j, ‖s,i−s,j‖∞≥2r||p_s,i-p_s,j||_∞≥ 2r, ‖g,i−g,j‖∞≥2r||p_g,i-p_g,j||_∞≥ 2r, and ‖s,i−g,j‖∞≥2r||p_s,i-p_g,j||_∞≥ 2r; (i) The robots are also assumed to be able to safely stay at their query start and goal positions indefinitely throughout the time horizon, that is, ∣.∈s,i,g,i,.t∈[0,tmax] \x .p∈\p_s,i,p_g,i\,x.t∈[0,t_max]\ ⊆⋃v∈V0Xv; _v∈ V_0X_v; (i) For each robot i=1,…,ni=1,…,n, query ((s,i,t0),g,i)((p_s,i,t_0),p_g,i) is feasible on ECD(G0,i(t0),2r)ECD(G_0,S_i(t_0),2r) and has an optimal trajectory cost of Ci∗C^*_i; (iv) The ST-GCS global time limit tmax≥t0+nCt_max≥ t_0+nC, where C=maxϵC1∗,…,ϵCn∗C= \ε C^*_1,…,ε C^*_n\ with ϵ≥1ε≥ 1. Theorem 8 (Completeness of P). For a well-formed MRMP instance, P with an ϵε-bounded-suboptimal low-level BFS solver returns a feasible trajectory set using an arbitrary total priority order. Proof 8. Fix any total priority order =⟨σ1,…,σn⟩ σ= _1,…, _n . Let tk=t0+(k−1)Ct_k=t_0+(k-1)C. We prove by induction that, before robot σk _k is planned, every higher-priority robot has reached its goal by time tkt_k. The claim is trivial for k=1k=1. Assume it holds for k. By induction, all higher-priority robots have reached their goals by time tkt_k, and they can safely stay at their corresponding goal positions onward by conditions (i-i). According to condition (i), we know that query ((s,σk,t0),g,σk)((p_s, _k,t_0),p_g, _k) is feasible on ECD(G0,σk(t0),2r)ECD(G_0,S_ _k(t_0),2r) with optimal cost Cσk∗C^*_ _k. In fact, σk(tk)S_ _k(t_k) is a time-padded copy of σk(t0)S_ _k(t_0) in the sense that each spatial position in σk(tk)S_ _k(t_k) has the same time offset Δt=tk−t0≥0 t=t_k-t_0≥ 0 compared to each corresponding spatial position in σk(t0)S_ _k(t_0). Therefore, it is easy to see that a feasible trajectory for query ((s,σk,tk),g,σk)((p_s, _k,t_k),p_g, _k) with at most ϵCσk∗ε C^*_ _k cost can always be found by shifting Δt t in each state time along an ϵε-optimal trajectory for query ((s,σk,t0),g,σk)((p_s, _k,t_0),p_g, _k), since we know that tk+ϵCσk∗≤t0+(n−1)C+ϵCσk∗≤tmax t_k+ε C^*_ _k≤ t_0+(n-1)C+ε C^*_ _k≤ t_max by condition (iv). Thus, robot σk _k starts at tkt_k, reaches its goal no later than tk+1=tk+C≥tk+ϵCσk∗t_k+1=t_k+C≥ t_k+ε C^*_ _k, and stays at its goal after arrival while being collision-free with σk+1,…,σn _k+1,…, _n staying at their starts and σ1,…,σk−1 _1,…, _k-1 staying at their goals. This proves the induction. It remains to show that an ϵε-bounded-suboptimal low-level solver in P is certified to plan a trajectory with a cost of at most C for robot σk _k on G(k)G^(k) defined in Eqn. (10). Notice that as σk _k departs at time tkt_k, its feasible solution space regarding each convex set XvX_v from G(k)G^(k) is a time-bounded subset X¯v=Xv∩∣.t≥tk X_v=X_v∩\x .t≥ t_k\. Denote the ST-GCS built from each such X¯v X_v as G¯(k) G^(k). It is easy to see that the union of all convex sets in ECD(G0,σk(tk),2r)ECD(G_0,S_ _k(t_k),2r) is a subset of the union in G¯(k) G^(k), and then subsequently a subset of the union in G(k)G^(k). Therefore, the query ((s,σk,tk),g,σk)((p_s, _k,t_k),p_g, _k) is always feasible on the larger solution space of G(k)G^(k), since we have seen from above that same query is feasible on ECD(G0,σk(tk),2r)ECD(G_0,S_ _k(t_k),2r). Thus, the ϵε-bounded-suboptimal low-level solver finds a trajectory for robot σk _k with at most ϵCσk∗≤Cε C^*_ _k≤ C cost, which concludes the proof. 7.3 PBS with ST-GCS and ECD We instantiate Priority-Based Search (PBS) (Ma et al., 2019) in the ST-GCS setting by using the BFS solver in Alg. 1 for low-level single-robot planning and ECD for reserving spatiotemporal occupancies. PBS searches over partial priority orders instead of committing to one total priority order. A PBS node N stores a set ≺N _N of ordered robot pairs and a trajectory set N.N.T. Let ≺N+ _N^+ denotes the transitive closure of ≺N _N such that if i≺N+ji ^+_Nj, then robot j must avoid robot i. Namely, its trajectory N.τjN. _j can be obtained by planning on GN(j)=ECD(G0,⟨N.τ~i1,…,N.τ~il⟩,2r), G^(j)_N=ECD(G_0, N. τ_i_1,…,N. τ_i_l ,2r), (11) where ⟨i1,…,il⟩ i_1,…,i_l is any fixed topological ordering of the set i∣i≺N+j\i i _N^+j\ of all high-priority robots. Here, N.τ~iN. τ_i denotes the endpoint-augmented reservation trajectory corresponding to the stored trajectory N.τiN. _i. Unlike P, PBS reserves only endpoint-augmented trajectories that are constrained to have higher priority than j. If two robots have no priority relation, neither is forced to avoid the other’s endpoint-augmented trajectory until a collision introduces such a constraint. Alg. 3 summarizes the procedure of PBS. It starts with an empty partial priority set and independently planned trajectories (Lines 3-3). PBS performs a depth-first search on a priority tree using STACK. When PBS selects a node N from STACK, if the endpoint-augmented trajectories corresponding to N.N.T are pairwise collision-free, PBS returns it (Line 3). Otherwise, it identifies a colliding robot pair i1,i2i_1,i_2 whose endpoint-augmented trajectories N.τ~i1N. τ_i_1 and N.τ~i2N. τ_i_2 collide (Line 3). PBS then creates two child nodes by orienting this collision in both directions. One child adds i1≺i2i_1 i_2, and the other adds i2≺i1i_2 i_1 (Lines 3-3). For any branch (i,j)(i,j), the child node N′N adds i≺ji j and calls UpdateNode(N′,j)(N ,j), where j is the newly lower-priority robot. UpdateNode builds the replanning set K, containing j and all robots whose higher-priority constraints may change after adding i≺ji j (Line 3). It then processes each robot k∈Kk∈ K in a topological ordering of ≺N′ _N . If N′.τ~kN . τ_k collides with N′.τ~i′N . τ_i of some higher-priority robot i′≺N′+ki _N ^+k, UpdateNode replans τk _k to respect all of higher-priority reservations of robot k (Lines 3–3). If UpdateNode succeeds, PBS adds the child node N′N to STACK (Lines 3–3). PBS returns failure if no valid solution can be found after exploring all nodes (Line 3). Theorem 9 (Completeness of PBS). For a well-formed MRMP instance, PBS with an ϵε-bounded-suboptimal low-level BFS solver returns a feasible trajectory set. Proof 9. We first show that the PBS search tree is finite. For every node inserted into STACK, the construction of UpdateNode ensures that each lower-priority robot avoids the endpoint-augmented trajectories of all higher-priority robots. Therefore, when PBS finds a colliding pair at a popped node, the two robots must be incomparable under ≺N+ _N^+; otherwise the lower-priority robot would already avoid the higher-priority reservation. Each child then adds one priority relation between a previously incomparable pair and preserves acyclicity. Since an acyclic priority relation on n robots contains at most n(n−1)/2n(n-1)/2 ordered pairs, every branch has finite depth, and the binary priority tree is finite. We now show that every UpdateNode call succeeds. For a PBS node N and robot j, define HN(j)=i∣i≺N+jH_N(j)=\i i _N^+j\ as the higher-priority set of j. We prove this by induction over inserted nodes with the invariant that each robot j reaches its goal by time t0+(|HN(j)|+1)Ct_0+(|H_N(j)|+1)C and stays there afterward. The root low-level calls succeed by the same argument used in the proof of Theorem 8. With no higher-priority reservations, each robot can reach its goal by time t0+Ct_0+C. Now consider UpdateNode(N′,j)(N ,j) for a child N′N obtained by adding i≺ji j to a popped node. Robots outside the replanning set K have the same trajectories and higher-priority sets, so the invariant remains true for them. Because UpdateNode replans the robots in K in topological order, the trajectory of every higher-priority robot of any k∈Kk∈ K is either unchanged from the popped node or has already been replanned in this UpdateNode call. Moreover, by transitivity, any higher-priority robot of such a robot is also a higher-priority robot of k; hence each higher-priority robot of k has at most |HN′(k)|−1|H_N (k)|-1 higher-priority robots. By the induction bound, each of them has reached its goal and is staying there by time tk=t0+|HN′(k)|Ct_k=t_0+|H_N (k)|C. Thus all higher-priority reservations for k are goal stays after time tkt_k. Condition (i) gives robot k a feasible trajectory of cost Ck∗C_k^* on ECD(G0,k(t0),2r)ECD(G_0,S_k(t_0),2r); shifting that trajectory to depart at tkt_k gives a feasible witness in GN′(k)G_N ^(k) because, after that time, GN′(k)G_N ^(k) reserves only those higher-priority goal stays. This witness fits in the horizon since |HN′(k)|≤n−1|H_N (k)|≤ n-1 and tmax≥t0+nCt_max≥ t_0+nC. Therefore, if UpdateNode replans k, the ϵε-bounded-suboptimal low-level solver succeeds and returns a trajectory with cost at most ϵCk∗≤Cε C_k^*≤ C, so k reaches its goal by time tk+C=t0+(|HN′(k)|+1)Ct_k+C=t_0+(|H_N (k)|+1)C. If k is not replanned, its current trajectory already avoids all higher-priority reservations, so it also remains valid. Its previous timing bound also remains valid because its higher-priority set has only grown. Hence, every processed robot is updated successfully and satisfies the invariant, and thus UpdateNode returns success. PBS therefore explores a finite tree without losing any acyclic child to low-level failure. At any total-priority node, every robot pair is comparable, and by construction each lower-priority trajectory respects each higher-priority trajectory. Such a node is collision-free, so PBS eventually pops a collision-free node and returns its feasible trajectory set. Input: queries (s,i,g,i)i=1n\(x_s,i,p_g,i)\_i=1^n, base ST-GCS G0G_0 Output: collision-free trajectory set 1 create root node NrootN_root with ≺Nroot←∅ _N_root← 2 Nroot.←N_root.T← solve all queries on G0G_0 using Alg. 1 3 STACK ←Nroot←\N_root\ 4 while STACK ≠∅≠ do 5 N←N← STACK.pop() 6 if N.τ~ii=1n\N. τ_i\_i=1^n is pairwise collision-free then 7 return N.N.T 8 9 i1,i2←i_1,i_2← robot pair with colliding N.τ~i1N. τ_i_1 and N.τ~i2N. τ_i_2 10 for (i,j)∈(i1,i2),(i2,i1)(i,j)∈\(i_1,i_2),(i_2,i_1)\ do 11 N′←N ← a copy of N with ≺N′←≺N∪i≺j _N ← _N∪\i j\ 12 if UpdateNode(N′,j)(N ,j) then 13 STACK.add(N′N ) 14 15return “no solution found” 16 Function UpdateNode(N′,jN ,j): 17 K←k∣k=j or j≺N′+kK←\k k=j or j _N ^+k\ 18 foreach k∈TopologicalSort(K,≺N′)k (K, _N ) do 19 if ∃i′≺N′+k:N′.τ~i′∃\,i _N ^+k:N . τ_i colliding with N′.τ~kN . τ_k then 20 construct GN′(k)G^(k)_N (Eqn. (11)) via ECD ⊳ Alg. 2 21 solve query (s,k,g,k)(x_s,k,p_g,k) on GN′(k)G^(k)_N using Alg. 1 22 if solving finds a feasible solution τ then 23 N′.τk←τN . _k←τ 24 25 else 26 return False 27 28 return True 29 Algorithm 3 PBS with ST-GCS 7.3.1 Node Evaluation and Ordering Rules (Lines 3-3 of Alg. 3) PBS can vary how child nodes are generated and expanded. A child node is generated once its new priority relation is added, and it is expanded once UpdateNode has replanned the affected robots under that relation. With lazy evaluation, both child nodes are generated before calling UpdateNode; expansion is delayed until a generated child is selected for evaluation. This avoids spending low-level planning effort on generated nodes that may never be expanded. Alternatively, PBS can expand both child nodes immediately and order the expanded children by a metric. A metric of sum-of-costs or makespan favors branches with better current solution quality, while a metric of the number of pairwise conflicts favors less congested branches. These metric-based rules require immediate node expansion via UpdateNode and therefore are not compatible with lazy evaluation. Since Alg. 3 uses a stack, the favored expanded child is pushed after the other child so that it is selected first. 7.4 Reuse of Base ST-GCS Heuristic Values In P or PBS, the low-level solver is repeatedly called on ECD-updated ST-GCSs. The motion-only heuristic hmoth_mot is evaluated directly on the current query-augmented graph using the actual prefix interface INI_N. In contrast, htrih_tri and htabh_tab rely on heuristic values computed on the base ST-GCS G0G_0, and we reuse these values on ECD-updated graphs through the corresponding base vertices and interfaces. Let G=(V,E,)G=(V,E,X) be any ST-GCS obtained from G0G_0 by a finite sequence of ECD reservations, and consider any query-updated (G♯,vs,vg)=QueryAugment(G,s,g)(G ,v_s,v_g)= QueryAugment(G,x_s,p_g). For every non-query vertex v∈Vv∈ V, let β(v)∈V0β(v)∈ V_0 denote the base vertex of G0G_0 from which v descends, so that Xv⊆Xβ(v)X_v X_β(v). This map is inherited through ECD subdivisions. Initially β(v)=vβ(v)=v for v∈V0v∈ V_0, and when ECD subdivides a vertex v into new vertices, each new vertex w is assigned β(w)=β(v)β(w)=β(v). Specifically, we define β(vs)=vsβ(v_s)=v_s and β(vg)=vgβ(v_g)=v_g for the auxiliary query vertices. Consider a non-goal search node N generated on G♯G , and write its prefix as N.π=⟨v0,…,vl⟩N.π= v_0,…,v_l , where v0=vsv_0=v_s and vl=N.v_l=N.v. Similar to the prefix interface in Eqn. (6), we define a projected base interface I~N I_N. If l=0l=0, then I~N=s I_N=\x_s\. Otherwise, let uNu_N be the latest vertex vkv_k in the prefix such that k<lk<l and β(vk)≠β(vl)β(v_k)≠β(v_l), and define I~N=Xβ(uN)∩Xβ(N.v). I_N=X_β(u_N)∩ X_β(N.v). In short, I~N I_N is the interface in G0G_0 where the base-projected prefix last enters the current base vertex. Thus, consecutive ECD-subdivision vertices with the same base vertex are collapsed before evaluating the reusable heuristics. The reused htrih_tri and htabh_tab are then evaluated on G0♯G_0 from I~N I_N using the precomputed values from G0G_0 and the same query goal, where (G0♯,vs,vg)=QueryAugment(G0,s,g)(G_0 ,v_s,v_g)= QueryAugment(G_0,x_s,p_g). Same as in Sec. 5.5, all heuristic values are defined to be zero when N.v=vgN.v=v_g. Lemma 10. For any non-goal search node N generated on G♯G , the value obtained by evaluating either htrih_tri or htabh_tab on G0♯G_0 from I~N I_N is an admissible heuristic for the remaining cost of the current low-level query on G♯G . Proof 10. Consider any N.πN.π-consistent solution trajectory τ¯ τ for the current low-level query on G♯G . By construction of I~N I_N, after collapsing consecutive ECD-subdivision vertices with the same base vertex, the suffix of any N.πN.π-consistent trajectory begins from a state in I~N I_N. Since every refined set in G is contained in the base set from which it was subdivided, replacing each refined vertex in this suffix by its base vertex and dropping the ECD reservations gives a feasible trajectory in the relaxed base query graph G0♯G_0 that starts from I~N I_N. Lemmas 5 and 6 show that the corresponding htrih_tri and htabh_tab on G0♯G_0 lower bound the remaining cost of this relaxed problem. Hence, the reused values also lower bound the remaining cost of any N.πN.π-consistent solution trajectory for the current low-level query on G♯G , and are admissible. 7.5 Windowed Coordination Full-horizon robot coordination can be computationally intensive, as low-level solver calls may reserve long trajectory occupancies, including portions that have no near-term spatiotemporal correlation with the current coordination step, causing ECD-updated ST-GCSs to grow rapidly. Windowed coordination reduces this computation by applying either P or PBS over a finite planning window, so high-level ECD reservations and PBS conflict detection use only trajectory portions inside that window. Let W=[t,t+Δtplan]W=[t,t+ t_plan] be the planning window, where Δtplan>0 t_plan>0 is the window span. Let Δtexec≤Δtplan t_exec≤ t_plan be the execution horizon. At coordination time t, each robot i starts from a window-start state s,iWx_s,i^W, equal to its original start at the first call and otherwise the endpoint of its last executed prefix. After each successful call, the coordinator commits the returned trajectory only over [t,t+Δtexec][t,\,t+ t_exec], appends this prefix to the global trajectory, discards the remaining suffix, and uses its endpoint as the next s,iWx_s,i^W. The selected high-level coordinator, either P or PBS, solves the current query (s,iW,g,i)(x_s,i^W,p_g,i) for every robot, including robots that have already reached their goals. For a goal-reached robot, this query starts at its current goal position and can still be replanned if priority constraints require it to move away temporarily and then return. For a trajectory τ, let τ|Wτ|_W denote the trajectory clipped to window W, with segment endpoints clipped to the window boundaries when necessary. For both windowed-P and windowed-PBS, ECD reservations use τ~|W τ|_W, so they reserve the same trajectory occupancy as in Eqn. (8) but only within W. Under the same window restriction, a robot pair i≠ji≠ j is in conflict within W if Ω(τ~j|W,r)∩Ω(τ~i|W,r)≠∅. ( τ_j|_W,r)∩ ( τ_i|_W,r)≠ . Since windowed ECD only subdivides the base ST-GCS to exclude reserved trajectory occupancy within W, any trajectory feasible on the windowed ECD-reserved ST-GCS remains feasible on the base ST-GCS G0G_0 when the reservations are ignored. Therefore, the admissible heuristics remain valid by Lemma 10. Windowed-P constructs the reserved graph sequence along a fixed priority order σ, starting from GW(1)=G0G_W^(1)=G_0. For each k, robot σk _k is planned on GW(k)G_W^(k); after this plan is found, the next reserved graph is updated as GW(k+1)=ECD(GW(k),τ~σk|W,2r)G_W^(k+1)=ECD(G_W^(k), τ_ _k|_W,2r). For windowed-PBS, replanning at a node uses the same window-restricted ECD reservations. When robot i is replanned at node N, let ⟨j1,…,jl⟩ j_1,…,j_l be any fixed topological ordering of the set j∣j≺N+i\j j _N^+i\ under ≺N+ _N^+. The query (s,iW,g,i)(x_s,i^W,p_g,i) is then solved on ECD(G0,⟨N.τ~j1|W,…,N.τ~jl|W⟩,2r).ECD(G_0, N. τ_j_1|_W,…,N. τ_j_l|_W ,2r). The child node expansion ordering is also adjusted to prefer progress by unreached robots. When a detected conflict involves one robot that has reached its goal and one that has not, PBS first considers the child node that prioritizes the unreached robot. Otherwise, it follows the node expansion rules as described in Sec. 7.3.1. This commit-and-replan cycle naturally matches a closed-loop execution with receding-horizon. After each execution horizon, the planner replans from the updated window-start states and incorporates updated reservations or dynamic obstacles. The special case Δtexec=Δtplan t_exec= t_plan commits the entire planned window before replanning; choosing Δtexec<Δtplan t_exec< t_plan retains a lookahead suffix that guides the current solve but is not committed. A smaller lookahead gap Δtplan−Δtexec t_plan- t_exec potentially leaves the robots in better states for subsequent calls, but increases total planning effort because replanning is invoked more often depending on the gap. On the other hand, as Δtplan t_plan approaches the remaining horizon, windowed-P or PBS recovers its full-horizon counterpart. For finite Δtplan t_plan, windowed coordination is incomplete in general because conflicts outside the current window are ignored until later. However, in practice, it greatly reduces the computation cost, since each low-level call reserves fewer trajectory segments and thus the ECD-updated ST-GCSs remain smaller. 7.5.1 Dynamic Window Adjustment A short fixed window can defer conflicts that should be exposed before the robots commit another prefix. To reduce this failure mode, we use a dynamic adjustment rule that enlarges the planning window only when the current horizon appears insufficient. For windowed-P, this retry is triggered when the fixed-priority solve fails in the current window. For windowed-PBS, it is triggered either by current-window failure or by repeatedly returning the same topological priority order among the same unreached robots without meaningful progress. In either case, the coordinator retries the same coordination step with doubled Δtplan t_plan and commits no new trajectory prefixes. The larger window exposes more future interactions and reserves longer portions of higher-priority occupancies. After a successful step, Δtplan t_plan is reset to its nominal value, keeping easy windows small while giving congested windows additional horizon. 8 Numerical Results This section presents our numerical results for spatiotemporal planning and MRMP on ST-GCS. We implement the proposed planners and algorithmic components in Python and evaluate on an Apple® M4 CPU machine with 16GB RAM. The GCS-related trajectory optimization uses Drake (Tedrake and the Drake Development Team, 2019) library and the Mosek solver (MOSEK ApS, 2026). The source code and numerical results are publicly available on https://github.com/reso1/stgcs. More detailed visualizations and simulation videos of the proposed planners can be found at https://sites.google.com/view/stgcs. Figure 9: Example instances of the generated spatial GCS. Colors indicate convex regions in the spatial decomposition. Black regions indicate environment static obstacles. 8.1 Experiment Setup We consider three two-dimensional base domains, rand, maze, and iris, to define the problem domains for benchmarking the spatiotemporal and MRMP planners. Each ST-GCS instance initializes a seeded spatial GCS as described in Sec. 8.1.1, and then extrudes the spatial sets through time t∈[0,tmax]t∈[0,t_max] with tmax=1000t_max=1000. We precompute the heuristics htrih_tri and htabh_tab on only these base ST-GCSs, although Sec. 8.1.2 additionally adds dynamic obstacles into each base ST-GCS instance. 8.1.1 Spatial GCS Generation For rand, we generate an n×n× n grid, sample a random convex polygon around each grid cell, connect intersecting convex sets, and retain the largest connected component. For maze, we first generate a random 10×1010× 10 maze via recursive division (Buck, 2011) and then merge adjacent free cells into larger axis-aligned rectangles whenever possible using a bipartite matching procedure similar to that of Lu et al. (2023). For iris, we sample 1010 polygonal static obstacles in a square workspace and grow a connected cover of collision-free convex sets with the IRIS algorithm (Deits and Tedrake, 2015). This presents a set of more realistic GCS commonly seen in practice. The robot radius is 0.10.1 in rand, and 0.250.25 in maze and iris. Fig. 9 shows examples of the generated spatial GCS. 8.1.2 Spatiotemporal Planning Instances We generate 6060 single-robot spatiotemporal planning instances, with 2020 instances from each of rand, maze, and iris. Each query is a feasible query on one seeded base ST-GCS and starts at time 0 with a velocity bound lim=[1.0,…,1.0]v_lim=[1.0,…,1.0]. We then add moving obstacles with piecewise-linear trajectories and the same object occupancy model as defined in Eqn. (8) while preserving query feasibility. The resulting ST-GCS instances have a median of 3333 vertices and 184184 edges. 8.1.3 MRMP Instances We generate 6060 MRMP instances, with 2020 instances from each of rand, maze, and iris. Each instance contains n-robot queries on a seeded base ST-GCS, where each robot query starts at time 0 with a velocity bound lim=[1.0,…,1.0]v_lim=[1.0,…,1.0]. The query generation for the start positions s,1,…,s,np_s,1,…,p_s,n and goal positions g,1,…,g,np_g,1,…,p_g,n follow the assumption as in condition (i), while keeping their independent trajectories still intersects to make the MRMP instances emphasize coordination rather than isolated planning. Figure 10: BFS heuristic ablation. Rows show runtime and expanded-node distributions on common-success instances. 8.2 Ablation Study This subsection follows our algorithmic stack for MRMP. We first ablate the low-level single-robot spatiotemporal planning BFS solver (Alg. 1), and then the child node expansion rules used by full-horizon PBS (Alg. 3), and at last the planning horizons in windowed coordination in Sec. 7.5. All ablation planners have a 1010-minute runtime budget. Unless otherwise stated, we set all their inflation factors to ϵ=1ε=1 for the heuristics used in the low-level BFS solvers. 8.2.1 BFS Heuristics Figure 11: Effect of heuristic inflation and offline heuristic precomputation cost. The top-left panel shows the median runtime reduction from increasing ϵε relative to ϵ=1ε=1 on common-success instances. The bottom-left panel shows the htabh_tab and htrih_tri precomputation median runtime versus the base ST-GCS G0G_0 size. The right panel shows optimality-gap distributions relative to the optimal ϵ=1ε=1 solutions. This ablation isolates the BFS heuristic (Sec. 5.5) with upper-bound pruning enabled and no optional pairwise dominance check (Sec. 5.4). Fig. 10 shows that hmaxh_max is the most robust choice. The hmaxh_max and htabh_tab heuristics solve all 6060 instances in Sec. 8.1.2, while hmoth_mot, htrih_tri, and hzeroh_zero solve 5858, 5656, and 5353, respectively. On common-success instances, hmaxh_max has the smallest median runtime and expanded-node count in every domain. Relative to hzeroh_zero, it reduces the median runtime by 5.35.3–56.2×56.2× and the median number of expanded nodes by 11.511.5–56.5×56.5× across the three domains. Among the single-component heuristics, hmoth_mot is competitive on rand and strongest on the most open iris, while htabh_tab solves all instances and is strongest on maze, where detours make the motion-only bound less informative; htrih_tri is weaker without inflation. Fig. 11 shows that increasing ϵε substantially reduces runtime, especially for htrih_tri and htabh_tab, but larger inflation factors produce wider optimality-gap tails. At ϵ=5ε=5, the median runtime reductions are about 65%65\% for hmoth_mot, 91%91\% for htrih_tri, 88%88\% for htabh_tab, and 56%56\% for hmaxh_max. The precomputation panel reports median preprocessing runtime. The htrih_tri heuristic stays in the seconds range, whereas htabh_tab requires substantially more precomputation and peaks over 2.52.5 hours. We therefore use hmaxh_max as the default BFS heuristic and inflate it when bounded suboptimality is allowed. It inherits the strongest available component lower bound at each prefix path, and its offline cost is amortized in MRMP because many low-level calls reuse the precomputation for the same base ST-GCSs. Figure 12: BFS dominance check ablation. Top and middle rows show runtime and expanded-node distributions for UB (upper-bound pruning with the incumbent cost cubc_ub), the safe δset _set check, and the heuristic δstate _state and δpos _pos checks. The bottom row reports the optimality gap relative to the optimal solution from the δset _set variant on common-success instances. 8.2.2 BFS Dominance Checks This ablation isolates the effect of the pairwise dominance check (Sec. 5.4) with the heuristic fixed to hmaxh_max. All variants use the same upper-bound pruning, and differ only in the optional pairwise dominance check. The alternatives are the safe set-containment check δset _set, the heuristic position-based check δpos _pos, and the heuristic arrival-state-containment check δstate _state. The auxiliary vertex-indexed search used to obtain the incumbent contributes only the scalar upper-bound cost cubc_ub, so differences in success, runtime, and solution cost come from the selected pairwise dominance check. Fig. 12 compares the upper-bound-only variant (UB) and the three pairwise dominance checks in runtime, expanded nodes, and solution-cost increase. UB, δset _set, and δstate _state solve all 6060 instances in Sec. 8.1.2, while δpos _pos solves 5959 instances. Relative to δset _set on common-success instances, δpos _pos and δstate _state reduce the median expanded-node count by 65.4%65.4\% and 42.7%42.7\%, and reduce the median runtime by 41.1%41.1\% and 42.1%42.1\%, respectively. Both heuristic checks have zero median solution-cost increase over all common-success instances. δstate _state has three positive solution-cost increases, with a 1.68%1.68\% median positive increase and a 4.8%4.8\% worst-case increase. In contrast, δpos _pos trades more aggressive pruning for fourteen positive solution-cost increases, with a 4.14%4.14\% median positive increase and a 15.1%15.1\% worst-case increase. Overall, these results suggest using δpos _pos as the default scalable pairwise dominance check when small heuristic losses are acceptable, and using δstate _state when low-level success and solution quality should stay closer to the δset _set reference. Figure 13: Full-horizon PBS child-node expansion rules ablation. The top row shows success rate over runtime, expanded PBS nodes, and generated PBS nodes in log scale. The bottom row shows the ratio of the generated/expanded nodes, and sum-of-costs and makespan distributions on common-success instances. 8.2.3 PBS Node Expansion Rules This ablation isolates the node expansion rule of full-horizon PBS in Alg. 3. As introduced in Sec. 7.3.1, we compare the four rules of lazy evaluation (Lazy), the sum-of-costs (SoC) metric, the makespan (MS) metric, and the number of conflicts (NC) metric. All variants share the same low-level BFS solver with hmaxh_max heuristic, upper-bound pruning, and δpos _pos dominance check. Recall that a PBS node is said to be generated once created by adding a new partial priority order, while it is expanded once the robots are replanned via UpdateNode. As shown in the first row of Fig. 13, the NC rule gives the strongest PBS scalability on 6060 1010-robot MRMP instances generated in Sec. 8.1.3. It solves 5858 instances within the 600600s budget, compared with 5555, 5252, and 4646 for the MS, SoC, and Lazy rules, respectively. It also exhibits the best anytime behavior, solving 4242 instances by 120120s and 5252 instances by 200200s, which is consistent with the two node-count curves. We now look at the second row containing 4545 instances solved by all four rules. The generated/expanded ratio subplot shows that the Lazy rule does postpone node expansions, with the largest ratio reaching 1.931.93 as PBS moves closest to a binary search tree. The two solution-quality subplots show that median solution-quality differences among expansion rules are modest. The NC and SoC rules have nearly tied median SoC (78.178.1 and 78.278.2), while Lazy and MS have higher median SoC (82.682.6 and 81.281.2). The MS rule gives the lowest median MS of 10.910.9, compared with at least 11.111.1 for the other rules. In general, the NC rule is suggested as the default node expansion rule because it gives the strongest scalability while remaining competitive in median SoC and MS. Figure 14: Windowed-PBS ablation comparing fixed windows (F), dynamic windows (D), and dynamic windows with half execution windows (H). Panels show success rate versus runtime, sum-of-costs (SoC), and makespan in log scale. 8.2.4 Windowed Coordination This ablation isolates the planning window tplant_plan and the execution window texect_exec for windowed-PBS as described in Sec. 7.5. We compare Δtplan=αr/‖lim‖∞ t_plan=α r/\|v_lim\|_∞ with a planning span factor α∈2.5,5,10α∈\2.5,5,10\, and Δtexec=βΔtplan t_exec=β t_plan with an execution span factor β∈0.5,1β∈\0.5,1\. In addition, we compare the variants with static windows and dynamically adjusted windows as introduced in Sec. 7.5.1. All variants use hmaxh_max heuristic with inflation factor ϵ=10ε=10, upper-bound pruning, δpos _pos dominance check, and the NC child-node expansion rule on 6060 2020-robot MRMP instances generated in Sec. 8.1.3, with a 600600s per-instance runtime budget. Fig. 14 shows that dynamic window adjustment improves the β=1β=1 success count for all spans, from 3636 to 5050 instances at α=2.5α=2.5, from 5050 to 5555 at α=5α=5, and from 4949 to 5050 at α=10α=10. Larger windows are not monotonically beneficial because they trade fewer coordination calls for harder local subproblems. Increasing α from 55 to 1010 raises the β=1β=1 median runtime from 40.140.1s to 65.165.1s and reduces success from 5555 to 5050 instances. For fixed α, dynamic β=0.5β=0.5 shifts the corresponding β=1β=1 curve toward larger runtimes. It recovers some instances missed by β=1β=1, but is not a strict improvement. For α=2.5,5,10α=2.5,5,10, it gains 55, 44, and 66 instances while losing 1212, 55, and 22 instances solved by the corresponding β=1β=1 variant. The solution-quality panels show the complementary tradeoff. Shorter windows can yield shorter successful trajectories, with fixed α=2.5α=2.5 giving the lowest median SoC/makespan (250.0/12.5250.0/12.5), but only 3636 successes. Among the more reliable variants, dynamic α=5,β=1α=5,β=1 achieves the highest success count (5555) with median SoC/makespan 325.0/16.2325.0/16.2. Using β=0.5β=0.5 improves median SoC/makespan for the same dynamic α values, but at the cost of longer runtimes. Overall, dynamic α=5,β=1α=5,β=1 gives the best scalability–quality tradeoff in this ablation. 8.3 Performance Comparison This subsection presents the performance comparison results on spatiotemporal planning and MRMP. 8.3.1 Spatiotemporal Planning Benchmark We evaluate the spatiotemporal planners under 6060s runtime budget on the 6060 instances from Sec. 8.1.2. The first two are BFS variants shortlisted from previous ablations. Both use hmaxh_max, upper-bound pruning, and ϵ=10ε=10, and differ only in whether they use δpos _pos or δset _set. The δpos _pos variant tests the more aggressive point-based dominance check, whereas the δset _set variant tests set-containment pruning with the same inflated max heuristic. The optimization-based baselines are MICP and MICP(g), which solve Eqns. (3). MICP directly solves the program to optimality, while MICP(g) solves its linear relaxation and then applies stochastic path rounding (see Sec. 4.2) with ⌈1000log|E|⌉ 1000 |E| rounded paths (Tang et al., 2025). We also compare against the sampling-based baseline ST-RRT∗ (Grothe et al., 2022) from OMPL (Şucan et al., 2012), reporting both the first feasible trajectory and the final incumbent solutions. Given the robot radius r, we also compare against the discrete search-based Zeta∗-SIPP (Zou and Borst, 2024) with cell sizes of r and 2r2r. 8.3.2 Spatiotemporal Planning Scalability Fig. 15 shows that the proposed BFS solvers on ST-GCS are the only planning variants that solve all 6060 instances while remaining consistently subsecond. The bounded-suboptimal δset _set variant has a 0.100.10s median runtime, while the δpos _pos variant has a 0.120.12s median runtime. Running the same δset _set solver with ϵ=1ε=1 increases the median runtime to 0.230.23s. The two optimization baselines on ST-GCS are substantially less scalable. MICP solves 3131 instances with a 6.036.03s median runtime on successful runs, whereas MICP(g) solves 5959 instances with a 1.301.30s median runtime. The non-ST-GCS baselines expose representation-dependent scalability and success behavior. For ST-RRT∗, the first feasible trajectory appears on 5757 instances with a 0.060.06s median time-to-first-solution, but the planner reaches its final incumbent only after the full anytime budget. The domain split still shows geometry dependence, since ST-RRT∗ solves all 2020 rand and iris instances but only 1717 maze instances. This dependence is visible in the success-rate curves. The iris instances reach 20/2020/20 successes by 0.0050.005s, rand reaches 16/2016/20 by 0.470.47s and 20/2020/20 only after the last successful first solution at 8.848.84s, and maze reaches 17/2017/20 by 3.823.82s and remains below full success through 6060s. ST-RRT∗ samples and connects directly in continuous collision-free space rather than using the GCS adjacency, so the maze domains make it harder to discover a temporally feasible connection within the budget. Zeta∗-SIPP with cell size r solves 4646 instances with a 0.340.34s median runtime, while the coarser 2r2r grid solves 4141 instances with a 0.130.13s median runtime. Its success depends on the spatial discretization. The finer grid improves success on rand instances (9/209/20 versus 4/204/20), while both grids solve 17/2017/20 maze instances and all 2020 iris instances. Zeta∗-SIPP searches safe intervals only on fixed spatial grids; when the grid cells and line-of-sight edges do not represent the relevant passage, a feasible trajectory may be absent from the discretized search space. Figure 15: Spatiotemporal planning performance comparison. The top row shows success rate over runtime in log scale. The middle row shows cost distributions on common-success instances, with planners whose success rates are below 25%25\% excluded. The bottom row shows cost increase relative to M1 on common-success instances. Figure 16: MRMP performance comparison. Runtime, sum-of-costs (SoC), and makespan are plotted in log scale and report medians over common-success instances among planners with at least 10%10\% success rate. 8.3.3 Spatiotemporal Planning Solution Quality The middle and bottom rows of Fig. 15 show that the search-based ST-GCS solvers preserve solution quality while improving scalability. The bounded-suboptimal δset _set variant has a 9.089.08 median cost, while the δpos _pos variant has a 9.189.18 median cost. Running the δset _set solver with ϵ=1ε=1 reduces the median cost to 8.538.53, compared with a 9.709.70 median cost for MICP(g). On the 3030 instances solved by MICP, the two inflated BFS variants, and MICP(g), the median cost increase relative to MICP is 0.0%0.0\% for all three non-MICP planners. Nonzero increases occur on 1212, 66, and 22 instances for δpos _pos, δset _set, and MICP(g), respectively, with median positive increases of 4.79%4.79\%, 5.95%5.95\%, and 3.23%3.23\%. For the anytime ST-RRT∗ baseline, the final incumbent substantially improves solution quality after the first feasible trajectory is found. Across its 5757 successful instances, the final incumbent reduces the median successful cost from 9.299.29 to 6.496.49. The single ST-RRT∗ legend entry uses first-feasibility time for success and final-incumbent cost for quality. The solution-cost comparisons should be interpreted relative to the solution space represented by each planner. For rand and maze, the ST-GCS-based planners produce costs close to the ST-GCS optimum, with small increases for the inflated BFS variants and MICP(g) on common-success instances. For iris, however, the ST-GCS-based planners generally have higher costs than the non-ST-GCS baselines. This is expected since IRIS-generated convex sets provide only partial coverage of the full collision-free space, so ST-GCS optimality is with respect to the ST-GCSs rather than the entire continuous free space. 8.3.4 MRMP Benchmark We evaluate various MRMP planners under 180180s runtime budget on n-robots MRMP instances, where each n=2,4,…,20n=2,4,…,20 corresponds to 6060 instances generated as in Sec. 8.1.3. The proposed Windowed-PBS + BFS applies windowed-PBS with NC node expansion rule and Δplan=Δexec=5r/‖lim‖∞ _plan= _exec=5r/||v_lim||_∞ as the high-level coordinator, and the BFS solver with hmaxh_max, ϵ=10ε=10, upper-bound pruning, and δpos _pos as the low-level solver, which empirically is most scalable under limited runtime. With the same low-level BFS solver, we also compare several composed baselines with different high-level coordinators, namely, windowed-P with Δplan=Δexec=5r/‖lim‖∞ _plan= _exec=5r/||v_lim||_∞, PBS with NC node expansion rule, and P. To test the alternative low-level planners in Sec. 8.3.1, we evaluate PBS + Zeta∗-SIPP with discretization of r, and P + ST-RRT∗. P + ST-RRT∗ is adopted as an anytime prioritized planning baseline. After the first priority-ordered pass produces a feasible joint solution, the remaining budget is used for improvement rounds that revisit the robots in the same order and replace an incumbent trajectory only when ST-RRT∗ finds a shorter collision-free trajectory against the current reservations. We report its first feasible solution for runtime and its final incumbent solution for solution quality. In addition, we compare with two standalone MRMP baselines. K-CBS (Kottinger et al., 2022) is a fast feasibility-oriented and sampling-based MRMP planner from OMPL (Şucan et al., 2012). CB-GCS (Zhao et al., 2025b) similarly solves MRMP on GCSs under a fixed time-step representation. Table 1: Windowed-PBS+BFS solving profile. Entries report median query density, median total runtime, and component shares. Opt. indicates convex optimization in PathOptimize of Alg. 1. Search includes only BFS and PBS without Opt. n Query Density Runtime Total Runtime Breakdown Search Opt. ECD Others 2 19.2% 0.41s 50.2% 37.2% 0.0% 12.6% 4 43.7% 1.29s 44.6% 36.7% 7.8% 11.0% 6 52.0% 2.67s 41.5% 37.7% 10.9% 9.9% 8 58.5% 4.99s 38.3% 38.1% 13.9% 9.7% 10 65.9% 8.04s 35.1% 39.3% 16.5% 9.2% 12 69.5% 12.6s 35.3% 39.2% 17.4% 8.1% 14 71.7% 17.7s 33.1% 37.7% 20.7% 8.6% 16 76.4% 25.9s 29.5% 39.6% 22.5% 8.4% 18 78.4% 48.9s 28.2% 42.1% 22.5% 7.2% 20 79.1% 40.2s 28.9% 40.6% 22.9% 7.6% 8.3.5 MRMP Scalability Fig. 16 shows that the proposed Windowed-PBS + BFS has the highest success rate. Over the n≥10n≥ 10 instances, it solves 338338 out of 360360 instances. All baselines solve fewer cases in the same regime. Among the other composed baselines, P + BFS solves 159159, P + ST-RRT∗ solves 151151, PBS + BFS solves 135135, Windowed-P + BFS solves 108108, and PBS + Zeta∗-SIPP solves 99. The two standalone baselines also degrade at high robot counts. K-CBS solves 242242 instances for n≥10n≥ 10, while CB-GCS solves only 66. At n=20n=20, Windowed-PBS + BFS solves 19/2019/20 rand, 16/2016/20 maze, and 16/2016/20 iris instances, giving 51/6051/60 successes overall. The next-best baseline at this scale is K-CBS with 20/6020/60 successes. The remaining baselines solve 13/6013/60 cases for P + ST-RRT∗, 7/607/60 for Windowed-P + BFS, 6/606/60 for P + BFS, 2/602/60 for PBS + BFS, and 0/600/60 for PBS + Zeta∗-SIPP and CB-GCS. These comparisons separate the two ingredients needed for scalability. Windowing alone is not sufficient. Windowed-P + BFS uses the same low-level solver and horizon schedule as Windowed-PBS + BFS, but solves only 108/360108/360 high-count cases and 7/607/60 cases at n=20n=20. PBS alone is also not sufficient. Full-horizon PBS + BFS solves 135/360135/360 high-count cases and only 2/602/60 cases at n=20n=20. The proposed combination avoids these two failure modes by resolving local priority conflicts inside each window while committing only the next execution interval. The comparison against P + ST-RRT∗ and K-CBS is clearest on the most challenging n≥16n≥ 16 instances. Windowed-PBS + BFS solves 55/6055/60, 51/6051/60, and 55/6055/60 cases on rand, maze, and iris, respectively. P + ST-RRT∗ solves 15/6015/60, 2/602/60, and 39/6039/60, while K-CBS solves 12/6012/60, 27/6027/60, and 49/6049/60. On successful runs in this regime, the median runtimes of Windowed-PBS + BFS are 37.137.1s, 20.820.8s, and 51.951.9s across the three domains. P + ST-RRT∗ has successful-run median runtimes of 15.115.1s, 88.588.5s, and 1.071.07s, while K-CBS has 105105s, 82.382.3s, and 37.137.1s. Thus, although P + ST-RRT∗ and K-CBS are very strong baselines on smaller instances with fewer robots, the proposed Windowed-PBS + BFS remains the only scalable choice for the most challenging MRMP instances across all three domains. Tab. 1 shows search and optimization dominate the runtime, while ECD becomes substantial at larger n and other costs remain small. The query density reports the area fraction of the entire spatial GCSs covered by the union of the start and goal hypercube occupancies as in Eqn. (7). Figure 17: Qualitative MRMP comparison on representative 1010-robot instances. Each subplot title reports the runtime, sum-of-costs, and makespan. Gray and black regions show the spatial GCS and static obstacles, respectively. For K-CBS and P-ST-RRT∗, the GCSs in rand are used for sampling since there are no explicit obstacles presented, while they are not used in maze and iris and thus not visualized. Figure 18: Large-scale 2D coordination with random start-goal pairs. (a1)–(a2) show 50 robots in a four-room workspace, and (b1)–(b2) show 100 robots in an empty workspace. In each pair, the first panel is our MRMP solution and the second is the trajectory-optimized result, solved in 1.64m/1.62m and 1.60m/3.45m, respectively. Hollow circles/squares mark starts/goals. 8.3.6 MRMP Solution Quality The SoC and makespan rows of Fig. 16 show that the proposed Windowed-PBS + BFS retains competitive solution quality while achieving the scalability reported above. We report the median SoC and makespan on the n≤10n≤ 10 instances where most planners succeed on substantial subsets. On the corresponding common-success subsets, the median SoC/makespan Windowed-PBS + BFS is 40.0/10.040.0/10.0. Full-horizon PBS + BFS and P + BFS have lower median SoC/makespan 26.7/8.7126.7/8.71 and 27.3/8.7927.3/8.79, respectively. Windowed-P + BFS gives a similar limited-horizon tradeoff, with median SoC/makespan 40.0/10.040.0/10.0. Thus, windowed coordination sacrifices some SoC since it plans and commits only within a finite horizon, while keeping the makespan close to the full-horizon BFS planners. In contrast, the feasibility-oriented K-CBS has substantially worse solution quality with median SoC/makespan 66.0/21.766.0/21.7. P + ST-RRT∗ is a stronger solution-quality baseline in favorable domains after using the full 180180s runtime budget. On iris, it achieves median SoC/makespan 33.6/8.3333.6/8.33, below the median 60.0/10.060.0/10.0 of Windowed-PBS + BFS. As in Sec. 8.3.3, this gap is expected since IRIS-generated convex sets only partially cover the full collision-free space. However, its quality advantage is domain-dependent. On maze, P + ST-RRT∗ has median SoC/makespan 42.6/15.042.6/15.0, while Windowed-PBS + BFS has median SoC/makespan 60.0/14.460.0/14.4. Fig. 17 illustrates these domain-dependent quality differences on representative 1010-robot examples with a 30s runtime budget. Overall, K-CBS emphasizes feasibility at the cost of longer joint plans. P + ST-RRT∗ can produce high-quality solutions in favorable domains but is less consistent. Windowed-PBS + BFS provides the most consistent quality-scalability tradeoff across domains. 9 Applications and Demonstrations This section complements the quantitative comparisons above with several demonstration scenarios. We first describe a trajectory-optimization postprocessing step for smoothing the piecewise-linear trajectories produced by our planner, then demonstrate the solutions on large-scale simulated problems and real-robot deployments. Figure 19: Top-down view snapshots of 32-UAV position-exchange demonstration in a 3D sphere. Top and bottom rows show our MRMP solution and trajectory-optimized result solved in 1.14m and in 0.86m, respectively. Hollow circles/squares mark starts/targets, filled circles mark window-end positions, and dotted circles and labels mark zoom bounds and factors. 9.1 Global Trajectory Optimization The proposed MRMP planner produces piecewise-linear trajectories with fixed vertex paths on ST-GCS. As a postprocessing step, we can smooth these trajectories by solving a Quadratic Program (QP) over sampled spatial positions along the trajectories at fixed time samples. Given n robot trajectories 0=τi0i=1nT^0=\ _i^0\_i=1^n, we choose a shared ordered sample sequence =⟨t1,…,tl⟩K= t_1,…,t_l , with t1<⋯<tlt_1<·s<t_l, formed from a uniform time grid together with the original trajectory knot times. We will use k to denote the index of the fixed time sample tkt_k. Let ¯i,k=τi0(tk). p_i,k= _i^0(t_k).p be the reference position of robot i at time tkt_k. The program keeps the times fixed and optimizes only the sampled spatial positions i,k∈ℝmp_i,k ^m. Similarly to Sec. 7.1, we assume a common radius r for the robots. For compactness, we define the sampled velocity and velocity-variation term as follows: i,k() _i,k(p) =i,k+1−i,ktk+1−tk, = p_i,k+1-p_i,kt_k+1-t_k, i,k() _i,k(p) =i,k+1−i,k−1tk+1−tk−1−i,k−i,k−2tk−tk−2. = p_i,k+1-p_i,k-1t_k+1-t_k-1- p_i,k-p_i,k-2t_k-t_k-2. Let ℱiF_i denote the samples at which robot i is fixed at its initial or terminal state. For each interval [tk,tk+1][t_k,t_k+1], let vi,kv_i,k be the ST-GCS vertex traversed by the input piecewise-linear trajectory at the interval midpoint. We formulate the trajectory optimization as: mini,k _\p_i,k\ ∑i=1n(λc∑k=3l−1‖i,k()‖22+λd∑k=1l‖i,k−¯i,k‖22) _i=1^n ( _c _k=3^l-1\|g_i,k(p)\|_2^2+ _d _k=1^l\|p_i,k- p_i,k\|_2^2 ) s.t. i,k=¯i,k, _i,k= p_i,k, (12a) −lim⪯i,k()⪯lim, -v_lim _i,k(p) _lim, (12b) (i,k,tk),(i,k+1,tk+1)∈Xvi,k, (p_i,k,t_k),\,(p_i,k+1,t_k+1)∈ X_v_i,k, (12c) ij,k⊤(i,k−j,k)≥2r, _ij,k (p_i,k-p_j,k)≥ 2r, (12d) where Eqns. (12a)–(12c) are enforced for every robot and all valid sample indices, with Eqn. (12a) restricted to k∈ℱik _i. Eqn. (12d) is enforced for every robot pair 1≤i<j≤n1≤ i<j≤ n and every sample k. The objectives regularize velocity variation and penalize deviations from the input reference trajectories. The two nonnegative weights λc _c and λd _d tune the relative emphasis of these two objective terms, respectively. Eqn. (12a) preserves the prescribed start and goal positions. Eqn. (12b) enforces the same component-wise velocity limits constraint as Eqn. (3f) in the original formulation. Eqn. (12c) constrains both endpoints of each optimized segment to lie in the same set Xvi,kX_v_i,k of the original trajectory segment. Since each optimized segment is a linear interpolation in space-time between the two endpoints, the convexity of Xvi,kX_v_i,k keeps the entire optimized segment inside this collision-free set. Eqn. (12d) is a sampled linearization of pairwise robot separation, where ij,k=(¯i,k−¯j,k)/‖¯i,k−¯j,k‖2n_ij,k=( p_i,k- p_j,k)/\| p_i,k- p_j,k\|_2 is the unit normal precomputed from the original sampled positions ¯i,k p_i,k and ¯j,k p_j,k at time tkt_k. When explicit obstacle representations are available, we can further replace Eqn. (12c) with direct sampled collision-avoidance constraints. At each sample time tkt_k, a static obstacle uses its fixed spatial geometry, while a space-time obstacle uses its spatial slice at tkt_k. For each robot i, obstacle o, and time sample tkt_k, let i,k,o∈ℝma_i,k,o ^m and bi,k,o∈ℝb_i,k,o jointly define a precomputed separating halfspace that contains the original trajectory sample ¯i,k p_i,k and excludes the obstacle geometry inflated by the robot radius r. We enforce the corresponding sampled collision-avoidance constraint i,k,o⊤i,k≥bi,k,oa_i,k,o p_i,k≥ b_i,k,o, which keeps the optimized sample on the same collision-free side of this halfspace. The resulting smoothed trajectories are certified to be space-time collision-free only at the sampled states (i,k,tk)(p_i,k,t_k) where the QP constraints are imposed. For larger instances, we improve scalability with a sequential rolling-horizon variant that optimizes a short block of consecutive time samples, fixes the accepted prefix, advances the block, and repeats until all samples have been processed. This decomposition keeps each local trajectory-optimization QP small and makes the full problem easier to solve, while retaining the same smoothing objective and sampled-state collision-free. 9.2 Large-Scale Coordination We demonstrate in large-scale MRMP instances that the proposed Windowed-PBS + BFS planner scales to confined and highly congested workspaces, with trajectory optimization applied afterward as a smoothing post-processing step. Fig. 18 shows two planar coordination settings with complementary sources of difficulty. Using the same query-density metric as Tab. 1, the 5050-robot and 100100-robot instances have query densities of 95.7%95.7\% and 77.1%77.1\%, respectively. In the 5050-robot four-room instance, the workspace funnels traffic through narrow inter-room passages, so difficulty comes from geometric bottlenecks and the need to sequence many robots through shared doorways. In the 100100-robot empty-workspace instance, the independent trajectories of 9797 robots are involved in collisions initially in one connected conflict component, so difficulty comes from dense pairwise interaction rather than static obstacles. These results show that the planner remains practical both when difficulty comes from geometric bottlenecks and when it comes from dense pairwise robot interactions. Fig. 19 further stresses coordination with a 3232-UAV position-exchange problem on a 3D sphere, where every robot conflicts with any other robots at the sphere center. This fully coupled exchange forces the coordinator to resolve simultaneous conflicts in 3D before trajectory optimization smooths the resulting paths. Fig. 20 further tests a 4848-UAV instance in a fastpathplanning-style 3D village with dense buildings and trees (Marcucci et al., 2024a). The generated village occupies an 8×8×38× 8× 3 workspace, includes building footprints together with randomly placed tree and bush obstacles, and decomposes the remaining free space into 119119 convex boxes. The start–goal pairs are sampled as long-distance trips across this fragmented free space, so many UAVs must simultaneously route around vertical clutter while avoiding one another in narrow local passages. Figure 20: 48-UAV MRMP in a cluttered village with trees and buildings. The trajectory-optimized result is shown and solved in 2.72m after our planner found a solution in 3.75m. 9.3 Mobile Robots Deployment We validate our MRMP planner in an open indoor environment with a system of 99 differential drive mobile robots. The system is centrally controlled by an Intel Core Ultra 5 125U laptop with 16GB RAM, ROS2 Humble (Macenski et al., 2022), and Vicon (Vicon Motion Systems Ltd., 2025) for robot localization. Each robot has a bounding radius of 7cm and is exposed to a velocity command (v,ω)(v,ω), with linear velocity v∈[0,0.6]v∈[0,0.6] m/s and angular velocity ω∈[−2.5,2.5]ω∈[-2.5,2.5] rad/s. Note that this velocity limit is set conservatively smaller than the physical robot limits for system robustness. We construct a rearrange task consisting of a sequence of MRMP queries within a 3m×3m bounding square. The goal positions in each query consecutively form a configuration of five letters: S, T, G, C, and S. All the robots start simultaneously from their previous goal configurations; specifically, they initialize and terminate in a 3×33× 3 grid configuration. We first compute piecewise-linear solution trajectories via Windowed-PBS+BFS, and then smooth them via the trajectory optimization described in Sec. 9.1. The safe clearance between the robots is set to 2020cm during planning, which is 66cm larger than twice the robot radius. The offline planning for the entire task took 16.28s. Figure 21: Real-robot demonstration of rearrange task for the letters S, T, G, C, S. Opaque robots mark captured poses, and translucent ghosts show preceding motion traces. During online execution, the smoothed trajectories are discretized with a 0.050.05s time step into time-indexed waypoints, and the tracker precomputes segment-wise reference linear and angular velocities from consecutive samples. All robots receive a common start time and evaluate their own trajectory at a system-wide shared elapsed time, preserving the simultaneity encoded by the offline solution. We use a simple trajectory tracking controller for the robots with a 60-Hz frequency. In each control cycle, the tracker first identifies the active trajectory segment whose time interval contains the system time. The final published velocity command combines the active segment’s reference velocities as feedforward terms with body-frame feedback corrections computed from the interpolated desired pose error. In our deployments, the conservative velocity limits and additional planned inter-robot clearance provided enough margin for the observed tracking errors, and the robots completed the rearrangement without inter-robot collisions. Fig. 21 demonstrates the actual robot trajectories during the rearrange task. 10 Conclusions We presented ST-GCS as a continuous-space framework for time-optimal spatiotemporal motion planning with dynamic obstacles and multi-robot interactions. By searching over path-indexed states on space-time convex sets, the proposed solver avoids a single large mixed-integer formulation while still optimizing continuous trajectories along candidate paths. The ECD scheme further allows planned occupancies to be reserved directly in the graph, enabling prioritized and windowed coordination for MRMP. Experiments show that the resulting planners substantially improve scalability while maintaining competitive solution quality. Large-scale demonstrations further show practical coordination of up to 100100 robots within a few minutes. Several scope limitations suggest natural directions for future work. First, the current ST-GCS formulation uses piecewise-linear trajectory representations with time monotonicity and component-wise velocity bounds, motivating richer trajectory representations for nonholonomic systems, kinodynamic constraints, and manipulation tasks. Second, this work targets offline MRMP with known environments, motivating online MRMP with frequent replanning under changing environments or lifelong task assignments, together with adaptive or approximate convex decompositions that keep such replanning more efficient. Third, finite-window coordination is incomplete in general because conflicts outside the current window are deferred, motivating stronger window-selection rules with completeness guarantees. Funding This work was supported by the NSERC under grant number RGPIN2020-06540 and a CFI JELF award. References A. Andreychuk, K. Yakovlev, P. Surynek, D. Atzmon, and R. Stern (2022) Multi-agent pathfinding with continuous time. Artificial Intelligence 305, p. 103662. Cited by: §2.2. D. Bertsimas and J. N. Tsitsiklis (1997) Introduction to linear optimization. Vol. 6, Athena scientific Belmont, MA. Cited by: §3.2, §4.1. A. Bhat, G. Gutow, B. Vundurthy, Z. Ren, S. Rathinam, and H. Choset (2025) A complete and bounded-suboptimal algorithm for a moving target traveling salesman problem with obstacles in 3d. In 2025 IEEE International Conference on Robotics and Automation (ICRA), p. 6132–6138. Cited by: §2.3. J. Buck (2011) Maze generation: recursive division. Note: https://weblog.jamisbuck.org/2011/1/12/maze-generation-recursive-division-algorithmBlog post, accessed 2026-03-09 Cited by: §8.1.1. S. Y. C. Chia, R. H. Jiang, B. P. Graesdal, L. P. Kaelbling, and R. Tedrake (2024) GCS*: forward heuristic search on implicit graphs of convex sets. arXiv preprint arXiv:2407.08848. Cited by: §2.3, §5.5.1. C. L. Clark and B. Xie (2025) Plan optimal collision-free trajectories with non-convex cost functions using graphs of convex sets. IEEE Transactions on Robotics. Cited by: §2.3. T. Cohn, M. Petersen, M. Simchowitz, and R. Tedrake (2023) Non-euclidean motion planning with graphs of geodesically convex sets. The International Journal of Robotics Research, p. 02783649241302419. Cited by: §2.3. H. Dai, A. Amice, P. Werner, A. Zhang, and R. Tedrake (2024) Certified polyhedral decompositions of collision-free configuration space. The International Journal of Robotics Research 43 (9), p. 1322–1341. Cited by: §2.3. R. Deits and R. Tedrake (2015) Computing large convex regions of obstacle-free space through semidefinite programming. In Algorithmic Foundations of Robotics XI: Selected Contributions of the Eleventh International Workshop on the Algorithmic Foundations of Robotics, p. 109–124. Cited by: §2.3, §8.1.1. S. Diamond, R. Takapoui, and S. Boyd (2018) A general system for heuristic minimization of convex functions over non-convex sets. Optimization Methods and Software 33 (1), p. 165–193. Cited by: §3.3. E. W. Dijkstra (2022) A note on two problems in connexion with graphs. In Edsger Wybe Dijkstra: his life, work, and legacy, p. 287–290. Cited by: §5.5.2. M. Erdmann and T. Lozano-Perez (1987) On multiple moving objects. Algorithmica 2, p. 477–521. Cited by: §2.1, §2.2, §7.2. S. Garg, T. Cohn, and R. Tedrake (2025) Planning shorter paths in graphs of convex sets by undistorting parametrized configuration spaces. IEEE Robotics and Automation Letters. Cited by: §2.3. B. P. Graesdal, S. Y. C. Chia, T. Marcucci, S. Morozov, A. Amice, P. A. Parrilo, and R. Tedrake (2024) Towards tight convex relaxations for contact-rich manipulation. arXiv preprint arXiv:2402.10312. Cited by: §2.3. F. Grothe, V. N. Hartmann, A. Orthey, and M. Toussaint (2022) St-rrt*: asymptotically-optimal bidirectional motion planning through space-time. In 2022 International Conference on Robotics and Automation (ICRA), p. 3314–3320. Cited by: §1, §2.1, §8.3.1. M. Hüppi, L. Bartolomei, R. Mascaro, and M. Chli (2022) T-prm: temporal probabilistic roadmap for path planning in dynamic environments. In 2022 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), p. 10320–10327. Cited by: §1, §2.1. K. Kasaura, M. Nishimura, and R. Yonetani (2022) Prioritized safe interval path planning for multi-agent pathfinding with continuous time on 2d roadmaps. IEEE Robotics and Automation Letters 7 (4), p. 10494–10501. Cited by: §2.2. L. E. Kavraki, P. Svestka, J. Latombe, and M. H. Overmars (1996) Probabilistic roadmaps for path planning in high-dimensional configuration spaces. IEEE transactions on Robotics and Automation 12 (4), p. 566–580. Cited by: §1. N. Kerimov, A. Onegin, and K. Yakovlev (2025) Safe interval randomized path planning for manipulators. In Proceedings of the International Conference on Automated Planning and Scheduling, Vol. 35, p. 213–217. Cited by: §2.1. J. Kottinger, S. Almagor, and M. Lahijanian (2022) Conflict-based search for multi-robot motion planning with kinodynamic constraints. In 2022 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), p. 13494–13499. Cited by: §2.2, §8.3.4. V. Kurtz and H. Lin (2023) Temporal logic motion planning with convex optimization via graphs of convex sets. IEEE Transactions on Robotics. Cited by: §2.3. S. LaValle (1998) Rapidly-exploring random trees: a new tool for path planning. Research Report 9811. Cited by: §1. J. Lu, B. Zeng, J. Tang, T. L. Lam, and J. Wen (2023) TMSTC*: a path planning algorithm for minimizing turns in multi-robot coverage. IEEE Robotics and Automation Letters 8 (8), p. 5275–5282. Cited by: §8.1.1. H. Ma, D. Harabor, P. J. Stuckey, J. Li, and S. Koenig (2019) Searching with consistent prioritization for multi-agent path finding. In Proceedings of the AAAI conference on artificial intelligence, Vol. 33, p. 7643–7650. Cited by: §2.2, §7.3. S. Macenski, T. Foote, B. Gerkey, C. Lalancette, and W. Woodall (2022) Robot operating system 2: design, architecture, and uses in the wild. Science Robotics 7 (66), p. eabm6074. External Links: Document, Link Cited by: §9.3. K. Mao, I. Spasojevic, M. Hopkins, M. A. Hsieh, and V. Kumar (2024) Collision-free time-optimal path parameterization for multi-robot teams. arXiv preprint arXiv:2409.17079. Cited by: §2.2. T. Marcucci, M. Halm, W. Yang, D. Lee, and A. D. Marchese (2025) A biconvex method for minimum-time motion planning through sequences of convex sets. arXiv preprint arXiv:2504.18978. Cited by: §2.3. T. Marcucci, P. Nobel, R. Tedrake, and S. Boyd (2024a) Fast path planning through large collections of safe boxes. IEEE Transactions on Robotics. Cited by: §2.3, §9.2. T. Marcucci, M. Petersen, D. von Wrangel, and R. Tedrake (2023) Motion planning around obstacles with convex optimization. Science robotics 8 (84), p. eadf7843. Cited by: §1, §2.3, §3.2. T. Marcucci, J. Umenberger, P. Parrilo, and R. Tedrake (2024b) Shortest paths in graphs of convex sets. SIAM Journal on Optimization 34 (1), p. 507–532. Cited by: §1, §2.3, §3.1, §3.3. T. Marcucci (2024) Graphs of convex sets with applications to optimal control and motion planning. Ph.D. Thesis, Massachusetts Institute of Technology. Cited by: §3.3. C. McBeth, J. D. Motes, I. Ngui, M. Morales, and N. M. Amato (2026) Scalable multi-robot motion planning using workspace guidance-informed hypergraphs. IEEE Robotics and Automation Letters. Cited by: §2.2. M. McNaughton, C. Urmson, J. M. Dolan, and J. Lee (2011) Motion planning for autonomous driving with a conformal spatiotemporal lattice. In 2011 IEEE International Conference on Robotics and Automation, p. 4889–4895. Cited by: §2.1. C. E. Miller, A. W. Tucker, and R. A. Zemlin (1960) Integer programming formulation of traveling salesman problems. Journal of the ACM (JACM) 7 (4), p. 326–329. Cited by: §3.2. S. Morozov, T. Marcucci, A. Amice, B. P. Graesdal, R. Bosworth, P. A. Parrilo, and R. Tedrake (2024) Multi-query shortest-path problem in graphs of convex sets. arXiv preprint arXiv:2409.19543. Cited by: §2.3. S. Morozov, T. Marcucci, B. P. Graesdal, A. Amice, P. A. Parrilo, and R. Tedrake (2025) Mixed discrete and continuous planning using shortest walks in graphs of convex sets. arXiv preprint arXiv:2507.10878. Cited by: §2.3. MOSEK ApS (2026) MOSEK Modeling Software. Note: Version 11.1 External Links: Link Cited by: §8. R. Natarajan, C. Liu, H. Choset, and M. Likhachev (2024) Implicit graph search for planning on graphs of convex sets. Robotics: Science and Systems (RSS). Cited by: §2.3, §3.3, §5.3.1, §5.5.2. M. D. Osburn, C. K. Peterson, and J. L. Salmon (2025) Systematic constraint formulation and collision-free trajectory planning using space-time graphs of convex sets. arXiv preprint arXiv:2508.10203. Cited by: §2.3. A. G. Philip, Z. Ren, S. Rathinam, and H. Choset (2024) A mixed-integer conic program for the moving-target traveling salesman problem based on a graph of convex sets. In 2024 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), p. 8847–8853. Cited by: §2.3. M. Phillips and M. Likhachev (2011) Sipp: safe interval path planning for dynamic environments. In 2011 IEEE international conference on robotics and automation, p. 5628–5635. Cited by: §2.1. M. Qin, I. Solis, J. D. Motes, M. Morales, and N. M. Amato (2025) K-arc: adaptive robot coordination for multi-robot kinodynamic planning. IEEE Robotics and Automation Letters. Cited by: §2.2. Z. Ren, A. G. Philip, S. Zhao, S. Rathinam, and H. Choset (2025) CP-milp: mixed integer linear programming for multi-agent motion planning with linear dynamics. IEEE Robotics and Automation Letters. Cited by: §2.2. S. J. Russell and P. Norvig (2020) Artificial intelligence: a modern approach (4th edition). Pearson. External Links: Link, ISBN 9781292401133 Cited by: §5.1. G. Sharon, R. Stern, A. Felner, and N. R. Sturtevant (2015) Conflict-based search for optimal multi-agent pathfinding. Artificial intelligence 219, p. 40–66. Cited by: §2.2. R. Shome, K. Solovey, A. Dobson, D. Halperin, and K. E. Bekris (2020) Drrt*: scalable and informed asymptotically-optimal multi-robot motion planning. Autonomous Robots 44 (3), p. 443–467. Cited by: §2.2. D. Silver (2005) Cooperative pathfinding. In Proceedings of the aaai conference on artificial intelligence and interactive digital entertainment, Vol. 1, p. 117–122. Cited by: §2.1, §2.2. J. Sim, J. Kim, and C. Nam (2024) Safe interval rrt* for scalable multi-robot path planning in continuous space. arXiv preprint arXiv:2404.01752. Cited by: §2.1. A. Sintov and A. Shapiro (2014) Time-based rrt algorithm for rendezvous planning of two dynamic systems. In 2014 IEEE International Conference on Robotics and Automation (ICRA), p. 6745–6750. Cited by: §2.1. I. Solis, J. Motes, M. Qin, M. Morales, and N. M. Amato (2024) Adaptive robot coordination: a subproblem-based approach for hybrid multi-robot motion planning. IEEE Robotics and Automation Letters 9 (8), p. 7238–7245. Cited by: §2.2. I. Solis, J. Motes, R. Sandström, and N. M. Amato (2021) Representation-optimal multi-robot motion planning using conflict-based search. IEEE Robotics and Automation Letters 6 (3), p. 4608–4615. Cited by: §2.2. K. Solovey, O. Salzman, and D. Halperin (2016) Finding a needle in an exponential haystack: discrete rrt for exploration of implicit roadmaps in multi-robot motion planning. The International Journal of Robotics Research 35 (5), p. 501–513. Cited by: §2.2. I. A. Şucan, M. Moll, and L. E. Kavraki (2012) The Open Motion Planning Library. IEEE Robotics & Automation Magazine 19 (4), p. 72–82. Note: https://ompl.kavrakilab.org External Links: Document Cited by: §8.3.1, §8.3.4. K. Sundar and S. Rathinam (2024) A* for graphs of convex sets. arXiv preprint arXiv:2407.17413. Cited by: §2.3. P. Švestka and M. H. Overmars (1998) Coordinated path planning for multiple robots. Robotics and autonomous systems 23 (3), p. 125–152. Cited by: §2.2. A. Tajbakhsh, L. T. Biegler, and A. M. Johnson (2024) Conflict-based model predictive control for scalable multi-robot motion planning. In 2024 IEEE International Conference on Robotics and Automation (ICRA), p. 14562–14568. Cited by: §2.2. J. Tang and H. Ma (2026) GHOST: solving the traveling salesman problem on graphs of convex sets. In Proceedings of the AAAI Conference on Artificial Intelligence, Vol. 40, p. 36421–36428. Cited by: §2.3, §3.3, §5.5.2. J. Tang, Z. Mao, L. Yang, and H. Ma (2025) Space-time graphs of convex sets for multi-robot motion planning. In 2025 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Vol. , p. 8683–8690. External Links: Document Cited by: §1, §4.2, §8.3.1. R. Tedrake and the Drake Development Team (2019) Drake: model-based design and verification for robotics. External Links: Link Cited by: §8. J. Tordesillas and J. P. How (2021) MADER: trajectory planner in multiagent and dynamic environments. IEEE Transactions on Robotics 38 (1), p. 463–476. Cited by: §2.2. J. P. Van Den Berg and M. H. Overmars (2005) Prioritized motion planning for multiple robots. In 2005 IEEE/RSJ International Conference on Intelligent Robots and Systems, p. 430–435. Cited by: §2.2. Vicon Motion Systems Ltd. (2025) Vicon Tracker. Vicon Motion Systems Ltd.. Note: Engineering motion capture and object-tracking softwareUsed for rigid-body localization / 6-DoF pose tracking; accessed 2026-06-28 External Links: Link Cited by: §9.3. D. von Wrangel and R. Tedrake (2024) Using graphs of convex sets to guide nonconvex trajectory optimization. In 2024 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), p. 9863–9870. Cited by: §2.3. G. Wagner and H. Choset (2015) Subdimensional expansion for multirobot path planning. Artificial intelligence 219, p. 1–24. Cited by: §2.2. P. Werner, A. Amice, T. Marcucci, D. Rus, and R. Tedrake (2024) Approximating robot configuration spaces with few convex sets using clique covers of visibility graphs. In 2024 IEEE International Conference on Robotics and Automation (ICRA), p. 10359–10365. Cited by: §2.3. P. Werner, R. Cheng, T. Stewart, R. Tedrake, and D. Rus (2025) Superfast configuration-space convex set computation on gpus for online motion planning. arXiv preprint arXiv:2504.10783. Cited by: §2.3. S. You, G. Luna, J. Shaikh, D. Gostin, Y. Xiang, J. Koeln, and T. Summers (2025) Motion planning with precedence specifications via augmented graphs of convex sets. arXiv preprint arXiv:2510.22015. Cited by: §2.3. S. Zhao, Y. Liu, H. Choset, and Z. Ren (2025a) Mixed integer conic programming for multi-agent motion planning in continuous space. In 2025 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), p. 10540–10547. Cited by: §2.2. S. Zhao, A. G. Philip, S. Rathinam, H. Choset, and Z. Ren (2025b) CB-gcs: conflict-based search on the graph of convex sets for multi-agent motion planning. In 2025 IEEE 21st International Conference on Automation Science and Engineering (CASE), p. 2208–2214. Cited by: §2.3, §8.3.4. Y. Zou and C. Borst (2024) Zeta*-sipp: improved time-optimal any-angle safe-interval path planning.. In IJCAI, p. 6823–6830. Cited by: §2.1, §8.3.1.