Density-Driven Optimal Control (D2OC) provides a principled approach to distributing multi-robot teams over non-uniform spatial distributions. Applying D2OC to nonholonomic robots, however, creates a gap between safety constraints imposed on a reference motion and the physical inputs that determine the actual robot motion. We address this issue by enforcing the safety constraint directly on the robot's physical inputs while preserving the density-driven coverage objective. The proposed framework combines D2OC with a control barrier function safety filter through a feedback-linearizing look-ahead point, allowing safety and actuator limits to be considered together during control. We further derive a safety margin that accounts for the look-ahead geometry, robot footprint, and motion during each control interval. Simulation results show that the proposed method maintains the required physical separation while achieving coverage performance comparable to a conventional reference-tracking approach, which can satisfy safety on the reference motion yet violate the corresponding physical clearance. Experiments on multiple nonholonomic robots in the Robotarium further demonstrate safe execution while driving the robots toward the desired spatial distribution. These results show that enforcing safety directly on the physical inputs can eliminate the mismatch between safety certification and physical robot motion in density-driven multi-robot coverage.
Figures & tables
Parameter
Description
Simulation
Robotarium
na
number of robots
5
5
T
sampling time [s]
0.1
0.066
Rs
interaction range [m]
15
0.30
Rc
communication range [m]
15
-
d
look-ahead offset [m]
0.4
0.008
Ds
required separation [m]
2.5
0.200
TABLE I: Controller parameters for the simulation study and for the Robotarium hardware deployment.
Fig. 1: Five-robot trajectories over the reference density for (a) the baseline and (b) the proposed method; Legend: × and ∘ denote initial and final positions, respectively.
Fig. 2: Minimum inter-robot distance. The proposed method maintains the look-ahead separation near Dcbf=4.11 m, whereas the two-layer baseline falls below the required physical distance Ds=2.50 m.
Proposed
Two-layer
Completion time [s]
109.6±5.0
106.0±3.3
Min. physical distance [m]
3.89±0.08
0.96±0.75
Worst physical distance [m]
3.76
0.075
Steps below Ds
0.0±0.0%
3.5±3.3%
Trials with violation
0/5
5/5
QP infeasibility count
12.4±6.2
53.4±30.3
TABLE II: Safety comparison over five trials. Mean ± standard deviation is reported for all repeated quantities. “Worst” denotes the minimum value observed across the five trials.
Fig. 3: 2 -Wasserstein distance to the reference density: proposed method (solid) and two-layer baseline (dotted).
Fig. 4: Robotarium deployment at the end of the run. The reference density and the completed robot paths are overlaid onto the overhead camera view, with one color per robot.
Fig. 5: Minimum inter-robot distance on hardware, evaluated at the look-ahead points pi on which the barrier acts. The recorded minimum of 0.2369 m stays above both Ds=0.200 m and Dcbf=0.227 m for the entire trial; the corresponding rear-axle minimum is 0.2327 m.
Fig. 6: 2 -Wasserstein distance between the time-averaged spatial distribution induced by the five-agent trajectories and the reference density. Mean over five Robotarium deployments from identical initial poses, with a band of one standard deviation. The mean decreases from 1.04 to 0.15 .
This paper presents a nonlinear extension of Density-Driven Optimal Control (D2OC) for multi-agent spatial coverage with prescribed density distributions. Rather than assigning individual target locations, D2OC drives the collective spatial distribution of agents toward a desired density through a Wasserstein-based objective. We extend this framework to multi-step finite-horizon control for discrete-time control-affine nonlinear systems using sequential convex programming. At each control update, the nonlinear dynamics are locally linearized over the prediction horizon, yielding a strictly convex quadratic program that preserves the Wasserstein barycentric structure while directly incorporating input constraints. We further characterize the effect of constrained control deviations and nonlinear Taylor remainders on the accuracy of the local linear prediction, establishing an explicit finite-horizon error bound and a two-step specialization for receding-horizon implementation. The resulting method retains the decentralized, distribution-driven nature of D2OC while providing a computationally efficient optimization procedure for nonlinear multi-agent systems. Simulations with unicycle and quadrotor teams show coverage performance comparable to nonlinear model predictive control, while substantially reducing computation time. These results demonstrate a tractable and theoretically characterized framework for density-driven spatial coverage under nonlinear dynamics.
Julian Martinez, Kooktae Lee
Department of Mechanical and Aerospace Engineering, Texas Tech University, Lubbock, TX 79409, USA
This paper presents a novel density control framework for multi-robot systems with spatial safety and energy sustainability guarantees. Stochastic robot motion is encoded through the Fokker-Planck Partial Differential Equation (PDE) at the density level. Control Lyapunov and control barrier functions are integrated with PDEs to enforce target density tracking, obstacle region avoidance, and energy sufficiency over multiple charging cycles. The resulting quadratic program enables fast in-the-loop implementation that adjusts commands in real-time. Multi-robot experiment and extensive simulations were conducted to demonstrate the effectiveness of the controller under localization and motion uncertainties.
Longchen Niu, Andrew Nasif, Gennaro Notomista
Department of Electrical and Computer Engineering, University of Waterloo, Waterloo, ON, Canada
This paper addresses the decentralized non-uniform area coverage problem for multi-agent systems, a critical task in missions with high spatial priority and resource constraints. While existing density-based methods often rely on computationally heavy Eulerian PDE solvers or heuristic planning, we propose Stochastic Density-Driven Optimal Control (D2OC). This is a rigorous Lagrangian framework that bridges the gap between individual agent dynamics and collective distribution matching. By formulating a stochastic MPC-like problem that minimizes the Wasserstein distance as a running cost, our approach ensures that the time-averaged empirical distribution converges to a non-parametric target density under stochastic LTI dynamics. A key contribution is the formal convergence guarantee established via reachability analysis, providing a bounded tracking error even in the presence of process and measurement noise. Numerical results verify that Stochastic D2OC achieves robust, decentralized coverage while outperforming previous heuristic methods in optimality and consistency.
Kooktae Lee
Department of Mechanical Engineering, New Mexico Institute of Mining and Technology, Socorro, NM 87801, USA