modelamiento, simulacion y control dinamico …
TRANSCRIPT
i
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
Con formato
ii
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
iii
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é
iv
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: 0cm
v
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.
Con formato
vi
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
vii
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
viii
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
ix
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
x
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
xi
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
12
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
13
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, Primeralínea: 0 cm, No ajustar espacio entretexto latino y asiático, No ajustarespacio entre texto asiático y números
14
Capítulo I
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
Con formato: Normal, Derecha,Sangría: Izquierda: 0 cm, Primeralínea: 0 cm, No ajustar espacio entretexto latino y asiático, No ajustarespacio entre texto asiático y números
Con formato: Normal, No ajustarespacio entre texto latino y asiático, Noajustar espacio entre texto asiático ynúmeros
Con formato: Normal
Con formato: Título 1, Centrado
15
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
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)
Con formato: Español (Colombia)
16
[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.
17
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
18
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 is , vectores de parámetros cinemáticos ip , 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
19
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.
Con formato: Español (alfab.internacional)
20
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.
21
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
22
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.
23
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 y sus interrelaciones, en
1 Notas de clase de la Asignatura Diseño Mecánico. Ph.D Heriberto Maury. Universidad del Norte.
Barranquilla Colombia.
24
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,
25
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
iS , 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
26
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.
27
CAPÍTULO II
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.
Con formato: Derecha, Sangría:Izquierda: 0 cm, Primera línea: 0 cm
Con formato: Título 1, Centrado,Punto de tabulación: 0 cm, Lista contabulaciones
28
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
29
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 nqqqfx ,...,, 21 , permite conocer
la posición y orientación del ET del robot, expresada en coordenadas cartesianas,
vector x , dados los valores de las coordenadas articulares, vector
nqqqq ,...,, 21 . 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 iA que
permite obtener la representación del marco de referencia iR expresado en el
marco de referencia 1iR . Finalmente, multiplicando las n matrices iA se obtiene
una matriz de transformación homogénea n
iT que representa el marco de
referencia nR expresado en el marco de referencia fijo 0R .
30
El algoritmo de D-H comienza identificando los n ejes articulares iq , n ... 2, 1,i ,
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 iq y 1iq . 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 ia . 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 ia con el eje articular 1iq ; el eje ix es la
prolongación de la normal común ia , mientras que su eje iz se alinea con el eje
articular 1iq . 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 ia se ubicarán
de tal modo que se anulan uno o más parámetros id , 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.
ix
iz
1iq
iq
i
1iq
1ix
1iz
iq
ixi
1ia
ia
id
Figura 2 Diagrama de Denavit-Hartemberg para la obtención de la cinemática
31
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 1q 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 nz paralelo a la dirección de la pinza, mientras que
el eje nx se extiende del extremo izquierdo al derecho de dicha pinza. Ahora ya se
puede definir la distancia nd entre los dos últimos eslabones como la distancia
entre los orígenes de los marcos de referencia 1nR y nR , medida a lo largo del eje
nz .
Se complementa la caracterización del i-ésimo eslabón en el quinto paso
definiendo i , como el ángulo que forman los ejes pasando por sus extremos, iq
y 1iq o bien 1iz y iz , medido alrededor del eje ix . Aquí se puede apreciar que el
eje ix coincide con el producto cruz ii zxz 1 . En el sexto paso se caracteriza cada
articulación por el ángulo i entre los eslabones que conecta y la distancia id
entre dichos eslabones. El ángulo i se mide alrededor del eje 1iz entre las dos
normales comunes que posee, 1ia y ia o bien entre los ejes 1ix y ix , mientras
que la distancia id se mide a lo largo del eje 1iz entre las intersecciones con las
dos normales comunes que posee: 1ia y ia . En el caso de la primera articulación
la distancia 1d se mide entre el origen del marco de referencia cero y la
intersección entre 1z y 1a . 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:
32
ia Longitud de la normal común a los ejes 1iz y iz , es decir a los ejes
articulares iq y 1iq .
i Ángulo entre los ejes 1iz y iz , medido en un plano normal al eje ix .
i Ángulo entre los ejes 1ix y ix medido en un plano normal al eje 1iz .
id Distancia medida a lo largo del eje iz entre las intersecciones con sus
normales comunes.
En Lla matriz homogénea de paso generalizado iA , es necesariose requiriere
aplicar las transformaciones homogéneas básicas necesarias para relacionar los
referenciales 1iR y iR . Dichas transformaciones elementales son: una rotación de
i grados alrededor del eje 1iz ; una translación de di centímetros a largo de ese
mismo eje, otra translación de ia centímetros a lo largo del eje 1ix , ahora
relacionados con el eje ix y, finalmente una rotación de i grados alrededor del
eje ix . De modo que la matriz de paso generalizado se estima como:
1000
0
1000
00
00
0001
1000
0100
0010
001
1000
100
0010
0001
1000
0100
00
00
0000
1
1
1
iii
iiiiiii
iiiiiii
i
i
ii
ii
i
i
ii
ii
i
i
iiii
i
i
dCS
SaCSCCS
CaSSSCC
A
CS
SC
a
d
CS
SC
A
xTaTdTzTA ,,,,,,
(Ec 1)
Con formato: Fuente: Sin Negrita
33
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 id 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:
101000
1
0
1
0
1
10
pRpzyxqAqT
iiii
i
n
i
i
i
n
; para i=1, 2 … n (Ec 2)
La cinemática inversa de un robot, expresada por la ecuación xfq 1 , nos
permite calcular el valor de las coordenadas articulares del robot,
n
T qqqq ,...,, 21 , que permiten llevar al ET del robot al punto deseado, expresado
por sus seis coordenadas cartesianas, tres para representar la posición
zyxtT ,, , 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
34
ESHED ROBOTEC
2z
2x
1x
3z
3x
1a 2a
1d
2d
3d
4d
1z
ESHED ROBOTEC
2z
2x
1x
3z
3x
1a 2a
1d
2d
3d
4d
1z
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
ESHED ROBOTEC
2z
2x
1x
3z
3x
1a 2a
1d
2d
3d
4d
1z
ESHED ROBOTEC
2z
2x
1x
3z
3x
1a
2a
1d
2d
3d
4d
1z
4d+
Eslabón 2
Eslabón 3
Eslabón 1
35
indicadas por la (Ec 2), para i=4, se obtiene la matriz T, 4
0TT , 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:
1000
10001000
4444
zzzz
yyyy
xxxx
paon
paon
paon
paonT
pzyxT (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:
1000
4
3
3
2
2
1
1
0
zzzz
yyyy
xxxx
paon
paon
paon
AAAAqT (Ec 4)
36
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:
1000
100
0
0
1
1111
1111
1
0d
qSaqCqS
qCaqSqC
A)()()(
)()()(
1000
100
0
0
2
2222
2222
2
1d
qSaqCqS
qCaqSqC
A)()()(
)()()(
1000
100
0010
0001
3
3
2d
A
1000
100
00
00
4
44
44
4
3d
qCqS
qSqC
A)()(
)()(
0z
y
x
n
n
n
)S(q)S(q)S(q-)C(q)C(q+)C(q)S(q)C(q+)C(qqS
)S(q)C(q)S(q-)S(q)C(q-+)C(q)S(q)S(q-)C(q)C(q
4212142121
4212142121
0
)C(q))S(q)S(q-)C(q)(C(q-)S(q))S(q)C(q+)C(q)(S(q
)C(q)C(q)S(q-)S(q)C(q--)S(q)S(q)S(q-)C(q)C(q
4212142121
4212142121
z
y
x
o
o
o
(Ec 5)
1
0
0
z
y
x
a
a
a
1234
11221221
11221221
d+d+d+d
)S(qa+)S(qa)C(q+)C(qa)S(q
)C(qa+)S(qa)S(q-)C(qa)C(q
z
y
x
p
p
p
Finalmente, la orientación de la pinza con respecto al marco de referencia fijo R0
queda como se muestra en la (Ec 6):
421 qqq roll (Ec 6)
37
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):
100
0
0
)()(
)()(
, CS
SC
zR (Ec 7)
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):
y
x
n
n
S
Croll 1-1- tan
)(
)(tan (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 T
nqqqq ,...,, 21 , 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):
Con formato: Fuente: Sin Negrita
Con formato: Fuente: Sin Negrita
Con formato: Fuente: Sin Negrita
38
4
3
3
2
2
1
4
0
11
0 AAAqTA (Ec 9)
Debido a que 1000
paonT se calculó anteriormente, todo los miembros de la
izquierda en la (Ec 9), son función de las variables articulares ki qq ,..., mientras
que la derecha es función de nk qq ,...,1 , por ende tenemos que (ver Ec 10).
1000
d+d+d100
)S(qa0)q+C(q-)q+S(q
)C(qa0)q+S(q)q+C(q
1000
p)C(qp)S(q-a)C(qa)S(q-o)C(qo)S(q-n)C(qn)S(q-
p)S(qp)C(qa)S(qa)C(qo)S(qo)C(qn)S(qn)C(q
234
224242
224242
1
y1x1y1x1y1x1y1x1
1y1x1y1x1y1x1y1x1
dpaon
a
zzzz
(Ec 10)
De las 12 opciones posibles, se escoge la ecuación donde 1q solo dependa de
constantes como se describe en la (Ec 11).
x
y
a
aq 1
1 tan (Ec 11)
º144º144 1q
donde tan-1 esta definido como (ver Ec 12).
y y para ;º0º90
y y para ;º90º180
y y para ;º180º90
y y para ;º90º0
tan
1
1
1
1
1
1
xq
xq
xq
xq
x
yq (Ec 12)
Ahora para determinar la expresión del modelo cinemático inverso para la
segunda articulación del manipulador se tiene que:
39
4
3
3
2
4
0
11
0
12
1 AAqTAA (Ec 13)
De manera similar tomando el elemento 2,4 se obtiene que:
0a)S(qp)qC(qp)qS(q- 12y21x21 (Ec 14)
Despejando la variable de interés, se tiene finalmente:
111
111
2 tanaqCpqSp
qCpqSpq
xy
yx (Ec 15)
º109º109 2q
Para la cinemática inversa de la articulación prismática se despeja de la (Ec 5):
1243 dddd zp (Ec 16)
Por ultimo el valor de 4q se puede obtener despejando de la (Ec 6).
214 qq q roll (Ec 17)
º527qº527 4
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.
40
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.
00.1
0.20.3
0.40.5
0.20.25
0.30.35
0.4-0.2
-0.15
-0.1
-0.05
0
0.05
0.1
0.15
0.2
Pos - deseada x
GRAFICA 3D DE LA POSICION DESEADA PARA LA TRAYECTORIA 1
Pos - deseada y
Po
s -
desead
a z
Figura 4. Grafica de 3D de las posiciones cartesiana deseadas para la trayectoria 1
41
0.5 1 1.5 2 2.5 3
-0.2
-0.1
0
0.1
0.2
0.3
0.4
0.5
Tiempo (seg)
Po
sic
ion
es d
esead
as -
esp
acio
cart
esia
no
(m
)
POSICIONES CRATESIANAS DESEADAS - TRAYECTORIA 1
Pos - deseadas x
Pos - deseadas y
Pos - deseadas z
Figura 5. Posiciones cartesianas deseadas en el tiempo para la trayectoria 1
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.
Con formato: Español (alfab.internacional)
Con formato: Normal, Izquierda
42
0 0.5 1 1.5 2 2.5 3-3
-2
-1
0
1
2
3
4
5
Tiempo (seg)
Po
sic
ion
es
de
se
ad
as
- e
sp
ac
io a
rtic
ula
r (r
ad
)
POSICIONES ARTICULARES DESEADAS - TRAYECTORIA 1
Pos - deseada q1
Pos - deseada q2
Pos - deseada q3
Pos - deseada q4
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.
43
0.35 0.3505 0.351 0.3515 0.352 0.35250.35
0.355
0.36
0.365
0.37
0.375
Pos - Deseada x (m)
Po
s -
De
se
ad
a y
(m
)
POSICIONES DESEADAS EN EL PLANO X-Y PARA LA TRAYECTORIA 2
Figura 7. Posiciones cartesiana deseadas para la trayectoria 2
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.
Con formato: Español (alfab.internacional)
Con formato: Normal, Izquierda
44
0 0.5 1 1.5 2 2.5 3-3
-2
-1
0
1
2
3
4
5
Tiempo (seg)
Po
sic
ion
de
se
ad
a -
Es
pa
cio
art
icu
lar
(ra
d)
POSICIONES ARTICULARES DESEADAS PARA LA TRAYECTORIA 2
Pos - deseada q1
Pos - deseada q2
Pos - deseada q3
Pos - deseada q4
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.
45
0 0.5 1 1.5 2 2.5 30.34
0.35
0.36
0.37
0.38
0.39
0.4
0.41
Tiempo (seg)
Po
sic
ion
de
se
ad
a -
Es
pa
cio
ca
rte
sia
no
(m
)
POSICIONES CARTESIANAS DESEADAS PARA LA TRAYECTORIA 2
Pos - deseada x
Pos - deseada y
Pos - deseada z
Figura 9. Sistema de coordenadas según la convención D-H
46
CAPÍTULO III
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], Newton-
Euler [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
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)
47
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..
Dinámica Directa
Dinámica Inversa
i
t
Coordenadas
articulares
i
t
Torques/Fuerzas
tt n . . . ,1
tt n . . . ,1
tt n . . . ,1
tt n . . . ,1
Dinámica Directa
Dinámica Inversa
i
t
Coordenadas
articulares
i
t
Torques/Fuerzas
tt n . . . ,1
tt n . . . ,1
tt n . . . ,1
tt n . . . ,1
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
if es la fuerza ejercida por el eslabón (i-1) sobre el eslabón (i) mientras que in es
el momento o torque ejercido sobre el eslabón (i) por el eslabón (i-1), y
48
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).
zi
Fini
xi
xi+1
zi+1
Fi+1
ni+1
ipi+1
mi*gipci
Figura 11 Diagrama de cuerpo libre del eslabón i del robot
ciiciiiii vmvmdt
dgmff
1 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).
iiiiiiiici
i
i
i
ici
i
ii wIwwIwIdt
dfppfpnn
111
para i=1, 2 … n (Ec 19)
donde iI 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
49
momento angular incluye ambos efectos en un término de aceleración angular
ii wI y un término conocido como torque giroscópico iii wIw .
x2
x1
x3
r(op)
o
pm(p)
n
Figura 12 Referencia de la masa de una partícula p
El tensor de inercia iI 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).
2N
1
p,n̂*mn̂,oI (Ec 20)
donde m es la masa de una partícula p del sistema y p,n̂ es la distancia de la
partícula p al eje n̂ . Por definición se tiene que: )op(rn̂)op(rn̂p,n̂2
, de
igual forma n̂ y )op(r
se puede expresar en términos de sus componentes como
en la (Ec 21).
50
3
3
2
2
1
1 en̂en̂en̂n̂
, 3
3
2
2
1
1 e)op(xe)op(xe)op(x)op(r
(Ec 21)
Realizando la operación expandida se obtiene la (Ec 22).
323231312121
2
1
2
2
2
3
2
1
2
3
2
2
2
3
2
2
2
1
2xxnn2xxnn2xxnn2xxnxxnxxnp,n̂
(Ec 22)
obteniendo la expresión del tensor de inercia respecto al punto o, en notación
matricial.
opxopxopr*mn̂,oI kiik
2N
1
donde 3. 2, 1,k,i (Ec 23)
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.
51
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
1i R y sus inversas T
i
1i1
i
1i
1i
i RRR .
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 oS , como se
muestra a continuación.
T
0
0 0 ,0 ,0 w velocidad angular de la base
Tw 0 ,0 ,0 0
0 aceleración angular de la base
T
0
0 0 ,0 ,0 v velocidad lineal de la base
Tv g ,g ,g zyx0
0 aceleración lineal de la base
0
0 w , 0
0w y 0
0 v son variables generalmente nuloas excepto para el caso en que la
base del robot esta en movimiento. Además debido a que el ET sostiene a una
carga, es posible decir que se tiene caracterizada in
in n y in
in f que son el
momento externo y la fuerza externa respectivamente. En los siguientes pasos
del presente algoritmo se utilizaráa la siguiente convención:
T
0 1 ,0 ,0 z , es un vector unitario en la dirección del eje z del marco de
referencia ubicado en la base.
52
Cd,Sd ,a p iiiiii
i , representa las coordenadas del origen del sistema del
sistema iS respecto al sistema 1iS .
i
is , son las coordenadas del centro de masa del eslabón i referenciadas al sistema
iS .
i
i I , es la matriz de inercia del eslabón i respecto a su centro de masa en función de
iS .
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 iS .
i01i
1i
1i
i
i
i qzwRw si el eslabón i es de rotación
1i
1i
1i
i
i
i wRw si el eslabón i es de traslación
Quinto: Cáalculo de la aceleración angular del sistema iS .
i01i
1i
i01i
1i
1i
i
i
i qzwqzwRw si el eslabón i es de rotación
1i
1i
1i
i
i
i wRw si el eslabón i es de traslación
Sexto: Se calcula la aceleración lineal del sistema iS .
1i
1i
1i
i
i
i
i
i
i
i
i
i
i
i
i
i vRpwwpwv si el eslabón i es de rotación
i
i
i
i
i
i
i01i
i
i
i
i
i
i
i
1i
1i
i01i
i
i
i
pww qzRw2
pwvqzRv
si el eslabón i es de traslación
Séptimo: Caálculo de la aceleración lineal del centro de gravedad del eslabón i.
53
i
i
i
i
i
i
i
i
i
i
i
i
i
i vswwswa
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
i1i
1i
1i
i
i
i amfRf
Noveno: Calculo del par ejercido sobre el eslabón i.
i
i
i
i
i
i
i
i
i
i
i
i
ii
i
i
i
1i
1i
i
i
i
1i
i
1i
1i
i
i
i wIwwIamspfpRnRn
Décimo: Calculo de la fuerza o par aplicado a la articulación i.
01i
iT
i
i
i zRn si el eslabón i es de rotación
01i
iT
i
i
i zRf si el eslabón i es de traslación
Donde 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:
54
1. El modelo Newton-Euler es lineal con respecto al tensor de inercia i
i I , lo
cual proviene de la recursión hacia atrás, que se computa en los pasos 8-
10. Las fuerzas/torques i en el paso 10 son lineales en el momento i
in .
2. Para articulaciones rotacionales, el modelo Newton-Euler es no lineal con
respecto a los vectores de centro de masa iS . Los pasos 7 y 8 muestran
que la fuerza neta es lineal en el vector de centro de masa. El producto cruz
i
i
i fxS es no lineal (cuadrático) en iS , por tal motivo el torque aplicado en
una junta rotacional es no lineal en iS .
3. El modelo Newton-Euler es no lineal con respecto al vector de parámetros
cinemáticas i
i p . La fuerza aplicada a cada eslabón es lineal en el vector
i
i p , pero su producto cruz es no lineal en i
i p , por las fuerzas/torques i en
el paso 10 son no lineales en i
i p .
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
i I , 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).
qgqqCqqqBqqA 2 (Ec 24)
55
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.
qA es la matriz simétrica de inercia en el espacio articular de tamaño n x n.
qB es la matriz de efectos de coriolis de tamaño n x n(n-1)/2.
qC es la matriz de torques centrípetos de tamaño n x n.
qg es el n-vector de torques gravitatorios.
Los símbolos qq y 2q son las notaciones para el vector de producto de
velocidad y el vector de velocidades al cuadrado. qq y 2q están dados por:
T
nnnnn ,qq ,qq ... ,qq ,qq ,qq ... ,qq ,qqqq 22423213121
T
nq ... ,q ,qq22
2
2
1
2
Se ejecuta el programa tal que se observa en el anexo ANEXO 5 y se obtiene las
siguientes ecuaciones:
56
21
2
2222244233
3
2
222222222
2
2224
2
222
2
2244
2332
2
2224
2
2223
2
22222222
2
2222
2
12332442222214221332
4
2
22214423
2
2222
2
2221244
12331222
2
2222142212222212
1422442
2
2222222
2
2
2
2322232
2
2
2
222
2
2
2
22244
2
2
2
24
2
22222334
1
2
222
2
241
2
2232123212221244
12441233122212421422
2
2
2
24
2
2
2
22
2
2
2
23111124222222
2
2222
2
112222331222244
2
111
2
112331
]422
4444[]
2222
2[]2
222
2
[]222
222[
]2
32
22222
23[
qqqCSmaamSamS
maqCqSSmaqCqSmaqCqSmaqCqSqamS
amSmaqCqSmaqCqSmaqCqSSmaqCqS
qCSmaqamSamSSmaqCqSamaqSamSqS
maqCqSamSqSmaqCqSmaqCqSaqCmS
aqCmSaqCmSqCSmaSmaqCamaqSSmaqS
SmaqSqIqSmSmaqCqSqCamSmaII
qCammSamamSqCamqCSmaamSI
qammSIISmIIaqCmaaqSmSaqCmS
aqSmSaqSmSaqCSmaqCmaSmaqSqCam
qCamqCamSamSqCmaSmaqCqSqCSma
amSmaamSaqCmaamSSmmSaqCmS
yyy
xy
yx
yyyxx
xy
yyyyx
xzzxyxzzzz
yxxxzz
yzzzzxzzzzyx
yyxy
xxyx
xxxxyx
(Ec 25)
2
2222244
2334421233244222
2
112441244
2222442331233123312241222
12321222122222
2
2
2
22
2
244233222
2442
2
22
2
23311242412331222
1244122212441232
2
222223
2
22
2
24244
2
23223312332
2
212222
]
[]222[]
2
[]23
[]
23
2[
qSmaamS
amSqIqqamSamSSmaqaqCmSaqSmS
SmaamSamSaqSmSaqCmSaqSamaqSma
aqSmaaqSSmaqCmSqmSamamIamSSma
amSISmamIqaqCmaIaqSmSaqSmS
aqCmSaqCmaaqSmSaqCmaSmSmaI
amamamSamIamSaqCmSmSaqCSm
yy
yzzyyyyx
yyyxy
xyyzzxx
xzzxzzzzyy
xyxxzz
xzzxxyx
(Ec 26)
3433 mmdg
(Ec 27)
44
2
2244
2
124412441244
212442244411244424412444
][][
]2[][][
qIqamSqamSaqCmSaqSmS
qqamSqamSIqaqCmSIamSaqSmS
zzyyyx
yxzzxzzxy
(Ec 28)
57
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 iS ,
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 v sF q F 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:
58
)qsign( FS qFVIq
wd)qC))qdqSQwd)qS)q(d)(C(qQ)w(wI
)))w(wdwd)qC))qdqS)C(q)wd)qS))q(d)C(q
qq(d)C(qQ)w(wdwd)qC))qdqS)S(q
wd)qS)qdqCqq(d)C(qQI)ww(w
qS))ww(wQQ)qqq()qq(d))w)(dC(q
))qdqS)S(qwd)qS)q(d)(C(q))qq(-()(d(C(qm
))ww(wQQqqq)))w(wd))w)(d(C(q))qd
qS)C(q))w)(dS(q)qdqCqqd)S(q(m
qCdqSqS))ww(wQQqqq)))w(wd
))w)(d(C(q))qdqS)C(q))w)(dS(q))q()(d(C(q))qq(-((d
)(S(qm))ww(wQQ)qqq()))w(wd))w)(dC(q
))qdqS)S(qwd)qS))q()(d(C(qqqd
)qC(m()(C(qwwQQqqqS)qqwQ
Qqqq)))w(wd))w)(dC(q))qdqS)S(q
)q(d)(C(q))qq(-()(d(C(q ((m))ww(wQQ)qqq(-(
)))w(wd))w)(dC(q)))q()(d(S(q(()C(q)wd)qS))q(d)C(q
)qqd)q(Sm)((C(q)ww((QQ)qq((()C(qd
ZZ
XYZZ
X
YZZ
YX
XY
XY
YX
YXY
X
Xy
XY
11111
122
2
1222122
2
1222212
213122
2
1223122
2
122
2
21333213122
2
1223
122
2
122
2
213333123
312333
2
213213122
2
1223122
2
122
2
21334
12333
2
213213122
2
12
23122
2
122
2
21334
332312333
2
213213
122
2
1223122
2
122
2
213
3412333
2
213213122
2
1223122
2
122
2
213
3432122
2
2132133
3
2
213213122
2
1223
2
122
2
2133412333
2
213
213122
2
1223122
2
122
2
2133432122
2
23221
1)).(
)))..(().().(((())..(().()(
)))..(().().(((.(()))..((.(.(
))(.(()))..((.().(((.((
)))..(().().((())(.(()).(((
))()))(()))).
).().(((.(()))..(().((((
))).))(((().).((
).((.(()).).().((())(.((((
)((()())).()).).))(((()
.).().(((.(().
((()))(().
).().(((.(()))..(())(.((
.)(.((.()).().)(((()))())).((
.)(()).).().(((.((
).())
)..)))..((.(.(
))(.(.()(.(()).(..(
(Ec 30)
)qsign( FS qFV) w)d C(qq) d( S(qQ)w)d S(qq)d( -C(qQ
q) d( S(qQ)w)d S(qq)d(-C(q)- Q)Iw w ((w))) w ( w d
w)d C(q q) d)( S(q C(q)w) d S(q q) d-C(q)q q()( d( -S(qQ
)))qq(dq)dC(q))q(-)d)( (-(S(q S(q)))q)(d S(q
)))q(-()(d(C(q))qq(-()(d(C(q) -Q)Iqqq ((())))S(qqQ
-)Q)qqq((-())))qq(d)q)(dC(q)))q(-)(d)( -(S(q S(q
)) q)dS(q)q()d(C(q))qq(-()(d(C(q ((m)))qqq(Q
Q)qqq(-()))))qq(d))q)(dC(q)))q()(d(S(q)((C(q
))))w)(dS(q))q()(d(C(q)qq(()(d(S(q()(((m(C(qd
xy
xyZZ
x
YzzY
X
X
y
222122
2
1222122
2
1222
2
1222122
2
12223213213
122
2
1223122
2
122
2
21333
213122
2
1223122
2
122
2
213333213323
3
2
2132131
2
1223
122
2
122
2
213343
3213213122
2
1223
122
2
12
2
21334332
2
22
213
2
2
(Ec 31)
59
)qsign(FS qFV]213222
21223122
2122
22133
[3
2132222
1223
1222
1222
3233332133
3334
))q q( d w)d C(q
q) d)( S(q C(q q) d S(q)q(-) d C(q )q q( )dS(qx
Q
))) q q( dq)d C(q) q() d)( S(q S(q
) q) d S(q)q(-) d C(q ( -W)( d( C(qy
- QZZ
)Iqq q (τ
(Ec 32)
)q)sign((FS) q( FV) q(IA) q ( -G Mτ4444444344 (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.
60
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.
0 0.01 0.02 0.03 0.04 0.05 0.06-1200
-1000
-800
-600
-400
-200
0
200
400
600
Tiempo (seg)
To
rqu
e (
N-m
)
COMPORTAMIENTO DE LOS TORQUE EN TIEMPO PARA LA TRAYECTORIA 1
Torque 1
Torque 2
Torque 3
Torque 4
Figura 13. Comportamiento de los torques en el tiempo para la trayectoria 1
61
0 0.5 1 1.5 2 2.5 3-20
-15
-10
-5
0
5
10
15COMPORTAMIENTO DE LOS TORQUES EN TIEMPO PARA LA TRAYECTORIA 2
Tiempo (seg)
To
rqu
es
(N
-m)
Torque 1
Torque 2
Torque 3
Torque 4
Figura 14. Comportamiento de los torques en el tiempo para la trayectoria 2
1.44 1.46 1.48 1.5 1.52 1.54 1.56 1.58
-4
-2
0
2
4
6
8
COMPORTAMIENTO DE LOS TORQUES EN TIEMPO PARA LA TRAYECTORIA 2
Tiempo (seg)
To
rqu
es
(N
-m)
Torque 1
Torque 2
Torque 3
Torque 4
Figura 15. Comportamiento de los torques en t=1.5 seg para la trayectoria 2
62
0.01 0.02 0.03 0.04 0.05 0.06
-6
-4
-2
0
2
4
6
8
10
COMPORTAMIENTO DE LOS TORQUES EN TIEMPO PARA LA TRAYECTORIA 2
Tiempo (seg)
To
rqu
es
(N
-m)
Torque 1
Torque 2
Torque 3
Torque 4
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
63
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.
JRa
Kf
Va
+
_
LaVemf
+
_aplicado
motor
JRa
Kf
Va
+
_
LaVemf
+
_aplicado
motor
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:
0emfLaRaa VVVV (Ec 34)
De acuerdo con la ley de Ohm, el voltaje a través de una resistencia puede ser
expresado como:
aaRa RiV (Ec 35)
donde ai 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:
dt
idLV a
aLa (Ec 36)
64
donde La es la inductancia de la bobina de la armadura. Finalmente, el voltaje
contraelectromotriz puede ser estimado como:
avemf wKV (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).
0ava
aaaa wKdt
idLRiV (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.
65
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.
Con formato: Sangría: Izquierda: 0cm, Interlineado: sencillo
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
66
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:
ddv qGqFqFqqVqqM )()(,)( (Ec 39)
O
dqqNqqM ,)( , (Ec 40)
Con la variable articular )(,)( tRtq n par de control de rotación, y )(td 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 tqd 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:
Con formato: Sin viñetas ninumeración
67
tqtqte d (Ec 41)
Para demostrar la influencia de la entrada )(t sobre el error de seguimiento,
derivemos dos veces y se obtiene:
qqe d
qqe d
Solucionando q en la (Ec 40)[] y sustituyendo en la (Ec 41) de los rendimientos se
obtiene:
)(1
dd NMqe (Ec 42)
Definiendo la función de control de entrada
)(1 NMqu d (Ec 43)
Y la función perturbación
dMw 1 (Ec 44)
Podemos definir un estado )(tx nR2 por
e
ex
(Ec 45)
Y escribiendo el error de seguimiento dinámico como
68
wI
uIe
eI
e
e
dt
d 00
00
0
(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:
NuqM d )( (Ec 47)
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).
NuqMNMq dd )(
o
,1
dMue (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
69
para un sistema lineal que consiste en la n subsistemas desacoplados, que
obedece a las leyes de Newton.
Manipulador
q
qM(q)
dq-
dq
Regeneración
de lazo externo
N(q,q)
Lazo no lineal
interior
Sistema
lineal
u
Manipulador
qM(q)
dqdq-
dqdq
Regeneración
de lazo externo
N(q,q)
Lazo no lineal
interior
Sistema
lineal
u
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:
)()()( sEsHsU (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
)()( 2 sHIssT . (Ec 50)
70
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 uqd
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 uqd en lugar de )(tq .
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.
eKeKu pv (Ec 51)
Luego la entrada neta del manipulador robótico resulta en:
),())(( qqNeKeKqqM pvd
(Ec 52)
Este mismo controlador se muestra en la Figura 19 con Ki = 0.
71
Modelo DinModelo Dináámicomico
Directo SCARADirecto SCARA qq
Modelo DinModelo Dináámicomico
Inverso SCARAInverso SCARA
dq
dq
dq
Kp
Kv
+ -
+ -
q
q
q
Modelo DinModelo Dináámicomico
Directo SCARADirecto SCARA qq
Modelo DinModelo Dináámicomico
Inverso SCARAInverso SCARA
dq
dq
dq
Kp
Kv
+ -
+ -
q
q
q
Figura 19 Esquema de control PID por par calculado
El error dinámico del lazo cerrado es:
weKeKe pv
(Ec 53)
O en la forma de espacio-estado:
wIe
e
KK
I
e
e
dt
d
vp
00
(Ec 54)
El polinomio característico del bucle es :
pvc KsKIss 2)( (Ec 55)
72
4.2.3 Selección de las ganancias (PD)
Es usual tomar el producto diagonal de las matrices n*n tal que:
,viv kdiagK ,pip kdiagK (Ec 56)
Entonces:
n
i
pivic kskss1
2)( (Ec 57)
Y el sistema respecto al error es estable asintóticamente cuando viK y piK 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
)(td .
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 )(qM y la adición de términos no lineales de salida
),(.
qqN en el bucle interno reparte la señal )(tu a todas las juntas. Así que es
necesaria la información de todas las posiciones )(tq y las velocidades )(.
tq 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:
22 2)( nnsssp (Ec 58)
73
Donde es el factor de amortiguamiento y n la frecuencia natural. De esta forma,
el rendimiento deseado en cada componente del error )(te puede ser alcanzado al
seleccionar ganancias del PD como:
2
npik y sk nvi 2 (Ec 59)
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:
pivi kk 2 , 4
22
vi
pi
kk (Ec 60)
La selección de la frecuencia natural n , 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:
74
KP KV
Articulación 1 125000 250
Articulación 2 140000 200
Articulación 3 150000 1000
Articulación 4 120000 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
75
0 0.5 1 1.5 2 2.5 3-3
-2
-1
0
1
2
3
4
5COMPARACION ENTRE q real (rad) - q deseado (rad) PARA LA TRAYECTORIA 1
Tiempo (seg)
q r
ea
l (r
ad
) -
q d
es
ea
do
(ra
d)
Pos - deseado q1
Pos - deseado q2
Pos - deseado q3
Pos - deseado q4
Pos - real q1
Pos - real q2
Pos - real q3
Pos - real q4
zona 3
zona 1
zona 4
zona 2
Figura 20. Comparación entre q real y q deseado para la Trayectoria 1
76
Figura 21. Comparación de q real y q deseado para la Trayectoria 1 en cada una de las zonas
1.49 1.495 1.5 1.505 1.51 1.515 1.52 1.525
0.08
0.1
0.12
0.14
0.16
0.18
0.2
0.22
0.24
0.26
COMPARACION ENTRE q4 real (rad) - q4 deseado (rad) PARA LA TRAYECTORIA 1
Tiempo (seg)
q4
re
al (
ra
d)
- q
4 d
es
ea
do
(ra
d)
Pos - deseado q4
Pos - real q4
1.5 1.5 1.5 1.5 1.5 1.5 1.5
4.264
4.2641
4.2641
4.2642
4.2642
4.2643
COMPARACION ENTRE q3 real (rad) - q3 deseado (rad) PARA LA TRAYECTORIA 1
Time (seg)
q3
re
al (r
ad
) -
q3
de
se
ad
o (
ra
d)
Pos - deseada q3
Pos - real q3
1.5 1.5 1.5 1.5 1.5 1.5
1.2247
1.2247
1.2248
1.2248
1.2248
1.2248
1.2248
1.2249
Tiempo (seg)
q1
re
al (r
ad
) -
q1
de
se
ad
o (
ra
d)
COMPARACION ENTRE q1 real (rad) - q1 deseado (rad) PARA LA TRAYECTORIA 1
Pos - deseada q1
Pos - real q1
1.5 1.5 1.5 1.5 1.5
-2.3474
-2.3474
-2.3474
-2.3474
-2.3474
-2.3473
-2.3473
-2.3473
Time (seg)
q2
re
al (r
ad
) -
q2
de
se
ad
o (
ra
d)
COMPARACION ENTRE q2 deseado (rad) - q2 real (rad) PARA LA TRAYECTORIA 1
Pos - deseado q2
Pos - real q2
77
0 0.5 1 1.5 2 2.5 3
-2
-1
0
1
2
3
4
Tiempo (seg)
q r
ea
l (r
ad
) -
q d
es
ea
do
(ra
d)
COMPARACION ENTRE q real (rad) - q deseado (rad) PARA LA TRAYECTORIA 2
Pos - deseada q1
Pos - deseada q2
Pos - deseada q3
Pos - deseada q4
Pos - real q1
Pos - real q2
Pos - real q3
Pos - real q4
zona 3
zona 2
zona 4
zona 1
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.
78
Figura 23. Comparación de q real y q deseado para la Trayectoria 2 en cada una de las zonas
1.497 1.498 1.499 1.5 1.501 1.502 1.503
0.3998
0.3999
0.3999
0.4
0.4001
0.4001
0.4002
Tiempo (seg)
q r
ea
l (r
ad
) -
q d
es
ea
do
(ra
d)
COMPARACION ENTRE q real (rad) - q deseado (rad) PARA LA TRAYECTORIA 2
Pos - desead q4
Pos - real q4
1.4998 1.4999 1.4999 1.5 1.5 1.5001 1.5001 1.5002 1.5002 1.5003 1.5003
3.5838
3.5838
3.5838
3.5838
3.5838
3.5838
3.5838
Tiempo (seg)
q r
ea
l (r
ad
) -
q d
es
ea
do
(ra
d)
COMPARACION ENTRE q real (rad) - q deseado (rad) PARA LA TRAYECTORIA 2
Pos - deseada q3
Pos - real q3
1.499 1.499 1.499 1.499 1.499 1.4991
1.4081
1.4081
1.4081
1.4081
1.4081
1.4081
Tiempo (seg)
q r
ea
l (r
ad
) -
q d
es
ea
do
(ra
d)
COMPARACION ENTRE q real (rad) - q deseado (rad) PARA LA TRAYECTORIA 2
Pos - deseada q1
Pos - real q1
1.499 1.499 1.499 1.499 1.499 1.499 1.499 1.499 1.499 1.499 1.499
-1.8503
-1.8503
-1.8503
-1.8503
-1.8503
-1.8503
-1.8503
-1.8503
-1.8503
-1.8503
Tiempo (seg)
q r
ea
l (r
ad
) -
q d
es
ea
do
(ra
d)
COMPARACION ENTRE q real (rad) - q deseado (rad) PARA LA TRAYECTORIA 2
Pos - deseada q2
Pos - real q2
79
0.348 0.35 0.352 0.354 0.356 0.358 0.36 0.362 0.364 0.366 0.3680.35
0.355
0.36
0.365
0.37
0.375
0.38
Posicion en el eje x (m)
Po
sic
ion
en
el e
je y
(m
)
COMPARACION ENTRE Pos - deseada (m) Y Pos - real (m) PARA LA TRAYECTORIA 2
0.3648 0.3649 0.3649 0.365 0.365
0.3649
0.365
0.3651
0.3652
Figura 24. Posición real y Deseado para la Trayectoria 2 en el espacio de la tarea
80
CAPÍTULO V
5. RESULTADOS Y ANÁLISIS DE RESULTADOS
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.
Con formato: Título 1, Centrado,Sangría: Izquierda: 0 cm, Primeralí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óndespués de: 0,85 cm + Sangría: 0,85cm, Punto de tabulación: 0 cm, Listacon tabulaciones + No en 0,85 cm
Con formato: Normal
Con formato: Español (alfab.internacional)
81
Figura 25 Simulación del Robot SCARA de Cuatro Grados de Libertad
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).
Con formato: Centrado
Con formato: Centrado
82
IAE= dtte0
)( (Ec 61)
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®.
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
Set
Point
Variable de proceso
Tiempo
(+
) ( - )
( - )
Error
Error
83
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.
0 0.01 0.02 0.03 0.04 0.05 0.06 0.07 0.08-2
-1.5
-1
-0.5
0
0.5
1
1.5
2
2.5x 10
-3
Tiempo (seg)
Err
ore
s A
rtic
ula
res
(ra
d)
ERRORES ARTICULARES PARA LA TRAYECTORIA 1
Errores q1
Errores q2
Errores q3
Errores q4
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.
84
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.
0 0.01 0.02 0.03 0.04 0.05 0.06 0.07 0.08-5
0
5
10x 10
-4
Tiempo (seg)
Err
ore
s C
art
es
ian
os
(m
)
ERRORES CARTESIANOS PARA LA TRAYECTORIA 1
Errores x
Errores y
Errores z
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
85
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. .
0 0.5 1 1.5 2 2.5 3-1
0
1
2
3
4
5
6
7
x 10-3
Tiempo (seg)
Err
or
ab
so
luto
ERRORES ABSOLUTOS ARTICULARES - TRAYECTORIA 1
2.9996 2.9997 2.9998
4.5
5
5.5
6
x 10-5
0 0.05 0.1
0
1
2
3
x 10-5
iaeq1
iaeq2
iaeq3
iaeq4
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%,
86
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.
0 0.5 1 1.5 2 2.5 3-1.5
-1
-0.5
0
0.5
1
1.5
2
2.5x 10
-5
Tiempo (seg)
Err
ore
s C
art
es
ian
os
(m
)
ERRORES CARTESIANOS PARA LA TRAYECTORIA 2
0 0.02 0.04 0.06 0.08
-1
-0.5
0
0.5
1
1.5
2
x 10-5
1.5 1.52 1.54 1.56 1.58
-6
-4
-2
0
2
x 10-6
1.9927 1.9928 1.9928
-1
0
1
2
3x 10
-9
Errores x
Errores y
Errores z
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
87
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.
0 0.5 1 1.5 2 2.5 3-6
-4
-2
0
2
4
6
8x 10
-5
Tiempo (seg)
Err
ore
s a
rtic
ula
res
(ra
d)
ERRORES ARTICULARES PARA LA TRAYECTORIA 2
0.01 0.02 0.03 0.04 0.05 0.06
-4
-2
0
2
4
6
x 10-5
1.5 1.52 1.54
-1.5
-1
-0.5
0
0.5
1
x 10-5
0.12 0.125 0.13
-2
0
2
4x 10
-8
Errores q1
Errores q2
Errores q3
Errores q4
Figura 31. Errores articulares para la trayectoria 2
CTC
Trayectoria 1 Trayectoria 2
IAE
Articulares
IAEQ1 5.2694e-005 0.0026619
IAEQ2 5.8618e-005 0.006139
IAEQ3 4.7653e-005 0.00067209
IAEQ4 0.006988 0
IAE
Cartesianas
IAEx 2.3544e-005 0.0019172
IAEy 7.9362e-006 0.00057782
IAEz 0.006988 0
88
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.
89
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.
90
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. .
91
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
Con formato: Inglés (Estados Unidos)
92
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.
Con formato: Inglés (Estados Unidos)
93
[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.
Con formato: Inglés (Estados Unidos)
Código de campo cambiado
Con formato: Inglés (Estados Unidos)
Con formato: Inglés (Estados Unidos)
94
[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.
95
[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.
96
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);
97
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
98
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];
99
ANEXO 4. PROGRAMAS DEL MODELO DINÁMICO
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)
Con formato: Español (alfab.internacional)
Código de campo cambiado
Con formato: Español (alfab.internacional)
100
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; M4=1.8; d2=0.5; d3=0.3;% r4=0.2;% ZZ1R=3.38; ZZ2R=0.063; ZZ3R=0.1;% MX2R=2*0.121; MX3R=0.2;% MY3R=0.1;% MY2=0.001; FV1=0.1; FV2=0.012; FV3=FV1; FV4=FV2; FS1=0.57; FS2=0.125; FS3=FS1; FS4=FS2; IA2=0; CZ1=0; FX2=0; FY2=0; FX3=0;% FY3=0;% FZ3=0;% CZ3=0;% CZ4=0;% CX3=0;% CX4=0;% CY3=0;% CY4=0;% FX4=0;% FY4=0;% FZ4=0;% IA3=0;% IA4=0.045;% GAM1=u(1); GAM2=u(2); GAM3=u(3); GAM4=u(4); S1=sin(x(1)); C1=cos(x(1)); S2=sin(x(2)); C2=cos(x(2)); S3=sin(x(3)); C3=cos(x(3)); QP1=x(5); QP2=x(6); QP3=x(7); QP4=x(8); W32=QP1 + QP2; JPR132=d2.*S2; JPR232=C2.*d2; W33=QP3 + W32; JPR133=d3.*S3;
101
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);
102
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;
103
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;
104
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(3) = x(3); sys(4) = x(4); sys(5) = x(5); sys(6) = x(6); sys(7) = x(7); sys(8) = x(8); %end mdlOutputs
105
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; M4=1.8; d2=0.5; d3=0.3;% ZZ1R=3.38; ZZ2R=0.063; ZZ3R=0.1;% MX2R=2*0.121; MX3R=0.2;% MY3R=0.1;% MY2=0.001; FV1=0.1; FV2=0.012; FV3=FV1; FV4=FV2; FS1=0.57; FS2=0.125; FS3=FS1; FS4=FS2; IA2=0; CZ1=0; FX2=0; FY2=0; FX3=0;% FY3=0;% FZ3=0;% CZ3=0;% CZ4=0;% FX4=0;% FY4=0;% FZ4=0;% IA3=0;% IA4=0.045;% %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; t3=position3; QP1=vitesse1; QP2=vitesse2; QP3=vitesse3; QP4=vitesse4; QDP1=w1; QDP2=w2; QDP3=w3; QDP4=w4; S2=sin(t2); C2=cos(t2); S3=sin(t3); C3=cos(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;
106
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;
Con formato: Español (alfab.internacional)
107
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);
108
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
109
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'));
110
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;
111
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;
112
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
113
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