trajectory planning and feedforward for motion · pdf filefaculty of mechanical engineering,...

42
12 Faculty of Mechanical Engineering, Control Systems Technology Group 1/42 Trajectory Planning, Setpoint Generation and Feedforward for Motion Systems Paul Lambrechts Digital Motion Control (4K410), 2003

Upload: ngongoc

Post on 17-Feb-2018

224 views

Category:

Documents


5 download

TRANSCRIPT

Page 1: Trajectory Planning and Feedforward for Motion · PDF fileFaculty of Mechanical Engineering, Control Systems Technology Group 1/42 Trajectory Planning, Setpoint Generation and Feedforward

12

Faculty of Mechanical Engineering, Control Systems Technology Group 1/42

Trajectory Planning, Setpoint Generationand Feedforward for Motion Systems

Paul Lambrechts

Digital Motion Control (4K410), 2003

Page 2: Trajectory Planning and Feedforward for Motion · PDF fileFaculty of Mechanical Engineering, Control Systems Technology Group 1/42 Trajectory Planning, Setpoint Generation and Feedforward

12

Faculty of Mechanical Engineering, Control Systems Technology Group 2/42

Overview

• Industrial motion control– Motion control and factory automation;

– Current methods for feedforward control

– Performance characteristics for trajectory planning

– Second order trajectory planning algorithm

• Fourth order feedforward for electromechanical motion systems

• Fourth order trajectory planning algorithm

• (Further) implementation aspects– Architecture

– Switching times

– Discrete time integration and synchronization

– First order filter implementation

• Simulation results

• Conclusions

Page 3: Trajectory Planning and Feedforward for Motion · PDF fileFaculty of Mechanical Engineering, Control Systems Technology Group 1/42 Trajectory Planning, Setpoint Generation and Feedforward

12

Faculty of Mechanical Engineering, Control Systems Technology Group 3/42

Industrial Robot

Industrial motion control

• Robots

• Pick-and-place units

• Wafersteppers

Motion Control Tasks:

• Safety, Communication, etc.

• System compensation

• Trajectory planning

• Feedforward control

• Feedback control

Page 4: Trajectory Planning and Feedforward for Motion · PDF fileFaculty of Mechanical Engineering, Control Systems Technology Group 1/42 Trajectory Planning, Setpoint Generation and Feedforward

12

Faculty of Mechanical Engineering, Control Systems Technology Group 4/42

#10An introduction to Digital Control

Main digital control functions

HcSetpoints+

-I/F I/F

I/F

Error

handling

Application

interface

Integrated

functions Hardware

interfaces

Page 5: Trajectory Planning and Feedforward for Motion · PDF fileFaculty of Mechanical Engineering, Control Systems Technology Group 1/42 Trajectory Planning, Setpoint Generation and Feedforward

12

Faculty of Mechanical Engineering, Control Systems Technology Group 5/42

‘Simple’ rigid-body feedforward

F m

x

k1 DOF rigid-body model

mx + kx = F ⇒ Fff = ma + kv

x is position, m is (equivalent) mass or inertia, k is viscous damp-ing, F is actuator force and Fff is feedforward force.

Page 6: Trajectory Planning and Feedforward for Motion · PDF fileFaculty of Mechanical Engineering, Control Systems Technology Group 1/42 Trajectory Planning, Setpoint Generation and Feedforward

12

Faculty of Mechanical Engineering, Control Systems Technology Group 6/42

Digital Motion Control

Feedforward design8

sx2ms

1sKK vp

2ms

Feedforward based on inverse model

x

Page 7: Trajectory Planning and Feedforward for Motion · PDF fileFaculty of Mechanical Engineering, Control Systems Technology Group 1/42 Trajectory Planning, Setpoint Generation and Feedforward

12

Faculty of Mechanical Engineering, Control Systems Technology Group 7/42

Trajectory profiles for rigid-body feedforward

0 0.2 0.4 0.6 0.8 1−5

0

5Second order trajectory profiles

a [m

/s2]

0 0.2 0.4 0.6 0.8 10

0.51

1.5

v [m

/s]

0 0.2 0.4 0.6 0.8 10

0.5

1

time [s]

x [m

]

Fff = ma + kv

Page 8: Trajectory Planning and Feedforward for Motion · PDF fileFaculty of Mechanical Engineering, Control Systems Technology Group 1/42 Trajectory Planning, Setpoint Generation and Feedforward

12

Faculty of Mechanical Engineering, Control Systems Technology Group 8/42

#38An introduction to Digital Control

Setpoint

acceleration

+-

LowPass

Kfst

SaturationLeadLagKv d

dt

+

+

+

+

-2 +1

Notch /

LeadLag

Setpoint

direction

Kfv

Setpoint

velocity

Kfa

Kp+

+

+

+

+

+

Kfc

+

+

+

Setpoint

position

Ki

integrator anti-windupmeasured position

static feedforward

Coulomb friction compensation

velocity feedforward

acceleration feedforward

Setpoint

generator

General controller implementation

Page 9: Trajectory Planning and Feedforward for Motion · PDF fileFaculty of Mechanical Engineering, Control Systems Technology Group 1/42 Trajectory Planning, Setpoint Generation and Feedforward

12

Faculty of Mechanical Engineering, Control Systems Technology Group 9/42

Second order feedforward:

F

a

vk

m

1s

Velocity

1s

Position

xPD

Controller

Accelerationprofile generator

1

m.s +k.s2

2nd order model

Page 10: Trajectory Planning and Feedforward for Motion · PDF fileFaculty of Mechanical Engineering, Control Systems Technology Group 1/42 Trajectory Planning, Setpoint Generation and Feedforward

12

Faculty of Mechanical Engineering, Control Systems Technology Group 10/42

Trajectory planning performance (p2p)

0 0.2 0.4 0.6 0.8 1−5

0

5Second order trajectory profiles

a [m

/s2]

0 0.2 0.4 0.6 0.8 10

0.51

1.5

v [m

/s]

0 0.2 0.4 0.6 0.8 10

0.5

1

time [s]

x [m

]

• Timing: minimal trajectoryexecution time

• Realizability: constraineddynamics (a and v)

• Accuracy: trajectory ends atdesired end position (x)

• Complexity: calculation time

• Reliability: alwaysvalid solution

• Implementation:discretization, quantization

Page 11: Trajectory Planning and Feedforward for Motion · PDF fileFaculty of Mechanical Engineering, Control Systems Technology Group 1/42 Trajectory Planning, Setpoint Generation and Feedforward

12

Faculty of Mechanical Engineering, Control Systems Technology Group 11/42

Profiles given a, v and x:

1. Forget v: x = 2× 12at2 ⇒ ta =

√xa ⇒ tx = 2ta

2. Calculate maximal velocity: v := a · ta

3. v > v ?; true: ta = va, false: ta = 1

2tx

4. xa := 2× 12at2a ≤ x

5. tv =(x−xa)

v

Page 12: Trajectory Planning and Feedforward for Motion · PDF fileFaculty of Mechanical Engineering, Control Systems Technology Group 1/42 Trajectory Planning, Setpoint Generation and Feedforward

12

Faculty of Mechanical Engineering, Control Systems Technology Group 12/42

Properties of trajectory planning algorithm

0 0.2 0.4 0.6 0.8 1−5

0

5Second order trajectory profiles

a [m

/s2] t

a tv

ta

0 0.2 0.4 0.6 0.8 10

0.5

1

1.5

v [m

/s]

0 0.2 0.4 0.6 0.8 10

0.5

1

time [s]

x [m

]

• Timing: minimal,determined by ta and tv

• Realizablity: guaranteedby a and v

• Accuracy: exact withinmachine accuracy

• Complexity: low

• Reliability: always validsolution

• Implementation: later

Page 13: Trajectory Planning and Feedforward for Motion · PDF fileFaculty of Mechanical Engineering, Control Systems Technology Group 1/42 Trajectory Planning, Setpoint Generation and Feedforward

12

Faculty of Mechanical Engineering, Control Systems Technology Group 13/42

Extensions of rigid body feedforward

• Smoothing and shaping

– third order trajectories with rigid body feedforward ?

– filtering of second order trajectories and feedforwards ?

• Model inversion

– using second or third order trajectories ?

– focus on frequency domain properties ?

– learning techniques ?

→ back to basics !

Page 14: Trajectory Planning and Feedforward for Motion · PDF fileFaculty of Mechanical Engineering, Control Systems Technology Group 1/42 Trajectory Planning, Setpoint Generation and Feedforward

12

Faculty of Mechanical Engineering, Control Systems Technology Group 14/42

4th order model for electro-mechanical motionsystem

F m 1

x 1

k 1

m 2

x 2

k 2

c

k 1 2

1 DOF 4th order model

x1 and x2 are actuator and load position, m1, m2 masses, k1, k2

viscous damping, c spring stiffness, k12 internal viscous damping,F is actuator force.

Page 15: Trajectory Planning and Feedforward for Motion · PDF fileFaculty of Mechanical Engineering, Control Systems Technology Group 1/42 Trajectory Planning, Setpoint Generation and Feedforward

12

Faculty of Mechanical Engineering, Control Systems Technology Group 15/42

4th order feedforward

Equations of motion:{m1x1 = −k1x1 − c(x1 − x2)− k12(x1 − x2) + F

m2x2 = −k2x2 + c(x1 − x2) + k12(x1 − x2)

Laplace transform and substitution:

F =q1s

4 + q2s3 + q3s

2 + q4s

k12s + c·x2

q1 = m1m2

q2 = (m1 + m2)k12 + m1k2 + m2k1

q3 = (m1 + m2)c + k1k2 + (k1 + k2)k12

q4 = (k1 + k2)c

Feedforward force calculation:

Fff =1

k12s + c· {q1d + q2 + q3a + q4v}

Page 16: Trajectory Planning and Feedforward for Motion · PDF fileFaculty of Mechanical Engineering, Control Systems Technology Group 1/42 Trajectory Planning, Setpoint Generation and Feedforward

12

Faculty of Mechanical Engineering, Control Systems Technology Group 16/42

0 0.2 0.4 0.6 0.8 1 1.2−1000

0

1000

d [m

/s4]

0 0.2 0.4 0.6 0.8 1 1.2−50

0

50

j [m

/s3]

0 0.2 0.4 0.6 0.8 1 1.2−5

0

5

a [m

/s2]

0 0.2 0.4 0.6 0.8 1 1.20

0.5

1

1.5

v [m

/s]

0 0.2 0.4 0.6 0.8 1 1.20

0.5

1

x [m

]

Trajectory profiles

for 4th orderfeedforward

Fff =q1d + q2 + q3a + q4v

k12s + c

Page 17: Trajectory Planning and Feedforward for Motion · PDF fileFaculty of Mechanical Engineering, Control Systems Technology Group 1/42 Trajectory Planning, Setpoint Generation and Feedforward

12

Faculty of Mechanical Engineering, Control Systems Technology Group 17/42

4th order feedforward:Fff =

q1d + q2 + q3a + q4v

k12s + c

F

d

Rigid Body Feedforward

q1

q2

q4

q3

1

k12.s+c

1s

Velocity

1s

Position

1s

Jerk

x

Derivative of jerkprofile generator

PD

Controller

1s

Acceleration

MotionSystem

4th order model

Page 18: Trajectory Planning and Feedforward for Motion · PDF fileFaculty of Mechanical Engineering, Control Systems Technology Group 1/42 Trajectory Planning, Setpoint Generation and Feedforward

12

Faculty of Mechanical Engineering, Control Systems Technology Group 18/42

0 0.2 0.4 0.6 0.8 1 1.2−1000

0

1000

d [m

/s4]

0 0.2 0.4 0.6 0.8 1 1.2−50

0

50

j [m

/s3]

0 0.2 0.4 0.6 0.8 1 1.2−5

0

5

a [m

/s2]

0 0.2 0.4 0.6 0.8 1 1.20

0.5

1

1.5

v [m

/s]

0 0.2 0.4 0.6 0.8 1 1.20

0.5

1

x [m

]

4th order trajectoryplanner ??

• Point-to-point move(all derivatives zeroat start and end)

• Given: displacement x

and bounds d, , a and v

• Performance criteria !?

Page 19: Trajectory Planning and Feedforward for Motion · PDF fileFaculty of Mechanical Engineering, Control Systems Technology Group 1/42 Trajectory Planning, Setpoint Generation and Feedforward

12

Faculty of Mechanical Engineering, Control Systems Technology Group 19/42

4th order trajectory specification:

0 0.2 0.4 0.6 0.8 1−5

0

5

Second order trajectory profiles: 3 time periods, 2 sizes and max a

a [m

/s2]

ta

tv

ta

0 0.2 0.4 0.6 0.8 1−1000

−500

0

500

1000

Fourth order trajectory profiles: 15 time periods, 4 sizes and max d

d [m

/s4]

time [s]

td

tj

td

ta

td

tj

td

tv

td

tj

td

ta

td

tj

td

Page 20: Trajectory Planning and Feedforward for Motion · PDF fileFaculty of Mechanical Engineering, Control Systems Technology Group 1/42 Trajectory Planning, Setpoint Generation and Feedforward

12

Faculty of Mechanical Engineering, Control Systems Technology Group 20/42

0.39 0.4 0.41 0.42 0.43 0.44 0.45 0.460

500

1000

Fourth order trajectory profiles

d [m

/s4] d

0

0.39 0.4 0.41 0.42 0.43 0.44 0.45 0.46

−40

−20

0

j [m

/s3]

j0

0.39 0.4 0.41 0.42 0.43 0.44 0.45 0.460

1

2

a [m

/s2]

a0

0.39 0.4 0.41 0.42 0.43 0.44 0.45 0.461.45

1.5

v [m

/s] v

0

0.39 0.4 0.41 0.42 0.43 0.44 0.45 0.460.25

0.3

0.35

time [s]

x [m

]

x0

4th order calculations

d(t) = d0

(t) = d0t + 0

a(t) = 12d0t

2 + 0t + a0

v(t) = 16d0t

3 + 120t

2 + a0t + v0

x(t) = 124d0t

4 + 160t

3 + a0t2

+v0t + x0

Page 21: Trajectory Planning and Feedforward for Motion · PDF fileFaculty of Mechanical Engineering, Control Systems Technology Group 1/42 Trajectory Planning, Setpoint Generation and Feedforward

12

Faculty of Mechanical Engineering, Control Systems Technology Group 21/42

0 0.2 0.4 0.6 0.8 1 1.2−1000

0

1000Fourth order trajectory profiles, no bounds on j, a or v

d [m

/s4]

td

td

td

td

td

td

td

td

0 0.2 0.4 0.6 0.8 1 1.2−100

0

100

j [m

/s3]

0 0.2 0.4 0.6 0.8 1 1.2−10

0

10

a [m

/s2]

0 0.2 0.4 0.6 0.8 1 1.20

1

2

v [m

/s]

0 0.2 0.4 0.6 0.8 1 1.20

0.5

1

time [s]

x [m

]

4th order planningcalculate td

td only depends on d and x

⇒ td = 4√

x8d

v violated: td = 3√

v2d

a violated: td =√

ad

violated: td = d

Page 22: Trajectory Planning and Feedforward for Motion · PDF fileFaculty of Mechanical Engineering, Control Systems Technology Group 1/42 Trajectory Planning, Setpoint Generation and Feedforward

12

Faculty of Mechanical Engineering, Control Systems Technology Group 22/42

0 0.2 0.4 0.6 0.8 1 1.2−1000

0

1000

Fourth order trajectory profiles, final td

d [m

/s4]

td

td

td

td

td

td

td

td

0 0.2 0.4 0.6 0.8 1 1.2−50

0

50

j [m

/s3]

0 0.2 0.4 0.6 0.8 1 1.2

−2

0

2

a [m

/s2]

0 0.2 0.4 0.6 0.8 1 1.20

0.1

0.2

v [m

/s]

0 0.2 0.4 0.6 0.8 1 1.20

0.02

0.04

time [s]

x [m

]

4th order planningfinal td

Note:

• No bounds violated

• Final td always ≤ first td

• Consequently: x not reached

Page 23: Trajectory Planning and Feedforward for Motion · PDF fileFaculty of Mechanical Engineering, Control Systems Technology Group 1/42 Trajectory Planning, Setpoint Generation and Feedforward

12

Faculty of Mechanical Engineering, Control Systems Technology Group 23/42

0 0.2 0.4 0.6 0.8 1 1.2−1000

0

1000Fourth order trajectory profiles, no bounds on a or v

d [m

/s4]

td t

j

td

td

tj

td

td

tj

td

td t

j

td

0 0.2 0.4 0.6 0.8 1 1.2−50

0

50

j [m

/s3]

0 0.2 0.4 0.6 0.8 1 1.2−10

0

10

a [m

/s2]

0 0.2 0.4 0.6 0.8 1 1.20

1

2

v [m

/s]

0 0.2 0.4 0.6 0.8 1 1.20

0.5

1

time [s]

x [m

]

4th order planningcalculate t

Add periods of constant jerkuntil x is reached again

t follows from:

t3 + (5td)t2 + (8t2

d)t + (4t3

d− x

2dtd) = 0

v violated: t2 + 3tdt + 2t2d− v

dtd= 0

a violated: t = a − td

Page 24: Trajectory Planning and Feedforward for Motion · PDF fileFaculty of Mechanical Engineering, Control Systems Technology Group 1/42 Trajectory Planning, Setpoint Generation and Feedforward

12

Faculty of Mechanical Engineering, Control Systems Technology Group 24/42

0 0.2 0.4 0.6 0.8 1 1.2−1000

0

1000

Fourth order trajectory profiles, final td and t

j

d [m

/s4]

td t

j

td

td

tj

td

td

tj

td

td t

j

td

0 0.2 0.4 0.6 0.8 1 1.2−50

0

50

j [m

/s3]

0 0.2 0.4 0.6 0.8 1 1.2−5

0

5

a [m

/s2]

0 0.2 0.4 0.6 0.8 1 1.20

0.20.40.60.8

v [m

/s]

0 0.2 0.4 0.6 0.8 1 1.20

0.1

0.2

time [s]

x [m

]

4th order planningfinal td and t

Note:

• No bounds violated

• Final t always ≤ first t

• Consequently: x not reached

Page 25: Trajectory Planning and Feedforward for Motion · PDF fileFaculty of Mechanical Engineering, Control Systems Technology Group 1/42 Trajectory Planning, Setpoint Generation and Feedforward

12

Faculty of Mechanical Engineering, Control Systems Technology Group 25/42

0 0.2 0.4 0.6 0.8 1 1.2−1000

0

1000Fourth order trajectory profiles, no bounds on v

d [m

/s4]

td t

j

td

ta

td

tj

td

td

tj

td t

a

td t

j

td

0 0.2 0.4 0.6 0.8 1 1.2−50

0

50

j [m

/s3]

0 0.2 0.4 0.6 0.8 1 1.2−5

0

5

a [m

/s2]

0 0.2 0.4 0.6 0.8 1 1.20

1

2

v [m

/s]

0 0.2 0.4 0.6 0.8 1 1.20

0.5

1

time [s]

x [m

]

4th order planningcalculate ta

Add periods of constantacceleration until x reached

ta follows from:

{t2d

+ tdt} t2a +

{6t3d

+ 9t2dt + 3tdt

2} ta +

{8t4d

+ 16t3dt + 10t2

dt2 + 2tdt

3 − x

d} = 0

v violated: ta =v−2dt3

d−3dt2

dt−dtdt

2

dt2d+dtdt

Page 26: Trajectory Planning and Feedforward for Motion · PDF fileFaculty of Mechanical Engineering, Control Systems Technology Group 1/42 Trajectory Planning, Setpoint Generation and Feedforward

12

Faculty of Mechanical Engineering, Control Systems Technology Group 26/42

0 0.2 0.4 0.6 0.8 1 1.2−1000

0

1000

Fourth order trajectory profiles, final ta, t

d and t

j

d [m

/s4]

td t

j

td

ta

td

tj

td

td

tj

td t

a

td t

j

td

0 0.2 0.4 0.6 0.8 1 1.2−50

0

50

j [m

/s3]

0 0.2 0.4 0.6 0.8 1 1.2−5

0

5

a [m

/s2]

0 0.2 0.4 0.6 0.8 1 1.20

0.5

1

1.5

v [m

/s]

0 0.2 0.4 0.6 0.8 1 1.20

0.20.40.6

time [s]

x [m

]

4th order planningfinal td, t and ta

Note:

• No bounds violated

• Final ta always ≤ first ta

• Consequently: x not reached

For final step:determine obtained position xa

Page 27: Trajectory Planning and Feedforward for Motion · PDF fileFaculty of Mechanical Engineering, Control Systems Technology Group 1/42 Trajectory Planning, Setpoint Generation and Feedforward

12

Faculty of Mechanical Engineering, Control Systems Technology Group 27/42

0 0.2 0.4 0.6 0.8 1 1.2−1000

0

1000

Fourth order trajectory profiles, final tv, t

a, t

d and t

j

d [m

/s4]

td t

j

td

ta

td

tj

td t

v

td

tj

td t

a

td t

j

td

0 0.2 0.4 0.6 0.8 1 1.2−50

0

50

j [m

/s3]

0 0.2 0.4 0.6 0.8 1 1.2−5

0

5

a [m

/s2]

0 0.2 0.4 0.6 0.8 1 1.20

0.5

1

1.5

v [m

/s]

0 0.2 0.4 0.6 0.8 1 1.20

0.5

1

time [s]

x [m

]

4th order planningcalculate tv

Final step: add period ofconstant velocity until x reached

tv =x− xa

v

Finished: trajectory completelydetermined by 5 parameters !

Page 28: Trajectory Planning and Feedforward for Motion · PDF fileFaculty of Mechanical Engineering, Control Systems Technology Group 1/42 Trajectory Planning, Setpoint Generation and Feedforward

12

Faculty of Mechanical Engineering, Control Systems Technology Group 28/42

(Further) Implementation aspects

• Architecture

• Switching times

• Discrete time integration and synchronization

• First order filter implementation

Page 29: Trajectory Planning and Feedforward for Motion · PDF fileFaculty of Mechanical Engineering, Control Systems Technology Group 1/42 Trajectory Planning, Setpoint Generation and Feedforward

12

Faculty of Mechanical Engineering, Control Systems Technology Group 29/42

#17An introduction to Digital Control

How to deliver setpoint data?(cont.)

Solution 3: Split off setpoint interpolation

GUI

CPU

setpoint

interpol.controller

CPU

Trajectory

generation

??

Additional requirement : Datalink CPU1 to CPU2

Page 30: Trajectory Planning and Feedforward for Motion · PDF fileFaculty of Mechanical Engineering, Control Systems Technology Group 1/42 Trajectory Planning, Setpoint Generation and Feedforward

12

Faculty of Mechanical Engineering, Control Systems Technology Group 30/42

Architecture

2Reference

1Force

1v

1p

45j

1000d

5a

Ts

1

Trigger/EnableStep Profiles

p,v,a,j,d

Ts

t,d

Plan Trajectory

t,d

enable

djerk

start

Generate Profile

djerk FF

GenerateFeedforward

Demux

0

Derivative of jerk

Jerk

Acceleration

Velocity

Position

Page 31: Trajectory Planning and Feedforward for Motion · PDF fileFaculty of Mechanical Engineering, Control Systems Technology Group 1/42 Trajectory Planning, Setpoint Generation and Feedforward

12

Faculty of Mechanical Engineering, Control Systems Technology Group 31/42

0 0.2 0.4 0.6 0.8 1−1000

−500

0

500

1000

Fourth order trajectory profiles: intervals

d [m

/s4]

td

tj

td

ta

td

tj

td

tv

td

tj

td

ta

td

tj

td

0 0.2 0.4 0.6 0.8 1−1000

−500

0

500

1000

Switching timesd

[m/s

4]

t0

t1

t2

t3

t4

t5

t6

t7

t8

t9

t10

t11

t12

t13

t14

t15

0.65 0.7 0.75 0.8−1000

−500

0

500

1000

Synchronisation with sampling instances at 100Hz?

d [m

/s4]

time [s]

t8

t9

t10

td

tj

Switchingtimes

Page 32: Trajectory Planning and Feedforward for Motion · PDF fileFaculty of Mechanical Engineering, Control Systems Technology Group 1/42 Trajectory Planning, Setpoint Generation and Feedforward

12

Faculty of Mechanical Engineering, Control Systems Technology Group 32/42

Switching times synchronization

Make sure that each interval is a multiple of the sampling time Ts

Example:

td = 4

√x

8d

t′d = ceil

(tdTs

)× Ts

Correct d

d′ =x

8t′d4

Note that with t′d≥ td we must have d′ ≤ d

Page 33: Trajectory Planning and Feedforward for Motion · PDF fileFaculty of Mechanical Engineering, Control Systems Technology Group 1/42 Trajectory Planning, Setpoint Generation and Feedforward

12

Faculty of Mechanical Engineering, Control Systems Technology Group 33/42

4th order feedforward in discrete time ?

F

d

Rigid Body Feedforward

q1

q2

q4

q3

1

k12.s+c

1s

Velocity

1s

Position

1s

Jerk

x

Derivative of jerkprofile generator

PD

Controller

1s

Acceleration

MotionSystem

4th order model

Page 34: Trajectory Planning and Feedforward for Motion · PDF fileFaculty of Mechanical Engineering, Control Systems Technology Group 1/42 Trajectory Planning, Setpoint Generation and Feedforward

12

Faculty of Mechanical Engineering, Control Systems Technology Group 34/42

Ts-----z-1----

Velocity

Ts-----z-1----

Position

Ts-----z-1----

Jerk

xva

d

j

d

Derivative of Jerkprofile generator

Ts-----z-1----

Acceleration

Normalized discrete time fourth order profiles, compared with continuous time profiles

0 0.2 0.4 0.6 0.8 1

a: not synchronized

time [s]

x

v

a

j

d

0 0.2 0.4 0.6 0.8 1

b: synchronized, delayed by 2Ts

time [s]

Discrete timeintegration

Synchronization ofprofiles:d with 2Ts,j with 11

2Ts,a with Ts,v with 1

2Ts.

→ 12Ts ?

1Out1

z

1

Unit Delay

0.5

1In1

Page 35: Trajectory Planning and Feedforward for Motion · PDF fileFaculty of Mechanical Engineering, Control Systems Technology Group 1/42 Trajectory Planning, Setpoint Generation and Feedforward

12

Faculty of Mechanical Engineering, Control Systems Technology Group 35/42

Algebraic loop !!

Algebraic loop !!

NO algebraic loop !!

Continuous time

ZOH

ZOH

ZOH

y

y

y

y

y

c/k12

c/k12

c/k12

z

1

z

11/2Ts

-K-

Ts/(2*k12+c*Ts)

1

k12.s+c

1s

Ts(z+1)--------2(z-1)-------

Discrete-TimeIntegrator: Trapezoidal

u

u

u

u

u

1/k12

1/k12

1/k12

-K-

(2*k12-c*Ts)/(2*k12+c*Ts)

First order filterimplementation

Transfer function:

y =

Ts2k12+cTs

(z + 1)

z − 2k12−cTs2k12+cTs

u

Page 36: Trajectory Planning and Feedforward for Motion · PDF fileFaculty of Mechanical Engineering, Control Systems Technology Group 1/42 Trajectory Planning, Setpoint Generation and Feedforward

12

Faculty of Mechanical Engineering, Control Systems Technology Group 36/42

Digital 4th order feedforward

2Ref

1Force

T

z-1

velocity

preventnegative

T

z-1

position

-K-m1*m2

T

z-1

jerk

T

z-1

acceleration

ZOH2

ZOH1

z

1

Unit Delay41/z

1/z

1/z1/z

Trajectory

Quantizer

Profiles

d

Profile GeneratordJerk with error correction

Position[p2]

[p1][v][a]

[d]

[j]

[v]

[a]

[j]

[d]

[p2]

[p1]

[v]

[a]

[p2]

[j]

[d]

Feedforward Force0.5*Ts

0.5*Ts

0.5*Ts

Demux

-K-

1/k12*Ts/(2+c/k12*Ts)

-K-

1/c

-K-(m1+m2)*k12+m1*k2+m2*k1

-K-(m1+m2)*c+k1*k2+(k1+k2)*k12

-K-(k1+k2)*c

-K-

(2-c/k12*Ts)/(2+c/k12*Ts)position

velocity

acceleration

jerk

derivative of jerk

position correction

positionderivative of jerk

Page 37: Trajectory Planning and Feedforward for Motion · PDF fileFaculty of Mechanical Engineering, Control Systems Technology Group 1/42 Trajectory Planning, Setpoint Generation and Feedforward

12

Faculty of Mechanical Engineering, Control Systems Technology Group 37/42

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

Page 38: Trajectory Planning and Feedforward for Motion · PDF fileFaculty of Mechanical Engineering, Control Systems Technology Group 1/42 Trajectory Planning, Setpoint Generation and Feedforward

12

Faculty of Mechanical Engineering, Control Systems Technology Group 38/42

0 0.5 1 1.5 2 2.5 3−6

−4

−2

0

2

4

6

8x 10

−5Open loop fourth order feedforward response of servo error with plant variations

Dashed line gives nominal rigid body FF response

time [s]

Pos

ition

err

or [m

]

SimulationresultsRobustnessagainst variationsin additionalparameters

Parameter Value Unit Variationm1 20 Kg m1 ∈ {15· · ·25},m2 10 Kg m2 =30−m1

k1 10 Ns/m k1 ∈ {5· · ·15},k2 10 Ns/m k2 =20−k1

c 6 ·105 N/m ±33 %k12 500 Ns/m ±100 %

F m 1

x 1

k 1

m 2

x 2

k 2

c

k 1 2

Page 39: Trajectory Planning and Feedforward for Motion · PDF fileFaculty of Mechanical Engineering, Control Systems Technology Group 1/42 Trajectory Planning, Setpoint Generation and Feedforward

12

Faculty of Mechanical Engineering, Control Systems Technology Group 39/42

Simulationresults

Effect of orderof feedforward

0 0.5 1 1.5 2 2.5 3−6

−4

−2

0

2

4

6

8x 10−5

Open loop feedforward response of servo errorOptimal second, third and fourth order feedforward

time [s]

Pos

ition

err

or [m

]

second order FFthird order FFfourth order FF

Page 40: Trajectory Planning and Feedforward for Motion · PDF fileFaculty of Mechanical Engineering, Control Systems Technology Group 1/42 Trajectory Planning, Setpoint Generation and Feedforward

12

Faculty of Mechanical Engineering, Control Systems Technology Group 40/42

Simulationresults

Discrete timevs. continuoustime

0 0.5 1 1.5 2 2.5 3−3

−2

−1

0

1

2

3

4x 10

−5

time [s]

Pos

ition

err

or [m

]

Digital ff response: 4th order, minimal c, Ts=5e−3

0.3 0.35 0.4 0.45−4−2

02468

x 10−6

Page 41: Trajectory Planning and Feedforward for Motion · PDF fileFaculty of Mechanical Engineering, Control Systems Technology Group 1/42 Trajectory Planning, Setpoint Generation and Feedforward

12

Faculty of Mechanical Engineering, Control Systems Technology Group 41/42

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-mechanicalmotion systems)

• Complete derivation (also for third order) available

• Simulink toolbox ‘motion’ available

• Report: ‘Trajectory planning and feedforward design for elec-tromechanical motion systems’ available (DCT 2003-18)

Page 42: Trajectory Planning and Feedforward for Motion · PDF fileFaculty of Mechanical Engineering, Control Systems Technology Group 1/42 Trajectory Planning, Setpoint Generation and Feedforward

12

Faculty of Mechanical Engineering, Control Systems Technology Group 42/42

Exercises

• .ZIP file available in OUTLOOK for Matlab release 12.1

• See exercises.pdf

– MAKE# and PROFILE# .M files

– The ‘motion’ library

– Feedforward in continuous and discrete time

– Real-time implementation