db-LaCAM: Fast and Scalable Multi-Robot Kinodynamic Motion Planning with Discontinuity-Bounded Search and Lightweight MAPF

TL;DR

<db-LaCAM> merges scalable MAPF coordination with kinodynamic feasibility by using discontinuity-bounded motion primitives and a hierarchical heuristic framework. The approach extends LaCAM with db-PIBT for horizon-based planning, and employs motion-primitive clustering (GOC and SC-GOC) plus livelock detection to maintain efficiency and robustness. It achieves resolution-complete planning with respect to the selected motion primitives, scales to up to 50 robots with significant speedups over state-of-the-art kinodynamic planners, and demonstrates safe execution on real flying and car-with-trailer robots. The work provides extensive ablations and scalability results, showing strong performance in both 2D and 3D environments and offering practical contributions for real-world multi-robot coordination.

Abstract

State-of-the-art multi-robot kinodynamic motion planners struggle to handle more than a few robots due to high computational burden, which limits their scalability and results in slow planning time. In this work, we combine the scalability and speed of modern multi-agent path finding (MAPF) algorithms with the dynamic-awareness of kinodynamic planners to address these limitations. To this end, we propose discontinuity-Bounded LaCAM (db-LaCAM), a planner that utilizes a precomputed set of motion primitives that respect robot dynamics to generate horizon-length motion sequences, while allowing a user-defined discontinuity between successive motions. The planner db-LaCAM is resolution-complete with respect to motion primitives and supports arbitrary robot dynamics. Extensive experiments demonstrate that db-LaCAM scales efficiently to scenarios with up to 50 robots, achieving up to ten times faster runtime compared to state-of-the-art planners, while maintaining comparable solution quality. The approach is validated in both 2D and 3D environments with dynamics such as the unicycle and 3D double integrator. We demonstrate the safe execution of trajectories planned with db-LaCAM in two distinct physical experiments involving teams of flying robots and car-with-trailer robots.

Figures (8)

• Figure 1: Performance and demonstration of db-LaCAM. Example problem setups (top and third rows) and corresponding quantitative comparisons (second row) over 35 instances grouped into ten representative environments (alternating shaded regions) over 10 trials. Each point represents a single trial outcome, while panels show failure rate, runtime, and normalized cost, respectively. Labels denote the evaluated environments: (a) alcove, (b) at goal, (c) circle-n10, (d) random-n8, (e) random-n8-u$_s$, (f) maze-n10, (g) passage-n6, (h) door-n4, (i) forest-n10, (j) swap-hetero-n8, (k) random-hetero-n8. Example random-hetero-n8 denotes a random problem with a heterogeneous team of eight robots. Instances (g–i) correspond to 3D environments. The bottom row illustrates real-world demonstrations with a team of ten flying robots and four car-like robots with trailers.
• Figure 2: Visual representation of motion primitive samples. Given state $\mathbf{x}$, applicable motions (black edges) start within a discontinuity lower than $\alpha \delta$ (gray circumference). Motions in collision with the environment are discarded (dashed edges). The action sequences of applicable motion primitives are used to forward-propagate the state $\mathbf{x}$.
• Figure 3: Top row: motion primitive clusters — middle: original motions, left: GOC, right: SC-GOC, with colors for each cluster. Motions toward left and right have similar distance to the goal state, yielding close $h$-values. Bottom row: example livelock between two robots.
• Figure 4: Runtime required to find a feasible solution over different numbers of robots.
• Figure 5: Computation time analysis for db-LaCAM. Upper row: two different methods for the heuristics $h$ computation. Bottom row: analysis of time spent on some key components of db-LaCAM. Experiments are conducted using circle 2D example with varying numbers of robots as labeled.

Theorems & Definitions (6)

• Example 1
• Definition 1
• Theorem 1
• Proposition 1
• Remark 1
• Remark 2