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  |