Trabajo Final Robot Manipulador Mentor (Educacional)

UNIVERSIDAD TECNOLOGICA NACIONAL REGIONAL MENDOZA LARMA

ROBOT MANIPULADOR MENTOR

1 de Octubre

2011Educational Robots Cybernetic Applications

El presente proyecto se enfoca al control de un robot manipulador por medio de una computadora. Sin disponer de la informacin de software ni de hardware del fabricante se proceder a disear y elaborar los circuitos, interfaz y programas que conformaran el sistema de control del robot manipulador, as como la integracin del mismo.Se adjunta CD-Rom con simulacin en Matlab y Proteus.

PROYECTO FINALCATEDRA: SISTEMA DE CONTROL PROYECTO: MODELADO CINEMATICO Y SISTEMA DE CONTROL DE POSICION BRAZO ROBOTICO ALUMNO: GONZALO BERARDO- LEGAJO: 26499 Mail: [email protected]

Desarrollado por Gonzalo Berardo en dependencia al grupo de investigacin LARMA (Laboratorio de Robots Mvil Autnomos), para la ctedra Sistemas de Control.

2 ROBOT MANIPULADOR MENTORINDICE

OBJETIVOS Objetivo General Objetivo de Ctedra DETALLE GENERAL DEL BRAZO ROBOTICO Estructura Mecnica Elementos Terminales Sistema Sensorial SISTEMA DE CONTROL Trminos Bsicos Planteamiento del problema Elementos Controlador PID Reglas de sintonizacin de Ziegler-Nichols Desarrollo Prctico Sintonizacin Fina Diseo del controlador PID Discreto Implementacin del controlador CINEMATICA Y DINAMICA DE UN SISTEMA Trminos Bsicos Dinmica Inversa Definicin de Parmetros Modelo Dinmico Ecuacin de Lagrange Friccin Dinmica Directa Simulacin en Matlab DOCUMENTOS DE REFERENCIA

Pg. 3 Pg. 5 Pg. 6 Pg. 6 Pg. 6 Pg. 6 Pg. 7 Pg. 7 Pg. 8 Pg. 9 Pg. 9 Pg. 10 Pg. 12 Pg. 16 Pg. 18 Pg. 19 Pg. 21 Pg. 21 Pg. 21 Pg. 22 Pg. 22 Pg. 25 Pg. 26 Pg. 29 Pg. 30 Pg. 42

LARMA Laboratorio de Robots Mviles Autnomos Gonzalo Berardo

3 ROBOT MANIPULADOR MENTOROBJETIVO GENERAL El presente proyecto se enfoca al control de un robot manipulador por medio de una computadora. Sin disponer de la informacin de software ni de hardware del fabricante se proceder a disear y elaborar los circuitos, interfaz y programas que conformaran el sistema de control del robot manipulador, as como la integracin del mismo. El proyecto se divide en Modelado Matemtico y Sistema de Control de Posicin. MODELADO MATEMATICO Los modelos cinemticos directo e inverso; estarn basados en las convenciones referidas en Denavit-Hartenberg y en el mtodo geomtrico respectivamente. Se pueden plantear dos problemas: - Conocer la posicin del extremo del robot basndose en la posicin de las articulaciones (Problema Cinemtico Directo) - Conocer la posicin de las articulaciones del robot basndose en la posicin del extremo del robot (Problema Cinemtico Inverso) Los modelos dinmicos directo e inverso estn fundamentados en la formulacin de movimiento de Lagrange. De igual manera, se pueden plantear dos problemas: - Dinmico directo: Calculo del movimiento del extremo del robot a partir de los pares y fuerzas de los accionadores de las articulaciones. - Dinmico inverso: Calculo de los valores de los pares y fuerzas de los accionadores de las articulaciones a partir de las posiciones, velocidades y aceleraciones requeridas para el extremo del robot. SISTEMA DE CONTROL DE POSICION Para el control de posicin de cada articulacin del robot manipulador, se recurre al diseo y construccin de un controlador PID discreto. Partiendo del desconocimiento del modelo matemtico de la planta a controlar, se implementa una serie de procedimientos experimentales para la sintona de los controladores PID. Estos procedimientos se basan en la teora de sintonizacin de Ziegler Nichols.

A continuacin se adjunta el diseo de diagrama en bloque general del proyecto.


4 ROBOT MANIPULADOR MENTOR

Ref 1

PID DSCRETO

POTENCIA

MOTOR CINTURA

POTE

ADCMICROCONTROLADOR 1

Ref 2

PID DSCRETO

POTENCIA

MOTOR HOMBRO

POTE


CINEMATICA DIRECTA/INVERSA

Ref 3

PID DSCRETO

POTENCIA

MOTOR CODO

POTE


Ref 4

PID DSCRETO

POTENCIA

MOTOR MUECA

POTE

(Giro y elevacin)ADCMICROCONTROLADOR 4

Ref 5

PID DSCRETO

POTENCIA

MOTOR MUECA

POTE

(Giro y elevacin)ADCPC MICROCONTROLADOR 5

Figura 1. Diagrama en bloque general.

LARMA Laboratorio de Robots Mviles Autnomos Gonzalo BerardoMICROCONTROLADOR 5

5 ROBOT MANIPULADOR MENTOROBJETIVO DE CATEDRA Se requiere disear y construir un controlador PID para regular la posicin de un motor de corriente continua (CC). La figura 2 muestra el diagrama de bloques del sistema controlado, en donde:

Figura 2. Diagrama de bloques del sistema controlado

-

La seal de salida, y, corresponde a la salida del terminal mvil del potencimetro. La seal de referencia, r, corresponde a la posicin deseada. La seal de error, e, corresponde a la diferencia entre la seal de referencia y la seal de salida. La seal de control, u, corresponde al voltaje producido por el controlador para disminuir o anular el error.

Partiendo del deconocimiento del modelo matematico de la planta, se sintonizara los parametros del controlador PID, por medio de la primer regla de sintonizacion de Ziegler Nichols. Se evaluaran las distintas contribucones proporcional, derivatica e integral para luego hacer un refinamiento del mismo.


6 ROBOT MANIPULADOR MENTORDETALLE GENERAL DEL BRAZO ROBOTICO Robot Manipulador: Mentor (N de Serie: 803132), fabricado por Cybernetic Applications, bajo fines educativos. El mismo presenta una configuracin de tipo manipulador articulado y cuenta con una arquitectura poliarticulada. Es un robot de 5 GDL (grados de libertad) los cuales estn denotados por sus similares al ser humano, estos son: cintura, hombro, codo, mueca-elevacin y mueca-giro. Cada articulacin realiza movimientos rotatorios desarrollados por actuadotes que son motores de 12 voltios de corriente continua (CC) acoplados a una caja reductora y su posicionamiento se registra a travs de encoders analgicos (potencimetros rotativos resistivos). Los elementos que lo integran estn construidos de acero, plstico y aluminio siendo este ltimo el que constituye la mayor parte de su estructura. Su capacidad mxima de carga es de un kilogramo. Estructura Mecnica Tipo: Robot Antropomrfico o Articulado de 5 GDL. Configuraciones Cinemticas: RRR Cintura rotacional, Hombro rotacional, Codo rotacional. Ventajas: Mxima flexibilidad. Volumen de trabajo mayor que el volumen del robot. Apto para entornos corrosivos. Permite accionadores elctricos. Desventajas: Modelo cinemtico complejo (movimientos lineales). Accionadores: Elctricos. Motores a DC.

-

Elementos Terminales Tipo: Pinza Mecnica. Herramientas: No posee.

Sistema Sensorial Sensores Internos: Encoders Analgicos. Localizados en la propia estructura del robot.


7 ROBOT MANIPULADOR MENTORSISTEMA DE CONTROL Trminos Bsicos Seal de salida: es la variable que se desea controlar (posicin, velocidad, presin, temperatura, etc.). Tambin se denomina variable controlada. Seal de referencia: es el valor que se desea que alcance la seal de salida. Error: es la diferencia entre la seal de referencia y la seal de salida real. Seal de control: es la seal que produce el controlador para modificar la variable controlada de tal forma que se disminuya, o elimine, el error. Seal anloga: es una seal continua en el tiempo. Seal digital: es una seal que solo toma valores de 1 y 0. Conversor anlogo/digital: es un dispositivo que convierte una seal analgica en una seal digital (1 y 0). Conversor digital/anlogo: es un dispositivo que convierte una seal digital en una seal analgica (corriente o voltaje). Planta: es el elemento fsico que se desea controlar. Planta puede ser: un motor, un horno, un sistema de disparo, un sistema de navegacin, un tanque de combustible, etc. Proceso: operacin que conduce a un resultado determinado. Sistema: consiste en un conjunto de elementos que actan coordinadamente para realizar un objetivo determinado. Perturbacin: es una seal que tiende a afectar la salida del sistema, desvindola del valor deseado. Sensor: es un dispositivo que convierte el valor de una magnitud fsica (presin, flujo, temperatura, etc.) en una seal elctrica codificada ya sea en forma analgica o digital. Tambin es llamado transductor. Los sensores, o transductores, analgicos envan, por lo regular, seales normalizadas de 0 a 5 voltios, 0 a 10 voltios o 4 a 20 mA. Sistema de control en lazo cerrado: es aquel en el cual continuamente se est monitoreando la seal de salida para compararla con la seal de referencia y calcular la seal de error, la cual a su vez es aplicada al controlador para generar la seal de control y tratar de llevar la seal de salida al valor deseado. Tambin es llamado control realimentado.

-


8 ROBOT MANIPULADOR MENTORSistema de control en lazo abierto: en estos sistemas de control la seal de salida no es monitoreada para generar una seal de control.

Planteamiento del problema Se requiere disear y construir un controlador PID para regular la posicin de un motor de corriente continua (CC). La figura 2 muestra el diagrama de bloques del sistema controlado, en donde: La seal de salida, y, corresponde a la salida del terminal mvil del potencimetro. Si ste se alimenta con 5 voltios en sus terminales fijos (a y b), producir un voltaje en su terminal mvil (c) equivalente a su posicin. Podemos decir entonces que cuando produce 0 voltios esta en la posicin equivalente a 0 grados, 1.25 voltios corresponder a 90 grados, 2.5 voltios a 180 grados, etc. La seal de referencia, r, corresponde a la posicin deseada. Es decir, si queremos que el motor alcance la posicin 180 grados debemos colocar una referencia de 2.5 voltios, si queremos 270 grados colocamos referencia de 3.75 voltios, etc. Esta seal de referencia ser determinada a partir de la cinemtica y de la dinmica del sistema. La seal de error, e, corresponde a la diferencia entre la seal de referencia y la seal de salida. Por ejemplo, si queremos que el motor alcance la posicin de 90 grados colocamos una seal de referencia de 1.25 voltios y esperamos dnde se ubica exactamente. Si se posiciona en 67.5 grados el potencimetro entregar una seal de salida de 0.9375 voltios y la seal de error, e, ser de 0.3125 voltios (22.5 grados). La seal de control, u, corresponde al voltaje producido por el controlador para disminuir o anular el error. Si la seal de error es positiva indica que la referencia es mayor que la salida real, entonces el controlador coloca un voltaje positivo al motor para que contine girando hasta minimizar o anular el error. Si por el contrario la seal de error resulta negativa indica que la salida sobrepas la referencia entonces el controlador debe poner un voltaje negativo para que el motor gire en sentido contrario hasta minimizar o anular el error.

Figura 2. Diagrama de bloques del sistema controlado


9 ROBOT MANIPULADOR MENTORElementos Motor de CC de imn permanente de 12V, 500mA y 500 r.p.m. Los motores de CC de imn permanente comerciales normalmente no giran a la misma velocidad en sentido dextrgiro que en sentido levgiro por lo que el controlador no tendr la misma respuesta en ambos sentidos. Potencimetro lineal de 5 K , de una sola vuelta. Acople mecnico entre el eje del motor y el eje del potencimetro. Fuente de 5 voltios de corriente contina para alimentar los terminales fijos del potencimetro. ACOPLE MOTOR POTENCIOMETRO

-

+5 V (Terminal a) 0 a +5 V (Terminal c) GND (Terminal b)) +5 V GND

Figura 3. Motor - Potencimetro.

Controlador PID Los controladores PID son ampliamente usados en los sistemas de control industrial , cuyas siglas corresponden; P: proporcional, I: integral, D: derivativo, se refieren a los tres trminos que operan sobre la seal de error para producir la seal a la salida del controlador. Se aplican a la mayora de los sistemas de control, pero se aprecia ms su utilidad cuando el modelo de la planta a controlar no se conoce y los mtodos analticos no pueden ser empleados. En este caso se debe recurrir a procedimientos experimentales para la sintona de los controladores PID.


10 ROBOT MANIPULADOR MENTORSi se pude obtener un modelo matemtico de la planta, es posible aplicar diversas tcnicas de diseo con el fin de determinar los parmetros del controlador que cumpla con las especificaciones del transitorio y del estado estacionario del sistema en lazo cerrado. Un controlador PID se define matemticamente como:

Donde e(t) es el error de la seal, u(t) es la entrada de control del proceso, es la ganancia proporcional, es la constante de tiempo integral y es la constante de tiempo derivativa. El primer sumando aplica una seal proporcional al error. El segundo evala la evolucin del error y se vuelve ms significativo cuando el error es pequeo pero constante. El tercer trmino considera la tendencia en el error y se nota ms cuando el error se produce por instantes.

En el dominio s, el controlador PID se puede escribir como:

El esquema habitual de uso del controlador PID es:

)

Planta

Figura 4. Control PID de una planta

Reglas de Ziegler Nichols El proceso de seleccionar los parmetros del controlador que cumplan con las especificaciones de comportamiento dadas se conoce sintona del controlador. Ziegler y Nichols sugirieron reglas para sintonizar los controladores PID (es decir, dar valores para , y ) basndose en las respuestas escaln experimentales (1er mtodo) o en el valor de (2do mtodo) que produce estabilidad marginal cuando solo se usa la accin de control proporcional.


11 ROBOT MANIPULADOR MENTORHay que tener presente que las reglas de sintona de Ziegler y Nichols dan una estimacin razonable de los parmetros del controlador proporcionando un punto de partida para la sintonizacin fina. En este proyecto se abordara el caso de que se desconoce el modelo matemtico de la planta, por lo que se recurrir a la 1er mtodo de Ziegler y Nichols para la sintona del controlador PID. 1er Mtodo de Sintonizacin: En el primer mtodo, la respuesta de la planta a una entrada escaln unitario se obtiene de manera experimental, tal como se muestra en la figura 5. Si la planta no contiene integradores ni polos dominantes complejos conjugados, la curva de respuesta escaln unitario puede tener forma de S, como se observa en la figura 6. La curva con forma de S se caracteriza por dos parmetros: el tiempo de retardo L y la constante de tiempo T.

1 u(t ) Planta c(t)

Figura 5. Respuesta a un escaln unitario de una planta

El tiempo de retardo y la constante de tiempo se determinan dibujando una recta tangente en el punto de inflexin de la curva con forma de S y determinando las intersecciones de esta tangente con el eje de tiempo y con la lnea C(t)=K, tal como se muestra en la figura 6.

Recta tangente al punto de inflexin

Los parmetros se pueden obtener de esta respuesta:

C(t)=K

Figura 6. Respuesta de salida a una entrada escaln


12 ROBOT MANIPULADOR MENTOREn este caso, la funcin de transferencia de primer orden con un retardo del modo siguiente: se aproxima mediante un sistema

Ziegler y Nichols sugirieron establecer los valores de aparece en la tabla 1.

de acuerdo con la formula que

Tipo de Controlador P PI PIDTabla 1. Regla de sintona de Ziegler Nichols basada en la respuesta escaln de la planta (primer mtodo).

0 0

Por lo tanto:

El controlador PID sintonizado mediante el primer mtodo de las reglas de Ziegler y Nichols presentara un polo en el origen y un cero doble en . Desarrollo Prctico: Aplicamos una seal escaln de 5V de amplitud a una de las articulaciones del manipulador, ms precisamente a la articulacin del hombro. Por medio de una placa de adquisicin de datos, obtenemos la seal de respuesta al escaln. Esta seal es proveda por el cursor mvil del potencimetro a medida que la articulacin se desplaza rotacionalmente.



Con la ayuda de la herramienta Matlab obtenemos la ubicacin del puno de inflexin de la curva S y graficamos una recta tangente a este, como se observa en la siguiente grafica.

Figura 7. Respuesta al escaln

Figura 8. Respuesta al escaln (LA) y Recta tangente al punto de inflexin.

Las intersecciones de la recta tangente con el eje de tiempo y con la lnea C(t)=5V obtenemos el tiempo de retardo L y la constante de tiempo T. Se adjunta el cdigo utilizado en Matlab para graficar la recta tangente sobre la respuesta al escaln (en la) y los correspondientes clculos de L y de T.



% SITONIZACION DE UN PID BASADO EN LA REGLA DE ZIEGLER NICHOLS %------------------------------------------------------------H; pp=pole(H); dt=0.05; t=0:dt:8; y=step(H,t)'; dy=diff(y)/dt; [m,p]=max(dy); d2y=diff(dy)/dt; yi=y(p); ti=t(p); L=ti-yi/m; Tau=(y(end)-yi)/m+ti-L; % Planta % Polos de lazo abierto % % % % Respuesta al escaln Derivada Punto de inflexin Segunda derivada

% Retardo % Constante de tiempo

figure(1) plot(t,y,'b',[0 L L+Tau t(end)],[0 0 y(end) y(end)],'k') title('Respuesta al escaln') ylabel('Amplitud') xlabel('tiempo (s)') legend('Exacta','Aproximacion Lineal') Figura 9. Respuesta al escaln

El algoritmo nos arroja los resultados de: ; ;

De igual modo, el cdigo en Matlab para el clculo de

,

,

conforme a la tabla 1.

% Control PID con primer metodo de ZIEGLER NICHOLS Kp=1.2*Tau/L; Ti=2*L; Td=0.5*L; Ki=Kp/Ti; Kd=Kp*Td; G=tf([Kd Kp Ki],[1 0]) % Controlador % Lazo cerrado HLC=feedback(G*H,1) figure(2) step(HLC) figure(3) pzmap(HLC) grid Figura 10. Calculo de parmetros y Funcin de Transferencia del controlador PID en LA y LC


15 ROBOT MANIPULADOR MENTORResultados: ; Cuya funcin de transferencia ser:Funcin de Transferencia en lazo abierto

;

El controlador PID sintonizado mediante el primer mtodo de las reglas de Ziegler y Nichols presentara un polo en el origen y un cero doble en .

Al final de cdigo de la figura 10, se representa la funcin de transferencia en lazo cerrado y se grafica la respuesta escaln y la constelacin de polos y ceros.Funcin de Transferencia en lazo cerrado

Figura 11. Constelacin de polos y ceros

Polos en LC: -3.7528 + 4.9994i ; -3.7528 - 4.9994i ; -2.6280LARMA Laboratorio de Robots Mviles Autnomos Gonzalo Berardo


Figura 12. Respuesta al escaln del PID

De la grafica 12, podemos observar que existe un sobre paso del 24% y un tiempo de establecimiento de 1.16 seg. Como se menciono anteriormente la sintonizacin de Ziegler Nichols es un procedimiento de punto de partida al momento de sintonizar un PID. Ahora en ms, solo basta con realizar un ajuste fino para disminuir el tiempo de establecimiento, el sobre paso, y otros aspectos de inters. Sintonizacin Fina A continuacin se grafica (figura 13) la variacin de la respuesta al escaln del PID (en LC) por medio de un refinamiento de los parmetros calculados. Cdigo:% Refinamiento Opcin 1 Kp1=1.5*Kp; Kd1=3*Kd; Ki1=1.0*Ki; G1=tf([Kd1 Kp1 Ki1],[1 0]); HLC1=feedback(G1*H,1); % Refinamiento Opcin 2 Kp2=1.5*Kp; Kd2=5*Kd; Ki2=1.0*Ki; G2=tf([Kd2 Kp2 Ki2],[1 0]); HLC3=feedback(G2*H,1); figure(4) step(HLC,HLC1,HLC3)


17 ROBOT MANIPULADOR MENTORlegend('PID Z-N','PID Refinado Opcin 1','PID Refinado Opcion 2')

Figura 13. Refinamiento del PID OBSERVACION ES:

P R O P O R C I O N A L

Cuando en el sistema se emplea un controlador proporcional, la seal a la salida del controlador es mltiplo (ganancia KP) del porcentaje de cambio en la seal medida. Existe un valor lmite de la ganancia del controlador proporcional. Ms all de este lmite el sistema alcanza valores superiores a los deseados, fenmeno conocido como sobreoscilacin. Por razones de seguridad, la sobreoscilacin no debe superar el 30%. El controlador proporcional no considera el tiempo. Por esta razn, para solucionar el error permanente y lograr que el sistema contenga alguna componente que considere la variacin respecto al tiempo, se incluyen las acciones integral y derivativa. El modo de control integral tiene como propsito disminuir y eliminar el error en estado estacionario, provocado por el modo proporcional. El control integral acta cuando hay una desviacin entre la variable y el punto de consigna, integrando esta desviacin en el tiempo y sumndola a la accin proporcional. El error es integrado, es decir es promediado o sumado por un perodo determinado, para posteriormente ser multiplicado por una constante KI. Cuando la respuesta integral es adicionada al modo proporcional para formar el control P + I, se obtiene una respuesta estable del sistema sin error estacionario. La accin derivativa se manifiesta cuando hay un cambio en el valor absoluto del error. La funcin de la accin derivativa es mantener el error al mnimo corrigindolo proporcionalmente con la misma velocidad que se produce; de esta manera evita que el error se incremente, y reducir el sobrepico. El error se deriva con respecto al tiempo y se multiplica por una constante KD. El control derivativo se caracteriza por el tiempo de accin derivada. Cuando el tiempo de accin derivada es grande, hay inestabilidad en el proceso. Cuando el tiempo de accin derivada es pequeo, la variable oscila demasiado con relacin al punto de consigna. El tiempo ptimo de accin derivativa es el que retorna la variable al punto de consigna con las mnimas oscilaciones Suele ser poco utilizada debido a la sensibilidad al ruido que manifiesta y a las complicaciones que ello conlleva.LARMA Laboratorio de Robots Mviles Autnomos Gonzalo Berardo

I N T E G R A L

D E R I V A T I V O

18 ROBOT MANIPULADOR MENTORDiseo del controlador PID Discreto El PID ser programado en un microcontrolador PIC, por lo que la realizacin del mismo en tiempo discreto estar dado por la transformada z:

Siendo:

Donde: para T=300

Existen distintas posibilidades de la realizacin prctica de un controlador PID. Este proyecto utiliza un diseo paralelo debido a facilidad que presenta a la hora de programar el algoritmo.+ + proporcional integral

Ki

T+ + u(kT)

e(kT)

Kp

Kd

derivativa

+ -

TFi gura 14. Diseo pa ralelo del controlador PID

El muestreo debe ser mucho menor que el tiempo de establecimiento del sistema en lazo abierto. En el modelo de Ziegler Nichols se toma un valor (tambin puede utilizarse


19 ROBOT MANIPULADOR MENTORImplementacin del controladorPosicin a alcanzar (Set Point)rT=4.5;

Por medio de la cinemtica del sistema obtenemos la seal de referencia rT

Clculo del trmino integraliT=Ki*eT+iT0;

SintonizacinKp=900; Ki=0; Kd=400; T=300; pwm_min=0.0; pwm_max=950.0; pote_min=0; pote_max=4.9; // Constante Proporcional // Constante Integral // Constante Derivativa // Tiempo de Muestreo // Valor PWM Mnimo // Valor PWM Mximo // Valor POTE Mnimo // Valor POTE Mximo

Clculo del trmino derivativo dT=Kd*(eT-eT0);

Clculo de la salida PIDuT=iT+Kp*eT+dT;

REFINAMIENTOKp=1.5*Kp; Ki=1.0*Ki; Kd=3.0*Kd;

Salida PID > MAXif (uT>pwm_max)

NO

Salida PID < MAXif(uT

Trabajo Final Robot Manipulador Mentor (Educacional)

Documents

Transcript of Trabajo Final Robot Manipulador Mentor (Educacional)