System Control Engineering

Transcription

1 System Control Engineering 0 Koichi Hashimoto Graduate School of Information Sciences Text: Feedback Systems: An Introduction for Scientists and Engineers Author: Karl J. Astrom and Richard M. Murray Text: Version_2.11b Web: SystemControlEngineering/

2 Today s topics 1 Linearization Robot Kinematics Robot Control Examples Robot Dynamics

3 Linearization (Lyapunov s indirect method)

4 Linearization of Nonlinear Systems 3 Consider the nonlinear system ẋ = f(x), f : D R n and assume that x = x e D is an equilibrium point. Taylor series expansion about the equilibrium f(x) = f(x e ) + f (x x e ) + h.o.ts x x=xe Neglecting the h.o.ts and recalling f(x e ) = 0, we have f(x) = f (x x e ) x x=xe Now defining x = x x e, x = ẋ, A = f = f x x=xe x we have x = A x. x=0

5 Lyapunov s Indirect Method 4 Theorem 3.11 Let x = 0 be an equilibrium point for a nonlinear system ẋ = f(x). Assume that A is a matrix obtained by liniarization. Then if the eigenvalues λ i of the matrix A satisfy Re λ i < 0, the origin is an exponentially stable equilibrium point.

6 Example: Pendulum with friction 5 Dynamical equation: ml θ + mg sin θ + bl θ = 0 l State variables: x 1 = θ, x 2 = θ ẋ 1 = x 2 θ mg ẋ 2 = g l sin x 1 b m x 2 Equilibrium points: [ ] [ ] x1 nπ =, n = 0, ±1, ±2,... 0 x 2

7 Linearization 6 System ẋ 1 = x 2 ẋ 2 = g l sin x 1 b m x 2 Around [x 1, x 2 ] = [0, 0] T [ ] ẋ1 ẋ 2 = f 1 f 1 x 1 x 2 f 2 f 2 x 1 x 2 Eigenvalues of A: x=[0,0] T [ x1 x 2 ] = [ 0 1 g l b m ] [ x1 x 2 ] s(s + b m ) + g l = 0, stable

8 Linearization 7 System ẋ 1 = x 2 ẋ 2 = g l sin x 1 b m x 2 Around [x 1, x 2 ] = [π, 0] T [ ] ẋ1 ẋ 2 = Eigenvalues of A: f 1 f 1 x 1 x 2 f 2 f 2 x 1 x 2 x=[π,0] T [ x1 x 2 ] = [ 0 1 g l b m ] [ x1 x 2 ] s(s + b m ) g l = 0, unstable

9 Feedback Linearization

10 Feedback Linearization 9 Example 5.1 Consider the first order system ẋ = ax 2 + u Is this system stable? We look for a state feedback u = ϕ(x) that make the equilibrium point at the origin asymptotically stabe. An obvious way is to cancels the nonlinear term to obtain u = ax 2 x ẋ = x which is linear and globally asymptotically stable.

11 Feedback Linearization 10 System ẋ = f(x) + g(x)u, y = h(x) Derivative ẏ = dh dxẋ = dh dh f(x) + dx dx g(x)u = L fh(x) + L g h(x)u Control input Closed loop u = a(x) + b(x)v ẏ = L f h(x) + L g h(x)[a(x) + b(x)v]

12 Feedback Linearization 11 Closed loop ẏ = L f h(x) + L g h(x)[a(x) + b(x)v] Thus, when We have b(x) = 1 L g h(x), a(x) = L fh(x) L g h(x) ẏ = v New input v = y d y will stabilize y d.

13 Robot Kinematics

14 Robot System 13 y (x, y) Endtip Position: x = l 1 cos θ 1 + l 2 cos(θ 1 + θ 2 ) y = l 1 sin θ 1 + l 2 sin(θ 1 + θ 2 ) l 1 θ 1 l 2 θ 2 x Output: r = (x, y) State: θ = (θ 1, θ 2 ) Input: [ ] [ ] u1 θ u = = 1. u 2 θ 2 i.e., the motor driver is velocity control.

15 Robot Kinematics 14 State [ θ1 ] y (x, y) Input θ = θ 2 [ θ 1 ] l 1 l 2 θ 1 θ 2 x Output Kinematics u = r = θ 2 [ x y x = l 1 cos θ 1 + l 2 cos(θ 1 + θ 2 ) ] y = l 1 sin θ 1 + l 2 sin(θ 1 + θ 2 )

16 Robot State Equation 15 State equation (kinematic nonlinearity): θ = u r = g(θ) where g(θ) = [ x y ] = [ l1 cos θ 1 + l 2 cos(θ 1 + θ 2 ) l 1 sin θ 1 + l 2 sin(θ 1 + θ 2 ) ] Dynamical equation: ẋ = l 1 sin θ 1 θ 1 l 2 sin(θ 1 + θ 2 )( θ 1 + θ 2 ) ẏ = l 1 cos θ 1 θ 1 + l 2 cos(θ 1 + θ 2 )( θ 1 + θ 2 ) i.e., [ ẋ ẏ ] = [ l1 sin θ 1 l 2 sin(θ 1 + θ 2 ) l 2 sin(θ 1 + θ 2 ) l 1 cos θ 1 + l 2 cos(θ 1 + θ 2 ) l 2 cos(θ 1 + θ 2 ) ] [ ] u1 u 2

17 Input-Output Dynamics 16 Input-Output (u-r) dynamics where ṙ = J(θ)u, u = θ and J(θ) = g θ = x θ 1 y x θ 2 y θ 1 θ 2 The matrix J(θ) is called Jacobi matrix. [ l1 sin θ J(θ) = 1 l 2 sin(θ 1 + θ 2 ) l 2 sin(θ 1 + θ 2 ) l 1 cos θ 1 + l 2 cos(θ 1 + θ 2 ) l 2 cos(θ 1 + θ 2 ). ]

18 Robot Control

19 Robot Control 18 The objective r = [ x y ] r d = [ ] xd Let K be a gain matrix and suppose the following velocity control law u = K(r d r) y d. where [ θ 1 ] [ k11 k 12 ] u = θ = θ 2, K = k 21 k 22. How to choose K?

20 Feedback linearization 19 Closed loop equation: ṙ = J(θ) θ = J(θ)K(r d r) A typical choice for K is λj 1, which yields ṙ = J(θ) θ = λj(θ)j 1 (θ)(r d r) = λ(r d r) Let s = r r d then we have a linearized system ṡ = λs, s = e λt s 0 and thus s 0, i.e., r r d (as t )

21 RMRC with Lyapunov 20 The control law K = λj 1 (θ), i.e., θ = λj 1 (θ)(r d r) is called resolved motion rate control. (Whitney, 1969) Consider the following Lyapunov function candidate: V = (r d r) T (r d r) 0 Then we have the derivative as follows V = 2(r d r) T ṙ = 2(r d r) T J(θ) θ = 2(r d r) T J(θ)K(r d r) = 2(r d r) T (r d r) 0 V = 0 and V = 0 if and only if r = r d.

22 RMRC with Lyapunov 21 Theorem 3.2 Let x = 0 be an equilibrium point of ẋ = f(x), f : D R n, and let V : D R be a continuously differentiable function such that (i) V (0) = 0, (ii) V (x) > 0 in D {0} (iii) V (x) < 0 in D {0}, then x = 0 is asymptotically stable. When K = J(θ) 1, we have V (s) > 0 and V (s) < 0 for s = r r d 0 while V (0) = 0 and V (0) = 0. Thus RMRC is asymptotically stable in lyapunov s sense.

23 Robot Control: J 1 d 22 Even if the state dependent feedback gain is not possible, we can select K = λj 1 d where J 1 d = inv(j(θ d )) (const.) where θ d is the desired joint angle set that satisfies This choice r d = f(θ d ) ensures that u = λj 1 d (r d r) V = 2(r d r) T J(θ)K(r d r) = 2λ(r d r) T J(θ)Jd 1 d r) < 0 around θ = θ d because J(θ)Jd 1 = I at θ = θ d.

24 Robot Control: J T d 23 The stability is yielded by the positive definiteness of J(θ)K around θ = θ d. Another choice can also ensures that u = λj T d (r d r) V = 2(r d r) T J(θ)K(r d r) = 2λ(r d r) T J(θ)J T d (r d r) < 0 around θ = θ d because J(θ)J T d is positive definite at θ = θ d.

25 Robot Control: ESM 24 Efficient second order minimization u = λ 1 2 (J(θ) + J d) 1 (r d r) Jesm = 1 2 (J(θ) + J d) can approximate the Taylor expansion of r d r to the second order. V = 2(r d r) T J(θ)K(r d r) = 2λ(r d r) T J(θ)Jesm 1 d r) < 0 around θ = θ d because J(θ)J 1 esm = I at θ = θ d.

26 Example

27 Example 26 y (x, y) l 2 θ 2 Suppose that l 1 = l 2 = 1 r d = [0, 1] T and r 0 = [1, 1] T At the desired position the Jacobi matrix is [ ] 1 1/2 J d = 0, 3/2 l 1 θ 1 x and J 1 d = [ ] 1 1/ 3 0 2/ 3

28 y x Example: RMRC 27 RMRC yeilds a straight line trajectory θ = λj 1 (θ)(r d r) ṙ = λ(r d r) θ1 = 97.19, θ2 =

29 y x Example: J 1 d 28 J d = J(θ d ) (const.) θ = λj 1 d (r d r) θ1 = , θ2 =

30 Example: J T d 29 J d = J(θ d ) (const.) θ = λj T d (r d r) θ1 = , θ2 =

31 Example: ESM 30 Jesm = (J(θ) + J(θ d ))/2 θ = λj 1 esm (r d r) θ1 = 84.33, θ2 =

32 Example: RMRC 31 RMRC yeilds a straight line trajectory θ = λj 1 (θ)(r d r) ṙ = λ(r d r) θ1 = 95.33, θ2 =

33 Example: J 1 d 32 J d = J(θ d ) (const.) θ = λj 1 d (r d r) θ1 = , θ2 =

34 Example: J T d 33 J d = J(θ d ) (const.) θ = λj T d (r d r) θ1 = , θ2 =

35 Example: ESM 34 Jesm = (J(θ) + J(θ d ))/2 θ = λj 1 esm (r d r) θ1 = 84.33, θ2 =

36 Example: RMRC 35 RMRC yeilds a straight line trajectory θ = λj 1 (θ)(r d r) ṙ = λ(r d r) θ1 = 3.57, θ2 =

37 Example: J 1 d 36 J d = J(θ d ) (const.) θ = λj 1 d (r d r) θ1 = 3.75, θ2 =

38 Example: J T d 37 J d = J(θ d ) (const.) θ = λj T d (r d r) θ1 = 14.39, θ2 =

39 Example: ESM 38 Jesm = (J(θ) + J(θ d ))/2 θ = λj 1 esm (r d r) θ1 = 0.00, θ2 =

40 MATLAB Simulation Code 39 method= 4 ; global l1; global l2; l1=1; l2=1; q0=[0;pi/8]; r0=kin(q0); Jac0=Jac(q0); rd=[1;1]; qd=invkin(rd,q0); Jacd=Jac(qd); t=0:0.1:20; dq=zeros(length(t),2); hold off; clf; switch method case 1 K=inv(Jacd); dq=twolinksim(q0,rd,k,t, JT ); case 2 K=Jacd ;

41 dq=twolinksim(q0,rd,k,t, JT ); case 3 K=[]; dq=twolinksim(q0,rd,k,t, Ji ); case 4 K=Jacd; dq=twolinksim(q0,rd,k,t, esm ); otherwise sprintf( no mthod defined: %s, method); end sum(abs(dq))

42 MATLAB Simulation Code 41 function dq=twolinksim(q0,rd,k,t,method) global l1; global l2; switch method case Ji case esm otherwise end x1=l1*cos(dq(:,1)); x2=x1+l2*cos(dq(:,1)+dq(:,2));

43 end y1=l1*sin(dq(:,1)); y2=y1+l2*sin(dq(:,1)+dq(:,2)); hold on N=length(dq(:,1)); for i = 1:2:N plot([0,x1(i)],[0,y1(i)]); plot([x1(i),x2(i)],[y1(i),y2(i)]); end

44 MATLAB Simulation Code 43 function dq=rob(t,q,rd,k) r=kin(q); dq=k*(rd-r); end function dq=rob0(t,q,rd,k) r=kin(q); Ja=Jac(q); dq=ja\(rd-r); end function dq=rob1(t,q,rd,k) r=kin(q); Ja=Jac(q); dq=(k+ja)\(rd-r)/2; end

45 Robot Dynamics 44 It is well know that robot system in general has the dynamical equation of the form M(θ) θ + C( θ, θ) + D θ + P (θ) = τ where M(θ) is inertia, C( θ, θ) is centrifugal and Coriolis force, D is friction coefficient, and P (θ) is potential. When we have the estimates of these parameters, then a control input where τ = ˆM(θ)v + Ĉ( θ, θ) + ˆD θ + ˆP (θ) v = θ d + k 2 ( θ d θ) + k 1 (θ d θ) will linearize and stabilize the trajectory θ = θ d.

46 Robot Dynamics 45 If the parameters are exactly known then substituting τ in the dynamical equation yields ë + k 2 ė + k 1 e = 0 where e = θ d θ. This control scheme is called inverse dynamics or resolved motion acceleration control. (J Luh, M Walker, R Paul, 1980)