Distributed Motion Planning for Multi-Robot Systems under Topological Constraints
Authors: Gianpietro Battocletti, Dimitris Boskos, Dimos V. Dimarogonas, Bart De Schutter
Organizations: Delft Center for Systems and Control, Delft University of Technology, Delft, The Netherlands. · Division of Decision And Control Systems, KTH Royal Institute of Technology, Stockholm, Sweden.
Efficient and distributed coordination of mobile robots is one of the main challenges in multi-robot systems. Topological constraints, often expressed as topological braids, are a popular tool to encode complex coordination patterns between multiple mobile robots, as they offer a compact and abstract representation of the desired qualitative relation between the space-time trajectories of the robots. However, execution of joint motion plans encoded as braid-based topological constraints via distributed controllers is challenging, with existing approaches, generally based on the execution of one braid generator at a time, producing slow and suboptimal trajectories. We propose a distributed controller based on Model Predictive Control (MPC) to efficiently execute braid-based topological specifications. Rather than directly tracking the braid specification, we propose to use winding numbers, which are topological invariants for braids, as a proxy. This has the twofold benefit of converting braids into a continuous function, which can be easily tracked by an MPC controller through an appropriate term in the cost function, and of decoupling the global braid specification into a set of pairwise specifications, which can be tracked distributedly through the solution of only local MPC problems. To maintain global coordination, we propose a consensus-based progress estimation approach, which allows the robots to synchronize their motion toward the desired specification. We validate the proposed approach in simulation and in real-world experiments, where we demonstrate the effectiveness of the proposed approach and the improvement over existing approaches in terms of execution speed and control effort.
Figures & tables
Figure 1: (a) Example of a geometric braid composed of 3 strands. A corresponding braid word is ω=σ1σ1−1σ2σ1−1 . (b) Two braid generators σ1 , σ1−1 , and the identity braid e . (c) Example of product operation between two braid generators, corresponding to their concatenation.
Figure 2: (a) a set of three space-time curves λ1 , λ2 , λ3 generated by the motion of 3 points x1,x2,x3 in X over time. The curves are projected on the plane Π to generate the braid diagram shown in (b). The braid diagram corresponds to the braid word ω=σ1−1σ2−1σ2−1σ1−1σ1−1σ1−1 .
Figure 3: On the left-hand side, an example of the relative angle θi,j for a pair of strands. On the right-hand side, the winding number corresponding to a pairwise swap between two strands, the sign of which depends on the rotation direction.
Figure 4: (a) Example of a geometric braid composed by 4 strands. A corresponding braid word is ω=σ3σ1−1σ2σ1σ1σ3−1 . (b) Winding numbers between each pair of strands in the given braid computed using the preprocessing strategy from Section 4.1.1 . (c) Winding numbers computed from Algorithm 1 . In (b) we have B=6 , in (c) B′=4 .
Figure 5: (a) Example of a sequence of four permutation grids, describing three swaps in the relative positions for a set of m=3 robots. (b) Example of a set of representative paths describing the same topological equivalence class as the sequence of permutation grids on the left.
Figure 6: (a) Projection of the initial positions of the robots on the projection plane Π . (b) Computation of the relative angles θi,j(0) and θi,jΠ(0) used to initialize the winding numbers set Wˉ .
Parameter
Value
Δt
0.25 s
K
21
ξu
0.05
ξg
1
ξw
20
s
20
Table 1: Values of the parameters of the D-WMPC and GBC approaches used in the simulations.
Figure 7: Simulation of the D-WMPC controller for a set of m=10 robots. (a) The braid diagram corresponding to the input specification ω . (b) The target winding numbers wij (top plot) and the realized winding numbers wˉij corresponding to the trajectories of the robots (bottom plot). (c) The local (colored lines) and global (black dashed line) estimation of the progress variable τ over time. (d) The trajectories of the 10 robots over time.
#
m
T [s]
tsol,avg [s]
tsol,max [s]
lavg
lmin
lmax
uavg
Σu
1
3
D-WMPC
32.50
0.0348 ± 0.0060
0.0539
11.33 ± 1.22
9.77
12.75
0.0007 ± 0.0044
8.52
3
GBC
115.50
0.0268 ± 0.0141
0.0785
32.18 ± 6.99
23.10
40.13
0.0020 ± 0.0071
24.18
2
5
D-WMPC
29.50
0.0354 ± 0.0083
0.0583
7.94 ± 2.71
4.40
12.69
0.0005 ± 0.0035
9.96
5
GBC
94.75
0.0278 ± 0.0152
0.0980
26.71 ± 3.78
22.66
33.26
0.0017 ± 0.0064
33.43
3
5
D-WMPC
44.75
0.0352 ± 0.0089
0.0584
9.47 ± 2.84
5.04
13.61
0.0006 ± 0.0035
11.89
5
GBC
145.25
0.0223 ± 0.0146
0.0983
27.64 ± 3.87
23.10
34.21
0.0017 ± 0.0064
34.59
Table 2: Comparison of the GBC and D-WMPC approaches in simulation.
Figure 8: Control architecture for the real-world experiments in the ATMOS platform. The red box represents the ROS 2 node with the proposed controller, the orange ones the ROS 2 nodes running the px4-mpc controllers, and the yellow ones the PX4 flight controllers running on board of the robots.
Robot 1
Robot 2
Robot 3
D-WMPC
10.59
9.44
9.66
GBC
13.24
13.48
15.01
Table 3: Length of the trajectories of the ATMOS robots in the experiment depicted in Figure 9 , expressed in meters.
Figure 9: Experiment with the ATMOS robots. The robots are tasked with tracking the specification ( 40 ). Both the sets of trajectories yield a topological braid corresponding to the specification.
Figure 10: Input forces Fx and Fy , expressed in N, generated by the D-WMPC (left) and GBC (right) approaches to execute the trajectories of the experiment depicted in Figure 9 .
Figure 11: Experiment with tethered ATMOS robots. (a) A close-up of the initial knot between the tethers. (b) The robots in the initial configuration, with the tethers entangled by the knot. (c) The robots in their final configuration with the tethers free from entanglement. (d) The paths followed by the robots to disentangle the tethers, corresponding to the specification ω=σ2σ1−1σ2σ1−1 . The top plot shows the trajectories under the D-WMPC controller, the bottom one those generated by the GBC controller.
Figure 12: Input forces Fx and Fy , expressed in N, generated by the D-WMPC (left) and GBC (right) approaches to execute the trajectories of the experiment depicted in Figure 11 .
Robot 1
Robot 2
Robot 3
D-WMPC
3.46
4.80
4.51
GBC
4.51
6.30
6.49
Table 4: Length of the trajectories of the ATMOS robots in the experiment depicted in Figure 11 , expressed in meters.
Appendix figures & tables1 asset
Supplementary material from the paper’s appendix.
Appendix
Figure 13: Maximum change in the relative heading between two robots over a time step. Two robots i,j are displayed, each moving of the maximum distance they can cover in one time step, i.e., vmaxΔt .
Multi-robot trajectory planning is a fundamental problem in multi-robot coordination but remains computationally challenging due to its nonconvex, multimodal, and high-dimensional nature. This work builds upon D4orm, a dynamics-aware diffusion-denoising framework, and develops a family of planning architectures for diverse operational requirements. Unlike conventional numerical optimization methods, D4orm employs sampling-based optimization to generate solution trajectories through massively parallel sampling, leveraging modern computing architectures such as GPUs. Its diffusion-denoising structure iteratively optimizes \textit{deformations} to candidate control trajectories, providing an efficient and versatile paradigm for generating kinodynamically feasible and conflict-free trajectories. Using D4orm as the building block for advanced planners, we present a decoupled planner for improved scalability, an online receding-horizon planner with feedback control, and a distributed planner for resource-constrained settings. Evaluations with differential-drive and holonomic robots in 2D and 3D environments demonstrate that D4orm-based approaches find high-quality solutions faster and more reliably than other sampling-based optimization methods, such as MPPI, as well as a learned diffusion-model-based method. We further demonstrate zero-shot deployment on ten real quadrotors with obstacles, large-scale deconfliction with 100 simulated robots, and fully onboard distributed `lifelong' operation with six ground robots. Overall, these results establish diffusion denoising as a scalable and reliable framework for multi-robot coordination. Code and video: https://github.com/proroklab/d4orm
Yuhao Zhang, Keisuke Okumura, Ajay Shankar +1
Department of Computer Science and Technology, University of Cambridge, U.K. · National Institute of Advanced Industrial Science and Technology (AIST), Japan
The simultaneous arrival of multiple mobile robots at a target point is crucial for cooperation tasks such as cooperative encirclement, disaster relief, and environmental monitoring. Although the simultaneous arrival problem itself is already complex, the problem becomes more challenging when there are constraints on the robot trajectory curvatures and the speeds are required to be constant (possibly different for different robots), and the control law for robots needs to be distributed. These constraints are typical for a multi-robot system consisting of, e.g., fixed-wing UAVs. To address this challenge, this paper proposes a distributed switching control method based on the maximum consensus protocol. By exploiting the geometric properties of Dubins paths along with optimization principles, a virtual time variable is introduced, and a hybrid control law that combines optimal control with saturated proportional control is designed. Under the proposed control law, each robot is driven to approach the maximum virtual time among its neighbors, thereby achieving simultaneous arrival under some mild conditions. Furthermore, we prove that in certain cases the proposed method attains a theoretically optimal arrival time. The approach is scalable and real-time, with low communication overhead. Its effectiveness and robustness are validated through extensive simulations and experiments.
Zhouru Xiao, Yang Lu, Weijia Yao +2
School of Artificial Intelligence and Robotics, Hunan University, China · College of Intelligence Science and Technology, National University of Defense Technology, China
Trajectory optimization for multi-robot systems remains a critical challenge, particularly when navigating highly non-convex, non-linear, and non-differentiable environments. While Model-Based Diffusion (MBD) has recently emerged as a promising sampling-based optimization paradigm for single-robot trajectory generation, extending it to multi-robot systems results in a centralized, high-dimensional inference problem that (i) suffers from poor sample efficiency due to the curse of dimensionality and (ii) requires global access to all robots' dynamics, constraints, and objectives. To address this, we propose Distributed Model-Based Diffusion (DMBD), a distributed server-robot method that decomposes the reverse diffusion process into local conditional reverse diffusion processes. This decomposition enables each robot to iteratively perform denoising independently within its own control subspace while conditioning on the current trajectory estimates of the other robots that are aggregated and broadcast by the server. Extensive simulations in goal swapping, multi-floor coverage, parking, and rush-hour scenarios demonstrate that DMBD achieves strong scalability, solving many challenging coordination tasks with sub-second computation time and outperforming existing baselines.
Haejoon Lee, Xinyi Wang, Taekyung Kim +1
Robotics Department, University of Michigan, Ann Arbor, MI, USA