SAFE--MA--RRT: Multi-Agent Motion Planning with Data-Driven Safety Certificates (2509.04413v1)

Abstract: This paper proposes a fully data-driven motion-planning framework for homogeneous linear multi-agent systems that operate in shared, obstacle-filled workspaces without access to explicit system models. Each agent independently learns its closed-loop behavior from experimental data by solving convex semidefinite programs that generate locally invariant ellipsoids and corresponding state-feedback gains. These ellipsoids, centered along grid-based waypoints, certify the dynamic feasibility of short-range transitions and define safe regions of operation. A sampling-based planner constructs a tree of such waypoints, where transitions are allowed only when adjacent ellipsoids overlap, ensuring invariant-to-invariant transitions and continuous safety. All agents expand their trees simultaneously and are coordinated through a space-time reservation table that guarantees inter-agent safety by preventing simultaneous occupancy and head-on collisions. Each successful edge in the tree is equipped with its own local controller, enabling execution without re-solving optimization problems at runtime. The resulting trajectories are not only dynamically feasible but also provably safe with respect to both environmental constraints and inter-agent collisions. Simulation results demonstrate the effectiveness of the approach in synthesizing synchronized, safe trajectories for multiple agents under shared dynamics and constraints, using only data and convex optimization tools.

• The paper introduces a framework that synthesizes per-edge invariant ellipsoids via data-driven semidefinite programs to ensure safe and dynamically feasible trajectories.
• It integrates RRT-based sampling with space–time reservations, enabling coordinated multi-agent planning in obstacle-rich environments while avoiding collisions.
• Simulation results demonstrate zero safety violations compared to a baseline, highlighting the framework's potential for real-time, robust deployment.

SAFE--MA--RRT: Multi-Agent Motion Planning with Data-Driven Safety Certificates

Introduction and Motivation

The paper introduces SAFE--MA--RRT, a data-driven multi-agent motion planning framework for homogeneous linear systems operating in shared, obstacle-rich workspaces without explicit model knowledge. The approach leverages experimental trajectory data to synthesize local invariant ellipsoids and corresponding state-feedback gains via convex semidefinite programming, certifying dynamic feasibility and safety for each agent's motion segment. The framework integrates sampling-based planning (RRT) with data-driven control, ensuring that each agent's trajectory is both dynamically feasible and provably safe with respect to workspace constraints and inter-agent collisions.

Data-Driven Safety Certification

The core technical contribution is the use of data-driven semidefinite programs (SDPs) to construct locally invariant, contractive ellipsoids for each agent at every waypoint. These ellipsoids are computed directly from measured state-input trajectories, bypassing the need for explicit system identification. For each candidate waypoint, the SDP maximizes the volume of the contractive ellipsoid subject to containment within the admissible polytope and contractivity under the closed-loop dynamics induced by the data-driven feedback gain. The resulting ellipsoid certifies that the agent can safely transition between waypoints while respecting state and input constraints.

Single-Agent Safe RRT

In the single-agent setting, the planner discretizes the workspace into a grid and grows a tree by proposing one-cell moves toward sampled targets. Each proposed move is accepted only if the SDP returns a feasible contractive ellipsoid that overlaps with the parent ellipsoid at the shared cell center, ensuring safe controller switching and continuous constraint satisfaction. The per-edge feedback gains and ellipsoids are stored for execution, enabling model-free rollout with guaranteed safety.

Figure 1: Single-agent Safe–RRT in the $x$–$y$ plane. (a) Magenta ellipses are contractive certificates covering each edge of the planned path. Overlap of consecutive ellipsoids guarantees safe controller switching and constraint satisfaction while avoiding debris. (b) The red dashed line shows the planned waypoint path, while the black solid line shows the executed closed-loop trajectory under per-edge affine state-feedback gains, verifying feasibility under system dynamics.

Multi-Agent Coordination and Space–Time Safety

The multi-agent extension synchronizes tree expansion across all agents using a global space–time reservation table. Each agent proposes one-cell moves at each time layer, certifies them via the SDP, and checks for overlap with its parent ellipsoid. Proposals are accepted only if they do not conflict with other agents' reservations or cause head-on swaps. This mechanism guarantees inter-agent separation and collision avoidance at every time step. The reservation table explicitly tracks the active invariant ellipsoids, ensuring that inter-agent safety is maintained throughout execution.

Figure 2: Two-agent SAFE--MA--RRT: exploration and safety certificates.

Figure 3: Two-agent SAFE--MA--RRT: planned waypoint paths. Each edge between adjacent cells is certified by a contractive ellipsoid; paths are shown without state traces for clarity.

Simulation Results and Comparative Analysis

The framework is evaluated on a spacecraft rendezvous scenario using the linearized Clohessy–Wiltshire model. In the single-agent case, the planner successfully synthesizes a safe trajectory that avoids debris and respects workspace constraints. In the two-agent scenario, both agents reach their goals via coordinated, safe paths, with all motion segments certified by disjoint contractive ellipsoids in space–time. The SAFE--MA--RRT approach is compared to a data-driven LQR–RRT baseline, which, while feasible, fails to guarantee safety: the LQR–RRT exhibits constraint violations in 15.6% and 6.7% of layers for the two agents, respectively, whereas SAFE--MA--RRT maintains zero violations.

Figure 4: Two-agent spacecraft rendezvous: comparison of executed closed-loop trajectories. (a) The proposed Safe--RRT ensures trajectories remain within workspace constraints and respect space–time reservations. (b) The baseline LQR--RRT produces feasible paths but lacks certified safety guarantees.

Theoretical and Practical Implications

The SAFE--MA--RRT framework demonstrates that data-driven invariant-set certification can be integrated with sampling-based planning to yield provably safe, dynamically feasible multi-agent trajectories without explicit model knowledge. The use of per-edge feedback gains and contractive ellipsoids enables local adaptation to workspace constraints and agent interactions, while the space–time reservation mechanism provides scalable coordination. The approach is robust to model uncertainty and is applicable to large teams of homogeneous agents.

Strong numerical results include zero safety violations in all tested multi-agent scenarios, in contrast to the LQR–RRT baseline. The framework's reliance on convex optimization and data-driven synthesis makes it computationally tractable and amenable to real-time deployment, provided sufficient informative data is available for each agent.

Limitations and Future Directions

The current formulation assumes homogeneous linear dynamics and full state measurement. Extension to heterogeneous or nonlinear systems would require new data-driven certificate constructions, potentially leveraging recent advances in nonlinear system identification and robust control. The framework also assumes deterministic dynamics; future work will address probabilistic safety guarantees under process and measurement noise, possibly via chance-constrained SDPs or tube-based invariance.

Conclusion

SAFE--MA--RRT provides a principled, data-driven solution to multi-agent motion planning under safety constraints, synthesizing per-edge invariant certificates and feedback gains directly from trajectory data. The integration of space–time reservations and local safety certificates yields coordinated, dynamically feasible, and provably safe trajectories for all agents. The approach bridges sampling-based planning and data-driven control, with strong empirical performance and clear avenues for extension to more complex and uncertain multi-agent systems.