Probabilistic Roadmaps for Path Planning in High-Dimensional Configuration Spaces

Probabilistic Roadmaps for Path Planning in High-Dimensional Configuration Spaces

Lydia E. KavrakiPetr Švetka

Jean-Claude LatombeMark H. Overmars

Presented by Derek Chan and Stephen RussellOctober 10, 2007

Outline

• Introduction

• Previous work

• Algorithm– Learning phase– Query phase

• Simulation/Results

• Conclusion

Introduction

• Applications where robot must execute several point-to-point motions in static workspace– e.g. maintenance of cooling pipes in nuclear plant,

welding in car assembly, cleaning of airplane fuselages

• Require many dof to achieve desired configurations and avoid collision

• Explicit programming is tedious• Efficient, reliable planner would considerably

reduce burden

Previous Work

• Potential field methods– Heuristic is easily adapted to problem, but

local minima produce problems– Can be overcome with learning methods or

randomized walks (RPP)

• Other roadmap methods– Visibility graph and Voronoi diagram (limited

to low-dimensional C-space)– Silhouette method (prohibitively complex)

General Roadmap Algorithm Learning Phase:

Generate a roadmap which is an undirected graph R = (N,E) in 2 steps

Construction Step:obtain a graph that relatively uniformly covers the free C-space

Expansion Stepincrease connectivity by adding nodes in 'difficult' regions of the C-space

Query Phase:

Given a starting configuration and an ending configuration use the roadmap to determine a feasible path for the robot

In theory these two steps could be interwoven for better performance and both successful and unsuccessful query nodes could be added to the roadmap

Graphs and Configurations Example Roadmaps

Example Configurations

Construction Algorithm• N 0

• E

• Loop

• c a randomly chosen free configuration

• Nc a set of candidate neighbours of c chosen from N

• N N U {c}

• Forall n Є Nc in order of increasing Distance(c,n) do

• If notSameConnectedComponent(c,n) and

• localPlanner(c,n) then

• E E U {(c,n)}

• Update R's connected components

(1) Set of configuration graph nodes (N) is empty

(2) Set of simple path graph edges (E) is empty

• Loop through the steps below

• Randomly choose a configuration and name it c

• Select a set, Nc, of neighbours to c from N

• Add c to the set N

• For each member, n, of Nc in order of the distance between n and c do the following

• If c and n are not part of the same connect component

• And if the local planner can find a path from c to n

• Add edge (c,n) to E

• Update the connected components of the roadmap

Construction Algorithm• N 0

• E

• Loop



• N N U {c}




• E E U {(c,n)}


X,Y,Z above are neighbour candidates

No cycles are created

c

X Y

Z

New Node

Dist(c,x) Dist(c,y)

Dist(c,z)

Selecting Configurations• N 0

• E

• Loop



• N N U {c}




• E E U {(c,n)}


Step 4:

create a random configuration by randomly sampling each DOF uniformly over the interval of possible values

Check if there is a collision with self or an obstacle

If there is a collision randomly pick another configuration

Distance Function• N 0

• E

• Loop



• N N U {c}




• E E U {(c,n)}


Step 5:

Select all nodes of N that are within a selected max distance from c according to some distance function Distance(c,n)

Distance Function

One possibility is to measure the area/volume swept by the robot between the two configurations

Function should reflect the failure rate of localPlanner(c,n)

Local Planner• N 0

• E

• Loop



• N N U {c}




• E E U {(c,n)}


Step 9:

The Local Planner:

Determines whether a path exists to connect two nodes

Represents whether a robot can move from a configuration to another

The planner should be deterministic and fast but not necessarily powerful

More nodes at expense of greater failure rate for planner

To save space the path is calculated but not stored

Expansion Step

When there are obstacles, the roadmap might not be entirely connected where connectivity could exist

Nodes can be added in 'difficult' areas to increase connectivity

Expansion Step

Randomly Select nodes using a weighted distribution based on the 'difficulty' of each node

Possible difficulty functions for nodes:

Check if the number of nearby connected nodes is low

Check if the nearest non-connected component is close

Learning Phase Conclusion

Small connected components of roadmap are discarded

Unnecessarily long paths can be created

These can be smoothed or cycles can be introduced

Query Phase

After the learning phase is finished, use the roadmap to determine paths between configurations

Given a start configuration s and a goal configuration g, first connect s and g to the roadmap

Failure occurs if they can not be connected to the same component

g'

s'

s

g

Query Phase

Traverse the graph to find a path within the graph component between s' and g'

Paths must be recomputed since they were not stored

Collisions do not need to be rechecked

g'

s'

s

g

Application

• Algorithm was applied to planar articulated robots

• Two implementations:– Customized– General

• Customized implementation employs specific techniques for:– Local path-planning– Distance computation– Collision checking

Simulation setup

• Test cases for customizedimplementation

• Planar robot arm

J4

J2

J3J1

J5

Results

– Expansion phase is key• Tc/Te = 2

With expansion

Without expansion

General ImplementationSimple environments

General Implementation

Revisit complex scene fromcustomized implementation

Results(w/ expansion)

Conclusions

• Two phase method for robot motion planning in static workspace– Learning phase: construct roadmap (with increased

density in difficult regions)– Query phase: connect start/end locations to roadmap

to quickly create entire trajectory• Verified algorithm speed for complex scene

(esp. with customization)– Faster than RPP, which took tens of minutes to solve

• Consistency• Learning phase is precomputation, one-time cost

Probabilistic Roadmaps for Path Planning in High-Dimensional Configuration Spaces

Documents

Transcript of Probabilistic Roadmaps for Path Planning in High-Dimensional Configuration Spaces