--- title: Discontinuity-Bounded LaCAM in Multi-Robot Planning url: https://www.emergentmind.com/topics/discontinuity-bounded-lacam-db-lacam type: topic --- # Discontinuity-Bounded LaCAM in Multi-Robot Planning 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 [2512.06796]. ## 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: ```plaintext 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}$. - 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. ## 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 $4\times 6\times 1.5\,\mathrm{m}^3$ 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 $N\leq10$, 5 min for $N>10$). ### 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 ($\sim$20s runtime); CBS/ECBS fail for $N\geq20$ 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 $7\times 4\times 2.5\,\mathrm{m}^3$ Vicon room, achieving tracking error $\leq$5 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 $\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 [2512.06796]. Source: https://www.emergentmind.com/topics/discontinuity-bounded-lacam-db-lacam