computational robotics

from tasks to motions

Path Planning in Configuration Spaces:

Sampling-based Roadmap of Trees (SRT)

The objective in path planning is to compute a path that enables a robot to move from an initial to a goal configuration while avoiding collisions with obstacles. Among the various approaches proposed over the years, sampling-based path planners have been quite successful is solving challenging path-planning problems. Probabilistic RoadMap (PRM) [Kavraki et. al.] planners sample numerous collision-free configurations and connect neighboring samples via collision-free paths to construct a roadmap that captures the connectivity of the configuration space. Tree-based planners, e.g., Rapidly-exploring Random Tree (RRT) [LaValle, Kuffner], Expansive-Space Tree (EST) [Hsu et. al.], expand a tree rooted at the initial configuration by adding new collision-free paths as tree branches.

SRT was developed to deal with high-dimensional path-planning problems, which often arise in planning with multiple robots, flexible objects, reconfigurable robots, and manipulation planning. When dealing with high-dimensional problems, it has been observed that sampling-based path planners that construct a roadmap, a single tree, or a bidirectional tree could not scale well in such cases. Drawing from the success that multi-tree searches had shown in discrete spaces in AI, SRT was developed to explore the continuous configuration space by constructing multiple trees that were connected with simple collision-free paths. In particular, SRT creates a roadmap of trees that integrates the global sampling properties of PRM with the local sampling properties of tree-based planners, such as RRT and EST. SRT is shown to be significantly faster and more robust than PRM and the tree-based planners it uses. The construction and connection of multiple trees also provides a natural framework for a large-scale distribution based on asynchronous communication, yielding near linear speedup on hundreds of CPUs, see DSRT

Algorithm Details

SRT Roadmap Construction Algorithm -- Pseudocode

Input:  K, number of trees
        N_c, number of nearest neighbors
	N_r, number of random neighbors 
Output: A roadmap G = (V, E)

Tree Computations
 1. V <-- empty set; E <-- empty set.
 2. while cardinality(V) < K do
 3.   s <-- sample collision-free configuration at random
 4.   T <-- build tree rooted at s
 5.   add the tree T to the vertex set V.

Candidate Edge Computations
 6. E_c <-- empty set.
 7. for all T in V do
 8.   S_close  <-- the N_c closest trees in V to T.
 9.   S_random <-- a set of N_r random trees in V.
10.   add to E_c all the edges (T, T') for T' in S_close or T' in S_random.

Edge Computations
11. for all candidate edges (T', T'') in E_c do
12.   if (not Connected(G, T', T'')) and Connect(T', T'')
13.     add the edge (T', T'') to E.

Tree computations: In SRT, the trees of the roadmap are computed by sampling their roots uniformly at random in the free configuration space and then using a sampling-based tree planner to explore the region around the root configuration.

Candidate edges: Neighboring trees are considered as candidate edges for connections, where a distance metric defines closeness between trees. Random neighbors are also used to offset any problems with the distance metric.

Tree connections: Connections between trees are computed by a sampling-based tree planner. For each candidate edge (T', T''), a small number of close pairs of configurations of T' and T'' are quickly checked with a fast local planner. If any local path is found, no further computation takes place. Otherwise, a more complex tree-connection algorithm is executed, e.g., bi-directional RRT or EST. During the tree connection, additional configurations are typically added to the trees T' and T''. If the tree-planner is successful, the edge (T', T'') is added to the roadmap and the graph components to which T' and T'' belong are merged into one.

Solve queries: Queries are solved by rooting two trees at the initial and goal configurations and then connecting the trees to the neighboring trees in the roadmap. A path is found if at any point the initial and goal tree lie on the same connected component of the roadmap.

Related Publications

  • Plaku E, Bekris KE, Chen BY, Ladd AM, and Kavraki LE (2005): "Sampling-Based Roadmap of Trees for Parallel Motion Planning." IEEE Transactions on Robotics, 2005, vol. 21(4), pp. 587--608   [publisher]   [preprint]
  • Plaku E and Kavraki LE (2005): "Distributed Sampling-Based Roadmap of Trees for Large-Scale Motion Planning." IEEE International Conference on Robotics and Automation, pp. 3879--3884  [publisher]  [preprint]
  • Akinc M, Bekris KE, Chen BY, Ladd AM, Plaku E, and Kavraki LE (2003): "Probabilistic Roadmaps of Trees for Parallel Computation of Multiple Query Roadmaps." Springer Tracts in Advanced Robotics, vol. 15, pp. 80--89  [publisher]  [preprint]
  • Bekris KE, Chen BY, Ladd AM, Plaku E, and Kavraki LE (2003): "Multiple Query Motion Planning using Single Query Primitives." IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 656--661  [publisher]  [preprint]