1 Single Robot Motion Planning - II Liang-Jun Zhang COMP790-058 Sep 24, 2008.

1

Single Robot Motion Planning - II

Liang-Jun ZhangCOMP790-058

Sep 24, 2008

2

Review: C-space

Workspace Configuration Space

xyRobot

Initial

Goal

Free

Obstacle C-obstacle

A 2D Translating Robot

3

Review: Computing C-obstacle

• Difficult due to geometric and space complexity• Practical solutions are only available for

– Translating rigid robots: Minkowski sum– Robots with no more than 3 DOFs

4

Outline

• Approximate cell decomposition

• Sampling-based motion planning

5

Approximate Cell Decomposition (ACD)

• Not compute the free space exactly at once• But compute it incrementally

• Relatively easy to implement– [Lozano-Pérez 83]– [Zhu et al. 91]– [Latombe 91]– [Zhang et al. 06]

Octree decomposition

6

full mixed

empty

Approximate Cell Decomposition

• Full cell

• Empty cell

• Mixed cell– Mixed– Uncertain

• Cell labelling algorithms– [Zhang et al 06]

Configuration Space

7

Finding a Path by ACD

Goal

Initial

8

Connectivity GraphGf : Free Connectivity Graph G: Connectivity Graph

Gf is a subgraph of G

9

Finding a Path by ACD

Goal

Initial Gf : Free Connectivity Graph

10

Finding a Path by ACD

L: Guiding Path• First Graph Cut Algorithm– Guiding path in connectivity

graph G

– Only subdivide along this path

– Update the graphs G and Gf

11

First Graph Cut Algorithm

Only subdivide the cells along L

L : Guiding Path

new Gf

12

Finding a Path by ACDGf

• A channel

• Can be used for path smoothing.

13

ACD for Path Non-existence

C-space

Goal

Initial

14

Connectivity Graph Guiding Path

ACD for Path Non-existence

15

ACD for Path Non-existence

Connectivity graph is not connected

No path!

A sufficient condition for deciding path non-existence

16

• Live Demo– Gear-2DOF– Gear-3DOF

17

Five-gear Example

Video

Initial

Goal

roadmap in free space

Total timing 85s

# of total cells 168K

18

Two-gear Example

no path!

Cells in C-obstacle

Initial

Goal

Roadmap in F

Video 3.356s

19

Motion Planning Framework

• Continuous representation

• Discretization

• Graph search

20

Summary: Approximate Cell Decomposition

• Simple and easy to implement

• Efficient and practical for low DOF robots – Inefficient for 5 or more DOFs robot

• Resolution-complete– Find a path if there is one– Otherwise, report path non-existence– Up to some resolution of the cell

21

Outline

• Approximate cell decomposition

• Sampling-based motion planning

22

Motivation

• Geometric complexity• Space dimensionality

23

Probabilistic Roadmap (PRM)

free space

qqinitinit

qqgoalgoal

milestone

[Kavraki, Svetska, Latombe,Overmars, 95]

local path

24

Basic PRM AalgorithmInput: geometry of the moving object & obstaclesOutput: roadmap G = (V, E)

1: V and E .

2: repeat3: q a configuration sampled uniformly at random from C.4: if CLEAR(q)then5: Add q to V.6: Nq a set of nodes in V that are close to q.

6: for each q’ Nq, in order of increasing d(q,q’)7: if LINK(q’,q)then8: Add an edge between q and q’ to E.

25

Two Geometric Primitives in C-space

• CLEAR(q)Is configuration q collision free or not?

• LINK(q, q’)

Is the straight-line path between q and q’ collision-free?

26

Query Processing

• Connect qinit and qgoal to the roadmap

• Start at qinit and qgoal, perform a random walk, and try to connect with one of the milestones nearby

• Try multiple times

27

Two Tenets of PRM Planning Checking sampled configurations and

connections between samples for collision can be done efficiently. Hierarchical collision checking[Hierarchical collision checking methods were developed independently from PRM, roughly at the same time]

A relatively small number of milestones and local paths are sufficient to capture the connectivity of the free space. Exponential convergence in expansive

free space (probabilistic completeness)

28

Why does it work? Intuition• A small number of milestones almost

“cover” the entire free space.

29

Two Tenets of PRM Planning Checking sampled configurations and

connections between samples for collision can be done efficiently. Hierarchical collision checking[Hierarchical collision checking methods were developed independently from PRM, roughly at the same time]

A relatively small number of milestones and local paths are sufficient to capture the connectivity of the free space. Exponential convergence in expansive

free space (probabilistic completeness)

30

Narrow Passage Problem

• Narrow passages are difficult to be sampled due to their small volumes in C-space

Narrow passage

Alpha puzzle

qinitqgoal

2F

3F

Configuration Space

1F

31

Difficulty• Many small connected components

32

Strategies to Improve PRM

• Where to sample new milestones?– Sampling strategy

• Which milestones to connect?– Connection strategy

• Goal: – Minimize roadmap size to correctly answer

motion-planning queries

33

Sampling Strategies

34

small visibility sets

good visibility

poor visibility

Poor Visibility in Narrow Passages

• Non-uniform sampling strategies

35

But how to identify poor visibility regions?

• What is the source of information?– Robot and environment geometry

• How to exploit it?– Workspace-guided strategies– Dilation-based strategies– Filtering strategies– Adaptive strategies

36

Identify narrow passages in the workspace and map them into the configuration space

Workspace-guided strategies

37

F

O

Dilation-based strategies • During roadmap construction, allow milestones

with small penetration• Dilate the free space

– [Hsu et al. 98, Saha et al. 05, Cheng et al. 06, Zhang et al. 07]

A milestone with small penetration

38

Error

• If a path is returned, the answer is always correct.

• If no path is found, the answer may or may not be correct. We hope it is correct with high probability.

39

Weaker Completeness

Complete planner Too slow Heuristic planner Too unreliable

Probabilistic completeness:If a solution path exists, then the probability that the planner will find one is a fast growing function that goes to 1 as the running time increases

40

Kinodynamic Planning

41

RRT for Kinodynamic Systems

• Rapidly-exploring Random Tree

• Randomly select a control input

42

More Examples

• Car pulling trailers (complicated kinematics -- no dynamics)

43

Summary: Sampling-based Motion Planning

+ Efficient in practice + Work for robots with many DOF (high-

dimensional configuration spaces)+ Has been applied for various motion planning

problems (non-holonomic, kinodynamic planning etc.)

- Narrow passages problems (one of the hot areas)

- May not terminate when no path exists

44

Summary

• Configuration space

• Visibility graph

• Approximate cell decomposition – Decompose the free space into simple cells and represent the

connectivity of the free space by the adjacency graph of these cells

• Sampling-based approach– High-dimensional Configuration Spaces– Capture the connectivity of the free space by sampling

45

References

• Books– J.C. Latombe. Robot Motion Planning, 1991.– S.M. LaValle, Planning Algorithms, 2006

• Free book: http://msl.cs.uiuc.edu/planning/

– H. Choset et al. Principles of Robot Motion: Theory, Algorithms, and Implementations, 2005

• Conferences– ICRA: IEEE International Conference on Robotics and

Automation– IROS: IEEE/RSJ International Conference on Intelligent RObots

and Systems– WAFR: Workshop on the Algorithmic Foundations of Robotics