1 Introduction↩︎

Consider a warehouse environment with numerous robots, where each robot attempts to reach a specified region while being subject to kinematic and dynamic constraints, such as acceleration limits and turning radius, and must avoid collisions with obstacles and other robots. The challenge lies in efficiently coordinating movements to avoid collisions while navigating long, continuous trajectories under the combinatorial growth of the joint trajectory space in different environments.

Given the complexity of the full Multi-Robot Motion Planning (MRMP) problem, researchers have often resorted to solving simpler problems that approximate MRMP. One reformulation of the MRMP problem is known as Multi-Agent Path-Finding (MAPF) [1], where robots move in a discretized space (often a regular grid) and discrete time [2], [3]. In particular, the Conflict-Based Search (CBS) [4] family of algorithms [5]–[7] has been influential due to its strong scalability and safety guarantees. However, this MAPF-based abstraction relies on discretization assumptions, such as constant velocities, rectilinear and grid-based motions, which limit their applicability to continuous MRMP settings with dynamic feasibility and robot interactions.

More recent work has explored motion planning through probabilistic inference, sampling trajectory posteriors with diffusion planners [8]. These planners leverage the strong generative capacity of diffusion models to learn a score function from expert demonstrations and represent complex, multimodal trajectory distributions in continuous spaces [9], [10]. By stochastically denoising trajectory priors, diffusion planners can produce smoother trajectories and avoid the restrictive assumptions imposed by grid-based simplifications. Building on these single-robot advances, recent work extends diffusion planners to MRMP [11], [12] and integrates them as low-level continuous-space planners within CBS for multi-robot settings [13].

However, existing diffusion planners remain fundamentally model-free, requiring large expert datasets to learn the score function and struggling to rigorously enforce dynamics during denoising the expert trajectories. As a result, they often rely on extensive learning from demonstrations and neglect existing model information, yielding trajectories that are not genuinely dynamics-aware. Additionally, current diffusion planners handle inter-robot collisions through soft constraints, where the hard safety constraints cannot be guaranteed, or through post-hoc safety projections [14]–[17], which massively increase computational planning cost on the full trajectory, making planners fail in large-scale multi-robot congested settings.

We propose Model-Based Diffusion Optimal Control (MDOC), a training-free diffusion planner that enforces collision avoidance and scales to MRMP without any demonstration data. Building on Model-Based Diffusion (MBD) [18], MDOC analytically estimates diffusion scores via Monte Carlo score ascent under a known dynamics model, interpreting denoising as stochastic optimal control so avoiding demonstration learning. Crucially, MDOC extends MBD to enforce rigorous collision avoidance through Control Barrier Function (CBF)-constrained projections directly inside the model-based diffusion rollouts. Capitalizing on these properties, we further propose MDOC-CBS, a scalable MRMP algorithm that resolves inter-robot collisions through the same safety mechanism while retaining the efficiency of high-level CBS. The contributions of this work are threefold:

• We develop MDOC, a model-based diffusion planner that enforces safety during sampling via CBF-constrained projections inside model-based rollouts, yielding dynamically feasible and collision-free trajectories.

• We propose MDOC-CBS, a CBS-based coordination that scales to 20 robots (40 in larger maps) while resolving inter-robot collisions via the same CBF-constrained projection by MDOC as the low-level planner.

• We evaluate our methods across diverse environments, start-goal configurations, constraint settings, and horizons and show that it improves success rate and trajectory quality while reducing computation and maintaining collision-free execution compared to representative baselines.

2 Related Work↩︎

We review prior work on single-robot motion planning and multi-robot motion planning.

2.1 Single-Robot Motion Planning↩︎

Arguably, the most popular paradigm for single-robot motion planning is sampling. Sampling-based motion planning has been extensively studied through methods such as probabilistic roadmaps (PRM) [19]–[21] and rapidly exploring random trees (RRT) [22]–[24]. Although they provide probabilistic completeness [25], [26] and finite sampling guarantees [27], those methods struggle as problem geometry becomes more constrained and dynamics become more involved.

Trajectory optimization, mostly from control theory, formulates nonlinear robot motion planning problems directly over trajectories with constraints and dynamics. Sequential Linear Quadratic methods [28], [29] have been successfully used for legged robots [30], [31], while other works directly embed kinematic and dynamic limits for trajectory optimization [32], [33]. These methods can generate dynamically feasible trajectories, but solving large nonlinear programs becomes computationally demanding in cluttered environments.

Diffusion-based methods mitigate expensive optimization by learning a trajectory distribution and sampling candidates via Denoising Diffusion Probabilistic Models (DDPMs) [9], [34], [35]. Following these developments, Motion Planning Diffusion (MPD) [8] proposes learning and sampling diffusion models as prior and posterior trajectories for robot motion planning. Despite their efficiency, MPD still requires learning from large collections of expert trajectories. More recent methods combine diffusion models with control on Model Predictive Control [36], Model-Based Diffusion (MBD) [18] for trajectory optimization, and diffusion-based constraints [14]. However, safety mechanisms in those methods are either underexplored or enforced via inefficient projections.

2.2 Multi-Robot Motion Planning↩︎

Early works address the PSPACE-hardness of MRMP [37] by extending single-robot sampling-based planners into the composite configuration space, where each sample lies in the high-dimensional joint configuration space of all robots. Although those methods provide asymptotic optimality [38], they struggle in multi-robot settings because their search space grows exponentially with the number of robots.

To improve scalability, recent works utilized the Conflict-Based Search (CBS) framework [4] and decoupled the multi-robot planning problem into low-level single-robot planning and high-level conflict resolution. For example, KCBS [39] and db-CBS [2] design sampling-based low-level planners with kinodynamic constraints, aiming for dynamic feasibility but still suffering from sampling inefficiency of kinodynamic, sampling-based tree expansion.

Multi-Robot Motion Planning with Diffusion Models (MMD) [13] combines MPD with CBS to provide collision-free paths in scalable multi-robot environments. Model-based approaches aim for trajectory optimization within three-dimensional holonomic cases [40] and annealing legs [41] without learning, but these methods still optimize all robots with a joint cost function, which limits scalability and makes hard safety constraints difficult to enforce.

Summary. We are the first to propose CBF-constrained projection with model-based diffusion as a single-robot planner, yielding dynamically feasible trajectories and ensuring safety during diffusion sampling, and to scale it to multi-robot settings through CBS.

3 Preliminaries↩︎

Let us turn to formally defining the problem at hand and cover the relevant background. In what follows, we use a lower right index \(i\) to denote the robot identity, an upper right index \(h\) for the time step within the planning horizon, and an upper left index \(k\) for the diffusion step (e.g., \({}^{k}\tau_i^h\)).

3.1 Problem Statement↩︎

In Multi-Robot Motion Planning (MRMP), we aim to convey a team of \(N\) robots from their start states to goal states while respecting each robot’s dynamics and avoiding collisions with obstacles and each other. Let \(s_i\) encode the state of robot \(i\), such as its configuration and velocity, and \(u_i\) encode its control input. A trajectory can be represented as a sequence of state and control input pairs over a finite time horizon \(H\) as \(\tau_i = \big[(s_i^1, u_i^1), (s_i^2, u_i^2) \dots, (s_i^H, u_i^H)\big].\) We denote by \(\boldsymbol{s}^h := \{s^h_i\}_{i=1}^N\) and \(\boldsymbol{u}^h := \{u^h_i\}_{i=1}^N\) the joint state and control of all robots at time \(h\). Let \(\boldsymbol{\tau}^{1:H} := \{\tau^{1:H}_i\}_{i=1}^N\) denote a multi-robot joint trajectory over time horizon 1 to \(H\). We define the problem objective as \[\mathcal{J}(\boldsymbol{\tau}^{1:H}) := \sum_{i=1}^N \Big(\gamma_i \sum_{h=1}^H c(\tau_i^{h})\Big) \;+\; \sum_{i=1}^N \beta_i\, c_f(s_i^{H}), \label{eq:objective}\tag{1}\] where \(\mathcal{J}\) indicates the objective function, \(c\) and \(c_f\) denote the intermediate and terminal cost functions (e.g., distance traveled between consecutive states and terminal distance to the goal), and \(\gamma_i\) and \(\beta_i\) are their associated positive weighting coefficients. Then, the MRMP problem is \[\begin{align} \boldsymbol{\tau}^* &\in \arg\min_{\boldsymbol{\tau}^{1:H}}\;\mathcal{J}(\boldsymbol{\tau}^{1:H}) \\ \text{s.t.}\quad & s_i^{h+1} = f(s_i^h,u_i^h),\quad \forall i,\;h=1,\dots,H-1,\\ & g^{h}(\boldsymbol{s}^{h}, \boldsymbol{u}^{h}) \le 0,\quad h=1,\dots,H, \end{align}\] with \(s_i^{h+1} = f(s_i^h,u_i^h)\) representing the discrete-time dynamics, and \(g^{h}(\boldsymbol{s}^{h}, \boldsymbol{u}^{h}) \le 0\) generically collecting all obstacle-avoidance and inter-robot collision-avoidance constraints at time \(h\) to ensure safety.

3.2 Model-Based Diffusion for Trajectory Sampling↩︎

The trajectory for robot \(i\), \(\tau^{1:H}_i\), can be treated as a random variable and sampled by iterative denoising from Gaussian noise. Since this subsection focuses on single-robot motion planning, we omit the robot index \(i\) for brevity from now on.

We follow the standard DDPMs and denote by \({}^{k}\tau^{1:H}\) the noisy trajectory at diffusion step \(k\): \[{}^{k+1}\tau^{1:H} \;=\; \sqrt{\bar\alpha_k}\,{}^{1}\tau^{1:H} \;+\; \sqrt{1-\bar\alpha_k}\,\epsilon, \quad \epsilon\sim\mathcal{N}(0,\mathbf{I}), \label{eq:ddpm95reparam}\tag{2}\] where \(\alpha_k := 1-\beta_k\) controls the amount of signal preserved at diffusion step \(k\) under the noise-schedule coefficient \(\beta_k\), \(\bar\alpha_k:=\prod_{n=1}^{k}\alpha_n\), and \(K\) is the total number of diffusion steps. The noise schedule is chosen such that the final noisy trajectory \({}^{K}\tau^{1:H}\) is close to standard Gaussian noise and can be initialized from \(\mathcal{N}(0,\mathbf{I})\). Let \({}^1p({}^1\tau^{1:H})\) denote the target distribution over clean trajectories. We define the noisy marginals using the forward noising kernel \(q\): \[{}^kp({}^{k}\tau^{1:H}) := \int q({}^{k}\tau^{1:H}\mid {}^{1}\tau^{1:H})\,{}^1p({}^{1}\tau^{1:H})\,d{}^{1}\tau^{1:H}. \label{eq:pk95def}\tag{3}\]

Based on this noisy marginal, a standard result in score-based diffusion is that the optimal reverse-time drift depends on the score function, \(\mathbf{S}_k({}^{k}\tau^{1:H}) \;:=\; \nabla_{{}^{k}\tau^{1:H}} \log {}^kp({}^{k}\tau^{1:H}), \label{eq:score95def}\) where \(\nabla_{{}^{k}\tau^{1:H}}\) denotes the gradient with respect to \({}^{k}\tau^{1:H}\). The score guides denoising toward higher-density regions of the noisy marginal \({}^kp\) at diffusion step \(k\) while removing noise. Consequently, the reverse update can be written in the score-guided form \[{}^{k-1}\tau^{1:H} = \frac{1}{\sqrt{\alpha_k}} \left( {}^{k}\tau^{1:H} + (1-\bar\alpha_k)\,\hat{\mathbf{S}}_k \right), \label{eq:reverse95update95prelim}\tag{4}\] where \(\hat{\mathbf{S}}_k\approx \mathbf{S}_k\) is a score estimate at step \(k\). In standard DDPMs, \(\hat{\mathbf{S}}_k\) is obtained from a learned noise predictor \(\epsilon_\theta({}^{k}\tau^{1:H},k)\), yielding the approximation \[\mathbf{S}_k({}^{k}\tau^{1:H}) \approx -\frac{1}{\sqrt{1-\bar\alpha_k}}\;\epsilon_\theta({}^{k}\tau^{1:H},k). \label{eq:score95eps}\tag{5}\]

Learning \(\epsilon_\theta\) (or a score network) typically requires a large amount of expert trajectories. When a dynamic model is available, MBD [18] avoids learning \(\epsilon_\theta\) by sampling control sequences \({}^{k}\bar{u}^{1:H-1}\) and using the known dynamics to roll them out into state trajectories \({}^{k}\tau^{1:H}\), then analytically constructing \(\hat{\mathbf{S}}_k\) with denoised rollouts. Concretely, at reverse step \(k\), we first sample denoised candidates from the Gaussian proposal induced by the forward kernel

\[\tilde{\tau}^{1:H} \sim \Omega_k := \mathcal{N}\!\left( \frac{{}^{k}\tau^{1:H}}{\sqrt{\bar{\alpha}_k}}, \left(\frac{1}{\bar{\alpha}_k} - 1\right)\mathbf{I} \right). \label{eq:proposal95prelim}\tag{6}\] Let \(\{\tilde{\tau}_m^{1:H}\}_{m=1}^{M}\) be \(M\) samples from \(\Omega_k\). We then evaluate each candidate under a target density \({}^{1}p(\cdot)\) and form the importance-weighted average \[{}^{k}\bar{\tau}^{1:H} = \frac{\sum_{m=1}^{M} {}^{1}p(\tilde{\tau}_m^{1:H})\,\tilde{\tau}_m^{1:H}}{\sum_{m=1}^{M} {}^{1}p(\tilde{\tau}_m^{1:H})}, \label{eq:mc95avg95prelim}\tag{7}\] which yields the Monte Carlo Score-Ascent (MCSA) estimator \[\hat{\mathbf{S}}_k \approx -\frac{{}^{k}\tau^{1:H}}{1-\bar{\alpha}_k} +\frac{\sqrt{\bar{\alpha}_k}}{1-\bar{\alpha}_k}\, {}^{k}\bar{\tau}^{1:H}. \label{eq:mc95score95prelim}\tag{8}\] Together with Eqs 4 –8 , this yields a training-free diffusion sampler that iteratively (1) proposes candidates, (2) reweights them by \({}^1p({}^{k}\tau^{1:H})\), and (3) updates \({}^{k}\tau^{1:H}\) toward high-density trajectories. The remaining question is how to instantiate \({}^{1}p({}^{k}\tau^{1:H})\) so that high-probability samples correspond to low-cost, dynamically feasible, and collision-free trajectories. This can be answered through a planning-as-inference target distribution.

3.3 Planning as Inference↩︎

In the following, we omit both robot index \(i\) and diffusion step \(k\) for brevity. That is, we write the trajectory for robot \(i\) at diffusion step \(k\), \({}^{k}\tau^{1:H}_i\), as \(\tau^{1:H}\).

A standard relaxation of deterministic optimal control is to model soft optimality through a Boltzmann distribution, extensively studied in [42]–[46]: \[p_J(\tau^{1:H}) \propto \exp\!\left(-\frac{\mathcal{J}(\tau^{1:H})}{\lambda}\right), \label{eq:boltzmann95cost95prelim}\tag{9}\] where \(\lambda>0\) is a temperature parameter. As \(\lambda\rightarrow 0\), \(p_J\) concentrates around minimizers of \(\mathcal{J}\).

Dynamic feasibility and safety can be encoded by a feasibility prior \(p_{\mathrm{feas}}(\tau^{1:H})\), so that \({}^{1}p(\tau^{1:H}) \;\propto\; p_{\mathrm{feas}}(\tau^{1:H})\, p_J(\tau^{1:H}), \label{eq:target95dist}\) whose support is restricted to dynamically feasible and collision-free trajectories. Conceptually, a hard-feasibility prior can be written as an indicator product \[\begin{align} p_{\mathrm{feas}}(\tau^{1:H}) &\propto p_d(\tau^{1:H})\,p_g(\tau^{1:H}), \\ p_d(\tau^{1:H}) &:= \prod_{h=1}^{H-1}\mathbf{1}\!\left(s^{h+1}=f(s^{h},u^{h})\right),\\ p_g(\tau^{1:H}) &:= \prod_{h=1}^{H}\mathbf{1}\!\left(g^{h}(s^{h},u^{h})\le 0\right). \end{align}\] However, directly using hard indicators is brittle in constrained environments: most candidates receive zero weight (“dead samples”), providing no useful score guidance for MCSA [47]. This motivates handling feasibility explicitly rather than through hard rejection. In practice, it is desirable to work with candidates that lie in the feasible set \(\mathcal{D}\cap\mathcal{C}\), where \(\mathcal{D}\) denotes dynamic feasibility by rollouts and \(\mathcal{C} := \{\tau^{1:H}\mid g^h(s^h,u^h)\le 0,\;\forall h=1,\ldots,H\}\) denotes the safety set induced by the inequality constraints. When trajectories are feasible, the MCSA importance weights in Eq. 7 can be computed using only the optimality term, \(w_m \propto p_J(\tilde{\tau}_m^{1:H}),\) without assigning zero weights to infeasible samples.

4 Methodology↩︎

We present our Model-Based Diffusion Optimal Control (MDOC) planner and its multi-robot extension, MDOC-CBS. Given a known dynamics model, diffusion rollouts maintain dynamic feasibility, yielding trajectories in \(\mathcal{D}\). Our contribution is to enforce safety during sampling, then scales to MRMP, by introducing a feasibility operator \(\mathcal{F}\) that maps each candidate into \(\mathcal{D}\cap\mathcal{C}\). Specifically, \(\mathcal{F}\) applies CBF-constrained projections on controls within the model-based diffusion rollouts.

4.1 Model-Based Diffusion Optimal Control (MDOC)↩︎

MDOC is a single-robot motion planner, and thus we omit the robot index \(i\) for brevity in the following. MDOC follows the diffusion paradigm. At each diffusion step \(k\), it invokes a sampler that proposes a nominal control sequence \({}^{k}\bar{u}^{1:H-1}\) (with \({}^{k}\bar{u}^{h}\) at time \(h\)), which may violate safety constraints. To maintain safety, MDOC projects \({}^{k}\bar{u}^{1:H-1}\) to a safe sequence \({}^{k}u^{1:H-1}\) that satisfies \(\mathcal{C}\) via a feasibility operator \(\mathcal{F}\), which enforces a discrete-time CBF condition during the model-based rollout.

Concretely, we require a barrier function \(b({}^ks^{h})\) (e.g., \(b({}^ks^{h})=\|{}^ks^{h}-s_{\mathrm{obs}}\|^2-r^2\), so \(b({}^ks^{h})\ge 0\) enforces a minimum separation \(r\) from the obstacle) that satisfies the discrete-time CBF condition \(b({}^ks^{h+1}) \ge (1-\gamma \Delta h)\,b({}^ks^{h})\), where \(\Delta h\) is the time step size, meaning that the safety margin \(b(\cdot)\) should not decay faster than rate \(\gamma\). We locally linearize the dynamics and the barrier function around \(({}^{k}s^{h}, {}^{k}\bar{u}^{h})\): \[{}^ks^{h+1} \approx {}^{k}\hat{s}^{h+1} + {}^{k}B^{h}({}^{k}u^{h} - {}^{k}\bar{u}^{h}),\; {}^{k}\hat{s}^{h+1} := f({}^{k}s^{h}, {}^{k}\bar{u}^{h}),\] where \({}^{k}B^{h} := \left.\frac{\partial f(s,u)}{\partial u}\right|_{({}^{k}s^{h}, {}^{k}\bar{u}^{h})}\) is the Jacobian that locally maps control deviations to changes in the next state. Applying the same first-order approximation to \(b(\cdot)\) yields the control–affine inequality [48]–[50] \[\begin{align} & \nabla b({}^{k}\hat{s}^{h+1})^{\!\top} {}^{k}B^{h} {}^ku^{h} \;\ge\; \\ & (1 - \gamma \Delta h)\, b({}^{k}s^{h}) - b({}^{k}\hat{s}^{h+1}) + \nabla b({}^{k}\hat{s}^{h+1})^{\!\top} {}^{k}B^{h} {}^{k}\bar{u}^{h}, \label{eq:disc-lin} \end{align}\tag{10}\] which is equivalent to \({}^{k}a^{h\top}{}^ku^{h} \ge {}^{k}d^{h}\), where \[\begin{align} & {}^{k}a^{h\top}:=\nabla b({}^{k}\hat{s}^{h+1})^\top {}^{k}B^h, \\ & {}^{k}d^h := (1-\gamma\Delta h)b({}^{k}s^h)-b({}^{k}\hat{s}^{h+1})+{}^{k}a^{h\top}{}^{k}\bar u^h. \label{eq:cbf95rhs95def} \end{align}\tag{11}\] These inequalities define a safe-control set in the control space \({}^{k}\mathcal{K}^h := \{u \mid {}^{k}A^h u \ge {}^{k}D^h\},\) where \({}^{k}D^h\) is the vector collecting all scalar offsets \({}^{k}d^h\).

Stacking the half-spaces from all obstacles yields the polyhedral safe-control set \({}^{k}\mathcal{K}^{h} = \left\{u\;\middle|\;{}^{k}A^{h}u \ge {}^{k}D^{h}\right\}, \label{eq:cbf95polytope}\) where each row \({}^{k}a_{j}^{h\top}\) of \({}^{k}A^{h}\) and entry \({}^{k}d_{j}^{h}\) of \({}^{k}D^{h}\) correspond to one linearized CBF constraint. For speed inside diffusion sampling, we enforce each violated half-space via a closed-form projection. Given a single constraint \({}^ka^{\!h\top}{}^{k}u^{h}\ge {}^{k}d^h\), the projection for nominal control \({}^{k}\bar u^{h}\) is \[{}^{k}u^{h} \leftarrow \begin{cases} {}^{k}\bar{u}^{h}, & {}^ka^{h\top} {}^{k}\bar{u}^{h} \ge {}^{k}d^h, \\ {}^{k}\bar{u}^{h} + \dfrac{{}^{k}d^h - {}^ka^{h\top} {}^{k}\bar{u}^{h}}{\|{}^{k}a^{h}\|^{2}}\, {}^{k}a^{h}, & \text{otherwise}. \end{cases} \label{eq:cbf95proj95method}\tag{12}\] The correction term moves \({}^{k}\bar u^{h}\) along the constraint normal \({}^{k}a^{h}\) by the minimal amount so that \({}^ka^{h\top }{}^{k}u^{h}={}^{k}d^h\), which is the Euclidean projection onto the corresponding half-space and solves \(\min_{{}^{k}u^{h}}\;\tfrac12\|{}^{k}u^{h}-{}^{k}\bar u^{h}\|^2\) subject to \({}^ka^{h\top} {}^{k}u^{h}\ge {}^{k}d^h\).

Therefore, given \({}^{k}\bar u^{1:H-1}\), we compute a feasible rollout by projecting per step \({}^{k}\bar u^{h}\) to \({}^{k}u^{h}\) through the half-space projection implied by Eq. 12 . In MDOC, \(\mathcal{F}\) is thus implemented by iterating over \(H\): (1) projecting each proposed control \({}^{k}\bar u^{h}\) onto \({}^{k}\mathcal{K}^{h}\) to enforce safety and (2) rolling out the known dynamics one step to obtain \({}^{k}s^{h+1}\), so dynamic feasibility is not broken.

We now combine (1) the MCSA sampler from Section 3.2 and (2) the feasibility operator \(\mathcal{F}\). At each diffusion step \(k\), MDOC samples \(M\) potentially unsafe candidates \(\{\tilde{\tau}_m^{1:H}\}_{m=1}^M\) from the Gaussian proposal with Eq. 6 parameterized by controls \({}^{k}\bar{u}^{1:H-1}\), then recovers dynamically feasible and CBF-constrained trajectories \(\{\tilde{\tau}_m^{1:H}\}_{m=1}^M\) by rolling out \({}^{k}u_m^{1:H-1}\) through the known dynamics and applying the projection with Eq. 12 , namely \(\tau_m^{1:H} = \mathcal{F}(\tilde{\tau}_m^{1:H})\), computes Monte Carlo mean with \[{}^{k}\bar{\tau}^{1:H}= \frac{\sum_{m=1}^{M} {}^1p(\tau_m^{1:H})\,\tau_m^{1:H}}{\sum_{m=1}^{M} {}^1p(\tau_m^{1:H})}, \label{eq:mc95avg95prelim2}\tag{13}\] and forms the score estimate from Eq. 8 to update the noisy trajectory through Eq. 4 , as summarized in Algorithm 1. As a result, all trajectories used to compute the Monte Carlo estimate in Eq. 8 satisfy the dynamics and constraints.

4.2 MDOC-CBS↩︎

We now show how the mechanisms in MDOC can directly generalize to support planning for multi-robot settings via CBS [4]. Briefly, the CBS algorithm decomposes the MRMP problem into two levels: a low-level where each robot plans for itself, and a high-level, where the planned trajectories are surveyed, conflicts (i.e., inter-robot collisions) are identified, and constraints are imposed on colliding robots such that they will avoid the collision configurations upon replanning.

Specifically, MDOC-CBS, as shown in Algorithm 2, first creates a root node \(V_{\text{root}}\) and stores it in a priority queue termed the constraint tree (CT). The root node includes (1) an constraint set \(V_{\text{root}}.\mathcal{C}_i\) for each robot \(i\), initialized with obstacle-avoidance constraints, and (2) a joint trajectory \(V_{\text{root}}.\boldsymbol{\tau}^{1:H}\), constructed by planning for each robot with MDOC over the horizon \(H\).

CBS iteratively selects the least-cost node \(V\) from the CT (in our case, the one with the fewest conflicts) and evaluates it. If \(V.\boldsymbol{\tau}^{1:H}\) is conflict-free, it returns it as a solution. Otherwise, if a conflict is found between robots \(i\) and \(j\) at time \(h\), CBS branches node \(V\) into two new CT nodes, \(V_i\) and \(V_j\), for robots \(i\) and \(j\): each child node copies the constraint sets and trajectories from \(V\) and adds a new constraint forbidding robot \(i\) (resp.\(j\)) from entering a small workspace sphere centered at robot \(j\)’s state \(s_j^h\) at time \(h\), with a safe margin \(r\). CBS then replans the trajectories of robots \(i\) and \(j\) using the low-level MDOC planner under the updated constraint sets \(V_i.\mathcal{C}_i\) and \(V_j.\mathcal{C}_j\), enforced by the single-robot feasibility operator \(\mathcal{F}\) via CBF-constrained projections within the model-based diffusion rollouts, as illustrated in Algorithm 1. When replanning robot \(i\), each CBS constraint generated from a conflict with robot \(j\) is represented as a time-indexed forbidden sphere centered at the conflicting state of robot \(j\). We convert the pairwise CBF \(b_{ij}(s_i,s_j)\) into a single-robot barrier by fixing the second argument to this sphere center, that \(b_{i\mid j}^h(s_i^h) = b_{ij}(s_i^h,s_j^h)\) , where \(s_j^h\) is fixed by the CBS constraint. If \(b_{ij}(s_i^h,s_j^h)=\|s_i^h-s_j^h\|^2-r^2\), then \(b_{i\mid j}^h(s_i^h)=\|s_i^h-s_j^h\|^2-r^2\ge 0\), enforced by \(\mathcal{F}\) with \(b_{i\mid j}^{h+1}\ge(1-\gamma\Delta h)b_{i\mid j}^{h}\) during diffusion rollouts. The two new CT nodes, with updated trajectories \(V_i.\boldsymbol{\tau}^{1:H}\) and \(V_j.\boldsymbol{\tau}^{1:H}\), are added to the CT priority queue. CBS then pops the node with the fewest conflicts for further expansion.

5 Experimental Analysis↩︎

We aim to explore the performance of our method through various environments to address the following three questions:

• What advantages does MDOC offer over state-of-the-art (SOTA) single-robot planners?

• How does MDOC-CBS compare to SOTA MRMP planners in terms of success rate, trajectory smoothness, and the ability to ensure constraints?

• Is MDOC-CBS robust and scalable with respect to map size, planning horizon, and number of robots?

Experimental Setup. We evaluate our method MDOC and its MRMP version MDOC-CBS on various map types (illustrated in Fig. 3): (1) we use the Narrow map of size \(2\times2\) to evaluate MDOC in single-robot settings; (2) we use the Empty map, Conveyor map [13], Drop-Region map [13], and Random Map (with an obstacle density of 13.7%), all of size 2\(\times\)​2, to evaluate MDOC-CBS in multi-robot settings; (3) we use Empty Large maps of size 4\(\times\)​4 and 6\(\times\)​6 for multi-robot scalability evaluation. The robot radius is fixed at 0.05 for all maps.

We consider three start-goal pairs setups: (1) Circle Setup requires robots to symmetrically swap positions between opposite points on the perimeter; (2) Weave Setup requires robots to exchange positions along uniformly spaced boundary points; (3) Random Setup randomly generates start-goal pairs in maps. Setup demonstrations are shown

Baselines. We compare our method, MDOC, with SOTA trajectory optimization and planning methods: (1) CEM [51], a sampling-based trajectory optimizer using importance-weighted elite selection; (2) MPPI [52], a stochastic model-predictive control method based on path integral sampling; and (3) RRT* [53], a sampling-based asymptotically optimal motion planner. We also compare MDOC-CBS with SOTA CBS-based MRMP methods: (4) CBS [4] and its variant Enhanced CBS (ECBS) [5], multi-agent path finding planners with grid-based A* in the discretized configuration space as its low-level planner; (5) KCBS [39], a continuous-space variant of CBS with kinematic RRT* as its lower-level planner; (6) MMD-CBS [13], a continuous-space variant of CBS with model-free diffusion, trained on RRT* trajectories, as its low-level planner. Note that we mainly focus on CBS-based MRMP planners, since MMD-CBS outperforms “composite” methods, which directly optimize joint constraints and objectives without CBS [13].

5.1 Experimental Results↩︎

We conduct experiments on an NVIDIA RTX A4500 GPU with 20 GB of memory and 12 CPU cores. We run 10 trials per method and map, each with a different random seed, under identical map conditions. We use a single-integrator model in the main experiments for clarity.

Why Model-Based Diffusion in Motion Planning? As illustrated in the right panel of Fig. 4, we showcase the sample distribution at 90%, 50%, and 10% of the diffusion denoising process. We observe that the early-stage (90%) distribution is wide, covering multiple homotopy classes and exploring diverse passages. As iterations progress (50%), the distribution is denoised towards feasible and low-cost paths by score guidance augmented with dynamics and constraint projections, traversing the bottleneck in both maps and safely avoiding obstacles in More-Constrained Narrow (bottom row), and finally converges to a high-quality feasible trajectory at the late stage (10%). CEM and MPPI refine the control sequence through importance weighting or elite resampling, but mainly rely on repeated stochastic resampling, which often leads to getting stuck in wrong homotopy classes or failing in highly multimodal or constrained environments. MDOC establishes a well-conditioned gradient structure over the trajectory distribution and explicitly incorporates dynamics and constraints into the denoising process, so that every reverse step is model-informed guidance rather than a blind perturbation. Together, these properties turn motion planning into a model-based diffusion process, where the dynamics model and constraints shape the score instead of relying solely on sampled rollouts. This allows each candidate trajectory to become progressively more feasible and cost-efficient within a single multi-step refinement procedure, achieving higher effectiveness under the same sampling budget compared to other optimizers that depend purely on random perturbation resampling. “Pass&Free”-Yield (PF-Yield) from Table [tab:pf] further verifies that MDOC achieves a 100.0% effective sample rate, while CEM and MPPI remain significantly lower on both narrow maps.

As a tree-search-based geometric planner, RRT* explores reachable paths by randomly sampling states and incrementally expanding a search tree, but it does not directly optimize trajectory cost or enforce dynamic feasibility. To quantify its effective sampling efficiency in narrow passages, we compute PF-Yield for RRT* as the fraction of expanded nodes whose associated paths both traverse the bottleneck and remain collision-free (Pass & Free) over all expanded nodes. Comparing PF-Yield across RRT* and MDOC therefore places a geometric planner and a trajectory optimizer on the same footing, by asking: out of all samples or rollouts under a fixed budget, how many actually produce useful, bottleneck-traversing, collision-free candidates? Under this metric, RRT* attains PF-Yield values of 66.1% and 41.7% on the two maps, substantially lower than MDOC’s 100%. This indicates that, under a limited sample budget, while RRT* possesses global reachability and probabilistic completeness, it still wastes many samples in unproductive homotopy classes and struggles to efficiently converge to a feasible and smooth solution in bottlenecked environments. In contrast, MDOC, through diffusion denoising in trajectory space, achieves continuous convergence from global exploration to local refinement guided by dynamics and constraints, thereby completing path selection and trajectory optimization within a single model-based diffusion process. Therefore, we consider MDOC as a model-based diffusion planner that inherits the global exploration behavior of RRT* while also enjoying the optimization capabilities of MPPI and CEM, using the underlying dynamics model and hard constraints to steer the diffusion updates, thereby achieving higher feasibility and convergence efficiency.

Performance for MRMP on 2\(\times\)​2 Maps. We first evaluate the scalability of MDOC-CBS on the Empty map and Conveyor map with the Circle and Weave setups up to 20 robots (Fig. 6). The experiment uses a fixed horizon \(H=64\). Overall, the results show that MDOC-CBS (blue lines) outperforms baseline methods under the same map and start-goal conditions in most cases. In contrast, MMD-CBS fails beyond 10 robots in the Empty Circle setup and 12 in the Conveyor Circle setup, while KCBS’s success rate steadily degrades after 5 robots. At the same time, MDOC-CBS achieves 5\(\times\)–10\(\times\) lower planning time than MMD-CBS at comparable CT expansion levels, since it computes the trajectory score analytically through MCSA with model-based rollouts rather than repeatedly invoking a large diffusion network.

Moreover, our MDOC-CBS consistently produces shorter and smoother trajectories than KCBS and MMD-CBS (Table [tab:path]). This improvement stems from the model-based diffusion objective with MCSA, which drives rich exploration directly in the continuous state space while regularizing dynamic coherence. KCBS, which relies on discrete RRT* primitives, tends to generate irregular, jittery motions, and MMD-CBS inherits these artifacts because its denoising objective is trained to imitate RRT*-generated demonstrations. In contrast, MDOC-CBS actively exploits open free space and promotes dynamically coordinated motion across agents (Fig. 5), yielding trajectories that are globally more symmetric and better synchronized. The difference is most pronounced in the Conveyor map, as shown in the bottom row of Fig. 5. MMD-CBS inherits the demonstration’s “queue-through-the-corridor” behavior: all 6 robots enter the narrow passage, causing oscillation and congestion at the bottleneck, which inflates path length and degrades geometric smoothness. In contrast, MDOC-CBS does not force all robots into the same homotopy. Through model-based score ascent and continuous-space exploration, only 2 robots choose the corridor, while the remaining robots naturally distribute themselves around the outer free space. This leads to significantly shorter average path lengths, fewer high-level CT expansions, and substantially smoother continuous trajectories while ensuring a safety margin. These gains arise from combining diffusion-based exploration with a model-based low-level objective within CBS, thereby improving scalability in dense settings.

We further evaluate MDOC-CBS on a Drop-Region map with four tightly packed central obstacles and a Random map with uniformly sampled start-goal pairs (Fig. 7). In both settings, MDOC-CBS and MMD-CBS achieve similar success rates and clearly outperform KCBS, while MDOC-CBS consistently attains lower planning time at comparable CT expansion levels, indicating robustness across different map structures. However, MDOC-CBS exhibits higher variability: its success rate does not always reach 100% in random maps, and most failures occur when a robot collides with an obstacle and no valid rollout is returned, reflecting higher Monte Carlo score ascent variance in compressed free space with tight random constraints. This suggests a nontrivial trade-off between open-space exploration and aggressive safety tightening within the CBF-constrained projection during model-based diffusion rollouts. Even so, MDOC-CBS solves instances with up to 15 robots, and we expect stability to improve with better variance control and constraint projections in future work.

Longer-Horizon MRMP on Larger Maps. We present a robustness study of MDOC-CBS on larger maps with longer horizons (\(H=128\) for larger \(4\times4\) and \(H=196\) for larger \(6\times6\)). As shown in Fig. 8, MDOC-CBS remains robust as the problem size grows, achieving the highest success rate up to 40 robots. In contrast, MMD-CBS degrades sharply beyond 30 robots: increasing \(H\) directly lengthens the denoising chain and inflates the inference cost of its model-free single-robot diffusion model, so many runs hit the time limit before CBS can complete the high-level search. MDOC-CBS, by comparison, scales much more gracefully with horizon length. Because MDOC performs model-based diffusion rollouts in parallel under known dynamics, the per-iteration cost grows sub-linearly with \(H\) compared to model-free denoising, and even when CBS introduces additional constraints on larger maps, the computation can be amortized across robots and time steps. This makes MDOC-CBS substantially more robust for long-horizon planning and explains its superior performance on the \(6\times6\) map.

6 Conclusion↩︎

In this work, we propose MDOC, a CBF-constrained model-based diffusion planner that satisfies dynamical feasibility and safety constraints simultaneously. We also extend the planner to the multi-robot settings to solve MRMP. Results showcase the superior performance of our method in various environmental setups. Future directions include optimizing Monte Carlo variance and constraint projection efficiency, adapting the planner to online tasks with receding-horizon methods, and exploring richer dynamics models and multi-robot variants such as Prioritized Planning.

Acknowledgments↩︎

This work was partially supported by the National Science Foundation under Grants #\(2328671\) and #\(2441629\).

References↩︎