Borra Dor

UNIVERSIDAD NACIONAL DEL ALTIPLANO PUNO
FACULTAD DE INGENIERA MECNICA ELCTRICA, ELECTRNICA Y SISTEMAS

ESCUELA PROFESIONAL DE INGENIERA ELECTRNICA

DISEO DE UN CONTROLADOR NO LINEAL MULTIVARIABLE POR
REALIMENTACIN DE ESTADO APLICADO A UN ROBOT
MANIPULADOR DE 3 GRADOS DE LIBERTAD

BORRADOR DE TESIS
PRESENTADO POR:
Bach. Magaly Pilar Miranda Aguilar
Bach. David Mamani Huacani

PARA OPTAR EL TTULO DE:
INGENIERO ELECTRNICO

PUNO PER
2012

UNIVERSIDAD NACIONAL DEL ALTIPLANO PUNO
FACULTAD DE INGENIERA MECNICA ELCTRICA, ELECTRNICA Y
SISTEMAS
ESCUELA PROFESIONAL DE INGENIERA ELECTRNICA
DISEO DE UN CONTROLADOR NO LINEAL MULTIVARIABLE
POR REALIMENTACIN DE ESTADO APLICADO A UN ROBOT
MANIPULADOR DE 3 GRADOS DE LIBERTAD

PARA OPTAR EL TITULO PROFESIONAL DE INGENIERO ELECTRNICO

APROBADA POR EL JURADO REVISOR CONFORMADO POR:

PRESIDENTE: _____________________________________
Ing. Marco Antonio Ramos Gonzales.

PRIMER MIEMBRO: _____________________________________
Ing. Gavino Jos Flores Chipana.

SEGUNDO MIEMBRO: _____________________________________
Ing. Eddy Torres Mamani.

DIRECTOR DE TESIS: _____________________________________
Ing. Marco Antonio Quispe Barra.

ASESOR DE TESIS: _____________________________________
Ing. Pedro Bejar Muoz.

DEDICATORIA

AGRADECIMIENTOS

NDICE
RESUMEN ....................................................................................................................... 9
Abstract ........................................................................................................................... 10
INTRODUCCIN .......................................................................................................... 11
CAPTULO I .................................................................................................................. 14
PLANTEAMIENTO DEL PROBLEMA ....................................................................... 14
1.1. Formulacin del Problema .......................................................................... 15
1.2. Antecedentes ................................................................................................. 16
1.3. Objetivos de la investigacin. ...................................................................... 18
1.3.1. Objetivo General .................................................................................... 18
1.3.2. Objetivos Especficos .............................................................................. 18
1.4. Alcance ........................................................................................................... 19
CAPITULO II ............................................................................................................... 20
MARCO TERICO Y CONCEPTUAL .................................................................... 20
2. Marco Terico ....................................................................................................... 21
2.1. Conceptos Generales .................................................................................... 21
2.2. Representacin de la Posicin y Orientacin ............................................. 23
2.3. Cinemtica y Dinmica de un Robot .......................................................... 25
2.4. Dinmica de un Robot .................................................................................. 27
2.4.1. Modelo Dinmico .................................................................................... 29
2.4.2. El procedimiento de Denavit-hartenberg ............................................. 30
2.4.3. Algoritmo de Lagrange-Euler ............................................................... 35
2.5. Realimentacin No Lineal ............................................................................ 38
2.5.1. Herramienta de la Geometria Diferencial ............................................ 57
2.5.2. Linealizacin por Realimentacin para Procesos SISO ...................... 62
2.5.3. Linealizacin Por Realimentacin para Procesos MIMO ................. 69
2.6. Hiptesis y Variables .................................................................................... 74
2.6.1. Hiptesis General .................................................................................... 75
2.6.2. Hipotesis Especfica ................................................................................ 75
2.7. Variables ........................................................................................................ 75
2.7.1. Variables Independientes ....................................................................... 75
2.7.2. Variables Dependdientes ........................................................................ 75
2.8. Metodologa ................................................................................................... 75
2.8.1. Tipo y Nivel de Investigacin ................................................................. 76
CAPTULO III ............................................................................................................... 76
3. MODELADO DE LA PLANTA .......................................................................... 76
3.1. Introduccin .................................................................................................. 78
3.2. Descripcin del Sistema ............................................................................... 78
3.3. Lista de Variables ......................................................................................... 80
3.4. Aplicacin y Programacin del Modelo ..................................................... 82
3.4.1. Clculo ..................................................................................................... 82
3.4.2. Programacin del Algoritmo de lagrange-Euler ................................. 88
3.4.3. El Modelo de los Actuadores ................................................................. 89
3.4.4. Modelo de Espacio de Estado ................................................................ 94
CAPTULO IV .............................................................................................................. 99
SIMULACIN DEL SISTEMA ................................................................................. 99
4. Simulacin del Sistema ....................................................................................... 100
4.1. Caso 1: Control de Trayectoria Constante .............................................. 101
4.1.1. Linealizacin por Realimentacin ....................................................... 102

4.1.2. Determinacin del Grado Relativo Total: r ....................................... 102
4.1.3. Forma Normal para Linealizacin ...................................................... 103
4.1.4. Diseo del Controlador No Lineal MIMO ......................................... 103
4.1.5. Programacin del Algoritmo de Control No Lineal Multivariable . 106
4.1.6. Simulaciones .......................................................................................... 106
4.2. Caso 2: Control de Trayectoria Senoidal ................................................. 110
4.2.1. Diseo del Controlador No Lineal MIMO ......................................... 111
4.2.2. Programacin del Algoritmo de Control No Lineal Multivariable . 111
4.2.3. Simulaciones .......................................................................................... 111
CAPTULO V ............................................................................................................. 115
ESPECIFICACIONES TESCNICAS ....................................................................... 115
5. Especificaciones Tcnicas .................................................................................. 116
5.1. Introduccin ................................................................................................ 116
5.2. El Hardware del Sistema ........................................................................... 116
5.2.1. La Planta ............................................................................................... 116
5.2.2. El Actuador ........................................................................................... 118
5.2.3. El Sensor ................................................................................................ 123
5.2.4. La Interfaz de Adquisicin de Datos .................................................. 125
CAPTULO VI ............................................................................................................ 127
CONCLUSIONES Y RECOMENDACIONES ....................................................... 127
6. Conclusiones y Recomendaciones ..................................................................... 128
CONCLUSIONES ........................................................................................................ 128
RECOMENDACIONES .............................................................................................. 129
BIBLIORAFIA ........................................................................................................... 130
ANEXOS ..................................................................................................................... 132
ANEXO 1 ..................................................................................................................... 133
Coeficiente de Friccin Viscosa ................................................................................. 133
ANEXO 2 ..................................................................................................................... 140
Programas en MATLAB .............................................................................................. 140

NDICE DE FIGURAS

Figura 3.1: Esquema del Robot Manipulador Esfrico de 3DOF
Figura 3.2: Esquema con las medidas del Manipulador
Figura 3.3: Sistemas de referencia del Manipulador
Figura 3.4: Momento de inercia del disco respecto al sistema de referencia 1(S1).
Figura 3.5: Momento de inercia del prisma respecto al sistema de referencia 1 (S1)
Figura 3.6: Servomotor DC1 de la base del Manipulador
Figura 3.7: Servomotor DC2 de la primera varilla del Manipulador
Figura 3.8: Servomotor DC3 de la segunda varilla del Manipulador
Figura 5.1: Diagrama de bloques del controlador no lineal
Figura 5.2: Trayectoria constante: salidas controladas para la base, primera y segunda
Varilla
Figura 5.3: Trayectoria constante: seales de control para los servomotores de la base,
primera y segunda varilla
Figura 5.4: Trayectoria constante: velocidad para los servomotores de la base, primera y
segunda varilla
Figura 5.5: Trayectoria senoidal: salidas controladas para la base, primera y segunda
varilla.
Figura 5.6: Trayectoria senoidal: seales de control para los servomotores de la base,
primera y segunda varilla.
Figura 5.7: Trayectoria senoidal: Velocidad de los servomotores de la base, primera y
segunda varilla.
Figura 6.1: Diagrama de Bloques de la implementacin del sistema.
Figura 6.2: Circuito Amplificador de Potencia.
Figura 6.3: Esquema del circuito de disparo y conmutacin.
Figura 6.4: Circuito Generador PWM
Figura 6.5: Estructura del Codificador ptico rotatorio

NDICE DE TABLAS

Tabla 3.1: Valores de las Variables.
Tabla 3.2: valores de los parmetros DH
Tabla 5.1: Conmutacin de los MOSFETs.

RESUMEN

En el presente trabajo se disea el control multivariable aplicado Robot de 3 Grados de
Libertad caracterizado por una dinmica no lineal. El problema de control conduce al
anlisis de la regulacin de un sistema multivariable con tres entradas y tres salidas,
representando un anlisis previo al diseo mediante procedimientos no lineales que
conducen a la implementacin de un controlador no lineal multivariable por
realimentacin de estado que tenga en cuenta las incertidumbres sobre los parmetros
del sistema. Este manipulador se compone de una base giratoria y dos eslabones, cuyas
posiciones angulares son sujetos de control y las salidas controladas deben seguir la
evolucin de trayectorias arbitrarias.

ABSTRACT

This paper is designed multivariable control applied Robot 3 degrees of freedom
characterized by nonlinear dynamics. The control problem leads to the analysis of the
regulation of a multivariable system with three inputs and three outputs, representing a
design prior to analysis by methods that lead to non-linear implementation of a
controller for multivariable nonlinear state feedback taking account uncertainties in the
system parameters. The manipulator consists of a rotatable base and two links, which
are angular positions and control subjects controlled outputs must follow the evolution
of arbitrary trajectories.

INTRODUCCIN

El control no lineal es una ciencia que trata del diseo de los sistemas de control no
lineal. Los sistemas fsicos del mundo real son no lineales y pueden ser descritos por
ecuaciones diferenciales.
En procesos multivariables de varias entradas y varias salidas, se caracterizan por
presentar un comportamiento dinmico muy complejo, sin embargo, se implementan
las tcnicas de control no lineal, los cuales en los ltimos aos ha alcanzado
un elevado nivel de aceptacin industrial en control de procesos. Las tcnicas de
control no lineal que han logrado mayor aceptacin son, el control no lineal por el
mtodo de linealizacin por realimentacin de estado, control no lineal adaptivo,
control no lineal por modos deslizantes, control predictivo no lineal, control no
lineal usando backstepping y control con inteligencia artificial.
La tcnica a emplear en la presente trabajo es la de Control No Lineal basada en el
mtodo de Linealizacin por Realimentacin de Estado y se basa en la linealizacin
del proceso mediante un cambio adecuado de coordenadas y una realimentacin del
vector de estado de dicho proceso, de modo tal que el proceso resultante en su forma
normal nos permita aplicar tcnicas de control lineal en el caso de linealizacin
exacta o bien atacar los problemas de control con un grado de dificultad menor en el
caso de linealizacin parcial.
El mtodo de linealizacin exacta se aplica cuando el proceso no lineal
MIMO (multiple-input-multiple-output) de orden n, de m entradas y m salidas es
completamente linealizable y desacoplado si su grado relativo total (r) es
igual al orden del proceso, o sea r = n, y para determinar dicho grado se aplica los
conceptos de geometra diferencial. Para el diseo del controlador no lineal

multivariable tienen nuevas leyes de control de entrada v
i
, los cuales son diseadas
por ubicacin de polos del correspondiente subsistema lineal equivalente.
Control por realimentacin de estado es bien entendible para sistemas lineales
con simples entradas. Sin embargo para sistemas no lineales de mltiples entradas
son tpicos de bsqueda muy lenta y debido a esto tiene que ser investigado
ms profundamente por el reciente desarrollo de mquinas con dinmicas no
lineales de aplicacin industrial como manipuladores, aeroplanos, submarinos,
y vehculos espaciales.
La aplicacin para la tcnica propuesta y explicada en la tesis es un manipulador de
tres grados de libertad (3 DOF) que es un proceso inherentemente no lineal, para el
cual se ha diseado un controlador no lineal multivariable de 3 entradas y 3 salidas
de tal manera que permita al robot seguir en forma asinttica una trayectoria
cualquiera continua y diferenciable donde las entradas al sistema son los voltajes de
armadura a los servomotores DC usados en las articulaciones y las salidas q
1
, q
2
y
q
3
son las posiciones del manipulador.

Para este estudio hemos organizado el trabajo en seis captulos de la siguiente
forma:
El Captulo I es denominado planteamiento del problema, se presenta la formulacin
del problema, los objetivos generales y especficos, antecedentes, el alcance del
trabajo.
El Captulo II se presenta el marco terico donde se explica la cinemtica y
dinmica de un robot, se presenta la descripcin general de la tcnica de control de
realimentacin no lineal y se presenta el fundamento terico de la tcnica de control
no lineal para procesos SISO (simple entrada y simple salida) y para procesos

MIMO (mltiple entrada y mltiple salida), las hiptesis, variables y metodologa de
la investigacin.
El Captulo III se realiza el estudio del modelado de la planta no lineal y se plantea
las ecuaciones dinmicas mediante el desarrollo del algoritmo Lagrange- Euler en
trminos de energa cintica, potencial y disipativa.
El Captulo IV se ilustra los resultados obtenidos de las simulaciones hechas en
MATLAB. Las simulaciones han sido realizadas para el control de trayectoria
constante y para control de trayectoria senoidal.
El Captulo V se presenta las especificaciones tcnicas, se define el hardware y
software para una implementacin para pruebas en tiempo real.
Finalmente se presenta las recomendaciones y conclusiones, bibliografa y los
anexos.
El Anexo 1 contiene el clculo del coeficiente de friccin viscosa del segundo
eslabn.
El Anexo 2 se presenta los programas del algoritmo de Lagrange-Euler y el
algoritmo de control no lineal, escritos en cdigo MATLAB.

CAPTULO I
PLANTEAMIENTO DEL PROBLEMA

1.1. Formulacin del Problema

Los manipuladores pueden ser construidos y modelados con muchos
cuerpos rgidos conectados en serie por articulaciones rotativas y
conducidas cada una de ellas por actuadores elctricos, que son
servomotores DC.

El robot manipulador esfrico de 3DOF est compuesto por un
servomotor DC con encoder ptico incorporado y un slido de aluminio
(disco y prisma) que vendra a ser la base para el primer grado de
libertad; otro servomotor DC tambin con encoder ptico y una varilla
rgida de aluminio para el segundo grado de libertad; y de la misma
manera otro servomotor DC tambin con encoder ptico y una varilla
rgida de aluminio que pueda cumplir la funcin de un brazo para el
tercer grado de libertad, pudindose instalar en su extremo de esta una
pinza rotrica.

El propsito de este trabajo es el control simultneo del movimiento
rotacional de la base acoplado al eje del primer servomotor DC en
posicin vertical; el otro movimiento rotacional de la primera varilla
acoplada al eje del segundo servomotor DC en posicin horizontal y
finalmente el movimiento rotacional de la segunda varilla acoplada al eje
del tercer servomotor DC en posicin horizontal.

Para este trabajo en estudio se disear un controlador no lineal MIMO
(Multiple-input-Multiple-output) de 3 entradas y 3 salidas basado en el
mtodo de linealizacin exacta por realimentacin de estado de tal
manera que el manipulador siga trayectorias de referencia arbitrarias
empleando fuerzas de control adecuada.

1.2. Antecedentes
En un esfuerzo por encontrar soluciones a los problemas relacionados a
mejorar el rendimiento de controladores para la determinacin de la
posicin y orientacin del efector final del manipulador, se han
propuesto una gran variedad de algoritmos y mtodos de control. En
general estos han sido clasificados en dos categoras:

Mtodos de control convencionales: Proporcional Derivativo,
Proporcional Integral Derivativo, Control de Torque Computado.
Mtodos avanzados de control: Control Adaptivo, Control Robusto,
Control Robusto Adaptivo, Control No lineal por mtodos deslizantes,
Control Predictivo no lineal, Control No lineal usando Backstepping y
Control con inteligencia artificial.

A continuacin se presentan algunos antecedentes relacionados al Robot
Manipulador Esfrico de 3 Grados de Libertad realizados:

En el Instituto Politcnico de Setbal, Portugal se dise un robot
antropomrfico denominado IROS, que combina una cabeza
binocular, brazo y una mano, para investigacin en coordinacin de
un visuomotor y aprendizaje por imitacin. El objetivo, producir un
sistema semejante a la cinemtica del brazo y mano. Los movimientos
del hombro de este robot fueron modelados a base de tres grados de
libertad, el codo con dos grados de libertad y la mueca del brazo
tiene un grado de libertad. La cinemtica inversa de este robot fue
realizada en dos partes: posicin de la mueca y orientacin de la
mano, los tres primeros grados de libertad sirven para posicionar la
mueca y los otros tres para orientar la mano.
Control por linealizacin exacta entrada salida de un manipulador
plano subactuado de los autores: Manuel Prez Polo, Jos Vicente
Rocamora, Jos ngel Bern Galiano, Javier Gil Chica del
Departamento de Fsica, Ingeniera de Sistemas y Teora de la Seal.
Universidad de Alicante. Escuela Politcnica Superior, Espaa. El
manipulador est formado por dos juntas rotacionales y una
prismtica, que permite al conjunto seguir trayectorias definidas en el
plano. El modelo matemtico del sistema se obtiene a travs de las
ecuaciones de Lagrange-Euler. Se comprueba que el sistema tiene una
dinmica interna estable, lo que permite disear una ley de control
PID.

En la Universidad Nacional del Altiplano, especficamente en la
Escuela Profesional de Ingeniera Electrnica, no se tiene alguna
investigacin similar. Por consiguiente, esto propicia la investigacin
en el control de un manipulador robtico de tres grados de libertad
usando la tcnica de control multivariable por realimentacin de
estado.

1.3. Objetivos de la investigacin.

1.3.1. Objetivo General
Disear el controlador no lineal multivariable por realimentacin
de estado aplicado al robot manipulador esfrico.
1.3.2. Objetivos Especficos
Analizar el modelo dinmico del Robot Manipulador
Esfrico de 3 Grados de Libertad empleando el algoritmo
Lagrange-Euler.
Aplicar la tcnica de control no lineal por realimentacin de
estado para sistemas de mltiples entradas y mltiples
salidas.
Realizar la simulacin del sistema aplicando la tcnica de
control de linealizacin exacta por realimentacin de estado
aplicado al robot manipulador esfrico.

1.4. Alcance

La culminacin del presente trabajo es un aporte ms a la Facultad de
Ingeniera Mecnica Elctrica, Electrnica y Sistemas de la UNA - Puno,
en particular a la Escuela Profesional de Ingeniera Electrnica en el
conocimiento de esta importante tcnica de control no lineal de mltiples
entradas y mltiples salidas. Para demostrar la tcnica expuesta se ha
escogido un tema de robtica ya que es una tecnologa con ms auge en
la actualidad y con mayor impacto en procesos de manufactura, ya que se
puede lograr mayor precisin en el control de trayectorias de trabajo. De
esta manera se ha escogido como aplicacin al Robot Manipulador
Esfrico de 3 Grados de Libertad adecuado para mostrar el
funcionamiento del control no lineal. As mismo queda explicado para
poder construir e implementar dicho sistema en el tiempo real ya que
permitira a la Facultad avanzar en el campo de la construccin y pruebas
de funcionamiento de manipuladores robticos con tcnicas de control no
lineal. Adems este know-how puede luego ser trasladado a alguna
aplicacin industrial para ejecutar mltiples tareas.

CAPTULO II
MARCO TERICO Y CONCEPTUAL

2. Marco Terico

2.1. Conceptos Generales

Robtica
Tcnica que aplica la informtica al diseo y empleo de aparatos que, en
sustitucin de personas, realizan operaciones o trabajos, por lo general en
instalaciones industriales.
La robtica es la rama de la tecnologa que se dedica al diseo,
construccin, operacin, disposicin estructural, manufactura y
aplicacin de los robots. La robtica combina diversas disciplinas como
son: la mecnica, la electrnica, la informtica, la inteligencia artificial,
la ingeniera de control y la fsica.3 Otras reas importantes en robtica
son el lgebra, los autmatas programables y las mquinas de estados.
Robots Manipuladores
La mayor parte de los Robots industriales actuales son esencialmente
brazos articulados. De hecho, segn la definicin del Robot Institute of
America, un robot industrial es un manipulador programable
multifuncional diseado para mover materiales, piezas, herramientas o
dispositivos especiales, mediante movimientos variados, programados
para la ejecucin de distintas tareas.
Sistema Mecnico.- El sistema mecnico est compuesto por
diversas articulaciones. Normalmente se distingue entre el brazo y el

rgano terminal o efector final que puede ser intercambiable, empleando
pinzas y/o dispositivos especficos para distintas tareas.
El aumento del nmero de articulaciones aporta mayor maniobrabilidad
pero dificulta el problema de control, obtenindose normalmente
menores precisiones por acumulacin de errores.
Actuadores.- Los actuadores generan las fuerzas o pares
necesarios para animar la estructura mecnica. Se utilizan tecnologas
hidrulicas, para desarrollar potencias importantes, y neumticas, pero en
la actualidad se ha extendido el empleo de motores elctricos, y en
particular motores de corriente continua servo controlados, emplendose
en algunos casos motores paso a paso y otros actuadores
electromecnicos sin escobillas.
Sensores y Sistemas de control.- Los sistemas de control de
Robots pueden considerarse funcionalmente descompuestos segn una
estructura jerrquica. En el nivel inferior se realizan las tareas de
servocontrol y supervisin de articulaciones. La mayor parte de los
Robots industriales actuales emplean servomecanismos convencionales
con realimentaciones de posicin y velocidad para generar seales de
control sobre los actuadores de las articulaciones.
Para mejorar las prestaciones se investiga en tcnicas para identificar
modelos suficientemente fiables de la dinmica del robot y en mtodos
de control de articulaciones que permitan compensar las no linealidades y
acoplamientos, y optimizar el comportamiento dinmico.

El segundo nivel de control se ocupa de la generacin de trayectorias,
entendiendo por tal la evolucin del rgano terminal cuando se desplaza
de una posicin a otra.
Los niveles superiores se ocupan de la comunicacin con el usuario,
interpretacin de los programas, percepcin sensorial y planificacin.

2.2. Representacin de la Posicin y Orientacin

La manipulacin de piezas llevada a cabo por un robot implica el
movimiento espacial de su extremo. Asimismo, para que el robot pueda
recoger una pieza, es necesario conocer la posicin y orientacin de est
con respecto a la base del robot. Se aprecia entonces la necesidad de
contar con una serie de herramientas matemticas que permitan
especificar la posicin y orientacin en el espacio de todas las
articulaciones que intervienen en nuestro robot.
Representacin de la posicin.- La forma ms intuitiva y
utilizada de especificar la posicin de un punto son coordenadas
cartesianas. Existen adems otros mtodos, igualmente vlidos, y
tambin ampliamente extendidos, como son las coordenadas polares para
dos dimensiones, y las cilndricas y esfricas para espacios de tres
dimensiones.

Figura 2.1: Sistema de Coordenadas Cartesianas
Representacin de la orientacin.- Un punto queda totalmente
definido en el espacio a travs de los datos de su posicin. Sin embargo,
para el caso de un slido, es necesario adems definir cul es su
orientacin con respecto a un sistema de referencia. En el caso de un
robot, no es suficiente con especificar cul debe ser la posicin de su
extremo, sino en general, es tambin necesario indicar su orientacin.
La matriz de Rotacin es el mtodo ms extendido para la descripcin de
orientaciones, debido principalmente a la comodidad que proporciona el
uso del lgebra matricial.

Figura 2.2: Sistema de Referencia para la matriz de rotacin

[
]
Matriz Homognea.- La representacin mediante coordenadas
homogneas de la localizacin de slidos en un espacio n-dimensional se
realiza a travs de coordenadas de un espacio (n+1)-dimensional.
Utilizando las coordenadas homogneas llegamos a la definicin de la
Matriz de transformacin homognea que representa la transformacin
de un vector de coordenadas homogneas de un sistema de coordenadas a
otro.
[
] [

]

2.3. Cinemtica y Dinmica de un Robot

Cinemtica de un robot.- La cinemtica de un robot estudia el
movimiento del mismo con respecto a un sistema de referencia. As la
cinemtica se interesa por la descripcin analtica del movimiento
espacial del robot como una funcin del tiempo, y en particular por las
relaciones entre posicin y la orientacin del extremo final del robot con
los valores que toman sus coordenadas articulares.
Existen dos problemas fundamentales a resolver en la cinemtica del
robot (Figura 4.1); el primero de ellos se conoce como el problema
cinemtico directo, y consiste en determinar cul es la posicin y
orientacin del extremo final del robot, con respecto a un sistema de
coordenadas que se toma como referencia, conocidos los valores de la

articulaciones y los parmetros geomtricos de los elementos del robot;
el segundo denominado problema cinemtico inverso, resuelve la
configuracin que debe adoptar el robot para una posicin y orientacin
del extremo conocidas.
Denavit y Hartenberg propusieron un mtodo sistemtico para describir y
representar la geometra espacial de los elementos de una cadena
cinemtica, y en particular de un robot, con respecto a un sistema de
referencia fijo. Este mtodo utiliza una matriz de transformacin
homognea para describir la relacin espacial entre dos elementos rgidos
adyacentes, reducindose el problema cinemtico directo a encontrar una
matriz de transformacin homognea 4x4 que relacione la localizacin
espacial del extremo del robot con respecto al sistema de coordenadas de
su base.
Por otra parte, la cinemtica del robot trata tambin de encontrar las
relaciones entre las velocidades del movimiento de las articulaciones y
las del extremo. Esta relacin viene dada por el modelo diferencial
expresado mediante la matriz Jacobiana.
Valor de las

Figura 2.3: Diagrama de relacin entre cinemtica directa e inversa.

El problema cinemtico directo:
Para representar y describir la localizacin de un objeto en el espacio
tridimensional con respecto a un sistema de referencia fijo se utiliza
fundamentalmente el algebra vectorial y matricial. Dado que un robot se
puede considerar como una cadena cinemtica formada por objetos
rgidos o eslabones unidos entre s mediante articulaciones, se puede
establecer un sistema de referencia fijo situado en la base del robot y
describir la localizacin de cada uno de los eslabones con respecto a
dicho sistema de referencia. De esta forma el problema cinemtico
directo se reduce a encontrar una matriz homognea de transformacin T
que relacione la posicin y orientacin del extremo del robot respecto del
sistema de referencia fijo situado en la base del mismo. Esta matriz T
ser funcin de las coordenadas articulares.

2.4. Dinmica de un Robot

La dinmica se ocupa de la relacin entre fuerzas que actan sobre un
cuerpo y el movimiento que en l se origina. Por lo tanto, el modelo
dinmico de un robot tiene por objetivo conocer la relacin entre el
movimiento del robot y las fuerzas implicadas en el mismo.
Esta relacin se obtiene mediante el denominado modelo dinmico, que
relaciona matemticamente:
1. La localizacin del robot definida por sus variables articulares o por
las coordenadas de localizacin de su extremo y sus derivadas: velocidad
y aceleracin.

2. Las fuerzas y pares aplicados en las articulaciones (o en el extremo del
robot).
3. Los parmetros dimensionales del robot, como longitud, masas e
inercias de sus elementos.
La obtencin de este modelo para mecanismos de uno o dos grados de
libertad no es excesivamente compleja, pero a medida que el nmero de
grados de libertad aumenta, el planteamiento y obtencin del modelo
dinmico se complica enormemente. Por este motivo no siempre es
posible obtener un modelo dinmico expresado de una forma cerrada,
esto es, mediante una serie de ecuaciones, normalmente de tipo
diferencial de 2 orden, cuya integracin permita conocer que
movimiento surge al aplicar unas fuerzas o que fuerzas hay que aplicar
para obtener un movimiento determinado. El modelo dinmico debe ser
resuelto entonces de manera iterativa mediante la utilizacin de un
procedimiento numrico.
El problema de la obtencin del modelo dinmico de un robot es, por lo
tanto, uno de los aspectos ms complejos de la robtica, lo que ha llevado
a ser obviado en numerosas ocasiones. Sin embargo, el modelo dinmico
es imprescindible para conseguir los siguientes fines:
1. Simulacin del movimiento del robot.
2. Diseo y evaluacin de la estructura mecnica del robot.
3. Dimensionamiento de los actuadores.
4. Diseo y evaluacin del control dinmico del robot.
Este ltimo fin es evidentemente de gran importancia, pues de la calidad
del control dinmico del robot depende la precisin y velocidad de sus

movimientos. La gran complejidad ya comentada existente en la
obtencin del modelo dinmico del robot, ha motivado que se realicen
ciertas simplificaciones, de manera que as pueda ser utilizado en el
diseo del controlador.
Es importante hacer notar que el modelo dinmico completo de robot
debe incluir no slo la dinmica de sus elementos (barras o eslabones)
sino tambin la propia de sus sistemas de transmisin, de los actuadores y
sus equipos electrnicos de mando. Estos elementos incorporan al
modelo dinmico nuevas inercias, rozamientos, saturaciones de los
circuitos electrnicos, etc. Aumentando an ms su complejidad.
Por ltimo, es preciso sealar que si bien en la mayor parte de las
aplicaciones reales de la robtica, las cargas e inercias manejadas no son
suficientes como para originar deformaciones en los eslabones del robot,
en determinadas ocasiones no ocurre as, siendo preciso considerar al
robot como un conjunto de eslabones no rgidos, Aplicaciones de este
tipo pueden encontrarse en la robtica espacial o en robots de grandes
dimensiones, entre otras.
2.4.1. Modelo Dinmico
Para determinar la ecuacin dinmica del manipulador existen
varios mtodos tales como el mtodo de Newton-Euler, el mtodo
de Lagrange-Euler, entre otros. Para el manipulador en estudio
usaremos el mtodo de Lagrange-Euler que emplea la formula
Lagraniana conjuntamente con la representacin de Denavit-
Hartenberg (D-H). Un completo estudio de dicho procedimiento
puede ser encontrado en [1], [2], [4], [7]. En la figura 2.5 se

muestra los sistemas de referencias usados para la representacin
D-H.

Figura 2.4: Esquema con las medidas del Manipulador
2.4.2. El procedimiento de Denavit-hartenberg
Un manipulador robtico est compuesto de una secuencia de
cuerpos rgidos (los eslabones) articulados por junturas
rotacionales o prismticas. Cada par de articulacin-eslabn
representa un grado de libertad (DOF). Un sistema de
coordenadas ortonormales (x
si
,y
si
,z
si
) puede ser establecido
usndola regla de la mano derecha para cada articulacin i, donde
i = 1,,n. As un manipulador con n DOF posee n pares de
articulaciones. El ltimo sistema de coordenadas (x
sn
,y
sn
, z
sn
)

constituye el sistema de coordenadas del efector final, el cuales
usualmente articulado al ltimo eslabn. El manipulador es unido
a una base de referencia 0. El sistema de coordenada base es
definido como (x
so
, y
so
, z
so
).las coordenadas de la base son
tambin las coordenadas inerciales del manipulador. El sistema de
coordenadas base aumentado es definido como (x
so
, y
so
, z
so, 0
).

Figura 2.5: Sistemas de referencia del Manipulador

El procedimiento D-H origina una matriz de transformacin
homognea que representa el sistema inercial del manipulador.
Para describir tal matriz, requerimos establecer el sistema de
coordenadas (x
si
, y
si
, z
si
) del manipulador en s
i
= 0, ,n, para
determinar los parmetros de las articulaciones y eslabones con el
fin de desarrollar la matriz de transferencia homognea.
Estableciendo el Sistema de Coordenadas D-H
Para establecer el sistema de coordenadas D-H (x
si
, y
si
, z
si
), s
i
= 0,
,n, se usa las siguientes reglas:
1. El sistema de coordenadas base (x
so
, y
so
, z
so
) se determina
usando la regla de la mano derecha. El eje z
so
se escoge
libremente y est localizado a lo largo del eje del movimiento del
primer eslabn.
2. El eje z
si-1
es alineado con el eje del movimiento (giratorio
o prismtico) de la i-sima articulacin.
3. El origen del sistema se coordenadas s
i
-sima est en la
interseccin del eje z
si
y z
si-1
o en la interseccin de la normal
comn entre los ejes de dos articulaciones consecutivas z
si
y z
si-1
y
el eje z
si
.
4. El eje x
si
es perpendicular l eje z
si-1
.
5. Aplicar la regla de la mano derecha en la articulacin i-
sima para determinar y
si
.
6. Generalmente, la articulacin n-sima es una articulacin
giratoria. El sistema de coordenadas (x
sn
, y
sn
, z
sn
), puede estar en

cualquier parte del efector final con tal que el eje z
sn
est a lo largo
de la direccin del eje z
si-1
y apuntado hacia fuera del
manipulador. El eje x
sn
es perpendicular a ambos ejes: z
sn
y

z
si-1
.
Parmetros D-H
Los cuatro parmetros geomtricos del manipulador (
i
, d
DHi
, a
DHi
,
y
i
), se asocian con cada par articulacin-eslabn y son
requeridos para la descripcin de una articulacin giratoria o
prismtica. Despus de establecer es sistema de coordenadas del
manipulador tales parmetros pueden ser determinados de la
siguiente manera:
1.
i
, es el ngulo de rotacin del eje x
si-1
con el eje x
si
en el
plano del eje z
si-1
. Este es variable si la articulacin es giratoria.
2. d
DHi
, es la distancia del origen del sistema de coordenadas
s
(i-1)
-sima a lo largo del eje z
si-1
, hasta la interseccin del eje z
si-1

con el eje x
si
. Esta distancia d
DHi
es variable si la articulacin i es
prismtica.
3. a
DHi
, es la distancia a lo largo del eje x
si
, que va desde la
interseccin del eje z
si-1
con el eje x
si
hasta el origen del sistema de
coordenadas s
i
-sima en el caso de articulaciones giratorias.
4.
i
, es el ngulo de separacin entre los ejes z
si-1
y z
si

medido en un plano perpendicular al eje x
si
, utilizando la regla de
la mano derecha.

Matriz de Transformacin Homognea D-H
La matriz de transformacin homognea D-H caracteriza cada
sistema de coordenadas del par de articulacin-eslabn con
respecto al sistema de coordenadas previo. Por consiguiente un
punto r
si
del sistema de coordenadas s
i
-simo se puede expresar
como (r
si-1
) en el sistema de coordenadas s
(i-1)
-simo ejecutando
sucesivamente las siguientes operaciones:
1. T(z
s
,
i
): rotacin de un ngulo
i
alrededor del eje z
si-1
,
para alinear el eje x
si-1
,

con el eje
2. T(z
s
, d
DHi
): traslacin a lo largo del eje z
si-1
de una distancia
d
DHi
, hasta hacer coincidir al eje z
si-1
con el eje z
si.

3. T(x
s
, a
DHi
): traslacin a lo largo del eje x
si
de una distancia
a
DHi
, que desde la interseccin del eje z
si-1
y el eje x
si
al origen del
sistema de coordenadas s
i
-simo.
4. T(x
s
,
i
):rotacin de un ngulo
i
, del eje z
si-1
con el eje z
si

alrededor del eje x
si
.
El producto de estas cuatro operaciones bsicas produce la matriz
de transformacin homognea A
i
i-1
, del eslabn i con respecto al
eslabn i-1 o articulacin i con respecto a la articulacin i-1. Por
lo tanto:

] [

]
[

]

[

]
Donde c y s, son las funciones cos y sen respectivamente. La
siguiente matriz de transformacin homognea:

Determina la localizacin del sistema de coordenada (s
i
)-sima
con respecto al sistema de coordenada base.
2.4.3. Algoritmo de Lagrange-Euler
El procedimiento de lagrange-Euler requiere ejecutar los
siguientes pasos:
1. Asignar a cada para articulacin-eslabn del manipulador un
sistema de coordenadas D-H.
2. Obtener las matrices de transformacin homognea
para i =
1,,n.
3. Obtener las matrices U
ij
que es el efecto del movimiento de la
articulacin j en todos los puntos del eslabn i:

Donde
relaciona al sistema de coordenadas (j - 1)-simo con

el sistema de coordenadas base, y
toma los siguientes valores:

] Si la articulacin es giratoria

] Si la articulacin es prismtica
4. Los efectos de interaccin entre las articulaciones son expresadas
por las siguientes matrices U
ijk
:
){

5. Obtener el tensor de inercia Ji del eslabn i-simo mediante:

Donde: (
) es el centro de gravedad (CG) del eslabn i

con respecto al sistema de coordenadas si-simo,

son los momentos inercia con respecto al sistema de coordenadas
(
),
es la masa del cuerpo i,
son las
distancias de CG del cuerpo i al sistema de coordenadas
(
), e
son los productos

correspondientes del momento de inercia.

6. Obtener la matriz H de inercia simtrica de dimensin , la
cual est relacionada con la aceleracin y cuyos elementos se
expresan como:

Donde, es la traza y es el nmero de grados de libertad.
7. Obtener los trminos h
ikm
definidos por:

8. Obtener el vector no lineal fuerza centrifuga y Coriolis de orden n
x 1 cuyos elementos C
i
son dados por:

9. Obtener el vector no lineal de fuerza de gravedad d de orden
cuyos elementos son:

Donde, *
+ es el vector gravedad fila expresado

en el sistema de coordenadas base y
(el
elemento 1 es un factor de escala) es el vector centro de masa del
eslabn j y expresado en el sistema de coordenadas
-simo.
Finalmente la ecuacin dinmica del manipulador, con el mtodo
L-E toma la forma:

Donde,
es el vector de fuerzas y torques aplicado a cada

coordenada generalizada
.

2.5. Realimentacin No Lineal
Luego de formulado el problema de control a resolver, el siguiente paso es encontrar un
modelo que sea capaz de capturar la dinmica del proceso en el rango de operacin. En
realidad todos los procesos son no lineales y muchas veces resulta ms conveniente
trabajar directamente con el modelo no lineal. As podemos disear sistemas de control
no lineal empleando diversas tcnicas tales como: El mtodo directo de Liapunov,
control deslizante, control adaptivo y linealizacin de la realimentacin.
La idea fundamental del control de procesos no lineales empleando la tcnica de
linealizacin por realimentacin, es linealizar (hallar un modelo lineal equivalente) el
proceso (parcial o totalmente, si fuera posible) mediante un cambio adecuado de
coordenadas y una realimentacin del vector de estado de dicho proceso, de modo tal
que el proceso resultante nos permita aplicar tcnicas de control lineal, o bien, atacar los
problemas de control con un grado de dificultad menor.
2.5.1. Herramientas Matemticas
Las herramientas matemticas a discutir corresponden a la geometra diferencial y a la
topologa. Solo se tratan los puntos directamente involucrados con la tcnica de
linealizacin por realimentacin.
La notacin:

Indica que a cada funcin vectorial f le corresponde un campo vectorial en el espacio n-
dimensional
. Los campos vectoriales de inters para nosotros son los denominados

suaves. Un campo vectorial f(x) es suave cuando tal funcin vectorial posee infinitas
derivadas parciales continuas.
Si es una funcin escalar de variable vectorial suave, donde x es el vector de
estado, entonces su gradiente es:

]
El Jacobiano de f(x) tiene la forma:

2.5.1.1.Derivadas y Corchete de Lie
Derivada de Lie
Definicin: Dada una funcin escalar suave
y un campo vectorial suave

, nosotros podemos definir una nueva funcin escalar

denominada la derivada de Lie, o simplemente la derivada direccional de a lo largo
de la direccin del vector f.
Sucesivas derivadas de Lie pueden ser definidas recursivamente
) (
)
Del mismo modo, si g es otro campo vectorial, la funcin escalar
es:

Corchete de Lie
Definicin: Sean f y g dos campos vectoriales en
. El corchete de Lie de f y g se
define como un tercer campo vectorial de la forma:
[ ]

Observar que al corchete de Lie se le denota tambin como
, donde significa
adjunta. Sucesivos corchetes de Lie se pueden definir recursivamente:
]
Los corchetes de Lie satisfacen las siguientes propiedades:
1) Bilinealidad
[
]
[
]
2) Anticonmutatividad
[ ] [ ]
3) Identidad de Jacobi

Donde
son campos vectoriales,
son constantes escalares y

es una funcin diferenciable.

2.5.1.2.Difeomorfismo y Transformacin de estados
El concepto de difeomorfismo, que puede ser visto como una generalizacin del
concepto familiar de transformacin de coordenadas, es el siguiente:
Definicin: Una funcin :
definida en una regin , se denomina un

difeomorfismo si y su inversa son suaves. Si =
, entonces (x) es un
difeomorfismo global. Si (x) se define como una regin finita y para un punto dado,
entonces el difeomorfismo es local.
El siguiente Lema permite determinar difeomorfismos locales.
Lema: Sea (x) un funcin suave definida en una regin en
. Si el jacobiano
es no singular en un punto
de , entonces
define un difeomorfismo local

en una regin de .
2.5.1.3.Transformacin no lineal de estados
Un difeomorfismo puede ser usado para transformar una representacin no lineal en
otra, en trminos de un nuevo vector de estado. Consideremos que el proceso no lineal
sea descrito por la siguiente representacin en el espacio de estado:
()
()
Donde [
es el vector de control (la variable manipulada) del

proceso de orden m, [
es el vector de estado del proceso de

orden n, [
es el vector de salida del proceso (la variable

controlada) de orden p y las funciones [
, [
y
[
corresponden a campos vectoriales infinitamente diferenciables.

Asumamos la siguiente transformacin no lineal de estados:
[
]
Donde la funcin de transformacin es invertible y tanto como su inversa
son
infinitamente diferenciables. Notar que la transformacin dada en la ecuacin (2.17) Es
un difeomorfismo. Mientras que la propiedad de inversin de permite un retorno a los
estados originales, la propiedad de diferenciacin garantiza que la descripcin del
proceso con las nuevas coordenadas sea tambin infinitamente diferenciable, tal como
lo es el proceso original.
Empleando la transformacin en la ecuacin (2.16) Obtenemos:
[
]

[
]

() [
()

Donde es un punto de equilibrio para el cual la matriz jacobiana es no singular, lo que
significa que su inversa existe. Luego, podemos obtener la siguiente representacin no
lineal para el vector de estado z, como sigue:

()
()
()

Observar que (2.19) Constituye un nuevo sistema no lineal con un vector de estado z.
2.5.1.4. El teorema de Frobenius
El teorema de Frobenius es una herramienta importante para el tratamiento formal de la
linealizacin por realimentacin de procesos de orden n. Este teorema proporciona las
condiciones necesarias y suficientes para resolver una clase especial de ecuaciones con
derivadas parciales. Antes de presentar el teorema en cuestin se requiere de las
siguientes definiciones:
Definicin: Se dice que un conjunto linealmente independiente de campos vectoriales
[
] en
es completamente integrable, si y solo si existen (n-m) funciones

escalares h
1
(x), h
2
(x),,h
n-m
(x) que satisfacen el siguiente sistema de ecuaciones
diferenciales parciales:

Donde . En adicin, los gradientes h
i
son tambin
linealmente independientes.
La definicin anterior implica la existencia de n(m-n) ecuaciones diferenciales parciales
de la forma
.
Definicin: Se dice que un conjunto linealmente independiente de campos vectoriales
[
] en
es involutivo, si y solo si existen funciones escalares
tales que:
[ ] [ ]

Ya vimos que la notacin [f, g] es conocida como el corchete de Lie. Partiendo de la
definicin anterior se puede deducir que:
1) Los campos Vectoriales constantes, es decir, si f
k
y g
k
son constantes para
cualquier , son siempre involutivos puesto que:
[ ]

2) Un conjunto compuesto por un solo campo vectorial f es involutivo porque
[ ] .
3) Si un conjunto de campos vectoriales [
] es involutivo, implica que para

todo i, j:
[
] *
]+
Ahora si podemos formular el teorema de Frobenius:
TEOREMA (Frobenius): Sea [
] un conjunto de campos vectoriales

linealmente independientes. Se dice que tal conjunto es integrable si y slo si es
involutivo.
2.5.2. Grado Relativo
Dado el siguiente proceso no lineal de una entra y una salida:

Se desea obtener una relacin directa entre la entrada u y la salida y. Para ello
derivamos la salida como sigue:

Si
, entonces se obtiene la relacin deseada entre y ; en caso contrario,

debemos seguir derivando la salida remanente
como se muestra a
continuacin:

Nuevamente, si
, entonces se tiene la relacin deseada. En caso contrario,

se debe seguir derivando la salida hasta que en la r-sima derivada la entrada u aparezca
en forma explcita, como sigue:

Definicin: El grado relativo del proceso univariable no lineal dado en la ecuacin
2.20, es el nmero de veces r que hay que derivar la salida hasta obtener:

Para todo x que pertenezca a una regin de
.
2.5.3. Forma Normal de Procesos No lineales
La tcnica de linealizacin por realimentacin requiere de una representacin no lineal
bastante til del proceso a controlar; la forma normal. La transformacin no lineal de

coordenadas que permite llevar un proceso no lineal de una entrada y una salida a su
forma normal, involucra los trminos
. Por consiguiente, tal

transformacin depende del grado relativo r del proceso. Empezaremos por el caso
, donde n es el orden del proceso. La siguiente transformacin de coordenadas:
[
] [
]
Con x [
, cumple con las condiciones de una transformacin no lineal;

es decir, es un difeomorfismo basado en la hiptesis de que el orden del sistema es
. Por consiguiente, dicha transformacin puede llevar al proceso descrito en la
ecuacin (2.20) a la forma siguiente:
[ ]

[ ]

Notar que en la ecuacin anterior, por definicin, ya que en este caso .
Cuando el proceso no lineal posee rango , entonces el cambio de coordenadas
desarrollado anteriormente conduce a una transformacin parcial, en vista de que slo
se pueden definir r funciones de la forma
linealmente independientes.
Sin embargo, es posible agregar funciones de la forma
, de modo tal que sea un difeomorfismo. Con ello, la

forma normal del proceso 2.20 Resulta en la siguiente representacin en el espacio de
estado:

Donde
son funciones suaves que dependen del nuevo estado z y no de

la entrada manipulada u.
2.5.4. Linealizacin Exacta y la Ley de Control
La linealizacin exacta se refiere a tratar con procesos no lineales de grado . En
tales procesos es posible determinar una adecuada realimentacin no lineal que
conduzca a una linealizacin exacta del proceso original. El proceso puede ser
univariable (con una entrada y una salida) o multivariable (con mltiples entradas y
mltiples salidas).
Hemos visto que un cambio de coordenadas de la forma , descrito en la
ecuacin (2.23) y aplicado al proceso no lineal de la ecuacin (2.20), produce un
conjunto de nuevas coordenadas (la nueva representacin en el espacio de estado) que
poseen la forma dada en (2.24), ecuacin que volvemos a reescribir a continuacin:

Si se define una seal de realimentacin u (la ley de control no lineal) de la forma:

[ ]
Donde es una nueva entrada, entonces el nuevo conjunto de ecuaciones viene a ser:

Que en forma compacta resulta en la siguiente representacin lineal en el espacio de
estado:

Donde:

[
[
]
Observar que la nueva entrada del proceso lineal es v. La figura 1 Muestra el proceso de
linealizacin descrito hasta ahora. El proceso lineal de la ecuacin (2.29) Es
completamente controlable y completamente observable, ya que sus matrices de
controlabilidad [
] y de observabilidad [
] poseen
rango n (rango completo). Por otra parte, el proceso resultante es un integrador de orden
n debido a que su funcin de transferencia resulta:

[ ]

Figura 1: Linealizacin por realimentacin
Respecto al proceso de linealizacin desarrollado, debemos remarcar lo siguiente:
El sistema de lazo cerrado resultante es lineal, completamente controlable y
completamente observable. Por consiguiente, el problema de controla resolver,
ya sea regulacin o de seguimiento de trayectorias, queda resuelto diseando v
con cualquier tcnica de control lineal conocida.
En general, la realimentacin no lineal requiere de todo el vector de estado z, por
lo que es necesario que todos sus elementos sean medibles. De lo contrario,
estamos obligados a estimarlos empleando un observador no lineal.
Condiciones para Linealizacin Exacta
La linealizacin desarrollada anteriormente fue posible debido a la existencia de una
funcin de salida h(x) respecto a la cual el proceso no lineal original tiene grado relativo
. Resaltemos que un proceso no lineal univariable como el de la ecuacin 2.20
Posee grado cuando la funcin h(x), para un entorno alrededor del punto de
operacin , satisface:

Las condiciones para linealizacin exacta pueden ahora ser rigurosamente formalizadas.
TEOREMA: El proceso univarible no lineal de la ecuacin (2.20):

Donde f(x) y g(x) son campos vectoriales suaves, es linealizable si y slo si existe una
regin en donde se cumplen las condiciones siguientes:
Los campos vectoriales:
[
]
Son linealmente independientes en , lo que equivale a decir, que el rango de la matriz
resultante [.] debe ser igual al orden n del proceso.
El conjunto:
[
]
Es involutivo en .
La primera condicin puede ser interpretada como una condicin de controlabilidad
dado que los campos vectoriales [
] aplicados a procesos lineales se

convierten en la conocida matriz de controlabilidad [
]. El rango de esta
matriz debe ser n para que el proceso lineal sea completamente controlable. Sin
embargo, puede ocurrir que un sistema no lineal sea controlable, mientras que su
modelo linealizado no lo es. Por ello, la primera condicin puede considerarse como
una generalizacin de la condicin de controlabilidad.
La condicin involutiva es menos intuitiva. Para sistemas lineales, tal condicin se
satisface trivialmente ya que los campos vectoriales son constantes. Pero, en general, no
se puede decir lo mismo cuando se trata con procesos no lineales.
2.5.5. Observadores No lineales

El estudio de la observabilidad en sistemas lineales tiene su base en la matriz de
observabilidad, que para sistemas univariables no forzados de la forma:

Se define como:
[
]
El caso no lineal y no forzado que nos ocupa posee la descripcin:

Cuya correspondiente matriz de observabilidad generalizada es:

Es fcil demostrar que si y , las matrices de observabilidad de la
ecuaciones (2.33) Y (2.35) se igualan. Supongamos que la matriz O(x) posea inversa
para todo x. En tal caso, la transformacin definida por:

Representa un cambio de coordenadas. Empleando los resultados anteriores y en
analoga con la ecuacin (2.26); la descripcin en el espacio de estado para las nuevas
coordenadas (notar que g=0) es:

[

Que posee la forma matricial siguiente:

Donde:

[
[
]
Se puede demostrar que la matriz de observabilidad del proceso descrito en la ecuacin
(2.38) es la matriz identidad. Por consiguiente, tal proceso es completamente
controlable. Conforme a las relaciones (2.34), (2.35) y (2.36) se tiene que:

El observador a disear debe estimar el vector de estado z; es decir, debe determinar su
estimado empleando las mediciones a la entrada y a la salida del proceso. Haciendo
una extensin de los bien conocidos observadores lineales, para el observador no lineal
podemos considerar la dinmica siguiente:
[ ]

Donde observamos que el trmino [ ] es proporcional al error entre la salida del
proceso y la salida del observador . Para obtener la expresin del observador para los
estados del proceso no lineal, empleamos la versin estimada de la ecuacin (2.40)
Que nos permite despejar el vector de estado estimado como sigue:

Y usando la relacin (2.41) Obtenemos:
[ ]}

{ [ ]}

[ ]
Que es la ecuacin del observador no lineal para el sistema no forzado
con condicin inicial , siempre que
exista. Notar que tambin hemos

usado el hecho de que:

Para el caso de procesos no lineales forzados con grado relativo , y teniendo en
cuenta la transformacin acostumbrada , entonces el sistema con las nuevas
coordenadas resulta:
[
)]
Luego, por extensin, la dinmica del observador para el sistema forzado:

Tiene la forma:

) [ ]
Que debe cumplir con los requerimientos siguientes: que el proceso posea grado
relativo , que su matriz de observabilidad sea invertible y que la entrada u sea
uniformemente acotada. La ecuacin del observador para el sistema original toma la
forma siguiente:
[ ]
Para una condicin inicial .
Calculo de la matriz de Ganancia L del Observador
Dado que el par [ ] dado en (2.39) es completamente observable, una forma de
obtener L es aplicar la conocida frmula del observador lineal:

]
Donde n es el orden del proceso, y:
[
]

[

Los parmetros
, se obtienen de la ecuacin:
| |

Mientras que los parmetros
, se determinan a partir de las races deseadas

del observador
Estas races conforman la ecuacin caractersticas del

observador:

Y deben tener parte real negativa para garantizar la estabilidad del observador no lineal.
2.5.6. Procedimiento de Diseo
El procedimiento para disear el sistema de control no lineal basado en la tcnica de
linealizacin por realimentacin es como sigue:
1) Formular el problema (definir especificaciones de diseo).
2) Determinar el modelo no lineal continuo del proceso.
3) Determinar el grado relativo del modelo no lineal continuo del proceso.
4) Comprobar que se cumplen las condiciones de linealizacin exacta del modelo
no lineal continuo del proceso.
5) Determinar la forma normal del proceso no lineal.
6) Implementar la linealizacin por realimentacin de estado y la ley de control no
lineal.
7) Disear el observador no lineal de estados (si fuera necesario).
8) Simular el sistema de control no lineal con el propsito de predecir un
rendimiento satisfactorio.
9) Implementar el hardware.
10) Implementar el software de control.
11) Realizar pruebas de funcionamiento.

2.5.7. Herramienta de la Geometra Diferencial
Los mtodos de geometra diferencial son principalmente
aplicados a procesos no lineales para determinar si tales procesos
son lineales por realimentacin de estado; esto es si la descripcin
dinmica de un proceso en particular puede ser linealizada
despus de una transformacin adecuada de coordenadas y la
realimentacin del vector de estado al proceso no lineal. De
acuerdo a la terminologa usada en la geometra diferencial,
llamaremos a una funcin vectorial dimensional:
[
] [
]
Un campo vectorial, si para cada uncin vectorial f le corresponde
un campo de vectores de un espacio n dimensional R
n
.
Posteriormente , solo trataremos con campo vectoriales suaves de
la forma f(x), lo que significa que tales vectores poseen derivadas
parciales continuas de cualquier orden. Suavidad tambin se
aplica a funciones.
El gradiente de una funcin escalar suave h(x) es definida como:

+
Los elementos deh, son:

Similarmente, el Jacobiano de f(x), de elementos:
, es definido como:

2.5.7.1. Derivada de Lie y Corchete de Lie
La derivada de Lie es una funcin escalar h(x) con
respecto a un campo vectorial f(x) es una nueva
funcin escalar L
f
h definida como:

Obsrvese que la derivada de Lie es el producto
interno entre y f(x). Las derivadas de Lie
repetidas pueden ser formuladas en forma
recursiva:
) (
)
Si f y g son campos vectoriales, luego:

Corchete Lie

El corchete de lie de dos campos vectoriales f y g
es otro campo vectorial [f, g], definido como:
[ ]
La notacin ad
f
g es frecuentemente usado en vez
de [f,g]. Los corchetes de Lie repetidos pueden ser
formulados de forma recursiva:
]
Si f,f
1
,f
2
,g,g
1
,g
2
, son campo vectoriales suaves,
1
y
2
son constantes escalares y h(x) es una funcin
escalar suave de x, podemos enunciar las siguiente
propiedades del corchete de Lie:
1) Bilinealidad:
[
]

[
]

2) Anticonmutatividad:
[ ] [ ]
3) Identidad de Jacobi:

2.5.7.2. Difeomorfismo y Transformacin de
Coordenadas
Una funcin definida en una regin es
conocida como un difeomorfismo si ambos, y su
inversa
-1
son funciones suaves. Si la regin es
igual a R
n
(el espacio total), entonces (x) es un
difeomorfismo global. Por otro lado un
difeomorfismo local es definido solo en un campo
finito de un punto dado x
o
.
Una funcin suave (x) definida en una regin
en R
n
es un difeomorfismo local si la matriz
Jacobiana x es no singular en un punto

de la regin .
Empleando un difeomorfismo, podemos
transformar los estados de un proceso no lineal
dentro de un nuevo conjunto de estados
correspondientes a la transformacin de un proceso
no lineal. Consideremos un proceso no lineal SISO
(single-input-single-output) descrito por:

Donde la entrada simple es u y la salida simple es
. Definiremos ahora un nuevo conjunto de estados
(x). Ejecutando la diferenciacin en z:

Donde:

2.5.7.3. Teorema de Frobenius
Sea f
1,
f
2
,,f
m
un conjunto de campos vectoriales
linealmente independiente en R
n
. El Teorema de
Frobenius establece que tal conjunto es
completamente integrable s u solo s es involutivo,
segn se explica posteriormente.
El conjunto de campos vectoriales definidos
anteriormente completamente integrable s y slo s
existen (n - m) funciones escalares h
1
(x),
h
2
(x),,h
n-m
(x), que satisfacen el siguiente sistema
de ecuaciones diferenciales parciales:

Donde los gradientes h
1,
h2hn-m, son
tambin linealmente independientes.
El conjunto de campos vectoriales considerado es
llamado a ser involutivo s y slo s hay funciones

escalares
ijk
en R (el espacio unidimensional)
satisfaciendo:
[

La ultima ecuacin indica que el campo vectorial
resultante [
] es una combinacin lineal del

conjunto original de campos vectoriales f
1,
f
2
,,f
m
.
De (3.5), tenemos los siguientes postulados:
1) Los campos vectoriales constantes son siempre
involutivos porque el corchete de Lie para dos
vectores f y g resulta [f,g] = 0, el cual puede ser
representado como una combinacin lineal de los
campos vectoriales f
1,
f
2
,,f
m
.
2) Un conjunto compuesto por un solo campo
vectorial f es involutivo porque [f,g]= 0.
3) El conjunto de los campos vectoriales f
1,
f
2
,,f
m
es
involutivo si :
[
] *
]+
Donde la notacin [ ] representa ell rango de la matriz
formada por la columna de los campos vectoriales

2.5.8. Linealizacin por Realimentacin para Procesos SISO
Esta seccin presenta la metodologa para disear controladores
estables basados en la linealizacin entrada-salida de procesos

SISO no lineales, [5], [10], [11], [12], los cuales pueden ser
descritos por:

[
] [
] [
]
Donde x es el vector de estado de orden , u es la seal de
control o entrada, y es la salida del proceso, f y g son campos
vectoriales suaves de orden y h es una funcin suave que
describe la salida.
2.5.8.1. Condiciones de Linealizacin
El proceso no lineal descrito en la ecuacin (3.6) es
linealizable s y slo s las siguientes condiciones
son verdaderas para una regin :
1) Los campos vectoriales g, ad
f
g, ,
, son
linealmente independientes en .
2) El conjunto g, ad
f
g,,
es involutivo en
Para procesos lineales SISO:

Donde A de orden y B de orden son
las matrices de estado y control (o distribucin)
respectivamente. La primera condicin arriba
mencionada, puede ser interpretada como la

condicin e controlabilidad de un proceso lineal de
la forma:

El sistema (3.7) es controlable si su matriz de
controlabilidad posee rango total; es decir:
[
]
Por consiguiente la primera condicin descrita
arriba puede ser interpretada como la Condicin de
Controlabilidad Generalizada para procesos no
lineales
2.5.8.2. Grado Relativo del Proceso
Diferenciando la salida de (3.6):

Si
para algn
, entonces la
siguiente transformacin de entrada:

Genera una relacin diferencial lineal entre la salida y
la nueva entrada v de la forma:

Caso contrario si
para todo x, la
diferenciacin de resulta:

Si
para todo x, necesitamos

diferenciar y as sucesivamente
como sigue:

Hasta que para algn entero y para algn

En esta situacin, sustituyendo la ley de control:

En:

Genera una relacin diferencial lineal entre la
salida y la nueva entrada v de la forma:

Por lo tanto, un proceso SISO posee un grado
relativo r en un punto x
0
de una regin , si para
todo x :

Obsrvese que r es el nmero de diferenciaciones
ejecutadas en la salida . En general (donde
n es el orden del proceso). Si , la
linealizacin entrada-salida se denomina exacta.
2.5.8.3. Forma Normal SISO para Linealizacin Exacta
El proceso no lineal (3.2) puede ser transformado a
una forma normal usando los resultados
establecidos en la seccin anterior. La lienalizacin
exacta de un proceso no lineal descrito por (3.2)
ocurre cuando el grado relativo r es igual al orden
n del proceso. Para linealizacin exacta, la
transformacin de estado:
[
] [
] [
] [
]
Origina la siguiente ecuacin de estado:

[

Donde:

)

2.5.8.4. Ley de Control SISO para Linealizacin Exacta
Consideremos la siguiente ley de control
linealizada:

[ ]
Donde v es una nueva entrada a ser determinada,
y son dadas por (3.23), y z es el estado
linealizado. Sustituyendo (3.14) en (3.12),
llegamos a la representacin cannica no lineal
invariante en el tiempo:

Donde:

[
[
]
La funcin de transferencia del proceso linealizado
es:
[ ]

Donde s es el operador de Laplace. La entrada v
puede ser diseada empleando la tcnica por

ubicacin de polos en el sistema lineal equivalente
dado en (3.15).
Luego, la entrada real u se calcula usando (3.14).
empleando (3.16), el sistema lineal equivalente se
puede describir como:

Donde es la trayectoria deseada e
es la seal de error de seguimiento. La
ecuacin caracterstica del sistema lineal
equivalente es hallada de:

Para un comportamiento asintticamente estable,
con para , ninguno de las
races de (3.18) debe ser localizado en el lado
derecho del plano s, incluyendo el eje imaginario.
Los parmetros del controlador
son
seleccionados para encontrar tales requerimientos.
La entrada v toma la forma (tener presente que
):

Donde:
[
] [
]
[

2.5.9. Linealizacin Por Realimentacin para Procesos MIMO
2.5.9.1. Modelando Procesos Cuadrados No Lineales
Un proceso no lineal MIMO [11], es llamado
cuadrado cuando el nmero de entradas del
proceso es igual al nmero de salidas del proceso.
El proceso cuadrado a ser considerado se describe
como:

Donde:
|
| |
| |
|
|
| |
| |
|
Donde x es el vector de estado de orden n 1, u es
el vector de control de entrada (de componentes u
i
)
de orden m1, y es la salida del vector (de

componentes y
i
) de orden m1, f es un campo
vectorial suave de orden n1, h es un campo
vectorial suave de orden m 1, y G es una matriz n
m cuyas columnas son los campos vectoriales g
j

de orden n 1. Los componentes de g y h son
funciones suaves.
2.5.9.2. Grado Relativo Total
El proceso cuadrado descrito por (3.20) posee un
grado relativo total

Si:

1) Para todo x alrededor de x
o
:

2) La siguiente matriz m m es no singular en
x = x
o
:

|
|
|
|
(2.41)
Observando las filas de A, podemos decir que cada
entero
est relacionado con la i-esima salida del

proceso. Tambin notamos que
es el mnimo
nmero de diferenciaciones ejecutadas en la salida
, requeridas para que las componentes de u no

desaparezcan. La no singularidad de A(x) en

es la versin multivariable de la condicin

impuesta por (3.10).
2.5.9.3. Forma Normal par Linealizacin Exacta MIMO
Si el grado relativo total de un proceso cuadrado
descrito por (3.20) es r, luego:

La linealizacin exacta de un proceso no lineal
MIMO descrito por (3.20), ocurre cuando el grado
relativo total :
, es igual al orden n
del proceso; es decir, la dimensin del vector de
estado. Para linealizacin exacta, se tiene la
siguiente transformacin de estado z:

|
|
|
|
|
|
|
|
|
|
|
|
(2.42)
Donde:

Da lugar a las siguientes ecuaciones de estado:

2.5.9.4. Ley Control de Desacoplamiento MIMO
Es importante sealar que el grado relativo r
i
, i = 1,
...,m, es el entero ms pequeo requerido para que
por lo menos una de las entradas aparezca en (ver
(3.23)):

Consideremos ahora la siguiente ley de control
desacoplada:

[ ]
Donde: [
, es una nueva entrada a ser

determinada, y A(z), (ver (3.21)), y B(z) son
matrices con elementos:
)
Respectivamente. Sustituyendo los elementos de
, (ecuacion (3.24)) en (3.23), se
obtiene la representacin cannica lineal invariante
en el tiempo del proceso MIMO; es decir, la
ecuacin (3.23) con:

Por otro lado, de la relacin se
puede demostrar que la transferencia del
proceso linealizado MIMO [11]:

[

Lo cual significa que

donde cada entrada vi es diseada por
ubicacin de polos aplicado al correspondiente

subsistema lineal equivalente. Tal subsistema
puede ser seleccionada como:

Donde
es la i-sima trayectoria deseada e
es el i-sima seal de error

de seguimiento. La i-sima ecuacin caracterstica
del subsistema es:

Para comportamiento asintticamente estable, es
decir con
para ,
ninguno de las races de (3.27) debe ser localizado
en el lado derecho del plano s, incluyendo el eje
imaginario. Los parmetros del controlador
, son
seleccionados para encontrar tales requerimientos,
y la entrada
tomar la forma:

Donde:
]

2.6. Hiptesis y Variables

2.6.1. Hiptesis General
Es posible disear el controlador no lineal multivariable por
realimentacin de estado aplicado al robot manipular esfrico.
2.6.2. Hiptesis Especfica
Es posible analizar el modelo dinmico del robot manipulador
esfrico de 3 grados de libertad empleando el algoritmo de
Lagrange Euler.
Es posible aplicar la tcnica de control no lineal por
realimentacin de estado para sistemas de mltiples entradas y
mltiples salidas.
Es posible realizar la simulacin el sistema aplicando la tcnica
de control de linealizacin exacta por realimentacin de estado
aplicado al robot manipulador esfrico.

2.7. Variables

2.7.1. Variables Independientes
Control no lineal multivariable por realimentacin de Estado.
2.7.2. Variables Dependientes
Robot Manipulador

2.8. Metodologa

2.8.1. Tipo y Nivel de Investigacin

CAPTULO III
3. MODELADO DE LA PLANTA

3.1. Introduccin

El modelado de la planta consiste en determinar la descripcin
matemtica (usualmente un conjunto de ecuaciones diferenciales) del
proceso a ser controlado, que en nuestro caso es el robot manipulador
esfrico. Tal modelado describe la dinmica del proceso y tiene por
objeto conocer la relacin entre el movimiento del robot y las fuerzas
implicadas en el mismo. Para determinar las ecuaciones del modelo
usaremos el algoritmo de Lagrange-Euler [1], [2], [4], [7].

3.2. Descripcin del Sistema

El Sistema Robot Manipulador Esfrico de 3 Grados de Libertad que en
adelante lo llamaremos SRM (Spherical Robot Manipulator) de 3DOF es
mostrado en la figura 3.1. A continuacin se describe los tres grados de
libertad usados en el proceso:

1. Primer Grado de Libertad:
Est compuesto por un servomotor DC1 de 24 VDC con encoder ptico
incorporado, y una base conformado por un disco y un prisma de
aluminio; de tal manera que en la parte inferior del disco de radio R
d
y
altura h
d
, ir acoplado con una bocina de bronce dicho servomotor en
posicin vertical y sobre la parte superior del disco ira un prisma de
seccin cuadrada a
p
, y de altura h
p
, a su vez en la parte superior de dicho
prisma ir el otro servomotor en posicin horizontal que corresponde al

segundo grado de libertad. En la figura 3.2 se muestra una vista del
esquema con las medidas del SRM de 3DOF.
2. Segundo Grado de Libertad:
Compuesto por un servomotor por un servomotor DC2 de 24VDC con
encoder ptico incorporado, y una varilla rgida de aluminio, de tal
manera que en el eje de dicho servomotor, en posicin horizontal, ira
acoplado con una bocina de bronce dicha varilla de longitud L
2
, en el
extremo de dicha varilla ir el otro servomotor en posicin horizontal que
corresponde al tercer grado de libertad.

Figura 3.1: Esquema del Robot Manipulador Esfrico de 3DOF
3. Tercer Grado de Libertad:
Compuesto por otro servomotor DC3 de 24 VDC con encoder ptico
incorporado, y una varilla rgida de aluminio, de tal manera que en el eje

de dicho servomotor, en posicin horizontal, ir acoplado con una bocina
de bronce dicha varilla de longitud L
3
, en el extremo de dicha varilla
podr ir un efector final.

3.3. Lista de Variables

Todas las variables que intervienen en el sistema se describen en la Tabla
1.1. Los valores que se presentan han sido tomados de los servomotores
DC con encoder ptico MATSUSHITA modelo GMX-7MCO19B y
modelo GMX-6MPO13A. Donde: C.F.I.: Coeficiente de Friccin
Viscosa y M.I.: Momento de Inercia.

Tabla 3.1: Valores de las Variables.
Smbolo Descripcin Valor
u
1
,u
2
,u
3
Seal de control servo DC1,DC2,DC3 (En simulacin) V
Aceleracin de la gravedad 9.81 m/seg
2

r
m2
Distancia extremo superior del prisma al DC2 0.05 m
C
c1
Coeficiente friccin de Coulomb 1 y 2 DOF 0.001488 kgm
2
/seg
C
c3
Coeficiente friccin de Coulomb 3DOF 0.00015 kgm
2
/seg
m
d
Masa del disco 0.55 kg
m
p
Masa del prisma 1.0 kg
m
2
, m
3
Masa de la 1ra. Y 2da. varilla 0.20 kg
h
d
Altura del disco 0.01 m
R
d
Radio del disco 0.07 m
h
p
Altura del prisma 0.20 m
a
p
Ancho y Largo de la seccin del prisma 0.044 m
L
2
, L
3
Longitud de la 1ra. y 2da. varilla 0.2 m
L
xs2
Long. Del eje de ref. S
1
al CG 1ra. varilla 0.14 m
L
xs3
2
al CG 2da. varilla 0.14 m
L
cs2
2
al CG 1ra. varilla (L
2
L
xs2
) m
L
cs3
3
al CG 2da. varilla (L
3
L
xs3
) m
I
1
M.I. del slido (disco ms prisma) 0.0013 kgm
2

I
2
M.I.de la 1ra. varilla 0.0273 kgm
2

I
3
M.I.de la 2da. varilla 0.0273 kgm
2

J
m1
, J
m2
M.I. servomotor DC1, DC2 0.00059 kgm
2

J
m3
M.I. servomotor DC3 0.000238 kgm
2

J
eq1
M.I. equivalente servomotor DC1 0.2177 kgm
2

B
eq1
C.F.V. equivalente servomotor DC1 0.054 Nms/rad
J
eq2
2

B
eq2
J
eq3
2

B
eq3
n
1
, n
2
Reduccin servomotor DC1, DC2 18.5
n
3
Reduccin servomotor DC3 12.5
R
a1
, R
a2
Resist. Armadura servomotor DC1, DC2 3.50 Ohmios
R
a3
Resist. Armadura servomotor DC3 5.30 Ohmios
L
a1
, L
a2
Inductancia armadura servomotor DC1, DC2 Despreciable
L
a3
Inductancia armadura servomotor DC3 Despreciable
V
a1
, V
a2
Voltage armadura servomotor DC1, DC2 24 V
V
a3
Voltage armadura servomotor DC3 24 V
K
A1
,K
A2
,K
A3
Constante ganancia driver 1,2,3 2.17
K
m1
,K
m2
Cte. Torque servomotor DC1, DC2 0.04364 N-m/A
K
m3
Cte. Torque servomotor DC3 0.04217 N-m/A
Kb1, Kb2 Cte. fem servomotor DC1,DC2 0.04364 V/rad/s
Kb3 Cte. fem servomotor DC3 0.4217 d/s

3.4. Aplicacin y Programacin del Modelo

3.4.1. Clculo
La aplicacin del trabajo es el robot manipulador esfrico de tres
grados de libertad. Para determinar la ecuacin dinmica de dicho
manipulador se aplica el algoritmo de Lagrange-Euler. A
continuacin se presenta los siguientes clculos que usaremos
para la programacin con matemtica simblica de Toolbox de
MATLAB [9].
Clculo de los Parmetros D-H
Primeramente se fijan los sistemas de referencia en las
articulaciones como se muestra en la figura 2.5, posteriormente se
determina los parmetros D-H, [1], [2], [4], [7], que se muestran
en la tabla 3.2:
Clculo de las Matrices de Transformacin Homognea
Reemplazando los valores de la Tabla 3.2 en la ecuacin (2.1),
obtenemos las matrices de transformacin homognea
:
Tabla 3.2: valores de los parmetros DH
Juntura

1 q
0 90
2 q 0 L
2
0

3 q 0 L
3
0

: Posicin y orientacin del sistema de referencia 1 (S

1
) de la
base con respecto al sistema de referencia base (S
0
):
: Posicin y orientacin del sistema del sistema de referencia 2

(S
2
) de la primera varilla con respecto al sistema de referencia 1
(S
1
):

3
) de la
segunda varilla con respecto al sistema de referencia 2 (S
2
):

2
) de la
primera varilla con respecto al sistema de referencia base (S
0
):

3
) de la
segunda varilla con respecto al sistema de referencia base (S
0
):
se determina mediante matemtica simblica de

MATLAB, y el programa se muestra en el apndice A.
Clculo del momento de inercia del disco
El momento de inercia del disco respecto al sistema de referencia
1 (S
1
)
, (ver Figura 3.2), est dado por:

Y los productos de los momentos de inercia son:

Figura 3.2: Momento de inercia del disco respecto al sistema de referencia
1(S
1
).

Clculo del momento de inercia del prisma
El momento de inercia del prisma respecto al sistema de
referencia 1 (S
1
)
, (ver Figura 3.3), est dado por:

Figura 3.3: Momento de inercia del prisma respecto al sistema de referencia
1 (S
1
)

Clculo del momento de inercia de la base (disco ms prisma)
El momento de inercia de la base es la suma de momentos de
inercia del disco ms el prisma, y est dado por:

Clculo de la matriz de la base J
1
De la ecuacin (2.5) se obtiene la matriz de pseudoinercia de la
base J
1
:

Donde,
es la longitud del eje de referencia 1 (S

1
) al C.G. de la
base,
es igual a
y la
inercia viene dada por:
.
Clculo de la matriz de la matriz de la primera varilla J
2

El momento de la inercia de la primera varilla I
2
, se obtiene de
[3], [6]:
. Reemplazando estos
valores en la ecuacin (3.5), obtenemos la matriz de
pseudoinercia J
2
.

Donde,

1
) al C.G. de la
primera varilla y
es igual a
.

Clculo de la matriz de Pseudoinercia de la segunda varilla J
3

El momento de inercia de la segunda varilla I
3
, se obtiene de [3],
[6]:
. Reemplazando estos
valores en la ecuacin (2.5), obtenemos la matriz de
pseudoinercia J
3
:

Donde,

2
) al C.G. de
la segunda varilla y
es igual a
.
3.4.2. Programacin del Algoritmo de lagrange-Euler
El programa de clculo de la ecuacin dinmica del robot
manipulador esfrico de tres grados de libertad (3DOF) se
determina aplicando el Toolbox de Matemtica Simblica de
MATLAB [9] y se presenta en el apndice A. la ecuacin
dinmica obtenida para los tres grados de libertad son:
Primer grado de libertad:

Segundo grado de libertad:

Tercer grado de libertad

Que son los elementos del torque T
i
, dado en la ecuacin (2.10).

3.4.3. El Modelo de los Actuadores
1. Modelado del Actuador 1 de la Base del Manipulador
En la Figura 3.4, se muestra el servomotor DC1, correspondiente
a la base. El voltaje de entrada
aplicado a la armadura est

dado por:

La fuerza electromotriz del servomotor DC es:

En el cual
es la relacin de piones. El torque
del
servomotor est dado por:

Por lo tanto la ecuacin del torque
requerido por el actuador

para la base tiene la forma de:

Donde
es el torque de la carga y
es el torque de friccin de
Coulomb. Reemplazando (3.10) en (3.9) resulta:

Donde:

Figura 3.4: Servomotor DC1 de la base del Manipulador

Despreciando la inductancia de armadura o sea
,
obtenemos de (3.7) y (3.8)

Despejando
en (3.11) y reemplazando en (3.22), obtenemos la

ley de control para la primera entrada:

Donde:

2. Modelado del Actuador 2 de la primera varilla del
Manipulador
En la Figura 3.5, se muestra el servomotor DC2 correspondiente a
la primera varilla. En la misma Figura se muestra el circuito
elctrico del servomotor DC2, controlado tambin por la
armadura. Dicho servomotor se encuentra ubicado en la parte
superior del prisma. Para obtener la ley de control para la segunda
entrada se procede de la misma manera que para el primer
servomotor, obteniendo de esta manera:

Figura 3.5: Servomotor DC2 de la primera varilla del Manipulador

3. Modelado del Actuador 3 de la segunda varilla del
Manipulador
En la Figura 3.6, se muestra el servomotor DC3 correspondiente a
la segunda varilla. En la misma Figura se muestra el circuito
elctrico del servomotor DC3, controlado tambin por la
armadura. Dicho servomotor se encuentra ubicado en extremo de
la primera varilla. Para obtener la ley de control para la tercera
entrada se procede de la misma manera que para el primer
servomotor, obteniendo de esta manera:

Donde:

3.4.4. Modelo de Espacio de Estado
La ecuacin dinmica del manipulador obtenida en las ecuaciones
(3.4), (3.5) y (3.6), se representa en el espacio de estados
mediante la siguiente asignacin de variables:

Figura 3.6: Servomotor DC3 de la segunda varilla del Manipulador

Donde:
Posicin angular de la base
Posicin angular de la primera varilla
Posicin angular de la segunda varilla
Velocidad angular de la base
Velocidad angular de la primera varilla
Velocidad angular de la segunda varilla

La ecuacin no lineal para sistemas multivariables es:

Su representacin en forma matricial:
[
]
Reemplazando las ecuaciones (3.16) y (3.19) en la ecuacin
(3.13) se obtiene
en variables de estado:

As mismo, reemplazando las ecuaciones (3.17) y (3.20) en la
ecuacin (3.14) se obtiene

De la misma manera, reemplazando las ecuaciones (3.18) y (3.21)
en la ecuacin (3.25) se obtiene

La ecuacin dinmica del manipulador obtenida en las ecuaciones
(3.4), (3.5) y (3.6), tambin se representa en variables de estado:

Posteriormente, se reemplaza la ecuacin (3.27) en (3.24) y se
obtiene la primera entrada

Posteriormente, se reemplaza la ecuacin (3.28) en (3.25) y se
obtiene la segunda entrada

Finalmente, se reemplaza la ecuacin (3.29) en (3.26) y se obtiene
la tercera entrada

As mismo de (3.30) se obtiene:
)

De (3.31) y (3.32) se obtiene

Donde:
)(

Finalmente reemplazando las ecuaciones (3.19), (3.20), (3.21), (3.33),
(3.34) y (3.35), en la ecuacin (3.23) se obtiene:

Como la planta tiene tres salidas se tiene:
[

]
[

CAPTULO IV
SIMULACIN DEL SISTEMA

4. Simulacin del Sistema

En esta seccin se muestran las simulaciones del sistema de control no lineal
multivariable de tres entradas y tres entradas y tres salidas, por realimentacin de
estado para el SRM de 3DOF.
Para obtener las simulaciones respectivas se ha tenido en cuenta:
1. El modelo en el espacio-estado, visto en la subseccin 3.4.4.
2. El diseo del controlador no lineal multrivariable basado en la tcnica de
control de linealizacin exacta or realimentacin de estado, visto en la subseccin
4.3.4.
3. El diagrama de bloques del controlador no lineal ilustrado en la Figura
4.1.

Figura 4.1: Diagrama de bloques del controlador no lineal

El diagrama de bloques de la Figura 5.1 es explicado de la siguiente manera: las
variables de estado x del proceso no lineal (robot manipulador esfrico de 3DOF)
genera un cambio de coordenadas en una nueva ecuacin de estado z. con dichas
variables realimentadas y las tres referencias
, se determinan las nuevas

seales de control lineal v, mediante la tcnica por ubicacin de polos. As mismo
con estas seales de control v y con las variables de estado realimentadas z, se
generan las seales de control real u que intervienen en el proceso no lineal.
Basado en los conceptos anteriores se han realizado las simulaciones utilizando el
programa MATLAB [8], [9]. El desarrollo de las simulaciones se ha dividido para
control de trayectoria constante y para control de trayectoria senoidal.

4.1. Caso 1: Control de Trayectoria Constante

Para esta simulacin se ha asumido una trayectoria constante con los siguientes
datos:
Condiciones iniciales:

Trayectorias deseadas:

Polos usados:

Para la base: -8 y -8
Para la primera varilla: -6 y -6
Para la segunda varilla: -40 y -40
tem Tiempo de muestreo: T

4.1.1. Linealizacin por Realimentacin
Un sistema no lineal multivariable de orden n de m entradas y m
salidas es completamente linealizable y desacoplado si su grado
relativo total es:

Si el grado relativo total r, es igual al nmero de orden del sistema
o sea , entonces se aplica el mtodo de linealizacin
exacta. Aplicando los ocnceptos de geometra diferencial
[5],[11],[12],[13], determinamos el grado relativo total.
4.1.2. Determinacin del Grado Relativo Total: r
Aplicando los conceptos de las subsecciones 4.2.2 y 4.3.2, se
obtiene:
Primera salida:
. La segunda derivada de
, es
diferente de cero, por lo tanto el grado relativo de
.
Segunda salida:
, es
.
Tercera salida:
, es
.

Grado Relativo Total:
, que es igual al
orden del sistema,por lo tanto se puede aplicar el mtodo de
linealizacin exacta.
4.1.3. Forma Normal para Linealizacin
Aplicndolos conceptos de las subsecciones 4.2.3 y 4.3.3, se
obtiene la transformacin de coordenadas , en la
siguiente forma matricial:

[

Por lo tanto la nueva ecuacin de estado es:

[
(4.1)

4.1.4. Diseo del Controlador No Lineal MIMO
Para la nueva ecuacin de estado (5.1) y aplicando la ecuacin
(4.36), se obtiene las nuevas entradas lineales equivalentes
, y son expresadas de la siguiente forma, [10], [11], [12]:

Donde:

Asimismo se tiene las siguientes equivalencias:

Reemplazando (5.5) en (5.2), se obtiene:

De la misma manera, reemplazando (5.6) en (5.3), se obtiene:

Finalmente, reemplazando (5.7) en (5.4), se obtiene:

Se sabe que para trayectoria constante se tiene:

Reemplazando estos datos en (5.8), (5.9) y (5.10) se obtiene:

Donde,
, son los valores de la ecuacin

caracteristica que depende de los polos del sistema equivalente.
Con (5.11) y (3.40) se obtiene la ley de control
para la primera
entrada:

Con (4.12) y (3.31) se obtiene la ley de control
para la segunda
entrada:

Finalmente con (4.13) y (3.32) se obtiene la ley de control
para
la tercera entrada:

Donde:
-+

4.1.5. Programacin del Algoritmo de Control No Lineal
Multivariable
Para la programacin del algoritmo de control se ah usado
MATLAB [9], teniendo como dato las condiciones iniciales
indicadas anteriormente y las ecuaciones (4.1), (4.11), (4.12),
(4.13), (4.14), (4.15) y (4.16). Dicho programa se muestra en el
Anexo 2.
4.1.6. Simulaciones

Las simulaciones respectivas son mostradas desde la Figura 4.2
hasta la 4.4. En la Figura 4.2 se muestra las salidas controladas
para la posicin angular de la base a una referencia de 90,
posicin angular de la segunda varilla a una referencia de 180; el
tiempo de establecimiento para la base es de 1.2 seg para la
primera varilla el tiempo de establecimiento es de 1.5 seg y para
la segunda varilla el tiempo de establecimiento es de 0.5 seg
presentando un mnimo sobreimpulso.

Figura 4.2: Trayectoria constante: salidas controladas para la base, primera y segunda
Varilla

En la Figura 4.3 se muestran las seales de control para el
servomotor de la base y los servomotores de las 2 varillas. La

seal del servomotor de la base vara de +10 voltios a -5 voltios
por 0.5 seg, para luego aproximarse a cero a los 1.2 seg, mientras
que la seal del servomotor de la primera varilla vara de +10
voltios a -5 voltios por 0.7 seg., para luego aproximarse a -1
voltio a los 1.7 seg, y finalmente la seal del servomotor de la
segunda varilla vara +10 voltios a -10 voltios por 0.3 seg, para
luego aproximarse a +0.55 voltios a los 1.2 seg; de lo anterior se
concluye que cuando las seales de control se hacen convergentes
el proceso ha sido controlado.
En la Figura 4.4 se muestra la respuesta del sistema para la
velocidad angular del servomotor de la base y de las 2 varillas, se
aprecia que en el arranque la velocidad para los 3 casos se
incrementa para luego aproximarse a cero a los 1.2 seg, para el
servomotor de la base, a los 1.8 seg, para el servomotor de la
primera varilla, a los 0.5 seg, para el servomotor de la segunda
varilla. Esto significa que cuando dichas velocidades convergen a
cero, el proceso ha sido controlado.

Figura 4.3: Trayectoria constante: seales de control para los servomotores de la base,
primera y segunda varilla

Figura 4.4: Trayectoria constante: velocidad para los servomotores de la base, primera
y segunda varilla

4.2. Caso 2: Control de Trayectoria Senoidal

Para esta simulacin se ha asumido una trayectoria senoidal con los
siguientes datos:
Condiciones Iniciales:

Trayectorias deseadas:
)
Polos usados:
Para la base: -8 y -8
Para la primera varilla: -6 y -6
Para la segunda varilla: -40 y -40
Tiempo de muestreo
T=0.01 seg.
4.2.1. Diseo del Controlador No Lineal MIMO
Para el diseo del controlador no lineal MIMO, se usan las
ecuaciones (5.8) y (5.14) para la primera entrada, las ecuaciones
(5.9) y (5.15) para la segunda entrada y las ecuaciones (5.10) y
(5.16) para la tercera entrada.
4.2.2. Programacin del Algoritmo de Control No Lineal
Multivariable
Para la programacin del algoritmo de control se ha usado Matlab
[9], teniendo como dato las condiciones iniciales indicadas
anteriormente y las ecuaciones (5.1), (5.9), (5.10), (5.14), (5.15),
(5.16), dicho programa se muestra en el apndice B.
4.2.3. Simulaciones
Las simulaciones respectivas son mostradas desde la figura 4.5
hasta la figura 4.7. En la Figura 4.5 se muestra las salidas

controladas para la posicin angular de la base, posicin de la
primera varilla y posicin angular de la segunda varilla,
realizando un seguimiento a la seal deseada. Las seales se han
fijado para un periodo de 11 seg. Las amplitudes de las seales
senoidales indican el movimiento de la base y las 2 varillas. El
tiempo de establecimiento es de 1.3 seg. Para la base, 2 seg. Para
la primera varilla y 0.8 seg. Para la segunda varilla; para despus
seguir a la trayectoria deseada.

Figura 4.5: Trayectoria senoidal: salidas controladas para la base, primera y
segunda varilla.
En la Figura 4.6 se muestran las seales de control para el
servomotor de la base, el servomotor de la primera varilla y el

servomotor de la segunda varilla. Las seales de los servomotores
de la base y las 2 varillas llegan hasta 10 voltios en forma
abrupta por un lapso de tiempo de 1.5, 2 y 0.8 seg. Posteriormente
la seal de control de la base disminuye en amplitud hasta hacerse
bien pequea 1.2 voltios, pero siguiendo una aproximacin a una
trayectoria senoidal abrupta. Seguidamente la seal de control de
la primera varilla disminuye en amplitud hasta hacerse pequea
4 voltios, pero siguiendo una trayectoria oscilatoria abrupta.
Finalmente la seal de control de la segunda varilla disminuye en
amplitud hasta hacerse bien pequea 1.5 voltios, pero siguiendo
tambin una trayectoria oscilatoria abrupta.
En la figura 4.7 se muestra la respuesta del sistema para la
velocidad angular del servomotor de la base, velocidad angular de
la primera varilla y velocidad angular de la segunda varilla. Se
aprecia que en el arranque la trayectoria de la velocidad se
incrementa para los 3 casos por un lapso de tiempo de 1.5, 2 y 0.8
seg. Posteriormente la velocidad del servomotor de la base
disminuye en amplitud hasta hacerse pequea 2rad/seg,
siguiendo una trayectoria senoidal. Seguidamente la seal de
control de la primera varilla disminuye en amplitud hasta hacerse
pequea 2.7 rad/seg, se observa que sigue tambin una
trayectoria senoidal. Finalmente la velocidad del servomotor de la
segunda varilla disminuye en amplitud hasta hacerse pequea 4
rad/seg; siguiendo tambin una trayectoria senoidal.

Figura 4.6: Trayectoria senoidal: seales de control para los servomotores de
la base, primera y segunda varilla.

Figura 4.7: Trayectoria senoidal: Velocidad de los servomotores de la base,
primera y segunda varilla.

CAPTULO V
ESPECIFICACIONES TESCNICAS

5. Especificaciones Tcnicas

5.1. Introduccin

En este captulo se tratar sobre las especificaciones tcnicas del sistema
para una futura implementacin del sistema del Robot Manipulador
Esfrico de 3 grados de libertad en tiempo real, el cual se compondr en
el hardware que son los componentes fsicos a usarse en el sistema de
control y en el software que es el lenguaje de programacin a usarse para
el algoritmo de control del sistema. El diagrama de bloques para la
implementacin del sistema de muestra en la figura 5.1.

5.2. El Hardware del Sistema

Los componentes fsicos a usarse en el sistema de control se componen
de las siguientes partes:
- La planta
- El actuador
- El sensor
- La interface de Adquisicin de Datos
5.2.1. La Planta
La descripcin de la planta que viene dado por:

- Tres servomotores DC con encoder ptico incorporado.
- Una base giratoria que vendr a ser el primer grado de
libertad estar formada por un disco metlico que ir acoplada
con una bocina de bronce al eje del primer servomotor en
posicin vertical y una barra metlica de seccin cuadrada que ir
acoplada al disco. Sobre el extremo superior de la barra ir sujeto
el segundo servomotor del segundo grado de libertad.
- El primer eslabn que vendr a ser el segundo grado de
libertad estar formada por una varilla rgida metlica que ir
acoplada de su extremo inferior con una bocina de bronce al eje
del segundo servomotor en posicin horizontal.
- El segundo eslabn que vendr a ser el tercer grado de
libertad estar formada por otra varilla rgida metlica que ir
acoplada de su extremo inferior con una bocina de bronce al
extremo superior de la primera varilla y al eje del segundo
servomotor.

Figura 5.1: Diagrama de Bloques de la implementacin del sistema.

5.2.2. El Actuador
El actuador estar formado por tres servomotores DC de campo
magntico permanente y escobillas conmutadas con encoder
ptico incremental incorporado. Los servomotores DC del primer
y segundo grado de libertad tendrn las siguientes caractersticas.
- Servomotores de los dos primeros grados de libertad.
Marca MATSUSHITA, modelo GMX 7MCO19B
Voltaje nominal: 24 VDC
Corriente en vaco: 0.7 A

Velocidad en vaco: 255 RPM
Relacin de engranajes (n1 y n2): 18.5:1
Torque del servomotor a plena carga: 5.65 Nm
Corriente de Armadura a plena carga: 7 A
Voltaje de Entrada del Encoder: 5 VDC
Salida del Encoder: 1850 Pulsos/Rev. De Salida de eje de 2
canales en cuadratura.

- Servomotor DC del tercer grado de libertad
Marca MATSUSHITA, modelo GMX 6MPO13A
Voltaje nominal: 24 VDC
Corriente en vaco: 0.2 A
Velocidad en vaco: 310 RPM
Relacin de engranajes (n3): 12.5:1
Torque del servomotor a plena carga: 2.37 in-lbs
Corriente de Armadura a plena carga: 4 A
Voltaje de Entrada del Encoder: 5 VDC
Salida del Encoder: 1250 con 2 canales en cuadratura.

- Tres Amplificadores de potencia, una para cada servomotor DC,
en la figura 5.2 se muestra el circuito del amplificador que estar
conformado por el transistor amplificador PN2222 y el transistor
preamplificador PN2907 que estn acoplados por un emisor
comn y que trabajan en la zona de corte y saturacin.

Figura 5.2: Circuito Amplificador de Potencia.

As mismo en dicho circuito se muestra dos pares de
conmutadores A
1,4
y A
2,3
conformado por los MOSFETs
IRFZ34 cuyo esquema simplificado se muestra en la figura 5.3. El
funcionamiento de este amplificador se basa en la lgica de

control para activar al circuito de disparo, es decir, cuando el
sistema de disparo cierra al conmutador A
1,4
y se abre A
2,3
, el
sentido de la corriente es la lnea con puntos induciendo de esta
forma una tensin +V
cc
en el servomotor. Luego si el sistema de
disparo abre al conmutador A
1,4
y se cierra A
2,3
, el sentido de la
corriente es la lnea continua induciendo as una tensin V
cc
en
el servomotor. Por lo tanto el servomotor ve en sus bornes una
onda de voltaje cuadrada, variando entre V
cc
y la corriente que
puede absorber depender de los conmutadores.

Figura 5.3: Esquema del circuito de disparo y conmutacin.
En resumen de lo anterior podemos decir que el objetivo del
sistema de disparo es realizar la conmutacin de cada par A
1,4
y
A
2,3
, de tal forma que se eviten cortocircuitos durante el tiempo de

conmutacin de A
1,4
y A
2,3
. El circuito amplificador de potencia
de la figura 6.3 tiene un circuito construido en base en compuertas
lgicas de tecnologa TTL (Transistor Transistor Logic) que
conformarn los circuitos integrados 7404, 7486 y 7408 y dos
resistencias de 500 . Este circuito gobernar los disparos de los
MOSFETs de manera que las seales PWM que ingresan al
circuito amplificador de potencia que tengan igual amplitud
estarn siempre en nivel bajo (OFF) tal como se muestra en la
tabla 5.1.
Tabla 5.1: Conmutacin de los MOSFETs.
PWM
1
PWM
2
A
1
A
2
A
3
A
4

0 0 OFF OFF OFF OFF
0 1 ON OFF OFF ON
1 0 OFF ON ON OFF
1 1 OFF OFF OFF OFF

- Tres generadores PWM, una para cada servomotor DC, en
la figura 5.4 se muestra el circuito del generador PWM que estar
conformado por el integrado LM3524 que genera una modulacin
por ancho de pulso a una frecuencia de 15.4 KHz , posteriormente
esta seal ser amplificada por el amplificador de potencia que
alimentar al servomotor DC. Los circuitos osciladores 74LS123
realizarn la funcin de sincronizacin para la generacin del tren
de pulsos en cuadratura, cuyas salidas sern las seales PWM1 y
PWM2 que sern las entradas al circuito amplificador.

Figura 5.4: Circuito Generador PWM
5.2.3. El Sensor
El sensor de posicin es el codificador ptico rotatorio, est
incorporado en el servomotor DC, se ilustra en la figura 5.5 y est
conformado por:
Una fuente de luz que puede ser un LED infrarrojo o lser.
Es el que genera un haz de luz perpendicular al disco.
Un disco ranurado acoplado al eje del servomotor y
ubicado entre la fuente de luz y el detector.
Un detector de luz, usualmente es un transistor ptico. Es
el que genera un pulso de corriente que ser la entrada a un
contador de pulsos.

Un conformador de pulsos que convierte la seal
senosoidal proveniente de la salida del detector en seal de onda
cuadrada.

Figura 5.5: Estructura del Codificador ptico rotatorio
El codificador que se usar, incluir dos pistas de marcas o
ranuras en cuadratura (Canales Ay B), cada una asociada a una
fuente de luz y un transistor ptico independientes. Los canales
Ay B producen dos ondas cuadradas desfasadas +90 entre s que
se utilizan para determinar la direccin del movimiento. Por
ejemplo si el canal B adelanta al canal A, el eje est girando en el
sentido anti horario y viceversa. El codificador a usarse tiene una
resolucin de 100 ppr (pulsos por revolucin).

5.2.4. La Interfaz de Adquisicin de Datos
La interface de Adquisicin de Datos ser la encargada de
manejar los datos de entrada y salida del sistema. En la figura 5.1
se muestra el esquema de la interface de adquisicin de Datos con
los componentes que se podran usar cuando se realice la
implementacin del sistema. Las caractersticas tcnicas de la
interface son:
- Marca: National Instruments
- Modelo: NI 6713, insertable en la PC con Bus PCI
- Salida Analgicas: 8
- Resolucin de Salidas Analgicas: 12 bits
- Voltaje de entrada: 10
- Frecuencia: 1 Msamples/s
- Contador/temporizador: 02 de 24 bits
- Entrada/Salida Digital: 8
- Cable modelo: CB 68LP
- Driver Software: NI DAQ para Windows 2000/NT
- Aplicacin de Software: Labview, Visual Basic, Lenguaje
C++
As mismo la interface que se muestra en la figura 6.7, estar
compuesto por:

Tres salida analgicas (DAC0, DAC1 y DAC2), estas
salidas sern seales de control que se enviarn por los terminales
del generador PWM (LM3524) con una resolucin de 12 bits.
Tres puertos digitales de entrada (PA, PB y PC) de 8 bits
cada uno. Estos puertos recibirn los datos provenientes del
decodificador de cuadratura respectivo, que a su vez recibir los
datos de los decodificadores pticos de los servomotores.
El terminal OUTB0 que se usar como salida de una seal
de reloj para el funcionamiento del sistema de control.
El terminal PD0 (bit 0 del puerto D) que se usar como
entrada de la seal de reloj proveniente del OUTB0.

CAPTULO VI
CONCLUSIONES Y RECOMENDACIONES

6. Conclusiones y Recomendaciones

Finalmente se presenta las conclusiones derivadas de este trabajo, as como
tambin algunas recomendaciones para investigaciones futuras.
CONCLUSIONES:

- Para el diseo del sistema de control fue necesario determinar y calcular
el modelo dinmico de la planta, ya que la tcnica de control no lineal
empleada lo requiere.
- La ecuacin dinmica del manipulador robtico se ha determinado
empleando el algoritmo de Lagrange-Euler, el cual tambin puede ser
aplicado a otros procesos fsicos de dos o ms grados de Libertad.
- El planteamiento del problema fue el que las posiciones de la base y de
los dos eslabones sigan trayectorias arbitrarias usando la tcnica de
control por realimentacin de estado. Esta estrategia de control, requiere
del modelo del proceso controlado. Las simulaciones realizadas en el
captulo 5 han demostrado que el modelo obtenido en el captulo 3 es
vlido y cumple con las exigencias de un modelo no lineal multivariable
con propsitos de control. De la misma manera, tales simulaciones
tambin han demostrado que el sistema de control no lineal
multivariable, cumple con las condiciones de diseo impuestas
previamente: seguimiento de las salidas controladas con respecto a
seales arbitrarias de referencia con mnimo sobreimpulso, mnimo de
tiempo de estabilizacin y error en estado estable nulo.

RECOMENDACIONES:

Algunas de las reas que necesitan investigarse, asumiendo que el proceso del
manipulador robtico esfrico ha sido construido, se listan a continuacin:
- El proceso diseado e implementarlo en tiempo real en hardware y
software, puede utilizarse para el estudio de otro tipo de sistemas de
control para el rea de control e instrumentacin, as por ejemplo en el
control avanzado y/o inteligencia artificial, donde se puede demostrar el
uso de estas tcnicas de control.
- Diseo de un controlador que trabaje expuesto a cambios dinmicos
desconocidos (disturbios) y que exijan al controlador afronta con xito el
problema de seguimiento de trayectorias ante la presencia de dichos
disturbios.
- Para un diseo ms general, es necesario agregar una mano al sistema.
En esta forma, Los diseos de los sistemas de control correspondientes
pueden ser orientados a aplicados industriales.
- Tambin se requiere investigar el caso cuando el manipulador robtico
esfrico presenta en su construccin eslabones y junturas flexibles.

BIBLIORAFIA

[1] A. Barrientos, L. Pen, C. Balaguer R. Aracil. Fundamentos de
Robtica. Mc- Graw Hill, 1997.

[2] F. Torres, J. Pomares, P. Gil S. Puente R. Aracil. Robot y Sistemas
Sensoriales. Prentice Hall, 2002.

[3] Ferdinand P. Berr, E. Russell Johnston. Dinmica. McGraw Hill, 1996.

[4] K. S. Fu, R. C. Gonzles, C.S.G. Lee. Robtica, Control, Deteccin,
Visin, Inteligencia. McGraw Hill, 1990.

[5] Khalil, Hassan K. NonLinear Systems. Prentice Hall, 1996.

[6] M. J. Daz. Mecnica Racional Esttica. Editorial Universo, 1993.

[7] Moshen, Shahinpoor. A Robot Engineering Textbook. Harper-Row
Publisher, 1987.

[8] Nakamura, Shoichiro. Anlisis Numrico y Visualizacin Grfica.
Prentice Hall, 1997.

[9] Part Enander, Eva and Anders Sjoberg. The Matlab 5 Handbook.
Addison Wesley, 1999.

[10] Rojas, Arturo. Control Avanzado. Publicacin Independiente, 2001.

[11] Rojas, Arturo. Applied MIMO NonLinear Control Real Time
Implementations.En construccin, 2002.

[12] Slotine, Jean-Jacques E. Applied NonLinear Control. Prentice Hall, 1991.

[13] Jos Angulo. Robtica Prctica (Cuarta Edicin). Editorial Paraninfo,
2000.

ANEXOS

ANEXO 1

Coeficiente de Friccin Viscosa

La obtencin del coeficiente de friccin viscosa del segundo eslabn, tiene que
ver con el modelamiento y compensacin de la friccin de Coulomb. La friccin
de Coulomb es el torque requerido para mantener el servomotor en movimiento
y es una friccin que se opone al movimiento. Se ha comprobado
experimentalmente que la friccin de Coulomb puede ser considerada una
constante. El efecto de la friccin de Coulomb es modelado como un torque
constante proporcional al signo de la velocidad, es decir:

}
Donde
es el torque de friccin de Coulomb, es la velocidad del eje del

servomotor.
El modelo discreto para el servomotor
a
considerando la friccin de
Coulomb, se deducen a partir de las siguientes ecuaciones:

Reemplazamos (A.3) en (A.4), luego igualamos con (A.2)

Las seales en tiempo discreto surgen si el sistema involucra la operacin de
muestreo de seales continuas. Sea
la velocidad angular muestreada en el k-

simo instante de tiempo, usamos ahora la aproximacin de Euler:
; para discretizar la ecuacin (A.5), obteniendo:
+
donde
modela el torque de friccin de Coulomb; y podemos asumir

que la aproximacin es dada por
es la mitad del
ancho de la zona muerta que ser determinada ms adelante). Si la velocidad es
distinta de cero, el torque tomar dos valores constantes dependiendo el sentido
de giro del servomotor.
La ecuacin (A.6) se puede expresar como:
+
El diagrama de bloques de la Figura A.1 muestra al modelo del servomotor
discreto.
El diagrama involucra la compensacin de la friccin de Coulomb determinada
por el parmetro G3, que ser objeto de anlisis para determinar el coeficiente de
friccin viscosa C
c
.

A.1: Modelo del servomotor DC

Los parmetros para el sistema sin compensar se describen en la Tabla A.1,
donde G
i
, i=1, 2, 3, 4.
Tabla A.1: Parmetros del Sistema a simular

Las simulaciones han sido realizadas en Simulink, cuya entrada V es una entrada
senoidal de amplitud 1.5 V y frecuencia de 2.5 rad/seg. El resultado de la
simulacin para la velocidad angular es mostrada en la Figura A.2, donde se
muestra la friccin de Coulomb sin compensar.

Figura A.2: Simulacin para obtener la Friccin de Coulomb.
La grfica de la velocidad angular en estado estacionario del servomotor DC vs.
El voltaje aplicado al amplificador de potencia, se muestra en la figura A.3,
donde nos permite apreciar una zona muerta a la velocidad cero. Esta zona
muerta es debido a la friccin esttica del servomotor. Note el efecto no lineal en
las cercanas de la zona muerta.

Figura A.3: Respuesta tensin de entrada versus velocidad.
Para Linealizar la ecuacin (A.5) se debe de adicionar una tensin de entrada a
la entrada del amplificador tal que compense la friccin de Coulomb cuando la
velocidad del servomotor
M

th
(
th
velocidad de umbral). Un esquema
general es el que presenta la compensacin de las fricciones esttica y de
Coulomb.
u = u
p
+ V
c
(A.8)
Siendo u la ley de control, u
p
el voltaje de compensacin de la friccin esttica y
V
c
el voltaje de compensacin de Coulomb. El objetivo del primer trmino
(dependiente de la direccin del voltaje de armadura) es sacar al servomotor de
la zona donde la friccin esttica tiene efecto. El objetivo del segundo trmino es
compensar el torque de friccin de Coulomb presente cuando el servomotor est
en movimiento a velocidades mayores al umbral
th
.
El voltaje de compensacin de Coulomb es dado por la relacin:

El coeficiente de friccin viscosa C
c
es pequeo para servomotores DC, siendo
difcil su identificacin en presencia de ruido. Usamos el mtodo de prueba y
error para el clculo de V
c
, segn la Figura A.3, el valor inicial de V
zm
es 0.025
este valor fue disminuido hasta encontrar el valor apropiado de V
zm
es 0.00868.
Despejamos el coeficiente de friccin C
c
de la ecuacin A.9 para reemplazar los
parmetros previamente identificados, como son:

Luego:

El valor V
zm
es 0.00868 permite compensar la friccin de Coulomb resultando el
coeficiente de friccin de valor C
c
= 1.5 e
-4
. Los efectos de compensacin son
mostrados en la figura A.4:

Figura A.4: Experimento de la compensacin de la friccin no lineal.

ANEXO 2
Programas en MATLAB
Listado de Programas en MATLAB
Los programas estn escritos en MATLAB [8], [9], y son los siguientes:
1. robot3dof_p1: este programa es usado en la seccin 3.4, para determinar la
ecuacin dinmica del manipulador de 3DOF, mediante el algoritmo
Lagrange Euler.
2. robot3dof_p2: este programa es usado en la seccin 5.1, para realizar las
simulaciones con trayectoria constante.
3. robot3dof_p3: este programa es usado nen la seccin 5.2, para realizar las
simulaciones con trayectoria senoidal.

Clculo de la Dinmica del Manipulador
robot3dof_p1.m
clear all; close all; clc

% Calculo de la ecuacion dinamica del Robot
% Manipulador Esferico de 3DOF aplicando el Algoritmo
% Lagrange-Euler con Matematica Simbolica de MATLAB.

syms q1 q2 q3 m1 hp hd rm2 hd Lys1 m2 L2 Lcs2 Lxs2 m3 L3 Lcs3 Lxs3
syms g g1 I1 I2 I3 Dq1 Dq2 Dq3 X Y S1 C1 S2 C2 TT1

% A. Datos de los parametros D-H

teta1=q1; teta2=q2; teta3=q3;
dDH1=hp+hd+rm2; dDH2=0; dDH3=0;
aDH1=0; aDH2=L2; aDH3=L3;
alfa1=90; alfa2=0; alfa3=0;

% B. Calculo de las matrices de transformacion: Aij

A01=[cos(q1) 0 sin(q1) 0
sin(q1) 0 -cos(q1) 0
0 1 0 hp+hd+rm2
0 0 0 1];

A12=[cos(q2) -sin(q2) 0 L2*cos(q2)
sin(q2) cos(q2) 0 L2*sin(q2)
0 0 1 0
0 0 0 1];

A02=A01*A12;

A23=[cos(q3) -sin(q3) 0 L3*cos(q3)
sin(q3) cos(q3) 0 L3*sin(q3)
0 0 1 0
0 0 0 1];

A03=A02*A23;

% X=cos(q2)*cos(q3)-sin(q2)*sin(q3)=cos(q2+q3);
% Y=sin(q2)*cos(q3)+sin(q3)*cos(q2)=sin(q2+q3);

% C.- Calculo de las matrices de transformacion homogenea: Uij}

U11=diff(A01,'q1'); U12=diff(A01,'q2'); U13=diff(A01,'q3');

A = -sin(q1)*L3*X-sin(q1)*L2*cos(q2);
B = cos(q1)*L3*X+cos(q1)*L2*cos(q2);

U31=[-sin(q1)*X sin(q1)*Y cos(q1) A
cos(q1)*X -cos(q1)*Y sin(q1) B
0 0 0 0
0 0 0 0];

U32=[-cos(q1)*Y -cos(q1)*X 0 -L3*cos(q1)*Y-L2*cos(q1)*sin(q2)
-sin(q1)*Y -sin(q1)*X 0 -L3*sin(q1)*Y-L2*sin(q1)*sin(q2)
X -Y 0 L3*X+L2*cos(q2)
0 0 0 0];

U33=[-cos(q1)*Y -cos(q1)*X 0 -L3*cos(q1)*Y
-sin(q1)*Y -sin(q1)*X 0 -L3*sin(q1)*Y
X -Y 0 L3*X
0 0 0 0];

X = cos(q2)*cos(q3)-sin(q2)*sin(q3);
Y = sin(q2)*cos(q3)+sin(q3)*cos(q2);

UU31=[-sin(q1)*X sin(q1)*Y cos(q1) A
cos(q1)*X -cos(q1)*Y sin(q1) B
0 0 0 0
0 0 0 0];

UU32=[-cos(q1)*Y -cos(q1)*X 0 -L3*cos(q1)*Y-L2*cos(q1)*sin(q2)
-sin(q1)*Y -sin(q1)*X 0 -L3*sin(q1)*Y-L2*sin(q1)*sin(q2)
X -Y 0 L3*X+L2*cos(q2)
0 0 0 0];

UU33=[-cos(q1)*Y -cos(q1)*X 0 -L3*cos(q1)*Y
-sin(q1)*Y -sin(q1)*X 0 -L3*sin(q1)*Y
X -Y 0 L3*X
0 0 0 0];

% D.- Matrices de seudoinercias: JJi}
% Para la base (disco m\s prisma): JJ1}

JJ1=[I1 0 0 0
0 -I1 0 -m1*Lys1
0 0 I1 0
0 -m1*Lys1 0 m1];

% Para la primera varilla: JJ2}

Lxs2=L2-Lcs2;
JJ2=[ I2 0 0 -m2*Lxs2
0 0 0 0
0 0 0 0
-m2*Lxs2 0 0 m2];

% Para la segunda varilla: JJ3}

Lxs3=L3-Lcs3;
JJ3=[ I3 0 0 -m3*Lxs3
0 0 0 0
0 0 0 0
-m3*Lxs3 0 0 m3];

% E. Calculo de la matriz de inercia simetrica: Hik(q)}
% Matriz H11

H11=simplify(trace(U11*JJ1*U11)) + ...
simple(simplify(trace(U21*JJ2*U21)))+ ...
simplify(trace(UU31*JJ3*UU31));

% Matriz H12
H12=simplify(trace(U22*JJ2*U21)+trace(UU32*JJ3*UU31));

% Matriz H13
H13=simplify(trace(UU33*JJ3*UU31));

% Matriz H21
H21=simplify(trace(U21*JJ2*U22)+trace(UU31*JJ3*UU32));

% Matriz H22
H22=simplify(trace(U22*JJ2*U22))+ simplify(trace(UU32*JJ3*UU32));

% Matriz H23

% Matriz H31}

% Matriz H32

% Matriz H33

% F. Calculo de las matrices: Uijk}

U111=diff(U11,'q1'); U112=diff(U11,'q2'); U113=diff(U11,'q3');

% G. Calculo del vector: hikm}

h111=simplify(trace(U111*JJ1*U11))+...
simple(simplify(trace(U211*JJ2*U21)));
h112=simple(simplify(trace(U212*JJ2*U21)));
hh113=simple(trace(U313*JJ3*UU31));
h131=simple(simplify(trace(U331*JJ3*UU31)));
hh223=simple(simplify(trace(U323*JJ3*UU32)));


X = cos(q2)*cos(q3)-sin(q2)*sin(q3);
Y = sin(q2)*cos(q3)+sin(q3)*cos(q2); h113=simple(hh113);

h223=simple(simplify(hh223)); h232=h223; h233=h232;
h311=simple(simplify(hh311)); h322=simple(simplify(hh322));

% H. Calculo del vector de fuerza Centrifuga y de
% Coriolis: Ci(q,dq)}

C1=simple(h111*Dq1*Dq1 + h112*Dq1*Dq2 + h113*Dq1*Dq3 + ...
h121*Dq1*Dq2 + h122*Dq2*Dq2 + h123*Dq2*Dq3 + ...
h131*Dq1*Dq3 + h132*Dq2*Dq3 + h133*Dq3*Dq3);

% I. Calculo del vector de fuerza gravitacional: di(q)}

r11=[0; -Lys1; 0; 1];
r22=[-Lxs2; 0; 0;1];
r33=[-Lxs3; 0; 0;1];
g1=[0 0 -g 0];
d1=-m1*g1*U11*r11 - m2*g1*U21*r22 - m3*g1*UU31*r33;
d2=simplify(-m2*g1*U22*r22 - m3*g1*UU32*r33);
d3=simplify(-m3*g1*UU33*r33);

% J. Parametros del sistema

a1=2*I1;
a2=I2 + 2*m2*L2*Lcs2 - m2*L2*L2;
a3=I3 + 2*m3*L3*Lcs3 - m3*L3*L3;
a4=m3*L2*Lcs3;
a5=m3*L2*L2;
a6=m2*g*Lcs2 + m3*g*L2;
a7=m3*g*Lcs3;

% K. Resultados: Matriz de inercia simetrica
% H11=a1+a2*cos(q2)*cos(q2)+a3*cos(q2+q3)*cos(q2+q3)+ ...
% 2*a4*cos(q2)*cos(q2+q3)+
% a5*cos(q2)*cos(q2);
% H12=0;
% H13=0;
% H21=0;
% H22=a2+a3+2*a4*cos(q3)+a5;
% H23=a3+a4*cos(q3);
% H31=0;
% H32=H23;
% H33=a3;
% Vector de fuerza centrifufa y Ciorolis

C1=-2*a2*sin(2*q2)*Dq1*Dq2-2*(a3*sin(q2+q3)*cos(q2+q3))+ ...
a4*sin(2*q2+2*q3)*cos(q2)*Dq1*Dq3;

C2=a2*sin(q2)*cos(q2)*Dq1*Dq1-2*a4*sin(q3)*Dq2*Dq2*Dq3*Dq3 - ...
a4*sin(q3)*Dq3*Dq3;

C3=(a3*sin(q2+q3)*cos(q2+q3)+a4*sin(2*q2+2*q3)*cos(q2))*Dq1*Dq1 + ...
a4*sin(q3)*Dq2*Dq2;

% Vector de fuerza gravitacional

d1=0;
d2=a6*cos(q2)+a7*cos(q2+q3);
d3=a7*cos(q2+q3);

Simulacin con Referencia Constante
robot3dof_p2.m
clear all; close all; clc

% Datos del sub-sistema mecanico base + disco

hd = 0.01;
Rd = 0.07; % altura y radio

% M_disco_total = M_disco + M_bocina_Motor1 = 0.3 + 0.25

m_dt = 0.55; % prisma
hp = 0.21; % altura
ap = 0.044; % ancho y largo de la seccion prisma

% M_prisma_total = M_prisma + M_Motor2 + M_accesorios
% M_prisma_total = 1+1.235+0.1+0.065+0.075

m_pt = 2.475-0.072;
m_hueco = 0.072;
R_hueco = 0.063;

% Inercia de la base mas servomotor DC 2

I1=0.5*(m_pt*(ap^2)/6 + m_dt*(Rd^2)/2-m_hueco*(R_hueco^2)/2)+0.00059;

% Primera varilla

m2=0.25+0.5+0.05; % masa varilla + masa M2 + masa accesorios
mo=0.025; % Masa del objeto
L2=0.20; % Longitud total
Lcs2=0.10; % Longitud del punto de ref. 2 al centro de
gravedad
I2=m2*(L2*L2)/3; % momento de inercia

% Segunda varilla

m3=0.25; % masa del brazo
mo=0.025; % masa del objeto
L3=0.20; % longitud total
Lcs3=0.10; % longitud del punto de ref. 3 al centro de
gravedad
Io=mo*(ap*ap/6); % momento de Inercia del objeto
Iv=m3*(L3*L3)/3; % momento de Inercia segunda varilla
I3=Iv+Io; % inercia total
g =9.81; % aceleracin de la gravedad

% Datos del sub-sistema elctrico

Km1=0.04364; Kb1=0.04364; Ra1=3.5; Jm1=0.00059; n1=18.5; KA1=2.17;
Km2=0.04364; Kb2=0.04364; Ra2=3.5; Jm2=0.00059; n2=18.5; KA2=2.17;
Km3=0.04217; Kb3=0.04217; Ra3=5.3; Jm3=0.000238; n3=12.5; KA3=2.17;
Cc1=0.001488; Cc3=0.00015;

% Carga

Jeq1 = 0.2177; Beq1 = 0.054;
Jeq2 = 0.3954; Beq2 = 0.0589;
Jeq3 = 0.000305; Beq3 = 0.000538;

% Equivalencia de resistencias

RT1 = Ra1/(n1*Km1*KA1);
RT2 = Ra2/(n2*Km2*KA2);
RT3 = Ra3/(n3*Km3*KA3);

% Equivalencia de N

NT1 = n1*Kb1/KA1;
NT2 = n2*Kb2/KA2;
NT3 = n3*Kb3/KA3;

% Parmetros del Robot

a1=2*I1;
a2=I2 + 2*m2*Lcs2*L2 - m2*L2*L2;
a3=I3 + 2*m3*Lcs3*L3 - m3*L3*L3;
a4=m3*Lcs3*L2;
a5=m3*L2*L2;
a6=m2*g*Lcs2+m3*g*L2;
a7=m3*g*Lcs3;

% Modelo lineal equivalente del proceso de 6to orden (Continuo)}
% M=[0 1 0 0 0 0
% 0 0 0 0 0 0
% 0 0 0 1 0 0
% 0 0 0 0 0 0
% 0 0 0 0 0 1
% 0 0 0 0 0 0];
% N=[0 0 0;1 0 0;0 0 0;0 1 0;0 0 0;0 0 1];
% C=[1 0 0 0 0 0;0 0 0 1 0 0;0 0 0 0 1 0];
% % Calculo del rango
% rangoNN=rank(N); % rango = 6
% Calculo de ganancias del sistema realimentado: K1, K2, K3, K4, K5,
K6

polo1 = -8; polo2 = -8;
aa=[1 -polo1]; bb=[1 -polo2];
pol1=conv(aa,bb);
K1=pol1(2); K2=pol1(3);

polo3 = -6; polo4 = -6;
aaa = [1 -polo3]; bbb = [1 -polo4];
pol22=conv(aaa,bbb);
K3=pol22(2); K4=pol22(3);

polo5 = -40; polo6 = -40;
aaaa = [1 -polo5]; bbbb = [1 -polo6];
pol33=conv(aaaa,bbbb);
K5 = pol33(2); K6 = pol33(3);

% Condiciones y parmetros iniciales

p1(1)=0; p1(2)=0; p1(3)=0; p1(4)=0;
p2(1)=0; p2(2)=0; p2(3)=0; p2(4)=0;

p3(1)=0; p3(2)=0; p3(3)=0; p3(4)=0;
x1(1)=0; x1(2)=0; x1(3)=0; x1(4)=0;
x2(1)=0; x2(2)=0; x2(3)=0; x2(4)=0;
x3(1)=0; x3(2)=0; x3(3)=0; x3(4)=0;
x4(1)=0; x4(2)=0; x4(3)=0; x4(4)=0;
x5(1)=0; x5(2)=0; x5(3)=0; x5(4)=0;
x6(1)=0; x6(2)=0; x6(3)=0; x6(4)=0;

% Tiempo de muestreo
Nm=2000; T =0.010;

% Lazo de control
for k=4:Nm;

% Trayectorias constantes deseadas: p1, p2, p3

p1(k)=pi/2;
p2(k)=pi;
p3(k)=pi;

% Transformacion de coordenadas

z1(k)=x1(k);
z2(k)=x4(k);
z3(k)=x2(k);
z4(k)=x5(k);
z5(k)=x3(k);
z6(k)=x6(k);

% Matriz de Inercias Hij:

H11(k)=a1+a2*cos(z3(k))*cos(z3(k))+...
a3*cos(z3(k)+z5(k))*cos(z3(k)+z5(k))+...
2*a4*cos(z3(k))*cos(z3(k)+z5(k))+a5*cos(z3(k))*cos(z3(k));
H22(k)=a2+a3+2*a4*cos(z5(k))+a5;
H23(k)=a3+a4*cos(z5(k));
H32(k)=H23(k);
H33(k)=a3;

% Vector de fuerzas centrifuga y de Coriolis Ci:

C1(k)=-a2*z2(k)*z4(k)*sin(2*z3(k))-...
2*a3*z2(k)*z6(k)*sin(z3(k)+z5(k))*cos(z3(k)+z5(k))...
-2*a4*z2(k)*z6(k)*sin(z3(k)+z5(k))*cos(z3(k));
C2(k)=a2*z2(k)*z2(k)*sin(z3(k))*cos(z3(k))-...
a4*z6(k)*z6(k)*sin(z5(k))-2*a4*z4(k)*z6(k)*sin(z5(k));
C3(k)=a3*z2(k)*z2(k)*sin(z3(k)+z5(k))*cos(z3(k)+z5(k))+...
a4*z2(k)*z2(k)*cos(z3(k))*sin(z3(k)+z5(k))+...
a4*z4(k)*z4(k)*sin(z5(k));

% Vector de fuerza gravitacional di:

d1(k)=0;
d2(k)=a6*cos(z3(k))+a7*cos(z3(k)+z5(k));
d3(k)=a7*cos(z3(k)+z5(k));

% Nuevas entradas equivalentes: v1, v2 y v3

v1(k)=K2*p1(k)-K1*z2(k)-K2*z1(k);

v2(k)=K4*p2(k)-K3*z4(k)-K4*z3(k);
v3(k)=K6*p3(k)-K5*z6(k)-K6*z5(k);

% Ley de Control u1, u2 y u3
% Valores de P1, P2, P3

P1(k)=(RT1*Beq1+NT1)*z2(k)+RT1*C1(k);
P2(k)=(RT2*Beq2+NT2)*z4(k)+RT2*(C2(k)+d2(k));

% Primer actuador: base

u1(k) = P1(k) + v1(k)*RT1*(H11(k)+Jeq1);

% Compensando friccin de Coulomb

u1(k) = u1(k) + Cc1*n1*RT1*x4(k);
if(u1(k) > z2(k))
u1(k) = u1(k) + Cc1*n1*RT1*z2(k);
elseif(u1(k) < -z2(k))
u1(k) = u1(k) - Cc1*n1*RT1*z2(k);
end

% Limitando fuerza de control

if(u1(k) > 10)
u1(k)=10;
elseif(u1(k) < -10)
u1(k)=-10;
end

% Segundo actuador: primera varilla

u2(k) = P2(k) + v2(k)*RT2*(H22(k)+Jeq2) + v3(k)*RT2*H23(k);


u2(k) = u2(k) + Cc1*n2*RT2*x5(k);
if(u2(k) > z4(k))
u2(k) = u2(k) + Cc1*n2*RT2*z4(k);
u2(k) = u2(k) - Cc1*n2*RT2*z4(k);
end


if(u2(k) > 10)
u2(k) = 10;
elseif(u2(k) < -10)
u2(k) = -10;
end

% Tercer actuador: segunda varilla

u3(k) = P3(k) + v2(k)*RT3*H32(k) + v3(k)*RT3*(H33(k)+Jeq3);


u3(k) = u3(k) + Cc3*n3*RT3*x6(k);
if(u3(k) > z6(k))
u3(k) = u3(k) + Cc3*n3*RT3*z6(k);
u3(k) = u3(k) - Cc3*n3*RT3*z6(k);
end


if(u3(k) > 10)
u3(k) = 10;
elseif(u3(k) < -10)
u3(k) = -10;
end

% Proceso de discretizacion

Tc1=Cc1*sign(x4(k));
x1(k+1)=x1(k)+T*x4(k);
x4(k+1)=x4(k)+(T*(-n1*RT1*Tc1-P1(k)+u1(k)))/(RT1*(H11(k)+Jeq1));
Det(k)=RT2*RT3*((H22(k)+Jeq2)*(H33(k)+Jeq3)-H23(k)*H23(k));
x2(k+1)=x2(k)+T*x5(k);
x5(k+1)=x5(k)+(T/Det(k))*(RT3*(H33(k)+Jeq3)*u2(k)-
RT2*H23(k)*u3(k)-...
RT3*(H33(k)+Jeq3)*P2(k)+RT2*H23(k)*P3(k)-...
RT3*(H33(k)+Jeq3)*n2*RT2*Tc2+RT2*H23(k)*n3*RT3*Tc3);
x3(k+1)=x3(k)+T*x6(k);
RT3*H32(k)*u2(k)-...
end

% Simulaciones:

% A: Posicion
ejex = linspace(0,Nm*T,Nm-3);
figure
subplot(311)
plot(ejex,p1(4:Nm)*180/pi,ejex,x1(4:Nm)*180/pi), grid
title('R-3DOF: Posicion de la base')
ylabel('q_1')
subplot(312)
title('R-3DOF: Posicion de la varilla 1')
ylabel('q_2')
subplot(313)
title('R-3DOF: Posicion de la varilla 2')
ylabel('q_3')
xlabel('t [seg]')

% B: Velocidad
figure
subplot(311)
plot(ejex,x4(4:Nm)), grid
title('R-3DOF: Velocidad de la base')
ylabel('dq_1 rad/seg')

subplot(312)
title('R-3DOF: Velocidad de la varilla 1')
ylabel('dq_2 rad/seg')
subplot(313)
title('R-3DOF: Velocidad de la varilla 2')
ylabel('dq_3 rad/seg'), xlabel('t [seg]')

% C: Seal de control
figure
subplot(311)
plot(ejex,u1(4:Nm)), grid
title('R-3DOF: Seal de control de la base')
ylabel('Voltios')
subplot(312)
title('R-3DOF: Seal de control de la varilla 1')
ylabel('Voltios')
subplot(313)
title('R-3DOF: Seal de control de la varilla 2')
ylabel('Voltios'), xlabel('t [seg]')

Simulacin con Trayectoria Senoidal
robot3dof_p3.m
clear all ; close all; clc

hd = 0.01;
Rd = 0.07; % altura y radio

% M_disco_total = M_disco + M_bocina_Motor1 = 0.3 + 0.25

m_dt = 0.55; % prisma
hp = 0.21; % altura
ap = 0.044; % ancho y largo de la seccion prisma
m_pt = 2.475-0.072;
m_hueco = 0.072;
R_hueco = 0.063;

% Inercia de la base mas servomotor DC 2

I1=0.5*(m_pt*(ap^2)/6 + m_dt*(Rd^2)/2-m_hueco*(R_hueco^2)/2)+0.00059;

% Primera varilla

m2=0.25+0.5+0.05; % masa varilla + masa M2 + masa accesorios
mo=0.025; % Masa del objeto
L2=0.20; % Longitud total
Lcs2=0.10; % Longitud del punto de ref. 2 al centro de
gravedad
I2=m2*(L2*L2)/3; % momento de inercia

% Segunda varilla

m3=0.25; % masa del brazo
mo=0.025; % masa del objeto
L3=0.20; % longitud total
Lcs3=0.10; % longitud del punto de ref. 3 al centro de
gravedad
Io=mo*(ap*ap/6); % momento de Inercia del objeto
Iv=m3*(L3*L3)/3; % momento de Inercia segunda varilla
I3=Iv+Io; % inercia total
g =9.81; % aceleracin de la gravedad

% Datos del sub-sistema elctrico

Km1=0.04364; Kb1=0.04364; Ra1=3.5; Jm1=0.00059; n1=18.5; KA1=2.17;
Km2=0.04364; Kb2=0.04364; Ra2=3.5; Jm2=0.00059; n2=18.5; KA2=2.17;
Km3=0.04217; Kb3=0.04217; Ra3=5.3; Jm3=0.000238; n3=12.5; KA3=2.17;
Cc1=0.001488; Cc3=0.00015;

% Carga

Jeq1 = 0.2177; Beq1 = 0.054;
Jeq2 = 0.3954; Beq2 = 0.0589;
Jeq3 = 0.000305; Beq3 = 0.000538;

% Equivalencia de resistencias

RT1 = Ra1/(n1*Km1*KA1);
RT2 = Ra2/(n2*Km2*KA2);
RT3 = Ra3/(n3*Km3*KA3);

% Equivalencia de N

NT1 = n1*Kb1/KA1;
NT2 = n2*Kb2/KA2;
NT3 = n3*Kb3/KA3;

% Parmetros del Robot

a1=2*I1;
a2=I2 + 2*m2*Lcs2*L2 - m2*L2*L2;
a3=I3 + 2*m3*Lcs3*L3 - m3*L3*L3;
a4=m3*Lcs3*L2;
a5=m3*L2*L2;
a6=m2*g*Lcs2+m3*g*L2;
a7=m3*g*Lcs3;

% Calculo de ganancias del Sistema realimentado: K1, K2, K3, K4, K5,
K6

polo1 = -8; polo2 = -8;
aa=[1 -polo1]; bb=[1 -polo2];
pol1=conv(aa,bb);
K1=pol1(2); K2=pol1(3);

polo3 = -6; polo4 = -6;
aaa = [1 -polo3]; bbb = [1 -polo4];
pol22=conv(aaa,bbb);
K3=pol22(2); K4=pol22(3);

polo5 = -40; polo6 = -40;
aaaa = [1 -polo5]; bbbb = [1 -polo6];
pol33=conv(aaaa,bbbb);
K5 = pol33(2); K6 = pol33(3);

% Condiciones y prametros iniciales

p1(1)=0; p1(2)=0; p1(3)=0; p1(4)=0;
dp1(1)=0; dp1(2)=0; dp1(3)=0; dp1(4)=0;
ddp1(1)=0; ddp1(2)=0; ddp1(3)=0; ddp1(4)=0;
p2(1)=0; p2(2)=0; p2(3)=0; p2(4)=0;
dp2(1)=0; dp2(2)=0; dp2(3)=0; dp2(4)=0;
ddp2(1)=0; ddp2(2)=0; ddp2(3)=0; ddp2(4)=0;
p3(1)=0; p3(2)=0; p3(3)=0; p3(4)=0;
dp3(1)=0; dp3(2)=0; dp3(3)=0; dp3(4)=0;
ddp3(1)=0; ddp3(2)=0; ddp3(3)=0; ddp3(4)=0;
x1(1)=0;x1(2)=0;x1(3)=0;x1(4)=0;
x2(1)=0;x2(2)=0;x2(3)=0;x2(4)=0;
x3(1)=0;x3(2)=0;x3(3)=0;x3(4)=0;
x4(1) =0;x4(2)=0;x4(3)=0;x4(4)=0;
x5(1)=0; x5(2)=0; x5(3)=0; x5(4)=0;
x6(1)=0; x6(2)=0; x6(3)=0; x6(4)=0;

% Tiempo de muestreo
Nm=2000; T =0.010;

% lazo de Control
for k=4:Nm;

% Trayectorias senoidales deseadas: p1, p2 y p3

p1(k)=pi*cos(2*pi*k*T/11);
p2(k)=3*pi/2*cos(2*pi*k*T/11);
p3(k)=2*pi*cos(2*pi*k*T/11);

% Transformacion de coordenadas

z1(k)=x1(k);
z2(k)=x4(k);
z3(k)=x2(k);
z4(k)=x5(k);
z5(k)=x3(k);
z6(k)=x6(k);

% Aproximacion de derivadas para discretizacion

dp1(k)=(p1(k)+3*p1(k-1)-3*p1(k-2)-p1(k-3))/(6*T);
ddp1(k)=(dp1(k)+ 3*dp1(k-1)-3*dp1(k-2)-dp1(k-3))/(6*T);
dp2(k)=(p2(k)+ 3*p2(k-1)-3*p2(k-2)-p2(k-3))/(6*T);
ddp2(k)=(dp2(k)+ 3*dp2(k-1)-3*dp2(k-2)-dp2(k-3))/(6*T);
dp3(k)=(p3(k)+3*p3(k-1)-3*p3(k-2)-p3(k-3))/(6*T);
ddp3(k)=(dp3(k)+3*dp3(k-1)-3*dp3(k-2)-dp3(k-3))/(6*T);

% Matriz de inercias Hij:

H11(k)=a1+a2*cos(z3(k))*cos(z3(k))+...
a3*cos(z3(k)+z5(k))*cos(z3(k)+z5(k))+...
2*a4*cos(z3(k))*cos(z3(k)+z5(k))+a5*cos(z3(k))*cos(z3(k));
H22(k)=a2+a3+2*a4*cos(z5(k))+a5;
H23(k)=a3+a4*cos(z5(k));
H32(k)=H23(k);
H33(k)=a3;

% Vector de fuerzas centrifuga y de Coriolis Ci:

C1(k)=-a2*z2(k)*z4(k)*sin(2*z3(k))-...
2*a3*z2(k)*z6(k)*sin(z3(k)+z5(k))*cos(z3(k)+z5(k))...
-2*a4*z2(k)*z6(k)*sin(z3(k)+z5(k))*cos(z3(k));
C2(k)=a2*z2(k)*z2(k)*sin(z3(k))*cos(z3(k))-...
a4*z6(k)*z6(k)*sin(z5(k))-2*a4*z4(k)*z6(k)*sin(z5(k));
C3(k)=a3*z2(k)*z2(k)*sin(z3(k)+z5(k))*cos(z3(k)+z5(k))+...
a4*z2(k)*z2(k)*cos(z3(k))*sin(z3(k)+z5(k))+...
a4*z4(k)*z4(k)*sin(z5(k));

% Vector de fuerza gravitacional di:

d1(k)=0;
d2(k)=a6*cos(z3(k))+a7*cos(z3(k)+z5(k));
d3(k)=a7*cos(z3(k)+z5(k));

% Nuevas entradas equivalentes: v1, v2 y v3

v1(k)=K2*p1(k)-K1*z2(k)-K2*z1(k);
v2(k)=K4*p2(k)-K3*z4(k)-K4*z3(k);
v3(k)=K6*p3(k)-K5*z6(k)-K6*z5(k);

% Ley de Control u1, u2 y u3
% Valores de P1, P2, P3

P1(k)=(RT1*Beq1+NT1)*z2(k)+RT1*C1(k);

% Primer actuador: base

u1(k) = P1(k) + v1(k)*RT1*(H11(k)+Jeq1);

% Compensando friccion de Coulomb

u1(k) = u1(k) + Cc1*n1*RT1*x4(k);
if(u1(k) > z2(k))
u1(k) = u1(k) + Cc1*n1*RT1*z2(k);
u1(k) = u1(k) - Cc1*n1*RT1*z2(k);
end


if(u1(k) > 10)
u1(k)=10;
elseif(u1(k) < -10)
u1(k)=-10;
end

% Segundo actuador: primera varilla

u2(k) = P2(k) + v2(k)*RT2*(H22(k)+Jeq2) + v3(k)*RT2*H23(k);


u2(k) = u2(k) + Cc1*n2*RT2*x5(k);
if(u2(k) > z4(k))
u2(k) = u2(k) + Cc1*n2*RT2*z4(k);
u2(k) = u2(k) - Cc1*n2*RT2*z4(k);
end


if(u2(k) > 10)
u2(k) = 10;
elseif(u2(k) < -10)
u2(k) = -10;
end

% Tercer actuador: segunda varilla

u3(k) = P3(k) + v2(k)*RT3*H32(k) + v3(k)*RT3*(H33(k)+Jeq3);

% Compensando friccion de Coulomb

u3(k) = u3(k) + Cc3*n3*RT3*x6(k);
if(u3(k) > z6(k))

u3(k) = u3(k) + Cc3*n3*RT3*z6(k);
u3(k) = u3(k) - Cc3*n3*RT3*z6(k);
end


if(u3(k) > 10)
u3(k) = 10;
elseif(u3(k) < -10)
u3(k) = -10;
end

% Proceso de discretizacion

x1(k+1)=x1(k)+T*x4(k);
x4(k+1)=x4(k)+(T*(-n1*RT1*Tc1-P1(k)+u1(k)))/(RT1*(H11(k)+Jeq1));
Det(k)=RT2*RT3*((H22(k)+Jeq2)*(H33(k)+Jeq3)-H23(k)*H23(k));
x2(k+1)=x2(k)+T*x5(k);
RT2*H23(k)*u3(k)-...
x3(k+1)=x3(k)+T*x6(k);
RT3*H32(k)*u2(k)-...
end

% Simulaciones
ejex = linspace(0,Nm*T,Nm-3);

figure
subplot(311)
plot(ejex,p1(4:Nm)*180/pi,ejex,x1(4:Nm)*180/pi,'k:'), grid
ylabel('q_1')
legend('q_{d1}','q_1',-1)
subplot(312)
ylabel('q_2')
legend('q_{d2}','q_2',-1)
subplot(313)
ylabel('q_3')
xlabel('t [seg]')
legend('q_{d3}','q_3',-1)

figure
subplot(311)
ylabel('[Rad/Seg.]')
subplot(312)
ylabel('[Rad/Seg.]')
subplot(313)

ylabel('[Rad/Seg.]'), xlabel('t [seg]')

figure
subplot(311)
ylabel('[Voltios]')
subplot(312)
ylabel('[Voltios]')
subplot(313)
ylabel('[Voltios]'), xlabel('t [seg]')

Borra Dor

Загружено:

Сведения о документе

Авторское право

Доступные форматы

Поделиться этим документом

Поделиться или встроить документ

Параметры публикации

Этот документ был вам полезен?

Это неприемлемый материал?

Авторское право:

Доступные форматы

Borra Dor

Загружено:

Авторское право:

Доступные форматы

UNIVERSIDAD NACIONAL DEL ALTIPLANO PUNO

FACULTAD DE INGENIERA MECNICA ELCTRICA, ELECTRNICA Y SISTEMAS

relaciona al sistema de coordenadas (j - 1)-simo con

toma los siguientes valores:

) es el centro de gravedad (CG) del eslabn i

es la masa del cuerpo i,

son los productos

+ es el vector gravedad fila expresado

es el vector de fuerzas y torques aplicado a cada

. Los campos vectoriales de inters para nosotros son los denominados

y un campo vectorial suave

, nosotros podemos definir una nueva funcin escalar

son campos vectoriales,

son constantes escalares y

definida en una regin , se denomina un

define un difeomorfismo local

es el vector de control (la variable manipulada) del

es el vector de estado del proceso de

es el vector de salida del proceso (la variable

corresponden a campos vectoriales infinitamente diferenciables.

es completamente integrable, si y solo si existen (n-m) funciones

es involutivo, si y solo si existen funciones escalares

] es involutivo, implica que para

] un conjunto de campos vectoriales

, entonces se obtiene la relacin deseada entre y ; en caso contrario,

, entonces se tiene la relacin deseada. En caso contrario,

. Por consiguiente, tal

, cumple con las condiciones de una transformacin no lineal;

, de modo tal que sea un difeomorfismo. Con ello, la

son funciones suaves que dependen del nuevo estado z y no de

] aplicados a procesos lineales se

exista. Notar que tambin hemos

, se determinan a partir de las races deseadas

Estas races conforman la ecuacin caractersticas del

] es una combinacin lineal del

para todo x, necesitamos

est relacionado con la i-esima salida del

, requeridas para que las componentes de u no

es la versin multivariable de la condicin

, es una nueva entrada a ser

es la i-sima trayectoria deseada e

es el i-sima seal de error

: Posicin y orientacin del sistema de referencia 1 (S

: Posicin y orientacin del sistema del sistema de referencia 2

: Posicin y orientacin del sistema de referencia 3 (S

: Posicin y orientacin del sistema de referencia 2 (S

: Posicin y orientacin del sistema de referencia 3 (S

se determina mediante matemtica simblica de

, (ver Figura 3.2), est dado por:

, (ver Figura 3.3), est dado por:

es la longitud del eje de referencia 1 (S

es la longitud del eje de referencia 1 (S

es la longitud del eje de referencia 2 (S

aplicado a la armadura est

es la relacin de piones. El torque

requerido por el actuador

en (3.11) y reemplazando en (3.22), obtenemos la

Posicin angular de la base

Posicin angular de la primera varilla

Posicin angular de la segunda varilla

Velocidad angular de la base

Velocidad angular de la primera varilla

Velocidad angular de la segunda varilla

, se determinan las nuevas