Discontinuity-Bounded LaCAM in Multi-Robot Planning

• The paper presents db-LaCAM, a novel kinodynamic planner that leverages precomputed motion primitives and bounded discontinuity to generate feasible trajectories.
• It integrates lightweight multi-agent path finding with priority inheritance and backtracking to ensure collision-free, scalable planning in complex environments.
• Empirical evaluations show db-LaCAM achieves 100% success with a 2-second runtime, outperforming traditional planners on tasks involving up to 50 robots.

Discontinuity-Bounded LaCAM (db-LaCAM) is a multi-robot kinodynamic motion planning algorithm that synergizes precomputed motion primitives, user-defined state discontinuity bounds, and lightweight multi-agent path finding coordination. db-LaCAM addresses the limitation of scalability in conventional kinodynamic multi-robot planners by leveraging fast MAPF abstraction and bounded, resolution-complete primitive sequencing, enabling kinodynamic planning for heterogeneous teams in complex, cluttered environments with up to 50 robots (Moldagalieva et al., 7 Dec 2025).

1. Motion Primitive Library and Discontinuity Handling

db-LaCAM constructs a finite library of motion primitives $P = \{p_1,\,\dotsc,\,p_N\}$ for each robot, where every primitive $p_i$ is defined by a tuple $p_i(t) = (x_i(t), u_i(t)),\,t\in[0,T_i]$ or equivalently $p_i = \langle X_i, U_i, K_i \rangle$ with $X_i = \langle x_0,\,\dotsc,\,x_{K_i}\rangle$, $U_i = \langle u_0,\,\dotsc,\,u_{K_i-1}\rangle$, satisfying discrete-time dynamics $x_{k+1} = x_k + f(x_k,u_k)\Delta t$ for arbitrary robot models $\dot x = f(x,u)$. Primitives are generated offline by two-point boundary-value optimization to cover diverse nonlinear dynamics, including unicycle, 3D double integrator, and car-with-trailer.

To accommodate the practical restriction that a primitive’s endpoint may not exactly match the start of the next primitive, the notion of discontinuity-boundedness is introduced. For successively chained primitives, db-LaCAM enforces a user-specified maximum state gap $\Delta_{\max}$, i.e., $\|x_i(T^-)-x_j(0^+)\| \leq \Delta_{\max}$, so long as physical safety/controllability is maintained. Selection of applicable primitives is implemented via k-d tree retrieval in state space over primitive starts, performing $p_i$0NN queries within radius $p_i$1.

2. Integrated Search Framework

db-LaCAM operates via a high-level search akin to A*, expanding “configuration” nodes $p_i$2state $p_i$3, constraint-tree $p_i$4 and recursively refining candidate motions. The procedure iterates as follows:

• If the state is within a goal region ($p_i$5), the solution is backtracked.
• Otherwise, candidate motion sets $p_i$6 for each robot are generated by process_Motions: pruning by discontinuity, forward rollout for end-state estimation, applying dynamic heuristics (HEST), and clustering for diversity.
• Constraint generation $p_i$7 via Set_Constraint_Tree follows “LaCAM style,” lazily expanding constraints in robot-priority order.
• The db-PIBT (Priority Inheritance with Backtracking) subroutine checks primitives for collision-freedom over the horizon using FCL-based swept-volume collision checking.
• Valid expansions insert new configurations into OPEN, recursively searching until a feasible multi-agent trajectory sequence is identified.

The algorithm is detailed in the following overall sketch:

$X_i = \langle x_0,\,\dotsc,\,x_{K_i}\rangle$4

Each call to process_Motions uses $p_i$8NN primitive queries, forward rollout using the system’s dynamics, HEST heuristic evaluation, and motion set clustering.

3. Lightweight MAPF Integration and Collision Avoidance

db-LaCAM abstracts each “horizon” as a MAPF timestep, where robot $p_i$9 transitions from $p_i(t) = (x_i(t), u_i(t)),\,t\in[0,T_i]$0 to $p_i(t) = (x_i(t), u_i(t)),\,t\in[0,T_i]$1 by selecting an appropriate primitive. The set of graph nodes thus corresponds to continuous configurations $p_i(t) = (x_i(t), u_i(t)),\,t\in[0,T_i]$2.

Constraint generation follows LaCAM’s lazy tree expansion: at depth $p_i(t) = (x_i(t), u_i(t)),\,t\in[0,T_i]$3, the $p_i(t) = (x_i(t), u_i(t)),\,t\in[0,T_i]$4-th robot in priority gets its candidate motions, yielding a minimal constraint-tree that db-PIBT traverses. db-PIBT recursively applies priority inheritance and backtracking to detect and resolve pairwise and collective inter-robot plan conflicts, executing exhaustive collision checking using FCL library on the swept-volumes of each primitive set over the horizon.

The optimization objective is minimization of total time, $p_i(t) = (x_i(t), u_i(t)),\,t\in[0,T_i]$5, where $p_i(t) = (x_i(t), u_i(t)),\,t\in[0,T_i]$6 is the length of robot $p_i(t) = (x_i(t), u_i(t)),\,t\in[0,T_i]$7's primitive-horizon. Inter-robot collision avoidance is ensured at each planning phase via exhaustive pairwise collision checks within db-PIBT.

4. Theoretical Properties

Resolution completeness is formally guaranteed with respect to the primitive set and bounded discontinuity. Theorem 1 (probabilistic resolution-completeness) establishes that, provided each cluster samples motions non-zero probability, exhaustive search over all possible sequences of $p_i(t) = (x_i(t), u_i(t)),\,t\in[0,T_i]$8-connected primitives will, with probability one, discover any valid solution that exists in the finite search space.

A high-level outline of relevant complexity results:

• A db-PIBT call for $p_i(t) = (x_i(t), u_i(t)),\,t\in[0,T_i]$9 robots and up to $p_i = \langle X_i, U_i, K_i \rangle$0 primitives per robot incurs $p_i = \langle X_i, U_i, K_i \rangle$1 time (pairwise collision checks).
• The overall search branching factor is $p_i = \langle X_i, U_i, K_i \rangle$2, with depth $p_i = \langle X_i, U_i, K_i \rangle$3\text{total_time} / \text{horizon_length}$.</li> <li>Overall complexity is$p_i = \langle X_i, U_i, K_i \rangle$4 in time and $p_i = \langle X_i, U_i, K_i \rangle$5 in space.

5. Empirical Evaluation

db-LaCAM was benchmarked on diverse robot models and environment types:

Dynamics and Environments

• 2D unicycle, 3D double-integrator, car-with-trailer
• 2D canonical/alcove, circle-swap, random box/spherical, maze
• 3D passage, forest, door within $p_i = \langle X_i, U_i, K_i \rangle$6 rooms
• Heterogeneous teams, up to 50 robots (scalability test)

Implementation employed C++ with FCL collision checking, running on an AMD Threadripper platform (64 GB RAM, 60s timelimit for $p_i = \langle X_i, U_i, K_i \rangle$7, 5 min for $p_i = \langle X_i, U_i, K_i \rangle$8).

Quantitative Results

Method	Success Rate	Mean Runtime [s]	Normalized Cost
db-CBS	30%	45s (fail)	—
db-ECBS	90%	12s	1.05×
db-LaCAM	100%	2s	1.00×

In dense/corridor settings, db-CBS frequently times out; db-LaCAM is approximately 10× faster than db-ECBS with equal or superior trajectory cost.

• Scalability: db-LaCAM solved all instances up to 50 unicycles ($p_i = \langle X_i, U_i, K_i \rangle$920s runtime); CBS/ECBS fail for $X_i = \langle x_0,\,\dotsc,\,x_{K_i}\rangle$0 within time limit.
• Ablations demonstrate that HEST heuristics are 3–5× faster than reverse db-A*, and the SC-GOC clustering method resolves livelocks 2× faster than GOC.

6. Physical Robot Demonstrations

Physical execution validated planned trajectories in two experimental settings:

1. Flying Robots: Ten custom Sanity drones with 3D double-integrator dynamics navigated a dense forest in a $X_i = \langle x_0,\,\dotsc,\,x_{K_i}\rangle$1 Vicon room, achieving tracking error $X_i = \langle x_0,\,\dotsc,\,x_{K_i}\rangle$25 cm and zero inter-robot collisions.
2. Car+Trailer Robots: Four Pololu 3pi+ robots with two-link trailers completed pairwise swaps in clutter with heading error $X_i = \langle x_0,\,\dotsc,\,x_{K_i}\rangle$30.05 rad and maintained safe following distances.

7. Context and Implications

db-LaCAM achieves significant advances in multi-robot kinodynamic planning, combining a bounded-discontinuity, primitive-based search with a lightweight MAPF coordination protocol (db-PIBT and LaCAM constraints). The method provides resolution-completeness, supports arbitrary system dynamics, offers scalability to 50+ robots in seconds, and has demonstrated feasibility both in simulation and on physical robot teams. A plausible implication is that discontinuity-bounded primitive chaining, allied with lazy constraint MAPF-style planning, may define a new paradigm for scalable, dynamic-aware multi-agent planning with practical guarantees and tractable computational complexity (Moldagalieva et al., 7 Dec 2025).