Probabilistic Roadmap for Path Planning in HighDimensional Configuration Spaces

1
Probabilistic Roadmap for Path Planning in
High-Dimensional Configuration Spaces

• Lionov - 3304515

2
The Problem

• Given a start and goal configurations of a Robot
  (with high DOF) in static workspace
• Computes collision-free path between those
  configurations.
• Sample Video
• (6 DOF rigid robot)

3
Previous Work

• Potential Field
• Main disadvantage local minima
• Solution is expensive in high dimensional
  configuration space
• Randomized Path Planner (RPP)
• Random walk to escape local minima
• Fail in difficult situation (area surrounded by
  obstacles with only narrow passage to escape)

4
Probabilistic RoadmapGeneral Method

• Use roadmap approach (instead of potential field
  or cell decomposition)
• Two phase
• The Learning Phase
• The Construction Step
• The Expansion Step
• The Query Phase
• The two phases do not have to be executed
  sequentially

5
General MethodTwo Phases

• The Learning Phase
• Construct probabilistic roadmap
• Store it as a graph
• Nodes collision-free configuration
• Edges feasible path between nodes (computes
  using simple and fast local planner)
• The Query Phase
• Start and goal configuration are connected to two
  nodes of the roadmap
• Search in roadmap for a path joining those two
  nodes

6
The Learning PhaseConstruction Step

• Construction Step Algorithm
• ContsructionStep ( )
• Roadmap Graph R(N,E) is empty
• Loop
• add random free configuration c to N
• Nc ? subset of N (Nc node of candidate
  neighbor)
• for each node n in Nc in increasing order
  do
• if c and n are not connected in R and
• there is a local path between c
  and n
• add edge between c and n to
  E
• update Rs connected
  component

7
Construction Step Random Free Configuration

• Uniform probability distribution
• Collision-free configuration is added to N
• Need collision detection
• Intersect an obstacles
• Two distinct bodies of robot intersect each other

8
Construction Step The Local Planner

• Deterministic vs non-deterministic
• Nondeterministic local paths should be stored
  in the roadmap
• Tradeoff time spent vs number of call
• Powerful local planner slow, low number of
  calls ? sparse roadmap
• Fast local planner fast, high number of calls ?
  dense roadmap
• Will affects query phase
• Roadmap should be dense enough
• Should easy to connect start/goal configuration
  to roadmap
• Very fast local planner seems reasonable (based
  on experiment)

9
Construction StepThe Local Planner

• General local planner
• Straight line segment connecting two
  configurations
• Collision detection
• Discretize the line segment into a number of
  configurations
• For each configuration, test it for collision

10
Construction StepThe Distance Function

• Used for construct and sort the set Nc
• Distance between any pair (c,n) of configurations
• Should reflects the chance that local planner
  will fail to compute a feasible path
• For general global planner
• x point on the robot
• x(c) position of x in the workspace
• x(n) x(c) Euclidean distance between
  x(n) and x(c)

11
Construction StepThe Node Neighbor

• The node neighbor
• Connect c to node neighbor using local planner
• Avoid call of local planner that are likely to
  fail
• Create some constant threshold (maxdist)
• Distance between c and each of node neighbor is
  less than maxdist
• Try to connect both nodes in increasing order and
  skip node that already in the same connected
  component reduce calls to local planner
• Bound the size of the set running time is
  linear to the size of the graph

12
The Learning Phase Expansion Step

• Why ?
• To improve connectivity of the roadmap
• How ?
• Expand nodes from N lie on difficult region
  increase density of the roadmap
• Positive weight w(c) for each node c
• measure the difficulty of the region around it

13
The Learning PhaseExpansion Step

• At the end of the construction step failure
  ratio
• At the beginning of the expansion step w(c)
• How to expand node ?
• Random-bounce walk until some final configuration
  n
• Add edge (c,n) to E and store the path
• Try to connect n to other connected component

14
Query Phase

• Connect start goal configuration to node in R
• Use local planner (to node in increasing
  distance)
• Ignore nodes that located far away (more than
  maxdist)
• If always fail use random-bounce walk and try
  local planner again
• Problems
• Roadmap contains several component
• Try to connect to the same component
• Path planning fail frequently
• Roadmap not adequately capture connectivity
  back to learning phase

15
ImplementationCustomized Implementation

• General method can be modified
• Perform better for particular application
• Planar articulated robot
• 5 link (can be any polygon) 5 revolute joints
• Internal joint limit
• prevent self collision between any two adjacent
  links

16
Customized ImplementationPlanar Articulated Robot

• Local planner
• Translate odd index joints with straight line
• Even index joints will follow using inverse
  kinematic
• Distance computation
• Ji(x) Ji(y) Euclidean distance between
  Ji(x) and Ji(y)
• Collision Detection
• Treat each link as distinct robot
• Create each links configuration-space bitmap

17
Planar Articulated Robot Fixed-Base Articulated
Robot
18
Planar Articulated RobotFixed-Base Articulated
Robot

• Time Learning Construction Expansion
• 30 roadmap for every row
• Average node largest roadmap component
• One trial for each roadmap (less than 2.5 second)
• Longer learning time ? higher success rate

19
Planar Articulated RobotFixed-Base Articulated
Robot

• No expansion leads to lower success rate
• Especially when learning time is short
• Spent learning time in expansion step is better
• Ratio TC/TE 2 gives good results

20
Planar Articulated RobotFixed-Base Articulated
Robot

• Only one roadmap for each row
• Thousand collision check random-bounce walk
• Higher learning rate ? different size of
  component
• Connection time (query phase) fraction sec to a
  few second
• More collision check on learning phase

21
Planar Articulated RobotFree-Base Articulated
Robot
22
Planar Articulated RobotFree-Base Articulated
Robot

• Successful connections to the roadmaps is greater
  with expansion
• Path planning is very fast a fraction of second

23
ImplementationGeneral Implementation

• Scene 1 (left) 3 revolute joints 1 prismatic
  joint
• Scene 2 (right) 5 revolute joints

24
General ImplementationResult

• 30 roadmaps per row
• Query answered within 2.5 seconds
• General implementation can be applied to rather
  complicated planning problem
• For more difficult problem, we need customized
  implementation
• From experiment needs 25 sec of learning phase
  (with expansion) to achieve 100 success rate to
  solve problem on free-base articulated robot
  (if general implementation is used)

25
Conclusion Challenge

• Conclusion
• PRM Learning Query phase
• Method is general applied to any type of
  holonomic robot
• Customizable for specific problem local planner,
  distance function collision detection
• Path planning is very fast if we have good
  roadmap
• Challenge
• Dynamic environment