P. 1
Trajectory Generation L5.MotionPlanning

Trajectory Generation L5.MotionPlanning

|Views: 3|Likes:
Published by Ivan Avramov

More info:

Published by: Ivan Avramov on Apr 18, 2013
Copyright:Attribution Non-commercial

Availability:

Read on Scribd mobile: iPhone, iPad and Android.
download as PDF, TXT or read online from Scribd
See more
See less

04/18/2013

pdf

text

original

1

Trajectory Generation
& Motion Planning
METR 4202: Advanced Control & Robotics
Drs Surya Singh, Paul Pounds, and Hanna Kurniawati
Lecture # 5 August 20, 2012
(with Motion Planning Figures from Latombe, CS326A)

metr4202@itee.uq.edu.au
http://itee.uq.edu.au/~metr4202/
© 2012 School of Information Technology and Electrical Engineering at the University of Queensland
RoboticsCourseWare Contributor
TexPoint fonts used in EMF.
Read the TexPoint manual before you delete this box.: AAAAA
Schedule
METR 4202: Robotics 20 August 2012 - 2
Week Date Lecture (M: 12-1:30, 43-102)
1 23-Jul Introduction
2 30-Jul
Representing Position & Orientation & State
(Frames, Transformation Matrices & Affine
Transformations)
3 6-Aug Robot Kinematics and Dynamics
4 13-Aug Robot Dynamics & Control
5 20-Aug [Trajectory Generation+Control]
Obstacle Avoidance
& Motion Planning
6 27-Aug Sensors, Measurement and Perception
7 3-Sep Localization and Navigation
8 10-Sep State-space modelling & Controller Design
9 17-Sep Vision-based control
24-Sep Study break
10 1-Oct Public Holiday
11 8-Oct Robot Machine Learning
12 15-Oct Guest Lecture
13 22-Oct Wrap-up & Course Review
2
Revisiting The Jacobian
• I told you (Lec 4, Slide 19):







• True, but we can be more “explicit”

METR 4202: Robotics 20 August 2012 - 3
Jacobian: Explicit Form
• For a serial chain (robot): The velocity of a link with
respect to the proceeding link is dependent on the type of
link that connects them
• If the joint is prismatic (ϵ=1), then
• If the joint is revolute (ϵ=0), then



• Combining them (with v=(Δx, Δθ))


METR 4202: Robotics 20 August 2012 - 4
dz
i dt
= v
ˆ
(in the direction)
d
k
dt
u
e =
( ) ( ) 1
1
N
i
i i i i i
i
v v c c e
÷
=
= + ×
¿
p
( ) ( ) ( ) ( )
1 1
N N
i i i i i
i i
e c c u
= =
= =
¿ ¿
θ z
v
v J ÷ = q J
e
= ω q
v
J
J
J
e
(
=
(
¸ ¸
3
Jacobian: Explicit Form [2]
• The overall Jacobian takes the form


• The Jacobian for a particular frame (F) can be expressed:



Where: &
METR 4202: Robotics 20 August 2012 - 5
1
1 1 1
P P
n
n
x x
q q J
z z c c
c c (
(
c c =
(
(
¸ ¸
1
1 1 1
F F
P P
F
n
F F
n
x x
q q J
z z c c
( c c
(
c c =
(
(
¸ ¸
F F i
i i i
R = z z ( ) 0 0 1
i
i
= z
Correction! Dynamics – Langrangian Mechanics
• Alternatively, we can use Langrangian
Mechanics to compute the dynamics of a
manipulator (or other robotic system)

• The Langrangian is defined as the difference
between the Kinetic and Potential energy in
the system

• Using this formulation and the concept of
virtual work we can find the forces and
torques acting on the system.

• This may seem more involved but is often
easier to formulate for complex systems
20 August 2012 - METR 4202: Robotics 6
4
Dynamics – Langrangian Mechanics [2]
METR 4202: Robotics 20 August 2012 - 7
, : Generalized Velocities, : Mass Matrix L K P M = ÷ θ
1
N
i
i
d K K P
dt
t
=
c c c | |
= = ÷ +
|
\ .
c c c
¿
τ
θ θ θ
( ) ( ) ( ) , M u ¬ = + + τ θ v θ θ g θ
Dynamics – Langrangian Mechanics [3]
• The Mass Matrix: Determining via the Jacobian!








! M is symmetric, positive definite
METR 4202: Robotics 20 August 2012 - 8
, 0
T
ij ji
m m M = > θ θ
5
Generalized Coordinates
• A significant feature of the Lagrangian Formulation is
that any convenient coordinates can be used to derive the
system.
• Go from Joint  Generalized
– Define p:

Thus: the kinetic energy and gravity terms become

where:
METR 4202: Robotics 20 August 2012 - 9
| | | |
1 1 n n
q q p p = ÷ = q p
d d = p J q
* 1
2
T
KE = p H p
( )
* 1 1
T
÷ ÷
= H J HJ
( )
* 1
T
÷
= G J G
Motivating Example:
Remotely Driven 2DOF Manipulator







• Introduce Q
1

1

2
and Q
2

2
• Derivation posted to website

METR 4202: Robotics 20 August 2012 - 10
Motor 1
θ
1
θ
2
τ
1
τ
2
Motor 2
Lin
k 2
L
i
n
k

1
Graphic based on Asada & Slotine p. 112
6
Inverse Dynamics
• Forward dynamics governs the dynamic responses of a manipulator arm to the
input torques generated by the actuators.
• The inverse problem:
– Going from joint angles
to torques
– Inputs are desired
trajectories described
as functions of time

– Outputs are joint torques
to be applied at each instance

• Computation “big” (6DOF arm: 66,271 multiplications),
but not scary (4.5 ms on PDP11/45)

METR 4202: Robotics 20 August 2012 - 11
| | ( ) ( ) ( )
1 1 2 3 n
q q t t t u u u = ÷ (
¸ ¸
q
| |
1 n
t t = τ
Graphic from Asada & Slotine p. 119
Trajectory Generation & Planning
METR 4202: Robotics 20 August 2012 - 12
7
Trajectory Generation
• The goal is to get from an initial position {i} to a final
position {f} via a path points {p}
METR 4202: Robotics 20 August 2012 - 13
{i}
{f}
{p}
Joint Space
Consider only the joint positions
as a function of time

• + Since we control the joints, this is
more direct
• -- If we want to follow a particular
trajectory, not easy
– at best lots of intermediate points
– No guarantee that you can solve
the Inverse Kinematics for all
path points


METR 4202: Robotics 20 August 2012 - 14
8
Cartesian Workspace
Consider the Cartesian positions
as a function of time

• + Can track shapes exactly
• -- We need to solve the inverse
kinematics and dynamics


METR 4202: Robotics 20 August 2012 - 15
Time
x
Polynomial Trajectories
• Straight line Trajectories





• Simpler
• Polynomial Trajectories





• Parabolic blends are
smoother
• Use “pseudo via points”
METR 4202: Robotics 20 August 2012 - 16
A
B
C
A
B
C
9
Problem
free space
s
g
free path
20 August 2012 - METR 4202: Robotics 17
Slide from Latombe, CS326A
Problem
semi-free path
20 August 2012 - METR 4202: Robotics 18
Slide from Latombe, CS326A
10
Motion-Planning Framework
Continuous representation
Discretization
Graph searching
(blind, best-first, A*)
20 August 2012 - METR 4202: Robotics 19
Slide from Latombe, CS326A
Path-Planning Approaches
• Roadmap
Represent the connectivity of the free space by a network
of 1-D curves
• Cell decomposition
Decompose the free space into simple cells and represent
the connectivity of the free space by the adjacency graph
of these cells
• Potential field
Define a function over the free space that has a global
minimum at the goal configuration and follow its steepest
descent
20 August 2012 - METR 4202: Robotics 20
Slide from Latombe, CS326A
11
I. Rotational Sweep
20 August 2012 - METR 4202: Robotics 21
Slide from Latombe, CS326A
Rotational Sweep
20 August 2012 - METR 4202: Robotics 22
Slide from Latombe, CS326A
12
Rotational Sweep
20 August 2012 - METR 4202: Robotics 23
Slide from Latombe, CS326A
Rotational Sweep
20 August 2012 - METR 4202: Robotics 24
Slide from Latombe, CS326A
13
Rotational Sweep
20 August 2012 - METR 4202: Robotics 25
Slide from Latombe, CS326A
II. Cell-Decomposition Methods
Two classes of methods:
• Exact cell decomposition
– The free space F is represented by a collection of non-
overlapping cells whose union is exactly F

– Example: trapezoidal decomposition

• Approximate cell decomposition
– F is represented by a collection of
non-overlapping cells whose union is contained in F
Examples: quadtree, octree, 2n-tree

20 August 2012 - METR 4202: Robotics 26
14
Trapezoidal decomposition
20 August 2012 - METR 4202: Robotics 27
Slide from Latombe, CS326A
Planar sweep  O(n log n) time, O(n) space
Trapezoidal decomposition
20 August 2012 - METR 4202: Robotics 28
Slide from Latombe, CS326A
15
Trapezoidal decomposition
20 August 2012 - METR 4202: Robotics 29
Slide from Latombe, CS326A
Trapezoidal decomposition
20 August 2012 - METR 4202: Robotics 30
Slide from Latombe, CS326A
16
Trapezoidal decomposition
20 August 2012 - METR 4202: Robotics 31
Slide from Latombe, CS326A
III. Roadmap Methods
• Visibility graph
• Voronoi diagram
• Silhouette
First complete general method that applies to spaces of
any dimension and is singly exponential in # of
dimensions [Canny, 87]
• Probabilistic roadmaps (PRMS)
and Rapidly-exploring Randomized Trees (RRTs)
20 August 2012 - METR 4202: Robotics 32
Slide from Latombe, CS326A
17
Roadmap Methods
• Visibility graph
Introduced in the Shakey project at SRI in the late 60s.
Can produce shortest paths in 2-D configuration spaces
g
s
20 August 2012 - METR 4202: Robotics 33
Slide from Latombe, CS326A
Roadmap Methods
• Voronoi diagram
Introduced by
Computational
Geometry researchers.
Generate paths that
maximizes clearance.

O(n log n) time
O(n) space
20 August 2012 - METR 4202: Robotics 34
Slide from Latombe, CS326A
18
II.Visibility Graph
tangent segments
 Eliminate concave obstacle vertices
can’t be shortest path
20 August 2012 - METR 4202: Robotics 35
Slide from Latombe, CS326A
Generalized (Reduced) -- Visibility Graph
tangency point
20 August 2012 - METR 4202: Robotics 36
Slide from Latombe, CS326A
19
Three-Dimensional Space
Computing the shortest collision-free path in a
polyhedral space is NP-hard
Shortest path passes
through none of the
vertices
locally shortest
path homotopic
to globally shortest
path
20 August 2012 - METR 4202: Robotics 37
Slide from Latombe, CS326A
Sketch of Grid Algorithm (with best-first search)
• Place regular grid G over space
• Search G using best-first search algorithm with potential
as heuristic function
20 August 2012 - METR 4202: Robotics 38
Slide from Latombe, CS326A
20
Simple Algorithm (for Visibility Graphs)
• Install all obstacles vertices in VG, plus the start and goal
positions
• For every pair of nodes u, v in VG
If segment(u,v) is an obstacle edge then
insert (u,v) into VG
else
for every obstacle edge e
if segment(u,v) intersects e
then go up to segment
insert (u,v) into VG
• Search VG using A*
20 August 2012 - METR 4202: Robotics 39
Slide based on Latombe, CS326A
IV. Potential Field Methods
• Approach initially proposed for
real-time collision avoidance [Khatib, 86]

Goal
G
o
a
l F
o
rc
e
O
b
s
t
a
c
l
e

F
o
r
c
e
Motion
Robot
20 August 2012 - METR 4202: Robotics 40
Slide based on Latombe, CS326A
21
Attractive and Repulsive fields
20 August 2012 - METR 4202: Robotics 41
Slide from Latombe, CS326A
Local-Minimum Issue
• Perform best-first search (possibility of
combining with approximate cell decomposition)
• Alternate descents and random walks
• Use local-minimum-free potential (navigation function)
20 August 2012 - METR 4202: Robotics 42
Slide from Latombe, CS326A
22
Configuration Space
• A robot configuration is a specification of the positions of all robot
points relative to a fixed coordinate system
• Usually a configuration is expressed as a “vector” of
position/orientation parameters

METR 4202: Robotics 20 August 2012 - 43
Slide from Latombe, CS326A
Motion Planning in C-Space

METR 4202: Robotics 20 August 2012 - 44
q=(q
1
,…,q
n
)
q
1

q
2

q
3

q
n

Slide from Latombe, CS326A
23
Configuration Space of a Robot
• Space of all its possible configurations
• But the topology of this space is usually not that of a
Cartesian space
C = S
1
x S
1

20 August 2012 - METR 4202: Robotics 45
Slide from Latombe, CS326A
Disc Robot in 2-D Workspace
20 August 2012 - METR 4202: Robotics 46
Slide from Latombe, CS326A
24
Rigid Robot Translating and Rotating in 2-D
20 August 2012 - METR 4202: Robotics 47
Slide from Latombe, CS326A
Cool Robotics Share
METR 4202: Robotics 20 August 2012 - 48

You're Reading a Free Preview

Download
scribd
/*********** DO NOT ALTER ANYTHING BELOW THIS LINE ! ************/ var s_code=s.t();if(s_code)document.write(s_code)//-->