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 $k$NN queries within radius $\Delta_{\max}$.

2. Integrated Search Framework

db-LaCAM operates via a high-level search akin to A*, expanding “configuration” nodes $Q = ($state $x$, constraint-tree $C)$ and recursively refining candidate motions. The procedure iterates as follows:

• If the state is within a goal region ($\delta_g$), the solution is backtracked.
• Otherwise, candidate motion sets $M'$ 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 $(C')$ 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:

procedure db-LaCAM(start x_s, goal x_g, library P) build k-d tree T_m over all primitive start-states OPEN ← { Q_init = (x_s, empty constraint, no-motions) } while OPEN ≠ ∅ do Q ← pop_best(OPEN) if dist(Q.x, x_g) ≤ δ_g then return backtrack(Q) M′ ← Process_Motions(Q.x, T_m, Δ_max) C_set ← Set_Constraint_Tree(Q, M′) for each constraint C′ in C_set do T′ ← db-PIBT(N, M′, C′) // priority inheritance if T′ valid then x′ ← final_states(T′) push(OPEN, (x′, empty-constraint, T′)) end for end while return FAILURE

Each call to process_Motions uses $k$NN 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 $i$ transitions from $x_i$ to $x_i'$ by selecting an appropriate primitive. The set of graph nodes thus corresponds to continuous configurations $x \in X^N$.

Constraint generation follows LaCAM’s lazy tree expansion: at depth $d$, the $d$-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, $\sum_i K^{(i)}$, where $K^{(i)}$ is the length of robot $i$'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 $\Delta_{\max}$-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 $R$ robots and up to $M$ primitives per robot incurs $O(R^2 M^2)$ time (pairwise collision checks).
• The overall search branching factor is $b \approx \prod_{i=1}^R |M_i| \leq M^R$, with depth $D =$\text{total_time} / \text{horizon_length}$.</li> <li>Overall complexity is$O(b^D \cdot (R^2 M^2)) = O((M^R)^D R^2 M^2)$in time and$O(b^D)$$in space.</li> </ul> <h2 class='paper-heading' id='empirical-evaluation'>5. Empirical Evaluation</h2> <p>db-LaCAM was benchmarked on diverse robot models and environment types:</p> <h3 class='paper-heading' id='dynamics-and-environments'>Dynamics and Environments</h3> <ul> <li><a href="https://www.emergentmind.com/topics/simple-recurrent-flowm-2d" title="" rel="nofollow" data-turbo="false" class="assistant-link" x-data x-tooltip.raw="">2D</a> unicycle, 3D double-integrator, car-with-trailer</li> <li>2D canonical/alcove, circle-swap, random box/spherical, maze</li> <li>3D passage, forest, door within$$4\times 6\times 1.5\,\mathrm{m}^3$$rooms</li> <li>Heterogeneous teams, up to 50 robots (scalability test)</li> </ul> <p>Implementation employed C++ with FCL collision checking, running on an <a href="https://www.emergentmind.com/topics/automated-mechanism-design-amd" title="" rel="nofollow" data-turbo="false" class="assistant-link" x-data x-tooltip.raw="">AMD</a> Threadripper platform (64 GB RAM, 60s timelimit for$$N\leq10$, 5 min for$N>10$$).</p> <h3 class='paper-heading' id='quantitative-results'>Quantitative Results</h3><div class='overflow-x-auto max-w-full my-4'><table class='table border-collapse w-full' style='table-layout: fixed'><thead><tr> <th>Method</th> <th>Success Rate</th> <th>Mean Runtime [s]</th> <th>Normalized Cost</th> </tr> </thead><tbody><tr> <td>db-CBS</td> <td>30<td>45s (fail)</td> <td>—</td> </tr> <tr> <td>db-ECBS</td> <td>90<td>12s</td> <td>1.05×</td> </tr> <tr> <td>db-LaCAM</td> <td>100<td>2s</td> <td>1.00×</td> </tr> </tbody></table></div> <p>In dense/corridor settings, db-CBS frequently times out; db-LaCAM is approximately 10× faster than db-ECBS with equal or superior trajectory cost.</p> <ul> <li>Scalability: db-LaCAM solved all instances up to 50 unicycles ($$\sim$20s runtime); CBS/ECBS fail for $N\geq20$$within time limit.</li> <li>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.</li> </ul> <h2 class='paper-heading' id='physical-robot-demonstrations'>6. Physical Robot Demonstrations</h2> <p>Physical execution validated planned trajectories in two experimental settings:</p> <ol> <li>Flying Robots: Ten custom Sanity drones with 3D double-integrator dynamics navigated a dense forest in a$$7\times 4\times 2.5\,\mathrm{m}^3$Vicon room, achieving tracking error$\leq$5 cm and zero inter-robot collisions.
• Car+Trailer Robots: Four Pololu 3pi+ robots with two-link trailers completed pairwise swaps in clutter with heading error $\leq$0.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).