Stable Sparse RRT (SST)

• Stable Sparse RRT (SST) is a sampling-based optimal motion planning algorithm that incrementally builds a sparse, directed tree using local pruning and witness sets.
• It employs efficient techniques like best-near selection and witness-based pruning to control computational complexity while ensuring near-optimality.
• Extensions such as HySST generalize the approach to hybrid dynamical systems and dynamic urban environments, improving efficiency and responsiveness.

Stable Sparse Rapidly-Exploring Random Trees (SST) is a class of sampling-based optimal motion planning algorithms that maintain sparsity and stability by means of local pruning and witness sets. Originally developed for kinodynamic planning problems, the SST paradigm has been extended to hybrid systems, as exemplified by the HySST algorithm. These methods address key challenges in motion planning—such as optimality under complex dynamics, bounded memory usage, and computational tractability—by employing witness-based pruning and best-near selection strategies. Recent work has further informed and adapted SST for dynamic urban environments, highlighting its flexibility and performance in highly structured, real-time domains.

1. Fundamental Algorithmic Structure

The core SST algorithm operates by incrementally growing a directed tree in the state–cost space. At each main iteration, three principal operations are performed: (1) Selection—choosing the “best” vertex near a randomly sampled point, (2) Propagation—generating a new state via system dynamics (flow/jump, or via sampled control), and (3) Pruning—removing or inactivating suboptimal vertices based on local cost dominance within static witness sets. Each vertex is associated with a state and an accumulated cost, while edges encapsulate trajectory segments and their control actions.

The critical innovation in SST is the use of a static witness set $S \subset \mathbb{R}^n$, with each witness $s \in S$ linked to a representative vertex $\mathrm{rep}(s)$—the lowest-cost vertex in its neighborhood ball $B_{\delta_s}(s)$. No more than one active vertex per witness region is retained, ensuring sparsity. The best-near selection uses a ball of radius $\delta_{\mathrm{BN}}$ about a sampled point, selecting the minimum-cost available vertex within this ball for propagation. The following table summarizes the essential radius parameters and their roles:

Parameter	Role	Typical Value
$\delta_{\mathrm{BN}}$	Selection (best-near) radius	$>0$ (set by design)
$\delta_s$	Pruning (witness) radius	$>0$ (set by design)

Additionally, the algorithm's pseudocode formalizes these phases and auxiliary functions (e.g., selection, pruning, state expansion, and witness assignment) (Wang et al., 2023, Smit et al., 2022).

2. Extension to Hybrid Dynamical Systems: HySST

HySST generalizes SST to systems exhibiting both continuous evolutions (flows) and discrete transitions (jumps). In HySST, the tree vertices encode both state and cost, and the propagation phase randomly chooses between a flow input from a set $\mathcal{U}_C$ or a jump input $\mathcal{U}_D$, simulating the system until the next regime switch or collision event.

A fundamental notion is the concatenation of trajectories defined on hybrid time domains. If two solution-pairs $(\phi_1, u_1)$, $(\phi_2, u_2)$ are defined on compatible domains, their concatenation $\phi_1|\phi_2$ forms a contiguous hybrid trajectory. The cost function $c$ is required to be additive over concatenation, Lipschitz, monotonic, and non-degenerate, which is critical for asymptotic approximation arguments (Wang et al., 2023).

Neighborhoods and witness sets are managed similarly as in classical SST, with the static witness property preserved: once a witness is created, its location remains fixed and only the minimal-cost representative is updated.

3. Theoretical Properties and Near-Optimality

The principal asymptotic guarantee is asymptotic near-optimality: Given mild assumptions on the cost function, system regularity, uniform sampling, and witness parameters, the probability that the algorithm produces a trajectory $\psi$ with $c(\psi) \leq (1+\alpha\delta)\, c(\psi^*)$ approaches one as the number of iterations increases, where $\psi^*$ denotes a cost-optimal trajectory and $\delta$ depends on safety and dynamics clearances:

$$\delta_{\mathrm{BN}} + 2\delta_s < \delta = \min\{\delta_s, \delta_f\}$$

Here, $\delta_f$ is an inflation parameter guaranteeing positive clearance from critical boundaries (Wang et al., 2023). No positive clearance is required between $\psi^*$ and the boundary of the continuous/discrete domains, which relaxes previous assumptions prevalent in classical RRT-based planning. These properties remain under extensions to domain-informed SST, provided the reachability of valid states is not impaired (Smit et al., 2022).

4. Witness Sets, Sparsity, and Pruning

The principal mechanism ensuring bounded computational complexity is witness-based pruning. At every candidate addition, the nearest witness is identified. If the proposed state lies outside any existing ball, a new witness is created. If it lies within, only lower-cost states are retained; previous representatives and their dominated subtrees are marked inactive or pruned as appropriate. The upshot is an $O(\mathrm{Vol}(\mathbb{R}^n)/\delta_s^n)$ upper bound on the number of active vertices, preventing unbounded memory and computational growth.

Formally:

• For every vertex $v$, $s_{\text{near}} = \arg\min_{s \in S} \| x_v - s \|$.
• If $\| x_v - s_{\text{near}} \| > \delta_s$, add a new witness and assign $v$ as its representative.
• Otherwise, retain $v$ only if $c_v < c_{\mathrm{rep}(s_{\text{near}})}$; else, $v$ is rejected.

This rigorously controls the size and structure of the search tree, enabling tractable nearest-neighbor and best-near queries even in high dimensions (Wang et al., 2023, Smit et al., 2022).

5. Adaptations for Domain Knowledge and Dynamic Environments

Recent extensions incorporate domain knowledge to improve convergence and robustness in environments such as autonomous urban driving. Strategies include:

• Road-layout branches: Initial exploration along high-value regions such as lane centers, parameterized by waypoints, accelerates convergence by biasing the tree growth toward desirable trajectories.
• Warm-start branches: Reinserting states from previous (valid) solutions, when within a similarity threshold, allows rapid adaptation and reuse of past computations.
• Accounting for dynamic obstacles: Predicted occupancies of moving agents are enforced via state and edge validity checks, as well as additional trajectory cost terms penalizing spatial–temporal proximity to predicted positions (Smit et al., 2022).

Empirical results demonstrate that these informed extensions preserve completeness and asymptotic properties while significantly reducing required computation and motion cost in challenging scenarios.

6. Computational Complexity and Practical Performance

Bounding the number of active vertices per $\delta_s$-ball yields favorable computational complexity. For example, in a 2D actuated bouncing ball scenario, HySST constructed 154 active and 35 inactive vertices in approximately 3.3 seconds, compared to $\sim$660 vertices and 18.4 seconds for standard hybrid-RRT. In a 6D tensegrity multicopter application, HySST built $\sim$2,094 active vertices in 54.7 seconds and found solutions exploiting environment interactions to minimize overall hybrid time (Wang et al., 2023).

In autonomous driving, extensions to SST (e.g., DKI-SST) achieved up to 4-fold reduction in acceleration demands and 20-fold reduction in velocity-tracking error relative to the uninformed baseline, with all trajectories maintaining safety constraints and road bounds (Smit et al., 2022).

7. Key Applications and Research Directions

Stable Sparse RRT and its variants have been applied to high-dimensional hybrid systems, kinodynamic planning for robotics, and real-time trajectory planning in dynamic urban environments. The witness paradigm and pruning framework enable scalability to complex, nonlinear domains. Research has investigated various informed sampling and branch seeding strategies, integration of predictive models for obstacle avoidance, and parameter selection for tradeoffs between sparsity and solution quality.

The source code for HySST is available from the authors’ repository: https://github.com/HybridSystemsLab/hybridSST (Wang et al., 2023).

• "HySST: A Stable Sparse Rapidly-Exploring Random Trees Optimal Motion Planning Algorithm for Hybrid Dynamical Systems" (Wang et al., 2023)
• "Informed sampling-based trajectory planner for automated driving in dynamic urban environments" (Smit et al., 2022)