Trajectory Planning, Setpoint Generation and

Feedforward Design for High Performance

Motion Systems

Paul Lambrechts

Senior Application Engineer, The MathWorks Benelux.

September 27, 2006

1/43
Overview
• (Industrial) motion control
– Motion control and factory automation;
– Current methods for feedforward control
– Performance characteristics for trajectory planning
– Rigid body feedforward and second order trajectory planning

• Fourth order feedforward and trajectory planning

• Implementation aspects
– Switching times
– Discrete time integration and synchronization
– First order filter implementation

• Simulation and experimental results

• Conclusions

2/43
Industrial motion control

• Robots
• Pick-and-place units
• Wafersteppers

Motion Control Tasks:

• Safety, Communication, etc.
• System compensation
• Trajectory planning
• Feedforward control
• Feedback control

CRT Tube Handler

3/43
Industrial motion control

• Robots
• Pick-and-place units
• Wafersteppers

Motion Control Tasks:

• Safety, Communication, etc.
• System compensation
• Trajectory planning
• Feedforward control
• Feedback control Component Mounter

3/43
Industrial motion control

• Robots
• Pick-and-place units
• Wafersteppers

Motion Control Tasks:

• Safety, Communication, etc.
• System compensation
• Trajectory planning
• Feedforward control
• Feedback control 300mm Waferstepper

3/43
Simple experimental setup:

4/43
Rigid-body feedforward

1 DOF rigid-body model

mẍ + bẋ = F ⇒ Fff = ma + bv

x is position, m is (equivalent) mass or inertia, b is viscous damping,
F is actuator force and Fff is feedforward force.

5/43
Trajectory profiles for rigid-body feedforward
Second order trajectory profiles
5
a [m/s2]

−5
0 0.2 0.4 0.6 0.8 1
1.5
v [m/s]

1
0.5 Fff = ma + bv
0
0 0.2 0.4 0.6 0.8 1
1
x [m]

0.5

0
0 0.2 0.4 0.6 0.8 1
time [s]

6/43
Rigid body feedforward: Fff = ma + bv

7/43
Rigid body feedforward: Fff = ma + bv

7/43
Rigid body feedforward: Fff = ma + bv

8/43
Trajectory planning performance (p2p)
Second order trajectory profiles
• Timing: minimal trajectory
5
execution time
a [m/s2]

0
• Realizability: constrained
−5 dynamics (ā and v̄)
0 0.2 0.4 0.6 0.8 1
1.5
v [m/s]

1 • Accuracy: trajectory ends at

0.5 desired end position (x̄)
0
0 0.2 0.4 0.6 0.8 1 • Complexity: calculation time
1
• Reliability:
x [m]

0.5 always valid solution

0 • Implementation:
0 0.2 0.4 0.6 0.8 1 discretization, quantization
time [s]

9/43
Profiles given ā, v̄ and x̄:
q
1. Forget v̄ : x̄ = 2 × 12 āt2 ⇒ tā = x̄ ⇒ t = 2t
ā x̄ ā

2. Calculate maximal velocity: v̂ := ā · tā

3. v̂ > v̄ ?; true: tā = āv̄ , false: tā = 12 tx̄

4. xā := 2 × 12 āt2ā ≤ x̄
(x̄−xā)
5. tv̄ = v̄ ⇒ ā, tā, tv̄

10/43
Properties of trajectory planning algorithm
Second order trajectory profiles
5
ta
• Timing: minimal,
a [m/s2]

tv determined by tā and tv̄

0
ta
• Realizablity: guaranteed
−5
0 0.2 0.4 0.6 0.8 1 by ā and v̄
1.5
v [m/s]

1
0.5 • Accuracy: exact within
0 machine accuracy
0 0.2 0.4 0.6 0.8 1
1
• Complexity: low
• Reliability:
x [m]

0.5
always valid solution
0
0 0.2 0.4 0.6 0.8 1 • Implementation: later
time [s]

11/43
Results of rigid body feedforward:
Trajectories [normalized] 1

0.5

0
acceleration
−0.5 velocity
position
−1
0 0.5 1 1.5 2
Closed loop error response
0.1

0.05
Error [mm]

0
0.01
−0.05
0
−0.1
0 0.5 1 −0.01 0
time [s] 1.5 1.6 1.7

12/43
Extensions of rigid body feedforward
• Smoothing and shaping
– third order trajectories with rigid body feedforward ?
– filtering of second order trajectories and feedforwards ?
• (Approximate) model inversion
– using second or third order trajectories ?
– focus on frequency domain properties ?
– learning techniques ?

→ back to basics !

13/43
4th order model for motion system

1 DOF 4th order model

x1 and x2 are actuator and load position, m1, m2 masses, b1, b2 vis-
cous damping, k spring stiffness, b12 internal viscous damping, F is
actuator force.

14/43
4th order feedforward
Equations of motion:

m1ẍ1 = −b1ẋ1 − k(x1 − x2) − b12(ẋ1 − ẋ2) + F
m2ẍ2 = −b2ẋ2 + k(x1 − x2) + b12(ẋ1 − ẋ2)

Laplace transform and substitution:




 q1 = m1 m2
q1 s 4 + q2 s 3 + q3 s 2 + q4 s q2 = (m1 + m2)b12 + m1b2 + m2b1

F = ·x2
b12s + k 
 q3 = (m1 + m2)k + b1b2 + (b1 + b2)b12

q
4 = (b1 + b2)k

Feedforward force calculation:

1
Fff = · {q1d + q2 + q3a + q4v}
b12s + k

15/43
 1000

.
d [m/s4]

−1000
Trajectory profiles
0 0.2 0.4 0.6 0.8 1 1.2
50
for 4th order
j [m/s3]

−50
feedforward
0 0.2 0.4 0.6 0.8 1 1.2
5
a [m/s2]

−5
0 0.2 0.4 0.6 0.8 1 1.2

1.5
q1 d + q 2  + q 3 a + q 4 v
v [m/s]

1
Fff =
0.5 b12s + k
0
0 0.2 0.4 0.6 0.8 1 1.2

1
x [m]

0.5

0
0 0.2 0.4 0.6 0.8 1 1.2

16/43
4th order feedforward: Fff = q1d+qb2+q
s+k
3a+q4v
12

17/43
 1000

d [m/s4]
0

4th order trajectory −1000

0 0.2 0.4 0.6 0.8 1 1.2
planner ? 50

j [m/s3]
0
• Point-to-point move
−50
(all derivatives zero 0 0.2 0.4 0.6 0.8 1 1.2
5
at start and end)

a [m/s2]
0
• Given: displacement x̄
and bounds d,¯ ̄, ā and v̄ −5
0 0.2 0.4 0.6 0.8 1 1.2

1.5
• Performance criteria !? v [m/s]
1
0.5
0
0 0.2 0.4 0.6 0.8 1 1.2

1
x [m]

0.5

0
0 0.2 0.4 0.6 0.8 1 1.2

18/43
4th order trajectory specification:
Second order trajectory profiles: 3 time periods, 2 sizes and max a
5
ta
tv
a [m/s2]

ta
−5
0 0.2 0.4 0.6 0.8 1
Fourth order trajectory profiles: 15 time periods, 4 sizes and max d
1000
t t t t
500 d d d d
t t t t t t t
d [m/s4]

j a j v j a j
0

−500 td td td td
−1000
0 0.2 0.4 0.6 0.8 1
time [s]

19/43
 Fourth order trajectory profiles

4th order calculations 1000

d0

d [m/s4]
500

0
0.39 0.4 0.41 0.42 0.43 0.44 0.45 0.46
d(t) = d0 0

j [m/s3]
−20
−40 j0
(t) = d0t + 0 0.39 0.4 0.41 0.42 0.43 0.44 0.45 0.46
2

a [m/s2]
1 2 1 a0
a(t) = 2 d0 t + 0t + a0
0
0.39 0.4 0.41 0.42 0.43 0.44 0.45 0.46

1 1.5
v(t) = 6 d0 t
3
+ 21 0t2 + a0t + v0 v
v [m/s]
0

1.45
0.39 0.4 0.41 0.42 0.43 0.44 0.45 0.46
1 4 1 3 2
x(t) = 24 d0 t + 6 0 t + a0t 0.35
x [m]

0.3
x0
+v0t + x0 0.25
0.39 0.4 0.41 0.42 0.43 0.44 0.45 0.46
time [s]

20/43
 Fourth order trajectory profiles, no bounds on j, a or v

4th order planning 1000

t
d
t
d
t
d
t
d

d [m/s4]
0
calculate td¯ −1000
t
d
t
d
t
d
t
d
0 0.2 0.4 0.6 0.8 1 1.2
100

j [m/s3]
td¯ only depends on d¯ and x̄ 0

−100
0 0.2 0.4 0.6 0.8 1 1.2
q
⇒ td¯ = 4 8x̄d¯ 10

a [m/s2]
0
q
v̄ −10
v̄ violated: td¯ = 3
2d¯ 0 0.2 0.4 0.6 0.8 1 1.2

v [m/s]
q
ā 1
ā violated: td¯ = d¯ 0
0 0.2 0.4 0.6 0.8 1 1.2

̄ 1
̄ violated: td¯ = d¯
x [m]

0.5

0
0 0.2 0.4 0.6 0.8 1 1.2
time [s]

21/43
 Fourth order trajectory profiles, final td

4th order planning 1000

t
d
t
d
t t
d d

d [m/s4]
0
final td¯ −1000
t t
d d
t
d
t
d
0 0.2 0.4 0.6 0.8 1 1.2
50

j [m/s3]
0

−50
Note: 0 0.2 0.4 0.6 0.8 1 1.2
2

a [m/s2]
• No bounds violated 0

−2
• Final td¯ always ≤ first td¯ 0 0.2 0.4 0.6 0.8 1 1.2

0.2
• Consequently: x̄ not reached
v [m/s]
0.1
0
0 0.2 0.4 0.6 0.8 1 1.2

0.04
x [m]

0.02
0
0 0.2 0.4 0.6 0.8 1 1.2
time [s]

22/43
 Fourth order trajectory profiles, no bounds on a or v

4th order planning 1000

t
d tj tj
t
d tj
t t
d d tj

d [m/s4]
0
calculate t̄ −1000
t t
d d
t
d
t
d
0 0.2 0.4 0.6 0.8 1 1.2
50

j [m/s3]
Add periods of constant jerk 0

until x̄ is reached again −50

0 0.2 0.4 0.6 0.8 1 1.2
10

a [m/s2]
t̄ follows from: 0

t3̄ + (5td¯)t2̄ + (8t2d¯)t̄ + (4t3d¯ − 2dtx̄¯ ¯) = 0 −10

0 0.2 0.4 0.6 0.8 1 1.2
d
2

v [m/s]
1

0
v̄ violated: t2̄ + 3td¯t̄ + 2t2d¯ − dt¯v̄ ¯ = 0 0 0.2 0.4 0.6 0.8 1 1.2
d 1
x [m]

0.5
ā violated: t̄ = ā̄ − td¯
0
0 0.2 0.4 0.6 0.8 1 1.2
time [s]

23/43
 Fourth order trajectory profiles, final td and tj

4th order planning 1000

t
d tj tj
t
d tj
t t
d d tj

d [m/s4]
0
final td¯ and t̄ −1000
t t
d d
t
d
t
d
0 0.2 0.4 0.6 0.8 1 1.2
50

j [m/s3]
0

−50
Note: 5
0 0.2 0.4 0.6 0.8 1 1.2

a [m/s2]
• No bounds violated 0

−5
• Final t̄ always ≤ first t̄ 0 0.2 0.4 0.6 0.8 1 1.2
0.8
0.6
• Consequently: x̄ not reached
v [m/s]
0.4
0.2
0
0 0.2 0.4 0.6 0.8 1 1.2

0.2
x [m]

0.1

0
0 0.2 0.4 0.6 0.8 1 1.2
time [s]

24/43
 Fourth order trajectory profiles, no bounds on v

4th order planning 1000

t
d tj ta tj
t
d tj
t
d ta
t
d tj

d [m/s4]
0
calculate tā −1000
t
d
t
d
t
d
t
d
0 0.2 0.4 0.6 0.8 1 1.2
50
Add periods of constant

j [m/s3]
0
acceleration until x̄ reached −50
0 0.2 0.4 0.6 0.8 1 1.2
tā follows from: 5

a [m/s2]
0
{t2d¯ + td¯t̄} t2ā +
−5
0 0.2 0.4 0.6 0.8 1 1.2
{6t3d¯ + 9t2d¯t̄ + 3td¯t2̄ } tā + 2

v [m/s]
{8t4d¯ + 16t3d¯t̄ + 10t2d¯t2̄ + 2td¯t3̄ − x̄d¯} = 0 1

0
0 0.2 0.4 0.6 0.8 1 1.2
1
¯ 3 −3dt
v̄−2dt ¯ 2 t̄ −dt
¯ ¯t2
x [m]

d¯ d¯ d ̄
v̄ violated: tā = ¯ 2 ¯
dt +dt ¯t̄
0.5
d¯ d
0
0 0.2 0.4 0.6 0.8 1 1.2
time [s]

25/43
 Fourth order trajectory profiles, final ta, td and tj

4th order planning 1000

t
d tj ta tj
t
d tj
t
d ta
t
d tj

d [m/s4]
0
final td¯, t̄ and tā −1000
t
d
t
d
t
d
t
d
0 0.2 0.4 0.6 0.8 1 1.2
50

j [m/s3]
0

−50
Note: 5
0 0.2 0.4 0.6 0.8 1 1.2

a [m/s2]
• No bounds violated 0

−5
• Final tā always ≤ first tā 0 0.2 0.4 0.6 0.8 1 1.2
1.5
• Consequently: x̄ not reached
v [m/s]
1
0.5
0
0 0.2 0.4 0.6 0.8 1 1.2

For final step: 0.6

x [m]

0.4
determine obtained position xā 0.2
0
0 0.2 0.4 0.6 0.8 1 1.2
time [s]

26/43
 Fourth order trajectory profiles, final tv, ta, td and tj

4th order planning 1000

t
d tj ta tj
t
d tv tj
t
d ta
t
d tj

d [m/s4]
0
calculate tv̄ −1000
t
d
t
d
t
d
t
d
0 0.2 0.4 0.6 0.8 1 1.2
50
Final step: add period of

j [m/s3]
0
constant velocity until x̄ reached −50
0 0.2 0.4 0.6 0.8 1 1.2
5

a [m/s2]
0
x̄ − xā
tv̄ = −5
v̄ 0 0.2 0.4 0.6 0.8 1 1.2
1.5

v [m/s]
1
0.5
0
Finished: trajectory completely 0 0.2 0.4 0.6 0.8 1 1.2
1
determined by 5 parameters !
x [m]

0.5

0
0 0.2 0.4 0.6 0.8 1 1.2
time [s]

27/43
(Further) Implementation aspects
• Switching times
• Discrete time integration and synchronization
• First order filter implementation

28/43
 Fourth order trajectory profiles: intervals
Implementation 1000
td td td td
500

d [m/s4]
tj ta tj tv tj ta tj
Switching times: 0

−500 t t t t
d d d d
−1000
Round off intervals up to 0 0.2 0.4 0.6 0.8 1
Switching times
multiple of sampling 1000
time Ts. 500
d [m/s4]
t t t t t t t t
2 3 4 5 8 9 14 15
0
t0 t1 t6 t7 t10 t t12 t13
Correct by reducing d¯ −500
11

to appropriate value. −1000

0 0.2 0.4 0.6 0.8 1
Synchronisation with sampling instances at 100Hz?
1000

500
d [m/s4]

t8 t9 tj
0
t
10
−500 t
d
−1000
0.65 0.7 0.75 0.8
time [s]

29/43
Switching times synchronization
Make sure that each interval is a multiple of the sampling time Ts

Example: r
x̄
td¯ = 4 ¯
8d
 
t¯
t0d¯ = ceil d × Ts
Ts

Correct d¯
x̄
d¯0 = 0 4
8td¯

Note that with t0d¯ ≥ td¯ we must have d¯0 ≤ d¯

30/43
4th order feedforward in discrete time ?

31/43
Implementation
Discrete time integration:

d
d

D e r iv a tiv e o f J e r k T s j T s a T s v T s
p r o file g e n e r a to r --------- --------- --------- --------- x
z -1 z -1 z -1 z -1
J e rk A c c e le r a tio n V e lo c ity P o s itio n

Synchronization of profiles is required !

32/43
Synchronization of profiles
Delay:
not synchronized synchronized, delayed by 0.1s

d with 2Ts,
d

 with 1 21 Ts,
j

a with Ts,
a

v with 12 Ts.
v

→ 12 Ts ?
x

0 0.2 0.4 0.6 0.8 1 0 0.2 0.4 0.6 0.8 1

time [s] time [s]

33/43
 1
u y
k 1 2 .s + c

First order filter C o n tin u o u s tim e

1
1 /k 1 2
implementation u
s
y

c /k 1 2

A lg e b ra ic lo o p !!
T s(z + 1 )
u 1 /k 1 2 --------------- y
2 (z -1 )
Z O H
D is c re te -T im e
In te g ra to r: T ra p e z o id a l

c /k 1 2

A lg e b ra ic lo o p !!
1
u 1 /k 1 2 T s 1 /2 y
z
Z O H
Transfer function:
c /k 1 2
Ts
2k12 +cTs (z + 1) N O a lg e b ra ic lo o p !!
y= u 1
12 −cTs
z − 2k2k12 +cTs
u -K -
z
y

Z O H T s /(2 * k 1 2 + c * T s )
-K -

(2 * k 1 2 -c * T s )/(2 * k 1 2 + c * T s )

34/43
Digital 4th order feedforward

[d ]
1 [fo rc e ]
Z O H 1 T [j] T [a ] T [v ] T [p 1 ]
In 1 [re f] 1
z -1 z -1 z -1 z -1 O u t1
[p 2 ] [m o n ]
je r k a c c e le r a tio n v e lo c it y p o s itio n

p o s itio n c o r r e c tio n Q u a n tiz e r p re v e n t

n e g a t iv e
Z O H 2

[d ]
d e r iv a tiv e o f je r k [d ] 1 /z 1 /z -K -
m 1 *m 2
je r k
[j] [j] 1 /z 0 .5 * T s -K -
(m 1 + m 2 )*k 1 2 + m 1 *k 2 + m 2 *k 1 1
a c c e le r a tio n -K - [fo rc e ]
[a ] [m o n ] [a ] 1 /z -K - z
1 /k 1 2 * T s /(2 + c /k 1 2 * T s )
U n it D e la y
v e lo c ity (m 1 + m 2 )*c + k 1 *k 2 + (k 1 + k 2 )*k 1 2
[v ]
[v ] 0 .5 * T s -K - -K -
c o r r e c te d p o s itio n (k 1 + k 2 )*c
[p 2 ] (2 -c /k 1 2 * T s )/(2 + c /k 1 2 * T s )

[p 2 ] 0 .5 * T s [re f]

35/43
Bound selection
• Velocity:
– back EMF smaller than power supply voltage
– motor or gearbox specification (temperature)
• Acceleration:
– maximum power supply or motor current
– mechanical restrictions
• Jerk:
– power amplifier rise time
– mechanical restrictions
̄
• Derivative of jerk upper bound: Ts

36/43
Experimental setup:

37/43
Experimental setup:

38/43
Experimental setup:

38/43
Simulation
results
Robustness against
variations in addi-
tional
parameters

39/43
Simulation Open loop response of servo error with optimal 2nd, 3rd and 4th order FF
results 0.1

0.08

0.06

0.04

Effect of order 0.02

Error [rad]

of feedforward 0

−0.02

−0.04

−0.06
Second order FF (i.e. rigid body)
−0.08 Third order FF
Fourth order FF (= 0)
−0.1
−0.1 0 0.1 0.2 0.3 0.4 0.5 0.6
time [s]

40/43
Simulation Discrete time response of servo error
0.1
results x 10
−4

0.05
Discrete time 2
Error [rad]

vs. continuous
0
time 0 1 2 3 4 5
−3
x 10
0

Continuous
Discrete
−0.05
−0.1 0 0.1 0.2 0.3 0.4 0.5 0.6
time [s]

41/43
 Simulated servo errors, 4th order profile
Measured 0.1
Rigid body FF
results 0.05
4th order FF
Acceleration (scaled)

Error [rad]
0

−0.05

−0.1
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7
time [s]
Measured servo errors, 4th order profile
0.1

0.05
Error [rad]

−0.05

−0.1
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7
time [s]
0.48 0.5 0.52 0.54

42/43
Conclusions
• Superior performance of 4th order vs. rigid-body feedforward
• Algorithm no problem for state-of-the-art motion controllers
• Especially feedforward of djerk effective (for electro-mechanical
motion systems)
• Complete derivation (also for third order) available
• Simulink toolbox ‘motion’ available
• Experimental verification using MATLAB, Simulink and Real-Time
Workshop

43/43