Stable Sparse RRT (SST)

• SST is a sampling-based motion planning algorithm that uses a fixed witness set to sparsely represent the state space and prune high-cost trajectories.
• It employs a best-near selection and extension procedure to efficiently grow the search tree while ensuring asymptotic near-optimality under mild conditions.
• HySST extends the approach to hybrid systems by integrating continuous flows and discrete jumps, optimizing motion planning in complex dynamic environments.

Stable Sparse Rapidly-Exploring Random Trees (SST) are advanced sampling-based motion planning algorithms designed for optimal and efficient path finding in high-dimensional state spaces, including those governed by hybrid dynamical systems. Unlike classical RRT and RRT* approaches, SST introduces a sparsification mechanism via a static set of witness points to prune high-cost trajectories, achieving provable asymptotic near-optimality under mild conditions and avoiding excessive tree growth as solutions are refined. Its extension to hybrid dynamics, known as HySST, randomly integrates both continuous (flow) and discrete (jump) state transitions, accommodating systems with mode switches and complex behaviors (Wang et al., 2023).

1. Data Structures and Key Notation

SST operates on a search tree $\mathcal{T} = (V, E)$, where:

• $V \subset \mathcal{X} \subseteq \mathbb{R}^n$ is the finite set of tree vertices, each representing a system state $x_v$ and cost-to-come $\hat{c}_v = c(\sigma_v)$ for the trajectory segment $\sigma_v$ concatenated from root to $v$.
• $E \subset V \times V$ are directed edges denoting feasible state transitions determined by dynamic constraints.

A central feature is the fixed witness set $W \subset \mathbb{R}^n$, with each witness $w$ associated to a single current representative $\mathrm{rep}(w) \in V$. The witness set enforces sparsity: all vertices lie within radius $\delta_s$ of some witness, and only the lowest-cost vertex in each witness neighborhood ball $B(w, \delta_s)$ is retained as active. Vertices are partitioned into $V_\text{active}$ (eligible for extension) and $V_\text{inactive}$ (preserving tree connectivity but not eligible for further growth).

2. Algorithm Workflow and Pseudocode

The SST (and HySST) algorithm proceeds as follows:

1. Initialization
   • Sample start states $X_0$; initialize the tree with root vertices $v_0$ for each $x_0 \in X_0$, set $\hat{c}_{v_0}=0$, and add witnesses accordingly.
2. Main Loop (Iteration for $k = 1 \ldots K$)
   • Sampling: Draw random sample $x_\text{rand}$ from free state space (for HySST, may sample discrete jump states with probability $p$).
   • Best-Near Selection: Identify candidate vertices within radius $\delta_{BN}$ of $x_\text{rand}$, pick the one with minimal cost-to-come ($\hat{c}_v$) as $v_\text{near}$. If none, select nearest $v_\text{near}$ by Euclidean distance.
   • Extend: Apply random control input to $x_{v_\text{near}}$, producing a new trajectory segment $(\sigma, x_\text{new}, \Delta c)$. Update cost: $$\text{cost}_\text{new} = \hat{c}_{v_\text{near}} + \Delta c$$.
   • Prune-and-Add:
     • Find nearest witness $w^*$ to $x_\text{new}$.
     • If $||x_\text{new} - w^*|| > \delta_s$, create new witness and associate a new active vertex.
     • Else if $$\text{cost}_\text{new} < \hat{c}_{\mathrm{rep}(w^*)}$$, update the representative, move previous one to inactive, and recursively prune leaves.
     • Otherwise, discard $x_\text{new}$.

A candidate solution is extracted by searching for goal-reaching vertex $v_{\text{goal}}$ and reconstructing its trajectory.

3. Formal Properties and Theoretical Guarantees

• Cost-to-Come: For $v$ with parent $u$ and edge $e=(u, v)$, $\hat{c}_v = \hat{c}_u + c(\sigma_e)$; by induction, this accumulates total cost for the trajectory leading to $v$.
• Witness Sparsity: For each $w \in W$, the ball $B(w, \delta_s)$ covers all vertices, and distinct witnesses maintain mutual separation of at least $\delta_s$.
• Asymptotic Near-Optimality (Main Theorem): Under assumptions of Lipschitz dynamics, additive cost, clearance $\delta > 0$ along the optimal plan, and parameter constraint $\delta_{BN} + 2\delta_s < \delta$, for any $\epsilon > 0$,

$$\lim_{K \to \infty} P\left(\min\{\hat{c}_v: x_v \in X_f\} \leq c^* + \epsilon\right) = 1,$$

where $c^*$ is the infimal cost among all feasible plans (Wang et al., 2023).

4. Proof Techniques and Underlying Principles

Established proof strategies utilize:

• Chain of overlapping balls of radius $\delta$ along an optimal plan $\varphi^*$ exploiting positive clearance.
• Inductive arguments show that, given an active vertex within the $i$th ball, there is fixed probability for extension into the next ball, leveraging uniform sampling, Lipschitz continuity, and the extend procedure.
• Stability is maintained since pruning only discards inferior-cost vertices; best-cost representatives persist throughout search.
• Markov chain and Borel-Cantelli lemma arguments guarantee—over infinite iterations—the discovery of near-optimal solutions.

A plausible implication is that the static pruning reduces unnecessary branches, focusing computational effort on promising trajectories.

5. Computational Complexity and Parameter Selection

Per iteration, SST requires:

• Nearest neighbor search in $V_\text{active}$ within $\delta_{BN}$ (naively $O(|V_\text{active}|)$, improved via spatial indexing such as KD-trees).
• Nearest witness search ($O(|W|)$), generally tractable as $|W| \ll$ total samples.
• Simulation cost for the Extend procedure.

Memory complexity scales as $O(|W| + |V_\text{active}| + |V_\text{inactive}|)$, with $|V_\text{active}|$ typically bounded by the packing number of $\mathcal{X}$ at resolution $\delta_s$.

Parameter tuning:

• $\delta_s$ (witness radius): Controls sparsity; smaller values yield finer approximation, increased vertex count.
• $\delta_{BN}$ (best-near radius): Regulates exploration versus exploitation; too small impedes growth, too large risks suboptimal branches.
• The condition $\delta_{BN} + 2\delta_s < \delta_\text{opt}$ (optimal clearance) is required for convergence guarantees. Practically, $\delta_s \sim 1$–$5\%$ of state space diameter and $\delta_{BN} \lesssim 2\delta_s$ are recommended.

6. Applications to Hybrid Dynamical Systems

HySST generalizes SST to hybrid systems with both flow (continuous evolution) and jump (discrete transitions) regimes. At each extension, the algorithm may randomly select flow or jump, dynamically growing the tree across hybrid state spaces.

• Actuated Bouncing Ball: State $$(\text{height, velocity, auxiliary time } \tau, \text{jump-count } k)$$; cost function is $\tau + k$. HySST identifies single-bounce optimal plans with $\sim 200$ vertices, outperforming unpruned HyRRT, which requires $\sim 600$ (Wang et al., 2023).
• Collision-Resilient Tensegrity Multicopter: State includes position, velocity, acceleration, with jumps modeling wall collisions. HySST discovers wall-assisted shortening of flight time while controlling tree growth in high (6D) state dimensions.

Summary Table: Example Domains and HySST Features

Application Domain	Hybrid Elements	HySST Performance
Actuated Bouncing Ball	Flow & Jump	Single-bounce plan, O(200) vertices
Tensegrity Multicopter	Flow & Collision	Efficient bounce exploitation in 6D

7. Context and Related Research

SST and HySST are grounded in the theoretical foundation of sampling-based kinodynamic planning as discussed by Li, Littlefield, and Bekris (IJRR 2016). The sparsification and pruning principles adapted to hybrid systems by Wang and Sanfelice (UCSC TR-HSL-02-2023) broaden their applicability to real-world robotic domains where discrete transitions and nontrivial cost landscapes are present.

This suggests future work may further generalize SST to richer hybrid systems or integrate adaptive sparsification, but all concrete claims strictly trace to the above sources (Wang et al., 2023).