Trajectory Generation
Basic Problem:
Move the manipulator arm from some initial position
{TA} to some desired final position {TC}.
(May be going through some via point {TB})
{TA}
{TB}
Path points : Initial, final and via points
Trajectory : Time history of position,
velocity and acceleration
for each DOF
{T }
C
(tool frame)
Trajectory Generation
Constraints: Spatial, time, smoothness
{S} (Stationary frame)
Solution Spaces :
Cartesian planning difficulties :
Joint space
• Easy to go through via points
(Solve inverse kinematics at all path points and plan)
• No problems with singularities
• Less calculations
• Can not follow straight line
Initial and Goal
Points are reachable.
A
Intermediate points
(C) unreachable.
C
Cartesian space
B
• We can track a shape
(for orientation : equivalent axes, Euler angles,…
• More expensive at run time
(after the path is calculated need joint angles
in a lot of points)
• Discontinuity problems
Cartesian planning difficulties :
Approaching singularities
some joint velocities
go to
causing deviation
from the path
Cartesian planning difficulties :
8
B
singularity
Start point (A) and
goal point (B) are
reachable in different
joint space solutions
(The middle points
are reachable from
below.)
A
B
A
1
Single Cubic Polynomial
Actual planning in any space:
Assume one generic variable u
(can be x, y, z, orientation - α, β, γ)
joint variables
Candidate curves :
direction cosines
straight line (discontinuous velocity at path points)
D
B
C
A
straight line with blends
B
D
C
A
cubic polynomials (splines)
B
A
D
C
higher order polynomials (quintic,…) or other curves
Single Cubic Polynomial
( )
Starts and ends at rest
Cubic Polynomials with via points
• If we come to rest at each point
use formula from previous slide
• For continuous motion (no stops)
need velocities at intermediate points:
u&(0) = u&0
Initial Conditions
u&(t f ) = u& f a = u
0
0
Solution :
a1 = u&0
a2 =
u ( 0) = u0 ; u ( t f ) = uf
Initial
Conditions:
Single Cubic Polynomial
2
u&( t) = a1 +2a2t +3at
3
u& ( 0) = 0 ; u& t f = 0
Initial
Conditions:
u ( t ) = a 0 + a1t + a 2 t 2 + a 3 t 3
3
2
1
(u f − u0 ) − u&0 − u& f
t 2f
tf
tf
u&&( t) =2a2 +6at3
&&&
u ( t ) = 6a3 (constant) 3
 2
u( t) =u0 + 2 ( uf −u0) t2 +− 3 ( uf −u0) t3
 tf 
Solution :
tf
 
How to find u&0 , u& f ,…(velocities at via points)
• if we know Cartesian linear and angular velocities






v
→ use J −1 : u& = J −1 ω






• the system chooses reasonable velocities using
heuristics (average of 2 sides etc.)
• the system chooses them for continuous
velocity
u& (t ) =u& (0)
1 f
2
acceleration
and
u&&1(t ) =u&&2(0)
f
2
1
a3 = − 3 (u f − u0 ) + 2 (u& f + u&0 )
tf
tf
2
Linear interpolation:
Linear interpolation:
u
Straight line
u
Parabolic blend
u(t) = 1 at 2
f
at blend regions
Linear velocity
2
u
0
t
t
0
t
f
f
u(t)=a +at
or
u(t) = 1ut
&& 2
2
u
0 1
2 conditions :
u&(t) = at
Constant acceleration u&&(t) = a
u
u
0
u(t ) =u
0
0
u(t ) =u
f
t0 t b
tf −tb t f
t
From continuous velocity:
ut
&& 2 − 4u&&(u f −u0)
t =t −
b 2
2u&&
f
Discontinuous velocity - can not be controlled
ui , u j , u k , ul , u m
• positions
with blends for several segments
u
• desired time durations
t jk
slope=
u& uk
jk
slope=
ui
tij
tk
tj
tdij
t dij , t djk , t dkl , t dlm
&&i , u&&j , u&&k , u&&l
• the magnitudes of the accelerations: u
u&
slope= ij
ti
where t =t f −t0
desired duration of motion
Given:
Linear Interpolation
uj
at blend
regions
u
l
u
&
slope= kl
tkl tl
tlm
• blends times ti , t j , t k , t l , t m
• straight segment times tij , t jk , t kl , t lm
• slopes (velocities)
First segment
u&ij , u& jk , u&kl , u&lm
• signed accelerations
Formulas (7.24), (7.26) and (7.28)
tm
t
tdjk
System usually calculates or uses default values for accelerations.
The system can also calculate desired time durations based on
default velocities.
Inside segments
u&&1 = sign(u2 − u1 ) u&&1
t1 = td12 − td212 −
Compute:
u&
lm
2(u2 − u1 )
u&&1
u −u
u&12 = 2 1
1
td12 − t1
2
1
t12 = t d12 − t1 − t2
2
u& jk =
uk − u j
tdjk
u&&k = sign(u&kl − u& jk ) u&&k
tk =
u&kl − u& jk
u&&k
1
1
t jk = tdjk − t j − tk
2
2
3
Last segment
To go through the actual via points:
• Introduce “Pseudo Via Points”
u&&n = sign(un −1 − un ) u&&n
tn = td ( n −1) n − t
u&( n −1) n =
2
d ( n −1) n
u
Pseudo Via Points
2(un − un −1 )
−
u&&n
Original
Via Point
un − un −1
1
t d ( n −1) n − tn
2
t
1
t( n −1) n = td ( n −1) n − tn − tn −1
2
• Use sufficiently high acceleration
• If we want to stop there, simply repeat the via point
Run Time Path Generation
& ,Θ
&& fed to the control system
• trajectory in terms of Θ, Θ
Higher Order Polynomials
• For example if given:
6 conditions
R
|Sposition
velocity
|Tacceleration
(initial u0 , final u f )
(u&0 , u& f )
(&&
u0 , u&&f )
2
3
4
Use quintic: u ( t ) = a 0 + a1t + a 2 t + a3 t + a 4 t + a5 t
and find ai (i=0 to 5)
(formulas (7.18) in the book)
Use different functions (exponential, trigonometric,…)
5
• Path generator computes at path update rate
• In joint space directly:
• cubic splines -- change set of coefficients at the end of
each segment
• linear with parabolic blends -- check on each update if
you are in linear or blend portion and use appropriate
formulas for u
• In Cartesian space:
• calculate Cartesian position and orientation at each update
point using same formulas
• convert into joint space using
inverse Jacobian and derivatives
or
find equivalent frame representation and use inverse
& ,Θ
&&
kinematics function to find Θ, Θ
Trajectory Planning with Obstacles
• Path planning for the whole manipulator
• Local vs. Global Motion Planning
• Gross motion planning for relatively uncluttered
environments
• Fine motion planning for the end-effector frame
• Configuration space (C-space) approach
• Planning for a point robot
• graph representation of the free space, quadtree
• Artificial Potential Field method
• Multiple robots, moving robots and/or obstacles
4