Trajectory Planning, Setpoint Generation and Feedforward for Motion Systems

Transcription

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

2 2 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 Faculty of Mechanical Engineering, Control Systems Technology Group 2/42

3 2 Industrial motion control Robots Pick-and-place units Wafersteppers Motion Control Tasks: Safety, Communication, etc. System compensation Trajectory planning Feedforward control Feedback control Industrial Robot Faculty of Mechanical Engineering, Control Systems Technology Group 3/42

4 2 Main digital control functions Error handling Application interface I/F Setpoints + - H c I/F Integrated functions Hardware interfaces I/F An introduction to Digital Control # Faculty of Mechanical Engineering, Control Systems Technology Group 4/42

5 . N 2 Simple rigid-body feedforward DOF rigid-body model mẍ + kẋ = F F ff = ma + kv x is position, m is (equivalent) mass or inertia, k is viscous damping, F is actuator force and F ff is feedforward force. Faculty of Mechanical Engineering, Control Systems Technology Group 5/42

6 2 Feedforward based on inverse model 2 ms x s K p K v s 2 ms x Digital Motion Control Feedforward design 8 Faculty of Mechanical Engineering, Control Systems Technology Group 6/42

7 2 Trajectory profiles for rigid-body feedforward a [m/s2] v [m/s] 5 Second order trajectory profiles F ff = ma + kv x [m] time [s] Faculty of Mechanical Engineering, Control Systems Technology Group 7/42

8 2 General controller implementation Setpoint generator + - Setpoint acceleration Setpoint position Setpoint direction d dt Setpoint velocity Kp Kv Kfst Kfc Kfv Kfa + + measured position Ki + + static feedforward Coulomb friction compensation velocity feedforward acceleration feedforward -2 + LowPass + + LeadLag integrator anti-windup Notch / LeadLag Saturation An introduction to Digital Control #38 Faculty of Mechanical Engineering, Control Systems Technology Group 8/42

9 2 Second order feedforward: a m Acceleration profile generator v k F s s PD m.s 2+k.s x Velocity Position Controller 2nd order model Faculty of Mechanical Engineering, Control Systems Technology Group 9/42

10 2 Trajectory planning performance (p2p) a [m/s2] v [m/s] x [m] 5 Second order trajectory profiles time [s] Timing: minimal trajectory execution time Realizability: constrained dynamics (ā and v) Accuracy: trajectory ends at desired end position ( x) Complexity: calculation time Reliability: always valid solution Implementation: discretization, quantization Faculty of Mechanical Engineering, Control Systems Technology Group /42

11 2 Profiles given ā, v and x:. Forget v: x = 2 2āt2 tā = xā t x = 2tā 2. Calculate maximal velocity: ˆv := ā tā 3. ˆv > v?; true: tā = v ā, false: t ā = 2 t x 4. xā := 2 2āt2 ā x 5. t v = ( x x ā) v Faculty of Mechanical Engineering, Control Systems Technology Group /42

12 2 Properties of trajectory planning algorithm a [m/s2] v [m/s] x [m] 5 Second order trajectory profiles t a time [s] tv t a Timing: minimal, determined by tā and t v Realizablity: guaranteed by ā and v Accuracy: exact within machine accuracy Complexity: low Reliability: always valid solution Implementation: later Faculty of Mechanical Engineering, Control Systems Technology Group 2/42

13 2 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! Faculty of Mechanical Engineering, Control Systems Technology Group 3/42

14 .? N 2 4th order model for electro-mechanical motion system N DOF 4th order model x and x 2 are actuator and load position, m, m 2 masses, k, k 2 viscous damping, c spring stiffness, k 2 internal viscous damping, F is actuator force. Faculty of Mechanical Engineering, Control Systems Technology Group 4/42

15 2 4th order feedforward Equations of motion: { m ẍ = k ẋ c(x x 2 ) k 2 (ẋ ẋ 2 ) + F m 2 ẍ 2 = k 2 ẋ 2 + c(x x 2 ) + k 2 (ẋ ẋ 2 ) Laplace transform and substitution: F = q s 4 + q 2 s 3 + q 3 s 2 + q 4 s x 2 k 2 s + c Feedforward force calculation: F ff = q = m m 2 q 2 = (m + m 2 )k 2 + m k 2 + m 2 k q 3 = (m + m 2 )c + k k 2 + (k + k 2 )k 2 q 4 = (k + k 2 )c k 2 s + c {q d + q 2 j + q 3 a + q 4 v} Faculty of Mechanical Engineering, Control Systems Technology Group 5/42

16 d [m/s4] j [m/s3] 5 Trajectory profiles for 4th order feedforward a [m/s2] v [m/s] F ff = q d + q 2 j + q 3 a + q 4 v k 2 s + c x [m] Faculty of Mechanical Engineering, Control Systems Technology Group 6/42

17 2 4th order feedforward: Derivative of jerk profile generator d F ff = q d + q 2 j + q 3 a + q 4 v k 2 s + c q q2 Rigid Body Feedforward q3 q4 k2.s+c F s s s s PD Motion System x Jerk Acceleration Velocity Position Controller 4th order model Faculty of Mechanical Engineering, Control Systems Technology Group 7/42

18 2 4th order trajectory planner?? d [m/s4] Point-to-point move (all derivatives zero at start and end) Given: displacement x and bounds d, j, ā and v Performance criteria!? j [m/s3] a [m/s2] v [m/s] x [m] Faculty of Mechanical Engineering, Control Systems Technology Group 8/42

19 2 4th order trajectory specification: a [m/s2] 5 Second order trajectory profiles: 3 time periods, 2 sizes and max a t a t v d [m/s4] t a Fourth order trajectory profiles: 5 time periods, 4 sizes and max d t t t t 5 d d d d tj t a t j t v t j t a tj time [s] Faculty of Mechanical Engineering, Control Systems Technology Group 9/42

20 2 4th order calculations d(t) = d j(t) = d t + j d [m/s4] j [m/s3] d Fourth order trajectory profiles j a(t) = 2 d t 2 + j t + a a [m/s2] a v(t) = 6 d t j t 2 + a t + v v [m/s].5 v x(t) = 24 d t j t 3 + a t v t + x x [m].3 x time [s] Faculty of Mechanical Engineering, Control Systems Technology Group 2/42

21 2 4th order planning calculate t d t d only depends on d and x v violated: t d = 4 t d = 3 ā violated: t d = j violated: t d = j d x 8 d v 2 d ā d d [m/s4] j [m/s3] Fourth order trajectory profiles, no bounds on j, a or v a [m/s2] x [m] Faculty of Mechanical Engineering, Control Systems Technology Group 2/ v [m/s] time [s]

22 2 4th order planning final t d d [m/s4] Fourth order trajectory profiles, final t d t d Note: No bounds violated Final t d always first t d Consequently: x not reached j [m/s3] a [m/s2] v [m/s] x [m] time [s] Faculty of Mechanical Engineering, Control Systems Technology Group 22/42

23 2 4th order planning calculate t j Add periods of constant jerk until x is reached again t j follows from: t 3 j + (5t d)t 2 j + (8t 2 d)t j + (4t 3 d v violated: ā violated: t 2 j + 3t dt j + 2t 2 d v dt d t j = ā j t d d [m/s4] x 2 dt d) = = Fourth order trajectory profiles, no bounds on a or v t t t tj j d tj j t t t d d d j [m/s3] a [m/s2] x [m] Faculty of Mechanical Engineering, Control Systems Technology Group 23/ v [m/s] time [s]

24 2 4th order planning final t d and t j d [m/s4] Fourth order trajectory profiles, final and t j t t t t j j d j t j t t t d d d Note: No bounds violated Final t j always first t j Consequently: x not reached j [m/s3] a [m/s2] v [m/s] x [m] time [s] Faculty of Mechanical Engineering, Control Systems Technology Group 24/42

25 2 4th order planning calculate tā Add periods of constant acceleration until x reached tā follows from: {t 2 d + t dt j } t 2 ā + {6t 3 d + 9t 2 dt j + 3t dt 2 j} tā + {8t 4 d + 6t 3 dt j + t 2 dt 2 j + 2t dt 3 j x d} = v violated: tā = v 2 dt 3 d 3 dt 2 dt j dt dt 2 j dt 2 d+ dt dt j d [m/s4] Fourth order trajectory profiles, no bounds on v t t t t j t a j ta d j t j t t t d d d j [m/s3] a [m/s2] x [m] v [m/s] Faculty of Mechanical Engineering, Control Systems Technology Group 25/ time [s]

26 2 4th order planning final t d, t j and tā Note: No bounds violated Final tā always first tā Consequently: x not reached For final step: determine obtained position xā d [m/s4] Fourth order trajectory profiles, final t a, and t j t t t t j t a j ta d j t j t t t d d d j [m/s3] a [m/s2] v [m/s] x [m] Faculty of Mechanical Engineering, Control Systems Technology Group 26/ time [s]

27 2 4th order planning calculate t v Final step: add period of constant velocity until x reached t v = x x ā v Finished: trajectory completely determined by 5 parameters! d [m/s4] Fourth order trajectory profiles, final t v, t a, and t j t t t t j t a tv j ta d j t j t t t d d d j [m/s3] a [m/s2] v [m/s] x [m] Faculty of Mechanical Engineering, Control Systems Technology Group 27/ time [s]

28 2 (Further) Implementation aspects Architecture Switching times Discrete time integration and synchronization First order filter implementation Faculty of Mechanical Engineering, Control Systems Technology Group 28/42

29 2 How to deliver setpoinata?(cont.) Solution 3: Split off setpoint interpolation GUI Trajectory generation setpoint interpol. controller CPU?? CPU Additional requirement : Datalink CPU to CPU2 An introduction to Digital Control #7 Faculty of Mechanical Engineering, Control Systems Technology Group 29/42

30 2 Architecture Force p 2 Reference v 5 p,v,a,j,d Derivative of jerk a 45 j Ts t,d Ts Plan Trajectory t,d djerk enable start Generate Profile djerk FF Generate Feedforward emu Jerk Acceleration d Velocity Trigger/Enable Step Position Profiles Faculty of Mechanical Engineering, Control Systems Technology Group 3/42

31 2 Switching times d [m/s4] 5 t j t a Fourth order trajectory profiles: intervals t j t v t j t a t j Switching times d [m/s4] 5 5 t t t 2 t 3 t 4 t 5 t6 t 7 t 8 t 9 t t t 2 t 3 t 4 t 5 d [m/s4] Synchronisation with sampling instances at Hz? 5 t 8 t t 9 j t time [s] Faculty of Mechanical Engineering, Control Systems Technology Group 3/42

32 2 Switching times synchronization Make sure that each interval is a multiple of the sampling time T s Example: x t d = 4 8 ( ) d t t d = ceil d T s T s Correct d d = x 8t d 4 Note that with t d t d we must have d d Faculty of Mechanical Engineering, Control Systems Technology Group 32/42

33 2 4th order feedforward in discrete time? Derivative of jerk profile generator d q q2 Rigid Body Feedforward q3 q4 k2.s+c F s s s s PD Motion System x Jerk Acceleration Velocity Position Controller 4th order model Faculty of Mechanical Engineering, Control Systems Technology Group 33/42

34 2 Discrete time integration Synchronization profiles: d with 2T s, j with 2 T s, a with T s, v with 2 T s. 2 T s? In z Unit Delay.5 Out of x v a j d d Derivative of Jerk profile generator Ts z- Jerk j Ts z- a Ts z- Acceleration Velocity v Ts z- Position Normalized discrete time fourth order profiles, compared with continuous time profiles a: not synchronized b: synchronized, delayed by 2Ts d x time [s] time [s] Faculty of Mechanical Engineering, Control Systems Technology Group 34/42

35 2 First order filter implementation u u /k2 Continuous time k2.s+c s y y c/k2 u ZOH /k2 Algebraic loop!! Ts(z+) (z-) Discrete-Time Integrator: Trapezoidal y c/k2 Algebraic loop!! Transfer function: y = T s 2k 2 +ct s (z + ) z 2k 2 ct s u 2k 2 +ct s u u ZOH ZOH /k2 Ts NO algebraic loop!! -K- Ts/(2*k2+c*Ts) z c/k2 z /2 y y -K- (2*k2-c*Ts)/(2*k2+c*Ts) Faculty of Mechanical Engineering, Control Systems Technology Group 35/42

36 2 Digital 4th order feedforward Profile Generator djerk with error correction d em ZOH [d] T z- jerk [j] T z- acceleration [a] T z- velocity derivative of jerk [v] T z- position [p] [p2] position Position ZOH2 position correction Quantizer prevent negative [d] [j] [a] [v] [p] [p2] derivative of jerk jerk acceleration velocity position Profiles [d] [j] [a] [v] [p2] /z /z /z /z.5*ts.5*ts.5*ts K- m*m2 (m+m2)*k2+m*k2+m2*k (m+m2)*c+k*k2+(k+k2)*k2 (k+k2)*c -K- /c -K- /k2*ts/(2+c/k2*ts) -K- -K- -K- z Unit Delay4 -K- (2-c/k2*Ts)/(2+c/k2*Ts) Feedforward Force Force Trajectory 2 Ref Faculty of Mechanical Engineering, Control Systems Technology Group 36/42

37 2 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: Faculty of Mechanical Engineering, Control Systems Technology Group 37/42 j T s

38 2 Simulation results Robustness against variations in additional parameters Position error [m] Open loop fourth order feedforward response of servo error with plant variations 8 x 5 Dashed line gives nominal rigid body FF response Parameter Value Unit Variation m 2 Kg m {5 25}, m 2 Kg m 2 =3 m k Ns/m k {5 5}, k 2 Ns/m k 2 =2 k c 6 5 N/m ±33 % k 2 5 Ns/m ± % time [s] Faculty of Mechanical Engineering, Control Systems Technology Group 38/42

39 2 Simulation results 8 x 5 6 Open loop feedforward response of servo error Optimal second, third and fourth order feedforward second order FF third order FF fourth order FF 4 Effect of order of feedforward Position error [m] time [s] Faculty of Mechanical Engineering, Control Systems Technology Group 39/42

40 2 Simulation results Discrete time vs. continuous time Position error [m] 4 x Digital ff response: 4th order, minimal c, Ts=5e 3 x time [s] Faculty of Mechanical Engineering, Control Systems Technology Group 4/42

41 2 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 Report: Trajectory planning and feedforward design for electromechanical motion systems available (DCT 23-8) Faculty of Mechanical Engineering, Control Systems Technology Group 4/42

42 2 Exercises.ZIP file available in OUTLOOK for Matlab release 2. See exercises.pdf MAKE# and PROFILE#.M files The motion library Feedforward in continuous and discrete time Real-time implementation Faculty of Mechanical Engineering, Control Systems Technology Group 42/42