Trabajo Final Robot Manipulador Mentor (Educacional)

UNIVERSIDAD TECNOLOGICA NACIONAL REGIONAL MENDOZA LARMA
ROBOT MANIPULADOR MENTOR
1 de Octubre
2011
Educational 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 FINAL
CATEDRA: SISTEMA DE CONTROL PROYECTO: MODELADO CINEMATICO Y SISTEMA DE CONTROL DE POSICION BRAZO ROBOTICO ALUMNO: GONZALO BERARDO
- LEGAJO: 26499 Mail: Gonzalo_b@msn.com
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 MENTOR

INDICE
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

OBJETIVO 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.
Ref 1
PID DSCRETO
POTENCIA
MOTOR CINTURA
POTE
ADC
MICROCONTROLADOR 1
Ref 2
PID DSCRETO
POTENCIA
MOTOR HOMBRO
POTE
ADC
MICROCONTROLADOR 2
CINEMATICA DIRECTA/INVERSA
Ref 3
PID DSCRETO
POTENCIA
MOTOR CODO
POTE
ADC
MICROCONTROLADOR 3
Ref 4
PID DSCRETO
POTENCIA
MOTOR MUECA
POTE
(Giro y elevacin)
ADC
MICROCONTROLADOR 4
Ref 5
PID DSCRETO
POTENCIA
MOTOR MUECA
POTE
(Giro y elevacin)
ADC
PC MICROCONTROLADOR 5
Figura 1. Diagrama en bloque general.

MICROCONTROLADOR 5

OBJETIVO 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.

DETALLE 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.

SISTEMA 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.

Sistema 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

Elementos 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.

Si 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.

Hay 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

En 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 PID

Tabla 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

Resultados: ; 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.6280

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)

legend('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.
I N T E G R A L
D E R I V A T I V O

Diseo 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
+ -
T
Fi 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

Implementacin del controlador
Posicin a alcanzar (Set Point)
rT=4.5;
Por medio de la cinemtica del sistema obtenemos la seal de referencia rT
Clculo del trmino integral

iT=Ki*eT+iT0;
Sintonizacin
Kp=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 PID

uT=iT+Kp*eT+dT;
REFINAMIENTO
Kp=1.5*Kp; Ki=1.0*Ki; Kd=3.0*Kd;
Salida PID > MAX

if (uT>pwm_max)
NO
Salida PID < MAX

if(uT<pwm_min
NO
Llave en ON?
if(input(pin_B2)==1)
NO
Mostramos por display

rT yT eT STOP
Salida PID = MAX

uT=pwm_max;
Salida PID = MAX

uT=pwm_max;
Bloquea motor ADC

valor=read_adc(); output_low(pin_B4); output_low(pin_B5);
Transferencia de salida PID a seal PWM

control=(int16)uT;
Conversin 1023 a 5V
yT=(float)valor*5.0/1023.0;
ADC
valor=read_adc();
Cargamos PWM
set_pwm1_duty(control);
Clculo error
eT=rT-yT;
Conversin 1023 a 5V
yT=(float)valor*5.0/1023.0;
Guardar variables
iT0=iT; eT0=eT;
Error es negativo?
If(eT<0)
NO
Pone bandera en 0
sentido=0;
Bandera = 1?
if(sentido==1)
NO
Calcula modulo Pone bandera en 1

eT=eT*(-1); sentido=1;
Motor giro a derecha

output_low(pin_B5); output_high(pin_B4);
Motor giro a izquierda

output_high(pin_B5); output_low(pin_B4);
Error es < Emin? if(eT<Emin)
Mostramos por display

rT yT eT control
Reinicio de variables
eT=0; iT0=0; eT0=0;
Tiempo de muestreo
delay_ms(T);
Figura 15. Diagrama en bloque. Cdigo PID ADC PMW en CCS:


La seal de referencia proveda por el modelo cinemtico (Directo/Inverso) y la seal de salida proveda por el cursor mvil del potencimetro (convertida previamente a digital) se restan para obtener la seal de error. Esta ltima es amplificada, derivada e integrada. La seal de salida del PID (seal actuante) es convertida en una seal PWM (de Modulacin de Ancho de Pulso) para comandar la etapa de potencia del motor por medio de un puente H (Figura 16). Cada articulacin poseer su propio e individual sistema de control de posicin desarrollado en un microcontrolador. Por su parte El modelo cinemtico correr en un programa en una PC (Figura 1).
CINEMATICA DIRECTA/INVERSA
Ref
Error
PID DSCRETO Kp ; Ki ;Kd ; T
Actuante
PWM
POTENCIA Puente H
MOTOR Articulacin
POTE
Salida Digital
ADC
PC Figura 16
MICROCONTRO LADOR

CINEMATICA Y DINAMICA DE UN SISTEMA NOTA: A continuacin se expone parte del material: Caracterizacin de un Robot CENIDET Trminos Bsicos Cinemtica Se pueden plantear dos problemas: Conocer la posicin del extremo del robot basndose en la posicin de las articulaciones (Problema CinemticoDirecto) Conocer la posicin de las articulaciones del robot bas ndose en la posicin del extremo del robot (Problema CinemticoInverso) Dinmica 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. Dinmica Inversa El modelo dinmico inverso expresa las fuerzas y pares que intervienen, en funcin de la evolucin temporal de las coordenadas articulares y sus derivadas. Resolver las ecuaciones dinmicas de movimiento para un robot no es una tarea sencilla debido a la cantidad de nmeros de grados de libertad y sistemas no lineales que lo componen; para poder definir dichas ecuaciones, se emple la formulacin de movimiento de Lagrange. Consideraciones Para el desarrollo del modelo se omitieron los dos ltimos grados de libertad, ya que pertenecen a la orientacin del efector final y sus efectos no son significativos para el comportamiento dinmico del robot. En general se consider al manipulador como un sistema rgido, concentrando las masas en el centro de cada eslabn tal y como lo muestra la figura 2; adems fueron considerados los efectos de friccin provocados por las transmisiones mecnicas que en este caso se da por engranajes (esta consideracin es importante ya que estos efectos pueden alcanzar un orden de un 25% del par requerido para mover al manipulador en situaciones tpicas [1]). Esto hace que el sistema sea no conservativo, sin embargo, el sistema puede ser resuelto por medio de la formulacin de Lagrange y finalmente aplicando la funcin de disipacin de Rayleigh se permite adicionar los efectos de friccin al modelo dinmico. En el anlisis de cada eslabn, los efectos de las Energas Cintica y Potencial de los eslabones anteriores (si existen) repercuten en el anlisis del eslabn en cuestin. Las prdidas debidas a

los huelgos y excentricidades en los engranajes, as como la eficiencia del motor fueron despreciadas dentro del anlisis de la ecuacin de movimiento de Lagrange. Definicin de Parmetros La figura 12 muestra las condiciones dinmicas del manipulador; donde cada variable est definida de la siguiente manera:
W = Carga aplicada mi = Masa del i-simo eslabn g = Constante de gravedad l = Radio de giro para la masa de la base a = Longitud del primer eslabn b = Longitud del segundo eslabn Ii = Inercia i-sima del eslabn Vi = Velocidad lineal del i-simo eslabn Ki = Energa cintica del i-simo eslabn Pi = Energa potencial del i-simo eslabn
FIGURA 12. DINAMICA INVERSA
Modelo Dinmico A continuacin se muestra el desarrollo del modelo dinmico inverso [1] [2] [3] [4] [5].

La energa potencial es cero debido a que un posible cambio en el ngulo 1 no altera la altura de la masa mb sobre la vertical.
Derivando y elevando al cuadrado las ecuaciones (9) y (10) para sustituir en (8) y aplicando identidades trigonomtricas tenemos:
Derivando y elevando al cuadrado las ecuaciones (18) y (19 ) para sustituir en (17 ) y aplicando identidades trigonomtricas tenemos:
Ecuacin de Lagrange La ecuacin de movimiento de Lagrange se obtiene con la sumatoria de las energas cinticas y potenciales en el manipulador [2].
Por lo que el Lagrangiano quedara como:
Para obtener el par aplicado a partir de la ecuacin de movimiento de Lagrange, se emplea:
Friccin Se sabe que la formulacin de movimiento de Lagrange slo hace referencia a sistemas conservativos, sin embargo, a travs de la funcin de disipacin de Rayleigh se pueden incluir prdidas como la friccin en un sistema conservativo. Un modelo en el que se incluyen estos efectos de friccin quedara de la siguiente manera:
Expresando la ecuacin 28 en forma matricial se tiene:
Tomando en cuenta los cofactores nulos finalmente se puede rescribir la ecuacin 29 finalmente como:
Por lo que la ecuacin 40 representa el modelo dinmico inverso del robot manipulador articulado.

Dinmica Directa El modelo dinmico directo expresa la evolucin temporal de las coordenadas articulares y sus derivadas, en funcin de las fuerzas y pares que interviene. Para la obtencin del modelo dinmico directo; se emplea el modelo dinmico inverso, partiendo de la ecuacin 40; y aplicando la regla de Cramer para resolver las siguientes ecuaciones.
Utilizando un cambio de variables para comodidad en el manejo de trminos se emplea:
Sustituyendo los pares en las ecuaciones 47, 48 y 49, sufren la siguiente modificacin:
Resultando:
Por lo que las ecuaciones 53, 54 y 55 representan el modelo dinmico directo del robot manipulador articulado. Simulacin en Matlab Cinemtica Directa:
%La cinemtica directa obtiene como resultado la posicin en que se encuentra el efector %final en coordenadas cartesianas (xyz), por lo que es necesario introducir como valores %iniciales los ngulos que tiene cada elemento del robot de acuerdo a su respectiva referencia. clear,clf,clc%Limpieza de las variables en memoria, de grficas y de pantalla hold on %Activa grficos %PRESENTACIN fprintf ('\n SIMULACIN DE LA CINEMTICA DIRECTA\n'); fprintf ('----------------------------------------------------------------\n\n\n'); %Se piden valores de los ngulos TETAS %Se consideran los lmites REALES del SCORBOT-ER V plus teta1 = input ('Valor del ngulo de la cintura en grados: '); while (teta1<0 || teta1>310) fprintf('\n Valor incorrecto\n'); teta1 = input ('Valor del ngulo de la cintura en grados: '); end teta2 = input ('Valor del ngulo del hombro en grados: '); while (teta2<-35 || teta2>130) fprintf('\n Valor incorrecto\n'); teta2 = input ('Valor del ngulo del hombro en grados: '); end teta3 = input ('Valor del ngulo del codo en grados: '); while (teta3<-130 || teta3>140) fprintf('\n Valor incorrecto\n'); teta3 = input ('Valor del ngulo del codo en grados: '); end teta4 = input ('Valor del ngulo de elevacin (pitch) en grados: '); while (teta4<-130 || teta4>130) fprintf('\n Valor incorrecto\n'); teta4 = input ('Valor del ngulo de elevacin (pitch ) en grados: '); end teta5 = input ('Valor del ngulo de giro (roll) en grados: '); while (teta5<-570 || teta5>570) fprintf('\n Valor incorrecto\n'); teta5 = input ('Valor del ngulo de giro (roll) en grados: '); end %Se dan las longitudes de los eslabones %pidindoselas al usuario link1 = input ('\n Longitud del primer eslabn en cm: '); link2 = input (' Longitud del segundo eslabn en cm: '); link3 = input (' Longitud de la garra (gripper) en cm: ');

%Lmites de los ejes eje = (link1+link2+link3); axis([-eje,eje,-eje,eje,-eje,eje]); title ('SIMULACIN DE LA CINEMTICA DIRECTA'); xlabel('X (cm)'); ylabel('Y (cm)'); zlabel('Z (cm)'); grid;%Activa cuadrcula %Conversiones de grados a radianes rad1=(teta1*pi)/180; rad2=(teta2*pi)/180; rad3=(teta3*pi)/180; rad4=(teta4*pi)/180; rad5=(teta5*pi)/180; %Dibujo de la base sphere (20) %Clculo y graficacin del link1 %Cintura-Hombro x1=0; y1=0; z1=0; x2=cos(rad1)*cos(rad2)*link1; y2=sin(rad1)*cos(rad2)*link1; z2=sin(rad2)*link1; %azul line ([x1,x2],[y1,y2],[z1,z2],'LineWidth',8,'Color',[0 0 1]) %fprintf('\n Los valores de las coordenadas finales del primer eslabn son:\n'); %fprintf('\tx2= %12.5f\n',x2); %fprintf('\ty2= %12.5f\n',y2); %fprintf('\tz2= %12.5f\n',z2); %Clculo y graficacin del link2 %Codo-mueca x3=x2; y3=y2; z3=z2; x4= (((cos(rad1)*cos(rad2)*cos(rad3))(cos(rad1)*sin(rad2)*sin(rad3)))*link2)+(cos(rad1)*cos(rad2)*link1); y4= (((sin(rad1)*cos(rad2)*cos(rad3))(sin(rad1)*sin(rad2)*sin(rad3)))*link2)+(sin(rad1)*cos(rad2)*link1); z4=(((sin(rad2)*cos(rad3))+(cos(rad2)*sin(rad3)))*link2)+(sin(rad2)*link1); line ([x3,x4],[y3,y4],[z3,z4],'LineWidth',8,'Color',[1 0 0]) fprintf('\n\n Los valores de las coordenadas finales del segundo eslabn son:\n\n'); fprintf('\tX = %12.5f\n',x4); %x4 fprintf('\tY = %12.5f\n',y4); %y4 fprintf('\tZ = %12.5f\n',z4); %z4 %Graficacin del gripper %Vector N fen=2;%Factor de escalamiento nxa= cos(rad1)*cos(rad2)*cos(rad3)*cos(rad4)*cos(rad5); nxb= -cos(rad1)*sin(rad2)*sin(rad3)*cos(rad4)*cos(rad5); nxc= -cos(rad1)*cos(rad2)*sin(rad3)*sin(rad4)*cos(rad5); nxd= -cos(rad1)*sin(rad2)*cos(rad3)*sin(rad4)*cos(rad5); nxe= -sin(rad1)*sin(rad5); nx= (nxa+nxb+nxc+nxd+nxe)*fen; nya= sin(rad1)*cos(rad2)*cos(rad3)*cos(rad4)*cos(rad5); nyb= -sin(rad1)*sin(rad2)*sin(rad3)*cos(rad4)*cos(rad5); nyc= -sin(rad1)*cos(rad2)*sin(rad3)*sin(rad4)*cos(rad5); nyd= -sin(rad1)*sin(rad2)*cos(rad3)*sin(rad4)*cos(rad5); nye= -cos(rad1)*sin(rad5); ny= (nya+nyb+nyc+nyd+nye)*fen; nza= -sin(rad2)*cos(rad3)*cos(rad4)*cos(rad5); nzb= -cos(rad2)*sin(rad3)*cos(rad4)*cos(rad5); LARMA Laboratorio de Robots Mviles Autnomos nzc= sin(rad2)*sin(rad3)*sin(rad4)*cos(rad5); Gonzalo Berardo nzd= -cos(rad2)*cos(rad3)*sin(rad4)*cos(rad5);

nye= -cos(rad1)*sin(rad5); ny= (nya+nyb+nyc+nyd+nye)*fen; nza= -sin(rad2)*cos(rad3)*cos(rad4)*cos(rad5); nzb= -cos(rad2)*sin(rad3)*cos(rad4)*cos(rad5); nzc= sin(rad2)*sin(rad3)*sin(rad4)*cos(rad5); nzd= -cos(rad2)*cos(rad3)*sin(rad4)*cos(rad5); nz= (nza+nzb+nzc+nzd)*-fen; %negra line ([x4,x4+nx],[y4,y4+ny],[z4,z4+nz],'LineWidth',2.5,'Color',[0 0 0]) %Vector S fes=1.5;%Factor de escalamiento sxa= -cos(rad1)*cos(rad2)*cos(rad3)*cos(rad4)*sin(rad5); sxb= cos(rad1)*sin(rad2)*sin(rad3)*cos(rad4)*sin(rad5); sxc= cos(rad1)*cos(rad2)*sin(rad3)*sin(rad4)*sin(rad5); sxd= cos(rad1)*sin(rad2)*cos(rad3)*sin(rad4)*sin(rad5); sxe= -sin(rad1)*cos(rad5); sx= (sxa+sxb+sxc+sxd+sxe)*fes; sya= -sin(rad1)*cos(rad2)*cos(rad3)*cos(rad4)*sin(rad5); syb= sin(rad1)*sin(rad2)*sin(rad3)*cos(rad4)*sin(rad5); syc= sin(rad1)*cos(rad2)*sin(rad3)*sin(rad4)*sin(rad5); syd= sin(rad1)*sin(rad2)*cos(rad3)*sin(rad4)*sin(rad5); sye= cos(rad1)*cos(rad5); sy= (sya+syb+syc+syd+sye)*fes; sza= sin(rad2)*cos(rad3)*cos(rad4)*sin(rad5); szb= cos(rad2)*sin(rad3)*cos(rad4)*sin(rad5); szc= -sin(rad2)*sin(rad3)*sin(rad4)*sin(rad5); szd= cos(rad2)*cos(rad3)*sin(rad4)*sin(rad5); sz= (sza+szb+szc+szd)*-fes; %verde line ([x4,x4-sx*2],[y4,y4-sy*2],[z4,z4-sz*2],'LineWidth',2.5,'Color',[0 1 0]) %gris line ([x4,x4+sx],[y4,y4+sy],[z4,z4+sz],'LineWidth',6,'Color',[0.5 0.5 0.5]) %gris line ([x4,x4-sx],[y4,y4-sy],[z4,z4-sz],'LineWidth',6,'Color',[0.5 0.5 0.5]) %Vector A fea=3.5;%Factor de escalamiento axa= cos(rad1)*cos(rad2)*cos(rad3)*sin(rad4); axb= -cos(rad1)*sin(rad2)*sin(rad3)*sin(rad4); axc= cos(rad1)*cos(rad2)*sin(rad3)*cos(rad4); axd= cos(rad1)*sin(rad2)*cos(rad3)*cos(rad4); ax= (axa+axb+axc+axd)*fea; aya= sin(rad1)*cos(rad2)*cos(rad3)*sin(rad4); ayb= -sin(rad1)*sin(rad2)*sin(rad3)*sin(rad4); ayc= sin(rad1)*cos(rad2)*sin(rad3)*cos(rad4); ayd= sin(rad1)*sin(rad2)*cos(rad3)*cos(rad4); ay= (aya+ayb+ayc+ayd)*fea; aza= -sin(rad2)*cos(rad3)*sin(rad4); azb= -cos(rad2)*sin(rad3)*sin(rad4); azc= -sin(rad2)*sin(rad3)*cos(rad4); azd= cos(rad2)*cos(rad3)*cos(rad4); az= (aza+azb+azc+azd)*-fea; %amarrilo line([x4,x4+ax*1.2],[y4,y4+ay*1.2],[z4,z4+az*1.2],'LineWidth',2.5,'Color',[ 1 1 0])
%tomando de origen el vector S verde %lnea gris (Dedo 1) line ([x4-sx,x4+ax-sx],[y4-sy,y4+ay-sy],[z4sz,z4+az+sz],'LineWidth',6,'Color',[0.5 0.5 0.5]) %tomando de origen el vector S verde %lnea gris (Dedo 2) line([x4+sx,x4+ax+sx],[y4+sy,y4+ay+sy],[z4+sz,z4+az+sz],'LineWidth',6,'Colo r',[0.5 0.5 0.5]) hold off %Desactiva grficos
Resultado:
SIMULACIN DE LA CINEMTICA INVERSA

%La cinemtica inversa obtiene como resultado los ngulos necesarios para llegar a una %posicin deseada en donde se encuentra el efector final, por lo que es necesario %introducir como valores iniciales las coordenadas cartesianas (xyz). clear,clf,clc %Limpieza de las variables en memoria, de grficas y de pantalla hold on %Activa grficos %PRESENTACIN fprintf ('\n SIMULACIN DE LA CINEMTICA INVERSA\n'); fprintf ('----------------------------------------------------------------\n\n\n'); %Se dan las longitudes de los eslabones %pidindoselas al usuario link1 = input ('Longitud del primer eslabn en cm: '); link2 = input ('Longitud del segundo eslabn en cm: '); link3 = input ('Longitud de la garra (gripper) en cm: '); %Se piden las coordenadas del efector final x = input ('\nValor de la coordenada en X: '); y = input ('Valor de la coordenada en Y: '); z = input ('Valor de la coordenada en Z: '); teta4 = input ('Valor del ngulo de elevacin (pitch)en grados: '); while (teta4<-130 || teta4>130) fprintf('\n Valor incorrecto\n'); teta4 = input ('Valor del ngulo de elevacin (pitch)en grados: '); end teta5 = input ('Valor del ngulo de giro (roll) en grados: '); while (teta5<-570 || teta5>570) fprintf('\n Valor incorrecto\n'); teta5 = input ('Valor del ngulo de giro (roll) en grados: '); end %Tipo de configuracin ca = input ('\nLa simulacin la desea con configuracin Codo Arriba (1) o Codo Abajo (0): '); %Condicin para asegurar el alcance de la posicin deseada R=sqrt(x^2+y^2+z^2); L=link1+link2+link3; while (R>L) fprintf('\n Posicin fuera de alcance!!!\n'); fprintf('\n Modifique algn parmetro\n\n'); %Se dan las longitudes de los eslabones %pidindoselas al usuario link1 = input ('Longitud del primer eslabn en cm: '); link2 = input ('Longitud del segundo eslabn en cm: '); link3 = input ('Longitud de la garra (gripper) en cm: '); %Se piden las coordenadas del efector final x = input ('Valor de la coordenada en X: '); y = input ('Valor de la coordenada en Y: '); z = input ('Valor de la coordenada en Z: '); ca = input ('\nLa solucin la desea con configuracin Codo Arriba(1) o Codo Abajo(0): '); R=sqrt(x^2+y^2+z^2); L=link1+link2+link3; end

%Lmites de los ejes eje = (link1+link2+link3); axis([-eje,eje,-eje,eje,-eje,eje]); xlabel('X (cm)'); ylabel('Y (cm)'); zlabel('Z (cm)'); grid;%Activa cuadrcula %Dibujo de la base sphere (20) if (x==0) %if y<0 % rxy=y*-1; %end rxy=y; %Clculo y graficacin de link1 %Teta1 de Cintura-Hombro teta1 = asin(y/rxy); ar=sqrt(y^2+z^2); beta=atan(z/y); else %Clculo y graficacin de link1 %Teta1 de Cintura-Hombro teta1 = atan(y/x); rxy=x/cos(teta1); ar=sqrt(rxy^2+z^2); beta=atan(z/rxy); end %Por ley de cosenos alfa=acos((link1^2+ar^2-link2^2)/(2*link1*ar)); %Si es configuracin codo arriba se mete al IF if ca==1 teta2=beta+alfa; if x<0 teta2=beta+alfa+180; end f=link1*sin(teta2); h=z-f; mu=asin(h/link2); teta3=teta2-mu; g=link1*cos(teta2); gx=g*cos(teta1); gy=g*sin(teta1); %Configuracin codo arriba x1=0; y1=0; z1=0; line ([x1,gx],[y1,gy],[z1,f],'LineWidth',8,'Color',[1 0 1]) %Clculo y graficacin del link2 %Codo-mueca x3=gx; y3=gy; z3=f; title ('SIMULACION DEL SCORBOT-ER V plus en Configuracin Codo Arriba'); line ([x3,x],[y3,y],[z3,z],'LineWidth',8,'Color',[0 1 0]) %Graficacin del gripper rad1=teta1; rad2=teta2; rad3=teta3; rad4=teta4; rad5=teta5; x4=x; y4=y; z4=z; LARMA Laboratorio de Robots Mviles Autnomos %Vector N Gonzalo Berardo fen=2;%Factor de escalamiento
rad5=teta5; x4=x; y4=y; z4=z; %Vector N fen=2;%Factor de escalamiento nxa= cos(rad1)*cos(rad2)*cos(rad3)*cos(rad4)*cos(rad5); nxb= -cos(rad1)*sin(rad2)*sin(rad3)*cos(rad4)*cos(rad5); nxc= -cos(rad1)*cos(rad2)*sin(rad3)*sin(rad4)*cos(rad5); nxd= -cos(rad1)*sin(rad2)*cos(rad3)*sin(rad4)*cos(rad5); nxe= -sin(rad1)*sin(rad5); nx= (nxa+nxb+nxc+nxd+nxe)*fen; nya= sin(rad1)*cos(rad2)*cos(rad3)*cos(rad4)*cos(rad5); nyb= -sin(rad1)*sin(rad2)*sin(rad3)*cos(rad4)*cos(rad5); nyc= -sin(rad1)*cos(rad2)*sin(rad3)*sin(rad4)*cos(rad5); nyd= -sin(rad1)*sin(rad2)*cos(rad3)*sin(rad4)*cos(rad5); nye= -cos(rad1)*sin(rad5); ny= (nya+nyb+nyc+nyd+nye)*fen; nza= -sin(rad2)*cos(rad3)*cos(rad4)*cos(rad5); nzb= -cos(rad2)*sin(rad3)*cos(rad4)*cos(rad5); nzc= sin(rad2)*sin(rad3)*sin(rad4)*cos(rad5); nzd= -cos(rad2)*cos(rad3)*sin(rad4)*cos(rad5); nz= (nza+nzb+nzc+nzd)*-fen; %negra line ([x4,x4+nx],[y4,y4+ny],[z4,z4+nz],'LineWidth',2.5,'Color',[0 0 0]) %Vector S fes=1.5;%Factor de escalamiento sxa= -cos(rad1)*cos(rad2)*cos(rad3)*cos(rad4)*sin(rad5); sxb= cos(rad1)*sin(rad2)*sin(rad3)*cos(rad4)*sin(rad5); sxc= cos(rad1)*cos(rad2)*sin(rad3)*sin(rad4)*sin(rad5); sxd= cos(rad1)*sin(rad2)*cos(rad3)*sin(rad4)*sin(rad5); sxe= -sin(rad1)*cos(rad5); sx= (sxa+sxb+sxc+sxd+sxe)*fes; sya= -sin(rad1)*cos(rad2)*cos(rad3)*cos(rad4)*sin(rad5); syb= sin(rad1)*sin(rad2)*sin(rad3)*cos(rad4)*sin(rad5); syc= sin(rad1)*cos(rad2)*sin(rad3)*sin(rad4)*sin(rad5); syd= sin(rad1)*sin(rad2)*cos(rad3)*sin(rad4)*sin(rad5); sye= cos(rad1)*cos(rad5); sy= (sya+syb+syc+syd+sye)*fes; sza= sin(rad2)*cos(rad3)*cos(rad4)*sin(rad5); szb= cos(rad2)*sin(rad3)*cos(rad4)*sin(rad5); szc= -sin(rad2)*sin(rad3)*sin(rad4)*sin(rad5); szd= cos(rad2)*cos(rad3)*sin(rad4)*sin(rad5); sz= (sza+szb+szc+szd)*-fes; %verde line ([x4,x4-sx*2],[y4,y4-sy*2],[z4,z4-sz*2],'LineWidth',2.5,'Color',[0 1 0]) %gris line ([x4,x4+sx],[y4,y4+sy],[z4,z4+sz],'LineWidth',6,'Color',[0.5 0.5 0.5]) %gris line ([x4,x4-sx],[y4,y4-sy],[z4,z4-sz],'LineWidth',6,'Color',[0.5 0.5 0.5]) %Vector A fea=3.5;%Factor de escalamiento axa= cos(rad1)*cos(rad2)*cos(rad3)*sin(rad4); axb= -cos(rad1)*sin(rad2)*sin(rad3)*sin(rad4); axc= cos(rad1)*cos(rad2)*sin(rad3)*cos(rad4); axd= cos(rad1)*sin(rad2)*cos(rad3)*cos(rad4); Laboratorio de Robots Mviles Autnomos LARMA ax= (axa+axb+axc+axd)*fea; Gonzalo Berardo aya= sin(rad1)*cos(rad2)*cos(rad3)*sin(rad4);
axc= cos(rad1)*cos(rad2)*sin(rad3)*cos(rad4); axd= cos(rad1)*sin(rad2)*cos(rad3)*cos(rad4); ax= (axa+axb+axc+axd)*fea; aya= sin(rad1)*cos(rad2)*cos(rad3)*sin(rad4); ayb= -sin(rad1)*sin(rad2)*sin(rad3)*sin(rad4); ayc= sin(rad1)*cos(rad2)*sin(rad3)*cos(rad4); ayd= sin(rad1)*sin(rad2)*cos(rad3)*cos(rad4); ay= (aya+ayb+ayc+ayd)*fea; aza= -sin(rad2)*cos(rad3)*sin(rad4); azb= -cos(rad2)*sin(rad3)*sin(rad4); azc= -sin(rad2)*sin(rad3)*cos(rad4); azd= cos(rad2)*cos(rad3)*cos(rad4); az= (aza+azb+azc+azd)*-fea; %amarillo line([x4,x4+ax*1.2],[y4,y4+ay*1.2],[z4,z4+az*1.2],'LineWidth',2.5,'Color', [1 1 0]) %tomando de origen el vector S verde %lnea gris (Dedo 1) line ([x4-sx,x4+ax-sx],[y4-sy,y4+ay-sy],[z4sz,z4+az+sz],'LineWidth',6,'Color',[0.5 0.5 0.5]) %tomando de origen el vector S verde %lnea gris (Dedo 2) line([x4+sx,x4+ax+sx],[y4+sy,y4+ay+sy],[z4+sz,z4+az+sz],'LineWidth',6,'Col or',[0.5 0.5 0.5]) %Conversiones de radianes a grados tetac1=(teta1*180)/pi; tetah1=(teta2*180)/pi; tetaco1=(teta3*180)/pi; %Cuando los ngulos son mayores a 360 while tetac1>360 tetac1=tetac1-360; end while tetah1>360 tetah1=tetah1-360; end while tetaco1>360 tetaco1=tetaco1-360; end %Cuando los ngulos son mayores a -360 while tetac1<-360 tetac1=tetac1+360; end while tetah1<-360 tetah1=tetah1+360; end while tetaco1<-360 tetaco1=tetaco1+360; end fprintf('\n Cuando los ngulos calculados se encuentran en el segundo '); fprintf('\n cuadrante, el ngulo mostrado es el complementario\n'); fprintf('\n Los valores de los ngulos en codo arriba son:\n'); fprintf('\tCintura =%12.2f\n',tetac1); fprintf('\tHombro =%12.2f\n',tetah1); fprintf('\tCodo =%12.2f\n',tetaco1); fprintf('\n Recuerde que los lmites fsicos (en grados) en el robot son:'); fprintf('\n\t Cintura de 0 a 310'); fprintf('\n\t Hombro de -35 a 130'); LARMA Laboratorio de Robots Mviles Autnomos fprintf('\n\t Codo de -130 a 130'); Gonzalo Berardo hold off %Desactiva grficos

fprintf('\n\t Hombro de -35 a 130'); fprintf('\n\t Codo de -130 a 130'); hold off %Desactiva grficos break%Finaliza el programa end teta2=beta-alfa; if x<0 teta2=beta-alfa+180; end a=link1*sin(teta2); b=link1*cos(teta2); c=rxy-b; d=z-a; w=atan(d/c); teta3=w-teta2; bx=b*cos(teta1); by=b*sin(teta1); %Configuracin codo abajo x1=0; y1=0; z1=0; title ('SIMULACIN en Configuracin Codo Abajo'); line ([x1,bx],[y1,by],[z1,a],'LineWidth',8,'Color',[0 0 1]) %Clculo y graficacin del link2 %Codo-mueca x3=bx; y3=by; z3=a; line ([x3,x],[y3,y],[z3,z],'LineWidth',8,'Color',[1 0 0]) %Graficacin del gripper rad1=teta1; rad2=teta2; rad3=teta3; rad4=teta4; rad5=teta5; x4=x; y4=y; z4=z; %Vector N fen=2;%Factor de escalamiento nxa= cos(rad1)*cos(rad2)*cos(rad3)*cos(rad4)*cos(rad5); nxb= -cos(rad1)*sin(rad2)*sin(rad3)*cos(rad4)*cos(rad5); nxc= -cos(rad1)*cos(rad2)*sin(rad3)*sin(rad4)*cos(rad5); nxd= -cos(rad1)*sin(rad2)*cos(rad3)*sin(rad4)*cos(rad5); nxe= -sin(rad1)*sin(rad5); nx= (nxa+nxb+nxc+nxd+nxe)*fen; nya= sin(rad1)*cos(rad2)*cos(rad3)*cos(rad4)*cos(rad5); nyb= -sin(rad1)*sin(rad2)*sin(rad3)*cos(rad4)*cos(rad5); nyc= -sin(rad1)*cos(rad2)*sin(rad3)*sin(rad4)*cos(rad5); nyd= -sin(rad1)*sin(rad2)*cos(rad3)*sin(rad4)*cos(rad5); nye= -cos(rad1)*sin(rad5); ny= (nya+nyb+nyc+nyd+nye)*fen; nza= -sin(rad2)*cos(rad3)*cos(rad4)*cos(rad5); nzb= -cos(rad2)*sin(rad3)*cos(rad4)*cos(rad5); nzc= sin(rad2)*sin(rad3)*sin(rad4)*cos(rad5); nzd= -cos(rad2)*cos(rad3)*sin(rad4)*cos(rad5); nz= (nza+nzb+nzc+nzd)*-fen; %negra line ([x4,x4+nx],[y4,y4+ny],[z4,z4+nz],'LineWidth',2.5,'Color',[0 0 0]) %Vector S fes=1.5;%Factor de escalamiento LARMA Laboratorio de Robots Mviles Autnomos sxa= -cos(rad1)*cos(rad2)*cos(rad3)*cos(rad4)*sin(rad5); Gonzalo Berardo sxb= cos(rad1)*sin(rad2)*sin(rad3)*cos(rad4)*sin(rad5);
%negra line ([x4,x4+nx],[y4,y4+ny],[z4,z4+nz],'LineWidth',2.5,'Color',[0 0 0]) %Vector S fes=1.5;%Factor de escalamiento sxa= -cos(rad1)*cos(rad2)*cos(rad3)*cos(rad4)*sin(rad5); sxb= cos(rad1)*sin(rad2)*sin(rad3)*cos(rad4)*sin(rad5); sxc= cos(rad1)*cos(rad2)*sin(rad3)*sin(rad4)*sin(rad5); sxd= cos(rad1)*sin(rad2)*cos(rad3)*sin(rad4)*sin(rad5); sxe= -sin(rad1)*cos(rad5); sx= (sxa+sxb+sxc+sxd+sxe)*fes; sya= -sin(rad1)*cos(rad2)*cos(rad3)*cos(rad4)*sin(rad5); syb= sin(rad1)*sin(rad2)*sin(rad3)*cos(rad4)*sin(rad5); syc= sin(rad1)*cos(rad2)*sin(rad3)*sin(rad4)*sin(rad5); syd= sin(rad1)*sin(rad2)*cos(rad3)*sin(rad4)*sin(rad5); sye= cos(rad1)*cos(rad5); sy= (sya+syb+syc+syd+sye)*fes; sza= sin(rad2)*cos(rad3)*cos(rad4)*sin(rad5); szb= cos(rad2)*sin(rad3)*cos(rad4)*sin(rad5); szc= -sin(rad2)*sin(rad3)*sin(rad4)*sin(rad5); szd= cos(rad2)*cos(rad3)*sin(rad4)*sin(rad5); sz= (sza+szb+szc+szd)*-fes; %verde line ([x4,x4-sx*2],[y4,y4-sy*2],[z4,z4-sz*2],'LineWidth',2.5,'Color',[0 1 0]) %gris line ([x4,x4+sx],[y4,y4+sy],[z4,z4+sz],'LineWidth',6,'Color',[0.5 0.5 0.5]) %gris line ([x4,x4-sx],[y4,y4-sy],[z4,z4-sz],'LineWidth',6,'Color',[0.5 0.5 0.5]) %Vector A fea=3.5;%Factor de escalamiento axa= cos(rad1)*cos(rad2)*cos(rad3)*sin(rad4); axb= -cos(rad1)*sin(rad2)*sin(rad3)*sin(rad4); axc= cos(rad1)*cos(rad2)*sin(rad3)*cos(rad4); axd= cos(rad1)*sin(rad2)*cos(rad3)*cos(rad4); ax= (axa+axb+axc+axd)*fea; aya= sin(rad1)*cos(rad2)*cos(rad3)*sin(rad4); ayb= -sin(rad1)*sin(rad2)*sin(rad3)*sin(rad4); ayc= sin(rad1)*cos(rad2)*sin(rad3)*cos(rad4); ayd= sin(rad1)*sin(rad2)*cos(rad3)*cos(rad4); ay= (aya+ayb+ayc+ayd)*fea; aza= -sin(rad2)*cos(rad3)*sin(rad4); azb= -cos(rad2)*sin(rad3)*sin(rad4); azc= -sin(rad2)*sin(rad3)*cos(rad4); azd= cos(rad2)*cos(rad3)*cos(rad4); az= (aza+azb+azc+azd)*-fea; %amarillo line([x4,x4+ax*1.2],[y4,y4+ay*1.2],[z4,z4+az*1.2],'LineWidth',2.5,'Color', [1 1 0]) %tomando de origen el vector S verde %lnea gris (Dedo 1) line ([x4-sx,x4+ax-sx],[y4-sy,y4+ay-sy],[z4sz,z4+az+sz],'LineWidth',6,'Color',[0.5 0.5 0.5]) %tomando de origen el vector S verde %lnea gris (Dedo 2) line([x4+sx,x4+ax+sx],[y4+sy,y4+ay+sy],[z4+sz,z4+az+sz],'LineWidth',6,'Col or',[0.5 0.5 0.5]) %Conversiones de radianes a grados tetac=(teta1*180)/pi; LARMA Laboratorio de Robots Mviles Autnomos tetah=(teta2*180)/pi; Gonzalo Berardo tetaco=(teta3*180)/pi;

,[0.5 0.5 0.5]) %Conversiones de radianes a grados tetac=(teta1*180)/pi; tetah=(teta2*180)/pi; tetaco=(teta3*180)/pi; %Cuando los ngulos son mayores a 360 while tetac>360 tetac=tetac-360; end while tetah>360 tetah=tetah-360; end while tetaco>360 tetaco=tetaco-360; end %Cuando los ngulos son mayores a -360 while tetac<-360 tetac=tetac+360; end while tetah<-360 tetah=tetah+360; end while tetaco<-360 tetaco=tetaco+360; end fprintf('\n Cuando los ngulos calculados se encuentran en el segundo '); fprintf('\n cuadrante, el ngulo mostrado es el complementario\n'); fprintf('\n Los valores de los ngulos en codo abajo son:\n'); fprintf('\tCintura =%12.2f\n',tetac); fprintf('\tHombro =%12.2f\n',tetah); fprintf('\tCodo =%12.2f\n',tetaco); fprintf('\n Recuerde que los lmites fsicos (en grados) en el robot son:'); fprintf('\n\t Cintura de 0 a 310'); fprintf('\n\t Hombro de -35 a 130'); fprintf('\n\t Codo de -130 a 130'); hold off %Desactiva grficos

Resultado:
SIMULACIN DE LA CINEMTICA INVERSA

DOCUMENTOS DE REFERENCIA http://www.automatas.org/hardware/teoria_pid.htm Caracterizacin de un Robot CENIDET Introduccin a la Robtica - Humberto A. Secchi - Instituto de Automtica - Universidad Nacional de San Juan Ingenieria de Control Moderna - K. Ogata Sistemas de control moderno 10a Ed. - Dorf y Bishop

Trabajo Final Robot Manipulador Mentor (Educacional)

Transféré par

Informations du document

Copyright

Formats disponibles

Partager ce document

Partager ou intégrer le document

Options de partage

Avez-vous trouvé ce document utile ?

Ce contenu est-il inapproprié ?

Droits d'auteur :

Formats disponibles

Trabajo Final Robot Manipulador Mentor (Educacional)

Transféré par

Droits d'auteur :

Formats disponibles

UNIVERSIDAD TECNOLOGICA NACIONAL REGIONAL MENDOZA LARMA

ROBOT MANIPULADOR MENTOR

2 ROBOT MANIPULADOR MENTOR

LARMA Laboratorio de Robots Mviles Autnomos Gonzalo Berardo

3 ROBOT MANIPULADOR MENTOR

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

LARMA Laboratorio de Robots Mviles Autnomos Gonzalo Berardo

4 ROBOT MANIPULADOR MENTOR

Figura 1. Diagrama en bloque general.

LARMA Laboratorio de Robots Mviles Autnomos Gonzalo Berardo

5 ROBOT MANIPULADOR MENTOR

Figura 2. Diagrama de bloques del sistema controlado

LARMA Laboratorio de Robots Mviles Autnomos Gonzalo Berardo

6 ROBOT MANIPULADOR MENTOR

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

LARMA Laboratorio de Robots Mviles Autnomos Gonzalo Berardo

7 ROBOT MANIPULADOR MENTOR

LARMA Laboratorio de Robots Mviles Autnomos Gonzalo Berardo

8 ROBOT MANIPULADOR MENTOR

Figura 2. Diagrama de bloques del sistema controlado

LARMA Laboratorio de Robots Mviles Autnomos Gonzalo Berardo

9 ROBOT MANIPULADOR MENTOR

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

Figura 3. Motor - Potencimetro.

LARMA Laboratorio de Robots Mviles Autnomos Gonzalo Berardo

10 ROBOT MANIPULADOR MENTOR

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

El esquema habitual de uso del controlador PID es:

Figura 4. Control PID de una planta

LARMA Laboratorio de Robots Mviles Autnomos Gonzalo Berardo

11 ROBOT MANIPULADOR MENTOR

1 u(t ) Planta c(t)

Figura 5. Respuesta a un escaln unitario de una planta

Recta tangente al punto de inflexin

Los parmetros se pueden obtener de esta respuesta:

Figura 6. Respuesta de salida a una entrada escaln

LARMA Laboratorio de Robots Mviles Autnomos Gonzalo Berardo

12 ROBOT MANIPULADOR MENTOR

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

de acuerdo con la formula que

Tipo de Controlador P PI PID

LARMA Laboratorio de Robots Mviles Autnomos Gonzalo Berardo

13 ROBOT MANIPULADOR MENTOR

Figura 7. Respuesta al escaln

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

LARMA Laboratorio de Robots Mviles Autnomos Gonzalo Berardo

14 ROBOT MANIPULADOR MENTOR

% Retardo % Constante de tiempo

El algoritmo nos arroja los resultados de: ; ;

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

LARMA Laboratorio de Robots Mviles Autnomos Gonzalo Berardo

15 ROBOT MANIPULADOR MENTOR

Figura 11. Constelacin de polos y ceros

Polos en LC: -3.7528 + 4.9994i ; -3.7528 - 4.9994i ; -2.6280

16 ROBOT MANIPULADOR MENTOR

Figura 12. Respuesta al escaln del PID

LARMA Laboratorio de Robots Mviles Autnomos Gonzalo Berardo

17 ROBOT MANIPULADOR MENTOR

Figura 13. Refinamiento del PID OBSERVACION ES:

18 ROBOT MANIPULADOR MENTOR

Donde: para T=300

LARMA Laboratorio de Robots Mviles Autnomos Gonzalo Berardo

19 ROBOT MANIPULADOR MENTOR

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