1. Universidad Politécnica del Centro
Ingeniería Mecatrónica
Proyecto Final Dinámica y Control de Robots
Ing. David Ramón Ricárdez
Dinámica y control de Robots
Integración de Sistemas Mecatrónicos
Presenta:
José Alberto Beristain Cornelio
9° M-1
Villahermosa Tabasco a 20 de Agosto de 2013
2. José Alberto Beristain Cornelio
2
Modelado Dinámico
I. Responda a los siguientes cuestionamientos:
1.- ¿Cuáles son las principales aplicaciones de los robots de 2GDL?
Pintado de objetos, ensamble, estibado, etc.
2.- ¿Cuál es la expresión que representa el modelo dinámico de un robot
de 2 GDL?
3.-En el caso de los robots de 3GDL, ¿Cuál es la principal característica y
aplicación en la industria?
La principal característica es que tienen la configuración antropomórfica
y su aplicación en la industria es la automatización de quirófanos
robotizados, asistencia personalizada a un sector vulnerable de la
población con capacidades diferentes, etc.
4.- ¿Cuál es la importancia del modelado dinámico de robots y su mayor
aplicación?
3. José Alberto Beristain Cornelio
3
Cuando se dispone de los valores numéricos de todos los parámetros del
robot y se obtiene un modelo dinámico para propósitos de simulación,
su aplicación es el control de posición, por el cual es posible estudiar a
detalle la respuesta que el robot presenta con una determinada
estructura de control.
5.- ¿Cuál es la expresión para el modelo dinámico de un robot
cartesiano?
II. Realizar mediante reporte de practica el modelo dinámico de un
robot manipulador de 2GDL, 3GDL y cartesiano, utilizando software de
simulación MatLab
2GDL
Primeramente tenemos la terminología y significado del robot a usar
para la práctica
4. José Alberto Beristain Cornelio
4
Hay que recordar que para nuestro propósito se crearan 2 archivos, el
primero será una función donde declararemos los valores de nuestras
variables, y otro que usara este, así como será quien represente la
dinámica del robot.
Se procedió a crear el primer archivo.
Se coloca el nombre que llevara la función
Se declara el vector de posición articular
Se declara también el vector de velocidad articular
Se les da valor a las variables a usar, así como algunos cálculos
matriciales necesarios
5. José Alberto Beristain Cornelio
5
Al final se define tau, qpp y xp con referencia los valores obtenidos, así
como cerrar la función
Se guarda el archivo con el nombre indicado para el correcto
funcionamiento de este.
Se crea el segundo archivo el cual se hará la dinámica del robot
Se comienza con los comandos clc, para que se tenga vacía la pantalla
del Matlab.
Se define el vector tiempo
6. José Alberto Beristain Cornelio
6
Se procede a configurar el robot, comenzando con la función odset en la
cual se configura las condiciones iniciales así como la integración
numérica de la dinámica del robot
Se crean los vectores de posición y velocidad articular
Se convierten los datos de radianes a grados y se grafican las posiciones
articulares en función del tiempo
Aquí el código completo de ambos programas
8. José Alberto Beristain Cornelio
8
Esta nos muestra la
respuesta de las
articulaciones (q1,q2) del
robot en configuración
antropomórfica cuando se
le aplica señales de
prueba del par aplicado
9. José Alberto Beristain Cornelio
9
3GDL
Aquí se muestra los parámetros del robot de 3gdl
Como en el anterior se crean 2 archivos para realizar el modelo.
Se procede a hacer el archivo de la función, comenzando con el nombre
Se crean los vector de posición y velocidad articular
Se declaran valores de variables
10. José Alberto Beristain Cornelio
10
Se crean las matrices y se calculan valores de gp, fr, qd, qt, asi como
de Kp, Kv y tau
Se determina la aceleración articular ademas como el vector de salida
Se guarda el archivo con el nombre correcto
Se crea el siguiente archivo
Se borra todo con el comando clc y clean all, asi como cerrar otras
pestañas con close all.
Se determinan los parámetros de simulación
Se configura la función ode45 asi como las condiciones iniciales
11. José Alberto Beristain Cornelio
11
Se manda a graficar las 3 articulaciones, incluyendo una conversión de
radianes a grados
Aquí tenemos los códigos de manera completa
Función
Simulación
12. José Alberto Beristain Cornelio
12
Aquí se muestra la respuesta del sistema
Se presenta la respuesta del robot a una entrada T de la ley de control
PD. Las posiciones de las articulaciones de la base, hombro y codo
convergen a [45,45,90] grados, respectivamente.
Cartesiano
Parámetros del robot cartesiano
13. José Alberto Beristain Cornelio
13
Como en las anteriores simulaciones, se procede a crear los 2 archivos
Se designa el nombre de la función
Se crean los vectores de posición y velocidad
Se configuran los parámetros del robot cartesiano, incluyendo masas de
los servomotores, coeficientes de fricción viscosa, fricción de Coulumb y
fricción estática.
Se crean las matrices pertinentes la mas importante la de masas
Se coloca el vector de par gravitacional
Al final se ponen las matrices de la fricción estática, y se define tau
14. José Alberto Beristain Cornelio
14
Se crea el otro archivo de simulación
Se borra la pantalla y se colocan los parámetros de simulación, tiempo
inicial, incremento de simulación, y tiempo de simulación.
Se configura la función ode45
Se colocan los valores iniciales del robot
Se grafica
16. José Alberto Beristain Cornelio
16
Esto nos resulta del programa.
La oscilación que
presenta cada
articulación es
intencional con la
finalidad de mover
a cada servo en
ambas direcciones
y al mismo tiempo
recorran la mayor
parte de su espacio
de trabajo.
17. José Alberto Beristain Cornelio
17
Control Proporcional Derivativo PD de un robot de 2GDL y 3GDL
1.- ¿Cuál es la expresión que representa el control PD de un robot de
2GDL?
2.- ¿Por qué se utiliza el control PD en los robots de 2GDL y 3GDL?
Porque tiene un punto de equilibrio global y asintóticamente estable.
CÓDIGOS
Cap8_robot2gdl.m
Vector de posición articular
q1=x(1);q2=x(2);q=[q1;q2];
Vector de velocidad articular
qp1=x(3);qp2=x(4);qp=[qp1;qp2];
m1=23.902;l1=0.45;lc1=0.091;I1=1.266;b1=2.288;
m2=3.880;l2=0.45;lc2=0.048;I2=0.093;b2=0.175;g=9.81;
theta1=m1*lc1*lc1+m2*l1*l1+m2*lc2*lc2+I1+I2;
theta2=l1*m2*lc2;
theta3=m2*lc2*lc2+I2;
theta4=g*(lc1*m1+m2*l1);
theta5=g*m2*lc2;theta6=b1;theta7=b2;
M=[theta1+2*theta2*cos(q2),theta3+theta2*cos(q2);
theta3+theta2*cos(q2),theta3];
C=[-2*theta2*sin(q2)*qp2,-theta2*sin(q2)*qp2;
theta2*sin(q2)*qp1,0];
gq11=theta4*sin(q1)+theta5*sin(q1+q2);
gq21=theta5*sin(q1+q2);
gq=[gq11;gq21];
fr=[theta6*qp1;
theta7*qp2];
[~,tau]=cap8_PDrobot2gdl(q,qp);
qpp=M^(-1)*(tau-C*qp-gq-fr);
Vector de salida
18. José Alberto Beristain Cornelio
18
xp=[qp1;qp2;qpp(1);qpp(2)];
Cap8_PDrobot2gdl.m
Vector de posiciones
q1=x(1);q2=x(2);q=[q1;q2];
Vector de Velocidades
qp1=xp(1);qp2=xp(2);qp=[qp1;qp2];
Parámetros del robot / Par gravitacional
m1=23.902;lc1=0.091;l1=0.45;
m2=3.880;lc2=0.048;g=9.81;
theta4=g*(lc1*m1+m2*l1);
theta5=g*m2*lc2;
Par gravitacional
par_grav=[theta4*sin(q1)+theta5*sin(q1+q2);
g*m2*lc2];
Ganancia Proporcional
kp1=3;kp2=0.15;
Kp=[kp1,0;
0,kp2];
Ganancia derivativa
kv1=0.20*kp1;kv2=0.20*kp2;
Kv=[kv1,0;
0,kv2];
qd1=45;qd2=90;
qd=[qd1;
qd2];
Vector de referencias
qtilde=pi/180*qd-q;
19. José Alberto Beristain Cornelio
19
Error de posición en grados
qtgrados=(180/pi)*qtilde;
Velocidad en grados/segundo
qpgrados=180*qp/pi;
tau=Kp*qtgrados-Kv*qpgrados+par_grav;
Cap8_robot2gdlsimu.m
Vector tiempo
ti=0;h=0.001;tf=5;t=ti:h:tf;
Condiciones iniciales
ci=[0;0;0;0];
Solución numérica del sistema dinámico líneal
[t,x]=ode45('cap8_robot2gdl',t,ci,opciones);
q1=x(:,1);q2=x(:,2);
qp1=x(:,3);qp2=x(:,4);
[n,m]=size(t);
tau1=zeros(n,1);tau2=zeros(n,1);
qtilde1=zeros(n,1);qtilde2=zeros(n,1);
xef=zeros(n,1);yef=zeros(n,1);
beta1=0.15;beta2=0.15;l1=0.45;l2=0.45;
for k=1:n
[qt tau]=cap8_PDrobot2gdl([q1(k);q2(k)],[qp1(k);qp2(k)]);
tau1(k)=tau(1);tau2(k)=tau(2);
qtilde1(k)=qt(1);qtilde2(k)=qt(2);
[xef(k), yef(k)]=cinematica_r2gdl( beta1,l1,q1(k),beta2,l2,q2(k));
% p=Rz(-pi/2)*[xef(k),yef(k),beta1+beta2];
% xef(k)=p(1);
%yef(k)=p(2);
end
20. José Alberto Beristain Cornelio
20
Gráficos
subplot(2,2,1);plot(t,qtilde1,t,qtilde2)
subplot(2,2,2);plot(t,tau1,t,tau2)
subplot(2,2,3);plot(xef,yef)
subplot(2,2,4);plot(qtilde1,qp1,qtilde2,qp2)
Este tipo de respuesta es consecuencia de una adecuada sintonía de las
ganancias, así como del factor de amortiguamiento, que en este caso es
del 20%.
La sintonía de las ganancias es empírica, es decir, a prueba y error; este
proceso depende en gran medida de la experiencia que tenga el usuario.
Errores de posición Pares aplicados
Cinemática Directa Diagrama Fase
21. José Alberto Beristain Cornelio
21
Cap8_robot2gdl.m
Vector de posición articular
q1=x(1);q2=x(2);q3=x(3);q=[q1;q2;q3];
Vector de velocidad articular
qp1=x(4);qp2=x(5);qp3=x(6);qp=[qp1;qp2;qp3];
Matriz de inercia
M=[m11,m12,m13;m21,m22,m23;m31,m32,m33];
gamma112=(Iy2-Ix2-m2*lc2*lc2)*cos(q2)*sin(q2)+(Iy3-
Iz3)*cos(q2+q3)*sin(q2+q3)-
m3*(l2*cos(q2)+lc3*cos(q2+q3))*(l2*sin(q2)+lc3*sin(q2+q3));
gamma113=(Iy3-Iz3)*cos(q2+q3)*sin(q2+q3)-
m3*lc3*sin(q2+q3)*(l2*cos(q2)+lc3*cos(q2+q3));
gamma121=(Iy2-Iz2-m2*lc2*lc2)*cos(q2)*sin(q2)+(Iy3-
Iz3)*cos(q2+q3)*sin(q2+q3)-
m3*(l2*cos(q2)*lc3*cos(q2+q3))*(l2*sin(q2)+lc3*sin(q2+q3));
gamma131=(Iy3-Iz3)*cos(q2+q3)*sin(q2+q3)-
m3*lc3*sin(q2+q3)*(l2*cos(q2)+lc3*cos(q2+q3));gamma211=(Ix2-
Iy2+m2*lc2*lc2)*cos(q2)*sin(q2)+(Iz3-
Iy3)*cos(q2+q3)*sin(q2+q3)+m3*(l2*cos(q2)+lc3*cos(q2+q3))*(l2*si
n(q2)+lc3*sin(q2+q3));gamma223=-l2*m3*lc3*sin(q3);
gamma232=-l2*m3*lc3*sin(q3);gamma233=-l2*l3*lc3*sin(q3);
gamma311=(Iz3-
Iy3)*cos(q2+q3)*sin(q2+q3)+m3*lc3*sin(q2+q3)*(l2*cos(q2)+lc3*cos
(q2+q3));gamma322=l2*m3*lc3*sin(q3);
c11=gamma112*qp2+gamma113*qp3;c12=gamma121*qp1;c13=gam
ma131*qp1;
c21=gamma211*qp1;c22=gamma223*qp3;c23=gamma232*qp2+gam
ma233*qp3;
Vector de salida
xp=[qp1;qp2;qp3;qpp(1);qpp(2);qpp(3)];
Cap8_PDrobot2gdl.m
22. José Alberto Beristain Cornelio
22
Posiciones
q1=x(1);q2=x(2);q3=x(3);
q=[q1;q2;q3];
Velocidades
qp1=xp(1);qp2=xp(2);qp3=xp(3);
qp=[qp1;qp2;qp3];
Parámetros del robot
m1=26.902;l1=0.45;m2=30;l2=0.45;lc2=0.038;
m3=3.380;l3=0.45;lc3=0.048;g=9.81;
gq11=0;
gq21=(lc2*m1++m2*l2)*sin(q1)+m2*lc3*sin(q1+q2);
gq31=m2*lc3*sin(q1+q2);
Par gravitacional
gq=g*[gq11;gq21;gq31];
Referencias o posiciones deseadas
qd=[30;45;90];
Error de posición
qtilde=qd*pi/180-q;
Error de posición en grados
qtildeg=(180/pi)*qtilde;
Velocidad en grados/segundos
qpgrados=180*qp/pi;
Ganancia proporcional
Kp=[1,0,0;
0,2,0;
0,0,0.15];
23. José Alberto Beristain Cornelio
23
Ganancia derivativa
Kv=[0.3,0,0;
0,0.6,0;
0,0,0.015];
Control proporcional derivativo PD
tau=Kp*qtildeg-Kv*qpgrados+gq;
Cap8_PDrobot2gdlsimu.m
Parámetros de simulación
ti=0;h=0.0025;
Vector tiempo
tf=5;ts=ti:h:tf;
Posiciones articulares
q1=x(:,1);q2=x(:,2);q3=x(:,3);
Velocidades articulares
qp1=x(:,4);qp2=x(:,5);qp3=x(:,6);
Solución numérica del control proporcional derivativo
[n,m]=size(t);
tau1=zeros(n,1);tau2=zeros(n,1);tau3=zeros(n,1);
qtilde1=zeros(n,1);qtilde2=zeros(n,1);qtilde3=zeros(n,1);
xef=zeros(n,1);yef=zeros(n,1);zef=zeros(n,1);
beta1=0.15;beta2=0.15;beta3=0.15;l1=1;l2=0.45;l3=0.45;
for k=1:n
[qt tau]=cap8_PDrobot3gdl([q1(k);q2(k);q3(k)],[qp1(k);qp2(k);
qp3(k)]);
tau1(k)=tau(1);tau2(k)=tau(2);tau3(k)=tau(3);
qtilde1(k)=qt(1);qtilde2(k)=qt(2);qtilde3(k)=qt(3);
[xef(k),yef(k),zef(k)]=cinematica_r3gdl(beta1,l1,q1(k),beta2,l2,q2(k),b
eta3,l3,q3(k));
End
24. José Alberto Beristain Cornelio
24
Gráficos
subplot(2,2,1);plot(t,qtilde1,t,qtilde2,t,qtilde3)
subplot(2,2,2);plot(t,tau1,t,tau2,t,tau3)
subplot(2,2,3);plot3(xef,yef,zef)
subplot(2,2,4);plot(qtilde1,qp1,qtilde2,qp2,qtilde3,qp3)
Aquí se muestra el comportamiento de los errores de posición para las
tres articulaciones; presentan un perfil suave, sin ruido mecánico, ni
sobre impulsos, tampoco se observan oscilaciones en régimen
estacionario.
Los pares aplicados evolucionan dentro de los límites de saturación y la
trayectoria que describe el extremo final del robot en su espacio de
trabajo es suave, tal y como se muestra en el diagrama fase.
Errores de posición Pares aplicados
Cinemática Directa Diagrama Fase