Differentiable Environment-Trajectory Co-Optimization for Safe Multi-Agent Navigation

Zhan Gao; Gabriele Fadini; Stelian Coros; Amanda Prorok

cs.ROApr 8, 2026

Differentiable Environment-Trajectory Co-Optimization for Safe Multi-Agent Navigation

Authors: Zhan Gao, Gabriele Fadini, Stelian Coros, Amanda Prorok

Organizations: Department of Computer Science and Technology, University of Cambridge, UK · Zurich University of Applied Sciences, ZHAW, Winterthur, Swizerland · Computational Robotics Lab, Department of Computer Science, ETH Zurich, Switzerland

Abstract

The environment plays a critical role in multi-agent navigation by imposing spatial constraints, rules, and limitations that agents must navigate around. Traditional approaches treat the environment as fixed, without exploring its impact on agents' performance. This work considers environment configurations as decision variables, alongside agent actions, to jointly achieve safe navigation. We formulate a bi-level problem, where the lower-level sub-problem optimizes agent trajectories that minimize navigation cost and the upper-level sub-problem optimizes environment configurations that maximize navigation safety. We develop a differentiable optimization method that iteratively solves the lower-level sub-problem with interior point methods and the upper-level sub-problem with gradient ascent. A key challenge lies in analytically coupling these two levels. We address this by leveraging KKT conditions and the Implicit Function Theorem to compute gradients of agent trajectories w.r.t. environment parameters, enabling differentiation throughout the bi-level structure. Moreover, we propose a novel metric that quantifies navigation safety as a criterion for the upper-level environment optimization, and prove its validity through measure theory. Our experiments validate the effectiveness of the proposed framework in a variety of safety-critical navigation scenarios, inspired from warehouse logistics to urban transportation. The results demonstrate that optimized environments provide navigation guidance, improving both agents' safety and efficiency.

Figures & tables

Panel of Figure 1 — Figure 1: Environment configurations can impact the safety and efficiency of agent trajectories. Moving obstacle regions further apart in the environment of Fig. 1(b) allows to generate agent trajectories that are less likely to collide and exhibit improved path efficiency, compared to those in the environment of Fig. 1(a) .

Figure 2: Agent

A_{i}

starts from the initial position

{\mathbf{s}}_{i}

and moves towards the goal position

{\mathbf{g}}_{i}

by following its trajectory

{\mathbf{x}}_{i}

, and avoids collisions with obstacle regions

\{\Delta_{j}\}_{j=1}^{3}

in the environment

{\mathcal{E}}

Figure 3: Agents

A_{i}

A_{i^{{}^{\prime}}}

start from initial positions

{\mathbf{s}}_{i}

{\mathbf{s}}_{i^{{}^{\prime}}}

and move towards goal positions

{\mathbf{g}}_{i}

{\mathbf{g}}_{i^{{}^{\prime}}}

by following their trajectories

{\mathbf{x}}_{i}

{\mathbf{x}}_{i^{{}^{\prime}}}

, and avoid collisions with obstacle regions

\{\Delta_{j}\}_{j=1}^{3}

and each other.

Figure 4: Safety metric as a function of environment parameters in the example environment of Fig. 1 , where environment parameters are the $y$ -axis positions of the left and right obstacle regions.

Panel of Figure 5 — Figure 5: (a) Multi-agent navigation in a poorly-designed environment. While two agents may navigate from ${\mathbf{s}}_{i},{\mathbf{s}}_{i^{\prime}}$ to ${\mathbf{g}}_{i},{\mathbf{g}}_{i^{\prime}}$ without collisions, there are higher collision risks and they are more likely to collide around the intersection of their trajectories (red circle). (b) Multi-agent navigation in a well-designed environment. By refining the spatial constraint of the obstacle region, it implicitly de-conflicts agents by guiding agent $A_{1}$ to move faster than $A_{2}$ . This avoids potential collisions of two agents and improves the safety level of the environment, while not compromising their navigation performance.

Panel of Figure 6 — Figure 6: The six scenarios considered in our experiments. (a) Optimization of shelf positions in a warehouse for safe multi-robot navigation. (b) Optimization of the radius of a roundabout in an urban transportation system for safe multi-vehicle driving. (c) Optimization of narrow passage angles to facilitate vehicles safely passing through the narrow passage. (d) Optimization of the exit angle of a highway to facilitate vehicles safely leaving the highway. (e) Optimization of the intersection angle of two roads for safe multi-vehicle driving. (f) Optimization of the track centerline to facilitate vehicles safely driving on the track.

Scenario		Safety metric $\uparrow$	SPL $\uparrow$	NumCOLL $\downarrow$	Computation time $\downarrow$	PCTSpeed $\downarrow$
1-1 Warehouse ( $4$ agents)	Optimized environment (ours)	0.932 $\pm$ 0.030	0.948 $\pm$ 0.030	0 $\pm$ 0	6.554 $\pm$ 5.135	0.825 $\pm$ 0.035
	Standard environment with a regular layout	0.878 $\pm$ 0.049	0.930 $\pm$ 0.030	0 $\pm$ 0	34.384 $\pm$ 41.470	0.836 $\pm$ 0.042
	Baseline environment with a random layout	0.870 $\pm$ 0.073	0.934 $\pm$ 0.022	0 $\pm$ 0	51.817 $\pm$ 61.533	0.838 $\pm$ 0.034
1-2 Warehouse ( $8$ agents)	Optimized environment (ours)	0.944 $\pm$ 0.019	0.944 $\pm$ 0.029	0 $\pm$ 0	40.677 $\pm$ 38.371	0.793 $\pm$ 0.030
	Standard environment with a regular layout	0.909 $\pm$ 0.027	0.916 $\pm$ 0.019	0.063 $\pm$ 0.134	93.067 $\pm$ 75.022	0.814 $\pm$ 0.022
	Baseline environment with a random layout	0.913 $\pm$ 0.030	0.909 $\pm$ 0.025	0.144 $\pm$ 0.334	98.649 $\pm$ 98.703	0.822 $\pm$ 0.022

Table I : Performance of the proposed method and baselines in our scenarios. Larger values of the safety metric or SPL and smaller values of NumCOLL, computation time, PCTSpeed, or distance ratio represent higher safety and efficiency of multi-agent navigation. Mean and standard deviation, where specified, are computed over

20

random navigation tasks.

Panel of Figure 7 — Figure 7: (a) Optimized environment of our method that creates obstacle-free trajectories and prioritizes / de-conflicts agents for safe navigation. (b) Standard environment with a regular shelf layout. (c) Random environment with randomly generated shelf positions.

Panel of Figure 8 — Figure 8: (a) Optimized environment that guides agents to move clockwise towards their goal positions. (b) Empty environment with no roundabout that provides no navigation guidance for agents, although imposing no obstacle hindrance. (c) Baseline environment with a large roundabout that blocks efficient pathways, although providing navigation guidance.

Panel of Figure 9 — Figure 9: (a) Optimized environment that creates an asymmetric structure to prioritize / de-conflict agents to pass through the narrow passage. Blue dashed boundaries mirror the lower passage boundaries to demonstrate the asymmetry. (b) Baseline environment with narrow passage angles. (c) Baseline environment with wide passage angles. Both baselines are designed with human intuition and commonly used in practice, while their symmetric structures do not provide de-confliction guidance.

Panel of Figure 10 — Figure 10: (a) Optimized environment that guides agents to leave the highway smoothly towards their goal positions. (b) Baseline environment with an exit angle $\pi/4$ . (c) Baseline environment with an exit angle $\pi/3$ . Both baselines follow common design practice. Blue dashed boundaries in Figs. 10(b) - 10(c) represent the optimized environment for reference.

Panel of Figure 11 — Figure 11: (a) Optimized environment that guides agents to pass through the intersection smoothly towards their goal positions. (b) Baseline environment with an intersection angle $\pi/4$ . (c) Baseline environment with an intersection angle $3\pi/4$ .

Figure 12: Approximation of the ensemble of possible tracks’ centerlines. In grey $50$ random track designs are selected by picking points within the boundaries of the minumum and maximum black control points at equally spaced angular locations. We also showcase the initial and optimized track designs for reference.

Panel of Figure 13 — Figure 13: (a) Optimized environment that guides agents to move smoothly along the track. (b) Baseline environment.

Scenario		Safety metric $\uparrow$	SPL $\uparrow$	NumCOLL $\downarrow$	Computation time $\downarrow$	PCTSpeed $\downarrow$
Narrow Passage (unicycle)	Optimized environment (ours)	0.833	0.981	0	41.275	0.227
	Narrow environment	0.777	0.756	0.167	57.331	0.229
	Wide environment	0.784	0.987	0	74.580	0.275
Road intersection (unicycle)	Optimized environment (ours)	0.949	0.990	0	26.043	0.701
	Baseline environment with an intersection angle $\pi/4$	0.932	0.984	0	56.251	0.705
	Baseline environment with an intersection angle $3\pi/4$	0.944	0.988	0	29.213	0.701

Table II : Performance of the proposed method and baselines with unicycle dynamics.

Appendix figures & tables3 assets

Supplementary material from the paper’s appendix.

Appendix

Panel of Figure 14 — Figure 14: (a) Optimized environment with unicycle dynamics in the scenario of Narrow Passage. (b) Optimized environment with unicycle dynamics in the scenario of Road Intersection.

Stochastic environment optimization	Safety metric $\uparrow$	SPL $\uparrow$	NumCOLL $\downarrow$	Computation time $\downarrow$	PCTSpeed $\downarrow$
Optimized environment (ours)	0.929 $\pm$ 0.013	0.943 $\pm$ 0.019	0 $\pm$ 0	10.465 $\pm$ 9.490	0.816 $\pm$ 0.029
Standard environment with a regular layout	0.896 $\pm$ 0.029	0.916 $\pm$ 0.020	0 $\pm$ 0	36.572 $\pm$ 42.818	0.826 $\pm$ 0.021
Baseline environment with a random layout	0.894 $\pm$ 0.035	0.923 $\pm$ 0.022	0.075 $\pm$ 0.225	71.270 $\pm$ 86.946	0.821 $\pm$ 0.027

Appendix

Table III : Performance of the proposed method and baselines for stochastic environment-trajectory co-optimization.

Figure 15: Optimized environment w.r.t. random navigation tasks, where the agent trajectories shown in the figure are generated based on one sampled navigation task.

Explore similar work

Differentiable Optimization Layered Safety-Critical Control for Risk-Aware Navigation via Conformal Prediction
May 5, 2026Jinyang Dong, Shizhen Wu, Yongchun FangSafety-Critical Control Control Barrier Functions
Institute of Robotics and Automatic Information Systems, College of Artificial Intelligence, Nankai University, Tianjin 300071, China · Academy for Advanced Interdisciplinary Studies, Nankai University, Tianjin 300071, China · Institute of Intelligence Technology and Robotic Systems, Shenzhen Research Institute of Nankai University, Shenzhen 518083, China
Cooperative Multi-UAV Navigation in Complex Environments via Systematic Multi-Agent Deep Reinforcement Learning
Jul 28, 2026Yu Su, Nabil AoufLearning from Demonstration Curriculum Learning
School of Science and Technology, City St George’s University of London, London, United Kingdom
Safe and Generalizable Hierarchical Multi-Agent RL via Constraint Manifold Control
Jun 22, 2026Zihao Guo, Jianing Zhao, Ling Li +3Robotic Control Multi-Agent Coordination
1King’s College London · University of California, Berkeley · 3The Alan Turing Institute

May 5, 2026J/K move · Enter open · S save

Differentiable Optimization Layered Safety-Critical Control for Risk-Aware Navigation via Conformal Prediction

Jinyang Dong, Shizhen Wu, Yongchun Fang

Institute of Robotics and Automatic Information Systems, College of Artificial Intelligence, Nankai University, Tianjin 300071, China · Academy for Advanced Interdisciplinary Studies, Nankai University, Tianjin 300071, China · Institute of Intelligence Technology and Robotic Systems, Shenzhen Research Institute of Nankai University, Shenzhen 518083, China

Safety-Critical Control Control Barrier Functions Safe Robot Navigation Robot Safety Differentiable Optimization

Open paper AskPDF arXiv

Risk-aware navigation in unknown environments is a fundamental challenge for autonomous vehicles operating in complex urban systems. To address this issue, this paper presents a differentiable optimization layered safety-critical control method based on conformal prediction. First, to handle uncertainties arising from sensor noise, the conformal prediction method is employed to generate risk-aware obstacle ellipsoids around an elliptical-shaped robot. Second, two nested differentiable optimization layers are introduced to build the control barrier functions for obstacle avoidance and feasibility guarantee, respectively. Then, a quadratic program based safety-critical control law is proposed to integrate the above control barrier function constraints as well as input constraints. In the end, the effectiveness of the proposed framework is demonstrated through numerical simulations.

Abstract

Explore similar work

Differentiable Optimization Layered Safety-Critical Control for Risk-Aware Navigation via Conformal Prediction

Cooperative Multi-UAV Navigation in Complex Environments via Systematic Multi-Agent Deep Reinforcement Learning

Safe and Generalizable Hierarchical Multi-Agent RL via Constraint Manifold Control