ROBOTICS Laboratory Problem 02

Transcription

1 ROBOTICS Laboratory Problem 02 Basilio Bona DAUIN PoliTo Problem formulation The planar system illustrated in Figure 1 consists of a cart C sliding with or without friction along the horizontal rail; at the pivot point C the cart supports a double pendulum, whose rotation axes in C and A are perpendicular to the plane. The pendulum arms are massless, but a solid mass B is attached to the end of the second arm. Each revolute joint may be affected by a linear dissipative effect and may have an internal linear spring-like elastic element. Problem objectives: 1. prepare and test the kinematic model that simulates the system motion; direct and inverse position and velocity kinematic functions are required; 2. design one ore more controllers of increasing complexity and simulate them; design specifications are left to the student s choice. The nonlinear model of the system Given the three coordinates q 1 (t), q 2 (t), q 3 (t) illustrated in Figure 1, q 1 is a linear coordinate, 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 1

2 Lab Problem Since the system is planar, the Jacobian J is both geometric and analytic. 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 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 L L 1 L 2 c 3 B = b 2 + d 2 = L 2 2 C(q 3 ) = ab + cd = L L 1 L 2 c 3 where, as usual and The kinematic co-energy is s 2 = sin(q 2 ), c 2 = cos(q 2 ), s 23 = sin(q 2 + q 3 ), c 23 = cos(q 2 + q 3 ) 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 K = 1 2 { mc ṙ C 2 + m B ṙ B 2 + ω T Γ B ω } 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 A q B q a q 1 q 2 + 2b q 1 q 3 + 2C q 2 q 3 hence K = 1 2 { m C q 2 1+ ω T Γ B ω = Γ B,z ( q 2 + q 3 ) 2 m B [ q A q B q a q 1 q 2 + 2b q 1 q 3 + 2C q 2 q 3 ] + Γ B,z ( q 2 + q 3 ) 2 }

3 Lab Problem The potential energy is P = m B g T r B = m B ( G)( L 1 c 2 L 2 c 23 ) = m B Ga If the model presents elastic elements in the joint, one shall add another term P e P e = 1 2 ( ka q k B q 2 3) with the assumption that the spring rest length is zero. The dissipation function depends on the assumed model friction: if all elements are subject to linear friction, we can write D = 1 2 ( βc q β A q β B q 2 3) The symbols used. i.e., k A, k B, β C, β A, β B have the standard 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 q 1 d K dt q 2 d K dt q 3 ( = (m C + m B ) q 1 + m B a q 2 + da dt q 2 + b q 3 + db ) dt q 3 ( = m B A q 2 + da dt q 2 + a q 1 + da dt q 1 + C q 3 + dc ) dt q 3 + Γ B,z ( q 2 + q 3 ) ( = m B 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 Lab 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 m B L 2 s 23 q 1 q 3 K q 3 = m B (L 1 L 2 s 3 q L 2 s 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 Lab 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 L L 1 L 2 c 3 ) + Γ B,z ] q 2 + Equation 3 + [m B (L L 1 L 2 c 3 ) + Γ B,z ] q 3 2m B L 1 L 2 s 3 q 2 q 3 m B L 1 L 2 s 3 q β 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 L 1 L 2 c 3 ) + Γ B,z ] q 2 + [m B L Γ B,z ] q 3 + m B L 1 L 2 s 3 q 2 2+ 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 + K q = 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 L L 1 L 2 c 3 ) + Γ B,z m B (L L 1 L 2 c 3 ) + Γ B,z m B L 2 c 23 m B (L L 1 L 2 c 3 ) + Γ B,z m B L Γ B,z is symmetric, positive definite, as expected, and represents the overall inertial characteristic of the system; it depends on the generalized coordinates q(t); B and K are simple constant diagonal matrices β C B = 0 β A 0 K = 0 0 β B 0 k A k B

6 Lab 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 to compute, since it contains the terms that contribute to the Coriolis and centripetal acceleration. Take note to not confuse the cosine of a sum of two angles, written as c ij, and a generic element of the matrix C, written as c ij (italic). Considering 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) each term c ij can be computed as (make reference to the course slides) c ij = k h ijk (q) q k where h ijk = 1 2 ( Hij q k + H ik q j are called Christoffel symbols of the first kind. As an example we can compute c 12 and c 23 as H ) jk = h ikj k 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 Lab 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 After performing all necessary computations, 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 = 0 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 + K q + f ) (5) This is the starting point for writing the state equations and use them for Matlab simulation.

8 Lab 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 6 (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 L L 1 L 2 c 3 ) + Γ B,z m B (L L 1 L 2 c 3 ) + Γ B,z m B L 2 c 23 m B (L L 1 L 2 c 3 ) + Γ B,z m B L Γ 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 Lab 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 33 and Solutions B t = β C β A β B C t = C 11 C 12 C C 21 C 22 C C 31 C 32 C K t = k A k B We solve this system of six nonlinear differential equations using two methods: 0 0 f t = 0 f 1 f 2 f 3 1. a MATLAB function, namely ode45, that is described in details at mathworks.com/help/matlab/ref/ode45.html and 2. a Simulink model. (9) (10) MATLAB function The MATLAB function ode45 solves nonstiff differential equations, with the following call: [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 generic 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 Lab 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 Lab 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 Lab 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 Lab Problem q3 = x(3); qp2 = x(5); qp3 = x(6); q23 = q2+q3; qp23 = qp2+qp3; Cmat(1,1) = 0; Cmat(2,1) = 0; 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 Lab 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 Lab 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) = 0; 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 Lab 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 point will not be investigated here.

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

18 25 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. 25

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

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

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

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

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

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