RAUL EDWIN ALEGRIA TORRES 1220701

CURSO: CONTROL DE ROBOTS

LABORATORIO CALIFICADO 2
Profesor: Dr.Sc(c) Walter Castellanos

Sede donde se toma la

Esquina Av. 28 de Julio y AV. Petit Thouars
evaluación
Periodo lectivo 2018-3 Sección 10102 Aula E209 Turno Noche
Fecha de evaluación 17/10/18 Horario 20:15-22:30 Fila (*) A

Indicaciones:
 No está permitido el uso de celulares, los cuales deben de ser apagados y guardados

PREGUNTA 1
En la figura 1 se muestra un manipulador con base fija y masas concentradas m1 y m2.
Considere l1=1 , l2 =1.5, m1= 1, m2= 1.2kg

Figura 1. Manipulador de 2 GL

C1  S1 0 l1C1  C 2  S2 0 l2C2 

S C1 
0 l1 S1  1 S C2 0 l 2 S 2 
0
A1   1 A2   2
0 0 1 0  0 0 1 0 
   
0 0 0 1  0 0 0 1 

C12  S12 0 l1C1  l2C12 

S C12 0 l1S1  l2 S12 
0
A2   12
 0 0 1 0 
 
 0 0 0 1 
 RAUL EDWIN ALEGRIA TORRES 1220701

MATRICES DE TRANSFORMACION
%% CALCULO SIMBOLICO DE LAS MATRICES DE TRANSFORMACION
syms q1
syms q2
syms L1
syms L2

%% MATRIZ DE TRANSFORMACION A01

%rotacion en el eje z
rotz1=[cos(q1) -sin(q1) 0 0;...
sin(q1) cos(q1) 0 0;...
0 0 1 0;...
0 0 0 1]
%translacion en el eje x
traslx1=[1 0 0 L1;...
0 1 0 0;...
0 0 1 0;...
0 0 0 1]
%matriz A01
A01= rotz1*traslx1

%% MATRIZ DE TRANSFORMACION A12

%rotacion en el eje z
rotz2=[cos(q2) -sin(q2) 0 0;...
sin(q2) cos(q2) 0 0;...
0 0 1 0;...
0 0 0 1]
%translacion en el eje x
traslx2=[1 0 0 L2;...
0 1 0 0;...
0 0 1 0;...
0 0 0 1]
%matriz A01
A12= rotz2*traslx2

%% MATRIZ DE TRASNFORMACION A02

A02= A01 * A12
 RAUL EDWIN ALEGRIA TORRES 1220701

a. Elaborar el modelo de Lagrange en Matlab (10p)

%:::::::::::::::::.ALGORITMO DE LAGRANGE:::::::::::::::::::::::
%Solución del modelo dinámico inverso Robot polar de 2GDL

%::::::::::::::DEFINICION DE LA TRAYECTORIA::::::::::::::::::::

%DEFINICION SIMBOLICA DE LA TRAYECTORIA

syms t % se declara t como variable simbolica

sq1='sin(2*t)+pi/2' % trayectoria del primer eslabon

sq2='3' % trayectoria del segundo eslabon

%OBTENCION SIMBOLICA DE LA VELOCIDAD

sqd1=diff(sq1,t) %derivada de la trayectoria del primer eslabon
sqd2=diff(sq2,t) %derivada de la trayectoria del segundo eslabon

%OBTENCION SIMBOLICA DE LA ACELERACION

sqdd1=diff(sqd1,t) %segunda derivada
sqdd2=diff(sqd2,t) %segunda derivada

%PARAMETROS DIMENSIONALES
L1=1 % Longitud del primer eslabon
L2=1.5 % longitud del segundo eslabon
m1=1 % masa del primer eslabon
m2=1.2 % masa del segundo eslabon
g=9.8 % aceleracion de la gravedad
G=[0 0 -g 0] % vector de gravedad expresado en el sistema S0

r11 = [0 0 0 1]' % vector de coordenadas homogeneas de

r22 = [0 0 0 1]' % pocision del centro de masa del eslabon

%MATRICES DE DERIVACION
Qr=[0 -1 0 0;...
1 0 0 0;... % SI LA ARTICULACION ES DE ROTACION
0 0 0 0;...
0 0 0 0]

Qd=[0 0 0 0;...
0 0 0 0;... % SI LA ARTICULACION ES DE TRASLACION
0 0 0 1;...
0 0 0 0]

%BUCLE DE TIEMPO

for tk= 1:1:50

t= (tk-1)/10

%EVALUACION DE POCISION, VELOCIDAD Y ACELERACION

q1=eval(sq1); Q1(tk)=q1
q2=eval(sq2); Q2(tk)=q2

qd1= eval(sqd1);
qd2= eval(sqd2);

qdd1= eval(sqdd1);
qdd2= eval(sqdd2);

%% PASOS 1 y 2 Evaluación de matrices de transformación Aij

A00=[1 0 0 0;...
0 1 0 0;...
0 0 1 0;...
0 0 0 1]

A11=[1 0 0 0;...
0 1 0 0;...
0 0 1 0;...
0 0 0 1]
 RAUL EDWIN ALEGRIA TORRES 1220701

A01=[cos(q1) -sin(q1) 0 L1*cos(q1);... %MATRIZ DE TRANSFORMACION

sin(q1) cos(q1) 0 L1*sin(q1);... % A01
0 0 1 0 ;...
0 0 0 1 ]

A12= [cos(q2) -sin(q2) 0 L2*cos(q2);... %MATRIZ DE TRANSFORMACION

sin(q2) cos(q2) 0 L2*sin(q2);... % A12
0 0 1 0 ;...
0 0 0 1 ]

A02= A01*A12 %MATRIZ DE TRANSFORMACION

% A02

% PASO 3 Evaluación de matrices Uij

U11=A00*Qr*A01
U12=zeros(4)
U21=A00*Qr*A02
U22=A01*Qd*A12

% PASO 4 Evaluación de matrices Uijk

U111=A00*Qr*A00*Qr*A01
U112=zeros(4)
U121=zeros(4)
U122=zeros(4)
U211=A00*Qr*A00*Qr*A02
U212=A00*Qr*A01*Qd*A12
U221=A00*Qr*A01*Qd*A12
U222=A01*Qd*A11*Qd*A12

% PASO 5 Evaluación de matrices de pseudoinercia J1

J1=zeros(4); %se crea la matriz de ceros de 4x4
J1(3,3)=L1^2*m1; %se le asigna el valor en cada
J1(3,4)=L1*m1; %pocision de la matriz segun la
J1(4,3)=L1*m1; %morfologia del robot
J1(4,4)=m1;

J2=zeros(4); %matriz J2 de pseudoinercia

J2(4,4)=m2;

%PASO 6 Evaluación de la matriz de inercia D

D(1,1)=trace (U11*J1*U11')+trace (U21*J2*U21')
D(1,2)=trace (U22*J2*U21')
D(2,1)=D(1,2)
D(2,2)=trace (U22*J2*U22')

%PASO 7 – 8 Evaluación de la matriz de Coriolis H

h111=trace (U111*J1*U11')+trace (U211*J2*U21')
h112=trace (U212*J2*U21')
h121=trace (U221*J2*U21')
h122=trace (U222*J2*U21')
h211=trace (U211*J2*U22')
h212=trace (U212*J2*U22')
h221=trace (U221*J2*U22')
h222=trace (U222*J2*U22')

H(1,1)=h111*qd1*qd1 +h112*qd1*qd2 +h121*qd2*qd1 +h122*qd2*qd2

H(2,1)=h211*qd1*qd1 +h212*qd1*qd2 +h221*qd2*qd1 +h222*qd2*qd2

%PASO 9 Evaluación de la matriz de Gravedad

C(1,1)= -m1*G*U11*r11 -m2*G*U21*r22
C(2,1)= -m1*G*U12*r11 -m2*G*U22*r22

%PASO 10 Evaluación de los pares

PARES(:,tk)= D*[qdd1 qdd2]'+ H+ C %ecuacion dinamica del robot

end

figure(1),plot (Q1)
figure(2),plot (Q2)
 RAUL EDWIN ALEGRIA TORRES 1220701

figure(3),plot(PARES(1,:))

trayectoria deseada en el primer eslabon:

Trayectoria deseada del segundo eslabon:

 RAUL EDWIN ALEGRIA TORRES 1220701

Grafica representativa de la ecuación dinámica del robot:

Par motor efectivo – fuerza motora efectiva que actúa en el eslabón 2
 RAUL EDWIN ALEGRIA TORRES 1220701

b. Elaborar el modelo de Newton Euler en Matlab (10p)

%:::::::::::::::::.ALGORITMO DE NEWTON EULER::::::::::::::::::::::

%Solución del modelo dinámico Robot de 2GDL

L{1}=link([-pi/2 0 0 0],'standard') %ESLABON 1

L{2}=link([ 0 0 0 1],'standard') %ESLABON 2
L{1}.m=20 %masa del eslabon 1
L{2}.m=10 %masa del eslabon 2
L{1}.r=[ 0 0 1] %distancia del centro de masa
L{2}.r=[ 0 0 1.5] %distancia del centro de masa
L{1}.I=[ 0 0 200 0 0 0] %matriz de inercia del eslabon1
L{2}.I=[ 0 0 0 0 0 0] %matriz de inercia del eslabon2
L{1}.Jm= 0
L{2}.Jm= 0
L{1}.G= 1
L{2}.G= 1
L{1}.B= 0
L{2}.B= 0
L{1}.Tc= [0 0]
L{2}.Tc= [0 0]
%definicion del robot
r=robot(L,'robot de 2GL')
grav=[-9.8, 0,0]

%DEFINICION SIMBOLICA DE LA TRAYECTORIA

syms t
sq1='sin(2*t)+pi/2'
sq2='1'

%OBTENCION SIMBOLICA DE LA VELOCIDAD Y ACELERACION

sqd1=diff(sq1,t);
sqd2=diff(sq2,t);

sqdd1=diff(sqd1,t);
sqdd2=diff(sqd2,t);

%EVALUACION DE LA POCISION, VELOCIDAD Y ACELERACION

for tk= 1:1:50
t= (tk-1)/10
q(tk,1)=eval(sq1); qd(tk,1)= eval(sqd1); qdd(tk,1)= eval(sqdd1)
q(tk,2)=eval(sq2); qd(tk,2)= eval(sqd2); qdd(tk,2)= eval(sqdd2)
XY(tk,1)=q(tk,2)*cos(q(tk,1))
XY(tk,2)=q(tk,2)*sin(q(tk,1))
end

%GRAFICAS DE LAS TRAYECTORIAS DESEADAS

subplot(2,3,1),plot(q), title('POCISION')
subplot(2,3,2),plot(qd), title('VELOCIDAD')
subplot(2,3,3),plot(qdd),title('ACELERACION')

% OBTENCION DE LOS PARES Y COMPONENTES DE INERCIA,

% CORIOLIS Y GRAVEDAD

Tau=rne(r,q,qd,qdd,grav) %funcion 'rne'

TauI=itorque(r,q,qd) %inercia
TauC=coriolis(r,q,qd) %coriolis
TauG=gravload(r,q,grav) %gravedad
 RAUL EDWIN ALEGRIA TORRES 1220701

%GRAFICA DE LOS COMPONENTES DE INERCIA, CORIOLIS Y GRAVEDAD

subplot(2,3,4), plot(TauI (:,1)), title('INERCIA')
subplot(2,3,5), plot(TauC (:,1)), title('CORIOLIS')
subplot(2,3,6), plot(TauG (:,1)), title('GRAVEDAD')

GRAFICAS DE LAS TRAYECTORIAS Y COMPONENTES DEL MODELO

DINAMICO