MOTION PLANNING WITH LOCALIZATION AND MAPPING

MOTION PLANNING WITH LOCALIZATION AND

MAPPING UNCERTAINTIES FOR A MOBILE

MANIPULATOR IN EXPLORATION AND INSPECTION

TASKS

by

Yifeng Huang

B.A.Sc., Tianjin University, P.R.China, 1996

M.A.Sc., China Academic Institute of Science, Shenyang Institute of Automation, 1999

a thesis submitted in partial fulfillment

of the requirements for the degree of

Doctor of Philosophy

in the School of

Engineering Science

c© Yifeng Huang 2009

SIMON FRASER UNIVERSITY

Spring 2009

All rights reserved. This work may not be

reproduced in whole or in part, by photocopy

or other means, without the permission of the author.

Title of thesis: Motion Planning with Localization and Nlapping Uncertain-

ties for a Nlobile Manipulator in Exploration and Inspection

Tasks

Examining Committee: Dr. N{irza Faisal Beg

Chair

Dr. Kamal K. Gupta. Scnior Supervisor

Dr. Ahmad Rad, Supervisor

Dr. Carlo Menon, SFU Examiner

Dr. Thierry Simeon, External Exarniner,

Director Research, LAAS

Toulouse. France

Name:

Degree:

APPROVAL

Yif'eng Huang

Doctor of Philosophy

Date Approved: A'{ '1 "J, z oo'

Last revision: Spring 09

Declaration of Partial Copyright Licence The author, whose copyright is declared on the title page of this work, has granted to Simon Fraser University the right to lend this thesis, project or extended essay to users of the Simon Fraser University Library, and to make partial or single copies only for such users or in response to a request from the library of any other university, or other educational institution, on its own behalf or for one of its users.

The author has further granted permission to Simon Fraser University to keep or make a digital copy for use in its circulating collection (currently available to the public at the “Institutional Repository” link of the SFU Library website <www.lib.sfu.ca> at: <http://ir.lib.sfu.ca/handle/1892/112>) and, without changing the content, to translate the thesis/project or extended essays, if technically possible, to any medium or format for the purpose of preservation of the digital work.

The author has further agreed that permission for multiple copying of this work for scholarly purposes may be granted by either the author or the Dean of Graduate Studies.

It is understood that copying or publication of this work for financial gain shall not be allowed without the author’s written permission.

Permission for public performance, or limited permission for private scholarly use, of any multimedia materials forming part of this work, may have been granted by the author. This information may be found on the separately catalogued multimedia material and in the signed Partial Copyright Licence.

While licensing SFU to permit the above uses, the author retains copyright in the thesis, project or extended essays, including the right to change the work for subsequent purposes, including editing and publishing the work in whole or in part, and licensing other parties, as the author may desire.

The original Partial Copyright Licence attesting to these terms, and signed by this author, may be found in the original bound copy of this work, retained in the Simon Fraser University Archive.

Simon Fraser University Library Burnaby, BC, Canada

Abstract

In this research, we address the motion planning (MP) problem in real world robotic ex-

ploration and inspection tasks, where robot localization and mapping uncertainties have to

be incorporated into the planned motions. The robot considered in this work is a mobile

manipulator system, which combines the mobility of the base with the dexterousness of

the manipulator, and therefore is suitable for the exploration and inspection task in both

open spaces and locally cluttered environments. To explore the environment, a laser range

scanner is mounted in the front of the mobile base.

The first part of this work considers localization and mapping uncertainties in the mo-

tion planning problem. We propose RRT-SLAM, which uses a rapidly-exploring random

tree (RRT) in conjunction with a simulated particle based simultaneous localization and

mapping (SLAM) algorithm to expand the tree. The simulated SLAM explicitly accounts

for localization and mapping uncertainties in the planning stage. Moreover, the RRT itself

is represented in the uncertainty-configuration space (UC-space), which is an augmented

configuration space with an extra dimension of uncertainty. The collision probability along

a planned path is explicitly calculated and is used to select a planned path. Since the

standard Euclidean metric does not reflect the structure of the UC-space well, we address

the efficiency of the RRT in the UC-space. We treat the issue from a data clustering point

of view and propose a fractal dimension (FD) based checking criterion for efficient node

generation. The advantages of our FD based approach are illustrated and discussed, and

we demonstrate the positive results in simulations.

The second part of this research addresses planning motions for the manipulator to

accomplish an inspection task, with the base staying stationary. Therefore, no sensor ob-

servation is available during the motion. We extend the probabilistic roadmap (PRM)

algorithm to plan motions. Since the base pose of the manipulator is not precisely known

iii

due to the localization uncertainty, the path query of the roadmap becomes a constrained

shortest path problem, and is fundamentally different from the query in the standard PRM.

We prove that this path query is an NP-hard problem, and propose an lazy path query

algorithm that judiciously combines a k-shortest path algorithm with a labeling algorithm.

The algorithm rules out large classes of paths, hence leading to efficiency.

The RRT-SLAM is tested on an actual PowerBot and the experimental results show the

effectiveness and benefits of our integrated approach. We also implemented and tested our

integrated planner for the mobile manipulator in simulations. The simulated system is a

mobile base with a 3-dof manipulator mounted on it. A Hokuyo laser range sensor, mounted

on the mobile base, is also simulated. The effectiveness of the combined integrated planner

is demonstrated via these simulations.

iv

To my wife

v

Acknowledgments

I would like to thank the following people for contribution of this thesis. With the support

of individuals below, I went through some of the best years in my life.

I would like to first express my appreciation to my supervisor, Dr. Kamal K. Gupta, for

his guidance and support. I can not convey how thankful I am to him.

I would also like to thank my examining committee, Dr. Ahmad Rad, Dr. Carlo Menon

and Dr. Thierry Simeon for their extremely valuable suggestions.

Special thanks to my lab-mates, Lila Torabi, Moslem Kazemi, Zhengwang Yao and Yi

Li, for their comments and support.

My sincere appreciation also goes to the brothers and sisters in SFU Fellowship. Thanks

for their company with me and my family, and from their sharing and testimonies, I acquired

the true treasure of life.

Last, I like to thank my family for their encouragements, support and patience.

vi

Contents

Approval ii

Abstract iii

Dedication v

Acknowledgments vi

Contents vii

List of Tables xi

List of Figures xii

Notation xvi

Publications xx

Acronyms xxi

1 Introduction 1

1.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1

1.2 Overview of the Problem and Our Algorithms . . . . . . . . . . . . . . . . . . 7

1.2.1 RRT-SLAM: a Motion Planner with Localization and Mapping Un-

certainties . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8

1.2.2 Lazy-PRM-BU: a Motion Planner for a Manipulator with Base Pose

Uncertainty . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10

vii

1.2.3 Delaunay Triangulation Inspired Adaptive k Nearest Neighbor Sample

Connection Strategy for PRM . . . . . . . . . . . . . . . . . . . . . . . 11

1.3 Related Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11

1.3.1 Model-Based Motion Planning . . . . . . . . . . . . . . . . . . . . . . 11

1.3.2 Motion Planning Under Control and Sensing Uncertainties in Known

Environments . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14

1.3.3 Motion Planning Under Map Uncertainty . . . . . . . . . . . . . . . . 18

1.3.4 Exploring Unknown Environments Under Control and Sensing Uncer-

tainties . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18

1.3.5 RRT-SLAM Contribution to Current Path Planning Techniques . . . 20

1.3.6 Representations of Localization and Mapping Uncertainties . . . . . . 21

1.3.7 Representations of Physical Space . . . . . . . . . . . . . . . . . . . . 21

1.3.8 Constrained Shortest Path problem (CSPP) and Lazy Path Query of

the Roadmap . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22

1.4 Publication Notes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22

1.5 Thesis Contributions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23

1.6 Thesis Outline . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24

2 RRT-SLAM 25

2.1 FastSLAM: an Overview . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25

2.2 Simulated SLAM Framework: Determining Collision Probability Along a

Planned Path . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27

2.2.1 Simulated SLAM Framework . . . . . . . . . . . . . . . . . . . . . . . 27

2.2.2 Particle Based Simulated Localization . . . . . . . . . . . . . . . . . . 32

2.3 Planning Motion with Localization and Mapping Uncertainties (RRT-SLAM) 39

2.4 Simulations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44

2.5 Experiments . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46

2.6 Parameter Values Related to RRT-SLAM . . . . . . . . . . . . . . . . . . . . 48

2.7 Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49

3 RRT-SLAM-FD 50

3.1 Tree Exploration in the Uncertainty-Configuration Space (UC-Space) . . . . . 50

3.1.1 Classic RRT’s Dependence on Metrics . . . . . . . . . . . . . . . . . . 51

3.1.2 RRT-Blossom . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52

viii

3.1.3 Original Regression Condition in the UC-Space . . . . . . . . . . . . . 53

3.2 Outlier Detection Mediated Regression Checking . . . . . . . . . . . . . . . . 57

3.2.1 Fractal Dimension . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57

3.2.2 Applying FD as a Regression Condition . . . . . . . . . . . . . . . . . 58

3.3 Improving RRT-SLAM Efficiency in the UC-Space: RRT-SLAM-FD . . . . . 58

3.3.1 Discussions on RRT-SLAM-FD . . . . . . . . . . . . . . . . . . . . . . 62

3.3.2 Box Counting: an Algorithm to Determine FD . . . . . . . . . . . . . 63

3.4 Simulations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64

4 Lazy-PRM-BU 69

4.1 Problem Formulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 69

4.1.1 Underlying Structure of PRM with Base Pose Uncertainty . . . . . . . 71

4.1.2 CP-CSPP: an NP-hard Problem . . . . . . . . . . . . . . . . . . . . . 73

4.1.3 Using the Labeling Algorithm to Solve CP-CSPP . . . . . . . . . . . . 75

4.2 Lazy-PRM with Base Pose Uncertainty . . . . . . . . . . . . . . . . . . . . . 77

4.2.1 Lazy-PRM Review . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 77

4.2.2 Lazy-PRM with Base Uncertainty . . . . . . . . . . . . . . . . . . . . 77

4.2.3 The k-shortest Path Algorithm . . . . . . . . . . . . . . . . . . . . . . 78

4.2.4 Calculating the Shortest Path in an Equivalent Class . . . . . . . . . . 80

4.2.5 Lazy-PRM-BU . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 81

4.2.6 Roadmap Construction and Node Enhancement . . . . . . . . . . . . 83

4.2.7 Detecting an N-edge . . . . . . . . . . . . . . . . . . . . . . . . . . . . 84

4.2.8 Path Verification from Both Directions . . . . . . . . . . . . . . . . . . 84

4.3 Simulations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 84

4.4 A Motion Planner for the Mobile Manipulator System in an Inspection Task 90

5 DTC 93

5.1 Delaunay Triangulation Review . . . . . . . . . . . . . . . . . . . . . . . . . . 93

5.2 PRM Sample Connection Strategy Review . . . . . . . . . . . . . . . . . . . . 94

5.3 Delaunay Triangulation Inspired Connection (DTC) Strategy . . . . . . . . . 95

5.3.1 Constructing a Delaunay Triangulation Neighborhood . . . . . . . . . 96

5.3.2 Delaunay Triangulation Inspired Adaptive Roadmap Connection (DTC)100

5.3.3 Node Enhancement . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 102

5.4 Simulations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 103

ix

6 Conclusions and Future work 106

6.1 Future Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 107

Appendix 109

A System Implementation 109

A.1 The Simulated Mobile Manipulator Test-bed System . . . . . . . . . . . . . . 109

A.1.1 The Mobile Manipulator Simulator . . . . . . . . . . . . . . . . . . . . 109

A.1.2 Robot System Software Implementation and Architecture . . . . . . . 110

A.2 Sonar-Based Local Collision Avoidance for the Mobile Base During Path Ex-

ecution . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 115

Bibliography 117

x

List of Tables

1.1 RRT-SLAM’s relationship with current existing path planning techniques

considering localization and/or mapping uncertainties. . . . . . . . . . . . . . 20

2.1 Parameter values applied in RRT-SLAM for the simulations and experiments.

Unless specified otherwise, all simulations and experiments used these values. 49

3.1 Simulation results for Problems A, B and C. . . . . . . . . . . . . . . . . . . . 68

4.1 Results for Lazy-PRM-BU with pruning invalid equivalent classes. . . . . . . 87

4.2 Results for PRM-BU. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 87

4.3 Number of collision checking for Lazy-PRM-BU and PRM-BU. . . . . . . . . 87

4.4 Results for Lazy-PRM-BU without pruning invalid equivalent classes. . . . . 89

4.5 The number of failure times (FT) and worst case time (WT) for Lazy-PRM-

BU (30 runs) with (w. pr.) and without (w.t. pr.) pruning invalid equivalent

classes. Time limit is 1000 and 1500 seconds for SMALL and LARGE uncer-

tainty, respectively. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 89

5.1 Simulation results for problems A, B, C, D and E. . . . . . . . . . . . . . . . 105

A.1 VFH parameters applied. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 116

xi

List of Figures

1.1 The SFU mobile manipulator system (with an enlarged picture of the Hokuyo

URG-04LX range scanner) in the RAMP laboratory at Simon Fraser University. 2

1.2 The overall exploration/inspection process consists of mainly three stages. . . 3

1.3 Two paths planned with different localization uncertainties considered. White

areas represent free space. Gray areas represent unknown areas, which are

treated as obstacles (in black color) as well. The ellipses along the path

represent the localization uncertainty along the path. The sensing range is

4m, and the grid size in the map is 1m. . . . . . . . . . . . . . . . . . . . . . 5

1.4 The simulated test-bed for the motion planner under robot localization and

mapping uncertainties. The mobile base is a simulated PowerBot of size 80cm

by 65cm. A simulated Hokuyo laser range finder is mounted in the front of

the mobile base. The manipulator has 3 links. The black areas are obstacles,

but unknown to the robot at beginning. Area covered by the scan of the

range finder is gray, and becomes known to the robot. The gray rays are

simulated sonar beams. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6

1.5 Robot motion planning in a wide open area. The ellipses along the two paths

represent the localization uncertainties. . . . . . . . . . . . . . . . . . . . . . 19

2.1 Sample trail particles from the state particle sets (L0, L1, ..., L4) at each

time instant (t=0 to 4). The number of trail particles in the trail particle set

is set as 4, which equals the number of state particles at each time instant. . 34

2.2 Sample trail particles sequentially over each time instant. . . . . . . . . . . . 36

2.3 A typical random tree returned by Algorithm-5 . . . . . . . . . . . . . . . . . 43

xii

2.4 The same goal but different goal uncertainties were given to the robot. The

paths found were different. For the low uncertainty case (b), robot prefers to

go closer to known obstacles to localize itself better. . . . . . . . . . . . . . . 44

2.5 Comparing solution paths returned by RRT-SLAM and the classic RRT in

the simulated environment. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46

2.6 A map of the RAMP lab in the Applied Science Building at SFU. The next

goal of the robot is marked. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 47

2.7 Comparing solution paths returned by RRT-SLAM and the standard RRT in

the RAMP lab environment shown in Figure 2.6. . . . . . . . . . . . . . . . . 47

2.8 Trace of the mobile base as it executes the path planned using RRT-SLAM

for the RAMP lab environment. . . . . . . . . . . . . . . . . . . . . . . . . . . 48

3.1 The regression checking mechanism for RRT-Blossom. . . . . . . . . . . . . . 53

3.2 Failure of the original regression condition in the UC-space. . . . . . . . . . . 54

3.3 The live tree nodes picked by the regression condition (Equation 3.1) intro-

duced in RRT-blossom. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 55

3.4 Distribution of the live nodes (dark) under the original regression condition.

The dormant nodes are shown in light color. . . . . . . . . . . . . . . . . . . . 56

3.5 An example of applying the original regression checking condition (Equation

3.1) in the UC-space. Only live nodes are shown. A small number of live

nodes (dark colored) are available for further extensions. Nodes in gray color

are also live nodes, but they have been extended more than 8 times and hence

will not be further extended (refer to the explanation for RRT-Blossom). . . 57

3.6 The live tree nodes picked by the new FD based regression condition (Equa-

tion 3.3). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62

3.7 Distribution of the live nodes under the new FD based regression condition. . 62

3.8 FD values for the data set of the RRT-SLAM nodes, with the initial data set

in the UC-space, for 5 runs. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64

3.9 Problems A and B. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65

3.10 Problem C. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65

xiii

3.11 Tree structures in the UC-space, shown in 2D view ((a1), (b1) and (c1))

and 3D view (the rests), acquired by applying RRT-SLAM ((a1) and (a2)),

RRT-SLAM-Blossom ((b1), (b2) and (b3)) and RRT-SLAM-FD ((c1), (c2)

and (c3)), for Problem C. For RRT-SLAM-Blossom and RRT-SLAM-FD,

the nodes (branches) in black and gray color are the live and dormant nodes,

respectively. In (b3) and (c3), only live nodes are shown. . . . . . . . . . . . . 67

4.1 For a manipulator path, there are correspondingly N possible sequences of

configurations swept by the whole mobile manipulator. . . . . . . . . . . . . . 70

4.2 Roadmap G in Cm, and the corresponding graph set G = {G[1], G[2], G[3]}constructed in Cbm (N = 3). . . . . . . . . . . . . . . . . . . . . . . . . . . . . 72

4.3 A graph G constructed to transform the PARTITION problem into CP-CSPP 74

4.4 Building of the branching structure of the shortest path tree T3. . . . . . . . . 80

4.5 Four different start/goal problems. Left: paths planned with PRM-BU with

base uncertainty (LARGE). Right: paths planned with classic PRM with no

uncertainty taken into account and with the base pose at the most weighted

particle. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 86

4.6 Running time for problems A to D for all the 30 runs. . . . . . . . . . . . . . 88

4.7 The robot is given an end effector position as goal shown in sub-figure (a),

which is a partially explored environment. The end effector position is marked

by the black square. In (b), the mobile base first moves close to the designated

end effector position (path planned with RRT-SLAM), and then execute the

manipulator path returned by the Lazy-PRM-BU, with the base being sta-

tionary (c). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 92

5.1 Delaunay triangulation and Voronoi diagram of a 2D points set. Square dots

are the points in the point set. Light gray polygon regions are the Voronoi

cells and dark triangles form the Delaunay triangulation. . . . . . . . . . . . . 95

5.2 Delaunay triangulation inherently uses both distance and direction informa-

tion. See text for explanation. . . . . . . . . . . . . . . . . . . . . . . . . . . . 96

5.3 Probability for a node of ranking ki to be DN of node nc, in 6-dimensional

space . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 98

5.4 Selection of kb for each dimension from 2 to 10 . . . . . . . . . . . . . . . . . 99

xiv

5.5 Relationship between the number of uniform nodes and radius selection (the

average value) in 2, 4 and 6-dimensional spaces. . . . . . . . . . . . . . . . . . 101

5.6 Problems A (left) and B (right) – 2D rigid mobile robot . . . . . . . . . . . . 103

5.7 Problems C (left) and D (right) – 4-link planar manipulator with fixed base. . 104

5.8 Problem E – 6-link planar manipulator with fixed base. . . . . . . . . . . . . 104

A.1 Modified MobileSim to support the control of an on-board manipulator.

Shaded areas indicate our modifications to the original MobileSim. . . . . . . 110

A.2 System software for the mobile base that consists of mainly three threads:

the user interface, the planner, and the ARIA path execution module. . . . . 111

A.3 The user interface consists of three windows (marked as A, B and C). Window

A and B are for displaying information and results, and Window C is for user

inputs. See the text for detailed explanation. For local sonar map with robot

at the center (right-bottom corner of window A), please refer to Section A.2

for detailed explanation about this area. . . . . . . . . . . . . . . . . . . . . . 114

xv

Notation

π a path, consisting of a sequence of robot configurations.

νπ a trail that is traversed by robot when it follows π.

t time index.

κπ potential sequence of sensor observations, when robot follows π.

κν the sequence of sensor observations along ν.

qsubscript a configuration or robot state indicated by the subscript.

m an environment model.

♯S the number of SLAM particles.

♯L the number of localization particles.

V a binary random variable indicates the event that robot trail ν is

collision-free (1) or not (0).

Π a binary random variable indicates the event that robot is collision-free

(1) or not (0), when it follows π.

ν [i] the ith trail particle.

m[i] the environment model associated with the ith SLAM particle.

κ[i] the sequence of sensor observations in the jth simulated sequence.

aπt when following π, the control command applied to the robot at time t.

qnext the next intermediate goal when following a path.

q∗t robot configuration corresponding to the most weighted particle in a

particle set.

M the number of repeated sequences of simulated SLAM (or localization)

along a path.

xvi

ν [i,j] the ith trail particles, which are acquired at the jth sequence of simulated

SLAM (or localization).

m[i,j] the environment model associated with the ith SLAM particles, which

are acquired in the jth sequence of simulated SLAM.

m∗ the map associated with the most weighted SLAM particle before plan-

ning the motion.

m∗b the binarized version of m∗.

ν [i]f the ith trail particle that is collision-free in map m∗b .

Y a set of SLAM particles.

Lt the set of particles that represents qt.

L′

t+1 the set of particles that represents the initial guess of robot state at

t + 1.

[x, y, θ] a pose (position x, y and orientation θ ) for the mobile base.

|A| size of area covered by the range sensor.

κ∗t the sequence of sensor observations received from 0 to t in one simulated

sequence.

Bugoalthreshold for the localization uncertainty at goal position.

Bu threshold for the localization uncertainty for a RRT-SLAM tree node.

Pthresh a threshold between [0, 1]. The probability of the solution path being

collision-free has to be higher than τf .

φt−1 the number of particles in Lt−1 , who have decedents in Lt.

ηt−1 ηt−1 = φt−1

♯L.

τp a threshold between [0, 1]. In case ηt−1 ≤ τp, we assume qt−1 is inde-

pendent of qt.

ω[i] the weight of the ith particle in a particles set.

ω[i] the weight of the ith trail particle in a particles set.

n a graph/tree node.

qn the configuration at graph node n.

Tt a data set that stores trail particle set information at time t.

Tnia data set that stores trail particle set information at graph node ni.

xvii

ν[i]ft+1

at time t + 1, the weight of the ith trail particle that is collision-free.

Pni

free the probability that robot is collision-free when arriving graph node ni.

ϕ a weight between [0, 1].

ρ(.) a distance measurement.

DQ the fractal dimension of a data set.

di a grid.

pi the frequency of the data points falling into the cell di.

r the size of a grid.

Nr the number of grids occupied by the data set for a given grid size gr.

fthresh the threshold for a reasonable change of fractal dimension.

fstep the adjustment step of fthresh.

△f the change of fractal dimension before and after a data point is added

into the data set.

Ti the shortest path tree structure constructed from the i shortest paths.

B(nu) a bundle of shortest paths corresponding to tree node nu in Ti.

Bi the set of first i shortest paths in G.

B the set of all the paths in G.

Bi complement of Bi, Bi = B\Bi.

C(nu) a class of paths in Bi corresponding to tree node nu of Ti.

C(nu, nv) a class of paths in Bi corresponding to a tree branch (nu, nv) of Ti.

G a roadmap graph for the manipulator.

G a set of graphs.

Gi the ith graph in G.

C(π) the length (cost) of the path π.

C the configuration space.

Cm the manipulator’s configuration space.

Cmb the mobile manipulator’s configuration space.

e a graph/tree edge/brach.

Πe a binary random variable indicating the event that robot is collision-free

(1) or not (0), when it follows a graph edge e.

xviii

nr a random generated node.

nc a tree node that is the nearest to nr, among all the tree nodes.

nnew a new tree node.

π[i] a manipulator path with base pose at the ith particle.

c[i] a binary variable indicating π[i] is free (1) or not (0).

e[na,nb] an edge between two neighbor graph nodes na and nb in G.

V the set of nodes in G.

E the set of edges in G.

πki the kth path reaching to graph node ni in G.

G[i] a roadmap graph for the manipulator, given the base pose is the ith

particle.

n[i]j the jth node of G[i].

e[i] an edge of G[i].

e[i][nj ,nk] an edge of G[i], connecting node n

[i]j and n

[i]k .

Cki accumulated cost (the length) of the πk

i .

P ki the probability of manipulator path πk

i being collision free.

S a data set.

s a data point in S.

V (s) Voronoi region of s.

P a problem.

xix

Publications

1. Y. Huang and K. Gupta. A Delaunay triangulation based node con-

nection strategy for probabilistic roadmap planners. In Proceedings of

IEEE International Conference on Robotics and Automation (ICRA04),

pages 908 - 913, 2004

2. Y. Huang and K. Gupta. An daptive configuration-space and

work-space based criterion for view planning. In Proceedings of

IEEE/RSJ/GI International Conference on Intelligent Robots and Sys-

tems (IROS05), pages 3366- 3371, 2005

3. Y. Huang and K. Gupta. RRT-SLAM for motion planning with mo-

tion and map uncertainty for robot exploration. In Proceedings of

IEEE/RSJ/GI International Conference on Intelligent Robots and Sys-

tems (IROS08), pages 1077-1082, 2008

4. Y. Huang and K. Gupta. Lazy-PRM for a Manipulator with Base Pose

Uncertainty. Submitted to IEEE/RSJ/GI International Conference on

Intelligent Robots and Systems (IROS09), 2009.

xx

Acronyms

ACA Ariadne’s Clew Algorithm

CSPP Constrained Shortest Path Problem

CP-CSPP Collision Probability Constrained Shortest Path Problem

DN Delaunay Neighbor

DOF Degree Of Freedom

DT Delaunay Triangulation

DTC Delaunay Triangulation inspired sample Connection technique

DW Dynamic Window

FD Fractal Dimension

Lazy-PRM PRM with Lazy query technique

Lazy-PRM-BU Lazy-PRM for manipulator with Base pose Uncertainty

LIDAR Light Detection And Ranging

LP Linear Programming

MDP Markov Decision Process

MP Motion Planning

MPK Motion Planning Kernel

NNC Nearest Neighbor Connection strategy

POMDP Partially Observable Markov Decision Process

PRM Probabilistic Roadmap Method

PRM-BU PRM for manipulator with Base pose Uncertainty

RAMP Robotic Algorithms and Motion Planning Laboratory

RRT Rapid-exploring Random Tree

RRT-Blossom RRT with regression checking technique

xxi

RRT-CT RRT with Collision Tendency technique

RRT-SLAM-

Blossom

RRT-SLAM with original regression checking technique in RRT-

Blossom

RRT-SLAM-FD RRT-SLAM with FD based regression checking technique

RRT-SLAM RRT in conjunction with SLAM technique

SFU Simon Fraser University

SLAM Simultaneously Localization And Mapping

VFH Vector Field Histogram

WCSPP Weight Constrained Shortest Path Problem

xxii

Chapter 1

Introduction

1.1 Introduction

Robot platforms that mix mobility and manipulation, such as humanoid robots or mobile

manipulators, have recently gained more attention. In the long run, expectations are that

these systems will be capable of substituting human beings in many challenging tasks, e.g.,

exploration/inspection in unknown environments [1], samples collection in hazard areas [2],

cargo picking up and transportation [3], and daily service [4].

This research is a step towards the long term goal of the Robotic Algorithms and Motion

Planning (RAMP) laboratory at Simon Fraser University, to build a fully autonomous mobile

manipulator system that is capable of carrying out exploration, inspection and manipulation

tasks in unknown environments. Building such a system requires integrating a variety of

robotic disciplines including, for example, path planning [5, 6], sensing and modeling [7],

vision [8], target tracking [9], and robot control [10].

The mobile manipulator system (Figure 1.1) in RAMP includes a PowerBotTM

mobile

base (with a weight of 120kg and a payload of up to 100kg), and a SchunkTM manipulator,

consisting of a 6-link arm, mounted on the top of the base. The base is heavy enough, so we

can assume that the manipulator motion is precise w.r.t. the base frame when the base is

kept still. The PowerBot can be considered as a holonomic mobile base since it has a very

small turning radius. A Hokuyo URG-04LX range scanner (based on Light Detection and

Ranging (LIDAR) technology), having a sensing range of 4.0m and a span angle of 180◦,

is fixed on the mobile robot and points to the front. Fourteen forward and fourteen rear

sonar sense obstacles from 15cm to 7m. At the end effector of the manipulator, one could

1

CHAPTER 1. INTRODUCTION 2

attach an “eye” sensor, (e.g., a camera or a range finder) for inspection or object modeling

tasks [11, 12, 13], and/or a gripper for visual based [9] grasping/manipulation tasks. Such a

specific system setup combines the mobility of the base with the manipulation ability of the

arm, and therefore is suitable for the exploration/inspection task in both open spaces (by

using the range scanner at base) [7] and cluttered environments (by using the eye sensor)

[14].

Figure 1.1: The SFU mobile manipulator system (with an enlarged picture of the HokuyoURG-04LX range scanner) in the RAMP laboratory at Simon Fraser University.

The particular application that motivates this research is planning motions for au-

tonomous exploration/inspection in unknown environments. As shown in Figure 1.2, the

whole exploration/inspection process is a plan-move and sense-update loop [11], which con-

sists of mainly three stages in each iteration: I) plan for robot’s next (reachable) configura-

tion, i.e., a view point, to further explore/inspect its environment; II) execute the plan, i.e.,

move to the planned configuration (and sense if further information about the environment


is available during the motion); and III) update the robot’s own position and/or the envi-

ronment model. Two subproblems are involved in stage I, the planning stage: I-a) where

to go, also called the next best view (NBV) [11, 12, 15, 16]; and I-b) how to get there, i.e.,

determine a reachable path to get to the next viewpoint. If a final goal is to be reached, it

can be easily incorporated in the overall process.

location and environment

model

Update the robot

Motion Planning

View Planning

Execute the plan

stage

Planning

Start

Figure 1.2: The overall exploration/inspection process consists of mainly three stages.

In this exploration/inspection task, a key sub-task for the robot is to plan its motion to

the next viewpoint without colliding with obstacles (the third block in Figure 1.2). This is

critical for safety of both the robot and the working environment, and is the domain of the

classic motion planning sub-field in robotics.

The very basic motion planning (MP) problem, in a narrow sense, is searching for a path

that can guide the robot from a start place to a goal place without colliding with obstacles.

It assumes that 1) the robot can be precisely controlled, and 2) the environment is also

perfectly known [5]. This type of motion planning problem is also referred to as the model-

based MP. A classic solution to the model-based MP is to search in the configuration space

(C-space, denoted as C) [17], which is the space of possible positions that a robot system

may attain. For example, the C-space of a single point robot moving in ordinary Euclidean


3D space is just R3, and the C-space of a open-loop manipulator is the joint space, where

each dimension is the value of one manipulator joint. Then the solution of the model-based

MP is a one-dimensional “path” in the free part of the C-space (denoted as Cfree).

The two assumptions mentioned above, however, are not realistic in many real world

scenarios. For example, the robot may have to explore and construct the environment

model (map) step by step, while moving around, with the collected information about the

environment from on-board sensors. In such a case, robot’s next motion is usually planned

within the known part of the environment, with the unknown part treated as obstacle [14].

However, if the robot control is not precise and the on-board sensors are not noise free, the

robot can not precisely know its own true location (the localization uncertainty), where the

sensor data is collected, which is crucial to environment modeling (i.e., mapping). Therefore,

the constructed environment model usually contains errors and is not precise (the mapping

uncertainty). Locating the true position of the robot (i.e., localization), on the other hand,

requires a precise map. Simultaneously localization and mapping (SLAM) [7], a key advance

in the mobile robotics literature in past decade, attempts to solve this problem, and returns

the best estimation of both the map and the robot position at the same time in the execution

stage.

In this work, we widely use the terms of localization and mapping uncertainty. These

uncertainties are a result of underlying uncertainties in control/motion and in sensing. The

localization uncertainty arises when robot can not perfectly localize itself due to underlying

errors in control/motion and in sensing. The mapping uncertainty aries when the robot

can not build a perfect map due to the control and/or sensing uncertainties [7]. Here, the

control/motion uncertainty arises when the robot execution mechanism can not precisely

execute a control command (for example, due to a slip of the wheel friction or due to the

robot internal design limitation [5]). The sensing uncertainty arises when the sensor can not

supply perfect information about the robot current location (e.g., the on-board odometry

sensors and other range sensors such as laser/sonar sensors are usually not perfect [7]).

We study how to plan robot motions that incorporate the robot localization and mapping

uncertainties. Our motion plan, similar to the classic model-based MP, is a path that consists

of a sequence of robot configurations. However, in the presence of uncertainties, both the

map and the exact robot trajectory along the path in the execution stage are not exactly

known at the planning stage.

Figure 1.3 illustrates an example how path planning is impacted by, e.g., the localization


(a) path1 (b) path2

Figure 1.3: Two paths planned with different localization uncertainties considered. Whiteareas represent free space. Gray areas represent unknown areas, which are treated as obsta-cles (in black color) as well. The ellipses along the path represent the localization uncertaintyalong the path. The sensing range is 4m, and the grid size in the map is 1m.

uncertainty, for a mobile base. Note that, in the two sub-figures, both paths are feasible

if the control and sensing are both perfect. However, with the localization uncertainty

considered, a safe motion may require the robot to well localize itself with the on-board

sensor, by staying close to the obstacle, as shown in the Figure 1.3 (a). In Figure 1.3 (b) the

uncertainty along the path increases quickly, because the robot is away from the obstacle

and can not well localize itself, leading to a higher chance of collision.

We use a probabilistic framework in the planning process itself, i.e., a planned path

corresponds to a set of possible executed trails, each with a probability attached to it. This

probabilistic approach is fundamentally different from the standard model-based MP, where

a planned path corresponds to a sequence of configurations.

With the probabilistic framework, the collision status of following a path is associated

with a collision probability. This is also different from the model-based MP, where the

collision status of a solution path is binary as either collision-free or in-collision [5]. We use

this collision probability as a travel cost, and refer to a path as “feasible” if its associated

probability of collision-free is higher than a user defined threshold.

Calculating/evaluating possible hazardous consequences of a path in terms of the col-

lision probability, and efficiently returning a robust solution path are contributions of this

thesis.


Figure 1.4: The simulated test-bed for the motion planner under robot localization andmapping uncertainties. The mobile base is a simulated PowerBot of size 80cm by 65cm.A simulated Hokuyo laser range finder is mounted in the front of the mobile base. Themanipulator has 3 links. The black areas are obstacles, but unknown to the robot atbeginning. Area covered by the scan of the range finder is gray, and becomes known to therobot. The gray rays are simulated sonar beams.


Figure 1.4 illustrates our simulated test-bed, that consists of a simulated PowerBot and

a 3-link manipulator in a planar environment. A simulated range scanner, with a sensing

range of 4.0 meters and a span angle of 180o (approximately the same field of view as the

Hokuyo URG-04LX range sensor), is fixed on the PowerBot and points to the front. Also,

a ring of simulated sonar sensors are also attached around the PowerBot. As a first step

towards building a robot capable of dealing with uncertainties, this research focuses on the

robot in a static environment.

1.2 Overview of the Problem and Our Algorithms

For the mobile manipulator system setup introduced in Figure 1.4, we assume that a next

best view (NBV) algorithm such as those presented in [11, 12, 18] is used to determine a

viewpoint, i.e., an end effector position for the manipulator, as a goal. The overall problem

we address in this thesis then is how to plan the robot’s motion that can guide the robot from

its current location to reach the given end effector goal pose in the presence of localization

and mapping uncertainties.

Note that planning the combined motion of the mobile base and manipulator arm re-

quires planning within a high dimensional C-space. As a first step towards solving this

problem, we consider a decomposed approach [19], where the basic idea is that the overall

motion planning problem is decomposed into two problems, i.e., an easy low-dimensional

one and a high-dimensional one, instead of computing the motion in the configuration space

associated with all the degrees of freedom. Then the solution to the low-dimensional sim-

pler problem can be used to significantly reduce the computations required for the original

problem.

Specifically, the low-dimensional simpler problem in our case, is looking for a path for the

mobile base, with the on-board manipulator in a fixed folded position when the base moves

close to the goal. Then the corresponding high dimensional problem becomes moving the

manipulator to reach the goal with the base kept still. Clearly, this decomposed approach, as

mentioned in [19] is not complete, but we trade completeness for efficiency. Correspondingly,

we study two subproblems.

1) Motions of the mobile base: in this case, the sensor observations (the range readings)

are available during the motion, and they are processed by the SLAM algorithm as the

mobile base moves. So the motion planner has to explicitly consider the possible sensor


readings along the candidate planned paths.

2) Motions of only the manipulator, with the base staying stationary: a critical issue for

this scenario is that the location of the mobile base may not be known precisely due to the

localization error (but the localization error does not change because the base stays still).

In this case, no sensor observations are available during the manipulator motion.

For the two cases of motions, two planning algorithms are outlined below.

1.2.1 RRT-SLAM: a Motion Planner with Localization and Mapping Un-

certainties

We use a rapidly-exploring random tree (RRT) to plan efficiently for a feasible path 1. In

order to extend the tree towards a randomly placed node, our RRT planner executes the

very same, albeit simulated, particle based simultaneous localization and mapping (SLAM)

algorithm that the robot would use during the execution, in the process fully accounting for

the control and sensing uncertainties inherent in the robot-sensor system.

SLAM is a technique widely used by autonomous vehicles to build up a map within an

unknown environment while at the same time keeping track of their current positions. The

main issue addressed by SLAM is the inherent control/motion and sensing uncertainties

in discerning the robot’s relative movement from its various sensors. If, at the next time

instant of map building, the measured robot travel distance and direction are not accurate,

any features being added to the map will contain corresponding errors, which will accumulate

and hence distorting the map and weakening the robot’s ability to know its precise location.

We use FastSLAM [20] (a particle filter based variation of the SLAM algorithm) to build

and update the map and robot pose. Therefore, the localization and mapping uncertainties

are inherently represented as a set of particles, and it is natural to extend this particle based

uncertainty representation to build and extend the RRT.

We formulate and explicitly calculate the collision probability of a planned path within

the particle filter based framework. Each time a new node is added into the random tree,

the collision probability of the path corresponding to the new node (from root to the node)

is updated. The new node is kept if the corresponding probability of collision-free is higher

than a threshold, and is discarded otherwise. We expand the tree until the goal is reached.

1Because of RRT’s well known efficiency in the high dimension of the C-spaces, subsequently we wouldlike to extend our planner to the entire mobile manipulator system as a future work. See Chapter 6 fordetail.


Note that if the robot takes different paths to reach the same configuration in the

global frame, the uncertainties at this configuration might be different [21]. Therefore, we

use an extra dimension, that of uncertainty, (similar to [22]), and build RRT-SLAM over

the composite Uncertainty-Configuration space (UC-space). This uncertainty dimension

represents the size of uncertainty, in our case the area of the bounding box that contains

all the particles. More sophisticated measures/techniques could be used here as mentioned

later in Section 2.

Applying classic random tree extending techniques [23] in the UC-space, as is the case in

our RRT-SLAM, raises an interesting issue about efficiency. The uncertainty dimension of a

new tree node is determined by four factors: 1) the father node; 2) the control command; 3)

the environment; and 4) the localization algorithm applied. Consequently, the true distance

between two tree nodes in the UC-space, can hardly be measured by the simple traditional

weighted Euclidean distance metric. Since RRT is well known for it sensitivity to the applied

distance metrics [6], the “rapid” flavor of the RRT tends to be compromised in the UC-space.

Several works in literature have attempted to reduce the RRT’s sensitivity on the dis-

tance metric applied. For instance, RRT-Blossom [24] checks whether a new node has been

generated in a place already “covered” by the tree, termed as “regression” in [24]. If a new

node is deemed to be regressing, its likelihood to be further extended is reduced.

As pointed out by [24], regression checking is not a trivial problem. One has to examine

the overall shape of the tree structure and detect new nodes that are “far” away from the

mass of tree nodes, which is not straightforward. They propose a heuristic based technique,

but it, as discussed in Chapter 3, does not work properly in the UC-space. We treat this

regression checking from a sequential data clustering point of view [25], which means that

determining if a new node is not in regression becomes an outlier detection problem. As

shown in [25], the outlier of a data set can be well indicated by reasonable change of the

fractal dimension (FD) after the outlier is included into the data set. Specifically, we treat

all the tree nodes as a data set, and measure the FD value before and after a new node

is incorporated into the data set. If the FD value change is smaller than a threshold, it

is deemed to be regressing, and vice versa. This FD based regression checking technique

is incorporated into our RRT-SLAM-FD, and improves the efficiency of the RRT in the

UC-space, as shown in Chapter 4.


1.2.2 Lazy-PRM-BU: a Motion Planner for a Manipulator with Base Pose

Uncertainty

Assume that RRT-SLAM has been used to move the base close to the goal end effector pose.

The mobile base now stays stationary, and the manipulator is deployed for the task. Note

that no sensing is taking place and the base pose uncertainty remains the same during the

motion of the manipulator.

Note that we can apply RRT technique to search for a feasible path for the manipulator.

However, to search for a path with an optimal length,we would like to apply the probabilistic

roadmap method (PRM) [26]. It also allows us to explore the challenges of optimality in

the presence of uncertainty, an interesting issue in itself.

After the roadmap is constructed in the C-space of the manipulator, in a similar way

to the classic PRM, we query whether a path exists over the roadmap. But when planning

motion for the manipulator with base pose uncertainty, a path over the roadmap, again,

is associated with a collision probability. We are looking for a path with the collision-free

probability higher than a given threshold.

Therefore, the path query becomes an interesting problem of searching for the shortest

path over the roadmap, subject that the required collision-free probability of the path is

higher than a threshold. This path query problem, in our case, is actually a constrained

shortest path problem (CSPP) [27]. We refer to this problem as the collision probability

constrained shortest path problem (CP-CSPP), which is fundamentally different from the

one in the classic PRM [26]. We show that the nature of the CP-CSPP is NP-hard, and

then propose a general solution based on the labeling algorithm [27], a general CSPP solver.

We call this PRM extension as PRM with Base pose Uncertainty (PRM-BU). Note that

in our application, the mobile base may often need to be moved to other working areas, so

the roadmap has to be completely reconstructed. A lazy version of PRM, i.e., Lazy-PRM,

for the fast single query [28] is therefore desired. Lazy-PRM is originally designed to answer

quickly for the single query, which may require only portion of the C-space to be explored,

or equivalently, partial edges of the roadmap to be checked for collision.

Note that we can not simply delete an edge in collision as what has been done in the

standard Lazy-PRM, because the collision status of the edge in our roadmap in not binary

(refer to Chapter 4 for detail). We propose to apply the k-shortest path algorithm [29] to

generate candidate paths for verification. But a naive implementation will cause an efficiency


problem, because the number of the candidate paths over the roadmap is exponential w.r.t.

the number of the nodes of the roadmap. This leads to our Lazy-PRM-BU, which modifies

the k-shortest path algorithm and integrates the above mentioned labeling algorithm for a

more efficient lazy query technique to address the base pose uncertainty.

1.2.3 Delaunay Triangulation Inspired Adaptive k Nearest Neighbor Sam-

ple Connection Strategy for PRM

We also study the sample connection, a necessary step in roadmap construction for all the

roadmap based planners. The major question in sample connection is how to assign the

edges efficiently in order to keep the ratio of edges to samples fixed, i.e., keep the graph’s

density limited. Many, indeed most, PRM algorithms use a Nearest Neighborhood Con-

nection (NNC) strategy to determine the neighbors of a given node. In [26], a node will

take the k nearest samples as neighbors, with k upper-bounded by a constant parameter

MaxNumNeighbors. This strategy would tend to leave out neighbors along certain direc-

tions if there are many close neighbors along other directions. This can easily adversely

affect the connectivity of the roadmap, leading to potentially large number of un-connected

components. To remedy this, we propose a Delaunay Triangulation inspired adaptive k

nearest neighbor Connection (DTC) strategy. Indeed, due to the intractable complexity of

computing Delaunay triangulation (DT) in high dimensions (exponential in the dimension

of the space), we do not calculate the DT explicitly. Instead, we build a network that would

“approximate” a DT. We show via simulations that our adaptive connection strategy leads

to better connectivity of the roadmap over the commonly used NNC strategy, and boosts

the performance of the classic PRM.

1.3 Related Work

1.3.1 Model-Based Motion Planning

Earlier approaches for the model-based MP depended on an explicit representation of Cfree

(or Cobs = C/Cfree). They can be categorized in three major categories [5]: I) roadmap

based methods, e.g., visibility graph [30] or Voronoi diagram based methods; (II) exact cell

decomposition based methods; and (III) approximate cell decomposition based methods.

These works are tractable only for low-dimensional C-spaces (say less than 4).


Potential field based approaches [31] define a potential field over the C-space, and use

a gradient descent method to search for the minimum (goal). But it is hard to build a

potential field that has a single global minimum at the goal without local minima [5], or a

navigation function. So, this approach may get stuck at a local minimum, in which case a

random walk [32] is often applied to help robot escape from the trap.

Sampling based approaches, introduced in last fifteen years or so, have successfully

addressed planning for high dimensional C-spaces [6]. These algorithms aim to “learn”

the C-space incrementally by placing samples in Cfree. Two nodes are connected by a

deterministic “local” planner. There exist variety of algorithms in this category, and we

present here some of these approaches.

Ariadne’s Clew Algorithm (ACA)

ACA [33] forms a tree that explores Cfree. In each iteration, a set of embryos are generated

from current landmarks by random walk in Cfree. Then, the furthest embryo from all the

current landmarks is kept as a new landmark in order to explore the Cfree efficiently.

Rapidly-Exploring Random Tree (RRT)

RRT expands a tree in Cfree in a similar fashion to ACA, but generates tree nodes somewhat

differently. In each iteration, a random configuration (node) is generated. The tree node

that is closest to this random node is selected. The selected tree node is extended towards

the random node to acquire a new node, which finally is added into the tree. Note that a

distance metric has to be introduced to determine the closest tree node to the random node.

In a probabilistic sense, RRT chooses, at each iteration, the tree node with a larger

Voronoi region (the larger the region is, the higher the chance it will be chosen) to extend

towards the center of the region to acquire a new node. Note that these large Voronoi

regions correspond to the open area that is not yet covered by the tree. It is this property

(i.e., generating, with a high chance, new nodes in these large empty Voronoi regions in each

iteration) that gives RRT a rapidly exploring flavor.

RRT has been applied to plan motions for different system setups, such as rigid body,

manipulator system of many degrees of freedom [23], non-holonomic systems, and systems

with dynamic constraints [34]. As pointed out in [6], the distance metric applied plays a

crucial role in RRT. Those tree nodes that are “close” to the unexplored areas will have


more chance to be chosen to be extended towards these unexplored areas. Extensions from

these nodes may not generate new nodes efficiently if the distance metric does not reflect

the true distance between two tree nodes (for instance, using the Euclidean distance in the

non-holonomic case). In this case, the tree expansion loses its “efficient” characteristic.

RRT-CT [34] was proposed to avoid numerous extensions from those nodes close to the

unexplored area. The idea is to gradually place penalty on the priority of a node being

chosen for next extension based on the collision tendency of its descendants.

RRT-Blossom [24] checks, at each iteration, for “regression”, i.e. whether the new node

has been generated in a place already “covered” by the tree. A new node which is in

regression is designated “dormant”, i.e., losing (temporarily) the right to be extended in the

further iterations. Consequently, only a portion (referred to as “live” nodes) of all the tree

nodes will have the chance to be extended in further iterations. Intuitively, this regression

checking mechanism avoids repeated search in the already explored area, hence leading to

efficiency. Note that there are cases where the dormant nodes are key to the solution path,

so RRT-Blossom also uses a mechanism to wake them up later. RRT-Blossom has shown

advantages over RRT-CT, especially in the regression prone cases, such as planning motions

for a non-holonomic robot.

However, RRT-Blossom’s regression checking condition does not work properly in the

UC-space. The main reason is that the uncertainty component is determined by several

factors, including uncertainty of the father node, the path taken to reach the node, and the

environment itself. Recall that the robot will conduct a sequence of simulated localization

steps to acquire the uncertainty component. Therefore it can arbitrarily change and hence

leads to branches of arbitrary lengths, which implying that the generated node may fall out-

side the Voronoi region of the father node, hence deemed in regression by RRT-BLOSSOM.

In our RRT-SLAM-FD, we revisit this issue and propose a Fractal Dimension based

regression checking approach from a sequential data clustering point of view, and apply this

new regression checking approach to address efficiency issue of RRT-SLAM in the UC-space.

Probabilistic Roadmap (PRM)

PRM based approaches places random samples in the C-space and retains the ones that

are collision-free as milestones. These milestones are connected using a local deterministic

planner to create the roadmap. Therefore the roadmap gradually captures Cfree, and can

be reused for multiple path queries (for different start/goal problems). After the roadmap


construction, PRM will query the roadmap, i.e., check whether a solution path exists over

the roadmap. If the query phase fails, the roadmap will be enhanced by adding more

milestones and edges. By iteratively repeating the enhancement and query phases, the

roadmap incrementally captures the connectivity of the Cfree..

Many, indeed most of the PRM’s variations focus on the sampling step to construct

the roadmap, where a difficult problem is to generate samples in the narrow passage, or to

identify the narrow passage. [35] uses a bridge (C-space line segments between two samples

in collision) testing technique to place samples in the middle point of the bridge. [28, 36]

generate samples that follow gaussian distributions with the mean positions as those samples

in collision. Workspace based sampling technique is also introduced [37, 38] to identify the

narrow passage. These approaches are also referred to as biased (to the narrow passages

area) sampling techniques.

A combination of the biased sampling with uniform sampling (a fixed ratio of samples is

uniformly distributed, and the rest of samples is generated by biased sampling techniques)

[35] have also been used. We apply, in this research, only the uniform sampling technique,

which as reported in [35], is also important for the overall efficiency of PRM.

For a single query problem, removing all the edges in collision may not be necessary. In

[28], the authors propose a fast single query technique, i.e. lazy-PRM, which checks collision

for only a portion of the roadmap edges. We will review this Lazy-PRM in detail in Chapter

4.

1.3.2 Motion Planning Under Control and Sensing Uncertainties in Known

Environments

Motion planning, considering robot control/motion and sensing uncertainties in known en-

vironment, especially for mobile robots, has been studied for decades.

Preimage Back-Chaining

Preimage back-chaining [39] is one of the earliest works that incorporate robot control/motion

uncertainty into the planning stage in known environments. No sensor readings are available

during the motion. This planner searches for a sequence of actions that guarantee to bring

the robot into a goal region. The algorithm starts with the worst-case geometric reasoning

about the goal region’s preimage, from within which if the robot executes a certain motion


command, it is guaranteed to attain the goal region. Then preimages of the preimages are

calculated iteratively until there exists a preimage that includes the start region.

Similar to the preimage technique in [39], [40] also represents the robot’s localization

uncertainty with geometric regions. But the authors assume that the robot can localize

itself in a special area that is referred to as the landmark area. With multiple landmark

areas distributed on the robot’s way from the start position to the goal region, the authors

search for a sequence of landmarks for robot to visit, such that robot’s travel distance is

optimized.

Graph Based Planners with Localization Uncertainty

The consequence of the control/motion and sensing uncertainties is the localization uncer-

tainty, i.e., the state of the robot at a given time instant is no longer a single deterministic

configuration, but is instead represented by either a geometric bounding set [39, 40] as above

mentioned, or by a probability distribution [41, 42, 43, 44, 45, 46].

When planning the motion under localization uncertainties, there exists a large body

of literatures, which could be treated as an extension of the model-base MP techniques.

They are similar in that the solution of motion plan is a path, which is a sequence of

configurations. In the execution stage, the robot will follow this solution path. A natural

question that arises here is how control and sensing uncertainties affect the cost of traveling

along a path. Many, indeed most works model the path following in a stochastic manner

[44, 47], which allows the use of Bayesian filtering techniques (e.g. a Kalman filter [48] or a

sequential particle filter [49]) to update the robot localization uncertainty along the path.

If the robot localization uncertainty can be estimated along a given path, the travel cost

can then be explicitly acquired depending on the applications. In [43, 50, 51], the path has

to be free of collision. In [42], energy consumed along the path is studied. [44, 45, 46, 50, 52]

applies the amount of the uncertainty along (or at the end of) the path as a constraint. In

this research, the collision probability is applied as the major evaluation criterion.

Once the travel cost along a path can be explicitly acquired, planning algorithms can be

applied to search for a solution path over workspace grids [42, 43], roadmap [45], random

tree [46, 51], or the path-space [50].

• Searching for an optimal path over workspace grids or roadmap In [21], the path

searching is conducted over a grid map. An optimal path is returned with the travel cost


defined by a weighted linear combination of the path length and the amount of uncertainty

along the path. However, choosing the proper weight for the path length and the uncertainty

is usually difficult, because their scales may vary widely.

[22, 42, 43, 44, 45, 53] perform the searching in the UC-space, where the C-space com-

ponent representation is either discretized grids [22, 42, 43, 44, 53], or a roadmap [45] (with

biased sample generation in areas where the robot is easy to localize itself), and the uncer-

tainty component in these works is an “area of ellipse” that is corresponding to the variance

of the Gaussian distribution used to represent robot pose distribution. We note that this

path searching problem in the UC-space is actually a special case of the constrained shortest

path problem, and the algorithms developed in these works (referred to as “modified A*”

in [22]) for searching in the UC-space are special cases of the CSPP solver, i.e., the labeling

algorithm, as applied in our PRM-BU, although these connections were not noticed by the

respective authors. The constraint in these works is the amount of localization uncertainty

at the goal. In [22, 43, 44, 45] the path is optimal in terms of the length. In [42, 53] the

path is optimal in terms of the energy consumed along the path.

• Searching with RRT [51] expands RRT in the UC-space, and also simulates the lo-

calization along the RRT extension step. But there are three major differences between

[51] and our RRT-SLAM: 1) we use particles, while [51] uses Gaussian distribution, whose

geometric shape is an ellipse in 2D, to represent the localization uncertainty; 2) the simu-

lated localization step is updated using a Kalman filter in [51]; 3) [51] does not explicitly

calculate the collision probability of a path. They simply require the bounding ellipse shape

(corresponding to the variance) of the Gaussian to be clear of obstacles (as far as we can

tell they consider for a point robot).

[46] presents an interesting variation of RRT, called particle RRT, to search for a feasible

motion plan to the goal under robot control uncertainty (due to unknown friction param-

eters, in their case). Each extension to the tree corresponds to a given command and is

attempted several times under different conditions (corresponding to different friction coef-

ficient values drawn from a priori distribution). Nodes in the tree are created by clustering

the results from these simulations. Our ideas have some similarity to those in [46] in that

both simulate the possible consequence of following a path. However, we explicitly consider

the sequence of observations in the planning stage because our system is equipped with an

on-board range sensor, which is not addressed in [46]. Secondly, we address the travel cost


in terms of collision probability along the path, which is not a concern in [46].

• Searching for a path in path-space [50] first searches for an initial path, whose

travel cost might be high due to the localization uncertainty. The authors then apply the

gradient descent approach, in the path space, to adjust the entire path until acquiring a

locally optimal path with its travel cost under the localization uncertainty being minimized.

Partially Observable Markov Decision Problem (POMDP)

Other than searching for a solution “path”, a more general framework for planning under

robot control and sensing uncertainties, with the localization uncertainty represented by

a probability distribution, is the Partially Observable Markov Decision Process (POMDP)

[41].

In POMDP, the workspace is often discretized into grids, and the robot position is

represented by a distribution over the entire workspace, which is referred to as a belief. The

solution to POMDP is then a policy, which maps a belief to an action. When the map is

accurate, the dimension of the belief space, i.e., a space that contains all the possible beliefs,

equals the number of the grids in the workspace.

The complexity of POMDP increases exponentially with the dimension of the belief

space, and therefore is impractical for solving many real problems requiring large grid sizes.

Recently, two works [41, 54] have tackled higher dimensional POMDP for practical problems

by acquiring approximated solutions. However, the preprocessing time in [41] may take

hours. [54] shows a successful example in target tracking problem but requires a precisely

known environment. For realistic motion planning/exploration problems where the map is

not accurate, the belief space would become a space of the joint of both the robot location

distribution and the map distribution, and therefore has a prohibitively large dimension for

current POMDP algorithms.

If there exist only the control/motion uncertainty, and the on-board sensor is perfect, i.e.,

providing precise information about robot’s location ( the outcome of a control command

execution is not precise, but robot precisely knows its true location after the execution),

POMDP is reduced to Markov Decision Process (MDP) [55]. In the MDP framework, the

sampling based roadmap, instead of workspace grids, can also be used to represent the robot

state, which naturally combines MDP with the PRM framework [56]. The solution to MDP

is obtained by the classic approach of dynamic programming, which finds the optimal policy


subject to the expected cost of actions being minimized. [55] shows an example of MDP

with grids of about 25 by 25 when planning a motion for the mobile robot with motion

uncertainty.

1.3.3 Motion Planning Under Map Uncertainty

Instead of localization uncertainty resulting from the control/motion and sensing uncertain-

ties, [57] extends the work of PRM and assumes that the map is not precisely known. It

assumes that the robot can follow an exact path, and that the travel cost of the path is

calculated using a linear combination of collision probability and the length of the path. The

map is a feature based map, and the map uncertainty is represented with a joint Gaussian

distribution for features.

1.3.4 Exploring Unknown Environments Under Control and Sensing Un-

certainties

Some recent works incorporate exploration within the SLAM framework, in particular, e.g.,

[16] and [58]. [58] simulates SLAM for a given set of commands and chooses the one that

maximizes information gain. The information gains for the known part and the unknown

part are evaluated separately. The known part is used to generate a sequence of simulated

range sensor readings for the SLAM simulation, and the result of SLAM simulation is used

to calculate explicitly the information gain for the known part. For the unknown part, the

authors applied the summation of the average entropy changes for each grid in the unknown

area that will be covered by the sensor.

Our RRT-SLAM has similarity to those in [16] and [58] in using simulated SLAM. RRT-

SLAM also acquires the simulated range readings from the known part by ray-casting for

the simulated SLAM. For the unknown part of the environment, we treat it as free when we

do the ray-casting. Theoretically, the unknown part may help reduce the robot localization

uncertainty in the SLAM process if there exist some obstacles inside the unknown area, and

correspondingly affects the collision probability calculation. However, such an evaluation

requires to consider all the possible sensor readings received from the unknown part, which is

computationally expensive. Currently there exists no clearly approach to efficiently address

this issue and we leave it as an open problem for future work 2.

2The work in [12] might shed a light on this issue.


Mainly different from RRT-SLAM, the set of potential paths considered in [16] and [58]

(the Voronoi diagram of the environment) is often small and the simulated SLAM is not

integrated within an overall path planning framework. In addition, they are not concerned

about the collision probability of a planned path.

B

A

sensing rangepath1

path2

Figure 1.5: Robot motion planning in a wide open area. The ellipses along the two pathsrepresent the localization uncertainties.

For instance, when following a Voronoi diagram based path from A to B in Figure

1.5 (shown as path1), the robot’s localization uncertainty (marked by the ellipses) may

accumulate quickly along the path (since the robot can not localize itself well due to the

absence of objects within the sensing range along the path), and the robot could get “lost”

and then remains lost as it reaches close to B and may collide with B. Our RRT-SLAM

planner will return an alternative solution such as path2, which, although longer, is a better

choice, since it stays close to the wall and allows the robot to localize itself when traveling

along the path with its on-board range sensor, thereby reducing the chance of collision

caused by the localization uncertainty when approaching the goal area near position B.

[59] proposes an integrated approach that uses the sensor-based Random Tree (SRT) to

build a roadmap of the explored area. The authors use localization potential ( presumably

would provide a fast approximation to the simulated localization) to evaluate view candi-

dates (in the local mode). However, a key assumption is that the sensor covers the entire

body of the robot, hence this approach is not applicable to our system. In addition, the

mapping uncertainty and the collision probability are not addressed.


1.3.5 RRT-SLAM Contribution to Current Path Planning Techniques

Table 1.1 shows how RRT-SLAM contributes the motion planning literatures when planning

a path with localization and/or mapping uncertainties. As mentioned before, to return a

robust path, the basic framework is to apply a stochastic test/simulation along the path

to evaluate the travel cost. Then the model-based MP path searching algorithm is applied

to search for a feasible or optimal path. RRT-SLAM falls into this category. Most of the

current works apply the extended Kalman filter (EKF) based simulated localization along

the path to address the path safety. Particle filter based simulations have rarely been used

for path evaluation in motion planning tasks. This is the gap that our proposed RRT-SLAM

algorithm fills in. We will discuss why we choose this particle filter based framework in the

next sub-section.

Model!based motion planning techniques

Path planning

uncertainty

A!star PRM RRT Voronoi

Diagram

Simulated

SLAM or

Localiza!

tion along

the path

EKF

based

Map only [57]

Localization only [22,42,43,

44,53]

[45] [51]

Map and localization [16]

Particle

filter

based

Mapping only

Localization only [46]*

Map and localization RRT!SLAM [58]

*No sensor observation is considered.

Table 1.1: RRT-SLAM’s relationship with current existing path planning techniques con-sidering localization and/or mapping uncertainties.


1.3.6 Representations of Localization and Mapping Uncertainties

Robot localization uncertainty is generally represented by a Gaussian distribution [48] or

by particles [7]. The Gaussian based representation of robot localization uncertainty is

usually used in a feature based map. The localization uncertainty (and also the features

uncertainty) can be updated with an Extended Kalman Filter (EKF) based localization (or

SLAM) algorithm.

The particle based localization uncertainty representation can be applied in either a

feature based [20] or a grid based map [60]. Each particle represents a base pose (or a base

pose together with a map, under the particle based SLAM framework) associated with a

weight, which indicates the chance of this particle being the true base pose (or joint state

of both base location and the associated map).

We choose particle based representation for robot localization uncertainty for mainly

three reasons: 1) the true distribution of the robot location (or the joint of robot location

and the map) is complex and may not be well represented by the Gaussian based model

[41]; 2) the particle based representation will allow the collision probability of a path (i.e.,

the travel cost) to be calculated numerically; 3) the Gaussian distribution could also be

sampled by particles [57], which indicates the possibility of transforming the Gaussian based

representation to the particle based one.

We use particles to represent the distribution of the robot location at each tree node.

However, when we apply RRT to conduct the search, a distance metric is required to mea-

sure the distance between two tree nodes, which contain two sets of particles, each of which

represents a distribution of robot pose. There are large body of literatures that could be

considered, for example the KL-distance [61] that measures distance between two distribu-

tions. We consider an aggregated representation of the particles set by the sample mean

(i.e., a configuration) and the size of bounding box of the particle set (uncertainty), together

form the UC-space.

1.3.7 Representations of Physical Space

How the uncertainties (for localization or mapping) are represented is usually tied closely

to physical space representations. In mobile robot research literature, occupancy grids[62]

and feature maps [48] are extensively used.

The occupancy grid model discretizes the workspace into grids. A binary value (0 or 1) or


a probability, is then assigned to each grid. This approach is easily extended to 3D space with

voxel maps/octrees [14]. The feature map uses features, such as line segments (or surfaces in

3D [63]), and simple geometries, to represent objects inside an environment. Locations of the

features could be either deterministic or represented by a joint distribution (e.g., a Gaussian

distribution is used in the presence of mapping uncertainty). Feature based representation

is memory efficient, and can be easily applied to sensor data for registration. However the

collision probability computation [57] is not as straightforward as for the occupancy grid

map.

We use the occupancy grid representation, even though it demands extensive memory,

primarily because it is particularly useful for path finding and facilitates collision probability

computation.

1.3.8 Constrained Shortest Path problem (CSPP) and Lazy Path Query

of the Roadmap

The path query problem addressed in our PRM-BU is a constrained shortest path problem

(CSPP) [64], well studied in computer science literature. The labeling algorithm has been

widely used as an effective solution to this problem.

As discussed in [64], the preprocessing, i.e, identifying those edges that cannot be part

of the solution, is important for the overall performance of the labeling algorithm. For some

CSPP, e.g., the weighted constrained shortest path problem (WCSPP), the preprocessing

cost is proportional to the size of the graph. However, the preprocessing in our path query,

which is also formulated as CSPP, involves expensive collision checking (instead of simple

weight comparisons as in WCSPP) for all the edges of the roadmap [28]. Therefore, PRM-

BU is computationally slow for single path query.

This issue is successfully addressed by our Lazy-PRM-BU. In conjunction with the

k-shortest path algorithm, our Lazy-PRM-BU substantially reduces the number of edges

needed to be checked for collision.

1.4 Publication Notes

Several of the main ideas in this thesis have been published or submitted for publication

during the course of this research. The combination of RRT and SLAM to plan motion

with localization and mapping uncertainties was presented in [65]. The manipulator motion


planning with base pose uncertainty is reported in [66]. The Delaunay triangulation inspired

adaptive sample connection strategy was given in [67]. The efficiency of the RRT in the

UC-space was reported in [68].

1.5 Thesis Contributions

The thesis focuses on motion planning for a mobile manipulator in the presence of localiza-

tion and mapping uncertainties. Two motion planners using random graph based techniques

are introduced in the context of exploration and inspection tasks. The novel contributions

and significant findings in this thesis are listed as follows.

• A probabilistic framework that extends sampling based planning techniques to incor-

porate localization and mapping uncertainties. More specific, RRT-SLAM, a specific

algorithm that combines RRT with particle based SLAM algorithm.

• A novel RRT-SLAM-FD algorithm, which reduces RRT’s sensitivity on distance met-

rics, particularly in the UC-space, by smartly incorporating techniques of sequential

data clustering.

• A framework that extends PRM for manipulator motion planning with base pose

uncertainty. With the uncertainty represented by particles, the NP-hardness of the

path query problem, i.e., CP-CSPP, is proved.

• Two specific algorithms to address manipulator planning under base pose uncertainty,

particularly on the path query issue.

– PRM-BU algorithm, a general labeling algorithm based solution to CP-CSPP.

– Lazy-PRM-BU, a algorithm combining the labeling algorithm with the k-shortest

path algorithm, substantially reducing the number of candidate path queries and

achieving fast single query.

• A novel Delaunay triangulation inspired adpative sample connection technique, to

achieve better connectivity of the roadmap.


1.6 Thesis Outline

The rest of this thesis is organized as follows: Chapter 2 discusses RRT-SLAM in a frame-

work of RRT in the UC-space. Chapter 3 introduces an efficient variation of RRT in the

UC-space. Chapter 4 presents our extension of Lazy-PRM for the manipulator under base

pose uncertainty. A novel roadmap construction from a node connection point of view is

discussed in Chapter 5. Finally, the conclusion and discussion of future work are given in

Chapter 6.

Chapter 2

RRT-SLAM for Motion Planning

Under Mapping and Localization

Uncertainties

In this chapter, we introduce RRT-SLAM, which uses a rapidly-exploring random tree

(RRT) to plan a path, and fully accounts for the localization and mapping uncertainties.

As mentioned earlier in Introduction chapter, the basic idea is to “simulate” the particle

based SLAM, i.e., FastSLAM, as we extend a RRT tree node, and update the collision

probability of the path corresponding to the new node (from the root to this node) based

on the outcome of the simulation.

Before RRT-SLAM is presented, we first introduce the FastSLAM algorithm as described

in [49]. Then we discuss how to conjunct this FastSLAM framework with RRT to plan a

path.

2.1 FastSLAM: an Overview

The content below about FastSLAM (Algorithm-1) is based on what is described in [7, 20].

The notations used in this section indicate the events for the execution stage (later, we will

use the similar notations, but for the planning stage, to indicate the events “potentially in

the future”). Suppose the robot has executed a series of actions from the time index 0 up

to t (current time index). The algorithm considers the joint distribution of the robot trail,

25

CHAPTER 2. RRT-SLAM 26

νt = {q0, · · · , qt}, and the environment model, m, given a sequence of robot motion controls,

at = {a0, · · · , at} and sensor readings, κt = {κ0, · · · , κt} up to time t in the execution stage

(qt, at, and κt are the robot configuration, the control command, and the sensor reading at

time t, respectively). This joint distribution can be formulated as:

p(νt,m|κt, at) = p(νt|κt, at)p(m|νt, κt, at), (2.1)

Algorithm 1: FastSLAM

begin1

while 1 do2

for each particle q[i]t−1 in St−1 do3

q[i]t ∼ p(qt|q[i]

t−1, at−1);4

S ′

t = {S ′

t , (q[i]t , νt−1,[i])};5

Calculate the importance factor w[i]t (resample step);6

Acquire St: draw from S ′

t based on the importance factor;7

t = t + 1;8

end9

notation:St: an SLAM particle set at time t

This multiplication in Equation 2.1 means that the SLAM problem can be decoupled into

two independent problems. Once the estimation of the posterior, p(νt|κt, at), is acquired,

p(m|νt, κt, at) can be easily obtained because it involves only the mapping problem [7]. The

posterior, p(νt|κt, at), can be estimated by FastSLAM using a particle filter. FastSLAM

maintains the posterior p(νt|κt, at) as a particle set at each time instant t. The particle set

at time t is incrementally built based on the particle set at t − 1, the robot control at−1,

and the new observation κt at time t.

Specifically, denote the particle set at time t as St, and each particle in St as νt,[i] =

{q[i]0 , · · · , q

[i]t }. As presented in Algorithm 1, each particle νt−1,[i] = {q[i]

0 , · · · , q[i]t−1} in St−1

is first used to generate a probabilistic guess of the robot pose at time t (i.e., q[i]t ), using

the probabilistic motion model of the robot, p(qt|q[i]t−1, at−1)(line-4). This estimate, q

[i]t , is

then added to the particle νt−1,[i] to form a temporary particle set at time t, denoted here

as S ′

t (line-5). S ′

t is distributed according to p(νt|κt−1, at) given the observation κt−1 at

time t− 1.. The key step of FastSLAM is to sample from S ′

t to obtain St with a probability


proportional to the important factor, w[i]t (line-7). This step is called resample step. The

important factor is related to the new observation κt at time t, and is defined as (referring

to [7, 20] for how this importance factor can be calculated),

w[i]t =

p(νt,[i]|κt, at)

p(νt,[i]|κt−1, at) (2.2)

2.2 Simulated SLAM Framework: Determining Collision Prob-

ability Along a Planned Path

Due to the localization and mapping uncertainties, the collision status of a path is no longer

simply collision-free/in-collision, but instead a collision probability is associated with the

path and is used as a travel cost. In this section, we describe how we evaluate the collision

probability along a planned path while taking into account the localization and mapping

uncertainties.

2.2.1 Simulated SLAM Framework

From now on, the notations are used for the planning stage, and indicate the events that

could potentially “happen in the future” unless otherwise specified. Please note that the

framework is developed for a holnomic mobile robot, but we do need the orientation in-

formation for collision checking and sensing scans during execution and during simulated

SLAM. This orientation information is easily derived and we will specify it as needed.

Let π denote a planned path, which consists of a sequence of robot positions (xi, yi).

Let νπ denote a possible trail (a sequence of robot configurations) the robot would traverse,

were it to execute the planned path π (we drop the subscript π to simplify notation in this

chapter). So essentially, the planned path π corresponds to a distribution of a set of potential

trails. This distribution depends on the possible potential maps and sensor readings. The

collision probability of π is simply an expectation of collisions over these trails.

We now set up the framework to calculate this collision probability within a particle

filter based SLAM. We use the standard SLAM notation [7]. Let t denote the current time.

Let κν (κπ) be the sequence of potential sensor measurements along trail ν (path π). At

times, we may omit the subscript ν (or π) for simplicity of notation. We use qsubscript to


denote the robot configuration or robot state at the indicated subscript (time or location).

We use [x, y, θ] to denote a mobile base pose at position x and y, with orientation of θ.

Denote m as the environment model. Let V (or Π) = {0, 1} be a binary random variable to

indicate the event that robot trail ν (or π) is collision-free (1) or not (0). We then have:

p(Π = 1)

=

∫∫∫

νmκ

p(V = 1|ν,m, π, κ)p(ν,m, κ|π)dνdmdκ

=

∫∫∫

νmκ

p(V = 1|ν,m)p(ν,m|π, κ)p(κ|π)dνdmdκ

(2.3)

Put in words, we have to consider the joint distribution of robot’s potential trails, sensor

readings, and the environment model updated along the given path π in order to acquire

the collision probability of following path π. The integrand comprises two terms, the first

one where p(V = 1|ν,m) is the probability of robot trail ν being free in the environment

model m, which is essentially a geometric calculation; and p(ν,m|π, κ) · p(κ|π), which is to

be acquired by running the particle filter based SLAM algorithm in simulation over possible

observations.

As in FastSLAM [7], we use particles to represent the joint distribution of the environ-

ment model m and the trail ν. Let m[i] and ν [i] be the environment model and trail in

the ith particle, respectively. Let ♯S be the number of SLAM particles. Equation 2.3 then

becomes

p(Π = 1)

.=

∫

κ

[

♯S∑

i=1

p(V = 1|ν [i],m[i])p(ν [i],m[i]|π, κ)

]

p(κ|π)dκ,(2.4)

where p(V = 1|ν [i],m[i]) is the probability of robot trail ν [i] being free in the environment

model m[i]; and p(ν [i],m[i]|π, κ), is acquired by running the particle filter based SLAM

algorithm in simulation. Note that the sequence of control commands at each time instant

t (denoted as aπt ) is not explicitly specified, but derived from the planned path π, and robot

state at t, e.g., [xnext, ynext] and q∗t , where q∗t is the sample mean configuration (or the most

weighted particle) of the particle set representing the robot state at time t, and [xnext, ynext]

is robot’s intermediate sub-goal, the point on π it would like to reach next in the path.

For the mobile base motion, aπt = [△dt,△θt], where △dt and △θt are translation and


rotation, respectively. Given q∗t = [x∗t , y

∗t , θ

∗t ], we set:

△θt = arctan (ynext − y∗txnext − x∗

t

) − θ∗t . (2.5)

△dt =

{

√

(xnext − x∗t )

2 + (ynext − y∗t )2, if|△θt| > εθ

0 else

where εθ is a user defined small value. Put in words, robot will first turn to the direction

pointing to the next intermediate goal and then move towards it.

We use the Equation 2.6 [69] below to obtain a discrete-time robot motion model:

xt+1 = xt + △dt cos(θt + 0.5△θt)

yt+1 = yt + △dt sin(θt + 0.5△θt)

θt+1 = θt + △θt

(2.6)

The additive noise to the translation △dt and rotation △θt are assumed by uncorrelated

Gaussian noises with zero mean, and variances σ2d and σ2

θ . Thus, the “motion model”

in classic SLAM [7] is replaced by a “path following model”, i.e., p(qt+1|qt, aπt ) ↔ qt+1 =

f(qt, aπt )+wt, where wt is the noise and f(.) represents the motion model given by Equation

2.6.

To determine p(Π = 1), we consider a “test-and-evaluate” algorithm (similar to [7, 58])

that conducts several simulated SLAM sequences, each of which consists of a series of

“simulated SLAM” steps: 1) draw a particle from the SLAM particle set with a drawn

probability equal to its weight; 2) apply the control command aπt to the drawn particle

based on the “path following model”, and generate a probabilistic guess of the robot pose;

3) use the map and the guessed robot pose to generate the sensor measurement by the

ray-casting; and 4) given aπt and the simulated sensor measurements, update the map and

the robot pose with the SLAM particle filter.

In the third step of above “test-and-evaluate” algorithm, for the ray-casting operation,

[58] treats the beam differently for information gain calculation in known and unknown

areas. For the known area, the distance between the robot pose and the known obstacle

along the beam (with extra range sensor noise if applicable) is returned, and is applied to

the simulated SLAM to estimate the information gain. For the unknown area, the average


entropy change is considered to be proportional to the size of the unknown area that will

be covered by the sensor beam (it is empirically verified).

In our work, when doing the ray-casting, we treat the known area in the same way

as described in [58], but treat the unknown area as free because we concern the collision

probability instead of the information gain 1. Theoretically, the unknown area may help

reduce the robot localization uncertainty in the SLAM process if there exist some obstacles

inside this unknown area, and correspondingly affects the collision probability calculation

(shown later). Intuitively, treating the unknown area as free in our case means that the

localization and mapping uncertainties acquired from the simulation tend to be “larger”

than those in the real execution, because we discard the possible environment information

acquired from the unknown that may help the robot better localize itself. And our estimation

is then, in general, a worse case than what will really happen in the future, i.e., in the

execution stage. In the future, we plan to consider more realistic models, e.g., [12], for the

unknown area, other than treating it as the free in our case.

The simulated SLAM steps are repeated until the robot is deemed to reach the end of

the path π. Therefore a stop condition has to be applied. We stop the simulation if the

sample mean configuration of the particle set representing the robot state is within a distance

threshold of the configuration at the end of the path. Intuitively, this distance threshold

should be correlated to the uncertainty at the end of the path π (a larger uncertainty should

correspond to a larger distance threshold). Since we require the uncertainty along the path

to be bounded (referring to Section 2.3 for details). In this case, we simply set this distance

threshold as a fixed value.

By repeating the simulated sequence M times, we can replace the integration step over

κ in Equation 2.4 with a summation. Let κ[j] be the sequence of sensor measurements in

the jth simulated sequence, and ν [i,j], m[i,j] be the trail and the environment model for the

ith particle in the jth simulated SLAM sequence,

p(Π = 1).=

M∑

j=1

[

♯S∑

i=1

p(V = 1|ν [i,j], m[i,j])p(ν [i,j], m[i,j]|π, κ[j])

]

· p(κ[j]|π)(2.7)

1This is only for ray casting; for collision detection purposes, as we need later to determine the collisionprobability, the unknown areas are treated as obstacles.


The probability p(κ[j]|π) is given by [58]:

p(κ[j]|π).=

♯S∑

i=1

p(κ[j]|ν [i,j], m[i,j]).p(ν [i,j], m[i,j]|π) (2.8)

The first term in the summation in Equation 2.8 can be approximated by ray-casting

operations performed in the map of each particle and the second term represents the weight

of that particle.

We now discuss how to compute the collision probability for a given trail, i.e., the first

factor term p(V = 1|ν [i,j], m[i,j]) in Equation 2.7. We use occupancy grids to represent the

map. The value associated with each cell indicates the probability of the cell being occupied.

The probability of a trail being collision-free in a given occupancy grid map m is simply the

product of probabilities of being free for all those cells swept by the robot along the trail in

a given map.

Using Equation 2.7 and 2.8, we can now determine the collision probability for a planned

path. The computation time is M multiplied by the time taken to simulate the particle based

SLAM algorithm and the embedded collision probability computation cost. The SLAM

algorithm using grid based map, has the update cost at O(♯S2|A|) at each time instant [60],

where |A| is the size of area swept by the range finder. Then, the overall cost of evaluating

the collision probability of a path is O(M ∗ (DSLAM ∗ ♯S2|A|+Dcoll ∗ ♯S ∗E), where DSLAM

is the number of times SLAM is called to update the location and the map along the path,

Dcoll is the number of times the collision checking is called along the path (for all the ♯S

particles each time, referring to Section 2.2.2 below for more detail), and E is the time

cost for one single collision checking. Here Dcoll and DSLAM is proportional to the length

of the path. The most time consuming part is the simulated SLAM update. For a typical

computer setup (referring to the Experiment section), it takes up to 0.8 second for one single

(one time instant) SLAM update.

Therefore, the simulated SLAM steps, are relatively expensive in terms of computational

cost, and are not practical for searching over a large amount of possible paths (such as in

RRT) in an online manner 2. In our implementation, we propose two simplifications for

online implementation on a real system: (i) we do not update the environment model

during the simulated SLAM, but instead plan robot’s motion based only on our current

2Given enough computational resources, for example, a parallel implementation, full simulated SLAMcould easily be implemented.


knowledge about the environment by ignoring possible information/knowledge gain about

the environment through simulations; and (ii) we use the environment model, denoted as m∗,

in the most weighted SLAM particle (essentially the best estimate of the environment and

the robot’s location in it), instead of using all the maps in the SLAM particles. Effectively,

this implies that instead of “simulated SLAM” we execute “simulated localization” with

respect to the map in the most weighted SLAM particle. Such simplifications save the time

cost on map updating step, the most time consuming part of the particle based SLAM

algorithm, in each time instant.

2.2.2 Particle Based Simulated Localization

Let ♯L be the number of particles from the simulated localization. Equation 2.7 simplifies

to

p(Π = 1)

.=

M∑

j=1

[

♯L∑

i=1

p(V = 1|ν [i,j], m∗)p(ν [i,j]|m∗, π, κ[j])

]

· p(κ[j]|m∗, π)(2.9)

Although faster than Equation 2.7, running multiple simulated localization sequences is

still computationally expensive.

We now discuss an alternative formulation (and the mathematical justification behind

it) in which the trail particle sets could be sampled sequentially over time from the particle

sets that represent the robot state at each time instant in one simulated sequence. The

collision probability when the robot travels along the planned path could also be updated

sequentially over time as we update the trail particle set.

From now on we need to distinguish between the particles representing the trail (we call

them trail particles) and the particles representing the robot state (pose) at a single time

instant t (we call them state particles). We denote the state particle set by Lt, which consists

of a set of robot configurations at time t, i.e., Lt = {q[1]t , · · · , q

[N ]t }. The trail particle set

consists of a set of trails.

From Equation 2.9, summing over κj , we have:

p(Π = 1).=

♯L∑

i

p(V = 1|ν [i], m∗)p(ν [i]|m∗, π) (2.10)


Now the key issue is to acquire particles that represent the distribution p(ν|m∗, π) in

a sequential manner. Note that, for one specific sequence of simulated localization along

the path, the acquired trail particle set, representing p(ν|m∗, π, κ), can not be used as the

approximation to p(ν|m∗, π). The main reason is that when a finite number of particles is

used, trajectory posterior acquired from the simulation is often sparse for the state points

in time lying further back in the history after a series of resample steps in the particle filter

[58].

We start with an approximation (similar to [58, 21]) of p(qt|m∗, π) with p(qt|m∗, π, κ∗t ),

where κ∗t is the sequence of range readings received from time 0 to t in the simulated

sequence:

p(qt|m∗, π).= p(qt|m∗, π, κ∗

t ) (2.11)

Given the approximation in Equation 2.11 we could run the simulated localization

sequence once, and use the returned robot state particle sets at each time instant, i.e.,

L0, · · · ,Lt, as approximated representations of p(q0|m∗, π), · · · p(qt|m∗, π), repectively.

We now need to calculate the likelihood p(ν|m∗, π) for a single trail ν. A possible trail ν

consists of a sequence of robot configurations, ν = {q0, · · · , qt}, with each configuration, qt,

as a particle from the state particle sets, Lt at successive time instant. Under the commonly

used assumption of Markov property about robot localization [7], we have,

p(q0 · · · qt|m∗, π) = p(qt|qt−1, m∗, π) · · · p(q1|q0, m

∗, π)p(q0|m∗, π, ), (2.12)

which means we need to calculate the conditional probability p(qt|qt−1, m∗, π) for all the

time instants to acquire the likelihood of the whole trail.

To determine p(qt|qt−1, m∗, π), consider two extreme cases: 1) no sensor readings, at

time t, are available or the readings do not supply extra information about robot location.

p(qt|qt−1, m∗, π) is then simply the “path following motion model”, i.e., p(qt|qt−1, a

πt ); 2) the

sensor readings provide enough information to localize the robot accurately at time t. In

this case, qt is independent of qt−1. Given that the range sensor is quite accurate, it tends

to localize the robot quite well. Hence, we consider the two extreme cases as reasonable

approximations in our case.

Note that the more information is supplied from the range sensor readings, the more


likely that qt is independent of qt−1 (this is Case 2 mentioned in above paragraph). Conse-

quently, only a small number of particles will have descendants in the resample step3. Let

the number of particles in Lt−1, who have descendants in Lt, be φt−1. We consider the ratio

ηt−1 = φt−1

♯L. If it is lower than a threshold τp, we assume that qt−1 is independent of qt.

Otherwise, p(qt|qt−1, m∗, π) is determined by the “motion model”. Formally,

p(qt|qt−1, π, m∗).=

p(qt|qt−1, at−1), ηt−1 ≤ τp

p(qt|m∗, π), otherwise(2.13)

where at−1 is the control command for the robot to follow the path between t − 1 and t.

Given the approximation techniques mentioned above, i.e., Equation 2.12 and Equation

2.13, we can now calculate the likelihood of any given robot trail. Then a trail particle set

that represents the distribution p(ν|m∗, π) can be acquired from the state particle sets at

successive time instants, with each trail going through one of the state particles at each time

instant, and a linear interpolation between two time instants. In our work, we keep the size

of the trail particle set equal to the size of the state particle set.

Figure 2.1 shows an example of sampling trail particle sets from state particle sets at five

successive time instants (t = 0 to t = 4). The size of state particle set is 4. Therefore there

exist 45 possible trails in total, with each trail going through one of the state particles at

each time instant. To acquire the trail particle set of size 4, we can use the trail likelihood

as the sampling weight to sample 4 trails (shown in different line style) among all these

possible trails.

t = 3t = 1 t = 2t = 0

L0 L1 L2 L3

t = 4

L4

Figure 2.1: Sample trail particles from the state particle sets (L0, L1, ..., L4) at each timeinstant (t=0 to 4). The number of trail particles in the trail particle set is set as 4, whichequals the number of state particles at each time instant.

In practice, for further efficiency, sampling the trail particle set is done sequentially over

3Usually this also corresponds to a large uncertainty drop from qt−1 to qt, which could also be used as acriterion for evaluating the dependence of qt on qt−1.


time as the state particle set is updated at each time instant. This is incorporated in our

simulated localization algorithm as follows. Let the robot state at time t be qt and the

corresponding state particle set be Lt. By applying the motion command, Lt is extended to

generate the initial guess of L′

t+1. Then, the robot resamples from L′

t+1 based on a simulated

sensor readings at time t+1, and acquires the state particle set Lt+1. Once the state particles

set is updated from t to t+1, the trail particle set at time t+1 can be sequentially updated

using the standard sequential sampling (without replacement) technique [7]. Based on this

trail particle sampling technique, coupled with the collision probability calculation as shown

next, one can determine the collision probability for a planned path.

Collision Likelihood for a Trail

Since the laser range finder is quite accurate. In practice, we binarize the map m∗ (denote

the binarized version of m∗ as m∗b). Then, p(V = 1|ν [i], m∗

b) becomes a binary distribution

(collision-free/in-collision) and could be simply determined with a collision detector.

Collision Probability for a Path

After binarizing the map, the collision probability for a path π (Equation 2.10) then becomes:

p(Π = 1).=

∑

i

p(ν[i]f |m∗

b , π) (2.14)

where ν[i]f is the ith trail particle that is collision-free in map m∗

b . Equation 2.14 basically

says that the probability of following a planned path without collisions is the summation

of the normalized probabilities (also called normalized weights) for all those trails that are

collision-free.

Algorithm for Sequentially Updating Collision Likelihood Along a Path

We consider a path π that consists of many segments in the C-space, over a graph structure,

such that each end point of the segments along the path corresponds to a graph node. When

the robot moves from graph node ni to ni+1, the trail particle set as well as the collision

probability is continuously updated at each time instant t (we set t = 0, when the robot is


at graph node ni), as stated in Algorithm 2 4.

Localization Particles after resample

Free localization particles after applying the motion model

Colliding localization particles after applying the motion model

Particles for Initial state

L′

1 L1 L′

2 L2

t = 0 t = 1 t = 2

path πn0

n1

Graph nodes along the path π

L0 = Ln0

Parti les being sampled in L′

tat the resample step

Figure 2.2: Sample trail particles sequentially over each time instant.

Figure 2.2 schematically illustrates the algorithm for a simple scenario with state particle

sets Lt of size 4, and three consecutive robot states returned by the simulated localization

sequence at time t = 0, 1, 2, when the robot moves from graph node n0 to n1. We stop the

the simulated localization if the robot arrives at the next graph node n1 (line-3, IsNotNextN-

ode(.) returns true). At each time instant t, we first acquire the motion command to follow

the path given Lt and π (referring to Equation 2.5). We then apply the motion command

(line-5 to 7, Algorithm 2), aπt , to Lt to acquire the initial guess of the robot state at t + 1,

denoted by L′

t+1 (shown as circles). We approximate the true trail between state particles

in two consecutive sets with straight lines between them. Computing collision checks and

updating trail probability (line-9 and 10) is O(♯L2) at each time instant t, and therefore is

slow for a real time solution. For efficiency, we only test the collision of the straight line

4In Algorithm 2, as mentioned before, the simulated localization technique (line-4 to 7, line-11 to 15) isthe same as described in [7].


Algorithm 2: SimLocal

input : Lni, Tni

, π[ni,ni+1] , mb∗, Pni

free.

output: Lni+1, Tni+1, Pni+1

free .

begin1

t = 0, Lt = Lni, Pt = Pni

free;2

while IsNotNextNode(Lt, ni+1) do3

aπt = AcquirePathFollowCommand(Lt, π);4

for each particle q[i]t in Lt do5

q[i]t+1 ∼ p(qt+1|q[i]

t , aπt );6

L′

t+1 = {L′

t+1, q[i]t+1}7

Extend the state particles in Lt to the correspondent descendant in L′

t+1;8

Check collision and update the probability for each extended trail based on9

the “path following model”;Update Pt+1, i.e., the collision probability of following the path π up to time10

t + 1; (Equation 2.14)

Sample one particle, say q[j]t from Lt based on its weight;11

q′

t+1 ∼ p(qt+1|q[j]t , aπ

t );12

Do ray-casting from q′

t+1, and acquire a simulated scan of range readings, i.e.,13

κt+1 ;Calculate the importance factor for each particle in L′

t+1, given aπt and κt+114

(Equation 2.2);Sample Lt+1: draw from L′

t+1 based on the importance factor;15

Sample ♯L trail particles based on Equation 2.13;16

For each particle in Lt+1, record its index along with its associated weight of17

the trail particle into Tt+1;t= t+1;18

return < Lt,Tt, Pt >19

end20

notation:Lni

: the state particle set at graph node ni.Tni

: a data set stored at graph node ni. |Tni| = |Lni

|.Pni

free: the probability that robot is collision-free when arriving ni along the pathπ[qn0 ,qni

].♯L: the size of the particle set Lt.aπ

t , the control command to follow the path at time t.


between state particles in Lt and their descendants in L′

t+1, shown as dashed lines (both

bold and thin) in Figure 2.2. The updated probability of a particular extended trail (line-9)

is simply the previous trail probability multiplied by the probability of the motion model

used for extending the trail.

At line-10, the probability that the robot is collision-free when following the path π is

updated sequentially over each time instant. Let p(Πt = 1) be the probability that the robot

is collision-free when the robot follows the path up to time t. Based on Equation 2.14, we

then have,

p(Πt+1 = 1) = p(Πt = 1)∑

i

p(ν[i]ft+1

|m∗, π), (2.15)

where p(ν[i]ft+1

|m∗, π) is the normalized weight of the ith extended trail particle that is free

of collision up to time t + 1.

The simulated localization step (line-11 to 15) for acquiring Lt+1 (shown as solid circles)

is similar to the “test-and-evaluate” steps mentioned in Section-2.2.1 (except that it runs

the simulated localization instead of SLAM).

The sampling step for trail particles (line-16, referring to Figure 2.1 for detailed expla-

nation) is based on Equation 2.13 and could be performed using the standard sequential

sampling (without replacement) technique [7]. Again, for efficiency, we acquire the updated

trails by sampling from a subset of all possible updated trails (to acquire all the possible

updated trails, we need to extend each trail particle at time t to all the state particles in

Lt+1). This subset includes only those trails that are extended from the fathers (in Lt,

those from which have extended solid-lines in Figure 2.2) of the state particles in Lt+1. In

addition, we do not re-test the updated trails for collision. The updated trail particle set is

shown as solid lines in Figure 2.2.

Finally, since the collision-free probability is updated in a sequential fashion, we do not

need to explicitly memorize the trail particles. Only the likelihood of the trail, and the

associated index of the state particle, are updated and recorded at each time instant and

finally stored at the graph node at the end of the path (line-17, Algorithm 2).


2.3 Planning Motion with Localization and Mapping Uncer-

tainties (RRT-SLAM)

Now we know how to simulate SLAM along a given path. We embed this simulated SLAM

into each RRT extension step and apply the classic RRT version as described in [70] (Algo-

rithm 3), where each tree node is a configuration. In the extension step of the classic RRT

(line-6), the tree is extended from qc to qr with a fixed step, dstep, to acquire a new tree

node, qnew, which will be added into the tree if it is collision-free (shown in Algorithm 4).

Each time a new configuration, qnew, is created, if it is within a fixed range εd of the goal

configuration (line-8), RRT will try to reach the goal from qnew (line-9) and returns if the

extension succeeds.

Algorithm 3: Classic RRT

input: qinit - initial configuration.dgoal - a distance threshold.qgoal - goal configuration.dstep - a user defined fixed step.output: tree - a random tree.begin1

while !TimeUp() do2

qr = RandomConfig().3

qc =NearestNeighbor(qr,tree).4

qnew = ExtendTo(qc, qr, dstep)5

qnew=Extend-Classic-RRT(tree,qnew,qc)6

if qnew 6= NULL then7

if ρ(qnew, qgoal) ≤ dgoal then8

if Extend-Classic-RRT(tree, qgoal,qnew) then9

break;10

end11

We introduce an extra dimension, that of uncertainty, to the C-space (similar to [22],

except that they used A*), and RRT is built over the composite uncertainty-configuration

space (UC-space). As mentioned earlier, since the PowerBot is treated as a holonomic mobile

base, the C-space component in the RRT search contains only the position, i.e. qc = (x, y).

However, for simulated SLAM, we do need to include the orientation component as explained

earlier in Section 2.2.2. As for the uncertainty component, there exist many alternatives for


Algorithm 4: Extend-Classic-RRT

input: tree - a random tree.qr - a random configuration.qc - a tree configuration.output: tree - a random treebegin1

if the edge (qnew,qc) is collision-free then2

add qnew to tree as son of qc.;3

return qnew;4

else5

return NULL;6

end7

the measurement. We use the area of the bounding box that contains all the particles and

is easy to calculate 5. Therefore each tree node is composed of a robot base positions and

an uncertainty component indicating the “size” of the localization uncertainty.

We could use more than one dimension to represent the uncertainty component. For

example, the length and width of the particles’ bounding box. This is intuitively helpful if

the solution path is hard to find due to the simplicity of the uncertainty component, i.e., a

scalar. However, this usually tends to increase the search cost because extra dimensions for

the uncertainty components means to search in a higher dimensional space.

The detailed algorithm, which incorporates our collision probability estimation algo-

rithm, is shown in Algorithm 5. We have two criteria for the planned path: 1) the collision-

free probability along the path is higher than a user defined threshold; and 2) the uncertainty

of the robot at the goal position is smaller than another user defined threshold. Note that

the major modification of the basic RRT is in the “Extend” step (Algorithm 6).

We extend our tree as follows. We first generate a random node nr in the UC-space, and

apply a distance metric to choose the nearest one among the tree nodes (denoted as nc) to

the nr. The distance metric used in our RRT-SLAM is:

|nr, nc| = ϕ ∗ ρ(qr, qc) + (1 − ϕ) ∗ |uc − ur|, (2.16)

where qr, qc and ur, uc are the configurations and uncertainty components of the tree nodes

5Other measurements can also be used. For example the area of the smallest ellipse that bounds theparticle set, etc.


Algorithm 5: RRT-SLAM

input: Y - SLAM particle set before robot planning the motion.output: tree - a random tree.begin1

< q∗,m∗b > := the most weighted particle in Y;2

for i = 1 to ♯L do3

q[i]ns = q∗; ω

[i]ns =

[i]ns = 1

♯L4

Pns

free = 1;5

tree.init(qs, us = 0,Lns , T Lns

, Pns

free);6

while !TimeUp() do7

< qr, ur >:= RandomState() ;8

nc := NearestNeighbor(< qr, ur >, tree);9

qnew = ExtendTo(qnc , qr, dstep);10

nnew = Extend-RRT-SLAM(tree, qnew, nc,m∗b) ;11

if qnew 6= NULL then12

if ρ(qnew, qgoal) ≤ dgoal then13

if Extend-RRT-SLAM(tree, qgoal, qc,m∗b , dstep) then14

break;15

end16


Algorithm 6: Extend-RRT-SLAM

input: nc - a tree node.< qr, ur > - a node state the tree will extend from nc towards to.tree - a random treem∗ - a map.dstep - a user defined fixed step lengthbegin1

< Lnnew ,Tnnew , Pnnew

free > =2

SimLocal( Lnc,Tnc ,m∗, π[qns ,qnnew ]

, Pnc

free, );3

if Lnnew 6= NULL and Pnnew

free > Pthresh then4

unew = UncertaintyScalar(Lnnew);5

if unew > Bu then6

return;7

nnew := {qnew, unew,Lnnew ,Tnnew , Pnnew

free };8

add nnew to tree as son of nc;9

return nnew10

else11

return NULL;12

end13

notation: Pthresh: the collision probability threshold for a feasible path to the goal.Bu : a user defined uncertainty threshold.


nr, nc, respectively, and ϕ is a user defined weight between 0 and 1.

To extend the chosen tree node nc towards nr, there are many alternatives that depend

on the system setup. For example, in [70], for a non-holonomic mobile system, the author

applies a set of control commands that moves the robot towards different directions for a

fixed time △t and acquires a set of new nodes. Then the one that is closest to nr is kept as

the new node. In our case, such an approach is relatively computationally expensive, because

we need to simulate the localization step. We extend the C-space component of nc (i.e., qc),

towards qr for a fixed step to acquire a new configuration qnew (line-2 Algorithm 6). Then

the simulated localization is applied to extend the tree from nc to the new configuration

qnew. If the extension step succeeds, the uncertainty component of the new tree node nnew

(i.e., unew) is set to the area size of the bounding box that contains the set of particles

acquired from the simulated localization step (line-6 Algorithm 6).

Figure 2.3: A typical random tree returned by Algorithm-5

Figure 2.3 shows a typical tree in 2D with an extra dimension of localization uncertainty

(shown by an ellipse around the node). Tree branches are marked by light gray segments,

and the planned path is shown with black segments. The goal point is marked by the

dark square. Black areas are sensed obstacle, and gray areas are unknown. Note that in

this specific example, the planned path passes through the goal point twice. When the

robot reaches the goal point the first time, the localization uncertainty is higher than the

localization uncertainty threshold at the goal point. The robot needs further motions around

the goal area to localize itself better, and finally reaches the goal again with the constraint


on the localization uncertainty at the goal less than the required threshold.

(a) path1

(b) path2

Figure 2.4: The same goal but different goal uncertainties were given to the robot. Thepaths found were different. For the low uncertainty case (b), robot prefers to go closer toknown obstacles to localize itself better.

2.4 Simulations

We have run preliminary tests in our simulated test-bed. We acquire online [71] the open

source code and libraries implementation (referred as Gmapping [72]) of FastSLAM, and

incorporate them in our RRT-SLAM implementation. We use the collision detector that is

incorporated in the Player-Stage library [73].

To show meaningful examples, we manually set a sequence of intermediate goal positions,


moving the robot along these positions while executing SLAM to explore the simulated

environment. In this subsection, the range sensor in the simulated environment is 4 meters.

and the environment size is indicated by grids shown in the map, with each grid of size 1

meter by 1 meter.

A typical scenario, which indicates the effectiveness of our RRT-SLAM during the ex-

ploration is shown in Figure 2.4, where the robot needs to go across the open free area with

a collision probability less than 0.15 (chosen somewhat arbitrarily 6). The goals in Figure

2.4(a) and Figure 2.4(b) are at the same location but the uncertainty threshold at goal in

Figure 2.4(a) (0.8) is much larger than in Figure 2.4(b) (0.2). In the UC-space, they cor-

respond to two different points separated along the uncertainty dimension. Our algorithm

returns paths that satisfy the collision probability threshold along the path(s) and the lo-

calization uncertainty threshold at the goal. Note that even though path1 is shorter than

path2, the localization uncertainty along the first path is higher than the second path, and

the collision probability for the first path (0.023) is higher than the second path (0.0). Here,

the higher probability of collision for the first path is mainly due to the localization uncer-

tainty around the third last path node. We do not explicitly show the state particle sets

along the path in Figure 2.4, but use the ellipse to demonstrate the localization uncertainty

at each tree node along the path.

Figure 2.5 shows the advantage of applying RRT-SLAM compared with the classic RRT

without considering the localization uncertainty. For the path returned by the classic RRT

we also simulate the localization uncertainty and calculate the expected collision probabil-

ity. The collision probabilities for the paths in Figure 2.5 (a) and (b) are 0.0 and 0.82,

respectively.

Clearly, RRT-SLAM gives a more robust path than the classic RRT because the latter

searches for the path in the C-space without considering the localization uncertainty, and

hence might increase the chance for the robot to collide with the environment.


(a) A solution path returned by (b) A solution path returned by standard RRTRRT-SLAM. in our simulated test-bed, with no

uncertainty and collision probabilitytaken into consideration.

Figure 2.5: Comparing solution paths returned by RRT-SLAM and the classic RRT in thesimulated environment.

2.5 Experiments

We also implemented RRT-SLAM on a real PowerBot system. The Hokuyo URG-04LX

range scanner having a sensing range of 4.0m and a span angle of 180◦, is fixed on the

PowerBot and points to the front. The on-board computer has an IntelTM

CPU of 3.0GHz,

with RedhatTM

Linux installed.

Again, we first manually set a sequence of intermediate goal positions, moving the robot

along these positions while executing SLAM to explore the real environment. Figure 2.6

shows this partially constructed map of RAMP lab in Applying Science Building at SFU.

The size of the map is about 7x7 meters. In the task shown in Figure 2.6, the robot tries

to get to the end of the corridor for further exploration.

Figure 2.7 (a) and (b) show the paths returned by RRT-SLAM and classic RRT (with

collision probability of 0.0 and 0.21), respectively. This result clearly shows that path

6Intuitively, allowing a path with non-zero probability of collision and higher localization uncertaintytends to reduce the difficulty level of the planning problem. In practice, the specific threshold would beexperimentally determined by the user.


Figure 2.6: A map of the RAMP lab in the Applied Science Building at SFU. The next goalof the robot is marked.

(a) A solution path returned by (b) A solution path returned by the classicRRT-SLAM in real environment. RRT in in real environment, with no

uncertainty and collision probabilitytaken into consideration.

Figure 2.7: Comparing solution paths returned by RRT-SLAM and the standard RRT inthe RAMP lab environment shown in Figure 2.6.


Figure 2.8: Trace of the mobile base as it executes the path planned using RRT-SLAM forthe RAMP lab environment.

returned by the RRT-SLAM gives the robot more clearance (especially in the horizontal

corridor area), and therefore is safer compared to the path returned by the classic RRT.

Finally, the robot successfully executed the path planned by RRT-SLAM (Figure 2.7

(a)) and reached the goal. The trajectory of the robot is plotted in Figure 2.8.

2.6 Parameter Values Related to RRT-SLAM

Unless specified otherwise, we used the parameter values that are listed in Table 2.1 for

all the simulations and experiments about RRT-SLAM and its variation i.e., RRT-FD (ex-

plained in detail in next chapter). These parameters are divided into two groups. The first

group of parameters are related to the simulated localization step, where ♯L is the number

of state particles; σd and σθ are the standard deviations of the motion error for translation

and rotation, respectively; εθ is used to determine whether robot’s next motion includes

translation or not(Equation 2.5). The parameters in the second group are applied to RRT-

SLAM, where dstep (line-2 Algorithm 6) is the fixed step length for each RRT extension in

the C-space component; ϕ is the weight of the distance metric in UC-space for RRT-SLAM

(Equation 2.16); Bu is the largest uncertainty allowed for a tree node; Pthresh is the collision-

free probability threshold for a feasible path to the goal, and Bugoalis the goal uncertainty

threshold. In the planning stage, the collision detector is called every time the simulated

motion of the robot rotates (or translates) a fixed angle △θcol (or distance △dcol).


Simulated localization RRT-SLAM

♯L σd σθ εθ

30 0.01m 0.015o 5◦dstep ϕ Bu Pthresh Bugoal

△θcol △dcol

0.8m 0.2 2.0 m2 0.90 0.15 5o 5cm

Table 2.1: Parameter values applied in RRT-SLAM for the simulations and experiments.Unless specified otherwise, all simulations and experiments used these values.

2.7 Discussion

In our RRT-SLAM implementation, each time a tree node is extended, we need to simulate

the localization process, which is time consuming (takes up to around 0.5 second to extend

a tree node in our simulation and experiments). Therefore in the RRT-SLAM framework,

which embeds only the standard RRT technique, an efficient tree node generation [6] be-

comes important. This motivates us to further study the RRT efficiency in the UC-space,

which is the major interest in next chapter.

Chapter 3

RRT-SLAM-FD: Fractal

Dimension Based RRT-SLAM in

the UC-Space

In Chapter 2, we presented RRT-SLAM, a RRT based motion planning technique that incor-

porates robot localization uncertainty and mapping uncertainties. The algorithm searches

for a path in the UC-space. We now discuss the efficiency issue of RRT-SLAM in the

UC-space.

3.1 Tree Exploration in the Uncertainty-Configuration Space

(UC-Space)

In RRT-SLAM, the localization uncertainty component of a new tree node is acquired by a

the simulated localization step using the current best map (refer to Chapter 2 for detail).

That means the uncertainty component of a new tree node is determined not only by the

uncertainty of its father node and the control command a, but also by the current best map

and the localization algorithm applied. Consequently, the true distance, i.e., the length of

the optimal path, between two tree nodes in the UC-space, can hardly be measured by the

traditional weighted Euclidean distance metric. Since RRT is well known for it sensitivity

to the applied distance metrics [6], the “rapid” flavor of the RRT tends to be compromised

in the UC-space.

50

CHAPTER 3. RRT-SLAM-FD 51

3.1.1 Classic RRT’s Dependence on Metrics

We now briefly review why the classic RRT is sensitive to the distance metric applied.

In each iteration, RRT aims to place the new tree node nnew, in an area that is not yet

“covered” by the tree. To achieve this, RRT first randomly generates a node (denoted as

nr). Since the larger the uncovered area is, the higher the probability for nr being placed

in this area. Intuitively, this random node actually “identifies” the area that is not covered

by the tree.

Next, RRT tries to extend the tree “towards” the random generated node nr, or equiv-

alently, the uncovered area. However, finding the optimal solution, i.e., the optimal path

from current tree nodes to nr, is as hard as solving the original motion planning problem.

RRT picks up the tree node that is the “closest” to nr using a distance metric defined over

tree nodes as a heuristic, and extends the tree from the chosen tree node “towards” the

random node. That means nr is inside the Voronoi region of the chosen tree node. In other

words, those tree nodes that have larger Voronoi region tend to be chosen to extend at each

iteration, the so-called Voronoi-bias [74].

Obviously, tree nodes that are “close” (measured by the distance metric applied) to the

uncovered area tend to have higher chance to be chosen to extend. However, extensions

from these nodes may lead new nodes to be generated in the area that has been covered by

the tree, if the distance metrics does not reflect the true distance between tree nodes. Also

these tree nodes might be chosen numerous times, which leads to inefficient node generation

[34].

For systems such as rigid body and open-chain, commonly used distance metrics, such

as the weighted Euclidean, works well [23], because the robot can move along a straight line

in the C-space from the chosen node towards the random node , i.e., the distance reflects

the shortest path taken modulo the presence of obstacles. This means that getting closer to

the random node from the chosen tree node is an easy task unless the robot is occluded by

obstacles. Therefore a new node is more likely to be inside the father node’s Voronoi region,

which is a large empty area that has not yet explored by the tree, hence achieving efficiency.

But clearly this is not the case for RRT in the UC-space, because the uncertainty component

of a new node, as mentioned before, also depends on the environment. Consequently, there

is no easy solution to move “towards” the random node from the chosen tree node, and it

is also hard to predict where the new node will be located(in our case due to unpredictable


change in uncertainty), hence compromising the efficiency of RRT.

3.1.2 RRT-Blossom

RRT-Blossom (Algorithm 7), instead of looking for a “better” distance metric, aims to

reduce the RRT’s sensitivity on the applied distance metric. An illustration of the algorithm

[24] is shown in Figure 3.1. Let nodes na and nb be the latest two nodes added into the tree.

However, nb does not contribute much to the tree exploration (or, equivalently, nb is deemed

in areas that has already been covered by the tree). This case is referred to as “regression”

(we will discuss the precise condition later in Section 3.1.3 and 3.2). So RRT-blossom forbids

(temporarily) further extension from nb, and nb is referred to as the “dormant” node (light

colored). na, on the other hand, contributes “well” and is referred to as “live” nodes (dark

colored). RRT-blossom allows only those live nodes to be extended in further iterations.

To understand RRT-Blossom, as shown in Figure 3.1, one way to interpret it is that

the set of live nodes is a coarse representation of the whole tree. This means that RRT-

blossom searches first at a lower resolution, and avoids spending time on extending from

those dormant nodes at local areas, hence leading to efficiency.

If those dormant nodes are necessary to the solution path, RRT-Blossom will later wake

them up (i.e., turn the dormant status into live), and allow further extensions from them.

To achieve this, the authors introduce a state-transition machine, where a tree node state

will transit among one of the three states: live, dormant, dead. Each time a new node is

acquired, it will be marked as dormant or live subject to the regression checking condition.

A node is dead if all of its sons are either dead or collided with obstacles. A dormant node

will be waken up if the blocking node (i.e., the node closest to the dormant node, at the time

the dormant node is generated, referring to the part about the regression checking condition

below) is dead or the child of the dominant node is waken up. Finally, in RRT-Blossom,

each node is allowed to be extended for a fixed number of times (we use 8 throughout our

work).

The basic idea of RRT-Blossom can be easily incorporated into RRT-SLAM, and hence

addresses the efficiency issue in the UC-space. We name this modified RRT-SLAM ver-

sion as RRT-SLAM-Blossom. Note that in our RRT-SLAM-Blossom, when extending the

chosen tree node towards the random node, we extend the C-space component of nc (i.e.,

qc), towards qr for a fixed step to acquire a new configuration qnew. Then, the simulated

localization is applied to extend the tree from nc to the new configuration qnew (refer to


RRT-SLAM for detail).

Algorithm 7: One iteration of tree extension in RRT-Blossom [24].

begin1

nr := random state()2

nc := nearest neighbor(nr, tree)3

for a ∈ A do4

nnew = sim(nc, a)5

if fail(nc, a, nnew) OR reg.(nc, a, nnew) then6

continue7

tree = tree + new edge(nc, a)8

return a new node closest to nr.9

end10

notation:a: a motion command.A: a discretized motion command set.

Free Area

na

Obstacle

nb

nsnr

Figure 3.1: The regression checking mechanism for RRT-Blossom.

3.1.3 Original Regression Condition in the UC-Space

The regression checking is crucial to RRT-Blossom [24]. It is indeed a measurement of the

new node’s “contribution” to the tree’s exploration progress in the space searched, and keeps

those nodes that “contribute well” (i.e. non-regression nodes) as live nodes. [24] proposes

a regression checking condition, shown in Equation 3.1. It basically says that a new node is

in regression if it is outside the Voronoi region of its father node (in other words, the node

closest to the new node is not the father node, and this closest node is referred to as the

“blocking” node).


reg.(nc, nnew, tree) :

∃n ∈ tree, such that ρ(n, nnew) < ρ(nc, nnew)(3.1)

As described in [24], Equation 3.1, which aims to detect redundant explorations, is a

practical solution that works effectively in their system setup (two typical systems including

a non-holonomic system and a kinodynamic system). However, we should notice that this

approach is heuristic based. The effectiveness of applying Equation 3.1 to detect redundant

explorations depends heavily on the distance metric applied ρ(.). We now discuss why

Equation 3.1 would perform poorly in the UC-space.

x

un ertainty

nr

nnew

nc a

Figure 3.2: Failure of the original regression condition in the UC-space.

Now consider a toy example of RRT in the UC-space as shown in Figure 3.2. The robot

has one degree of freedom and moves along x axis with motion uncertainty. The 1D on-

board range sensor helps the robot to localize itself by detecting the markers (black stars)

in the environment. Here, an extra dimension of uncertainty is indicated by the vertical

axis. In Figure 3.2, nc is chosen to be extended towards the randomly generated node

nr by applying a motion command a. If the localization algorithm successfully reduces


localization uncertainty, the new node will be in position nnew (with low uncertainty),

outside the Voronoi region of nc, i.e., the father node. Hence it will be reported as regression

and marked as dormant (3.1). Intuitively, nnew in Figure 3.2 contributes well to the tree

expansion process in the UC-space, and should be kept as a live node.

The situation as shown in Figure 3.2 could arise frequently when the robot with higher

localization uncertainty approaches features in the environment. In other words, the original

regression condition tends to put more new nodes into the dormant state than necessary in

the UC-space.

To illustrate what has been discussed in Figure 3.2, we give in Figure 3.3 an example.

In this subsection, all the figures are acquired in the environment setup as shown in Figure

3.9 for Problem B (refer to Section 3.4). We generate a random tree by applying the RRT-

SLAM-Blossom mechanism in the UC-space after a small number of iterations, and show

the nodes that are non-regression (live nodes) as determined by Equation 3.1. Obviously,

the live nodes tend to be closer to their fathers (i.e., do not have dramatic changes in the

uncertainty component). The main reason is that, similar to the discussion about Figure

3.2, those new nodes out of their fathers’ Voronoi regions (due to the dramatic change in

the uncertainty component, and therefore far away from their fathers) will be reported as

in regression if the original regression is applied.

Figure 3.3: The live tree nodes picked by the regression condition (Equation 3.1) introducedin RRT-blossom.

In Figure 3.4, we illustrate both the live nodes (dark colored) and the dormant nodes

(light colored) for RRT-SLAM-Blossom. As mentioned before, this set of live nodes is

preferably a coarse representation of the area that has been covered by the tree (refer to the


explanation to Figure 3.1), such that RRT-Blossom can search first in a coarse resolution.

However, as a consequence of the aforementioned shortcoming of the original regression

checking condition, the live nodes identified using the original regression condition cover

only a limited portion of the area that has been covered by the tree.

Figure 3.4: Distribution of the live nodes (dark) under the original regression condition.The dormant nodes are shown in light color.

Another drawback when applying the original regression condition in the UC-space is

shown in Figure 3.5. Because many tree nodes are “mistakenly” marked as dormant, we

observe that the number of live nodes that are available for further extension might be

very small (as mentioned before, in RRT-SLAM-Blossom, a live node extended more than

8 times will not be considered in further extension). In Figure 3.5, the live nodes available

for further extension are in dark color, and otherwise are in light color. This tends to cause

RRT-SLAM-Blossom to spend time on searching at some particular local areas around those

available live nodes. Since it is hard to predict where this small amount of available live

nodes will appear in practice, this becomes problematic when there are some key areas that

require more attention, but those available live nodes do not appear in these areas even

after many iterations.

With these drawbacks of the original regression checking condition in mind, we now

propose a new regression checking mechanism that works well in the UC-space.


Figure 3.5: An example of applying the original regression checking condition (Equation3.1) in the UC-space. Only live nodes are shown. A small number of live nodes (darkcolored) are available for further extensions. Nodes in gray color are also live nodes, butthey have been extended more than 8 times and hence will not be further extended (referto the explanation for RRT-Blossom).

3.2 Outlier Detection Mediated Regression Checking

Since regression checking is used to answer whether a new tree node is being placed in an

area that is not yet “covered” by the tree nodes, it could be treated as an outlier w.r.t.

the set of existing tree nodes. Thus, the regression checking indeed becomes an outlier

detection problem from sequential data clustering point of view [25]. Several criteria have

been used for outlier detection in sequential data clustering literature [75]. We choose to

use the Fractal Dimension (FD) of a data set.

3.2.1 Fractal Dimension

The fractal dimension is originally used to characterize fractal sets, i.e., structures that

appear as a consequence of self-similarity. In research areas such as information theory,

data mining, etc., the fractal dimension is used to measure a data set’s intrinsic dimension,

which reflects self-similarity of the data set.

The fractal dimension is defined as follows [25]. Let di be a grid of size r in each

dimension in an n-dimensional space, and pi be the frequency of the data points fall into

the cell di. The generalized fractal dimension Dq is defined as:

Dq =1

q − 1

∂ log∑

i pqi

∂ log r. (3.2)

In particular, the Hausdorff fractal dimension (q = 0), the information dimension


limq→1Dq, and the correlation dimension (q = 2) are widely used. In this work, we se-

lect q = 1, i.e., the information dimension, “which is the entropy and therefore reflects

changes in trends” [25], and denote it as FD.

3.2.2 Applying FD as a Regression Condition

Whether a new datum is an outlier of the data set could be well detected based on the

change of the FD value after the new datum is included in the data set. Our new FD based

regression checking condition is given by,

reg.FD(nc, nnew, tree) :

if ∆f < fthresh AND reg.(nc, nnew, tree)(3.3)

where ∆f is the change in the FD value for the tree node set before and after the new node

is included in the tree, and fthresh is a user defined threshold. Equation 3.3 basically claims

that a new node is in regression if the original regression condition is satisfied AND the FD

value of the tree node set (with the new node included) does not increase significantly, i.e.,

the increment is less than a user defined threshold after a new node is included in the tree.

The complexity of calculating FD is of N log N , where N is the size of the data set

[25]. When we extend the random tree in the UC-space, the most computationally time-

consuming step, at each iteration, is in the simulated motion step, which uses the simulated

SLAM and is much more significant compared to the cost of FD calculation. As shown below

in our simulations, even though the FD based regression checking introduces extra cost of

N log N at each iteration, it results in an efficient node generation, hence still improving

the overall efficiency.

3.3 Improving RRT-SLAM Efficiency in the UC-Space: RRT-

SLAM-FD

The detailed RRT-SLAM-FD algorithm is given in Algorithm 8 and 9. There are two major

differences between RRT-SLAM-FD and RRT-Blossom.

The first distinction between RRT-SLAM-FD and RRT-Blossom is, of course, the re-

gression checking mechanism applied (line-11 Algorithm 9).


Algorithm 8: RRT-SLAM-FD

input: Y - SLAM particle set before robot planning the motion.output: tree - a random tree.begin1

< q∗,m∗b > := the most weighted particle in Y;2

for i = 1 to ♯L do3

q[i]ns = q∗; ω

[i]ns =

[i]ns = 1

♯L4

Pns

free = 1;5

tree.init(qs, us = 0,Lns , T Lns

, Pns

free);6

DEADNUM COUNT = 87

k = 2, e num = 0, prev e num = 08

w num = 0; prev w num = 09

while !TimeUp() and !(ReachGoal()) do10

< qr, ur >:= RandomState() ;11

nc := NearestNeighbor(< qr, ur >, tree);12

qnew = ExtendTo(qnc , qr, dstep);13

Extend-RRT-SLAM-FD(tree, qnew, nc,m∗b);14

e num = getExtensionNum()15

w num = getLiveNodesNum()16

if e num > prev e num + k then17

if w num <= prev w num then18

fthresh = fthresh − fstep19

k + +20

if k >= DEADNUM COUNT then21

k = DEADNUM COUNT22

else23

fthresh = fthresh + fstep/224

prev w num = w num25

prev e num = e num26

end27

notation: q[i]nj : the ith particle in Lnj

.

ω[i]nj : the weight of q

[i]nj .

[i]nj : the weight of ith trail particle stored in Tnj

.Tnj

: a data set stored at tree node nj.


Algorithm 9: Extend-RRT-SLAM-FD

input: nc - a tree node.qnew - a configuration, towards which the tree will extend from nc.tree - a random treem∗ - a map.dstep - a user defined fixed step.begin1

< Lnnew ,Tnnew , Pnnew

free > =2

SimLocal( Lnc,Tnc ,m∗, π[qns ,qnnew ]

, Pnc

free, );3

if Lnnew 6= NULL and Pnnew

free > Pthresh then4

unew = UncertaintyScalar(Lnnew);5

if unew > ubound then6

return;7

else8

nnew := {qnew, unew,Lnnew ,Tnnew , Pnnew

free };9

if reg.FD(nc, nnew, tree) then10

return;11

else12

add nnew to tree as son of nc;13

else14

return;15

end16

notation: Pthresh: the collision probability threshold for a feasible path to the goal.ubound : a user defined uncertainty threshold.


Secondly, RRT-SLAM-FD is different from RRT-Blossom in how to deal with the dor-

mant nodes. As previously discussed, if those dormant nodes are crucial for the solution

path, RRT-blossom wakes those dormant nodes, and searches at a finer resolution for al-

ternative paths. RRT-blossom wakes a dormant node (as well as all its precedents that are

dormant) based on the very local information around this node: a dormant node is waken

when a nearby live node is dead, i.e., all the extensions from this live node are failed, or all

its descendants are dead. Therefore, as reported by the authors, the planner might “spend

much time unnecessarily exhausting the free-space” before these dormant nodes, which are

keys to the solution path, are waken up.

Our solution to the above issue is that, instead of waiting for the dormant node to

be waken up, we reduce the FD threshold adaptively. With a reduced threshold, a new

node that “contributes” less to the tree exploration (which would have been deemed as in

regression before fthresh is reduced) will be kept as the live node. So the new live node will

be generated close to the dormant node. The motivation of waking the dormant node is,

indeed, to search for alternative paths at a finer resolution, so reducing the FD threshold

has the similar effect to waking the dormant node up.

Intuitively, we should search at a finer resolution, when the tree stops making progress

at current resolution, which could be well indicated by checking whether the new nodes are

frequently in regression (new nodes often in the area that has been covered by the tree) in

the next few iterations. Or equivalently, it could be reflected by the amount of decrease

of the number of nodes that are both live and available for extension. In our work, we

control the increment of “searching resolution” by monitoring the change of the number of

live nodes available for extension.

In RRT-SLAM-FD, after k extensions, we check the number of available live nodes wnum

(line-17 Algorithm 8). If wnum does not increase, we reduce the FD threshold by a fixed

step, fstep, to increase the search resolution. If wnum does increase (line-23 Algorithm 8),

to avoid the resolution to increase too rapidly, we increase the threshold by a small amount

(set as half of fstep so that the overall trend of the threshold keeps dropping). Users can

control the increment speed of search resolution by adjusting the value of k. k is initially

set to a low value, and is progressively increased each time the threshold is decreased. The

higher k is, the slower the increment speed of search resolution. Note that we bound the

value of k to avoid the search resolution to increase too slowly. In our case, we set its value

between 2 and 8, which is the maximum number of extensions allowed for one node.


3.3.1 Discussions on RRT-SLAM-FD

As previously illustrated in Figure 3.2, the original regression condition may fail to work

properly in the UC-space. Here, we show the effectiveness of the new regression condi-

tion with specific examples. Again, in this subsection, all the figures are acquired in the

environment setup as shown in Figure 3.9 for Problem B (refer to Section 3.4).

Figure 3.6 shows the nodes that are non-regressive (live nodes) after applying RRT-

SLAM-FD, as determined by the new regression condition (Equation 3.3). Our FD based

regression checking condition treats many nodes out of their fathers’ Voronoi regions as the

live nodes, and therefore many long branches appear due to the change of the uncertainty

component w.r.t. the father node (refer to Figure 3.3 for a comparison).

Figure 3.6: The live tree nodes picked by the new FD based regression condition (Equation3.3).

Figure 3.7: Distribution of the live nodes under the new FD based regression condition.


Figure 3.7 illustrate both the live nodes (dark colored) and the dormant nodes (light

colored). Comparing to the original regression condition (Figure 3.4 shown in Section 3.1.3),

the nodes picked by the FD based regression condition are distributed in a more uniform

fashion, and hence provide a better coarse representation of the whole tree. As mentioned

before, preferably, those chosen live nodes should be a coarse representation of the area that

has been covered by the tree such that RRT-Blossom can search first in a coarse resolution

(refer to the explanation to Figure 3.1). Clearly, RRT-SLAM-FD keeps this very spirit of

RRT-Blossom in the UC-space.

3.3.2 Box Counting: an Algorithm to Determine FD

In our work, we adopt the box counting algorithm as described in [25], and address some

issues regarding this algorithm used to calculate the FD value.

The box counting algorithm divides the space into cells of size r, and counts how many

cells are occupied by data set points denoted as N(r). A plot of N(r) versus r is created,

and the slope of the plot is the Hausdorff FD. Other fractal dimensions could be acquired

in a similar fashion.

The box counting algorithm needs a data set of enough size in order to find a meaningful

slope of the line that fits N(r) and r. Therefore, for the box counting to return a value as a

meaningful approximation of FD, the size of the data set has to be larger than a threshold,

which increases exponentially with the dimensionality of the data set [76]. Thus in the

first several iterations of RRT (experimentally around the first 400-600 iterations in our 3

dimensional UC-space), the FD value returned by the box counting algorithm should be

discarded.

To address this issue, we generate (somewhat arbitrarily) an initial data set (the size is

1000) in the UC-space. For each data, the C-space component is randomly generated, and

the uncertainty-component is a constant, which is 2.0 times the largest uncertainty allowed

for a tree node. One can imagine the initial data set as a layer of cloud on the top of all tree

nodes. How the initial data set is arranged (position, shape, etc.) will affect the FD value

at each iteration. However, this will not be a concern in practice because we care about just

the FD difference (∆f) at each iteration.

Figure 3.8 shows how the FD value of a data set, including the tree nodes plus the initial

data set, changes at each iteration of RRT-SLAM-FD in the UC-space for 5 runs (acquired


Figure 3.8: FD values for the data set of the RRT-SLAM nodes, with the initial data set inthe UC-space, for 5 runs.

in the environment as shown in Figure 3.9, Problem B). In each run, we run RRT-SLAM-

FD, record the FD value at each iteration, and plot these values as one curve in Figure 3.8.

The first observation is that adding a new node tends to increase the FD value. Secondly, we

can see that the increment of FD value tends to be larger at the earlier iterations. The main

reason is that, in later iterations, most of the places have been covered by the tree, and hence

the new node tends to “contribute less” than those in earlier iterations. This also indicates

that the change of the FD value could indeed be a measurement of the “contribution” to

the tree expansion for each tree node.

3.4 Simulations

We test our RRT-SLAM-FD in our simulated test-bed and compare it with RRT-SLAM

and RRT-SLAM-Blossom in three problems as shown in Figure 3.9 and 3.10, where the

white area, the black areas, and the gray areas represent the free, known obstacles, and

unknown areas, respectively. We assume that the robot is in the middle of exploration task,

and needs to go to another place to continue the exploration. In the first problem, the goal

is set at A in Figure 3.9, which is closer to the relatively large open region, but is further

away from the known obstacles. The second problem needs the robot to reach position B,

which is surrounded by the unknown areas (hence in a narrower space, since robot can not


Figure 3.9: Problems A and B.

Figure 3.10: Problem C.


enter the unknown areas) in Figure 3.9, and is closer to the known obstacles than A. In

the third problem (Figure 3.10), we have a larger environment size of 16 by 12 meters (the

environment in Figure 3.9 is about 9 by 7 meters), and larger free space than Figure 3.9.

The goal point C is in open free region.

We show in Figure 3.11 (a), (b) and (c), the tree structures acquired with RRT-SLAM,

RRT-SLAM-FD and RRT-SLAM-Blossom, respectively, for Problem, C. For RRT-SLAM-

FD and RRT-SLAM-Blossom, nodes and branches in dark (gray) color are respectively

the live (dormant) nodes and branches. We observe that both RRT-SLAM-blossom and

RRT-SLAM-FD only allow a portion of the nodes to be further extended, hence avoiding to

spend much time at local areas, which is the major difference from RRT-SLAM in achieving

efficiency. We also observe how the FD based regression checking impacts on the selection

of the live nodes (Figure 3.11 (c)), compared to the original regression condition in RRT-

SLAM-blossom (Figure 3.11 (b)). First, in Figure 3.11 (b3), we can tell that the uncertainty

difference between a live node and its father tends to be small. A new node that has dramatic

change of localization uncertainty, w.r.t. its father node tends to be marked as the dormant

by RRT-SLAM-blossom (refer to Section 3.3.1 for detail). Secondly, the original regression

condition leaves many local areas without a single live node (marked by hand as A, B,

C in Figure 3.11 (b2)), which means that the exploration in these local areas is limited.

RRT-SLAM-FD clearly remedies these issues, hence successfully improving efficiency in the

UC-space.

We performed 60 trials for each algorithm in all three problems. Table 3.4 shows the

number of tree nodes N , the number of extension failures (Nfail), time taken (all in av-

erages) to solve each problem for RRT-SLAM, RRT-SLAM-blossom and RRT-SLAM-FD,

respectively. The variance of the time taken and the average time spent on the FD calcula-

tion are listed as well. The parameters in RRT-SLAM-FD is as follows: fthresh is initially

set at 0.00022; fstep is 0.000003. These parameters are kept the same in all runs.

We can see that the performance of RRT-SLAM-blossom is hard to predict due to the

fact that the original regression condition does not fit properly in the UC-space. In our sim-

ulations for Problems A and B, RRT-SLAM-blossom is even outperformed by RRT-SLAM,

while in simulation for Problem C, RRT-SLAM-blossom performes better than RRT-SLAM.

However, after switching to the new FD based regression condition, and adaptively adjusting

the FD threshold, our new RRT-SLAM-FD consistently shows performance improvements

in the UC-space for all the three problems.


(a1) (a2)

(b1) (b2) (b3)

(c1) (c2) (c3)

Figure 3.11: Tree structures in the UC-space, shown in 2D view ((a1), (b1) and (c1)) and 3Dview (the rests), acquired by applying RRT-SLAM ((a1) and (a2)), RRT-SLAM-Blossom((b1), (b2) and (b3)) and RRT-SLAM-FD ((c1), (c2) and (c3)), for Problem C. For RRT-SLAM-Blossom and RRT-SLAM-FD, the nodes (branches) in black and gray color are thelive and dormant nodes, respectively. In (b3) and (c3), only live nodes are shown.


Pro Alg. N Nfail Time(sec.) VarT TimeFD(sec.)

RRT-SLAM 686 399 163 18996 0A RRT-SLAM-Blossom 814 263 204 48485 0

RRT-SLAM-FD 500 289 129 15102 7.4

RRT-SLAM 378 186 82 3075 0B RRT-SLAM-Blossom 700 179 166 21187 0

RRT-SLAM-FD 277 140 70 4270 3.5

RRT-SLAM 740 846 176 73345 0C RRT-SLAM-Blossom 772 161 134 13784 0

RRT-SLAM-FD 446 207 86 6693 7.0

Table 3.1: Simulation results for Problems A, B and C.

Chapter 4

Lazy-PRM for a Manipulator with

Base Pose Uncertainty

Represented by Particles

In this chapter, we consider the scenario, where the base is kept still while the on-board

manipulator is performing a motion in a given environment. However, the mobile base’s

true pose w.r.t. the environment might not be perfectly known due to the localization

uncertainty. We choose to extend PRM, instead of RRT, to plan the manipulator motion

because we aim to found a solution path of an optimal length. Correspondingly, the PRM

framework has to take this base pose uncertainty into the consideration.

4.1 Problem Formulation

We assume the manipulator motion can be precisely controlled. Also, the environment is

assumed to be known (or acquired via previous sensing). However, the manipulator base

pose (position and orientation) is not precisely known, and is represented with a set of N

weighted particles. Each particle indicates a possible base configuration qb = [x, y, θ], where

q[i]b is the ith particle. The weight of the ith particle, i.e., ω[i], represents the probability of

q[i]b being the true base configuration [7], with

∑Ni ω[i] = 1. Let q = [θ1, · · · , θd] ∈ Cm, the

C-space of the manipulator, be a configuration for the manipulator w.r.t. the base frame,

where d is the number of degrees of freedom of the manipulator. We use q[i] = [q[i]b , q] to

69

CHAPTER 4. LAZY-PRM-BU 70

represent a configuration for whole mobile manipulator, corresponding to the ith particle

q[i]b and the manipulator configuration q. Ideally, we should have used q

[i]bm instead of q[i],

but it becomes cumbersome. So superscript [i] on q (or path π mentioned later) will always

indicate mobile manipulator configuration as stated above.

qs

qg

π[0] : ω

[0], c

[0]

π[N ] : ω

[N ], c

[N ]

Collision status for π is not binary, but isasso iated with a probability.

Collision status for π[i] is binary, whi h iseither in- ollision (c[i] = 0) or ollision-free (c[i] = 1)

π[i] : ω

[i], c

[i]

q[0]s

q[i]s

q[N ]s

q[N ]g

q[i]g

q[0]g

π = (qs, · · · , qg)

Figure 4.1: For a manipulator path, there are correspondingly N possible sequences ofconfigurations swept by the whole mobile manipulator.

A path π consists of a sequence of manipulator configurations: π = (qs, ...qg), which

connect the start configuration qs and the goal configuration qg. Were the manipulator to

execute path π (the base remains stationary), as shown in Figure 4.1, with the base pose

uncertainty represented by N particles, there are correspondingly N different volumes in

workspace swept by the manipulator corresponding to π[i] = (q[i]s , ...q

[i]g ), i = (1, · · · , N),

each with the probability ω[i], the weight of the ith particle.

The collision status of π[i] is binary, denoted by c[i] = 0 (collision) or 1 (collision-free).

Let Ππ be the binary random variable that represents if the manipulator would be in-collision

or free (Ππ = 0 or 1), were the manipulator to execute the path π. The probability of path

π being collision-free, i.e., p(Ππ = 1), then is:

p(Ππ = 1) =N

∑

i

(ω[i] ∗ c[i]) (4.1)


We would like to extend PRM to plan a path for the manipulator with the base pose

uncertainty. To construct a roadmap G in Cm, we generate samples of the manipulator

configuration q ∈ Cm, and connect samples within a distance threshold, as in the standard

PRM construction [26]. A node n of G is a configuration for the manipulator (denoted

as qn), and edges between two nodes ni,nj (denoted as e[ni,nj ]) are line segments in Cm.

Note that the collision status for samples and edges of G are not binary, but are associated

with a probability of being collision-free, which can be easily calculated in a way similar to

Equation 4.1.

For a roadmap G, a path π that connects the start node ns and goal node ng is feasible,

if p(Ππ = 1) ≥ δ, where δ ∈ [0, 1] is a user defined threshold. The problem in the query

phase is to return an optimized solution path π∗:

π∗ = argminπ

C(π)

s.t. p(Ππ∗ = 1) ≥ δ

(4.2)

where C(π) is the length (or cost) of the path π. We refer to this problem as the collision

probability constrained shortest path problem (CP-CSPP).

4.1.1 Underlying Structure of PRM with Base Pose Uncertainty

The graph G is constructed in Cm. But it has an underlying structure, because of base

uncertainty. Corresponding to each node n and edge e of G, we use n[i], e[i] (e[i][nj ,nk]) to

denote a node or edge in the configuration space for the whole mobile manipulator (denoted

as Cbm), with the base pose indicated by the ith particle. G[i] denotes the graph composed

of nodes n[i] and edges e[i], and let G = {G[1], · · · , GN}. Therefore, for a roadmap G in Cm,

there exists correspondingly a set of N graphs in Cbm. Figure 4.2 shows an example, where

N = 3, the weight of each particle is set equal to 13 . The dashed lines in G[1], G[2] and G[3]

are edges in-collision, and the solid line are collision-free.

Let Πe be the binary random variable that the mobile manipulator is in-collision or

free (Πe = 0 or 1), were the manipulator to travel along the edge e. Note for a path π

in G consisting of a sequence of M edges (e1, e2, · · · , eM ), c[i] in Equation 4.1 is given by

c[i]e1 ∧ c

[i]e2 · · · , c

[i]eM

, where c[i]ej = 1(0) if e

[i]j is collision-free (in-collision). As a consequence, we

have:


ng

ne

nc

nb

na

nd

n[2]s

n[1]s

n[3]s

n[1]g

n[3]g

ω[1] = ω

[2] = ω[3] = 1

3

ns

G[1]

G[2]

G[3]

n[2]g

roadmap G in Cm

δ = 0.6

G[1], G

[2], G[3] in Cbm

Figure 4.2: Roadmap G in Cm, and the corresponding graph set G = {G[1], G[2], G[3]}constructed in Cbm (N = 3).


p(Ππ = 1) =

N∑

i

(ω[i] ∗ [c[i]e1

∧ c[i]e2· · · , c[i]

eM])

≤N

∑

i

(ω[i] ∗ c[i]ej

) = p(Πej= 1)

(4.3)

where ej ∈ π, j = 1, · · · ,M . Therefore, if p(Πe = 1) is lower than the threshold δ, the edge

e can not be a part of a feasible path. We call such an edge in the roadmap G, as “N-edge”

(“N” stands for edges that can not be part of solution), and the rest as “S-edge” (“S” stands

for edges possibly be part of the solution). To calculate p(Πe = 1) (second row of Equation

4.3), c[i]e for all the i = 1 · · ·N can be easily computed with a collision detector [77]. For

example, in Figure 4.2, if δ = 0.6, both e[nc,na] and e[nd,ne] are N-edges and the rest are

S-edges. In a similar way, we distinguish nodes in G as N-nodes and S-nodes, which again

can be easily determined by using a collision detector.

4.1.2 CP-CSPP: an NP-hard Problem

CP-CSPP is a particular version of the well known constrained shortest path problem

(CSPP) [27], which is finding the shortest path that satisfies a given constraint. In our

case, the constraint over the solution path is the probability of being free. Another version,

the weight-constrained shortest path (WCSPP) problem has been shown as NP-hard w.r.t.

the size of the graph [27]. In our case, the constraints have a strong underlaying structure,

which we exploit to show that CP-CSPP is NP-hard as well, but in terms of the number of

particles N . The technique is similar to that employed in [78] which shows the NP-hardness

of WCSPP. We now show the NP-hardness of CP-CSPP.

First, we transform CP-CSPP, the problem in Equation 4.2 into another form. Corre-

sponding to each graph G[i] ∈ G, we associate it with the ith particle weight, ω[i]. For each

G[i], i = 1, · · · , N , we check collision for the edges and remove those in collision. Each G[i]

is then a subgraph of the roadmap G.

Now, it is easy to see that, the problem shown in Equation 4.2 is equivalent to solving the

following problem (call it P): search for a subset G∗ ⊆ G, such that the shortest path that

is common to all the graphs in G∗ is the smallest of all, subject that the weight summation

of the graphs in G∗ is larger than the threshold δ.


G∗ = argminG′⊆G

C(πcom∗G′ ),

s.t.∑

G[i]∈G∗

ω[i] ≥ δ(4.4)

where πcom∗G′ is the shortest path that is common to all the graphs in G′

.

We transform the PARTITION problem, known to be NP-complete, into an instance of

our problem P.

Suppose we have a set S with N elements and every element ai, i = 1, · · · , N has a

corresponding size s(ai) ∈ Z+, ai ∈ S,∑

i s(ai) = 2A. The PARTITION problem is to

find a partition of S into Sa and Sb, such that the summation of elements’ size in these two

subsets are equal to A.

Given the partition problem, construct an instance of problem P as follows. We first

construct a graph G as shown in Figure 4.3, with the start and goal node (ns,ng) at two

ends. G contains 3N edges and 2N + 1 nodes, and is composed of a sequence of adjacent

triangles sharing a vertex. For ith triangle, the three edges are of the same length (equal

to s(ai)). Then, we construct a set of N graphs G[i], · · · , G[N ] from the graph G, such that

G[1] = G \ e1, · · · , G[N ] = G \ eN . Also we set ω[i] = s(ai). The set of graphs is denoted by

G = {G[1], · · · , G[N ]}. Above steps can be done in polynomial time.

|ei| = s(ai)

e2e1ns

G eN−1 eNng

Figure 4.3: A graph G constructed to transform the PARTITION problem into CP-CSPP

The problem P instance then is to find a subset G∗ of G such that the weight summation,

of G∗ is no less than A, and C(πcomm∗G′ ), is minimized.

From Figure 4.3, we can tell that

C(πcomm∗G′ ) = 2A +

∑

G[i]∈G′

ω[i]. (4.5)

If there exists a partition for S = Sa ∪Sb such that the summation of the size in Sa and


Sb are equal to A, we can easily determine G∗ = {Gi : ai ∈ Sa}, i.e., G∗ shares the same

index set as Sa. Based on Equation 4.5, the weight summation and C(πcomm∗G′ ) are exactly

A and 3A, respectively. This is the minimal path length we can get, thus we have a solution

for problem P.

If a solution exists for our problem P, i.e., we find G∗, there are two possibilities for its

weight summation. If the weight summation is higher than A, there is no solution for the

PARTITION problem. If the weight summation is exactly A, we will have a solution for

the PARTITION problem, such that Sa shares the same index set as G∗.

This shows that CP-CSPP is NP-hard.

4.1.3 Using the Labeling Algorithm to Solve CP-CSPP

We now introduce the labeling algorithm, with our specific variant, to solve CP-CSPP. The

detailed algorithm below is mainly based on what is described in [27]:

Let the roadmap graph be G = (V,E), which consists of a set of nodes V and edges E.

The start node and goal node are ns and ng, respectively. Let Ii be the index set of labels

on the graph node ni. Essentially, each k ∈ Ii corresponds to a path πki from the start node

ns to ni. Let the probability of being free for the path πki , i.e. p(Ππk

ni= 1), be denoted as

P ki , and the length of the path πk

i be Cki . The kth label at node ni is then a pair (P k

i , Cki )

that is associated with path πki .

Clearly, the number of paths to a node could be exponential (in terms of number of

nodes in the graph) in the worst case. To help reduce the search, two key concepts are used:

a) dominant: We say label (P ki , Ck

i ) dominates label (P li , C

li) if 1) Ck

i < C li , and 2) for

two paths πli and πk

i , for all j = 1, · · · , N if π[j],li is collision-free, π

[j],ki is collision-free as

well. Clearly, if (P ki , Ck

i ) dominates (P li , C

li), πl

i can not be a portion of the optimal solution

path π∗, since it can be replaced by πki , which has lower cost. The domination relationship

between two labels can be efficiently determined in O(N).

b) efficient: We will call a label (P ki , Ck

i ) as efficient if it is not dominated by any other

label at node ni and P ki ≥ δ.

The detailed algorithm is shown in Algorithm 10. The general idea is to maintain all

possible paths to each node, and to eliminate paths whose labels are not efficient. The set

of labels is maintained as a tree structure. We refer to it as the label tree. In InitLabeling()

(line-2, Algorithm 10), the first label is initialized at node ns, i.e., (P 1s , C1

s ) with P 1s = 1

and C1s = 0. Correspondingly, the root node of the label tree contains [(P 1

s , C1s ), ns].


The algorithm proceeds iteratively from the start node/label. At each iteration, among

all the labels that have not been extended before, a label at node ni, (P ki , Ck

i ) is chosen (the

one with minimal cost), and is extended to all the neighbors of ni. This involves, for all

neighbors nj of ni, 1) computing the label (Pj , Cj), associated with the concatenated path

[πki , e[ni,nj ]], 2) discarding (Pj , Cj) if it is not efficient w.r.t. existing labels at nj, otherwise,

a) storing it at node nj and create a new index for it. Let l be the index of this label at nj

(line-4, Algorithm 11), b) creating a new tree node [(P lj , C

lj), nj ], and adding it to the label

tree as son of [(P ki , Ck

i ), ni] (line-8, Algorithm 10). This procedure is repeated until the label

tree contains a node [(P lg, C

lg), ng], such that ng is the goal node, and C l

g is minimal among

all the labels that have not been extended. The solution path can be retrieved from the

tree by tracing back from this tree node [(P lg, C

lg), ng] to the root of the label tree. To check

the domination between labels at a graph node ni (line-4, Algorithm 11), a list of labels Li

is stored at ni. Readers can refer to [27] for more detailed information about the labeling

algorithm.

Algorithm 10: The labeling algorithm

begin1

InitLabeling();2

while⋃

ni∈V (Ii \ Ti 6= ∅) do3

Choose a label with minimal cost, say (P ki , Ck

i );4

for all nj ∈ ǫ+(ni) do5

(W,C) = ExtendLabel((P ki , Ck

i ), nj);6

if (W,C) 6= (−1,−1) then7

Add ((W,C), nj) to the tree as son of ((P ki , Ck

i ), ni);8

if nj = ng and C is minimal among labels not yet extended then9

return SUCCESS;10

return FAIL;11

end12

notation: ǫ+(ni) - set of neighbors for node ni in the roadmap.;

The labeling algorithm clearly is a general solution to the query phase of the PRM with

base pose uncertainty, i.e., the CP-CSPP. We now adapt it to Lazy-PRM.


Algorithm 11: ExtendLabel()

input: (P ki , Ck

i ): a label to extend from;nj: the graph node where the label (P k

i , Cki ) extend towards;

output: (W,C): a new label at node nj.begin1

(W,C) = AcquireNewLabel((P ki , Ck

i ), nj);2

if (W,C) is efficient then3

Lj = Lj

⋃

(W,C), Update Injaccordingly;4

Set Ti = Ti

⋃

k;5

return (W,C);6

else7

return (-1,-1);8

end9

notation: Lj - set of labels at node nj;

4.2 Lazy-PRM with Base Pose Uncertainty

4.2.1 Lazy-PRM Review

For a given roadmap, Lazy-PRM first searches for a shortest path over the roadmap without

considering their collision-free status (edges in classic roadmap either collide or are collision-

free). Then, the path is verified (check for collision) from the start node to the goal node.

The algorithm stops if the path is collision-free. Otherwise, there must exist an edge along

the path that is in collision. This edge is deleted from the roadmap and the shortest path

algorithm is applied again over the roadmap to acquire an alternative path for verification.

This is repeated until a collision-free path is found.

4.2.2 Lazy-PRM with Base Uncertainty

However, to apply the Lazy-PRM to our problem with the base pose uncertainty represented

by weighted particles, looking for an alternative path for verification is non-trivial. For

example, in Figure 4.2, the roadmap with 3 particles, each equally weighted as 13 , the

threshold δ for collision-free probability for a solution path is set as 0.6. Assume that the

first path being verified be ns, nb, nc, ne, ng. It is easy to tell that the probability of being

free for the entire path is 0, but all the four edges along the path are S-edges. If we delete

the S-edge e[nc,ne], we will miss the solution path ns, na, nd, nc, ne, ng, which is the only path


that satisfies the collision probability constraint in this example.

Bottom line is that since the collision status of edges in our roadmap is not binary

(collision/free) anymore, and there is a probability of collision associated with it, we can not

propose an alternative path by simply deleting an edge along the path from the roadmap

G and searching again as in the classic Lazy-PRM case. Instead, we must use the next

shortest path that satisfies the collision probability constraint. This suggests the use of a

k-shortest path algorithm [29] to acquire alternative paths for verification.

Obviously, a simple version of our algorithm would be to iteratively call the k-shortest

path algorithm and verify the path until success is reported or all the possible paths have

been exhausted by the k-shortest path algorithm. This is also a possible approach to address

the general CSPP, but, as reported in [27], is not practical, because the number of paths is

exponential w.r.t. the number of nodes and edges in the roadmap.

The key insight of our proposed technique is that if a path is not feasible, i.e. it does not

satisfy the collision probability constraint, the next shortest path can exploit the information

acquired with verifying the previous path. For instance, in Figure 4.2, say the first shortest

path being verified is π1 = {ns, nb, nc, ne, ng}. We can tell that the probability of being

collision-free for a portion of π1, i.e., {ns, nb, nc} is lower than the threshold. Then all the

paths from ns to ng that go through this portion, i.e, paths that have ns, nb, nc then branch

out at node nc, will not be the solution path. All these paths can be eliminated from further

consideration, thereby reducing the number of paths to be considered significantly. We now

explain the precise mechanism to do so.

4.2.3 The k-shortest Path Algorithm

We use a recent k-shortest path algorithm [29], which has run time complexity of O(k|V |(|E|+|V |log|V |)) (|V |, |E| are numbers of the graph vertices and edges). The algorithm returns

the k shortest paths iteratively. Each time the next shortest path, say the ith shortest path,

is returned, the algorithm iteratively updates a tree structure (we call it the shortest path

tree, denoted as Ti−1) constructed from previous i − 1 shortest paths {π1, · · · , πi−1} and

builds Ti, which “compactly encodes how the first i shortest paths branch off from each

other topologically”. Tree nodes in Ti have the same ID as nodes in the graph G. We use

(nu, nv) to denote a branch of of the tree, where nv is a child of nu.

A tree node nu in Ti, encodes a subset (or a bundle) of the i shortest paths in G that

share common portion up to nu, and branch off at nu in G. This set is denoted by B(nu).


A leaf node of Ti encodes one of the i shortest path, say πj, in G. We use nπjg to denote

this leaf node. For example, in T3 in Figure 4.4(b), B(na) = {π1, π2, π3}. Leaf node nπ2g

corresponds to the π2. We define prefixPath(nu) as the common portion shared by all the

paths in B(nu).

Note that B(nv) ⊂ B(nu) if nv is a child of nu. A tree branch (nu, nv) in Ti, encodes

the common portion (between nu and nv) of the graph paths in Bv. In T3, shown in Figure

4.4(b), corresponding to the tree branch (na, nb), B(nb) = {π1, π2} ⊂ B(na) = {π1, π2, π3}.For a tree branch (nu, nv), we define branchPath(nu, nv), as the common portion from nu to

nv in G, shared by all the paths in B(nv). The first graph edge of branchPath(nu, nv) from

nu is referred to as lead edge, lead(nu, nv). In T3 shown in Figure 4.4(b), branchPath(na, nb)

is the path (in G) from na to nb shared by π1 and π2. It consists of two edges [e[na,nb] and

e[nb,nc]]. lead(na, nb) is then e[na,nb] in G.

T0 is initialized to the singleton root node ns. The first shortest path π1 is calculated

with standard Dijkstra’s algorithm. Assume ith shortest path πi is returned. A branch (or

together with a new tree node) is inserted into Ti−1 as follows. Consider the portion of πi in

G, that starts from ns, and is the longest portion common with at least one of the previous

i − 1 shortest paths (call this subset Bπi). Let the last graph node of this longest common

portion in G be nw. Paths in Bπishare common portion (among themselves) from ns up to

some node nv (at least one will then branch off at nv), which by construction, much exist

in Ti−1 and encodes B(nv) (also, Bπi= B(nv)). Note that, πi shares common portion with

all the paths in B(nv) up to nw first. Then paths in B(nv) may, 1) have further common

portion then branch off at nv, or 2) branch off right at nw, which means nw = nv. In the

first case, we break the branch (nu, nv), where nu is the father tree node of nv, by inserting

a new tree node nw and add branch (nw, nπig ) to Ti−1, where nπi

g is a new leaf node. In the

second case, we add a new tree branch (nv, nπig ) to Ti−1. Figure 4.4(b) shows example of

tree structure constructed for i = 1, 2, 3.

Let the set of first i shortest paths be Bi. To acquire the next shortest path πi+1, we

consider all the possible candidate paths from ns to ng in G, whose lengths are greater than

ith shortest path. We denote the set of these candidate paths as Bi = B \Bi (\ denotes set

complement), where B is the set of all paths from ns to ng. Paths in Bi could be divided

into equivalent classes corresponding to nodes and branches of Ti (denoted as C(nu) or

C(nu, nv)). For tree node nu, the equivalent class C(nu) represents all the paths in G such

that they share the common portion with prefixPath(nu), and then use an edge distinct


na

π2

nd

π3ne

ns

ng

π1

π2

π3

π1π3

nf

nh

nc

nb

π2 π1

ns

nsns

[1] [2]

[1, 2]

nπ1g n

π2g

ns

nπ1g n

π2gn

π3g

[1][3]

[1, 2, 3]

[2]

na[1, 2]

nπ1g

nb

nb

T0 T1

T2 T3

(a) shortest paths over the graph (b) Updating of the associated branch structure

Figure 4.4: Building of the branching structure of the shortest path tree T3.

from any of the lead edges lead(nu, nvj), j = 1, · · · , α, where nvj is the jth child of nu in Ti.

For tree branch (nu, nv), the equivalent class C(nu, nv) denotes all the paths in G, which

diverge from prefixPath(nv) somewhere strictly between nu and nv. [29] showed that,

[∪nu∈TiC(nu)]

⋃

[∪(nv,nw)∈TiC(nv, nw)] = Bi (4.6)

The shortest path for each equivalent class is calculated (line-8, Algorithm 12) and these

paths are stored in a heap. In each iteration, the shortest path in the heap is popped out,

and is the next shortest path. The tree structure Ti is updated (line-6, Algorithm 12).

4.2.4 Calculating the Shortest Path in an Equivalent Class

The key step in the above k-shortest path algorithm is to compute the shortest path in an

equivalent class, which can be formulated as a replacement paths problem. The description

in this subsection is roughly based on [29]. We briefly describe the corresponding algorithm

below for completeness.

We first introduce the replacement paths problem. Let π∗ be the shortest path from ns

to ng, where π∗ = {n1, n2, · · · , nl − 1, nl}, such that n1 = ns and nl = ng. We want to

compute the shortest path from ns to ng, that does not include the edge (ni, ni+1, for each

i ∈ 1, 2 · · · , l − 1. We call this the best replacement path for (ni, ni+1).


Algorithm 12: k-shortest path algorithm

begin1

Initialize T0 to ns; H = ∅2

Calculate the 1st shortest path π1, insert π1 into heap H3

. for i=1; i < k; i++ do4

Extract the next shortest path πi from H.5

Ti = updateTreeStructure(π,Ti−1)6

for Each new node and branch in Ti do7

Apply replacement paths algorithm to compute the shortest path π, from8

ns to ng, in the equivalent class, corresponding to the new node (orbranch) .Insert π into heap H9

end10

notation: H - a heap.

The subroutine, which determines all the replacement paths, takes O(|E| + |V |log|V |)time, and can be used to compute the shortest path in an equivalent class as follows.

Consider an equivalent class C(nu, nv) with a branch (nu, nv) in T . Let nb be the graph

node next to nu, along with the branchPath(nu, nv) in the graph G. We determine the

shortest path in C(nu, nv)), using the replacement paths problem in a subgraph H of G. H

is acquired by deleting from G all the nodes on the prefixPath(nu). In H, we can tell that

the shortest path from nb to ng (denoted as π∗H) coincides with the branchPath(nu, nv) up

through nv. Let [nb = n1, n2, · · · , nl = nv] be the sub-path of π∗H (from nb to nv). We solve

the replacement paths problem in H, from nb to ng for all the edges (ni, ni+1), i = 1, · · · , l.

The shortest of these replacement paths, appended to the prefixPath(nu), is the shortest

path in the equivalent class C(nu, nv).

The shortest path in the equivalent class C(nu) for a tree node in T is acquired as

follows. We obtain a graph H by deleting from G all the vertices in the prefixPath(nu)

except nu, plus all the lead edges that leave from nu. We compute the shortest path from

nu to ng in H, then append it to the prefixPath(nu).

4.2.5 Lazy-PRM-BU

Our lazy-PRM-BU algorithm (Algorithm 13) works as follows. We maintain a label tree,

using a labeling algorithm that creates labels but only for graph nodes along the paths being

verified (line-11). Given a path to verify, the labeling algorithm starts labeling nodes from


the start node ns by extending labels along the path. If the generated label at a graph node

is determined as efficient (as explained in labeling algorithm in Section 4.1.3), it is stored

and is added as a child of the tree node, from which this new efficient label was extended.

Then the next node in the path is considered, and procedure is repeated. If an extended

label is determined as in-efficient, the path verification step stops. The path is deemed

invalid, and the next shortest path is considered for verification.

Assume that i paths have been verified and failed. To get the next path for verification,

we modify the k-shortest path algorithm. Let us focus on the equivalent classes correspond-

ing to the nodes and branches of the branching structure Ti. The key is to identify an

equivalent class such that none of the paths in it will be the solution path. This entire class

can be eliminated from consideration for next shortest path, hence leading to efficiency.

Given Ti, consider an equivalent class C(nu, nv) (or C(nu)), which is a subset of all

candidates paths in Bi. Note that, starting from ns, all candidate paths in C(nu, nv) (or

C(nu)) share common portion with prefixPath(nu) up to nu. A key insight is that if the

label that corresponds to prefixPath(nu) is not efficient, none of the candidate paths in

the entire equivalent class will be the optimal solution path and the entire class can be

eliminated from further consideration (line-17, Algorithm 13). We refer to these equivalent

classes as invalid.

To tell whether an equivalent class is valid or not (line-17), we check the label tree

(created in line-11). For the equivalent class C(nu, nv) (or C(nu)) that corresponds to a

tree branch (nu, nv) (or a node nu) of Ti, prefixPath(nu) is a portion of at least one of

previous i shortest paths in G, which has been verified. The verification information has

been stored as labels in the label tree. For equivalent class (C(nu, nv) or C(nu)), we track

along the label tree from the root node along the prefixPath(nu). If we encounter a label tree

node (the leaf node included) that contains node nu, the equivalent class is valid. Otherwise

the equivalent class is invalid.

It is straightforward to show that the proposed method mentioned above returns the

shortest path that satisfies the constraint on likelihood of being collision-free if one exists,

and returns failure otherwise. Based on Equation 4.6, any path in G other than those

encoded in Ti belongs to at least one equivalent class. We excludes those invalid equivalent

classes (denoted as Biinvalid

), which only contains paths that can not be the solution path,

from Bi. Then, it is the shortest path in Bi \ Biinvalid

that is our returned path, and is

verified at each iteration. Obviously, the first valid path for verification will be the shortest


Algorithm 13: Lazy-PRM-BU: Lazy PRM algorithm with base pose uncertainty

begin1

R = BuildInitRoadMap()2

Initialize T0 to ns; H = ∅3

Calculate the 1st shortest path π1, insert π1 into heap H.4

InitLabeling(), i=1.5

while T imeUp() do6

Extract the shortest path from the heap, which will be the next shortest path7

πi.if πi == NULL then8

EnhanceRoadMap();9

else10

if verifyPath(πi) then11

return πi;12

else13

Extract the next shortest path πi from H.14

Ti = updateTreeStructure(π,Ti−1)15

for Each new node and branch in Ti do16

if the corresponding equivalent class is valid then17

Apply path replacement algorithm to compute the shortest18

path π, from ns to ng, in the equivalent class, corresponding tothe new node (or branch).Insert π into heap H19

i++20

end21

one that satisfies the constraint on collision probability.

4.2.6 Roadmap Construction and Node Enhancement

We ensure that all the nodes in the constructed roadmap are S-nodes, which can be easily

achieved as described in Section 4.1.1. If our lazy query reports a failure, we enhance the

roadmap by inserting more S-Nodes and connecting them to the current roadmap. All the

data structure for k shortest path calculation, such as the branch tree structure Ti as well

as the queue etc., are initialized again. But the label tree can be reused, because all the

labels in current the label tree are efficient, and they do not need to be created again.


4.2.7 Detecting an N-edge

In the path verification step, if we encounter an N-edge, there are two alternations: (I) keep

searching for the next shortest paths using current graph, (II) remove the edge from the

graph, delete the old shortest path tree and build a new one from start for the new graph,

which is similar to the original Lazy-PRM . However, if we already have a large shortest

path tree, removing an N-edge from the graph and rebuilding the shortest path branch tree

structure Ti may not be efficient 1.

In our implementation, we found a practical solution that works well in our experiments.

If the ratio between the number of the nodes in Ti and the number of the N-edges in the

roadmap is smaller than a threshold (we use 50 throughout this work) we will delete the N-

edge from the graph and rebuild the shortest path tree. Otherwise, we keep on the searching

using the same graph and the current shortest path tree.

4.2.8 Path Verification from Both Directions

As suggested in [28], checking end nodes (nodes close to start and goal) of the path benefits

the overall searching performance. In our implementation, we root two label trees from both

start and goal, and verify paths from both directions. Each time we detect an N-edge and

rebuild the shortest path tree, we will switch the direction (from the start node to the goal

or from the goal node to the start node) for the path verification step, and update the label

trees correspondingly.

4.3 Simulations

We have run preliminary tests of PRM-BU and Lazy-PRM-BU in our simulated test-bed

(introduced in chapter of introduction). We use the MobileSim [79] program as our mobile

robot simulator to simulate a PowerBotTM

[79], with a size of about 80cm by 65cm. A

simulated range sensor, with sensing range of 4.0 meters (approximately the same range

as the Hokuyo range sensor), is mounted on the mobile base and points to the front. The

simulated manipulator on-board has three degrees of freedom, and each link is 90cm long.

We simulate an “inspection” task, i.e., the desired goal configuration is as if the arm (say,

1Note that after an N-edge is removed from the graph G, it might be possible to acquire the branchstructure Ti efficiently from the old branch structure incrementally without rebuilding the whole structurefrom scratch. See Chapter 6 for future work.


with a camera mounted on the end-effector) was inspecting an area behind the obstacles in

Figure 4.5 (b). We run our simulation under Linux on an Intel Core-2 due 3.0Ghz computer

with 4GB RAM.

We compare the lazy-PRM-BU algorithm with the PRM-BU in four tasks as shown

in Figure 4.5, where sub-figures on the left show paths planned with PRM-BU with base

uncertainty, and sub-figures on the right show paths planned with classic PRM with no

uncertainty taken into account and with the base pose at the most weighted particle. We

assume the robot is in the middle of an exploration task. The black regions are sensed

obstacles, gray regions are unknown and white regions are free. Figure 4.5 (a) and (b) show

the manipulator moving from an unfolded configuration to another unfolded goal configu-

ration. Figure 4.5 (c) and (d) show the manipulator moving from a folded configuration to

an unfolded goal configuration.

We simulate the base uncertainty as follows. We use the true position of the base as the

mean and apply a Gaussian pdf to generate 30 particles (a number that is commonly used

in particle based localization algorithms [7]). The covariance matrix of the Gaussian pdf is

diagonal with the diagonal element values being (0.12m,0.12m,3o) for LARGE uncertainty

and (0.07m,0.07m,1o) for SMALL uncertainty, in x,y and θ dimension, respectively.

To acquire a goal configuration, the most weighted particle used as the base pose, and

a sampling based inverse kinematics technique [80] is used to generate a configuration,

whose end effector is pointed to the designated end effector position. Although we could

set a threshold for probability of being collision free for the goal configuration, we require

(conservatively) this manipulator goal configuration is free for all the base particles. This

generated configuration is used as the goal configuration.

In all our simulations, we set the threshold δ for the collision-free probability of a valid

path as 0.8. For raodmap construction step, the initial roadmap has 500 nodes, and is

enhanced with 50 nodes each time. We ran the planner(s) 30 times for each problem. Note

that for each task, we also set a time limit, which is 1000, and 1500 seconds for SMALL

and LARGE uncertainty, respectively. The results are listed in Table 4.1 to 4.3.

Our first observation is how the base uncertainty affects the planned path. In Figure 4.5,

the sub-figures on the left illustrate some paths planned with the base uncertainty taken into

consideration (the uncertainty size is LARGE), while sub-figures on the right illustrate paths

planned with the true base pose (without any uncertainty) using classic PRM. Note that

paths planned with classic PRM are, at times, closer to the obstacles, as in Figure 4.5 (b).


(a) Problem A

(b) Problem B

(c) Problem C

(d) Problem D

Figure 4.5: Four different start/goal problems. Left: paths planned with PRM-BU withbase uncertainty (LARGE). Right: paths planned with classic PRM with no uncertaintytaken into account and with the base pose at the most weighted particle.


Unc. G. size Av. Time Av. En. Searching Av.|E| |V | Total Cons. Search Coll. Av. Paths Inv-Cls.

ASL

6698 5026668 502

18.9 2.5 2.1 14.324.4 2.0 6.5 15.8

00

118 39544 575

BSL

6838 5077678 527

13.8 2.5 1.7 9.626.5 1.5 12.7 12.3

0.10.5

97 371229 1461

CSL

6678 5036801 507

51.4 2.1 22.2 27.188.6 2.8 55.1 31.7

0.030.1

1885 24594178 5816

DSL

6838 5076767 505

97.7 3.0 49.6 45.2129.5 2.7 76.4 50.4

0.070.23

3283 46035478 7300

Table 4.1: Results for Lazy-PRM-BU with pruning invalid equivalent classes.


ASL

6632 5026690 502

325 322 2.9 320318 315 3.2 313

00

1.9 1.16.9 5.4

BSL

7301 5207749 528

323 321 2.5 319326 321 4.7 320

0.370.53

3 179 87

CSL

6737 5046781 508

327 324 3.3 322324 321 3.1 319

0.030.13

3 34 4

DSL

6697 5026791 507

320 315 4.6 318323 315 8.4 312

00.1

30 39149 211

Table 4.2: Results for PRM-BU.

Unc. PRM-BU Lazy-PRM-BU♯Edges ♯Total ♯Edges ♯Total

ASL

6632 45183786690 4534507

139 203246179 252654

BSL

7301 49109117749 5277889

79 153531134 190784

CSL

6737 45917206781 4674345

232 319204321 445487

DSL

6697 45762016791 4566823

395 533990461 631613

Table 4.3: Number of collision checking for Lazy-PRM-BU and PRM-BU.


700

800

Env C

Sec.

500

600

Env.C

300

400

500

PRM

200

300

Lazy P

0

100 Lazy-P

0

Uncertainty size: SMALL

Env. D

M-BU

PRM BUPRM-BU

30

(a) SMALL uncertainty1400

Sec.

1000

1200

Env D

800

Env.CEnv. D

400

600

PRM-

0

200

0

Uncertainty size: LARGE

Env. D

-BU

Lazy-PRM-BU

30

(b) LARGE uncertainty

Figure 4.6: Running time for problems A to D for all the 30 runs.



ASL

7186 5207278 505

49.2 2.7 38.5 8.1156 2.4 140 13.7

0.40.5

984 05533 0

BSL

6723 5026689 502

89.8 2.7 71.9 15.2107 2.2 89.1 15.9

00

2313 03407 0

CSL

6630 5026824 507

95.4 3.3 65.2 26.9185 2.3 154 28.0

00.1

2215 05349 0

DSL

6996 5127656 527

185 3.3 149 32.2543 2.9 509 30.8

0.20.5

4340 014586 0

Table 4.4: Results for Lazy-PRM-BU without pruning invalid equivalent classes.

A B C DS L S L S L S L

♯F w. pr. 0 0 0 0 0 0 0 0w.t. pr. 1 1 2 2 2 3 4 10

WT w. pr. 25 145 31 137 589 311 538 903(Sec.) w.t. pr. > 1000 > 1500 > 1000 > 1500 > 1000 > 1500 > 1000 > 1500

Table 4.5: The number of failure times (FT) and worst case time (WT) for Lazy-PRM-BU(30 runs) with (w. pr.) and without (w.t. pr.) pruning invalid equivalent classes. Timelimit is 1000 and 1500 seconds for SMALL and LARGE uncertainty, respectively.

Clearly, the paths planned with the base uncertainty tend to be consistently safer. For the

four start/goal problems in Figure 4.5, the probability of being collision-free for the paths

planned with PRM-BU are 0.960, 0.985, 0.808 and 0.815, respectively. We also calculated

this probability for the paths planned using classic PRM and they were, 0.760, 0.303, 0.740

and 0.825, respectively. This clearly illustrates the desirability of using PRM-BU.

Table 4.1, 4.2 and 4.3 show the efficiency of our proposed Lazy-PRM-BU v.s. the PRM-

BU. In these tables, “Unc.” is the size of the base uncertainty, which is marked as “S”

and “L”, standing for SMALL and LARGE uncertainty as mentioned before. The second

column “G. size Av.” is the average graph size, including number of edges (|E|) and nodes

|V |, used to find a solution. The third column, “Time Av.” records the average time spent

to find the path (Total), to construct the roadmap (Cons.), to search the graph (sear.),

and to check collisions (col.). The average number of times the roadmap is enhanced is

recorded in the fourth column. In the fifth column, i.e. “Searching Av.” shows the number

of shortest paths verified before the solution path is found (“paths”), and the number of the

equivalent classes that are invalid (“Iv-Cls”). Finally, the average number of edges being

checked for collision, and the average total number calls of the collision checker are listed in

Table 4.3.


Our Lazy-PRM-BU, as shown in Table 4.1, is capable of solving the single query ef-

ficiently. Lazy-PRM-BU, saves significant time on collision checking, but does spend, as

expected, bit more time on the k-shortest path calculation. Please note that in our envi-

ronment setup, the cost of a collision check is quite small (checking intersections of simple

polygons in the planar). In real scenarios (3-D case), collision checking cost is high, partic-

ularly if the geometry of the robot and environment is complex. In such cases we expect

significantly more saving in the overall time for Lazy-PRM-BU for single query problem, as

compared to PRM-BU.

We also observe, as expected, that increasing the base uncertainty tends to make it

harder to find a path. For example, in problem shown in Figure 4.5 (a) the number of paths

needed to be verified increases almost 10 times.

Our lazy-PRM-BU is efficient because it prunes all invalid equivalent classes. We also

ran it without pruning the invalid classes, for each of these four tasks, and show the results

in Table 4.4. Clearly, the number of paths to be verified increases dramatically (up to by an

order of 10) in these tasks. Table 4.3 shows the number of times lazy-PRM-BU, with and

without pruning those in-valid classes, runs over the time limit, as well as the worst case

run time. Our Lazy-PRM-BU clearly benefits from pruning those invalid equivalent classes.

In Figure 4.6, we also plot the time spent on each problem for all the 30 runs for Lazy-

PRM-BU and PRM-BU. In problem C and D, we did observe that the worst cases time

spent for the Lazy-PRM-BU (those high peaks pointed by arrows) is even longer than the

average of time for PRM-BU. But this happens only in a very occasionally manner (2-3

times out of 30 runs), and the worst time is still with in a reasonable range.

4.4 A Motion Planner for the Mobile Manipulator System in

an Inspection Task

In this section, we briefly demonstrate an integrated motion planner for the mobile manip-

ulator that combines, in a decoupled manner, RRT-SLAM for mobile base with Lazy-PRM-

BU in a specific environment inspection task, which originally motivated this research. We

assume an end effector goal position is given in advance, and the manipulator is initially in

a fixed folded configuration.

As mentioned earlier, our planner decouples the base planning with the manipulator

planning similar to [1, 19]. The planner will first invoke RRT-SLAM to move the base


close to the end effector position, such that I) the localization uncertainty of the node is

smaller than a threshold ubound (the localization uncertainty is measured by the area of the

bounding box for the base pose particles. We use 0.1m2 in our simulations below), II) the

goal end effector position is within the reachable workspace of the manipulator. A coarse

approximation is sufficient here, i.e., in our case, the base position must be within a distance

lmax (lmax =∑

li, li is the length of the ith link) of the end-effector position.

Then, the planner uses Lazy-PRM-BU to find a path for the manipulator with the base

pose uncertainty given by the latest tree node. The goal configuration for Lazy-PRM-BU

is acquired with inverse kinematic calculation in the same way as what has been described

for problems in Section 4.3.

This planner is implemented and tested in our simulated test-bed, as given in Figure

4.7. After a portion of the environment is explored, an inspection point (shown as a black

square in Figure 4.7 (a)) is to be reached by the arm end-effector. This point is hard for

the mobile base to access by itself. The threshold for the collision-free probability for the

base motion and the manipulator motion are set as 0.85 and 0.8, respectively. A sequence

of decomposed planned motions for the mobile base and the manipulator in this inspection

task are illustrated in Figure 4.7 (b) and (c), respectively. In Figure 4.7 (b) the probability

of the planned base path being free is 1. In Figure 4.7 (c), the probability of the planned

manipulator path being free is 0.943.


(a)

(b)

(c)

Figure 4.7: The robot is given an end effector position as goal shown in sub-figure (a),which is a partially explored environment. The end effector position is marked by the blacksquare. In (b), the mobile base first moves close to the designated end effector position(path planned with RRT-SLAM), and then execute the manipulator path returned by theLazy-PRM-BU, with the base being stationary (c).

Chapter 5

DTC: Delaunay Triangulation

Inspired Adaptive k Nearest

Neighbor Roadmap Connection

Technique

In this chapter, we revisit one of the key issues, i.e., node connection, in the probabilistic

roadmap motion planner (PRM) framework, which is applied in Lazy-PRM-BU. We propose

a new neighborhood selection strategy based on certain empirically observed properties of

the Delaunay triangulation of a uniformly distributed random point set. This allows a

node in the network to have neighbors that are close to itself in the sense of the Delaunay

Neighborhood.

We first review the Delaunay triangulation, a basic concept in computational geometry

[81].

5.1 Delaunay Triangulation Review

Given a set S of n distinct points in Rd, the Delaunay triangulation is the geometric dual

of the Voronoi diagram. The Voronoi diagram of a point set S is the partition of Rd into

n polyhedral regions V (s), s ∈ S. Each region V (s) is a Voronoi cell of s, and is defined as

93

CHAPTER 5. DTC 94

the set of points in Rd which are closer to s than any other points in S:

V (s) = (r ∈ Rd | ρ(r, s) ≤ ρ(r, s′

),∀s′ ∈ (S − s))

where ρ(.) is a distance function.

The set of all Voronoi cells and their faces form a cell complex. The vertices of this

complex are called Voronoi vertices. For each vertex v ∈ Rd, the nearest neighbor set

(denoted as nb(S, v)) of v in S is the set of points s ∈ (S − v) which are closest to v.

The convex hull conv(nb(S, v)) of the nearest neighbor set of a Voronoi vertex v in S is

called the Delaunay cell of v. The Delaunay triangulation of S is a partition of the convex

hull of S into Delaunay cells of Voronoi vertices together with their faces. The Delaunay

triangulation consists of a set of triangles in 2-dimensional space (see Figure 5.1), and an

aggregation of disjoint, irregular tetrahedra in 3-dimensional space, assuming that the input

points are non-degenerate. An example of a Delaunay triangulation and Voronoi diagram

of a point set in [0, 1]2, in 2-dimensional space is shown in Figure 5.1. Throughout this

chapter, we use Euclidean metric for Delaunay triangulation calculation.

5.2 PRM Sample Connection Strategy Review

Of main interest here is the Nearest Neighborhood Connection (NNC) strategy used in most

PRM algorithms. To determine the neighbor nodes for a given node nc (the ones the node

is connected to via a collision-free edge), a candidate neighborhood set Snc based purely on

the Euclidean metric is selected. In [26], a threshold Maxdist is used to bound the set of Snc

to be the samples located inside a ball that is centered at nc and has a radius of Maxdist .

Another constant MaxNumNeighbors bounds the size of the set Snc and guarantees that

the ratio between the number of the edges and nodes is approximately fixed. In [26] and

[28], those numbers are chosen experimentally, MaxNumNeighbors is typically of order of

30 when the dimension is 6. Maxdist is selected such that the number of the initial samples

inside a ball of radius of Maxdist is approximately equal to MaxNumNeighbors .

CHAPTER 5. DTC 95

5.3 Delaunay Triangulation Inspired Connection (DTC) Strat-

egy

Our DTC strategy aims to choose the neighbors adjacent to a node, say nc, based on the

Delaunay triangulation. A node in the C-space will be a Delaunay Neighbor (DN) of node

nc iff their Voronoi cells are adjacent to each other, or alternatively, they belong to the same

Delaunay cell.

Figure 5.1: Delaunay triangulation and Voronoi diagram of a 2D points set. Square dotsare the points in the point set. Light gray polygon regions are the Voronoi cells and darktriangles form the Delaunay triangulation.

The Delaunay triangulation of a uniformly distributed point set has some key advantages

in comparison to the NNC strategy. The properties listed below shed some light on the

question why we choose DTC as our connection strategy for PRM.

One could conceptually conceive of the PRM construction process as a two-step process:

(I) construct a network that connects each node to all its candidate neighbors (over the

candidate neighborhood set); followed by (II) test all edges of the network by calling a

local planner, and delete those edges that are not collision-free. We advocate a network

that allows a node to have candidate neighbors that are not only close to itself, but are also

spread along different directions. NNC does not have this characteristic, because nodes are

CHAPTER 5. DTC 96

connected based only on the distance between them. Instead, the Delaunay triangulation

network is constructed based not only on “distance” but also on “direction” information.

Consider a simple intuitive scenario as shown in Figure 5.2. In the left figure, if node B

has some neighbors approximately along a particular direction, say−−→BC, nodes further away

along the same direction, e.g., C, will not be direct neighbors of B. But if B does not have

many neighbors approximately along a direction, say−−→BA, nodes that are even though sort

of far away from B, e.g., A, still will be connected to B so long as they are “close neighbor

in that direction”. However, as shown in the right figure, as A is moved further along the

direction−−→BA, it will get disconnected from node B because it is too far away from B. Hence

the Delaunay neighborhood selection for a uniformly distributed point set keeps a balance

between “distance” and “direction”.

Figure 5.2: Delaunay triangulation inherently uses both distance and direction information.See text for explanation.

5.3.1 Constructing a Delaunay Triangulation Neighborhood

There exist efficient algorithms for computing the Delaunay triangulation and the Voronoi

diagram for lower dimensions, specifically O(N log N) (N is the number of the set points)

algorithms in 2D, and almost O(N log N) algorithms in 3D [81]. Explicit calculation of

Delaunay triangulation will output all the Delaunay cells and a data structure that facilitates

visiting all the cells. When the dimension is high, it has exponential complexity (there exist

an incremental O(N ⌈ d2⌉) algorithm [82]) with respect to the dimensionality, hence intractable

CHAPTER 5. DTC 97

[83]. However, we do not need to build an explicit Delaunay triangulation. We are only

interested in constructing a Delaunay network, inside which two nodes will be connected

by an edge when they belong to the same Delaunay Cell, i.e., they are Delaunay Neighbors

(DN). The problem of deciding if two nodes are Delaunay Neighbors or not, can be posed as

a linear programming (LP) problem, and the description of the corresponding LP problem

below is roughly based on [84].

The Voronoi diagram of a set S of N points in Rd is the projection of the following

(d + 1) polyhedron to Rd space of the first d components:

P = {x ∈ Rd+1|d

∑

j=1

s2j −

d∑

j=1

2sjxj + xd+1 ≥ 0,∀ s ∈ S}

It can be relisted as:

P = {x ∈ Rd+1|b − Ax ≥ 0},

where A is an N × (d + 1) matrix and b is an N -vector. Now, we formulate the following

LP for any distinct i, j = 1, 2, . . . , n

minimize f(x) := bj − Ajx

subject to b′ − Ax ≥ 0

bi − Aix ≤ 0,

(5.1)

where Aj stands for the jth row of A and vector b′

is equal to b except that the jth component

of b′

(denoted by b′

j) is equal to bj + 1 (b′

j = bj + 1).

The Voronoi cell of points i and j are adjacent, i.e, i and j will be Delaunay Neighbors,

if and only if the objective value f(x) is negative at an optimal solution.

So we can decide whether two nodes are Delaunay Neighbors by solving a LP problem

which has N + 1 constraints and can be solved in polynomial time in N . Unfortunately,

it would involve solving O(N2) LP problems to construct the whole Delaunay network. It

would be computationally expensive, to build the Delaunay network exactly because each

LP problem includes N + 1 constraints and every constraint has in turn d variables for d

dimensional space.

We alternatively suggest an efficient approach that will build a network that is “similar”

to Delaunay network. The motivation behind our approach comes from some empirically

CHAPTER 5. DTC 98

observed “statistical” properties of Delaunay neighbors.

Let S be a set of n points that is uniformly distributed inside a unit cube in a d-

dimensional space. Let nc be a node in S that is closest to the center of the cube. When

| S |= N is large enough, a key question arises:

Question 1: How are the Delaunay Neighbors (DNs) of nc distributed?

We answer the question by conducting an experiment below.

Experiment A: We rank all the nodes in S′ = S\nc by their distance to node nc and

we denote the distance ranking of a node, say ni ∈ S′, as ki. Given a ki, we empirically

determine the probability that node ni is a DN of nc.

Since we can easily tell whether two nodes are DNs by solving an LP problem, it is

easy to conduct the experiment by generating a uniformly distributed random point set S

several times and then computing the probability for a given ki. We varied cardinality of

the set S in a range from 1000 to 3000 with an increment of 100. We repeated the testing

100 times for every sample size, and estimated the probability by computing the average

over all sample sizes.

Figure 5.3: Probability for a node of ranking ki to be DN of node nc, in 6-dimensional space

The result of Experiment A is presented in Figure 5.3. It clearly shows that for a given

dimensionality, the higher is the ranking of a node (i.e., the closer it is to node nc), the higher

is the probability that this node is a DN of nc and vice versa. There is also a boundary like

area, (let us denote the two edges of this boundary with k−b and k+

b , respectively) such that

we can tell, with a high degree of certainty that a node with a ranking outside this area is a

CHAPTER 5. DTC 99

DN of nc or not. In Figure 5.3 k−b and k+

b are around 100 and 240, respectively. The width

of the band [k−b , k+

b ] is always small compared to the size of set S. It should be pointed out

that the distribution shown in Figure 5.3 is not sensitive to the size of the sample set S so

long as the size of S is large enough. Note that learning precise value of k−b and k+

b is not

important here, since we only build a network that is “similar” to Delaunay network (see

Algorithm 15 later for detail). We now can choose a number kb inside this band [k−b , k+

b ]

as a boundary (see STEP 1 in Algorithm 14 for detail). Obviously kb can be used as an

estimate of the expected number of DNs of nc. We choose kb as follows. From the plot as

shown in Figure 5.3, traversing from left to right, kb will be the ranking number, where the

first time the probability falls below a threshold (we choose 0.4 in all our simulations).

Question 2: For large enough S, how does kb vary with respect to dimension?

Figure 5.4: Selection of kb for each dimension from 2 to 10

We repeated the above experiment for point sets in different dimensions, ranging from

2 to 10. Figure 5.4 shows the empirical variation of kb (determined by applying STEP 1,

Algorithm 14) for different dimension d, when the set size is large enough. The dependence

of kb on d seems to be of exponential nature1.

Indeed, it is possible to find a ball centered at nc such that the expected number of DN

nodes inside the ball is kb. The radius of the ball corresponds to the expected value of actual

distance between nc and its kthb closest neighbor. We denote the radius as Rd. Figure 5.5

shows the empirically determined minimal, average and maximal radii of ball that contains

1Based on this empirical data, if we were to have a guess, kb ≃ O(f(d) ∗ 2d), where f(d) is larger than 1,and increases slowly with d.

CHAPTER 5. DTC 100

exactly kthb neighbors, for different sample sizes, with 100 runs for each sample size, in 2D,

4D and 6D. The corresponding value for kb are 12, 56 and 150, respectively. In the plot,

the y-axis represents the radius that is normalized as Rd/√

d, where√

d is the diagonal of

length of a unit cube in dimension d.

5.3.2 Delaunay Triangulation Inspired Adaptive Roadmap Connection

(DTC)

Algorithm 14: Calculating Rd for given dimension d:

begin1

STEP 1: Calculating kb2

Select an arbitrary number k (approximately around 2d is a good start). Here k3

represents the ranking of distance from nc to all the other samples.while 1 do4

for counter = 0 to 100 do5

Put n (We set n = 3000) random uniform samples inside a cube [0, 1]d .6

Pick up a sample closest to the center of the cube [0, 1]d, and denote it as7

nc.Calculate the distance from nc to all the other samples, and select the kth

8

closest neighbor of nc. Denote it as nk.Test whether nk and nc are Delaunay Neighbors by solving the LP9

problem in Equation 5.1.

Calculate the fraction of instances that nk and nc are Delaunay Neighbors.10

This is an estimation of probability (denoted as Pk) that nk is a DN of nc .if Pk is less than a threshold (we choose 0.4 in all the simulations) then11

set kb = k.12

break13

else14

k = k + 1.15

STEP 2: Calculating Rd16

Having determined kb, generate another uniform sample set (let the size of sets17

start from 100) 100 times and take the average of the distance value, denote asRd, between nc and nkb

, i.e., the sample whose distance ranking to nc is kb.Increase the number of uniform samples and repeat step 8 to construct a lookup18

table T that stores Rd for different sample sizes.end19

In Algorithm 14, we give the detailed algorithm to calculate kd for different d (STEP

CHAPTER 5. DTC 101

Figure 5.5: Relationship between the number of uniform nodes and radius selection (theaverage value) in 2, 4 and 6-dimensional spaces.

CHAPTER 5. DTC 102

1), and Rd for different sample sizes for a given dimension d (STEP 2). The relationship

shown in Figure 5.5, i.e., the normalized radius Rd (we used the average value) for different

sample sizes, can be stored in a lookup table T (line 13) for each dimension.

We now state our Delaunay triangulation inspired connection algorithm (Algorithm

15). For each node ni, we consider all the nodes whose distances to ni are less than Rd,

which is acquired from table T , as the candidate neighbors. But connecting ni with all

the candidate neighbors may lead the roadmap contains too many edges, especially when

dimension d is high, say above 6. We introduce the same parameter as standard PRM,

MaxNumNeighbors, which works as an upper-limit to the number of Delaunay neighbors.

However we randomly choose MaxNumNeighbor nodes among the candidate neighbor set

within radius Rd.

Algorithm 15: Delaunay triangulation Inspired Connection (DTC)

begin1

For a node ni, select the radius Rd from the lookup table T based on current2

number of uniform nodes.Store all the neighbors, whose distances to ni are less than Rd, in a list L.3

if the length of list L less than MaxNumNeighbors then4

connect the node ni with all the nodes in L.5

else6

randomly choose MaxNumNeighbors neighbors (exclude the nearest7

neighbor of ni) from the list L as the neighbors of ni.

end8

Typically, the number of nodes inside the radius will be around 100 when the dimension

is up to 6 or 7. Our strategy of randomly choosing neighbors among the candidates in a

list L (line 3, Algorithm 15) will permit some “long distance” neighbors and not simply k

closest neighbors, as is the case in NNC.

5.3.3 Node Enhancement

Our enhanced samples are generated in the same way as described in [26] but with some

corresponding modifications to the enhanced node connection step. We introduce a scalar

parameter α ≥ 1 and use αRd as the radius of neighborhood selection for the enhanced

nodes. This is based on the observation that inside narrow passages, the number of the

nodes is sparse, hence requiring a larger radius to include more nodes. In our simulations

CHAPTER 5. DTC 103

we choose α = 1.5.

5.4 Simulations

We now present some tests with DTC algorithm for 2D, 4D and 6D configuration spaces. We

choose the parameter MaxNumNeighbors as 8, 15 and 25, respectively. The planner has

been implemented within Motion Planning Kernel (MPK) Library [85] developed in RAMP

lab, SFU. All the simulations have been run on a 2.0GHz Pentium4 PC. The collision

detector used in all these examples is V-Collide library developed at University of North

Carolina [77].

We first tested our planner on a 2D rigid mobile robot. In environments A and B (see

Figure 5.6 left and right respectively), the robot needs to go through some narrow passages

to reach the goal. Environment C and D (Figure 5.7 left and right, respectively) depicts

solutions for two 4D planning problems and environment E (Figure 5.8) is a 6D planning

problem. Configurations in light color represent the start and goal, respectively. Dark

colored configurations are some intermediate configurations along the solution path. The

sample generation strategies in all our simulations are the same for both NNC and DTC.

Figure 5.6: Problems A (left) and B (right) – 2D rigid mobile robot

We performed 20 trials for each example. Table 5.4 shows the average time taken and

the average number of nodes used to solve each problem for NNC and DTC, respectively.

In our simulations the time spent on the node connection for NNC and DTC is very similar.

CHAPTER 5. DTC 104

Figure 5.7: Problems C (left) and D (right) – 4-link planar manipulator with fixed base.

Figure 5.8: Problem E – 6-link planar manipulator with fixed base.

CHAPTER 5. DTC 105

DTC shows obvious improvements in performance in terms of planning time. The variance

of the run time for the same problem is significantly lower for DTC. This supports our

assertion that DTC is a better connection strategy than NNC.

Pro. Connecter ♯Nodes Time(sec.) Time-Var

NNC 1023 85.6 2812A DTC 750 53.0 730

NNC 1038 64.4 1920B DTC 718 39.8 536

NNC 2183 665 543068C DTC 955 273 144571

NNC 270 155 7526D DTC 201 105 4870

NNC 2350 4298 10571372E DTC 1440 2857 2770558

Table 5.1: Simulation results for problems A, B, C, D and E.

Chapter 6

Conclusions and Future work

In this thesis, we studied two key motion planning sub-problems arising in exploration and

inspection tasks for a mobile manipulator, where the motion planner has to incorporate

robot localization and mapping uncertainties. The sub-problems correspond to I) motion of

the mobile base, considering both the localization and the mapping uncertainties, II) motion

of the manipulator only (with the mobile base being stationary), considering the base pose

uncertainty. We used particle based uncertainty representation, and stochastically formulate

the collision probability of a path. We used the collision probability as the travel cost. Our

planners return, a feasible (for mobile base) and an optimal (in terms of the path length)

path (for manipulator) path, subject to constraint that the probability of being collision

free is higher than a user define threshold.

We proposed RRT-SLAM to solve the first subproblem. It takes the sensor observations

into consideration, and combines RRT with a simulated particle based SLAM algorithm

(FastSLAM) to expand the tree in the UC-space. The collision probability along a planned

path is explicitly computed and is used to select a planned path. To improve efficiency of

RRT in the UC-space, we applied a fractal dimension (FD) based sequential data clustering

approach. A series of simulations show that the efficiency improvement of RRT-FD in

terms of planning time is up to 20% compared with standard RRT. The planners return a

feasible path, subject that the probability of being collision free is higher than a user defined

threshold.

We extended the probability roadmap approach to deal with the second subproblem, i.e.,

to plan motion of a manipulator system with base pose uncertainty, represented with a set of

particles. We formulated the path query as a constrained shortest path problem. We proved

106

CHAPTER 6. CONCLUSIONS AND FUTURE WORK 107

that the complexity of our path query is NP-hard w.r.t. the the number of the particles.

We solved the general query problem over a given roadmap, with a labeling algorithm, and

presented a novel lazy version of it. The lazy query technique, so called LAZY-PRM-BU,

aims to reduce the number of collision checking for fast single path queries. The key idea

to the efficiency improvement of our LAZY-PRM-BU approach is to combine the labeling

algorithm with the k-shortest path algorithm, such that a large number of invalid candidate

paths can be skipped from further consideration. Simulations show that our approach can

efficiently query the path over graphs with the size of up to 1000 nodes and 10000 edges. The

planners return an optimal path (in terms of the path length), subject that the probability

of being collision free is higher than a user defined threshold.

Finally, we studied the sample connection step in PRM’s roadmap construction, a key

step for all the roadmap based approaches. We proposed a new method of neighborhood

selection strategy based on certain empirically observed properties of Delaunay triangulation

of a uniformly distributed random point set. Our method allows a node in the network to

have neighbors that are close to itself in the sense of Delaunay Neighborhood. The Delaunay

triangulation connection (DTC) algorithm is easy to implement and we show the boost in

performance (20% to 50% running time) via a series of simulations.

6.1 Future Work

There is ample scope for future work here, both for algorithmic improvement, and for

several broad issues related to motion planning under uncertainties. First, we discuss the

algorithmic improvements.

We would like to extend RRT-SLAM, implemented on a mobile base in this research, to

the whole mobile manipulator system. The challenge would be the cost of collision checking

for the whole mobile manipulator system, and the significantly higher computational cost

introduced by the high dimensional C-space.

In Lazy-PRM-BU, when an N-edge is detected, we remove it from the graph. But the

shortest path tree has to be reconstructed in our implementation. It might be possible to

acquire the branch structure (refer to Chapter 4 for details) efficiently from the existing

branch structure incrementally without rebuilding the whole structure from scratch, hence

further improving the overall efficiency of the Lazy-PRM-BU.

With the base uncertainty in mind, PRM-BU and Lazy-PRM-BU focus mainly on the

CHAPTER 6. CONCLUSIONS AND FUTURE WORK 108

query phase. The sampling technique is the same as in the classic PRM, which uses uniform

samples generated in C-space. As shown in the classic PRM case, many biased roadmap

sample generation techniques tend to improve the PRM’s efficiency. It will be interesting to

study how these biased sampling techniques could be adapted to further improve efficiency

of PRM-BU (or Lazy-PRM-BU).

The DTC is investigated and tested in a model-based PRM framework. We have not

extended it to the case of PRM-BU. It will be interesting to see how this sample connection

strategy impacts the overall performance of PRM-BU. It is also worth investigating how

to extend DTC with other connection strategies, such as those using different metrics.

Weighted DTC may be one way to approach this. A good review of weighted metrics can

be found in [86].

We now discuss some practical and theoretical issues related to the planning under

uncertainty.

Throughout the work, we assume that a threshold for collision probability is given by

the user in advance (similarly, which is fixed in RRT-SLAM and PRM-BU). In practice, the

specific threshold would be experimentally determined by the user. We need further efforts

to determine the effects of the threshold on the planned path.

The eye sensor in our case is used for inspection task only, and is not used for unknown

environment exploration. How the eye sensor and the sensor on the base can be fused

together [13] for a more efficient exploration is an interesting problem.

In this work, we use a simulated planar mobile manipulator system as atest-bed. We

would like to extend our results on the actual mobile manipulator system 3D environments.

The Hokuyo type sensor can be mounted on a pan/tilt unit on the base for a 3D environment

modeling [63] or on the end effector for envrionment [11] and/or object modeling [13]. This

extension to the 3D case will bring further important practical issues, such as the model

representation, collision detector design, control of the actual robot system, view planning,

and higher degrees of freedom (6 dofs) for the manipulator to the fore.

Appendix A

System Implementation

A.1 The Simulated Mobile Manipulator Test-bed System

In this appendix, we detail the software structure of the motion planner and the simulated

mobile manipulator system that are used to demonstrate the proposed motion planning

techniques in this research. Our implementation of the system introduced here is mainly for

testing the proposed ideas. Similar architectures for fully autonomous systems have been

reported by many researchers, e.g., the one in [87].

A.1.1 The Mobile Manipulator Simulator

The mobile manipulator simulator in our test-bed is a modified version of MobileSimTM

software from MobileRobotsTM

, for simulating mobile robots and their environments, and

for debugging and experimenting with ARIA (Advanced Robotics Interface Application,

i.e., an object oriented interface to robots from MobileRobots) or other softwares that

support MobileRobots platforms. MobileSim is developed based on the Stage library [73], a

simulated environment for mobile robots. An ARIA interface layer is introduced on top of

the Stage library, such that MobileSim can be controlled by ARIA based programs through

a TCP/IP port.

The MobileSim was originally designed for mobile robots only. Figure A.1 shows the

architecture of the modified MobileSim for our simulated mobile manipulator. We inserted

a manipulator model into the Stage library and added extra manipulator control commands

to the ARIA interface. Hence, we control the whole simulated mobile manipulator through

109

APPENDIX A. SYSTEM IMPLEMENTATION 110

our augmented interface with ARIA.

InterfaceCommunation package

MobileSim with on−board manipulator model

the environment, return

information etc..

robot pose, sensor

Stage

Manipulator model

ARIA control command information

On−board sensor

manipulator

Display the robot and

TCP/IP port

interperetation for

ARIA Netowrking

Robot applications(Motion planner)

Figure A.1: Modified MobileSim to support the control of an on-board manipulator. Shadedareas indicate our modifications to the original MobileSim.

A.1.2 Robot System Software Implementation and Architecture

The robot software system mainly consists of three components, i.e., the planner, the ARIA

path execution module, and the user interface (shown in Figure A.2). In the planning stage,

the robot plans its next motion and returns a solution path. Then ARIA path execution

module is responsible for controlling the robot to follow the solution path in the execution

stage. The user interface accepts user input, such as next gaol, as well as displays information

related to the planning and execution stage, such as the constructed environment model,

robot status and the solution path.

Each of the three components, i.e., the planner, the user interface, and the ARIA path

execution module, runs at a different thread. The user interface runs as the main thread,

which is a loop to continually update the display. After the main thread is initialized, it

creates the planner thread, which then creates the ARIA thread. We use the mutex locking


No

Planner start

plan to ARIA

Send the motion

SLAM/localization

Yes

finished?Plan execution

Plan robot next motion

ARIA initialize

Exe

cutio

n of

the

mot

ion

plan

fini

shed

No

Yes

Yes

main

interface

Initialize user

thread

interface

Start the planner

(update UI)

Update user

(execute the path)Main thread Planner thread ARIA thread

Intialize the ARIA

initialize the planner

Odo

met

ry, r

ange

sen

sor

read

ings

No

afte

r th

e pa

th e

xecu

tion

Rob

ot p

ose

and

map

Rob

ot p

ose

estim

atio

n

The

sol

utio

n pa

th

Upd

ated

env

rionm

ent

Plan next view

Path ready?

VFH ollisionGoal rea hed?

avoiderNext goal point

MobileSim (Figure A.1) or Powerbot

modelandrobotpose

Solution path

△t U

I=

35m

s

△t s

enso

r=

20m

s

△t S

LA

M=

20m

s

△tcon

trol=

20m

s

Legend:data paths△

t sen

sorl

=20

ms

Sonarreadings

Control ommands(△θt ,△

dt )torobots

RS-232 (Powerbot) or TCP/IP (MobileSim)Figure A.2: System software for the mobile base that consists of mainly three threads: theuser interface, the planner, and the ARIA path execution module.


mechanism [79] to avoid the data access conflicts when these three threads communicate

with one another .

Figure A.2 shows the details of the three robot software components and the data sharing

(shown by bold arrowed lines) among the corresponding threads. In the execution state,

the planner thread and the ARIA thread also collect on-board sensor reading from or send

out control commands (depicted as the dashed thick lines) to the Powerbot (or MobileSim)

through RS232 serial port (or TCP/IP). The low-level software (referred to as Advanced

Robotics Control Operating System, ARCOS [79]) within the Powerbot embedded system

is responsible for communication with robot system software, and handles motor controls

and raw sensor reading.

In Figure A.2, how often the data is updated in the execution stage is also indicated.

The user interface in the main thread is updated approximately every 35ms (△tUI). How

often the SLAM algorithm (△tSLAM) in planner thread, and the path execution module in

the ARIA thread are called is controlled by the ARIA. The sensor readings (△tsensor) from

and the control commands (△tcontrol) to the Powerbot are updated as the same frequency.

The Planner Thread

The planner thread is mainly based on the overall exploration flowchart shown in Chapter

of Introduction (Figure 1.1). Currently, we do not have the view planner incorporated

(dash-lined rectangular in Figure A.2) in our implementation. Instead, we manually set the

robot’s next goal point from the user interface.

When the robot is planning the next view point, or planning the path to the view point,

the robot will be kept still and no task is given to the ARIA thread, which waits in an

empty loop. Once the solution path is returned by the motion planner, the planner thread

will notify the ARIA thread to execute the path, and call the SLAM algorithm to update

the robot pose and the map during the path execution.

The ARIA Thread: Path Execution

After the solution path is read, it is given to the ARIA thread, which starts the path exe-

cution process. During the path execution stage, the three tasks of updating user interface

(in main thread), updating the environment and the robot location (the SLAM algorithm

in the planner thread), and path execution control module (the ARIA thread) are running


simultaneously. SLAM and the path execution module run at the same priority, but the

user interface is updated at a lower priority (we let the main thread sleep for 20ms, each

time the user interface is updated).

In the SLAM algorithm we used (i.e., Gmapping), the re-sampling step (it includes

sensor data registration and map update) is carried out only if the inverse of the variance of

the weights of the particles drops below a threshold (refer to [72] for detail). Consequently,

in practice, we have noticed that the map will be updated each time the robot rotates about

40o or translates about 0.6m. Map update is the most expensive part of the re-sampling

step, if the re-sampling step is carried out during the SLAM algorithm. It takes about 1

second (.2 seconds for sensor data registration and .8 seconds for map update) to finish, so

the robot will not response to sensor readings or send the corresponding control commands

in time. In this case, we temporarily stop the robot motion (by hanging the ARIA thread),

wait until the SLAM process is finished, and then resume the path execution module.

At each iteration of the ARIA thread, the VFH collision avoider will calculate the control

command based on the current robot pose and map, the path, and the sonar readings (refer

to Section A.2). The control command will be sent to the Powerbot.

The Main Thread: User Interface Updating

Figure A.3 shows the user interface of the system software which consists of mainly three

windows (marked as A, B and C). The Opengl window A mainly displays information

regarding the planning stage, e.g., the RRT in UC-space and the solution path. Window B

is used to display the resulting environment map, the robot, and the trail of the robot. The

terminal window C is used for user inputs, such as next sub-goal.

In window A the solution path is in color yellow. The ellipse along the path indicates

the localization uncertainty. The local sonar map with robot at the center is depicted in the

right-bottom corner of window A. Please refer to Section A.2 for more detailed explanation

about this area.

In window B, the robot trail along the solution path is marked by yellow color. For the

environment map, the white areas are free, the dark gray areas are obstacle and the light

blue areas are unknown.


Figure A.3: The user interface consists of three windows (marked as A, B and C). WindowA and B are for displaying information and results, and Window C is for user inputs. See thetext for detailed explanation. For local sonar map with robot at the center (right-bottomcorner of window A), please refer to Section A.2 for detailed explanation about this area.


A.2 Sonar-Based Local Collision Avoidance for the Mobile

Base During Path Execution

The LIDAR technique based range sensor often misses objects in black color [88]. To further

address safety issue, we incorporate the on-board sonar sensor for local collision avoidance,

in the execution stage, as the mobile base follows the solution path returned by RRT-SLAM.

Many techniques can be applied for sonar based local collision avoidance, such as the

Vector Field Histogram (VFH) approach [89] and the Dynamic Window (DW) approach

[90]. We used the VFH approach, which is briefly described below.

The VFH approach maintains a dynamic local window of a fixed size centered at the

current mobile base position located at the center of the window. The window area is

discretized into grid cells, each of which stores a certainty value (CV) to indicate the certainty

level of that grid cell being occupied/free. There exist many alternatives to calculate the

certainty value of a grid cell. In our implementation, we apply the work in [62] to calculate,

for each grid, the “occupancy probability” (a number between 0 and 1). The “occupancy

probability” is the probability that the cell is an obstacle cell, which particularly addresses

the issue about sonar noise, and is used as the CV for each grid cell.

Based on this local window, the heading directions of the base are discretized. If along a

discretized direction, the cumulated grids’ occupancy probability is higher than a threshold,

this direction is marked as “forbidden”. The available heading directions are referred to

as “valleys”. Finally, the robot chooses the valley which is closest to the heading direction

pointing to the goal.

The VFH approach was implemented by Aidin Miresaidi, an undergraduate student in

the RAMP lab, School of Engineering Science, SFU, and tested in the simulated environ-

ments, i.e., MobileSim [79]. We modified Aidin’s source code and incorporated this VFH

local avoidance approach into our path following process.

The detailed steps for the path following process in the ARIA thread (Figure A.2) are

given below. After the planner, e.g., RRT-SLAM, returns a solution path, which consists

of a sequence of line segments in the x − y plane, the robot will start to follow the solution

path with the VFH approach incorporated.

At each time instant t (every △tSLAM ), the robot applies SLAM to estimate its pose

and the map. Given the estimated pose by SLAM, the robot calculates its next motion

command as follows. First, the robot finds its preferred heading direction by calculating the


heading direction towards the next intermediate subgoal on the path. Then, it calculates

the valleys using VFH. If the preferred heading direction of the robot is not inside the

valleys, we choose, in the valleys, a heading direction closest to the this preferred heading .

Otherwise, the robot uses the preferred heading direction. Once the robot heading direction

(θh) is acquired, the motion command aπt = [△dt,△θt] (△dt and △θt are translation and

rotation, respectively) is given by:

△θt = θh − θ∗t . (A.1)

△dt =

{

√

(xnext − x∗t )

2 + (ynext − y∗t )2, if|△θt| > εθ

0 else

where (x∗, y∗, θ∗t ) is the estimated robot pose returned by SLAM, (xnext, ynext) is the next

intermediate subgoal position along the path, and εθ is a user defined threshold. Basically,

Equation A.1 means that the robot will first turn to the direction of the next sub-goal and

then move straight towards it.

In Figure A.3, the local window is depicted at the right-bottom corner of the window A,

where white areas are free, black areas are obstacles sensed by the sonar. The VFH valleys

are indicated with green beams. Red line segment points to the robot preferred heading

direction and thick green line segments is to the current heading.

Table A.2 lists some parameters of VFH that are used in our PowerBot planner.

Window size (m) Grid size (m) Resolution of motion Direction (Degree)

1.6 0.05 5

Table A.1: VFH parameters applied.

Bibliography

[1] V. A. Sujan and S. Dubowsky. Efficient information-based visual robotic mapping in

unstructured environments. The International Journal of Robotics Research, 24(4):275–

295, 2005.

[2] M. Seelinger, J. D. Yoder, E. T. Baumgartner, and S. B. Skaar. High-precision vi-

sual control of mobile manipulators. IEEE Transactions on Robotics and Automation,

18(6):957–965, 2002.

[3] T. Tomizawa, A. Ohya, and S. Yuta. Remote book browsing system using a mobile

manipulator. In Proceedings of IEEE International Conference on Robotics and Au-

tomation (ICRA03), pages 256–261, 2003.

[4] G. Hirzinger, L. Zlajpah., R. Franci, and V. Parenti-Castelli. Integration of a redundant

mobile manipulator system : a drink serving task. In 15th International Workshop on

Robotics in Alpe-Adria-Danube Region (RAAD06), pages 301–306, 2006.

[5] J. C. Latombe. Robot Motion Planning. Kluwer Academic Publishers, 1991.

[6] S. M. LaValle. Planning Algorithms. Cambridge Unversity Press, U.K., 2006.

[7] S. Thrun, W. Burgard, and D. Fox. Probabilistic Robotics. The MIT Press, 2005.

[8] B. K. P. Horn. Robot vision. MIT press, 1986.

[9] M. Kazemi and K. Gupta. Global path planning for robust visual servoing in com-

plex environments. In Proceedings of IEEE International Conference on Robotics and

Automation (ICRA09), May 2009.

[10] H. Choset, K. M. Lynch, S. Hutchinson, G. Kantor, W. Burgard, L.E. Kavraki, and

S. Thrun. Principles of robot motion. MIT press, 2005.

117

BIBLIOGRAPHY 118

[11] Y. Yu and K. Gupta. C-space entropy: A measure for view planning and exploration

for general robot-sensor systems in unknown environments. International Journal of

Robotics Research, 23(12):1197–1223, Feb. 2004.

[12] L. Torabi, M. Kazemi, and K. Gupta. Configuration space based efficient view planning

and exploration with occupancy grids. In Proceedings of IEEE International Conference

on Intelligent Robots and Systems (IROS 2007), pages 2827–2832, 2007.

[13] L. Torabi. Fully automated 3D surface modeling by a mobile manipulator. Ph.D thesis

proposal, Engineering Science, Simon Fraser University, 2008.

[14] Y. Yu. An information theoretical incremental approach to sensor-based motion plan-

ning for eye-in-hand systems. PhD thesis, Engineering Science, Simon Fraser University,

2000.

[15] P. K. Allen, M. K. Reed, and I. Stamos. View planning for site modeling. In Proceedings

of the DARPA Image Understanding Workshop, pages 21–23, Nov. 1998.

[16] F. Bourgault, A. A. Makarenko, S. B. Williams, B. Grocholsky, and H. F Durrant-

Whyte. Information based adaptive robotic exploration. In Proceedings of IEEE In-

ternational conferenece on intelligent Robots and System (IROS02), pages 540–545,

2002.

[17] T. Lozano-Perez. Spatial planning: A configuration space approach. IEEE transactions

on computers, 100(32):108–120, 1983.

[18] B. Yamauchi. A frontier-based approach for autonomous exploration. In Proceedings

of IEEE International Symposium on Computational Intelligence in Robotics and Au-

tomation (CIRA97), pages 146–151, 1997.

[19] O. Brock and L.E. Kavraki. Decomposition-based motion planning: A framework for

real-time motion planning in high-dimensional configuration spaces. In Proceedings of

IEEE International Conference on Robotics and Automation (ICRA01), pages 1469–

1474, 2001.

[20] M. Montemerlo, S. Thrun, D. Koller, and B. Wegbreit. FastSLAM: A factored solution

to the simultaneous localization and mapping problem. In Proceedings of the AAAI

National Conference on Artificial Intelligence, pages 593–598, 2002.

BIBLIOGRAPHY 119

[21] N. Roy and S. Thrun. Coastal navigation with mobile robots. Advances in Neural

Processing Systems, 12:1043–1049, 2000.

[22] A. Lambert and D. Gruyer. Safe path planning in an uncertain-configuration space. In

Proceedings of the 2002 IEEE International conferenece on Robotics and Automation

(ICRA02), pages 4185–4190, Sept 2003.

[23] J. J. Kuffner Jr. and S. M. LaValle. RRT-connect: An efficient approach to single-

query path planning. In Proceedings of IEEE International Conference on Robotics

and Automation (ICRA 2000), pages 995–1001, 2000.

[24] M. Kalisiak and M. V. de Panne. RRT-blossom: RRT with a local flood-fill behavior. In

Proceedings of IEEE International Conference on Robotics and Automation (ICRA06),

pages 1237–1242, April 2006.

[25] D. Barbara and P. Chen. Using the fractal dimension to cluster datasets. In Proceedings

of the sixth ACM SIGKDD international conference on Knownledge discovery and data

mining (KDD2000), pages 260–264, 2000.

[26] L. E. Kavraki, P. Svestka, J. C. Latombe, and M. H. Overmars. Probabilistic roadmaps

for path planning in high-dimensional configuration spaces. IEEE Transactions on

Robotics and Automation, 12(4):566–580, Aug. 1996.

[27] I. Dumitresce and N. Boland. Algorithms for the weight constrained shortest path

problem. International Transactions in Operational Research, 8(1):15–29, 2001.

[28] R. Bohlin and L. E. Kavraki. Path planning using lazy PRM. In Proceedings of IEEE

International Conference on Robotics and Automation (ICRA00), pages 521–528, 2000.

[29] J. Hershberger, M. Maxel, and S. Suri. Finding the k shortest simple paths: A new

algorithm and its implementation. In Proceedings of the Fifth Workshop on Algorithm

Engineering and Experiments, pages 26–36, 2003.

[30] N. J. Nilsson. A mobile automation: An application of artificial techniques. In Proceed-

ings of the 1st International Joint Conference on Artificial Intelligence, pages 509–520,

1969.

BIBLIOGRAPHY 120

[31] O. Khatib. Real-time obstacle avoidance for manipulators and mobile robots. In Pro-

ceedings of IEEE International Conference on Robotics and Automation (ICRA85),

pages 90–98, 1985.

[32] J. Barraquand and J. C. Latombe. A Monte-Carlo algorithm for path planning with

many degrees of freedom. In Proceedings of IEEE International Conference on Robotics

and Automation 1990 (ICRA90), pages 1712–1217, 1990.

[33] J. M. Ahuactzin, K. Gupta, and E. Mazer. Manipulation planning for redundant robots:

a practical approach. The International Journal of Robotics Research, 17(7):731–741,

1998.

[34] P. Cheng. Reducing RRT metric sensitivity for motion planning with differential con-

straints. Master’s thesis, Computing Science, Iowa State University, 2002.

[35] D. Hsu, T. Jiang, J. Reif, and Z. Sun. The bridge test for sampling narrow passages with

probabilistc roadmap planners. In Proceedings of the IEEE International Conference

on Robotics and Automation (ICRA03), volume 3, pages 4420–4426, 2003.

[36] S. A. Wilmarth, N. M. Amato, and P. F. Stiller. MAPRM: A probabilistic roadmap

planner with sampling on the medial axis of the free space. In Proceedings of the

IEEE International Conference on Robotics and Automation (ICRA99), volume 2,

pages 1024–1031, 1999.

[37] N. M. Amato, O. B. Bayazit, L. K. Dale, C. Jones, and D. Vallejo. OBPRM: An

obstacle-based PRM for 3D workspaces. In Robotics: The Algorithmic Perspective:

Workshop on the Algorithmic Foundations of Robotics, pages 155–168, 1998.

[38] D. Hsu, L. E. Kavraki, J. C. Latombe, R. Motwani, and S. Sorkin. On finding narrow

passages with probabilistic roadmap planners. In Robotics: The Algorithmic Perspec-

tive: Workshop on the Algorithmic Foundations of Robotics, pages 141–154, 1998.

[39] T. Lozano-Perez, M. T. Mason, and R. H. Taylor. Automatic synthesis of fine-motion

strategies for robots. The International Journal of Robotics Research, 3(1):3–24, 1984.

[40] A. Lazanas and J. C. Latombe. Landmark-based robot navigation. Algorithmica,

13(5):472–501, 2005.

BIBLIOGRAPHY 121

[41] N. Roy and G. Gordon. Exponential family PCA for belief compression in POMDPs.

In Proceedings of Advances in Neural Information Processing (NIPS), pages 1667–1674,

June 2002.

[42] J. P. Gonzalez and A. Stentz. Replanning with uncertainty in position: Sensor updates

vs. prior map updates. In Proceedings of the IEEE International Conference on Robotics

and Automation (ICRA08), pages 1806–1813, 2008.

[43] B. Bouilly, T. Simeon, and R. Alami. A numerical technique for planning motion strate-

gies of a mobile robot in presence of uncertainty. In Proceedings of IEEE International

Conference on Robotics and Automation (ICRA95), pages 1327–1332, May 1995.

[44] A. Censi, D. Calisi, A. D. Luca, and G. Oriolo. A Bayesian framework for optimal

motion planning with uncertainty. In Proceedings of IEEE International Conference

on Robotics and Automation (ICRA08), pages 1798–1805, 2008.

[45] R. He, S. Prentice, and N. Roy. Planning in information space for a quadrotor helicopter

in a GPS-denied environment. In Proceedings of the IEEE International Conference on

Robotics and Automation (ICRA08), pages 1814–1820, 2008.

[46] N. A. Melchior and R. Simmons. Particle RRT for path planning with unceratinty. In


pages 1617–1624, April 2007.

[47] S. M. LaValle and S. A. Hutchinson. An objective-based stochastic framework for

manipulation planning. In Proceedings of IEEE/RSJ/GI International Conference on

Intelligent Robots and Systems (IROS94), pages 1772–1779, 1994.

[48] R. Smith, M. Self, and P. Cheeseman. Estimating uncertain spatial relationships in

robotics. Autonomous robot vehicles, 1:167–193, 1990.

[49] S. Thrun. Probabilistic algorithms in robotics. AI Magazine, 21(4):93–109, Winter

2000.

[50] L.A. Page and A.C. Sanderson. A path-space search algorithm for motion planning

with uncertainties. In Proceedings of IEEE International Symposium on Assembly and

Task Planning, pages 334–340, 1995.

BIBLIOGRAPHY 122

[51] R. Pepy and A. Lambert. Safe path planning in an uncertain-configuration space using

RRT. In Proceedings of IEEE/RSJ International Conference on Intelligent Robots and

Systems (IROS06), pages 5376–5381, 2006.

[52] H. Takeda and J. C. Latombe. Sensory uncertainty field for mobile robot navigation. In


pages 2465–2472, 1992.

[53] J. P. Gonzalez and A. Stentz. Planning with uncertainty in position: an optimal and

efficient planner. In Proceedings of IEEE International Conference on Intelligent Robots

and Systems (IROS 2005), pages 1327–1332, May 2005.

[54] D. Hsu, W.S. Lee, and N. Rong. A point-based POMDP planner for target tracking.

In Proceedings of IEEE International Conference on Robotics and Automation (ICRA

2008), pages 2644–2650, 2008.

[55] S. M. LaValle. A game-theoretic framework for robot motion planning. PhD thesis,

Electrical Engineering, University of Illinois at Urbana-Champaign, 1995.

[56] R. Alterovitz, T. Simeon, and K. Goldberg. The stochastic motion roadmap: A

sampling framework for planning with Markov motion uncertainty. In Proceeding of

Robotics: Science and Systems, 2007.

[57] P. E. Missiuro and N. Roy. Adapting probabilitic roadmaps to handle uncertain

maps. In Proceedings of IEEE International Conference on Robotics and Automation

(ICRA95), pages 1261–1267, May 2006.

[58] C. Stachniss, G. Grisetti, and W. Burgard. Information gain-based exploration us-

ing rao-blackwellized particle filters. In Proceedings of Robotics: Science and Systems

(RSS05), pages 65–72, 2005.

[59] L. Freda, F. Loiudice, and G. Oriolo. A randomized method for integrated exploration.

In Proceedings of the IEEE International Conference on Intelligent Robots and Systems

(IROS2006), pages 2457–2464, 2006.

[60] A. Eliazar and R. Parr. DP-SLAM 2.0. In Proceedings of IEEE International Conference

on Robotics and Automation (ICRA 2004), volume 2, pages 1314– 1320, 2004.

BIBLIOGRAPHY 123

[61] T.M. Cover and J.A. Thomas. Elements of information theory. Wiley InnerScience,

New York, 1991.

[62] A. Elfes. Using occupancy grids for mobile robot perception and navigation. Computer,

22(6):46–57, 1989.

[63] J. Weingarten and R. Siegwart. EKF-based 3D SLAM for structured environment

reconstruction. In Proceedings of IEEE Intelligent Robots and Systems (IROS05), pages

3834–3839, 2005.

[64] I. Dumitrescu and N. Boland. Algorithms for the weight contrained shortest path

problem. International Transactions in Operational Research, 8(1):15–29, 2001.

[65] Y. Huang and K. Gupta. RRT-SLAM for motion planning with motion and map un-

certainty for robot exploration. In Proceedings of IEEE/RSJ International Conference

on Robotics and System (IROS08), 2008.

[66] Y. Huang and K. Gupta. Lazy-prm for a manipulator with base pose uncertainty.

submitted to ieee/rsj international conference on robotics and system (iros09). 2009.

[67] Y. Huang and K. Gupta. A delaunay triangulation based node connection strategy for

probabilistic roadmap planners. In Proceedings of IEEE International Conference on

Robotics and Automation (ICRA04), pages 908 – 913, June 2004.

[68] Y. Huang and K. Gupta. Random tree techniques using fractal dimension for motion

planning in uncertainty-configuration space. In Technique report, Engineering Science,

Simon Fraser University, 2008.

[69] S. Lee and D. Kim. Recursive unscented Kalman filtering based SLAM using a large

number of noisy observations. International Journal of Control, Automation, and Sys-

tems, 4(6):736–747, 2006.

[70] S. M. LaValle. Rapidly-exploring random trees: A new tool for path planning. Technical

report, Computer Science Dept, Iowa State University, 1998.

[71] Gmapping. URL: http://www.slam.org.

[72] G. Grisetti, C. Stachniss, and W. Burgard. Improved techniques for grid mapping with

rao-blackwellized particle filters. IEEE Transactions on Robotics, 23(1):34–46, 2007.

BIBLIOGRAPHY 124

[73] B. Gerkey, R. T. Vaughan, and A. Howard. The player/stage project: Tools for multi-

robot and distributed sensor systems. In Proceedings of the 11th International Confer-

ence on Advanced Robotics (ICAR 2003), pages 317–323, June 2003.

[74] S. R. Lindemann and S. M. LaValle. Incrementally reducing dispersion by increasing

Voronoi bias in RRTs. In Proceedings of IEEE International Conference on Robotics

and Automation 2004 (ICRA 2004), pages 3251–3257, 2004.

[75] R. Xu and W. Donald II. Survey of clustering algorithms. IEEE Transactions on

Neural Networks, 16(3):645, 2005.

[76] J. J. Sarraille and L. S. Myers. FD3: A program for measuring fractal dimension.

Educational and Psychological Measurement, 54(1):94–97, 1994.

[77] A. Pattekar, J. Cohen, T. Hudson, S. Gottschalk, M. Lin, and D. Manocha. V-

COLLIDE User’s Manual - Release 1.1. Department of Computer Science, University

of North Carolina at Chapel Hill, 1998.

[78] G. Chen and G. Xue. A PTAS for weight constrained steiner trees in series parallel

graphs. In Theoretical Computer Science, pages 237–247. Elsevier, 2003.

[79] http://www.mobilerobots.com.

[80] Z. Yao and K. Gupta. Path planning with general end-effector constraints. Robotics

and Autonomous Systems, 55(4):316–327, 2007.

[81] D. F. Watson. Computing the n-dimensional Delaunay tessellation with application to

Voronoi polytopes. The computer journal, 24(2):167–172, 1981.

[82] J.E. Goodman and J. O’Rourke. Handbook of discrete and computational geometry.

Chapman & Hall/CRC, 2004.

[83] K. Mehlhorn and M. Seel. Delaunay triangulations in higher-

dimensional space, implementation report. URL: http://www.mpi-

sb.mpg.de/LEDA/www/leps/dd geokernal.html, 1998.

[84] K. Fukuda. Frequently asked questions in polyhedral computation. URL:

http://www.ifor.math.ethz.ch/∼fukuda/polyfaq/polyfaq.html, 23:111, 2000.

BIBLIOGRAPHY 125

[85] I. Gipson, K. Gupta, and M. Greenspan. MPK: An open extensible motion planning

kernel. Journal of Robotic Systems, 18(8):433–443, 2001.

[86] N. M. Amato, O. B. Bayazit, L. K. Dale, C. Jones, and D. Vallejo. Choosing good dis-

tance metrics and local planners for probabilistic roadmap methods. In Proceedings of

the IEEE International Conference on Robotics and Automation (ICRA00), volume 16,

pages 442–447, 2000.

[87] R. Philippsen, S. Kolski, K. Macek, and B. Jensen. Mobile robot planning in dynamic

environments and on growable costmaps. In Workshop on Path Planning on Costmaps,

IEEE International Conference on Robotics and Automation (ICRA08), 2008.

[88] H. Kawata, A. Ohya, S. Yuta, W. Santosh, and T. Mori. Development of ultra-small

lightweight optical range sensor system. In Proceedings of IEEE/RSJ/GI International

Conference on Intelligent Robots and Systems (IROS05), pages 1078–1083, 2005.

[89] J. Borenstein and Y. Koren. The vector field histogram-fast obstacle avoidance for

mobile robots. IEEE Transactions on Robotics and Automation, 7(3):278–288, 1991.

[90] D. Fox, W. Burgard, and S. Thrun. The dynamic window approach to collision avoid-

ance. IEEE Robotics & Automation Magazine, 4(1):23–33, 1997.

Documents

MOTION PLANNING WITH LOCALIZATION AND MAPPING