Proyecto final MODELADO DE ROBOTS UPC TABASCO

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

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

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

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

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

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


7

Esta es la respuesta que manda el sistema


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

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

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

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

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

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

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


15

Códigos


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

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

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

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

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

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*sin(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=gamma131*qp1; c21=gamma211*qp1;c22=gamma223*qp3;c23=gamma232*qp2+gamma233*qp3; Vector de salida

xp=[qp1;qp2;qp3;qpp(1);qpp(2);qpp(3)];

Cap8_PDrobot2gdl.m


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

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),beta3,l3,q3(k)); End


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

Documents

Proyecto final MODELADO DE ROBOTS UPC TABASCO