Robots with Elastic Joints: Modeling and Control

Transcription

1 Scuola di Dottorato CIRA Controllo di Sistemi Robotici per la Manipolazione e la Cooperazione Bertinoro (FC), Luglio 2003 Robots with Elastic Joints: Modeling and Control Alessandro De Luca Dipartimento di Informatica e Sistemistica (DIS) Università di Roma La Sapienza

2 Outline Motivation for considering joint elasticity Dynamic modeling of EJ robots Formulation of control problems and sensing requirements Controllers for regulation tasks Controllers for trajectory tracking tasks Some modeling and control extensions Open research issues References 2

3 Motivation in industrial robots, the presence of transmission elements such as harmonic drives and transmission belts (typically, Scara arms) long shafts (e.g., last 3-dofs of Puma) introduce elasticity effects between actuating inputs and driven outputs the desire to increase mechanical compliance while keeping cartesian accuracy leads to the use of elastic transmissions in research robots for human cooperation actuator relocation plus cables and pulleys harmonic drives and lightweight (but rigid) link design redundant (macro-mini or parallel) actuation these phenomena are captured by modeling the elasticity at the robot joints neglected joint elasticity limits dynamic performance of controllers (vibrations, poor tracking, chattering during environment contact) 3

4 Robots with joint elasticity DEXTER 8R-arm by Scienzia Machinale, available at DIS DC motors with reductions for joints 1,2 DC motors with reductions, steel cables and pulleys for joints 3 8 (all located in link 2) encoders on motor sides 4

5 Robots with joint elasticity DLR III 7R-arm by DLR Institute of Robotics and Mechatronics DC brushless motors with harmonic drives encoders on motor and link sides, joint torque sensors weight 18 kg, payload 7 kg! 5

6 Robots with joint elasticity DECMMA 2R and 4R prototype arms by Stanford University Robotics Laboratory parallel macro (at base, with elastic coupling) mini (at joints) actuation 6

7 Robots with joint elasticity UB Hand dextrous hand mounted as end-effector of a Puma robot tendon-driven (static compliance in the grasp) 7

8 Joint elasticity in harmonic drives industrial robots compact, in-line, high reduction (1:200), power efficient transmission element teflon teeth of flexspline introduce small angular displacement 8

9 Typical frequency response of a single elastic joint 20 Motor velocity output 20 Link velocity output Magnitude (db) Magnitude (db) Frequency (rad/sec) Frequency (rad/sec) Phase (deg) Phase (deg) Frequency (rad/sec) Frequency (rad/sec) antiresonance/resonance behavior on motor velocity output, pure resonance on link velocity output (weak or no zeros) 9

10 Dynamic modeling open-chain robot with N (rotary or prismatic) elastic joints and N rigid links, driven by electrical actuators Lagrangian formulation using motor variables θ IR N (as reflected through reduction ratios) and link variables q IR N as generalized coordinates m 2 I 2 J m2 r 2 K 2 θ 2 m r2 q 2 J m1 K 1 m 1 I 1 θ m2 r 1 θ 1 q1 θ m1 10

11 standing assumptions A1) small displacements at joints (linear elasticity domain) A2) axis-balanced motors (i.e., center of mass of rotors on rotation axes) link kinetic energy and link potential energy due to gravity (including the mass of each motor as additional mass of the carrying link) T l = 1 2 qt M(q) q U g = U g (q) with symmetric, positive definite inertia matrix M(q) potential energy due to joint elasticity U e = 1 2 (q θ)t K(q θ) with diagonal, positive definite joint stiffness matrix K 11

12 simplifying assumption (Spong, 1987) A3) angular kinetic energy of each motor is due only to its own spinning T m = 1 2 θ T J θ with diagonal, positive definite motor inertia matrix J (reflected through the reduction ratios: J i = J mi r 2 i, i =1,...,N) system Lagrangian L = T U =(T l + T m ) (U g + U e )=L(q, θ, q, θ) 12

13 Euler-Lagrange equations given the set of generalized coordinates p =(q, θ) IR 2N, the Lagrangian L satisfies the usual vector equation d dt ( L ṗ ) T ( ) T L = u being u IR 2N the non-conservative forces/torques performing work on p assuming no dissipative terms, since the motor torques τ IR N only perform work on the motor variables θ we obtain p M(q) q + C(q, q) q + g(q)+k(q θ) = 0 J θ + K(θ q) = τ link equation motor equation with centrifugal/coriolis terms C(q, q) q and gravity terms g(q) =( U g / q) T 13

14 Model properties Ṁ(q) C(q, q) is skew-symmetric nonlinear dynamic model, but linear in a set of coefficients a =(a r,a K,a J ) (including K and J) Y r (q, q, q) a r + diag{q θ} a K = 0 diag{ θ} a J + diag{θ q} a K = τ for K (rigid joints): θ q and K(q θ) finite value, so that the equivalent rigid model is recovered (summing up link and motor equation) ( M(q)+J ) q + C(q, q) q + g(q) =τ there exists a bound on the norm of the gravity gradient matrix g(q) q α g(q 1) g(q 2 ) α q 1 q 2, q 1,q 2 IR N 14

15 Control problems regulation to a constant equilibrium configuration (q, θ, q, θ) =(q d,θ d, 0, 0) only the desired link position q d is assigned, while θ d has to be determined q d may come from the kinematic inversion of a desired cartesian pose x d direct kinematics of EJ robots is a function of link variables: x = kin(q) tracking of a sufficiently smooth trajectory q = q d (t) the associated motor trajectory θ d (t) has to be determined again, q d (t) is uniquely associated to a desired cartesian trajectory x d (t) other relevant planning/control problems not considered here include rest-to-rest trajectory planning in given time T cartesian control (regulation or tracking directly defined in the task space) force/impedance/hybrid control of EJ robots in contact with the environment 15

16 Sensing requirements full state feedback requires sensing of four variables: q, θ (link/motor position) and q, θ (link/motor velocity) only motor variables (θ, θ) are available with standard sensing arrangements (encoder + tachometer on the motor axis) velocities also through numerical differentiation of high-resolution encoders more advanced systems have also measurements beyond elasticity of the joints (e.g., link position q and joint torque z = K(θ q) sensors in DLR III arm) 16

17 Regulation a simple linear example two elastically coupled masses (motor/link) driven on one side (Quanser LEJ) dynamic model m q + k(q θ) =0 j θ + k(θ q) =τ and transfer function from force input τ to second mass position q P (s) = q(s) τ(s) = k mjs 2 +(m + j)k 1 s 2 NOTE: unstable but controllable system ( any pole assignment via full state feedback) with no zeros! 17

18 Feedback strategies with reduced measurements 1) τ = u 1 (k Pl q + k Dl q) (link PD feedback) W ll (s) = q(s) u 1 (s) = k mjs 4 +(m + j)ks 2 + kk Dl s + kk Pl always unstable (spring damping/friction leads to small stability intervals) 2) τ = u 2 (k Pm θ + k Dm θ) (motor PD feedback) k W mm (s) = mjs 4 + mk Dm s 3 + [m(k + k Pm )+jk] s 2 + kk Dm s + kk Pm asympotically stable for k Pm > 0, k Dm > 0 (Routh criterion) as in rigid systems! 18

19 3) τ = u 3 (k Pl q + k Dm θ) (link position and motor velocity feedback) k W lm (s) = mjs 4 + mk Dm s 3 +(m + j)ks 2 + kk Dm s + kk Pl asympotically stable for 0 <k Pl <k, k Dm > 0 (limited proportional gain) 4) with τ = u 4 (k Pm θ + k Dl q) (motor position and link velocity feedback) the closed-loop system is always unstable caution must be used in dealing with different output measures generalization to a nonlinear multidimensional setting (under gravity) of the most efficient scheme (motor PD feedback) video 19

20 Regulation with motor PD + feedforward for regulation tasks, consider the control law τ = K P (θ d θ) K D θ + g(q d ) with symmetric (diagonal) K P > 0, K D > 0, and the motor reference position θ d := q d + K 1 g(q d ) Theorem (Tomei, 1991) If ([ K K λ min (K E ):=λ min K K + K P ]) >α then the closed-loop equilibrium state (q d,θ d, 0, 0) is globally asymptotically stable 20

21 Lyapunov-based proof closed-loop equilibria ( q = θ = q = θ =0) satisfy K(q θ)+g(q) = 0 K(θ q) K P (θ d θ) g(q d ) = 0 adding/subtracting K(θ d q d ) g(q d ) (= 0, by definition of θ d ) yields K(q q d ) K(θ θ d )+g(q) g(q d ) = 0 K(q q d )+(K P + K)(θ θ d ) = 0 or, in matrix form, [ ] q qd K E θ θ d = [ g(qd ) g(q) 0 ] 21

22 using the Theorem assumption, for all (q, θ) (q d,θ d ) we have [ ] [ ] q qd K q qd E λ θ θ min (K E ) d θ θ [ ] d q qd > α θ θ α q q d d [ g(qd ) g(q) g(q d ) g(q) = 0 and hence (q d,θ d ) is the unique equilibrium configuration define the position-dependent energy function P (q, θ) = 1 2 (q θ)t K(q θ)+ 1 2 (θ d θ) T K P (θ d θ)+u g (q) θ T g(q d ) its gradient P (q, θ) =0only at (q d,θ d ) (using the same above argument) and 2 P (q d,θ d ) > 0 (q d,θ d ) is an absolute minimum for P (q, θ) ] 22

23 the following is thus a candidate Lyapunov function V (q, θ, q, θ) = 1 2 qt M(q) q θ T J θ + P (q, θ) P (q d,θ d ) 0 its time derivative, evaluated along the closed-loop system trajectories, is V = q T M(q) q qt Ṁ(q) q + θ T J θ + ( q θ ) T K (q θ) ( ) T θ T K P (θ d θ) + q T Ug (q) q θ T g(q d ) = q T ( C(q, q) q g(q) K(q θ)+ 1 2Ṁ(q) q + K(q θ)+g(q)) + θ T (u K(θ q) K(q θ) K P (θ d θ) g(q d )) = θ T ( K P (θ d θ) K D θ + g(q d ) K P (θ d θ) g(q d ) ) = θ T K D θ 0 where the skew-symmetry of Ṁ 2C has been used 23

24 since V =0 θ =0, the proof is completed using LaSalle Theorem substituting θ(t) 0 in the closed-loop equations yields M(q) q + C(q, q) q + g(q)+kq = Kθ = constant ( ) Kq = Kθ K P (θ d θ) g(q d ) = constant ( ) from ( ) it follows that q(t) 0, which in turn simplifies ( ) into g(q)+k(q θ) =0 ( ) from the fist part of the proof, q = q d, θ = θ d, q = θ =0is the unique solution to ( ) ( ) and thus the largest invariant set contained in the set of states such that V =0 global asymptotic stability of the set point 24

25 Remarks on regulation control if stiffness K is large enough (non-pathological cases), the Theorem assumption λ min (K E ) >αcan always be satisfied by increasing λ min (K P ) in the presence of model uncertainties, the control law τ = K P (ˆθ d θ) K D θ +ĝ(q d ) ˆθ d := q d + ˆK 1 ĝ(q d ) provides asymptotic stability, but with a different (and still unique) equilibrium configuration ( q, θ) version with on-line gravity compensation (Zollo, De Luca, Siciliano, 2003) τ = K P (θ d θ) K D θ + g( θ) where θ := θ K 1 g(q d ) is a biased motor position measurement 25

26 assuming that Trajectory tracking q d (t) C 4 (fourth derivative w.r.t. time exists) full state is measurable we proceed by system inversion from the link position output q a nonlinear static state feedback will be obtained that decouples and exactly linearizes the closed-loop robot dynamics (bunch of input-output integrators) exponential stabilization of the tracking error is then performed on the linear side of the transformed problem a generalized computed torque law original result (Spong, 1987), revisited without the need of state-space concepts 26

27 System inversion differentiate the link equation of the dynamic model (independent of input τ) M(q) q + n(q, q)+k(q θ) =0 to obtain (notation: q [i] = d i q/dt i ) n(q, q) :=C(q, q) q + g(q) M(q)q [3] + Ṁ(q) q +ṅ(q, q)+k( q θ) =0 still independent from τ differentiating once more (fourth derivative of q appears) M(q)q [4] +2Ṁ(q)q [3] + M(q) q + n(q, q)+k( q θ) =0 the input τ appears through θ and the motor equation J θ + K(θ q) =τ 27

28 substitution of θ gives M(q)q [4] + c(q, q, q, q [3] )+KJ 1 K(θ q) =KJ 1 τ with c(q, q, q, q [3] ):=2Ṁ(q)q [3] +( M(q)+K) q + n(q, q) the control law τ = JK 1 ( M(q)a + c(q, q, q, q [3] )+KJ 1 K(θ q) ) leads to the closed-loop system q [4] = a N separate input-output chains of four integrators (linearization and decoupling) 28

29 (q, q, q, q [3] ) is an alternative globally defined state representation from the link equation q = M 1 (q) (K(θ q) n(q, q)) i.e., link acceleration is a function of (q, θ, q) from the first differentiation of the link equation q [3] = M 1 (q) ( K( θ q) Ṁ(q) q ṅ(q, q) ) i.e., link jerk is a function of (q, θ, q, θ) (using the above expression for q) the controller term c(q, q, q, q [3] ) can be expressed in terms of (q, θ, q, θ), with an efficient organization of computations the control law τ = τ(q, θ, q, θ, a) can be implemented as a nonlinear static (instantaneous) feedback from the original state 29

30 Tracking error stabilization control design is completed on the linear side of the problem by choosing a = q [4] d + K 3 (q [3] d q [3] )+K 2 ( q d q)+k 1 ( q d q)+k 0 (q d q) with q and q [3] expressed in terms of (q, θ, q, θ) and diagonal gain matrices K 0,...,K 3 chosen such that s 4 + K 3i s 3 + K 2i s 2 + K 1i s + K 0i i =1,...,N are Hurwitz polynomials the tracking errors e i = q di q i on each link coordinate satisfy e [4] i + K 3i e [3] i + K 2i ë i + K 1i ė i + K 0i e i =0 i.e., exponentially stable linear differential equations (with chosen eigenvalues) 30

31 Remarks on trajectory tracking control the shown feedback linearization result is the nonlinear/mimo counterpart of the transfer function τ q being without zeros (no zero dynamics) the same result can be rephrased as q is a flat output for EJ robots a nominal feedforward torque can be computed off line τ d = J θ d + K(θ d q d ) where, using the previous developments, ( θ d = q d +K 1 (M(q d ) q d + n(q d, q d )) θ d = K 1 M(q d )q [4] d + c(q d, q d, q d,q [3] ) d ) a simpler tracking controller (of local validity around the reference trajectory) is τ = τ d + K P (θ d θ)+k D ( θ d θ) 31

32 An extension of dynamic modeling what happens if we remove the simplifying assumption A3)? for a planar 2R EJ robot, the complete angular kinetic energy of the motors is T m1 = 1 2 J m1r 2 1 θ 2 1 T m2 = 1 2 J m2( q 1 + r 2 θ 2 ) 2 with no changes at base motor and new terms for elbow motor; as a result T m = 1 2 [ qt θ T ] = 1 2 [ qt θ T ] J m2 0 0 J m2 r J m1 r1 2 0 J m2 r J m2 r2 2 J m S [ ] q S T θ J the blue terms contribute to M(q) (the diagonal 0 should not worry here!) [ ] q θ 32

33 for NR planar EJ robots, we obtain M(q) q + S θ + C(q, q) q + g(q)+k(q θ) = 0 S T q + J θ + K(θ q) = τ link equation motor equation with the strictly upper triangular matrix S representing inertial couplings between motor and link accelerations in general, a non-constant matrix S may arise (see De Luca, Tomei, 1996) S(q) = 0 S 12 (q 1 ) S 13 (q 1,q 2 ) S 1N (q 1,...,q N 1 ) 0 0 S 23 (q 2 ) S 2N (q 2,...,q N 1 ) S N 1,N (q N 1 ) with new associated centrifugal/coriolis terms in both link and motor equations 33

34 Control-oriented remarks specific kinematic structures with elastic joints (single link, polar 2R, PRP,...) have S 0, so that the reduced model is also complete and feedback linearizable the presented controllers for regulation tasks work as such also for the complete model (with a more complex LaSalle final analysis) as soon as S 0, exact linearization and input-output decoupling both fail if we rely on the use of a nonlinear but static state feedback structure in order to mimic a generalized computed torque approach (linearization and decoupling for tracking tasks), we need a dynamic state feedback controller τ = α(x, ξ)+β(x, ξ)v ξ = γ(x, ξ)+δ(x, ξ)v with robot state x =(q, θ, q, θ) IR 4N, dynamic compensator state ξ IR m (yet to be defined), and new control input v IR N 34

35 A control extension Dynamic feedback linearization of EJ robots a three-step design that achieves full linearization and input-output decoupling, incrementally building the compensator dynamics through the solution of two intermediate subproblems: DFL algorithm in (De Luca, Lucibello, 1998) presented for constant S 0, works also for S(q) transformation of the dynamic equations in state-space format is not needed collapses in the usual linearizing control by static state feedback when S =0 can be applied also to the case of joints of mixed type some rigid, other elastic (see De Luca, 1998) 35

36 Step 1: I-O decoupling w.r.t. θ apply the static control law τ = Ju + S T q + K(θ q) or, eliminating link acceleration q (and dropping dependencies) τ = ( J S T M 1 S ) u S T M 1( C q + g + K(q θ) ) + K(θ q) the resulting system is θ = u M(q) q =... (2N dynamics unobservable from θ) u N x 2 states θ unobservable subsystem 36

37 Step 2: I-O decoupling w.r.t. f define as output the generalized force f = M(q) q + C(q, q) q + g(q)+kq the link equation, after Step 1, is rewritten as f(q, q, q)+su Kθ =0 dynamic extension: add 2(i 1) integrators on input u i (i =1,...,N)soas to avoid successive input differentiation w N(N-1) states φ u 37

38 differentiate 2i times the component f i (i =1,...,N) and apply a linear static control law (depending on K and S) w = F w φ + G w w so that the resulting input-output system is d 2i f i dt 2i = w i i =1,...,N N x (N+1) states w f unobservable subsystem 38

39 Step 3: I-O decoupling w.r.t. q dynamic balancing: add 2(N i) integrators on input w i (i =1,...,N)so as to avoid successive input differentiation v N(N-1) states ψ differentiate 2(N i) times the ith input-output equation (i =1,...,N) after Step 2, thus obtaining d 2N f dt 2N = d2n dt 2N (M(q) q + C(q, q) q + g(q)+kq) = v 39 w

40 apply the nonlinear static control law (globally defined) v = M(q)v +ñ(q, q,...,q [2N+1] )+g [2N] (q)+kq [2N] where ñ = 2N k=1 ( 2N) M [k] (q)q [2(N+1) k] + k 2N k=0 the final resulting system is fully linearized and decoupled ( 2N) C [k] (q, q)q [2N+1 k] k q [2(N+1)] = v v q N x 2(N+1) states 40

41 Comments on the DFLalgorithm using recursion, the output q and all its derivatives (linearizing coordinates) can be expressed in terms of the robot + compensator states [ q q q q [3] q [2N+1] ] = T (q, θ, q, θ, φ, ψ) the overall nonlinear dynamic feedback for the original torque [ ] φ τ = τ(q, θ, q, θ, ξ, v) ξ = =... ψ is of dimension m =2N(N 1) for a planar 2R EJ robot, m =4and final system with 2 chains of 6 integrators v 1 y 1 = q 1 v 2 y 2 = q 2 41

42 for trajectory tracking purposes, given a (sufficiently smooth) q d (t) C 2(N+1), the tracking error e i = q di q i on each channel is exponentially stabilized by v i = q [2(n+1)] di + 2N+1 j=0 ( K ji q [j] ) di q[j] i i =1,...,N where K 0i,...,K 2N+1,i are the coefficients of a desired Hurwitz polynomial 42

43 Open research issues kinetostatic calibration of EJ robots using only motor measurements unified dynamic identification of model parameters (including K and J) robust control for trajectory tracking in the presence of uncertainties adaptive control: available (and too complex) only for the reduced model with S =0(Lozano, Brogliato, 1992) inclusion of spring damping (visco-elastic joints): static feedback linearization becomes ill-conditioned (inclusion of viscous friction on motors and links is instead trivial) cartesian impedance control with proved stability fitting the results into parallel/redundant actuated devices with joint elasticity... 43

44 References (De Luca, 1998) Decoupling and feedback linearization of robots with mixed rigid/elastic joints, IJRNC, 8(11), (De Luca, Lucibello, 1998) A general algorithm for dynamic feedback linearization of robots with elastic joints, IEEE ICRA, (De Luca, Tomei, 1996) Elastic joints, in Theory of Robot Control (Canudas de Wit, Siciliano, Bastin Eds), Springer, (Lozano, Brogliato, 1992) Adaptive control of robot manipulators with flexible joints, IEEE TAC, 37(2), (errata exists) (Spong, 1987) Modeling and control of elastic joint robots, ASME JDSMC, 109, (Tomei, 1991) A simple PD controller for robots with elastic joints, IEEE TAC, 36(10), (Zollo, De Luca, Siciliano, 2003) A PD controller with on-line gravity compensation for robots with elastic joints, (in preparation) 44