UNIVERSIDAD NACIONAL DE INGENIERÍA -1- 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 respuesta 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 Sus parámetros son: 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. MARCO TEORICO 1) Cálculo de la Dinámica Directa Los parámetros DH son: 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.
Las matrices de transformación serán: %matrices DH T10=[cos(q1) 0 sin(q1) 0 sin(q1) 0 -cos(q1) 0 0 1 0 L1 0 0 0 1] T21=[-sin(q2) 0 cos(q2) 0 cos(q2) 0 sin(q2) 0 0 10 0 0 0 0 1] T32=[1 0 0 0 0100 001r 0 0 0 1] T20=T10*T21 T30=T10*T21*T32
T10 = 0, sin(q1), 0, -cos(q1), 1, 0, 0, 0, 0] 0] L1] 1] [ cos(q1), [ sin(q1), [ 0, [ 0, T21 =
. dt ?qi ? Analisis y Control de Robots 0, cos(q2), 0, sin(q2), 1, 0, 0, 0, 0] 0] 0] 1] [ -sin(q2), [ cos(q2), [ 0, [ 0, T32 = [ 1, 0, 0, 0] [ 0, 1, 0, 0] [ 0, 0, 1, r] [ 0, 0, 0, 1] T20 = [ -cos(q1)*sin(q2), sin(q1), cos(q1)*cos(q2), [ -sin(q1)*sin(q2), -cos(q1), sin(q1)*cos(q2), 0] 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] 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 eslabones coo 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]
Tensores inerceiales 3×3 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))
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)*L2 1/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
UNIVERSIDAD NACIONAL DE INGENIERÍA 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]
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^2- 3*m3*r*L3+m3*L3^2)+1/2*dq2^2*(1/3*m2*L2^2+m3*r^2- m3*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))
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^2- 3*m3*r*L3+m3*L3^2)+1/2*dq2^2*(1/3*m2*L2^2+m3*r^2- m3*r*L3+1/3*m3*L3^2)+1/2*dr^2*m3- g*(1/2*m1*L1+m2*(L1+1/2*sin(q2)*L2)+m3*(sin(q2)*r+L1- 1/2*sin(q2)*L3))
Los torques y la fuerza se calculan usando el Lagrangiano mediante la formulación de Lagrange, esto es: ' Fi Ti L ' ?qi (q(t),q (t)) L(q(t),q'(t)) – d ? = 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 y fuerzas %—————– q=[q1; q2; r]; ddq=[ddq1; ddq2; ddr]; m=[m1; m2; m3]; C=[C1 C2 C3]; %Torque 1 %——– DD1=d(1,1:3) cor1=0; h1=0; for k=1:3
-6 –
= -0.020833m/s VX = = 0.066666m/s VY = = 0.025m/s VZ = for j=1:3 cor1=simple(cor1+(diff(d(1,j),q(k))- 1/2*diff(d(k,j),q(1)))*dq(k)*dq(j)); end end cor1 h1=simple(-[0 0 – g]*(m1*diff(C1,q(1))+m2*diff(C2,q(1))+m3*diff(C3,q(1)))) T1=simple(DD1*ddq+cor1+h1) %Torque 2 %——– DD2=d(2,1:3) cor2=0; h2=0; for k=1:3 for j=1:3 cor2=simple(cor2+(diff(d(2,j),q(k))- 1/2*diff(d(k,j),q(2)))*dq(k)*dq(j)); end end cor2 h2=simple(-[0 0 – g]*(m1*diff(C1,q(2))+m2*diff(C2,q(2))+m3*dif
| Página siguiente |