Con Formato

i
MODELAMIENTO DE LA CINEMATICA Y DINAMICA DE UN ROBOT

MANIPULADOR DE CUATRO GRADOS DE LIBERTAD TIPO SCARA PARA EL
DISEO E IMPLEMENTACION EN SIMULACION DE UN CONTROLADOR
LINEAL POR RETROALIMENTACION TIPO CTC (COMPUTER TORQUE
CONTROL)

ING. GUILLERMO ELIECER VALENCIA OCHOA

UNIVERSIDAD DEL NORTE
DIVISION DE INGENIERAS
DEPARTAMENTO DE INGENIERA MECNICA
MAESTRA EN INGENIERA MECNICA
BARRANQUILLA
2008
Con formato
ii
MODELAMIENTO DE LA CINEMATICA Y DINAMICA DE UN ROBOT
MANIPULADOR DE CUATRO GRADOS DE LIBERTAD TIPO SCARA PARA EL
DISEO E IMPLEMENTACION EN SIMULACION DE UN CONTROLADOR
LINEAL POR RETROALIMENTACION TIPO CTC (COMPUTER TORQUE
CONTROL)

ING. GUILLERMO ELIECER VALENCIA OCHOA.

Monografa presentada como requisito para optar al ttulo de Magster en
Ingeniera Mecnica

DIRECTOR:
ING. MARCO SANJUAN MEJIA, Ph. D

UNIVERSIDAD DEL NORTE
DIVISION DE INGENIERAS
DEPARTAMENTO DE INGENIERA MECNICA
MAESTRA EN INGENIERA MECNICA
BARRANQUILLA
2008
iii
Aprobado por la Divisin de Ingenieras
en cumplimiento de los requisitos
exigidos para otorgar el ttulo de
Magster en Ingeniera Mecnica.

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

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: Sangra: Izquierda: 0
cm
v
AGRADECIMIENTOS

El autor expresa su agradecimiento:

Al DEPARTAMENTO DE INGENIERIA MECANICA MECNICA - UNINORTE, por
permitirme ser parte de su familia y realizar todos mis estudios de Pregrado y
Maestra en Ingeniera Mecnica.

A MARCO SANJUAN, por ser ms 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 acompaarlo en investigacin en el
Laboratorio de Maquinas Mquinas Hidrulicas adscrito al programa de Ingeniera
Mecnica de la Universidad del Norte, en el rea de Transferencia de Calor de
Nanofluido y en general por toda su valiosa colaboracin durante este proceso.

A mis amigos de la MAESTRA EN INGENIERA MECANICA: 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. INTRODUCCIN ........................................................................................ 14
1.1. Antecedentes ....................................................................................... 14
1.2. Planteamiento del Problema e Hiptesis .............................................. 18
1.3. Justificacin .......................................................................................... 20
1.4. Objetivos .............................................................................................. 20
1.4.1. Objetivos Generales .......................................................................... 21
1.4.2. Objetivos Especficos ........................................................................ 21
1.5. Metodologa de la investigacin ........................................................... 21
1.6. Contribuciones ..................................................................................... 25
1.7. Organizacin del Documento ............................................................... 25
2. MODELADO CINEMTICO DEL ROBOT INDUSTRIAL ........................... 27
2.1 Introduccin. .......................................................................................... 27
2.2 Descripcin del Robot. .......................................................................... 28
2.3 Modelado de la Cinemtica de Robots .................................................. 29
2.4 Cinemtica Directa del Robot SCARA................................................... 33
2.5 Cinemtica Inversa del Robot SCARA .................................................. 37
3. MODELADO DINMICO DEL ROBOT INDUSTRIAL ................................ 46
3.1 Dinmica del Robot manipulador. ......................................................... 46
3.2 Formulacin Newton-Euler. ................................................................... 47
vii
3.3 Algoritmo computacional para determinar el modelo dinmico por
Newton-Euler. .............................................................................................. 50
3.4 Modelo dinmico del Robot SCARA. ..................................................... 54
3.5 Modelo dinmico de los motores DC ..................................................... 62
4. CONTROL DINMICO DEL ROBOT INDUSTRIAL ................................... 65
4.1 Introduccin. .......................................................................................... 65
4.2 Computer Torque Control CTC. ............................................................ 66
4.2.1 Derivacin de Lazo Interior por retroalimentacin .............................. 66
4.2.2 Diseo del lazo externo (PD) .............................................................. 70
4.2.3 Seleccin de las ganancias (PD) ........................................................ 72
4.2.4 Respuesta del Computer Torque Control. .......................................... 74
5. RESULTADOS Y ANLISIS 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 obtencin de la cinemtica .... 30
Figura 3 Sistema de coordenadas segn la convencin 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 segn la convencin D-H ............................... 44
Figura 9. Sistema de coordenadas segn la convencin D-H ............................... 45
Figura 10 Diagrama de relacin entre Dinmica Directa e Inversa ........................ 47
Figura 11 Diagrama de cuerpo libre del eslabn i del robot ................................... 48
Figura 12 Referencia de la masa de una partcula 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 Representacin elctrica 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. Comparacin entre q real y q deseado para la Trayectoria 1 ............... 75
Figura 21. Comparacin de q real y q deseado para la Trayectoria 1 en cada una
de las zonas ........................................................................................................... 76
Figura 22. Comparacin entre q real y q deseado para la Trayectoria 2 ............... 77
Figura 23. Comparacin de q real y q deseado para la Trayectoria 2 en cada una
de las zonas ........................................................................................................... 78
Figura 24. Posicin real y Deseado para la Trayectoria 2 en el espacio de la tarea
............................................................................................................................... 79
Figura 25 Simulacin 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 Parmetros D-H del robot SCARA ............................................................ 34
Tabla 2 Parmetros de robot SCARA .................................................................... 59
Tabla 3 Parmetros de sintonizacin del Controlador CTC ................................... 74
Tabla 4 ndices de desempeo para el Controlador CTC ...................................... 88

xi

LISTA DE ANEXOS

PAG
ANEXO 1. CODIGOS PARA LA GENERACIN DE TRAYECTORIAS ................ 96
ANEXO 2. PROGRAMA DE INICIALIZACIN DEL CONTROLADOR CTC ........ 97
ANEXO 3. PROGRAMAS DEL MODELO CINEMTICO ..................................... 98
ANEXO 4. PROGRAMAS DEL MODELO DINMICO .......................................... 99
ANEXO 5. PROGRAMA PARA EL CALCULO DE LA D.I. ROBOT HASTA 5 DOF
............................................................................................................................. 107

12
RESUMEN

Esta investigacin muestra detalladamente el modelo cinemtico de un robot tipo
SCARA, basado en la determinacin de la direccin del elemento terminal del
robot a partir de las variables de cada junta. Una parte importante de esta seccin
es el uso de la representacin de DenavitHartenberg [6]. Adicionalmente, se
muestra el estudio de la relacin matemtica entre el movimiento del robot y las
fuerzas implicadas en el mismo, lo cual representa el modelo dinmico directo e
inverso del manipulador.

Posteriormente se presenta la implementacin de un controlador basado en el
modelo de referencia, en nuestro caso el Computer Torque Control (CTC), donde
el sistema dinmico se linealiza y se desacopla por realimentacin, lo que
transforma el sistema dinmico del manipulador a controlar en un simple par de
integradores. El controlador estudiado se simulo sobre el modelo del robot SCARA
con valores numricos 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
sbito de direccin en el elemento terminal que se experimenta en la trayectoria 2,
adems de un cambio de direccin suave junto a una excitacin muy fuerte en el
eje z, como lo es la trayectoria 1.

Los resultados simulados en la presente investigacin se obtuvieron sobre el
modelo de un robot de cuatro grados de libertad (GDL), tipo SCARA. El
desempeo 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 retroalimentacin con compensador basado en el modelo
dinmico no lineal tipo CTC (Computer Torque Control) con un ndice de
desempeo 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,
Sangra: Izquierda: 0 cm, Primera
lnea: 0 cm, No ajustar espacio entre
texto latino y asitico, No ajustar
espacio entre texto asitico y nmeros
14

Captulo I

1. NTRODUCCIN

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 mtodos de control en aplicaciones de robtica
y reas relacionadas. En general, stos han sido clasificados en dos categoras:

Mtodos de control convencionales: Proporcional Derivativo (PD),
Proporcional Integral Derivativo (PID), Computer Torque Control (CTC).
Mtodos avanzados de control: Control Adaptativo (AC), Control Robusto
(RC), Control Robusto Adaptativo (ARC).

Mtodos de control convencionales.
En el campo de la robtica, 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 disear un controlador para cada junta como
un sistema una-entrada/una-salida. Esto es, el manipulador dinmico es reducido
Con formato: Normal, Derecha,
lnea: 0 cm, No ajustar espacio entre
texto latino y asitico, No ajustar
espacio entre texto asitico y nmeros
Con formato: Normal, No ajustar
espacio entre texto latino y asitico, No
ajustar espacio entre texto asitico y
nmeros
Con formato: Normal
Con formato: Ttulo 1, Centrado
15
a un sistema de segundo orden lineal acoplado. Por lo tanto, mtodos 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 dinmica de manipuladores es mucho masms
compleja que la dinmica asumida por el acercamiento (SISO) [32]. La dinmica
de manipuladores incluye no slo efectos dobles acoplados sino tambin otros
trminos no lineales como la friccin y la gravedad, que explican las falencias de
disean disear mtodos de control SISO en muchos casos. Estas falencias
ocurren regularmente cuando se incurre en ignorar trminos 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 Dinmico Inverso simplificado, el error de seguimiento no
se evala en trminos 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 linealizacin
por realimentacin no lineal [32],[22],[19] ha sido propuesta para dar solucin a
estas dificultades. Aunque algunas mejoras fueron reportadas, se levantaron
inconvenientes debido a los requerimientos de conocimientos acerca de la
dinmica del manipulador. En consecuencia, la comunidad cientfica dedicada al
control de manipuladores ha avanzado hacia controladores con mejor rendimiento
pero tambin con mayor complejidad.

Mtodos de control Avanzados.
En control robtico, controladores avanzados tales como Adaptative Control (AC)
[17],[30], Robust Control (RC) [15],[43], Adaptative Robust Control (ARC)
Con formato: Espaol (Colombia)
16
[14],[28],[34], han estado altamente ligados con incertidumbres paramtricas. De
cualquier manera, su rpido desarrollo aconteci recientemente cuando el
conocimiento terico acerca de los sistemas no lineales y el entendimiento de la
dinmica de los sistemas de control robtico, llego a ser mejor explorado con
avances en computacin y tecnologa de microprocesadores [1],[2],[3].
Las ultimasltimas dos dcadas, tuvieron como testigo una gran variedad de
nuevos mtodos de control modernos como los AC, RC, ARC, en sistemas no
lineales en general (p. ej., [14],[17],[28],[34]) y en sistemas robticos en particular
(p. ej., [41],[43],[30],[15]).

En el desarrollo inicial del control robtico adaptativo, solo slo algunos
controladores robticos adaptativos fueron propuestos debido a la complejidad de
la dinmica de los robots. Uno de los primeros algoritmos es el Model Reference
Adaptative Systems (MRAS) presentado en [10]. ste es diseado 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 dinmica comprehensiva de un sistema robtico y sus
propiedades. Un gran paso adelante del control robtico adaptativo se dio di a
principios de la dcada de los 80s cuando la propiedad lineal en parmetros fue
fu reconocida y aplicada por Slotine y Li [36], y Craig [7] para disear 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 robtica. De cualquier manera, algunos autores en
[12],[28],[29] sealaron 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 funcin de satisfacer la hasta entonces inmoderada dinmica 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. Mtodos AC son
capaces de proveer un rendimiento estable en plataforma, es decir, el resultado
ms pobre del rendimiento en traansitorio de de informacin que los controladores
adaptativos usualmente es proyectadose presenta cuando solo en el caso ideal
hay de incertidumbres paramtricas y su rendimiento en transito depende de
parmetros estimados y condiciones iniciales. Durante la plataforma en
transitotransitorio, especialmente en el inicio, los parmetros son pobremente
estimados desde el proceso de estimacin de parmetros 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 parmetros y condiciones de estado inicial
son la causa de un pobre bajo rendimiento en transito de los sistemas de control
adaptativo. Adems, cuando las alteraciones perturbaciones externas son tenidas
en cuenta no es posible evitar un decrecimiento asinttico de la estabilidad. La
razn es debida a la derivacin de parmetros como resultado de la introduccin
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 mtodo de Control por Modos Deslizantes (SMC, por sus siglas en ingls) es un
popular mtodo de control robusto, que puede trabajar de manera satisfactoria con
la mayora de las debilidades de los controles adaptativos. El uso inicial del modo
de control mvil es elevar la fortaleza de los sistemas de control robtico, acorde
con las incertidumbres originadas en [38],[35],[42]. Dicho mtodo ha sido mejorado
por otros autores [46],[31],[41], especialmente para cubrir enlaces y el control
prolongado de entradas que los controladores mviles clsicos no cubran. En los
controladores mviles clsicos, errores en curso eran minimizados gracias a la
aplicacin de una entrada de control cambiante. Como un resultado durante la
plataforma en trnsito cuando grandes incertidumbres se presentan, los sistemas
18
son fuertes y presentan un buen rendimiento en trnsito. Debido a la ausencia de
parmetros de adaptacin, RC en general y SMC en particular no discriminan
entre alteraciones externas e incertidumbres paramtricas y la plataforma estable.
Esto es, RC produce un rendimiento similar en ambas operaciones.

Por otra parte, desde hace muchos aos se han realizado estudios alrededor del
Control Predictivo Basado en Modelos (MPC), a finales de los aos setenta
surgieron los primeros algoritmos que usaban explcitamente un modelo dinmico
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 Hiptesis
Como se ha mencionado en el inciso 1.1, el desempeo de los Manipuladores
Industriales de varios grados de libertad en aplicaciones donde se requiere el
control continuo de su posicin 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 dinmica de dichos Manipuladores
Industriales vara segn su posicin, lo cual provoca serias perturbaciones al
sistema. Caractersticas como las no linealidades en los vectores de centros de
masa
i
s , vectores de parmetros cinemticos
i
p , provocan el comportamiento no
lineal del modelo dinmico que relaciona pare/fuerza aplicado en las juntas con la
cinemtica articular y cartesiana.

Los aspectos anteriores no estn totalmente caracterizados y descritos desde el
punto de vista matemtico cuando se trata de manipuladores de un nmero
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 desempeos limitados o
19
costos muy elevados para aplicaciones industriales. Esto se debe, en buena
medida, a un conocimiento limitado de los parmetros del modelo dinmico de los
manipuladores, tanto analtico como experimental, lo que limita las posibilidades
de sntesis de estrategias de control adecuadas. Todo lo anterior constituye el
problema cientfico abordado en este trabajo.
Dndole solucin a este problema se podran 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
vehculos; todo lo planteado anteriormente lleva a los siguientes interrogantes
cientficos:

Cmo mejorar el modelamiento de la dinmica de los Manipuladores
Industriales tipo SCARA para usarlos de base en la sntesis de los
controladores?
Cmo 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 prcticos ofrece la implementacin 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 hiptesis que, el controlador
tipo CTC presenta un buen comportamiento en cuanto a minimizacin del
IAE(integral Absolute Error), cuanto se trata de controlar la posicin de un
manipulado de cuatro grados de libertad tipo SCARA, pudindose aplicar en
sistemas comerciales de varios grados de libertad.

Con formato: Espaol (alfab.
internacional)
20
1.3. J ustificacin
La Robtica Industrial es una de las reas de la Tecnologa que presentan los
retos ms estimulantes para los prximos aos y la teora 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 tcnicas de Control
por retroalimentacin lineal, el cual en su forma ms general, acepta cualquier tipo
de modelos, funciones o restricciones. Las principales razones por las cuales el
control por retroalimentacin lineal es una de las metodologas 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 dinmica relativamente simple
hasta otros ms complejos incluyendo sistemas con grandes retardos, permite
tratar con facilidad el caso multivariable, es una metodologa completamente
abierta basada en algunos principios bsicos que permite futuras extensiones, no
es una metodologa 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 sintonizacin es relativamente fcil, resulta
conceptualmente simple la extensin al tratamiento de restricciones, que pueden
ser incluidas durante el proceso de diseo.

1.4. Objetivos
A continuacin se encuentran los objetivos generales y especficos de la presente
investigacin.
21
1.4.1. Objetivo General
Disear e Implementar en simulacin un Controlador por Retroalimentacin Lineal
tipo Computer Torque Control (CTC) sobre el modelo Cinemtico y Dinmico de
un Manipulador Industrial tipo SCARA de cuatro grados de libertad.

1.4.2. Objetivos Especficos
Para poder alcanzar el objetivo general descrito es necesario apuntar hacia los
siguientes objetivos especficos:

Obtener el Modelo Cinemtico Inverso y Directo del Manipulador SCARA
que involucre el estudio y la descripcin analtica de la geometra y
propiedades de movimiento en el tiempo del manipulador.

Desarrollar el Modelo Dinmico Inverso y Directo del Manipulador SCARA
bajo la formulacin Newton-Euler.

Implementar en simulacin una estrategia de control clsica 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 anlisis comparativo de los resultados obtenidos por el
Controlador CTC para la dos Trayectorias simuladas.

1.5. Metodologa de la investigacin
La metodologa que se implemento implement en esta investigacin, se orienta a
producir y comprobar nuevos conocimientos que forman parte del modelamiento y
22
control de procesos dinmicos no lineales, a partir del estudio en particular de un
manipulador SCARA.

A continuacin se muestran una serie de pasos que comienzan con la recoleccin
de informacin, posteriormente el estudio y anlisis de dicha informacin que
permita generar un marco conceptual idneo, luego pruebas y simulaciones,
seguido de la realizacin de experiencias en simulacin del modelo del
manipulador y por ltimo la obtencin de resultados que den la oportunidad de
evaluar el rendimiento del controlador bajo estudio.

.Ahora se detallaran los principales mdulos que componen la metodologa bajo
la cual se realiza el presente trabajo.

Documentacin Bibliografa
La documentacin bibliogrfica comienza con la recopilacin de informacin,
donde se realiza una bsqueda cuidadosa en fuentes electrnicas, bibliotecas
especializadas, bases de datos, banco de patentes, investigacin de la
informacin pertinente a los ltimos avances de la temtica en anlisis en
universidades pblicas y privadas, adems de entidades especializadas y
publicaciones de especialistas, posteriormente se realiza la recopilacin de
informacin a travs de los recursos y medios disponibles en la Universidad del
Norte referente a las investigaciones y trabajos que se han hecho en esta lnea de
investigacin.

Obtencin del modelo cinemtico del robot SCARA
Aqu se implemento la metodologa de Denavit y Hartenberg [6], mtodo
sistemtico para descubrir y representar la geometra espacial de los elementos
de una cadena cinemtica, para solucionar los dos problemas de la cinemtica de
manipuladores, cinemtica directa e inversa respectivamente.

23
Obtencin del modelo dinmico del robot SCARA
Primera etapa: Para la obtencin del modelo dinmico, inicialmente se define
claramente la necesidad del modelo, propsitos, tipos de anlisis a ejecutar y
condiciones, adems de los parmetros y medidas apropiadas del desempeo del
modelo, en general definir los objetivos de la modelacin.
Segunda etapa: se comprender el fenmeno de acuerdo con las variables
cinemticas y cinticas que intervienen en el proceso, que permitan determinar las
leyes o principios fsicos que aplican, consideraciones o hiptesis necesarias y por
ultimo establecer las inter-relaciones entre las variables y los diversos fenmenos
asociados.
Tercera etapa: es la formulacin o sntesis del modelo, donde una vez
comprendido el fenmeno, se plantean las ecuaciones de cada una de las leyes
fsicas que gobiernan el problema; que por lo general son ecuaciones
diferenciales, cuya integracin o solucin de las mismas constituyen el modelo.
Cuarta etapa: una vez sea obtenido el modelo se realizara su verificacin inicial,
donde se har una estimacin inicial de su validez o se conseguirn indicios de
que es aceptable el modelo confrontndolo ante soluciones ya obtenidas en
condiciones ms simplificadas, adems de confrontar dicho modelo contra uno
obtenido bajo una formulacin Lagrange-Euler.
Quinta etapa: revalidacin e inferencia, donde se obtendrn resultados simulados
que conduzcan a la consecucin de argumentos definitivos que demuestren la
validez del modelo o los ajustes necesarios.
1

Simulacin del modelo dinmico del robot SCARA
El modelo dinmico del manipulador, es simulado mediante el estudio y anlisis de
las variables parciales que lo componen y sus interrelaciones, en

1
Notas de clase de la Asignatura Diseo Mecnico. Ph.D Heriberto Maury. Universidad del Norte.
Barranquilla Colombia.

24
Matlab/Simulink. Dicha simulacin permite reproducir su comportamiento global,
y visualizar el impacto de distintas respuestas sobre las variables de inters.

Implementacin del Control Cinemtico 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 operacin, trayectoria y clase de trayectoria a seguir por el efector del
manipulador), obedeciendo al conjunto de restricciones fsicas de los
accionamientos, adems de los criterios de calidad como son la suavidad y
precisin.

Implementacin del Control Dinmico del manipulador
Aqu se garantizo que las trayectorias en el espacio articular seguidas por el
robot sean lo ms parecido a las planteadas por el control cinemtico. Esto se
consigui con la ayuda del modelo dinmico del robot y la aplicacin de toda la
teora del control dinmico por realimentacin lineal.

Anlisis de los resultados
Una vez obtenido todos los resultados, se evalu el desempeo del controlador
CTC en este tipo de procesos no lineales, adems 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.

Elaboracin del documento final
Por ltimo, se redacto redact un documento y un artculo, donde se encuentra
detallado todas las partes de la investigacin, con sus respectivos anlisis,
25
conclusiones y recomendaciones a tener en cuenta para las investigaciones
futuras en modelacin y control predictivo de manipuladores.
1.6. Contribuciones
A continuacin se muestra las cuatro contribuciones del presente trabajo:

Algoritmo computacional que determina la cinemtica directa e inversa de
cualquier manipulador industrial hasta cinco grados de libertad, a partir de
los parmetros Denavit-Hartenberg para el clculo de las matrices de
transformacin homognea.
Algoritmo que genera en forma simblica el modelo dinmico inverso de
manipuladores industriales hasta cinco grados de libertad. El algoritmo es
una aplicaron del mtodo recursivo de Newton-Euler para el calculo de las
ecuaciones de un robot.
Modelo Dinmico Inverso del manipulador SCARA ER-14, considerando
que la ubicacin del centro de masa del eslabn i referenciadas al sistema
i
S , pueden tener componente a lo largo de de los marcos de referencia
DenavitHartenberg.
Tabla comparativa del desempeo de error de seguimiento del controlador
CTC para dos trayectorias tpicas en procesos de manufactura, a partir de
la cuantificacin de la integral absoluta del error articular y cartesiano.

1.7. Organizacin del Documento
El informe final de esta investigacin tiene la siguiente estructura.

En su primer captulo se hace una evaluacin del estado del arte acerca del
modelado cinemtico y dinmico de manipuladores, adems de la implementacin
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 caractersticas de estos sistemas a la hora de modelarlos, como son las
26
ubicaciones de los centros de masa, se asume una distribucin uniforme de la
masa de los eslabones, etc. Adems se explica detalladamente el alcance,
objetivos y metodologa de la investigacin. Como resultado de este captulo se
ratifica la hiptesis de este trabajo: no se encuentran suficiente informacin
respecto al desempeo de controladores predictivos basados en modelos sobre la
base de la modelacin dinmica, del manipulador tipo SCARA.

El segundo captulo est dedicado al estudio de la cinemtica del manipulador.
En l se analiza la variacin de las posiciones articulares y posiciones cartesiana
en el tiempo para las dos trayectorias a estudiar en esta investigacin.

El tercer captulo est dedicado al estudio de la dinmica del manipulador bajo el
enfoque Newton-Euler. En l se analiza la variacin de la dinmica del modelo con
respecto a la posicin de los eslabones de cadena cinemtica.
A la sntesis de la estrategia de control propuesta para el modelo dinmico es
dedicado el cuarto captulo. En este se muestran confrontados los rendimientos
del controlador ante dos trayectorias, una de seguimiento y otra de cambio de
direccin sbito.

Finalmente se exponen las conclusiones y recomendaciones a las que se arrib
tras el desarrollo de este trabajo, as como la bibliografa referenciada en el
documento.
27
CAPTULO II
2. MMODELADO CINEMTICO DEL ROBOT INDUSTRIALODELADO
CINEMTICO DEL ROBOT INDUSTRIAL

2.1 Introduccin.
La cinemtica es el estudio del movimiento con respecto a un sistema de
referencia sin tener en cuenta las fuerzas que lo causan [9].

La cinemtica de manipuladores involucra el estudio y la descripcin analtica de
la geometra y propiedades de movimiento basado en el tiempo, en particular
responde al interrogante de como los eslabones se mueven en funcin del tiempo
respecto a otros; adems, de las relaciones entre la posicin y la orientacin del
elemento final del robot manipulador con los valores que toman sus coordenadas
articulares [9].

La implementacin de nuevos algoritmos de control para robot manipuladores
industriales de n grados de libertad requiere necesariamente del conocimiento
previo de su modelo dinmico. De igual forma, es necesario un algoritmo de
generacin de trayectoria que de la misma manera se articula al conocimiento del
modelo cinemtico del robot, por ende dicho modelo cinemtico es indispensable
en el desarrollo de cualquier sistema de control para robot manipuladores
industriales.

Con formato: Derecha, Sangra:
Izquierda: 0 cm, Primera lnea: 0 cm
Con formato: Ttulo 1, Centrado,
Punto de tabulacin: 0 cm, Lista con
tabulaciones
28
2.2 Descripcin del Robot.
El presente capitulo se enfoca en el estudio e implementacin de la cinemtica 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 translacin. Su peso es de 30 kg, su alcance mnimo es
de 230 mm y mximo 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 mxima 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 transmisin armnica, para el control de
cada eslabn, estos tienen instalados en sus ejes encoders pticos incrementales
con ndice de pulso de 2048 pulsos por revolucin; cada motor se controla con un
PWM H-mando de motores, 33 KHz, 7 A, 40-50 V con realimentacin.

Figura 1. Manipulador SCARA

29
2.3 Modelado de la Cinemtica de Robots

La cinemtica es la relacin que hay entre las variables articulares y las
coordenadas cartesianas correspondientes a la posicin y orientacin del
elemento final o Terminal del robot (ET).

Bsicamente hay dos casos para estudiar en la cinemtica de un robot
manipulador, el primero de ellos se conoce como el problema cinemtico directo,
que fundamentalmente nos ayuda a encontrar la posicin en el espacio cartesiano
y la orientacin de un robot, tomando como referencia las variables articulares,
que son los ngulos de orientacin de cada eslabn con respecto a un eje en
particular, o bien, con respecto a un eslabn anterior. El segundo es el problema
cinemtico inverso que resuelve la configuracin que debe adoptar el robot para
una posicin y orientacin conocida del extremo.

La cinemtica directa, definida como la funcin
n
q q q f x ,..., ,
2 1
, permite conocer
la posicin y orientacin del ET del robot, expresada en coordenadas cartesianas,
vector x , dados los valores de las coordenadas articulares, vector
n
q q q q ,..., ,
2 1
. Existen varios mtodos para obtener una expresin analtica de
la cinemtica directa, pero el ms empleado en Robtica es el que propusieron
Denavit y Hartenberg (D-H) en [5]. En esta metodologa se obtiene la
caracterizacin de cada GDL mediante los parmetros del eslabn y la articulacin
asociada (el ngulo y la distancia), como los cuatro parmetros D-H. Con cada
cuarteta de parmetros D-H se obtiene la matriz de paso generalizado
i
A que
permite obtener la representacin del marco de referencia
i
R expresado en el
marco de referencia
1 i
R . Finalmente, multiplicando las n matrices
i
A se obtiene
una matriz de transformacin homognea
n
i
T que representa el marco de
referencia
n
R expresado en el marco de referencia fijo
0
R .

30
El algoritmo de D-H comienza identificando los n ejes articulares
i
q , n ... 2, 1, i ,
numerando los eslabones del robot del 1 al n, de modo que el i-simo eslabn
tiene en sus extremos a los ejes articulares
i
q y
1 i
q . El segundo paso consiste en
caracterizar a cada eslabn por su longitud, definida como la longitud de la normal
comn a los ejes articulares pasando por sus extremos y designada como
i
a . 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 eslabn debe quedar en la
interseccin de la normal comn
i
a con el eje articular
1 i
q ; el eje
i
x es la
prolongacin de la normal comn
i
a , mientras que su eje
i
z se alinea con el eje
articular
1 i
q . En el caso de que los ejes articulares sucesivos sean paralelos, la
ubicacin de las normales comunes no es nica, por tal motivo, las
i
a se ubicarn
de tal modo que se anulan uno o ms parmetros
i
d , definidos en el sexto paso
de este algoritmo. En la Figura 2, se muestra el diagrama de dos articulaciones
consecutivas en una cadena cinemtica con el fin de observar grficamente el
significado de cada uno de los parmetros que conforman la cuarteta de cada eje.

i
x
i
z
1 i
q
i
q
i
1 i
q
1 i
x
1 i
z
i
q
i
x
i
1 i
a
i
a
i
d

Figura 2 Diagrama de Denavit-Hartemberg para la obtencin de la cinemtica

31

Es en el cuarto paso donde se asigna el marco de referencia al n-simo eslabn,
adems del marco de referencia fijo asociado al eslabn cero. El origen del marco
del eslabn cero se ubica en la interseccin del eje articular
1
q con la superficie de
la mesa que soporta al robot, mientras que el origen del marco del ltimo eslabn
se pone en el punto del ET del robot que es de inters posicionar y orientar.
Generalmente, ese punto se encuentra en el punto medio de la pinza del
manipulador, definiendo el eje
n
z paralelo a la direccin de la pinza, mientras que
el eje
n
x se extiende del extremo izquierdo al derecho de dicha pinza. Ahora ya se
puede definir la distancia
n
d entre los dos ltimos eslabones como la distancia
entre los orgenes de los marcos de referencia
1 n
R y
n
R , medida a lo largo del eje
n
z .

Se complementa la caracterizacin del i-simo eslabn en el quinto paso
definiendo
i
, como el ngulo que forman los ejes pasando por sus extremos,
i
q
y
1 i
q o bien
1 i
z y
i
z , medido alrededor del eje
i
x . Aqu se puede apreciar que el
eje
i
x coincide con el producto cruz
i i
z x z
1
. En el sexto paso se caracteriza cada
articulacin por el ngulo
i
entre los eslabones que conecta y la distancia
i
d
entre dichos eslabones. El ngulo
i
se mide alrededor del eje
1 i
z entre las dos
normales comunes que posee,
1 i
a y
i
a o bien entre los ejes
1 i
x y
i
x , mientras
que la distancia
i
d se mide a lo largo del eje
1 i
z entre las intersecciones con las
dos normales comunes que posee:
1 i
a y
i
a . En el caso de la primera articulacin
la distancia
1
d se mide entre el origen del marco de referencia cero y la
interseccin entre
1
z y
1
a . Resumiendo, los parmetros de Denavit-Hartenberg
son aquellos que caracterizan cinemticamente al eslabn (2) y a la articulacin
(2), de modo que habr cuatro parmetros D-H por cada GDL del robot, definidos
como:
32

i
a Longitud de la normal comn a los ejes
1 i
z y
i
z , es decir a los ejes
articulares
i
q y
1 i
q .
i
ngulo entre los ejes
1 i
z y
i
z , medido en un plano normal al eje
i
x .
i
ngulo entre los ejes
1 i
x y
i
x medido en un plano normal al eje
1 i
z .
i
d Distancia medida a lo largo del eje
i
z entre las intersecciones con sus
normales comunes.

En Lla matriz homognea de paso generalizado
i
A , es necesariose requiriere
aplicar las transformaciones homogneas bsicas necesarias para relacionar los
referenciales
1 i
R y
i
R . Dichas transformaciones elementales son: una rotacin de
i
grados alrededor del eje
1 i
z ; una translacin de d
i
centmetros a largo de ese
mismo eje, otra translacin de
i
a centmetros a lo largo del eje
1 i
x , ahora
relacionados con el eje
i
x y, finalmente una rotacin de
i
grados alrededor del
eje
i
x . De modo que la matriz de paso generalizado se estima como:

1 0 0 0
0
1 0 0 0
0 0
0 0
0 0 0 1
1 0 0 0
0 1 0 0
0 0 1 0
0 0 1
1 0 0 0
1 0 0
0 0 1 0
0 0 0 1
1 0 0 0
0 1 0 0
0 0
0 0
0 0 0 0
1
1
1
i i i
i i i i i i i
i i i i i i i
i
i
i i
i i
i
i
i i
i i
i
i
i i i i
i
i
d C S
S a C S C C S
C a S S S C C
A
C S
S C
a
d
C S
S C
A
x T a T d T z T A , , , , , ,
(Ec 1)

Con formato: Fuente: Sin Negrita
33
Se utilizo en la anterior matriz la nomenclatura clsica en Robtica Industrial, s
para designar al seno, c para el coseno [9]. Cabe mencionar aqu que, cuando la i-
sima articulacin es de rotacin,
i
es variable y
i
d es constante; mientras que
en el caso de articulaciones de translacin es al contrario. Con las n matrices de
paso generalizado se calcula la matriz de transformacin homognea que define
la cinemtica directa del robot de la siguiente manera:

1 0 1 0 0 0
1
0
1
0
1
1 0
p R p z y x
q A q T
i i i i
i
n
i
i
i
n
; para i=1, 2 n (Ec 2)

La cinemtica inversa de un robot, expresada por la ecuacin x f q
1
, nos
permite calcular el valor de las coordenadas articulares del robot,
n
T
q q q q ,..., ,
2 1
, que permiten llevar al ET del robot al punto deseado, expresado
por sus seis coordenadas cartesianas, tres para representar la posicin
z y x t
T
, , , y tres ms para representar la orientacin. Existen muchos mtodos
para calcular las coordenadas articulares del robot a partir de la expresin de la
cinemtica directa dada por la (Ec 2) [9]. Cuando se presente detalladamente la
cinemtica inversa en este capitulo, se mostrar el respectivo mtodo
implementado al robot SCARA.

2.4 Cinemtica Directa del Robot SCARA

La Figura 3 muestra el diagrama cinemtico del robot SCARA, donde se muestra
detalladamente la determinacin 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 numricos de dichos parmetros a fin de calcular el modelo
cinemtico directo de este robot. Es importante aclarar que dichos valores
numricos se obtuvieron midiendo
34

ESHED ROBOTEC
2
z
2
x
1
x
3
z
3
x
1
a
2
a
1
d
2
d
3
d
4
d
1
z
ESHED ROBOTEC
2
z
2
x
1
x
3
z
3
x
1
a
2
a
1
d
2
d
3
d
4
d
1
z

Figura 3 Sistema de coordenadas segn la convencin D-H

i
i
() d
i
(mm) a
i
(mm)
i

1 q
1
765 270 0
2 q
2
-315 230 0
3 0 -d
3
-40 0 0
4 q
4
-187.95 0 0

Tabla 1 Parmetros D-H del robot SCARA

Sustituyendo en la (Ec 1) los valores de los parmetros D-H del robot SCARA que
se muestran en la Tabla 1, y realizando todas las multiplicaciones matriciales
ESHED ROBOTEC
2
z
2
x
1
x
3
z
3
x
1
a
2
a
1
d
2
d
3
d
4
d
1
z
ESHED ROBOTEC
2
z
2
x
1
x
3
z
3
x
1
a
2
a
1
d
2
d
3
d
4
d
1
z
4
d +

Eslabn 2
Eslabn 3
Eslabn 1
35
indicadas por la (Ec 2), para i=4, se obtiene la matriz T,
4
0
T T , la cual especifica
la posicin y orientacin del ET del manipulador con respecto al sistema de
coordenadas de la base. Esta matriz T es usada frecuentemente en la cinemtica
de robot manipuladores, y es llamada usualmente como matriz del manipulador.
Considere la matriz de transformacin homognea T como:

1 0 0 0
1 0 0 0 1 0 0 0
4 4 4 4
z z z z
y y y y
x x x x
p a o n
p a o n
p a o n
p a o n
T
p z y x
T (Ec 3)
donde:
n Vector normal a la mano. Asumiendo un Yaw o guiada paralelo de la
mano, es ortogonal a los dedos de la mano del robot.
O Vector deslizante de la mano. Este apunta en la direccin de movimiento
de los dedos, como el gripper abre o cierra.
A Principal vector de la mano. Este apunta en una direccin normal a la
palma de la mano (normal a la herramienta montada como elemento
Terminal del robot).
P Vector posicin 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 estn
totalmente cerrados.

Entonces la solucin al problema cinemtico directo para el robot SCAORA -ER 14
es:
1 0 0 0
4
3
3
2
2
1
1
0
z z z z
y y y y
x x x x
p a o n
p a o n
p a o n
A A A A q T (Ec 4)
36
Donde los elementos de la matriz de rotacin y del vector de translacin se
definen teniendo en cuenta que C cos, S sen y son:

1 0 0 0
1 0 0
0
0
1
1 1 1 1
1 1 1 1
1
0
d
q S a q C q S
q C a q S q C
A
) ( ) ( ) (
) ( ) ( ) (

1 0 0 0
1 0 0
0
0
2
2 2 2 2
2 2 2 2
2
1
d
q S a q C q S
q C a q S q C
A
) ( ) ( ) (
) ( ) ( ) (

1 0 0 0
1 0 0
0 0 1 0
0 0 0 1
3
3
2
d
A
1 0 0 0
1 0 0
0 0
0 0
4
4 4
4 4
4
3
d
q C q S
q S q C
A
) ( ) (
) ( ) (

0
z
y
x
n
n
n
) S(q ) S(q ) S(q - ) C(q ) C(q + ) C(q ) S(q ) C(q + ) C(q q S
) S(q ) C(q ) S(q - ) S(q ) C(q - + ) C(q ) S(q ) S(q - ) C(q ) C(q
4 2 1 2 1 4 2 1 2 1
4 2 1 2 1 4 2 1 2 1

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
4 2 1 2 1 4 2 1 2 1
4 2 1 2 1 4 2 1 2 1
z
y
x
o
o
o
(Ec 5)
1
0
0
z
y
x
a
a
a

1 2 3 4
1 1 2 2 1 2 2 1
1 1 2 2 1 2 2 1
d + d + d + d
) S(q a + ) S(q a ) C(q + ) C(q a ) S(q
) C(q a + ) S(q a ) S(q - ) C(q a ) C(q
z
y
x
p
p
p

Finalmente, la orientacin de la pinza con respecto al marco de referencia fijo R
0

queda como se muestra en la (Ec 6):

4 2 1
q q q roll (Ec 6)

37
Debido a que las tres rotaciones se efectan alrededor de ejes paralelos en un
plano vertical al xi-yi de cada uno de los eslabones. Adems, esta orientacin
puede calcularse a partir la informacin contenida en la submatriz de rotacin
incluida en la matriz solucin de la cinemtica del robot SCARA dada por la (Ec 4),
dicha matriz equivale a una rotacin elemental de grados alrededor del eje z, la
cual se expresa como en (Ec 7):

1 0 0
0
0
) ( ) (
) ( ) (
, C S
S C
z R (Ec 7)

Igualando la matriz de rotacin dada por la (Ec 7) con la cinemtica directa del
robot dada por la (Ec 4), se puede despejar el valor de la orientacin del ET del
robot, utilizando la expresin de la funcin arco tangente de dos argumentos,
descrita por la (Ec 8):

y
x
n
n
S
C
roll
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 posicin y la orientacin del ET del robot como una funcin de
sus variables articulares.

2.5 Cinemtica Inversa del Robot SCARA
A continuacin, se determinaran las ecuaciones que permiten calcular los valores
que deben tener las coordenadas articulares del robot
T
n
q q q q ,..., ,
2 1
, para que
el ET se localice y oriente en el espacio de la tarea.
A partir del modelo cinemtico directo es posible conocer el modelo cinemtico
inverso, mediante el procedimiento que se muestra en la (Ec 9):
38
4
3
3
2
2
1
4
0
1
1
0
A A A q T A (Ec 9)
Debido a que
1 0 0 0
p a o n
T se calcul anteriormente, todo los miembros de la
izquierda en la (Ec 9), son funcin de las variables articulares
k i
q q ,..., mientras
que la derecha es funcin de
n k
q q ,...,
1
, por ende tenemos que (ver Ec 10).
1 0 0 0
d + d + d 1 0 0
) S(q a 0 ) q + C(q - ) q + S(q
) C(q a 0 ) q + S(q ) q + C(q
1 0 0 0
p ) C(q p ) S(q - a ) C(q a ) S(q - o ) C(q o ) S(q - n ) C(q n ) S(q -
p ) S(q p ) C(q a ) S(q a ) C(q o ) S(q o ) C(q n ) S(q n ) C(q
2 3 4
2 2 4 2 4 2
2 2 4 2 4 2
1
y 1 x 1 y 1 x 1 y 1 x 1 y 1 x 1
1 y 1 x 1 y 1 x 1 y 1 x 1 y 1 x 1
d p a o n
a
z z z z
(Ec 10)

De las 12 opciones posibles, se escoge la ecuacin donde
1
q solo dependa de
constantes como se describe en la (Ec 11).
x
y
a
a
q
1
1
tan (Ec 11)
144 144
1
q

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
x q
x q
x q
x q
x
y
q (Ec 12)

Ahora para determinar la expresin del modelo cinemtico inverso para la
segunda articulacin del manipulador se tiene que:
39

4
3
3
2
4
0
1
1
0
1
2
1
A A q T A A (Ec 13)

De manera similar tomando el elemento 2,4 se obtiene que:

0 a ) S(q p ) q C(q p ) q S(q -
1 2 y 2 1 x 2 1
(Ec 14)

Despejando la variable de inters, se tiene finalmente:

1 1 1
1 1 1
2
tan
a q C p q S p
q C p q S p
q
x y
y x
(Ec 15)
109 109
2
q

Para la cinemtica inversa de la articulacin prismtica se despeja de la (Ec 5):

1 2 4 3
d d d d
z
p (Ec 16)

Por ultimo el valor de
4
q se puede obtener despejando de la (Ec 6).

2 1 4
q q q roll (Ec 17)
527 q 527
4

Las ecuaciones (Ec 11), (Ec 15), (Ec 16) y (Ec 17) permiten determinar los valores
en el espacio articular ante una posicin y orientacin del ET del robot, con
respecto a un sistema de coordenadas que se toma como referencia, conocidos
los parmetros geomtricos de los elementos del robot, es decir, estas ecuaciones
son la solucin al problema cinemtico inverso del robot SCARA.

40
La presente investigacin se prueba en dos trayectorias tpicas. A continuacin se
muestran las grficas del comportamiento cinemtico deseado por el robot en el
espacio articular y cartesiano. Es importante aclarar que estos clculos estn
mediados por el modelo cinemtico directo e inverso. A continuacin se muestra la
grafica en 3D de la primera trayectoria a simular (ver Figura 4). Bsicamente el
objetivo de esta trayectoria es excitar al modelo con una fuerte dinmica en el eje
z, por esto la alta frecuencia pero con una amplitud variable. Para los ejes x-y esta
trayectoria presenta diferentes dimetros, por lo que la respuesta se espera que
sea diferente, mientras que para la trayectoria 2 hay un cambio de direccin sbito
lo cual evaluar el rendimiento del controlador ante un cambio de sentido
repentino.

0
0.1
0.2
0.3
0.4
0.5
0.2
0.25
0.3
0.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
P
o
s

-

d
e
s
e
a
d
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)
P
o
s
i
c
i
o
n
e
s

d
e
s
e
a
d
a
s

-

e
s
p
a
c
i
o

c
a
r
t
e
s
i
a
n
o

(
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 dimetro mayor y menor de 0.4 m y 0.2m respectivamente, en el
eje z se observa una oscilacin sinusoidal con amplitud mxima de 0.2 m al cabo
de 1.5 seg, la mitad del tiempo total de ejecucin de la trayectoria.

En la Figura 6 se muestra el comportamiento en el tiempo deseado para la
trayectoria 1, en el espacio articular.

internacional)
42
0 0.5 1 1.5 2 2.5 3
-3
-2
-1
0
1
2
3
4
5
Tiempo (seg)
P
o
s
i
c
i
o
n
e
s

d
e
s
e
a
d
a
s

-

e
s
p
a
c
i
o

a
r
t
i
c
u
l
a
r

(
r
a
d
)
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 continuacin se muestran las respectivas grficas 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 direccin, 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.3525
0.35
0.355
0.36
0.365
0.37
0.375
Pos - Deseada x (m)
P
o
s

-

D
e
s
e
a
d
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 posicin para la trayectoria 2,
tanto en el espacio articular y cartesiano. Cabe resaltar que la relacin entre
coordenadas articulares y cartesiana esta mediada por el modelo cinemtico antes
calculado y presentado.
internacional)
44
0 0.5 1 1.5 2 2.5 3
-3
-2
-1
0
1
2
3
4
5
Tiempo (seg)
P
o
s
i
c
i
o
n

d
e
s
e
a
d
a

-

E
s
p
a
c
i
o

a
r
t
i
c
u
l
a
r

(
r
a
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 segn la convencin D-H

Ahora se muestra detalladamente los perfiles de posicin para la trayectoria 2,
tanto en el espacio articular y cartesiano. Cabe resaltar que la relacin entre
coordenadas articulares y cartesiana esta mediada por el modelo cinemtico antes
calculado y presentado (ver seccin 2.4).

En la Figura 9, se observa que la posicin 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 3
0.34
0.35
0.36
0.37
0.38
0.39
0.4
0.41
Tiempo (seg)
P
o
s
i
c
i
o
n

d
e
s
e
a
d
a

-

E
s
p
a
c
i
o

c
a
r
t
e
s
i
a
n
o

(
m
)
POSICIONES CARTESIANAS DESEADAS PARA LA TRAYECTORIA 2
Pos - deseada x
Pos - deseada y
Pos - deseada z

Figura 9. Sistema de coordenadas segn la convencin D-H

46

CAPTULO III

3. MODELADO DINMICO DEL ROBOT INDUSTRIALODELADO
DINMICO DEL ROBOT INDUSTRIAL

3.1 Dinmica del Robot manipulador.
La dinmica del manipulador establece la relacin entre las fuerzas y pares
aplicadas en los actuadores y el movimiento del manipulador. Estas relaciones
pueden ser expresadas matemticamente por un conjunto de ecuaciones
diferenciales, comnmente llamadas ecuaciones de movimiento (E.O.M). Para
obtener el modelo dinmico 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 formulacin del principio generalizado
d'Alembert [5]. En el presente capitulo se desarrolla el modelo dinmico de un
manipulador SCARA solo bajo la formulacin Newton-Euler, adems el modelo
dinmico de los actuadores es presentado, permitiendo simular el movimiento del
robot, evaluar la estructura mecnica del robot, redimensionar los actuadores si se
requiere para alguna aplicacin especifica, as como disear y evaluar nuevos
controladores dinmicos del robot.

Es importante tener bien claro que en la dinmica de manipuladores existen dos
problemas bsicamente, modelo dinmico directo, el cual determina el
comportamiento en el tiempo de las coordenadas articulares del robot en funcin
Con formato: Justificado, Sangra:
Izquierda: 0,21 cm, Sangra francesa:
0,63 cm
internacional)
internacional)
47
de las fuerzas y torques aplicadas, como se muestra en la Figura 10, mientras que
el modelo dinmico inverso, determina explcitamente los torques y fuerzas en
funcin de las coordenadas articulares y sus derivadas, que el problema que se
resolver a continuacin..
Dinmica Directa
Dinmica Inversa
i
t
Coordenadas
articulares
i
t
Torques/Fuerzas
t t
n
. . . ,
1
t t
n
. . . ,
1
t t
n
. . . ,
1
t t
n
. . . ,
1
Dinmica Directa
Dinmica Inversa
i
t
Coordenadas
articulares
i
t
Torques/Fuerzas
t t
n
. . . ,
1
t t
n
. . . ,
1
t t
n
. . . ,
1
t t
n
. . . ,
1

Figura 10 Diagrama de relacin entre Dinmica Directa e Inversa

3.2 Formulacin Newton-Euler.
Una directa interpretacin de la segunda ley de Newton, conlleva al desarrollo de
las ecuaciones que gobiernan el comportamiento dinmico de cualquier
manipulador bajo la formulacin 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 relacin entre el movimiento
del robot y las fuerzas implicadas en el mismo, relacin matemtica entre la
localizacin del robot definida por sus variables articulares o por las coordenadas
de localizacin de su extremo, y sus derivadas: velocidad y aceleracin.

Para derivar las ecuaciones de movimiento bajo el enfoque de Newton Euler,
para su posterior programacin, hay que plantear claramente la segunda ley de
Newton de movimiento y la ley de Euler para el eslabn i.

Consideremos el diagrama de cuerpo libre (DCL) que muestra la Figura 11, donde
i
f es la fuerza ejercida por el eslabn (i-1) sobre el eslabn (i) mientras que
i
n es
el momento o torque ejercido sobre el eslabn (i) por el eslabn (i-1), y
48
despreciando los efectos de amortiguamiento viscoso, debido a que este efecto se
tendr en cuenta en la seccin 3.4, las ecuaciones de movimiento aplicando
Segunda Ley de Newton queda como se observa en la (Ec 18).

zi
Fi
ni
xi
xi+1
zi+1
Fi+1
ni+1
ipi+1
mi*g
ipci

Figura 11 Diagrama de cuerpo libre del eslabn i del robot

ci i ci i i i i
v m v m
dt
d
g m f f

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

i i i i i i i i ci
i
i
i
i ci
i
i i
w I w w I w I
dt
d
f p p f p n n

1 1 1

para i=1, 2 n (Ec 19)

donde
i
I es el tensor de inercia centroidal del eslabn i, el cual vara a medida
que cambia la orientacin y posicin del eslabn (i). La razn de cambio del
49
momento angular incluye ambos efectos en un trmino de aceleracin angular
i i
w I

y un trmino conocido como torque giroscpico
i i i
w I w .
x2
x1
x3
r(op)
o
p
m(p)
n

Figura 12 Referencia de la masa de una partcula p

El tensor de inercia
i
I es una matriz simtrica (3x3), que describe las propiedades
de masa de un eslabn (i) con respecto a su centro de masa, a continuacin se
muestra un desarrollo matemtico 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).

2
N
1
p , n * m n , o I (Ec 20)

donde m es la masa de una partcula p del sistema y p , n es la distancia de la
partcula p al eje n . Por definicin se tiene que: ) op ( r n ) op ( r n p , n
2

, de
igual forma n y ) op ( r
se puede expresar en trminos de sus componentes como

en la (Ec 21).

50
3
3
2
2
1
1
e n e n e n n

,
3
3
2
2
1
1
e ) op ( x e ) op ( x e ) op ( x ) op ( r

(Ec 21)

Realizando la operacin expandida se obtiene la (Ec 22).

3 2 3 2 3 1 3 1 2 1 2 1
2
1
2
2
2
3
2
1
2
3
2
2
2
3
2
2
2
1
2
x x n n 2 x x n n 2 x x n n 2 x x n x x n x x n p , n

(Ec 22)

obteniendo la expresin del tensor de inercia respecto al punto o, en notacin
matricial.

op x op x op r * m n , o I
k i i k
2
N
1
donde 3. 2, 1, k , i (Ec 23)

3.3 Algoritmo computacional para determinar el modelo dinmico por
Newton-Euler.
Las (Ec 18) y (Ec 19) gobiernan el comportamiento dinmico de un slo eslabn
del robot. El conjunto completo de ecuaciones para el robot manipulador es
obtenido a partir de una evaluacin de ambas ecuaciones a cada uno de los
eslabones que conforman al manipulador, que para el caso en estudio, son cuatro
eslabones.

La formulacin de Newton-Euler a partir del computo de los pasos 1-9 determinan
el modelo dinmico inverso fundamentado en la unin 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 continuacin, transformando las variables cinemticas
desde la base hasta el ET del robot, mientras que la iteracin hacia atrs 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 continuacin se muestra detalladamente cada unos de los pasos que se tuvieron
en cuenta para obtener el modelo dinmico inverso del SCARA.

Primero: Teniendo en cuenta la metodologa de DH, se asignan a cada uno de los
eslabones un sistema de referencia, estos marcos de referencia se mostraron en
el captulo II del presente documento.

Segundo: Se determinan las matrices de transformacin homognea, que
contienen a la submatriz de rotacin
i
1 i
R y sus inversas
T
i
1 i
1
i
1 i
1 i
i
R R R .

Tercero: Se establecen todas las condiciones iniciales al algoritmo, es decir, todas
las condiciones cinemticas del eslabn fijo o sistema de la base
o
S , como se
muestra a continuacin.

T
0
0
0 , 0 , 0 w velocidad angular de la base
T
w 0 , 0 , 0
0
0

aceleracin angular de la base
T
0
0
0 , 0 , 0 v velocidad lineal de la base
T
v g , g , g
z y x 0
0

aceleracin lineal de la base

0
0
w ,
0
0
w
y
0
0
v son variables generalmente nuloas excepto para el caso en que la
base del robot esta en movimiento. Adems debido a que el ET sostiene a una
carga, es posible decir que se tiene caracterizada
i n
i n
n y
i n
i n
f que son el
momento externo y la fuerza externa respectivamente. En los siguientes pasos
del presente algoritmo se utilizara la siguiente convencin:

T
0
1 , 0 , 0 z , es un vector unitario en la direccin del eje z del marco de
referencia ubicado en la base.
52
C d , S d , a p
i i i i i i
i
, representa las coordenadas del origen del sistema del
sistema
i
S respecto al sistema
1 i
S .
i
i
s , son las coordenadas del centro de masa del eslabn i referenciadas al sistema
i
S .
i
i
I , es la matriz de inercia del eslabn i respecto a su centro de masa en funcin de
i
S .

A continuacin se muestran los pasos del algoritmo que computan las
transformaciones de las variables cinemticas, desde la base del robot hasta el ET
del robot y deben ser calculados para i=1, 2n.

Cuarto: Se calcula la velocidad angular del sistema
i
S .

i 0 1 i
1 i
1 i
i
i
i
q z w R w

si el eslabn i es de rotacin
1 i
1 i
1 i
i
i
i
w R w si el eslabn i es de traslacin

Quinto: Calculo de la aceleracin angular del sistema
i
S .

i 0 1 i
1 i
i 0 1 i
1 i
1 i
i
i
i
q z w q z w R w

1 i
1 i
1 i
i
i
i
w R w

si el eslabn i es de traslacin
Sexto: Se calcula la aceleracin lineal del sistema
i
S .

1 i
1 i
1 i
i
i
i
i
i
i
i
i
i
i
i
i
i
v R p w w p w v

i
i
i
i
i
i
i 0 1 i
i
i
i
i
i
i
i
1 i
1 i
i 0 1 i
i
i
i
p w w q z R w 2
p w v q z R v

si el eslabn i es de traslacin

Sptimo: Calculo de la aceleracin lineal del centro de gravedad del eslabn i.
53

i
i
i
i
i
i
i
i
i
i
i
i
i
i
v s w w s w a

A continuacin 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 eslabn i.

i
i
i 1 i
1 i
1 i
i
i
i
a m f R f

Noveno: Calculo del par ejercido sobre el eslabn i.

i
i
i
i
i
i
i
i
i
i
i
i
i i
i
i
i
1 i
1 i
i
i
i
1 i
i
1 i
1 i
i
i
i
w I w w I a m s p f p R n R n

Dcimo: Calculo de la fuerza o par aplicado a la articulacin i.

0 1 i
i T
i
i
i
z R n si el eslabn i es de rotacin
0 1 i
i T
i
i
i
z R f si el eslabn i es de traslacin

Donde puede ser el par o la fuerza aplicada a la articulacin i y a su vez es par
que entrega el motor menos el par de la friccin.

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 recursin hacia atrs, que se computa en los pasos 8-
10. Las fuerzas/torques
i
en el paso 10 son lineales en el momento
i
i
n .

2. Para articulaciones rotacionales, el modelo Newton-Euler es no lineal con
respecto a los vectores de centro de masa
i
S . 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
f x S es no lineal (cuadrtico) en
i
S , por tal motivo el torque aplicado en
una junta rotacional es no lineal en
i
S .

3. El modelo Newton-Euler es no lineal con respecto al vector de parmetros
cinemticas
i
i
p . La fuerza aplicada a cada eslabn 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 dinmicas de los eslabones i+1 para el torque son
independiente de la masa del eslabn anterior y el clsico tensor de inercia
i
i
I , lo cual es consecuencia de la recursin.

3.4 Modelo dinmico del Robot SCARA.
El movimiento articular real de un robot es gobernado por su dinmica, teniendo
en cuenta la masa de cada eslabn y junta, la velocidad y aceleracin articular, y
las fuerzas/fuerzas aplicadas a cada junta. El modelo dinmico del robot SCARA
es un conjunto de ecuaciones diferenciales que estn dadas por la (Ec 24).

q g q q C q q q B q q A
2
(Ec 24)

55
donde:

q es el n-vector de coordenadas espaciales generalizado que describe la
posicin del manipulador.
q es el n-vector de velocidades angulares.
q es el n-vector de aceleraciones angulares.
q A es la matriz simtrica de inercia en el espacio articular de tamao n x n.
q B es la matriz de efectos de coriolis de tamao n x n(n-1)/2.
q C es la matriz de torques centrpetos de tamao n x n.
q g es el n-vector de torques gravitatorios.

Los smbolos q q y
2
q son las notaciones para el vector de producto de
velocidad y el vector de velocidades al cuadrado. q q y
2
q estn dados por:

T
n n n n n
, q q , q q ... , q q , q q , q q ... , q q , q q q q
2 2 4 2 3 2 1 3 1 2 1

T
n
q ... , q , q q
2 2
2
2
1
2

Se ejecuta el programa tal que se observa en el anexo ANEXO 5 y se obtiene las
siguientes ecuaciones:

56
2 1
2
2 2 2 2 2 4 4 2 3 3
3
2
2 2 2 2 2 2 2 2 2
2
2 2 2 4
2
2 2 2
2
2 2 4 4
2 3 3 2
2
2 2 2 4
2
2 2 2 3
2
2 2 2 2 2 2 2 2
2
2 2 2 2
2
1 2 3 3 2 4 4 2 2 2 2 2 1 4 2 2 1 3 3 2
4
2
2 2 2 1 4 4 2 3
2
2 2 2 2
2
2 2 2 1 2 4 4
1 2 3 3 1 2 2 2
2
2 2 2 2 1 4 2 2 1 2 2 2 2 2 1 2
1 4 2 2 4 4 2
2
2 2 2 2 2 2 2
2
2
2
2 3 2 2 2 3 2
2
2
2
2 2 2
2
2
2
2 2 2 4 4
2
2
2
2 4
2
2 2 2 2 2 3 3 4
1
2
2 2 2
2
2 4 1
2
2 2 3 2 1 2 3 2 1 2 2 2 1 2 4 4
1 2 4 4 1 2 3 3 1 2 2 2 1 2 4 2 1 4 2 2
2
2
2
2 4
2
2
2
2 2
2
2
2
2 3 1 1 1 1 2 4 2 2 2 2 2 2
2
2 2 2 2
2
1 1 2 2 2 2 3 3 1 2 2 2 2 4 4
2
1 1 1
2
1 1 2 3 3 1
] 4 2 2
4 4 4 4 [ ]
2 2 2 2
2 [ ] 2
2 2 2
2
[ ] 2 2 2
2 2 2 [
] 2
3 2
2 2 2 2 2
2 3 [
q q q C S m a a m S a m S
m a q C q S S m a q C q S m a q C q S m a q C q S q a m S
a m S m a q C q S m a q C q S m a q C q S S m a q C q S
q C S m a q a m S a m S S m a q C q S a m a q S a m S q S
m a q C q S a m S q S m a q C q S m a q C q S a q C m S
a q C m S a q C m S q C S m a S m a q C a m a q S S m a q S
S m a q S q I q S m S m a q C q S q C a m S m a I I
q C a m m S a m a m S q C a m q C S m a a m S I
q a m m S I I S m I I a q C m a a q S m S a q C m S
a q S m S a q S m S a q C S m a q C m a S m a q S q C a m
q C a m q C a m S a m S q C m a S m a q C q S q C S m a
a m S m a a m S a q C m a a m S S m m S a q C m S
y y y
x y
y x
y y y x x
x y
y y y y x
x zz x y x zz zz
y x x x zz
y zz zz x zz zz y x
y y x y
x x y x
x x x x y x

(Ec 25)

2
2 2 2 2 2 4 4
2 3 3 4 4 2 1 2 3 3 2 4 4 2 2 2
2
1 1 2 4 4 1 2 4 4
2 2 2 2 4 4 2 3 3 1 2 3 3 1 2 3 3 1 2 2 4 1 2 2 2
1 2 3 2 1 2 2 2 1 2 2 2 2 2
2
2
2
2 2
2
2 4 4 2 3 3 2 2 2
2 4 4 2
2
2 2
2
2 3 3 1 1 2 4 2 4 1 2 3 3 1 2 2 2
1 2 4 4 1 2 2 2 1 2 4 4 1 2 3 2
2
2 2 2 2 2 3
2
2 2
2
2 4 2 4 4
2
2 3 2 2 3 3 1 2 3 3 2
2
2 1 2 2 2 2
]
[ ] 2 2 2 [ ]
2
[ ] 2 3
[ ]
2 3
2 [
q S m a a m S
a m S q I q q a m S a m S S m a q a q C m S a q S m S
S m a a m S a m S a q S m S a q C m S a q S a m a q S m a
a q S m a a q S S m a q C m S q m S a m a m I a m S S m a
a m S I S m a m I q a q C m a I a q S m S a q S m S
a q C m S a q C m a a q S m S a q C m a S m S m a I
a m a m a m S a m I a m S a q C m S m S a q C S m
y y
y zz y y y y x
y y y x y
x y y zz x x
x zz x zz zz y y
x y x x zz
x zz x x y x

(Ec 26)

3 4 3 3
m m d g

(Ec 27)

4 4
2
2 2 4 4
2
1 2 4 4 1 2 4 4 1 2 4 4
2 1 2 4 4 2 2 4 4 4 1 1 2 4 4 4 2 4 4 1 2 4 4 4
] [ ] [
] 2 [ ] [ ] [
q I q a m S q a m S a q C m S a q S m S
q q a m S q a m S I q a q C m S I a m S a q S m S
zz y y y x
y x zz x zz x y

(Ec 28)

57
El modelo dinmico de este robot es fuertemente no lineal, multivariable, acoplado
y de parmetros 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 ubicacin del centro de masa del eslabn i referenciadas al sistema
i
S ,
solo tienen componente a lo largo de de los marcos de referencia Denavit
Hartenberg, se reagrupa cada uno de los trminos de los primeros momento de
inercia de cada uno de los eslabones. Adems, 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 expresin completa es mostrada en la (Ec 29).

( )
f v s
F q F sign q (Ec 29)

Las anteriores simplificaciones para el caso de esta investigacin 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 diseo del sistema de control
proporcionando unos resultados de seguimiento articular de trayectoria de 5E-5,
como se observa en la seccin 5. Ahora las ecuaciones finales que se
programaron son:

58

) q sign( FS q FV I q
w d ) q C )) q d q S Q w d ) q S ) q ( d ) (C(q Q ) w (w I
))) w (w d w d ) q C )) q d q S ) C(q ) w d ) q S )) q (d ) C(q
q q ( d ) C(q Q ) w (w d w d ) q C )) q d q S ) S(q
w d ) q S ) q d q C q q ( d ) C(q Q I ) w w (w
q S )) w w (w Q Q ) q q q ( ) q q ( d )) w )(d C(q
)) q d q S ) S(q w d ) q S ) q ( d ) (C(q ) ) q q (-( )(d (C(q m
)) w w (w Q Q q q q ))) w (w d )) w )(d (C(q )) q d
q S ) C(q )) w )(d S(q ) q d q C q q d ) S(q ( m
q C d q S q S )) w w (w Q Q q q q ))) w (w d
)) w )(d (C(q )) q d q S ) C(q )) w )(d S(q )) q ( )(d (C(q ) ) q q (-( (d
) (S(q m )) w w (w Q Q ) q q q ( ))) w (w d )) w )(d C(q
)) q d q S ) S(q w d ) q S )) q ( )(d (C(q q q d
) q C ( m ( ) (C(q w w Q Q q q q S ) q q w Q
Q q q q ))) w (w d )) w )(d C(q )) q d q S ) S(q
) q ( d ) (C(q ) ) q q (-( )(d (C(q ((m )) w w (w Q Q ) q q q (-(
))) w (w d )) w )(d C(q ))) q ( )(d (S(q (( ) C(q ) w d ) q S )) q (d ) C(q
) q q d ) q (S m )( (C(q ) w w ( (Q Q ) q q ( (( ) C(q d
ZZ
X Y ZZ
X
Y ZZ
Y X
X Y
X Y
Y X
Y X Y
X
X y
X Y
1 1 1 1 1
1 2 2
2
1 2 2 2 1 2 2
2
1 2 2 2 2 1 2
2 1 3 1 2 2
2
1 2 2 3 1 2 2
2
1 2 2
2
2 1 3 3 3 2 1 3 1 2 2
2
1 2 2 3
1 2 2
2
1 2 2
2
2 1 3 3 3 3 1 2 3
3 1 2 3 3 3
2
2 1 3 2 1 3 1 2 2
2
1 2 2 3 1 2 2
2
1 2 2
2
2 1 3 3 4
1 2 3 3 3
2
2 1 3 2 1 3 1 2 2
2
1 2
2 3 1 2 2
2
1 2 2
2
2 1 3 3 4
3 3 2 3 1 2 3 3 3
2
2 1 3 2 1 3
1 2 2
2
1 2 2 3 1 2 2
2
1 2 2
2
2 1 3
3 4 1 2 3 3 3
2
2 1 3 2 1 3 1 2 2
2
1 2 2 3 1 2 2
2
1 2 2
2
2 1 3
3 4 3 2 1 2 2
2
2 1 3 2 1 3 3
3
2
2 1 3 2 1 3 1 2 2
2
1 2 2 3
2
1 2 2
2
2 1 3 3 4 1 2 3 3 3
2
2 1 3
2 1 3 1 2 2
2
1 2 2 3 1 2 2
2
1 2 2
2
2 1 3 3 4 3 2 1 2 2
2
2 3 2 2 1
1 )) . (
))) . .( ( ) .( ).( ( ( ( )) . .( ( ) .( ) (
) )) . .( ( ) .( ).( ( ( .(( )) ) . .( ( .( . (
) ) ( .( ( ) )) . .( ( .( ).( ( ( .((
))) . .( ( ) .( ).( ( ( ) ) ( .( ( ) ). ( ((
)) ( )) ) (( )))) .
) .( ).( ( ( .(( ))) . .( ( ) .( (((
)) ). )) ( ( (( ) . ) .( (
). ( ( .(( )) . ) .( ).( ( ( ) ) ( .( ( ( (
) ( ( ( ) ( ))). ( )). ). )) ( ( (( )
. ) .( ).( ( ( .(( ) .
((( )) ) (( ) .
) .( ).( ( ( .(( ))) . .( ( ) ) ( .( (
. ) ( .( ( .( )) .( ). ) ( ((( ))) ( ))). ( (
. ) ( ( )) . ) .( ).( ( ( .((
) .( ))
) . . )) ) . .( ( .( . (
) ) ( .( .( ) ( .( ( ) ) . ( . . (

(Ec 30)

) q sign( FS q FV ) w )d C(q q ) d ( S(q Q ) w )d S(q q )d ( -C(q Q
q ) d ( S(q Q ) w )d S(q q )d (-C(q )- Q )I w 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(q Q
))) q q ( d q )d C(q )) q (- )d )( (-(S(q S(q ))) q )(d S(q
))) q (-( )(d (C(q ) ) q q (-( )(d (C(q ) -Q )I q q q ((( )) ))S(q q Q
- )Q ) q q q ((-( )))) q q ( d ) q )(d C(q ))) q (- )(d )( -(S(q S(q
)) q )d S(q ) q ( )d (C(q ) ) q q (-( )(d (C(q ((m ))) q q q ( Q
Q ) q q q (-( ))))) q q ( d )) q )(d C(q ))) q ( )(d (S(q )(( C(q
)))) w )(d S(q )) q ( )(d (C(q ) q q ( ( )(d (S(q ( )(((m (C(q d
x y
x y ZZ
x
Y zz Y
X
X
y
2 2 2 1 2 2
2
1 2 2 2 1 2 2
2
1 2 2 2
2
1 2 2 2 1 2 2
2
1 2 2 2 3 2 1 3 2 1 3
1 2 2
2
1 2 2 3 1 2 2
2
1 2 2
2
2 1 3 3 3
2 1 3 1 2 2
2
1 2 2 3 1 2 2
2
1 2 2
2
2 1 3 3 3 3 2 1 3 3 2 3
3
2
2 1 3 2 1 3 1
2
1 2 2 3
1 2 2
2
1 2 2
2
2 1 3 3 4 3
3 2 1 3 2 1 3 1 2 2
2
1 2 2 3
1 2 2
2
1 2
2
2 1 3 3 4 3 3 2
2
2 2
2 1 3
2
2

(Ec 31)
59
) q sign( FS q FV ]
2 1 3 2 2 2
2
1 2 2 3 1 2 2
2
1 2 2
2
2 1 3 3
[
3
2 1 3 2 2 2
2
1 2 2 3
1 2 2
2
1 2 2
2
32 3 3 3 3 2 1 3 3
3 3 3 4

)) q q ( d w )d C(q
q ) d )( S(q C(q q ) d S(q ) q (- ) d C(q ) q q ( )d S(q
x
Q
))) q q ( d q )d C(q ) q ( ) d )( S(q S(q
) q ) d S(q ) q (- ) d C(q ( -W )( d ( C(q
y
- Q
ZZ
)I q q q (

(Ec 32)
) q )sign( (FS ) q ( FV ) q ( IA ) q ( -G M
4 4 4 4 4 4 4 3 4 4
(Ec 33)
Los valores numricos que se tuvieron en cuenta para la simulacin de la
dinmica y Control dinmico, se encuentran tabulados a continuacin
2
:

Parmetro Valor Parmetro Valor Parmetro Valor
I
zz1
3.38 Q
x3
0.1 F
v1
= F
v3
0.1
I
zz2
0.063 Q
y2
0.001 F
v2
= F
v4
0.012
I
zz3
0.1 Q
y3
0.1 F
s1
= F
s3
0.57
Q
x2
0.242 I
a4
0.045 F
s2
= F
s4
0.125
Tabla 2 Parmetros de robot SCARA
Las unidades para el tensor de inercia son Kg.m
2
, para el primer momento de
inercia Kg.m y para la inercia del motor Kg.m
2
.
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
clsico 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 ampliacin del
comportamiento de dichos torques para los tiempos t=1.5 seg y t=0 seg. Dentro de
cada grfica se muestra la respectiva convencin. 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 Automtica e Informtica.
Politecnico di Torino. Italia.

60
importante aclarar que hasta el momento en este CapituloCaptulo, 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)
T
o
r
q
u
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
15
COMPORTAMIENTO DE LOS TORQUES EN TIEMPO PARA LA TRAYECTORIA 2
Tiempo (seg)
T
o
r
q
u
e
s

(
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
Tiempo (seg)
T
o
r
q
u
e
s

(
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
Tiempo (seg)
T
o
r
q
u
e
s

(
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 dinmico de los motores DC
El propsito en el desarrollo del modelo matemtico del motor es expresar el
voltaje aplicado a la armadura en funcin de la velocidad del motor. Dos
ecuaciones de balance pueden ser desarrolladas considerando las caractersticas
elctricas y mecnicas del sistema.
Caractersticas Elctricas
El circuito equivalente elctrico de un motor DC es ilustrado en la Figura 17Figura
17 . Este puede ser representado por una fuente de voltaje (V
a
)

a travs de la
bobina de la armadura. El equivalencia elctrica de la bobina de la armadura
puede ser descrita por una inductancia (L
a
) en serie con una resistencia (R
a
) en
serie con un voltaje inducido (V
c
) el cual se opone a la fuente de voltaje. El voltaje
63
inducido es generado por la rotacin de la bobina elctrica a travs de las lneas
fijas de flujo de los imanes permanentes. Este voltaje es comnmente llamado
voltaje contraelectromotriz.
J
Ra
K
f
Va
+
_
La
V
emf
+
_
aplicado
motor
J
Ra
K
f
Va
+
_
La
V
emf
+
_
aplicado
motor

Figura 17 Representacin elctrica de los motores DC del SCARA
Una ecuacin diferencial para el circuito equivalente puede ser derivada usando la
ley de voltaje de Kirchoff alrededor del lazo elctrico. La ley de voltaje de Kirchoff
establece que la suma de todos los voltajes alrededor en el lazo son cero, es
decir:
0
emf La Ra a
V V V V (Ec 34)
De acuerdo con la ley de Ohm, el voltaje a travs de una resistencia puede ser
expresado como:
a a Ra
R i V (Ec 35)
donde
a
i es la corriente de armadura. La cada de potencial a travs de un
inductor es proporcional al cambio de la corriente por la bobina con respecto al
tiempo y puede ser expresada como:
dt
i d
L V
a
a La
(Ec 36)
64
donde L
a
es la inductancia de la bobina de la armadura. Finalmente, el voltaje
contraelectromotriz puede ser estimado como:
a v emf
w K V (Ec 37)
donde k
v
es la constante de velocidad determinada por la densidad de flujo del
imn 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, w
a
es la velocidad angular
de la armadura. Substituyendo las Ec 35, Ec 36 y Ec 37 en la Ec 34 se obtiene la
siguiente ecuacin diferencial (Ec 38).
0
a v
a
a a a a
w K
dt
i d
L R i V (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 parmetros elctricos, por que la
variable manipulada por el controlador es directamente Torque como lo muestra el
Modelo Dinmico Directo e Inverso en el ANEXO 5.

65

CAPTULO IV
4. CONTROL DINMICO DEL ROBOT INDUSTRIAL

4.1 Introduccin.
El desacoplamiento de la estrategia de control se puede lograr mediante el uso de
la dinmica inversa de control, control no lineal, diseo de Lyaponov, o
linealizacin por retroalimentacin de los sistemas dinmicos no lineales de los
manipuladores.

A medida que se incrementan los grados de libertad de la cadena cinemtica, las
ecuaciones dinmicas del manipulador se vuelven ms complejaso. Por lo tanto es
muy difcil calcular la inversa de una ecuacin dinmica general de un robot en
serie multi-eslabn.

Aunque hay muchos algoritmos de control de sistemas no lineales, la mayora de
ellos son de difcil aplicacin 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 tcnica de linealizacin por retroalimentacin lineal.

Con formato: Sangra: Izquierda: 0
cm, Interlineado: sencillo
Con formato: Derecha, Sangra:
Izquierda: 0 cm, Primera lnea: 0 cm,
Interlineado: sencillo, Numerado +
Nivel: 1 + Estilo de numeracin: 1, 2, 3,
+ Iniciar en: 1 + Alineacin:
Izquierda + Alineacin: 0,21 cm +
Tabulacin despus de: 0,85 cm +
Sangra: 0,85 cm, Punto de tabulacin:
0 cm, Lista con tabulaciones + No en
0,85 cm
66
4.2 Computer Torque Control CTC.
Durante aos 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 linealizacin por
retroalimentacin de sistemas no lineales, que ha ganado la popularidad en la
teora 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 clsico independiente y algunas
tcnicas de diseo modernas.

4.2.1 Derivacin de Lazo Interior por realimentacin
La dinmica de brazo de robot es:

d d v
q G q F q F q q V q q M ) ( ) ( , ) ( (Ec 39)

O

d
q q N q q M , ) ( , (Ec 40)

Con la variable articular ) ( , ) ( t R t q
n
par de control de rotacin, y ) (t
d
una
perturbacin. Si esta ecuacin incluyese la dinmica de actuador de motor,
entonces ) (t seria un voltaje de entrada.

Supongamos que ha seleccionado una trayectoria deseada t q
d
para el
movimiento de brazo, como se observo 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 vietas ni
numeracin
67
t q t q t e
d
(Ec 41)

Para demostrar la influencia de la entrada ) (t sobre el error de seguimiento,
derivemos dos veces y se obtiene:

q q e
d

q q e
d

Solucionando q en la (Ec 40)[] y sustituyendo en la (Ec 41) de los rendimientos se
obtiene:

) (
1
d d
N M q e (Ec 42)

Definiendo la funcin de control de entrada

) (
1
N M q u
d
(Ec 43)

Y la funcin perturbacin
d
M w
1
(Ec 44)

Podemos definir un estado ) (t x
n
R
2
por

e
e
x
(Ec 45)

Y escribiendo el error de seguimiento dinmico como

68
w
I
u
I e
e I
e
e
dt
d 0 0
0 0
0

(Ec 46)

Este es un sistema lineal de error en la forma cannica de Brunovsky [24] que
consiste en n pares de doble integradores 1/s
2
, uno por cada conjunto. Es
conducida por la entrada de control U(t) y la perturbacin w(t). Pero hay que tener
en cuenta que esta derivacin es un caso especial del procedimiento de
linealizacin antes mencionado.

La (Ec 43) puede ser reescrita de la forma:

N u q M
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).

N u q M N M q
d d
) (
o
,
1
d
M u e (Ec 48)

que es exactamente la (Ec 46).

La estabilizacin (Ec 46)[] no es difcil. De hecho, la transformacin no lineal (Ec
43)] ha convertido un problema no lineal en un problema de diseo mas sencillo
69
para un sistema lineal que consiste en la n subsistemas desacoplados, que
obedece a las leyes de Newton.
Manipulador
q
q
M(q)
d
q
-
d
q
Regeneracin
de lazo externo
N(q,q)
Lazo no lineal
interior
Sistema
lineal
u
Manipulador
q q
q
M(q)
d
q
d
q
-
d
q
d
q
Regeneracin
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 ms una seal 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 realimentacin.
En general, podemos seleccionar un compensador dinmico H (s) de modo que:

) ( ) ( ) ( s E s H s U (Ec 49)

La H (s) puede ser seleccionada para el comportamiento de lazo-cerrado. Segn
(Ec 48)[], el sistema de error de lazo-cerrado entonces tiene la funcin de
transferencia

) ( ) (
2
s H I s s T . (Ec 50)

70
Es importante saber que el torque calculado depende de la dinmica del robot, el
cual es llamado a veces control dinmico inverso, de hecho en (Ec 47) se muestra
que el t es calculado por medio de la substitucin de u q
d
por q en (Ec 40)[];

resolviendo la dinmica inversa del problema del robot. Todas las salvedades
asociadas con el problema de inversin, incluyendo los problemas provenientes de
cuando el sistema no tiene una mnima fase de ceros, aplican aqu.

Existen muchas formas de calcular (Ec 47)]. La multiplicacin formal de matrices
en cada muestra de tiempo debe evitarse. En algunos casos las expresiones
pueden ser resueltas analticamente. Una buena manera de calcular el t es por
medio del uso eficiente de la formulacin de la dinmica inversa de Newton-Euler
con u q
d
en lugar de ) (t q .

4.2.2 Diseo del lazo externo (PD)
Una forma de seleccionar la seal de control u(t) es por medio del uso de una
realimentacin 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 funcin de transferencia que modela la respuesta del manipulador en
lazo cerrado, lo cual se traduce en un polo mucho ms cercano a la zona de
inestabilidad.

e K e K u
p v
(Ec 51)

Luego la entrada neta del manipulador robtico resulta en:

) , ( ) )( ( q q N e K e K q q M
p v d

(Ec 52)

Este mismo controlador se muestra en la Figura 19 con K
i
= 0.
71

Modelo Din Modelo Din mico mico
Directo SCARA Directo SCARA q
q
Inverso SCARA Inverso SCARA
d
q
d
q
d
q
Kp
Kv
+ -
+ -
q
q
q
Directo SCARA Directo SCARA q
q
Inverso SCARA Inverso SCARA
d
q
d
q
d
q
Kp
Kv
+ -
+ -
q
q
q

Figura 19 Esquema de control PID por par calculado

El error dinmico del lazo cerrado es:

w e K e K e
p v

(Ec 53)

O en la forma de espacio-estado:

w
I e
e
K K
I
e
e
dt
d
v p
0 0

(Ec 54)

El polinomio caracterstico del bucle es :

p v c
K s K I s s
2
) ( (Ec 55)

72
4.2.3 Seleccin de las ganancias (PD)
Es usual tomar el producto diagonal de las matrices n*n tal que:

,
vi v
k diag K ,
pi p
k diag K (Ec 56)

Entonces:

n
i
pi vi c
k s k s s
1
2
) ( (Ec 57)

Y el sistema respecto al error es estable asintticamente cuando
vi
K y
pi
K son
positivos. De esta forma, mientras que la perturbacin w(t) est limitada, tambin
lo estar el error e(t). Y examinando la (Ec 44) se puede concluir que M
-1
tiene
lmite superior. Esto indica que el limitante de w(t) es equivalente al limitante de
) (t
d
.

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 repeticin
exterior no resulta en una estrategia de unin de control desacoplado. Esto se
debe a que al multiplicar por ) (q M y la adicin de trminos no lineales de salida
) , (
.
q q N en el bucle interno reparte la seal ) (t u a todas las juntas. As que es
necesaria la informacin de todas las posiciones ) (t q y las velocidades ) (
.
t q de las
articulaciones para estimar el algoritmo de control ) (t para cualquier articulacin
especifica.

La forma estndar del polinomio caracterstico de segundo orden es:

2 2
2 ) (
n n
s s s p (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 ) (t e puede ser alcanzado al
seleccionar ganancias del PD como:

2
n pi
k y s k
n vi
2 (Ec 59)

Con los factores de amortiguamiento y las frecuencias naturales deseadas para
los errores en cada articulacin. Puede ser mas til seleccionar las respuestas
deseadas al final del brazo que en la base, donde las masas que deben ser
movidas son ms pesadas. No es deseable para el robot presentar overshoot,
porque puede causar impacto, por ejemplo, si una trayectoria deseada repercute
en una colisin del TCP en la superficie de trabajo. Es por esta razn, que las
ganancias del PD son usualmente escogidas para amortiguamiento crtico 1.
En este caso:

pi vi
k k 2 ,
4
2
2
vi
pi
k
k (Ec 60)

La seleccin 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 rpidas 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 proporcin a partir de un valor unitario y nulo en la ganancia de
velocidad hasta obtener respuesta crticamente 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 sintona del controlador PD en el lazo externo se muestran a
continuacin en la Tabla 3:
74
KP KV
Articulacin 1 125000 250
Tabla 3. Parmetros de sintonizacin 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 mximo) por tal motivo las
graficas se muestran superpuestas, sin embargo en la Figura 21 se muestra un
acercamiento para cada articulacin para la primera trayectoria. El controlador por
par calculado va mejorando su respuesta segn las posiciones deseadas en el eje
z se alejen de las crestas o valles. Para la trayectoria 2, el cambio de direccin
sbito, produce que el controlador PD instalado con el CTC presente errores ms
grandes, como se observa en la Figura 22 y Figura 23, es importante aclarar que
en el capitulo siguiente se profundizara 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 direccin 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
5
COMPARACION ENTRE q real (rad) - q deseado (rad) PARA LA TRAYECTORIA 1
Tiempo (seg)
q

r
e
a
l

(
r
a
d
)

-

q

d
e
s
e
a
d
o

(
r
a
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. Comparacin entre q real y q deseado para la Trayectoria 1

76

Figura 21. Comparacin 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
COMPARACIONENTREq4 real (rad) - q4 deseado (rad) PARALATRAYECTORIA1
Tiempo (seg)
q
4

r
e
a
l

(
r
a
d
)

-

q
4

d
e
s
e
a
d
o

(
r
a
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
Time (seg)
q
3

r
e
a
l

(
r
a
d
)

-

q
3

d
e
s
e
a
d
o

(
r
a
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)
q
1

r
e
a
l

(
r
a
d
)

-

q
1

d
e
s
e
a
d
o

(
r
a
d
)
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)
q
2

r
e
a
l

(
r
a
d
)

-

q
2

d
e
s
e
a
d
o

(
r
a
d
)
COMPARACIONENTREq2 deseado (rad) - q2 real (rad) PARALATRAYECTORIA1
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
e
a
l

(
r
a
d
)

-

q

d
e
s
e
a
d
o

(
r
a
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. Comparacin 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 posicin deseada especfica, sino con una desviacin
pequea, esto es debido a que el controlador instalado no presenta la accin
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 posicin 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 ningn momento llega a la consigna deseada.

78
Figura 23. Comparacin 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
e
a
l

(
r
a
d
)

-

q

d
e
s
e
a
d
o

(
r
a
d
)
COMPARACIONENTREq real (rad) - q deseado (rad) PARALATRAYECTORIA2
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
e
a
l

(
r
a
d
)

-

q

d
e
s
e
a
d
o

(
r
a
d
)
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
e
a
l

(
r
a
d
)

-

q

d
e
s
e
a
d
o

(
r
a
d
)
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
e
a
l

(
r
a
d
)

-

q

d
e
s
e
a
d
o

(
r
a
d
)
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.368
0.35
0.355
0.36
0.365
0.37
0.375
0.38
Posicion en el eje x (m)
P
o
s
i
c
i
o
n

e
n

e
l

e
j
e

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. Posicin real y Deseado para la Trayectoria 2 en el espacio de la tarea

80

CAPTULO V

5. RESULTADOS Y ANLISIS DE RESULTADOS

Todos los resultados y grficos obtenidos en la presente investigacin que se han
venido mostrando y comentando en cada uno de los captulos anteriores, y que
culminan con este capitulocaptulo se realizaroon en Matlab/Simulink, la Figura
25 muestra el diagrama del manipulador en el entorno grafico de programacin
antes especificado, en el se muestra claramente cada una de las conexiones
realizas para el modelado del sistema.

Con formato: Ttulo 1, Centrado,
lnea: 0 cm, Numerado + Nivel: 1 +
Estilo de numeracin: 1, 2, 3, +
Iniciar en: 1 + Alineacin: Izquierda +
Alineacin: 0,21 cm + Tabulacin
despus de: 0,85 cm + Sangra: 0,85
cm, Punto de tabulacin: 0 cm, Lista
con tabulaciones + No en 0,85 cm
Con formato: Normal
internacional)
81

Figura 25 Simulacin del Robot SCARA de Cuatro Grados de Libertad

En la seccin anterior se presentaron las graficas comparativas de las trayectorias
seguida por el manipulador en simulacin y deseadas tanto en el espacio articular
como el espacio cartesiano para ambas trayectorias mediadas por el algoritmo de
control PD-CTC. En esta seccin se presentan los valores de los ndices de
desempeo IAE para los errores de posicin 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 perturbacin (t=0) hasta un tiempo posterior muy largo (t= ).
Este criterio de optimizacin esta dado por la siguiente ecuacin (Ec 61).

Con formato: Centrado
Con formato: Centrado
82
IAE= dt t e
0
) ( (Ec 61)

La interpretacin geomtrica 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 posicin 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 grficos en Matlab.

Figura 26. Integral del Valor Absoluto del error en el tiempo real

A continuacin 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 mximo error articular

Set
Point
Variable de
proceso
Tiempo
(+
)
( - )
( - )
Error
Error
83
se presenta en la articulacin 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 articulacin 1 el valor mximo
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 dimetro.
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.5
x 10
-3
Tiempo (seg)
E
r
r
o
r
e
s

A
r
t
i
c
u
l
a
r
e
s

(
r
a
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 grfica se alcanza un error mximo de 9.766x10
-4
m en el eje x en el
mismo tiempo de 0.004 seg, y un porcentaje de reduccin 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 mximos locales en el mismo
instante de tiempo, debido a que no necesariamente son puntos mximos globales
de toda la trayectoria, adems se puede concluir que el error cartesiano de
seguimiento se incrementa en la medida en que se le permite al manipulador
desarrollar una trayectoria masms 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
10
x 10
-4
Tiempo (seg)
E
r
r
o
r
e
s

C
a
r
t
e
s
i
a
n
o
s

(
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 mxima de 0.2 m al cabo de 1.5 seg, punto
85
donde se presenta la mxima razn de cambio de los errores absolutos articulares
para la trayectoria 1. Inicialmente hay un incremento sbito en todos los errores
absolutos articulares, pero a medida que el controlador sigue la trayectoria
especfica los tres primeros errores absolutos articulares se estabilizan en cuanto
a su razn 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)
E
r
r
o
r

a
b
s
o
l
u
t
o
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
direccin es ms repentino, cuando el Tiempo es de 1.511 se obtiene un Error
mximo 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 direccin
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.5
x 10
-5
Tiempo (seg)
E
r
r
o
r
e
s

C
a
r
t
e
s
i
a
n
o
s

(
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
3
x 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 articulacin 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 crtico, pero a medida que el controlador ejerce su
trabaja minizacin de dichos errores, hasta que en 1.5 seg, se obtiene el otro pico
en cuanto al error producto del cambio de direccin ms 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 ms pequeos tanto en coordenadas
articulares como cartesiana, pero presenta errores puntuales ms grandes en esta
87
trayectoria, mientras que para la trayectoria de cambio de sentido y direccin este
presenta un IAE ligeramente superior, pero valores de picos ms bajos.

0 0.5 1 1.5 2 2.5 3
-6
-4
-2
0
2
4
6
8
x 10
-5
Tiempo (seg)
E
r
r
o
r
e
s

a
r
t
i
c
u
l
a
r
e
s

(
r
a
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
4
x 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 desempeo para el Controlador CTC

Los valores en los que el IAE en nulo para la segunda trayectoria, son producto de
que la posicin a lo largo del eje z no se vario para la segunda trayectoria.

89
CONCLUSIONES

Despus de haber logrado los objetivos especficos planteados en la presente
investigacin, se pueden mencionar las siguientes conclusiones:

El modelo cinemtico representa al manipulador sin considerar efectos debido a
fricciones, gravedad, inercia y peso,; se verifico su desempeo con los
parmetros Denavit-Hartenberg (distancias y ngulos) y su simulacin permiti
verificar el movimiento del manipulador tanto pasando del espacio cartesiano al
articular o viceversa con la ayuda de la cinemtica directa e inversa.

Se realiz un programa capaz de determinar el modelo dinmico inverso de
cualquier robot hasta un numero mximo de cinco grados de libertad,; aes este
le es indiferente si la articulacin es prismtica o rotacional. Este algoritmo utiliza
la formulacin Recursiva de Newton Euler para el cmputo de las relaciones
dinmicas requeridas.

El modelo dinmico obtenido permite incorporar los efectos reales del sistema
para que se aproxime al manipulador real. Este se simul en Matlab con
parmetros de literatura, pero este permite la incorporacin de parmetros 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 retroalimentacin con compensador basado en el modelo
dinmico no lineal tipo CTC (Computer Torque Control) con un buen ndice de
desempeo.

Se desarroll una comparacin del desempeo IAE del controlador de seguimiento
de trayectoria por medio de la simulacin de dos trayectorias diferentes, lo que
indicara que en la prctica este controlador puede ser utilizado confiando en tener
una buena respuesta por parte del robot para las dos trayectorias simuladas.
90

TRABAJO FUTURO

Despus de esta experiencia, se recopilaron una serie de recomendaciones con el
fin de hacer ms fcil la realizacin de proyectos que involucren el modelamiento y
simulaciones de controladores en manipuladores industriales:

* Trabajar el modelo con valores reales del manipulador, puesto que los
parmetros que se utilizaron en la simulacin no corresponden al manipulador que
se encuentra en el Laboratorio de Robtica de la Universidad del Norte.

* Incluir en el modelo dinmico 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 simulacin una estrategia de control predictivo con el fin
de confrontar la estrategia de control implementada en este trabajo en trminos de
integral absoluta de error e seguimiento.

* Desarrollar la ingeniera requerida para la implementacin 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, 1
st
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, Pen, Balaguer, Aracil., McGraw Hill, 1997.
[4] ARRETO Jair, RUBIO Martn. Controlador DMC Escalable Para Procesos
SISO de Ganancia No Lineal, (2003). 143p. Trabajo de Grado (Ingeniera
Electrnica). Universidad del Norte. Departamento de Ingeniera Electrnica.
[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. Diseo e Implementacin de un
Controlador Predictivo Tipo GPC con Compensacin de Tiempo Muerto Basado
en el Algoritmo de Dahlin Para un Reactor Tipo CSTR, (2005). 71p. Trabajo de
Grado (Ingeniera Mecnica). Universidad del Norte. Departamento de Ingeniera
Mecnica.
[7] J. J. Craig, Adaptive Control of Mechanical Manipulators. Addison-Wesley,
1988.
Con formato: Ingls (Estados Unidos)
92
Mecnica.
Mecnica.
[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. 215221, 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 Nstor, MARTELO Gabriel. Implementacin de un control DMC
con Predictor de Smith en un reactor tipo CSTR modificado 100p (2005). Trabajo
de Grado (Ingeniera Mecnica). Universidad del Norte. Departamento de
Ingeniera Mecnica.
[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. 2431, 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.
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
Dinmica Aplicados a Procesos de Refinacin. 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.
Cdigo de campo cambiado
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 Ingeniera, 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 GENERACIN 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 INICIALIZACIN DEL CONTROLADOR CTC

% ----------Inicializacin control dinmico 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 CINEMTICO

Modelo cinemtico directo de un ESCARA

function x = mgd(q1,q2,q3,q4)
% ---------Modelo geomtrico 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 cinemtico inverso de un ESCARA

function q = mgi(x1,x2,x3)
% ----------Modelo geomtrico 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 DINMICO

Modelo dinmico 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)
internacional)
Cdigo de campo cambiado
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;
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;
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;
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;
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;
WP33=QDP3 + WP32;
GU4=-(G3.*JU64);
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 dinmico 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;

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 eslabn mvil,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)';
DH_art=input(['Para la articulacin ',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
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);
R23=[a; b; c]; R32=inv(R23); R13=R12*R23;
R31=inv(R13); R03=R01*R13;
R30=inv(R03);
R14=R13*R34; R41=inv(R14); R04=R03*R34; R40=inv(R04);
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))];
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
Datos(1,3) ];
elseif i==3
Datos(1,3) ];
elseif i==4
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
w22=R21*(w11+zo*diff(th2,'t'));
else
w22=R21*(w11);
end

tam=size(temp_arti);
gevo=tam(1,1);

if gevo==3
w33=R32*(w22+zo*diff(th3,'t'));
else
w33=R32*(w22);
end
elseif gevo==4
w33=R32*(w22+zo*diff(th3,'t'));
else
w33=R32*(w22);
end

w44=R43*(w33+zo*diff(th4,'t'));
else
w44=R43*(w33);
end
elseif gevo==5
w33=R32*(w22+zo*diff(th3,'t'));
else
w33=R32*(w22);
end

w44=R43*(w33+zo*diff(th4,'t'));
else
w44=R43*(w33);
end
w55=R54*(w44+zo*diff(th5,'t'));
else
w55=R54*(w44);
end
end

% PASO 5. ACELERACIONES ANGULARES DEL SISTEMA Si
dw11=R10*(dw00+zo*diff(th1,'t',2))+cross(w00,zo*diff(th1,'t'));
else
dw11=R10*(dw00);
end
110
else
dw22=R21*(dw11);
end

if gevo==3
else temp_arti(3,1)==p
dw33=R32*(dw22);
end
elseif gevo==4
dw33=R32*(dw22);
end

else
dw44=R43*(dw33);
end
elseif gevo==5
dw33=R32*(dw22);
end

else
dw44=R43*(dw33);
end
dw55=R54*(dw44);
end
end

% PASO 6. ACELERACIONES LINEALES DEL SISTEMA Si
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
else

));
end

if gevo==3
111

));
end
elseif gevo==4

));
end

else

));
end
elseif gevo==5

));
end

else

));
end


));
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
elseif gevo==5
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
f33=R34*f44+m3*a33;
f22=R23*f33+m2*a22;
f11=R12*f22+m4*a11;
elseif gevo==3
f22=R23*f33+m2*a22;
f11=R12*f22+m1*a11;
elseif gevo==2
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));
elseif gevo==4

elseif gevo==3

elseif gevo==2

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
Tao1=(n11.')*R10*zo;
else
F1=(f11.')*R10*zo;
end
Tao2=(n22.')*R21*zo;
else
F2=(f22.')*R21*zo;
end
113

if gevo==3
Tao3=(n33.')*R32*zo;
F3=(f33.')*R32*zo;
end
elseif gevo==4
Tao3=(n33.')*R32*zo;
F3=(f33.')*R32*zo;
end

Tao4=(n44.')*R43*zo;
else
F4=(f44.')*R43*zo;
end
elseif gevo==5
Tao3=(n33.')*R32*zo;
F3=(f33.')*R32*zo;
end

Tao4=(n44.')*R43*zo;
else
F4=(f44.')*R43*zo;
end

Tao5=(n55.')*R54*zo;
else
F5=(f55.')*R54*zo;
end
end

Con Formato

Cargado por

Información del documento

Título original

Derechos de autor

Formatos disponibles

Compartir este documento

Compartir o incrustar documentos

Opciones para compartir

¿Le pareció útil este documento?

¿Este contenido es inapropiado?

Copyright:

Formatos disponibles

Con Formato

Cargado por

Copyright:

Formatos disponibles

i

MODELAMIENTO DE LA CINEMATICA Y DINAMICA DE UN ROBOT

se puede expresar en trminos de sus componentes como

por q en (Ec 40)[];

También podría gustarte