trajectory planning and feedforward for motion · pdf filefaculty of mechanical engineering,...
TRANSCRIPT
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
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
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
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
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.
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
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
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
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
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
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
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
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 !
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.
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}
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
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
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 !?
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
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
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
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
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
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
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
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
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 !
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
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
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
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
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
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
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
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
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
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
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
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
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
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)
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