Tarea 2

Maestra en Ciencias en Ingeniera Elctrica

CONTROL AVANZADO

PRESENTA:

Ing. ISMAEL MEDINA L PEZ Matricula: M1513050

Catedrtico: Dr. Jos Luis Meza Medina

TORREN, COAH. MXICO 20 DE OCTUBRE DE 2015

TAREA 2

Modelacin y Simulacin de un

Robot de 2gdl

TECNOLGICO NACIONAL DE MXICO Instituto Tecnolgico de la Laguna

Control Avanzado

Ismael Medina Lpez 1

Modelacin y Simulacin de un Robot de 2gdl

Objetivo

Analizar el comportamiento por simulacin de un robot planar de dos grados de libertad (2gdl)

bajo la accin de diferentes controladores.

Objetivos especficos

1) Obtener una representacin en variables de estado de un Robot de dos grados de Libertad.

2) Simular el modelo del robot utilizando Simulink-Matlab y el paquete Simnon.

3) Analizar la respuesta del robot de dos grados de libertad en lazo abierto.

4) Analizar la respuesta del Robot de dos grados de libertad en lazo cerrado con controladores proporcional (P), proporcional- derivativo (PD) y con un Control

Proporcional, Integral, Derivativo (PID) para entradas escaln.

Desarrollo

Descripcin del robot experimental de dos grados de libertad:

Figura 1. Robot planar de dos grados de libertad ubicado en las instalaciones del Instituto

Tecnolgico de la Laguna (Laboratorio de Mecatrnica y Control).

Control Avanzado


Descripcin Notacin Valor Unidades

Longitud del eslabn 1 l1 0.450 m

Longitud del eslabn 2 l2 0.450 m

Distancia al centro de masa (eslabn 1) lc1 0.091 m

Distancia al centro de masa (eslabn 2) lc2 0.048 m

Masa eslabn 1 m1 23.902 kg

Masa eslabn 2 m2 3.880 kg

Inercia eslabn 1 respecto al centro de masa I1 1.266 kg m2

Inercia eslabn 2 respecto al centro de masa I2 0.093 kg m2

Aceleracin de la gravedad g 9.81 m/s2

Modelo dinmico en forma compacta del robot experimental de dos grados de

libertad

Dnde:

Control Avanzado


1) Representacin en variables de estado del robot de dos grados de

libertad

Para poder hacer las simulaciones es necesario expresar la dinmica del robot en forma de

variables de estado.

Con ayuda del paquete o software Matlab despejamos el vector , de la siguiente forma:

syms m11 m12 m21 m22; syms c11 c12 c21 c22 ; syms q1p q2p g1 g2 tau1 tau2 ; M=[m11 m12;m21 m22]; C=[c11 c12;c21 c22]; qp=[q1p;q2p]; tau=[tau1;tau2]; g=[g1;g2]; qpp=inv(M)*(tau - C*qp - g);

pretty(qpp(1));

m12 (g2 - tau2 + c21 q1p + c22 q2p) m22 (g1 - tau1 + c11 q1p + c12 q2p) ----------------------------------- - ---------- ------------------------- m11 m22 - m12 m21 m11 m22 - m12 m21

pretty(qpp(2));

m21 (g1 - tau1 + c11 q1p + c12 q2p) m11 (g2 - tau2 + c21 q1p + c22 q2p) ----------------------------------- - -------------------------- --------- m11 m22 - m12 m21 m11 m22 - m12 m21

Dado que es una matriz simtrica, es decir, , tenemos:

syms m11 m12 m22; syms c11 c12 c21 c22 ; syms q1p q2p g1 g2 tau1 tau2 ; M=[m11 m12;m12 m22]; C=[c11 c12;c21 c22]; tau=[tau1;tau2]; g=[g1;g2]; qp=[q1p;q2p]; qpp=inv(M)*(tau - C*qp - g); pretty(qpp(1));

Control Avanzado


pretty(qpp(1));

m12 (g2 - tau2 + c21 q1p + c22 q2p) m22 (g1 - tau1 + c11 q1p + c12 q2p) ----------------------------------- - ------------------ ----------------- 2 2 - m12 + m11 m22 - m12 + m11 m22

pretty(qpp(2));

m12 (g1 - tau1 + c11 q1p + c12 q2p) m11 (g2 - tau2 + c21 q1p + c22 q2p) --------------- -------------------- - ----------------------------------- 2 2 - m12 + m11 m22 - m12 + m11 m22

La dinmica del robot en variables de estado, quedara de la forma:

Modelo dinmico del robot con friccin

Es importante notar que la ecuacin genrica del modelo dinmico en su forma compacta supone

que los eslabones son rgidos, es decir, que no presentan ninguna torsin o cualquier otro

fenmeno de deformacin. Por otro lado, tambin consideramos que las articulaciones entre cada

par de eslabones son rgidas y sin friccin.

Efectos de friccin en los sistemas mecnicos son fenmenos que dependen de mltiples factores

tales como la naturaleza de los materiales en contacto, lubricacin, temperatura, etc. Por esta

razn, tpicamente slo estn disponibles modelos aproximados de las fuerzas de friccin y pares

de torsin. Sin embargo, se acepta que estas fuerzas y pares de torsin dependen de la velocidad

relativa entre los cuerpos en contacto. Por lo tanto, podemos distinguir dos familias de modelos

de friccin: los modelos estticos, en los que la fuerza de friccin o par de torsin depende de la

velocidad relativa momentnea entre los cuerpos y, modelos dinmicos, que dependen de los

valores pasados de la velocidad relativa.

Control Avanzado


As, en los modelos estticos, la friccin se modela mediante un vector que solo

depende de la velocidad de la articulacin . Los efectos de friccin son locales, es decir,

puede ser escrito como:

para el caso de un robot de dos grados de libertad.

es uno que combina los llamados fenmenos de friccin

viscosa y de Coulomb. Este modelo establece que el vector viene dado por:

Donde y son matrices diagonal definidas positivas. Los elementos de la diagonal de

corresponden a los parmetros de friccin viscosa mientas que los elementos de

corresponden a los parmetros de la friccin de Coulomb. Adems, en el modelo dado por la

expresin anterior

Teniendo en cuenta la friccin en las articulaciones, la ecuacin dinmica general del

manipulador est ahora dado por

Considerando en nuestro modelo solamente la friccin viscosa

Donde es una matriz diagonal definida positiva de , cuyos elementos de la diagonal de

corresponden a los parmetros de la friccin viscosa ( , como ya se haba mencionado. Por

lo tanto tenemos:

Control Avanzado


Dinmica del robot en forma de variables de estado.

syms m11 m12 m21 m22; syms c11 c12 c21 c22 ; syms q1p q2p g1 g2 tau1 tau2 fv ; M=[m11 m12;m21 m22]; C=[c11 c12;c21 c22]; tau=[tau1;tau2]; g=[g1;g2]; qp=[q1p; q2p]; Fm=[fv 0;0 fv]; qpp=inv(M)*(tau - C*qp - g- Fm*qp);

pretty(qpp(1));

m12 (g2 - tau2 + c21 q1p + c22 q2p + fv q2p) m22 (g1 - tau1 + c11 q1p + c12 q2p + fv q1p)

-------------------------------------------- - ---------------------------------------- ----

m11 m22 - m12 m21 m11 m22 - m12 m21

pretty(qpp(2));

m21 (g1 - tau1 + c11 q1p + c12 q2p + fv q1p) m11 (g2 - tau2 + c21 q1p + c22 q2p + fv q2p)

-------------------------------------------- - ------ --------------------------------------

m11 m22 - m12 m21 m11 m22 - m12 m21

Considerando ahora en nuestro modelo tanto la friccin viscosa como de Coulomb

Control Avanzado


syms m11 m12 m21 m22; syms c11 c12 c21 c22 ; syms q1p q2p g1 g2 tau1 tau2 fv fc ; M=[m11 m12;m21 m22]; C=[c11 c12;c21 c22]; tau=[tau1;tau2]; g=[g1;g2]; qp=[q1p;q2p]; Fm1=[fv 0;0 fv]; Fm2=[fc 0; 0 fc]; qpp=inv(M)*(tau - C*qp - g- Fm1*qp- Fm2*sign(qp)); pretty(qpp(1));

m12 (g2 - tau2 + c21 q1p + c22 q2p + fv q2p + fc sign(q2p))

----------------------------------------------------------- - m11 m22 - m12 m21

m22 (g1 - tau1 + c11 q1p + c12 q2p + fv q1p + fc sign(q1p)) ---------------------------------------------------------- -

m11 m22 - m12 m21 pretty(qpp(2));

m21 (g1 - tau1 + c11 q1p + c12 q2p + fv q1p + fc sign(q1p))

----------------------------------------------------------- - m11 m22 - m12 m21

m11 (g2 - tau2 + c21 q1p + c22 q2p + fv q2p + fc sign(q2p)) ----------------- ------------------------------------------

m11 m22 - m12 m21

Control Avanzado


2) Anlisis de la respuesta del robot de dos grados de libertad en lazo

abierto

Simulacin en el paquete SIMNON

Programa desarrollado

CONTINUOUS SYSTEM r2gdl " Version: 1.0 " Abs tract: " Description: " Revision: 1.0 " Author: ISMAEL MEDINA LOPEZ " Created: 21 /10/2015 " Inputs and outputs: " INPUT " OUTPUT " States, derivates and time: STATE q1 q2 qp1 qp2 DER dq1 dq2 ddq1 ddq2 " TIME t " Initializat ions: " Equations: dq1=qp1 dq2=qp2 ddq11=(m22/(m11*m22 - m12*m21))*(tau1 - g1- c11*qp1 - c12*qp2) ddq1=ddq11+(m12/(m11*m22 - m12*m21))*(g2 - tau2+c21*qp1+c22*qp2) ddq22=(m11/(m11*m22 - m12*m21))*(tau2 - g2- c21*qp1 - c22*qp2) ddq2=ddq22+(m21/(m11*m22 - m12*m21))*(g1 - tau1+c11*qp1+c12*qp2) m11=m1*lc1*lc1+m2*(l1*l1+lc2*lc2+2*l1*lc2*cos(q2))+I1+I2 m12=m2*(lc2*lc2+l1*lc2*cos(q2))+I2 m21=m2*(lc2*lc2+l1*lc2*cos(q2))+I2 m22=m2*lc2*lc2+I2 c11= - m2*l1*lc2*sin(q2)*qp2 c12= - m2*l1*lc2*sin(q2)*(qp1+qp2) c21=m2*l 1*lc2*sin(q2)*qp1 c22=0 g1=(m1*lc1+m2*l1)*g*sin(q1)+m2*lc2*g*sin(q1+q2) g2=m2*lc2*g*sin(q1+q2) tau1= 0.785398 tau2= 0. 174533 q1n= q1*(180/3.1416) q2n= q2*(180/3.1416)

Control Avanzado


" Parameter values: l1=0.450 l2=0.450 lc1=0.091 lc2=0.048 m1=23.902 m2=3.880 I 1=1.266 I2=0.093 g=9.81 END

Figura 2. Grafica obtenida como resultado de la simulacin en SIMNON con pares

y . Para las variables articulares q1 se muestra en color negro (inferior) y q2 en

color azul (superior).

Control Avanzado


Figura 3. Variacin en los pares aplicados, y .

Simulacin en el MATLAB -Simulink

Figura 4. Diagrama de bloques en Matlab-Simulink para la simulacin del robot de dos grados

de libertad en lazo abierto.

Control Avanzado


Funcin r2gdl implementada en el bloque Interpreted MATLAB Fcn

function [out] = r2gdl(x ) % Funcin R2GDL de Matlab para simular un robot de dos grados de libertad % 20 de Octubre de 2015

q=[x(1);x(6)]; tau=[x(3);x(4)]; qp=[x(2);x(5)];

% Valores de los paramatros del robot l1 = 0.450; % Longitud del eslabon 1 (mts) l2 = 0.450; % Longitud del eslabon 2 (mts) lc1 = 0.091; % Distancia al centro de masa (eslabon 1) lc2 = 0.048; % Distancia al centro de masa (eslabon 2) m1 = 23.902; % Masa eslabon 1 (kg) m2 = 3.880; % Masa eslabon 2 (kg) I1 = 1.266; % Inercia es labon 1 respecto al centro de masa (kg m^2) I2 = 0.093; % Inercia eslabon 2 respecto al centro de masa (kg m^2) g = 9.81; % Aceleracin de la Gravedad (m/s^2)

% Elementos de la matriz de Inercia m11 = m1*lc1*lc1+m2*(l1*l1+lc2*lc2+2*l1*lc2*co s(q(2)))+I1+I2; m12 = m2*(lc2*lc2+l1*lc2*cos(q(2)))+I2; m21 = m2*(lc2*lc2+l1*lc2*cos(q(2)))+I2; m22 = m2*lc2*lc2+I2; M = [m11 m12;m21 m22];

% Elementos de la Matriz de Coriolis c11 = - m2*l1*lc2*sin(q(2))*qp(2); c12 = - m2*l1*lc2*sin(q(2))*(qp(1)+qp(2)); c21 = m2*l1*lc2*sin(q(2))*qp(1); c22 = 0; C = [c11 c12;c21 c22];

% Vector de pares gravitacionales g1 = (m1*lc1+m2*l1)*g*sin(q(1))+m2*lc2*g*sin(q(1)+q(2)); g2 = m2*lc2*g*sin(q(1)+q(2)); g = [g1;g2];

% Vector de aceleracin qpp = [qpp1;qpp2] qpp = inv(M)*(tau - C*qp - g); %out = zeros(1,2); out(1) = qpp(1); out(2) = qpp(2);

Control Avanzado


Figura 5. Graficas obtenidas de la simulacin en Matlab-Simulink.

Figura 6. Graficas obtenidas de la simulacin en Matlab-Simulink, con valores de pares iguales

a y .

0 1 2 3 4 5 6 7 8 9 10-2

0

2

4

6

8

10

Tiempo [s]

0 1 2 3 4 5 6 7 8 9 10-5

0

5

10

15

20

25

30

35

Control Avanzado


3) Anlisis de la respuesta del robot de dos grados de libertad en lazo

cerrado

Simulacin en Matlab-Simulink

Figura 7. Diagrama de bloques en Matlab-Simulink para la simulacin del robot de dos grados

de libertad en lazo cerrado.

Control Proporcional (P)

Figura 8. Esquema de control en lazo cerrado del robot de dos grados de libertad con

controlador Proporcional (P).

Control Avanzado


Figura 9. Graficas obtenida de la simulacin en Matlab-Simulink del robot 2gdl con controlador

Proporcional (P).

Control Proporcional-Derivativo (PD)

Figura 10. Esquema de control en lazo cerrado del robot de dos grados de libertad con

controlador Proporcional-Derivativo (P).

0 1 2 3 4 5 6 7 8 9 10-10

0

10

20

30

40

50

60

70

Control Avanzado



Proporcional-Derivativo (P).

Control Proporcional-Integral-Derivativo (PD)

Figura 12. Diagrama de bloques en Matlab-Simulink para la simulacin del robot de dos

grados de libertad en lazo cerrado con controlador PID, caso especial con 3 bloques step en

cascada.

0 1 2 3 4 5 6 7 8 9 100

5

10

15

20

25

30

35

40

45

50

Control Avanzado



Proporcional-Integral-Derivativo (PID).

0 1 2 3 4 5 6 7 8 9 100

5

10

15

20

25

30

35

40

45

50

Control Avanzado


Simulacin en SIMNON

Las simulaciones se llevaron de acuerdo a las siguientes variantes para 5 diferentes controladores:

Controlador

Controlador Proporcional

(P)

Constante

Friccin viscosa

Friccin Viscosa + friccin de

Coulomb

Variable

Friccin viscosa


Coulomb

Controlador

Proporcional-Derivativo

(PD)

Constante

Friccin viscosa


Coulomb

Variable

Friccin viscosa


Coulomb

Controlador PD con

compensacin de

gravedad (PD+g)

Constante

Friccin viscosa


Coulomb

Variable

Friccin viscosa


Coulomb

Controlador Tanh-D con

compensacin de

gravedad (Tanh-D+g)

Constante

Friccin viscosa


Coulomb

Variable

Friccin viscosa


Coulomb

Controlador

Proporcional-Integral-

Derivativo (PID)

Constante

Friccin viscosa


Coulomb

Variable

Friccin viscosa


Coulomb

Se consideran valores constantes de los parmetros mostrados en la siguiente tabla:

Parmetro Valor Parmetro Valor Parmetro Valor

constante 10 30 10

constante 10 30 10

variable 20 0.5

variable 20 0.5

Control Avanzado


Controlador Proporcional (P)

De acuerdo a la tabla anterior se presentaran cuatro graficas diferentes que corresponden cada

variante de los cinco controladores utilizados en el robot de dos grados de libertad.

CONTINUOUS SYSTEM 2GDL " Version: 1.0 " Abstract: " Description: " Revision: 1.0 " Author: ISMAEL MEDINA LOPEZ " Created: 14/10/2015 " Inputs and outputs: " INPUT " OUTPUT " States, derivates and time: STATE q 1 q2 qp1 qp2 DER dq1 dq2 ddq1 ddq2 " TIME " Initializations: " Equations: dq1=qp1 dq2=qp2 ddq11=(m22/(m11*m22 - m12*m21))*(tau1 - g1- c11*qp1 - c12*qp2 - fv*qp1) " - fc*sign(qp1) ddq1=ddq11+(m12/(m11*m22 - m12*m21))*(g2 - tau2+c21*qp1+c22*qp2+fv*qp2) " + fc*sign(qp2) ddq22=(m11/(m11*m22 - m12*m21))*(tau2 - g2- c21*qp1 - c22*qp2 - fv*qp2) " - fc*sign(qp2) ddq2=ddq22+(m21/(m11*m22 - m12*m21))*(g1 - tau1+c11*qp1+c12*qp2+fv*qp1) " +fc*sign(qp1) m11=m1*lc1*lc1+m2*(l1*l1+lc2*lc2+2*l1*lc2*cos(q2))+I1+I2 m12=m2*(lc2*lc 2+l1*lc2*cos(q2))+I2 m21=m2*(lc2*lc2+l1*lc2*cos(q2))+I2 m22=m2*lc2*lc2+I2 c11= - m2*l1*lc2*sin(q2)*qp2 c12= - m2*l1*lc2*sin(q2)*(qp1+qp2) c21=m2*l1*lc2*sin(q2)*qp1 c22=0 g1=(m1*lc1+m2*l1)*g*sin(q1)+m2*lc2*g*sin(q1+q2) g2=m2*lc2*g*sin(q1+q2) tau1=kp1*(qd1 - q1) tau2=kp2*(qd2 - q2) kp1=30 kp2=30 qd1= 10 qd2= 10 fv=0.5 " Parameter values:

Control Avanzado


l1=0.450 l2=0.450 lc1=0.091 lc2=0.048 m1=23.902 m2=3.880 I1=1.266 I2=0.093 g=9.81 END

a) b)

Control Avanzado


Figura 14. Respuestas de las variables articulares del robot de 2gdl con controlador

Proporcional (P), para: a) Entrada qd constante con friccin viscosa, b) Entrada qd constante

con friccin viscosa ms friccin de Coulomb, c) Entrada qd variable con friccin viscosa, d)

Entrada qd variable con friccin viscosa ms friccin de Coulomb.

Controlador Proporcional-Derivativo (PD)

tau1=kp1*(qd1 - q1) - kd1*qp1 " +kd1*(((5*3.1416)/2)*cos( (3.1416/2)*t) - qp1) tau2=kp2*(qd2 - q2) - kd2*qp2 " +kd2 *(((5*3.1416)/2)*cos((3.1416/2)*t) - qp2 ) fv=0.5 fc=0.5 kp1=30 kp2 =30 kd1=20 kd 2=20 qd1=10 " 5*sin((3.1416/2)*t) qd2=10 " 5*sin((3.1416/2)*t)

c) d)

Control Avanzado


Figura 15. Respuestas de las variables articulares del robot de 2gdl con controlador Proporcional-

Derivativo (PD), para: a) Entrada qd constante con friccin viscosa, b) Entrada qd constante con friccin

viscosa ms friccin de Coulomb, c) Entrada qd variable con friccin viscosa, d) Entrada qd variable con

friccin viscosa ms friccin de Coulomb.

a) b)

c) d)

Control Avanzado


Controlador Proporcional-Derivativo (PD) con compensacin de gravedad

g1=(m1*lc1+m2*l1)*g*sin(q1)+m2*lc2*g*sin(q1+q2) g2=m2*lc2*g*sin(q1+q2) tau1=kp1*(qd1 - q1) - kd1*qp1+g1 tau2=kp2*(qd2 - q2) - kd2*qp2+ g2 fv=0.5 fc=0.5 kp1=30 kp2=30 kd1=20 kd2=20 qd1=10 " 5*sin((3.1416/2)*t) qd2=10 " 5*sin((3.1416/2)*t )

a) b)

Control Avanzado


Figura 16. Respuestas de las variables articulares del robot de 2gdl con controlador Proporcional-

Derivativo (PD) con compensacin de gravedad, para: a) Entrada qd constante con friccin viscosa, b)

Entrada qd constante con friccin viscosa ms friccin de Coulomb, c) Entrada qd variable con friccin

viscosa, d) Entrada qd variable con friccin viscosa ms friccin de Coulomb

Control Tanh-D con compensacin de gravedad

g1=(m1*lc1+m2*l1)*g*sin(q1)+m2*lc2*g*sin(q1+q2) g2=m2*lc 2*g*sin(q1+q2) tau1=kp1*tanh(qt1) - kd1*qp1+g1 tau2=kp2*tanh(qt2) - kd2*qp2+g2 qt1=(qd1 - q1) qt2=(qd2 - q2) fv=0.5 fc=0.5 kp1=30 kp2=30 kd1=20 kd2=20 qd1=10 qd2=10

c) d)

Control Avanzado


Figura 17. Respuestas de las variables articulares del robot de 2gdl con controlador Tanh-D con

compensacin de gravedad, para: a) Entrada qd constante con friccin viscosa, b) Entrada qd constante con

friccin viscosa ms friccin de Coulomb, c) Entrada qd variable con friccin viscosa, d) Entrada qd

variable con friccin viscosa ms friccin de Coulomb

a) b)

c) d)

Control Avanzado


Control Proporcional-Integral-Derivativo (PID)

STATE q1 q2 qp1 qp2 x1 x2 DER dq1 dq2 ddq1 ddq2 dx1 dx2 . . . dx1=qt1 dx2=qt2 tau1=kp1*qt1 - kd1*qp1+ki1*x1 tau2=kp2*qt2 - kd2*qp2+ki2*x2 qt1=(qd1 - q1) qt2=(qd2 - q2) fv=0.3 kp1= 30 kp2=30 kd1=20 kd2=20 ki1=10 ki2=10 qd1=10 qd2=10

a) b)

Control Avanzado


Figura 18. Respuestas de las variables articulares del robot de 2gdl con controlador Proporcional-Integral-

Derivativo, para: a) Entrada qd constante con friccin viscosa, b) Entrada qd constante con friccin viscosa

ms friccin de Coulomb, c) Entrada qd variable con friccin viscosa, d) Entrada qd variable con friccin

viscosa ms friccin de Coulomb

Conclusin

En este documento se present el modelo dinmico de un sistema mecatrnico (robot) de dos

grados de libertad, el cual, puede ser motivo de un amplio estudio para el diseo de diferentes

controladores. En esta ocasin el sistema fue simulado con los controladores clsicos

Proporcional (P), Proporcional-Derivativo y Proporcional-Integral-Derivativo (PID), tanto en

Matlab-Simulink como en el paquete SIMORO. Se pudo constatar una gran similitud en la

obtencin de los resultados para cada uno de los controladores y por supuesto como la respuesta,

que es el comportamiento de las variables articulares, cambia significativamente con los

controladores, obteniendo mejores resultados cuando el controlador es ms robusto, el sistema

por lo tanto tendr un comportamiento ms estable.

Referencias Bibliogrficas

R. Kelly, V. Santibez & A. Lora. Control of Robot Manipulators in Joint Space. 2da Ed. 2005. (pp. 92-95).

c) d)

Tarea 2

Documents

Transcript of Tarea 2