Robot Esferico 3 Gradoos

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 yrespuesta ante perturbaciones mediante el control monoarticular y multiarticular de un Robot Esférico de tres grados de libertad, así como el cálculo de su cinemática y dinámica. Se usa la herramientas de Simulink y MatLab como software de simulación del motor, el robot y el controlador.

ESQUEMA DEL ROBOT 1 2 3

ak
0 0 0

αk
π /2 π /2
0

dk
L1 0 r

θk
q1 q2 + π / 2
0

Con estainformación se procede a calcular los parámetros necesarios para obtener la dinámica directa, es decir obtener T1 , T2 , y F 3 . 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

Sus parámetros son:

L1 = 0.8m L 2 =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. MARCO TEORICO 1) Cálculo de la Dinámica Directa Los parámetros DH son:
UNIVERSIDAD NACIONAL DE INGENIERÍA

T10 = [ cos(q1), [ sin(q1), [ 0, [ 0, T21 =

0, sin(q1), 0, -cos(q1), 1, 0, 0, 0,

0] 0] L1] 1]

-1-

Analisis y Control de Robots

[ -sin(q2), [ cos(q2), [ 0, [ 0, T32 =[ 1, 0, 0, 0] [ 0, 1, 0, 0] [ 0, 0, 1, r] [ 0, 0, 0, 1]

0, cos(q2), 0, sin(q2), 1, 0, 0, 0,

0] 0] 0] 1]

Jacobianas de velocidad lineal y angular respecto a los Centros de Masa CM.
Jv=[diff(T30(1:3,4),q1) diff(T30(1:3,4),q2) diff(T30(1:3,4),r)] Jinv=simple(inv(Jv)) Jv1=[diff(C1,q1) diff(C1,q2) diff(C1,r)] Jv2=[diff(C2,q1) diff(C2,q2) diff(C2,r)] Jv3=[diff(C3,q1) diff(C3,q2) diff(C3,r)]Jw1=[0 0 0; 0 0 0; 1 0 0] Jw2=[0 sin(q1) 0; 0 -cos(q1) 0; 1 0 0] Jw3=[0 sin(q1) 0; 0 -cos(q1) 0; 1 0 0]

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]

Tensores inerciales k, describe el movimiento trasnacional y rotacional del elemento k
d1=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)

Energía cinética total y energía potencia total
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))

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]

Momentos de Inercia, se considera a los eslabonescoo 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 inerceiales 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:

Ti

Posición de los...