Introduction

This page includes information about Single-Agent guided planning. For Multi-Agent guided planning, please visit Multi-Agent Guided Planning.

Sampling based motion planning uses randomization to construct a graph or tree (roadmap) in C-space on which queries (start/goal configurations) may be solved. We explore different general purpose techniques to improve planner performance. Some techniques adapt to different inputs, bias planning via features of the environment or via the medial axis, or employ user guidance to more efficiently plan.

Workspace-guided algorithms leverage information about how the workspace is connected to provide planners with a high-level perspective. In many motion planning problems, valid solutions are highly correlated with the workspace geometry. We can construct a compact model of the workspace topology and use it as a workspace skeleton that captures all of the distinct homotopy classes in the environment. This balances the benefits of localized reasoning (sampling-based exploration) with global exploration (workspace skeleton). We have used the workspace skeleton in conjunction with dynamic sampling regions to guide single and multi robot systems in the planning space.

Dynamic Regions Planning

Dynamic Region sampling employs a workspace skeleton to model solution classes in \(C_{free}\), guide sampling, and track progress. A workspace skeleton is a deformation retract of the workspace. Its graph representation has vertices as points and edges as paths, making it a compact model of the topology.

DR-RRT: Dynamic Region RRT

Dynamic Region RRT moves sampling regions along a workspace skeleton to direct RRT growth. It focuses effort on relevant portions of the workspace and quickly discovers feasible paths.

Algorithm Overview

Initialization

  • Locate the skeleton vertices \(k_s, k_g\) nearest to the query start \(q_s\) and goal \(q_g\)
  • Prune components which are not relevant to the query (\(q_s, q_g\)).
  • Initialize sampling regions on edges outbound from \(k_s\)

Tree Growth

  • Sample RRT growth targets within the sampling regions
  • When the tree reaches a region \(r\), advance \(r\) forward along its skeleton edge
  • When \(r\) reaches the end of its skeleton edge, initialize new regions at the target vertex

Evaluation

Dynamic Region RRT was compared with three other workspace-guided planners to evaluate its efficacy.

  • Workspace Importance Sampling (WIS) analyzes decomposition cells to increase PRM sampling density in narrow passages (Kurniawati, Hsu 2004)
  • Dynamic Domain RRT shrinks the extension domain for roadmap nodes near the boundary of the free space (Yershova et. al. 2005)
  • Synergistic Combination of Layers of Planning (SyCLoP) finds a path through a decomposition graph and directs RRT grwoth along it (Plaku et. al. 2007)

The algorithms were tested on a grid maze problem with a 6 DOF rigid-body stick robot and constrained tunnels, to measure the planner’s ability to find routes through a moderately complex workspace. The results show that:

  • Dynamic Region RRT shows significantly better run time with high consistency
  • SyCLoP takes longer due to redundant exploration
  • Dynamic Domain heuristic hurts because it assumes omnidirectional obstacle space

DR-RRT Results

DR-PRM: Dynamic Regions PRM

Dynamic Region PRM evolves the Dynamic Region sampling concepts to account for the growth of multiple connected components.

Algorithm Overview

Initialization

  • Create sampling regions around each skeleton vertex
  • Sample and connect several configurations within each region to generate local components
  • Initialize sampling regions on the outbound edges for each local component.

Roadmap Expansion

  • Sample configurations \(Q\) in a region \(r\) for a local component \(C\)
  • For all \(q \in Q\) that connect to \(C\), add them to \(C\) and advance \(r\)
  • Try to merge \(C\) with components from the other side to form bridges
  • If \(C\) reaches the end of its segment without merging, spawn new outbound regions to continue expansion

Evaluation

Dynamic Regions PRM was compared with its RRT counterpart and Workspace Importance Sampling (WIS) in multi-query problems. The PRM methods build a roadmap of fixed size before answering a number of queries. Dynamic Region RRT must solve each query from scratch to illustrate the importance of multi-query methods. The algorithms were tested on a garage environment with a 6 DOF quadcopter robot and multiple levels and thin separating floors, to measure the planner’s ability to find routes through a realistic environment where the shortest valid path does not correlate to the shortest path in configuration space. The results show that:

  • Dynamic Region PRM has consistent, low path costs and competitive times
  • Dynamic Region RRT fails to find some paths in the time limit
  • The quality of the skeleton matters, and the skeleton needs to have reasonable clearance to allow roadmap expansion.

DR-PRM Results

Hierarchical Guided Planning

The hierarchical planning strategy with annotated skeleton guidance adapts planning effort based on the solution’s acceptance criteria. It improves its reliance on the skeleton as planning evolves, and takes into consideration additional information to bias its path finding efficiency.

HASPRM: Hierarchical Annotated Skeleton PRM

Planners are evaluated in terms of efficiency to find suboptimal paths and c-space coverage, and previous work on Dynamic Regions planning strategies showed that skeleton guidance is useful when it maps a significant subspace of the c-space. The skeleton can be annotated with additional information to adapt to the solution’s acceptance criteria. For example, the planner can infer collision detection based on workspace clearance.

Algorithm Overview

Initialization

The hierarchical annotated skeleton planner prioritizes finding the paths that are marked by the skeleton first, and fixing the path only when needed. Local connected components are initialized in sampling regions around the skeleton vertices. In addition, connected components in adjacent regions across the skeleton edge are connected lazily, without validation, by assuming c-space connectivity between them.

Hierarchial Path Finding

In the next step, a path is queried, and evaluated. If no valid path is found, all the partially valid ones are returned to the planner. The planner attempts fixing invalid paths, by iteratively advancing the regions from each end of the skeleton edge corresponding to the invalid edge.

Evaluation

We compared our strategy against five PRM variants including the skeleton-guided Dynamic Regions PRM. The algorithms were tested on a store environment with many narrow obstacles and traffic variation. The results showed that:

  • HASPRM’s overall planning time is improved due to the initial roadmap construction with lazy conection
  • HASPRM’s graph is sparse due to the long edges between local connected components
  • In large environments, the advantages of HASPRM to its PRM counterparts are more noticeable

This video presents more details on the implementation and experimental results presented at the 2022 IROS conference.

Watch the video Watch this video on YouTube

Publications

Updated: