Motion Planning

Department of Informatics
Intelligent Autonomous Systems
Technische Universitt Mnchen
Technical Cognitive Systems

Michael Beetz
Technische Universit
at M
unchen
Summer Term 2012
Part XI
Motion Planning
Outline
Motion Planning
Motion Planning
Michael Beetz
Summer Term 2012

3
Outline
Motion Planning
Motion Planning
Michael Beetz
Summer Term 2012

4
Motion Planning
Motion Planning
Michael Beetz
Summer Term 2012

5
Motion Planning
http://planning.cs.uiuc.edu/
Motion Planning
Michael Beetz
Summer Term 2012

6
The basic Path Planning Problem
Given obstacles, a robot, and its motion capabilities compute collision-free robot
motions from the start to goal.
Motion Planning
Michael Beetz
Summer Term 2012

7
Geometric Models
The robot and obstacles live in a world or workspace W.

Usually, W = R2 or W = R3 .
The obstacle region O W is a closed set.
The robot A(q) W is a closed set (placed at configuration q).
Can it be obtained automatically or with little processing?
What is the complexity of the representation?
Can collision queries be efficiently resolved?
Can a solid or surface be easily inferred?
Motion Planning
Michael Beetz
Summer Term 2012

8
Geometric Models: Linear Primitives
Motion Planning
Michael Beetz
Summer Term 2012

9
Geometric Models: Semi-Algebraic Sets

Consider primitives of the form:
Hi = {(x, y , z) W|fi (x, y , z) < 0},

which is a half-space if fi is linear. Now let fi be any polynomial, such as
f (x, y ) = x 2 + y 2 1. Obstacles can be formed from finite intersections:
O = H1 H2 H3 H4 .
and from finite unions of those:
O = H1 H2 H3 H4 .
O could then become any semi-algebraic set.
Motion Planning
Michael Beetz
Summer Term 2012

10
Geometric Models: Polygon Soups and Pointclouds
Whats inside?
Motion Planning
Michael Beetz
Summer Term 2012

11
Configuration Space: Example
All configurations of our robot lie on a manifold, here a torus.

Motion Planning
Michael Beetz
Summer Term 2012

12
Excursion: Flat Manifolds: Cylinder

Start with an open square (0, 1)2 ) R2
Let (x, y ) denote a point on the manifold.

Include the x = 0 points and define equivalence relation :
(0, y ) (1, y )
for all y (0, 1).
Motion Planning
Michael Beetz
Summer Term 2012

13
Excursion: Flat Moebius Band
Change the equivalence relation :

(0, y ) (1, 1 y )
for all y (0, 1).
Motion Planning
Michael Beetz
Summer Term 2012

14
Excursion: Other Flat Manifolds
Motion Planning
Michael Beetz
Summer Term 2012

15
Configuration Space: Obstacle Region
Given world W, a closed obstacle O W, a closed robot A, and configuration

space C.
Let A(q) W denote the placement of the robot into configuration q.
The obstacle region Cobs in C is
Cobs = {q C|A(q) O =
6 },
which is a closed set. The free space Cfree is an open subset of C:
Cfree = C \ Cobs
We want to keep the configuration in Cfree at all times!
Motion Planning
Michael Beetz
Summer Term 2012

16
Obstacle Region: Example
For the case of two links, C = S 1 S 1 , the obstacle region already becomes
strange and complicated!
Motion Planning
Michael Beetz
Summer Term 2012

17
Obstacle Region: Minkowski Sum
Also known as Convolution.

Motion Planning
Michael Beetz
Summer Term 2012

18
Obstacle Region: Polygonal Obstacles
Motion Planning
Michael Beetz
Summer Term 2012

19
Obstacle Region: Polygonal Obstacles
Motion Planning
Michael Beetz
Summer Term 2012

20
Configuration Space: Planning Problem Revisited

Given robot A and obstacle models O, C-space C and qI , qG Cfree .
Compute a path : [0, 1] Cfree so that (0) = qI and (1) = qG .

Motion Planning
Michael Beetz
Summer Term 2012

21
Combinatorial vs. Sampling-Based planning: the two families

Two families of motion planning algorithms exist:
Combinatorial Planning (exact planning)
Sampling-Based-Planning (probabilistic. randomized planning)
Motion Planning
Michael Beetz
Summer Term 2012

22
Completeness Notions
A planning algorithm may be:

Complete: If a solution exists, it finds one; otherwise, it reports failure.
Semi-complete: If a solution exists, it finds one; otherwise, it may run
forever.
Resolution complete: If a solution exists at a given resolution, it finds
one; otherwise, it terminates and reports no solution within this resolution

exists.
Probabilistically complete: If a solution exists, the probability that it is
found tends to one as the number of iterations tends to infinity.

Compare with decidability/computability!
Motion Planning
Michael Beetz
Summer Term 2012

23
Combinatorial Planning:
Mostly developed in the 1980s

Explicit construction of Cobs
Influence from computational geometry and computational real algebraic
geometry
All algorithms are complete
Usuall produce a roadmap in Cfree
Extremely efficient for low-dimensional problems but dont scale well for
higher dimensional problems

Some are difficult to implement (numerical issues)
Motion Planning
Michael Beetz
Summer Term 2012

24
Combinatorial Planning: Roadmaps
Combinatorial Planning Methods produce a topological graph G:

Each vertex is a configuration q Cfree
Each edge is a path : [0, 1] Cfree for which (0) and (1) are vertices.
A roadmap is a topological graph G with two properties:

Accessibility: From anywhere in Cfree it is trivial to compute a path that
reaches at least one point along any edge in G.
Connectivity-preserving: If there exists a path through Cfree from qI to
qG , then there must also exist one that travels through G.

Motion Planning
Michael Beetz
Summer Term 2012

25
Combinatorial Planning in a Polygonal Obstacle Region

Assume that Cobs (and Cfree ) are piecewise linear.
Could be a point robot among polygonal obstacles.
Could be a polygonal, translating robot among polygonal obstacles.
This methods tend to extend well to disc robots (e.g. roomba).
Use clever datastructures to encode vertices, edges, regions.

Example: Doubly connected edge list.
Motion Planning
Michael Beetz
Summer Term 2012

26
Combinatorial Planning in a Polygonal Obstacle Region
We consider four methods:

Trapezoidal Decomposition
Triangulation
Maximum Clearance Roadmap (retraction method)
Shortest-path roadmap (reduced visibility graph)
Motion Planning
Michael Beetz
Summer Term 2012

27
Combinatorial Planning Algorithms: Trapezoidal Decomposition I
There are 4 cases:
Motion Planning
Michael Beetz
Summer Term 2012

28
Combinatorial Planning Algorithms: Trapezoidal Decomposition II
O(n lg n) running time.

Easy to implement.
Scales to higher dimensions.
Motion Planning
Michael Beetz
Summer Term 2012

29
Combinatorial Planning Algorithms: Triangulation
O(n2 ) naive, O(n) optimal, O(n lg n) good tradeoff.
Motion Planning
Michael Beetz
Summer Term 2012

30
Combinatorial Planning Algorithms: Maximum Clearance Roadmap
Based on deformation retract from topology.

Imagine obtaining a skeleton by gradually thinning Cfree .
Kind of a generalized Voronoi diagram.
Motion Planning
Michael Beetz
Summer Term 2012

31
Combinatorial Planning Algorithms: Maximum Clearance Roadmap
Three cases:
O(n4 ), O(n lg n) optimal.

Motion Planning
Michael Beetz
Summer Term 2012

32
Combinatorial Planning Algorithms: Shortest Path Roadmap
Every reflex vertex (internal angle > ) is a roadmap vertex

Edges in the roadmap correspond to two cases
Consecutive reflex vertices
Bitanget edges
Motion Planning
Michael Beetz
Summer Term 2012

33
Combinatorial Planning Algorithms: Higher Dimensions

If C is 3 or more dimensions, most methods do not extend.
Optimal path planning for 3D polyhedra is NP-hard.
Maximul clearance roadmaps become disconnected in 3D.
Exception: Trapezoidal decomposition extends.
Motion Planning
Michael Beetz
Summer Term 2012

34
Sampling-Based Planning
Explicit construction of the obstacle space is often intractable!
Use collision detector to separate planning from input geometry

Systematically sample (random vs. deterministic) the configuration space
Probe for freespace by querying a collision detection algorithm
Single-query: Incremental sampling and searching
Multiple-query: Precompute a roadmap, then search it for each query
Motion Planning
Michael Beetz
Summer Term 2012

35
Sampling: Denseness
In topology, a set U is called dense in V if cl(U) = V .
Implication: Every open subset of V contains at least one point in U.
If U is dense and countable, a dense sequence can be formed:
:NU
Example: The rational numbers Q are dense in R.
Example: A random sequence is dense with probability one.

Motion Planning
Michael Beetz
Summer Term 2012

36
Sampling: Quick refresher of topology
Let X be a topological space, and U be any subset of X.

If there exists an open set O1 so that x O1 and O1 U, then x is called
an interior point of U. The set of all such points is denoted int(U).

If there exists an open set O2 so that x O2 and O1 X \ U, then x is
called an exterior point of U.

If x is neither an interior nor an exterior point, it is called a boundary point.
The set of all those points is denoted U.

If x is either an interior or boundary point, its called a limit point of U.
The set of all limit points of U is a closed set called the closure of U, and is
denoted by cl(U). Note: cl(U) = int(u) U.
Motion Planning
Michael Beetz
Summer Term 2012

37
Sampling: Deterministic Alternative: The van der Corput sequence
Halton sequence: For each coordinate, use relatively prime bases. More uniform
than random (which needs O((lg n)1/d ) as many samples to produce the same
expected dispersion.
Motion Planning
Michael Beetz
Summer Term 2012

38
Sampling: Dispersion
Let P be a finite set of points in metric space (X,).

The dispersion of P is:
(P) = sup xX {minpP {(x, p)}}
In a bounded space, a dense sequence drives the dispersion to zero.
Motion Planning
Michael Beetz
Summer Term 2012

39
Best possible Dispersion: Sukharev theorem

For any set P of k samples in [0, 1]d :
(P)
1
1
2bk d c
in which is the L dispersion.

The best placement of k points:
Think: points per axis for any sample set.

Holding the dispersion fixed requires exponentially many points in
dimension.
Motion Planning
Michael Beetz
Summer Term 2012

40
Sample-Based Planning: Framework
Given a single query: qI , qG Cfree .

1. Initialization: Form G(V , E ) with vertices qI , qG and no edges.
2. Vertex Selection Method (VSM): Choose a vertex qcur V for
expansion.
3. Local Planning Method (LPM): For some qnew Cfree , attempt to
construct a path s : [0, 1] Cfree so that (0) = qcur and (1) = qnew .
4. Insert an Edge in the Graph: Insert s into E, as an edge from qcur to
qnew . If qnew is not already in V, it is inserted.
5. Check for a Solution:: Determine whether G encodes a solution path.
6. Return to Step 2: Iterate unless a solution has beed found or the
algorithm reports failure.
Motion Planning
Michael Beetz
Summer Term 2012

41
Sample-Based Planning: Random Tree vs. RRT
Rather than picking a vertex by chance, pick a configuration at random!
Motion Planning
Michael Beetz
Summer Term 2012

42
Sample-Based Planning: RRT

Simpe Rapidly Exploring Random Tree:
In each step, select a new configuration at random.
Extend the nearest vertex (using the metric!) to this random point:
If there is an obstacle, then stop short:
If the closest point is an edge, its better to extend from there.
Motion Planning
Michael Beetz
Summer Term 2012

43
Rapidly Exploring Random Trees (RRT) are one variant of Rapidly Exploring
Dense Trees (RDT), others may use a non-random dense sequence.
Motion Planning
Michael Beetz
Summer Term 2012

44
Motion Planning
Michael Beetz
Summer Term 2012

45
Sample-Based Planning: RRT: bi-directional search
Both trees are extended for each drawn random configuration.

The trees are alternated, and the other currently second tree gets extended
using the configuration that was added to the first.
If connected, the solution is found.
Motion Planning
Michael Beetz
Summer Term 2012

46
Grow RRT in the usual way

When a new vertex xnew is added, try to connect to other RRT vertices
within radius .
Among all paths to the root from xnew , add a new RRT edge only for the
shortest one.
If possible to reduce cost for other vertices within radius by connecting to
xnew , then disconnect them from their parents and connect them through
xnew .
The radius is prescribed through careful percolation theory analysis
(related to dispersion)
RRT yield asymptotically optimal paths through Cfree .
Motion Planning
Michael Beetz
Summer Term 2012

47
Motion Planning
Michael Beetz
Summer Term 2012

48
Sample-Based Planning: Bugtraps
Motion Planning
Michael Beetz
Summer Term 2012

49
Sample-Based Planning: Probabilistic Roadmaps (PRM)

If multiple queries are expected for the same Cfree , building a roadmap may pay
off.
Motion Planning
Michael Beetz
Summer Term 2012

50
Sample-Based Planning: PRM - Variants
Connection Rules:
Nearest K: The K closest points to (i) are considered.
Component K: Try to obtain up to K closest points of each connected
component of G.
Radius: Take all points within a ball of radius r centered at (i).
Visibility: Try connecting (i) to all vertices in G.
Motion Planning
Michael Beetz
Summer Term 2012

51
Planning under Differential Constraints
Due to robot kinematics and dynamics, most systems are locally constrained in
addition to global obstacles.
Let q represent the C-space velocity.
In ordinary planning, any direction is allowed and the magnitude does not
matter.
Thus we could say
q = u
and u Rn may be any velocity vector.
Motion Planning
Michael Beetz
Summer Term 2012

52
Planning under Differential Constraints
More generally, a control system (or state transition equation) constrains the
velocity:
q = f (q, u)
and u belongs to some set U (usually bounded).
A function u : T U is applied over a time interval T = [0, tf ] and the
configuration q(t) at time t is given by
Z
q(t) = q(0) +
f (q(t 0 ), u(t 0 ))dt 0 .
in which q(0) is the initial configuration.
Motion Planning
Michael Beetz
Summer Term 2012

53
Planning under Differential Constraints: Dubins Car

This car drives only forward:
C = R2 S 1 .
Let u = (us , u ) and U = [0, 1] [max , max ].
Control system of the form q = f (q, u) :
x = cos
y = sin
us
=
tan u
L
Motion Planning
Michael Beetz
Summer Term 2012

54
Planning under Differential Constraints: Dubins Car

Stepping forward in the Dubins car:
Motion Planning
Michael Beetz
Summer Term 2012

55
Planning under Differential Constraints: Boundary Value Problems
Motion Planning
Michael Beetz
Summer Term 2012

56
Planning under Differential Constraints: RRT

For a RRT, just replace the straight line connection with a local planner:
Problems: need good metrics and primitives!

Motion Planning
Michael Beetz
Summer Term 2012

57
Manipulation Planning
In most forms of motion planning, the robot is not allowed to touch objects!
In manipulation planning, two phases are distinguished:
Transit mode: The robot moves towards a part.
Transfer mode: The robot carries a part.
Transitions between these phases require specific grasping or stability

requirements, otherwise the part would fall out of the gripper when lifting or
move/topple after it is released.
Motion Planning
Michael Beetz
Summer Term 2012

58
Planning in Mobile Manipulation - Our Take
Motion Planning
Michael Beetz
Summer Term 2012

59
Planning in Mobile Manipulation - Our Take
We distinguish further:
Base Transit: The base moves to bring the object into reachability of the
arms.
Pregrasp: The gripper is brought into a pose from where it can go straight.
Grasping: The gripper goes straight to the grapsing pose and closes.
Carry Pose: The object is moved into a specific place close to the robot to
facilitate navigation.
Base Transit: The base moves to bring the goal into reachability.
Placing: The object is placed at the goal pose.
Arm Retract: Similar to the Pregrasp, we move the gripper out in a
straight way and then park the arm.
Motion Planning
Michael Beetz
Summer Term 2012

60
Practical Considerations: Self Filtering

Our sensors do not discriminate between the robot and the environment, so we
have to filter out parts of the robot from the sensor data before using it for
collision checking purposes.
Occluded obstacles may not be seen, so we need some persistancy in our

environment model!
Motion Planning
Michael Beetz
Summer Term 2012

61
Practical Considerations: Representation
Octomaps are used to represent the environment, as they are they have a small
memory footprint and allow for hierarchical queries.
Often, we want to encode free space, obstace space and unknown space.
Therefore, 2 bits per node are needed.
Motion Planning
Michael Beetz
Summer Term 2012

62
Practical Considerations: Attaching Object Models
As we use planning for manipulation, we want to be able to plan for trajectories

while holding objects.
Whenever an object is carried, its model gets attached to the robot model.
Motion Planning
Michael Beetz
Summer Term 2012

63
Mobile Manipulation - Inverse Reachability
Where can i stand when i want to reach pose P with my gripper?
The precalculated inverse reachability map quickly answers this question.
Motion Planning
Michael Beetz
Summer Term 2012

64
Mobile Manipulation - Inverse Reachability
Robot pose samples are drawn until a promising one is found.

Static collision: When placed there, would the robot, the arms collide with
the environment?
Arm planning: Can we find a trajectory via planning or can we execute the
given trajectory without collision?

Note: We sacrifice completeness.
Motion Planning
Michael Beetz
Summer Term 2012

65
Mobile Manipulation - 3D Navigation
The robot is sliced vertically and convex hulls for each slice are used to speed up
collision checks.
We can then use RRT or any other planner as usual.
Motion Planning
Michael Beetz
Summer Term 2012

66
Exam Question
For an infinite sample sequence : N X , let k denote the first k samples.

Find a metric space X Rn and so that:
1. The dispersion of k is for all k.
2. The dispersion of is 0.
Hint: dont make it too complicated!
Motion Planning
Michael Beetz
Summer Term 2012

67

Motion Planning

Hochgeladen von

Dokumentinformationen

Copyright

Verfügbare Formate

Dieses Dokument teilen

Dokument teilen oder einbetten

Freigabeoptionen

Stufen Sie dieses Dokument als nützlich ein?

Sind diese Inhalte unangemessen?

Copyright:

Verfügbare Formate

Motion Planning

Hochgeladen von

Copyright:

Verfügbare Formate

Department of Informatics

Intelligent Autonomous Systems

Technische Universitt Mnchen

Technical Cognitive Systems

Summer Term 2012

Technische Universitt Mnchen

Technical Cognitive Systems

Technische Universitt Mnchen

Technical Cognitive Systems

Technische Universitt Mnchen

Technical Cognitive Systems

Technische Universitt Mnchen

Technical Cognitive Systems

Technische Universitt Mnchen

The basic Path Planning Problem

Technical Cognitive Systems

Technische Universitt Mnchen

The robot and obstacles live in a world or workspace W.

Technical Cognitive Systems

Technische Universitt Mnchen

Geometric Models: Linear Primitives

Technical Cognitive Systems

Technische Universitt Mnchen

Geometric Models: Semi-Algebraic Sets

Hi = {(x, y , z) W|fi (x, y , z) < 0},

Technical Cognitive Systems

Technische Universitt Mnchen

Geometric Models: Polygon Soups and Pointclouds

Technical Cognitive Systems

Technische Universitt Mnchen

Configuration Space: Example

All configurations of our robot lie on a manifold, here a torus.

Technical Cognitive Systems

Technische Universitt Mnchen

Excursion: Flat Manifolds: Cylinder

Let (x, y ) denote a point on the manifold.

Technical Cognitive Systems

Technische Universitt Mnchen

Excursion: Flat Moebius Band

Change the equivalence relation :

Technical Cognitive Systems

Technische Universitt Mnchen

Excursion: Other Flat Manifolds

Technical Cognitive Systems

Technische Universitt Mnchen

Configuration Space: Obstacle Region

Given world W, a closed obstacle O W, a closed robot A, and configuration

Technical Cognitive Systems

Technische Universitt Mnchen

Obstacle Region: Example

Technical Cognitive Systems

Technische Universitt Mnchen

Obstacle Region: Minkowski Sum

Also known as Convolution.

Technical Cognitive Systems

Technische Universitt Mnchen

Obstacle Region: Polygonal Obstacles

Technical Cognitive Systems

Technische Universitt Mnchen

Obstacle Region: Polygonal Obstacles

Technical Cognitive Systems

Technische Universitt Mnchen

Configuration Space: Planning Problem Revisited

Compute a path : [0, 1] Cfree so that (0) = qI and (1) = qG .