IEEE TRANSACTIONS ON ROBOTICS 1 Parallel Motion Planning using Poisson-Disk Sampling Chonhyon Park, Student Member, IEEE, Jia Pan, Member, IEEE, and Dinesh Manocha, Fellow, IEEE Abstract—We present a RRT-based parallel motion planning algorithm that uses the maximal Poisson-disk sampling scheme. Our approach exploits the free-disk property of the maximal Poisson-disk samples to generate nodes and perform tree expan- sion. Furthermore, we use an adaptive scheme to generate more samples in challenging regions of the configuration space. The Poisson-disk sampling results in improved parallel performance and we highlight the performance benefits on multi-core CPUs as well as many-core GPUs on different benchmarks. Index Terms—motion planning, Poisson-disk sampling, parallel algorithm I. I NTRODUCTION S AMPLING-BASED approaches are widely used to com- pute collision-free paths for motion planning. The most influential sampling-based motion planning schemes include probabilistic roadmaps (PRM) [1] and rapidly-exploring ran- dom trees (RRT) [2]. The key idea in these planners is to gen- erate samples in the free configuration space of the robot and connect them with collision-free edges to construct a graph. PRM planners are mostly used for multiple-query planners and involve considerable preprocessing in terms of roadmap computation. On the other hand, most motion planning ap- plications do not perform multiple queries. These situations arise when the robot does not know the entire environment a priori, or when it moves to a new environment. In such cases, incremental sampling-based algorithms, such as RRT, are widely used. The RRT algorithm has been extended in several aspects for use in systems with differential constraints, nonlinear dynamics, and hybrid systems. Moreover, it has also been integrated with physical robot platforms. The simplest RRT algorithms are based on generating uniform random samples and connecting the nearby samples until a collision-free path from the initial configuration to the goal configuration has been computed. In this paper, we present a novel approach that uses Poisson-disk samples for RRT planners and constructs the trees using parallel algorithm. Poisson-disk sampling is a well-known scheme that can be used in high dimensions to generate a random set of points with two properties: the points are tightly packed This work was supported by the Army Research Office under Con- tract W911NF-10-1-0506, by the National Science Foundation under Award 1000579, Award 1117127, and Award 1305286, and by Sandia Labs. A pre- liminary version of this paper was presented in part at the IEEE International Conference on Robotics and Automation, Hong Kong, China, May 2014. Chonhyon Park and Dinesh Manocha are with the Department of Computer Science, University of North Carolina at Chapel Hill, Chapel Hill, NC 27599 USA (E-mail: {chpark, dm}@cs.unc.edu). Jia Pan is with the Department of Computer Science, University of Hong Kong, Pokfulam, Hong Kong (E-mail: [email protected]). together, yet remain separated from each other by a specified minimum distance [3], [4], [5]. Poisson-disk distributions are known to have good blue-noise characteristics and are widely used in statistics, computer graphics, mesh algorithms, AI, image processing, and random object placement. Poisson-disk sampling is a sequential random process for selecting points in a region. The sampling process is maximal if no more points can be added, which implies that the entire region is completely covered by the disks of radius r centered at each sample. In this paper, we present an extended version of our Poisson- RRT algorithm that originally introduced in a conference paper [6]. The algorithm uses precomputed maximal Poisson- disk samples to generate an RRT tree. We use an adaptive sampling scheme that increases the sampling rate in the challenging regions of the configuration space (e.g. narrow passages). Main Results: The main contribution of the paper is that the use of one-time precomputation of the maximal Poisson-disk samples result in 1) fewer redundant nodes in the configuration space and 2) are more amenable to parallelization on com- modity multi-core CPUs and many-core GPUs. We highlight the performance of our multi-threaded CPU and GPU-based implementations on well-known benchmarks and demonstrate improved parallelization as compared to prior parallel RRT algorithms. The rest of the paper is organized as follows. In Section II, we survey prior work on RRT-based motion planning and high- light properties of Poisson-disk sampling. We give an overview of our Poisson-RRT planning algorithm in Section III. We present the details of the planning algorithm in Section IV and describe the parallel extension in Section V. Theoretical analysis of the algorithm is given in Section VI. We highlight the performance on different benchmarks of low- and high- dimensional spaces in Section VII. II. RELATED WORK AND BACKGROUND In this section, we give a brief overview of prior work on RRT-based motion planning, parallel planning algorithms, Poisson-disk sampling, and Lattice-based Sampling. A. RRT-based Motion Planning There is extensive work on RRT-based motion planning due to its efficiency. The original RRT algorithm [7] grows the RRT tree based on the Voronoi property, biasing the search towards unexplored regions of free configuration space. Many variants to improve this original RRT have been proposed.
Chonhyon Park, Student Member, IEEE, Jia Pan, Member, IEEE, and Dinesh Manocha, Fellow, IEEE
Abstract—We present a RRT-based parallel motion planningalgorithm that uses the maximal Poisson-disk sampling scheme.Our approach exploits the free-disk property of the maximalPoisson-disk samples to generate nodes and perform tree expan-sion. Furthermore, we use an adaptive scheme to generate moresamples in challenging regions of the configuration space. ThePoisson-disk sampling results in improved parallel performanceand we highlight the performance benefits on multi-core CPUsas well as many-core GPUs on different benchmarks.
Index Terms—motion planning, Poisson-disk sampling, parallelalgorithm
I. INTRODUCTION
SAMPLING-BASED approaches are widely used to com-pute collision-free paths for motion planning. The most
influential sampling-based motion planning schemes includeprobabilistic roadmaps (PRM) [1] and rapidly-exploring ran-dom trees (RRT) [2]. The key idea in these planners is to gen-erate samples in the free configuration space of the robot andconnect them with collision-free edges to construct a graph.PRM planners are mostly used for multiple-query plannersand involve considerable preprocessing in terms of roadmapcomputation. On the other hand, most motion planning ap-plications do not perform multiple queries. These situationsarise when the robot does not know the entire environmenta priori, or when it moves to a new environment. In suchcases, incremental sampling-based algorithms, such as RRT,are widely used. The RRT algorithm has been extended inseveral aspects for use in systems with differential constraints,nonlinear dynamics, and hybrid systems. Moreover, it has alsobeen integrated with physical robot platforms.
The simplest RRT algorithms are based on generatinguniform random samples and connecting the nearby samplesuntil a collision-free path from the initial configuration tothe goal configuration has been computed. In this paper, wepresent a novel approach that uses Poisson-disk samples forRRT planners and constructs the trees using parallel algorithm.
Poisson-disk sampling is a well-known scheme that canbe used in high dimensions to generate a random set ofpoints with two properties: the points are tightly packed
This work was supported by the Army Research Office under Con-tract W911NF-10-1-0506, by the National Science Foundation under Award1000579, Award 1117127, and Award 1305286, and by Sandia Labs. A pre-liminary version of this paper was presented in part at the IEEE InternationalConference on Robotics and Automation, Hong Kong, China, May 2014.
Chonhyon Park and Dinesh Manocha are with the Department of ComputerScience, University of North Carolina at Chapel Hill, Chapel Hill, NC 27599USA (E-mail: chpark, [email protected]).
Jia Pan is with the Department of Computer Science, University of HongKong, Pokfulam, Hong Kong (E-mail: [email protected]).
together, yet remain separated from each other by a specifiedminimum distance [3], [4], [5]. Poisson-disk distributions areknown to have good blue-noise characteristics and are widelyused in statistics, computer graphics, mesh algorithms, AI,image processing, and random object placement. Poisson-disksampling is a sequential random process for selecting pointsin a region. The sampling process is maximal if no morepoints can be added, which implies that the entire region iscompletely covered by the disks of radius r centered at eachsample.
In this paper, we present an extended version of our Poisson-RRT algorithm that originally introduced in a conferencepaper [6]. The algorithm uses precomputed maximal Poisson-disk samples to generate an RRT tree. We use an adaptivesampling scheme that increases the sampling rate in thechallenging regions of the configuration space (e.g. narrowpassages).
Main Results: The main contribution of the paper is that theuse of one-time precomputation of the maximal Poisson-disksamples result in 1) fewer redundant nodes in the configurationspace and 2) are more amenable to parallelization on com-modity multi-core CPUs and many-core GPUs. We highlightthe performance of our multi-threaded CPU and GPU-basedimplementations on well-known benchmarks and demonstrateimproved parallelization as compared to prior parallel RRTalgorithms.
The rest of the paper is organized as follows. In Section II,we survey prior work on RRT-based motion planning and high-light properties of Poisson-disk sampling. We give an overviewof our Poisson-RRT planning algorithm in Section III. Wepresent the details of the planning algorithm in Section IVand describe the parallel extension in Section V. Theoreticalanalysis of the algorithm is given in Section VI. We highlightthe performance on different benchmarks of low- and high-dimensional spaces in Section VII.
II. RELATED WORK AND BACKGROUND
In this section, we give a brief overview of prior workon RRT-based motion planning, parallel planning algorithms,Poisson-disk sampling, and Lattice-based Sampling.
A. RRT-based Motion Planning
There is extensive work on RRT-based motion planning dueto its efficiency. The original RRT algorithm [7] grows theRRT tree based on the Voronoi property, biasing the searchtowards unexplored regions of free configuration space. Manyvariants to improve this original RRT have been proposed.
IEEE TRANSACTIONS ON ROBOTICS 2
Dynamic-domain RRT [8] adaptively controls the Voronoi biasof the nodes, which results in a better exploration. Somealgorithms [9], [10] use workspace information to guide thegrowth of the RRT tree. RESAMPL [11] adaptively choosesdifferent sampling strategies for RRT according to the localproperties of different regions. Shkolnik and Tedrake’s BallTree algorithm [12] adaptively approximates the free configu-ration space using hyperspheres with varying radii. Visibilityof the expanded nodes is used to adjust the tree expansion inAdaptive RRT [13]. RRT has also been extended for optimalor near-optimal motion planning [14], [15], [16].
B. Parallel Planning Algorithms
The performance of motion planning can be improved usingthe parallel computation. There are many planning approachesthat exploit distributed clusters or shared-memory systems orcommodity parallel processors.
1) Algorithms for Distributed Clusters: Distributed clustershave been widely used for solving compute-intensive prob-lems. Clusters are defined as a large number of connectedmachines or nodes, where each of them has local memory,and a big computational problem is divided into small piecesand assigned to different processors in the cluster for parallelcomputation.
Many parallel techniques have been proposed to improvethe performance of planning using distributed clusters. Perezand O’Donnell [17] compute the primitive map of a 3D con-figuration space using parallel computation. Amato et al. [18]propose a parallel PRM planning approach which has scalablespeedups.
Many planning algorithms exploit parallelism based onsubdividing the configuration space [19] and use clusters toexpand the tree in a different region of the configurationspace. Different subdivision techniques have been proposedfor roadmap-based planning [20] or tree-based planning algo-rithms [21], [22].
Some approaches combine PRM and RRT in order to usethe massive parallelism [23]. Many parallel RRT algorithmscan be classified into AND parallelization and OR paral-lelization [24]. AND parallelization uses multiple cores toexpand a single tree by adding multiple new nodes in parallel.The multi-node expansion of AND parallelization allows thetree to expand faster. However, it generates redundant nodes,which can causes the performance degradation due to thecollision checking and nearest neighbor search overheads. InOR parallelization, each thread running on a separate coremaintains its own tree and solves the motion planning problemindependently. OR parallelization improves the average cost ofthe solution [25], [26], but it usually expects less speedup thanAND parallelization
2) Algorithms for Shared-Memory System: Nowadays,commodity processors in a single machine have multiplecores. Although these systems have fewer cores and overallprocessing power as compared to large distributed clusters,multiple threads running on such shared-memory processorshave access to the same memory and there is no major over-head of transferring the data between the nodes in a cluster. It
is especially useful for parallel algorithms of RRT, which doesnot have the massive parallelism like the graph constructionstep of PRM. Many AND and OR parallel RRT algorithms areproposed for shared-memory systems [27], [28]. AND parallelapproaches on shared-memory systems have better efficiencythan clusters because the multiple threads can share the sametree data structure on shared memory [29]. Updates of theshared tree requires synchronization, and the performance canbe improved using lock-free data structures [30].
Many approaches also exploit many-core GPUs for acceler-ating the planning algorithms. Pisula et al. [31] use the raster-ization hardware for improving the sample generation in nar-row passages. Recently, the general purpose GPU technologyallows efficient use of the GPUs using appropriate interfaces(e.g. CUDA, OpenCL). G-Planner [32] uses many-core GPUprocessors to parallelize and accelerate PRM approach. Kideret al. [33] propose a GPU-based R* algorithm for 6 DOFproblems. Park et al. [34] use multiple CPU and GPU coresfor parallel optimization-based planning. Bialkowski et al. [35]use multiple cores on GPUs to perform parallel collisionchecking along different edges of RRT.
3) Our Approach: Our planning algorithm is based onAND parallelization on shared-memory system. However, ourapproach has a better performance than the original ANDparallel RRT by guiding the tree expansion using Poisson-disksamples to reduce the generation of redundant nodes.
C. Maximal Poisson-Disk Sampling
Poisson-disk sampling [3], [4], [5] ensures that each sampleis at least a minimum distance, r, from the other samples. Eachsample has an associated disk, which is a hypersphere of radiusr, and no additional samples can be placed in the disk. Thearea of a disk (or the volume of the hypersphere) is called thecoverage volume of the associated sample. Maximal Poisson-disk sampling requires that there is no room or space to place anew Poisson-disk sample in the domain, i.e., the entire domainis covered by the disks of samples. Fig. 1(a) shows a set ofmaximal Poisson-disk samples for the same domain. Overall,maximal Poisson-disk sampling satisfies following propertiesin any dimensions:
where X = xi is the set of samples in domain Ω. Givena non-maximal sampling, a new Poisson-disk sample can begenerated in a bias-free manner, i.e., the probability of select-ing a sample from any uncovered subregion is proportional tothe subregion’s volume:
∀A ∈ S(X) : P(x ∈ A) =|A||S(X)| , (2)
where S(X) = x ∈ Ω : ‖x−xi‖ ≥ r, ∀xi ∈ X is the regionuncovered by existing disks. For one Poisson-disk sample x,another Poisson-disk sample y is its neighbor if the diskscorresponding to the two samples overlap, i.e., ‖x−y‖ < 2r,as shown in Fig. 1(a).
IEEE TRANSACTIONS ON ROBOTICS 3
These properties are useful when maximal Poisson-disksamples are used for the RRT algorithm. The free-disk prop-erty ensures that the new sample is not too close to anexisting node in the RRT tree, which thereby ensures goodcoverage of the free space. For a fixed number of samples, themaximal property generates the best distribution of samplesin the configuration space. Furthermore, we use an adaptivescheme based on Poisson-disk samples that makes it possibleto find paths in challenging areas or in narrow passages of theconfiguration space, as described in Section IV-C. The bias-free property of the Poisson-disk samples (which functionssimilarly to the Voronoi diagram bias used in the original RRTalgorithm [7]).
However, the computation of the Poisson-disk sampleswhich satisfy the maximal property is compute intensive.Most of the sampling algorithms have been practical only for2D or 3D spaces [36], or up to six-dimensional space [37].Recently, relaxed sampling algorithms are suggested for high-dimensional space [38].
Our planning algorithm does not depend on a particularPoisson-disk sampling algorithm because it uses sampleswhich are precomputed in offline. We use exact [37] and re-laxed [38] maximal Poisson-disk sampling approach for our 6-and 23-dimensional benchmarks in Section VII, respectively.
D. Lattice-based Sampling
Although random sampling is widely used in motionplanning, many other sampling techniques have been pro-posed [39]. Grid-based sampling is used in many applicationsdue to its low dispersion, which implies that the samplesare generated in such a manner that the largest uncoveredarea in the configuration space is as small as possible, andthat the size of the uncovered space is governed by the gridresolution. However, grid-based approaches generate samplesthat are aligned with the coordinate axis; these aligned samplesare undesirable, as they increase the variance in the planningalgorithm’s running time [40]. Lattices are a generalizationof grids that allow non-orthogonal axes or other spatial de-compositions; common lattices include the Sukharev grid andthe nongrid lattice, both of which give samples with lowdispersion, low discrepancy, and low environmental sensitivity.Discrepancy is a criterion that measures the largest axis-aligned rectangular area which is not covered by samples.Multi-resolution approaches [41], [42] are used to increase thenumber of samples in lattice-based planning algorithms, andhave been combined with replanning [43]. As compared togrid-based samples, Maximal Poisson-disk samples have lowdispersion and low discrepancy, and in addition, the resultingsamples are not aligned with any axes.
III. OVERVIEW
Our goal is to use Poisson-disk sampling as the underlyingsample generation process for RRT-based planning. The nodesof the RRT tree correspond to Poisson-disk samples, and thetree expansion step can be performed in parallel using multiplethreads. In this section we give an overview of the proposedalgorithm.
A. Assumptions and Notations
The configuration x of a robot is a point in a configurationspace C, which consists of collision-free region Cfree andC-obstacle region Cobs; our goal is to find a continuous,collision-free path from an initial configuration, xinit, to agoal configuration, xgoal.
The RRT tree T is initialized with the root node ofxinit, and the algorithm expands the tree incrementally. Eachiteration of RRT planning executes two main procedures:
1) Sampling: The sample() procedure generates a newrandom configuration x, which determines the directionof the tree expansion.
2) Expansion: The expansion() procedure includes twosteps, 1) nearest node search and 2) local planning.Given a configuration x, nearest node search finds anode v in T: the closest node to x according to the givenmetric of the configuration space, ρ (e.g., the weightedEuclidean metric). For high-dimensional space, approx-imate algorithms [44] with computational complexityO(d log n) are used, where d is the dimension of theconfiguration space.The local planning step checks whether the shortest pathbetween v and x lies in Cfree (i.e., that the configurationof the path does not collide with the obstacles). If thepath is collision-free, x is added to T as a new nodeconnected to the node v. If the path has a collision,the collision-free configuration xnew on the path that isfarthest from v is added to T instead of x.
B. RRT Planning using Maximal Poisson-disk Sampling
The RRT algorithm is efficient for single-query problems,since the algorithm incrementally expands the RRT tree to theunexplored regions and terminates when the solution is found.However, this incremental expansion of the tree means that itis difficult to make an efficient parallel algorithm for planning.The AND parallelization can expand the tree faster than theoriginal RRT. However, as the number of threads increases,the algorithm results in more redundant nodes in the RRTtree, degenerating the performance of overall planning.
Input: start configuration xinit, goal configuration xgoal,precomputed Poisson-disk (radius r) sample set X
Output: RRT Tree T1: T.add(xinit)2: X.add(xinit)3: /* Can handle multiple threads easily */
4: for i = 1 to m do in parallel5: while xgoal /∈ T do6: x← sample()7: T← extendMPDS(T,x,X)8: end while9: end for
Fig. 2. RRT Planning using maximal Poisson-disk Sampling
IEEE TRANSACTIONS ON ROBOTICS 4
xy1
y2 y3
y4
y5y6
(a) Maximal Poisson-disk samples
xinit
xgoal
(b) Motion planning using Poisson-disk sampling
v
xinit
x1x2
x3
x4
y1 y2
y3
e1e2
e3
(c) Parallel Poisson-RRT tree expansion
Fig. 1. (a) Maximal Poisson-disk sampling. Each black point is a Poisson-disk sample and the red circle is the corresponding Poisson disk. yi are theneighbors of x. (b) Poisson-disk sampling is used to generate the RRT tree and compute a collision-free path from xinit to xgoal. (c) Parallel Poisson-RRTtree expansion using 4 threads. The i-th thread expands the tree toward sample xi, i = 1, 2, 3, 4. The red vectors ei show the new RRT edges added. Sincex2 and x4 correspond to the identical Poisson-disk sample (y2), both of them result in adding the edge e2 to the tree. There is no redundant node added tothe tree.
The overall Poisson-RRT algorithm is shown in Fig. 2. Inorder to lessen the overhead caused by the redundant nodes,our algorithm uses precomputed Poisson-disk samples in thetree expansion. The precomputed samples satisfy the free-diskproperty in (2), where X is set of samples and r is a predefinedminimum distance between any of two samples. Our algorithmmaintains the RRT Tree data structure T and the Poisson-disksample set X during the planning, and each node v in T hasa pointer to the corresponding Poisson-disk sample x in X toaccess additional information (e.g. neighboring samples or thedisk radius) needed in the extendMPDS() procedure.
Unlike the standard RRT, which performs local plan-ning between the nearest node v and the configuration x,extendMPDS() procedure in Fig. 4 chooses a Poisson-disk sample xnbr that is closest to x among v’s neighboringPoisson-disk samples. The free-disk property ensures that thechosen sample is at least a minimum distance, denoted hereby r, from v. If the local planning finds a collision-free pathbetween xnbr and v, xnbr is added to the RRT tree as a newnode. The tree expansion is repeated until the goal config-uration xgoal is added to the tree. Our approach eliminatesthe problem of multiple threads of the algorithm choosing thesame direction, which generates redundant nodes that are tooclose to each other in the standard RRT tree expansion. Inour algorithm, the threads that choose the same direction donot generate redundant nodes; instead, they choose the samePoisson-disk sample and stop the redundancy problem fromdeveloping. We add the sample only once to the tree. Anexample of tree construction in our algorithm is shown inFig. 1(c).
Fig. 3 shows the RRT trees generated by an original RRT,an AND parallelization RRT, and our algorithm. The treegenerated by AND parallelization has many redundant nodesthat are close to other tree nodes, while the tree generatedusing Poisson-disk sampling has efficiently spaced nodes.
IV. POISSON-RRT ALGORITHM
In this section, we present the details of our planningalgorithm, including precomputation of maximal Poisson-disksamples, tree expansion, and adaptive sampling.
(a) RRT (b) AND Parallel RRT (c) Parallel Poisson-RRT
Fig. 3. Comparison of RRT trees generated using different planning ap-proaches. (a) The tree corresponding to the original RRT algorithm isgenerated according to the Voronoi bias of the sequential algorithm. (b) Theparallel RRT tree generated by AND parallelism has many redundant nodesthat are close to other nodes in the tree (e.g., the new nodes y2, y3, andy4 are close to y). (c) The tree generated with Poisson-disk sampling hasfewer redundant nodes due to the free-disk property of samples, although itis generated using the parallel sampling.
A. Precomputation of Maximal Poisson-disk Samples
As a precomputation step, Poisson-disk samples are gener-ated in the d-dimensional configuration space. The computa-tion of Poisson-disk samples is time-consuming, however thesesamples are independent of obstacles and do not need to berecomputed frequently. We can use a precomputed sample setcomputed offline for multiple planning queries, and only needto recompute the precomputed samples when the number ofdegree of freedom of the robot is changed, which would rarelyoccur.
Although our planning algorithm is not dependent on aspecific Poisson-disk sampling algorithm, the experimentalresults of our paper is based on the parallel version of thesampling algorithm proposed by Ebeida et al. [37]. For a givendisk radius r, Ebeida et al.’s algorithm generates uniform basegrids that cover the entire configuration space C. Each gridcell is a square with the side length r/
√d, and each cell
can contain at most one sample. The algorithm repeatedlysubdivides grid cells which are not fully covered by Poisson-
IEEE TRANSACTIONS ON ROBOTICS 5
Input: RRT Tree T, a new random sample x, Poisson-disk sample set X
Output: RRT Tree T1: v← nearestNode(T,x)2: xnbr ← argminy∈v’s neighbor ρ(y,x)3: (success,xfree)← collisionCheck(v,xnbr)4: if success then5: /* no collision along that edge */
6: T.add(xnbr)7: else8: /* if there is collision, perform
adaptive sampling */
9: if ρ(xfree,v) < v.r then10: /* If the collision occurs in the
disk of v, reduce the coverage of v
11: v.r ← v.r/212: end if13: /* If the collision occurs in the
disk of xnbr, reduce the coverage of xnbr
*/
14: if ρ(xfree,xnbr) < xnbr.r then15: xnbr.r ← xnbr.r/216: end if17: X.add(adaptiveSampling(v,xnbr,xfree))18: end if
Fig. 4. RRT tree extendMPDS() procedure using maximal Poisson-disksampling.
disks, and generate samples in those cells.The complexity of the algorithm is linear in the number
of generated samples, which is exponential to the number ofdimensions and the disk radius. The parallel version of thealgorithm improves the sampling performance by processingmultiple grid cells simultaneously.
B. Tree ExpansionGiven a new random sample x, our algorithm extends the
planning tree T using the extendMPDS() procedure, whichis summarized in Fig. 4.
For a sample point x, the algorithm finds the nearest treenode v in T. From a node v, The Poisson-RRT algorithmchooses a sample xnbr, which is a point closest to x amongv’s neighboring Poisson-disk samples. These steps utilize thenearest neighbor search.
It is possible that a Poisson-disk sample can be chosen bymore than one thread in the nearest neighbor search (line 1-2).However, when the algorithm adds samples (line 6), it preventsadding a sample in X to T more than once. This approachhelps the algorithm to avoid adding redundant nodes whileusing the parallel tree extension.
The collisionCheck() procedure, used for local plan-ning, checks for collisions along the edge that joins v to xnbr.This allows only the precomputed Poisson-disk samples canbe added as a node to T. However, the precomputed Poisson-disk samples may not have a large-enough number of samples
to find a collision-free solution. It requires a way to finda path which does not passes the precomputed samples ifa collision is detected. As a result, the algorithm performsadaptive Poisson subsampling at runtime to generate moresamples with reduced distance between them (line 8-17).
C. Adaptive Sampling
v
xinit
xnbrx
Cobs(a)
v
xinit
xnbr
Cobs
xfree
(b)
xinit
Cobs
vv
(c)
xinit
Cobs
vv
xnew
(d)
Fig. 5. Tree extension and adaptive sampling. (a) The sample xnbr is thepoint closest to x among the neighboring Poisson-disk samples of v. (b)If vxnbr intersects Cobs, collisionCheck() procedure returns the lastcollision-free point xfree. If the collision occurs within the disk associatedwith v, the radius of this disk is reduced by half. (c) A precomputed templateof Poisson-disk samples is applied to v to find a point which is close toxfree and satisfies the maximal property in the disk of v. (d) A new samplexnew is connected to T if there is no collision on the local path joining vand xnew .
As shown in lines 8-17 of Fig. 4, we perform adaptive sam-pling in the regions where the local planning algorithm finds acollision between an edge of the tree and an obstacle; in thatsub-region of the configuration space, we generate additionalsamples with a reduced disk radii. This process is illustrated inFig. 5. In the initialization of the data structure X, all samplesx have the same disk radius r. If the collisionCheck()procedure detects a collision during local planning along theedge that joins v to xnbr, the procedure computes xfree as thelast collision-free point in the direction from v to xnbr. If acollision occurs within the disk associated with v or xnbr, theadaptive sampling algorithm reduces the radius of the disk byhalf (line 9-16 in Fig. 4). This reduction changes some regionsthat were covered in the original disk to become uncovered(Fig. 5(b)). Therefore, we need to generate samples to coverthese regions.
The computation of maximal Poisson-disk samples can beslow and is performed only during the preprocessing, and notat runtime. Instead of using the exact Poisson-disk sampling al-gorithm, we generate new samples by precomputed templates.
IEEE TRANSACTIONS ON ROBOTICS 6
In the precomputation step, we compute n templates fromthe Poisson-disk sample set, which is also used for adaptivesampling. In each template, there is a sample with radius r/2placed at the origin. We use the same algorithms [37], [38],used for the sample precomputation, to add maximal Poisson-disk samples of radius r/2 within a hyper-sphere of radius rand dimension d.
In a runtime adaptive sampling (line 17 in Fig. 4) for v, werandomly select one of n templates, and scale it to make thesamples in the template have the same radius of v. We rotatethe scaled template for a randomly selected orientation [45]and add samples in the rotated template to X, except thesample at the origin which has the same position with v(Fig. 5(c)). Using the positions of v and xfree, we computewhich sample in the template is closest to xfree when thetemplate is applied to v. The new sample is connected to Tfor future expansion if there is no collision on the local pathjoining v and the new sample (Fig. 5(d)).
This adaptive sampling approach locally breaks the free-disk property, but allows the algorithm to handle any width ofnarrow passages, since it adaptively generates more samplesin the difficult regions of the configuration space.
V. PARALLELIZATION OF THE ALGORITHM
In sampling-based planning algorithms like RRT, the mostof the computation time is spent in the nearest neighbor searchor collision checking procedures, as shown in Fig. 9. This sec-tion presents how our parallel planning algorithm acceleratesthose procedures in two ways: 1) we use AND parallel RRTwhich provides high parallelism to utilize the computationalresources, and 2) we use parallel GPU algorithms for nearestneighbor search and collision checking.
A. Massively Parallel Computation
Our parallel planning algorithm can be implemented usingeither multicore CPUs or manycore GPUs to improve theoverall performance, as described in Section VII. We assumea shared memory system, which is common for GPU-basedalgorithms and does not require message passing interfaces ofdistributed systems. Our algorithm achieves the massive par-allelism by performing parallel extendMPDS() evaluationson multiple threads, as shown in Fig. 2. Theoretical analysis ofthe planning complexity improvement is given in Section VI.
However, parallel tree expansion results in synchronizationissues among multiple threads which shares the data structureT and X. Furthermore, the Poisson-RRT algorithm needsto check whether the new node being added to the tree isalready in the tree or not. Our data structures are designedto minimize the use of functions that can be used for threadsynchronization, as they have additional overhead. A sampleonce added to X from either the initialization step or adaptivesampling, is never modified or deleted, which requires thesynchronization for accessing operations. One exception is thateach sample has a marker that indicates whether the sampleis already added as a tree node to T or not, which is used toprevent adding the sample multiple times to T. A tree node vis added to T only if the marker of the corresponding sample
x is false, and set it to true. However, it can be handled byan atomic compare-and swap (CAS) operation that is availableon current commodity CPU and GPU processors, instead oflocks.
B. Parallel Nearest Neighbor Search
As described in Section IV-B, the planning algorithm findsthe nearest tree node for a sample by utilizing the near-est neighbor search which has a computational complexityO(d log n), where d is the dimension of the configurationspace. There has been extensive work on nearest neighborsearch using GPUs [46], [47], [32]. We use the algorithm pro-posed by Pan et al. [47], which uses Locality-Sensitive Hash-ing (LSH) for clustering nearby points in high-dimensionalspaces. The algorithm generates the same hash value for pointsnear one another; points with the same hash value are stored inthe same bucket of the hash table. Using this data structure, thenearest neighbor search for a point can be computed in nearlyconstant time since it requires only looking up one bucket inthe hash table.
C. Parallel Collision Checking
In order to accelerate collision checking, we computebounding volume hierarchies (BVH) for the robot and the ob-stacles in the environment. We construct the oriented boundingbox (OBB) trees [48] for the triangle model representationsof the robot and obstacles using a GPU-based constructionalgorithm [49]. The OBB trees improve the performance ofcollision checking because of their high culling efficiency.
When the tree node and the nearest Poisson-disk sampleare computed, the algorithm performs local planning to checkfor a feasible path between the two configurations. We usediscrete collision detection (DCD), which discretizes the pathbetween two configurations into multiple steps, between therobot and obstacles; we then check for collisions during eachstep. GPU uses multiple threads to perform this multiple-stepcollision checking in parallel.
VI. THEORETICAL ANALYSIS
In this section, we analyze the computational complexity ofour parallel Poisson-RRT algorithm and compare it with theAND parallel RRT algorithm.
In order to compute the time complexity for RRT algo-rithms, we use the concept of attraction sequence borrowedfrom [2]. An attraction sequence is a finite sequence A =A0, A1, ..., Ak of sets with the following properties: 1)A0 = xi and Ak = xgoal; 2) for each set Ai, thereexists a set Bi, called the basin, such that for any x ∈ Ai−1,y ∈ Ai, and z ∈ C\Bi , there is ρ(x,y) < ρ(y, z),where ρ is a metric defined in the configuration space; 3)for all x ∈ Bi, there exists an l such that the sequence ofactions u1, ..., ul selected by the RRT’s extend algorithmor Poisson-RRT’s extendMPDS algorithm will bring the stateinto Ai ⊆ Bi. Intuitively, the property 2 ensures that anelement in Bi will always be selected by the nearest neighborquery nearestNode in Figure 4, and the property 3 implies
IEEE TRANSACTIONS ON ROBOTICS 7
that Bi is a potential well and can attracts the nearby states intoAi. Given a scenario, we should choose an attraction sequencewith each node Ai as large as possible and the sequence lengthk as small as possible. The values of Ai’s size and k providea rough estimation about how difficult a scenario is for themotion planning. If the space is open, k would be small andeach Ai would be large. If the space contains narrow passages,then k will be large and each Ai would be small. Since thevalues of Ai and k are only related with the scenarios andare independent with the underlying planning algorithms, theyprovide a consistent manner to compare different planningalgorithms. In addition, the potential well property of Bi helpsus to be free from the details of local planning or adaptivesampling in different approaches.
Given an attraction sequence A, let p be defined as p =miniµ(Ai)/µ(Cfree), which corresponds to a lower boundon the probability that a random state will lie in a particularregion Ai. Here µ(Ai) represents the area of Ai. Basedon the attraction sequence, we can compute the expectedcomputational complexity for sequential RRT algorithms:
Theorem 6.1: If an attraction sequence of length k exists,then the expected time complexity of the sequential originalRRT and Poisson-RRT planner is O(dk
p ).Proof: The time complexity of sequential RRT planner
includes three parts: random sample O(n), local planningO(c·n) and nearest neighbor computation
∑nj=1O(d log j) =
O(d log(n!)), where n is the number of iterations, c is thenumber of collision checking for a local planning, which isbounded as a constant with the maximum edge length 2r, andd is the dimension of the configuration space. As a result, theoverall time complexity is O(c ·n) if local planning dominatesthe overall complexity or O(d log(n!)) if nearest neighborcomputation dominates the overall computation. Given anattraction sequence of length k, the probability that the plannercan find a path after n iterations is
(n−1k−1)pk(1− p)n−k. If the
complexity is dominated by nearest neighbor computation, theexpected computational complexity of sequential RRT is:
TRRT =
∞∑n=k
(n− 1
k − 1
)pk(1− p)n−kO(d log(n!))
'∞∑
n=k
(n− 1
k − 1
)pk(1− p)n−kO(dn log n)
=dpkk
(1− p)kO(
∞∑n=k
(n
k
)(1− p)n log n).
Let F (k) =∑
n
(nk
)(1 − p)n log n, then F (k) ≤ (1 −
p) log(k+1)log k (F (k−1) +F (k)), which implies that
∑n
(nk
)(1−
p)n log n ≤ (1−p)k
( log klog(k+1)
−1+p)k+1. As a result, the expected
computational complexity is
TRRT = O(dk
p
1
(1− 1p ( log(k+1)
log k − 1))k+1) ' O(d
k
p).
If the timing cost of each iteration is dominated by local plan-ning, it is easy to prove that the complexity is TRRT = O(k
p ).
Next, we analyze the complexity of parallel RRT algorithms,which use m threads simultaneously for tree expansion. Wefirst show that parallel Poisson-RRT algorithm can reducethe number of redundant tree nodes as compared with ANDparallel RRT:
Theorem 6.2: If a parallel Poisson-RRT algorithm extendsits tree by adding m nodes in parallel, then during eachiteration, the expected number of tree nodes generated ism′ = 1
q (1− (1− q)m), where q = 1/N and N is the size ofthe Poisson-disk sample set X.
Proof: When parallel Poisson-RRT extends the RRT tree,m different random samples are generated. Some of thesamples may belong to the same Poisson-disks, while othersmay not. We now compute a bound on the expected numberof distinct disks associated with these m samples.
Let Y be the number of distinct disks. Let Di be 1 if one ofthe random samples is located inside the i-th disk, and 0 if not.We have E(Di) = 1− P(Di = 0) = 1− (1− q)m. ThereforeE(Y ) = E(
∑Ni=1Di) = NE(Di) = 1
q (1− (1− q)m).According to Poisson-RRT algorithm, a new node is gener-
ated for each distinct disk. Therefore m′ = E(Y ).Remark 6.3: For AND parallel RRT, m different tree nodes
are generated during each iteration. Obviously, m′ ≤ m,so Poisson-RRT reduces the size of RRT tree. Moreover,Poisson-RRT can adaptively change the number of new treenodes depending on whether the region is open or not. Fora challenging region, Poisson-RRT uses a smaller r, whichresults in large q and in this case, m′ ' m, i.e., Poisson-RRT is similar to the AND parallel RRT. For an open region,Poisson-RRT will use a large r, which results in a small q andm′ m, i.e., parallel Poisson-RRT will generate a tree withfewer nodes.
The computational complexity of AND parallel RRT can becomputed as follows:
Theorem 6.4: If an AND parallel RRT algorithm expands itstree by adding m nodes in parallel, the planning complexity isO((d + logm) k
1−(1−p)m ) if an attraction sequence of lengthk exists.
Proof: We define p′ as the lower bound on the probabilitythat one of the m random states generated during one iterationwill lie in a particular region Ai. Then p′ = 1 − (1 − p)m.In a manner similar to that laid out in Theorem 6.2, if thetiming cost is dominated by nearest neighbor computation,the expected time complexity is:
TAND =
∞∑n=k
(n− 1
k − 1
)p′k(1− p′)n−kO(d log(n!mn))
'∞∑
n=k
(n− 1
k − 1
)p′k(1− p′)n−kO(n log n+ n logm)
= O((d+ logm)k
p′).
When the timing cost is dominated by collision detection andlocal planning, the resulting bound is O( k
p′ ).Corollary 6.5: The planning complexity of the parallel
Poisson-RRT algorithm is smaller than the complexity of theAND parallel RRT.
IEEE TRANSACTIONS ON ROBOTICS 8
Proof: From Theorem 6.2, the expected number of treenodes generated in an iteration is m′ for the parallel Poisson-RRT algorithm. The complexity of parallel Poisson-RRT canbe computed by substituting m in Theorem 6.4 with m′:
TPoisson = O((d+ logm′)k
p′) (3)
≤ O((d+ logm)k
p′= TAND.
Remark 6.6: When p is nearly 1, which corresponds to arelatively easy planning problem, then TPoisson ≥ TRRT andthe speedup is small. When p is small, which corresponds tomore challenging planning scenario, then TPoisson ' (d +logm′) k
mp = 1mTRRT .
The proofs in this section assumes that the effect of theruntime adaptive sampling is neglectable in the planningperformance. If the planner needs to generate a lot of samplesusing adaptive sampling, it would degrade the performance.However, in Section VII, we show in Table III that theadaptive sampling is used rarely and only a small numberof samples are generated at runtime using adaptive samplingin our experiments. As a result, the analysis described in thissection is applicable to most scenarios.
VII. RESULTS
In this section, we present our experimental results and high-light the performance of our planning algorithm on differentbenchmarks. We implement the algorithm using OMPL [50].For parallel implementations, we use Boost and NVIDIACUDA libraries for CPU-based and GPU-based planners,respectively. All the timings described in this section weregenerated on a commodity PC with an Intel i7-2600 8-coreCPU and a NVIDIA GTX 680 GPU (for GPU-based Poisson-RRT).
A. Poisson-RRT on OMPL Benchmarks
For the first experiment, we used four well-known bench-mark scenarios from OMPL, shown in Fig. 6. These planningproblems are all in 3D space, but vary in their complexities.Some have narrow passages and are more challenging thanothers.
For each benchmark, we evaluate the performance of ourdifferent GPU-based planner implementations, the GPU-basedAND parallel RRT and the parallel Poisson-RRT with theadaptive sampling. We set these GPU-based planners to ex-pand 32 nodes in parallel, which can exploit GPU many-coresfor the nearest neighbor search and the collision checkingcomputations using parallel algorithms of Locality-SensitiveHashing and OBB tree bounding volume hierarchies. Wecompare the GPU-based planners with the following existingCPU-based RRT variant algorithms available in OMPL andthe details of the comparison are given in [51]:• Standard RRT (RRT-Extend) [2] : Sequential RRT that
uses random uniform sampling.• RRT-Connect [7] : Bidirectional algorithm that expands
trees from both the initial and the goal configurations.
(a) Easy (b) Cubicle
(c) Alpha Puzzle (d) Apartment
Fig. 6. The planning problems used as the benchmarks of various planners.Easy moves a robot from the left room to the right room by passing a window;Cubicles moves the robot in an office environment; Alpha puzzle contains avery narrow passage; Apartment moves the piano to the hallway near thedoor entrance. Alpha puzzle and Apartment benchmarks are relatively morechallenging than Easy and Cubicles benchmarks.
0
5
10
15
20
25
Easy Cubicle AlphaPuzzle Apartment
Spe
ed
up
RRT (Single CPU core)
pRRT (CPU AND Parallel RRT)
GPU AND Parallel RRT
GPU Poisson-RRT
4.9x
12.1x
6.4x
24.9x
16.1x
8.1x
3.6x
16.9x
9.6x
1.9x 3.9x
1.5x
Fig. 7. Speedup of GPU-based algorithms from the original RRT algorithm,which uses a single CPU core. GPU-based Poisson-RRT improves theperformance of CPU-based algorithm up to 25 times. Poisson-RRT providesadditional 50-100% speedup as compare to the GPU-based AND Parallel RRTimplementation. The CPU-based pRRT algorithm would expand 8 nodes inparallel. The GPU-based implementation would expand 32 nodes in parallel,and also used GPU-based parallel nearest neighbor and collision detectionalgorithms.
• Lazy-RRT : A variant of RRT algorithm that deferscollision checks until it finds a solution, which is basedon Lazy-PRM [52] technique.
• pRRT [24] : AND parallel RRT algorithm on CPU 8-cores.
The performance of RRT-based planning algorithms is gov-erned by the maximum extension distance ε. A smaller εneeds to generate more nodes to find the solution, while alarger ε causes more failures in the local planning. Similarly,the performance of the Poisson-RRT algorithm is affected bythe radius of the precomputed Poisson-disk samples r. Weset the ε for different benchmarks using the default OMPLcomputation, which is proportional to the workspace size ofthe benchmark. We set r = 2
3ε for Poisson-RRT algorithms.
IEEE TRANSACTIONS ON ROBOTICS 9
CPU-based GPU-based (32 threads)# of threads single-threaded 8 threads 32 threadsAlgorithm RRT RRT-Connect LazyRRT Poisson-RRT pRRT pRRT Poisson-RRTBenchmark Mean Std.dev. Mean Std.dev. Mean Std.dev. Mean Std.dev. Mean Std.dev. Mean Std.dev. Mean Std.dev.
TABLE IPERFORMANCE OF RRT-BASED PLANNING ALGORITHMS ON DIFFERENT BENCHMARKS. WE REPORT PLANNING TIME FOR EACH CASE. THE MEAN AND
STANDARD DEVIATION ARE COMPUTED FROM 100 TRIALS ON EACH BENCHMARK. CPU-BASED PRRT UTILIZES 8 THREADS TO FULLY EXPLOIT THE8-CORE CPU. GPU-BASED ALGORITHMS USE 32 THREADS FOR THE COMPUTATION. *RRT ALGORITHM CANNOT FIND SOLUTION IN SOME INSTANCES
AND THOSE ARE TAKEN IN ACCOUNT IN COMPUTING THE AVERAGE.
The mean and standard deviation of the total time takenby the planner are shown in Table I. The means and standarddeviations are computed from 100 trials for each benchmark.Fig. 7 shows the parallel algorithm’s planning-time speedupon the OPML benchmarks as compared to the original CPU-based RRT algorithm, which uses a single core.
Based on these experimental results, we observe that:• The performance of single-threaded Poisson-RRT is not
always better than the original RRT, which is due tothe required additional computations such as adaptivesampling.
• In general, our GPU-based Poisson-RRT is faster thanthe original CPU-based algorithms, providing up to 25Xspeedup over the CPU algorithms.
• The performance improvement of the parallel planners(pRRT and Poisson-RRT) over the sequential planners ismore significant in narrow passage scenarios (AlphaPuz-zle and Apartment) than in open space scenarios (Easyand Cubicle). This is because the main advantage ofparallel RRTs is their capability to perform explorationand exploitation simultaneously. Suppose there are twoways of connecting the initial and the goal configurations.One is closer to the initial configuration but has a narrowpassage, and the other is further away but is moreopen. Sequential RRTs may get stuck before the narrowpassage for a while, and cannot make progress until theyeventually find the open corridor. The parallel plannerscan perform exploration more efficiently, and thus wouldfind the further away open passage earlier than traditionalRRTs. However, in open space scenarios, the explorationadvantage of parallel RRTs is less significant becausesequential RRTs can easily find a solution without toomuch exploration and the computational overhead ofparallel RRTs becomes more important.
B. Comparison of Sampling Algorithms
In order to evaluate the benefit which comes solely from theuse of Poisson-disk samples, we compare the performance ofplanners with different sampling algorithms. In addition to thePoisson-disk sampling and random sampling, we also evaluatethe performance of grid-based and Hammersley [53] samples,and modify Poisson-RRT to use grid-based or Hammersleysamples instead of the precomputed Poisson-disk samples. Forthe grid-based samples, we generate precomputed samples onan axis-aligned grid with a cell size 2r/
TABLE IIPERFORMANCE OF GPU RRT PLANNING WITH DIFFERENT SAMPLING
ALGORITHMS. PLANNER USE THE SAME NEAREST NEIGHBOR ANDCOLLISION CHECKING ALGORITHMS.
samples in half-size grids when the adaptive sampling isrequired. For the Hammersley samples, which have an de-terministic order satisfying the low-discrepancy, we generatethe same number of samples with the Poisson-RRT for thesamples generated in the precomputation and the runtime.Table II shows the result of the four GPU-based planners,GPU Poisson-RRT, GPU AND Parallel RRT, and planners usesgrid-based and Hammersley samples. It shows that the use ofPoisson-disk sampling improves 50-100% performance thanthe use of random samples, even though they use the sameGPU parallelism. As mentioned in Remark 6.3 (Section VI),the speedup is more significant in benchmarks with large openspaces. However, the performance of a planner that uses grid-based samples varies in different benchmarks. In particular, inthe benchmarks where the the axis-aligned grid based samplescan find a good solution, the performance of that planner withgrid-based samples is close to that of Poisson-RRT algorithm.However, the performance of grid-based path planner is worsethan random-sampling in the Alpha puzzle benchmark. Thesebenchmarks have a narrow passage that does not along withthe axes and therefore, grid-based planners do not performwell. The planner with Hammersley samples shows betterperformance in narrow passage scenarios, but not as good asthe planner with Poisson-disk samples.
In the next set of experiments, we compared the planningperformance of precomputed Poisson-disk samples using dif-ferent radii. We also compare our adaptive-sampling plan-ners’ planning time to that of the samplers using only theprecomputed Poisson-disk samples. The result for benchmark‘Easy’ (Fig. 6(a)) is shown in Table III. The uniform samplingplanner has the best performance when the sample radius is 32,but the adaptive sampling planner shows better performancewith a larger radius; this indicates that our adaptive-samplingapproach improves the performance by generating fewer sam-
TABLE IIIPERFORMANCE OF POISSON-RRT ALGORITHM WITH DIFFERENT SAMPLE
RADII FOR ‘EASY’ BENCHMARK (FIG. 6(A)). WE COMPARE THEPLANNING TIME OF OUR ADAPTIVE SAMPLING APPROACH WITH A
PLANNER THAT ONLY USES PRECOMPUTED SAMPLES. WE OBSERVEIMPROVED PERFORMANCE WITH OUR ADAPTIVE SAMPLING APPROACH.
0
8
16
24
32
0 8 16 24 32
Spe
ed
up
# of Threads
PoissonRRTpRRT
Fig. 8. Speedup of parallel Poisson-RRT with the number of parallelthreads. The speedup is computed based on the sequential RRT algorithmfor Apartment benchmark.
ples. The result also shows that a too-small sample radiusdecreases the planning performance due to the exponentialincrease in the number of samples.
C. Scalability Comparison Among Planners
In order to evaluate the scalability of the planners, weshow the speedup of the parallel Poisson-RRT algorithmbased on increasing the number of threads on a 24 core (48threads) workstation in Fig. 8. Table IV shows the planningtime for both CPU and GPU-based Poisson-RRT plannerswith different number of threads. Our Poisson-RRT algorithmshows near-linear speedup as the number of parallel threadsincreases for both CPU and GPU versions in the commonthread number range (∼ 16) of commodity PCs, while pRRT(AND parallel RRT) does not. At the large numbers (> 24)of threads, the CPU-based planner is affected by the threadsynchronization and has a sublinear performance. However,the GPU-based planner maintains the scalability (see the resultof AlphaPuzzle in Table IV) with 32 threads.
There are several parallel RRT algorithms that can alsoachieve linear speedups on different computing systems. Ja-cobs et al. [20] use configuration space subdivision andparallel nearest neighbor search. This algorithm also includesa technique to balance the load between local computationand global computation for distributed systems. Ichnowski
0%
20%
40%
60%
80%
100%
120%
Pla
nn
ing
Tim
e
Alpha Puzzle 1.5
Communication
CollisionCheck
Nearest
Sampling
Easy Apartments
Fig. 9. Timing breakdown among various components for RRT, pRRT, andparallel Poisson-RRT algorithms for different benchmarks.
and Alterovitz [30] use a similar approach, partition-basedsampling on shared-memory systems. In order to reduce thesynchronization overhead, they use lock-free data structures.However, none of these methods have been evaluated oncommodity many-core GPUs. As described in [30], thesealgorithms does not map well to single-instruction multiple-data (SIMD) GPU architectures. In partition-based algorithms,node expansions avoid generation of redundant close nodes bygenerating new nodes in separated regions of the configurationspace, but it also makes the SIMD execution of multipleexpansions less efficient due to lack of locality. There aretree-based planning approaches that use GPUs [35], but theparallelization in these algorithms is only limited to collisionchecking, and not the entire tree expansion, which requiresextra CPU-GPU data transmission for each iteration.
Our extendMPDS() procedure maps well to SIMD-basedGPU architectures. The use of precomputed Poisson-disksamples allows efficient expansion of the tree on GPUs with-out generating redundant nodes. It also exploits the parallelnearest neighbor search (Section V-B) and collision checking(Section V-C).
Furthermore, the use of precomputed samples allows ourPoisson-RRT to outperform the original RRT algorithm insome complex benchmarks, even for single-threaded cases. Asdiscussed in Section VII-A, the precomputed samples reducethe number of samples generated at run-time and improve theoverall performance.
On the other hand, the results in Table. II shows thatthe performance improvement from the use of Poisson-disksampling for some benchmark (e.g. Alpha Puzzle) is less thanothers. As it can be inferred from the results in Table. III, thespeed up mainly comes from the exploiting the precomputedsamples. If the number of precomputed samples is inadequatefor the complexity of the benchmark scenarios, it causesadditional runtime adaptive sample generations which degen-erate the planning performance. However, in such complexenvironments the Poisson-RRT shows better performance thanthe planner with grid-based sampling which is worse than therandom sampling-based planner.
Fig. 9 shows the timing breakdown of the parallel planningalgorithms corresponding to pRRT and parallel Poisson-RRT.The percentage of time spent in nearest neighbor computation
TABLE IVPERFORMANCE OF PARALLEL POISSON-RRT ALGORITHMS WITH DIFFERENT NUMBER OF THREADS. WE REPORT THE PLANNING TIME FOR EACH
BENCHMARK CASE.
is reduced in Poisson-RRT computation, as it exploits themaximal properties of Poisson-disk samples. On the otherhand, nearest neighbor computation takes a higher fractionof total time in pRRT and this nearestNode computationis a major source of inefficiency for pRRT.
D. High-DOF Robot Planning
RRT (Single CPU Core) 6.17sGPU Poisson-RRT 0.32sSpeed up 19.28x
Fig. 10. Motion planning of 23 DOFs HRP-4 robot using parallel Poisson-RRT algorithm.
Sampling-based planning algorithms like RRT are preferredfor high-DOF planning, because they do not suffer from thecurse-of-dimensionality. As described in Section II-C, thereare no practical algorithms for computing maximal Poisson-disk samples in high-dimensional spaces, but relaxed Poisson-disk sampling algorithm [38] can be used to generate appro-priate samples.
We evaluate our GPU-based parallel Poisson-RRT algorithmwith the precomputed relaxed Poisson-disk samples for plan-ning of HRP-4 robot in a simulation environment shown inFig. 10. The environment has several static obstacles, and abook is attached to the right hand of the robot. We computea collision-free motion for given initial and goal poses of theupper body of HRP-4 robot, which has 23 DOFs. We assumethat the lower body of the robot is fixed, and do not considerthe dynamics constraints of the robot.
We measure the planning time of the GPU-based parallelPoisson-RRT with respect to single-core CPU algorithm. Asshown in table of Fig. 10, our parallel Poisson-RRT algorithmcomputes a collision-free path in real-time using a precom-puted relaxed Poisson-disk sample set, while the single-core
CPU RRT takes several seconds to find a solution. It showsthat our parallel Poisson-RRT algorithm can improves theperformance of the planner for high-DOF scenarios.
VIII. LIMITATIONS, CONCLUSIONS, AND FUTURE WORK
In this paper, we have presented a new RRT-based motionplanning algorithm based on Poisson-disk sampling. It uses anadaptive maximal Poisson-disk sampling approach to reducethe number of nodes in the resulting tree and explore the freespace. Our algorithm is based on the RRT motion-planningalgorithm and exploits the multiple cores on GPUs.
Our algorithm has some limitations. The maximal Poisson-disk sampling algorithm that we used may require a largeamount of memory to execute its precomputation step in high-dimensional spaces, especially when r is small. Our currentformulation takes into account only collision-free constraints,not non-holonomic or dynamic constraints. We only observegood speedups in challenging scenarios and in the parallelversion of the algorithm.
There are many avenues for future work. The performanceof our planning algorithm can be considerably improved byvarious optimizations, used for adaptive sampling or treeexpansions, including bidirectional search similar to that usedby RRT-Connect. The performance of our CPU-based parallelplanner on workstations or clusters with very large num-bers of CPU cores can be improved using lock-free datastructures [30]. We would like to investigate techniques forautomatically computing the optimal r for Poisson-disk sam-pling, for varying configuration space boundaries of higher-dimensional problems. It would also be useful to take intoaccount non-holonomic constraints.
IX. ACKNOWLEDGMENTS
This research is supported in part by ARO ContractW911NF-10-1-0506, NSF awards 1000579, 1117127 and1305286, and a grant from Sandia Labs.
REFERENCES
[1] L. Kavraki, P. Svestka, J. Latombe, and M. Overmars, “Probabilisticroadmaps for path planning in high-dimensional configuration spaces,”Transactions on Robotics and Automation, vol. 12, no. 4, pp. 566–580,1996.
[2] S. LaValle and J. Kuffner, “Randomized kinodynamic planning,” In-ternational Journal of Robotics Research, vol. 20, no. 5, pp. 378–400,2001.
[3] R. L. Cook, “Stochastic sampling and distributed ray tracing,” in Anintroduction to ray tracing. Academic Press Ltd., 1989, pp. 161–199.
[4] A. Glassner, An introduction to ray tracing. Morgan Kaufmann, 1989.
IEEE TRANSACTIONS ON ROBOTICS 12
[5] A. Lagae and P. Dutre, “A comparison of methods for generating poissondisk distributions,” in Computer Graphics Forum, vol. 27, no. 1. WileyOnline Library, 2008, pp. 114–129.
[6] C. Park, J. Pan, and D. Manocha, “Poisson-RRT,” in Robotics andAutomation, 2014. Proceedings., 2014 IEEE International Conferenceon. IEEE, 2014, pp. 4667–4673.
[7] J. Kuffner Jr and S. LaValle, “RRT-connect: An efficient approach tosingle-query path planning,” in International Conference on Roboticsand Automation, vol. 2, 2000, pp. 995–1001.
[8] A. Yershova, L. Jaillet, T. Simon, and S. M. LaValle, “Dynamic-domainRRTs: Efficient exploration by controlling the sampling domain,” inInternational Conference on Robotics and Automation, 2005, pp. 3867–3872.
[9] S. Rodriguez, X. Tang, J.-M. Lien, and N. M. Amato, “An obstacle-basedrapidly-exploring random tree,” in Robotics and Automation, 2006. ICRA2006. Proceedings 2006 IEEE International Conference on. IEEE,2006, pp. 895–900.
[10] R. Diankov, N. Ratliff, D. Ferguson, S. Srinivasa, and J. Kuffner,“Bispace planning: Concurrent multi-space exploration,” in Robotics:Science and Systems, 2008.
[11] S. Rodriguez, S. Thomas, R. Pearce, and N. M. Amato, “Resampl: Aregion-sensitive adaptive motion planner,” in Algorithmic Foundation ofRobotics VII. Springer, 2008, pp. 285–300.
[12] A. Shkolnik and R. Tedrake, “Sample-based planning with volumes inconfiguration space,” 2011, coRR, vol. abs/1109.3145.
[13] J. Denny, M. Morales, S. Rodriguez, and N. M. Amato, “Adaptingrrt growth for heterogeneous environments,” in Intelligent Robots andSystems (IROS), 2013 IEEE/RSJ International Conference on. IEEE,2013, pp. 1772–1778.
[14] S. Karaman and E. Frazzoli, “Sampling-based algorithms for optimalmotion planning,” International Journal of Robotics Research, vol. 30,no. 7, pp. 846–894, 2011.
[15] O. Arslan and P. Tsiotras, “Use of relaxation methods in sampling-basedalgorithms for optimal motion planning,” in Robotics and Automation(ICRA), 2013 IEEE International Conference on. IEEE, 2013, pp.2421–2428.
[16] O. Salzman and D. Halperin, “Asymptotically near-optimal rrt for fast,high-quality, motion planning,” in Robotics and Automation (ICRA),2014 IEEE International Conference on. IEEE, 2014, pp. 4680–4685.
[17] T. Lozano-Perez and P. A. O’Donnell, “Parallel robot motion planning,”in Robotics and Automation, 1991. Proceedings., 1991 IEEE Interna-tional Conference on. IEEE, 1991, pp. 1000–1007.
[18] N. M. Amato and L. K. Dale, “Probabilistic roadmap methods are em-barrassingly parallel,” in Robotics and Automation, 1999. Proceedings.1999 IEEE International Conference on, vol. 1. IEEE, 1999, pp. 688–694.
[19] R. Brooks and T. Lozano-Perez, “A subdivision algorithm in configu-ration space for findpath with rotation,” Transactions on Systems, Manand Cybernetics, vol. 15, no. 2, pp. 224–233, 1985.
[20] S. Jacobs, K. Manavi, J. Burgos, J. Denny, S. Thomas, and N. Amato,“A scalable method for parallelizing sampling-based motion planningalgorithms,” in International Conference on Robotics and Automation,2012, pp. 2529–2536.
[21] S. A. Jacobs, N. Stradford, C. Rodriguez, S. Thomas, and N. M.Amato, “A scalable distributed rrt for motion planning,” in Robotics andAutomation (ICRA), 2013 IEEE International Conference on. IEEE,2013, pp. 5088–5095.
[22] C. Rodriguez, J. Denny, S. A. Jacobs, S. Thomas, and N. M. Amato,“Blind rrt: A probabilistically complete distributed rrt,” in IntelligentRobots and Systems (IROS), 2013 IEEE/RSJ International Conferenceon. IEEE, 2013, pp. 1758–1765.
[23] E. Plaku and L. Kavraki, “Distributed sampling-based roadmap oftrees for large-scale motion planning,” in International Conference onRobotics and Automation, 2005, pp. 3868–3873.
[24] D. Devaurs, T. Simeon, and J. Cortes, “Parallelizing rrt on distributed-memory architectures,” in International Conference on Robotics andautomation, 2011, pp. 2261–2266.
[25] M. Otte and N. Correll, “Path planning with forests of random trees:Parallelization with super linear speedup,” Department of ComputerScience University of Colorado at Boulder, Tech. Rep. CU-CS, pp. 1079–11, 2011.
[26] B. Raveh, A. Enosh, and D. Halperin, “A little more, a lot better:Improving path quality by a path-merging algorithm,” Robotics, IEEETransactions on, vol. 27, no. 2, pp. 365–371, 2011.
[27] S. Carpin and E. Pagello, “On parallel rrts for multi-robot systems,” inItalian Association for Artificial Intelligence, 2002, pp. 834–841.
[28] I. Aguinaga, D. Borro, and L. Matey, “Parallel rrt-based path planningfor selective disassembly planning,” International Journal of AdvancedManufacturing Technology, vol. 36, no. 11, pp. 1221–1233, 2008.
[29] I. Sucan and L. E. Kavraki, “A sampling-based tree planner for systemswith complex dynamics,” Robotics, IEEE Transactions on, vol. 28, no. 1,pp. 116–131, 2012.
[30] J. Ichnowski and R. Alterovitz, “Parallel sampling-based motion plan-ning with superlinear speedup,” in Intelligent Robots and Systems(IROS), 2012 IEEE/RSJ International Conference on. IEEE, 2012,pp. 1206–1212.
[31] C. Pisula, K. Hoff, M. Lin, and D. Manocha, “Randomized path planningfor a rigid body based on hardware accelerated voronoi sampling,”in Proc. Workshop on Algorithmic Foundation of Robotics, vol. 18.Citeseer, 2000.
[32] J. Pan, C. Lauterbach, and D. Manocha, “g-planner: Real-time motionplanning and global navigation using gpus,” in AAAI Conference onArtificial Intelligence, 2010.
[33] J. T. Kider, M. Henderson, M. Likhachev, and A. Safonova, “High-dimensional planning on the gpu,” in Robotics and Automation (ICRA),2010 IEEE International Conference on. IEEE, 2010, pp. 2515–2522.
[34] C. Park, J. Pan, and D. Manocha, “Real-time optimization-based plan-ning in dynamic environments using gpus,” in Robotics and Automation(ICRA), 2013 IEEE International Conference on. IEEE, 2013, pp.4090–4097.
[35] J. Bialkowski, S. Karaman, and E. Frazzoli, “Massively parallelizing therrt and the rrt,” in International Conference on Intelligent Robots andSystems, 2011, pp. 3513–3518.
[36] A. Lagae and P. Dutre, “A procedural object distribution function,”Transactions on Graphics, vol. 24, no. 4, pp. 1442–1461, 2005.
[37] M. Ebeida, S. Mitchell, A. Patney, A. Davidson, and J. Owens, “A simplealgorithm for maximal poisson-disk sampling in high dimensions,”Computer Graphics Forum, vol. 31, no. 2, pp. 785–794, 2012.
[38] M. S. Ebeida, S. A. Mitchell, M. A. Awad, C. Park, L. P. Swiler,D. Manocha, and L. Wei, “Spoke darts for efficient high dimensionalblue noise sampling,” CoRR, vol. abs/1408.1118, 2014. [Online].Available: http://arxiv.org/abs/1408.1118
[39] S. M. LaValle, Planning algorithms. Cambridge university press, 2006.[40] H. Niederreiter, Quasi-Monte Carlo Methods. Wiley Online Library,
1992.[41] R. Bohlin, “Path planning in practice; lazy evaluation on a multi-
resolution grid,” in Intelligent Robots and Systems, 2001. Proceedings.2001 IEEE/RSJ International Conference on, vol. 1. IEEE, 2001, pp.49–54.
[42] M. Likhachev and D. Ferguson, “Planning long dynamically feasiblemaneuvers for autonomous vehicles,” The International Journal ofRobotics Research, vol. 28, no. 8, pp. 933–945, 2009.
[43] M. Pivtoraiko and A. Kelly, “Differentially constrained motion replan-ning using state lattices with graduated fidelity,” in Intelligent Robotsand Systems, 2008. IROS 2008. IEEE/RSJ International Conference on.IEEE, 2008, pp. 2611–2616.
[44] E. Kushilevitz, R. Ostrovsky, and Y. Rabani, “Efficient search for ap-proximate nearest neighbor in high dimensional spaces,” SIAM Journalon Computing, vol. 30, no. 2, pp. 457–474, 2000.
[45] G. Marsaglia et al., “Choosing a point from the surface of a sphere,” TheAnnals of Mathematical Statistics, vol. 43, no. 2, pp. 645–646, 1972.
[46] V. Garcia, E. Debreuve, and M. Barlaud, “Fast k nearest neighbor searchusing GPU,” in Computer Vision and Pattern Recognition Workshops,2008. CVPRW’08. IEEE Computer Society Conference on. IEEE, 2008,pp. 1–6.
[47] J. Pan, C. Lauterbach, and D. Manocha, “Efficient nearest-neighborcomputation for GPU-based motion planning,” in Intelligent Robots andSystems (IROS), 2010 IEEE/RSJ International Conference on. IEEE,2010, pp. 2243–2248.
[48] S. Gottschalk, M. C. Lin, and D. Manocha, “OBBTree: a hierarchicalstructure for rapid interference detection,” in Proceedings of the 23rdannual conference on Computer graphics and interactive techniques.ACM, 1996, pp. 171–180.
[49] C. Lauterbach, M. Garland, S. Sengupta, D. Luebke, and D. Manocha,“Fast BVH construction on GPUs,” in Computer Graphics Forum,vol. 28, no. 2. Wiley Online Library, 2009, pp. 375–384.
[50] I. A. Sucan, M. Moll, and L. E. Kavraki, “The Open Motion PlanningLibrary,” IEEE Robotics & Automation Magazine, vol. 19, no. 4, pp.72–82, 2012, http://ompl.kavrakilab.org.
[51] C. Park, J. Pan, and D. Manocha, “Parallel RRT using Poisson-disk sam-pling,” Department of Computer Science, University of North Carolinaat Chapel Hill, Tech. Rep., 2013.
IEEE TRANSACTIONS ON ROBOTICS 13
[52] R. Bohlin and L. Kavraki, “Path planning using lazy prm,” in Interna-tional Conference on Robotics and Automation, vol. 1, 2000, pp. 521–528.
[53] J. M. Hammersley, “Monte carlo methods for solving multivariableproblems,” Annals of the New York Academy of Sciences, vol. 86, no. 3,pp. 844–874, 1960.
Chonhyon Park received the B.S. degree and theM.S. degree in computer science and engineeringfrom the Seoul National University, Seoul, SouthKorea in 2005 and 2007, respectively. He is currentlyworking toward the Ph.D. degree with the Depart-ment of Computer Science, University of NorthCarolina at Chapel Hill, Chapel Hill, NC.
He was an Intern with Honda Research Institute,Mountain View, CA, Samsung Research America,San Jose, CA, and Disney Research, Glendale, CA.His current research interests include motion and
path planning, navigation of virtual characters, and many-core computing.
Jia Pan received his B.E. degree from the Depart-ment of Automation, Tsinghua University in 2005,a M.S. degree from the National Laboratory of Pat-tern Recognition, Institute of Automation, ChineseAcademy of Sciences in 2008, and his Ph.D. degreein Computer Science from the University of NorthCarolina at Chapel Hill, NC in 2013. He was apostdoctoral researcher in the EECS department atUniversity of California, Berkeley.
He joined the faculty in the Department of Com-puter Science at the University of Hong Kong in
2014. His research interests include motion planning, GPGPU, and machinelearning for robotics.
Dinesh Manocha is currently the Phi DeltaTheta/Mason Distinguished Professor of ComputerScience at the University of North Carolina atChapel Hill. He received his Ph.D. in ComputerScience at the University of California at Berke-ley 1992. Along with his students, Manocha hasalso received 14 best paper awards at the lead-ing conferences. He has published more than 450papers and some of the software systems relatedto collision detection, GPU-based algorithms andgeometric computing developed by his group have
been downloaded by more than 150,000 users and are widely used in theindustry. He has supervised 30 Ph.D. dissertations and is a fellow of ACM,AAAS, and IEEE. He received Distinguished Alumni Award from IndianInstitute of Technology, Delhi.
