SAFE--MA--RRT: Data-Driven Multi-Agent Planning

• The paper introduces SAFE--MA--RRT, a data-driven multi-agent motion planning framework that computes local invariant ellipsoids via convex SDPs from experimental input–state data.
• It employs grid-based discretization with overlapping ellipsoidal safety certificates to ensure that each transition is dynamically feasible and passes strict local invariance tests.
• Multi-agent coordination is achieved through a global space–time reservation table that prevents simultaneous occupancy and head-on collisions, ensuring safe, certified paths.

SAFE--MA--RRT is a fully data-driven, multi-agent motion planning framework that synthesizes synchronized, dynamically feasible, and provably safe trajectories for homogeneous linear agents navigating shared, obstacle-filled workspaces, even in the absence of explicit system models. The approach centers on the computation of local invariant ellipsoids and state-feedback gains at grid-based waypoints using convex semidefinite programs (SDPs) derived directly from experimental input–state data. These ellipsoidal safety certificates govern edge validity and controller switching, while a sampling-based planner—rapidly exploring the waypoint lattice—assembles agent trajectories as sequences of overlapping invariants. Multi-agent coordination and collision avoidance are achieved through a global space–time reservation table that prevents simultaneous occupancy and head-on collisions, with all executed paths certified for dynamic feasibility and local safety.

1. Data-Driven Safety Certification

At the heart of SAFE--MA--RRT is the synthesis of local safety certificates for each agent purely from input–state trajectory data, circumventing reliance on explicit system matrices. For a linear system, each agent collects input and output data triples: $U_0$, $X_0$, $X_1$, $Y_0$, typically by executing a short closed-loop trajectory. From these, local convex SDPs compute for every grid-based waypoint:

• An ellipsoidal invariant set $$E(P, c) = \{ x \mid (x - c)^T P^{-1} (x - c) \leq 1 \}$$, where $P \succ 0$ is to be determined,
• A corresponding state-feedback matrix $K = U_0 S P^{-1}$, with $S$ as a lifting variable related to $P$.

The SDP maximizes $-\log\det(P)$ subject to contractivity and box constraints, ensuring that under control $K$, the closed-loop dynamics keep the agent's state inside $E(P, c)$ for short-range transitions. These certificates serve as local safety guarantees that, under agent execution, preclude excursions into forbidden regions or violation of imposed state/output constraints.

2. Ellipsoidal Overlap and Invariant-to-Invariant Transitions

For planning, the workspace is discretized into a grid, and for each cell (typically its center) an invariant ellipsoid is certified. A tree edge (i.e., a candidate move from cell $c_\text{parent}$ to cell $c_\text{child}$) is added only if:

• The local ellipsoid at $c_\text{child}$ is contractive and lies within the axis-aligned box formed by the union of the two cells (i.e., within state constraints and avoiding obstacles),
• The ellipsoid projected into the output space via $C$ overlaps with the parent’s ellipsoid: for overlap, the center $c_\text{mid}$ of the shared cell must satisfy $$(c_\text{mid} - r)^T(P_\text{proj})^{-1}(c_\text{mid} - r) \leq 1$$ for both certificates.

This overlap ensures that, during execution, an agent can switch its controller at cell $c_\text{child}$ without compromising invariance; the system state is simultaneously contained within both certificates. Consequently, the tree represents a concatenation of controller-invariant segments, guaranteeing uninterrupted safety at each step.

3. Convex Semidefinite Programming for Certificate Synthesis

The SDP for each local edge operates over the data matrices and a contractivity factor $\lambda \in (0, 1]$:

$$\begin{aligned} &\text{maximize}~-\log\det(P) \ &\text{subject to:} \ &\begin{bmatrix} P & X_1 S \ S^T X_1^T & \lambda P \end{bmatrix} \succeq 0, \ &\begin{bmatrix} P & P F^T \ * & g^2 \end{bmatrix} \succeq 0~\text{for each polytope facet} \ & X_0 S = P \ & U_0 S P^{-1} = K \end{aligned}$$

where $F$ encodes the box constraints of the cell pair, and $g$ is the allowable bound per facet. Once solved, the SDP provides both the invariant matrix $P$ and controller $K$ for the edge, with contractivity encoding forward invariance. Every feasible tree edge receives its own controller, eliminating the need for online optimization during agent execution.

4. Sampling-Based Tree Planning and Grid Discretization

The planner employs a rapid, goal-biased sampling procedure:

• Random samples are snapped to the closest grid cell,
• The nearest tree vertex is located,
• Possible neighbor cells (N, S, E, W) are considered for extension,
• For each, the axis-aligned rectangle is constructed and the associated SDP safety certificate is computed,
• Edges are accepted only if both the box containment and the overlap tests are passed.

The tree grows incrementally, always certifying edge safety, until a path from start to goal is found. Grid-based discretization makes certificate synthesis tractable and ensures that each transition can be efficiently analyzed for contracted invariance.

5. Multi-Agent Coordination via Space–Time Reservation Table

Multi-agent synchronization is governed by a space–time reservation table. Each tree expansion step for each agent is mapped to a discrete time layer:

• Each agent can reserve one cell per time layer,
• Simultaneous occupancy and head-on swaps are forbidden; attempted conflicts are detected and resolved via pre-defined tie-breakers,
• Edges that would violate reservations are rejected.

During planning, all agents expand their trees in synchrony, always checking the reservation table before accepting an edge. This guarantees that no two agents plan to occupy the same space at the same time, and that coordinated collision-free motion is enforced at the global level, complementing the local invariance provided by certificates.

6. Simulation Studies and Comparative Safety Analysis

In simulation (using linearized Clohessy–Wiltshire spacecraft rendezvous dynamics), the effectiveness of SAFE--MA--RRT is demonstrated for both single-agent and multi-agent scenarios:

• Single-agent tests show trajectories as sequences of magenta ellipsoids (i.e., certified safe regions) that successfully guide the agent around workspace debris.
• Two-agent studies display synchronous tree expansion with no reservations violated and no crossover in safety certificates.
• Comparative analysis against LQR-based baseline RRT (which uses data-derived LQR controllers without explicit safety certificate checks) reveals the SAFE--MA--RRT approach achieves zero safety violations (the state always remains inside certified ellipsoids), while the baseline exhibits multiple excursions outside contractivity bounds.
• The framework certifies both environmental safety and dynamic feasibility over all planned motion segments.

7. Mathematical Notation and Certificate Validity Conditions

• Ellipsoidal invariant: $$E(P, c) = \{ x \in \mathbb{R}^n \mid (x - c)^T P^{-1} (x - c) \leq 1 \}$$
• Contractive error dynamics: $$e(k+1)^T P^{-1} e(k+1) \leq \lambda e(k)^T P^{-1} e(k)$$
• Projected ellipsoid for output safety: $P_\text{proj}^{-1} = C P^{-1} C^T$
• Overlap test: $$(c_\text{mid} - r)^T P_\text{proj}^{-1} (c_\text{mid} - r) \leq 1$$ for both certificates.

The contractivity and overlap conditions ensure that safety and dynamic feasibility are retained for every controller switch along the planned trajectory.

---

SAFE--MA--RRT establishes a formal bridge between data-driven control synthesis and scalable sampling-based motion planning, enabling multiple homogeneous agents to safely and synchronously navigate obstacle-rich workspaces via only experimental data and convex optimization. It is characterized by locally invariant safety guarantees, decentralized data-driven feedback controllers anchored to grid waypoints, global space–time collision prevention, and efficient certification of all motion segments, with comparative studies affirming its robust safety profile against alternative approaches (Esmaeili et al., 4 Sep 2025).