Robot esferico de tres grados de libertad

Analisis y Control de Robots

CONTROL DE TRAYECTORIA DE UN ROBOT ESFERICO DE TRES GRADOS DE LIBERTAD
Integrantes: Renato Masias Galarza Alex Ramón Castillo Rony Rivas Quispe Leonel Orco Castillo Felipe Gómez Fernandez Javier Morales Cabello Profesor: Ing. Anchayhua Arestegui, Nilton Cesar Facultad de Ingeniería Mecánica ABSTRACT Este artículo describe el seguimiento de trayectoria y respuestaante perturbaciones usando el control monoarticular para un Robot Esférico de tres grados de libertad, así como el cálculo de su cinemática y dinámica. Se usa las herramientas de Simulink y MatLab como software de simulación del motor, el robot y el controlador.

ESQUEMA DEL ROBOT

1) Cálculo de la Dinámica Directa Los parámetros DH son:

ak
1 2 3 0 0 0

αk
π /2 π /2
0

dk
L1 0 rθk
q1

q2 + π / 2
0

Con esta información se procede a calcular los parámetros necesarios para obtener la dinámica directa, es decir obtener T1 , T2 , y F 3 . Sus parámetros son: Las matrices de transformación serán:
%matrices DH T10=[cos(q1) 0 sin(q1) 0 sin(q1) 0 -cos(q1) 0 0 10 L1 0 00 1] T21=[-sin(q2) 0 cos(q2) 0 cos(q2) 0 sin(q2) 0 0 10 0 0 00 1] T32=[1 0 0 0 0100 001r 0 0 0 1]T20=T10*T21 T30=T10*T21*T32

L1 = 0.8m L2 = 0.6m L3 = 0.6m m1 = 10kg m2 = 2kg m3 = 0.2kg
Los Centros de Masa están ubicados en el centro de cada eslabón, se supone eslabones cilíndricos huecos para el cálculo de los momentos de inercia I) MARCO TEORICO

UNIVERSIDAD NACIONAL DE INGENIERÍA

-1-

Analisis y Control de Robots

T20 = [ -cos(q1)*sin(q2), sin(q1), cos(q1)*cos(q2), 0] [-sin(q1)*sin(q2), -cos(q1), sin(q1)*cos(q2), 0] [ cos(q2), 0, sin(q2), L1] [ 0, 0, 0, 1] T30 = [-cos(q1)*sin(q2),, sin(q1), cos(q1)*cos(q2), cos(q1)*cos(q2)*r] [-sin(q1)*sin(q2), -cos(q1), sin(q1)*cos(q2), sin(q1)*cos(q2)*r] [ cos(q2), 0, sin(q2), sin(q2)*r+L1] [ 0, 0, 0, 1]

Jw3=[0 sin(q1) 0; 0 -cos(q1) 0; 1 0 0]

Tensores inerciales k, describe los movimientos de traslación y rotación del elemento kd1=transpose(Jv1)*m1*Jv1+transpose(Jw1)*D1*Jw1 d2=simple(transpose(Jv2)*m2*Jv2+transpose(Jw2)*D2*Jw2) d3=simple(transpose(Jv3)*m3*Jv3+transpose(Jw3)*D3*Jw3) d=simple(d1+d2+d3)

Posición de los CMk respecto a su propio sistema XkYkZk
c1=[0; -L1/2; 0] c2=[0; 0; L2/2] c3=[0; 0; -L3/2]

Energía cinética total y energía potencia total son:
dq=[dq1; dq2; dr]; Ec=1/2*transpose(dq)*d*dq Ep=-[0 0-g]*(m1*C1+m2*C2+m3*C3) Ec = 1/6*dq1^2*cos(q2)^2*(m2*L2^2+3*m3*r^23*m3*r*L3+m3*L3^2)+1/2*dq2^2*(1/3*m2*L2^2+m3*r^2m3*r*L3+1/3*m3*L3^2)+1/2*dr^2*m3 Ep = g*(1/2*m1*L1+m2*(L1+1/2*sin(q2)*L2)+m3*(sin(q2)*r+ L1-1/2*sin(q2)*L3))

Momentos de Inercia, se considera a los eslabones como cilindros huecos:
I1=m1*L1^2/12*[1 0 0; 0 0 0; 0 0 1] I2=m2*L2^2/12*[1 0 0; 0 1 0; 0 0 0] I3=m3*L3^2/12*[1 0 0; 0 1 0; 0 0 0]El Lagrangiano será entonces la resta de la energía cinética y la potencial, dando entonces:
L=1/6*dq1^2*cos(q2)^2*(m2*L2^2+3*m3*r^23*m3*r*L3+m3*L3^2)+1/2*dq2^2*(1/3*m2*L2^2+m3*r^2m3*r*L3+1/3*m3*L3^2)+1/2*dr^2*m3g*(1/2*m1*L1+m2*(L1+1/2*sin(q2)*L2)+m3*(sin(q2)*r+L11/2*sin(q2)*L3))

Tensores inerciales 3x3 del eslabón k respecto a XoYoZo y trasladado a su Centro de Masa
R10=[T10(1:3,1:3)];R20=[T20(1:3,1:3)]; R30=[T30(1:3,1:3)]; D1=simple(R10*I1*transpose(R10)) D2=simple(R20*I2*transpose(R20)) D3=simple(R30*I3*transpose(R30))

Los torques y la fuerza se calculan usando el Lagrangiano mediante la formulación de Lagrange, esto es:

Posición de los CM respecto a XoYoZo
C1=T10(1:3,4)+R10*c1 C2=T20(1:3,4)+R20*c2 C3=T30(1:3,4)+R30*c3 C1 = 0 0 1/2*L1 C2 = 1/2*cos(q1)*cos(q2)*L21/2*sin(q1)*cos(q2)*L2 L1+1/2*sin(q2)*L2 C3 = cos(q1)*cos(q2)*r-1/2*cos(q1)*cos(q2)*L3 sin(q1)*cos(q2)*r-1/2*sin(q1)*cos(q2)*L3 sin(q2)*r+L1-1/2*sin(q2)*L3

Ti

Fi

=

d ∂ ∂ . L L − ' ' dt ∂qi' ( q (t ),q (t )) ∂qi ( q (t ),q (t ))

Pero en MatLab se usa el método computacional donde no se deriva respecto al tiempo, el procedimiento para los dos torques y la fuerza es el siguiente:
%Torques...