RRT$^η$: Sampling-based Motion Planning and Control from STL Specifications using Arithmetic-Geometric Mean Robustness

TL;DR

RRT is proposed, a sampling-based planning framework that integrates the Arithmetic-Geometric Mean (AGM) robustness measure to evaluate satisfaction across all time points and subformulae and synthesizes dynamically feasible control sequences satisfying STL specifications with high robustness while maintaining the probabilistic completeness and asymptotic optimality of RRT.

Abstract

Sampling-based motion planning has emerged as a powerful approach for robotics, enabling exploration of complex, high-dimensional configuration spaces. When combined with Signal Temporal Logic (STL), a temporal logic widely used for formalizing interpretable robotic tasks, these methods can address complex spatiotemporal constraints. However, traditional approaches rely on min-max robustness measures that focus only on critical time points and subformulae, creating non-smooth optimization landscapes with sharp decision boundaries that hinder efficient tree exploration. We propose RRT$^η$, a sampling-based planning framework that integrates the Arithmetic-Geometric Mean (AGM) robustness measure to evaluate satisfaction across all time points and subformulae. Our key contributions include: (1) AGM robustness interval semantics for reasoning about partial trajectories during tree construction, (2) an efficient incremental monitoring algorithm computing these intervals, and (3) enhanced Direction of Increasing Satisfaction vectors leveraging Fulfillment Priority Logic (FPL) for principled objective composition. Our framework synthesizes dynamically feasible control sequences satisfying STL specifications with high robustness while maintaining the probabilistic completeness and asymptotic optimality of RRT$^\ast$. We validate our approach on three robotic systems. A double integrator point robot, a unicycle mobile robot, and a 7-DOF robot arm, demonstrating superior performance over traditional STL robustness-based planners in multi-constraint scenarios with limited guidance signals.

Figures (3)

• Figure 1: Illustration of the Direction of Increasing Satisfaction (DIAS) for a spatial predicate $\mu = \{q \mid \|q - c\|^2 \leq r^2\}$ with center $c = [3.5, 3.5]^\top$ and radius $r = 1$. (a) The robustness landscape $\eta(q, \mu)$ showing the satisfaction region (red circle) where $\eta > 0$. (b) The gradient field $\nabla_q \eta(q,\mu)$ pointing toward the direction of increasing satisfaction. (c) The state transition field $\mathbf{J}_f(q) \cdot u$ resulting from the control input $u = [0.5, 0.5]^\top$ and discrete-time dynamics with Jacobian $\mathbf{J}_f = \mathbf{I}_2$. (d) The DIAS field $\chi_\eta(q,\mu) = \nabla\eta(q,\mu)^\top \cdot \mathbf{J}_f$, which is nonzero only in regions where $\nabla\eta(q,\mu)^\top \cdot (q_{k+1} - q_k) > 0$ (condition satisfied).
• Figure 2: Performance comparison on unicycle robot sequential reach-avoid task, demonstrating critical advantages of AGM robustness. (a-c) Traditional robustness (blue, normalized from $[-4.4] \rightarrow [-1.1]$) completely fails with negative lower bound ($\underline{\eta} \approx -0.35$), near-zero upper bound ($\overline{\eta} \approx 0.05$), and stagnant gap ($\approx 0.4$), unable to discover any satisfying trajectory. Both AGM-based methods succeed: choose-blend (red) achieves $\underline{\eta} \approx 0.93$, $\overline{\eta} \approx 0.90$, gap $\approx 0.1$; FPL (green) reaches $\underline{\eta} \approx 0.95$, $\overline{\eta} \approx 0.98$, gap $< 0.05$. The additive structure of AGM robustness enables both methods to balance constraint satisfaction across temporal phases, discovering feasible trajectories that traditional min-max semantics cannot recognize. Among AGM methods, FPL's forward prediction provides $2\times$ computational advantage, reaching convergence at 400 iterations vs. 800 for choose-blend. (d) Tree construction shows solution path (color indicates temporal progression: blue $\rightarrow$ yellow $\rightarrow$ red). The specification requires visiting Region 1 (gray, $[2.0,3.0] \times [1.0,2.0]$m) during $t \in [0,15]$, then Region 2 (orange, $[0.5,1.5] \times [2.5,3.0]$m) during $t \in [15,40]$, while avoiding obstacle region (beige, $[0.5,1.5] \times [1.0,2.0]$m) for $t \in [0,20]$. The successful upper-arc trajectory demonstrates AGM's ability to discover feasible solutions for complex temporal-spatial constraints where traditional approaches fail entirely.
• Figure 3: Performance comparison of STL-RRT* heuristics on the KUKA iiwa cascading choice problem. (a-b) Lower and upper RoSI bounds evolution over planning iterations: FPL (green) demonstrates rapid, structured convergence toward tight bounds, while traditional (blue) and choose-blend (red) methods show slower, less directed exploration. The monotonic convergence of FPL bounds indicates its ability to systematically resolve the choice dilemma by prioritizing high-robustness region pairs. (c) Gap metric quantifies the exploration efficiency—FPL reaches near-zero gap ($< 0.1$) within 1000 iterations, whereas traditional and choose-blend plateau at 1.2 and 0.8 respectively, demonstrating continued exploration of suboptimal paths. The specification $\phi_{\text{KUKA}}$ requires visiting Region A (yellow) or B (cyan) during $t \in [2,7]$s, then Region D (blue) or E (magenta) during $t \in [8,15]$s, while continuously avoiding the obstacle (black sphere) and respecting joint limits. (d-f) Ghost trail visualization from multiple viewpoints shows the reference trajectory with 10 time-sampled configurations (opacity indicates temporal progression: transparent $\rightarrow$ opaque). The robot successfully executes the A-then-E path, with red spheres marking joint positions throughout motion. The structured, collision-free trajectory demonstrates successful resolution of the cascading choice problem.

Theorems & Definitions (20)

• Example 2.1: Illustrative Example
• Definition 4.1: Arithmetics on interval semantics
• Definition 4.2: Incremental Modification Functions
• Lemma 4.1: Correctness of Incremental Modification Functions
• proof : Sketch
• Definition 4.3: Prefix, Completions
• Lemma 4.2: AGM Robustness Interval Soundness
• proof : Sketch
• Theorem 4.1: AGM Robustness Interval Chain Inclusion
• proof : Sketch