Demonstration for CFOO

CFOO: a Conflict-Free Online Orchestration framework for continuous formation navigation
of wheeled mobile robots (WMRs).

1Tsinghua University, 2National University of Singapore, 3University of Michigan

Video loading: Videos may take a moment to load on the hosting platform.
Thank you for your patience.

Simulation

These results demonstrate CFOO's effectiveness in enabling continuous formation navigation of WMRs in varying environments. The TVFR mechanism effectively reduces motion oscillations caused by abrupt updates to JSTP’s desired formation during formation transitions.

Real-World Experiments

Continuous Formation Navigation

These results demonstrate CFOO's ability to adapt WMR formations to changes in the environmental conditions while maintaining smooth, continuous navigation.

Results & Metrics

The following plots show formation error and individual WMR lateral positions. For the correspondence between WMR indices in the video above and those in the figures, see the WMR Indices tab.

Formation Error Metric
Formation Error Metric
Lateral Position
Lateral Position

Smoother Formation Transitions with TVFR

TVFR: the Time-Varying Formation Reference mechanism

TVFR gradually evolves the desired formation instead of imposing an abrupt target change. The event-aligned simulation replays below compare Without TVFR (instantaneous reference update) and With TVFR (smoothly evolving references) during the 3-to-2 and 1-to-3 formation transitions, using identical obstacle geometry, reference route, initial conditions, and transition trigger rules.

Left: Without TVFR. Right: With TVFR. Both views use 1× playback and identical spatial scales. The 3→2 comparison is followed by 1→3.

Transition Trajectories & Motion Smoothness

Recorded motion of all seven WMRs, aligned at the transition event (\(t=0\)).

Transition Trajectories

Recorded trajectories for the 3-to-2 and 1-to-3 transitions: Without TVFR on the left and With TVFR on the right.

Matched x–y limits and equal metric aspect ratios within each comparison. Colored dots mark each WMR's position at the transition event. Click the figure to enlarge.

Motion Smoothness

Lateral and longitudinal velocity traces and per-WMR excess lateral travel: Without TVFR versus With TVFR.

Top: lateral velocity. Middle: longitudinal velocity. Bottom: excess lateral travel during the first 16 s after transition onset. This measures lateral back-and-forth motion beyond the net displacement.

\[E_{y,u}=\sum_k |y_{u,k+1}-y_{u,k}|-|y_{u,\mathrm{end}}-y_{u,\mathrm{start}}|.\]

Here, \(u\) indexes WMRs and \(k\) indexes samples.

Motion Regularization \(\mathcal{P}_{\mathrm{add}}\)

The motion regularization term \(\mathcal P_{\mathrm{add}}\) complements the graph-based formation maintenance cost \(\mathcal P_{\mathrm{form}}\) with relative-position, relative-velocity, and lateral route-anchoring costs. Here, \(\mathbf p_u\) and \(\dot{\mathbf p}_u\) denote the position and velocity of WMR \(u\in\mathcal V\), while \(\mathbf r_u\) and \(\dot{\mathbf r}_u\) denote its TVFR reference and reference velocity. Overbars denote averages over all \(N\) WMRs.

Each WMR optimizes its own trajectory using received trajectories of the other WMRs. All positions, velocities, and references are evaluated at the same global timestamp \(\tau_j\). The ego position is \(\mathbf p_i(t_j)\), while the position \(\mathbf p_u(\tau_j)\) of each other WMR \(u\) is evaluated from its received trajectory. The local sample time is \(t_j=jT_i/K_i\), with \(\tau_j=\sum_{r=1}^{i-1}T_r+t_j\). Here, \(u\) indexes WMRs, while \(i\) and \(j\) index polynomial segments and samples, respectively. Time arguments are omitted below for brevity.

1. Relative-Position Cost

The graph-based formation cost does not uniquely determine formation orientation. With isotropic distance weights (\(a=1\)), it is invariant to global rotation. We therefore penalize centroid-relative position errors to align the formation while preserving translational freedom:

\[ \begin{gathered} \mathbf e_{\mathrm{pos},u}=(\mathbf p_u-\bar{\mathbf p})-(\mathbf r_u-\bar{\mathbf r}),\\ g_{\mathrm{pos}}=\frac{1}{N}\sum_{u\in\mathcal V}\|\mathbf e_{\mathrm{pos},u}\|_2^2,\\ \frac{\partial g_{\mathrm{pos}}}{\partial\mathbf p_u}=\frac{2}{N}\mathbf e_{\mathrm{pos},u}. \end{gathered} \]

This term promotes the relative positions and orientation specified by the TVFR reference without fixing the formation centroid.

2. Relative-Velocity Cost

Tracking relative positions alone may cause repeated acceleration and braking. We therefore penalize the difference between centroid-relative velocities and the corresponding TVFR reference derivatives:

\[ \begin{gathered} \mathbf e_{\mathrm{vel},u}=(\dot{\mathbf p}_u-\overline{\dot{\mathbf p}})-(\dot{\mathbf r}_u-\overline{\dot{\mathbf r}}),\\ g_{\mathrm{vel}}=\frac{1}{N}\sum_{u\in\mathcal V}\|\mathbf e_{\mathrm{vel},u}\|_2^2,\\ \frac{\partial g_{\mathrm{vel}}}{\partial\dot{\mathbf p}_u}=\frac{2}{N}\mathbf e_{\mathrm{vel},u}. \end{gathered} \]

During formation transitions, this term coordinates the relative motion specified by TVFR. Once the reference becomes constant, it encourages a common team velocity without requiring the WMRs to stop. With acceleration \(\ddot{\mathbf p}_u\), the common-time derivatives of the position and velocity costs are:

\[ \begin{gathered} \frac{\mathrm d g_{\mathrm{pos}}}{\mathrm dt}=\frac{2}{N}\sum_{u\in\mathcal V}\mathbf e_{\mathrm{pos},u}^{\top}(\dot{\mathbf p}_u-\dot{\mathbf r}_u),\\ \frac{\mathrm d g_{\mathrm{vel}}}{\mathrm dt}=\frac{2}{N}\sum_{u\in\mathcal V}\mathbf e_{\mathrm{vel},u}^{\top}(\ddot{\mathbf p}_u-\ddot{\mathbf r}_u). \end{gathered} \]

3. Lateral Route Anchoring

Relative-position and relative-velocity costs do not prevent collective lateral drift. During formation transitions along straight route segments, we penalize lateral centroid position error and velocity. The fixed lateral target \(y_c=y_{\mathrm{route}}+\delta_y\) combines the route coordinate with the mean assigned-slot offset \(\delta_y\):

\[ \begin{gathered} g_y=(\bar y-y_c)^2, \qquad g_{\dot y}=\overline{\dot y}^{\,2},\\ \frac{\partial g_y}{\partial\mathbf p_u}=\frac{2}{N}(\bar y-y_c)\mathbf e_y, \qquad \frac{\partial g_{\dot y}}{\partial\dot{\mathbf p}_u}=\frac{2}{N}\overline{\dot y}\,\mathbf e_y. \\ \mathbf e_y=(0,1)^{\mathsf T}. \end{gathered} \]

These terms suppress collective lateral drift while leaving longitudinal motion unpenalized. With \(y_c\) fixed on each straight segment, \(\mathrm d g_y/\mathrm dt=2(\bar y-y_c)\overline{\dot y}\) and \(\mathrm d g_{\dot y}/\mathrm dt=2\overline{\dot y}\,\overline{\ddot y}\).

The instantaneous motion regularization cost combines the three components:

\[ \begin{gathered} g_{\mathrm{lat}}=\alpha_p g_y+\alpha_v g_{\dot y},\\ g_{\mathrm{add}}=\alpha_p g_{\mathrm{pos}}+\alpha_v g_{\mathrm{vel}}+g_{\mathrm{lat}}, \qquad\alpha_p,\alpha_v\geq0. \end{gathered} \]

Trapezoidal integration over the trajectory yields the motion regularization term used in JSTP:

\[ \begin{gathered} \mathcal P_{\mathrm{add}} =\sum_{i=1}^{M}\frac{T_i}{K_i} \sum_{j=0}^{K_i}\omega_j\,g_{\mathrm{add}}(t_j,\tau_j),\\ t_j=\frac{jT_i}{K_i},\qquad \tau_j=\sum_{r=1}^{i-1}T_r+t_j. \end{gathered} \]

Here \(\omega_0=\omega_{K_i}=1/2\) and the interior weights equal one. The ego state is evaluated at local time \(t_j\); received trajectories of other WMRs and TVFR references are evaluated at the corresponding global time \(\tau_j\), using a common time origin. The nonnegative weights \(\alpha_p\) and \(\alpha_v\) balance the position and velocity costs.

Gradients are propagated through the \(\mathfrak{T}_{\mathrm{MINCO}}\) trajectory parameterization, accounting for both local sampling times and global timestamps when differentiating segment durations.

Abstract

Formation control of wheeled mobile robots (WMRs) has been widely studied due to its broad applications. However, existing studies primarily focus on tracking predefined formations, with limited adaptability to varying environments. To address this limitation, we propose CFOO, a Conflict-Free Online Orchestration framework for continuous formation navigation of WMRs that integrates centralized formation generation with distributed formation maintenance. The upper layer monitors the environment in real time and generates adaptive formations as needed, using the proposed TCF-R2T (Trajectory-Conflict-Free Robot-to-Target assignment) algorithm to compute optimal assignments in polynomial time for timely, conflict-free formation transitions. At the lower layer, each WMR continuously employs JSTP (Joint Spatio-Temporal trajectory Planning) to maintain the given formation by jointly optimizing spatial positions and segment durations, improving coordination over spatial-only optimization with fixed durations. By coordinating these two layers, CFOO orchestrates WMRs to adapt their formations to varying environments and thus enable continuous navigation. Moreover, we design a Time-Varying Formation Reference (TVFR) mechanism to facilitate smooth transitions between successive desired formations. Both simulation and real-world experiments validate the effectiveness and adaptability of CFOO. Experimental videos are available on our project website.

Cite This Work

If you refer to CFOO in your research, please cite this paper. In BibTeX format:

@article{dong2026orchestrating,
      title={Orchestrating Wheeled Mobile Robots Online without Conflicts}, 
      author={Jianghong Dong and Yifeng Zhang and Wenhan Cao and Jiawei Wang and Mengchi Cai and Keqiang Li and Guillaume Sartoretti},
      journal={arXiv preprint arXiv:2605.18441},
      year={2026}
}