MSMS Matlab Problem 02

Transcription

1 MSMS Matlab Problem 02 Basilio Bona DAUIN PoliTo Problem formulation The planar system illustrated in Figure 1 consists of a cart C sliding with friction along the horizontal rail; the cart supports at the pivot point C a double pendulum whose rotation axes are perpendicular to the plane. The pendulum arms are massless, but a solid mass B is attached to the extremity of the second arm. Each rotation is affected by a linear dissipation and has an internal linear spring-like elastic element. The problem consists of preparing the Simulink model that simulates the system motion. The nonlinear model of the system Giventhethreecoordinatesq 1 (t),q 2 (t),q 3 (t)illustratedinfigure1,q 1 isalinearcoordinate, while q 2 and q 3 are angular coordinates. The two masses have the following center-of-mass coordinates q 1 q 1 +L 1 s 2 +L 2 s 23 r C = 0 r B = L 1 c 2 L 2 c and the velocities q 1 q 1 +L 1 c 2 q 2 +L 2 c 23 ( q 2 + q 3 ) 1 L 1 c 2 +L 2 c 23 L 2 c 23 q 1 ṙ C = 0 ṙ B = L 1 s 2 q 2 +L 2 s 23 ( q 2 + q 3 ) = 0 L 1 s 2 +L 2 s 23 L 2 s 23 q } 0 {{ 0 } q 3 J The angular velocity of the mass B is ω(t) = [ 0 0 ( q 2 + q 3 ) ]T. Having assumed the symbolic structure below 1 a b J = 0 c d

2 Matlab Problem the matrix J T J can be computed as a b 1 a b J T J = a c 0 0 c d = a A C b d b C B where a(q 2,q 3 ) = L 1 c 2 +L 2 c 23 b(q 2,q 3 ) = L 2 c 23 c(q 2,q 3 ) = L 1 s 2 +L 2 s 23 d(q 2,q 3 ) = L 2 s 23 A(q 3 ) = a 2 +c 2 = L 2 1 +L2 2 +2L 1L 2 c 3 B = b 2 +d 2 = L 2 2 C(q 3 ) = ab+cd = L 2 2 +L 1L 2 c 3 where, as usual s 2 = sin(q 2 ),c 2 = cos(q 2 ),s 23 = sin(q 2 +q 3 ),c 23 = cos(q 2 +q 3 ) and that s 23 = s 2 c 3 +c 2 s 3 c 23 = c 2 c 3 s 2 s 3 c 3 = s 2 s 23 +c 2 c 23 The kinematic co-energy is given as K = 1 { mc ṙ C 2 +m B ṙ B 2 +ω T Γ B ω } 2 The computation of m C ṙ C 2, ṙ B 2 and ω T Γ B ω gives the following expressions ṙ C 2 = q 2 1 ṙ B 2 = q T J T J q = q 2 1 +A q2 2 +B q2 3 +2a q 1 q 2 +2b q 1 q 3 +2C q 2 q 3 ω T Γ B ω = Γ B,z ( q 2 + q 3 ) 2 hence K = 1 2 { m C q 2 1+ m B [ q 2 1 +A q2 2 +B q2 3 +2a q 1 q 2 +2b q 1 q 3 +2C q 2 q 3 ]+ Γ B,z ( q 2 + q 3 ) 2 } The potential energy is given as P = m B g T r B = m B ( G)( L 1 c 2 L 2 c 23 ) = m B Ga

3 Matlab Problem If the model presents elastic elements in the joint, one shall add a second term P e = 1 ( ka q2 2 +k B q 2 2 3) and this assumes that the spring rest length is zero. The dissipation function depends on the assumed model friction: if all elements are subject to friction we can write D = 1 ( βc q β A q 2 2 +β B q 3) 2 The symbols used. i.e., k A,k B,β C,β A,β B have an obvious meaning. Lagrange equations It is convenient to compute the various terms in order to derive the Lagrange equations. Velocity derivatives Time derivatives K q 1 = m C q 1 +m B ( q 1 +a q 2 +b q 3 ) = (m C +m B ) q 1 +m B (a q 2 +b q 3 ) K q 2 = m B (A q 2 +a q 1 +C q 3 )+Γ B,z ( q 2 + q 3 ) K q 3 = m B (B q 3 +b q 1 +C q 2 )+Γ B,z ( q 2 + q 3 ) d K dt = (m C +m B ) q 1 +m B q 1 ( A q 2 + da d K = m B dt q 2 d K = m B dt q 3 ( a q 2 + da dt q 2 +b q 3 + db ) dt q 3 dt q 2 +a q 1 + da dt q 1 +C q 3 + dc ) dt q 3 +Γ B,z ( q 2 + q 3 ) ( B q 3 + db dt q 3 +b q 1 + db dt q 1 +C q 2 + dc ) dt q 2 +Γ B,z ( q 2 + q 3 )

4 Matlab Problem where da(q 2,q 3 ) dt db(q 2,q 3 ) dt da(q 3 ) dt db dt = 0 dc(q 3 ) dt = a q 2 q 2 + a q 3 q 3 = (L 1 s 2 +L 2 s 23 ) q 2 L 2 s 23 q 3 = a q 2 q 2 + a q 3 q 3 = L 2 s 23 ( q 2 + q 3 ) = A q 3 q 3 = 2L 1 L 2 s 3 q 3 = C q 3 q 3 = L 1 L 2 s 3 q 3 Coordinate derivatives Dissipation K q 1 = 0 K q 2 = m B (L 1 s 2 +L 2 s 23 ) q 1 q 2 K q 3 = m B (L 1 L 2 s 3 q 2 2 +L 2s 23 q 1 q 2 +L 2 s 23 q 1 q 3 +L 1 L 2 s 3 q 2 q 3 ) P q 1 = 0 Generalized forces P = m B G(L 1 s 2 +L 2 s 23 )+ k A q 2 q 2 }{{} if present P = m B GL 2 s 23 + k B q 3 q 3 }{{} if present D q 1 = β C q 1 D q 2 = β A q 2 D q 3 = β B q 3

5 Matlab Problem If an horizontal force f = [ f x 0 0 ] is applied to the cart, then F 1 = f x, F 2 = F 3 = 0 We recall that the generic i th Lagrange equation is written as Equation 1 d K K + P + D = F i dt q i q i q i q i (m C +m B ) q 1 +m B (L 1 c 2 +L 2 c 23 ) q 2 +m B L 2 c 23 q 3 + Equation 2 m B (L 1 s 2 +L 2 s 23 ) q 2 2 m B L 2 s 23 q 2 3 2m B L 2 s 23 q 2 q 3 +β C q 1 = f x (1) m B (L 1 c 2 +L 2 c 23 ) q 1 +[m B (L 2 1 +L2 2 +2L 1L 2 c 3 )+Γ B,z ] q 2 + +[m B (L 2 2 +L 1L 2 c 3 )+Γ B,z ] q 3 2m B L 1 L 2 s 3 q 2 q 3 m B L 2 s 3 q 1 q 3 m B L 1 L 2 s 3 q Equation 3 +β A q 2 +k A q 2 = m B G(L 1 s 2 +L 2 s 23 ) (2) m B L 2 c 23 q 1 +[m B (L 2 2 +L 1L 2 c 3 )+Γ B,z ] q 2 +[m B L 2 2 +Γ B,z] q 3 +m B L 1 L 2 s 3 q The three equations can be written in matrix form as +β B q 3 +k B q 3 = m B GL 2 s 23 (3) H(q) q +C( q,q) q +B q +Kq = f (4) where the matrix m C +m B m B (L 1 c 2 +L 2 c 23 ) m B L 2 c 23 H(q) = m B (L 1 c 2 +L 2 c 23 ) m B (L 2 1 +L2 2 +2L 1L 2 c 3 )+Γ B,z m B (L 2 2 +L 1L 2 c 3 )+Γ B,z m B L 2 c 23 m B (L 2 2 +L 1 L 2 c 3 )+Γ B,z m B L 2 2 +Γ B,z is symmetric, positive definite and represents the overall inertial characteristic of the system and depends on the generalized coordinates q(t); B and K are simple constant diagonal matrices β C B = 0 β A 0 K = 0 k A β B 0 0 k B

6 Matlab Problem representing friction and elastic properties of the system. The vector f includes external and gravity forces affecting the system The matrix f 1 f x f = f 2 = m B G(L 1 s 2 +L 2 s 23 ) f 3 m B GL 2 s 23 c 11 c 12 c 13 C( q,q) = c 21 c 22 c 23 c 31 c 32 c 33 is more complex, since it contains the terms that contribute to the Coriolis and centripetal acceleration. Take note to do not confuse the cosine of a sum of two angles, written as c ij, with a generic element of the matrix C, written as c ij. Considering each term c ij can be computed as H 11 (q) H 12 (q) H 13 (q) H(q) = H 21 (q) H 22 (q) H 23 (q) H 31 (q) H 32 (q) H 33 (q) c ij = k h ijk (q) q k where h ijk = 1 2 are called Christoffel symbols of the first kind. As an example we can compute c 12 and c 23 as ( Hij + H ik H ) jk = h ikj k q k q j q i c 12 = h 121 q 1 +h 122 q 2 +h 122 q 3 c 23 = h 231 q 1 +h 232 q 2 +h 232 q 3

7 Matlab Problem where that gives ( h 121 = 1 H12 + H 11 H ) 21 = 0 2 q 1 q 2 q 1 ( h 122 = 1 H12 + H 12 H ) 22 = m 2 B L 1 s 2 m B L 2 s 23 q 2 q 2 q 1 ( h 123 = 1 H12 + H 13 H ) 23 = m 2 B L 2 s 23 q 3 q 2 q 1 ( h 231 = 1 H23 + H 21 H ) 31 = 1 2 q 1 q 3 q ( m 2 BL 2 s 23 +m B L 2 s 23 ) = 0 2 ( h 232 = 1 H23 + H 22 H ) 32 = 1 2 q 2 q 3 q ( 2m 2 BL 1 L 2 s 3 ) = m B L 1 L 2 s 3 2 ( h 233 = 1 H23 + H 23 H ) 33 = 1 2 q 3 q 3 q ( 2m 2 BL 1 L 2 s 3 ) = m B L 1 L 2 s 3 2 c 12 = (m B L 1 s 2 +m B L 2 s 23 ) q 2 m B L 2 s 23 q 3 c 23 = m B L 1 L 2 s 3 q 2 m B L 1 L 2 s 3 q 3 In conclusion the matrix C is 0 m B (L 1 s 2 +L 2 s 23 ) q 2 m B L 2 s 23 q 3 m B L 2 s 23 q 2 m B L 2 s 23 q 3 C = m B L 2 s 3 q 3 m B L 1 L 2 s 3 q 3 m B L 1 L 2 s 3 q 2 m B L 1 L 2 s 3 q 3 0 m B L 1 L 2 s 3 q 2 0 and it is evident that depends both on q(t) and q(t). Second-order differential equations Considering that the inertial matrix H(q) in eqn. (4) is always invertible for any q(t), we can write the following system of second order equations as q(t) = H(q) 1 (C( q,q) q +B q +Kq +f) (5)

8 Matlab Problem Preparing the Matlab model State equations Assuming the states as x 1 q 1 x 2 q 2 x = x 3 x 4 = q 3 q 1 x 5 q 2 x 6 q 3 we can write the following first order nonlinear differential equations ẋ 1 = x 4 (6) ẋ 2 = x 5 (7) ẋ 3 = x 5 (8) ẋ 4 = q 1 from eqn. (1) ẋ 5 = q 2 from eqn. (2) ẋ 6 = q 3 from eqn. (3) and in particular the mass and Coriolis matrices become m C +m B m B (L 1 c 2 +L 2 c 23 ) m B L 2 c 23 H(x) = m B (L 1 c 2 +L 2 c 23 ) m B (L 2 1 +L L 1 L 2 c 3 )+Γ B,z m B (L 2 2 +L 1 L 2 c 3 )+Γ B,z m B L 2 c 23 m B (L 2 2 +L 1L 2 c 3 )+Γ B,z m B L 2 2 +Γ B,z and 0 m B (L 1 s 2 +L 2 s 23 )x 5 m B L 2 s 23 x 6 m B L 2 s 23 x 5 m B L 2 s 23 x 6 C = m B L 2 s 3 x 6 m B L 1 L 2 s 3 x 6 m B L 1 L 2 s 3 x 5 m B L 1 L 2 s 3 x 6 0 m B L 1 L 2 s 3 x 5 0 where, now: Equation (1) becomes c 2 = cos(x 2 ), c 23 = cos(x 2 +x 3 ) H 11 ẋ 4 +H 12 ẋ 5 +H 13 ẋ 6 +C 11 x 4 +C 12 x 5 +C 13 x 6 +β C x 4 = f 1 Equation (2) becomes H 21 ẋ 4 +H 22 ẋ 5 +H 23 ẋ 6 +C 21 x 4 +C 22 x 5 +C 23 x 6 +β A x 5 +k A x 2 = f 2 Equation (3) becomes H 31 ẋ 4 +H 32 ẋ 5 +H 33 ẋ 6 +C 31 x 4 +C 32 x 5 +C 33 x 6 +β B x 6 +k B x 3 = f 3

9 Matlab Problem These three equations together with the previous equations (6-8), can be written in matrix form as H t ẋ +C t x +B t x +K t x = f t where H t = H 11 H 12 H H 21 H 22 H H 31 H 32 H C t = C 11 C 12 C C 21 C 22 C C 31 C 32 C f t = 0 f 1 f 2 f 3 (9) and B t = β C β A β B K t = k A k B (10) Solutions We solve this system of six nonlinear differential equations using two methods; a MATLAB function, namely ode45, that is described in details at matlab/ref/ode45.html and a Simulink model. MATLAB function The MATLAB function ode45 solves nonstiff differential equations, with the following instruction: [T,X] = ode45(@odefun,tspan,x0) This MATLAB function integrates the system of differential equations x = f(t,x) from initial time t0 to final time tf with initial conditions x0 and tspan = [t0 tf]; in our case we are interested in solving a time derivative ẋ = f(t,x). odefun is the name of the function (and the file that contains it) that one shall prepare to performs the computation of the required derivative. The function used to solve this problem is called diff eq cart and its listing is the following

10 Matlab Problem function dx = diff_eq_cart(t,x) % Differential function to be integrated % The mass matrix must be inverted here global Bmat Kmat Cmat Mmat force Cmat11 = zeros(3); Cmat12 = -eye(3); Cmat21 = zeros(3); Cmat22 = coriolis(x); Cmat=[Cmat11 Cmat12; Cmat21 Cmat22]; force = zeros(6,1); force(4:6) = external_force(x); Mmat = mass6(0,x); Minv = inv(mmat); dx = Minv*(-(Cmat+Bmat+Kmat)*x+force); where x present state 6 1 dx computed state derivative 6 1 Bmat Friction coefficients matrix 6 6 Cmat Coriolis terms matrix 6 6 Kmat Elastic coefficients matrix 6 6 Mmat Mass matrix 6 6 Minv Mass matrix inverse 6 6 force Force vector 6 1 The algorithm is quite simple; it starts building the various matrices involved in eqn. 5, and then compute the derivative dx; notice that the input parameter t is not used, but is required by the ode45 function. The ode45 function is called in the main program, listed here % Matlab_Problem_02 % x =f(x) % % Set system parameters and data

11 Matlab Problem clear all global m_c m_b L1 L2 Gz Bmat Kmat Cmat Mmat Grav x0 fx force m_c=200; % cart mass m_b=2; % ball mass L1=1; % Link 1 length L2=1; % Link 2 length Gz=1; % Inertia moment of the mas B fx=0; % applied horizontal force k_a=0; % elastic constant link 1 k_b=0; % elastic constant link 1 beta_c=0; % friction constant cart beta_a=10; % friction constant link 1 beta_b=10; % friction constant link 2 Grav=10; % gravity acceleration Tinitial = 0; Tfinal = 10; x0=[0 0 pi/ ]; % initial state Bmat=zeros(6); Kmat=zeros(6); Bmat(4,4)=beta_C; Bmat(5,5)=beta_A; Bmat(6,6)=beta_B; Kmat(5,5)=k_A; Kmat(6,6)=k_B; [T,X]=ode45(@diff_eq_cart,[Tinitial Tfinal],x0);... Two matrices, namely Bmat and Kmat in (10) are constant and can be computed once for all in the main program, while Mmat, Cmat and force in (9) are state-dependent and must be computed inside diff eq cart. In particular Mmat is computed by the MATLAB function mass6

12 Matlab Problem function Mmat = mass6(t,x) % Mass Matrix 6x6 global m_c m_b L1 L2 Gz Mmat x1 = x(1); x2 = x(2); x3 = x(3); x4 = x(4); x5 = x(5); x6 = x(6); x23 = x2+x3; H(1,1) = m_c+m_b; H(1,2) = m_b*(l1*cos(x2) + L2*cos(x23)); H(1,3) = m_b*l2*cos(x23); H(2,1) = H(1,2); H(2,2) = m_b*(l1^2 + L2^2 + 2*L1*L2*cos(x3)) + Gz; H(2,3) = m_b*(l2^2 + L1*L2*cos(x3)) + Gz; H(3,1) = H(1,3); H(3,2) = H(2,3); H(3,3) = m_b*l2^2 + Gz; M11 = eye(3); M12 = zeros(3); M21 = zeros(3); M22 = H; Mmat=[M11 M12; M21 M22]; Cmat is computed by the MATLAB function coriolis function Cmat = coriolis(x) global m_b L1 L2 Cmat = eye(3); q2 = x(2);

13 Matlab Problem q3 = x(3); qp2 = x(5); qp3 = x(6); q23 = q2+q3; qp23 = qp2+qp3; Cmat(1,1) = 0; Cmat(2,1) = -m_b*l2*sin(q3)*qp3; Cmat(3,1) = 0; Cmat(1,2) = -m_b*l2*sin(q23)*qp23-m_b*l1*sin(q2)*qp2; Cmat(2,2) = -m_b*l1*l2*sin(q3)*qp3; Cmat(3,2) = m_b*l1*l2*sin(q3)*qp2; Cmat(1,3) = -m_b*l2*sin(q23)*qp23; Cmat(2,3) = -m_b*l2*l2*sin(q3)*qp23; Cmat(3,3) = 0; force is computed by the MATLAB function external force function Force = external_force(x) % Force vector computation global m_b L1 L2 Grav fx Force=zeros(3,1); q2 = x(2); q3 = x(3); q23 = q2+q3; Force(1) = fx; Force(2) = -m_b*grav*(l1* sin(q2) + L2* sin(q23)); Force(3) = -m_b*grav*l2*sin(q23); The structure of these three functions is such that they can be used also in the SIMULINK approach, as specified in the next section. Results Using the parameters initially set in MatlabProblem 02, namely,

14 Matlab Problem m_c=200; % cart mass m_b=2; % ball mass L1=1; % Link 1 length L2=1; % Link 2 length Gz=1; % Inertia moment of the mas B fx=0; % applied horizontal force k_a=0; % elastic constant link 1 k_b=0; % elastic constant link 1 beta_c=0; % friction constant cart beta_a=10; % friction constant link 1 beta_b=10; % friction constant link 2 Grav=10; % gravity acceleration x0=[0 0 pi/ ]; % initial state Tinitial = 0; % initial time Tfinal = 10; % final time The simulation computes the time history reported in Figure 1. One can see that the behaviour is approximately linear. The only initial non-zero state is x 3, i.e., the angle of the second link. After a short transient the angle goes to zero due to the presence of joint frictions; the cart position x 3 evolves from zero to a non-zero value (approx m ) due to the effect of the torques transmitted by the two revolute joints. If the mass m B is increased to m B = 20, as in m_c=200; % cart mass m_b=20; % ball mass L1=1; % Link 1 length L2=1; % Link 2 length Gz=1; % Inertia moment of the mas B fx=0; % applied horizontal force k_a=0; % elastic constant link 1 k_b=0; % elastic constant link 1 beta_c=0; % friction constant cart beta_a=10; % friction constant link 1 beta_b=10; % friction constant link 2 Grav=10; % gravity acceleration x0=[0 0 pi/ ]; % initial state Tinitial = 0; % initial time Tfinal = 10; % final time the states change as reported in Figure 2.

15 Matlab Problem Simulink The same results can be achieved using the Simulink block-based approach; it represents a good exercise, even if the simulation time is considerably longer than that of the previous approach. Figure 3 gives the general overview of the Sim Problem 02 Simulink model, where six Sections or sub-models have been identified Section 1 models the integrators with their initial states x 0 ; the states are collected in a vector called X all. Section 2 contains the data blocks of the various parameters; this is a method to make them global, i.e., available to all parts of the model. Section 3 is a visual aid for the user who has a direct knowledge of the most relevant parameters; this part can be omitted without damage. Section 4 block Matrix computation contains the code required to compute the various state-dependent matrices; its content is show in Figure 4; it contains three Matlab function blocks, whose listing follows ================================================================== function Cmat = fcn_coriolis(x) global m_b L1 L2 Cmat = eye(3); q2 = x(2); q3 = x(3); qp2 = x(5); qp3 = x(6); q23 = q2+q3; qp23 = qp2+qp3; Cmat(1,1) = 0; Cmat(2,1) = -m_b*l2*sin(q3)*qp3; Cmat(3,1) = 0; Cmat(1,2) = -m_b*l2*sin(q23)*qp23-m_b*l1*sin(q2)*qp2; Cmat(2,2) = -m_b*l1*l2*sin(q3)*qp3; Cmat(3,2) = m_b*l1*l2*sin(q3)*qp2; Cmat(1,3) = -m_b*l2*sin(q23)*qp23; Cmat(2,3) = -m_b*l2*l2*sin(q3)*qp23; Cmat(3,3) = 0; ================================================================== function Mmat = fcn_mass(x) global m_c m_b L1 L2 Gz

16 Matlab Problem Mmat = eye(3); q2 = x(2); q3 = x(3); q23 = q2+q3; Mmat(1,1) = m_c+m_b; Mmat(2,1) = m_b*(l1*cos(q2) + L2*cos(q23)); Mmat(3,1) = m_b*l2*cos(q23); Mmat(1,2) = Mmat(2,1); Mmat(2,2) = m_b*(l1^2 + L2^2 + 2*L1*L2*cos(q3)) + Gz; Mmat(3,2) = m_b*(l2^2 + L1*L2*cos(q3)) + Gz; Mmat(1,3) = Mmat(3,1); Mmat(2,3) = Mmat(3,2); Mmat(3,3) = m_b*l2^2 + Gz; ================================================================== function Force = fcn_force(x) global m_b L1 L2 Grav fx Force=zeros(3,1); q2 = x(2); q3 = x(3); q23 = q2+q3; Force(1) = fx; Force(2) = -m_b*grav*(l1* sin(q2) + L2* sin(q23)); Force(3) = -m_b*grav*l2*sin(q23); ================================================================== Section 5 block Nonlinear equations (see Figure 5) contains the model that produces the three accelerations, namely ẋ 4 = q 1,ẋ 5 = q 2 and ẋ 6 = q 3. Section 6 block Signal scopes (see Figure 6) contains the six scopes to visualize the states time history and the pipeline to file and workspace of the six states. The results of the simulation are presented in Figure 7; as one can see, comparing them with those presented in Figure 1 shows that they are equal, at least qualitatively. To verify that they are the same, we used the data passed to MATLAB by the X states block in the Nonlinear equations block. Unfortunately the number of data points generated by the two approaches are different; MATLAB produces 779 time values, while Simulink only 181. The comparison is made plotting both on a common plot, as shown in Figure 8. Other more sophisticated approaches, able to compute the error between the two data set are possible, but this issue will not be investigated here.

17 Matlab Problem Figure 1: Problem 2: choice of q i.

18 9 FIGURES Matlab_Problem_02 Figure 1: Time history of the states with m_b=2.

19 Figure 2:. Time history of the states with m_b=20. 10

20 11 FIGURES Sim_Problem_02 Figure 3: The block structure for simulation.

21 Figure 4: Sub-model block 4: matrix computation 12

22 Figure 5: Sub-model block 5: the nonlinear equations. 13

23 Figure 6: Sub-model block 6: the signal scopes. 14

24 Figure 7: Time history of the states with m_b=2; compare with Figure 1 15

25 Figure 8: Time history of both states with m_b=2; MATLAB (+), SIMULINK (o) 16