Greedy Heuristics for Sampling-Based Motion Planning in High-Dimensional State Spaces

TL;DR

This work introduces the greedy informed set, defined by $\mathcal{X}_{greedy}=\{\mathbf{x}\in\mathcal{X}_{free}\mid \hat{f}(\mathbf{x})\le \hat{f}(\mathbf{x}_{\max})\}$ where $\hat{f}$ is the $L^2$ heuristic and $\mathbf{x}_{\max}$ is the maximum-cost state along the current solution. Building on this, Greedy RRT* (G-RRT*) is proposed as a bidirectional, anytime planner that alternates sampling from the greedy informed set and the full informed set, controlled by a bias parameter $\epsilon$, to rapidly find initial solutions and converge to optimal paths. The authors prove probabilistic completeness and characterize asymptotic optimality under mixed sampling, deriving worst-case sample-cost impacts and recall/volume bounds. Empirical results on abstract benchmarks and robotic manipulation tasks show that G-RRT* achieves faster initial solutions and competitive final path costs compared to state-of-the-art planners, particularly in high-dimensional or tightly constrained settings. Overall, the greedy informed sampling approach substantially accelerates convergence in informed planning while preserving global optimality under appropriate exploration/exploitation balance.

Abstract

Informed sampling techniques accelerate the convergence of sampling-based motion planners by biasing sampling toward regions of the state space that are most likely to yield better solutions. However, when the current solution path contains redundant or tortuous segments, the resulting informed subset may remain unnecessarily large, slowing convergence. Our prior work addressed this issue by introducing the greedy informed set, which reduces the sampling region based on the maximum heuristic cost along the current solution path. In this article, we formally characterize the behavior of the greedy informed set within Rapidly-exploring Random Tree (RRT*)-like planners and analyze how greedy sampling affects exploration and asymptotic optimality. We then present Greedy RRT* (G-RRT*), a bi-directional anytime variant of RRT* that leverages the greedy informed set to focus sampling in the most promising regions of the search space. Experiments on abstract planning benchmarks, manipulation tasks from the MotionBenchMaker dataset, and a dual-arm Barrett WAM problem demonstrate that G-RRT* rapidly finds initial solutions and converges asymptotically to optimal paths, outperforming state-of-the-art sampling-based planners.

Figures (11)

• Figure 1: Comparison of informed set sizes for a planning problem with a tortuous solution path (yellow) from the start (red) to the goal (green). The $L^2$ informed set (left) is defined by the current solution cost and covers a large ellipsoidal region. In contrast, the $L^2$greedy informed set (right) is defined using the state with the maximum admissible heuristic along the current solution path, substantially reducing the ellipsoidal area.
• Figure 2: Illustration of the $L^2$ greedy informed set ${ \mathcal{X}\IfNoValueTF{\mathrm{greedy}}{}{_{\mathrm{greedy}}}}$ in a planning problem with a path-length objective in $\mathbb{R}^2$. The greedy subset (dashed ellipse) is defined by the hypothetical minimum cost $c_{\text{min}}$ from the initial state $\mathbf{x}_I$ to the goal state $\mathbf{x}_G$, and by the heuristic cost of the state along the solution path with the highest admissible value ${\Hat{f}(\mathbf{x}_{\text{max}})}$, used as a transverse diameter.
• Figure 3: Illustration of the current solution path $\pi$ and another path $\pi^*$ within the same homotopy class, sharing the same initial and goal states, $\mathbf{x}_I$ and $\mathbf{x}_G$. The greedy informed set $\mathcal{X}\IfNoValueTF{\mathrm{greedy}}{}{_{\mathrm{greedy}}}$ (dashed gray ellipse) is constructed using the maximum heuristic cost along $\pi$. The path $\pi^*$ intersects the boundary of $\mathcal{X}\IfNoValueTF{\mathrm{greedy}}{}{_{\mathrm{greedy}}}$ at $\mathbf{x}_a$ and $\mathbf{x}_b$ and passes through a state $\mathbf{x}_c$ that lies outside this set.
• Figure 4: Illustration of the suboptimality of the greedy informed set in an example planning scenario. The greedy informed set $\mathcal{X}\IfNoValueTF{\mathrm{greedy}}{}{_{\mathrm{greedy}}}$, constructed from the current tortuous solution path $\pi$, is shown as a dashed ellipse and the optimal path $\pi^{*}$ as a dotted line. In this case, $\mathcal{X}\IfNoValueTF{\mathrm{greedy}}{}{_{\mathrm{greedy}}}$ fails to include some states that could improve the current solution cost (i.e., those leading towards $\pi^{*}$ and lying outside of the hyperellipsoid) due to the nature of its greedy exploitation.
• Figure 5: Illustrations of the progress of bi-directional sampling-based search performed by the G-RRT* algorithm. The initial and goal states are shown as large black dots, sampled states as small black dots, and the start and goal trees in blue and orange, respectively. The current solution path is highlighted in yellow, and the $L^2$ greedy informed set---the set of states that could yield better solutions---is shown with gray dashed lines. G-RRT* grows two trees rooted at the start and goal (a), where expansion is guided by a greedy connection heuristic that guides the two trees towards each other, producing an initial solution (b). Subsequent sampling is then focused within the greedy informed set to incrementally refine the path (c--d), almost-surely asymptotically converging to the optimal solution.

Theorems & Definitions (10)

• Definition 1: Feasible path planning problem
• Definition 2: Optimal path planning problem
• Definition 3: $L^2$ greedy informed set
• Theorem 1: Inclusion of the optimal path in the greedy informed set
• proof
• Remark 1: Non-inclusion of the optimal path in the greedy informed set
• Theorem 2: Worst-case sample complexity for probabilistic optimality
• proof
• Theorem 3: Expected sample complexity under mixed greedy sampling
• proof