Sampling based time efficient path planning algorithm for mobile platforms

Abstract

In this paper we present a time efficient one step path planning
algorithm for navigating a large robotic platform in indoor
environments. The proposed strategy, based on the generation
of a novel search space [1], relies on non-uniform
density sampling of the free areas to direct the computational
resources to troubled and difficult regions, such as narrow
passages, leaving the larger open spaces sparsely populated.
A smoothing penalty is also associated to the nodes to encourage
the generation of gentle paths along the middle of
the empty spaces. Collision detection is carried out off-line
during the creation of the configuration space to speed up
the actual search for the path, which is done on-line. Results
compared to currently available path planning algorithms
such as Randomly-exploring Random Trees (RRTs)
and Probabilistic Road Maps (PRMs) proved that the proposed
approach considerably reduces the searching time and
produces smoother paths with less jagged path segments than
those from randomized planners.

Full text

Sampling Based Time Efficient Path Planning Algorithm for Mobile Platforms
Tarek Taha, Jaime Valls Miró and Gamini Dissanayake
ARC Centre of Excellence for Autonomous Systems
Mechatronics and Intelligent Systems Group
University of Technology Sydney
NSW2007, Australia
{t.taha, j.vallsmiro, g.dissanayake}@cas.edu.au
Abstract
In this paper we present a time efficient one step path plan-
ning algorithm for navigating a large robotic platform in in-
door environments. The proposed strategy, based on the gen-
eration of a novel search space [1], relies on non-uniform
density sampling of the free areas to direct the computational
resources to troubled and difficult regions, such as narrow
passages, leaving the larger open spaces sparsely populated.
A smoothing penalty is also associated to the nodes to en-
courage the generation of gentle paths along the middle of
the empty spaces. Collision detection is carried out off-line
during the creation of the configuration space to speed up
the actual search for the path, which is done on-line. Re-
sults compared to currently available path planning algo-
rithms such as Randomly-exploring Random Trees (RRTs)
and Probabilistic Road Maps (PRMs) proved that the pro-
posed approach considerably reduces the searching time and
produces smoother paths with less jagged path segments than
those from randomized planners.
Keywords
Motion planning, path planning, navigation
1 Introduction
Motion planning has been studied for several decades and
many algorithms have been described in the literature [2].
It has been proved that the motion planning problem is a
PSPACE hard problem [3]. Randomized Path Planner (RPP)
[4] is an example of one of the important practical algorithms
that has been used in several applications. Motion planning
is considered as one of the most important fields of study in
the task of building autonomous or semi-autonomous robot
systems and is characterized by the ability to compute a
collision-free path from an initial pose to a goal position in
between static obstacles or maintaining a set of constraints in
the state of the world such as following a target, achieving a
knowledge about the world like map building or exploration
in an unknown environment. Many applications where built
that rely on motion planning such as: systems for robot guid-
ance [5], assembly and disassembly [6], computer anima-
tion, fold proteins and dock ligands [7]. Most of the current
approaches to path planning are based on the concept of con-
figuration space (C-space) introduced by Lozano-Pérez [8]
and Wesley [9]. Here, a robot with kdegrees of freedom can
be described by kvalues, which can in turn be considered
as a single point in a k-dimensional C-space of the robot.
This configuration is considered free if two parts touch and
blocked when two parts overlap.
The exact construction of the C-space is however a com-
putationally expensive solution to the path planning prob-
lem [10]. The need to move away from complete path plan-
ning algorithms inspired the development of sampling-based
techniques. Hence, the majority of techniques make further
assumptions and construct approximate representation of the
C-space using sampling-based techniques. These techniques
provided a faster practical solution by sacrificing complete-
ness, in which a set of sampling points are used to represent
the C-space that is used in constructing solutions. Tradition-
ally, sampling-based algorithms are based on uniform sam-
pling which considers the whole environment as uniformly
complex and thus the overall sampling density will be equiv-
alent to the density needed by the most complex region. The
result is that every region in the C-space has the same com-
putational complexity, reaching its worst case when narrow
passage areas exist in the environment [11]. Furthermore,
paths produced by randomized planners usually contain non-
smooth segments because of this randomness and the ab-
sence of optimization criteria.
For the problem of navigating large robots in narrow
and cluttered environments, conventional path planning al-
gorithms based on free C-space construction also tend to
fail: in order to be able to consider the robot as a k-
dimensional point, they generally expand the obstacles in an
over-simplistic manner by the length of the larger robot di-
mension, which very often will prevent reaching a solution
even when it exists [8].
In this paper we propose a hybrid time-efficient path plan-
ning algorithm inspired by the C-space approach, where we
avoid the computational complexity of generating a denser
search area by employing a non-uniform sampling density:
this is increased in complex areas, leaving simple areas with
lower resolution density, hence directing computational re-
sources towards the complex areas, also know as narrow pas-
sages. A reduction of the information embedded in the C-
1

space, and a smoothing cost function are also introduced to
generate smoother paths in an efficient manner. The algo-
rithm takes further advantage of techniques like the bridge
test [12] and an optimized obstacle expansion method to fur-
ther reduce the number of samples and the points to be check
for obstacle collision. A modified A* search is then imple-
mented to find suitable paths on this space. The result is
a time-efficient path planner with smooth and cost optimal
paths.
The remainder of this paper is organized as follows: latest
proposals to the path planning problem and where our ap-
proach represents an improvement is analyzed in Section 2.
The methodology used for the creation of the search space
is presented in Section 3, with Section 3.1 focusing on the
non-uniform random discretization. Section 3.2 summarizes
the customizations to the A* graph search technique to take
advantage of the search framework proposed here. Detailed
simulation results and a comparison with randomized plan-
ners is provided in Section 4. Finally, Section 5 summarizes
the contributions of this paper and future work.
2 Background
In general terms, the sampling algorithms developed to
construct an approximate representation of the free space
currently available can be divided into two: single-query
and multiple-query approaches. Multiple-query approaches
starts with a pre-processing step that usually takes a large
amount of time but makes solving path planning problems
in the same environment faster. Probabilistic Roadmaps
(PRMs) [13] is an example of a multi-query approach that
initially used uniform sampling in constructing the path. This
method was problematic because the entire C-space will be
sampled with a density required by the most complex area
of the environment, such as a narrow passage area. Nowa-
days, PRMs are moving into a non-uniform methods for sam-
pling such as the Gaussian sampling method [14], and the
bridge test to insure that most of the configurations in C-
space are actually close to obstacles or inside a narrow pas-
sage, thus reducing the unnecessary samples and decreas-
ing the computational time. Single-query methods were
developed to avoid the large pre-computational time that
the multi-query methods take, and they have been proved
to be efficient [15]. Randomly-exploring Random Trees
(RRTs) [16, 17] are mainly based on single-query meth-
ods. They have gained popularity from their good perfor-
mances, which has lead to a number of extensions specifi-
cally targeting the solution to complicated geometrical prob-
lems [18], such as the deterministic resolution-complete al-
ternatives that have been proposed to replace the random
sampling methods in [19]. In many cases, an optimal and not
just a feasible path is required. As a result of randomness,
the paths generated by the execution of the above planners
are very often sub-optimal and non-smooth. A two-phase ap-
proach was proposed in [20] to optimize paths generated in
the special case where the first-phase path planned is made
up of straight line segments connected by way-points. An-
other two-phase planning algorithm based on RRT was de-
veloped in [21]. This algorithm can compute low cost paths
given a desired cost function by a numerical gradient descent
algorithm that minimizes the Hamiltonian of the entire path.
The approach proposed here is based on a simple multi-query
one-phase planner that addresses the optimality and smooth-
ness weaknesses of probabilistic path planning algorithms,
a desirable characteristic for the general case of motion of
large platforms. The planner uses an a-priory map of the en-
vironment to calculate an offline, minimal free search space,
where and additional smoothness cost function is used to ad-
dress the issues associated with smoothly navigating a large
robot in an environment with narrow passages and obstacles.
3 Path planning algorithm
An overview of the proposed algorithm is given below. Fur-
ther details can be obtained from [1].
3.1 Generating the search space
The pre-processing step aims at minimizing the on-line com-
putation by pre-generating a search space to contain all the
information that will be used during the on-line path plan-
ning, while at the same time avoid generating an unnecessar-
ily complete and complex space. The steps used during the
search space creation can be defined as follows:
Algorithm 1 Search-space generation
Input: map, robot dimensions
1. Expand Obstacles.
2. Generate Regular Grid with low resolution.
3. Apply bridge test to add dense narrow passages.
4. Penalise nodes by adding a smoothing cost.
5. Connect nodes to form search space discretization.
6. Eliminate those that cause collision.
Output: free search space.
3.1.1 Collision Detection
In this step obstacles are enlarged with a radius Rto sim-
plify the on-line collision detection by reducing the number
of points on the robots to be checked for collision. The tradi-
tional approach is based on expanding the obstacles by a ra-
dius requivalent to the robots largest dimension, hence plan-
ning as if the robot could navigate as a point in the environ-
ment. This over-simplification, however, is not suitable for
the case of large robots in constrained spaces, as expanding
the obstacles along narrow passages will effectively block the
passage, as shown in Figure 1.
A more suitable solution is proposed by finding the largest
possible expansion radius Rthat allows the robot to pass
through the narrowest path and then divide the area of the
robot into circles of that radius, as depicted in Figure 2. The
centre points of those circles will then be used to check for
obstacle collision. The expansion radius Ris determined
2

Figure 1: Narrow passage blocked as a result of largest robot
dimension obstacle expansion
Figure 2: The area of the robot is covered by circles of radius
R, the centres of these circles will be the points to be checked
for collision
based on the a-priory knowledge of the environment: sup-
pose that the narrowest passages is of width land the largest
robot dimension is rthen the largest expansion that allows
the robot to pass through can be determined by:
R=½l−ε
2if l<2r
rotherwise (1)
where εis a minimal safety distance to make sure the plat-
form does not get uncomfortably close to the obstacles.
3.1.2 Regular grid discretization
The C-space is then populated with nodes using a low reso-
lution regular grid. This will help in maintaining the connec-
tivity of the graph by defining a minimum discretization for
the open spaces. The discretization density is adjusted to suit
the environment, selecting as sparse a grid as possible. Up to
this stage the nodes hold only position information.
3.1.3 Bridge test
The bridge test [12] was introduced to boost the sampling
density inside narrow passages using only a simple test of
the local geometry. A short line segment of length dcan
sample randomly through a point min the free space such
that the end points of the line segment lie in obstacles. This
line segment is what we call a bridge because it acts like a
bridge across the narrow passage with its endpoints in an oc-
cupied location and the point min a free space. If we are able
to build a bridge through point m, then the bridge test is suc-
cessful at this point and point mis added to the search space.
Building short bridges is easier in narrow passages than in
Algorithm 2 Bridge Test
1. repeat
2. Pick a point pfrom the regular-grid map
3. If pis in an occupied location then
4. Pick a point p0that is ddistance away from p
5. If p0is in an occupied location then
6. Let mbe the midpoint of p p0line segment
7. If mis in a free location then
8. Insert minto the search space as a new node
free space and by favouring short bridges we increase the
chance of getting point in the narrow passages. The off-line
test increases the density of free-space sample points to our
search space where it matters most, in the narrow passages,
instead of the whole region. Figure 4 (further described later
in Section 4) shows the result of the bridge test on a map with
narrow passages.
3.1.4 Clearance (smoothness) penalty
To insure that the paths generated are directed to the middle
of the empty space not adjacent to the obstacles, we intro-
duced a cost added to the the nodes in the search space in-
dicating how far they are from an obstacle. The cost Cpis a
normalized cost that is inversely proportional to this distance
d, so that the closer the point is from an obstacle the higher
its cost, according to:
Cp=D−d
D(2)
where Dindicates the clearance distance beyond which the
node will be assigned a zero cost. This cost will be used
during the on-line path planning process to plan smoother
paths in one step.
3.1.5 Node connections
In order to find a path among the resulting nodes, these need
to be connected together in a tree structure. This is done by
establishing a link between each node and its neighboring
nodes a certain distance away. The more neighbors a node is
linked to, the more discrete poses (position and orientation)
with be available during the search for a viable path.
3.1.6 Connections Reduction
In a final step we eliminate those connections that cause a
collision of the platform with the obstacles. The connec-
tions between nodes determines the possible orientations of
3

the robot should it follow that path. The center of the cir-
cles that describe the area of the robot along that path can
be rotated and translated accordingly. Hence we can then
determine if any of them falls into an occupied area or not,
removing those connecting nodes that will cause a collision.
The result will be a collision free search space of connected
nodes in which to efficiently carry out the on-line search for
a path.
3.2 On-line Path Planning
The A* path planning algorithm [22] is a well known tech-
nique, well regarded for its accuracy and calculation speed
in searching for an optimal solution. A* works by exploring
nodes based a cost function which is the sum of g(n), the cost
from the start node to node n, and the estimated cost from
node n to the goal h(n). It uses an heuristic search to esti-
mates the cost to the goal node and minimizes the cost of the
path so far. A* is optimal if the estimated cost to the goal is
always underestimated. Since the shortest distance between
two points is a straight line, euclidean distance serves as an
excellent estimated cost to goal, making A* well suited for
fast computations. In the algorithm proposed here, the cost
function J(d)combines the sum of the partial path distances
∆d, the sum of all the distances travelled as a result of chang-
ing orientation ∆θ xwhere xis the length of the axis of the
rear wheels, the sum of the clearance penalties Cppreviously
computed offline - which is directly proportional to the dis-
tance ∆d, the number of reversals (backward motion) in the
path nrev and the heuristic function hrepresenting the dis-
tance to goal at each step. The cost function is defined as
follows:
J(d) = ∑∆d+∑∆θ x+∑∆d Cp+∑nrev +h(3)
encourages the robot to avoid whenever possible turns and
reversing actions, while at the same time directing it towards
the middle of free space. The result is a smooth and secure
path - in the context of the obstacles around the platform -
efficiently generated in a single step.
4 Experimental Results
In order to compare the efficiency of the proposed algorithm,
we first compared the performance of a traditional uniform
regular sampling method with our random non-uniform sam-
pling approach for a simple navigation problem. The simu-
lations were carried out in a PC with a 1.8 GHz Pentium
IV processor, 512 Mb RAM, 2 MB L2 cache and an ATI
Radeon x700 graphics card. Using or method, a map of size
45x20mwas discretized uniformly by 0.1mto generate the
search space, a detailed section of which is shown in Fig-
ure 3, where white spaces represent the obstacles in the map
after having been expanded. This is the same density em-
ployed to discretized the tight passages with the non-uniform
sample method proposed here, where a bridge test with 2m
length was exercised. The resulting search map is that de-
picted in Figure 4, where a 0.2muniform sampling density
Figure 3: Uniform sampling: the whole environment is
equally mapped with a constant density of search nodes
Figure 4: Non uniform sampling: density is increased around
tight places, leaving open spaces with a lower number of
search nodes
was employed for the open spaces. In both cases the ob-
stacle expansion was set to 0.2m, and the starting and goal
configurations were the same. The comparison results in Ta-
ble 1 clearly show the computational advantages in terms of
time taken to plan the path with the random sampling tech-
nique proposed here. It can be seen how the addition of
search nodes where it really matters not only makes finding a
collision-free path feasible, but the additional penalties also
tend to direct the robot as far as possible from obstacles along
a smooth path. Further details of the actual implementation
on a real autonomous wheelchair can be found in [1].
4.1 Comparison with Randomized Planners
The performace of the proposed algorithm was compared
against Randomly-exploring Random Trees (RRTs) and
Probabilistic Road Maps (PRMs) with identical settings us-
ing the Motion Standard Library MSL developed by Steve
LaValle [23]. This library has the implementations of many
path planning algorithms and their different variants such as
4

Figure 5: Non uniform sampling: path generated using our
method
Table 1: Comparison of uniform and non-uniform C-space
generation
Uniform Sampling Random Sampling
no. nodes 32364 15634
no. connections 691155 129479
time 1.8589 sec 0.3228 sec
RRT, RRTConnet, RRTDual and PRM.
In this experiment we used the same map and computer
setup mentioned above, and provided the same initial and fi-
nal poses and motion model for all of the algorithms. For
our algorithm, the regular grid resolution was 0.5mand the
bridge test was applied with length 2.5mand a 0.2mreso-
lution. The generated search space and path are depicted in
Figure 5. In MSL we used the same map and set the rele-
vant delta time (DeltaT) applied to control actions to 0.1sec.
For RRT we chose RRTDual and RRTExtExt variants in our
experiments because of their computational advantages over
other variants. RRTDual is a dual-tree planner used in [24]
where a tree is extended toward a randomly-sampled point.
RRTExtExt is generally better than RRTDual in that it tries to
balance between growing trees toward each other and explor-
ing. The average results - collected over 20 runs - represent
the time taken to find the path, the number of collision detec-
tion calls and the number of nodes expanded, always using
the same settings. The comparison summarized in Table 2
showed that our algorithm, under the given conditions, per-
formed faster and required less collision detection calls than
RRTs and PRMs. It can be clearly seen from Figure 5 and
Figure 6 that the paths generated by the randomized planners
contain some unnecessary jagged path segments that might
not be optimal or realistic for traversing simple paths, whilst
the optimization cost function used in our algorithm helped
in generating smoother paths.
Figure 6: Path generated by MSL: this is an example of a path
generated using MSL library with the RRTExtExt planner
Table 2: Comparison between our method and some of the
most significant randomized planners available in the MSL
Algorithm C.D Nodes Exp. Time
RRTExtExt 154271 31734 34.44s
RRTDual 114229 3688 21.86s
PRM 4691 5000 8.05s
Our Algorithm 3423 6979 1.58s
5 Conclusion and further work
In this paper we have presented a new approach for generat-
ing time-efficient and optimized smooth paths. Whilst suit-
able for the generalal case of mobile platforms, it is maybe of
greater relevance when planning for large mobile platforms
in cluttered environments, such as a wheelchair at home,
where smoothnes is also an important factor to consider. A
non-uniform sampling technique has been proposed that can
efficiently target the narrow passage problem. Results were
compared with other randomized algorithms which proved
that for such environments our method performs faster and
generates smoother paths that can consequently be easily tra-
versed.
We are currently planning to extend the algorithm to han-
dle dynamic changes in the environment - such people mov-
ing about - and also to deal with motion planning in unknown
or partially known environments, where the map needs to be
built or at least modified while navigating, also known as
SLAM [25].
Acknowledgment
This work is supported by the Australian Research Council
(ARC) through its Centre of Excellence programme, and by
the New South Wales State Government. The ARC Centre of
Excellence for Autonomous Systems (CAS) is a partnership
between the University of Technology Sydney, the Univer-
5

sity of Sydney and the University of New South Wales.
References
[1] T. Taha, J. Valls Miró, D. Liu. An Efficient Path Planner
for Large Mobile Platforms in Cluttered Environments.
IEEE International Conference on Robotics, Automa-
tion and Mechatronics, 2006. (To be published)
[2] J. C. Latombe. Motion planning : A journey of robots,
molecules, digital actors, and other artifacts. Interna-
tional Journal of Robotics Research, vol. 18, no. 11,
pp. 1119-1128, 1999.
[3] J.H. Reif, Complexity of the Mover’s Problem and Gen-
eralizations, Proc. of the 20th IEEE Symposium on
Foundations of Computer Science, IEEE, New York,
421-427, 1979.
[4] J. Barraquand, J.C. Latombe, Robot Motion Planning:
A Distributed Representation Approach, The Interna-
tional Journal of Robotics Research 10, no. 6, 628-649,
Dec. 1991.
[5] W. Burgard, A.B. Cremers, D. Fox, D. Hahnel, G.
Lakemeyer, D. Schulz, W. Steiner, S. Thrun. The inter-
active museum tourguide robot. In Proc.of the Fifteenth
National Conference on Artificial Intelligence, Madi-
son, Wi, 1998.
[6] Chang, H., T.-Y. Lai, Assembly Maintainability Study
with Motion Planning, Proc. of the IEEE International
Conference on Robotics and Automation, Los Alami-
tos, CA, 1012-1019, 1995.
[7] D. Brutlag, M.S. Apaydin, C. Guestrin, D. Hsu, C.
Varma, A. Singh, J.-C. Latombe. Using robotics to fold
proteins and dock ligands. Bioinformatics, 18:S74-S83,
2002.
[8] T. Lozano-Pérez. Spatial planning: a configuration
space approach. IEEE Transactions on Computers, vol.
C-32, pp. 108-120, IEEE Press, 1983.
[9] T. Lozano-Pérez, M. Wesley, An Algorithm for Plan-
ning Collision-free Paths among Polyhedral Obstacles.
Communications of the ACM, Vol. 22, No. 10, 560-570,
October 1979.
[10] Y. K. Hwang, N. Ahuja, Gross Motion Planning - A
Survey, ACM Computing Surveys 24, no. 3, 219-291,
Sep. 1992.
[11] D. Hsu, L. E. Kavaki, J. C. Latombe, R. Motwani, S.
Sorkin. On finding narrow passages with probabilistic
roadmap planners. In Proceedings of the Workshop on
the Algorithm Foundations of Robotics, pages 141-154.
A K Peters, 1998.
[12] D. Hsu, T. Jiang, J. Reif, Z. Sun. The bridge test for
sampling narrow passages with probabilistic roadmap
planners. In Proceedings of the IEEE International
Conference on Robotics and Automation, pp. 4420-
4426, 2003.
[13] L. E. Kavraki, P. Svestka, J. C Latombe, M. H. Over-
mars. Probabilistic roadmaps for path planning in high-
dimensional configuration spaces. IEEE Transactions
on Robotics and Automation, vol. 12, no. 4, pp. 566-
580, 1996.
[14] V. Boor, M. H. Overmars, A. F. van Der Stappen. The
gaussian sampling strategy for probabilistic roadmap
planners. In Proceedings of the 1999 IEEE Interna-
tional Conference on Robotics and Automation,pp.
1018-1023, 1999.
[15] G. Sanchez, J. C. Latombe. A single-query bi-
directional probabilistic roadmap planner with lazy col-
lision checking. In Int. Symp. Robotics Research 2001.
[16] S. M. LaValle. Rapidly-exploring random trees: a new
tool for path planning. TR 98-11, Computer Science
Dept., Iowa State University, Oct. 1998.
[17] P. Cheng and S.M. La Valle . Resolution complete
rapidly exploring random trees . In Proc. IEEE Intl.
Conf. on Robotics and Automation, pages 267-272,
2002.
[18] P. Choudhury, K. Lynch. Trajectory planning for
second-order under actuated mechanical systems in
presence of obstacles. In Proceedings of the Workshop
on Algorithmic Foundation of Robotics, 2002.
[19] L. E. Kavraki, P. Ostrowski. Motion planning of aerial
robot using rapidly-exploring random trees with dy-
namic constraints. In IEEE Int. Conf. Robot. & Autom.,
2003.
[20] J. M. Philips, N. Bedrossian, L. E. Kavraki. Guided ex-
pansive spaces trees: a search strategy for motion and
cost-constrained state spaces . Proc. IEEE Int. Conf.
Robot. & Autom. , pp. 3968-3973, 2004.
[21] S. G. Vougiouksa. Optimization of robot paths com-
puted by randomised planners.Proc. of Int. Conf. on
Robot. & Autom., pp. 2160-2165, 2005.
[22] J. Barraquand, L. E. Kavraki , J. C. Latombe, T. Y.
Li, R. Motwani, P. A. Raghavan. Random sampling
scheme for path planning. International Journal of
Robotics Research, vol.16 no. 6, pp. 759-774, 1997.
[23] S. M. La Valle. http://msl.cs.uiuc.edu/msl
[24] S. M. LaValle, J. J. Kuffner. Randomized kinodynamic
planning. In Proc. IEEE Int’l Conf. on Robotics and Au-
tomation, pages 473–479, 1999.
[25] G. Dissanayake, P. Newman, S. Clark, H. Durrant-
Whyte, M. Csorba, “A solution to the simultaneous lo-
calization and map building (SLAM) problem,” IEEE
Trans. on Robotics and Automation, vol. 17, pp. 229–
241, 2001.
6