Trajectory Planning

Trajectory Planning

Goal: to generate the reference inputs to the

Goal: to generate the reference inputs to the
motion control system which ensures that the
motion control system which ensures that the
manipulator executes the planned trajectory
manipulator executes the planned trajectory
Motion control
system
Robot
Trajectory
planning system
torques
Position, velocity, acceleration

Path and Trajectory

Path: the locus of points in the joint space or in

Path: the locus of points in the joint space or in
the operational space
the operational space

Trajectory: a path on which a time law is

Trajectory: a path on which a time law is
specified in terms of velocities and/or
specified in terms of velocities and/or
accelerations
accelerations
t

Path and Trajectory
Trajectory
planning algorithm
Path description
path constraints(obstacles)
constraints imposed by
robot dynamics (smooth)
(limits, not modeled
resonant modes)
Joint (end-effector)
trajectories in terms of
position, velocity and
acceleration

Path and Trajectory

Specification of geometric path

Specification of geometric path

Extremal points, possile intermediate Extremal points, possile intermediate

points,geometric primitives interpolating the points points,geometric primitives interpolating the points

Specification of motion time law

Specification of motion time law

Total trajectory time, maximum velocity and Total trajectory time, maximum velocity and
acceleration, velocity and acceleration at points of acceleration, velocity and acceleration at points of
interests interests

Joint Space Trajectory
Inverse
inematics
algorithm
Trajectory
parameters in
operation space
Joint (end-effector)
trajectories in terms of
position, velocity and
acceleration
Trajectory
parameters in
joint space
Trajectory
planning
algorithm
Initial and final
end-effector
location, traveling
time, etc!

Joint Space Trajectory

!e"uirements for joint space trajectory

!e"uirements for joint space trajectory
planning algorithm
planning algorithm

The generated trajectory e easy to compute The generated trajectory e easy to compute

Position and velocity #acceleration$ e continuous Position and velocity #acceleration$ e continuous
function of time function of time

%ndesirale effects e minimi&ed %ndesirale effects e minimi&ed

Point'to'point motion
Point'to'point motion

(oving from an initial to a final joint configuration in (oving from an initial to a final joint configuration in
a given time t a given time t
f f

Point-to-point Motion
)igure *'+ from )u, Gon&ale& and ,ee
#t
f
$
#t
+
$
#t
-
$
#t
.
$
/,ift'off0
/Set'down0
)inal
1nitial

Polynomial interpolation
Polynomial interpolation

Example: initial and final position and velocity e

Example: initial and final position and velocity e
given2
given2
Point-to-point Motion
" #
#
#
) ( a t a t a t a t q
n n
n n
+ + + +

Point-to-point Motion

Point-to-point Motion

Example: initial and final acceleration also e

Example: initial and final acceleration also e
given2
given2

Six constraints #initial and final position, velocity and Six constraints #initial and final position, velocity and
acceleration acceleration

3rder at least five 3rder at least five

Point-to-point Motion

Trape&oidal velocity profile

Trape&oidal velocity profile

4irectly verifying whether the velocity and

4irectly verifying whether the velocity and
acceleration violate the mechanical limits
acceleration violate the mechanical limits
Point-to-point Motion

5rea enclosed y the velocity profile

5rea enclosed y the velocity profile


given acceleration
given acceleration
Point-to-point Motion

Point-to-point Motion

Point-to-point Motion

Given the following conditions:

Given the following conditions:

initial and final position are given initial and final position are given

initial and final velocity are set to &ero initial and final velocity are set to &ero

(aximum velocity and acceleration are given (aximum velocity and acceleration are given

6hat7s the
6hat7s the
minimum
minimum
traveling time8
traveling time8

Path Motion

4isadvantages of single high order polynomial

4isadvantages of single high order polynomial

5 suitale numer of low order polynomials

5 suitale numer of low order polynomials

Operation Space Trajectory

9ot easy to predict end'effector motion due to

9ot easy to predict end'effector motion due to
:inematics nonlinearity
:inematics nonlinearity

Path motion planning similar to joint space

Path motion planning similar to joint space

4ifferent method if the end'effector motion has to

4ifferent method if the end'effector motion has to
follow a prescried trajectory of motion such as
follow a prescried trajectory of motion such as
line, circle, etc2
line, circle, etc2

Two-link Planar Arm
Parameters

;ams

(otion programming historically

(otion programming historically
associated with mechanical cams
associated with mechanical cams

;onstant speed rotation of camshaft

;onstant speed rotation of camshaft
converted to variale linear
converted to variale linear
displacement of valve #or other device
displacement of valve #or other device
attached to cam follower$
attached to cam follower$
<
;amshafts in auto engines #all * stro:es$
;amshafts in auto engines #all * stro:es$
<
Sewing machine #older mechanical style$
Sewing machine #older mechanical style$

Pictures of ;ams
http://www2 http://www2howstuffwor:s howstuffwor:s2com/camshaft.2 2com/camshaft.2htm htm
1ndustrial 1ndustrial
;ar Engines ;ar Engines

;am (otion Profiles ' 4!4

4well < !ise < 4well

4well < !ise < 4well
<
initial period of no motion #/dwell0$
initial period of no motion #/dwell0$
<
/
/
rise0 to a maximum displacement
rise0 to a maximum displacement
<
final period of no motion #/dwell0$
final period of no motion #/dwell0$
/dwell0
/rise0
/dwell0
Time, t
4isplacement, s
s=s
max
, v=-, a=-
s=-, v=-, a=-

;am (otion Profiles ' 4!!4

4well < !ise < !eturn < 4well 4well < !ise < !eturn < 4well
<
initial period of no motion #/dwell0$ initial period of no motion #/dwell0$
<
/ /rise0 to a maximum displacement rise0 to a maximum displacement
<
1mmediately /return0 to origin 1mmediately /return0 to origin
<
final period of no motion #/dwell0$ final period of no motion #/dwell0$
/dwell0
/rise0
/dwell0
Time, t
4isplacement, s s=s
max
, v=-, a-
s=-, v=-, a=-
/return0
s=-, v=-, a=-

;am (otion Profiles ' !!

!ise < !eturn

!ise < !eturn
<
/ /rise0 to a maximum displacement rise0 to a maximum displacement
<
1mmediately /return0 to origin 1mmediately /return0 to origin
<
9o /dwell0 < do same thing over again 9o /dwell0 < do same thing over again
/rise0
Time, t
4isplacement, s s=s
max
, v=-, a-
/return0
s=-, v=-, a-
s=-, v=-, a-

5ccel2'>el2'4isp2 ?.
Time, t
5cceleration, 5
@ero order, 5 = constant
Time, t
>elocity, >
Time, t
4isplacement, S
T
T
T
)irst order, >=:
.
t
Second order, S=:
+
t
+

5ccel2'>el2'4isp2 ?.a
Time, t
5cceleration, 5
Time, t
>elocity, >
Time, t
4isplacement, S
T
T
T
AT V
this area
e"uals this value
$
$
#
AT S
this area
e"uals this value

5ccel2'>el2'4isp2 ?.
Time, sec
5cceleration, 5
>elocity, >
4isplacement, S
T
- -2. -2+ -2A
T
+B m/s
+
>.
>+
>A
S.
S+
SA
)ind numerical values
for >., >+, and >A
)ind numerical values
for S., S+, and SA
Suitale for a /rise0

General ;urve Shape: y=Cx
n
D
E
n=.
n=+
n=A
n=*
n=B
#
#
#
"
#
+

+

n
Kx
dx Kx Area
n
x
n
5rea under the curve y=Cx
n

etween x=- and x=x
.
is
9ote that y
.
=Cx
.
n
, so
# #
# # # #
+

n
x y
n
x Kx
Area
n

5ccel2'>el2'4isp2 ?+
Time, sec
5cceleration, 5
>elocity, >
4isplacement, S
T
- -2. -2+ -2A
T
+B m/s
+
>.
>+
>A
S.
S+
SA
)ind numerical values
for >., >+, and >A
)ind numerical values
for S., S+, and SA
Suitale for a /dwell ' rise0

5ccel2'>el2'4isp2 ?A
Time, sec
5cceleration, 5
>elocity, >
4isplacement, S
T
- -2.B -2A
T
+B m/s
+
>.
>+
S.
S+
)ind numerical values
for >. and >+
)ind numerical values
for S. and S+
Suitale for a /dwell ' rise0

5ccel2'>el2'4isp2 ?*
Time, sec
5cceleration, 5
>elocity, >
4isplacement, S
T
- -2. -2+ -2A
T
+B m/s
+
>.
>+
>A
S.
S+
SA
)ind numerical values
for >., >+, and >A
)ind numerical values
for S., S+, and SA
Suitale for a /dwell ' rise0

Time,
sec
5
>
S
T
T
+B m/s
+
>.
>+
>A
S.
S+
SA
)ind numerical values
for >* and >B
)ind numerical values
for S*, SB, and SF
Suitale for a /rise'return0
'+B m/s
+
-2. -2+ -2A
-2* -2B -2F
>B
>*
S*
SB
SF

5nalytical Solution

Solve the previous prolem analytically:

Solve the previous prolem analytically:

'

<
+

sec % ! " & ! " ' $(

sec & ! " " ' $(
) (
$
$
t for s m
t for s m
t a
" ) " ( , ) ( ) (
" ) " ( , ) ( ) (
"
"

s dt t v t s
v dt t a t v
t
t
Gint < solve first parts #for tH-2A sec$, find oundary conditions for +
nd
parts

Solve 9umerically

%se Excel and trape&oidal integration

%se Excel and trape&oidal integration
time 5cc >el 4isp
- +B - -
-2-. +B -2+B -2--.+B
-2-+ +B -2B -2--B
-2-A +B -2IB -2-..+B
-2-* +B . -2-+
sec) "& ! " ") ! " ( *
sec
) "& ! " ( ) ") ! " (
$
#
sec
) "& ! " (
sec
) ") ! " (
$

,
_

+ +
m
a a
m
v
m
v
sec) "& ! " ") ! " ( *
sec
) "& ! " ( ) ") ! " (
$
#
) "& ! " ( ) ") ! " (

,
_

+ +
m
v v m s m s

5 9umerical Gimmic:

%se this gimmic: to improve accuracy when

%se this gimmic: to improve accuracy when
you have arupt changes in acceleration
you have arupt changes in acceleration
time 5cc >el 4isp
- +B - -
-2-. +B -2+B -2--.+B
-2-+ +B -2B -2--B
-2-A +B -2IB -2-..+B
-2-* +B . -2-+
-2+J +B I -2KJ
-2+K +B I2+B .2-B.+B
-2+KKKKKKK +B I2B---- .2.+B
-2A------. '+B I2B---- .2.+B
-2A. '+B I2+B--- .2.KJIB
-2A+ '+B I2----- .2+I
/doule up0 at any
sharp transitions

(otion Programming ?+
!oot Loint (otions
!oot Loint (otions

Typical !oot (otion
)igure *'+ from )u, Gon&ale& and ,ee
#t
f
$
#t
+
$
#t
-
$
#t
.
$
/,ift'off0
/Set'down0
)inal
1nitial

Position ;onstraints

1nitial position,
1nitial position,

. .
<
initial velocity and acceleration #normally=-$ initial velocity and acceleration #normally=-$

,ift off position,

,ift off position,

+ +

<
velocity and acceleration must match here velocity and acceleration must match here

Set'down position,
Set'down position,

A A

<
velocity and acceleration must match here velocity and acceleration must match here

)inal position,
)inal position,

f f
<
final velocity and acceleration #normally=-$ final velocity and acceleration #normally=-$

Typical Solution

/
/
*'A'*0 trajectory
*'A'*0 trajectory
M
* *
th th
order polynomial from initial to lift'off order polynomial from initial to lift'off
M
A A
rd rd
order polynomial from lift'off to set'down order polynomial from lift'off to set'down
M
* *
th th
order polynomial from set'down to final order polynomial from set'down to final

/
/
A'B'A0 trajectory
A'B'A0 trajectory
M
same as aove ut A same as aove ut A
rd rd
and B and B
th th
order poly order poly

B'cuic trajectory
B'cuic trajectory
M
;uic splines used for B segments ;uic splines used for B segments

lift'off to set'down split into A segments lift'off to set'down split into A segments

/*'A'*0 Trajectory

.
.
st st
segment:
segment:

+
+
nd nd
segment:
segment:

A
A
rd rd
segment:
segment:

.* un:nowns < need .* e"uationsN

.* un:nowns < need .* e"uationsN
#" ##
$
#$
&
#&
)
#) #
) ( a t a t a t a t a t h + + + +
&" &#
$
&$
&
&&
)
&) &
) ( a t a t a t a t a t h + + + +
$" $#
$
$$
&
$& $
) ( a t a t a t a t h + + +

Ooundary ;onditions ?.' ?A
M .2
.2
1nitial position,
1nitial position,

- -
=
=

#t
#t
- -
$ #set
$ #set
t
t
- -
=-$
=-$

+2
+2
1nitial velocity =
1nitial velocity =

- -
#typically
#typically
-$
-$
+2 +2
A2
A2
1nitial acceleration =
1nitial acceleration =

- -

#typically -$
#typically -$
## " #
) " ( a h

#$ " #
$ ) " ( a h

#" " #
) " ( a h

Ooundary ;onditions ?*' ?B
M *2
*2
,ift'off position,
,ift'off position,

#t
#t
. .
$
$

*2 *2
B2
B2
,ift'off position,
,ift'off position,

#t
#t
. .
$
$
#" # ##
$
# #$
&
# #&
)
# #) # # #
) ( ) ( a t a t a t a t a t h t + + + +
( )
$" # $#
$
# $$
&
# $& # $ #
) ( a t a t a t a t h t + + +

Ooundary ;onditions ?*' ?B
M *2
*2
,ift'off position,
,ift'off position,

#t
#t
. .
$
$

*2 *2
B2
B2
,ift'off position,
,ift'off position,

#t
#t
. .
$
$
#" # ##
$
# #$
&
# #&
)
# #) # # #
) ( ) ( a t a t a t a t a t h t + + + +
( )
$" # $#
$
# $$
&
# $& # $ #
) ( a t a t a t a t h t + + +

Ooundary ;onditions ?F' ?I
M
F2
F2
,ift'off velocity match from oth
,ift'off velocity match from oth
sides
sides
+2 +2
I2
I2
,ift'off acceleration match oth
,ift'off acceleration match oth
sides
sides
$# # $$
$
# $& ## # #$
$
# #&
&
# #) # $ # #
$ & $ & ) ) ( ) ( a t a t a a t a t a t a t h t h + + + + +

$$ # $& #$ # #&
$
# #) # $ # #
$ % $ % #$ ) ( ) ( a t a a t a t a t h t h + + +


Ooundary ;onditions ?J' ?K
M J2
J2
Set'down position,
Set'down position,

#t
#t
+ +
$
$

K2
K2
Set'down position,
Set'down position,

#t
#t
+ +
$
$

$" $ $#
$
$ $$
&
$ $& $
) ( a t a t a t a t + + +
&" $ &#
$
$ &$
&
$ &&
)
$ &) $
) ( a t a t a t a t a t + + + +

Ooundary ;onditions ?.- ' ?..
M
.-2 Set'down velocity match from oth
.-2 Set'down velocity match from oth
sides
sides
+2 +2
..2
..2
Set'down acceleration match
Set'down acceleration match
oth sides
oth sides
$# $ $$
$
$ $& &# $ &$
$
$ &&
&
$ &) $ $ $ &
$ & $ & ) ) ( ) ( a t a t a a t a t a t a t h t h + + + + +

$$ $ $& $$ $ $&
$
$ $) $ $ $ &
$ % $ % #$ ) ( ) ( a t a a t a t a t h t h + + +


Ooundary ;onditions ?.+' ?.*
M .+2
.+2
)inal position,
)inal position,

#t
#t
f f
$
$

.A2 )inal velocity, v

.A2 )inal velocity, v
f f
#typically -$
#typically -$
+2 +2
.*2
.*2
)inal acceleration, a
)inal acceleration, a
f f
#typically
#typically
-$
-$
&# &$
$
&&
&
&) &
$ & ) ) ( a t a t a t a t h
f f f f f
+ + +

&$ &&
$
&) &
$ % #$ ) ( a t a t a t h
f f f f
+ +

&" &#
$
&$
&
&&
)
&) &
) ( ) ( a t a t a t a t a t h t
f f f f f f
+ + + +

Solution )ormat

.* simultaneous linear e"uations with

.* simultaneous linear e"uations with
.* un:nowns:
.* un:nowns:

.. values re"uired to find solution:

.. values re"uired to find solution:
#" ## #$ #& #)
, , , , a a a a a
&" &# &$ && &)
, , , , a a a a a
$" $# $$ $&
, , , a a a a
f f f
f
t t t t t



, ,
, , ), ( ), (
, ,
$ # $ #
" " "

Solution )ormat

Solve using (atla #or possily (aple$

Solve using (atla #or possily (aple$
( )
( )
1
1
1
1
1
1
1
1
1
1
1
1
]
1

1
1
1
1
1
1
1
1
1
1
1
1
]
1

1
1
1
1
1
1
1
1
1
1
1
1
]
1

f f
t
t
a
a
a
a
a
a
t
t t
t
t t
t

"
"
" #$ " " " "
" ) " & " "
# " " " "
" " " & " )
" " # " "
" " " " # "
$
#
"
&"
&)
$"
$&
#"
#)
)
&
$
$
$
)
$
&
#
&
#
&
#

5fter (atla Solution

3nce we find the .* coefficients, how

3nce we find the .* coefficients, how
do we find velocities and accelerations8
do we find velocities and accelerations8
<
Ta:e derivative of h
Ta:e derivative of h
. .
#t$, h
#t$, h
+ +
#t$, h
#t$, h
A A
#t$ to get
#t$ to get
velocity
velocity
<
Ta:e derivative of velocity to get accel2
Ta:e derivative of velocity to get accel2
<
Ooth are easily plotted in (atla or Excel
Ooth are easily plotted in (atla or Excel

Prolems

6hat if the velocities or accelerations

6hat if the velocities or accelerations
we find are too large8
we find are too large8
<
1ncrease times t
1ncrease times t
. .
, t
, t
+ +
, t
, t
f f
<
(ove the pic:'up point closer to
(ove the pic:'up point closer to

#t
#t
- -
$
$
<
(ove the drop'off point closer to
(ove the drop'off point closer to

#t
#t
f f
$
$

(otion Programming ?A
)riday, 3ctoer .K, +--.
)riday, 3ctoer .K, +--.

Plan (otion Program
+
"
,
"
-
"
y
"
"

1
,
#
+
#
"
-
$
a
#
v
v v
v

2
R
a
$
y
$
from
1
1
1
]
1

1
1
1
]
1

#
"
)"
#
y
-
R
"
"
"
cm
cm
to
1
1
1
]
1

1
1
1
]
1

#
&"
)"
#
y
-
R
"
"
cm
cm
f

a
#
. %" cm, a
$
. )" cm,
-
$
. ( cm, y
$
. ( cm

#,ma-
.
$,ma-
. % rad'sec,

#,ma-
.
$,ma-
. $" rad'sec
$


1nverse Cinematics

6hat is the minimum time T that meets

6hat is the minimum time T that meets
velocity and acceleration constraints for
velocity and acceleration constraints for
oth joints8
oth joints8
x- y-

.

+

.

+
*- - -2JBB A2IF. *J2KK +.B2*I
*- A- .2*IA A2KJA J*2*+ ++J2+A
cm radians degrees
from 1nverse Cinematics:

(otion Programming ?.
M
6or: in groups of +:
6or: in groups of +:

4etermine an /S0 curve for

4etermine an /S0 curve for
each joint
each joint

9eed something suitale for 4!4

9eed something suitale for 4!4
motion
motion

Turn in solution
Turn in solution
M
will have two groups show solution
will have two groups show solution
%se T = .2- second as a /reasonale0 time
%se T = .2- second as a /reasonale0 time

Straight',ine (otion

4efine .- /:not0 points along the straight line

4efine .- /:not0 points along the straight line

)ind inverse :inematic solution at each point:

)ind inverse :inematic solution at each point:
x- y- . + . +
*- - -2JBB A2IF. *J2KK +.B2*I
*- A -2KA- A2IFA BA2+I +.B2F.
*- F .2--* A2II- BI2B- +.F2-A
*- K .2-IF A2IJA F.2FA +.F2IA
*- .+ .2.*B A2IKK FB2F- +.I2I-
*- .B .2+.. A2J+. FK2AJ +.J2K.
*- .J .2+IA A2J*F I+2KA ++-2AF
*- +. .2AA- A2JIB IF2++ +++2-*
*- +* .2AJA A2K-J IK2+A ++A2K.
*- +I .2*A. A2K** J.2KI ++B2KJ
*- A- .2*IA A2KJA J*2*+ ++J2+A
degrees cm radians

(otion Programming ?+

Select T = . second as a /reasonale0

Select T = . second as a /reasonale0
time for complete straight line motion
time for complete straight line motion

6or: in groups of +:
6or: in groups of +:

5re there any constraints that need to e

5re there any constraints that need to e
considered8 6hat do you recommend to
considered8 6hat do you recommend to
determine desired positions at intermediate
determine desired positions at intermediate
times8
times8