Content uploaded by Jaime Valls Miro

Author content

All content in this area was uploaded by Jaime Valls Miro on Aug 30, 2014

Content may be subject to copyright.

Sampling Based Time Efﬁcient 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 efﬁcient 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 difﬁcult 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 conﬁguration 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 ﬁelds 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-

ﬁguration 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 conﬁguration 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 sacriﬁcing 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-efﬁcient 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 efﬁcient 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 modiﬁed A* search is then imple-

mented to ﬁnd suitable paths on this space. The result is

a time-efﬁcient 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 conﬁgurations 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 efﬁcient [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 speciﬁ-

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 ﬁrst-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 ofﬂine, 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 deﬁned 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-simpliﬁcation, 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 ﬁnding 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 deﬁning 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 ﬁnd 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 ﬁnal 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 efﬁciently 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 ofﬂine - 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 deﬁned 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 -

efﬁciently generated in a single step.

4 Experimental Results

In order to compare the efﬁciency of the proposed algorithm,

we ﬁrst 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

conﬁgurations 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 ﬁnding 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 ﬁ-

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 ﬁnd 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 signiﬁcant 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-efﬁcient 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

efﬁciently 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 modiﬁed 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 Efﬁcient 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 Artiﬁcial 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 conﬁguration

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 ﬁnding 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 conﬁguration 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