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

1. Problem Statement

The paper addresses the challenge of sampling-based motion planning for robotic systems operating under complex Signal Temporal Logic (STL) specifications. STL allows for the formalization of rich spatiotemporal constraints (e.g., "Visit region A between $t_1$ and $t_2$, then visit region B within $t_3$ units while avoiding region C").

Key Challenges Identified:

• Non-Smooth Optimization Landscapes: Traditional approaches rely on min-max robustness semantics. These metrics evaluate satisfaction based solely on the "worst-case" time point or subformula. This creates sharp decision boundaries and non-smooth optimization landscapes, causing small trajectory changes to result in abrupt shifts in robustness scores. This brittleness hinders efficient tree exploration and gradient-based steering.
• Inconsistent Guidance: Min-max approaches often ignore improvements in non-critical parts of a trajectory, providing inconsistent guidance to the planner.
• Objective Composition: Principled composition of competing temporal objectives (e.g., balancing safety vs. goal achievement) remains difficult with traditional metrics, which often fail to balance requirements adequately.

2. Methodology: The RRTη Framework

The authors propose RRTη, a sampling-based planning framework that integrates Arithmetic-Geometric Mean (AGM) robustness into the RRT* algorithm. Unlike min-max, AGM evaluates satisfaction holistically across all time points and subformulae.

Core Components:

1. AGM Robustness Semantics:

   • Instead of taking the minimum (for conjunctions) or maximum (for disjunctions) of robustness values, AGM uses geometric means when all values share the same sign (capturing compound effects) and arithmetic means otherwise.
   • This produces a smoother optimization landscape, allowing small improvements anywhere in the trajectory to contribute to the overall score, providing consistent gradient information.

2. Incremental Robustness Monitoring (IRTM):

   • Since RRT builds trajectories incrementally, the planner often deals with partial trajectories shorter than the STL formula's time horizon.
   • The authors introduce AGM robustness interval semantics $[\eta]_{st', \phi}$. This computes a sound interval bounding all possible robustness values for any completion of a partial trajectory.
   • An incremental monitoring algorithm updates these intervals efficiently ($O(|\phi|)$ per step) rather than recomputing from scratch, enabling informed decisions during tree expansion before a full path is formed.

3. Direction of Increasing AGM Satisfaction (DIAS):

   • The algorithm computes a vector field $\chi_\eta$ that guides the steering of the tree toward regions of higher robustness.
   • Composition Strategies: For Boolean operators (AND/OR), the paper proposes two methods to combine DIAS vectors from sub-formulas:
     • Stochastic Composition (Choose-Blend): Adapts previous work, using randomized selection to preserve probabilistic completeness.
     • Fulfillment Priority Logic (FPL): A novel, principled approach. It maps robustness intervals to "fulfillment values" ($f \in [0,1]$) and uses power means to compute weights. This allows the planner to dynamically prioritize less-fulfilled objectives, balancing competing constraints (e.g., safety vs. progress) more effectively than random selection.

4. Algorithm Structure:

   • Tree Construction: Samples states biased toward active STL predicates.
   • Steering: Uses the DIAS vector to guide control inputs toward the sampled state while maximizing robustness.
   • Rewiring: Unlike standard RRT* which rewires to minimize path cost, RRTη rewires to maximize the lower bound of the AGM robustness interval. This ensures that even the worst-case completion of a partial trajectory remains robust.

3. Key Contributions

1. AGM Robustness Interval Semantics: A formal framework for reasoning about partial trajectories with efficient incremental monitoring, enabling safe exploration before full paths are known.
2. Enhanced Steering with FPL: Introduction of FPL-based direction composition, which provides a mathematically grounded method for balancing multi-objective constraints by prioritizing under-satisfied goals.
3. Theoretical Guarantees: Proof that RRTη maintains the probabilistic completeness and asymptotic optimality of RRT*, despite the complex robustness measure.
4. Computational Efficiency: Demonstration that the incremental approach reduces monitoring overhead from quadratic to linear complexity relative to trajectory length.

4. Experimental Results

The framework was validated on three robotic systems: a double integrator, a unicycle robot, and a 7-DOF KUKA iiwa manipulator.

• Unicycle Robot (Sequential Reach-Avoid):

  • Scenario: Navigate through two regions while avoiding an obstacle in between.
  • Result: Traditional min-max planners failed completely (negative lower bounds, unable to find feasible paths) because the "worst-case" moment near the obstacle dominated the score.
  • RRTη Performance: Both AGM methods found high-quality solutions ($\eta \approx 0.93-0.98$). The FPL-based approach converged 2x faster than the stochastic baseline by intelligently balancing the sequential constraints.

• 7-DOF Manipulator (Cascading Choice):

  • Scenario: Visit one of two regions, then one of two other regions, while avoiding a spherical obstacle. This creates a "cascading choice" problem where early decisions constrain later feasibility.
  • Result: Traditional methods explored suboptimal paths with large gaps between upper and lower robustness bounds.
  • RRTη Performance: FPL successfully resolved the choice dilemma, prioritizing the path (Region A $\to$ Region E) that maintained maximum clearance from the obstacle.
  • Efficiency: FPL converged in ~1000 iterations (70s) vs. ~2500 for the stochastic method.
  • Optimization: The use of Inverse Kinematics (IK) based sampling with cached workspace coordinates reduced forward kinematics computations by 50x, and the overall planning time was reduced from ~180s to ~70s compared to traditional methods.

5. Significance and Impact

• Overcoming Brittleness: RRTη solves the fundamental issue of "brittle" planning caused by min-max semantics, enabling planners to navigate complex, high-dimensional spaces where traditional methods get stuck in local optima or fail to find feasible paths.
• Holistic Optimization: By considering the entire trajectory's satisfaction rather than just the critical bottleneck, the method produces smoother, more robust behaviors suitable for high-confidence autonomy.
• Principled Multi-Objective Control: The integration of FPL offers a new paradigm for handling competing temporal objectives, moving beyond heuristic or random balancing to mathematically grounded prioritization.
• Scalability: The combination of incremental monitoring and IK-based sampling makes the approach viable for high-dimensional systems (like 7-DOF arms) with complex temporal constraints.

In summary, RRTη represents a significant advancement in formal methods for robotics, bridging the gap between rigorous temporal logic specifications and efficient, robust sampling-based planning.