This action might not be possible to undo. Are you sure you want to continue?
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: 121:30, 43102)
1 23Jul Introduction
2 30Jul
Representing Position & Orientation & State
(Frames, Transformation Matrices & Affine
Transformations)
3 6Aug Robot Kinematics and Dynamics
4 13Aug Robot Dynamics & Control
5 20Aug [Trajectory Generation+Control]
Obstacle Avoidance
& Motion Planning
6 27Aug Sensors, Measurement and Perception
7 3Sep Localization and Navigation
8 10Sep Statespace modelling & Controller Design
9 17Sep Visionbased control
24Sep Study break
10 1Oct Public Holiday
11 8Oct Robot Machine Learning
12 15Oct Guest Lecture
13 22Oct Wrapup & 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
semifree path
20 August 2012  METR 4202: Robotics 18
Slide from Latombe, CS326A
10
MotionPlanning Framework
Continuous representation
Discretization
Graph searching
(blind, bestfirst, A*)
20 August 2012  METR 4202: Robotics 19
Slide from Latombe, CS326A
PathPlanning Approaches
• Roadmap
Represent the connectivity of the free space by a network
of 1D 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. CellDecomposition 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
nonoverlapping cells whose union is contained in F
Examples: quadtree, octree, 2ntree
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 Rapidlyexploring 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 2D 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
ThreeDimensional Space
Computing the shortest collisionfree path in a
polyhedral space is NPhard
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 bestfirst search)
• Place regular grid G over space
• Search G using bestfirst 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
realtime 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
LocalMinimum Issue
• Perform bestfirst search (possibility of
combining with approximate cell decomposition)
• Alternate descents and random walks
• Use localminimumfree 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 CSpace
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 2D Workspace
20 August 2012  METR 4202: Robotics 46
Slide from Latombe, CS326A
24
Rigid Robot Translating and Rotating in 2D
20 August 2012  METR 4202: Robotics 47
Slide from Latombe, CS326A
Cool Robotics Share
METR 4202: Robotics 20 August 2012  48
This action might not be possible to undo. Are you sure you want to continue?