Instituto Politécnico Nacional
Unidad Profesional Interdisciplinaria en Ingeniería y
Tecnologías Avanzadas
Control De Sistemas Robóticos
ALUMNO:
Martínez Barrón Carlos Alberto
GRUPO:
4MV6
PROFESOR:
Guillermo Puriel Gil
FECHA:
13 de julio de 2020
Reportes Proyecto
Índice
1 Introducción ................................................................................................................................ 3
2 Metodología ................................................................................................................................ 3
3 Resultados ................................................................................................................................. 15
4 Conclusiones.............................................................................................................................. 17
5 Bibliografía ................................................................................................................................ 17
1 INTRODUCCIÓN
En el presente documento se muestra como se realizó un brazo robot de 2 GDL en Solidworks para
posteriormente exportarlo a simulink con SimMechanics segunda generación, posteriormente se
mostrará como se obtuvieron los parámetros para validar el modelo y después la programación de
la trayectoria en el espacio cartesiano que seguirá dicho robot con ayuda de un controlador.
2 METODOLOGÍA
Lo primero que se realizó fueron los eslabones del brazo robot de 2 GDL en SolidWorks los cuales
su diseño fue tomado del libro Robótica Control de Robots Manipuladores [1]. Para realizar el
eslabón primero se dibujó el perfil del mismo el cual se muestra a continuación.
Figura 1 Perfil del eslabón.
Una vez hecho el perfil del eslabón se le aplico una operación de extruir en 20 mm. El eslabón queda
como se muestra a continuación.
Figura 2 Eslabón extruido.
Después lo que se realizó fueron los huecos que tiene el eslabón para lo cual se realizó un croquis
sobre la cara frontal del eslabón.
Figura 3 Croquis para realizar los huecos.
Una vez hecho este croquis se utilizó la herramienta de extruir corte y se aplicó por 20 mm.
Figura 4 Eslabón con los huecos.
Finalmente, para hacer que el eslabón se vea más estético se hicieron dos huecos uno por 10 mm y
otro por 5 mm donde está el hueco superior
Figura 5 Eslabón totalmente terminado.
Una vez terminado el eslabón se procedió a realizar el hombro para lo cual se realizó un croquis
cuadrado con un barreno central el cual se muestra a continuación.
Figura 6 Croquis del hombro.
Una vez hecho este croquis se aplicó la herramienta de extruir por 20 mm.
Figura 7 Hombro terminado.
Una vez terminadas ambas piezas se procedió a realizar el ensamble en SolidWorks, para lo cual
primero se posicionaron en el espacio de trabajo el hombro y dos eslabones, luego se aplicaron
relaciones de coincidencia entre la cara frontal del hombro y la cara trasera de un eslabón, luego se
aplicó una relación de concentricidad entre el barreno del hombro y el barreno superior de un
eslabón, después se aplicó una relación de coincidencia entre la parte frontal del eslabón que ya
está fijo en el hombro con la parte trasera del eslabón que aun está libre, finalmente se aplica una
relación de concentricidad entre el barreno inferior del eslabón que esta fijo al hombro con el
eslabón restante.
Figura 8Brazo robot de 2 GDL terminado.
Una vez terminado el ensamble se procedió a exportar el modelo desde el menú herramientas, de
ahí se selecciona SimMechanics Link luego se selecciona exportar y finalmente elegimos la segunda
generación de SimMechanics y se guarda el modelo.
Figura 9 Ruta para exportar el modelo.
Una vez exportado el ensamble desde Matlab se utiliza el comando smimport que recibe como
parámetro el nombre del ensamble con terminación xml lo cual nos exportara el modelo a simulink.
Figura 10 Brazo robot de 2 GDL exportado a simulink.
Una vez exportado el modelo se procedió a obtener los parámetros del robot los cuales se muestran
a continuación:
Figura 11 Parámetros de los eslabones.
El único parámetro que se propuso fu el del coeficiente de fricción viscosa. Para su mejor apreciación
a continuación se muestra un desglose de los parámetros que se utilizaron en la validación.
Masa
𝑚 = 0.55626880927785183 𝑘𝑔
Centro de masa
𝑙𝑐 = 131.57552469897729 𝑚𝑚
Inercia
𝐼 = 12499.261280529634 𝑘𝑔𝑚𝑚2
Coeficientes de fricción viscosa
𝑁𝑚 𝑁𝑚
𝐵1 = 0.0017 𝐵2 = 0.0013
𝑟𝑎𝑑 𝑟𝑎𝑑
𝑠 𝑠
Una vez obtenidos estos parámetros se procedió a programar la planta del archivo de validación, el
código se muestra a continuación.
function qpp = fcn(tau1,tau2,q1,q2,q1p,q2p)
%PARAMETROS
m1 = 0.55626880927785183;
l1 = (500/1e3)-(60/1e3)-(30/1e3);
lc1 = 131.57552469897729/1e3;
I1 = 12499.261280529634/1e6;
b1 = 0.0017;
m2 = 0.55626880927785183;
l2 = (500/1e3)-(60/1e3)-(30/1e3);
lc2 = 131.57552469897729/1e3;
I2 = 12499.261280529634/1e6;
b2 = 0.0013;
g = 9.81;
%MATRIZ DE MASAS
m11=m1*lc1^2+m2*l1^2+m2*lc2^2+2*m2*l1*lc2*cos(q2)+I1+I2;
m12=m2*lc2^2+m2*l1*lc2*cos(q2)+I2;
m21=m2*lc2^2+m2*l1*lc2*cos(q2)+I2;
m22=m2*lc2^2+I2;
M = [m11 m12; m21 m22];
%MATRIZ DE INERCIA
c11=-2*m2*l1*lc2*sin(q2)*q2p;
c12=-m2*l1*lc2*sin(q2)*q2p;
c21=m2*l1*lc2*sin(q2)*q1p;
c22=0;
C=[c11 c12;c21 c22];
%MATRIZ DE FRICCION VISCOSA
b11=b1;
b12=0;
b21=0;
b22=b2;
B=[b11 b12;b21 b22];
%VECTOR DE GRAVEDAD
g11=(m1*lc1+m2*l1)*g*sin(q1)+m2*g*lc2*sin(q1+q2);
g21=m2*g*lc2*sin(q1+q2);
G=[g11;g21];
%VECTOR DE POSICIONES
q=[q1;q2];
%VECTOR DE VELOCIDADES
qp=[q1p;q2p];
%VECTOR DE ENTRADAS
T=[tau1;tau2];
%PLANTA
qpp=M\(T-C*qp-B*qp-G);
Una vez programada la planta se procedió a programar la animación, código que se muestra a
continuación.
function P=animacion(u)
l1=500/1e3;
l2=500/1e3;
q1=u(1);
q2=u(2);
x=[0 l1*sin(q1) l1*sin(q1)+l2*sin(q1+q2)];
y=[0 -l1*cos(q1) -(l1*cos(q1)+l2*cos(q1+q2))];
P(1,1)=0;
plot(x,y,'o-')
grid
axis([-1.1 1.1 -1.1 1.1])
𝜋
Las condiciones iniciales para la validación son 𝑞1 = 2
y 𝑞2 = 0
Figura 12 Validación del robot hecho en SolidWorks.
Como se puede observar en la figura anterior las gráficas de posición del eslabón 1 y del eslabón 2
quedan sobre las de la validación por lo cual se puede concluir que el robot elaborado en SolidWorks
se comporta de acuerdo a como lo dictan las ecuaciones dinámicas de un robot de 2 GDL y que los
parámetros que nos brindó SimMechanics de los eslabones son correctos.
Una vez validado el modelo se procedió a buscar la trayectoria a seguir la cual es el perfil de un alce,
la ecuación fue obtenida de la página de Wolfram Alpha [2]
Figura 13 Ecuación del alce en Wolfram.
Posteriormente esta ecuación se trasladó a Matlab dando como resultado la siguiente gráfica.
Figura 14 Perfil del alce.
Algo que hay que aclarar es que la ecuación fue modificada ya que era muy grande por lo cual se
escaló tanto en x como en y dividiendo entre un factor de 1500 y también se aplicó un corrimiento
de 0.5 en el eje x. Una vez ajustado el perfil del alce se procedió a realizar la programación del
controlador la cual se muestra a continuación.
function T = fcn(q1d,q2d,qd1p,qd2p,qd1pp,qd2pp,q1,q2,q1p,q2p)
%PARAMETROS
m1 = 0.55626880927785183;
l1 = (500/1e3)-(60/1e3)-(30/1e3);
lc1 = 131.57552469897729/1e3;
I1 = 12499.261280529634/1e6;
b1 = 0.0017;
m2 = 0.55626880927785183;
l2 = (500/1e3)-(60/1e3)-(30/1e3);
lc2 = 131.57552469897729/1e3;
I2 = 12499.261280529634/1e6;
b2 = 0.0013;
g = 9.81;
%MATRIZ DE MASAS
m11=m1*lc1^2+m2*l1^2+m2*lc2^2+2*m2*l1*lc2*cos(q2)+I1+I2;
m12=m2*lc2^2+m2*l1*lc2*cos(q2)+I2;
m21=m2*lc2^2+m2*l1*lc2*cos(q2)+I2;
m22=m2*lc2^2+I2;
M = [m11 m12; m21 m22];
%MATRIZ DE INERCIA
c11=-2*m2*l1*lc2*sin(q2)*q2p;
c12=-m2*l1*lc2*sin(q2)*q2p;
c21=m2*l1*lc2*sin(q2)*q1p;
c22=0;
C=[c11 c12;c21 c22];
% %MATRIZ DE FRICCION VISCOSA
b11=b1;
b12=0;
b21=0;
b22=b2;
B=[b11 b12;b21 b22];
%VECTOR DE GRAVEDAD
g11=(m1*lc1+m2*l1)*g*sin(q1)+m2*g*lc2*sin(q1+q2);
g21=m2*g*lc2*sin(q1+q2);
G=[g11;g21];
kp1=75;%75
kp2=60;%60
kd1=70;%70
kd2=58;%58
%GANANCIAS
kp11=kp1;
kp12=0;
kp21=0;
kp22=kp2;
Kp=[kp11 kp12;kp21 kp22];
kd11=kd1;
kd12=0;
kd21=0;
kd22=kd2;
Kd=[kd11 kd12;kd21 kd22];
%VECTOR DE VELOCIDADES
qp=[q1p;q2p];
%VECTOR DE POSICIONES DESEADO
qd=[q1d;q2d];
%VECTOR DE VELOCIDADES DESEADO
qdp=[qd1p;qd2p];
%VECTOR DE ACELERACIONES DESEADO
qdpp=[qd1pp;qd2pp];
%VECTOR DE ERROR POSICIONES
qh=[q1d-q1;q2d-q2];
%VECTOR DEL ERROR DE VELOCIDAD
qhp=[qd1p-q1p;qd2p-q2p];
%%%CONTROL PAR CALCULADO%%%
T=M*(qdpp+Kd*qhp+Kp*qh)+C*qp+B*qp+G;
%%%CONTROL PD +%%%
%T=Kp*qh+Kd*qhp+M*qdpp+C*qdp+B*qdp+G;
%MATRIZ DE MASAS M(qd)
m11d=m1*lc1^2+m2*l1^2+m2*lc2^2+2*m2*l1*lc2*cos(q2d)+I1+I2;
m12d=m2*lc2^2+m2*l1*lc2*cos(q2d)+I2;
m21d=m2*lc2^2+m2*l1*lc2*cos(q2d)+I2;
m22d=m2*lc2^2+I2;
Mqd = [m11d m12d; m21d m22d];
%MATRIZ DE INERCIA
c11d=-2*m2*l1*lc2*sin(q2d)*qd2p;
c12d=-m2*l1*lc2*sin(q2d)*qd2p;
c21d=m2*l1*lc2*sin(q2d)*qd1p;
c22d=0;
Cqd=[c11d c12d;c21d c22d];
%VECTOR DE GRAVEDAD
g11d=(m1*lc1+m2*l1)*g*sin(q1d)+m2*g*lc2*sin(q1d+q2d);
g21d=m2*g*lc2*sin(q1d+q2d);
Gqd=[g11d;g21d];
%%%PD CON PRECOMPENSACION
%T=Kp*qh+Kd*qhp+Mqd*qdpp+Cqd*qdp+B*qdp+Gqd;
Finalmente, para la visualización adecuada del seguimiento de trayectoria se puso fija la trayectoria
del alce en el código de la animación, por lo cual el código modificado de la animación se muestra a
continuación.
function P=animacion(u)
l1=500/1e3;
l2=500/1e3;
q1=u(1);
q2=u(2);
x=[0 l1*sin(q1) l1*sin(q1)+l2*sin(q1+q2)];
y=[0 -l1*cos(q1) -(l1*cos(q1)+l2*cos(q1+q2))];
P(1,1)=0;
plot(x,y,'o-')
grid
hold on
t=0:0.01:2*pi;
hx=0.5;
hy=0;
xd=hx+(1/2*sin(55/13-100*t)+2/7*sin(20/7-97*t)+8/11*sin(69/17-
96*t)+2/11*...
sin(4/3-93*t)+5/11*sin(67/19-92*t)+4/15*sin(7/11-91*t)+6/17*sin(5/7-
...
88*t)+3/5*sin(27/10-87*t)+1/2*sin(20/11-85*t)+2/5*sin(32/21-83*t)+...
1/5*sin(29/14-81*t)+4/13*sin(2/5-79*t)+5/8*sin(5/7-77*t)+5/7*...
sin(53/21-75*t)+2/5*sin(11/7-74*t)+8/9*sin(18/7-73*t)+2/5*sin(1/2-...
72*t)+11/12*sin(53/18-71*t)+7/8*sin(10/11-69*t)+7/5*sin(17/8-68*t)+...
11/9*sin(33/8-67*t)+1/3*sin(25/11-66*t)+7/10*sin(34/11-65*t)+7/4*...
sin(14/9-63*t)+sin(31/10-62*t)+sin(1/8-61*t)+4/9*sin(12/7-60*t)+...
17/10*sin(46/11-59*t)+8/9*sin(11/8-58*t)+7/9*sin(1/9-57*t)+5/8*...
sin(37/13-56*t)+1/20*sin(53/16-55*t)+27/16*sin(7/6-54*t)+16/9*...
sin(17/4-53*t)+16/11*sin(26/9-51*t)+28/11*sin(37/8-50*t)+17/9*...
sin(14/9-49*t)+1/8*sin(3/5-48*t)+7/11*sin(10/3-47*t)+7/8*sin(33/14-...
46*t)+8/5*sin(67/15-45*t)+3/5*sin(29/10-43*t)+53/16*sin(37/11-
42*t)+...
29/9*sin(7/11-41*t)+59/16*sin(3-40*t)+14/9*sin(86/19-39*t)+17/10*...
sin(11/4-38*t)+29/8*sin(43/17-34*t)+21/8*sin(41/11-33*t)+30/7*...
sin(5/11-32*t)+13/9*sin(37/8-31*t)+52/9*sin(1/3-28*t)+62/13*sin(21/5-
...
27*t)+38/13*sin(13/3-26*t)+11/4*sin(28/11-25*t)+48/13*sin(1/4-
23*t)+...
19/11*sin(4/3-22*t)+49/11*sin(31/9-21*t)+17/3*sin(1/11-20*t)+10*...
sin(17/6-18*t)+85/6*sin(11/6-16*t)+228/11*sin(6/13-15*t)+9/10*...
sin(12/11-14*t)+244/13*sin(10/7-13*t)+40/7*sin(1/4-11*t)+433/17*...
sin(1/5-10*t)+111/5*sin(27/8-9*t)+871/29*sin(1/22-7*t)+292/15*...
sin(19/7-6*t)+127/11*sin(74/17-5*t)+1150/19*sin(1/10-4*t)+211/6*...
sin(21/10-3*t)+1077/8*sin(9/5-2*t)+3027/7*sin(57/14-t)-
95/3*sin(8*t+...
1/4)-49/5*sin(12*t+7/9)-107/9*sin(17*t+5/6)-57/14*sin(19*t+5/4)-
15/4*...
sin(24*t+7/9)-65/11*sin(29*t+1/31)-43/9*sin(30*t+15/14)-
8/3*sin(35*t+...
37/38)-137/46*sin(36*t+3/2)-3/7*sin(37*t+5/7)-31/14*sin(44*t+5/4)-...
5/3*sin(52*t+6/13)-7/5*sin(64*t+7/9)-7/12*sin(70*t+1/8)-
1/2*sin(76*t+...
7/8)-3/8*sin(78*t+1/16)-1/3*sin(80*t+19/15)-9/14*sin(82*t+1/9)-7/9*...
sin(84*t+56/55)-28/29*sin(86*t+5/7)-1/3*sin(89*t+5/12)-
2/7*sin(90*t+...
9/14)-1/33*sin(94*t+3/11)-1/11*sin(95*t+11/9)-1/15*sin(98*t+13/11)-...
1/12*sin(99*t+1/2))/1500;
yd=hy+(1/5*sin(17/10-100*t)+3/13*sin(7/12-99*t)+1/4*sin(37/15-
98*t)+2/5*...
sin(25/11-96*t)+5/8*sin(13/7-94*t)+5/12*sin(41/10-93*t)+1/2*sin(3/8-
...
92*t)+5/11*sin(15/4-91*t)+1/16*sin(21/5-90*t)+3/8*sin(31/8-
88*t)+4/7*...
sin(2/11-87*t)+1/21*sin(19/7-86*t)+3/8*sin(22/9-84*t)+1/7*sin(4/9-
83*...
t)+4/9*sin(29/13-82*t)+1/20*sin(29/7-80*t)+11/17*sin(61/15-
79*t)+5/7*...
sin(26/11-78*t)+5/9*sin(19/13-76*t)+4/7*sin(41/10-75*t)+22/21*...
sin(9/10-74*t)+15/16*sin(52/17-73*t)+5/11*sin(17/18-72*t)+4/9*...
sin(10/7-71*t)+4/13*sin(44/15-70*t)+2/3*sin(16/9-69*t)+5/7*sin(56/13-
...
68*t)+14/11*sin(26/9-66*t)+7/8*sin(74/21-63*t)+26/11*sin(29/12-
61*t)+...
49/48*sin(7/13-59*t)+13/7*sin(35/9-58*t)+4/5*sin(28/9-57*t)+33/14*...
sin(32/11 - 55*t)+9/5*sin(23/24-53*t)+95/48*sin(19/5-52*t)+89/22*...
sin(20/13-50*t)+93/23*sin(55/12-49*t)+26/5*sin(15/13-48*t)+47/12*...
sin(13/3-47*t)+23/12*sin(11/16-46*t)+13/9*sin(31/9-45*t)+19/9*...
sin(7/10-44*t)+15/7*sin(5/3-42*t)+21/8*sin(19/8-40*t)+43/11*sin(8/9-
...
38*t)+26/11*sin(47/11-37*t)+30/7*sin(8/7-36*t)+19/8*sin(13/3-35*t)+...
11/5*sin(47/13-34*t)+53/15*sin(11/8-33*t)+7/4*sin(51/14-
31*t)+53/15*...
sin(5/12-30*t)+41/13*sin(26/7-29*t)+30/7*sin(19/9-28*t)+37/7*...
sin(1/10-27*t)+43/17*sin(1/10-26*t)+38/37*sin(19/5-25*t)+26/7*...
sin(13/11-24*t)+97/15*sin(27/10-23*t)+43/5*sin(2-22*t)+1/10*sin(7/2-
...
21*t)+17/10*sin(21/8-20*t)+32/9*sin(19/13-16*t)+21/4*sin(17/7-
15*t)+...
51/5*sin(12/5-12*t)+198/13*sin(7/5-11*t)+21/4*sin(9/5-10*t)+669/8*...
sin(39/10-8*t)+499/4*sin(11/3-7*t)+283/9*sin(18/5-6*t)+255/7*...
sin(3/10-4*t)+563/6*sin(26/11-3*t)+1225/9*sin(47/24-2*t)-12407/24*...
sin(t+11/9)-112/3*sin(5*t+7/12)-116/5*sin(9*t+4/3)-18/7*sin(13*t+...
25/19)-121/9*sin(14*t+6/5)-55/12*sin(17*t+1/43)-14/5*sin(18*t+5/8)-...
81/40*sin(19*t+1/23)-16/5*sin(32*t+7/5)-32/13*sin(39*t+11/8)-40/9*...
sin(41*t+6/5)-14/5*sin(43*t+14/11)-13/5*sin(51*t+5/8)-
20/11*sin(54*t+...
48/47)-35/36*sin(56*t+2/7)-3/2*sin(60*t+13/9)-18/11*sin(62*t+7/10)-...
1/4*sin(64*t+11/8)-19/11*sin(65*t+7/13)-3/8*sin(67*t+10/19)-2/7*...
sin(77*t+1/10)-1/8*sin(81*t+10/7)-4/13*sin(85*t+6/7)-5/14*sin(89*t+...
1/16)-3/5*sin(95*t+7/6)-1/2*sin(97*t+3/7))/1500;
plot(xd,yd);
hold off
axis([-1.1 1.1 -1.1 1.1])
El diagrama a bloques terminado se muestra a continuación.
Figura 15 Diagrama a bloques terminado.
3 RESULTADOS
Primero con ayuda de unos scopes se determinaron las condiciones iniciales del brazo robot para
que inicien sobre la trayectoria del alce, dichos valores son 𝑞1𝑖 = −0.142 𝑦 𝑞2𝑖 = 2.145. Se
utilizaron las ganancias 𝑘𝑝1 = 75, 𝑘𝑝2 = 60, 𝑘𝑑1 = 70, 𝑘𝑑2 = 58. Con estos valores se obtuvieron
las siguiente grafica a la salida.
Figura 16Seguimiento de trayectoria del perfil del alce.
Como se puede observar en la figura anterior el seguimiento de la trayectoria es perfecto ya que el
robot sigue adecuadamente la trayectoria del alce con errores mínimos, estos errores son en donde
hay un cambio brusco en la trayectoria, como lo es en las patas del alce. A continuación, se muestra
una imagen de la animación del brazo siguiendo el perfil del alce.
Figura 17 Seguimiento del perfil del alce.
Una vez verificado el funcionamiento adecuado del controlador se procedió a copiar los bloques de
control y trayectoria al archivo hecho con SimMechanics. El diagrama a bloques se muestra a
continuación.
Figura 18 Diagrama a bloques del brazo robot exportado con SimMechanics.
El seguimiento de la trayectoria es el siguiente.
Figura 19 Seguimiento de trayectorias.
Como se puede observar en la imagen anterior el seguimiento de la trayectoria fue muy bueno
como se esperaba.
4 CONCLUSIONES
Con la elaboración de este proyecto se puede concluir primero que para poder exportar los
ensambles de SolidWorks a simulink hay que tener cuidado con las rotaciones de las piezas ya que
si se rotan puede generar un desfase a la hora de exportarlo y que las gráficas a la salida en simulink
queden invertidas, por otro lado se puede concluir que el controlador por par calculado es el más
eficiente para el seguimiento de trayectorias ya que como se observo fue capaz de seguir
adecuadamente una trayectoria un poco compleja como lo es la del alce, no me fue posible probar
otros controladores ya que cuando implementaba otro controlador el programa me arrojaba un
error con el paso, trate de reducir el paso pero la simulación se hacía sumamente lenta y nunca
acababa de ejecutarse, ya que mi computadora se trababa, esto sucedía con el controlador PD+.
Otra cosa que se puede concluir es que es muy importante obtener los parámetros reales del robot
ya que si estos son precisos el rendimiento del controlador será muy bueno, y si llegaran a existir
variaciones pequeñas entre estos parámetros ya habría variaciones en el seguimiento de la
trayectoria.
5 BIBLIOGRAFÍA
[1] Reyes Fernando, Robótica Control de robots manipuladores, primera edición, México, Alfaomega
Grupo Editor, 2011, pp. 273.
[2] Wolfram Alpha Computational Intelligence, “moose curve (popular curve)”, WOLFRAM. [Online].
Disponible en: [Link] consultado el: 9 de julio de
2020