Control of robot manipulators

Transcription

1 Control of robot manipulators Claudio Melchiorri Dipartimento di Elettronica, Informatica e Sistemistica (DEIS) Università di Bologna claudio.melchiorri@unibo.it C. Melchiorri (DEIS) Control 1 / 85

2 Summary 1 Robot Position Control Introduction Cascade Control Position feedback Position and velocity feedback Position, velocity and acceleration feedback Feed-forward control Centralized position control PD controller with gravity compensation Inverse dynamics control C. Melchiorri (DEIS) Control 2 / 85

3 Introduction Robot control Control problem: definition of the input signals for the joints (e.g. torques or actuator input voltages) in order to achieve a predefined behavior for the manipulator. The achievable performances can be very different because of: The many control techniques available to solve such a problem The hardware used to implement the control algorithms The mechanical configuration of the robot (anthropomorphic, cartesian,...) The robot performances are mainly influenced by the mechanical design and by the actuation system. For example: The cartesian configuration decouples the dynamics of the joints; DC motor with gearboxes have a linear dynamics that results decoupled from the non-linear dynamics of the robot. However, gearboxes usually introduce non linear effects such as dead-zones, friction, elasticity,...; Direct Drive motors on one hand ensure better performances and do not introduce non-linearities in the transmission chain; on the other hand a more relevant dynamic coupling between joints is present, and the (nonlinear) load dynamics is directly applied to the actuators without reduction effects. C. Melchiorri (DEIS) Control 3 / 85

4 Introduction Robot control Control problems: Control of the robot s motion (position control schemes); joint-space control workspace control. Control of the interaction with the workspace (force control schemes). Control schemes: Decentralized (or independent) control schemes (SISO) Centralized control schemes (MIMO). Dynamic model of a manipulator: M(q) q+c(q, q) q+d q+g(q) = τ +J T (q)f a Control problem: define the generalized forces τ to be applied to the joints in order to obtain a desired trajectory q d (t). C. Melchiorri (DEIS) Control 4 / 85

5 Joint space control Robot Position Control Consideration #1 q m τ m q τ Actuator Gearbox Joint Actuators: positions q m(t), torques τ m(t); Gearboxes: reduction ratio K r; Joints: positions q(t), generalized forces τ(t); K r q = q m τ m = K 1 r τ K r is a diagonal matrix with elements 1. C. Melchiorri (DEIS) Control 5 / 85

6 Joint space control Robot Position Control Consideration #2 The diagonal of the matrix M(q) is composed by two kinds of elements Inertia terms that do not depend on the robot s configuration Terms that depend on the robot s configuration. Therefore: M(q) = M+ M(q) where M is a diagonal matrix with constant elements (i.e. the mean values of the joints inertia). From the robot dynamic model it follows τ m = (K 1 r MK 1 r ) q m +D m q m +d where D m = K 1 r DK 1 r is the matrix collecting the motors friction coefficients, and d = (K 1 r M(q)K 1 r ) q m +(K 1 r C(q, q)k 1 r ) q m +K 1 r g(q) is a term that can be considered as a disturbance. C. Melchiorri (DEIS) Control 6 / 85

7 Joint space control Robot Position Control q m = K r ˆM 1 K r(τ m D m q m d) = τ m d r C(.,.)K 1 r K 1 K 1 r r M(.)K 1 Non-linear System K 1 K 1 r g(.) r K 1 r K 1 r K 1 r - q K r ˆM 1 m K r q m q m - D m Linear system A manipulator (+ the actuation system) can be regarded as the composition of: a system with input q m, q m,q m and output d, non linear and with couplings a system with input τ m and output q m, linear and decoupled C. Melchiorri (DEIS) Control 7 / 85

8 Decentralized control Robot Position Control Each joint is considered independently, and the term d is considered as an external disturbance. These considerations can be applied with proficiency when there is no direct coupling between the actuator and the joint (i.e. K r I). Joint independent control Let us consider a DC motor. Electric dynamics (armature) d i A (t) v A (t) = R A i A (t)+l A +v M (t) d t where v A, i A, R A and L A are the voltage, the electric current, and the armature resistance and inductance respectively. Electro-mechanical coupling τ m (t) = k t i A (t) where k t is the electro-mechanical constant of the motor. C. Melchiorri (DEIS) Control 8 / 85

9 Decentralized control Robot Position Control Mechanical dynamics (rotor) d ω m (t) I = τ m (t) b ω m (t) d t where I is the moment of inertia of the motor and of an eventually connected payload and b the coefficient of friction of the overall system. Mechano-electrical coupling v M (t) = k v ω m (t) Numerically, it results k v = k t if all the quantities are expressed in the International System. C. Melchiorri (DEIS) Control 9 / 85

10 Decentralized control Robot Position Control Using Laplace transformation, it results: V A (s) = R A I A (s)+sl A I A (s)+v M (s) C m (s) = k t I A (s) s I Ω m (s) = C m (s) b Ω m (s) V M (s) = k v Ω m (s) The two dynamics are then expressed by the transfer functions: I A (s) = V A(s) V M (s) R A +s L A Ω m (s) C(s) = 1 b +s I C. Melchiorri (DEIS) Control 10 / 85

11 Decentralized control Robot Position Control V A (s) 1 I A (s) C m(s) 1 Ω 1 k t m(s) Θ m(s) + R A +sl A b +s I s V M (s) k v Considering both the transfer functions: If b 0: Ω m (s) = k t (b +s I)(R A +s L A )+k t k v V A (s) = G(s)V A (s) k t G(s) = s 2 L A I +s R A I +k t k v C. Melchiorri (DEIS) Control 11 / 85

12 Decentralized control Robot Position Control The system has two poles: p 1,2 = R A I ± (R A I) 2 4 L A I k tk v 2 L A I If L A is sufficiently small and such that (R A I) 2 4 L A I k tk v > 0 then the two poles are real and negative. Moreover, if L A R2 A I k tk v and 1 x 1 x/2 (for small values of x), it results p 1 ktkv p 2 R A R A I L A where p 1 is the electromechanical pole and p 2 the electrical pole. Then k t/(l A I) G(s) = (s + ktkv )(s + R A R A L L A ) = 1/k t (1+sT m)(1+st e) T m = R A I k tk v T e = L A mechanical time constant electrical time constant Usually T m T e R A C. Melchiorri (DEIS) Control 12 / 85

13 Decentralized control Robot Position Control Since usually T m T e (T e is negligible if compared with T m ), then the following simpler scheme can be considered: d V c V a k G t v Θ m Θm Θ m R A I s s k v where G v represents the voltage gain of the motor drive unit. In this case, the input-output transfer function is G(s) = Θ(s) V A (s) = k m s(1+st m ) where k m = 1/k v T m = R AI k tk v velocity-voltage gain time constant of the motor C. Melchiorri (DEIS) Control 13 / 85

14 Decentralized control with payload If a gearbox is present between the motor and the load, then: ω m τ m Gearbox ω τ The power at the input of the gearbox equals the power at the output: τ m (t) ω m (t) = τ(t) ω(t) (ω < ω m ) and, since K r ω = ω m then τ(t) = K r τ m (t) (τ > τ m ). Thus, considering the variables before the gearbox (motor side) and using the Laplace transforms, one gets C r,m (s) = C r = 1 (b c Ω(s)+s I c Ω(s)) = 1 K r K r Kr 2 (b c +s I c )Ω m (s) C. Melchiorri (DEIS) Control 14 / 85

15 Decentralized control - Presence of a payload By considering both the motor and the load, one obtains: which leads to: C(s) = C m (s)+c r,m (s) = (bω m (s)+s IΩ m (s))+ 1 Kr 2 (b c +s I c )Ω m (s) = (b + b c )Ω m (s)+s(i + I c )Ω m (s) K 2 r = b T Ω m (s)+s I T Ω m (s) K 2 r b T = b + b c K 2 r Ω m (s) = C(s) b T +s I T I T = I + I c K 2 r C. Melchiorri (DEIS) Control 15 / 85

16 Cascade control Robot Position Control d y u R(s) G 1 (s) v G 2 (s) y - A cascade control scheme can be profitably used if: = the dynamics of the process to be controlled can be schematized with two (or more) distinct dynamics G 1 (s) and G 2 (s) = G 1 (s) is faster than G 2 (s) = it is possible to measure the input variable v of G 2 (s) In these cases, better performances can often be obtained by using two or more control loops in a cascade configuration. y d v u R 2 (s) d R 1 (s) G 1 (s) d v G 2 (s) y - - C. Melchiorri (DEIS) Control 16 / 85

17 Cascade control Robot Position Control y d R 2 (s) R 1 (s) u G 1 (s) d v G 2 (s) y - - With a cascade control scheme, the following positive features can be obtained: 1 local compensation of disturbances acting in the inner control loops, e.g. the disturbance d can be compensated before it affects the output y; 2 the dynamics between v d and v (with control) is faster than the dynamics between u e v (without control); 3 the internal loop is more robust with respect to variations of the parameters; 4 it is possible to apply anti-saturation techniques to the variables v d and v; 5 it is possible to obtain a predefined dynamic behavior between v d and y so that the design of R 2(s) is easier. In particular, if G 1(s) is faster than G 2(s), it is possible to define R 1(s) so that, for the design of R 2(s), it is possible to consider v v d (the design process is easier). C. Melchiorri (DEIS) Control 17 / 85

18 Cascade control Robot Position Control With a single controller R(s), the overall transfer function is: G t0(s) = R(s)G1(s)G2(s) 1+R(s)G = 1 1(s)G 2(s) 1 1+ R(s)G 1(s)G 2(s) By using the cascade control scheme, the system transfer function is: (1) G t1(s) = = R 1(s)R 2(s)G 1(s)G 2(s) 1+R 1(s)G 1(s)+R 1(s)R 2(s)G 1(s)G 2(s) 1+ 1 R 2(s)G 2(s) 1 ( 1+ 1 ) (2) R 1(s)G 1(s) y d v u R 2 (s) d R 1 (s) G 1 (s) d v G 2 (s) y - - C. Melchiorri (DEIS) Control 18 / 85

19 Cascade control Robot Position Control In a cascade control configuration, there are more degrees of freedom than in standard control schemes. Moreover, it is easier to affect the internal dynamics of the system. To conclude, if it is possible to assume that R 1 (s)g 1 (s) 1 in the frequency range interested by G 2 (s), then the overall transfer function mostly depends on the external control loop: G t1 (s) 1/(1+1/(R 2 G 2 )). More in general, in case of n internal loops, the system control function is: G t(s) = = R 1(s)R 2(s) R n(s)g 1(s)G 2(s)...G n(s) 1+R 1(s)G 1(s)+R 1(s)G 1(s)R 2(s)G 2(s)+...+R 1(s)G 1(s) R n(s)g n(s) 1+ ( 1 1+ R n(s)g n(s) 1 1 R n 1(s)G n 1(s) ( ( ))) 1 R 1(s)G 1(s) C. Melchiorri (DEIS) Control 19 / 85

20 Feedback control Reduction of the effects of the disturbance d: high gain; integral action to annihilate the steady state error (e.g. the effect of the gravitational term). Possible solution: to use a PI controller C(s) = K c 1+sT c s C. Melchiorri (DEIS) Control 20 / 85

21 Feedback control Robot Position Control Possible control scheme d θm θm θ m θ r V A k C P (s) C V (s) C A (s) t R A I k v k TP k TV k TA where d = dr A k T and C P (s) = position control C V (s) = velocity control C A (s) = acceleration control In the inner control loop, an integral action must be present. C. Melchiorri (DEIS) Control 21 / 85

22 Position feedback Robot Position Control θ r - K P 1+sT P s d θ k m θm t 1 1 R A I θ m s s - k v k TP 1+sT P C P (s) = K P C V (s) = 1 C A (s) = 1 s k TV = k TA = 0 C. Melchiorri (DEIS) Control 22 / 85

23 Position feedback Robot Position Control Transfer functions of the direct path, of the feedback path and of the closed loop. (1) C(s)G(s) = kmk P(1+sT P ) s 2 (1+sT m) (3) Θ(s) Θ r(s) = 1+ 1 k TP s 2 (1+sT m) k mk P k TP (1+sT P ) = (2) H(s) = k TP 1 k TP (1+sT P ) (1+ 2δs ω n + s2 ωn 2 )(1+sτ) There are three poles: δ, ω n are respectively the damping coefficient and the natural frequency of the pair of complex poles, and 1/τ defines a real pole. The value of δ,ω n,τ depends on the choices made for the control parameters: if T P < T m the system is unstable; if T P > T m then 1/δω n > T P > τ; if T P T m then, in case of high closed-loop gain, δω n > 1/τ 1/T P and the zero in 1/T P almost cancels out the real pole; anyway, it results δω n 1/2 T m. C. Melchiorri (DEIS) Control 23 / 85

24 Position feedback Robot Position Control 1. Case T P < T m 100 root locus o + x + x seconds Root locus and typical step response of the system for k = k m K P k TP T P /T m C. Melchiorri (DEIS) Control 24 / 85

25 Position feedback Robot Position Control 2. Case T P > T m = δω n < 1/T P < 1/τ root locus + x o + x seconds Root locus and typical step response of the system for k = k m K P k TP T P /T m Remark: the root locus has an asymptote at x = σ a = 1 2 [ 1 T p 1 T m ]. C. Melchiorri (DEIS) Control 25 / 85

26 Position feedback Robot Position Control 3. Case T P T m = δω n > 1/τ 1/T P (for high values of K) 40 root locus x+ o + x seconds Root locus and typical step response of the system for k = k m K P k TP T P /T m C. Melchiorri (DEIS) Control 26 / 85

27 Position feedback Robot Position Control The disturbance-output transfer function is: sr A Θ(s) D(s) = k t K P k TP (1+sT P ) s 1+ 2 (1+sT m ) k m K P k TP (1+sT P ) Then, by choosing high values of K P, it is possible to reduce the effects of d(t) during the transient. There are two complex conjugate poles in ( δω n ±i 1 δ 2 ω n ), a real pole in ( 1/τ) and a zero in the origin due to the use of the PI controller. For this reason, all the constant disturbances (e.g. the gravity terms) are compensated for when the robot reaches a predefined configuration. C. Melchiorri (DEIS) Control 27 / 85

28 Position feedback Robot Position Control For comparison purposes, let us compute the transfer function of the system without and with the control action: Θ(s) D (s) Θ(s) D (s) k m = s(1+st m ) sk m = s 2 (1+sT m )+K P k m k TP (1+sT P ) The disturbance reduction factor X R due to the position feedback depends on K P and results: X R = K P k PT On the other hand, to avoid oscillations in the closed loop system, it is not convenient to assign high values of K P. C. Melchiorri (DEIS) Control 28 / 85

29 Position feedback Robot Position Control Output with an impulse disturbance (without and with control): An estimation of the time necessary to compensate the disturbance effects on the output signal is given by the output recovery time T R = max{t P, seconds 1 δω n } C. Melchiorri (DEIS) Control 29 / 85

30 Position and velocity feedback θ r - K P k TP - K V 1+sT V s k TV d k θ m t 1 1 R A I θm θ m s s - k v C P (s) = K P 1+sT V C V (s) = K V s k TA = 0 C A (s) = 1 C. Melchiorri (DEIS) Control 30 / 85

31 Position and velocity feedback Transfer functions of the direct and feedback paths respectively: C(s)G(s) = k mk P K V (1+sT V ) s 2 (1+sT m ) k TV H(s) = k TP (1+s ) K P k TP It might be convenient to choose the zero of the controller ( 1/T V ) in order to cancel the real pole in 1/T m (i.e. by assigning T V = T M ). In this case: C(s)G(s) = k mk P K V s 2 and the overall transfer function is: Θ(s) Θ r (s) = C(s)G(s) 1+H(s)C(s)G(s) = 1 k TP 1+ sk TV K P k TP + s 2 k m K P k TP K V = 1 k TP 1+ 2δs ω n + s2 ω 2 n C. Melchiorri (DEIS) Control 31 / 85

32 Position and velocity feedback With proper choices for the control parameters, it is possible to obtain desired values for δ and ω n. If these are given, then the control gains are computed as: K V k TV = 2δωn k m K P k TP K V = ω2 n k m Note that k TV,k TP are known constants. The disturbance-output transfer function is: sr a Θ(s) D(s) = k tk P k TP K V (1+sT m) 1+ sk TV + s 2 K P k TP k mk P k TP K V This transfer function has a disturbance reduction factor given by: X R = K P k TP K V There are two complex poles with real part δω n(= k mk V k TV ), a real pole in s = 1/T m, and a zero in the origin due to the PI controller. The estimated time to compensate the disturbance effects on the output is: 1 T R = max{t m, } δω n better than in the previous case since T m T P, and the real part of the poles is not constrained by δ ω n < 1/2T m. C. Melchiorri (DEIS) Control 32 / 85

33 Position and velocity feedback Root locus and typical step response of the system with k = k m K V k TV root locus o x seconds For the plot of the root locus, notice that the loop gain is ( ) s + K Pk TP { k H(s)C(s)G(s) = k mk V k TV 2 poles s = 0 TV s 2 1 zero s = K Pk TP k TV C. Melchiorri (DEIS) Control 33 / 85

34 Position, velocity and acceleration feedback d θm θm θ m θ r 1+sT K P K V K a k t A s R A I k v k TP k TV k TA C P (s) = K P C V (s) = K V C A (s) = K A 1+sT A s C. Melchiorri (DEIS) Control 34 / 85

35 Position, velocity and acceleration feedback Given the transfer function between the voltage V A and the motor velocity Θ m G (s) = k m (1+k m K A k TA )[1+ stm(1+kmk T Ak A TA Tm ) 1+k mk A k TA ] the transfer functions of the direct and the feedback path and of the closed loop are, respectively: C(s)G(s) = K PK V K A (1+sT A ) s 2 G (s) ( H(s) = k TP 1+ sk ) TV K P k TP 1 Θ(s) k = TP Θ r (s) 1+ sk TV K P k + s2 (1+k m K A k TA ) TP k m K P k TP K V K A C. Melchiorri (DEIS) Control 35 / 85

36 Position, velocity and acceleration feedback Also in this case, it is advisable to cancel the pole in 1/T m. This can be done by one of the two following (equivalent) choices: 1) T A = T m 2) k m K A k TA T A T m k m K A k TA 1 C. Melchiorri (DEIS) Control 36 / 85

37 Position, velocity and acceleration feedback The disturbance-output transfer function is: sr a Θ(s) D(s) = k t K P k TP K V K A (1+sT A ) 1+ sk TV + s2 (1+k m K A k TA ) K P k TP k m K P k TP K V K A The noise reduction factor is: X R = K P k TP K V K A The estimated time to compensate the disturbance effect on the output is: 1 T R = max{t A, } δω n that can be better than the previous case because it is possible to choose T A < T m. With reference to a second order function W(s), if δ,ω n,x R are given, it results: 2K P k TP k TV = ωn δ, kmk Ak TA = kmx R ω 2 n 1, K P k TP K V K A = X R C. Melchiorri (DEIS) Control 37 / 85

38 Position, velocity and acceleration feedback Root locus and typical step response of the system with k = k m k TV K V K A /(1+k m K A k TA ). 2 root locus 1.2 time response o x seconds These results are equivalent to those obtained with the position and velocity feedback control loop (second order system). C. Melchiorri (DEIS) Control 38 / 85

39 Remarks Robot Position Control 1. The position and velocity signals can be easily measured, while in general the acceleration is not available. A state variable filter can be used to estimate the acceleration value. k TP Θ - - k 1 k 2 k TA k TP k 2 k TV k TP k 1 k 2 s s k TA Θf k TV Θf k TP Θ f The filter transfer function is Θ f (s) Θ(s) = k 1k 2 s 2 +k 1s +k 1k 2 thus its dynamics is characterized by ω nf = k 1k 2 δ f = 0.5 k 1/k 2 The values have to be chosen so that the filter bandwidth is higher (at lead one decade) than the motor bandwidth. On the other hand, this could generate problems related to the presence of high-frequency noise. 2. The controllers have been designed neglecting the actuation system nonlinearities, which could cause problems with high gains values. C. Melchiorri (DEIS) Control 39 / 85

40 Example Robot Position Control Given a position trajectory defined as a trapezoidal velocity profile, the velocity and acceleration profiles can be estimated by using a filter state variables (k 1 = k 2 = 10,k TP = k TV = k TA = 1). The estimated values are: Posizione reale e stimata Velocita reale e stimata Accelerazione reale e stimata Note the overshoot of the estimated acceleration (can be reduced with a proper choice of the filter s parameters). C. Melchiorri (DEIS) Control 40 / 85

41 Example Robot Position Control Consider now the same example, but with a white noise (amplitude = ) added to the position signal. 30 Posizione reale e stimata Velocita reale e stimata Accelerazione reale e stimata C. Melchiorri (DEIS) Control 41 / 85

42 Feed-forward control action When the request of performance increases, and therefore faster trajectories are specified (i.e. with higher values for the velocity/acceleration signals), normally a performance degradation takes place in terms of tracking capabilities (the disturbance d becomes more relevant). A possible solution to overcome this problem is to include in the controller feed-forward actions, which can be computed on the basis of the values of the desired position, velocity and acceleration signals. C a(s) y d C f (s) + R(s) + + u G(s) y - C. Melchiorri (DEIS) Control 42 / 85

43 Feed-forward control action The positive effects related to the use of a feedback controller are widely known from the basic control theory. In particular, the most well known advantages are: noise rejection larger bandwidth for the controlled system if compared to the original system robustness in case of parametric uncertainties. On the other hand, beside feedback control scheme, it is known that it is possible to include in the control scheme one or more feed-forward control actions, i.e. control actions that are not based on the feedback paradigm. C. Melchiorri (DEIS) Control 43 / 85

44 Feed-forward control action In principle, under ideal conditions, a feed-forward control scheme allows to track perfectly the reference signal. On the other hand, the feed-forward actions can be computed only if the process to be controlled and the noise signals are perfectly known, and therefore it is not realistic to use only this control principle for real plants where modeling errors and noise/disturbances are always present. In any case, when high-performances are required, the use of feed-forward controllers can significantly increases the results achievable with the feedback controlled system. C a(s) y d C f (s) + R(s) + + u G(s) y - C. Melchiorri (DEIS) Control 44 / 85

45 Feed-forward control action Control actions are usually included in the forward path of a control scheme for the following main reasons: 1 to properly filter the reference input, by modifying the transfer function between the desired input y d and the output y, in order to obtain some predefined static or dynamic properties for the system; 2 to improve tracking performance; 3 to compensate known (or at least measurable) disturbances acting on the system. C a(s) y d C f (s) + R(s) + + u G(s) y - C. Melchiorri (DEIS) Control 45 / 85

46 Feed-forward control action One of the main goal in the design of a trajectory tracking control law is to have, ideally, a null error, i.e. y d y. On the other hand, by using only feedback control actions (intrinsically based on the error e = y d y), in general it is not possible to ensure that this requirement is met. A possible solution is based on control schemes including feed-forward control actions. C a (s) y d + C f (s) R(s) ++ u y G(s) - The overall general scheme includes the classical feedback controller R(s), and two feed-forward control actions C f (s) and C a (s). C. Melchiorri (DEIS) Control 46 / 85

47 Reference signal compensation The relationship between the output signal and the reference signal is now: R(s)G(s) Y(s) = C f (s) 1+R(s)G(s) Y d(s)+ Ca(s)G(s) 1+R(s)G(s) Y d(s) = C f(s)r(s)g(s)+c a(s)g(s) Y d (s) (3) 1+R(s)G(s) By imposing the desired (ideal) condition Y(s) = Y d (s), it follows: or, if C f (s) = 1, C a(s) = 1 G(s) +R(s)[1 C f(s)] (4) C a(s) = G 1 (s) (5) The last relationship highlights the fact that, once the process dynamics G(s) and the reference signal Y d (s) are known, for a perfect tracking of the input signal (i.e. y y d ) the control input must be computed by inverting the system dynamics. Formally: U(s) = G 1 (s)y d (s) C. Melchiorri (DEIS) Control 47 / 85

48 Reference signal compensation It is not always possible to use feed-forward control actions. In particular, it is not possible when: 1 G(s) has at least a zero with positive real part (non minimum phase systems) 2 G(s) has significant time delay (such as e t ds ) 3 G(s) is not a proper system (i.e. the degree of the numerator is not equal to the degree of the denominator) In the first two cases a non-casual scheme should be used, while in the third one an approximation of G(s) should be introduced, removing part of the dynamics to define a proper transfer function for the computation of G 1 (s). C. Melchiorri (DEIS) Control 48 / 85

49 Reference signal compensation Let us consider an electrical actuator. If its electrical dynamics is neglected, it can be approximated as a double integrator, i.e. G(s) = 1/s 2. With this approximation, the inverse dynamic model needed to implement a feed-forward action is G 1 (s) = s 2, that results in a non-feasible operations since it corresponds to a double time-derivation of the reference signal. On the other hand, since the desired trajectories are usually specified in terms of position, velocity and acceleration signals, in this case this does not constitute a problem. In order to implement the feed-forward control actions, it is not necessary to derive (twice) the reference signal, since the velocity and acceleration (jerk,...) reference values are already available. To conclude, notice that in case of perfect knowledge of the transfer function G(s), and in case of physical realizability of G 1 (s), the feedback loop (i.e. R(s)) does not substantially contribute to the tracking of the desired signal y d, and it is used only to compensate for disturbances possibly acting on the system. C. Melchiorri (DEIS) Control 49 / 85

50 Reference signal compensation As an example, consider y d G(s) = 1 s +10 C a(s) C f (s) + R(s) + + u G(s) y - R(s) = 100(s +10) s +100 The feed-forward action is not physically feasible. In fact: C a (s) = 1 G(s) = s C f (s) = 1 first order time-derivative! By considering a partial compensation only (i.e. the static gain) C a (s) = 10 in steady state conditions (for constant input values) one obtains y = y d. C. Melchiorri (DEIS) Control 50 / 85

51 Reference signal compensation 500 (a) Reference (dashed) and output signals 500 (a) Reference (dashed) and output signals Error 4 Error C. Melchiorri (DEIS) Control 51 / 85

52 Reference signal compensation On the other hand, by considering C a(s) = 1 G(s) = s it follows that U(s) = C a(s)y d (s) = 10Y d (s)+sy d (s) Since the s in the Laplace notation corresponds to a time derivative, if the reference velocity is known then the following control scheme can be applied, with k a,p = 10, k a,v = 1. ẏ d k a,v k a,p + + y d + R(s) ++ u y G(s) - C. Melchiorri (DEIS) Control 52 / 85

53 Reference signal compensation 500 (a) Reference (dashed) and output signals 500 (b) Reference (dashed) and output signals Error 6 x Error Notice the perfect tracking of the reference signal. C. Melchiorri (DEIS) Control 53 / 85

54 Reference signal compensation 1. Position feedback: By approximating the relationship between the voltage V A and the acceleration Θ m as Θ m = kt R A I V A = km T m V A and by approximating the relationship between the voltage V A and the velocity Θ m (in steady state) as it is possible to express V A as: Θ m = k mv A V A = K P(1+sT P ) (k TP Θ d k TP Θ 1 m)+ Θd + Tm Θd s k m k m This is equivalent to assume an input signal equal to: Θ r(s) = [k TP + s2 (1+sT m) k mk P (1+sT P ) ]Θ d(s) Remark: when not directly provided, the reference values of velocity and acceleration can be easily computed if the control trajectory is expressed in analytical form. C. Melchiorri (DEIS) Control 54 / 85

55 Decentralized feed-forward compensation The following scheme is obtained (position feedback + decentralized feed-forward action): C. Melchiorri (DEIS) Control 55 / 85

56 Decentralized feed-forward compensation 2. Position/Velocity feedback Similarly to the previous case, in case of position and velocity feedback loops it is possible to compute Then, the overall control scheme is: Θ r(s) = [k TP + sk TV s 2 + ]Θ d (s) K P k m K P K V C. Melchiorri (DEIS) Control 56 / 85

57 Decentralized feed-forward compensation 3. Position/Velocity/Acceleration feedback: In this case, it is: Θ r (s) = [k TP + sk TV + s2 (1+k m K A k TA ) ]Θ d (s) K P k m K P K V K A The scheme for the position/velocity/acceleration feedback + decentralized feed-forward control action is: C. Melchiorri (DEIS) Control 57 / 85

58 Example Robot Position Control Without feed-forward action With feed-forward action 2.5 Traiettoria desiderata e effettiva 2.5 Traiettoria desiderata e effettiva Time [s] Errore di inseguimento Time [s] 2.5 x Errore di inseguimento Coppia 3 Coppia Time [s] Time [s] C. Melchiorri (DEIS) Control 58 / 85

59 Decentralized feed-forward compensation On the basis of the previous schemes, it is possible to remark the fact that if the number of inner loops increases, then the required knowledge of the dynamics model is less important. As a matter of fact: Position loop: the two parameters T m,k m are necessary; Position and velocity loops: only k m ; Position, velocity and acceleration loops: k m (with reduced importance since it appears in the term k TA + 1 k mk A ). Perfect trajectory tracking = Perfect knowledge of the model. Saturation problem: can be more easily solved with control scheme with inner loops. C. Melchiorri (DEIS) Control 59 / 85

60 Decentralized feed-forward compensation It is possible to obtain control structures equivalent to those introduced above but based the position feedback only, with standard controllers of the P, PI, PID,...types. These control schemes are equivalent if the input/output transfer function and the disturbance reduction properties are considered; However, in this way, it is not possible to apply suitable techniques to avoid saturation problems of the internal variables (electrical currents, accelerations, velocities). These architectures describe most of the control schemes currently adopted in industrial robots (based on PID, although with many configurations). Once again, it is important to remark that on one hand it is desirable to have high gains in the inner control loops in order to minimize the effects of modeling errors but, on the other hand, that there are limits to these gains resulting from inaccuracies in the model, digital implementation of the control algorithms, non-modeled dynamics (e.g. elastic effects, friction,...), noise. C. Melchiorri (DEIS) Control 60 / 85

61 Decentralized feed-forward compensation Control scheme equivalent to the position feedback control (PI) Control scheme equivalent to the position/velocity feedback control (PID) C. Melchiorri (DEIS) Control 61 / 85

62 Decentralized feed-forward compensation Control scheme equivalent to the position/velocity/acceleration feedback control (PIDD 2 ) C. Melchiorri (DEIS) Control 62 / 85

63 Pre-calculated torque forward compensation Let us consider the most general controller (i.e. the PIDD 2 ) with feed-forward actions on velocity and acceleration: The output variable u of the controller is: t u = a 2 ë +a 1 ė +a 0 e +a 1 e(ζ)dζ where e = θ d θ, and the parameters a 1,a 0,a 1,a 2 depend on the adopted design criteria. C. Melchiorri (DEIS) Control 63 / 85 0

64 Pre-calculated torque forward compensation The contributions of the feed-forward actions and of the disturbance term d can be computed as 1 k m θd + Tm k m θd R A k t d where T m k m = IR A k t and k m = 1 k v 1 k m θd + Tm k m θd By calculating θ, it follows: d θ k t θ 1 1 R A I θ θ r PIDD u 2 s s - - k TP k v a 2ë +a 1ė +a 0e +a 1 t e(ζ)dζ + 1 k m ( θ d θ)+ Tm k m ( θ d θ) = R A k t d or, after simple calculations: a 2ë +a 1ė +a 0e +a 1 t e(ζ)dζ = R A k t d C. Melchiorri (DEIS) Control 64 / 85

65 Pre-calculated torque forward compensation If d(t) = 0, this equation shows that the error goes asymptotically to zero for all the feasible trajectories (i.e. considering voltage, velocity, acceleration and workspace limits). If d(t) 0, the disturbance-output transfer function is: E(s) D(s) = W(s) = R A s k t a 2 s3 +a 1 s2 +a 0 s +a 1 Therefore, the tracking error is small if the disturbance frequency is lower than the bandwidth where the amplitude of W(s) is high (i.e. for slow motions). 0 Ampiezza 1 Uscita con disturbo (dash) a 1 rad/s Gain db Fasi 1 Uscita con disturbo (dash) a 5 rad/s Phase deg Frequency (rad/sec) C. Melchiorri (DEIS) Control 65 / 85

66 Pre-computed torque feed-forward compensation Since the term d(t) is not an exogenous disturbance, but it is known from the dynamic model, it is possible to improve further the control performances. In fact, given the desired position, velocity and acceleration signals for the actuators, it is always possible to compute a feed-forward control action able to compensate for d(t): d d = K 1 r M(q d )K 1 r q md +K 1 r C(q d, q d )K 1 r q md +K 1 r g(q d ) Centralized feed-forward action d d q md q md Decentralized feed-forward action q md d Decentralized - controller - Robot q m C. Melchiorri (DEIS) Control 66 / 85

67 Pre-computed torque feed-forward compensation Some remarks: The residual disturbance d = dd d is null only in the ideal case of perfect tracking (q = q d ) and if the dynamic model is perfectly known. In any case, d is much smaller if compared to d. Usually, the computation of the feed-forward action d d is computationally expensive (a centralized computation is needed): its real-time implementation could require a processing time too long if compared to the sampling time T s; A possible solution could be to achieve a partial compensation only, by computing the most significative terms of d d such as those related to the robot inertia (on the diagonal of the M matrix) and those related to gravity. In fact, all the terms related to the velocity are relatively small for the velocities typically used for industrial robots (a few ripples/second). In this way, it is possible to compensate only for terms related to the robot configuration, and not to terms due to motion or to the interaction with the environment; Note that in case of repetitive trajectories, the compensation terms can be computed off-line. C. Melchiorri (DEIS) Control 67 / 85

68 Centralized control Robot Position Control Centralized position control Dynamic model of a manipulator: M(q) q+c(q, q) q+d v q+g(q) = τ Actuators and gearboxes may be described by: K r q = q m v A = G v v c v A = R A I A +K v q m = K t I A = Kr 1 τ τ m where K r is the matrix with the reduction coefficients; G v the gain of the motor drives; R A the armature resistances; K v,k t the electro-mechanic constants of the motors. Consider the manipulator as a MIMO system with n input and n output. V A,I A Actuators Manipulator q C. Melchiorri (DEIS) Control 68 / 85

69 Centralized control Robot Position Control Centralized position control Voltage control mode: I A = R 1 A (v A K v q m ) = R 1 A G vv c R 1 A K vk r q τ = K r K t I A = K r K t R 1 A G vv c K r K t R 1 A K vk r q Thus, the robot dynamic equaiton becomes: where M(q) q+c(q, q) q+d q+g(q) = u D = D v +K r K t R 1 A K vk r = diagonal matrix with friction terms u = K r K t R 1 A G vv c = control input C. Melchiorri (DEIS) Control 69 / 85

70 Centralized control Robot Position Control Centralized position control Manipulator dynamic model: M(q) q+c(q, q) q+d q+g(q) = u The torque τ (real input generating the motion of the manipulator) is not equal to u (algebraically related to v c ) because of the counter-electromotive force proportional to the joints velocity q. v c K rk tr 1 A Gv u τ - Manipulator q q q K rk tr 1 A KvKr C. Melchiorri (DEIS) Control 70 / 85

71 Centralized control Robot Position Control Centralized position control Torque control mode: With the voltage-control mode, it is not possible to directly provide to the joints the exact torques needed to move the robot (complex computation should be introduced to compensate for the velocity-dependent terms). It is preferable to use for the motor a current-control modality: the actuator acts as a torque generator. It follows: where I A = G i v c M(q) q+c(q, q) q+d q+g(q) = u v c K rk tg i D = D v u = K r K t G i v c u Manipulator q q q C. Melchiorri (DEIS) Control 71 / 85

72 Centralized control Robot Position Control Centralized position control After this preliminary comments, two important centralized control schemes are now introduced: 1 PD + gravity compensation 2 Inverse dynamics control C. Melchiorri (DEIS) Control 72 / 85

73 Centralized position control PD controller with gravity compensation Given a desired reference configuration q d, the goal is to define a controller ensuring the global asymptotic stability of the nonlinear dynamical system (i.e the robot) described by: M(q) q+c(q, q) q+d q+g(q) = u For this purpose, let us define the error as q = q d q Consider a dynamic system with state x expressed as [ ] x = q q The direct Lyapunov method is exploited for the control law definition. C. Melchiorri (DEIS) Control 73 / 85

74 Centralized position control PD controller with gravity compensation Let us consider the following candidate Lyapunov function V(q, q) = 1 2 qt M(q) q+ 1 2 qt K P q > 0 q, q 0 where K P is a square (n n) positive-definite matrix. Function V(q, q) is composed by two terms: 1/2 q T M(q) q expressing the kinetic energy of the system; 1/2 q T K P q that can be interpreted as elastic energy stored by springs with stiffness K P ; these springs are a physical interpretation of the position control loops. C. Melchiorri (DEIS) Control 74 / 85

75 Centralized position control PD controller with gravity compensation The reference configuration q d is constant, then q = q and the time derivative of V is: V = q T M q+ 1 2 qt Ṁ q q T K P q Since the robot dynamics can be rewritten as M q = u C q D q g, then V = q T M q+ 1 2 qt Ṁ q q T K P q = q T (u C q D q g)+ 1 2 qt Ṁ q q T K P q = 1 2 qt [Ṁ(q) 2C(q, q)] q qt D q+ q T [u g(q) K P q] In order to compute the control input u, note that: T q [Ṁ(q) 2C(q, q)] q = 0 due to the choice of C q T D q is negative-definite Thus, by setting u = g(q)+k P q it is possible to guarantee that V is negative-semidefinite. In fact: V = 0 q = 0, q C. Melchiorri (DEIS) Control 75 / 85

76 Centralized position control PD controller with gravity compensation The same result can be achieved also by adding a second term to the control u: u = g(q)+k P q K D q By defining K D as a positive-definite matrix, it results V = q T (D+K D ) q As a consequence, the convergence ratio of the system to the equilibrium is increased. Note that the terms K D q is equivalent to a derivative action in the control loop (PD and gravity compensations). C. Melchiorri (DEIS) Control 76 / 85

77 Centralized position control PD controller with gravity compensation K D q d q + u K P Manipulator - - g( ) q q Remarks: The control law is a linear PD controller with a nonlinear term (for gravity compensation). The system is globally asymptotically stable for any choice of K P, K D (positive-definite); The derivative action is fundamental in systems with low friction effects D. Typical examples are manipulators equipped with Direct Drive motors: the low electrical dumping in this case is increased by the control action (derivative actions). C. Melchiorri (DEIS) Control 77 / 85

78 Centralized position control PD controller with gravity compensation The system evolves, and V decreases, as long as q 0. Since V does not depend on q ( V = q T (D+K D ) q), it is not possible to guarantee that in steady state (when q = 0) also q = 0. On the other hand, the steady state can be computed from the system equation M(q) q+c(q, q) q+d q+g(q) = g(q)+k P q K D q }{{}}{{} Robot dynamics PD+g(q) In fact, in steady state ( q = q = 0) it results that is (K P is positive definite): K P q = 0 q = q d A perfect compensation of the gravity term g(q) is necessary, otherwise it is not possible to guarantee the stability of the system (robust control problem). C. Melchiorri (DEIS) Control 78 / 85

79 Centralized position control Inverse dynamics control The manipulator is considered as a nonlinear MIMO system described by M(q) q+c(q, q) q+d q+g(q) = u or, in short: M(q) q+n(q, q) = u The goal is now to define a control input u such that the overall system can be regarded as a linear MIMO system. This result (global linearization) can be achieved by using a nonlinear state feedback. It can be shown that this is possible because: the model is linear in the control input u; the matrix M(q) is invertible for any configuration of the manipulator. Let us choose the control input u (based on the state feedback): u = M(q)y+n(q, q) C. Melchiorri (DEIS) Control 79 / 85

80 Inverse dynamics control Centralized position control By using the control input u defined as it follows that u = M(q)y+n(q, q) M q+n = My+n and thus (since M is invertible) q = y where y is the new input of the system. y u M(q) Manipulator n(q, q) q y q q q C. Melchiorri (DEIS) Control 80 / 85

81 Inverse dynamics control Centralized position control This is called an inverse dynamics control scheme because the inverse dynamics of the manipulator must be calculated and compensated. As long as y i affects only q i (y i = q i ), the overall system is linear and decoupled with respect to y. y q q Now, it is necessary to define a control law y that stabilizes the system. By choosing from q = y it follows y = K P q K D q+r q+k D q+k P q = r that is asymptotically stable if the matrices K P,K D are positive-definite. C. Melchiorri (DEIS) Control 81 / 85

82 Centralized position control Inverse dynamics control If matrices K P,K D are diagonal matrices defined by K P = diag{ω 2 ni} K D = diag{2δ i ω ni } the dynamics of the i-th component is characterized by the natural frequency ω ni and by the damping coefficient δ i. A predefined trajectory q d (t) can be tracked by defining r = q d +K D q d +K P q d Then, the dynamics of the tracking error is: q+k D q+k P q = 0 The error is not null if and only if q(0) 0, q(0) 0 and converges to zero with a dynamics defined by K P,K D. C. Melchiorri (DEIS) Control 82 / 85

83 Inverse dynamics control Centralized position control q d q d q q d q K D K P y u M(q) Manipulator n(q, q) q q Two control loops are present: the first loop is based on the nonlinear feedback of the state and it provides a linear and decoupled model between y and q (double integrator); the second loop is linear and is used to stabilize the whole system; the design of this outer loop is quite simple because it has to stabilize a linear system. As the inverse dynamics controller is based on state feedback, all the terms in the manipulator dynamic model (M(q), C(q, q), D, g(q)) must be known and computed in real-time. C. Melchiorri (DEIS) Control 83 / 85

84 Centralized position control Inverse dynamics control This kind of controller has some implementation problems: it requires the exact knowledge of the manipulator model (including payload, non-modeled dynamics, mechanical and geometrical approximations,...); the real-time computation of all the dynamic terms involved in the control loop. If, for computational reasons, only the principal terms are considered, then the control action cannot be precise due to the introduced approximations. It follows that control techniques able to compensate modeling errors are required: Robust control (sliding mode,...) Adaptive control. C. Melchiorri (DEIS) Control 84 / 85

85 Inverse dynamics control - Example Centralized position control Nonlinear system: (2+sinq) q + q cosq + 1+q 2 = u Desired trajectory: trapezoidal velocity profile. k p = 100, k d = Posizione 20 Velocita x 10-3 Errore di Posizione 0.02 Errore di Velocita Accelerazione Errore di Accelerazione C. Melchiorri (DEIS) Control 85 / 85