Story Transcript
MODELAMIENTO DE LA CINEMATICA Y DINAMICA DE UN ROBOT MANIPULADOR DE CUATRO GRADOS DE LIBERTAD TIPO SCARA PARA EL DISEÑO E IMPLEMENTACION EN SIMULACIÓON DE UN CONTROLADOR LINEAL POR RETROALIMENTACIÓON TIPO CTC (COMPUTER TORQUE CONTROL)
ING. GUILLERMO ELIECER VALENCIA OCHOA
UNIVERSIDAD DEL NORTE DIVISIÓON DE INGENIERÍAS DEPARTAMENTO DE INGENIERÍA MECÁNICA MAESTRÍA EN INGENIERÍA MECÁNICA BARRANQUILLA 2008
i
Con formato
MODELAMIENTO DE LA CINEMATICA Y DINAMICA DE UN ROBOT MANIPULADOR DE CUATRO GRADOS DE LIBERTAD TIPO SCARA PARA EL DISEÑO E IMPLEMENTACION EN SIMULACIÓON DE UN CONTROLADOR LINEAL POR RETROALIMENTACIÓON TIPO CTC (COMPUTER TORQUE CONTROL)
ING. GUILLERMO ELIECER VALENCIA OCHOA.
Monografía presentada como requisito para optar al título de Magíster en Ingeniería Mecánica
DIRECTOR: ING. MARCO SANJUAN MEJIA, Ph. D
UNIVERSIDAD DEL NORTE DIVISIÓON DE INGENIERÍAS DEPARTAMENTO DE INGENIERÍA MECÁNICA MAESTRÍA EN INGENIERÍA MECÁNICA BARRANQUILLA 2008
ii
Aprobado por la División de Ingenierías en cumplimiento de los requisitos exigidos para otorgar el título de Magíster en Ingeniería Mecánica.
Director del Proyecto
Miembro del Comité
Miembro del Comité
Miembro del Comité
iii
Dedicatoria:
A Dios, por la oportunidad que me da para vivir y realizar mis sueños. A mi Padres Omaira y Guillermo, por todos sus esfuerzos para hacerme un hombre de bien. A mis hermanos, Geova y Jair por su total apoyo durante todo este tiempo. A toda mi familia porque cuando se quiere se puede.
Con formato: Sangría: Izquierda: 0 cm
iv
Con formato
AGRADECIMIENTOS
El autor expresa su agradecimiento:
Al DEPARTAMENTO DE INGENIERÍIA MECANICA MECÁNICA - UNINORTE, por permitirme ser parte de su familia y realizar todos mis estudios de Pregrado y Maestría en Ingeniería Mecánica.
A MARCO SANJUAN, por ser más que mi Tutor y Jefe, la persona que me ha brindado consejos personales y oportunidades profesionales en la Universidad del Norte. ¡Marco muchas gracias!
A ANTONIO BULA, por la oportunidad de acompañarlo en investigación en el Laboratorio de Maquinas Máquinas Hidráulicas adscrito al programa de Ingeniería Mecánica de la Universidad del Norte, en el área de Transferencia de Calor de Nanofluido y en general por toda su valiosa colaboración durante este proceso.
A mis amigos de la MAESTRÌA EN INGENIERÌA MECÁANICA: MIGUEL RAMOS, LUIS MORALES, PEDRO VILLALBA, OSCAR ÁLVAREZ, CARLOS CUBAS; personas sencillamente excepcionales de los cuales aprendí tanto profesional como personalmente. Recuerden que la riqueza de una persona se mide por la cantidad y calidad de sus amigos.! ¡Gracias por ser parte de mi fortuna…! Al Laboratorio de “Fluidos” de la Universidad del Norte, por la gran hospitalidad que cada uno de sus integrantes tuvieron conmigo en el curso de mis Estudios de Postgrado.
v
TABLA DE CONTENIDO
AGRADECIMIENTOS ............................................................................................ V LISTA DE FIGURAS ........................................................................................... VIII LISTA DE TABLAS ................................................................................................ X RESUMEN ............................................................................................................. 12 1.
INTRODUCCIÓN ........................................................................................14 1.1. Antecedentes .......................................................................................14 1.2. Planteamiento del Problema e Hipótesis .............................................. 18 1.3. Justificación ..........................................................................................20 1.4. Objetivos ..............................................................................................20 1.4.1. Objetivos Generales .......................................................................... 21 1.4.2. Objetivos Específicos ........................................................................ 21 1.5. Metodología de la investigación ...........................................................21 1.6. Contribuciones .....................................................................................25 1.7. Organización del Documento ...............................................................25
2.
MODELADO CINEMÁTICO DEL ROBOT INDUSTRIAL ...........................27 2.1 Introducción. ..........................................................................................27 2.2 Descripción del Robot. .......................................................................... 28 2.3 Modelado de la Cinemática de Robots ..................................................29 2.4 Cinemática Directa del Robot SCARA...................................................33 2.5 Cinemática Inversa del Robot SCARA ..................................................37
3.
MODELADO DINÁMICO DEL ROBOT INDUSTRIAL ................................46 3.1 Dinámica del Robot manipulador. .........................................................46 3.2 Formulación Newton-Euler. ................................................................... 47
vi
3.3 Algoritmo computacional para determinar el modelo dinámico por Newton-Euler...............................................................................................50 3.4 Modelo dinámico del Robot SCARA. .....................................................54 3.5 Modelo dinámico de los motores DC .....................................................62 4.
CONTROL DINÁMICO DEL ROBOT INDUSTRIAL ................................... 65 4.1 Introducción. ..........................................................................................65 4.2 Computer Torque Control CTC. ............................................................66 4.2.1 Derivación de Lazo Interior por retroalimentación ..............................66 4.2.2 Diseño del lazo externo (PD) ..............................................................70 4.2.3 Selección de las ganancias (PD) ........................................................72 4.2.4 Respuesta del Computer Torque Control. .......................................... 74
5.
RESULTADOS Y ANÁLISIS DE RESULTADOS ....................................... 80
CONCLUSIONES .................................................................................................. 88 TRABAJO FUTURO ..............................................................................................90 REFERENCIAS ..................................................................................................... 91 ANEXOS ................................................................................................................ 96
Con formato: Normal, Izquierda
vii
LISTA DE FIGURAS
Figura 1. Manipulador SCARA .............................................................................. 28 Figura 2 Diagrama de Denavit-Hartemberg para la obtención de la cinemática .... 30 Figura 3 Sistema de coordenadas según la convención D-H ................................34 Figura 4. Grafica de 3D de las posiciones cartesiana deseadas para la trayectoria 1 .............................................................................................................................40 Figura 5. Posiciones cartesianas deseadas en el tiempo para la trayectoria 1 ...... 41 Figura 6 Posiciones articulares deseadas en el tiempo para la trayectoria 1 ........ 42 Figura 7. Posiciones cartesiana deseadas para la trayectoria 2 ............................43 Figura 8. Sistema de coordenadas según la convención D-H ...............................44 Figura 9. Sistema de coordenadas según la convención D-H ...............................45 Figura 10 Diagrama de relación entre Dinámica Directa e Inversa ........................47 Figura 11 Diagrama de cuerpo libre del eslabón i del robot ................................... 48 Figura 12 Referencia de la masa de una partícula p ............................................. 49 Figura 13. Comportamiento de los torques en el tiempo para la trayectoria 1 ....... 60 Figura 14. Comportamiento de los torques en el tiempo para la trayectoria 2 ....... 61 Figura 15. Comportamiento de los torques en t=1.5 seg para la trayectoria 2....... 61 Figura 16. Comportamiento de los torques en t= 0 seg para la trayectoria 2........ 62 Figura 17 Representación eléctrica de los motores DC del SCARA .....................63 Figura 18 Esquema de Computer Torque Control, mostrando a lazos internos y externos .................................................................................................................69 Figura 19 Esquema de control PID por par calculado............................................ 71 Figura 20. Comparación entre q real y q deseado para la Trayectoria 1 ............... 75 Figura 21. Comparación de q real y q deseado para la Trayectoria 1 en cada una de las zonas ........................................................................................................... 76 Figura 22. Comparación entre q real y q deseado para la Trayectoria 2 ............... 77 Figura 23. Comparación de q real y q deseado para la Trayectoria 2 en cada una de las zonas ........................................................................................................... 78 Figura 24. Posición real y Deseado para la Trayectoria 2 en el espacio de la tarea ...............................................................................................................................79 Figura 25 Simulación del Robot SCARA de Cuatro Grados de Libertad ............... 81
viii
Figura 26. Integral del Valor Absoluto del error en el tiempo real ..........................82 Figura 27. Errores articulares para la trayectoria 1 ................................................ 83 Figura 28. Errores cartesianos para la trayectoria 1 .............................................. 84 Figura 29. Errores cartesianos para la trayectoria 1 .............................................. 85 Figura 30 Errores cartesianos para la trayectoria 2 ............................................... 86 Figura 31. Errores articulares para la trayectoria 2 ................................................ 87
ix
LISTA DE TABLAS
PAG
Tabla 1 Parámetros D-H del robot SCARA ............................................................34 Tabla 2 Parámetros de robot SCARA .................................................................... 59 Tabla 3 Parámetros de sintonización del Controlador CTC ................................... 74 Tabla 4 Índices de desempeño para el Controlador CTC ...................................... 88
x
LISTA DE ANEXOS
PAG ANEXO 1. CODIGOS PARA LA GENERACIÓN DE TRAYECTORIAS................ 96 ANEXO 2. PROGRAMA DE INICIALIZACIÓN DEL CONTROLADOR CTC ........ 97 ANEXO 3. PROGRAMAS DEL MODELO CINEMÁTICO ..................................... 98 ANEXO 4. PROGRAMAS DEL MODELO DINÁMICO .......................................... 99 ANEXO 5. PROGRAMA PARA EL CALCULO DE LA D.I. ROBOT HASTA 5 DOF .............................................................................................................................107
xi
RESUMEN
Esta investigación muestra detalladamente el modelo cinemático de un robot tipo SCARA, basado en la determinación de la dirección del elemento terminal del robot a partir de las variables de cada junta. Una parte importante de esta sección es el uso de la representación de Denavit–Hartenberg [6]. Adicionalmente, se muestra el estudio de la relación matemática entre el movimiento del robot y las fuerzas implicadas en el mismo, lo cual representa el modelo dinámico directo e inverso del manipulador.
Posteriormente se presenta la implementación de un controlador basado en el modelo de referencia, en nuestro caso el Computer Torque Control (CTC), donde el sistema dinámico se linealiza y se desacopla por realimentación, lo que transforma el sistema dinámico del manipulador a controlar en un simple par de integradores. El controlador estudiado se simulo sobre el modelo del robot SCARA con valores numéricos reales. Las simulaciones realizadas permiten valorar las respuestas del controlador CTC en seguimiento de trayectorias deseadas complejas muy comunes en los procesos de manufactura, como lo son un cambio súbito de dirección en el elemento terminal que se experimenta en la trayectoria 2, además de un cambio de dirección suave junto a una excitación muy fuerte en el eje z, como lo es la trayectoria 1.
Los resultados simulados en la presente investigación se obtuvieron sobre el modelo de un robot de cuatro grados de libertad (GDL), tipo SCARA. El desempeño del controlador basado en el modelo de referencia (CTC), se midió calculando la integral absoluta del error (IAE) para dos trayectorias deseadas. En general, se puede decir que para las dos trayectorias se puede utilizar un
12
controlador lineal por retroalimentación con compensador basado en el modelo dinámico no lineal tipo CTC (Computer Torque Control) con un índice de desempeño del orden de 1e-005, debido a que se obtuvieron errores absolutos de 5.2694e-005 rad en el espacio articular y 2.3544e-005 m en el espacio cartesiano para la trayectoria 1 y errores absolutos del orden de 0.0026619 rad en el espacio articular y 0.0019172 m en el espacio cartesiano para la trayectoria 2.
Con formato: Normal, Izquierda, Sangría: Izquierda: 0 cm, Primera línea: 0 cm, No ajustar espacio entre texto latino y asiático, No ajustar espacio entre texto asiático y números
13
Capítulo I
Con formato: Normal, Derecha, Sangría: Izquierda: 0 cm, Primera línea: 0 cm, No ajustar espacio entre texto latino y asiático, No ajustar espacio entre texto asiático y números Con formato: Normal, No ajustar espacio entre texto latino y asiático, No ajustar espacio entre texto asiático y números Con formato: Normal Con formato: Título 1, Centrado
1. NTRODUCCIÓN
1.1. Antecedentes En un esfuerzo por encontrar soluciones a los problemas relacionados a mejorar el rendimiento de controladores en el seguimiento de trayectorias, se han propuesto una gran variedad de algoritmos y métodos de control en aplicaciones de robótica y áreas relacionadas. En general, éstos han sido clasificados en dos categorías:
Métodos
de
control
convencionales:
Proporcional
Derivativo
(PD),
Proporcional Integral Derivativo (PID), Computer Torque Control (CTC). Métodos avanzados de control: Control Adaptativo (AC), Control Robusto (RC), Control Robusto Adaptativo (ARC).
Métodos de control convencionales. En el campo de la robótica, los robot manipuladores fueron inicialmente concebidos como un conjunto de n sistemas independientes (p. ej., n juntas independientes)
[6],[32].
Por
consiguiente,
las
reacciones
entre
juntas
consecutivas son ignoradas o consideradas como alteraciones externas. La tarea de control en este acercamiento es diseñar un controlador para cada junta como un sistema una-entrada/una-salida. Esto es, el manipulador dinámico es reducido
14
a un sistema de segundo orden lineal acoplado. Por lo tanto, métodos de control tradicionales con controladores lineales son sugeridos para reconocer los requerimientos del sistema. Algunos ejemplos de estos controladores tradicionales son PI, PID como se menciona en [32],[39][36]. De cualquier manera se ha encontrado que en la realidad la dinámica de manipuladores es mucho masmás compleja que la dinámica asumida por el acercamiento (SISO) [32]. La dinámica de manipuladores incluye no sólo efectos dobles acoplados sino también otros términos no lineales como la fricción y la gravedad, que explican las falencias de diseñan diseñar métodos de control SISO en muchos casos. Estas falencias ocurren regularmente cuando se incurre en ignorar términos no lineales (p. ej., incrementando la velocidad de los manipuladores, trabajando en condiciones severas) como en [32],[3]. Al respecto, muchos trabajos se han hecho al respecto para compensar las incertidumbres y las no linealidades. Por un lado, los controladores tradicionales como los PD, y PID fueron modificados para compensar las no linealidades de los sistemas, como en [20]. Por otro lado, aparecen nuevos controladores, llamados CTC [44],[24]. Sin embargo, en [44] se trabaja con un Modelo Dinámico Inverso simplificado, el error de seguimiento no se evalúa en términos de IAE(Integral Absolute Error) cartesiano ni articular, debido a que se determina la robustez del controlador por medio de rechazo de perturbaciones y la presencia de errores en el modelado. En estos, la linealización por realimentación no lineal [32],[22],[19] ha sido propuesta para dar solución a estas dificultades. Aunque algunas mejoras fueron reportadas, se levantaron inconvenientes debido a los requerimientos de conocimientos acerca de la dinámica del manipulador. En consecuencia, la comunidad científica dedicada al control de manipuladores ha avanzado hacia controladores con mejor rendimiento Con formato: Español (Colombia)
pero también con mayor complejidad.
Métodos de control Avanzados. En control robótico, controladores avanzados tales como Adaptative Control (AC) [17],[30], Robust Control (RC) [15],[43], Adaptative Robust Control (ARC)
15
[14],[28],[34], han estado altamente ligados con incertidumbres paramétricas. De cualquier manera, su rápido desarrollo aconteció recientemente cuando el conocimiento teórico acerca de los sistemas no lineales y el entendimiento de la dinámica de los sistemas de control robótico, llego a ser mejor explorado con avances en computación y tecnología de microprocesadores [1],[2],[3]. Las ultimasúltimas dos décadas, tuvieron como testigo una gran variedad de nuevos métodos de control modernos como los AC, RC, ARC, en sistemas no lineales en general (p. ej., [14],[17],[28],[34]) y en sistemas robóticos en particular (p. ej., [41],[43],[30],[15]).
En el desarrollo inicial del control robótico adaptativo, solo sólo algunos controladores robóticos adaptativos fueron propuestos debido a la complejidad de la dinámica de los robots. Uno de los primeros algoritmos es el “Model Reference Adaptative Systems” (MRAS) presentado en [10]. Éste es diseñado bajo el supuesto de que cada junta del robot manipulador es un sistema de segundo orden lineal independiente. En [4],[23],[25], los autores corrigieron este algoritmo considerando el modelo de dinámica comprehensiva de un sistema robótico y sus propiedades. Un gran paso adelante del control robótico adaptativo se dio dió a principios de la década de los 80s cuando la propiedad “lineal en parámetros” fue fué reconocida y aplicada por Slotine y Li [36], y Craig [7] para diseñar con un amplio conocimiento el modelo basado en controladores adaptativos. Desde entonces, muchos otros controladores adaptativos [24],[45],[30],[18],[21] han sido propuestos en la literatura robótica. De cualquier manera, algunos autores en [12],[28],[29] señalaron algunas desventajas de estos controladores adaptativos como la sensibilidad al ruido, y un rendimiento en transitorio indeseado. Como una mejora, los autores modificaron algunas leyes de los controladores adaptativos existentes en función de satisfacer la hasta entonces inmoderada dinámica de los robots y las alteraciones externas.
16
En general los resultados obtenidos con AC indican que el rendimiento es pobre e incierto en la iniciativa de medir seguimiento de trayectoria. Métodos AC son capaces de proveer un rendimiento estable en plataforma, es decir, el resultado más pobre del rendimiento en traansitorio de de información que los controladores adaptativos usualmente es proyectadose presenta cuando solo en el caso ideal hay de incertidumbres paramétricas y su rendimiento en transito depende de parámetros estimados y condiciones iniciales. Durante la plataforma en transitotransitorio, especialmente en el inicio, los parámetros son pobremente estimados desde el proceso de estimación de parámetros que toma cierta cantidad de tiempo en converger. Para las condiciones iniciales, hay algunas desviaciones con las condiciones deseadas que son preescritas por una trayectoria ideal. Esta incertidumbre de parámetros y condiciones de estado inicial son la causa de un pobre bajo rendimiento en transito de los sistemas de control adaptativo. Además, cuando las alteraciones perturbaciones externas son tenidas en cuenta no es posible evitar un decrecimiento asintótico de la estabilidad. La razón es debida a la derivación de parámetros como resultado de la introducción de alteraciones influencia de perturbaciones externas. Si estas alteraciones perturbaciones externas son considerables, éstas pueden causar inestabilidad en el sistema. Estos problemas son bien conocidos y descritos en [28],[23],[22].
El método de Control por Modos Deslizantes (SMC, por sus siglas en inglés) es un popular método de control robusto, que puede trabajar de manera satisfactoria con la mayoría de las debilidades de los controles adaptativos. El uso inicial del modo de control móvil es elevar la fortaleza de los sistemas de control robótico, acorde con las incertidumbres originadas en [38],[35],[42]. Dicho método ha sido mejorado por otros autores [46],[31],[41], especialmente para cubrir enlaces y el control prolongado de entradas que los controladores móviles clásicos no cubrían. En los controladores móviles clásicos, errores en curso eran minimizados gracias a la aplicación de una entrada de control cambiante. Como un resultado durante la plataforma en tránsito cuando grandes incertidumbres se presentan, los sistemas
17
son fuertes y presentan un buen rendimiento en tránsito. Debido a la ausencia de parámetros de adaptación, RC en general y SMC en particular no discriminan entre alteraciones externas e incertidumbres paramétricas y la plataforma estable. Esto es, RC produce un rendimiento similar en ambas operaciones.
Por otra parte, desde hace muchos años se han realizado estudios alrededor del Control Predictivo Basado en Modelos (MPC), a finales de los años setenta surgieron los primeros algoritmos que usaban explícitamente un modelo dinámico del proceso para predecir el efecto de las acciones de control futuras en la salida. Desde entonces se han adelantado investigaciones acerca de los diferentes tipos de control predictivo, entre los cuales se destacan el GPC (Generalizad Predictive Control) [4] y el DMC (Dynamic Matrix Control) [11] ],[24]. 1.2. Planteamiento del Problema e Hipótesis Como se ha mencionado en el inciso 1.1, el desempeño de los Manipuladores Industriales de varios grados de libertad en aplicaciones donde se requiere el control continuo de su posición en el espacio articular y cartesiano en todo el espacio de tarea, no responde a las expectativas de los desarrolladores en cuanto a error de seguimiento. Esto se debe a que la dinámica de dichos Manipuladores Industriales varía según su posición, lo cual provoca serias perturbaciones al sistema. Características como las no linealidades en los vectores de centros de masa si , vectores de parámetros cinemáticos pi , provocan el comportamiento no lineal del modelo dinámico que relaciona pare/fuerza aplicado en las juntas con la cinemática articular y cartesiana.
Los aspectos anteriores no están totalmente caracterizados y descritos desde el punto de vista matemático cuando se trata de manipuladores de un número considerable de grados de libertad, lo que influye en que las estrategias de control reportadas
requieran
un
gran
costo
computacional,
causando
que
los
controladores de Manipuladores Industriales presenten desempeños limitados o
18
costos muy elevados para aplicaciones industriales. Esto se debe, en buena medida, a un conocimiento limitado de los parámetros del modelo dinámico de los manipuladores, tanto analítico como experimental, lo que limita las posibilidades de síntesis de estrategias de control adecuadas. Todo lo anterior constituye el problema científico abordado en este trabajo. Dándole solución a este problema se podrían emplear en un futuro Manipuladores Industriales tipo SCARA en procesos de manufactura en aplicaciones industriales competitivas en precio y prestaciones tales como los procesos de ensamble de vehículos; todo lo planteado anteriormente lleva a los siguientes interrogantes científicos:
¿Cómo mejorar el modelamiento de la dinámica de los Manipuladores Industriales tipo SCARA para usarlos de base en la síntesis de los controladores? ¿Cómo sintetizar una estrategia de control que brinde adecuadas prestaciones para el posicionamiento continuo de estos dispositivos, que no incurra en errores de seguimiento elevados? ¿Qué resultados prácticos ofrece la implementación de la estrategia de control tipo CTC para el caso de un manipulador de cuatro grados de libertad)? Para el desarrollo de este trabajo se asume como hipótesis que, el controlador tipo CTC presenta un buen comportamiento en cuanto a minimización del IAE(integral Absolute Error), cuanto se trata de controlar la posición de un manipulado de cuatro grados de libertad tipo SCARA, pudiéndose aplicar en sistemas comerciales de varios grados de libertad.
19
Con formato: Español (alfab. internacional)
1.3. Justificación La Robótica Industrial es una de las áreas de la Tecnología que presentan los retos más estimulantes para los próximos años y la teoría de control no lineal se encuentra ubicada como punto nuclear en este campo, puesto que su desarrollo depende de manera directa de la eficiencia y robustez de los algoritmos computacionales de control. Por tal motivo, no resulta tan laborioso imaginarse la complejidad del proceso de control que hace que un robot industrial como el SCARA en el presente caso sea capaz de seguir una trayectoria deseada. Con el fin de superar los anteriores retos es posible implementar las técnicas de Control por retroalimentación lineal, el cual en su forma más general, acepta cualquier tipo de modelos, funciones o restricciones. Las principales razones por las cuales el control por retroalimentación lineal es una de las metodologías atractivas al momento de controlar es debido a que el CTC [24] puede ser usado para controlar una gran variedad de procesos, desde aquellos con dinámica relativamente simple hasta otros más complejos incluyendo sistemas con grandes retardos, permite tratar con facilidad el caso multivariable, es una metodología completamente abierta basada en algunos principios básicos que permite futuras extensiones, no es una metodología complicada, por lo tanto resulta particularmente atractivo para personal sin un conocimiento profundo de control, puesto que los conceptos resultan muy intuitivos, a la vez que la sintonización es relativamente fácil, resulta conceptualmente simple la extensión al tratamiento de restricciones, que pueden ser incluidas durante el proceso de diseño.
1.4. Objetivos A continuación se encuentran los objetivos generales y específicos de la presente investigación.
20
1.4.1. Objetivo General Diseñar e Implementar en simulación un Controlador por Retroalimentación Lineal tipo Computer Torque Control (CTC) sobre el modelo Cinemático y Dinámico de un Manipulador Industrial tipo SCARA de cuatro grados de libertad.
1.4.2. Objetivos Específicos Para poder alcanzar el objetivo general descrito es necesario apuntar hacia los siguientes objetivos específicos: Obtener el Modelo Cinemático Inverso y Directo del Manipulador SCARA que involucre el estudio y la descripción analítica de la geometría y propiedades de movimiento en el tiempo del manipulador.
Desarrollar el Modelo Dinámico Inverso y Directo del Manipulador SCARA bajo la formulación Newton-Euler.
Implementar en simulación una estrategia de control clásica como lo es un Computer Torque Control (CTC) basado en el modelo de referencia para el manipulador SCARA.
Simular dos trayectorias que permitan evaluar la respuesta global del proceso midiendo el seguimiento de trayectoria del Manipulador mediante un índice de rendimiento seleccionado. Realizar un análisis comparativo de los resultados obtenidos por el Controlador CTC para la dos Trayectorias simuladas.
1.5. Metodología de la investigación La metodología que se implemento implementó en esta investigación, se orienta a producir y comprobar nuevos conocimientos que forman parte del modelamiento y
21
control de procesos dinámicos no lineales, a partir del estudio en particular de un manipulador SCARA.
A continuación se muestran una serie de pasos que comienzan con la recolección de información, posteriormente el estudio y análisis de dicha información que permita generar un marco conceptual idóneo, luego pruebas y simulaciones, seguido de la realización de experiencias en simulación del modelo del manipulador y por último la obtención de resultados que den la oportunidad de evaluar el rendimiento del controlador bajo estudio.
.Ahora se detallaráan los principales módulos que componen la metodología bajo la cual se realizóa el presente trabajo. Documentación Bibliografía La documentación bibliográfica comienza con la recopilación de información, donde se realiza una búsqueda cuidadosa en fuentes electrónicas, bibliotecas especializadas, bases de datos, banco de patentes, investigación de la información pertinente a los últimos avances de la temática en análisis en universidades públicas y privadas, además de entidades especializadas y publicaciones de especialistas, posteriormente se realiza la recopilación de información a través de los recursos y medios disponibles en la Universidad del Norte referente a las investigaciones y trabajos que se han hecho en esta línea de investigación.
Obtención del modelo cinemático del robot SCARA Aquí se implementóo la metodología de Denavit y Hartenberg [6], método sistemático para descubrir y representar la geometría espacial de los elementos de una cadena cinemática, para solucionar los dos problemas de la cinemática de manipuladores, cinemática directa e inversa respectivamente.
22
Obtención del modelo dinámico del robot SCARA Primera etapa: Para la obtención del modelo dinámico, inicialmente se define claramente la necesidad del modelo, propósitos, tipos de análisis a ejecutar y condiciones, además de los parámetros y medidas apropiadas del desempeño del modelo, en general definir los objetivos de la modelación. Segunda etapa: se comprenderá el fenómeno de acuerdo con las variables cinemáticas y cinéticas que intervienen en el proceso, que permitan determinar las leyes o principios físicos que aplican, consideraciones o hipótesis necesarias y por ultimo establecer las inter-relaciones entre las variables y los diversos fenómenos asociados. Tercera etapa: es la formulación o síntesis del modelo, donde una vez comprendido el fenómeno, se plantean las ecuaciones de cada una de las leyes físicas que gobiernan el problema; que por lo general son ecuaciones diferenciales, cuya integración o solución de las mismas constituyen el modelo. Cuarta etapa: una vez sea obtenido el modelo se realizaraá su verificación inicial, donde se hará una estimación inicial de su validez o se conseguirán indicios de que es aceptable el modelo confrontándolo ante soluciones ya obtenidas en condiciones más simplificadas, además de confrontar dicho modelo contra uno obtenido bajo una formulación Lagrange-Euler. Quinta etapa: revalidación e inferencia, donde se obtendrán resultados simulados que conduzcan a la consecución de argumentos definitivos que demuestren la validez del modelo o los ajustes necesarios. 1
Simulación del modelo dinámico del robot SCARA El modelo dinámico del manipulador, es simulado mediante el estudio y análisis de las
variables
parciales
que
lo
componen
1
y
sus
interrelaciones,
en
Notas de clase de la Asignatura Diseño Mecánico. Ph.D Heriberto Maury. Universidad del Norte. Barranquilla Colombia.
23
Matlab/Simulink®. Dicha simulación permite reproducir su comportamiento global, y visualizar el impacto de distintas respuestas sobre las variables de interés.
Implementación del Control Cinemático del manipulador SCARA Se determinaron las trayectorias en el espacio articular a utilizar con el fin de lograr las premisas planteadas por el usuario, (punto de destino, tiempo empleado en la operación, trayectoria y clase de trayectoria a seguir por el efector del manipulador),
obedeciendo
al
conjunto
de
restricciones
físicas
de
los
accionamientos, además de los criterios de calidad como son la suavidad y precisión.
Implementación del Control Dinámico del manipulador Aquí se garantizóo que las trayectorias en el espacio articular seguidas por el robot sean lo más parecido a las planteadas por el control cinemático. Esto se consiguió con la ayuda del modelo dinámico del robot y la aplicación de toda la teoría del control dinámico por realimentación lineal.
Análisis de los resultados Una vez obtenido todos los resultados, se evaluó el desempeño del controlador CTC en este tipo de procesos no lineales, además de realizar comparaciones de rendimiento en cuanto a IAE para diferentes trayectorias, debido a que puede observarse que el error toma magnitudes positivas y negativas en ambas trayectorias simuladas. Por lo tanto para tener una idea realista del verdadero impacto que éste tiene en la respuesta se debe tomar una medida que sea independiente del signo, como es el índice seleccionado.
Elaboración del documento final Por último, se redacto redactó un documento y un artículo, donde se encuentra detallado todas las partes de la investigación, con sus respectivos análisis,
24
conclusiones y recomendaciones a tener en cuenta para las investigaciones futuras en modelación y control predictivo de manipuladores. 1.6. Contribuciones A continuación se muestra las cuatro contribuciones del presente trabajo:
Algoritmo computacional que determina la cinemática directa e inversa de cualquier manipulador industrial hasta cinco grados de libertad, a partir de los parámetros Denavit-Hartenberg para el cálculo de las matrices de transformación homogénea. Algoritmo que genera en forma simbólica el modelo dinámico inverso de manipuladores industriales hasta cinco grados de libertad. El algoritmo es una aplicaron del método recursivo de Newton-Euler para el calculo de las ecuaciones de un robot. Modelo Dinámico Inverso del manipulador SCARA ER-14, considerando que la ubicación del centro de masa del eslabón i referenciadas al sistema
Si , pueden tener componente a lo largo de de los marcos de referencia Denavit–Hartenberg. Tabla comparativa del desempeño de error de seguimiento del controlador CTC para dos trayectorias típicas en procesos de manufactura, a partir de la cuantificación de la integral absoluta del error articular y cartesiano.
1.7. Organización del Documento El informe final de esta investigación tiene la siguiente estructura.
En su primer capítulo se hace una evaluación del estado del arte acerca del modelado cinemático y dinámico de manipuladores, además de la implementación de sistemas de control tipo CTC reportadas en la literatura especializada. En cuanto al modelado, se detecta que hay una marcada tendencia a no considerar todas las características de estos sistemas a la hora de modelarlos, como son las
25
ubicaciones de los centros de masa, se asume una distribución uniforme de la masa de los eslabones, etc. Además se explica detalladamente el alcance, objetivos y metodología de la investigación. Como resultado de este capítulo se ratifica la hipótesis de este trabajo: no se encuentran suficiente información respecto al desempeño de controladores predictivos basados en modelos sobre la base de la modelación dinámica, del manipulador tipo SCARA.
El segundo capítulo está dedicado al estudio de la cinemática del manipulador. En él se analiza la variación de las posiciones articulares y posiciones cartesiana en el tiempo para las dos trayectorias a estudiar en esta investigación.
El tercer capítulo está dedicado al estudio de la dinámica del manipulador bajo el enfoque Newton-Euler. En él se analiza la variación de la dinámica del modelo con respecto a la posición de los eslabones de cadena cinemática. A la síntesis de la estrategia de control propuesta para el modelo dinámico es dedicado el cuarto capítulo. En este se muestran confrontados los rendimientos del controlador ante dos trayectorias, una de seguimiento y otra de cambio de dirección súbito.
Finalmente se exponen las conclusiones y recomendaciones a las que se arribó tras el desarrollo de este trabajo, así como la bibliografía referenciada en el documento.
26
Con formato: Derecha, Sangría: Izquierda: 0 cm, Primera línea: 0 cm
CAPÍTULO II Con formato: Título 1, Centrado, Punto de tabulación: 0 cm, Lista con tabulaciones
2.
MMODELADO CINEMÁTICO DEL ROBOT INDUSTRIALODELADO CINEMÁTICO DEL ROBOT INDUSTRIAL
2.1 Introducción. La cinemática es el estudio del movimiento con respecto a un sistema de referencia sin tener en cuenta las fuerzas que lo causan [9].
La cinemática de manipuladores involucra el estudio y la descripción analítica de la geometría y propiedades de movimiento basado en el tiempo, en particular responde al interrogante de como los eslabones se mueven en función del tiempo respecto a otros; además, de las relaciones entre la posición y la orientación del elemento final del robot manipulador con los valores que toman sus coordenadas articulares [9].
La implementación de nuevos algoritmos de control para robot manipuladores industriales de n grados de libertad requiere necesariamente del conocimiento previo de su modelo dinámico. De igual forma, es necesario un algoritmo de generación de trayectoria que de la misma manera se articula al conocimiento del modelo cinemático del robot, por ende dicho modelo cinemático es indispensable en el desarrollo de cualquier sistema de control para robot manipuladores industriales.
27
2.2 Descripción del Robot. El presente capitulo se enfoca en el estudio e implementación de la cinemática de un robot industrial de 4 grados de libertad, SCARA, como se muestra en la Figura 1, de alta repetibilidad (+/- 0.05 mm) y velocidad efectiva de 1514 mm/seg, tres ejes rotacionales y uno de translación. Su peso es de 30 kg, su alcance mínimo es de 230 mm y máximo de 500 mm. En el extremo final se anexa la herramienta, en este caso la pinza con la cual se realiza la tarea programada a la carga, y cuya masa máxima no debe ser mayor que 2 Kg y 3 Kg a altas y bajas velocidades respectivamente. El movimiento de sus eslabones es producido por medio de motores de corriente directa 24 VDC, con transmisión armónica, para el control de cada eslabón, estos tienen instalados en sus ejes encoders ópticos incrementales con índice de pulso de 2048 pulsos por revolución; cada motor se controla con un PWM H-mando de motores, 33 KHz, 7 A, 40-50 V con realimentación.
Figura 1. Manipulador SCARA
28
2.3 Modelado de la Cinemática de Robots La cinemática es la relación que hay entre las variables articulares y las coordenadas cartesianas correspondientes a la posición y orientación del elemento final o Terminal del robot (ET).
Básicamente hay dos casos para estudiar en la cinemática de un robot manipulador, el primero de ellos se conoce como el problema cinemático directo, que fundamentalmente nos ayuda a encontrar la posición en el espacio cartesiano y la orientación de un robot, tomando como referencia las variables articulares, que son los ángulos de orientación de cada eslabón con respecto a un eje en particular, o bien, con respecto a un eslabón anterior. El segundo es el problema cinemático inverso que resuelve la configuración que debe adoptar el robot para una posición y orientación conocida del extremo.
La cinemática directa, definida como la función x
f q1, q2 ,...,qn , permite conocer
la posición y orientación del ET del robot, expresada en coordenadas cartesianas, vector
q
x,
dados
los
valores
de
las
coordenadas
articulares,
vector
q1, q2 ,...,qn . Existen varios métodos para obtener una expresión analítica de
la cinemática directa, pero el más empleado en Robótica es el que propusieron Denavit y Hartenberg (D-H) en [5]. En esta metodología se obtiene la caracterización de cada GDL mediante los parámetros del eslabón y la articulación asociada (el ángulo y la distancia), como los cuatro parámetros D-H. Con cada cuarteta de parámetros D-H se obtiene la matriz de paso generalizado Ai que permite obtener la representación del marco de referencia Ri expresado en el marco de referencia Ri 1 . Finalmente, multiplicando las n matrices Ai se obtiene una matriz de transformación homogénea Ti n que representa el marco de referencia Rn expresado en el marco de referencia fijo R 0 .
29
El algoritmo de D-H comienza identificando los n ejes articulares qi , i 1, 2, ... n , numerando los eslabones del robot del 1 al n, de modo que el i-ésimo eslabón tiene en sus extremos a los ejes articulares qi y qi 1 . El segundo paso consiste en caracterizar a cada eslabón por su longitud, definida como la longitud de la normal común a los ejes articulares pasando por sus extremos y designada como ai . En el tercer paso se asigna un marco de referencia a cada uno de los primeros n-1 eslabones del robot, de modo que el origen del i-ésimo eslabón debe quedar en la intersección de la normal común ai con el eje articular q i 1 ; el eje xi es la prolongación de la normal común ai , mientras que su eje zi se alinea con el eje articular q i 1 . En el caso de que los ejes articulares sucesivos sean paralelos, la ubicación de las normales comunes no es única, por tal motivo, las ai se ubicarán de tal modo que se anulan uno o más parámetros di , definidos en el sexto paso de este algoritmo. En la Figura 2, se muestra el diagrama de dos articulaciones consecutivas en una cadena cinemática con el fin de observar gráficamente el significado de cada uno de los parámetros que conforman la cuarteta de cada eje.
qi
qi
zi
qi
qi
ai 1
di
zi
xi ai
1
i
1
xi
1
i
xi
Figura 2 Diagrama de Denavit-Hartemberg para la obtención de la cinemática
30
1
Es en el cuarto paso donde se asigna el marco de referencia al n-ésimo eslabón, además del marco de referencia fijo asociado al eslabón cero. El origen del marco del eslabón cero se ubica en la intersección del eje articular q1 con la superficie de la mesa que soporta al robot, mientras que el origen del marco del último eslabón se pone en el punto del ET del robot que es de interés posicionar y orientar. Generalmente, ese punto se encuentra en el punto medio de la pinza del manipulador, definiendo el eje zn paralelo a la dirección de la pinza, mientras que el eje xn se extiende del extremo izquierdo al derecho de dicha pinza. Ahora ya se puede definir la distancia d n entre los dos últimos eslabones como la distancia entre los orígenes de los marcos de referencia Rn
1
y Rn , medida a lo largo del eje
zn .
Se complementa la caracterización del i-ésimo eslabón en el quinto paso definiendo y qi
1
i
, como el ángulo que forman los ejes pasando por sus extremos, qi
o bien zi
1
y zi , medido alrededor del eje xi . Aquí se puede apreciar que el
eje xi coincide con el producto cruz zi articulación por el ángulo
i
1
x zi . En el sexto paso se caracteriza cada
entre los eslabones que conecta y la distancia d i
entre dichos eslabones. El ángulo normales comunes que posee, ai
i
1
se mide alrededor del eje zi 1 entre las dos
y ai o bien entre los ejes x i 1 y xi , mientras
que la distancia d i se mide a lo largo del eje zi dos normales comunes que posee: ai
1
1
entre las intersecciones con las
y ai . En el caso de la primera articulación
la distancia d1 se mide entre el origen del marco de referencia cero y la intersección entre z1 y a1 . Resumiendo, los parámetros de Denavit-Hartenberg son aquellos que caracterizan cinemáticamente al eslabón (2) y a la articulación (2), de modo que habrá cuatro parámetros D-H por cada GDL del robot, definidos como:
31
ai
Longitud de la normal común a los ejes zi
1
y zi , es decir a los ejes
articulares qi y q i 1 . i i
di
Ángulo entre los ejes zi
1
y zi , medido en un plano normal al eje xi .
Ángulo entre los ejes x i 1 y xi medido en un plano normal al eje zi 1 . Distancia medida a lo largo del eje zi entre las intersecciones con sus normales comunes.
En Lla matriz homogénea de paso generalizado Ai , es necesariose requiriere aplicar las transformaciones homogéneas básicas necesarias para relacionar los referenciales Ri 1 y Ri . Dichas transformaciones elementales son: una rotación de grados alrededor del eje zi 1 ; una translación de di centímetros a largo de ese
i
mismo eje, otra translación de ai centímetros a lo largo del eje x i 1 , ahora relacionados con el eje xi y, finalmente una rotación de
i
grados alrededor del
eje xi . De modo que la matriz de paso generalizado se estima como:
Aii
Aii
1
1
i i 1
A
T z,
i
T 0,0, d i T ai ,0,0 T x,
C i S i 0 0
S i C i 0 0
C i S i 0 0
C iS i C iC i S i 0
0 0 1 0
0 0 0 1
1 0 0 0
0 1 0 0
S iS i S iC i C i 0
Con formato: Fuente: Sin Negrita
i
0 0 0 0 1 di 0 1 ai C ai S di 1
1 0 0 0
i i
32
0 1 0 0
0 ai 0 0 1 0 0 1
1 0 0 C i 0 S i 0 0
0 S i C i 0
0 0 0 1
(Ec 1)
Se utilizóo en la anterior matriz la nomenclatura clásica en Robótica Industrial, s para designar al seno, c para el coseno [9]. Cabe mencionar aquí que, cuando la iésima articulación es de rotación,
i
es variable y d i es constante; mientras que
en el caso de articulaciones de translación es al contrario. Con las n matrices de paso generalizado se calcula la matriz de transformación homogénea que define la cinemática directa del robot de la siguiente manera:
n
T0n q
Aii 1 q i i 1
xi
yi
zi
pi
R01
0
0
0
1
0
p01 1 ; para i=1, 2 … n
(Ec 2)
La cinemática inversa de un robot, expresada por la ecuación q
f
1
x , nos
permite
del
robot,
q
T
calcular
el
valor
de
las
coordenadas
articulares
q1, q2 ,...,qn , que permiten llevar al ET del robot al punto deseado, expresado
por sus seis coordenadas cartesianas, tres para representar la posición tT
x, y , z , y tres más para representar la orientación. Existen muchos métodos
para calcular las coordenadas articulares del robot a partir de la expresión de la cinemática directa dada por la (Ec 2) [9]. Cuando se presente detalladamente la cinemática inversa en este capitulo, se mostrará el respectivo método implementado al robot SCARA.
2.4 Cinemática Directa del Robot SCARA La Figura 3 muestra el diagrama cinemático del robot SCARA, donde se muestra detalladamente la determinación de los marcos de referencia de cada uno de los eslabones del robot, de igual forma se encuentran de manera complementaria, en la Tabla 1 los valores numéricos de dichos parámetros a fin de calcular el modelo cinemático directo de este robot. Es importante aclarar que dichos valores numéricos se obtuvieron midiendo
33
a1
z1
a 1a
1
Eslabón 2
a2
a a2 z1 2 x1 x1
x1
z2
Eslabón x3
d2 z2
d2
z2
2 ESHED ROBOTEC
z3
d
3 ESHED ROBOTEC
d1
x3
d1
d4
x2
z3
Eslabón 1
x2
z3 x3
d2
d3 + d 4
d3
d4
x3
d4
Figura 3 Sistema de coordenadas según la convención D-H
i
θi (°)
di (mm)
ai (mm)
αi
1
q1
765
270
0
2
q2
-315
230
0
3
0
-d3-40
0
0
4
q4
-187.95
0
0
Tabla 1 Parámetros D-H del robot SCARA
Sustituyendo en la (Ec 1) los valores de los parámetros D-H del robot SCARA que se muestran en la Tabla 1, y realizando todas las multiplicaciones matriciales
34
indicadas por la (Ec 2), para i=4, se obtiene la matriz T, T
T04 , la cual especifica
la posición y orientación del ET del manipulador con respecto al sistema de coordenadas de la base. Esta matriz T es usada frecuentemente en la cinemática de robot manipuladores, y es llamada usualmente como “matriz del manipulador”. Considere la matriz de transformación homogénea T como:
T
x4
y4
z4
p4
0
0
0
1
T
n o a p 0 0 0 1
nx ny
ox oy
ax ay
px py
nz 0
oz 0
az 0
pz 1
(Ec 3)
donde: n
Vector normal a la mano. Asumiendo un Yaw o guiñada paralelo de la mano, es ortogonal a los dedos de la mano del robot.
O
Vector deslizante de la mano. Este apunta en la dirección de movimiento de los dedos, como el gripper abre o cierra.
A
Principal vector de la mano. Este apunta en una dirección normal a la palma de la mano (normal a la herramienta montada como elemento Terminal del robot).
P
Vector posición de la mano. Este apunta del origen de la base del sistema de coordenadas al origen del sistema de coordenadas de la mano, el cual esta localizada en el punto centro de la mano cuando sus dedos están totalmente cerrados.
Entonces la solución al problema cinemático directo para el robot SCAORA -ER 14 es:
T q
A01 A12 A23 A34
nx ny
ox oy
ax ay
px py
nz 0
oz 0
az 0
pz 1
35
(Ec 4)
Donde los elementos de la matriz de rotación y del vector de translación se definen teniendo en cuenta que C ≡ cos, S ≡ sen y son:
A01
A23
nx ny
C(q1 ) S(q1 )
S(q1 ) 0 a1 C(q1 ) C(q1 ) 0 a1 S(q1 )
0 0 1 0 0 0 1 0
0 0
1 0
A12
d1 1
0 0
A34
0 0 1 d3 0 0 0 1
C (q 2 ) S(q 2 )
S(q2 ) 0 a2 C(q2 ) C(q2 ) 0 a2 S(q2 )
0 0
0 0
C (q 4 ) S (q 4 )
S (q 4 ) C (q 4 )
0 0
0 0
d2 1
1 0 0 0
0 0
1 d4 0 1
C(q1 ) C(q2 ) - S(q1 ) S(q2 ) C(q4 ) + - C(q1 ) S(q2 ) - S(q1 ) C(q2 ) S(q4 ) S q1 C(q2 ) + C(q1 ) S(q2 ) C(q4 ) + C(q1 ) C(q2 ) - S(q1 ) S(q2 ) S(q4 )
nz
0
ox
C(q1 ) C(q2 ) - S(q1 ) S(q2 ) S(q4 ) - - C(q1 ) S(q2 ) - S(q1 ) C(q2 ) C(q4 )
oy
(S(q1 ) C(q2 ) + C(q1 ) S(q2 )) S(q4 ) - (C(q1 ) C(q2 ) - S(q1 ) S(q2 )) C(q4 )
oz ax
0 0
ay
0
az px
(Ec
5)
1 C(q1 ) a 2 C(q2 ) - S(q1 ) a 2 S(q2 ) + a1 C(q1 )
py
S(q1 ) a 2 C(q2 ) + C(q1 ) a 2 S(q2 ) + a1 S(q1 )
pz
d 4 + d3 + d2 + d1
Finalmente, la orientación de la pinza con respecto al marco de referencia fijo R0 queda como se muestra en la (Ec 6):
roll
q1 q2
36
q4
(Ec
6)
Debido a que las tres rotaciones se efectúan alrededor de ejes paralelos en un plano vertical al xi-yi de cada uno de los eslabones. Además, esta orientación puede calcularse a partir la información contenida en la submatriz de rotación incluida en la matriz solución de la cinemática del robot SCARA dada por la (Ec 4), dicha matriz equivale a una rotación elemental de θ grados alrededor del eje z, la cual se expresa como en (Ec 7):
R z,
C( ) S( )
S( ) C( )
0
0
Con formato: Fuente: Sin Negrita
0 0
(Ec
7)
Con formato: Fuente: Sin Negrita
1
Igualando la matriz de rotación dada por la (Ec 7) con la cinemática directa del robot dada por la (Ec 4), se puede despejar el valor de la orientación del ET del robot, utilizando la expresión de la función arco tangente de dos argumentos, descrita por la (Ec 8):
roll
tan -1
C( ) S( )
tan -1
nx ny
(Ec
8)
Así, con las expresiones dadas por las (Ec 5) y (Ec 6), o bien la (Ec 8), se obtienen respectivamente la posición y la orientación del ET del robot como una función de sus variables articulares.
2.5 Cinemática Inversa del Robot SCARA A continuación, se determinaran las ecuaciones que permiten calcular los valores que deben tener las coordenadas articulares del robot q
q1, q2 ,...,qn
T
, para que
el ET se localice y oriente en el espacio de la tarea. A partir del modelo cinemático directo es posible conocer el modelo cinemático inverso, mediante el procedimiento que se muestra en la (Ec 9):
37
Con formato: Fuente: Sin Negrita
A01
1
T04 q
A12 A23 A34
(Ec
9)
n o a p se calculó anteriormente, todo los miembros de la 0 0 0 1
Debido a que T
izquierda en la (Ec 9), son función de las variables articulares qi ,...,qk mientras que la derecha es función de qk 1,...,qn , por ende tenemos que (ver Ec 10). C(q1 ) n x
S(q1 ) n y
C(q1 ) o x
S(q1 ) o y
C(q1 ) a x
S(q1 ) a y
- S(q1 ) n x
C(q1 ) n y
- S(q1 ) o x
C(q1 ) o y
- S(q1 ) a x
C(q1 ) a y
nz
oz
az
0
0
0
C(q 2 + q 4 )
S(q 2 + q 4 )
0
a 2 C(q 2 )
S(q 2 + q 4 )
- C(q 2 + q 4 )
0
a 2 S(q 2 )
0
0
0
0
C(q1 ) p x
S(q1 ) p y
- S(q1 ) p x
a1
C(q1 ) p y
pz
d1 1
1 d 4 + d3 + d2 0
1
(Ec 10)
De las 12 opciones posibles, se escoge la ecuación donde q1 solo dependa de constantes como se describe en la (Ec 11).
q1
tan
144 º q1
1
ay
(Ec 11)
ax 144 º
donde tan-1 esta definido como (ver Ec 12).
0º q1 q1
tan
1
y x
90 º ;
para
90 º q1 180 º ; 180 º q1 90 º q1
x y
y
para
x y
y
90 º ; para
x y
y
x y
y
0º ;
para
(Ec 12)
Ahora para determinar la expresión del modelo cinemático inverso para la segunda articulación del manipulador se tiene que:
38
A12
1
1
A01 T04 q
A23 A34
(Ec
13)
De manera similar tomando el elemento 2,4 se obtiene que:
- S(q1 q2 ) p x
C(q1 q2 ) p y
S(q2 ) a1
0
(Ec 14)
Despejando la variable de interés, se tiene finalmente:
q2
tan
px S q1
1
py S q1
py C q1 px C q1
109 º q 2
a1
(Ec 15)
109 º
Para la cinemática inversa de la articulación prismática se despeja de la (Ec 5):
d3
pz
d4
d2
d1
(Ec
16)
Por ultimo el valor de q 4 se puede obtener despejando de la (Ec 6).
q4
roll
q1 q2
527 º q4
(Ec 17)
527 º
Las ecuaciones (Ec 11), (Ec 15), (Ec 16) y (Ec 17) permiten determinar los valores en el espacio articular ante una posición y orientación del ET del robot, con respecto a un sistema de coordenadas que se toma como referencia, conocidos los parámetros geométricos de los elementos del robot, es decir, estas ecuaciones son la solución al problema cinemático inverso del robot SCARA.
39
La presente investigación se prueba en dos trayectorias típicas. A continuación se muestran las gráficas del comportamiento cinemático deseado por el robot en el espacio articular y cartesiano. Es importante aclarar que estos cálculos están mediados por el modelo cinemático directo e inverso. A continuación se muestra la grafica en 3D de la primera trayectoria a simular (ver Figura 4). Básicamente el objetivo de esta trayectoria es excitar al modelo con una fuerte dinámica en el eje z, por esto la alta frecuencia pero con una amplitud variable. Para los ejes x-y esta trayectoria presenta diferentes diámetros, por lo que la respuesta se espera que sea diferente, mientras que para la trayectoria 2 hay un cambio de dirección súbito lo cual evaluará el rendimiento del controlador ante un cambio de
sentido
repentino.
GRAFICA 3D DE LA POSICION DESEADA PARA LA TRAYECTORIA 1
0.2 0.15
Pos - deseada z
0.1 0.05 0 -0.05 -0.1 -0.15 -0.2 0.4 0.35 0.3 0.25 0.2
0.1
0 Pos - deseada y
0.2
0.3
0.4
0.5
Pos - deseada x
Figura 4. Grafica de 3D de las posiciones cartesiana deseadas para la trayectoria 1
40
POSICIONES CRATESIANAS DESEADAS - TRAYECTORIA 1 Pos - deseadas x Pos - deseadas y Pos - deseadas z
Posiciones deseadas - espacio cartesiano (m)
0.5
0.4
0.3
0.2
0.1
0
-0.1
-0.2 0.5
1
1.5 Tiempo (seg)
2
2.5
3
Figura 5. Posiciones cartesianas deseadas en el tiempo para la trayectoria 1 Con formato: Español (alfab. internacional)
Se puede observar claramente en la Figura 5 que la trayectoria 1 es una elipse en el plano x-y con diámetro mayor y menor de 0.4 m y 0.2m respectivamente, en el eje z se observa una oscilación sinusoidal con amplitud máxima de 0.2 m al cabo de 1.5 seg, la mitad del tiempo total de ejecución de la trayectoria.
En la Figura 6 se muestra el comportamiento en el tiempo deseado para la trayectoria 1, en el espacio articular.
41
Con formato: Normal, Izquierda
POSICIONES ARTICULARES DESEADAS - TRAYECTORIA 1
Pos Pos Pos Pos
Posiciones deseadas - espacio articular (rad)
5
4
- deseada q1 - deseada q2 - deseada q3 - deseada q4
3
2
1
0
-1
-2
-3
0
0.5
1
1.5
2
2.5
3
Tiempo (seg)
Figura 6 Posiciones articulares deseadas en el tiempo para la trayectoria 1
A continuación se muestran las respectivas gráficas para la segunda trayectoria (ver Figura 7,8), la cual se caracteriza por mantener al robot a una altura fija (eje z), y llevar al elemento terminal del robot a seguir un cambio de dirección, debido a que las distancias de los lados son diferentes, pero con tiempos cada uno de 1 seg, con una totalidad de 3 seg para realizar toda la trayectoria. Todo lo anterior conlleva a velocidades del TCP diferentes en cada tramo.
42
POSICIONES DESEADAS EN EL PLANO X-Y PARA LA TRAYECTORIA 2
0.375
Pos - Deseada y (m)
0.37
0.365
0.36
0.355
0.35
0.35
0.3505
0.351
0.3515
0.352
0.3525
Pos - Deseada x (m)
Figura 7. Posiciones cartesiana deseadas para la trayectoria 2 Con formato: Español (alfab. internacional) Con formato: Normal, Izquierda
Ahora se muestra detalladamente los perfiles de posición para la trayectoria 2, tanto en el espacio articular y cartesiano. Cabe resaltar que la relación entre coordenadas articulares y cartesiana esta mediada por el modelo cinemático antes calculado y presentado.
43
POSICIONES ARTICULARES DESEADAS PARA LA TRAYECTORIA 2
Pos Pos Pos Pos
5
Posicion deseada - Espacio articular (rad)
4
- deseada q1 - deseada q2 - deseada q3 - deseada q4
3
2
1
0
-1
-2
-3
0
0.5
1
1.5
2
2.5
3
Tiempo (seg)
Figura 8. Sistema de coordenadas según la convención D-H
Ahora se muestra detalladamente los perfiles de posición para la trayectoria 2, tanto en el espacio articular y cartesiano. Cabe resaltar que la relación entre coordenadas articulares y cartesiana esta mediada por el modelo cinemático antes calculado y presentado (ver sección 2.4).
En la Figura 9, se observa que la posición deseada en el eje z es totalmente horizontal en 0.4 m, debido a que en esta trayectoria no se experimento con este eje.
44
POSICIONES CARTESIANAS DESEADAS PARA LA TRAYECTORIA 2
Pos - deseada x Pos - deseada y Pos - deseada z
Posicion deseada - Espacio cartesiano (m)
0.41
0.4
0.39
0.38
0.37
0.36
0.35
0.34
0
0.5
1
1.5
2
2.5
Tiempo (seg)
Figura 9. Sistema de coordenadas según la convención D-H
45
3
CAPÍTULO III Con formato: Justificado, Sangría: Izquierda: 0,21 cm, Sangría francesa: 0,63 cm
Con formato: Español (alfab. internacional) Con formato: Español (alfab. internacional)
3.
MODELADO DINÁMICO DEL ROBOT INDUSTRIALODELADO DINÁMICO DEL ROBOT INDUSTRIAL
3.1 Dinámica del Robot manipulador. La dinámica del manipulador establece la relación entre las fuerzas y pares aplicadas en los actuadores y el movimiento del manipulador. Estas relaciones pueden ser expresadas matemáticamente por un conjunto de ecuaciones diferenciales, comúnmente llamadas ecuaciones de movimiento (E.O.M). Para obtener el modelo dinámico de un robot manipulador industrial, varias formulaciones se han desarrollado, tales como la de Lagrange-Euler [5], NewtonEuler [5], Lagrange-Euler Recursiva [5], y la formulación del principio generalizado d'Alembert [5]. En el presente capitulo se desarrolla el modelo dinámico de un manipulador SCARA solóo bajo la formulación Newton-Euler, además el modelo dinámico de los actuadores es presentado, permitiendo simular el movimiento del robot, evaluar la estructura mecánica del robot, redimensionar los actuadores si se requiere para alguna aplicación especifica, así como diseñar y evaluar nuevos controladores dinámicos del robot.
Es importante tener bien claro que en la dinámica de manipuladores existen dos problemas básicamente, modelo dinámico directo, el cual determina el comportamiento en el tiempo de las coordenadas articulares del robot en función
46
de las fuerzas y torques aplicadas, como se muestra en la Figura 10, mientras que el modelo dinámico inverso, determina explícitamente los torques y fuerzas en función de las coordenadas articulares y sus derivadas, que el problema que se resolverá a continuación.. 1
i
t ,...
n
t
1
t ,...
n
t
Dinámica Directa
i
Torques/Fuerzas
Coordenadas articulares
t
t Dinámica Inversa
1
t ,...
n
t
1
t ,...
n
t
Figura 10 Diagrama de relación entre Dinámica Directa e Inversa
3.2 Formulación Newton-Euler. Una directa interpretación de la segunda ley de Newton, conlleva al desarrollo de las ecuaciones que gobiernan el comportamiento dinámico de cualquier manipulador bajo la formulación de Newton Euler (NE) [5], que utilizan un enfoque vectorial, es decir, se expresan las fuerzas y aceleraciones en forma de vector, por lo anterior se hace necesario tener la habilidad suficiente para incluir todas la fuerzas requeridas. En general aquí se conocerá la relación entre el movimiento del robot y las fuerzas implicadas en el mismo, relación matemática entre la localización del robot definida por sus variables articulares o por las coordenadas de localización de su extremo, y sus derivadas: velocidad y aceleración. Para derivar las ecuaciones de movimiento bajo el enfoque de Newton – Euler, para su posterior programación, hay que plantear claramente la segunda ley de Newton de movimiento y la ley de Euler para el eslabón i.
Consideremos el diagrama de cuerpo libre (DCL) que muestra la Figura 11, donde
fi es la fuerza ejercida por el eslabón (i-1) sobre el eslabón (i) mientras que ni es el momento o torque ejercido sobre el eslabón (i) por el eslabón (i-1), y
47
despreciando los efectos de amortiguamiento viscoso, debido a que este efecto se tendrá en cuenta en la sección 3.4, las ecuaciones de movimiento aplicando Segunda Ley de Newton queda como se observa en la (Ec 18).
Fi+1 zi+1
ni+1 xi+1
xi
ipi+1
zi
m i*g ipci
ni
Fi
Figura 11 Diagrama de cuerpo libre del eslabón i del robot
fi
fi
1
d mi v ci dt
mi g
mi v ci
para i=1, 2 … n
(Ec 18)
Las ecuaciones de Euler para describir el movimiento rotacional de los eslabones queda como se observa en la (Ec 19).
ni
ni
i 1
pci
fi
i
pi
i 1
pci
fi
1
d Ii w i dt
I i w i
wi
Ii w i
para i=1, 2 … n
(Ec 19)
donde I i es el tensor de inercia centroidal del eslabón i, el cual varía a medida que cambia la orientación y posición del eslabón (i). La razón de cambio del
48
momento angular incluye ambos efectos en un término de aceleración angular
I i w i y un término conocido como torque giroscópico w i
Ii w i .
x2 p m(p) r(op)
n
x1
o
x3 Figura 12 Referencia de la masa de una partícula p
El tensor de inercia I i es una matriz simétrica (3x3), que describe las propiedades de masa de un eslabón (i) con respecto a su centro de masa, a continuación se muestra un desarrollo matemático de dicho tensor referenciado a la Figura 11. El momento de inercia respecto al eje nˆ del sistema se define como se observa en la (Ec 20).
I o, nˆ
N
m
*
nˆ, p
2
(Ec 20)
1
donde m
es la masa de una partícula p del sistema y
nˆ, p es la distancia de la nˆ r (op ) nˆ r (op ) , de
2 partícula p al eje nˆ . Por definición se tiene que: nˆ, p igual forma nˆ y r (op) se puede expresar en términos de sus componentes como
en la (Ec 21).
49
nˆ 1 e1 nˆ 2 e2 nˆ 3 e3 , r (op)
nˆ
x1(op) e1 x 2 (op) e2
x 3 (op) e3 (Ec 21)
Realizando la operación expandida se obtiene la (Ec 22).
nˆ, p
2
2
n1
x2
2
x3
2
n2
2
x3
2
2
x1
n3
2
x2
2
2
x1
2n1n2
x 1x 2
2n1n3
x 1x 3
2n2n3
x2 x3
(Ec 22)
obteniendo la expresión del tensor de inercia respecto al punto o, en notación matricial.
I o, nˆ
N
m
* r 2 op
ik
x i op x k op
donde i, k
1, 2, 3.
(Ec 23)
1
3.3 Algoritmo computacional para determinar el modelo dinámico por Newton-Euler. Las (Ec 18) y (Ec 19) gobiernan el comportamiento dinámico de un sólo eslabón del robot. El conjunto completo de ecuaciones para el robot manipulador
es
obtenido a partir de una evaluación de ambas ecuaciones a cada uno de los eslabones que conforman al manipulador, que para el caso en estudio, son cuatro eslabones.
La formulación de Newton-Euler a partir del computo de los pasos 1-9 determinan el modelo dinámico inverso fundamentado en la unión de dos iteraciones como sigue: Las iteraciones hacia adelante que se realiza en los pasos 4, 5, 6 y 7 del algoritmo que se muestra a continuación, transformando las variables cinemáticas desde la base hasta el ET del robot, mientras que la iteración hacia atrás se desarrolla en las transformaciones de las fuerzas y momentos del ET del robot hasta la base que se calculan en los pasos 8 al 10.
50
A continuación se muestra detalladamente cada unos de los pasos que se tuvieron en cuenta para obtener el modelo dinámico inverso del SCARA.
Primero: Teniendo en cuenta la metodología de DH, se asignan a cada uno de los eslabones un sistema de referencia, estos marcos de referencia se mostraron en el capítulo II del presente documento.
Segundo: Se determinan las matrices de transformación homogénea, que contienen a la submatriz de rotación
i 1
R i y sus inversas i Ri
i 1
Ri
1
1
i 1
T
Ri .
Tercero: Se establecen todas las condiciones iniciales al algoritmo, es decir, todas las condiciones cinemáticas del eslabón fijo o sistema de la base S o , como se muestra a continuación.
0
w0
0, 0, 0
T
velocidad angular de la base
0
w 0
0, 0, 0
T
aceleración angular de la base
0
v0
0, 0, 0
0
v 0
gx , gy , gz
0
w 0 , 0 w 0 y 0 v 0 son variables generalmente nuloas excepto para el caso en que la
T
velocidad lineal de la base T
aceleración lineal de la base
base del robot esta en movimiento. Además debido a que el ET sostiene a una carga, es posible decir que se tiene caracterizada
n i
nn
i
y
n i
fn
i
que son el
momento externo y la fuerza externa respectivamente. En los siguientes pasos del presente algoritmo se utilizaráa la siguiente convención:
z0
0, 0, 1
T
, es un vector unitario en la dirección del eje z del marco de
referencia ubicado en la base.
51
i
pi
ai , di Si , di Ci , representa las coordenadas del origen del sistema del
sistema Si respecto al sistema Si i
1
.
si , son las coordenadas del centro de masa del eslabón i referenciadas al sistema
Si . i
Ii , es la matriz de inercia del eslabón i respecto a su centro de masa en función de
Si .
A continuación se muestran los pasos del algoritmo que computan las transformaciones de las variables cinemáticas, desde la base del robot hasta el ET del robot y deben ser calculados para i=1, 2…n.
Cuarto: Se calcula la velocidad angular del sistema Si .
i
w i i Ri 1
i 1
i
w i i Ri 1 i 1 w i 1
z0 q i
wi 1
si el eslabón i es de rotación si el eslabón i es de traslación
Quinto: Cáalculo de la aceleración angular del sistema Si .
i
i
i i Ri w
1
i w
1
i
Ri
i 1
i 1
i w
i w
z 0 qi
1
i 1
wi
z 0 q i
1
si el eslabón i es de rotación si el eslabón i es de traslación
1
Sexto: Se calcula la aceleración lineal del sistema Si .
i
i
i i pi i w i v i i w
v i
i
Ri
1
i
z 0 q i
2 i w i i Ri
w i i pi i 1
1
v i
i
Ri
i 1
z 0 q i
i 1 1
v i
si el eslabón i es de rotación
1
i i pi w i
wi
i
si el eslabón i es de traslación
w i i pi
Séptimo: Caálculo de la aceleración lineal del centro de gravedad del eslabón i.
52
i
i
ai
i
w i
i
si
i
wi
wi
i
i
si
v i
A continuación se muestran los pasos del algoritmo que computan las transformaciones de las fuerzas y momentos del ET del robot hasta la base y deben ser calculados para i=n, n-1, …1.
Octavo: Calculo de la fuerza ejercida sobre el eslabón i.
i
i
fi
Ri
i 1 1
fi
1
mi i a i
Noveno: Calculo del par ejercido sobre el eslabón i.
i
ni i R i
i 1 1
ni
i 1
Ri i pi
i 1
fi
i 1
pi i si
i iwi mi i a i i Ii i w
i
Ii i w i
Décimo: Calculo de la fuerza o par aplicado a la articulación i.
i i
i
niT i Ri 1 z 0
i T i i
f
Donde
Ri 1 z 0
si el eslabón i es de rotación si el eslabón i es de traslación
puede ser el par o la fuerza aplicada a la articulación i y a su vez es par
que entrega el motor menos el par de la fricción.
Analizando detenidamente el algoritmo se pueden observar las siguientes propiedades:
53
1. El modelo Newton-Euler es lineal con respecto al tensor de inercia i Ii , lo cual proviene de la recursión hacia atrás, que se computa en los pasos 810. Las fuerzas/torques
i
en el paso 10 son lineales en el momento i ni .
2. Para articulaciones rotacionales, el modelo Newton-Euler es no lineal con respecto a los vectores de centro de masa Si . Los pasos 7 y 8 muestran que la fuerza neta es lineal en el vector de centro de masa. El producto cruz
S i x if i es no lineal (cuadrático) en Si , por tal motivo el torque aplicado en una junta rotacional es no lineal en Si .
3. El modelo Newton-Euler es no lineal con respecto al vector de parámetros cinemáticas i pi . La fuerza aplicada a cada eslabón es lineal en el vector i
pi , pero su producto cruz es no lineal en i pi , por las fuerzas/torques
i
en
el paso 10 son no lineales en i pi .
4. Las ecuaciones dinámicas de los eslabones i+1 para el torque son independiente de la masa del eslabón anterior y el clásico tensor de inercia i
Ii , lo cual es consecuencia de la recursión.
3.4 Modelo dinámico del Robot SCARA. El movimiento articular real de un robot es gobernado por su dinámica, teniendo en cuenta la masa de cada eslabón y junta, la velocidad y aceleración articular, y las fuerzas/fuerzas aplicadas a cada junta. El modelo dinámico del robot SCARA es un conjunto de ecuaciones diferenciales que están dadas por la (Ec 24).
A q q B q
q q
54
Cq
q 2
gq
(Ec 24)
donde:
q
es el n-vector de coordenadas espaciales generalizado que describe la posición del manipulador.
q
es el n-vector de velocidades angulares.
q
es el n-vector de aceleraciones angulares.
Aq
es la matriz simétrica de inercia en el espacio articular de tamaño n x n.
Bq
es la matriz de efectos de coriolis de tamaño n x n(n-1)/2.
Cq
es la matriz de torques centrípetos de tamaño n x n.
gq
es el n-vector de torques gravitatorios.
Los símbolos q q y q 2
son las notaciones para el vector de producto de
velocidad y el vector de velocidades al cuadrado. q q y q 2 están dados por:
q q
q 1 q 2 ,
q 1 q 3, ... q 1 q n ,
q 2
q 2 q 3, 2
2
q 2 q 4 , ... q n
q 1 , q 2 , ... q n
2
q n ,
q n
2
q n ,
T
2 T
Se ejecuta el programa tal que se observa en el anexo ANEXO 5 y se obtiene las siguientes ecuaciones:
55
[ S x 3 m3C q2 a1 S y21m1 m1S x21 S x 4 m4 a2 3a2 m2C q2 a1 S x 3 m3 a2 2a2 m2 S x 2
1
2a2 m2 S x 2C q2 2m4 a22C q2
2
2
2S q2 C q2 a2 m2 S y 2 a2 m4C q2 S x1 2m1a1S x1 2m3 a22C q2
I zz 2
I zz 3 m2 S x22
2
2
2m4 a22C q2
I zz 3 2a2 m2 S x 2 2m3 a22C q2
2
2 S q2 C q2 a2 m2 S y 2 m2 S x22 ]q2
S x 4 m4 a2 m2 a22
S q2 a2 m2 a1 C q2 a2 m4 S y1 2a2 m2 S y 2C q2
2
S y22 m2
I zz1 I zz 4
2a2 m2 S x 2C q2
S x 3 m3 a2
S q2 a1m2 S x 2
2m2 a22C q2
2
S q2 a2 m4 S y1 3a2 m4C q2 a1 m2 S x 2C q2 a1 S y 3 m3 S q2 a1 S y 4 m4 S q2 a1
S x 4 m4C q2 a1 S y 2 m2 S q2 a1 2a2 m3C q2 a1 I zz 2 [ I zz 4
2
m1a12
m2 a22 ]q1
S y22 m2 2m2 a22C q2
2
I zz 4 q4 [ S q2 a2 m4 S x1
S y 2 m2C q2 a1 S y 3 m3C q2 a1
S y 4 m4C q2 a1 2S q2 C q2 a m2 2S q2 C q 2 a m3 S q2 S x 4 m4 a1 2S q2 C q2 a22 m4 2 2
2 2
S q2 S x 3 m3 a1 S q2 a2 m4 a1 2 S q2 C q2 a2 m2 S x 2
S y 3 m3 a2 ]q12 [2a2 m2 S y 2C q2
S y 4 m4 a2
2 S q2 C q2 a2 m2 S x 2 2 S q2 C q2 a 22m3 2 S q2 C q2 a22 m4 2 S q2 C q2 a22 m2 S y 4 m4 a2 ]q
2 2
2 2
2 2
[ 4 S q2 C q2 a m4 4S q2 C q2 a m2
2 S y 3 m3 a2 2S y 4 m4 a2
2
S y 3 m3 a2
4 S q2 C q2 a2 m2 S x 2 4 S q2 C q2 a22 m3
2
4a2 m2 S y 2C q2 ]q1q 2
(Ec 25)
[ m2 S x 2C q2 a1 S y22 m2
2
I zz 3 3a2 m2 S x 2
m2 S x22
S x 3 m3C q2 a1 S x 3 m3 a2
a2 m4C q2 a1 ]q1 [ I zz 3 m3 a22 m2 S x22
I zz 4 m4 a22 2m2 a22
S x 3 m3 a2
S x 4 m4 a2
2m2 a22
2 1
S x 4 m4 S q2 a1 S y 4 m4C q2 a1 ]q
I zz 2
S x 4 m4 a2
S y22 m2 ]q2 [ S y 2 m2C q2 a1 m2 S x 2 S q2 a1 a2 m3 S q2 a1
2a2 m2 S q2 a1 m4 a2 S q2 a1 S y 3 m3C q2 a1 S x 3 m3 S q2 a1 S y 3 m3 a2 S y 4 m4 a2
m4 a22
a2 m3C q2 a1 S y 4 m4 S q2 a1 2a2 m2C q2 a1 S x 4 m4C q2 a1
S y 2 m2 S q2 a1 S y 3 m3 S q2 a1 I zz 4 3a2 m2 S x 2
m3 a22
I zz 2
[2a2 m2 S y 2 2S y 4 m4 a2 2S y 3 m3 a2 ]q1q 2
S y 4 m4 a2
a2 m2 S y 2
I zz 4 q4 [ S y 3 m3 a2
a2 m2 S y 2 ]q22
(Ec 26)
3
g
d3 m4
m3
(Ec 27)
4
[ S y 4 m4 S q2 a1 S x 4 m4 a2
I zz 4
S x 4 m4C q2 a1 ]q1 [ I zz 4 2 1
[ S x 4 m4 S q2 a1 S y 4 m4C q2 a1 S y 4 m4 a2 ]q
[ S y 4 m4 a2 ]q
2 2
S x 4 m4 a2 ]q2 [ 2 S y 4 m4 a2 ]q1q 2 I zz 4 q4
(Ec 28)
56
El modelo dinámico de este robot es fuertemente no lineal, multivariable, acoplado y de parámetros variantes, como se observa en las anteriores ecuaciones, lo que repercute generalmente en un sistema de control es extremadamente complejo. Por tal motivo, se realizan un conjunto de simplificaciones que parten de asumir que la ubicación del centro de masa del eslabón i referenciadas al sistema Si , solo tienen componente a lo largo de de los marcos de referencia Denavit– Hartenberg, se reagrupa cada uno de los términos de los primeros momento de inercia de cada uno de los eslabones. Además, se introduce el modelo de frotamiento seco o de Coulomb, que hace referencia a una fuerza constante opuesta al movimiento. Al inicio del movimiento una fuerza superior al frotamiento seco debe ser aplicada con el fin de mover las articulaciones. El modelo de frotamiento viscoso hace referencia al frotamiento existente en presencia de movimiento. La expresión completa es mostrada en la (Ec 29).
f
Fv q Fs sign(q)
(Ec 29)
Las anteriores simplificaciones para el caso de esta investigación son aceptables debido a que se esta trabajando con un robot comercial a velocidades de avance del ET alrededor de 0.01 m/s, lo cual facilita el diseño del sistema de control proporcionando unos resultados de seguimiento articular de trayectoria de 5E-5, como se observa en la sección 5. Ahora las ecuaciones finales que se programaron son:
57
q 2 )2 .QY 2 ) (Q2 X (
(d 2 .C(q 2 ).(( ( (q 3
1
w1 w2 )) (C(q 3 )( (m4 .( (S (q3 ) ).( d 3 .( (q1 q 2 ) 2 ) )
2
2 q1 ))) C(q 2 )(d 2 .w1 )) d 3(w1 w2 ))))
(C(q 2 ).(d 2 .( q1 )) S (q2 ).( d 2 .w1 ) ))) C(q 3 ).(( (S(q 2 )(d 2( (-(q 3
q1 q 2 )2 Q y 3 Q X 3(w3
w2
2 ((m 4(C(q 3 )(d 3(-(q1 q 2 )2 ) (C(q 2 ).( d 2(q1 ))
w1 ))))
2 S(q3 ).(( ( S (q2 ).(d 2 .( q1 ))) C(q 2 )(d 2 .w1 )) d 3(w1 w2 ))))) ( (q 3 q 1 q 2 ) 2 .Q X 3
QY 3 ( w3 (q1 q 2 )))).S (q3 ))) ((( (q1 q 2 ) 2 ).Q X 2 QY 2 .( w1 w2 )) (C(q 3 ).(( (m4 .( ( C (q3 ) ). (d 3 .( (q1 q 2 ) 2 ) (C(q 2 )(d 2(
2 2 q1 )) S (q2 ).( d 2 .w1 ))) S(q3 ).(( ( S (q 2 ).(d 2 .( q1 )))
q1 q 2 )2 )Q X 3 QY 3(w3
C(q 2 )(d 2 .w1 )) d 3(w1 w2 )))) (( (q 3
w2
w1 )))) (((m4(S(q 3 )
2
2 q1 )) S(q 2 )(d 2 .w1 ))) C(q 3 ).(( ( S (q2 ).(d 2 .( q1 ))) (C(q 2 )(d 2 .w1 ))
(d 3(-(q1 q 2 ) ) (C(q 2 )(d 2( 2
d 3(w1 w2 )))) (( (q 3 (q 1 q 2 )) 2 ).QY 3 Q X 3(w3
w2
w1 )))).S (q3 ))).S (q2 ) (d 3 (C (q3 )
2
( S(q3 )(d 3 .( (q1 q 2 ) ) (C (q2 ).(d 2 .( q1 )) S(q 2 )(d 2 .w1 )))) C(q 3 ).(( ( S (q2 ). 2
( m 4(
2 (d 2 .( q1 ))) (C(q 2 )(d 2 .w1 )) d 3(w1 w2 )))) (( (q 3 (q 1 q 2 )) 2 ).QY 3 Q X 3(w3
w2
w1 ))))
2
2 (((m4(C(q 3 )(d 3(-(q1 q 2 ) ) (C(q 2 ).( d 2(q1 )) S (q2 ).( d 2 .w1 ))) S(q3 ).(( ( S (q 2 ).(d 2 .( q1 ))) 2
C(q 2 )(d 2 .w1 )) d 3(q1 q 2 ))))) (( (q 3 (((w3 ( w2
q1 q 2 )2 )Q X 3 QY 3(w3
w2
w1 ))))S (q3 ))
2
2
w1 )).I ZZ 3 ) QY 3 (C(q 3 ).( d 3 ( (q1 q 2 ) ) (C (q2 ).(d 2 .( q1 )) S (q2 ).( d 2 .w1 )))
2 S(q3 ).(( ( S (q2 ).(d 2 .( q1 )) C (q2 ).( d 2 .w1 )) d 3(w1 w2 )) Q X 3 (C(q 3 ).( d 3 ( (q1 q 2 ) 2 ) 2 2 (C(q 2 ).(d 2 .( q1 )) S (q2 ).( d 2 .w1 ) ))) C(q 3 ).(( ( S (q2 ).(d 2 .( q1 ))) C (q2 ).( d 2 .w1 )) d 3(w1 w2 )))) 2 2 ( I ZZ 2(w1 w2 )) QY 2(C(q 2 ).( d 2(q1 )) S (q2 ).( d 2 .w1 )) Q X 2 ( ( S (q2 ).(d 2 .( q1 ))) C (q2 ).( d 2 .w1 ))) (q1 .I ZZ1 )) FV1q1 FS 1sign(q1 )
(Ec 30)
2
d 3(C(q 3 )(((m 4(
C(q 3 )((
(S(q 3 )(d 3(
q 2 ) 2
(q1
2 q1 ))) C(q 2 )(d 2 q1 )) d 3(q1
(S(q 2 )(d 2(
Q X 3(q3 q1 q2 )))
((m 4(C(q 3 )(d 3(-(q1
q2 ))))) (-(q 3
q2 )))) ((-(q 3
QY 3 q2 ))S(q 3 ))
((( q3
S(q 2 )(d 2 q1 )))
S(q 3 )( (-(S(q 2 )d 2(-q1 )) C(q 2 )d 2 q1
2
Q x 3( -S(q 3 )( d 3(q1 d 3( w1
q 2 ) -C(q 2 ) d 2 q1
w2 )))
Q y 2( -C(q 2 )d 2 q1
((w 3 2
w1
S(q 2 ) d 2 w1 )
d 3(q1
2
q 2 ) 2 )Q X 3 -
q2 )))
C(q 3 )( S(q 2 ) d 2 q1
w2 )I ZZ 3 )- Q y 2(-C(q 2 )d 2 q1
S(q 2 )d 2 w1 ) Q x 2( S(q 2 ) d 2 q1
q1
2 q 2 ) 2 ) (C(q 2 )(d 2(-(q1 )))
q2 )I zz 3 ) -QY 3(C(q 3 )(d 3(-(q1 2
q 2 )2Q y 3
q 2 ) ) (C(q 2 )d 2(q1 ) S(q 2 )d 2 q1 ))
S(q 3 )( -(S(q 2 )(d 2(-q1 ))) C(q 2 )(d 2q1 ) d 3(q1
2
q1
2
2
2
q1
2 q1 )) S(q 2 )(d 2 w1 ))))
(C(q 2 )(d 2(
2
2
C(q 2 )d 2 w1
S(q 2 )d 2 w1 ) Q x 2( S(q 2 ) d 2 q1
C(q 2 )d 2 w1 )
FV 2 q 2
2
FS 2sign(q 2 )
(Ec 31)
58
τ
3
q q )I - Q ( C(q )( d ( -W 2 C(q ) d (-q 2 ) S(q ) d q ) 1 2 ZZ 3 y3 3 3 32 2 2 1 2 2 1 S(q )( S(q ) d (q 2 ) C(q )d q d (q q ))) 3 2 2 1 2 2 2 3 1 2 2 2 Q [ S(q )d ( q q ) C(q ) d (-q ) S(q ) d q C(q )( S(q ) d q 2 x3 3 3 1 2 2 2 1 2 2 1 3 2 2 1 C(q )d w d ( q q ))] FV4q 3 FS3sign(q 3 ) 2 2 2 3 1 2 (q 3
(Ec 32)
τ
4
M ( -G 4 3
q ) 4
IA (q ) 4 4
FV (q ) 4 4
(FS )sign(q ) 4 4
(Ec 33)
Los valores numéricos que se tuvieron en cuenta para la simulación de la dinámica y Control dinámico, se encuentran tabulados a continuación2:
Parámetro
Valor
Parámetro
Valor
Parámetro
Valor
Izz1
3.38
Qx3
0.1
Fv1= Fv3
0.1
Izz2
0.063
Qy2
0.001
Fv2= Fv4
0.012
Izz3
0.1
Qy3
0.1
Fs1= Fs3
0.57
Qx2
0.242
Ia4
0.045
Fs2= Fs4
0.125
Tabla 2 Parámetros de robot SCARA
Las unidades para el tensor de inercia son Kg.m2, para el primer momento de inercia Kg.m y para la inercia del motor Kg.m2. Las relaciones de aceleraciones y torques antes calculadas son importantes debido a que el conjunto de fuerzas y/o torques aplicados al sistema las afectan directamente como se observó anteriormente, determinando de esta manera su movimiento. Estas fuerzas y torques se grafican y se muestran para el controlador clásico CTC en la Figura 13 para la trayectoria 1, en Figura 14 para la trayectoria 2, mientras que en la Figura 15 y Figura 16 se muestra una ampliación del comportamiento de dichos torques para los tiempos t=1.5 seg y t=0 seg. Dentro de cada gráfica se muestra la respectiva convención. Los resultados obtenidos permiten conocer los requerimientos de torque y potencia en los motores. Es 2
http://www.ladispe.polito.it/robotica/Activities/Arch/. Departamento de Automática e Informática. Politecnico di Torino. Italia.
59
importante aclarar que hasta el momento en este CapituloCapítulo, no se ha tenido en cuenta el algoritmo de control que gobierna al controlador instalado, en este caso el CTC, debido a que esto será objeto de estudio en el siguiente Capitulo. COMPORTAMIENTO DE LOS TORQUE EN TIEMPO PARA LA TRAYECTORIA 1
600 Torque 1 Torque 2 Torque 3 Torque 4
400
200
Torque (N-m)
0
-200
-400
-600
-800
-1000
-1200
0
0.01
0.02
0.03
0.04
0.05
0.06
Tiempo (seg)
Figura 13. Comportamiento de los torques en el tiempo para la trayectoria 1
60
COMPORTAMIENTO DE LOS TORQUES EN TIEMPO PARA LA TRAYECTORIA 2
15 Torque Torque Torque Torque
10
1 2 3 4
Torques (N-m)
5
0
-5
-10
-15
-20
0
0.5
1
1.5
2
2.5
3
Tiempo (seg)
Figura 14. Comportamiento de los torques en el tiempo para la trayectoria 2 COMPORTAMIENTO DE LOS TORQUES EN TIEMPO PARA LA TRAYECTORIA 2
Torque Torque Torque Torque
8
1 2 3 4
6
Torques (N-m)
4
2
0
-2
-4 1.44
1.46
1.48
1.5
1.52
1.54
1.56
1.58
Tiempo (seg)
Figura 15. Comportamiento de los torques en t=1.5 seg para la trayectoria 2
61
COMPORTAMIENTO DE LOS TORQUES EN TIEMPO PARA LA TRAYECTORIA 2
Torque 1 Torque 2 Torque 3 Torque 4
10 8
Torques (N-m)
6 4 2 0 -2 -4 -6
0.01
0.02
0.03
0.04
0.05
0.06
Tiempo (seg)
Figura 16. Comportamiento de los torques en t= 0 seg para la trayectoria 2
3.5 Modelo dinámico de los motores DC El propósito en el desarrollo del modelo matemático del motor es expresar el voltaje aplicado a la armadura en función de la velocidad del motor. Dos ecuaciones de balance pueden ser desarrolladas considerando las características eléctricas y mecánicas del sistema. Características Eléctricas El circuito equivalente eléctrico de un motor DC es ilustrado en la Figura 17Figura 17 . Este puede ser representado por una fuente de voltaje (Va) a través de la bobina de la armadura. El equivalencia eléctrica de la bobina de la armadura puede ser descrita por una inductancia (La) en serie con una resistencia (Ra) en serie con un voltaje inducido (Vc) el cual se opone a la fuente de voltaje. El voltaje
62
inducido es generado por la rotación de la bobina eléctrica a través de las líneas fijas de flujo de los imanes permanentes. Este voltaje es comúnmente llamado voltaje contraelectromotriz.
motor
+
+ Ra
La
Vemf
Va
J
_
_
aplicado
Kf Figura 17 Representación eléctrica de los motores DC del SCARA
Una ecuación diferencial para el circuito equivalente puede ser derivada usando la ley de voltaje de Kirchoff alrededor del lazo eléctrico. La ley de voltaje de Kirchoff establece que la suma de todos los voltajes alrededor en el lazo son cero, es decir: Va
VRa
VLa Vemf
0
(Ec
34)
De acuerdo con la ley de Ohm, el voltaje a través de una resistencia puede ser expresado como: VRa
i a Ra
(Ec
35)
donde i a es la corriente de armadura. La caída de potencial a través de un inductor es proporcional al cambio de la corriente por la bobina con respecto al tiempo y puede ser expresada como:
VLa
La
63
d ia dt
(Ec
36)
donde La es la inductancia de la bobina de la armadura. Finalmente, el voltaje contraelectromotriz puede ser estimado como: Vemf
Kv w a
(Ec
37)
donde kv es la constante de velocidad determinada por la densidad de flujo del imán permanente, la reluctancia de la base de hierro de la armadura y el numero de vueltas de la bobina de la armadura. Por otro lado, wa es la velocidad angular de la armadura. Substituyendo las Ec 35, Ec 36 y Ec 37 en la Ec 34 se obtiene la siguiente ecuación diferencial (Ec 38).
Va
i a Ra
La
d ia dt
Kv w a
0
(Ec
38)
En el presente documento no se tuvo en cuenta el modelo de los actuadores antes presentado debido a que no se conocen sus parámetros eléctricos, por que la variable manipulada por el controlador es directamente Torque como lo muestra el Modelo Dinámico Directo e Inverso en el ANEXO 5.
64
Con formato: Sangría: Izquierda: 0 cm, Interlineado: sencillo
CAPÍTULO IV
4.
CONTROL DINÁMICO DEL ROBOT INDUSTRIAL
4.1 Introducción. El desacoplamiento de la estrategia de control se puede lograr mediante el uso de la dinámica inversa de control, control no lineal, diseño de Lyaponov, o linealización por retroalimentación de los sistemas dinámicos no lineales de los manipuladores.
A medida que se incrementan los grados de libertad de la cadena cinemática, las ecuaciones dinámicas del manipulador se vuelven más complejaso. Por lo tanto es muy difícil calcular la inversa de una ecuación dinámica general de un robot en serie multi-eslabón.
Aunque hay muchos algoritmos de control de sistemas no lineales, la mayoría de ellos son de difícil aplicación en tiempo real, debido a la ley de control, ya que son computacionalmente complejos. Por tal motivo se implementó el Computer Torque Control CTC, el cual es una técnica de linealización por retroalimentación lineal.
65
Con formato: Derecha, Sangría: Izquierda: 0 cm, Primera línea: 0 cm, Interlineado: sencillo, Numerado + Nivel: 1 + Estilo de numeración: 1, 2, 3, … + Iniciar en: 1 + Alineación: Izquierda + Alineación: 0,21 cm + Tabulación después de: 0,85 cm + Sangría: 0,85 cm, Punto de tabulación: 0 cm, Lista con tabulaciones + No en 0,85 cm
4.2 Computer Torque Control CTC. Durante años se han propuesto muchos tipos de esquemas de control de robot industrial. Como pasa en la mayor parte de ellos pueden ser considerados como los casos especiales de la clase de controladores por torque calculado. El par calculado, al mismo tiempo, es de uso especial para la linealización por retroalimentación de sistemas no lineales, que ha ganado la popularidad en la teoría de sistemas moderna [16],[13]. De hecho, un modo de clasificar esquemas de control de robot es de dividirlos en " par calculado " o "par no calculado". Este sistema de control nos permite conseguir reguladores de robot muy eficaces, proporcionando un marco para unir el control clásico independiente y algunas técnicas de diseño modernas.
4.2.1 Derivación de Lazo Interior por realimentación La dinámica de brazo de robot es: M (q)q V q, q
Fv q
Fd (q ) G (q)
d
(Ec 39)
O M (q)q N q, q
Con la variable articular
q(t )
d
,
R n , (t ) par de control de rotación, y
(Ec 40)
d
(t ) una
perturbación. Si esta ecuación incluyese la dinámica de actuador de motor, entonces
(t ) seria un voltaje de entrada.
Supongamos que ha seleccionado una trayectoria deseada q d t
para el
movimiento de brazo, como se observóo en el capitulo 2. Con el fin de asegurarse del seguimiento de la trayectoria en el espacio articular, definimos una salida o el error de seguimiento como:
66
Con formato: Sin viñetas ni numeración
et
qd t
(Ec 41)
qt
Para demostrar la influencia de la entrada
(t ) sobre el error de seguimiento,
derivemos dos veces y se obtiene: e
q d
q
e qd
q
Solucionando q en la (Ec 40)[] y sustituyendo en la (Ec 41) de los rendimientos se obtiene:
M 1 (N
e qd
d
)
(Ec 42)
Definiendo la función de control de entrada
u
qd
M 1 (N
)
(Ec 43)
Y la función perturbación
w
Podemos definir un estado x(t )
1
M
d
(Ec 44)
R 2 n por
x
e e
(Ec 45)
Y escribiendo el error de seguimiento dinámico como
67
d e dt e
0 I e 0 0 e
0 u I
0 w I
(Ec 46)
Este es un sistema lineal de error en la forma canónica de Brunovsky [24] que consiste en n pares de doble integradores 1/s2, uno por cada conjunto. Es conducida por la entrada de control U(t) y la perturbación w(t). Pero hay que tener en cuenta que esta derivación es un caso especial del procedimiento de linealización antes mencionado.
La (Ec 43) puede ser reescrita de la forma: M (qd
(Ec 47)
u) N
A esto se le llama Ley de Control del Par Calculado. La importancia de estas manipulaciones son las siguientes. No ha habido transformaciones de espacio estado en las (Ec 39)[], (Ec 46)[]. Por lo tanto, si queremos seleccionar un control u(t) que estabilice (Ec 46)[Ec 46]. [], de forma que e(t) sea a cero, entonces el control no lineal de la entrada dada por
t (Ec 47) provocará la siguiente
trayectoria en el brazo robot Ec 39[]. De hecho, Sustituyendo la (Ec 47)] en (Ec 40)[] en los rendimientos se tiene la (Ec 48). qM
N
d
M (qd
u)
N
o
e u M
1 d
,
(Ec 48)
que es exactamente la (Ec 46).
La estabilización (Ec 46)[] no es difícil. De hecho, la transformación no lineal (Ec 43)] ha convertido un problema no lineal en un problema de diseño mas sencillo
68
para un sistema lineal que consiste en la n subsistemas desacoplados, que obedece a las leyes de Newton. Sistema lineal
Lazo no lineal interior
N(q,q)
qd
M(q)
Manipulador
-
u
q q
Regeneración de lazo externo
Figura 18 Esquema de Computer Torque Control, mostrando a lazos internos y externos
El esquema de control final se muestra en la Figura 18. Es importante notar que ésto consiste en un lazo interior no lineal más una señal de control externa u(t). Nosotros veremos varios caminos para seleccionar la u (t). Ya que la u (t) dependerá de la q (t) y la q. (t), el lazo externo será un bucle de realimentación. En general, podemos seleccionar un compensador dinámico H (s) de modo que:
U ( s)
H ( s) E ( s)
(Ec 49)
La H (s) puede ser seleccionada para el comportamiento de lazo-cerrado. Según (Ec 48)[], el sistema de error de lazo-cerrado entonces tiene la función de transferencia
T ( s)
s2I
69
H (s) .
(Ec 50)
Es importante saber que el torque calculado depende de la dinámica del robot, el cual es llamado a veces control dinámico inverso, de hecho en (Ec 47) se muestra que el t es calculado por medio de la substitución de q d u por q en (Ec 40)[]; resolviendo la dinámica inversa del problema del robot. Todas las salvedades asociadas con el problema de inversión, incluyendo los problemas provenientes de cuando el sistema no tiene una mínima fase de ceros, aplican aquí.
Existen muchas formas de calcular (Ec 47)]. La multiplicación formal de matrices en cada muestra de tiempo debe evitarse. En algunos casos las expresiones pueden ser resueltas analíticamente. Una buena manera de calcular el
t es por
medio del uso eficiente de la formulación de la dinámica inversa de Newton-Euler con qd
u en lugar de q(t ) .
4.2.2 Diseño del lazo externo (PD) Una forma de seleccionar la señal de control u(t) es por medio del uso de una realimentación de la de la parte proporcional y derivativa (PD), es importante aclarar que no se tuvo en cuenta la parte integral pues este agrega un integrador mas a la función de transferencia que modela la respuesta del manipulador en lazo cerrado, lo cual se traduce en un polo mucho más cercano a la zona de inestabilidad.
u
K v e K p e
(Ec 51)
Luego la entrada neta del manipulador robótico resulta en:
M (q)(q d
K v e
K p e)
N (q, q )
Este mismo controlador se muestra en la Figura 19 con Ki = 0.
70
(Ec 52)
qd
+
-
Kp
q
qd
q
Modelo Diná Dinámico Inverso SCARA
+
q q
q
Kv
q d
Modelo Diná Dinámico Directo SCARA
-
Figura 19 Esquema de control PID por par calculado
El error dinámico del lazo cerrado es:
e K v e K p e
(Ec 53)
w
O en la forma de espacio-estado:
d e dt e
0
I
Kp
Kv
e e
0 I
w
(Ec 54)
El polinomio característico del bucle es :
c
( s)
s2I
Kv s
Kp
71
(Ec 55)
4.2.3 Selección de las ganancias (PD) Es usual tomar el producto diagonal de las matrices n*n tal que:
Kv
diag k vi , K p
(Ec 56)
diag k pi ,
Entonces:
n c
s2
( s)
(Ec 57)
k vi s k pi
i 1
Y el sistema respecto al error es estable asintóticamente cuando K vi y K pi son positivos. De esta forma, mientras que la perturbación w(t) esté limitada, también lo estará el error e(t). Y examinando la (Ec 44) se puede concluir que M-1 tiene límite superior. Esto indica que el limitante de w(t) es equivalente al limitante de d
(t ) .
Es importante notar que a pesar de seleccionar los resultados diagonales de las matrices de ganancia de PD en control desacoplado en el nivel de repetición exterior no resulta en una estrategia de unión de control desacoplado. Esto se debe a que al multiplicar por M (q) y la adición de términos no lineales de salida .
N (q, q) en el bucle interno reparte la señal u(t ) a todas las juntas. Así que es .
necesaria la información de todas las posiciones q(t ) y las velocidades q(t ) de las articulaciones para estimar el algoritmo de control (t ) para cualquier articulación especifica.
La forma estándar del polinomio característico de segundo orden es:
p( s )
s2
2
72
n
s
2 n
(Ec 58)
Donde
es el factor de amortiguamiento y
n
la frecuencia natural. De esta forma,
el rendimiento deseado en cada componente del error e(t ) puede ser alcanzado al seleccionar ganancias del PD como:
2 n
k pi
y
k vi
2
n
(Ec 59)
s
Con los factores de amortiguamiento y las frecuencias naturales deseadas para los errores en cada articulación. Puede ser mas útil seleccionar las respuestas deseadas al final del brazo que en la base, donde las masas que deben ser movidas son más pesadas. No es deseable para el robot presentar overshoot, porque puede causar impacto, por ejemplo, si una trayectoria deseada repercute en una colisión del TCP en la superficie de trabajo. Es por esta razón, que las ganancias del PD son usualmente escogidas para amortiguamiento crítico
1.
En este caso:
k vi
2 k pi ,
La selección de la frecuencia natural
n
k pi
2
k vi2 4
(Ec 60)
, la cual es la que gobierna la velocidad de
respuesta en cada componente de error, debe ser alta para respuestas rápidas y se selecciona dependiendo de los objetivos de rendimiento. Por eso las trayectorias deseadas deben ser tenidas en cuenta al seleccionar
n
. De esta
forma se realizaron unas pruebas por cada eje, manipulado los valores de las ganancias de proporción a partir de un valor unitario y nulo en la ganancia de velocidad hasta obtener respuesta críticamente amortiguada
1 . Luego se
variaron las ganancias de velocidad hasta obtener una respuesta en error de seguimiento articula y cartesiano del orden de 1E-003, los valores finales que se estimaron para la sintonía del controlador PD en el lazo externo se muestran a continuación en la Tabla 3:
73
Articulación 1 Articulación 2 Articulación 3 Articulación 4
KP 125000 140000 150000 120000
KV 250 200 1000 700
Tabla 3. Parámetros de sintonización del Controlador CTC
4.2.4 Respuesta del Computer Torque Control. Como se observa en la Figura 20, los errores del controlador se encuentran en niveles muy bajos (alrededor del 0.0001 en su caso máximo) por tal motivo las graáficas se muestran superpuestas, sin embargo en la Figura 21 se muestra un acercamiento para cada articulación para la primera trayectoria. El controlador por par calculado va mejorando su respuesta según las posiciones deseadas en el eje z se alejen de las crestas o valles. Para la trayectoria 2, el cambio de dirección súbito, produce que el controlador PD instalado con el CTC presente errores más grandes, como se observa en la Figura 22 y Figura 23, es importante aclarar que en el capitulo siguiente se profundizaráa mas acerca de los errores de seguimiento del controlador, sin embargo en la Figura 24, se observa claramente que el error de seguimiento es mayor cuando se obtiene un cambio de dirección de 90 grados a una velocidad lineal del TCP de 1.41 cm/seg des pues de haber recorrido 2.12 cm. Este error se debe a la misma inercia que lleva el
74
COMPARACION ENTRE q real (rad) - q deseado (rad) PARA LA TRAYECTORIA 1
5 Pos Pos Pos Pos Pos Pos Pos Pos
4 zona 3
q real (rad) - q deseado (rad)
3
2
- deseado q1 - deseado q2 - deseado q3 - deseado q4 - real q1 - real q2 - real q3 - real q4
zona 1
1
0 zona 4
-1 zona 2
-2
-3
0
0.5
1
1.5 Tiempo (seg)
2
2.5
Figura 20. Comparación entre q real y q deseado para la Trayectoria 1
75
3
COMPARACION ENTRE q2 deseado (rad) - q2 real (rad) PARA LA TRAYECTORIA 1
COMPARACION ENTRE q1 real (rad) - q1 deseado (rad) PARA LA TRAYECTORIA 1
-2.3473
1.2249
Pos - deseado q2 Pos - real q2
1.2248
-2.3473
1.2248
-2.3473 q2 real (rad) - q2 deseado (rad)
q1 real (rad) - q1 deseado (rad)
Pos - deseada q1 Pos - real q1
1.2248
1.2248
1.2248
1.2247
-2.3474
-2.3474
-2.3474
-2.3474
1.2247 -2.3474
1.5
1.5
1.5
1.5
1.5
1.5
1.5
1.5
COMPARACION ENTRE q3 real (rad) - q3 deseado (rad) PARA LA TRAYECTORIA 1
1.5
1.5
COMPARACION ENTRE q4 real (rad) - q4 deseado (rad) PARA LA TRAYECTORIA 1
Pos - deseada q3 Pos - real q3
4.2643
1.5 Time (seg)
Tiempo (seg)
Pos - deseado q4 Pos - real q4
0.26 0.24 0.22 q4 real (rad) - q4 deseado (rad)
q3 real (rad) - q3 deseado (rad)
4.2642
4.2642
4.2641
4.2641
0.2 0.18 0.16 0.14 0.12 0.1
4.264
0.08 1.5
1.5
1.5
1.5
1.5
1.5
1.5
1.49
Time (seg)
1.495
1.5
1.505
1.51
1.515
1.52
Tiempo (seg)
Figura 21. Comparación de q real y q deseado para la Trayectoria 1 en cada una de las zonas
76
1.525
COMPARACION ENTRE q real (rad) - q deseado (rad) PARA LA TRAYECTORIA 2
Pos Pos Pos Pos Pos Pos Pos Pos
4
zona 3
q real (rad) - q deseado (rad)
3
2
- deseada q1 - deseada q2 - deseada q3 - deseada q4 - real q1 - real q2 - real q3 - real q4
zona 1
1
0 zona 4
-1 zona 2 -2 0
0.5
1
1.5
2
2.5
3
Tiempo (seg)
Figura 22. Comparación entre q real y q deseado para la Trayectoria 2
mecanismo, pero el controlador PD-CTC no consigue exactamente la trayectoria 2, es decir no garantiza la posición deseada específica, sino con una desviación pequeña, esto es debido a que el controlador instalado no presenta la acción integral, sino la proporcional y derivativa. La parte proporcional trata de compensar proporcional al error entre el punto de control o posiciones deseadas y la variable que se controla, en nuestro caso la posición en el espacio de la tarea del TCP, mientras que la parte derivativa brinda la capacidad de anticipar hacia donde se dirige el proceso, es decir, “ver hacia adelante”, mediante el calculo de la derivada del error, pero en ningún momento llega a la consigna deseada.
77
COMPARACION ENTRE q real (rad) - q deseado (rad) PARA LA TRAYECTORIA 2
COMPARACION ENTRE q real (rad) - q deseado (rad) PARA LA TRAYECTORIA 2
Pos - deseada q1 Pos - real q1
Pos - deseada q2 Pos - real q2
-1.8503
1.4081
-1.8503
q real (rad) - q deseado (rad)
q real (rad) - q deseado (rad)
-1.8503
1.4081
1.4081
1.4081
-1.8503 -1.8503 -1.8503 -1.8503 -1.8503
1.4081 -1.8503 -1.8503
1.4081 1.499
1.499
1.499
1.499
1.499
1.499
1.4991
1.499
1.499
1.499
Pos - deseada q3 Pos - real q3
1.499
1.499
1.499
1.499
1.499
Pos - desead q4 Pos - real q4
0.4002
3.5838
0.4001 q real (rad) - q deseado (rad)
q real (rad) - q deseado (rad)
1.499
COMPARACION ENTRE q real (rad) - q deseado (rad) PARA LA TRAYECTORIA 2
COMPARACION ENTRE q real (rad) - q deseado (rad) PARA LA TRAYECTORIA 2
3.5838
1.499
Tiempo (seg)
Tiempo (seg)
3.5838
3.5838
3.5838
0.4001
0.4
0.3999
0.3999
3.5838 0.3998
3.5838 1.4998 1.4999 1.4999
1.5
1.5
1.497
1.5001 1.5001 1.5002 1.5002 1.5003 1.5003
1.498
1.499
1.5
1.501
1.502
Tiempo (seg)
Tiempo (seg)
Figura 23. Comparación de q real y q deseado para la Trayectoria 2 en cada una de las zonas
78
1.503
COMPARACION ENTRE Pos - deseada (m) Y Pos - real (m) PARA LA TRAYECTORIA 2
0.38
Posicion en el eje y (m)
0.375
0.37
0.3652 0.3651
0.365
0.365 0.3649
0.36
0.3648 0.3649 0.3649 0.365
0.365
0.355
0.35 0.348
0.35
0.352
0.354
0.356
0.358
0.36
0.362
0.364
0.366
0.368
Posicion en el eje x (m)
Figura 24. Posición real y Deseado para la Trayectoria 2 en el espacio de la tarea
79
CAPÍTULO V
5.
RESULTADOS Y ANÁLISIS DE RESULTADOS
Con formato: Título 1, Centrado, Sangría: Izquierda: 0 cm, Primera línea: 0 cm, Numerado + Nivel: 1 + Estilo de numeración: 1, 2, 3, … + Iniciar en: 1 + Alineación: Izquierda + Alineación: 0,21 cm + Tabulación después de: 0,85 cm + Sangría: 0,85 cm, Punto de tabulación: 0 cm, Lista con tabulaciones + No en 0,85 cm Con formato: Normal Con formato: Español (alfab. internacional)
Todos los resultados y gráficos obtenidos en la presente investigación que se han venido mostrando y comentando en cada uno de los capítulos anteriores, y que culminan con este capitulocapítulo se realizaroon en Matlab/Simulink®, la Figura 25 muestra el diagrama del manipulador en el entorno grafico de programación antes especificado, en el se muestra claramente cada una de las conexiones realizas para el modelado del sistema.
80
Con formato: Centrado
Figura 25 Simulación del Robot SCARA de Cuatro Grados de Libertad
Con formato: Centrado
En la sección anterior se presentaron las graficas comparativas de las trayectorias seguida por el manipulador en simulación y deseadas tanto en el espacio articular como el espacio cartesiano para ambas trayectorias mediadas por el algoritmo de control PD-CTC. En esta sección se presentan los valores de los índices de desempeño IAE para los errores de posición articular y cartesiano.
El IAE (Integral of the Absolut Error) es el criterio que acumula el valor absoluto del error extendiendo la integral desde el tiempo en el cual se produce el cambio en el Set Point o en la perturbación (t=0) hasta un tiempo posterior muy largo (t= Este criterio de optimización esta dado por la siguiente ecuación (Ec 61).
81
).
IAE= e(t ) dt
(Ec 61)
0
La interpretación geométrica del IAE consiste en la suma de las áreas debajo la curva en valor absoluto, como se observa en la Figura 26, donde el set point en este caso en la posición deseada articular y cartesiana en el instante especifico de la trayectoria.
Con la ayuda de Matlab/Simulink® se obtuvieron los datos relacionados en el tiempo de la variable de proceso controladas ante el cambio continuo del Set point, estos datos fueron exportados del entorno Simulink® al Workspace de Matlab y se realizaron los gráficos en Matlab®.
Error Error (+ )
Set
(-)
Point
(-)
Variable de proceso
Tiempo Figura 26. Integral del Valor Absoluto del error en el tiempo real
A continuación se presenta la Figura 27, que muestra el comportamiento de los errores en seguimiento de trayectoria articular para el Computer Torque Control en la trayectoria 1 antes mencionada. Aquí se observa que el máximo error articular
82
se presenta en la articulación 2 con valor de 2.0909x10-3 rad al cabo de 0.004 seg, y al cabo de 0.017 seg, este error se reduce a 6.6962x10-4 rad lo que equivale aproximadamente en un 67.97%. Con respecto a la articulación 1 el valor máximo de error fue de -1.9208x10-3 rad al mismo tiempo de 0.004 seg, y al cabo de 0.017 seg, este error se reduce a -3.1421x10-4 rad lo que equivale aproximadamente en un 83.6417%.
En la Figura 28 se muestra el comportamiento de los errores en seguimiento de trayectoria cartesiana para el Controlador por Torque Calculado de igual forma para la trayectoria 1, donde se puede ver claramente que el error mas grande se presenta a lo largo del eje en el cual la elipse tiene mayor diámetro. -3
2.5
ERRORES ARTICULARES PARA LA TRAYECTORIA 1
x 10
Errores Errores Errores Errores
2
q1 q2 q3 q4
Errores Articulares (rad)
1.5
1
0.5
0
-0.5
-1
-1.5
-2
0
0.01
0.02
0.03
0.04
0.05
0.06
0.07
0.08
Tiempo (seg)
Figura 27. Errores articulares para la trayectoria 1
En dicha gráfica se alcanza un error máximo de 9.766x10-4 m en el eje x en el mismo tiempo de 0.004 seg, y un porcentaje de reducción de error del 80.7147% al cabo de los mismos 17 ms.
83
El anterior resultado es muy importante, porque nos permite concluir que la curva de errores cartesianos y articulares presentan máximos locales en el mismo instante de tiempo, debido a que no necesariamente son puntos máximos globales de toda la trayectoria, además se puede concluir que el error cartesiano de seguimiento se incrementa en la medida en que se le permite al manipulador desarrollar una trayectoria masmás larga a lo largo del eje respectivo. Estos resultados son validos para las dos trayectorias especificadas.
-4
10
ERRORES CARTESIANOS PARA LA TRAYECTORIA 1
x 10
Errores Cartesianos (m)
Errores x Errores y Errores z
5
0
-5
0
0.01
0.02
0.03
0.04
0.05
0.06
0.07
0.08
Tiempo (seg)
Figura 28. Errores cartesianos para la trayectoria 1
La Figura 29, nos permite observar que los mayores valores de errores absolutos acumulados se presentan en el eje z, donde el manipulador tiene que seguir una trayectoria sinusoidal con amplitud máxima de 0.2 m al cabo de 1.5 seg, punto
84
donde se presenta la máxima razón de cambio de los errores absolutos articulares para la trayectoria 1. Inicialmente hay un incremento súbito en todos los errores absolutos articulares, pero a medida que el controlador sigue la trayectoria específica los tres primeros errores absolutos articulares se estabilizan en cuanto a su razón de cambio, sin embargo el controlador nunca encuentra exactamente el punto deseado, debido a su misma ley de control, pues como bien se conoce, la parte integral del controlador PID es la encargada de eliminar Offset, es decir el error en estado estable. .
-3
ERRORES ABSOLUTOS ARTICULARES - TRAYECTORIA 1
x 10 7
iaeq1 iaeq2 iaeq3 iaeq4
6
5 -5
-5
x 10
Error absoluto
x 10 4
3
2
3
6
2
5.5
1
5
0
4.5 0
0.05
2.9996 2.9997 2.9998
0.1
1
0
-1
0
0.5
1
1.5
2
2.5
3
Tiempo (seg)
Figura 29. Errores cartesianos para la trayectoria 1
Para la trayectoria dos es importante tener en cuenta que aquí en cambio de dirección es más repentino, cuando el Tiempo es de 1.511 se obtiene un Error máximo cartesiano en el eje x de -6.6747x10-6 m, al cabo de 0.009 se reduce a 1.386 x10-6, lo que corresponde un porcentaje de decaimiento del error del 79.2%,
85
pero el comportamiento se conserva en cuanto al error en estado estable, debido a que este permanece constante fuera del punto donde hay cambio de dirección de la trayectoria deseada o de consigna.
-5
2.5
ERRORES CARTESIANOS PARA LA TRAYECTORIA 2
x 10
-6
-5
x 10
x 10
2
2
2
1.5 1.5
0
1 -2
0.5 Errores Cartesianos (m)
Errores x Errores y Errores z
1
0
-4
-0.5 0.5
-6
-1 0
0.02
0.04
0.06
1.5
0.08
1.52
0
1.54
1.56
1.58
-9
x 10 3 2
-0.5
1 0
-1
-1 -1.5
0
0.5
1
1.5
1.9927 2
1.9928
1.9928 2.5
3
Tiempo (seg)
Figura 30 Errores cartesianos para la trayectoria 2
De igual forma cuando se trata de coordenadas articulares para la segunda trayectoria se observa claramente que la segunda articulación presenta errores mas grande que las otras, y que al inicio de la trayectoria se obtienen errores de 2.5x10-5 rad en el caso mas crítico, pero a medida que el controlador ejerce su trabaja minización de dichos errores, hasta que en 1.5 seg, se obtiene el otro pico en cuanto al error producto del cambio de dirección más no de la velocidad. En la Tabla 4, se ve que el controlador CTC se destaca para la trayectoria 1 pues presenta los índices de error absoluto más pequeños tanto en coordenadas articulares como cartesiana, pero presenta errores puntuales más grandes en esta
86
trayectoria, mientras que para la trayectoria de cambio de sentido y dirección este presenta un IAE ligeramente superior, pero valores de picos más bajos.
-5
8
ERRORES ARTICULARES PARA LA TRAYECTORIA 2
x 10
-5
-5
x 10
x 10
6
1
4
0.5
2
0
0
-0.5
-2
-1
Errores Errores Errores Errores
6
Errores articulares (rad)
4
2
-4
-1.5 1.5
0.01 0.02 0.03 0.04 0.05 0.06 -8 x 10 4
0
-2
q1 q2 q3 q4
1.52
1.54
2 0
-4
-6
-2 0.12 0
0.5
0.125
0.13
1
1.5
2
2.5
Tiempo (seg)
Figura 31. Errores articulares para la trayectoria 2
CTC Trayectoria 1
Trayectoria 2
IAE
IAEQ1
5.2694e-005
0.0026619
Articulares
IAEQ2
5.8618e-005
0.006139
IAEQ3
4.7653e-005
0.00067209
IAEQ4
0.006988
0
IAE
IAEx
2.3544e-005
0.0019172
Cartesianas
IAEy
7.9362e-006
0.00057782
IAEz
0.006988
0
87
3
Tabla 4. Índices de desempeño para el Controlador CTC
Los valores en los que el IAE en nulo para la segunda trayectoria, son producto de que la posición a lo largo del eje z no se vario para la segunda trayectoria.
88
CONCLUSIONES
Después de haber logrado los objetivos específicos planteados en la presente investigación, se pueden mencionar las siguientes conclusiones:
El modelo cinemático representa al manipulador sin considerar efectos debido a fricciones, gravedad, inercia y peso,; se verificóo su desempeño con los parámetros Denavit-Hartenberg (distancias y ángulos) y su simulación permitió verificar el movimiento del manipulador tanto pasando del espacio cartesiano al articular o viceversa con la ayuda de la cinemática directa e inversa. Se realizó un programa capaz de determinar el modelo dinámico inverso de cualquier robot hasta un núumero máximo de cinco grados de libertad,; aes éeste le es indiferente si la articulación es prismática o rotacional. Este algoritmo utiliza la formulación Recursiva de Newton Euler para el cómputo de las relaciones dinámicas requeridas. El modelo dinámico obtenido permite incorporar los efectos reales del sistema para que se aproxime al manipulador real. Este se simuló en Matlab con parámetros de literatura, pero este permite la incorporación de parámetros reales de cualquier manipulador SCARA de cuatro grados de libertad. Se determinó que para el control y seguimiento de trayectoria se puede utilizar un controlador lineal por retroalimentación con compensador basado en el modelo dinámico no lineal tipo CTC (Computer Torque Control) con un buen índice de desempeño. Se desarrolló una comparación del desempeño IAE del controlador de seguimiento de trayectoria por medio de la simulación de dos trayectorias diferentes, lo que indicaría que en la práctica este controlador puede ser utilizado confiando en tener una buena respuesta por parte del robot para las dos trayectorias simuladas.
89
TRABAJO FUTURO
Después de esta experiencia, se recopilaron una serie de recomendaciones con el fin de hacer más fácil la realización de proyectos que involucren el modelamiento y simulaciones de controladores en manipuladores industriales:
*
Trabajar el modelo con valores reales del manipulador, puesto que los
parámetros que se utilizaron en la simulación no corresponden al manipulador que se encuentra en el Laboratorio de Robótica de la Universidad del Norte.
*
Incluir en el modelo dinámico del manipulador, el modelo de cada uno de
los actuadortes, con el fin de manipular la corrienteel voltaje aplicadao al motor para garantizar las posiciones deseadas.
*
Iimplementara en simulación una estrategia de control predictivo con el fin
de confrontar la estrategia de control implementada en este trabajo en términos de integral absoluta de error e seguimiento.
*
Desarrollar la ingeniería requerida para la implementación de estos
algoritmos de control en el manipulador SCARA. .
90
REFERENCIAS [1] R. J. Anderson, “Passive computed torque algorithms for robots,” IEEE Conf. on Decision and Control, pp. 1638-1644, 1989. [2] R. Andersson. Computer architectures for robot control: a comparison and a new processor delivering 20 real M_ops. In Proc. IEEE Int. Conf. Robotics and Automation, pages 1162.1167, May 1989. [3] S. Arimoto and F. Miyasaki, “stability and robustness of PID feedback control for robot manipulators,” 1st int. Symp. Robotics Research, pp. 783-789, 1983. [4] K. J. Astrom, “Theory and applications of adaptive control – a survey,” Automatica, vol. 19, pp. 471-486, 1983. [5] Barrientos, Peñín, Balaguer, Aracil., McGraw Hill, 1997. [4] ARRETO Jair, RUBIO Martín.
Controlador DMC Escalable Para Procesos
SISO de Ganancia No Lineal, (2003).
143p.
Trabajo de Grado (Ingeniería
Electrónica). Universidad del Norte. Departamento de Ingeniería Electrónica. [6] S.P. Boyd and G.H Barrat, linear controller Designs: Limits of Performance. N.J: Prentice Hall-Englewood Cliffs, 1991. [6] CHAMORRO Ramiro, MEJIA Pablo.
Diseño e Implementación de un
Controlador Predictivo Tipo GPC con Compensación de Tiempo Muerto Basado en el Algoritmo de Dahlin Para un Reactor Tipo CSTR, (2005). 71p. Trabajo de Grado (Ingeniería Mecánica). Universidad del Norte. Departamento de Ingeniería Mecánica. [7] J. J. Craig, Adaptive Control of Mechanical Manipulators. Addison-Wesley, 1988. [8] CHAMORRO Ramiro, MEJIA Pablo.
Diseño e Implementación de un
Controlador Predictivo Tipo GPC con Compensación de Tiempo Muerto Basado en el Algoritmo de Dahlin Para un Reactor Tipo CSTR, (2005). 71p. Trabajo de
91
Con formato: Inglés (Estados Unidos)
Grado (Ingeniería Mecánica). Universidad del Norte. Departamento de Ingeniería Mecánica. [6] CHAMORRO Ramiro, MEJIA Pablo.
Diseño e Implementación de un
Controlador Predictivo Tipo GPC con Compensación de Tiempo Muerto Basado en el Algoritmo de Dahlin Para un Reactor Tipo CSTR, (2005). 71p. Trabajo de Grado (Ingeniería Mecánica). Universidad del Norte. Departamento de Ingeniería Mecánica. [9] Denavit, J. and Hartenberg, R. "A Kinematic Notation for Lower Pair Mechanisms Based on Matrices," Transactions of the ASME, Journal of Applied Mechanics, 22, pp. 215–221, 1955 [10] S. Dubowsky and D. T. DesForges, “The application of model-referenced Adaptive control to robotic manipulators,” Journal of dynamic Systems, Measurement, and Control, vol. 101, pp. 193-200, 1979. [11] DURANGO Néstor, MARTELO Gabriel. Implementación de un control DMC con Predictor de Smith en un reactor tipo CSTR modificado 100p (2005). Trabajo de Grado (Ingeniería Mecánica).
Universidad del Norte.
Departamento de
Ingeniería Mecánica. [12] R. A. Freeman, M. Krstic, and P. Kokotovic, “Robustness of adaptive nonlinear control to bounded uncertainties,” IFAC World Congress, vol. F, 1996. [13] Gilbert, E.G., and I.J.Ha, “An approach to nonlinear feedback control with applications to robotics,” IEEE Trans. Syst. Man Cybern., vol 5, 1989. [14] S. Gopalswamy and J. K. Hedrick, “robust adaptive control of multivariable Nonlinear systems,” In Proc. Of American Control Conference, 1990. [15] Hashimoto, Koichi, Kawabata, Masashi, Kimura and Hidenori, “H infinity Controller design for robust manipulator control,” Proc 92 Jpn USA Symp Flexible Automation, pp.591-594, 1992. [16] Hunt, L.R., R.Su, and G.Meyer, “Global transformations of nonlinear systems,” IEEE Trans. Autom. Control, vol. AC-28, no. 1, pp. 24–31, Jan. 1983. [17] P. A. Ionnou and P. V. Kokotovic, “Instability analysis and improvement of robustness of adaptive control,” Automatica, vol.20, pp. 583-594, 1984.
92
Con formato: Inglés (Estados Unidos)
[18] R. Johansson, “Adaptive control of robot manipulator motion,” IEEE Trans., vol. 6, pp. 483.490, 1990. [19] M. Kabuka and R. Escoto. Real-time implementation of the Newton-Euler equations of motion on the NEC µPD77230 DSP. Micro Magazine, 9(1):66.76, February 1990. [20] P. K. Khosla and T. Kanade, “Experimental evaluation of nonlinear feedback and feedforward control schemes for manipulators,” Int. J. Robotics Research, vol. 7, pp. 18-28, 1988. [21] K. S. Kumpati, O. Romeo, and D. Peter, eds., Advanced Adaptive Control. USA: IEEE Press, 1991. [22] N. H. McClamorch and D. Wang, “Feedback stabilization and tracking of constrained robots,” IEEE Transactions on Automation and Control, vol. 33, pp. 419-426, 1988. [23] K. S. Narendra and A. M. Annaswamy, Stable Adaptive Systems, USA: Prentice-Hall International Inc., 1989. [24] R. H. Middleton and G. C Goodwin, “Adaptive computed torque control for Rigid link manipulators,” Systems and Control Letter, vol. 10, no. 1, pp. 9-16, 1988. [25] K. S. Narendra and R. V. Monopoli, Applications of Adaptive Control. New York: Academic Press, 1980. [26] Paul, R.P., Robot Manipulators. Cambridge, MA: MIT Press, 1981. [27] K. Przytula and J. Nash. A special purpose coprocessor for robotics and signal processing. In D. Lyons, G. Pocock, and J. Korein, editors, Workshop on Special Computer Architectures for Robotics, pages 74.82. IEEE, Philadelphia, April 1988. In conjunction with IEEE Conf. Robotics and Automation. [24] RAMIREZ Carlos, RUIZ Marcelo, LAGO Carlos. Controladores por Matriz Dinámica Aplicados a Procesos de Refinación. ISA Show Brasil (1995), 13p. [28] J. S. Reed and P.A. Ioannou, “Instability analysis and robust adaptive control of robotic manipulators,” IEEE transactions on robotics and automation, vol. 5, pp.381-386, 1989.
93
Con formato: Inglés (Estados Unidos) Código de campo cambiado Con formato: Inglés (Estados Unidos) Con formato: Inglés (Estados Unidos)
[29] N. Sadegh and R. Horowitz, “Stability and robutness analysis of a class of adaptive controllers for robot,” Int. journal of Robotics Research, vol. 9, pp. 74-92, 1990. [30] N. Sadegh and R. Horowitz, “An exponentially stable adaptive control law for Robotic manipulators,” IEEE Trans. on Robotics and Automation, 1990. [31] U. Sawut, N. Umeda, K. Park, and T. Hanamoto, “Frictions control of robot arm with sliding more observer,” IEEE, pp. 61-66, 2001. [32] B. Siciliano and L. Sciavicco, Modelling and Control of Robot Manipulators. UK: Springer Verlag, second ed., 200. [33] U. Sawut, N. Umeda, K. Park, and T. Hanamoto, “Frictions control of robot arm with sliding more observer,” IEEE, pp. 61-66, 2001. [34] Y. Stephanenko and J. Yuan, “Robust adaptive control of a class of nonlinear mechanical systems with unbounded and fast-varying uncertainties,” Automatica, 1992. [35] J. J. E. Slotine and S. S. Sastry, “Tracking control of nonlinear systems using sliding surface with application to robot manipulator,” Int. J. Control, vol. 38, pp. 465.-492, 1983. [36] J. J. E. Slotine and W. Li, “Adaptive manipulator control: A case study,” IEEE Trans. on Automatic Control, vol. 33, no. 11, pp. 995-1003, 1988. [37] J.E. Slotine and W. Li, Applied Nonlinear Control. USA: Prentice Hall Inc., 1991. [38] J. J. E. Slotine, “The robust control of robot manipulators,” Int Journal of Robotics Research, vol. 2, pp. 49-64, 1985. [39] [38] J. Somlo, B. lantos, and P. T. Cat, Advanced Robot Control.
Budapest:
Akademiai Kiado, 1997. [40] T. J. Tam, A.K. Bejczy, A. Isdori, and Y. L. Chen, “Nonlinear feedback in Robot arm control,” IEEE Conf. on Decision and Control, pp. 736-741, 1984. [41] K.
D. Young, Variable Structure Control for Robotics and Aeropace
Applications. Netherlands: Elsevie, 1993.
94
[42] K. K. D. Young, “Controller design for a manipulator using theory of variable structure systems,” IEEE Trans. on Systems, Machines, and Cybernetics, vol. 8, 1978. [43] H. Yu, “Robust combined adaptive and variable structure adaptive control of robot manipulators,” Robotica, vol. 16, pp. 623-650, 1998. [44] A. Vivas. “Control Predictivo de un Robot tipo Scara”. Ingeniare -Revista Chilena de Ingeniería, vol. 14 Nº 2, pp. 137-147. 2006. [45] L. L. Whitcomb, A. A. Rizzi, and D. E. Koditschek, “Comparative Experiments with a new adaptive controller for robot arms,” IEEE Trans. on Robotics and Automation, 1993. [46] R. R. Y. Zhen and A. A. Goldberg, “Approaches to robust control of Manipulators,” IEEE, pp. 1602-1609. 1989.
95
ANEXOS
ANEXO 1. CODIGOS PARA LA GENERACIÓN DE TRAYECTORIAS Trayectoria 1 %-------Trayectoria Eliptica-------Tfinal=3.0; Tem=0.001; nbech=(Tfinal/Tem)+1; if ((round(nbech)-nbech) == 0) instant=[0:Tem:Tfinal]'; else nbech=nbech+1; instant=[0:Tem:Tfinal+Tem]'; end t=0; for h=1:1:nbech t=t+Tem; x1(h)=0.2*sin(2.0943951*t); y1(h)=0.1*cos(2.0943951*t); z1(h)=0.2*sin(t)*cos(50*t); end x1=x1'; y1=y1'; z1=z1'; cons1= 0.3 + x1; cons2= 0.3 + y1; cons3=z1;
Trayectoria 2 Tfinal=3.0; Tem=0.001; nbech=(Tfinal/Tem)+1; if ((round(nbech)-nbech) == 0) instant=[0:Tem:Tfinal]'; else nbech=nbech+1; instant=[0:Tem:Tfinal+Tem]'; end instant = instant(1:3000,:); t=0; for h=1:1:1500 t=t+Tem; x1(h)=t; y1(h)=t; end t=1.5; for h=1:1:1500 t=t+Tem; x2(h)=-t + 3.0; y2(h)= t; end xx= [x1 x2]; yy= [y1 y2]; xx = xx'; yy = yy'; %-------------------cons1= 0.35 + 0.01*xx; cons2= 0.35 + 0.01*yy; cons3 = 0.4*ones(3000,1);
96
ANEXO 2. PROGRAMA DE INICIALIZACIÓN DEL CONTROLADOR CTC % ----------Inicialización control dinámico SCARA 4 Ejes----------: clear all clc Tem = 0.001; %Trayectoria1; %circular; %Elipse; %lineal; Triangulo; QI = mgi(cons1(1),cons2(1),cons3(1)); KP1=125000; KV1=250; KP2=140000; KV2=200; KP3=150000; KV3=1000; KP4=120000; KV4=700;z
97
ANEXO 3. PROGRAMAS DEL MODELO CINEMÁTICO
Modelo cinemático directo de un ESCARA function x = mgd(q1,q2,q3,q4) % ---------Modelo geométrico directo del SCARA de cuatro ejes-------d2 = 0.5; d3 = 0.3; TTT = [cos(q1+q2+q3) -sin(q1+q2+q3) 0 (d3*cos(q1+q2) + d2*cos(q1)); sin(q1+q2+q3) cos(q1+q2+q3) 0 (d3*sin(q1+q2) + d2*sin(q1)); 0 0 1 q4; 0 0 0 1]; xa = TTT(1,4); ya = TTT(2,4); za = TTT(3,4); x = [xa ya za];
Modelo cinemático inverso de un ESCARA function q = mgi(x1,x2,x3) % ----------Modelo geométrico inverso del SCARA de cuatro ejes--------: d2 = 0.5;d3 = 0.3; C2 = (x1^2 + x2^2 - d2^2 - d3^2)/(2*d2*d3); B1 = d2 + d3*C2; q2 = atan2((-sqrt(1 - C2^2)),C2); %q2 = atan2((-sqrt(((1 + C2)*(1 - C2)^2))/(1 - C2)),C2); B2 = d3*sin(q2); S1 = (B1*x2 - B2*x1)/(B1^2 + B2^2); C1 = (B1*x1 + B2*x2)/(B1^2 + B2^2); q1 = atan2(S1,C1); q3 = atan2(0,-1) - q2 - q1;% sy, sx q4 = x3; q = [q1 q2 q3 q4];
98
ANEXO 4. PROGRAMAS DEL MODELO DINÁMICO
Con formato: Español (alfab. internacional) Código de campo cambiado
Modelo dinámico directo del manipulador SCARA de 4 DOF function [sys, x0,str,ts] = f_scara(t,x,u,flag,QI) % function [sys,x0,str,ts] = f_scara(t,x,u,flag,L1,L2,ZZ1,ZZ2,MX2,MY2,FV1,FV2,FS1,FS2,QI) % f_scara An example M-file S-function for defining a continuous system. % Example M-file S-function implementing non linear continuous equations of a 2 dof scara robot % Etats : x(1)=q1,x(2)=q2,x(3)=qp1,x(4)=qp2 % qpp =f(q,qp,u) % y =x % % See csfunc.m for a continuous linear state space example % See sfuntmpl.m for a general S-function template. % switch flag, %%%%%%%%% % Initialization % %%%%%%%%% case 0, [sys,x0,str,ts]=mdlInitializeSizes(QI); %%%%%%%% % Derivatives % %%%%%%%% case 1, % sys=mdlDerivatives(t,x,u,L1,L2,ZZ1,ZZ2,MX2,MY2,FV1,FV2,FS1,FS2); sys=mdlDerivatives(t,x,u); %%%%%% % Outputs % %%%%%% case 3, sys=mdlOutputs(t,x,u); %%%%%%%%%% % Unhandled flags % %%%%%%%%%% case { 2, 4, 9 }, sys = []; %%%%%%%%%%% % Unexpected flags % %%%%%%%%%%% otherwise error(['Unhandled flag = ',num2str(flag)]); end % end f_scara % mdlInitializeSizes % Return the sizes, initial conditions, and sample times for the S-function. function [sys,x0,str,ts]=mdlInitializeSizes(QI)
99
Con formato: Español (alfab. internacional)
sizes = simsizes; sizes.NumContStates = 8; sizes.NumDiscStates = 0; sizes.NumOutputs = 8; sizes.NumInputs = 4; sizes.DirFeedthrough = 0; sizes.NumSampleTimes = 1; sys = simsizes(sizes); % % initialize the initial conditions % %x0=[QI(1);0;QI(2);0]; x0=[QI(1);QI(2);QI(3);QI(4);0;0;0;0]; % x=[q1,q2,q3,q4,qp1,qp2,qp3,qp4] % % str is always an empty matrix % str = []; % % initialize the array of sample times % ts = [0 0]; % end mdlInitializeSizes % %============================================================================= % mdlDerivatives % Return the derivatives for the continuous states. %============================================================================= % %function sys=mdlDerivatives(t,x,u,L1,L2,ZZ1,ZZ2,MX2,MY2,FV1,FV2,FS1,FS2) function sys=mdlDerivatives(t,x,u) % Parametros del SCARA para la simulacion: G3=9.81; ZZ2R=0.063; MY3R=0.1;% FV3=FV1; FS4=FS2; FY3=0;% CY3=0;% IA4=0.045;%
M4=1.8; d2=0.5; d3=0.3;% r4=0.2;% ZZ1R=3.38; ZZ3R=0.1;% MX2R=2*0.121; MX3R=0.2;% MY2=0.001; FV1=0.1; FV2=0.012; FV4=FV2; FS1=0.57; FS2=0.125; IA2=0; CZ1=0; FX2=0; FY2=0; FX3=0;% FZ3=0;% CZ3=0;% CZ4=0;% CX3=0;% CY4=0;% FX4=0;% FY4=0;% FZ4=0;%
GAM1=u(1); GAM2=u(2); GAM3=u(3); GAM4=u(4); S1=sin(x(1)); S3=sin(x(3)); QP3=x(7);
C1=cos(x(1)); C3=cos(x(3)); QP4=x(8);
S2=sin(x(2)); QP1=x(5);
W32=QP1 + QP2; JPR132=d2.*S2; JPR232=C2.*d2; W33=QP3 + W32; JPR133=d3.*S3;
100
C2=cos(x(2)); QP2=x(6);
FS3=FS1; CX4=0;% IA3=0;%
JPR233=C3.*d3; JW31=QP1.*ZZ1R; JW32=W32.*ZZ2R; SW12=-(MX2R.*W32.^2); SW22=-(MY2.*W32.^2); LW12=-(C2.*d2.*QP1.^2); LW22=d2.*QP1.^2.*S2; JW33=W33.*ZZ3R; SW13=-(MX3R.*W33.^2); SW23=-(MY3R.*W33.^2); LW13=-(C3.*d3.*W32.^2); LW23=d3.*S3.*W32.^2; JD4=1./(IA4 + M4); JU64=JD4.*M4; GW4=-FZ4 + GAM4 - FV4.*QP4 - FS4.*sign(QP4); GK664=M4 - JU64.*M4; VS64=GW4.*JU64; AP64=FZ4 + VS64; GX154=-(M4.*r4); GX244=M4.*r4; TKT114=-(GX154.*r4); TKT514=-(M4.*r4); TKT224=GX244.*r4; TKT424=M4.*r4; VBE13=-CX3 - CX4 + FY4.*r4; VBE23=-CY3 - CY4 - FX4.*r4; VBE33=-CZ3 - CZ4; VBE43=-FX3 - FX4 - SW13; VBE53=-FY3 - FY4 - SW23; VBE63=-AP64 - FZ3; JD3=1./(IA3 + ZZ3R); JU33=JD3.*ZZ3R; JU43=-(JD3.*MY3R); JU53=JD3.*MX3R; GW3=GAM3 - FV3.*QP3 + VBE33 - FS3.*sign(QP3); GK333=ZZ3R - JU33.*ZZ3R; GK343=-MY3R + JU33.*MY3R; GK353=MX3R - JU33.*MX3R; GK433=-MY3R - JU43.*ZZ3R; GK443=M4 + JU43.*MY3R; GK453=-(JU43.*MX3R); GK533=MX3R - JU53.*ZZ3R; GK543=JU53.*MY3R; GK553=M4 - JU53.*MX3R; NG13=LW23.*TKT514; NG23=LW13.*TKT424; NG33=GK343.*LW13 + GK353.*LW23; NG43=GK443.*LW13 + GK453.*LW23; NG53=GK543.*LW13 + GK553.*LW23; VS33=GW3.*JU33 + NG33; VS43=GW3.*JU43 + NG43; VS53=GW3.*JU53 + NG53; AP13=NG13 - VBE13; AP23=NG23 - VBE23; AP33=-VBE33 + VS33; AP43=-VBE43 + VS43; AP53=-VBE53 + VS53; GX113=C3.*TKT114; GX123=-(S3.*TKT224); GX143=-(S3.*TKT424);
101
GX153=C3.*TKT514; GX163=C3.*MY3R + MX3R.*S3; GX213=-(d3.*MY3R) + S3.*TKT114; GX223=d3.*MX3R + C3.*TKT224; GX243=C3.*TKT424; GX253=S3.*TKT514; GX263=-(d3.*GK664) - C3.*MX3R + MY3R.*S3; GX313=JPR233.*TKT514; GX323=JPR133.*TKT424; GX333=GK333 + GK433.*JPR133 + GK533.*JPR233; GX343=GK343 + GK443.*JPR133 + GK543.*JPR233; GX353=GK353 + GK453.*JPR133 + GK553.*JPR233; GX413=-(S3.*TKT514); GX423=C3.*TKT424; GX433=C3.*GK433 - GK533.*S3; GX443=C3.*GK443 - GK543.*S3; GX453=C3.*GK453 - GK553.*S3; GX513=C3.*TKT514; GX523=S3.*TKT424; GX533=C3.*GK533 + GK433.*S3; GX543=C3.*GK543 + GK443.*S3; GX553=C3.*GK553 + GK453.*S3; TKT113=C3.*GX113 - GX123.*S3; TKT213=C3.*GX213 - GX223.*S3; TKT313=C3.*GX313 - GX323.*S3; TKT413=C3.*GX413 - GX423.*S3; TKT513=C3.*GX513 - GX523.*S3; TKT613=C3.*MY3R + MX3R.*S3; TKT223=C3.*GX223 - d3.*GX263 + GX213.*S3; TKT323=C3.*GX323 + GX313.*S3; TKT423=C3.*GX423 + GX413.*S3; TKT523=C3.*GX523 + GX513.*S3; TKT623=-(d3.*GK664) - C3.*MX3R + MY3R.*S3; TKT333=GX333 + GX343.*JPR133 + GX353.*JPR233; TKT433=GX433 + GX443.*JPR133 + GX453.*JPR233; TKT533=GX533 + GX543.*JPR133 + GX553.*JPR233; TKT443=C3.*GX443 - GX453.*S3; TKT543=C3.*GX543 - GX553.*S3; TKT553=C3.*GX553 + GX543.*S3; MJE612=MY2 + TKT613; MJE622=-MX2R + TKT623; MJE332=TKT333 + ZZ2R; MJE432=-MY2 + TKT433; MJE532=MX2R + TKT533; VBE12=-(AP13.*C3) + AP23.*S3; VBE22=-(AP23.*C3) - AP13.*S3 - d3.*VBE63; VBE32=-AP33 - AP43.*JPR133 - AP53.*JPR233; VBE42=-(AP43.*C3) + AP53.*S3 - SW12; VBE52=-(AP53.*C3) - AP43.*S3 - SW22; JD2=1./(IA2 + MJE332); JU12=JD2.*TKT313; JU22=JD2.*TKT323; JU32=JD2.*MJE332; JU42=JD2.*MJE432; JU52=JD2.*MJE532; GW2=GAM2 - FV2.*QP2 + VBE32 - FS2.*sign(QP2); GK112=TKT113 - JU12.*TKT313; GK122=TKT213 - JU12.*TKT323; GK132=-(JU12.*MJE332) + TKT313; GK142=-(JU12.*MJE432) + TKT413;
102
GK152=-(JU12.*MJE532) + TKT513; GK212=TKT213 - JU22.*TKT313; GK222=TKT223 - JU22.*TKT323; GK232=-(JU22.*MJE332) + TKT323; GK242=-(JU22.*MJE432) + TKT423; GK252=-(JU22.*MJE532) + TKT523; GK312=TKT313 - JU32.*TKT313; GK322=TKT323 - JU32.*TKT323; GK332=MJE332 - JU32.*MJE332; GK342=MJE432 - JU32.*MJE432; GK352=MJE532 - JU32.*MJE532; GK412=-(JU42.*TKT313) + TKT413; GK422=-(JU42.*TKT323) + TKT423; GK432=-(JU42.*MJE332) + MJE432; GK442=-(JU42.*MJE432) + TKT443; GK452=-(JU42.*MJE532) + TKT543; GK512=-(JU52.*TKT313) + TKT513; GK522=-(JU52.*TKT323) + TKT523; GK532=-(JU52.*MJE332) + MJE532; GK542=-(JU52.*MJE432) + TKT543; GK552=-(JU52.*MJE532) + TKT553; NG12=GK142.*LW12 + GK152.*LW22; NG22=GK242.*LW12 + GK252.*LW22; NG32=GK342.*LW12 + GK352.*LW22; NG42=GK442.*LW12 + GK452.*LW22; NG52=GK542.*LW12 + GK552.*LW22; VS12=GW2.*JU12 + NG12; VS22=GW2.*JU22 + NG22; VS32=GW2.*JU32 + NG32; VS42=GW2.*JU42 + NG42; VS52=GW2.*JU52 + NG52; AP12=-VBE12 + VS12; AP22=-VBE22 + VS22; AP32=-VBE32 + VS32; AP42=-VBE42 + VS42; AP52=-VBE52 + VS52; GX112=C2.*GK112 - GK212.*S2; GX122=C2.*GK122 - GK222.*S2; GX132=C2.*GK132 - GK232.*S2; GX142=C2.*GK142 - GK242.*S2; GX152=C2.*GK152 - GK252.*S2; GX162=C2.*MJE612 - MJE622.*S2; GX212=C2.*GK212 - d2.*MJE612 + GK112.*S2; GX222=C2.*GK222 - d2.*MJE622 + GK122.*S2; GX232=C2.*GK232 + GK132.*S2; GX242=C2.*GK242 + GK142.*S2; GX252=C2.*GK252 + GK152.*S2; GX262=-(d2.*GK664) + C2.*MJE622 + MJE612.*S2; GX312=GK312 + GK412.*JPR132 + GK512.*JPR232; GX322=GK322 + GK422.*JPR132 + GK522.*JPR232; GX332=GK332 + GK432.*JPR132 + GK532.*JPR232; GX342=GK342 + GK442.*JPR132 + GK542.*JPR232; GX352=GK352 + GK452.*JPR132 + GK552.*JPR232; GX412=C2.*GK412 - GK512.*S2; GX422=C2.*GK422 - GK522.*S2; GX432=C2.*GK432 - GK532.*S2; GX442=C2.*GK442 - GK542.*S2; GX452=C2.*GK452 - GK552.*S2; GX512=C2.*GK512 + GK412.*S2; GX522=C2.*GK522 + GK422.*S2;
103
GX532=C2.*GK532 + GK432.*S2; GX542=C2.*GK542 + GK442.*S2; GX552=C2.*GK552 + GK452.*S2; TKT112=C2.*GX112 - GX122.*S2; TKT212=C2.*GX212 - GX222.*S2; TKT312=C2.*GX312 - GX322.*S2; TKT412=C2.*GX412 - GX422.*S2; TKT512=C2.*GX512 - GX522.*S2; TKT612=C2.*MJE612 - MJE622.*S2; TKT222=C2.*GX222 - d2.*GX262 + GX212.*S2; TKT322=C2.*GX322 + GX312.*S2; TKT422=C2.*GX422 + GX412.*S2; TKT522=C2.*GX522 + GX512.*S2; TKT622=-(d2.*GK664) + C2.*MJE622 + MJE612.*S2; TKT332=GX332 + GX342.*JPR132 + GX352.*JPR232; TKT432=GX432 + GX442.*JPR132 + GX452.*JPR232; TKT532=GX532 + GX542.*JPR132 + GX552.*JPR232; TKT442=C2.*GX442 - GX452.*S2; TKT542=C2.*GX542 - GX552.*S2; TKT552=C2.*GX552 + GX542.*S2; MJE331=TKT332 + ZZ1R; VBE11=-(AP12.*C2) + AP22.*S2; VBE21=-(AP22.*C2) - AP12.*S2 - d2.*VBE63; VBE31=-AP32 - AP42.*JPR132 - AP52.*JPR232; VBE41=-(AP42.*C2) + AP52.*S2; VBE51=-(AP52.*C2) - AP42.*S2; JD1=1./MJE331; JU11=JD1.*TKT312; JU21=JD1.*TKT322; JU31=JD1.*MJE331; JU41=JD1.*TKT432; JU51=JD1.*TKT532; GW1=GAM1 - FV1.*QP1 + VBE31 - FS1.*sign(QP1); QDP1=GW1.*JD1; VR42=LW12 + JPR132.*QDP1; VR52=LW22 + JPR232.*QDP1; GU2=JU32.*QDP1 + JU42.*VR42 + JU52.*VR52; QDP2=-GU2 + GW2.*JD2; WP32=QDP1 + QDP2; VR43=LW13 + C3.*VR42 + S3.*VR52 + JPR133.*WP32; VR53=LW23 - S3.*VR42 + C3.*VR52 + JPR233.*WP32; GU3=JU43.*VR43 + JU53.*VR53 + JU33.*WP32; QDP3=-GU3 + GW3.*JD3; WP33=QDP3 + WP32; GU4=-(G3.*JU64); QDP4=-GU4 + GW4.*JD4; sys(1) = x(5) ; sys(2) = x(6) ; sys(3) = x(7) ; sys(4) = x(8) ; sys(5) = QDP1; sys(6) = QDP2; sys(7) = QDP3; sys(8) = QDP4; % end mdlDerivatives % mdlOutputs % Return the block outputs. function sys=mdlOutputs(t,x,u) sys(1) = x(1); sys(2) = x(2); sys(5) = x(5); sys(6) = x(6); %end mdlOutputs
sys(3) = x(3); sys(7) = x(7);
sys(4) = x(4); sys(8) = x(8);
104
Modelo dinámico inverso del manipulador SCARA de 4 DOF function GAM scara_inverso(position1,position2,position3,position4,vitesse1,vitesse2,vitesse3,vitesse4,w1,w2,w3,w4) G3=9.81; ZZ1R=3.38; MX3R=0.2;% FV2=0.012; FS2=0.125; CZ1=0; FY3=0;% FX4=0;% IA4=0.045;%
M4=1.8; d2=0.5; ZZ2R=0.063; MY3R=0.1;% MY2=0.001; FV3=FV1; FS3=FS1; FX2=0; FY2=0; FZ3=0;% FY4=0;%
d3=0.3;% ZZ3R=0.1;% FV1=0.1; FV4=FV2; FS4=FS2; FX3=0;% CZ3=0;% FZ4=0;%
MX2R=2*0.121; FS1=0.57; IA2=0; CZ4=0;% IA3=0;%
%Errores en el modelo: % aa=1.35; % M4=M4*aa; % ZZ1R=ZZ1R*aa; % ZZ2R=ZZ2R*aa; % ZZ3R=ZZ3R*aa; % MX2R=MX2R*aa; % MX3R=MX3R*aa; % MY3R=MY3R*aa; % MY2=MY2*aa; % FV1=FV1*aa; % FV2=FV2*aa; % FV3=FV1; % FV4=FV2; % FS1=FS1*aa; % FS2=FS2*aa; % FS3=FS1; % FS4=FS2; % IA4=IA4*aa;
t2=position2; QP1=vitesse1; QDP1=w1; S2=sin(t2);
t3=position3; QP2=vitesse2; QP3=vitesse3; QDP2=w2; C2=cos(t2);
QP4=vitesse4; QDP3=w3; S3=sin(t3);
DV331=-QP1.^2; No31=QDP1.*ZZ1R; W32=QP1 + QP2; WP32=QDP1 + QDP2; DV332=-W32.^2; VSP12=d2.*DV331; VSP22=d2.*QDP1; VP12=C2.*VSP12 + S2.*VSP22; VP22=-(S2.*VSP12) + C2.*VSP22; F12=DV332.*MX2R - MY2.*WP32; F22=DV332.*MY2 + MX2R.*WP32; No32=WP32.*ZZ2R; W33=QP3 + W32; WP33=QDP3 + WP32; DV333=-W33.^2; VSP13=d3.*DV332 + VP12; VSP23=VP22 + d3.*WP32; VP13=C3.*VSP13 + S3.*VSP23;
105
QDP4=w4; C3=cos(t3);
=
VP23=-(S3.*VSP13) + C3.*VSP23; F13=DV333.*MX3R - MY3R.*WP33; F23=DV333.*MY3R + MX3R.*WP33; No33=WP33.*ZZ3R; VP34=-G3 + QDP4; F14=M4.*VP13; F24=M4.*VP23; F34=M4.*VP34; E14=F14 + FX4; E24=F24 + FY4; E34=F34 + FZ4; E13=E14 + F13 + FX3; E23=E24 + F23 + FY3; N33=CZ3 + CZ4 + No33 - MY3R.*VP13 + MX3R.*VP23; FDI13=C3.*E13 - E23.*S3; FDI23=C3.*E23 + E13.*S3; E12=F12 + FDI13; E22=F22 + FDI23; N32=d3.*FDI23 + N33 + No32 - MY2.*VP12 + MX2R.*VP22; FDI22=C2.*E22 + E12.*S2; N31=d2.*FDI22 + N32 + No31; GAM1=N31 + FV1.*QP1 + FS1.*sign(QP1); GAM2=N32 + IA2.*QDP2 + FV2.*QP2 + FS2.*sign(QP2); GAM3=N33 + IA3.*QDP3 + FV3.*QP3 + FS3.*sign(QP3); GAM4=E34 + IA4.*QDP4 + FV4.*QP4 + FS4.*sign(QP4); GAM(1) = GAM1; GAM(2) = GAM2; GAM(3) = GAM3; GAM(4) = GAM4;
106
Con formato: Español (alfab. internacional)
ANEXO 5. PROGRAMA PARA EL CALCULO DE LA D.I. ROBOT HASTA 5 DOF %_____________________________________________________ % ALGORITMO DE DENAVIT-HARTENBERG + DINAMICA NEWTON EULER %_____________________________________________________________________________ clear %clc %input('Numerar cada eslabon comenzando con 1 para primer eslabón móvil,la base del robot se numerará con 0 '); No_eslabones_moviles=input('Cuantos eslabones moviles tiene el manipulador: '); T_No_eslabones=No_eslabones_moviles+1; %input('Numerar cada articulacion comenzando con 1 para el primer gdl '); No_articulaciones=input('Cuantas articulaciones tiene el manipulador: '); r=1; p=2; for i=1:No_articulaciones; temp_arti(i,1)=input(['Si la artitulacion ' ,int2str(i),' es rotativa marque r sino marque p: ']); end % PASO 1. OBTENCION DE LOS PARAMETROS DE D-H.(DENAVIT - HATENBERG) syms th1 th2 th3 th4 th5 d1 d2 d3 d4 d5 a1 a2 a3 a4 a5 t syms alp1 alp2 alp3 alp4 alp5 g m1 m2 L1 th1=sym('th1(t)'); d2=sym('d2(t)'); % DEFINICION SIMBOLICA DE LA POSICION %th1='th1(t)'; %d2='d2(t)'; for i=1:No_articulaciones; DH_art=input(['Para la articulación ',int2str(i),' digite los parametros de D-H: [theta,d,a,alpha*(pi/180)]= ']); for j=1:4; DH(i,j)=DH_art(1,j); end end % PASO 2. OBTENCION DE LAS MATRICES DE ROTACION i-1Ri Y SUS INVERSAS iRi-1 for i=1:No_articulaciones; a=[cos(DH(i,1)) -cos(DH(i,4))*sin(DH(i,1)) sin(DH(i,4))*sin(DH(i,1))]; b=[sin(DH(i,1)) cos(DH(i,4))*cos(DH(i,1)) -sin(DH(i,4))*cos(DH(i,1))]; c=[0 sin(DH(i,4)) cos(DH(i,4))]; if T_No_eslabones>1 & i==1 R01=[a; b; c]; R10=inv(R01); elseif T_No_eslabones>2 & i==2 R12=[a; b; c]; R21=inv(R12); R02=R01*R12; R20=inv(R02); elseif T_No_eslabones>3 & i==3 R23=[a; b; c]; R32=inv(R23); R13=R12*R23; R31=inv(R13); R03=R01*R13; R30=inv(R03); elseif T_No_eslabones>4 & i==4 R34=[a; b; c]; R43=inv(R23); R24=R23*R34; R42=inv(R24); R14=R13*R34; R41=inv(R14); R04=R03*R34; R40=inv(R04); elseif T_No_eslabones>5 & i==5 R45=[a; b; c]; R54=inv(R45); R15=R14*R45; R51=inv(R15); R25=R24*R45; R52=inv(R25); R53=R51*R13; R35=inv(R53); R05=R02*R25; R50=inv(R05);
107
end end % PASO 3. RESTABLECIMIENTO DE LAS CONDICIONES INICIALES. w00=[0,0,0]'; %Velocidad angular de la base respecto al marco DH_0. dw00=[0,0,0]'; %Aceleracion angular de la base respecto al marco DH_0. v00=[0,0,0]'; %Velocidad angular de la base respecto al marco DH_0. dv00=[0,0,g].'; %Aceleracion lineal de la base. fext=[0,0,0]'; %Fuerza externa ejercida en el eflector. next=[0,0,0]'; %Par externo ejercido en el eflector. Ruext=[0,0,0;0,0,0;0,0,0]; %Matriz de rotacion del extremo del robot a la carga externo Rextu=[0,0,0;0,0,0;0,0,0]; zo=[0,0,1]'; for i=1:No_articulaciones; %coordenadas del origen del sistema Si respecto al sistema Si-1. if T_No_eslabones>1 & i==1 p11=[DH(i,3); DH(i,2)*sin(DH(i,4)); DH(i,2)*cos(DH(i,4))]; elseif T_No_eslabones>2 & i==2 p22=[DH(i,3); DH(i,2)*sin(DH(i,4)); DH(i,2)*cos(DH(i,4))]; elseif T_No_eslabones>3 & i==3 p33=[DH(i,3); DH(i,2)*sin(DH(i,4)); DH(i,2)*cos(DH(i,4))]; elseif T_No_eslabones>4 & i==4 p44=[DH(i,3); DH(i,2)*sin(DH(i,4)); DH(i,2)*cos(DH(i,4))]; elseif T_No_eslabones>5 & i==5 p55=[DH(i,3); DH(i,2)*sin(DH(i,4)); DH(i,2)*cos(DH(i,4))]; end end %Ubicacion del centro de masa del eslabon i respecto al sistema Si. for i=1:No_eslabones_moviles; S_eslab=input(['En el elabon ',int2str(i),' digite las coordenada de su CM respecto al marco D-H: [sx,sy,sz]= ']); for j=1:3; S(i,j)=S_eslab(1,j); end end %Matriz de inercia del eslabon i respecto al centro de masa. Toda la masa esta concentrada en el CM. for i=1:No_eslabones_moviles; Datos=input(['Para el eslabon ',int2str(i),' digite los momentos, productos de inercia y la masa: [Ixx,Iyy,Izz,Ixy,Ixz,Iyz,m]= ']); if i==1 I11=[Datos(1,1) Datos(1,4) Datos(1,5); Datos(1,4) Datos(1,2) Datos(1,6); Datos(1,5) Datos(1,6) Datos(1,3) ]; elseif i==2 I22=[Datos(1,1) Datos(1,4) Datos(1,5); Datos(1,4) Datos(1,2) Datos(1,6); Datos(1,5) Datos(1,6) Datos(1,3) ]; elseif i==3 I33=[Datos(1,1) Datos(1,4) Datos(1,5); Datos(1,4) Datos(1,2) Datos(1,6); Datos(1,5) Datos(1,6) Datos(1,3) ]; elseif i==4 I44=[Datos(1,1) Datos(1,4) Datos(1,5); Datos(1,4) Datos(1,2) Datos(1,6); Datos(1,5) Datos(1,6) Datos(1,3) ]; elseif i==5 I55=[Datos(1,1) Datos(1,4) Datos(1,5); Datos(1,4) Datos(1,2) Datos(1,6); Datos(1,5) Datos(1,6) Datos(1,3) ]; end end % PASO 4. VELOCIDADES ANGULARES DEL SISTEMA Si
108
if temp_arti(1,1)==r w11=R10*(w00+zo*diff(th1,'t')); else w11=R10*(w00); end if temp_arti(2,1)==r w22=R21*(w11+zo*diff(th2,'t')); else w22=R21*(w11); end tam=size(temp_arti); gevo=tam(1,1); if gevo==3 if temp_arti(3,1)==r w33=R32*(w22+zo*diff(th3,'t')); else w33=R32*(w22); end elseif gevo==4 if temp_arti(3,1)==r w33=R32*(w22+zo*diff(th3,'t')); else w33=R32*(w22); end if temp_arti(4,1)==r w44=R43*(w33+zo*diff(th4,'t')); else w44=R43*(w33); end elseif gevo==5 if temp_arti(3,1)==r w33=R32*(w22+zo*diff(th3,'t')); else w33=R32*(w22); end if temp_arti(4,1)==r w44=R43*(w33+zo*diff(th4,'t')); else w44=R43*(w33); end if temp_arti(5,1)==r w55=R54*(w44+zo*diff(th5,'t')); else w55=R54*(w44); end end % PASO 5. ACELERACIONES ANGULARES DEL SISTEMA Si if temp_arti(1,1)==r dw11=R10*(dw00+zo*diff(th1,'t',2))+cross(w00,zo*diff(th1,'t')); else dw11=R10*(dw00); end if temp_arti(2,1)==r dw22=R21*(dw11+zo*diff(th2,'t',2))+cross(w11,zo*diff(th2,'t'));
109
else dw22=R21*(dw11); end if gevo==3 if temp_arti(3,1)==r dw33=R32*(dw22+zo*diff(th3,'t',2))+cross(w22,zo*diff(th3,'t')); else temp_arti(3,1)==p dw33=R32*(dw22); end elseif gevo==4 if temp_arti(3,1)==r dw33=R32*(dw22+zo*diff(th3,'t',2))+cross(w22,zo*diff(th3,'t')); else temp_arti(3,1)==p dw33=R32*(dw22); end if temp_arti(4,1)==r dw44=R43*(dw33+zo*diff(th4,'t',2))+cross(w33,zo*diff(th4,'t')); else dw44=R43*(dw33); end elseif gevo==5 if temp_arti(3,1)==r dw33=R32*(dw22+zo*diff(th3,'t',2))+cross(w22,zo*diff(th3,'t')); else temp_arti(3,1)==p dw33=R32*(dw22); end if temp_arti(4,1)==r dw44=R43*(dw33+zo*diff(th4,'t',2))+cross(w33,zo*diff(th4,'t')); else dw44=R43*(dw33); end if temp_arti(5,1)==r dw55=R54*(dw44+zo*diff(th5,'t',2))+cross(w44,zo*diff(th5,'t')); else temp_arti(5,1)==p dw55=R54*(dw44); end end % PASO 6. ACELERACIONES LINEALES DEL SISTEMA Si if temp_arti(1,1)==r dv11=cross(dw11,p11)+cross(w11,(cross(w11,p11)))+R10*dv00; else dv11=cross(dw11,p11)+2*cross(w11,R10*zo*diff(d1,t))+cross(w11,(cross(w11,p11)))+R10*(dv00+zo*diff(d1,t,2 )); end if temp_arti(2,1)==r dv22=cross(dw22,p22)+cross(w22,(cross(w22,p22)))+R21*dv11; else dv22=cross(dw22,p22)+2*cross(w22,R21*zo*diff(d2,t))+cross(w22,(cross(w22,p22)))+R21*(dv11+zo*diff(d2,t,2 )); end if gevo==3 if temp_arti(3,1)==r dv33=cross(dw33,p33)+cross(w33,(cross(w33,p33)))+R32*dv22;
110
else temp_arti(3,1)==p dv33=cross(dw33,p33)+2*cross(w33,R32*zo*diff(d3,t))+cross(w33,(cross(w33,p33)))+R32*(dv22+zo*diff(d3,t,2 )); end elseif gevo==4 if temp_arti(3,1)==r dv33=cross(dw33,p33)+cross(w33,(cross(w33,p33)))+R32*dv22; else temp_arti(3,1)==p dv33=cross(dw33,p33)+2*cross(w33,R32*zo*diff(d3,t))+cross(w33,(cross(w33,p33)))+R32*(dv22+zo*diff(d3,t,2 )); end if temp_arti(4,1)==r dv44=cross(dw44,p44)+cross(w44,(cross(w44,p44)))+R43*dv33; else dv44=cross(dw44,p44)+2*cross(w44,R43*zo*diff(d4,t))+cross(w44,(cross(w44,p44)))+R43*(dv33+zo*diff(d4,t,2 )); end elseif gevo==5 if temp_arti(3,1)==r dv33=cross(dw33,p33)+cross(w33,(cross(w33,p33)))+R32*dv22; else temp_arti(3,1)==p dv33=cross(dw33,p33)+2*cross(w33,R32*zo*diff(d3,t))+cross(w33,(cross(w33,p33)))+R32*(dv22+zo*diff(d3,t,2 )); end if temp_arti(4,1)==r dv44=cross(dw44,p44)+cross(w44,(cross(w44,p44)))+R43*dv33; else dv44=cross(dw44,p44)+2*cross(w44,R43*zo*diff(d4,t))+cross(w44,(cross(w44,p44)))+R43*(dv33+zo*diff(d4,t,2 )); end if temp_arti(5,1)==r dv55=cross(dw55,p55)+cross(w55,(cross(w55,p55)))+R54*dv44; else temp_arti(5,1)==p dv55=cross(dw55,p55)+2*cross(w55,R54*zo*diff(d5,t))+cross(w55,(cross(w55,p55)))+R43*(dv44+zo*diff(d5,t,2 )); end end % PASO 7. ACELERACIONES LINEAL DEL CENTRO DE GRAVEDAD DEL ESLABON i a11=cross(dw11,S(1,:).')+cross(w11,(cross(w11,S(1,:).')))+dv11; a22=cross(dw22,S(2,:).')+cross(w22,(cross(w22,S(2,:).')))+dv22; if gevo==3 a33=cross(dw33,p33)+cross(w33,(cross(w33,p33)))+dv33; elseif gevo==4 a33=cross(dw33,p33)+cross(w33,(cross(w33,p33)))+dv33; a44=cross(dw44,p44)+cross(w44,(cross(w44,p44)))+dv44; elseif gevo==5 a33=cross(dw33,p33)+cross(w33,(cross(w33,p33)))+dv33; a44=cross(dw44,p44)+cross(w44,(cross(w44,p44)))+dv44; a55=cross(dw55,p55)+cross(w55,(cross(w55,p55)))+dv55;
111
end % PASO 8. OBTENCION DE LA FUERZA EJERCIDA SOBRE EL ESLABON i if gevo==5 f55=Ruext*fext+m5*a55; f44=R45*f55+m4*a44; f33=R34*f44+m3*a33; f22=R23*f33+m2*a22; f11=R12*f22+m1*a11; elseif gevo==4 f44=Ruext*fext+m4*a44; f33=R34*f44+m3*a33; f22=R23*f33+m2*a22; f11=R12*f22+m4*a11; elseif gevo==3 f33=Ruext*fext+m3*a33; f22=R23*f33+m2*a22; f11=R12*f22+m1*a11; elseif gevo==2 f22=Ruext*fext+m2*a22; f11=R12*f22+m1*a11; end % PASO 9. OBTENCION DEL PAR EJERCIDO SOBRE EL ESLABON i if gevo==5 n55=Ruext*(next+cross((Rextu*p55),fext))+cross((p55+S(5,:).'),m5*a55)+I55*dw55+cross(w55,(I55*w55)); n44=R45*(n55+cross((R54*p44),f44))+cross((p44+S(4,:).'),m4*a44)+I44*dw44+cross(w44,(I44*w44)); n33=R34*(n44+cross((R43*p33),f33))+cross((p33+S(3,:).'),m3*a33)+I33*dw33+cross(w33,(I33*w33)); n22=R23*(n33+cross((R32*p22),f22))+cross((p22+S(2,:).'),m2*a22)+I22*dw22+cross(w22,(I22*w22)); n11=R12*(n22+cross((R21*p22),f11))+cross((p11+S(1,:).'),m1*a11)+I11*dw11+cross(w11,(I11*w11)); elseif gevo==4 n44=Ruext*(next+cross((Rextu*p44),fext))+cross((p44+S(4,:).'),m4*a44)+I44*dw44+cross(w44,(I44*w44)); n33=R34*(n44+cross((R43*p33),f33))+cross((p33+S(3,:).'),m3*a33)+I33*dw33+cross(w33,(I33*w33)); n22=R23*(n33+cross((R32*p22),f22))+cross((p22+S(2,:).'),m2*a22)+I22*dw22+cross(w22,(I22*w22)); n11=R12*(n22+cross((R21*p22),f11))+cross((p11+S(1,:).'),m1*a11)+I11*dw11+cross(w11,(I11*w11)); elseif gevo==3 n33=Ruext*(next+cross((Rextu*p33),fext))+cross((p33+S(3,:).'),m3*a33)+I33*dw33+cross(w33,(I33*w33)); n22=R23*(n33+cross((R32*p22),f33))+cross((p22+S(2,:).'),m2*a22)+I22*dw22+cross(w22,(I22*w22)); n11=R12*(n22+cross((R21*p22),f22))+cross((p11+S(1,:).'),m1*a11)+I11*dw11+cross(w11,(I11*w11)); elseif gevo==2 n22=Ruext*(next+cross((Rextu*p22),fext))+cross((p22+S(2,:).'),m2*a22)+I22*dw22+cross(w22,(I22*w22)); n11=R12*(n22+cross((R21*p11),f22))+cross((p11+S(1,:).'),m1*a11)+I11*dw11+cross(w11,(I11*w11)); end % PASO 10. FUERZA O PAR APLICADO A LA ARTICULACION i % Tao es el par o fuerza efectiva(par motor menos par de rozamiento o perturbacion if temp_arti(1,1)==r Tao1=(n11.')*R10*zo; else F1=(f11.')*R10*zo; end if temp_arti(2,1)==r Tao2=(n22.')*R21*zo; else F2=(f22.')*R21*zo; end
112
if gevo==3 if temp_arti(3,1)==r Tao3=(n33.')*R32*zo; else temp_arti(3,1)==p F3=(f33.')*R32*zo; end elseif gevo==4 if temp_arti(3,1)==r Tao3=(n33.')*R32*zo; else temp_arti(3,1)==p F3=(f33.')*R32*zo; end if temp_arti(4,1)==r Tao4=(n44.')*R43*zo; else F4=(f44.')*R43*zo; end elseif gevo==5 if temp_arti(3,1)==r Tao3=(n33.')*R32*zo; else temp_arti(3,1)==p F3=(f33.')*R32*zo; end if temp_arti(4,1)==r Tao4=(n44.')*R43*zo; else F4=(f44.')*R43*zo; end if temp_arti(5,1)==r Tao5=(n55.')*R54*zo; else F5=(f55.')*R54*zo; end end
113