Robotica

See discussions, stats, and author profiles for this publication at: https://www.researchgate.
net/publication/280444733
Análisis, Diseño y Control de un Manipulador Paralelo 3-SPS-1S Utilizado en

Aplicaciones de Seguridad
Thesis · June 2015

DOI: 10.13140/RG.2.1.2158.3206
CITATIONS READS
0 838
3 authors:
Daniel Chaparro Altamirano Ricardo Zavala-Yoe

Tecnológico de Monterrey Tecnológico de Monterrey
8 PUBLICATIONS 23 CITATIONS 74 PUBLICATIONS 120 CITATIONS
SEE PROFILE SEE PROFILE
Ricardo A. Ramírez-Mendoza
Tecnológico de Monterrey
310 PUBLICATIONS 646 CITATIONS
SEE PROFILE
Some of the authors of this publication are also working on these related projects:
Modelling and control of magnetorheological suspensions in cars View project
Behavioral Approach for Systems and Control and Numerical Linear Algebra View project
All content following this page was uploaded by Ricardo A. Ramírez-Mendoza on 26 July 2015.
The user has requested enhancement of the downloaded file.

Instituto Tecnológico y de Estudios
Superiores de Monterrey
Tesis de Maestría
Análisis, Diseño y Control de un

Manipulador Paralelo 3-SPS-1S Utilizado
en Aplicaciones de Seguridad
Asesor:
Autor: Dr. Ricardo Zavala Yoe
Daniel Chaparro Co-asesor:
Altamirano Dr. Ricardo Ambrocio
Ramirez Mendoza
Tesis presentada en cumplimiento de los requisitos

para el grado de Maestro en Ciencias de la Ingeniería
en la
Escuela de Diseño, Ingenieniería y Arquitectura
23 de julio de 2015
INSTITUTO TECNOLÓGICO Y DE ESTUDIOS SUPERIORES DE MONTERREY
Abstract
Escuela de Diseño, Ingenieniería y Arquitectura
Maestro en Ciencias de la Ingeniería
Análisis, Diseño y Control de un Manipulador Paralelo 3-SPS-1S Utilizado

en Aplicaciones de Seguridad
por Daniel Chaparro Altamirano
La presente tesis presenta un método geométrico para obtener la cinemática directa

del robot 3-SPS-1-S. También presenta un método que utiliza el método de Newton-
Raphson y una red neuronal artificial para obtener la cinemática directa de manera
local, esto es, el resultado de la cinemática directa más cercano a la posición actual de
la plataforma. Se muestra una forma de representar el espacio de trabajo, así como la
manera de encontrarlo. Se diseñó un algoritmo genético capaz de aumentar el espacio de
trabajo del robot y de mantener los parámetros del mismo lo más cercanos a un valor
deseado. Se muestra la forma de obtener las matrices jacobianas, así como las condiciones
que al cumplirse hacen que el robot se encuentre en un punto singular. Se presenta
también la solución de la dinámica inversa mediante el método de trabajo virtual, y se
adapta el método que utiliza Merlet para obtener la dinámica del robot 6-UPS al robot
3-SPS-1-S. Se diseñaron dos controles sencillos y se mostró la importancia de tener
una trayectoria con varios puntos. Por último se presentan dos métodos para calcular
esta trayectoria de posición y dos métodos para obtener las trayectorias de velocidad y
aceleración.
En apoyo a los cálculos, se diseñó un modelo del robot 3-SPS-1-S en Solidworks, que es
un software de dibujo mecánico en 3D, en donde se simuló el movimiento y se comprobó
que esta arquitectura presenta solamente movimientos rotacionales. Por último, se imple-
mentaron todos los algoritmos y modelos en MATLAB para poder realizar simulaciones
y obtener gráficas.
Agradecimeintos
ii
Contenido
Abstract i
Agradecimeintos ii
Lista de Figuras vi
Lista de Tablas viii
1. Motivación 1
1.1. Trabajo a futuro . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2
2. Introducción 3
2.1. Cadenas cinemáticas, mecanismos y máquinas . . . . . . . . . . . . . . . . 3
2.2. Grados de libertad de un mecanismo . . . . . . . . . . . . . . . . . . . . . 4
2.3. Espacio de trabajo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
2.4. Clasificación de robots manipuladores . . . . . . . . . . . . . . . . . . . . 7
2.4.1. Según sus grados de libertad . . . . . . . . . . . . . . . . . . . . . 7
2.4.2. Según su estructura cinemática . . . . . . . . . . . . . . . . . . . . 8
2.4.3. Según la geometría del espacio de trabajo . . . . . . . . . . . . . . 8
2.5. Arquitectura y configuración . . . . . . . . . . . . . . . . . . . . . . . . . . 9
2.6. Cinemática directa e inversa . . . . . . . . . . . . . . . . . . . . . . . . . . 9
2.7. Robots paralelos . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
3. Manipuladores paralelos esféricos 14

3.1. 3-RRRRR . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
3.1.1. 3-RUU . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
3.1.2. 3-URU . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17
3.1.3. 3-RSR . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18
3.2. 3-UPU . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19
3.3. 3-RRR . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19
3.4. 3-SPS-1-S . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20
4. Cinemática 23
4.1. Cinemática Inversa . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23
4.2. Cinemática directa . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27
4.2.1. Método geométrico . . . . . . . . . . . . . . . . . . . . . . . . . . . 28
4.2.2. Newton-Raphson y redes neuronales artificiales . . . . . . . . . . . 31
iii
Contenido iv
5. Espacio de trabajo 38
5.1. Cálculo del espacio de trabajo . . . . . . . . . . . . . . . . . . . . . . . . . 39
5.1.1. Longitud permitida . . . . . . . . . . . . . . . . . . . . . . . . . . . 39
5.1.2. Ángulo máximo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40
5.1.3. Colisión entre patas . . . . . . . . . . . . . . . . . . . . . . . . . . 41
5.2. Optimización mediante un algoritmo genético . . . . . . . . . . . . . . . . 44
6. Análisis Jacobiano 50
6.1. Singularidades . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51
7. Dinámica 54
7.1. Dinámica Inversa . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54
7.1.1. Trabajo virtual (Principio de d0 Alembert) . . . . . . . . . . . . . . 54
7.1.2. Método presentado por Merlet . . . . . . . . . . . . . . . . . . . . 64
7.2. Dinámica Directa . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 69
8. Control de posición 72
8.1. Control PID . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 72
8.2. Control PD difuso . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 74
9. Trayectoria 80
9.1. Trayectorias de posición . . . . . . . . . . . . . . . . . . . . . . . . . . . . 80
9.1.1. Interpolación lineal . . . . . . . . . . . . . . . . . . . . . . . . . . . 80
9.1.2. Mapa de caminos probabilístico (PRM) . . . . . . . . . . . . . . . 81
9.2. Trayectorias de velocidad y aceleración . . . . . . . . . . . . . . . . . . . . 85
9.2.1. Movimiento uniformemente acelerado . . . . . . . . . . . . . . . . . 85
9.2.2. Trayectoria polinomial . . . . . . . . . . . . . . . . . . . . . . . . . 88
10.Conclusiones 94
A. Códigos de MATLAB 95
A.1. Cinemática inversa . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 95
A.2. Cinemática directa por método geométrico . . . . . . . . . . . . . . . . . . 96
A.3. Cinemática directa por Newton-Raphson y RNA . . . . . . . . . . . . . . 102
A.3.1. Entrenamiento de red neuronal . . . . . . . . . . . . . . . . . . . . 103
A.3.2. Método de Newton-Raphson . . . . . . . . . . . . . . . . . . . . . . 104
A.3.2.1. Función f utilizada en el método de Newton-Raphson . . 107
A.3.2.2. Función df utilizada en el método de Newton-Raphson . . 108
A.4. Cálculo del espacio de trabajo . . . . . . . . . . . . . . . . . . . . . . . . . 109
A.4.1. Condición de longitud . . . . . . . . . . . . . . . . . . . . . . . . . 114
A.4.2. Condición de ángulo . . . . . . . . . . . . . . . . . . . . . . . . . . 115
A.4.3. Condición de colisión . . . . . . . . . . . . . . . . . . . . . . . . . . 116
A.5. Algoritmo genético . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 118
A.5.1. Función de aptitud . . . . . . . . . . . . . . . . . . . . . . . . . . . 119
A.5.2. Selección mediante torneo . . . . . . . . . . . . . . . . . . . . . . . 120
A.5.3. Recombinación genética . . . . . . . . . . . . . . . . . . . . . . . . 121
A.5.4. Mutación . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 122
Contenido v
A.5.5. Decodificación de cromosomas . . . . . . . . . . . . . . . . . . . . . 123

A.6. Cálculo de singularidades . . . . . . . . . . . . . . . . . . . . . . . . . . . 124
A.7. Dinámica inversa mediante trabajo virtual . . . . . . . . . . . . . . . . . . 125
A.8. Dinámica inversa mediante trabajo virtual para robot 6-UPS . . . . . . . 128
A.9. Dinámica inversa mediante método de Merlet . . . . . . . . . . . . . . . . 132
A.10.Dinámica directa . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 134
A.11.Control PID . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 136
A.11.1. Cálculo de ángulos de Euler mediante dinámica directa . . . . . . . 137
A.12.Control difuso . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 138
A.13.Cálculo de trayectoria de posición por interpolación lineal . . . . . . . . . 139
A.14.Cálculo de trayectoria de posición por PRM . . . . . . . . . . . . . . . . . 141
A.15.Cálculo de trayectoria de velocidad y aceleración mediante MUA . . . . . 147
A.16.Cálculo de trayectoria de velocidad y aceleración mediante MUA y función
polinomial . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 148
A.17.Cálculo de trayectoria de velocidad y aceleración mediante funciones po-
linomiales . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 150
A.18.Programa general del robot . . . . . . . . . . . . . . . . . . . . . . . . . . 152
A.18.1. Gráficas del robot . . . . . . . . . . . . . . . . . . . . . . . . . . . 157
A.18.1.1. Cálculo de coordenadas del punto b1 . . . . . . . . . . . . 168
Bibliografía 169
Lista de figuras
2.1. a) Unión rotacional y b) unión prismática [4]. . . . . . . . . . . . . . . . . 4

2.2. Robot planar con dos grados de libertad. . . . . . . . . . . . . . . . . . . . 5
2.3. Robot PUMA. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
2.4. Representación del espacio de trabajo de orientación [23]. . . . . . . . . . 7
2.5. Robot SCARA [23]. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8
2.6. Plataforma Stewart-Gough [29]. . . . . . . . . . . . . . . . . . . . . . . . . 9
2.7. Robot 6-PUS [29]. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
2.8. Robot Delta [2]. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
3.1. Manipuladores con tres movimientos rotacionales . . . . . . . . . . . . . . 15

3.2. Robot 3-RUU . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16
3.3. Condiciones necesarias para tener movimientos rotacionales en el robot
3-RUU [8]. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16
3.4. Robot DYMO en (a) Modo de traslación y (b) modo de rotación [38]. . . 17
3.5. Robot 3-URU . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17
3.6. Robot 3-RSR [9]. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18
3.7. Pata i de un robot de orientación 3-RSR [9]. . . . . . . . . . . . . . . . . 18
3.8. Robot 3-UPU [23]. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19
3.9. Robot 3-RRR [23]. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20
3.10. Robot 3-SPS-1S . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20
3.11. Robot 3-UPS-1S. Los cuadros negros representan articulaciones universa-
les [23]. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21
3.12. Robot 3-SPS-1S [5]. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21
3.13. Modelo del robot 3-SPS-1S utilizado. . . . . . . . . . . . . . . . . . . . . . 22
4.1. Manipulador 3-SPS-1-S . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24

4.2. Ángulos γa/bi de la base (plataforma) . . . . . . . . . . . . . . . . . . . . . 25
4.3. Representación gráfica de ai , bi y ABi . . . . . . . . . . . . . . . . . . . . 26
4.4. Longitudes calculadas mediante cinemática inversa . . . . . . . . . . . . . 27
4.5. Esferas que representan todas las posibles ubicaciones de Bi . . . . . . . . 28
√
4.6. Pares de puntos de los círculos 1 y 2 con una separación de rp 3 . . . . . 29
4.7. Resultado de la cinemática directa por el método geométrico . . . . . . . . 30
4.8. Modelo de una neurona [28]. . . . . . . . . . . . . . . . . . . . . . . . . . . 33
4.9. Gráfica de algunas funciones de activación comunes: a) Lineal: f (x) =
ex − e−x
x. b) Tangente hiperbólica: f (x) = x . c) Sigmoidal: f (x) =
e + e−x
2
− 1. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33
1 + e−x
4.10. Red neuronal artificial utilizada para hacer la aproximación inicial . . . . 36
vi
Lista de Figuras vii
4.11. Ángulos calculados mediante cinemática directa . . . . . . . . . . . . . . . 36

4.12. Ángulos calculados mediante cinemática directa . . . . . . . . . . . . . . . 37
5.1. Espacio de trabajo con h = 20 cm, rb = 15 cm y rp = 15 cm . . . . . . . . 43

5.4. Espacio de trabajo con h = 25.17 cm, rb = 10.98 cm y rp = 13.54 cm . . . 47
6.1. Singularidades del robot en el espacio de configuraciones de la plataforma 53
7.1. Ángulos de la pata i [34]. . . . . . . . . . . . . . . . . . . . . . . . . . . . 59

7.2. Trayectoria utilizada para encontrar la dinámica inversa . . . . . . . . . . 67
7.3. Fuerzas calculadas mediante los dos métodos de dinámica inversa . . . . . 68
7.4. Comparación de fuerzas calculadas para el robot 6-UPS . . . . . . . . . . 69
7.5. Aceleraciones calculadas mediante dinámica directa . . . . . . . . . . . . . 71
7.6. Trayectoria generada mediante dinámica directa . . . . . . . . . . . . . . . 71
8.1. Posición de la plataforma y error en actuadores con un controlador PID . 73

8.2. Evaluación de reglas de control de temperatura. . . . . . . . . . . . . . . . 75
8.3. Funciones de pertenencia de la entrada derr . . . . . . . . . . . . . . . . . 76
8.4. Funciones de pertenencia de la entrada d˙err . . . . . . . . . . . . . . . . . 76
8.5. Funciones de pertenencia de la salida f . . . . . . . . . . . . . . . . . . . . 77
8.6. Posición de la plataforma y error en actuadores con controlador difuso . . 78
8.7. Posición de la plataforma y error en actuadores con ambos controladores . 78
8.8. Posición de la plataforma y error en actuadores con ambos controladores . 79
9.1. Ejemplo de trayectoria mediante interpolación lineal . . . . . . . . . . . . 81

9.2. Cálculo de la trayectoria mediante PRM . . . . . . . . . . . . . . . . . . . 84
9.3. Ejemplo de trayectoria mediante PRM . . . . . . . . . . . . . . . . . . . . 85
9.4. Trayectoria mediante MUA con 14 puntos intermedios . . . . . . . . . . . 87
9.5. Trayectoria mediante MUA con 9 puntos intermedios . . . . . . . . . . . . 87
9.6. Trayectoria mediante PRM y MUA . . . . . . . . . . . . . . . . . . . . . . 88
9.7. Trayectoria mediante interpolación lineal y MUA con final polinomial . . . 90
9.8. Trayectoria mediante PRM y MUA con final polinomial . . . . . . . . . . 90
9.9. Trayectoria polinomial (interpolación lineal) . . . . . . . . . . . . . . . . . 92
9.10. Trayectoria polinomial (PRM) . . . . . . . . . . . . . . . . . . . . . . . . . 93
9.11. Trayectoria polinomial (PRM) cruzando una posición singular . . . . . . . 93
Lista de tablas
3.1. Combinaciones posibles de articulaciones para tener 3 GDL en la plataforma. 15
4.1. Problemas que se resuelven mediante RNA. . . . . . . . . . . . . . . . . . 34
5.1. Comparación de ángulos máximos y mínimos . . . . . . . . . . . . . . . . 49
8.1. Matriz de asociación difusa del controlador PD . . . . . . . . . . . . . . . 76
viii
Capítulo 1
Motivación
Existen muchas actividades en la industria que son peligrosas para las personas, otras
en donde las actividades son muy repetitivas y otras en las cuales se necesita una gran
precisión y velocidad. Para realizar este tipo de actividades se han desarrollado varios
tipos de robots, los cuales se dividen en dos grandes tipos de acuerdo al tipo de cadena
cinemática que los constituye: seriales y paralelos.
Los robots seriales se han utilizado más que los paralelos, razón por la cual se han
estudiado más, sin embargo, hay ciertas ventajas que tienen los robots paralelos, las
cuales se mencionarán más adelante.
Hay ciertas configuraciones de robots paralelos que sirven de una mejor manera que otros
en una aplicación específica, sin embargo, incluso teniendo la mejor arquitectura para una
cierta aplicación, si el tamaño del robot no es el adecuado, puede tener un rendimiento
menor que un manipulador con una peor arquitectura para esa aplicación, pero con un
tamaño adecuado [23]. Por esta razón uno de los objetivos de este trabajo es diseñar un
método que genere un tamaño óptimo del robot.
La inseguridad es un problema en el mundo y hoy en día en México se ha convertido en

una gran preocupación para la mayoría de los ciudadanos. Para disminuir este problema
el gobierno ha tomado ciertas medidas, sin embargo, hay una gran corrupción que evita
que se resuelva este problema, por ejemplo, en febrero del 2012 se fugaron alrededor
de 30 presos del penal de Apodaca en Nuevo León y asesinaron a aproximadamente 40
personas con ayuda de los guardias de la prisión.
Por esta razón contar con sistemas de seguridad automatizados se ha convertido en una
necesidad, ya que se ha llegado a un punto en donde no se puede confiar ni siquiera en
los elementos de seguridad del país.
1
Capítulo 1. Motivaón 2
Es por esto que este proyecto busca diseñar un sistema para detectar, apuntar y disparar
hacia las personas que se encuentren en una zona prohibida con una pistola de aire
comprimido y balas de gas pimienta, de tal manera que sean neutralizados por unos
momentos mientras los elementos de seguridad son alertados.
Este tipo de armas por lo general no son peligrosas, sin embargo, han existido algunos
casos en donde alguna persona ha muerto a causa de un disparo en zonas críticas, como
el cuello o el ojo, con este tipo de armas. Por esta razón es muy importante generar un
sistema que sea capaz de calcular el lugar donde se encuentra el pecho de la persona y
disparar exactamente ahí para evitar que el disparo pueda causar la muerte.
Esta gran precisión que se busca hace que la mejor opción para utilizar en este sistema
sea un manipulador paralelo que controle el movimiento de la pistola, ya que cuentan
con una gran precisión.
Para esta aplicación se necesita solamente orientar el arma, por lo que un robot con tres
grados de libertad, el cual pueda orientar su efector final es suficiente. Este tipo de robots
ha sido menos estudiado que los de seis grados de libertad. La mayoría de este tipo de
robots cuenta con ciertas restricciones de manufactura y ensamble, es por esto que se
decidió estudiar la variante 3-SPS-1-S, la cual no cuenta con estas desventajas.
1.1. Trabajo a futuro
En este trabajo se estudia a fondo el robot 3-SPS-1-S, pero para completar la aplicación
deseada, es necesario identificar a los objetivos mediante alguna técnica de visión artificial
para calcular el punto al que se debe orientar el robot antes de disparar.
Además de esto es necesario generar una interfaz para comunicar al robot con el sistema
de visión, y una interfaz de usuario para interactuar con el robot.
Capítulo 2
Introducción
En este capítulo se revisarán algunos conceptos básicos de mecanismos que son utilizados
en la robótica, así como algunos conceptos de esta área que se deben de conocer ya que
serán utilizados en este trabajo.
2.1. Cadenas cinemáticas, mecanismos y máquinas
Una cadena cinemática es un ensamble de eslabones que se conectan mediante juntas.

Cuando cada eslabón de una cadena cinemática se encuentra conectado a los demás por
un solo camino, se dice que es una cadena cinemática cerrada. Si en cambio, cada eslabón
se encuentra conectado a los demás por al menos dos caminos, se dice que es una cadena
cinem’tica cerrada [34].
Existen distintos tipos de uniones o juntas, sin embargo, las más usadas son:
Unión rotacional (R). Esta unión permite solamente rotación sobre un eje.
Unión universal (U ). Esta unión permite rotación sobre dos ejes.
Unión prismática (P ). Esta unión permite traslación sobre un eje.
Unión cilíndrica (C ). Esta unión permite rotación y traslación sobre el mismo

eje.
Unión esférica (S ). Esta unión permite rotación sobre los tres ejes.
Un mecanismo es un ensamble de sólidos rígidos o elásticos, que interactúan entre

sí mediante distintos tipos de uniones o articulaciones, con la finalidad de transmitir
3
Capítulo 2. Introducción 4
Figura 2.1: a) Unión rotacional y b) unión prismática [4].
energía mecáica [25]. También se puede decir que un mecanismo es una cadena cinemática
que tiene al menos uno de sus extremos fijos en una superficie, comunmente llamda base.
Una máquina es un ensamble de uno o varios mecanismos, junto con otros componentes
eléctricos y/o hidráulicos que se usa para transformar algún tipo de energía externa en
trabajo o algún otro tio de energía [34].
El manipulador mecánico es un mecanismo ya que solamente se encarga de transmitir

energía mecánica a través de sus eslabones. Una vez que al manipulador se le agrega un
microcontrolador, sensores, sistemas de visión, etc., entonces se considera una máquina.
Si esta máquina tiene ciertas características se le conoce como robot.
El Robot Institute of America define a un robot industrial como “manipulador programa-

ble multifuncional deseñado para mover materiales, piezas, herramientas o dispositivos
especiales, mediante movimientos variados, programados para la ejecución de distintas
tareas ”[26].
2.2. Grados de libertad de un mecanismo
Un concepto muy importante en el estudio de mecanismos es el de grado de libertad

(GDL). Cuando se habla de uniones, los grados de libertad se refieren a la cantidad de
movimientos independientes que permite realizar la articulación, por ejemplo una unión
R tiene un grado de libertad, mientras que una U tiene dos grados de libertad, a esto
también se le conoce como conectividad. En el caso de un mecanismo, el número de
grados de libertad es el número de variables de posición independientes que se deben de
especificar o definir para poder localizar todas las piezas del mecanismo [3].
La movilidad es el número de grados de libertad del efector final de un robot, en otras

palabras, el número de ejes sobre los cuales se puede trasladar y/o rotar su efector
final. Esto a su vez se relaciona con el número de actuadores que debe tener el robot.
Por ejemplo el robot mostrado en la figura 2.2 tiene dos uniones rotacionales y dos
movimientos en el efector final, por lo que su movilidad es de dos, pero necesita un

actuador en cada una de las dos uniones para lograr esto.
Figura 2.2: Robot planar con dos grados de libertad.
Un robot PUMA como el que se muestra en la figura 2.3 tiene seis grados de libertad, lo
que significa que puede moverse en cualquier dirección del espacio y orientar su efector
final hacia donde desee.
Figura 2.3: Robot PUMA.
Existe una fórmula general conocida como fórmula de Grübler [23] con la cual podemos
calcular la movilidad de un mecanismo en el espacio (3 dimensiones):
n
X
m = 6 (l − n − 1) + di (2.1)
i=1
donde m es la movilidad, n el número de eslabones del mecanismo (incluyendo la base

y efector final), l el número de uniones que hay en el mecanismo y di es la conectividad
de la unión i.
Esta fórmula sirve en la mayoría de los casos, sin embargo, en ocasiones se pueden
obtener resultados falsos. Esto se debe en ocasiones a la geometría de los mecanismos o
relaciones entre articulaciones. Por ejemplo, una plataforma Stewart-Gough 6-SPS nos
da un resultado de 12, pero existen 6 grados de libertad ociosos, uno por cada pata, ya
que las patas pueden girar sobre su propio eje, no obstante, esto no afecta a la posición
de la plataforma.
2.3. Espacio de trabajo
El espacio de trabajo de un robot se compone de todos los puntos del espacio que puede
alcanzar el efector final de un robot, tomando en cuenta las restricciones de tamaño de
los eslabones, movimiento permitido por las articulaciones, colisiones entre eslabones y
limitaciones debido a singularidades.
En robots seriales el espacio de trabajo se representa mediante el volúmen que puede ser
alcanzado por el centro del efector final. En robots paralelos suele ser más complicado,
por lo que se han generado distintas representaciones del espacio de trabajo [23]. Las
más comunes son:
Espacio de trabajo de orientación constante o traslación⇒ Todas las posibles ubi-

caciones que puede alcanzar un punto de operación C dada una orientación.
Espacio de trabajo de orientación⇒ Todas las posibles orientaciones que puede

tener el manipulador cuando C se encuentra en una ubicación fija (figura 2.4).
Espacio de trabajo máximo o alcanzable⇒ Todas las ubicaciones que puede alcanzar
C con al menos una orientación de la plataforma.
Espacio de trabajo de orientación inclusiva⇒ Todas las ubicaciones que puede

alcanzar C con al menos una orientación de la plataforma dentro de un rango de
orientación definido. Es un subconjunto del espacio de trabajo máximo.
Espacio de trabajo de orientación total ⇒ Todas las ubicaciones de C que pueden ser
alcanzadas con todas las orientaciones definidas mediante un rango en los ángulos
de orientación.
Espacio de trabajo diestro⇒ Todas las ubicaciones de C que se pueden alcanzar

con todas las orientaciones posibles.
Espacio de trabajo de orientación total reducido⇒ Todas las ubicaciones de C que

pueden ser alcanzadas con todas las orientaciones definidas por un subconjunto de
los ángulos de orientación.
Figura 2.4: Representación del espacio de trabajo de orientación [23].
2.4. Clasificación de robots manipuladores
Los robots manipuladores se pueden clasificar utilizando varios criterios como número de
grados de libertad, estructura cinemática, tipo de actuadores o espacio de trabajo [34].
2.4.1. Según sus grados de libertad
La primera forma de clasificar a los robots es mediante el número de grados de libertad

que poseen. Por ejemplo, entre los robots con 6 grados de libertad encontramos al robot
PUMA (figura 2.3) , mientras que entre los robots de 4 grados de libertad se encuentra
el robot SCARA (figura 2.5). Aunque un robot con 6 grados de libertad nos permite
posicionar su efector final en cualquier posición y con cualquier orientación, en ocasiones
las aplicaciones no requieren todos los movimientos y por eso se utilizan robots con menos
de 6 grados de libertad.
Figura 2.5: Robot SCARA [23].
2.4.2. Según su estructura cinemática
Otra forma de clasificar a los robots es la manera como se encuentran conectados sus
eslabones. Mediante este criterio podemos dividir a los robots en dos categorías: seriales
y paralelos.
Los robots seriales son aquellos en los cuales cada eslabón se encuentra unido a dos
eslabones, con excepción de la base y el efector final, los cuales se encuentran unidos
solamente a un eslabón. En otras palabras, la base está unida al efector final mediante
una cadena cinemática abierta. Un ejemplo de este tipo de robots es el PUMA que se
observa en la figura 2.3.
Los robots paralelos por otra parte son manipuladores en donde el efector final está
unido a la base a través de varias cadenas cinemáticas independientes. La plataforma
Stewart-Gough, mostrada en la figura 2.6 es uno de los robots paralelos más estudiados.
2.4.3. Según la geometría del espacio de trabajo
Se mencionó que el espacio de trabajo en robots seriales es el volúmen en el cual se puede

mover su efector final. Es posible clasificar a los robots por la forma de este volúmen.
Cuando el espacio de trabajo se representa por un prisma rectangular se denominan
robots cartesianos, si se representa por una esfera se les conoce como esféricos, si se
representa mediante un cilindro, se les llama cilíndricos. El problema con esta clasificación
es que hay robots cuyo espacio de trabajo es muy particular como el robot SCARA,
además se mencionó que en los robots paralelos hay muchos tipos de espacio de trabajo,
lo cual impide que sean clasificados fácilmente mediante este método.
Figura 2.6: Plataforma Stewart-Gough [29].
2.5. Arquitectura y configuración
La arquitectura de un robot se refiere a la estructura mecánica que tiene el mismo. Esto

se refiere al número de eslabones, el tipo de uniones que tienen y la forma en la que se
encuentran acomodados. Por ejemplo tanto el robot 6-UPS como el robot 6-PUS tienen
el mismo número de eslabones y utilizan el mismo tipo de uniones, sin embargo el orden
de las uniones es distinto, uno tiene primero la unión tipo U entre la base y el primer
eslabón de la pata i, mientras que el segundo tiene primero la unión tipo P, lo cual los
convierte arquitecturas distintas.
Por otra parte se le llama configuración a cada una de las distintas combinaciones que
pueden tomar los eslabones de una arquitectura. En otras palabras, son todas las posi-
ciones en las que se puede encontrar el robot.
2.6. Cinemática directa e inversa
La posición y orientación del efector final de un manipulador se relaciona con la posición

de cada uno de sus actuadores.
La cinemática directa busca determinar la posición y orientación de un manipulador

dada la posición de sus actuadores. En el caso de actuadores rotacionales, se conoce el
ángulo, mientras que en los actuadores prismáticos se conoce la longitud.
Figura 2.7: Robot 6-PUS [29].
La cinemática inversa es lo opuesto. Dada la posición y orientación del efector final,

busca encontrar la posición de los actuadores que lleve al efector final a esa posición y
orientación.
Estos dos problemas existen tanto en robots seriales como en robots paralelos, sin em-
bargo, la forma de resolverlos es distinta. En el caso de los robots seriales resolver la
cinemática directa es bastante sencillo, ya que se puede especificar la posición y orienta-
ción de un eslabón respecto al eslabón anterior mediante una matriz de transformación
[4]: " #
AR AP
A B B0
TB = (2.2)
01×3 I1×1
donde A RB representa la rotación del sistema de coordenadas de un eslabón respecto al

eslabón anterior y A PB0 la traslación del sistema de coordenadas.
Obteniendo estas matrices de rotación para cada par de eslabones y multiplicándolas, es

posible obtener la posición del efector final y así resolver la cinemática directa.
La cinemática inversa de robots seriales es un poco más complicada ya que existen varias
combinaciones de posiciones en los actuadores que pueden resultar en una misma posición
y orientación del efector final.
En los robots paralelos sucede lo contrario, la cinemática inversa no es muy complicada

de resolver ya que solamente tiene una solución, mientras que la cinemática directa tiene
varias soluciones.
2.7. Robots paralelos
Se mencionó en la sección 2.4.2 que podemos clasificar a los robots en seriales y paralelos.
Los robots seriales tienen la ventaja de contar con espacios de trabajo mayores que los
robots paralelos, sin embargo presentan ciertos problemas.
En primer lugar tenemos que en promedio la relación entre la carga que soporta un robot
serial y el peso del mismo es de 25 % aproximadamente [23], ya que cada motor debe de
ser capaz de sostener a todos los eslabones que se encuentran entre él y el efector final.
Otro problema es que cada eslabón presenta un error en su posición que puede ser
causado por deformaciones en el mismo debido al peso que está sosteniendo o errores
en los actuadores que se encuentran en las articulaciones. Estos errores pequeños se van
acumulando conforme avanzan los eslabones, y al final se puede convertir en un error
considerable.
Los robots paralelos tienen espacios de trabajo menores que los de los robots seriales, sin
embargo no presentan los problemas antes mencionados ya que los actuadores no necesi-
tan sostener el peso de los demás actuadores. Esto permite que se utilicen actuadores más
pequeños, por lo que la relación entre el peso del robot y la carga que soporta aumenta.
Además de esto los errores debido a las deformaciones no afectan tanto al efector final
ya que no se suman directamente, de hecho se puede dar el caso de que la deformación
de una pata se compense con la deformación en otra pata.
La definición de robot paralelo mencionada en la sección 2.4.2 es bastante general, ya

que incluye manipuladores con más actuadores que grados de libertad en el efector final,
así como manipuladores trabajando en cooperación.
Merlet [23] presenta una serie de puntos que se deben de tomar en cuenta para que un
manipulador se considere como paralelo:
Por lo menos dos cadenas cinemáticas deben de soportar al efector final. Cada una
de estas cadenas debe contar con, por lo menos, un actuador. Debe de existir un
sensor que permita medir la variable actuada.
La movilidad del robot debe de ser igual al número de actuadores.
La movilidad del robot debe de ser cero cuando los actuadores están bloqueados
(no se mueven).
Tomando en cuenta estos puntos se define a un robot paralelo de la siguiente manera:

Un robot paralelo está hecho de un efector final con n grados de libertad, y una base
fija, unidas entre sí por al menos dos cadenas cinemáticas. El movimiento se lleva a cabo
mediante n actuadores simples [23].
Los robots paralelos pueden ser clasificados de diversas maneras, por ejemplo dependien-
do del número de grados de libertad o del tipo de movimiento que presentan. Según el
tipo de movimiento que presentan, los robots paralelos se pueden dividir en tres grupos:
traslacionales, esféricos o de orientación y mixtos.
1. Los robots paralelos de traslación, como su nombre lo indica, son aquellos

cuyo efector final (plataforma) solamente puede realizar movimientos de traslación.
Estos robots de tres grados de libertad pueden trasladarse en cualquier dirección,
un ejemplo es el robot Delta propuesto por Clavel [2].
Figura 2.8: Robot Delta [2].
2. Los robots paralelos esféricos o de orientación son robots que no pueden

mover la posición de su efector final, solamente lo pueden orientar. A este tipo de
robots también se les conoce como muñecas paralelas esféricas (SPW por sus
siglas en inglés) debido al tipo de movimientos que realizan. Por lo general en los
robots de este tipo es necesario que se presenten ciertas condiciones de manufactura
y ensamble para que tengan el movimiento esférico deseado.
3. Los robots paralelos mixtos son aquellos cuyo efector final posee tanto movi-
mientos de traslación como de rotación. A diferencia de los manipuladores paralelos
de traslación y esféricos, este tipo de robots puede tener hasta 6 grados de libertad.
Un ejemplo de este tipo de robots es la plataforma Stewart-Gough mostrada en la
figura 2.6.
Existe un gran número de arquitecturas de robots paralelos y no hay ninguna notación

que sea capaz de distinguirlos a todos, sin embargo la notación que más se utiliza consiste
en utilizar un número para indicar la cantidad de patas, y luego letras que indican el
tipo de articulación que existe entre cada eslabón de las patas. Por ejemplo la plataforma
Stewart-Gough tiene seis patas unidas a la base mediante articulaciones universales, des-
pués se tiene una articulación prismática actuada y por último una articulación esférica
entre el efector final y los últimos eslabones. Si se utiliza esta notación el nombre de este
robot sería 6-UPS.
En ocasiones un robot no tiene todas sus patas iguales, por lo que se escriben primero
unas patas y luego otras, separadas por un guión, por ejemplo el manipulador estudiado
en este trabajo se conoce como 3-SPS-1S. Esto quiere decir que el robot tiene cuatro
patas, tres de las cuales tienen una articulación esférica, seguida de una prismática y por
último una esférica, mientras que la cuarta pata es una unión esférica entre la plataforma
y la base.
Cuando se desea mencionar a las articulaciones que contienen a los actuadores, se coloca
una línea sobre o bajo la letra que corresponde a la articulación actuada, por ejemplo co-
mo en la plataforma Stewart-Gough tenemos que la articulación prismática es la actuada,
podemos escribir su nombre como 6-SPS.
Capítulo 3
Manipuladores paralelos esféricos
Se mencionó en el capítulo 2 que existen manipuladores paralelos de tres grados de

libertad que solamente tienen movimientos de rotación. Este tipo de manipuladores son
muy útiles en aplicaciones donde se necesita orientar algo pesado, una antena por ejemplo,
ya que la relación capacidad de carga/peso del robot es mayor que la de los robots
seriales, por lo tanto es posible mover objetos pesados con mecanismos pequeños. Por
otra parte, este tipo de robots tienen una mayor precisión que los manipuladores seriales,
por lo tanto, en aplicaciones donde se necesitan movimientos muy pequeños, este tipo de
manipuladores son de gran utilidad.
Si utilizamos mecanismos con una base, una plataforma con tres grados de libertad y
movimientos solamente de orientación y tres patas, cada una con dos eslabones, podemos
escribir la fórmula de Grübler como
9
X 9
X
3 = 6 (8 − 9 − 1) + di = di − 12 (3.1)
i=1 i=1
9
X
di = 15 (3.2)
i=1
Si se utilizan robots con sus tres patas iguales se tiene que entre las tres articulaciones
de cada pata deben de existir cinco grados de libertad. Las posibles combinaciones de
articulaciones se muestran en la tabla 3.1.
Usar una combinación de las que se observan en la tabla 3.1, no garantiza que el robot
tendrá tres movimientos rotacionales. La figura 3.1 muestra algunos manipuladores que
sí cumplen con tener solamente tres movimientos rotacionales.
14
Capítulo 3. Manipuladores paralelos esféricos 15
Articulación 1 Articulación 2 Articulación 3 Ejemplos

1 GDL 1 GDL 3 GDL RRS, PRS
1 GDL 3 GDL 1 GDL RSR, PSR
2 GDL 2 GDL 1 GDL UUR, UUP, CUP, UCR
2 GDL 1 GDL 2 GDL URU, UPU, UPC, CRC, CRU
Tabla 3.1: Combinaciones posibles de articulaciones para tener 3 GDL en la platafor-

ma.
Figura 3.1: Manipuladores con tres movimientos rotacionales
3.1. 3-RRRRR
Este tipo de robots cuentan con cinco articulaciones rotacionales ya sea de manera ais-
lada, ordenadas en pares para formar uniones tipo U, o en tercias para formar uniones
tipo S. Podemos encontrar distintas variantes de robots que utilizan cinco articulaciones
rotacionales como los robots 3-RUU, 3-URU y 3-RSR.
3.1.1. 3-RUU
Esta configuración propuesta por Di Gregorio [8] consiste en un robot con tres patas
iguales, cada una con una articulación rotacional y dos universales. La figura 3.2 nos
muestra un robot 3-RUU dibujado en Solidworks.
Esta configuración fue propuesta originalmente como un robot traslacional [10], sin em-
bargo se comporta como un robot de orientación si se presentan ciertas condiciones de
manufactura y ensamblaje:
1. Los tres ejes de rotación montados en la plataforma/base deben de converger en

un punto de la plataforma/base.
Figura 3.2: Robot 3-RUU
2. Los seis ejes rotacionales, colocados en pares en los tres eslabones adyacentes a la
base, deben de converger en un punto de la base.
3. El punto de la plataforma localizado en la intersección de los tres ejes de rotación

(punto P de la figura 3.3) debe coincidir con el punto de la base localizado en la
intersección de los tres ejes rotacionales colocados en la base.
4. En cada pierna los dos ejes de rotación que no convergen a un mismo punto, deben
de ser paralelos entre sí y perpendiculares a la línea que cruza por el centro de la
articulación universal.
Figura 3.3: Condiciones necesarias para tener movimientos rotacionales en el robot

3-RUU [8].
La figura 3.3 nos muestra las condiciones que se deben de cumplir para que el robot
3-RUU tenga movimientos rotacionales.
Este robot presenta algunos problemas al ser utilizado en esta modalidad de orientación.
En primer lugar, está sujeto a condiciones de manufactura muy estrictas, lo cual significa
que pequeños errores en la manufactura de las piezas del robot pueden ocasionar que no
se comporte como se desea. Además existen posiciones singulares que pueden hacer que
el robot tenga movimientos de traslación.
3.1.2. 3-URU
La segunda variante de la estructura 3-RRRRR es la configuración 3-URU. Este robot,

el cual se muestra en la figura 3.5, fue analizado por Zlatanov y al igual que la variante
3-RUU puede presentar movimientos tanto de rotación como de traslación, como se ve
en la figura 3.4, e incluso si se cumplen ciertas características puede combinar traslación
y rotación al mismo tiempo.
Figura 3.4: Robot DYMO en (a) Modo de traslación y (b) modo de rotación [38].
Este robot conocido como Double-Y Multi-Operational o DYMO por sus siglas en inglés,
se comporta como un manipulador de orientación cuando el eje de la primera articulación
de revolución de la base coincide con el eje de la primera articulación de revolución de
la plataforma como se muestra en la figura 3.5.
Figura 3.5: Robot 3-URU
Zlatanov también nos menciona que es posible salirse del modo de rotación y entrar al
modo de traslación sin tener que desarmar el robot [38].
3.1.3. 3-RSR
Una tercera variante de la estructura 3-RRRRR es la 3-RSR propuesta por Kong y

Gosselin [19] e, independientemente, por Karouia y Hervé [14], la cual se observa en la
figura 3.6.
Figura 3.6: Robot 3-RSR [9].
Para que este robot se comporte como un robot de orientación, es necesario que tanto
los ejes de las articulaciones rotacionales de la base como los de la plataforma intersecten
en el mismo punto (P ). La figura 3.7 nos muestra la manera en la que deben de estar
acomodadas las articulaciones de una pata para que los ejes coincidan en el punto p.
Figura 3.7: Pata i de un robot de orientación 3-RSR [9].
Existen un par de problemas con este robot. El primero es que si la manufactura y el

ensamble tienen errores, entonces es posible que los ejes de las articulaciones rotacionales
no coincidan en el punto P, con lo que el robot perdería su movimiento puramente de
rotación. El segundo problema es que como menciona Di Gregorio [9] existen posiciones
singulares en las cuales el punto P se desplaza y el robot adquiere movimientos de
traslación.
3.2. 3-UPU
El robot 3-UPU fue propuesto inicialmente como un robot de traslación [32], sin embargo,
puede ser utilizado como un robot de orientación si existe una disposición de los ejes de
las articulaciones pasivas [16] como se observa en la figura 3.8.
Figura 3.8: Robot 3-UPU [23].
Este robot es muy similar al 3-URU ya que los ejes de la primera articulación rotacional
de cada articulación tipo U deben de coincidir en un mismo punto. En caso de que el
robot no se ensamble adecuadamente, éste puede presentar movimientos de traslación.
3.3. 3-RRR
Los robots mencionados anteriormente contaban con tres articulaciones, en donde la

suma de los GDL de éstas era 5, ya que ese fue el resultado obtenido mediante la fórmula
de Grübler, sin embargo, Gosselin propuso un robot con tres articulaciones de un GDL
cada una, que al igual que los robots ya mencionados, cuenta con tres grados de libertad
rotacionales [13]. Esto se debe a que los eslabones son esféricos y no lineales como en los
otros robots. Podemos observar la arquitectura de este robot en la figura 3.9.
La condición que se debe cumplir para que un robot 3-RRR tenga solamente movimientos
de rotación es que los ejes de las nueve articulaciones rotacionales coincidan en el centro
de la plataforma.
Al igual que en los robots 3-RSR, 3-URU, 3-RUU y 3-UPU, en este robot es necesario
contar con una buena manufactura y ensamble de los componentes, de lo contrario puede
presentar movimientos no deseados.
Figura 3.9: Robot 3-RRR [23].
3.4. 3-SPS-1-S
En [5] se propone el modelo general del robot 3-SPS-1-S el cual tiene tres grados de
libertad y solamente movimientos de rotación. Este robot, el cual se observa en la figura
3.10, cuenta con tres patas actuadas las cuales tienen la estructura SPS pero además tiene
una articulación tipo S entre la plataforma y la base, que funciona como una restricción (y
soporte) y es gracias a ella que el robot solamente puede tener movimientos rotacionales.
Figura 3.10: Robot 3-SPS-1S
Analizando la fórmula de Grübler obtenemos lo siguiente:
10
X 10
X
3 = 6 (8 − 10 − 1) + di = di − 18 (3.3)
i=1 i=1
9
X
di = 21. (3.4)
i=1
Dado que a la articulación central S le corresponden 3 GDL, tenemos que quedan 18

GDL para las tres patas, es decir cada pata debe de tener 6 GDL en sus articulaciones.
Una pata SPS tiene 7 GDL en sus articulaciones, sin embargo uno de éstos es ocioso, es
decir, existe un movimiento local pero no afecta el movimiento de la plataforma, puesto
que se trata de la pata girando sobre su propio eje. Esto significa que se puede utilizar
en las patas tanto la configuración SPS como la configuración UPS, como se observa en
la figura 3.11.
Figura 3.11: Robot 3-UPS-1S. Los cuadros negros representan articulaciones univer-
sales [23].
Podemos notar que esta configuración no está sujeta a condiciones de manufactura o

ensamble y por esa razón se eligió trabajar con esta configuración.
Figura 3.12: Robot 3-SPS-1S [5].

El modelo propuesto por Cui y Zhang se puede observar en la figura 3.12. La unión tipo
S que se encuentra entre la base y la plataforma es el punto sobre el cual va a rotar esta
última.
Si se desea que la plataforma gire sobre su propio eje, debemos de colocar la unión esférica
en el centro de la plataforma como se observa en la figura 3.13.
Figura 3.13: Modelo del robot 3-SPS-1S utilizado.
Esta arquitectura tiene una gran rigidez gracias a la pata central, la cual transmite hacia
la base las fuerzas ejercidas sobre la plataforma (incluyendo el peso de la misma). Esto a
su vez, permite que la fuerza necesaria para mover la plataforma sea menor que en otro
tipo de configuraciones, lo cual se traduce en actuadores de menor costo. Esto, junto con
el hecho de que no se necesite un proceso de manufactura y ensamble tan preciso como en
otros manipuladores paralelos, es la razón por la que se decidió utilizar esta arquitectura
en el presente trabajo.
Capítulo 4
Cinemática
La cinemática estudia el movimiento de los cuerpos sin tomar en cuenta las fuerzas que
lo producen. Al diseñar un robot es muy importante revisar la cinemática del mismo ya
que de esta manera se puede conocer la relación que existe entre la posición del efector
final y la de los actuadores. Dependiendo del tipo de relación que se desee conocer, se
debe resolver la cinemática inversa o la directa.
Antes de iniciar el análisis cinemático del robot es necesario describirlo. Como se men-
cionó en el capítulo 3 el robot 3-SPS-1-S tiene tres patas con uniones esféricas tanto en
la base como en la plataforma. Estas patas están compuestas por dos eslabones unidos
mediante una articulación prismática la cual está actuada, esto quiere decir que para
mover el robot se controla la longitud de la pata mediante este actuador lineal. Por otra
parte, la base y la plataforma se encuentran unidas directamente a través de una pata
central con una articulación esférica.
En la figura 4.1 podemos ver la estructura básica del manipulador 3-SPS-1-S en donde Ai
representa la unión esférica entre la base y el primer eslabón de la pata i, Bi la unión entre
el segundo eslabón de la pata i y la plataforma, O representa el marco de referencia de
la base (fijo), mientras que P representa el marco de referencia de la plataforma (móvil).
4.1. Cinemática Inversa
Cuando se desea conocer la posición de los actuadores dada la orientación de la plata-

forma, entonces el problema cinemático que surge es el de cinemática inversa.
Observando el modelo del robot de la figura 4.1, podemos ver que se cumple la siguiente
suma de vectores:
23
Capítulo 4. Cinemática 24
OP + P Bi − OAi = ABi para i =1, 2, 3 (4.1)
Por otra parte tenemos que:

kABi k = di (4.2)
donde di es la longitud de la pata i.
Figura 4.1: Manipulador 3-SPS-1-S
Para poder resolver la ecuación (4.1) obtenemos las componentes x, y y z de cada ele-
mento de la siguiente manera:
 
0
 
OP =   0 
 (4.3a)
h
 
rp
A   A B
P Bi = RB Rbi 
 0  = RB bi
 (4.3b)
0
 
rb
 
OAi = Rai  0 
 (4.3c)
0
donde Rai y Rbi son matrices de rotación sobre el eje z siendo γa/bi el ángulo que existe
entre el eje x de la plataforma (o de la base) y la línea que va del centro de la plataforma
(o de la base) hasta la pata i como se ve en la figura 4.2. Las matrices se muestran a

continuación:
 
cγai −sγai 0
 
Rai = 
 sγai cγai 0 
 (4.4)
0 0 1
 
cγbi −sγbi 0
 
Rbi =  sγbi
 cγbi 0 
 (4.5)
0 0 1
siendo cα = cos(α) y sα = sin(α)
Figura 4.2: Ángulos γa/bi de la base (plataforma)
Por otra parte AR es la matriz de rotación que representa la orientación del sistema
B
de coordenadas de la plataforma respecto al de la base. Si definimos la rotación de la
plataforma como un giro ψ alrededor del eje z de la plataforma, un giro φ alrededor del
eje v ’(el eje y rotado) y por último un giro ω alrededor del eje u”(el eje x rotado dos
veces), obtenemos la siguiente matriz de rotación:
 
cφcψ −cφsψ sφ
A
 
RB = 
 cωsψ + cψsωsφ cωcψ − sωsφsψ −cφsω 
 (4.6)
sωsψ − cωcψsφ cψsω + cωsφsψ cωcφ
Para manejar una notación más sencilla escribimos las ecuaciones 4.3a. 4.3b y 4.3c de la
siguiente manera:
 
0
 
OP = p =   0 
 (4.7a)
h
 
bxi
 
P Bi = bi = 
 byi

 (4.7b)
bzi
 
axi
 
OAi = ai =  ayi 
 (4.7c)
0
En la figura 4.3 podemos ver la representación gráfica de ai , bi y ABi .
Figura 4.3: Representación gráfica de ai , bi y ABi
Sustituyendo las ecuaciones (4.7a), (4.7b) y (4.7c) en la ecuación (4.1) obtenemos:
       
0 bxi axi bxi − axi
       
ABi =  0  +  byi  −  ayi  =  byi − ayi  (4.8)
       
h bzi 0 bzi + h
Para encontrar la longitud que debe de tener cada actuador(di ) simplemente se necesita
resolver la ecuación (4.2), es decir, encontrar la magnitud del vector presentado en la
ecuación (4.8). La solución de la cinemática inversa se muestra a continuación:
q
di = kABi k = (bxi − axi )2 + (byi − ayi )2 + (bzi + h)2 (4.9)
Figura 4.4: Longitudes calculadas mediante cinemática inversa
En la figura 4.4 se muestra una simulación de la cinemática inversa. En la gráfica superior

observamos el valor de los ángulos de Euler a través del tiempo, mientras que en la gráfica
inferior se muestra la longitud de los actuadores calculada. El código para calcular la
cinemática inversa se muestra en el apéndice A.1. Todos los códigos están escritos en
MATLAB.
Los parámetros utilizados en la simulación son h = 25.17, rb = 10.98, rp = 13.54 y

π
γai = γbi = (i − 1).
3
4.2. Cinemática directa
En el caso de que el valor conocido sea la posición de los actuadores y el valor que se desea
encontrar es la orientación de la plataforma, entonces se debe de resolver el problema de
la cinemática directa.
Gan, Seneviratne y Dias nos muestran un método utilizando la fórmula de Cayley [11]. Al
utilizar este método se obtienen ocho resultados que resuelven la cinemática directa, sin
embargo, algunos de ellos contienen números comlpejos, por lo tanto no son soluciones
que se puedan lograr físicamente. En esta sección se propone un método geométrico
Figura 4.5: Esferas que representan todas las posibles ubicaciones de Bi
para encontrar soluciones a la cinemática directa físicamente posibles y un método para

encontrar una solución única mediante el método de Newton-Raphson, que depende de
la posición actual del manipulador.
4.2.1. Método geométrico
Se sabe que la plataforma gira sobre su centro, razón por la cual los puntos Bi se encon-
trarán forzosamente sobre una esfera con centro en el origen de la plataforma y de radio
rp . Por otra parte se sabe que dado que la pata i con una longitud di gira alrededor del
punto Ai , el punto Bi se encontrará también sobre una esfera centrada en Ai de radio
di . Podemos ver estas esferas en la figura 4.5.
Dado que el punto Bi tiene que estar sobre estas dos esferas al mismo tiempo, el punto
Bi tiene que encontrarse en algún punto en donde estas dos esferas se intersecten. La
intersección de dos esferas forma una circunferencia en el espacio, sobre la cual se debe
encontrar el punto Bi .
La distancia dB entre el punto Bi y el punto Bj , donde i 6= j, es constante en todo

momento: q
dB = (Bix − Bjx )2 + (Biy − Bjy )2 + (Biz − Bjz )2 (4.10)
En el caso particular de este trabajo B1 , B2 y B3 se encuentran separados 120◦ entre

ellos, por lo que si elegimos i = 1 y j = 2, tenemos que Bi = Bb = [rp , 0, 0]T y
1
√
Bb rp 3
Bj = 2 = [− , rp , 0]T , entonces podemos escribir la ecuación (4.10) como
2 2
√ !2
v
u
u 3r 2 3rp √
p
dB = t + = rp 3 (4.11)
2 2
Una vez que se conoce dB , se deben encontrar los puntos pertenecientes al primer círculo
que se encuentren a una distancia dB del segundo círculo. Después se debe buscar para
cada par de puntos si existe un punto perteneciente al tercer círculo cuya distancia a
ambos puntos sea dB . El resultado es un conjunto de pares de puntos pertenecientes al
primer y segundo círculo, como se observa en la figura 4.6.
√
Figura 4.6: Pares de puntos de los círculos 1 y 2 con una separación de rp 3
Si se encuentra un punto en el tercer círculo que cumpla esta condición, entonces se

encontró una solución de la cinemática directa. Al terminar de revisar todos los puntos,
se tendrán todas las posiciones posibles en las cuales se puede encontrar la plataforma
dada una longitud de actuadores dada. En el caso de la figura 4.7 observamos que se
encontraron dos posiciones posibles.
Para encontrar los ángulos de Euler correspondientes a la posición de los puntos Bi

encontrados se puede hacer lo siguiente. En primer lugar definimos una matriz de rotación
RBtemp , como una matriz generada a través de un giro de α sobre el eje x, seguido de un
Figura 4.7: Resultado de la cinemática directa por el método geométrico
giro de β sobre el eje z y por último un giro de γ sobre el eje x :

   
cβ −cγsβ sβsγ ux vx wx
   
RBtemp = 
 cαsβ cαcβcγ − sαsγ −cγsα − cαcβsγ  =  uy
  vy wy 
 (4.12)
sαsβ cαsγ + cβcγsα cαcγ − cβsαsγ uz vz wz
Si bi = [bix , biy , biz ]T es el punto perteneciente al círculo i que resuelve la cinemática

directa, entonces  
B bix
 
bi = RBtemp  Bb  (4.13)
 iy 
0
Si i = 1, entonces podemos escribir la ecuación (4.13) como

     
b1x rp rp cβ
     
b1 = 
 b1y  = RBtemp  0  =  rp cαsβ 
     (4.14)
b1z 0 rp sαsβ
Resolviendo (4.14) para α y β tenemos que

b1x
β = arc cos
rp

b1y
α = arc cos (4.15)
rp sβ
Haciendo lo mismo para i = 2 tenemos

√
  1 3

1

− rp − rp cβ − rp cγsβ
   2    2 2 

  
b2x


 √
 
  √ 
3   − 1 r cαsβ + 3 r (cαcβcγ − sαsγ)
  
b2 = 
 b2y
 = RBtemp  =  (4.16)
 2 rp   p p
 
2 2
 
b2z    


  
   √ 
1 3

0 − rp sαsβ + rp (cαsγ + cβcγsα)
2 2
Resolviendo (4.16) para γ obtenemos

!
2 1
γ = arc cos − √ b2x + rp cβ (4.17)
rp sβ 3 2
Una vez que se conocen α, β y γ, podemos encontrar la matriz RBtemp . Si igualamos

nuestra matriz deseada A RB con la matriz conocida RBtemp , podemos encontrar nuestros
ángulos de Euler y de esta manera finalizar la cinemática directa:
φ = arcsin (wx )

wz
ω = arc cos
cφ

vx
ψ = arcsin − (4.18)
cφ
Un ejemplo de implementación se puede ver en el apéndice A.2.
4.2.2. Newton-Raphson y redes neuronales artificiales
El método geométrico nos da más de una solución dada una longitud de actuadores, por
lo que si se quiere saber la posición a la que llegará la plataforma dada su orientación
inicial y la longitud final de los actuadores, sería necesario analizar la cinemática directa
de toda la trayectoria, eligiendo en cada momento la solución que se encuentre más
cercana a la posición anterior de la plataforma.
Además de esto, el método geométrico implica un gran número de operaciones, por lo

que hacer el cálculo para toda la trayectoria tomaría demasiado tiempo.
Otro método para encontrar la cinemática directa puede ser escribir la ecuación (4.9)
tres veces, una para cada actuador, y así obtener un sistema de tres ecuaciones con tres
incógnitas:
d21 = (bx1 − ax1 )2 + (by1 − ay1 )2 + (bz1 + h)2 (4.19a)

d22 = (bx2 − ax2 )2 + (by2 − ay2 )2 + (bz2 + h)2 (4.19b)
d23 = (bx3 − ax3 )2 + (by3 − ay3 )2 + (bz3 + h)2 (4.19c)
Sabemos que B bi = [B bix , B biy , 0]T , por lo que podemos obtener de la ecuación (4.3b) los
términos bix , biy y biz de la siguiente manera
bix = cφcψ B bix − cφsψ B biy (4.20a)
biy = (cωsψ + cψsωsφ)B bix + (cωcψ − sωsφsψ)B biy (4.20b)
biz = (sωsψ − cωcψsφ)B bix + (cψsω + cωsφsψ)B biy (4.20c)
Sustituyendo las ecuaciones (4.20a), (4.20b) y (4.20c) en las ecuaciones (4.19a), (4.19b)
y (4.19c) tenemos el sistema de ecuaciones listo para resolver, sin embargo no podemos
despejar los ángulos y por lo tanto no podemos resolverlo de manera analítica.
Para poder resolver este sistema es necesario utilizar algún método numérico capaz de
resolver un sistema de ecuaciones no lineales como el método de Newton-Raphson. El
problema con los métodos numéricos es que para que funcionen de una manera correcta
es necesario dar un valor inicial cercano al valor real, el cual no se conoce ya que es lo
que se desea calcular.
Para encontrar el valor aproximado de la cinemática directa y utilizarlo como valor

inicial para el método de Newton-Raphson, es posible utilizar una red neuronal artificial
(RNA), la cual reciba como entradas las longitudes de los actuadores y la posición de la
plataforma y de como salidas los ángulos de Euler.
Una neurona biológica tiene dos componentes principales: axón y dendritas. El funcio-
namiento de una neurona de manera simplificada es el siguiente: una neurona recibe (o
no) impulsos eléctricos a través de las dendritas provenientes de los axones de otras neu-
ronas. La neurona suma los impulsos y si esta suma excede un nivel de umbral, entonces
la neurona manda un impulso a través de se axón [28].
Una neurona artificial imita este comportamiento. Recibe una o varias señales que son
multiplicadas por un peso. La neurona suma todos los valores de sus entradas y evalúa
el resultado en una función de activación. La salida de la neurona es el valor entregado
por esa función de activación. El modelo de una neurona artificial se puede observar en
la figura 4.8.
Figura 4.8: Modelo de una neurona [28].
Existen distintas funciones de activación, algunas de ellas son: lineal, escalón, tangente
hiperbólica, sigmoidal y gaussiana. La figura 4.9 muestra algunas de estas funciones.
Figura 4.9: Gráfica de algunas funciones de activación comunes: a) Lineal: f (x) = x.

ex − e−x 2
b) Tangente hiperbólica: f (x) = x . c) Sigmoidal: f (x) = − 1.
e + e−x 1 + e−x
Una RNA se puede definir como un sistema de mapeo no lineal [27]. Las RNA están
compuestas por varias neuronas artificiales divididas en capas, las cuales se conectan con
las neuronas de las otras capas. La primera capa se conoce como capa de entrada. Las
neuronas de esta capa reciben las entradas de la RNA y constituye la primera etapa de
procesamiento. La última capa se llama capa de salida, la cual entrega los valores de
salida de la RNA. Pueden o no existir capas intermedias entre la capa de entrada y la
capa de salida, las cuales son conocidas como capas ocultas y funcionan como etapas de
procesamiento. Si no existen capas ocultas se dice que la RNA es una red monocapa,
mientras que si la RNA cuenta con capas ocultas se dice que es una red multicapa.
Además de clasificar a las RNA por el número de capas que tiene, existen otras for-
mas de hacerlo, por ejemplo por su tipo de aprendizaje (supervisadas, no supervisadas,
competitivas) o por su estructura (hacia adelante, recurrentes).
Cuando en una RNA las neuronas solamente reciben información de las neuronas de la
capa anterior y entregan información a las de la capa siguiente, se dice que tiene una
estructura de tipo feed-forward o hacia adelante debido a que el flujo de la información
va de una capa hacia la siguiente [27]. En ocasiones la salida de una neurona se conecta
a la entrada de una neurona de una capa anterior, en este caso se dice que es una red
recurrente.
Existen cinco principales problemas para los que se utilizan las RNA [28], los cuales se
muestran en la tabla 4.1.
Tabla 4.1: Problemas que se resuelven mediante RNA.
Problema Descripción
• Aproximación de fun- Aproximar funciones lineales o no lineales.
ciones
• Clasificación Clasificación de datos para encontrar patrones o clasi-
ficación de señales para identificar objetos.
• Agrupamiento no su- Agrupar los datos en un número determinado de gru-
pervisado pos.
• Pronosticación Predecir los siguientes valores de una serie temporal.
• Sistemas de control Modelado y control de sistemas.
Para que una RNA resuelva el problema para el que fue diseñada, es necesario que
pase por una etapa de aprendizaje. El aprendizaje consiste en ajustar los pesos que se
encuentran en la entrada de cada neurona, de tal forma que al final la RNA tenga el
comportamiento deseado. En un aprendizaje supervisado, se le proporciona a la red un
conjunto de valores de entrada y sus respectivas salidas, de tal manera que la red ajuste
sus pesos para imitar esta relación entrada-salida. En un aprendizaje no supervisado no
se tiene esta relación entre entradas y salidas, por lo que la red intenta encontrar patrones
en la entrada para realizar el aprendizaje.
El algoritmo de backpropagation o retropropagación del error nos sirve para encontrar

los pesos en una RNA. Para aplicar este método es necesario contar con varios valores
de entrda y sus respectivos valores de salida, lo que significa que es un aprendizaje
supervisado.
El algoritmo de retropropagación del error calcula el error entre el valor de salida obtenido
y el deseado, el cual se va propagando hacia las neuronas anteriores y encuentra un valor
de error para cada neurona. Mediante estos errores calcula los nuevos pesos de cada
neurona. Este procedimiento se repite para todos los valores de entrenamiento. Se repite
esto hasta que se clasifiquen correctamente todos los valores de entrada o hasta que se
cumpla un criterio de término.
En la tabla 4.1 observamos que una RNA puede apoximar una función no lineal, por lo
que es posible generar un conjunto de datos (longitudes y ángulos de Euler) y entrenar a
una RNA para que calcule los ángulos dadas las longitudes, es decir genere tres funciones
que calculen los ángulos dadas las longitudes de las patas.
En la sección 4.2.1 se mencionó que un conjunto de longitudes [d1 , d2 , d3 ] puede ser el

mismo para distintos ángulos de Euler lo cual nos indica que no existe una función que
dependa de las longitudes de los actuadores y tenga como salida los ángulos de Euler,
ya que una función se puede definir como una regla que asigna a cada elemento de un
conjunto (dominio) un único valor de un segundo conjunto (codominio) y en este caso
no es un valor único.
Este problema se puede resolver generando una función local de la cinemática directa a
través de una RNA. Esto quiere decir que dada la posición actual de la plataforma y la
longitud deseada, se calcule la posición final de la plataforma.
Si se utilizaran los ángulos de Euler anteriores como la posición actual de la plataforma,

sería complicado calcular los pesos de la RNA que funcionaran en la mayoría de los casos,
por esta razón se utiliza la función signo y se aplica sobre el punto b1 , de tal forma que
se obtenga el cuadrante en el cual se encuentra el punto b1 dada una posición de la
plataforma. La función signo es



−1 if x < 0,

sgn(x) := 0 if x = 0, (4.21)



1 if x > 0.

La RNA utilizada se puede observar en la figura 4.10 y consiste en seis entradas (tres
que corresponden a las longitudes de los actuadores y tres indicando el cuadrante de la
plataforma), cuatro neuronas en la capa de entrada, ocho en la capa oculta y tres de
salida, una para cada ángulo.
Al simular el movimiento de el robot a través de diferentes trayectorias de la plataforma

y mediante cinemática inversa encontrar la longitud de los actuadores, se puede obtener
un conjunto de datos de entrenamiento en donde se encuentren las seis entradas y las tres
salidas. Utilizando estos datos es posible entrenar a la RNA mediante la retropropagación
del error.
d1
d2
ω
d3
φ
sgn(b1x )
ψ
sgn(b1y )
sgn(b1z )
Figura 4.10: Red neuronal artificial utilizada para hacer la aproximación inicial
Una vez que la RNA es entrenada, ésta funciona como un método para encontrar una
solución aproximada al sistema de ecuaciones formado por las ecuaciones (4.19a), (4.19b)
y (4.19c), que finalmente puede ser resuelto mediante el método de Newton-Raphson.
Cabe mencionar que la RNA, al ser un instrumento de generalización, puede arrojar un

resultado que no sea lo suficientemente cercano al valor real y ocasione que el método de
Newton-Raphson no encuentre la solución correcta, pero con un buen entrenamiento de
la red podemos disminuir la probabilidad de que eso suceda.
Utilizando las longitudes calculadas mediante cinemática directa y que se observan en la

figura 4.4, se calculó la posición de la plataforma mediante el método de Newton-Raphson
y RNA. La figura 4.11 nos muestra en la parte superior las posiciones originales y en la
parte inferior el error entre la posición calculada y la original.
Figura 4.11: Ángulos calculados mediante cinemática directa

Observamos que en este caso el error es prácticamente cero durante toda la trayectoria,
lo cual significa que la aproximación mediante la RNA fue bastante buena, lo cual facilitó
que el método de Newton-Raphson encontrara una solución adecuada.
Para demostrar que no siempre se encuentra la posición deseada se generó otra trayec-
toria para la posición de la plataforma. Mediante cinemática inversa se calcularon las
longitudes de los actuadores y después mediante cinemática directa se calculó la posición
de la plataforma.
Figura 4.12: Ángulos calculados mediante cinemática directa
El resultado lo podemos ver en la figura 4.12, en donde todos los ángulos son iguales con
excepción del último, cuyo error es de casi 400◦ . Este error se debe a que la aproximación
inicial de ese ángulo no fue muy buena, lo que ocasionó que el método de Newton-Raphson
además de no encontrar la solución, se alejó demasiado de la aproximación inicial. El
código para implementar este método se puede observar en el apéndice A.3.
Capítulo 5
Espacio de trabajo
Encontrar el espacio de trabajo es otro aspecto muy importante al momento de diseñar

un manipulador paralelo, debido a que generalmente cuentan con espacios de trabajo
reducidos. Para una aplicación de seguridad, es importante conocer el espacio de trabajo
ya que de esta manera podemos saber el área de cobertura que va a tener la cámara o el
arma montada sobre el manipulador, y de esta manera saber en dónde se pueden colocar
dentro de una industria, casa, penal, etcétera.
Se mencionó en el capítulo 2 que existen muchas maneras de representar el espacio de

trabajo de un manipulador paralelo.
Una posibilidad para representar el espacio de trabajo es utilizar el espacio de configu-

raciones del efector final, lo cual implica graficar las posibles combinaciones de valores
que pueden tomar los tres ángulos de Euler siendo uno de los ejes de la gráfica el ángulo
ω, otro el ángulo φ y el último el ángulo ψ. Esta representación puede servir para saber
si un robot tiene un mayor espacio de trabajo que otro, sin embargo no podemos utilizar
esta representación junto a un dibujo del robot, lo cual hace que no se entienda tan claro
como cuando se dibuja el espacio de trabajo de un robot traslacional.
Otra posibilidad es calcular la posición de un punto del robot en todas las posibles
orientaciones y graficar estas posiciones. Esta forma nos permite ver de una manera
más clara lo que está sucediendo físicamente con el robot, pero tiene la desventaja de
que puede que el punto elegido tenga la misma posición para dos o más orientaciones
distintas, por lo que no se aprecia el espacio de trabajo completamente. A pesar de esta
desventaja, esta representación puede ser más útil que la del espacio de configuraciones
en algunos casos como el del robot presentado en este trabajo, ya que solamente importa
la posición del punto al que va a estar apuntando el robot, en este caso el punto B1 .
38
Capítulo 5. Espacio de trabajo 39
También se ha representado como el volúmen que se genera mediante todas las rotaciones
posibles de un manipulador paralelo esférico [1].
5.1. Cálculo del espacio de trabajo
Una manera de encontrar el espacio de trabajo es probar para todas las combinaciones
de ω, φ y ψ, y calcular si el punto b1 resultante pertenece al espacio de trabajo. Para
esto es necesario definir la resolución deseada para nuestro espacio de trabajo, ya sea de
10◦ , 1◦ , 0,1◦ , etc.
Para este robot es necesario que en cada punto se cumplan tres condiciones mecánicas:
longitud permitida, ángulo máximo y colisión entre patas. El algoritmo 1 nos muestra el
procedimiento para calcular el espacio de trabajo. El código de este algoritmo se observa
en el apéndice A.4.
Algoritmo 1 Cálculo de los puntos pertenecientes al espacio de trabajo

1: Definir el paso deseado (p).
2: Calcular el rango de valores posibles para los tres ángulos de Euler mediante el
algoritmo 2.
3: para ω = ωmin → ωmax con paso p hacer
4: para φ = φmin → φmax con paso p hacer
5: para ψ = ψmin → ψmax con paso p hacer
6: Revisar condiciones de longitud, ángulo y colisión.
7: si se cumplen las tres condiciones entonces
8: Incluir al punto en el espacio de trabajo.
9: fin si
10: fin para
11: fin para
12: fin para
5.1.1. Longitud permitida
El robot cuenta con tres actuadores lineales que físicamente tienen una longitud máxima
a la que se pueden estirar y una mínima a la que se pueden contraer.
Esta condición se cumple si físicamente posible para el robot estirar o contraer sus ac-
tuadores a la distancia requerida por la orientación deseada. El algoritmo 3 nos muestra
como calcular si se cumple esta condición mientras que el apéndice A.4.1 nos muestra el
código utilizado para el cálculo de la misma.
Algoritmo 2 Cálculo del rango de valores de los ángulos de Euler

1: Definir el paso deseado (p).
2: para todo ángulo de Euler (αi ) hacer
3: αimin = 0, αimax = 0.
4: para j = 1 → 3, j 6= i hacer
5: αj = 0.
6: fin para
7: para αi = 0 → π con paso p hacer
9: si las tres condiciones se cumplen entonces
10: αimax = αi
11: si no
12: romper para
13: fin si
14: fin para
15: para αi = 0 → pi con paso -p hacer
17: si las tres condiciones se cumplen entonces
18: αimin = αi
19: si no
20: romper para
21: fin si
22: fin para
23: fin para
Algoritmo 3 Condición de longitud permitida

1: Establecer la longitud mínima y máxima (lmin , lmax ).
2: para todo actuador hacer
3: Calcular la longitud del actuador (di ) mediante la ecuación (4.9).
4: si di < lmin o di > lmax entonces
5: devolver no cumple
6: fin si
7: fin para
8: devolver sí cumple
5.1.2. Ángulo máximo
Físicamente una articulación de tipo S puede estar hecha de dos formas distintas. La
primera es mediante una rótula que permite el giro de la articulación sobre sus tres ejes.
La segunda es mediante tres articulaciones rotacionales, cada una correspondiendo a uno
de los tres ejes de giro.
El problema con el primer tipo de articulaciones es que generalmente tienen un ángulo de

giro muy pequeño (±15◦ ), razón por la que es más conveniente utilizar tres articulaciones
rotacionales que simulen el movimiento esférico.
Sin importar la articulación que se utilice, el ángulo máximo que puede girar la pata
sobre su eje x y el eje y (siendo z el eje que apunta del punto Ai al punto Bi ) no puede
ser mayor que 90◦ . Dependiendo del diseño mecánico de la articulación este ángulo puede
ser todavía menor, especialmente en el caso de las rótulas, lo cual hay que considerar
para saber si una configuración pertenece al espacio de trabajo.
Sea αij el ángulo entre la pata i y el plano j, β el ángulo máximo que pueden girar las
articulaciones, nj el vector normal al plano j y ui el vector que va del punto Ai a Bi . El
ángulo αij se puede calcular como

|nj · ui |
αij = arcsin (5.1)
|nj | · |ui |
donde  
bxi − axi
 
ui = Bi − Ai = 
 byi − ayi  .
 (5.2)
bzi
Dado que αij es el ángulo entre la pata i y el plano j, pero el ángulo máximo (β) se mide
entre la recta normal al plano j y la pata i, para poder comparar estos ángulos definimos
π
γij = − αij .
2
Tenemos que n1 = [0, 0, 1]T ya que es el vector normal al plano xy. Para calcular n2
necesitamos conocer tres puntos sobre el plano 2. De la ecuación (4.14) podemos obtener
las coordenadas x, y y z de los puntos bi , utilizando estos tres puntos obtenemos el vector
n2 de la siguiente manera
P1 = b1 − b2 (5.3)
P2 = b3 − b2 (5.4)
n2 = P1 × P2 (5.5)
Para ver si se cumple la condición de ángulo máximo hay que seguir los pasos del algorit-
mo 4. En el apéndice A.4.2 podemos ver un ejemplo de código para revisar si se cumple
esta condición.
5.1.3. Colisión entre patas
La tercera condición que se necesita para que una configuración pertenezca al espacio de
trabajo es que las patas no choquen entre ellas. Tsai y Lin presentan un método para
Algoritmo 4 Condición de ángulo máximo

1: Establecer el ángulo máximo (β).
2: para todo actuador hacer
3: para j = 1 → 2 hacer
4: Calcular el ángulo entre el actuador i y la base/plataforma (αij ) mediante la
ecuación (5.1).
π
5: γij = − αij
2
6: si γij > β entonces
8: fin si
9: fin para
10: fin para
detectar colisiones en una plataforma Stewart-Gough [31], el cual puede ser adaptado
al robot 3-SPS-1-S. Para cada conjunto de patas i, j (incluyendo la pata central no
actuada) se debe calcular:
d = |n · (Ai − Aj )| (5.6)
donde d representa la distancia entre las líneas asociadas a las patas i y j y
si × sj
n= , (5.7)
||si × sj ||
bi − Ai
si = . (5.8)
||bi − Ai ||
Si representamos a las patas como cilindros de diámetro D, entonces si d es mayor que el

diámetro de las patas entonces las patas i, j no chocan entre ellas, porque esto significa
que la separación entre las patas es mayor al diámetro de éstas. Si d es menor o igual
que el diámetro de las patas, entonces existe la posibilidad de que choquen.
Podemos escribir las ecuaciones paramétricas de las líneas rectas asociadas a cada una
de las patas como
Li = Ai + (bi − Ai )λi (5.9)
Lj = Aj + (bj − Aj )λj . (5.10)
Los puntos de intersección se pueden encontrar resolviendo un sistema de ecuaciones

(para λi y λj ) dadas las condiciones:
(Li − Lj ) · si = 0 (5.11)
(Li − Lj ) · sj = 0 (5.12)
Una vez que se tienen λi y λj se calcula
∆k = λk (1 − λk ) para k = i, j (5.13)
Para ver si se cumple la condición de colisión entre patas se deben seguir los pasos del
algoritmo 5. Un ejemplo de código para probar esta condición lo podemos encontrar en
el apéndice A.4.3.
Algoritmo 5 Condición de colisión

1: Establecer el diámetro de las patas (D).
2: para i = 1 → 3 hacer
3: para j = i + 1 → 4 hacer
4: Calcular d mediante la ecuación 5.6.
5: si d ≤ D entonces
6: Calcular ∆i y ∆j mediante las ecuaciones (5.9), (5.10), (5.11), (5.12) y
(5.13).
7: si ∆i ≥ 0 y ∆j ≥ 0 entonces
9: fin si
10: fin si
11: fin para
12: fin para
Figura 5.1: Espacio de trabajo con h = 20 cm, rb = 15 cm y rp = 15 cm
Mediante estas tres condiciones se calculó el espacio de trabajo para un robot con los
siguientes parámetros: h = 20 cm, rb = 15 cm, rp = 15 cm, β = 80◦ y D = 3 cm. El
resultado se observa en las figuras 5.1, 5.2 y 5.3.
5.2. Optimización mediante un algoritmo genético
Dado que la principal desventaja de un manipulador paralelo respecto a un manipulador

serial es la gran diferencia en espacios de trabajo, se han propuesto distintas formas
de optimizar los parámetros del robot como el tamaño de la plataforma, base y patas
[15, 18, 22, 35, 37].
En la mayoría de los casos se busca incrementar el espacio de trabajo, en otros casos

mejorar el comportamiento dinámico o la destreza del robot y en otros casos reducir las
singularidades. En este trabajo se desea maximizar el espacio de trabajo del robot pero
a la vez se busca que el tamaño del robot sea lo más parecido a un tamaño propuesto.
Entre los métodos de optimización se encuentran los algoritmos genéticos (AG), los cuales
son métodos que imitan el proceso de evolución natural para encontrar la solución a un
problema mediante las operaciones de selección, recombinación y mutación.
La estructura básica de un AG se muestra en el algoritmo 6 [24].
Algoritmo 6 Algoritmo genético

1: Establecer tamaño de población (n).
2: Generar una población inicial (p) de tamaño n.
3: mientras no se cumpla la condición de término hacer
4: Calcular la función de aptitud de cada cromosoma.
5: Elegir pares cromosomas (padres) mediante un método de selección.
6: Generar nueva generación aplicando el operador de recombinación genética a los
padres.
7: Aplicar la operación de mutación a la nueva generación.
8: Reemplazar total o parcialmente la población con los elementos de la nueva ge-
neración.
9: fin mientras
El diseño de un AG se basa en elegir el tamaño de la población, el tipo de cromosoma,

las condiciones de término, la función de aptitud y los operadores genéticos (selección,
recombinación, mutación y reemplazo).
Algunas condiciones de término pueden ser:
Se encontró una solución que satisface algún criterio mínimo.
Se alcanzó un número definido de generaciones.
La función de aptitud de los nuevos elementos deja de mejorar por varias genera-
ciones.
Un cromosoma es la forma en la que se representan los elementos de la población. Por

ejemplo, en este robot el espacio de trabajo depende de los valores h, rb y rp , por lo que
los elementos de la población se podrían representar como arreglos de tres elementos en
donde cada elemento representa uno de los parámetros. Otra forma de representarlos es
escribiendo el valor de los tres elementos en binario y unir los tres valores en una sola
cadena.
La función de aptitud es una forma de evaluar numéricamente que tan bueno es cada
cromosoma. Retomando el ejemplo de este robot, dado que se quiere aumentar el espacio
de trabajo, la función de aptitud puede ser la suma del rango que pueden alcanzar los
ángulos de Euler.
El operador de recombinación nos sirve para pasar información de los cromosomas de

una generación a los de la siguiente. Existen varios métodos de recombinación [7, 24, 30]
algunos de ellos son:
Un punto: Se selecciona un punto al azar en los padres. El primer hijo tiene la

información del primer padre hasta el punto generado y la información del segundo
a partir de ese punto. El segundo hijo contiene primero la información del segundo
padre y luego la del primer padre.
Dos puntos: Se seleccionan dos puntos al azar. El primer hijo contiene la informa-
ción del padre hasta el primer punto, después la información del segundo padre
entre los dos puntos y después la información del primer padre a partir del segundo
punto. Con el otro hijo sucede lo opuesto.
El operador de selección se utiliza para elegir al par de cromosomas sobre el cual se va

a aplicar la recombinación genética. Existen varios métodos de selección que evalúan la
función de aptitud de los elementos de la población. Los elementos con valores mayores
tienen más probabilidad de ser elegidos.
El operador de mutación consiste en generar un número aleatorio, si éste es menor que

un valor definido con anterioridad, un gen del cromosoma se cambia. En el caso que los
genes sean bits, se aplica la operación de negación sobre bit de manera aleatoria.
El reemplazo consiste en elegir a los elementos de la nueva generación. La manera más

sencilla es reemplazar a toda la población por los cromosomas generados mediante la
recombinación genética. También se puede mantener un porcentaje de la generación
anterior con el mayor valor de aptitud. Es posible incluso evaluar la función de aptitud
de todos los cromosomas (viejos y nuevos) y quedarse con los mejores elementos, lo cual
podría resultar que la generación nueva sea igual a la anterior.
Para optimizar los parámetros del robot se diseñó un algoritmo genético con las siguientes
características:
Los elementos de la población contienen tres datos: rb , rp y h.
Los cromosomas son cadenas de treinta y nueve bits. A cada parámetro (h, rb y
rp ) le corresponden trece bits.
Para calcular el valor de un parámetro se hace lo siguiente:
• Convertir los primeros seis bits del número a decimal.

• Convertir los siguientes siete bits a decimal y dividirlos entre cien.
• Sumar ambos números. El rango de valores de cada parámetro es [0,64.27].
Se utiliza una población inicial aleatoria de cien elementos.

Figura 5.4: Espacio de trabajo con h = 25.17 cm, rb = 10.98 cm y rp = 13.54 cm
La función de aptitud es la siguiente :
f (h, rb , rp , ω, φ, ψ) = 2∆φ + 2∆ψ + .05∆ω

− 10|h − hd | − 5|rb − rbd | − 5|rp − rpd | (5.14)
donde ∆α = αmax − αmin y hd , rbd y rpd son los valores deseados de los parámetros
del robot.
Se selecciona a los padres mediante un método de torneo. El método de torneo

utilizado [24] consiste en lo siguiente:
• Se seleccionan dos cromosomas aleatoriamente.

• Se evalúa la función de aptitud de ambos padres.
• Se genera un número aleatorio k entre cero y uno.
• Si k ≤ 0.75, se selecciona al cromosoma cuyo valor de la función de aptitud
sea mayor, de lo contrario se elige al menor.
• Se repite el proceso para encontrar al segundo padre.
Los padres se recombinan mediante el método de recombinación en dos puntos.
Para decidir si un elemento de la población va a mutar se genera un número alea-

torio k entre cero y uno. Si k ≤ 0.01 se procede con la mutación.
La mutación cambia el valor de un bit de manera aleatoria.
La población se reemplaza totalmente.
Mediante un algoritmo genético con la función de aptitud mencionada se premia a aque-

llos elementos con grandes rangos de movimiento en φ y ψ, mientras que se castiga a
aquellos cuyo tamaño sea distitno al deseado. Esto nos ayuda a cumplir el objetivo de
maximizar el espacio de trabajo sin que varíe mucho el tamaño del robot respecto al
deseado.
Se programó el algoritmo genético utilizando como valores deseados hd = 20 cm, rbd =

15 cm, rpd = 15 cm. Se utilizaron además los parámetros β = 80◦ y D = 3 cm. Como
criterio de término se estableció que se llegara a quinientas generaciones y se utilizó
una población de cincuenta elementos. Los códigos utilizados se muestran en el anexo
A.5. Como resultado se obtuvieron los parámetros h = 25.17 cm, rb = 10.98 cm, rp =
13.54 cm. El espacio de trabajo resultante se muestra en las figuras 5.4, 5.5 y 5.6.
Si comparamos el espacio de trabajo del robot con los parámetros iniciales y con los
generados por el algoritmo genético nos damos cuenta que el espacio de trabajo con
los parámetros generados con el algoritmo genético es mucho mayor que el otro. La
comparación de los valores máximos y mínimos de los ángulos ω, φ y ψ de los dos casos
se puede observar en la tabla 5.1.
Tabla 5.1: Comparación de ángulos máximos y mínimos
Ángulo Parámetros iniciales Algoritmo genético

ωmin −30◦ −45◦
ωmax 30◦ 45◦
φmin −26◦ −39◦
φmax 26◦ 37◦
ψmin 72◦ −132◦
ψmax 72◦ 132◦
Observamos en la tabla 5.1 que en la mayoría de los casos el ángulo utilizando los pará-
metros calculados por el algoritmo genético es 50 % más grande que con los parámetros
iniciales y 80 % en el caso más extremo.
Capítulo 6
Análisis Jacobiano
La cinemática nos da una relación entre la longitud de los actuadores y la orientación

de la plataforma, sin embargo, a veces es útil conocer la relación que existe entre la
velocidad de los actuadores y la velocidad de la plataforma, especialmente cuando se va
a estudiar la dinámica del manipulador. La matriz Jacobian nos da esa relación y es por
esa razón que es importante analizarla.
Si denotamos a las articulaciones actuadas como un vector q y la orientación de la

plataforma como un vector x, podemos escribir las restricciones cinemáticas que ejercen
los miembros como:
f (x, q) = 0 (6.1)
Si derivamos la ecuación (6.1) respecto al tiempo obtenemos una relación entre la veloci-
dad de entrada de los actuadores y la velocidad de salida del efector final de la siguiente
manera:
Jx ẋ = Jq q̇ (6.2)
donde
∂f ∂f
Jx = y Jq =
∂x ∂q
Si llamamos J a la matriz jacobiana del sistema y la escribimos como J = Jq−1 Jx tenemos

que:
q̇ = J ẋ (6.3)
50
Capítulo 6. Análisis Jacobiano 51
Para el manipulador 3-SPS-1-S el vector de entradas es la velocidad de cada uno de

los actuadores lineales q̇ = [d˙1 , d˙2 , d˙3 ]T , mientras que el vector de salida es la velocidad
angular de la plataforma ẋ = ωp = [ωx , ωy , ωz ]T .
Podemos expresar la ecuación (4.1) de la siguiente manera
OP + P Bi = OAi + Ai Bi (6.4)
Derivando esta ecuación y tomando en cuenta que OP y OAi son vectores constantes
obtenemos:
ωp × bi = di ωi × si + d˙i si para i =1, 2, 3 (6.5)
donde bi representa al vector OBi , si representa a un vector unitario que apunta de Ai

a Bi y ωi la velocidad angular del miembro i respexto a la base.
Para eliminar a ωi de la ecuación (6.5) aplicamos al producto punto de si en ambos lados

de la ecuación, obteniendo:
(bi × si )ωp = d˙i para i =1, 2, 3 (6.6)
Si escribimos la ecuación (6.6) tres veces(una para cada actuador) obtenemos tres ecua-
ciones escalares, las cuales ordenadas en forma matricial nos da:
  
ωx ˙
d1
   
Jx   ˙ 
 ωy  = Jq  d2  (6.7)

ωz ˙
d3
donde  
1 0 0
 
Jq = 
 0 1 0 
 (6.8)
0 0 1
 
(b1 × s1 )T
 
Jx = J =  T , (6.9)
 (b2 × s2 ) 
(b3 × s3 )T
6.1. Singularidades
Observamos en la ecuación (6.9) que cuando los vectores bi y si son paralelos, un renglón
de la matriz Jx se vuelve cero, por ende el determinante de la matriz jacobiana se vuelve
cero.
Las singularidades son todas las posiciones en las cuales el determinante de la matriz
Jacobiana del manipulador es igual a cero. Las singularidades causan distintos problemas
en los robots paralelos, entre los cuales se encuentran: aumento de grados de libertad
en ese punto, cambio del tipo de movimiento (un robot paralelo esférico puede adquirir
movimientos de traslación por ejemplo) y pérdida de la controlabilidad.
Si se calcula el jacobiano del robot cuando ω = 0, ψ = 0 y φ = 0.5 radianes, podemos ver

un ejemplo de este comportamiento. Los parámetros h, rb y rp calculados por el algoritmo
genético son los que se utilizarán en todos los ejemplos. Resolviendo la ecuación (6.9)
obtenemos  
0 −12.1819 0
 
J =
 11.4366 5.8711 −0.2765 
 (6.10)
−11.4366 5.8711 0.2765
Se puede ver claramente que el determinante de esta matriz es cero, por lo tanto esta es
una configuración singular. Si utilizamos φ = 0.274 y φ = −0.395 obtenemos
 
0 −13.3243 0
 
J =
 11.5285 6.4215 ,
−0.0886  (6.11)
−11.5285 6.4215 0.0886
 
0 −12.2219 0
 
J =
 11.9179 6.3024 .
−0.2185  (6.12)
−11.9179 6.3024 0.2185
Observamos que siempre nos da el determinante igual a cero, es decir, es una posición
singular. Mediante estas observaciones podemos decir que cuando ψ = 0 y ω = 0, en-
tonces det(J) = 0. En el robot 3-SPS-1S la unión directa entre la base y la plataforma
evita que el robot adquiera más grados de libertad o movimientos de traslación, otra
razón por la cual este robot es muy bueno en este tipo de aplicaciones, sin embargo, no
es posible calcular la dinámica inversa, lo cual tendría repercusiones en un controlador
que utilizara la dinámica.
Se utilizó el código presentado en el anexo A.6, el cual se basa en el algoritmo 7, pa-

ra calcular las singularidades posibles dentro del espacio de trabajo de nuestro robot.
La gráfica de la figura 6.1 nos muestra el espacio de configuraciones de la plataforma,
representando con puntos azules aquellas posiciones que presentan una singularidad. Po-
demos ver que los únicos puntos singulares son aquellos en donde ψ = 0 y ω = 0 como
se mencionó anteriormente.
Algoritmo 7 Cálculo de singularidades

1: Calcular todos los puntos del espacio de trabajo mediante el algoritmo 1.
2: para todo punto del espacio de trabajo hacer
3: Obtener la matriz Jacobiana.
4: si el rango de la matriz es menor a 3 entonces
5: Guardar punto como singularidad.
6: fin si
7: fin para
Figura 6.1: Singularidades del robot en el espacio de configuraciones de la plataforma

Capítulo 7
Dinámica
A diferencia de la cinemática, la dinámica estudia el movimiento de los cueros tomando en

cuenta las fuerzas que lo producen. Cuando se estudia la dinámica de un manipulador, se
busca establecer una relación entre las fuerzas ejercidas por los actuadores y la aceleración
del efector final. Cuando se habla de dinámica inversa, se busca encontrar el vector
de fuerzas/troques capaz de producir una cierta aceleración, mientras que cuando se
habla de dinámica directa se busca encontrar la aceleración producida dado un vector de
fuerzas/torques.
Existen distintos métodos para calcular la dinámica de un manipulador paralelo. Algunos

métodos son el método de Euler-Lagrange, Newton, trabajo virtual y Kane [6, 12, 21, 33].
A continuación se mostrará um métodos para calcular la dinámica inversa del robot 3-
SPS-1-S y un método para calcular tanto la dinámica inversa como la dinámica directa.
7.1. Dinámica Inversa
7.1.1. Trabajo virtual (Principio de d0 Alembert)
El principio de d0 Alembert, basado en el principio del trabajo virtual, establece que un

sistema de cuerpos rígidos se encuentra en equilibrio dinámico cuando el trabajo virtual
de la suma de las fuerzas aplicadas y de inercia es cero para cualquier desplazamiento
virtual del sistema. Tsai [34] nos muestra que en el caso de robots paralelos esto se
traduce en la siguiente ecuación:
X
δqT τ + δxTp F̂p + δxTi F̂i = 0 (7.1)
i
54
Capítulo 7. Dimámica 55
En donde xp es un vector que describe la posición y/o orientación de la plataforma, q

y τ son vectores que representan la posición y la fuerza o momento ejercidos por cada
actuador respectivamente, F̂p es un wrench 1 (una combinación de una fuerza y un par
que actúan sobre el mismo eje) de seis dimensiones (en el caso más general) que viene
dado por la suma de los wrenches aplicados y de inercia en el centro de la plataforma y F̂i
es un wrench de seis dimensiones que viene dado por la suma de los wrenches aplicados
y de inercia en el centro del eslabón i.
 
f̂px
 

 f̂py 
 
 f̂pz 
F̂p =   (7.2a)
n̂px 
 

 

 n̂py 
n̂pz
 
f̂ix
 

 f̂iy 
 
 f̂iz 
F̂i =   (7.2b)
n̂ix 
 

 

 n̂iy 
n̂iz
Donde f̂ representa la suma de una fuerza aplicada y una debida a la inercia y n̂ la suma
de un momento aplicado y uno debido a la inercia.
El desplazamiento virtual de la plataforma se relaciona con el desplazamiento virtual de

los actuadores mediante la matriz jacobiana Jp de la siguiente manera:
δq = Jp δxp (7.3)
Además, el desplazamiento virtual del eslabón i de una pata se puede relacionar con el
desplazamiento virtual de la plataforma mediante una matriz jacobiana Ji :
δxi = Ji δxp (7.4)
Sabiendo que para cualquier desplazamiento virtual de la plataforma (δxp ) se cumple

la ecuación (7.1), podemos utilizar las ecuaciones (7.3) y (7.4) para reescribirla de la
siguiente manera:
1
La palabra wrench se traduce al español como llave inglesa o torcer si se usa como verbo. Dado que
ninguna de estas definiciones suena bien en este contexto, se utiliza el anglicismo wrench.
X
JpT τ + F̂p + JiT F̂i = 0 (7.5)
i
Si despejamos τ de la ecuación (7.5) obtenemos la solución al problema de la dinámica

inversa como se muestra a continuación:
!
X
τ= −Jp−T F̂p + JiT F̂i (7.6)
i
Hay que tomar en cuenta que para resolver la ecuación (7.6) es necesario encontrar la
matriz inversa de Jp , por lo que si la matriz tiene una singularidad en algún punto no se
puede resolver la dinámica inversa.
Para el robot 3-SPS-1S tenemos tres patas, por lo que la sumatoria de la ecuación (7.6) va
de 1 hasta 3. Li y Xu [20] mencionan que en un robot con tres grados de libertad podemos
utilizar un subconjunto de F̂p de tres elementos, en donde los elementos restantes (fuerzas
o momentos) son soportados por las uniones pasivas. Dado que la plataforma no tiene
movimientos lineales sino que solamente gira, el subconjunto de F̂p a utilizar solamente
debe de contener momentos, además la pata central genera una fuerza de reacción que
contrarresta a la fuerza ejercida por la gravedad sobre la plataforma, por lo que en este
caso podemos escribir a F̂p como
 
n̂px
  A A

 n̂py  = − Ip ω̇ p − ω p × ( Ip ω p )
F̂p =  (7.7)

n̂pz
done A Ip = A RB B Ip B RA es la matriz de inercia de la plataforma alrededor de su centro

de masa y expresada en el marco de referencia fijo de la base.
Por otra parte podemos dividir a F̂i en dos wrenches, uno para cada elemento de la pata
i (cilindro y pistón). Al hacer esto la ecuación (7.6) nos queda de la siguiente manera:
3
!
X
τ = −Jp−T F̂p + i Ti
J1i F̂1i + i J2i
Ti
F̂2i (7.8)
1
donde i J1i y i F̂1i son la matriz jacobiana y el wrench relacionados con el cilindro i, i J2i
y i F̂2i son la matriz jacobiana y el wrench relacionados con el pistón i:
 
i f̂
1ix
 
i f̂
1iy
 
  " #
 i f̂ 
m i R g − m i v̇
i
 1iz  1i A 1i 1i
F̂1i =  = (7.9)
 i n̂ 
− I1i ω̇ i − ω i × (i I1i i ω i )
i i i
 1ix 
 
 i n̂ 
 1iy 
i n̂
1iz
 
i f̂
2ix
 
i f̂
2iy
 
  " #
 i f̂ 
m i R g − m i v̇
i
 2iz  2i A 2i 2i
F̂2i =  = (7.10)
 i n̂ 
−i I2i i ω̇ i − i ω i × (i I2i i ω i )
 2ix 
 
 i n̂ 
 2iy 
i n̂
2iz
Para obtener los momentos de inercia de cada uno de los cuerpos se harán las siguientes
consideraciones. El cilindro se considera como un cilindro hueco con paredes gruesas de
masa m1 , radio externo r1 , radio interno rr1 y longitud 2e1 , el pistón como un cilindro
hueco de paredes gruesas de masa m2 , radio externo r2 , radio interno rr2 y longitud 2e2
y la plataforma se como un cilindro sólido de masa mp , radio rp y altura hp .
Los momentos de inercia quedan de la siguiente manera:
1
m1 3 r12 + rr12 + (2e1 )2 + m1 e21

Ix1 = Iy1 = (7.11a)
12
1
Iz1 = m1 r12 + rr12

(7.11b)
2
1
= m2 3 r22 + rr22 + (2e2 )2 + m2 e22

Ix2 = Iy2 (7.11c)
12
1
Iz2 = m2 r22 + rr22

(7.11d)
2
1
Ixp = Iyp = mp 3rp 2 + hp 2 (7.11e)
12
mp rp 2
Izp = (7.11f)
2
Las matrices de inercia quedan de la siguiente manera:

 
Ix1 0 0
 
I1 = 
 0 Iy1 0  (7.12a)
0 0 Iz1
 
Ix2 0 0
 
I2 = 
 0 Iy2 0  (7.12b)
0 0 Iz2
 
Ixp 0 0
 
Ip =  0
 Iyp 0  (7.12c)
0 0 Izp
Para resolver la ecuación (7.8) es necesario encontrar las tres matrices jacobianas Jp , i J1i
y i J2i . Para esto se debe hacer un análisis de posiciones, velocidades y aceleraciones de
cada uno de los elementos que conforman al robot.
En primer lugar tenemos que la posición de la plataforma está dada por la matriz de
rotación descrita en la ecuación (4.6). La velocidad angular en términos de los mismos
movimientos utilizados para calcular la matriz de rotación A RB es
ωp = ω̇u + φ̇v 0 + ψ̇w00 . (7.13)
Si expresamos los vectores u, v ’ y w ” de la ecuación (7.13) en el marco de referencia fijo

A(el sistema de coordenadas de la base) obtenemos
 
ω̇ + ψ̇sφ
 
ωp = 
 φ̇cω − ψ̇cφsω  .
 (7.14)
φ̇sω + ψ̇cφcω
Derivando la ecuación (7.14) respecto al tiempo obtenemos la aceleración angular de la

plataforma:  
ω̈ + ψ̈sφ + ψ̇ φ̇cφ
 
ω̇p = 
 φ̈cω − φ̇ ω̇sω − ψ̈cφsω + ψ̇ φ̇sφsω − ψ̇ ω̇cφcω .
 (7.15)
φ̈sω + φ̇ω̇cω + ψ̈cφcω − ψ̇ φ̇sφcω − ψ̇ ω̇cφsω
Si denotamos al vector unitario que apunta del punto Ai al punto Bi podemos escribir
la ecuación (4.8) como
si di + ai = p + bi . (7.16)
Figura 7.1: Ángulos de la pata i [34].
Si despejamos si de la ecuación (7.16) obtenemos lo siguiente
p + bi − ai
si = (7.17)
di
donde
di = |p + bi − ai | (7.18)
La orientación de la pata i respecto a la base se puede expresar mediante un giro de φi

alrededor del eje zi , seguido de una rotación de θi alrededor del nuevo eje y0i como se ve
en la figura 7.1. Estos giros se pueden expresar mediante la siguiente matriz de rotación:
 
cφi cθi −sφi cφi sθi
A
 
Ri = 
 cθi sφi cφi .
sφi sθi  (7.19)
−sθi 0 cθi
Si colocamos al eje zi apuntando de Ai a Bi , el vector unitario si expresado con la pata

i como marco de referencia es  
0
i
 
si = 
 0 .
 (7.20)
1
Podemos encontrar el vector si multiplicando el vector i si y la matriz de rotación A Ri .

 
cφi sθi
si = A Ri i si = 
 
 sφi sθi 
 (7.21)
cθi
Si resolvemos la ecuación (7.21) para θi y φi obtenemos:
cθi = siz
q
sθi = s2ix + s2iy
sφi = siy /sθi
sφi = six /sθi (7.22)
Utilizando las ecuaciones (7.17) y (7.22) podemos encontrar la orientación de la pata i

en términos de la orientación de la plataforma.
La posición del centro de masa del cilindro (r1i ) y del pistón (r2i ) de la pata i los podemos
expresar de la siguiente manera:
r1i = ai + e1 si (7.23)
r2i = ai + (di − e2 ) si (7.24)
Por otra parte podemos encontrar la velocidad del punto Bi si derivamos el lado derecho
de la ecuación (7.16) respecto al tiempo
vbi = ω p × bi (7.25)
Podemos expresar la velocidad del punto Bi en el marco de referencia de la pata i de la

siguiente manera
i
vbi = i RA vbi (7.26)
donde i RA es A RiT y i vbi = [i vbix , i vbiy , i vbiz ]T
También podemos expresar la velocidad del punto Bi en términos de la velocidad angular

de la pata i si derivamos el lado izquierdo de la ecuación (7.16) respecto al tiempo
i
vbi = di i ω i × i si + d˙i i si (7.27)
Si aplicamos el producto punto de ambos lados de la ecuación (7.27) con i si obtenemos
i
vbiz = d˙i (7.28)
Si ahora aplicamos el producto cruz de ambos lados de la ecuación (7.27) con si obtenemos
la velocidad angular de la pata i:
 
−i vbiy
i 1 i 1 
si × i vbi = 

ωi = iv  (7.29)
bix
di di  
0
Una vez que se conoce i ω i podemos encontrar las velocidades del cilindro y del pistón
derivando las ecuaciones (7.23) y (7.24) respectivamente:
 
iv
bix
i e1 
v1i = e1 i ω i × i si =

 i vbiy  (7.30)
di  
0
 
(di − e2 )i vbix
1 
i
v2i = (di − e2 )i ω i × i si + d˙i i si = 

(di − e2 )i vbiy  (7.31)
di  
i
di vbiz
Si derivamos la ecuación (7.25) respecto al tiempo encontramos la aceleración del punto

Bi :
v̇bi = ω̇ p × bi + ω p × (ω p × bi ) (7.32)
Expresando v̇bi en el marco de referencia de la pata i tenemos
i
v̇bi = i RA v̇bi (7.33)
La aceleración del punto Bi se puede expresar en términos de la aceleración de la pata i

si obtenemos el producto cruz de la derivada de la ecuación (7.27) con i si :
2i vbiz i vbiy
 
−i v̇biy +
di
2d˙i i
 
i 1 i i
1  2i vbiz i vbix

ω̇ i = si × v̇bi − ωi =  i v̇  (7.34)
di di di  bix − 
 di 
0
Conociendo la aceleración angular de la pata i podemos encontrar la aceleración del

cilindro y del pistón si derivamos las ecuaciones (7.30) y (7.31) respectivamente:
 
i v̇ 2i vbiz i vbix
bix −

 di 

i i i i i i e1  i v̇ 2i vbiz i vbiy 
v̇1i = e1 ω̇ i × si + e1 ω i × ( ω i × si ) = 
biy −
 (7.35)
di  di 
iv2 + iv2
 
 
− bix bix
di
i
v̇2i = d¨i i si + (di − e2 )i ω̇ i × i si + (di − e2 )i ω i × (i ω i × i si )
2e2 i vbiz i vbix
 
i
 (di − e2 ) v̇bix + di 
i i
 
1 2e 2 v biz v biy
+ 2d˙i ω i × si = 
i
 
(di − e2 )i v̇biy +  (7.36)
di  di 
e2 i vbix
2 + iv2

 bix

i
di v̇biz +
di
Una vez que se conocen las posiciones, velocidades y aceleraciones se procede a calcular
las matrices jacobianas. Si escribimos la ecuación (7.25) en forma matricial tenemos:
vbi = Jbi ẋp (7.37)
donde  
0 biz −biy
 
Jbi = 
 −biz 0 bix 
 (7.38)
biy −bix 0
Si transformamos la ecuación (7.37) al marco de referencia de la pata i obtenemos:
i
vbi = i Jbi ẋp (7.39)
donde  
iJ
bix
i
Jbi = i RA Jbi = 
 
iJ 
 biy 
iJ
biz
 
−biz cθi sφi − biy sθi biz cθi cφi + bix sθi −biy cθi cφi + bix cθi sφi
 
=
 −biz cφi −biz sφi biy sφi + bix cφi 
 (7.40)
−biz sθi sφi + biy cθi biz cφi sθi − bix cθi −biy cφi sθi + bix sθi sφi
Utilizando los renglones de la matriz anterior, podemos escribir la ecuación (7.28) como:
d˙i = i Jbiz ẋp (7.41)
Si escribimos la ecuación (7.41) tres veces, una para cada actuador, y las acomodamos
en forma de matriz obtenemos:
q̇ = Jp ẋp (7.42)
donde  
1J
b1z
 
Jp =  2J  (7.43)
 b2z 
3J
b3z
es equivalente a la matriz jacobiana de la ecuación (6.9).
De manera similar podemos escribir las ecuaciones (7.29), (7.30) y (7.31) como:
 
−i Jbiy
i 1  iJ

ωi =  bix
 (7.44)
di  
01×3
 
iJ
bix
i e1  iJ

v1i =  biy
 (7.45)
di  
01×3
 
(di − e2 )i Jbix
i 1  
 (di − e2 )i Jbiy 
v2i = (7.46)
di  
i
di Jbiz
Uniendo las ecuaciones (7.44), (7.45) y (7.46) obtenemos
i
ẋ1i = i J1i ẋp , (7.47)
i
ẋ2i = i J2i ẋp , (7.48)
donde  
e1 i Jbix
 
 e1 i Jbiy 
 
 
1  01×3 
iJ1i =  , (7.49)
di  −i Jbiy
 

 
 iJ 
 bix 
01×3
 
(di − e2 )i Jbix
 
 (di − e2 )i Jbiy 
 
di i Jbiz
 
1  
iJ2i =   (7.50)
di 
 −i Jbiy


 
 iJ 
 bix 
01×3
son las llamadas matrices jacobianas de eslabón del cilindro y pistón i respectivamente.
Una vez calculando las matrices jacobianas mediante las ecuaciones (7.43), (7.49) y (7.50),
podemos resolver la ecuación (7.8) y de esta manera encontrar el vector τ = [f1 , f2 , f3 ]T .
El apéndice A.7 nos muestra el código para calcular el vector de fuerzas τ .
7.1.2. Método presentado por Merlet
El método que utiliza Merlet para calcular la dinámica del robot 6-UPS [23] puede
ser adaptado para calcular la dinámica del robot 3-SPS-1S, tomando en cuenta ciertas
consideraciones que se mencionarán más adelante.
En primer lugar se considera al cilindro y al pistón como un solo eslabón, se desprecia

la masa de las patas y se utiliza la siguiente matriz de inercia para la pata i
 
Ji 0 0
 
Ii = 
 0 Ji 0 
 (7.51)
0 0 0
.
Para simplificar la notación, para un vector e cualquiera con componentes (x,y,z ) se

definirá como e∗ a una matriz tal que:
 
0 −z y
e × a = e∗ a e∗ = 
 
con  z 0 −x 
 (7.52)
−y x 0
donde a es un vector arbitrario.
La fuerza fi que actúa en un punto Bi se puede descomponer en dos componentes.
La fuerza τi proporcionada por el actuador i a lo largo del vector unitario si .
Una fuerza perpendicular a si , debida a la inercia, que se denotará fNi

De esta manera tenemos que:

fi = τi si + fNi (7.53)
Llamemos FN a la suma de todas las fNi y MN a la suma del torque alrededor de un

punto C. Si F y M son la fuerza y el torque aplicados en la plataforma sobre el punto
C, podemos escribir las ecuaciones de equilibrio como
3
X
F= τi si + FN (7.54)
i=1
3
X
M= τi (PBi × si ) + MN . (7.55)
i=1
Sean σN y σ los vectores de dimensión seis:

" #
FN
σN = , (7.56)
MN
" #
F
σ= . (7.57)
M
Podemos escribir las ecuaciones (7.54) y (7.55) en forma matricial como
σ = J T τ + σN (7.58)
donde J es la matriz jacobiana del robot.
Observamos que dado que σ y σN son vectores de dimensión seis, necesitaríamos tener
un vector de fuerzas τ de dimensión seis y una matriz jacobiana de 6 × 6, sin embargo,
nuestro vector de fuerzas es de dimensión tres y nuestra matriz jacobiana es de 3 × 3.
Aunque es posible encontrar una matriz jacobiana de 6 × 6 para un robot paralelo con
menos de seis grados de libertad, conocida como matriz jacobiana completa, y multipli-
carla por un vector de fuerzas τ de dimensión seis, el cual contiene N fuerzas (siendo
N el número de actuadores) y 6 − N valores igual a cero, en este caso se simplificará el
modelo tomando como consideración, al igual que en el método de trabajo virtual, que la
pata pasiva del robot provee fuerzas de reacción que contrarrestan a las fuerzas aplicadas
sobre la plataforma, de tal forma que solamente utilizaremos los momentos para hacer
el cálculo de la dinámica. Las ecuaciones (7.57) y (7.56) quedan de la siguiente manera
σN = MN , (7.59)
σ = M. (7.60)
De la ecuación (7.58) podemos despejar τ para resolver la dinámica inversa:
τ = J −T (σ − σN ) (7.61)
Podemos escribir la ecuación de Newton-Euler del momento de la plataforma de la si-

guiente manera:
M = Ip ω̇ p + ω p × (Ip ω p ) (7.62)
Si llamamos xp a la orientación de la plataforma, podemos escribir la ecuación (7.57)

como
σ = T1 ẍp + T2 (7.63)
donde
T 1 = Ip , T2 = ω p × (Ip ω p ) (7.64)
Por otra parte, podemos expresar la aceleración del punto Bi como
ai = ω̇ p × bi + ω p × (ω p × bi ) (7.65)
la cual, en forma matricial nos queda
ai = U1i ẍp + U2i (7.66)
donde
U1i = −b∗i (7.67)
U2i = ω p × (ω p × bi ) (7.68)
La proyección aN i de ai en un plano perpendicular a si está dada por
aN i = (si × ai ) × si (7.69)
la cual podemos escribir en forma matricial utilizando la ecuación (7.66) como
aN i = −s∗i 2 ẍp U1i − s∗i 2 U2i . (7.70)
Además tenemos que

Ji
fN i = − aN i (7.71)
d2i
Figura 7.2: Trayectoria utilizada para encontrar la dinámica inversa
Si la componente MN de σN es
3
X
MN = bi × fN i , (7.72)
i=1
podemos escribir σN en forma matricial como
σN = V1 ẍp + V2 (7.73)
donde
3 3
X Ji ∗ ∗ 2 X Ji ∗ ∗ 2
V1 = 2 bi si U1i , V2 = 2 bi si U2i (7.74)
d
i=1 i
d
i=1 i
Sustituyendo las ecuaciones (7.63) y (7.73) en (7.61) obtenemos
τ = J −T (T1 − V1 )ẍp + J −T (T2 − V2 ) (7.75)
El apéndice A.9 contiene el código que resuelve la dinámica inversa por el método pre-
sentado por Merlet.
Mediante la dinámica inversa se encontraron las fuerzas necesarias para que el robot se
mueva de la posición inicial [ω, φ, ψ]T = [1◦ , −15◦ , −10◦ ]T a la posición final [ω, φ, ψ]T =
[1◦ , −5◦ , −35◦ ]T con las velocidades y aceleraciones mostradas en la figura 7.2.
Figura 7.3: Fuerzas calculadas mediante los dos métodos de dinámica inversa
La figura 7.3 nos muestra el resultado obtenido tanto por el método de trabajo virtual,
como por el método de Merlet. Se utilizaron los valores: r1 = 1.5 cm, r2 = 1.25 cm, rr1 =
Ix1 + Ix2
1.3 cm, rr2 = 1 cm, e1 = 8.4333 cm, e2 = 8.4333 cm y Ji = . Se hizo la suposi-
4
ción de que los elementos están hechos de aluminio, cuya densidad es ρAl = 2700 kg/m3
aproximadamente. Las masas se calcularon mediante las siguientes ecuaciones
mp = πrp2 hp ρAl (7.76)
m1 = π r12 − rr12 2e1 ρAlg

(7.77)
m2 = π r22 − rr22 2e2 ρAlg

(7.78)
1
donde ρAlg = ρAl , esto para convertir la densidad a unidades de g/cm3 .
1000
Podemos observar que las fuerzas calculadas por ámbos métodos aunque no son iguales,
son bastante similares. Algo que notamos en las gráficas es que las fuerzas calculadas son
muy pequeñas, pero además tienen valores positivos y negativos, lo que implica que se
necesitan actuadores lineales que puedan ejercer fuerzas en ambos sentidos, por ejemplo
un pistón de doble efecto.
Si recordamos en el análisis se hizo la suposición de que las fuerzas son contrarrestadas

por la pata pasiva y al final solamente nos quedan los momentos ejercidos por cada pata,
por lo tanto es posible transformar la fuerza negativa de una pata en fuerzas positivas
Figura 7.4: Comparación de fuerzas calculadas para el robot 6-UPS
de las otras patas siempre y cuando el momento resultante sobre la plataforma sea el
mismo.
Para revisar la validez de los códigos y a su vez de las gráficas obtenidas, se adaptó el
código del apéndice A.7 para un robot 6-UPS, el cual se muestra en el apéndice A.8.
Utilizando este código se graficaron las fuerzas cuando existe una aceleración sobre el eje
x de 5 m/s2 . La figura 7.4 muestra del lado izquierdo las fuerzas obtenidas mediante el
código del apéndice A.8, mientras que en el lado derecho se observan las fuerzas calculadas
por Tsai[34].
7.2. Dinámica Directa
En la dinámica directa se busca encontrar la aceleración de la plataforma dado el vector

de fuerzas τ . La dinámica directa nos sirve para simular el comportamiento dinámico
del robot sin necesidad de tener un sistema físico, esto es muy útil en algunos casos, por
ejemplo cuando se está diseñando un controlador y se quiere ver el comportamiento del
robot aunque no se cuente con uno. De las ecuaciones (7.8) y (7.75), podemos observar que
aunque ambas nos sirven para encontrar el vector de fuerzas, solamente la ecuación (7.75)
tiene separado al vector de aceleraciones ẍp , por lo que es bastante sencillo encontrar la
dinámica directa mediante el método de Merlet.
Despejando ẍp de la ecuación (7.75) obtenemos
ẍp = (T1 − V1 )−1 J T τ − (T1 − V1 )−1 (T2 − V2 ) (7.79)
la cual es la solución de la dinámica directa.

Cabe mencionar que la aceleración angular encontrada en la ecuación (7.79) no corres-

ponde a la aceleración de cada uno de los ángulos de Euler, sino al vector de aceleración
mostrado en la ecuación (7.15). Para obtener la aceleración de los ángulos de Euler
podemos reescribir la ecuación (7.15) como
   
ω̈ ψ̇ φ̇cφ
ẍp = ω̇p = ωp Rωφψ 
   
φ̈  +  −φ̇ω̇sω + ψ̇ φ̇sφsω − ψ̇ ω̇cφcω  (7.80)
   
ψ̈ φ̇ω̇cω − ψ̇ φ̇sφcω − ψ̇ ω̇cφsω
donde  
1 0 sφ
ωp
 
Rωφψ = 
 0 cω −cφsω 
 (7.81)
0 sω cωcφ
Podemos escribir la ecuación (7.80) como

   
ω̈ ψ̇ φ̇cφ
 φ̈  = ωp R−1 ω̇p − ωp R−1  −φ̇ω̇sω + ψ̇ φ̇sφsω − ψ̇ ω̇cφcω 
   
  ωφψ ωφψ   (7.82)
ψ̈ φ̇ω̇cω − ψ̇ φ̇sφcω − ψ̇ ω̇cφsω
con
(2k + 1)π
φ 6= para k = 0, 1, 2, ... (7.83)
2
Observamos en la ecuación (7.82) que para encontrar la aceleración angular necesitamos

conocer los valores de ω, φ, ψ, ω̇, φ̇ y ψ̇, sin embargo para encontrar estos valores
necesitamos la aceleración angular.
Una posible solución a este problema es dividir el tiempo en partes muy pequeñas, de tal
forma que se cumpla que α(t) ≈ α(t−1) y α̇(t) ≈ α̇(t−1). Mediante estas aproximaciones
podemos calcular la aceleración de los ángulos de Euler y a su vez la posición y velocidad.
Este método solamente funciona en casos donde la aceleración angular es lo suficiente-

mente pequeña para poder hacer las aproximaciones de ángulos y velocidades. Además
solamente sirve para tiempos muy pequeños, ya que va a existir un error en la aceleración
debido a la aproximación que se genera y entre mayor sea el tiempo, los errores se van a
ir acumulando.
Utilizando las fuerzas de la gráfica superior de la figura 7.3 y la posición inicial [ω, φ, ψ]T =
[1◦ , −15◦ , −10◦ ]T , se simuló la dinámica directa del robot. La figura 7.5 nos muestra una
comparación de las aceleraciones iniciales (con las que se calcularon las fuerzas) y las
aceleraciones calculadas mediante la dinámica directa.
Figura 7.5: Aceleraciones calculadas mediante dinámica directa
Podemos ver que aunque el comportamiento es similar existe una variación, ésto debido a
que como las aceleraciones son grandes y no se calcularon puntos intermedios de posición,
las aproximaciones α(t) ≈ α(t − 1) y α̇(t) ≈ α̇(t − 1) no son muy buenas.
La figura 7.6 nos muestra la trayectoria del robot simulada mediante la dinámica directa,
y aunque no es igual a la trayectoria original de la figura 7.2, es bastante parecida, lo
cual significa que si se aplican las fuerzas mostradas en un robot real, la trayectoria no
será igual a la simulada, sin embargo será muy parecida. Esto mismo sucede en la figura
7.5.
Figura 7.6: Trayectoria generada mediante dinámica directa

Capítulo 8
Control de posición
Existen diversas maneras de controlar al manipulador, entre las que tenemos un control
que utilice una longitud de las patas deseada como variable a controlar, con lo cual se
controla la posición del manipulador, aunque hay ciertos casos donde la posición final es
errónea como se verá más adelante.
Este tipo de controladores se utilizan ya que son muy sencillos de generar y funcionan
bastante bien, sin embargo, en ocasiones es mejor utilizar un controlador que utilice la
dinámica, especialmente si las fuerzas necesarias son muy grandes.
8.1. Control PID
Si se controla la posición de cada actuador de manera independiente es posible controlar

la posición de la plataforma. Por esta razón se decidió implementar, en primera instancia,
un controlador PID independiente en cada uno de los actuadores.
El controlador PID utiliza el error de alguna variable (longitud de las patas en este
caso), su derivada y su integral, para calcular la salida que debe de ejercer el controlador
con el fin de desaparecer el error. Este tipo de controladores son muy utilizados debido
a su simplicidad y facilidad de uso y es por esta misma razón por la cual se decidió
implementar un controlador de este tipo para el manipulador 3-SPS-1-S.
Gracias a la arquitectura simétrica del robot podemos utilizar las mismas constantes Kp ,
Kd y Ki para cada uno de los controladores. Las constantes se ajustaron manualmente,
observando el comportamiento de la respuesta.
El algoritmo utilizado para simular el control es el siguiente:
72
Capítulo 8. Control de posición 73
Algoritmo 8 Simulación del controlador PID

1: Establecer condiciones iniciales y finales de posición.
2: Establecer la velocidad y aceleración inicial.
3: Establecer las constantes Kp , Kd y Ki .
4: Establecer tiempo inicial t = 0, tiempo de simulación T y paso ∆t.
5: Calcular el valor deseado de la longitud de los actuadores mediante la ecuación (4.9).
6: mientras t ≤ T hacer
7: Obtener el valor de los actuadores mediante la ecuación (4.9).
R
8: Obtener los errores, sus derivadas e integrales Rdei , ḋei y dei .
9: Calcular las fuerzas fi = Kp dei + Kd ḋei + Ki dei .
10: Calcular la aceleración de la plataforma mediante la ecuación (7.79).
11: Calcular la aceleración de los ángulos de Euler (α̈) utilizando la ecuación (7.82).
12: Actualizar la velocidad angular α̇(t + ∆t) = α̇(t) + α̈(t + ∆t)∆t.
13: Actualizar la posición de la plataforma α(t+∆t) = α(t)+α̇(t)∆t+α̈(t+∆t)∆t2 /2.
14: Actualizar tiempo t = t + ∆t.
15: fin mientras
Usando los valores Kp = 4, Kd = 1 y Ki = 0.02, la posición inicial ωini = 3◦ , φini = −20◦

y ψini = −15◦ , la posición final ωf in = 2◦ , φf in = −5◦ , ψf in = −50◦ , velocidad inicial
ωp (0) = 0, aceleración inicial ω̇p (0) = 0, T = 2 y ∆t = T /1000, se obtuvieron los
resultados mostrados en la figura 8.1.
Figura 8.1: Posición de la plataforma y error en actuadores con un controlador PID
La gráfica que se observa en la parte superior muestra el error en la longitud de los

actuadores a través del tiempo, mientras que la gráfica inferior muestra la posición de la
plataforma. Observamos que el error de la plataforma en estado estacionario es de 0.02◦
y entra a la banda de estabilidad del 5 % a los 1.296 segundos en el peor de los casos.
Cabe mencionar que el modelo se simula mediante cinemática directa y como ya se

mencionó anteriormente, el método para obtener la aceleración de los ángulos de Euler
es una aproximación que varía dependiendo del valor de ∆t, entre más pequeño sea, mejor
será la aproximación. En este ejemplo donde ∆t = 0.002, los cambios de posición entre
un punto y el siguiente son pequeños, por lo que se puede decir que la aproximación es
buena, sin embargo el modelo real tendrá una ligera variación en los tiempos de respuesta.
8.2. Control PD difuso
Una vez que se logró controlar al sistema mediante un controlador PID bastante sencillo,
se diseñó un controlador difuso tipo PD.
Este tipo de controladores se basa en la lógica difusa para calcular el valor de salida. La
base de la lógica difusa son los conjuntos difusos presentados por Zadeh [36], en donde
un elemento tiene un grado de pertenencia a cada conjunto entre cero y uno, es decir
puede pertenecer parcial o totalmente a un conjunto.
Para las personas resulta más fácil pensar en términos lingüísticos que en términos numé-
ricos, por ejemplo es más fácil imaginarse la temperatura del agua si escuchamos la frase
“el agua está caliente"que si escuchamos la frase “el agua está a 31◦ C ". Para imitar forma
de pensamiento “natural ”, la lógica difusa utiliza las llamadas variables lingüísticas que
son palabras que permiten describir el estado de un objeto [27]. Las variables lingüísticas
toman valores lingüísticos (temperatura= “caliente ”) y pueden ser representadas por
conjuntos difusos.
Un sistema de control difuso cuenta con tres etapas básicas: difusificación, inferencia y
desdifusificación.
La difusificación consiste en mapear un valor nítido al espacio difuso. De manera muy

general se desea saber el grado de pertenencia de un valor numérico a cada una de las
variables lingüísticas (conjuntos difusos). Retomando el ejemplo de la temperatura, si
suponemos que solamente existen dos conjuntos de temperatura, frío y caliente, pode-
mos decir que el valor 31◦ C pertenece en un 80 % a caliente y un 20 % a frío. Cuando
realizamos la fusificación podemos cortar el conjunto difuso en el valor de pertenencia
(80 %) o podemos escalar la función al 80 % de su tamaño.
La inferencia consiste en evaluar una serie de reglas de la forma SI-ENTONCES y obtener

un valor de salida difuso. Suponiendo que queremos mantener la temperatura del agua
de la regadera en una temperatura ni caliente ni fría, y que existen dos salidas: la llave
de agua caliente y la llave de agua fría, podemos escribir las siguientes dos reglas:
Si la temperatura es fría entonces cerrar la llave de agua fría y abrir la llave de

agua caliente.
Si la temperatura es caliente entonces cerrar la llave de agua caliente y abrir la

llave de agua fría.
Dado que la temperatura es 80 % caliente y 20 % fría, entonces la primera regla se activará

en un 20 % y la segunda regla en un 80 %. La primer regla nos da como reultado cerrar
la llave de agua fría 20 % y abrir la de agua caliente 20 %. El resultado de la segunda
regla sería abrir la llave de agua fría hasta el 80 % y cerrar la de agua caliente 80 %.
Observando el resultado de ambas reglas, notamos que tenemos que cerrar la llave de
agua fría 20 %, abrir la llave de agua caliente 20 %, abrir la llave de agua fría 80 % y
cerrar la llave de agua caliente 80 %. El resultado de estas reglas lo podemos observar
en la figura 8.2, en donde el primer renglón nos muestra la primer regla, el segundo la
segunda regla y el tercero la unión de los resultados.
Figura 8.2: Evaluación de reglas de control de temperatura.
La última etapa es la desdifusificación, en donde tenemos que mapear de los conjuntos

difusos de salida a un valor nítido. Para esto existen varios métodos, siendo el método
del centroide uno de los más utilizados, en donde se obtiene el centroide del conjunto
resultante y ese es el valor de salida. La línea roja del tercer renglón de la figura 8.2
representa la desdifusificación por este método. Otros métodos son el de centro máximo,
derecha máximo e izquierda máximo.
En el ejemplo mencionado solamente existe una entrada, sin embargo es posible tener
varias entradas unidas por los operadores Y y O, los cuales se representan matemáti-
camente con los operadores mínimo y máximo respectivamente. También en ocasiones
existen distintas reglas cuya salida es el mismo conjunto difuso. Para elegir la pertenencia
de ese conjunto de salida generalmente se utiliza el operador de agregación máximo.
Las variables lingüísticas utilizadas para el desarrollo del controlador PD fueron el error,
la derivada del error y la fuerza, en donde el error se refiere a la diferencia entre el valor
deseado del actuador i y el valor actual.
Utilizando MATLAB se generaron cinco funciones de pertenencia triangulares para ca-

da variable lingüísticas las cuales se observan en las figuras 8.3, 8.4 y 8.5, en donde
Figura 8.3: Funciones de pertenencia de la entrada derr
Figura 8.4: Funciones de pertenencia de la entrada d˙err
mn = muy negativo, n = negativo, c = cero, p = positivo y mp = muy positivo. Las

funciones de pertenencia se generaron inicialmente basándose en los resultados obtenidos
mediante el control PID y se fueron ajustando a prueba y error hasta que se encontró el
comportamiento deseado del sistema.
Por otra parte podemos observar las reglas en forma de matriz de asociación difusa (FAM)
en la tabla 8.1. Para la implicación se utilizó el operador mínimo, para la agergación el
operador máximo y para la desdifusificación el método del centroide.
Error (derr )
MN N C P MP
MN MN MN MN N C
N MN MN N C P
d˙err C MN N C P MP
P N C P MP MP
MP C P MP MP MP
Tabla 8.1: Matriz de asociación difusa del controlador PD
Para simular el control difuso utilizamos el algoritmo 9, el cual es muy similar al algoritmo
del control PID.
Figura 8.5: Funciones de pertenencia de la salida f
Algoritmo 9 Simulación del controlador difuso

2: Establecer la velocidad y aceleración inicial.
3: Preparar el modelo del controlador difuso (generar funciones de pertenencia y reglas).
4: Establecer tiempo inicial t = 0, tiempo de simulación T y paso ∆t.
5: Calcular el valor deseado de la longitud de los actuadores mediante la ecuación (4.9).
7: Obtener el valor de los actuadores mediante la ecuación (4.9).
8: Obtener los errores y sus derivadas dei , ḋei .
9: Calcular las fuerzas utilizando el controlador difuso.
10: Calcular la aceleración de la plataforma mediante la ecuación (7.79).
11: Calcular la aceleración de los ángulos de Euler (α̈) utilizando la ecuación (7.82).
12: Actualizar la velocidad angular α̇(t + ∆t) = α̇(t) + α̈(t + ∆t)∆t.
13: Actualizar la posición de la plataforma α(t+∆t) = α(t)+α̇(t)∆t+α̈(t+∆t)∆t2 /2.
15: fin mientras
Se utilizaron los mismos parámetros (posiciones y tiempo) del ejemplo del controlador
PID para la simulación del controlador difuso. Podemos ver en la parte superior de la
figura 8.6 el error en los actuadores y en la parte inferior la posición de la plataforma. Se
puede observar que el error de la plataforma en estado estacionario es de 0.004◦ y entra
a la banda de estabilidad del 5 % a los 1.284 segundos en el peor de los casos.
Utilizando la posición inicial ωini = 1◦ , φini = 10◦ y ψini = −5◦ , la posición final
ωf in = 1◦ , φf in = −10◦ , ψf in = −5◦ , T = 2 y ∆t = T /1000, se simularon ambos
controladores simultáneamente y los resultados los podemos observar en la figura 8.7.
Observamos que en esta ocasión el error máximo al final del tiempo de simulación fue de
0.372◦ para el controlador PID, mientras que para el controlador difuso fue de 0.179◦ en
el peor de los casos, lo cual nos muestra que la respuesta del controlador difuso fue un
poco mejor.
Por último se compararon nuevamente los controladores pero esta vez se utilizó la posición
inicial ωini = 1◦ , φini = 10◦ y ψini = −50◦ y la posición final ωf in = 1◦ , φf in = 10◦ ,
Figura 8.6: Posición de la plataforma y error en actuadores con controlador difuso
Figura 8.7: Posición de la plataforma y error en actuadores con ambos controladores
ψf in = 35◦ . El resultado se puede observar en la figura 8.8. Podemos ver que aunque
el error en los actuadores es de 0.00129 cm en el peor de los casos, la posición de la
plataforma en estado estacionario no es la posición buscada.
Esta diferencia entre la posición deseada y la real de la plataforma se debe a que como
se mencionó en el capítulo 4.2, es posible encontrar varias orientaciones del robot que
tengan las mismas longitudes di . Por esta razón no se debe de realizar el control dado
el punto inicial y el punto final, sino que se debe de calcular una trayectoria con puntos
intermedios y se debe realizar el control a través de estos puntos intermedios para asegurar
que la posición final sea la deseada. Si no se hace esto en las aplicaciones de seguridad
para las cuales se está diseñando el robot, se podría desear mover el manipulador a una
orientación y el robot se movería a otra, por lo que perdería de vista al objetivo.
Figura 8.8: Posición de la plataforma y error en actuadores con ambos controladores

Capítulo 9
Trayectoria
Se mencionó en el capítulo 8 que es posible controlar la posición de la plataforma calcu-

lando mediante cinemática inversa, la longitud deseada de los actuadores y controlando
cada uno de manera independiente, sin embargo se observó en la sección 8.2 que si no se
plantea una trayectoria es posible que la plataforma termine en una posición no deseada.
9.1. Trayectorias de posición
Para asegurarse que el robot de seguridad apunte al lugar donde se encuentra el objetivo,
es necesario generar una trayectoria que guíe a la plataforma a la orientación deseada
mediante el uso de puntos intermedios. Con el número suficiente de puntos, podemos
asegurar que el robot se moverá siempre hacia la posición donde se encuentre el intruso.
A continuación se mostrarán algunas formas posibles para generar la trayectoria, par-

tiendo de la suposición de que el robot no se encuentra en movimiento al inicio.
9.1.1. Interpolación lineal
Un método muy sencillo es mediante una interpolación lineal entre la posición inicial y
la deseada, el algoritmo 10 muestra la manera de utilizar este método.
El resultado del algoritmo 10 será una trayectoria en línea recta para cada uno de los
ángulos de Euler como se muestra en la figura 9.1
El principal problema con este tipo de trayectoria es que puede ocasionar que en algún
punto en el tiempo el robot se encuentre en una posición singular, lo cual no es deseable.
80
Capítulo 9. Trayectoria 81
Algoritmo 10 Trayectoria por interpolación lineal

2: Establecer tiempo inicial t = 0, tiempo final T y paso ∆t.
3: para todo αi (siendo αi un ángulo de Euler) hacer
4: Calcular la pendiente de la recta como ∆αi = (αi (T ) − αi (0))/2 .
6: Calcular el nuevo ángulo αi (t + ∆t) = α(t) + ∆αi .
8: fin mientras
9: fin para
Figura 9.1: Ejemplo de trayectoria mediante interpolación lineal
9.1.2. Mapa de caminos probabilístico (PRM)
A lo largo de los años se han desarrollado varios métodos para planear trayectorias
en robots evitando colisiones con obstáculos, uno de los cuales es el mapa de caminos
probabilistico [17].
En el caso de esta aplicación, no existen obstáculos físicos que el robot tenga que evitar,
sin embargo es posible utilizar las singularidades como obstáculos de tal forma que se
planee una trayectoria que vaya del punto inicial al final y esquive estas singularidades.
El algoritmo 11 muestra el procedimiento para calcular este tipo de trayectorias.
Este tipo de trayectoria nos permite ir del punto inicial al punto final asegurándonos que
el robot evitará colocarse en una posición singular, sin embargo, hay que tener en cuenta
Algoritmo 11 Trayectoria por PRM

2: Establecer tiempo inicial t = 0 y tiempo final T .
3: Establecer número de puntos a generar (n), número de conexiones (k ) y distancia
máxima entre puntos (r ).
4: i = 1
5: mientras i ≤ n hacer
6: Generar una posición de la plataforma aleatoria (q).
7: si q no es una posición singular entonces
8: Guardar el punto qi .
9: i = i + 1.
10: fin si
11: fin mientras
12: para todo qi hacer
13: para j = i + 1 hasta j = n hacer
14: Calcular la distancia (rij ) con los puntos qj .
15: si rij ≤ r entonces
16: Trazar una línea (lij ) entre qi y qj .
17: si la línea lij no choca con ninguna singularidad entonces
18: Guardar la línea lij .
19: fin si
20: fin si
21: fin para
22: Elegir las k líneas con la menor distancia y borrar las demás.
23: fin para
24: Calcular la distancia entre el punto de origen y todos los puntos qi
25: Unir el punto de origen al punto qi más cercano y que la línea que los une no cruce
por una singularidad.
26: Calcular la distancia entre el punto final y todos los puntos qi
27: Unir el punto final al punto qi más cercano y que la línea que los une no cruce por
una singularidad.
28: Generar todas las trayectorias posibles (utilizando las líneas lij ) desde el punto inicial
al punto final.
29: Mediante un algoritmo de búsqueda elegir una de las trayectorias.
30: Contar el número de puntos con los que cuenta la trayectoria (p).
31: ∆t = T /(p − 1).
32: para i = i hasta i = p − 1 hacer
33: Colocar al punto pi en el tiempo ∆p ∗ (i − 1).
34: fin para
que este algoritmo depende de los parámetros n, k y r elegidos, por lo que si no tenemos
cuidado, el algoritmo puede fallar.
En el caso de elegir un valor de n, k y r muy pequeños, existe la posibilidad de que los

puntos generados queden muy separados y no exista un camino que nos lleve desde el
punto inicial hasta el punto final. Si elegimos un valor muy grande para r, una tercera
parte del ángulo máximo por ejemplo, es posible que la trayectoria generada sea simple-
mente el punto inicial y el punto final, por lo que podríamos tener el mismo problema
que cuando no se generó la trayectoria. Si los valores de n y k son muy grandes, entonces
los cálculos pueden tomar bastante tiempo, lo cual en la aplicación real no es deseado.
Una solución a estos problemas es realizar simulaciones con distintos valores de n, k y

r hasta encontrar una combinación que genere trayectorias en la mayoría de los casos
(90 %). Esto con la finalidad de asegurarnos que casi siempre va a funcionar el algoritmo
pero además, el hecho de que falle el 10 % de las veces nos indica que el valor de n y k
no es muy grande. Una vez que se tienen estos valores debemos de seguir los pasos del
algoritmo 12.
Algoritmo 12 Trayectoria por PRM modificado

1: T rayectoria = 0.
2: mientras T rayectoria = 0 hacer
3: Generar trayectoria mediante el algoritmo 11.
4: si se generó una trayectoria adecuada entonces
5: T rayectoria = 1
6: fin si
7: fin mientras
8: para todo par de puntos de la trayectoria hacer
9: Generar una trayectoria entre puntos mediante el método de interpolación lineal
mediante el algoritmo 10.
10: fin para
Si graficamos el resultado del algoritmo 11 y el algoritmo 12, las gráficas serán iguales
(suponiendo que los valores aleatorios fueron los mismos), sin embargo, el algoritmo 12
nos asegura que aunque se de el caso en el que la trayectoria no nos lleve hasta el punto
final, como en el 10 % de las pruebas para calcular n y k, simplemente se repetirá el
algoritmo hasta llegar a una solución. Por otra parte si se da el caso de que r es muy
grande y se generaron muy pocos puntos, la interpolación lineal hecha a la trayectoria
generada por el PRM generará puntos intermedios para evitar que la plataforma se
posicione en un lugar no deseado.
Retomemos el ejemplo de la figura 8.8, pero cambiando el valor inicial de ω a −5◦ y el

valor final a 3.5◦ . Graficando la trayectoria mediante el método de interpolación lineal
obtenemos el resultado de la figura 9.1. Si observamos esta trayectoria, aproximadamente

a los 0.6 segundos tanto el ángulo ψ como el ángulo ω tienen un valor de cero.
Figura 9.2: Cálculo de la trayectoria mediante PRM
En el capítulo 6 se observó que cuando ψ = 0 los otros dos ángulos deben de ser distintos
a cero para no caer en una posición singular, lo que significa que la trayectoria generada
llevó a nuestro robot a una singularidad.
Sabiendo que la recta del ángulo φ no cruza con cero, podemos realizar una trayectoria
mediante interpolación lineal para este ángulo y una trayectoria mediante PRM para los
otros dos, esto con fin demostrativo, ya que de esta manera la gráfica es más sencilla y
fácil de apreciar.
En la figura 9.2 podemos ver el resultado del algoritmo 11 en donde el punto negro es el
punto inicial y el punto rosa el final. Las posiciones generadas están representadas por
taches rojos, mientras que las líneas negras representan las uniones entre puntos lij . El
cuadro azul representa la singularidad dada por φ = 0 y ω = 0. Por último, la línea roja
es la trayectoria encontrada por el algoritmo de búsqueda a lo ancho.
La figura 9.3 por otra parte muestra la trayectoria de los tres ángulos a través del tiempo,
es decir, el último paso del algoritmo 11. Podemos observar que a diferencia de la gráfica
de la figura 9.1, en este caso el robot llega a la misma posición final evitando caer en
singularidades.
Figura 9.3: Ejemplo de trayectoria mediante PRM
9.2. Trayectorias de velocidad y aceleración
En la sección 9.1 se muestran métodos para obtener una trayectoria que nos lleve de una
posición iicial a una final. Esta trayectoria no es mas que un conjunto de puntos por los
que debe pasar el robot en un determinado tiempo.
9.2.1. Movimiento uniformemente acelerado
Sabemos que la velocidad es la derivada de la posición respecto al tiempo, pero al no tener

la posición expresada como una función del tiempo sino como un conjunto de puntos, la
αi (t + ∆t) − αi (t)
única forma de expresar la velocidad sería α̇i (t + ∆t) = siendo αi un
∆t
ángulo de Euler.
Un detalle que debemos de observar es que tendríamos velocidades constantes entre cada
par de puntos, pero la velocidad no sería necesariamente constante para toda la trayec-
toria. Esto gráficamente se vería como una serie de escalones de velocidad que cambian
de valor en un tiempo infinitamente pequeño. El resultado de esto es que tendríamos
aceleraciones infinitas cada que hay un cambio de escalón, lo cual no es posible mas que
en una simulación.
Por esta razón es importante generar trayectorias que no solamente cumplan con los
requisitos de posición, sino además con algunas restricciones de velocidad y aceleración.
La manera más sencilla es generar un movimiento uniformemente acelerado (MUA) entre
cada par de puntos. Sabemos que las ecuaciones del movimiento uniformemente acelerado
son
1
r(t) = r0 + v0 t + at2 , (9.1)
2
v(t) = v0 + at (9.2)
donde r es la posición, r0 la posición inicial, v la velocidad, v0 la velocidad inicial y a la

aceleración.
En el caso de haber planteado la trayectoria mediante un método mencionado en la

sección 9.1 se tienen los valores de posición y velocidad inicial, así como la posición
del primer punto de la trayectoria y el tiempo que debe de haber entre ambos puntos.
Podemos ver que con estos valores podemos encontrar la aceleración despejándola de la
ecuación (9.1) como se muestra a continuación:
2
a= (r(t) − r0 − v0 t) . (9.3)
t2
Calculando la aceleración para el primer par de puntos, podemos calcular la velocidad

justo al inicio del segundo punto mediante la ecuación (9.2) y de esta manera conseguir
los valores necesarios para calcular la aceleración entre cada par de puntos. El algoritmo
13 nos muestra el procedimiento para generar la trayectoria de velocidad y aceleración.
Algoritmo 13 Trayectoria de velocidad y aceleración mediante MUA

1: Generar los n puntos de la trayectoria de posición (p) mediante algún método como
interpolación lineal o PRM.
2: v(1) = v0 .
3: para i = 1 → (n − 1) hacer
2
4: a(i) = (p(i + 1) − p(i) − v(i)∆t)
∆t2
5: v(i + 1) = v(i) + a∆t
6: fin para
En la figura 9.4 podemos ver un ejemplo en donde se desea ir de la posición inicial

[ω, φ, ψ]T = [1, −10, −10]T a la posición [7, 10, −35]T en un tiempo de un segundo.
Para generar la trayectoria de posición se utilizó el método de interpolación lineal, con
el cual se generaron catorce puntos entre la posición inicial y la posición final.
Observamos que la aceleración es constante entre cada par de puntos, lo cual ocasiona que
la aceleración tenga un comportamiento lineal. También vemos que cuando la plataforma
llega al punto deseado, la velocidad es distinta de cero, lo cual significa que se debería
generar una aceleración extremadamente grande al final del movimiento para detener
completamente al robot y evitar que se mueva una vez que llegó a la posición deseada.
En la figura 9.5 se muestra el mismo problema pero ahora generando nueve puntos in-
termedios solamente. A diferencia del ejemplo pasado observamos que en este caso la
Figura 9.4: Trayectoria mediante MUA con 14 puntos intermedios
Figura 9.5: Trayectoria mediante MUA con 9 puntos intermedios
velocidad final es igual a cero. Si observamos el comportamiento general del sistema uti-
lizando este tipo de trayectorias nos damos cuenta que si el número de puntos intermedios
es par, la velocidad final será distinta a cero, mientras que si es impar, la velocidad final
será cero.
Parecería que simplemente con utilizar un número de puntos impar logramos llevar al
robot a la posición deseada y tener una velocidad final de cero, sin embargo esto es
cierto solamente cuando la trayectoria se generó mediante interpolación lineal. En la
figura 9.6 podemos ver la trayectoria generada mediante el método de mapa de caminos
probabilístico y se observa que el número de puntos entre la posición inicial y la final es
de 5, es decir, es impar. Según lo que se observó anteriormente al ser impar, la velocidad
Figura 9.6: Trayectoria mediante PRM y MUA
final debería de ser cero, lo cual no ocurre y por esa razón no podemos generalizar que
con un número de puntos impar y trayectoria de velocidad mediante MUA la velocidad
final será cero.
9.2.2. Trayectoria polinomial
El problema de la trayectoria mediante MUA es que nos preocupamos solamente de tener

aceleraciones constantes, pero no ponemos ningún tipo de restricción a la velocidad final.
Ahora bien, supongamos que nos encontramos en el último par de puntos, podemos
generar una subtrayectoria entre estos dos puntos representando a la posición de la
plataforma mediante una ecuación de tercer grado como se muestra a continuación:
r(t) = a0 (t − tini )3 + a1 (t − tini )2 + a2 (t − tini ) + a3 (9.4)
donde tini es el tiempo que se encuentra el penúltimo punto generado inicialmente. Si de-
rivamos la ecuación (9.4) obtenemos la ecuación de la velocidad, si derivamos la velocidad
obtenemos la ecuación de la aceleración:
v(t) = 3a0 (t − tini )2 + 2a1 (t − tini ) + a2 , (9.5)
a(t) = 6a0 (t − tini ) + 2a1 . (9.6)
Para calcular los coeficientes normalizamos al tiempo entre 0 y 1 de tal forma que tini = 0
y tf in = 1. Dado que conocemos la posición y velocidad del penúltimo punto podemos
expresar las ecuaciones (9.4) y (9.5) como
a3 = rini , (9.7)
a2 = vini . (9.8)
Nótese que estamos permitiendo un salto en la aceleración ya que no estamos usando la

aceleración inicial como una condición para generar las ecuaciones. Cuando t = tf inal = 1
conocemos el valor de la posición final y si además establecemos la velocidad y aceleración
final igual a cero podemos escribir las ecuaciones (9.4), (9.5) y (9.6) como
rf in = a0 + a1 + vini + rini (9.9)
vf in = 3a0 + 2a1 + vini = 0, (9.10)
Resolviendo el sistema de ecuaciones formado por las ecuaciones (9.9) y (9.10) obtenemos
a0 = vini + 2rini + rf in , (9.11)
a1 = 3rf in − 3vini − 3rini . (9.12)
Como se normalizó el tiempo entre cero y uno para obtener los coeficientes, es necesario
normalizar el tiempo cuando se va a calcular una posición, velocidad o aceleración. Por
ejemplo supongamos que el tiempo total de nuestra trayectoria es de un segundo y que
el penúltimo punto se encuentra en el tiempo t = 0.9. La diferencia de tiempo entre
estos dos puntos es de 0.1 segundos, por lo que para normalizar el tiempo es necesario
dividirlo entre 0.1. Si se genera una subtrayectoria de 5 puntos, éstos se encontrarían en
los tiempos t = 0.02, 0.04, 0.06, 0.08, 0.1. Al dividirlos entre 0.1, se encontrarían en los
tiempos t = 0.2, 0.4, 0.6, 0.8, 1, con lo que podemos aplicar las ecuaciones (9.4), (9.5) y
(9.6). Una vez calculado el valor de cada punto simplemente los regresamos a su tiempo
original.
Utilizando el mismo ejemplo de la figura 9.4 se generó una trayectoria con final polinomial
la cual se puede observar en la figura 9.7. Mientras que en la figura 9.4 la velocidad final es
distinta de cero, en la figura 9.7 el valor de la velocidad final es cero. Este comportamiento
es mucho mejor que el anterior, ya que la aceleración requerida para frenar al robot fue
muy pequeña en comparación con la que se hubiera necesitado si no se hubiera usado la
función polinomial en el último punto.
Figura 9.7: Trayectoria mediante interpolación lineal y MUA con final polinomial
Figura 9.8: Trayectoria mediante PRM y MUA con final polinomial
De la misma manera se simuló una trayectoria generada mediante PRM utilizando una
trayectoria polinomial para el último punto. El resultado lo observamos en la figura 9.8
y nuevamente observamos que la velocidad final es igual a cero.
Observando el comportamiento de la aceleración notamos que existen cambios muy brus-

cos y además de esto son aceleraciones bastante grandes para nuestro robot (más de
10 rad/s2 ). Al tener aceleraciones grandes, la fuerza que tienen que ejercer los actua-
dores será alta ya que depende de la aceleración. Mejores actuadores significa un mayor
costo del robot, por lo que al reducir las aceleraciones podemos reducir el costo de nuestro
sistema.
Para esto podemos aproximar al conjunto de posiciones intermedias de la plataforma

a una función polinomial mediante el método de mínimos cuadrados. Con esto surgen
dos problemas: el primero es que la función aproximada puede hacer que el robot caiga
en una singularidad que se había evitado, el segundo es que el valor inicial no va a ser
exactamente el mismo que se tiene.
Para resolver el primer problema podemos hacer la aproximación y verificar que la tra-
yectoria no haya sido modificada lo suficiente como para caer en una singularidad. En
caso de que si caiga en una singularidad tenemos dos opciones:
Aumentar el orden del polinomio para que la aproximación sea mejor.
En caso de ser una trayectoria aleatoria como PRM, simplemente generar una
nueva trayectoria.
El segundo problema es más preocupante ya que la posición y velocidad inicial del robot
ya está dada, así que para resolver este problema podemos hacer lo siguiente. Hacemos
la aproximación de la función desde el segundo punto hasta el penúltimo y después
utilizamos el método ya mencionado para encontrar la subtrayectoria entre los últimos
dos puntos. Por último hacemos algo similar en el primer par de puntos, pero esta vez
utilizando un polinomio de quinto orden, cuya ecuación es
r(t) = a0 t5 + a1 t4 + a2 t3 + a3 t2 + a4 t + a5 (9.13)
donde t se encuentra normalizado entre cero y uno.
Derivando la ecuación (9.13) obtenemos la velocidad y derivando ésta obtenemos la

aceleración:
v(t) = 5a0 t4 + 4a1 t3 + 3a2 t2 + 2a3 t + a4 , (9.14)
a(t) = 20a0 t3 + 12a1 t2 + 6a2 t + 2a3 . (9.15)
Como conocemos los valores de posición, velocidad y aceleración iniciales podemos eva-
luar las ecuaciones (9.13), (9.14) y (9.15) en t = 0:
r(0) = a5 = rini , (9.16)
v(0) = a4 = vini , (9.17)
a(0) = 2a3 = aini . (9.18)
Al hacer la aproximación de los puntos intermedios mediante mínimos cuadrados, pode-

mos obtener la posición, velocidad y aceleración inicial de la curva, las cuales podemos
Figura 9.9: Trayectoria polinomial (interpolación lineal)
igualar a la posición, velocidad y aceleración final de la trayectoria entre los primeros

dos puntos. Si evaluamos las ecuaciones (9.13) y (9.14) en t = 1 tenemos:
r(1) = a0 + a1 + a2 + a3 + a4 + a5 = rf in (9.19)
v(1) = 5a0 + 4a1 + 3a2 + 2a3 + a4 = vf in , (9.20)
a(1) = 20a0 + 12a1 + 6a2 + 2a3 = af in . (9.21)
Resolviendo este sistema de tres ecuaciones y tres incógnitas obtenemos
af in − 6vf in + 12rf in − 2a3 − 6a4 − 12a5

a0 = , (9.22)
2
a2 = 4rf in − vf in + a0 − 2a3 − 3a4 − 4a5 , (9.23)
a1 = rf in − a0 − a2 − a3 − a4 − a5 . (9.24)
Una vez que se tienen los coeficientes podemos generar n puntos intermedios entre la
posición inicial y el primer punto. Al final obtenemos el resultado que se observa en la
figura 9.9.
Observamos que la trayectoria de posición es un poco diferente de la original mostrada

en la figura 9.7, pero tanto el comportamiento de la velocidad como el de la aceleración
cambia bastante; la aceleración no tiene ni cambios tan bruscos ni valores tan grandes
como en la trayectoria mediante MUA, mientras que la velocidad termina en cero y
además no tiene muchos cambios, de hecho es constante la mayor parte del tiempo.
Figura 9.10: Trayectoria polinomial (PRM)
Se probó este mismo método pero ahora basándose en una trayectoria generada mediante
PRM y el resultado se observa en la figura 9.10.
El resultado en este caso no es una velocidad constante como en el caso de interpolación

lineal pero varía menos que con el MUA. Observamos que tampoco existen grandes
cambios en la aceleración, inclusive ésta es constante un poco más de la mitad del tiempo
y la aceleración máxima es de 5 rad/s2 en lugar de 35 rad/s2 como en el caso del MUA.
El problema es que al alterar la trayectoria de posición puede que el robot caiga en

una singularidad que se había evitado mediante el uso de PRM. Un ejemplo de esto lo
podemos observar en la figura 9.11, donde aproximadamente a los 0.18 segundos el robot
tiene los ángulos ψ y ω iguales a cero, que como vimos en el capítulo 6 es una posición
singular.
Figura 9.11: Trayectoria polinomial (PRM) cruzando una posición singular

Capítulo 10
Conclusiones
El uso de un robot paralelo esférico como mecanismo de orientación en un sistema de

seguridad tiene varias ventajas respecto al uso de un manipulador serial como el hecho
de contar con una mayor precisión y capacidad de carga, sin embargo, tiene la desventaja
de tener un menor espacio de trabajo.
El estudio total de un manipulador paralelo es importante ya que para la aplicación final,

se necesita el conocimiento de la cinemática inversa para poder calcular la longitud que
deben de tener los actuadores si se desea alcanzar alguna orientación, sin embargo, se
debe de generar una trayectoria con ayuda de la cinemática directa local (RNA) para
evitar que el robot termine en una posición no deseada.
Por otra parte el estudio de la dinámica inversa es importante si se quiere controlar

al manipulador como un sistema completo y no controlar los actuadores de manera
independiente, mientras que la dinámica directa nos ayuda a simular el manipulador
y nuestros controladores. El análisis jacobiano es importante dentro del estudio de la
dinámica, ya que nos indica las singularidades con las que cuenta el robot y que se deben
de evitar en la trayectoria, ya que no es posible calcular las fuerzas en esos puntos.
Al estudiar el espacio de trabajo podemos optimizar los parámetros del manipulador para
maximizarlo y a su vez mantener el tamaño del robot lo más cercano a uno deseado. De
esta manera el robot de seguridad tiene un campo de visión mayor y al mantener el
tamaño cerca de uno deseado, se puede diseñar para que quepa tanto en un espacio
pequeño como en uno grande.
Si se toman en cuenta todos los aspectos del manipulador, es posible diseñar un manipu-
lador paralelo de orientación que tenga un movimiento considerable, un tamaño deseado,
sea capaz de seguir a sus objetivos con gran precisión y pueda sostener grandes cargas,
como sistema de visión o algún tipo de arma.
94
Apéndice A
Códigos de MATLAB
A.1. Cinemática inversa
1 function [L1,L2,L3] = inversa(omega,fi,psi,h,rb,rp)

2 % Funcion que recibe los angulos de Euler y los parametros del
3 % robot y regresa la longitud de las patas
4
5 Ai=[rb 0 −h; −.5∗rb sqrt(3)∗rb/2 −h ; −.5∗rb −sqrt(3)∗rb/2 −h ];

6 Bi=[rp 0 0; −.5∗rp sqrt(3)∗rp/2 0 ; −.5∗rp −sqrt(3)∗rp/2 0 ];
7
8 Ra=[1 0 0; 0 cos(omega) −sin(omega);0 sin(omega) cos(omega)];

9 Rb=[cos(fi) 0 sin(fi);0 1 0;−sin(fi) 0 cos(fi)];
10 Rc=[cos(psi) −sin(psi) 0;sin(psi) cos(psi) 0;0 0 1];
11 Rot=Ra∗Rb∗Rc;
12
13 Bn=(Rot∗Bi’)’;
14
15 L1=norm(Bn(1,:)−Ai(1,:));
16 L2=norm(Bn(2,:)−Ai(2,:));
17 L3=norm(Bn(3,:)−Ai(3,:));
95
Apéndice A. Códigos de MATLAB 96
A.2. Cinemática directa por método geométrico
1 function Bt=cinematica_directa(h,rb,rp,L1,L2,L3,graficar,paso)
2 % Funcion que recibe las longitudes de patas deseadas, el paso y los
3 % paraametros del robot y regresa las posibles combinaciones de puntos Bi que
4 % cumplan.
7 verts1=[];
8 faces=[];
9 i=1;
10 for t = 0:0.1:2∗pi;
11 verts1 = [verts1;cos(t)∗rb sin(t)∗rb −h];
12 faces=[faces i];
13 i=i+1;
14 end
15
16 %Circulo para l1
17 A=−2∗Ai(1,1);
18 B=−2∗Ai(1,2);
19 C=−2∗Ai(1,3);
20 D=rp^2+(A/2)^2+(B/2)^2+(C/2)^2−L1^2;
21 circ1=[];
22 circ12=[];
23
24
25 t1=[0 0 1i];
26 t2=[0 0 1i];
27 for x=−rp:paso:rp
28 yint=B^2∗(2∗D+2∗A∗x)^2−4∗(B^2+C^2)∗...
29 (D^2−C^2∗rp^2+2∗A∗D∗x+A^2∗x^2+C^2∗x^2);
30 zint=C^2∗(2∗D+2∗A∗x)^2−4∗(C^2+B^2)∗...
31 (D^2−B^2∗rp^2+2∗A∗D∗x+A^2∗x^2+B^2∗x^2);
32 y1=(−2∗B∗D−2∗A∗B∗x−sqrt(yint))/(2∗(B^2+C^2));
33 z1=(−2∗C∗D−2∗A∗C∗x−sqrt(zint))/(2∗(C^2+B^2));
34 y2=(−2∗B∗D−2∗A∗B∗x+sqrt(yint))/(2∗(B^2+C^2));
35 z2=(−2∗C∗D−2∗A∗C∗x+sqrt(zint))/(2∗(C^2+B^2));
36
37 if isreal(t1)&&isreal(t2)
38 agregar=1;
39 elseif isreal(y1)&&isreal(y2)&&isreal(z1)&&isreal(z2)
40 agregar=1;
41 else
42 agregar=0;
43 end
44
45 if agregar==1
46 circ1=[circ1; x real(y1) real(z1)];
48 end
49 t1=[x y1 z1];
50 t2=[x y2 z2];
51 end
52 circ1=[circ1;sortrows((circ12),−1)];
53
54
55
56
57 %Circulo para l2
58 A2=−2∗Ai(2,1);
59 B2=−2∗Ai(2,2);
60 C2=−2∗Ai(2,3);
61 D2=rp^2+(A2/2)^2+(B2/2)^2+(C2/2)^2−L2^2;
62 circ2=[];
63 circ22=[];
64
65 t1=[0 0 1i];
66 t2=[0 0 1i];
67
69 yint=B2^2∗(2∗D2+2∗A2∗x)^2−4∗(B2^2+C2^2)∗...
70 (D2^2−C2^2∗rp^2+2∗A2∗D2∗x+A2^2∗x^2+C2^2∗x^2);
71 zint=C2^2∗(2∗D2+2∗A2∗x)^2−4∗(C2^2+B2^2)∗...
72 (D2^2−B2^2∗rp^2+2∗A2∗D2∗x+A2^2∗x^2+B2^2∗x^2);
73 y1=(−2∗B2∗D2−2∗A2∗B2∗x−sqrt(yint))/(2∗(B2^2+C2^2));
74 z1=(−2∗C2∗D2−2∗A2∗C2∗x−sqrt(zint))/(2∗(C2^2+B2^2));
75 y2=(−2∗B2∗D2−2∗A2∗B2∗x+sqrt(yint))/(2∗(B2^2+C2^2));
76 z2=(−2∗C2∗D2−2∗A2∗C2∗x+sqrt(zint))/(2∗(C2^2+B2^2));
77
78
80 agregar=1;
82 agregar=1;
83 else
84 agregar=0;
85 end
86
87 if agregar==1
90 end
91 t1=[x y1 z1];
92 t2=[x y2 z2];
93 end
95
96 %Circulo para l3
97 A3=−2∗Ai(3,1);
98 B3=−2∗Ai(3,2);
99 C3=−2∗Ai(3,3);
100 D3=rp^2+(A3/2)^2+(B3/2)^2+(C3/2)^2−L3^2;
101 circ3=[];
102 circ32=[];
103
104
105 t1=[0 0 1i];

106 t2=[0 0 1i];
107

109 yint=B3^2∗(2∗D3+2∗A3∗x)^2−4∗(B3^2+C3^2)∗...
110 (D3^2−C3^2∗rp^2+2∗A3∗D3∗x+A3^2∗x^2+C3^2∗x^2);
111 zint=C3^2∗(2∗D3+2∗A3∗x)^2−4∗(C3^2+B3^2)∗...
112 (D3^2−B3^2∗rp^2+2∗A3∗D3∗x+A3^2∗x^2+B3^2∗x^2);
113 y1=(−2∗B3∗D3−2∗A3∗B3∗x−sqrt(yint))/(2∗(B3^2+C3^2));
114 z1=(−2∗C3∗D3−2∗A3∗C3∗x−sqrt(zint))/(2∗(C3^2+B3^2));
115 y2=(−2∗B3∗D3−2∗A3∗B3∗x+sqrt(yint))/(2∗(B3^2+C3^2));
116 z2=(−2∗C3∗D3−2∗A3∗C3∗x+sqrt(zint))/(2∗(C3^2+B3^2));
117
118
120 agregar=1;
122 agregar=1;
123 else
124 agregar=0;
125 end
126
127 if agregar==1
130 end
131 t1=[x y1 z1];
132 t2=[x y2 z2];
133 end
135
136 %En este punto ya se tienen los tres circulos, falta buscar los puntos de
137 %union
138 c1=[0 0 0];
139 c2=[0 0 0];
140
141 u=[];
142 u2=[];
143 for i=1:size(circ1,1)
144 for j=1:size(circ2,1)
145 posible=0;
146
147 temp1=circ1(i,:)−circ2(j,:);
148 d1=round(1000∗(temp1∗temp1’)/(3∗rp^2))/1000;
149
150 if d1==1
151 c1=[circ1(i,1) circ1(i,2) circ1(i,3)];
152 c2=[circ2(j,1) circ2(j,2) circ2(j,3)];
153 posible=1;
154 u2=[u2;c1 c2];
155 end
156
157
158 if posible==1
159 for k=1:size(circ3,1)
160 temp1=c1(1,:)−circ3(k,:);
161 temp2=c2(1,:)−circ3(k,:);
164 if d2==1&&d3==1
165 u=[u;c1 c2 circ3(k,:)];
166 end
167 end
168 end
169 end
170 end
171
172 u=unique(u,’rows’);
173 q=u;
174 verts=[];
175 face=[];
176 borr=[];
177 thres=5;
178
179 if size(q,1)>1
180 for i=size(q,1):−1:2
181 for j=i−1:−1:1
182 dist=sqrt(sum((q(i,:)−q(j,:)).^2));
183 if dist<thres
184 borr=[borr j];
185 end
186 end
187 end
188 end
189 borr=unique(borr);
190 if ~isempty(borr)
191 for i=1:length(borr)
192 u(borr(i),:)=[];
193 borr=borr−1;
194 end
195 end
196
197 if graficar==1
198 for i=1:size(u,1)
199 verts=[verts;u(i,1),u(i,2),u(i,3);...
200 u(i,4),u(i,5),u(i,6);u(i,7),u(i,8),u(i,9)];
201 face{i}=[(i−1)∗3+1 (i−1)∗3+2 (i−1)∗3+3];
202 end
203
204 %Dibujar base

205 patch(’Faces’,faces,’Vertices’,verts1,’FaceColor’,’b’,’FaceAlpha’,0.75);
206 xlabel(’x’); ylabel(’y’); zlabel(’z’)
207 hold on
208
209 % Codigo para graficar los circulos

210 plot3(circ1(:,1),circ1(:,2),circ1(:,3),’k’,’LineWidth’,4)
211 plot3(circ2(:,1),circ2(:,2),circ2(:,3),’m’,’LineWidth’,4)
212 % plot3(circ3(:,1),circ3(:,2),circ3(:,3),’Color’,[.5 .5 .5],’LineWidth’,4)
213
214 grid on
215
216 % Codigo para graficar uniones entre circulos 1 y 2

217 for i=1:2:size(u2,1)
218 xx=[u2(i,1) u2(i,4)];
219 yy=[u2(i,2) u2(i,5)];
220 zz=[u2(i,3) u2(i,6)];
221 plot3(xx,yy,zz,’r’,’LineWidth’,.5)
222 end
223 % scatter3(Ai(:,1),Ai(:,2),Ai(:,3),’filled’,’MarkerFaceColor’,’k’)
224 end
225
226 co=[.8 0 0;0 .8 0;.4 0 .8;.8 0 .8; 0 .8 .8;.8 .8 0;.8 .4 0];

227 % Grafica de posibles combinaciones
228 % for i=1:size(u,1)
229 % Bt{i}=[u(i,1) u(i,2) u(i,3); u(i,4) u(i,5) u(i,6);...
230 % u(i,7) u(i,8) u(i,9)];
231 % if graficar==1
232 % patch(’Faces’,face{i},’Vertices’,verts,’FaceColor’,co(i,:));
233 %
234 % l1=[Ai(1,1) Bt{i}(1,1);Ai(1,2) Bt{i}(1,2);Ai(1,3) Bt{i}(1,3)];
237 %
238 % plot3(l1(1,:),l1(2,:),l1(3,:),’LineWidth’,4,’Color’,co(i,:))
241 % end
242 % end
A.3. Cinemática directa por Newton-Raphson y RNA
1 function [omega,fi,psi] = directa(L1,L2,L3,oma,fia,psia,rp,red)

2 % Funcion que recibe los angulos de Euler actuales, la longitud de
3 % los actuadores propuesta, los parametros del robot y la red
4 % neuronal y regresa los angulos de Euler en donde terminara el robot.
5 global l1 l2 l3
6
7 l1 = L1;
8 l2 = L2;
9 l3 = L3;
10
11 Bi=[rp 0 0];
12
13 Ra=[1 0 0; 0 cos(oma) −sin(oma);0 sin(oma) cos(oma)];

14 Rb=[cos(fia) 0 sin(fia);0 1 0;−sin(fia) 0 cos(fia)];
15 Rc=[cos(psia) −sin(psia) 0;sin(psia) cos(psia) 0;0 0 1];
16 Rot=Ra∗Rb∗Rc;
17
18 B=(Rot∗Bi’)’;
19
20 in=[L1; L2; L3; sign(B(1)); sign(B(2)); sign(B(3))];

21
22 y=sim(red,in);
23
24 MaxIter=500;
25 TolX=1e−10;
26 x = newtonraphson(@f, @df, y(:,1)∗pi/180, TolX, MaxIter);
27 salidas = x(:,size(x,2));
28
29 fi = salidas(1);
30 psi = salidas(2);
31 omega = salidas(3);
A.3.1. Entrenamiento de red neuronal
1 function net = red(datos)

2
3 % Entradas−>[L1 L2 L3 x y z]
4 % Salidas−>[fi psi omega]
5
6 % Estas matrices son especificas para ciertos parametros rb, rp y h.

7 % Matriz de #entradas filas y #muestras columnas (6xN)
8 I=datos(:,1:6)’;
9 % Matriz de #salidas filas y #muestras columnas (3xN)
10 O=datos(:,7:9)’;
11
12 % Neuronas por capa (Salidas calculadas automaticamente)

13 S=[4,8];
14 % Creacion de red
15 net=newff(I,O,S,{’tansig’,’tansig’,’purelin’},’trainlm’);
16
17 % Parametros de entrenamiento de red

18 net.trainParam.epochs = 500;
19 net.trainParam.max_fail = round(size(I,2)∗.25);
20
21 net = train(net,I,O);
22 save(’red.mat’,’net’)
A.3.2. Método de Newton-Raphson
1 function [x, results] = newtonraphson(f, Df, x0, TolX, MaxIter)

2 %−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−
3 % PURPOSE:
4 % Newton Raphson Optimization Procedure for a set of Non−Linear Equations
5 %−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−
6 % USAGE:
7 % results = newtonraphson(f, df, startingvals, TolX, MaxIter)
8 %
9 % INPUTS:
10 % f: the M−file containing the functions,
11 % df: the M−file containing the Jacobian vector,
12 % x0: initial set of parameters,
13 % TolX: Termination tolerance of the variables,
14 % MaxIter: Maximum Number of Iterations,
15 %
16 % NOTES:
17 % In case no Jacobian vector is presented,
18 % then the derivatives are estimated using the Broyden Method.
19 %
20 % The default values of TolX and MaxIter are 1e−8 and 500, respectivelly.
21 %−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−
22 % OUTPUTS:
23 % xf: the estimated values
24 % results: further results
25 %−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−
26 % Author:
27 % Alexandros Gabrielsen, a.gabrielsen@city.ac.uk
28 % Date: 07/2010
29 %−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−
30
31 if nargin == 0
32 error(’Function, Derivative of the Function, Initial Values and Options’)
33 end
34
35 if isempty(TolX)
36 TolX = 1e−8;
37 end
38
39 if isempty(MaxIter)
40 MaxIter = 500;
41 end
42
43 x(:,1)= x0; % Initial Parameters

44 F(:,1)= f(x); % Initial Values of the Function
45 index = 1; % Index
46 e=1; % Error
47 iter=0; % Iterations
48 n=length(x0); % Number of Parameters
49 m1=’No Jacobian Vector was presented, continue estimation with’;
50 m2=’initial Broyden procedure followed by Sherman−Morrison formula’;
51 if isempty(Df)
52 disp([m1 m2]);
53 results.method = ’Newton Raphson, Broyden Method initial Jacobian estimation’;
54 Dt=1e−0; % Specify Time Step, Function Dependent
55 else
56 results.method = ’Newton Raphson’;
57 end
58
59 while e>TolX & iter <=MaxIter

60 % Estimating Jacobian Vector using Broyden Method
61 if iter == 0 & isempty(Df) % Estimating the initial Jacobian vector
62 ii=1; % Index used in the estimation of the initial Jacobian vector
63 for jj = 1:n
64 t1=x(:,1); t1(jj)=x(jj)+Dt; f1=f(t1);
65 t2=x(:,1); t2(jj)=x(jj)−Dt; f2=f(t2);
66 df(:,ii) = ((f1−f2)/2∗Dt)’;
67 ii=ii+1;
68 end
69 clear ii jj
70 x(:,index+1) = x(:,index) − inv(df)∗(f(x(:,index))’);
71 elseif iter >= 1 & isempty(Df)
72 df = df + ((F(:,index)−F(:,index−1))−df∗(x(:,index)−...
73 x(:,index−1)))∗((x(:,index)−x(:,index−1))’)/...
74 (((x(:,index)−x(:,index−1))’)∗(x(:,index)−x(:,index−1)));
75 x(:,index+1) = x(:,index) − inv(df)∗(f(x(:,index))’);
76 D(:,:,index) = df;
77 else
78 x(:,index+1) = x(:,index) − inv(Df(x(:,index)))∗(f(x(:,index))’);
79 D(:,:,index) = Df(x(:,index));
80 end
81 F(:,index+1) = f(x(:,index+1));
82 e=abs(x(:,index)−x(:,index+1));
83 index=index+1;
84 iter=iter+1;
85 end
86
87 % Saving Results
88 results.parameters = x; % Parameters
89 results.function=F; % Saving the function values at each step
90 results.Jacobian=D; % Saving the Jacobian Matrix at each step
91 results.iter=iter; % Number of iterations required for convergence
92
93 % Displaying estimated parameters

94 % disp(’The estimated parameters are’);
95 % disp(x(:,iter));
96
97 clear x0 index e iter

98 end % End of Function
A.3.2.1. Función f utilizada en el método de Newton-Raphson
1 function y = f(x)
2 %x(1)−>fi
3 %x(2)−>psi
4 %x(3)−>omega
5 global h rb rp l1 l2 l3
6
7 y(1) = h^2−l1^2+rb^2+rp^2−2∗rb∗rp∗cos(x(1))∗cos(x(2))...
8 −2∗h∗rp∗cos(x(2))∗cos(x(3))∗sin(x(1))+2∗h∗rp∗sin(x(2))∗sin(x(3));
9 y(2) = 1/2∗(2∗h^2−2∗l2^2+2∗rb^2+2∗rp^2+sqrt(3)∗rb∗rp∗cos(x(3))...
10 ∗sin(x(2))+2∗sqrt(3)∗h∗rp∗cos(x(3))∗sin(x(1))∗sin(x(2))−rb∗rp∗...
11 cos(x(1))∗(cos(x(2))+sqrt(3)∗sin(x(2)))−2∗h∗rp∗sin(x(2))∗sin(x(3))...
12 +3∗rb∗rp∗sin(x(1))∗sin(x(2))∗sin(x(3))+rp∗cos(x(2))∗(cos(x(3))...
13 ∗(−3∗rb+2∗h∗sin(x(1)))+sqrt(3)∗(2∗h+rb∗sin(x(1)))∗sin(x(3))));
14 y(3) = 1/4∗(−4∗l3^2+(rb+rp∗cos(x(1))∗(−cos(x(2))+sqrt(3)∗sin(x(2))))^2+...
15 (−2∗h+sqrt(3)∗rp∗cos(x(3))∗sin(x(1))∗sin(x(2))+rp∗sin(x(2))...
16 ∗sin(x(3))+cos(x(2))∗(−rp∗cos(x(3))∗sin(x(1))+sqrt(3)∗rp...
17 ∗sin(x(3))))^2+(rp∗cos(x(3))∗sin(x(2))+rp∗cos(x(2))∗...
18 (sqrt(3)∗cos(x(3))+sin(x(1))∗sin(x(3)))−sqrt(3)∗...
19 (rb+rp∗sin(x(1))∗sin(x(2))∗sin(x(3))))^2);
20 end
A.3.2.2. Función df utilizada en el método de Newton-Raphson
1 function J=df(x)
2 % x(1)−>fi
3 % x(2)−>psi
4 % x(3)−>omega
5 global h rb rp
6
7 f1x1 = 2∗rp∗cos(x(2))∗(−h∗cos(x(1))∗cos(x(3))+rb∗sin(x(1)));
8 f2x1 = 1/2∗rp∗(rb∗sin(x(1))∗(cos(x(2))+sqrt(3)∗sin(x(2)))+cos(x(1))...
9 ∗(sin(x(2))∗(2∗sqrt(3)∗h∗cos(x(3))+3∗rb∗sin(x(3)))+cos(x(2))...
10 ∗(2∗h∗cos(x(3))+sqrt(3)∗rb∗sin(x(3)))));
11 f3x1 = 1/2∗rp∗(rb∗sin(x(1))∗(cos(x(2))−sqrt(3)∗sin(x(2)))+cos(x(1))...
12 ∗(sin(x(2))∗(−2∗sqrt(3)∗h∗cos(x(3))+3∗rb∗sin(x(3)))+cos(x(2))...
13 ∗(2∗h∗cos(x(3))−sqrt(3)∗rb∗sin(x(3)))));
14 f1x2 = 2∗rp∗(rb∗cos(x(1))∗sin(x(2))+h∗cos(x(3))∗sin(x(1))∗sin(x(2))...
15 +h∗cos(x(2))∗sin(x(3)));
16 f2x2 = 1/2∗rp∗(rb∗cos(x(1))∗(−sqrt(3)∗cos(x(2))+sin(x(2)))+sin(x(2))...
17 ∗(cos(x(3))∗(3∗rb−2∗h∗sin(x(1)))−sqrt(3)∗(2∗h+rb∗sin(x(1)))...
18 ∗sin(x(3)))+cos(x(2))∗(sqrt(3)∗cos(x(3))∗(rb+2∗h∗sin(x(1)))...
19 +(−2∗h+3∗rb∗sin(x(1)))∗sin(x(3))));
20 f3x2 = 1/2∗rp∗(rb∗cos(x(1))∗(sqrt(3)∗cos(x(2))+sin(x(2)))−cos(x(2))...
21 ∗(sqrt(3)∗cos(x(3))∗(rb+2∗h∗sin(x(1)))+(2∗h−3∗rb∗sin(x(1)))...
22 ∗sin(x(3)))+sin(x(2))∗(cos(x(3))∗(3∗rb−2∗h∗sin(x(1)))+sqrt(3)...
23 ∗(2∗h+rb∗sin(x(1)))∗sin(x(3))));
24 f1x3 = 2∗h∗rp∗(cos(x(3))∗sin(x(2))+cos(x(2))∗sin(x(1))∗sin(x(3)));
25 f2x3 = 1/2∗rp∗(cos(x(2))∗(sqrt(3)∗cos(x(3))∗(2∗h+rb∗sin(x(1)))...
26 +(3∗rb−2∗h∗sin(x(1)))∗sin(x(3)))−sin(x(2))∗(cos(x(3))...
27 ∗(2∗h−3∗rb∗sin(x(1)))+sqrt(3)∗(rb+2∗h∗sin(x(1)))∗sin(x(3))));
28 f3x3 = 1/2∗rp∗(−cos(x(2))∗(sqrt(3)∗cos(x(3))∗(2∗h+rb∗sin(x(1)))...
29 +(−3∗rb+2∗h∗sin(x(1)))∗sin(x(3)))+sin(x(2))∗(cos(x(3))...
30 ∗(−2∗h+3∗rb∗sin(x(1)))+sqrt(3)∗(rb+2∗h∗sin(x(1)))∗sin(x(3))));
31
32 J = [f1x1 f1x2 f1x3; f2x1 f2x2 f2x3; f3x1 f3x2 f3x3]; %Jacobiano
33 end
A.4. Cálculo del espacio de trabajo
1 function [puntos,o,f,p] = Wspace(h, rb, rp,ang,D,paso)

2 % Funcion que recibe los parametros del robot h, rb, rp, el angulo permitido
3 % y el paso deseado y regresa los puntos que pertenecen al espacio de
4 % trabajo del robot, asi como los angulos de Euler maximos y minimos
5 puntos=[];
6 Bi=[rp , 0, 0;...
7 rp ∗ cos(120∗pi/180), rp ∗ sin(120∗pi/180), 0;...
8 rp ∗ cos(240∗pi/180), rp ∗ sin(240∗pi/180), 0];
9 Ai=[rb 0 −h;...
10 rb ∗ cos(120∗pi/180), rb ∗ sin(120∗pi/180), −h;...
11 rb ∗ cos(240∗pi/180), rb ∗ sin(240∗pi/180), −h];
12 % longitud en posicion cero
13 l=norm(Ai(1,:)−Bi(1,:));
14 %lmax − lmin = e1 = e2
15 lmin = 2 ∗ l / 3;
16 lmax = 4 ∗ l / 3;
17
18 maxf=0;
19 maxp=0;
20 maxo=0;
21 minf=0;
22 minp=0;
23 mino=0;
24 %% Calculo de fi
25 Ra=[1 0 0;0 1 0;0 0 1];
26 Rc=[1 0 0;0 1 0;0 0 1];
27
28 for l=0:1:180
29 fi=pi∗l/180;
31 R=Ra∗Rb∗Rc;
32 %Calculo de plano de la plataforma
33 B=(R∗Bi’)’;
34 pos=angulo(Ai,B,ang);
35 if pos==1
36 pos=longitud(Ai,B,lmin,lmax);
37 if pos==1
38 pos=colision(Ai,B,h,D);
39 end
40 end
41 if pos==1
42 maxf=fi;
43 else
44 break
45 end
46 end
47
48 for l=0:−1:−180
49 fi=pi∗l/180;
51 R=Ra∗Rb∗Rc;
53 B=(R∗Bi’)’;
55 if pos==1
57 if pos==1
59 end
60 end
61 if pos==1
62 minf=fi;
63 else
64 break
65 end
66 end
67
68 %% Calculo de psi
69 Ra=[1 0 0;0 1 0;0 0 1];
70 Rb=[1 0 0;0 1 0;0 0 1];
71
72 for l=0:1:180
73 psi=pi∗l/180;
75 R=Ra∗Rb∗Rc;
77 B=(R∗Bi’)’;
79 if pos==1
81 if pos==1
83 end
84 end
85 if pos==1
86 maxp=psi;
87 else
88 break
89 end
90 end
91
92 for l=0:−1:−180
93 psi=pi∗l/180;
95 R=Ra∗Rb∗Rc;
97 B=(R∗Bi’)’;
99 if pos==1
101 if pos==1
103 end
104 end
105 if pos==1
106 minp=psi;
107 else
108 break
109 end
110 end
111
112 %% Calculo de omega

113 Rc=[1 0 0;0 1 0;0 0 1];
114 Rb=[1 0 0;0 1 0;0 0 1];
115
116 for l=0:1:180

117 om=pi∗l/180;
118 Ra=[1 0 0; 0 cos(om) −sin(om);0 sin(om) cos(om)];
119 R=Ra∗Rb∗Rc;
121 B=(R∗Bi’)’;
123 if pos==1
125 if pos==1
127 end
128 end
129 if pos==1
130 maxo=om;
131 else
132 break
133 end
134 end
135
136 for l=0:−1:−180

137 om=pi∗l/180;
139 R=Ra∗Rb∗Rc;
141 B=(R∗Bi’)’;
143 if pos==1
145 if pos==1
147 end
148 end
149 if pos==1
150 mino=om;
151 else
152 break
153 end
154 end
155
156 %%
157 o=[mino;maxo];
158 f=[minf;maxf];
159 p=[minp;maxp];
160
161 minf=round(minf∗180/pi);
162 mino=round(mino∗180/pi);
163 minp=round(minp∗180/pi);
164 maxf=round(maxf∗180/pi);
165 maxo=round(maxo∗180/pi);
166 maxp=round(maxp∗180/pi);
167
168 %%
169 % Calculo de los puntos pertenecientes al espacio de trabajo
170 x=[];
171 y=[];
172 z=[];
173
174 for l=minf:paso:maxf

175 fi=pi∗l/180;
176 for j=minp:paso:maxp
177 psi=pi∗j/180;
178 for k=mino:paso:maxo
179 om=pi∗k/180;
180 %Matriz de rotacion
184 R=Ra∗Rb∗Rc;
186 B=(R∗Bi’)’;
187 posible=angulo(Ai,B,ang);
188 if posible==1
189 posible=longitud(Ai,B,lmin,lmax);
190 if posible==1
191 posible=colision(Ai,B,h,D);
192 end
193 end
194 if posible==1
195 %agregar punto al arreglo
196 fip=round(10∗k)/10∗pi/180;
197 psip=round(10∗l)/10∗pi/180;
198 omp=round(10∗j)/10∗pi/180;
199 p = [fip;psip;omp];
200 puntos=[puntos p];
201 end
202 end
203 end
204 end
A.4.1. Condición de longitud
1 function pos=longitud(A,B,lmin,lmax)
2 % Funcion que recibe la matriz A, la matriz B (rotada), la longitud minima
3 % y maxima de las patas y regresa 1 si es posible alcanzar esa posicion
4 % con la longitud permirida de las patas o cero si no es posible
5 L=A−B;
6 L1=norm(L(1,:));
7 L2=norm(L(2,:));
8 L3=norm(L(3,:));
9 pos=1;
10
11 if L1<lmin||L2<lmin||L3<lmin
12 pos=0;
13 end
14
15 if L1>lmax||L2>lmax||L3>lmax
16 pos=0;
17 end
A.4.2. Condición de ángulo
1 function pos=angulo(A,B,ang)
2 % Funcion que recibe la matriz A, la matriz B (rotada), el angulo maximo
3 % permitido por las articulaciones y regresa 1 si es posible alcanzar esa
4 % posicion con un angulo menor al maximo o cero si no es posible
5
6 P=B(1,:)−B(2,:);
7 p=B(3,:)−B(2,:);
8 n=cross(P,p);
9 b=[0 0 1];
10
11 pos=1;
12
13 for i=1:3
14 u=B(i,:)−A(i,:);
15 a1=round(100∗asin((norm(dot(b,u)))/(norm(b)∗norm(u)))∗180/pi)/100;
16 a2=round(100∗asin((norm(dot(n,u)))/(norm(n)∗norm(u)))∗180/pi)/100;
17 if a1<ang||a2<ang
18 pos=0;
19 end
20 end
A.4.3. Condición de colisión
1 function pos=colision(A,B,h,D)
2 % Funcion que recibe la matriz A, la matriz B (rotada), la altura de la
3 % plataforma y el di’ametro de las paras y regresa 1 si es posible alcanzar
4 % esa posicion sin que choquen las patas o cero si no es posible
5
6 pos = 1;
7 A = [A; 0 0 −h];
8 B = [B; 0 0 0];
9 for i=1:3
10 si = (B(i,:)−A(i,:))/norm(B(i,:)−A(i,:));
11 for j=i+1:4
12 sj = (B(j,:)−A(j,:))/norm(B(j,:)−A(j,:));
13 n = cross(si,sj)/norm(cross(si,sj));
14 d = abs(dot(n,(A(i,:)−A(j,:))));
15 if d<=D
16 % Puede existir una colision. Calcular deltas
17 C=B−A;
18 l1=−(−(−C(j,1)∗si(1)−C(j,2)∗si(2)−C(j,3)∗si(3))∗(A(i,1)∗...
19 sj(1)−A(j,1)∗sj(1)+A(i,2)∗sj(2)−A(j,2)∗sj(2)+A(i,3)∗...
20 sj(3)−A(j,3)∗sj(3))+(A(i,1)∗si(1)−A(j,1)∗si(1)+A(i,2)∗...
21 si(2)−A(j,2)∗si(2)+A(i,3)∗si(3)−A(j,3)∗si(3))∗(−C(j,1)∗...
22 sj(1)−C(j,2)∗sj(2)−C(j,3)∗sj(3)))/(−(−C(j,1)∗si(1)−C(j,2)∗...
23 si(2)−C(j,3)∗si(3))∗(C(i,1)∗sj(1)+C(i,2)∗sj(2)+C(i,3)∗...
24 sj(3))+(C(i,1)∗si(1)+C(i,2)∗si(2)+C(i,3)∗si(3))∗(−C(j,1)∗...
25 sj(1)−C(j,2)∗sj(2)−C(j,3)∗sj(3)));
26 l2=−((−A(i,1)∗sj(1)+A(j,1)∗sj(1)−A(i,2)∗sj(2)+A(j,2)∗...
27 sj(2)−A(i,3)∗sj(3)+A(j,3)∗sj(3))/(C(j,1)∗sj(1)+C(j,2)∗...
28 sj(2)+C(j,3)∗sj(3)))+((C(i,1)∗sj(1)+C(i,2)∗sj(2)+C(i,3)∗...
29 sj(3))∗(−(−C(j,1)∗si(1)−C(j,2)∗si(2)−C(j,3)∗si(3))∗...
30 (A(i,1)∗sj(1)−A(j,1)∗sj(1)+A(i,2)∗sj(2)−A(j,2)∗sj(2)+...
31 A(i,3)∗sj(3)−A(j,3)∗sj(3))+(A(i,1)∗si(1)−A(j,1)∗si(1)+...
32 A(i,2)∗si(2)−A(j,2)∗si(2)+A(i,3)∗si(3)−A(j,3)∗si(3))∗...
33 (−C(j,1)∗sj(1)−C(j,2)∗sj(2)−C(j,3)∗sj(3))))/((−C(j,1)∗...
34 sj(1)−C(j,2)∗sj(2)−C(j,3)∗sj(3))∗(−(−C(j,1)∗si(1)−C(j,2)∗...
35 si(2)−C(j,3)∗si(3))∗(C(i,1)∗sj(1)+C(i,2)∗sj(2)+C(i,3)∗...
36 sj(3))+(C(i,1)∗si(1)+C(i,2)∗si(2)+C(i,3)∗si(3))∗(−C(j,1)∗...
37 sj(1)−C(j,2)∗sj(2)−C(j,3)∗sj(3))));
38
39 d1=l1∗(1−l1);
40 d2=l2∗(1−l2);
41 if d1>=0&&d2>=0
42 pos=0;
43 end
44 end
45 end
46 end
A.5. Algoritmo genético
1 function [h rp rb pob] = optimizar(maxgen,tamPob,pob)

2 % Funcion que recibe el numero maximo de generaciones, el tamano de la
3 % poblacion y la poblacion inicial(si no se recibe se genera aleatoriamente)
4 % y regresa los parametros de aquel elemento con la mejor funcion de aptitud,
5 % asi como todos los elementos de la ultima generacion
6
7 tam = 6 ∗ 3 + 7 ∗ 3; %6 bits para la parte entera y 7 para la decimal

8 if ~exist(’pob’,’var’)
9 pob = round(rand(tamPob,tam)); % Poblacion inicial aleatoria
10 end
11
12 for gen = 0:maxgen

13 % disp(gen) % Muestra la generacion actual, quitar el comentario si
14 % se desea saber la iteracion en la que se encuentra
15 hijos = zeros(size(pob));
16 fit = fitness(pob); % Obtiene el valor de la funcion de costo para cada
17 % elemento
18 for i = 1:25
19 p = seleccion(fit); %Selecciona dos padres mediante torneo
20 hi = cruzar(pob(p(1),:),pob(p(2),:)); % Recombinacion en dos puntos
21 [h1,h2] = mutar(hi(1,:),hi(2,:));
22 hijos(2∗i−1,:) = h1;
23 hijos(2∗i,:) = h2;
24 end
25 pob = hijos; % Reemplazar poblacion entera
26 end
27 fit = fitness(pob);
28 mejores = find(fit==max(fit));
29 for i = 1 : length(mejores)
30 mejor = pob(mejores(i),:);
31 [ht rpt rbt] = decodificar(mejor);
32 h(i) = ht;
33 rp(i) = rpt;
34 rb(i) = rbt;
35 end
36
37 save(’parametros.mat’,’h’,’rp’,’rb’)
A.5.1. Función de aptitud
1 function fit = fitness(pob)

2 fit = zeros(size(pob,1),1);
3
4 hdeseado = 20;
5 rbdeseado = 15;
6 rpdeseado = 15;
7
8 for i = 1 : length(fit)
9 [h,rp,rb] = decodificar(pob(i,:));
10 [fi,psi,omega] = espacioTrabajo(h,rb,rp);
11 fit(i) = 2∗fi + 2∗psi + .05∗omega − abs(h−hdeseado)∗10 − ...
12 abs(rb−rbdeseado)∗5 − abs(rp−rpdeseado)∗5;
13 end
A.5.2. Selección mediante torneo
1 function p = seleccion(fit)
2 % Seleccion por medio de torneo
3 p=[0 0];
4 for i = 1 : 2
5 parents = ceil(rand(2,1)∗length(fit));
6 parent1 = fit(parents(1));
7 parent2 = fit(parents(2));
8 if parent1>parent2
9 ganador = parents(1);
10 perdedor = parents(2);
11 else
12 ganador = parents(2);
13 perdedor = parents(1);
14 end
15
16 k = .75; % parametro de seleccion

17 if rand()<k
18 p(i) = ganador;
19 else
20 p(i) = perdedor;
21 end
22 end
A.5.3. Recombinación genética
1 function h = cruzar(p1,p2)
2 punto1 = ceil(rand()∗length(p1));
3 punto2 = ceil(rand()∗length(p1));
4
5 temp = max(punto1,punto2);
6 punto1 = min(punto1,punto2);
7 punto2 = temp;
8
9 h = zeros(2,length(p1));
10 h(1,punto1:punto2) = p1(punto1:punto2);
11 h(2,punto1:punto2) = p2(punto1:punto2);
12
13 temp = find(h(1,:)~=0);
14 for i=1:length(temp)
15 p2(find(p2==h(1,temp(i))))=[];
16 end
17 for i=1:length(p2)
18 h(1,find(h(1,:)==0,1)) = p2(i);
19 end
20
21 temp = find(h(2,:)~=0);
22 for i=1:length(temp)
23 p1(find(p1==h(2,temp(i))))=[];
24 end
25 for i=1:length(p1)
26 h(2,find(h(2,:)==0,1)) = p1(i);
27 end
A.5.4. Mutación
1 function [h1,h2] = mutar(h1,h2)

2
3 if rand()<.01
4 bit = ceil(rand()∗length(h1));
5 h1(bit) = not(h1(bit));
6 end
7
8 if rand()<.01
9 bit = ceil(rand()∗length(h2));
10 h2(bit) = not(h2(bit));
11 end
A.5.5. Decodificación de cromosomas
1 function [h rp rb] = decodificar(elemento)

2
3 h= bin2dec(num2str(elemento(1:6)))+bin2dec(num2str(elemento(7:13)))/100;
4 rp= bin2dec(num2str(elemento(14:19)))+bin2dec(num2str(elemento(20:26)))/100;
5 rb= bin2dec(num2str(elemento(27:32)))+bin2dec(num2str(elemento(33:39)))/100;
A.6. Cálculo de singularidades
1 function obstaculos = Qspace(rb,rp,h,puntos)

2 % Puntos −> Matriz de 3xn en donde la primera fila le corresponde al
3 % angulo omega, la segunda a fi y la tercera a psi. En esta matriz
4 % se colocan todos los puntos posibles a los que puede moverse el robot
5 % y regresa los puntos en los cuales el Jacobiano se indetermina
6
7 obstaculos = [];
8
9 Bi=[rp , 0, 0;...
10 rp ∗ cos(120∗pi/180), rp ∗ sin(120∗pi/180), 0;...
11 rp ∗ cos(240∗pi/180), rp ∗ sin(240∗pi/180), 0];
12 Ai=[rb 0 0;...
13 rb ∗ cos(120∗pi/180), rb ∗ sin(120∗pi/180), 0;...
14 rb ∗ cos(240∗pi/180), rb ∗ sin(240∗pi/180), 0];
15 p = [0; 0; h];
16
17 for j=1:size(puntos,2)
18 om=puntos(1,j);
19 fi=puntos(2,j);
20 psi=puntos(3,j);
21
22 % Obtener puntos bi
26 aRb=Ra∗Rb∗Rc;
27
28 Bn = (aRb∗Bi’)’;
29 J = zeros(3,3);
30 for i=1:3
31 bi = Bn(i,:)’;
32 ai = Ai(i,:)’;
33 d = norm(p + bi − ai);
34 s = (p + bi − ai) / d;
35 J(i,:) = cross(bi,s);
36 end
37 if rank(J)<3
38 obstaculos = [obstaculos puntos(:,j)];
39 end
40 end
A.7. Dinámica inversa mediante trabajo virtual
1 function [f,Jp] = dinamicaInversa(om,fi,psi,omp,fip,psip,ompp,fipp,...

2 psipp,h,rb,rp,hp,r1,r2,rr1,rr2,e1,e2,mp,m1,m2)
3 % Implementacion de la dinamica directa por metodo de trabajo virtual.
4 % Se utiliza el sistema cgs, sin embargo la salida se da en Newtons. Las
5 % salidas son las fuerzas y las matrices jacobianas.
6 % Las entradas son la posicion, velocidad y aceleracion de los angulos de
7 % Euler, asi como los parametros del robot.
8
9 Bi=[rp , 0, 0;...
10 rp ∗ cos(120∗pi/180), rp ∗ sin(120∗pi/180), 0;...
11 rp ∗ cos(240∗pi/180), rp ∗ sin(240∗pi/180), 0];
12 Ai=[rb 0 0;...
13 rb ∗ cos(120∗pi/180), rb ∗ sin(120∗pi/180), 0;...
14 rb ∗ cos(240∗pi/180), rb ∗ sin(240∗pi/180), 0];
15
16 p = [0; 0; h];
17 g = [0; 0; −981]; %Gravedad en cm/s^2
18

23
24 Bn=(aRb∗Bi’)’;
25
26 wp=[omp + psip ∗ sin(fi);...

27 fip ∗ cos(om) − psip ∗ cos(fi) ∗ sin(om);...
28 fip ∗ sin(om) − psip ∗ cos(fi) ∗ cos(om)];
29
30 wpp1=ompp + psipp ∗ sin(fi) + psip ∗ fip ∗ cos(fi);

31 wpp2=fipp ∗ cos(om) − fip ∗ omp ∗ sin(om) − psipp ∗ cos(fi) ∗ sin(om)...
32 + psip ∗ fip ∗ sin(fi) ∗ sin(om) − psip ∗ omp ∗ cos(fi) ∗ cos(om);
33 wpp3=fipp ∗ sin(om) − fip ∗ omp ∗ cos(om) + psipp ∗ cos(fi) ∗ cos(om)...
34 − psip ∗ fip ∗ sin(fi) ∗ cos(om) − psip ∗ omp ∗ cos(fi) ∗ sin(om);
35
36 wpp=[wpp1;wpp2;wpp3];
37
38 I1x = m1 ∗ (3 ∗ (r1^2+rr1^2) + (2 ∗ e1)^2) / 12;

39 I1z = m1 ∗ (r1^2 + rr1^2) / 2;
40 I2x = m2 ∗ (3 ∗ (r2^2+rr2^2) + (2 ∗ e2)^2) / 12;
41 I2z = m2 ∗ (r2^2 + rr2^2) / 2;
42 Ipx = mp ∗ (3 ∗ rp^2 + hp^2) / 12;
43 Ipz = mp ∗ rp^2 / 2;
44
45
46 I1 = [I1x 0 0;0 I1x 0;0 0 I1z];

47 I2 = [I2x 0 0;0 I2x 0;0 0 I2z];
48 bIp = [Ipx 0 0;0 Ipx 0;0 0 Ipz];
49 aIp = aRb ∗ bIp ∗ aRb’;
50
51
52 Fp = −aIp ∗ wpp − cross(wp,aIp∗wp);

53
54 Fa = zeros(3,1);
55 d=zeros(3,1);
56 Jp = zeros(3,3);
57 J = Jp;
58 for i=1:3
59 bi = Bn(i,:)’;
60 ai = Ai(i,:)’;
61 d(i) = norm(p + bi − ai);
62 s = (p + bi − ai) / d(i);
63 ca = s(3);
64 sa = sqrt(s(1)^2 + s(2)^2);
65 sb = s(2) / sa;
66 cb = s(1) / sa;
67 aRi(:,:) = [cb∗ca −sb cb∗sa;...
68 sb∗ca cb sb∗sa;...
69 −sa 0 ca];
70 iRa(:,:) = aRi(:,:)’;
71
72 bx = bi(1);
73 by = bi(2);
74 bz = bi(3);
75
76 vb = cross(wp,bi);
77 vb=vb(:);
78 ivb = iRa ∗ vb;
79
80 iw = [−ivb(2);ivb(1);0]/d(i);
81
82 vbp = cross(wpp,bi) + cross(wp,cross(wp,bi));

83 vbp = vbp(:);
84
85 ivbp = iRa ∗ vbp;

86
87 iwp = [−ivbp(2) + (2 ∗ ivb(3) ∗ ivb(2) / d(i));...

88 ivbp(1) − (2 ∗ ivb(3) ∗ ivb(1) / d(i)); 0] / d(i);
89
90 iv1p = [ivbp(1) − (2 ∗ ivb(3) ∗ ivb(1) / d(i));...

91 ivbp(2) − (2 ∗ ivb(3) ∗ ivb(2) / d(i));...
92 −(ivb(1)^2 + ivb(2)^2) / d(i)] ∗ e1 / d(i);
93
94 iv2p = [(d(i) − e2) ∗ ivbp(1) + (2 ∗ e2 ∗ ivb(3) ∗ ivb(1) / d(i));...

95 (d(i) − e2) ∗ ivbp(2) + (2 ∗ e2 ∗ ivb(3) ∗ ivb(2) / d(i));...
96 d(i) ∗ ivbp(3) + (e2 ∗ (ivb(1)^2 + ivb(2)^2) / d(i))] / d(i);
97
98 Jb = [0 bz −by;...
99 −bz 0 bx;...
100 by −bx 0];
101 iJb= iRa(:,:) ∗ Jb;
102 iJbx = iJb(1,:);
103 iJby = iJb(2,:);
104 iJbz = iJb(3,:);
105
106 J(i,:) = iJbz;

107 Jp(i,:) = cross(bi,s);
108
109 iJ1 = [e1 ∗ iJbx;...

110 e1 ∗ iJby;...
111 0 0 0 ;...
112 −iJby;...
113 iJbx;...
114 0 0 0] / d(i);
115
116 iJ2 = [(d(i) − e2) ∗ iJbx;...

117 (d(i) − e2) ∗ iJby;...
118 d(i) ∗ iJbz;...
119 −iJby;...
120 iJbx;...
121 0 0 0] / d(i);
122
123 iF1 = [m1 ∗ iRa ∗ g − m1 ∗ iv1p;...

124 −I1 ∗ iwp − cross(iw,I1∗iw)];
125 iF2 = [m2 ∗ iRa ∗ g − m2 ∗ iv2p;...
127 Fa = Fa + iJ1’ ∗ iF1 + iJ2’ ∗ iF2;
128 end
129
130 f = inv(Jp)’ ∗ (−Fa − Fp);

131
132 % Pasar de g∗cm/s^2 a Newtons

133 f = f / (1000 ∗ 100);
A.8. Dinámica inversa mediante trabajo virtual para robot

6-UPS
1 clc
2 clear all
3 close all
4
5 t = 0:.05:.4;
6 bbi = .5;
7 ai = 1;
8 e1 = .2;
9 e2 = .75;
10 mp = 32;
11 m1 = 2;
12 m2 = 2;
13 g = [0; 0; −9.8];
14 pp = [0; 0; 1];
15
16 % Aceleraciones
17
18 thpp = 0;
19 psipp = 0;
20 fipp = 0;
21 vppx= 5;
22 vppy= 0;
23 vppz= 0;
24 vpp = [vppx;vppy;vppz];
25 Bi=[bbi ∗ cos(50∗pi/180), bbi ∗ sin(50∗pi/180), 0;...
26 bbi ∗ cos(60∗pi/180), bbi ∗ sin(60∗pi/180), 0;...
30 bbi ∗ cos(300∗pi/180), bbi ∗ sin(300∗pi/180), 0];
31 Ai=[ai 0 0;...
32 ai ∗ cos(110∗pi/180), ai ∗ sin(110∗pi/180), 0;...
33 ai ∗ cos(120∗pi/180), ai ∗ sin(120∗pi/180), 0;...
34 ai ∗ cos(230∗pi/180), ai ∗ sin(230∗pi/180), 0;...
35 ai ∗ cos(240∗pi/180), ai ∗ sin(240∗pi/180), 0;...
36 ai ∗ cos(350∗pi/180), ai ∗ sin(350∗pi/180), 0];
37 f = zeros(6,length(t));
38 I1 = [1 0 0;0 1 0;0 0 .001];
39 I2 = [.4 0 0;0 .4 0;0 0 .0001];
40 bIp = [2 0 0;0 2 0;0 0 4];
41 for l = 1:length(t)
42 fi = fipp ∗ t(l)^2/2;
43 fip = t(l)∗fipp;
44 th = thpp ∗ t(l)^2/2;
45 psi = psipp ∗ t(l)^2/2;
46 thp = t(l)∗thpp;
47 psip = t(l)∗psipp;
48 vp = [vppx∗t(l);vppy∗t(l);vppz∗t(l)];
49 p = pp + [vppx∗t(l)^2 /2;vppy∗t(l)^2/2;vppz∗t(l)^2/2];
50
51 Ra=[cos(psi) −sin(psi) 0;sin(psi) cos(psi) 0;0 0 1];

52 Rb=[cos(th) 0 sin(th);0 1 0;−sin(th) 0 cos(th)];
53 Rc=[cos(fi) −sin(fi) 0;sin(fi) cos(fi) 0;0 0 1];
57
58 wp=[psip ∗ cos(fi) ∗ sin(th) − thp ∗ sin(fi);...

59 psip ∗ sin(fi) ∗ sin(th) + thp ∗ cos(fi);...
60 psip ∗ cos(th) + fip];
61 wpp1=psipp ∗ cos(fi) ∗ sin(th) − fip ∗ psip ∗ sin(fi) ∗ sin(th)...
62 + thp ∗ psip ∗ cos(fi) ∗ cos(th) − thpp ∗ sin(fi) − thp...
63 ∗ fip ∗ cos(fi);
64 wpp2=psipp ∗ sin(fi) ∗ sin(th) + fip ∗ psip ∗ cos(fi) ∗ sin(th)...
65 + thp ∗ psip ∗ sin(fi) ∗ cos(th) + thpp ∗ cos(fi) − thp...
66 ∗ fip ∗ sin(fi);
67 wpp3=psipp ∗ cos(th) − psip ∗ thp ∗ sin(th) + fipp;
69
70 Fp = [mp ∗ g − mp ∗ vpp; −aIp ∗ wpp − cross(wp,aIp∗wp)];

71 Fa = zeros(6,1);
72 d=zeros(6,1);
73 Jp = zeros(6,6);
74 for i=1:6
75 bi = Bn(i,:)’;
76 ai = Ai(i,:)’;
77 d(i) = norm(p + bi − ai);
78 s = (p + bi − ai) / d(i);
79 ca = s(3);
80 sa = sqrt(s(1)^2 + s(2)^2);
81 sb = s(2) / sa;
82 cb = s(1) / sa;
83 aRi(:,:) = [cb∗ca −sb cb∗sa;...
84 sb∗ca cb sb∗sa;...
85 −sa 0 ca];
86 iRa(:,:) = aRi(:,:)’;
87 bx = bi(1);
88 by = bi(2);
89 bz = bi(3);
90 vb = cross(wp,bi);
91 vb=vb(:) + vp;
92 ivb = iRa ∗ vb;
93 iw = [−ivb(2);ivb(1);0] / d(i);
94 vbp = cross(wpp,bi) + cross(wp,cross(wp,bi));
95 vbp = vpp + vbp(:);
96 ivbp = iRa ∗ vbp;
97 iwp = [−ivbp(2) + (2 ∗ ivb(3) ∗ ivb(2) / d(i));...
98 ivbp(1) − (2 ∗ ivb(3) ∗ ivb(1) / d(i)); 0] / d(i);
99 iv1p = [ivbp(1) − (2 ∗ ivb(3) ∗ ivb(1) / d(i));...
100 ivbp(2) − (2 ∗ ivb(3) ∗ ivb(2) / d(i));...
101 −(ivb(1)^2 + ivb(2)^2) / d(i)] ∗ e1 / d(i);
102 iv2p = [(d(i) − e2) ∗ ivbp(1) + (2 ∗ e2 ∗ ivb(3) ∗ ivb(1) / d(i));...
103 (d(i) − e2) ∗ ivbp(2) + (2 ∗ e2 ∗ ivb(3) ∗ ivb(2) / d(i));...
104 d(i) ∗ ivbp(3) + (e2 ∗ (ivb(1)^2 + ivb(2)^2) / d(i))] / d(i);
105 Jb = [1 0 0 0 bz −by;...
106 0 1 0 −bz 0 bx;...
107 0 0 1 by −bx 0];
108 iJb= iRa(:,:) ∗ Jb;
109 iJbx = iJb(1,:);
110 iJby = iJb(2,:);
111 iJbz = iJb(3,:);
112 Jp(i,:) = iJbz;
113 iJ1 = [e1 ∗ iJbx;...
114 e1 ∗ iJby;...
115 0 0 0 0 0 0;...
116 −iJby;...
117 iJbx;...
118 0 0 0 0 0 0] / d(i);
119 iJ2 = [(d(i) − e2) ∗ iJbx;...
120 (d(i) − e2) ∗ iJby;...
121 d(i) ∗ iJbz;...
122 −iJby;...
123 iJbx;...
124 0 0 0 0 0 0] / d(i);
125 iF1 = [m1 ∗ iRa ∗ g − m1 ∗ iv1p;...
127 iF2 = [m2 ∗ iRa ∗ g − m2 ∗ iv2p;...
129 Fa = Fa + iJ1’ ∗ iF1 + iJ2’ ∗ iF2;
130 end
131 % Resolver sistema Ax = B
132 f(:,l) = linsolve(Jp’,(−Fa − Fp));
133 end
134
135 plot(t,f(1,:),’−g’)
136 hold on
137 plot(t,f(2,:),’or’)
138 plot(t,f(3,:),’−.k’)
139 plot(t,f(4,:),’+b’)
140 plot(t,f(5,:),’−−m’)
141 plot(t,f(6,:),’∗k’)
142 xlim([0 .45])
143 xlabel(’Tiempo(s)’)
144 ylabel(’Fuerza(N)’)
145 legend(’Actuador 1’,’Actuador 2’,’Actuador 3’,’Actuador 4’,...
146 ’Actuador 5’,’Actuador 6’)
A.9. Dinámica inversa mediante método de Merlet
1 function [f,J] = dinamicaInv(om,fi,psi,omp,fip,psip,ompp,fipp,psipp,...

2 h,rb,rp,hp,r1,r2,rr1,rr2,e1,e2,mp,m1,m2)
3 % Implementacion de la dinamica directa por el metodo de Merlet.
4 % Se utiliza el sistema cgs, sin embargo la salida se da en Newtons. Las
5 % salidas son las fuerzas y las matrices jacobianas.
6 % Las entradas son la posicion, velocidad y aceleracion de los angulos de
7 % Euler, asi como los parametros del robot.
8
9 Bi=[rp , 0, 0;...
10 rp ∗ cos(120∗pi/180), rp ∗ sin(120∗pi/180), 0;...
11 rp ∗ cos(240∗pi/180), rp ∗ sin(240∗pi/180), 0];
12 Ai=[rb 0 0;...
13 rb ∗ cos(120∗pi/180), rb ∗ sin(120∗pi/180), 0;...
14 rb ∗ cos(240∗pi/180), rb ∗ sin(240∗pi/180), 0];
15
16 p = [0; 0; h];
17

22
24
25 wp=[omp + psip ∗ sin(fi);...

26 fip ∗ cos(om) − psip ∗ cos(fi) ∗ sin(om);...
27 fip ∗ sin(om) − psip ∗ cos(fi) ∗ cos(om)];
28
29 wpp1=ompp + psipp ∗ sin(fi) + psip ∗ fip ∗ cos(fi);

30 wpp2=fipp ∗ cos(om) − fip ∗ omp ∗ sin(om) − psipp ∗ cos(fi) ∗ sin(om)...
31 + psip ∗ fip ∗ sin(fi) ∗ sin(om) − psip ∗ omp ∗ cos(fi) ∗ cos(om);
32 wpp3=fipp ∗ sin(om) − fip ∗ omp ∗ cos(om) + psipp ∗ cos(fi) ∗ cos(om)...
33 − psip ∗ fip ∗ sin(fi) ∗ cos(om) − psip ∗ omp ∗ cos(fi) ∗ sin(om);
34
36
37 I1x = m1 ∗ (3 ∗ (r1^2+rr1^2) + (2 ∗ e1)^2) / 12;

38 I2x = m2 ∗ (3 ∗ (r2^2+rr2^2) + (2 ∗ e2)^2) / 12;
39
40 Ipx = mp ∗ (3 ∗ rp^2 + hp^2) / 12;

41 Ipz = mp ∗ rp^2 / 2;
42
43 bIp = [Ipx 0 0;0 Ipx 0;0 0 Ipz];

45 %% % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % %
46
47 T1 = aIp;
48 T2 = cross(wp,aIp∗wp);
49 V1=zeros(3,3);
50 V2=zeros(3,1);
51 J = zeros(3,3);
52 for i=1:3
53 bi = Bn(i,:)’;
54 ai = Ai(i,:)’;
56 s = (p + bi − ai) / d;
57 SB = skewmatrix(bi);
58 ni = skewmatrix(s);
59 U1 = − SB;
60 U2 = cross(wp,cross(wp,bi));
61 I = (I1x+I2x) / 4;
62 V1 = V1 + I / d ∗ SB ∗ ni^2 ∗ U1;
63 V2 = V2 + I / d ∗ SB ∗ ni^2 ∗ U2;
65 end
66
67 f = inv(J)’∗((T1−V1)∗wpp+(T2−V2));
68
69 %Regresar de g∗cm/s^2 a kg∗m/s^2

70 f = f / (1000 ∗ 100);
71
72 function SM=skewmatrix(a)
73 SM = [0 −a(3) a(2) ; a(3) 0 −a(1); −a(2) a(1) 0] ;
A.10. Dinámica directa
1 function wpp = dinamicaDirecta(f,w,wp,h,rb,rp,hp,r1,r2,rr1,rr2,e1,e2,mp,m1,m2)

2
3 f = f ∗ 1000 ∗ 100;
4
5 Bi=[rp , 0, 0;...
6 rp ∗ cos(120∗pi/180), rp ∗ sin(120∗pi/180), 0;...
7 rp ∗ cos(240∗pi/180), rp ∗ sin(240∗pi/180), 0];
8 Ai=[rb 0 0;...
9 rb ∗ cos(120∗pi/180), rb ∗ sin(120∗pi/180), 0;...
10 rb ∗ cos(240∗pi/180), rb ∗ sin(240∗pi/180), 0];
11
12 p = [0; 0; h];
13
14 om = w(1);
15 fi = w(2);
16 psi = w(3);
17

22
24
25 I1x = m1 ∗ (3 ∗ (r1^2+rr1^2) + (2 ∗ e1)^2) / 12;

26 I2x = m2 ∗ (3 ∗ (r2^2+rr2^2) + (2 ∗ e2)^2) / 12;
27
28
29 Ipx = mp ∗ (3 ∗ rp^2 + hp^2) / 12;

30 Ipz = mp ∗ rp^2 / 2;
31
32 bIp = [Ipx 0 0;0 Ipx 0;0 0 Ipz];

34 %% % % % % % % % % % % % % % % % % % % % % %
35
36 T1 = aIp;
37 T2 = cross(wp,aIp∗wp);
38 V1=zeros(3,3);
39 V2=zeros(3,1);
40 J = zeros(3,3);
41 for i=1:3
42 bi = Bn(i,:)’;
43 ai = Ai(i,:)’;
45 s = (p + bi − ai) / d;
46 SB = skewmatrix(bi);
47 ni = skewmatrix(s);
48 U1 = − SB;
49 U2 = cross(wp,cross(wp,bi));
50 I = (I1x+I2x) / 4;
51 V1 = V1 + I / d ∗ SB ∗ ni^2 ∗ U1;
52 V2 = V2 + I / d ∗ SB ∗ ni^2 ∗ U2;
54 end
55 f=f(:);
56 TV = inv(T1−V1);
57
58 wpp = TV ∗ ((J’ ∗ f) − (T2−V2));

59
60
61 function SM=skewmatrix(a)
62 SM = [0 −a(3) a(2) ; a(3) 0 −a(1); −a(2) a(1) 0] ;
A.11. Control PID
1 function [E,W,Wp,W2,F,t] = controlPID(wi,fi,pi,wf,ff,pf,T,h,rp,rb,hp,...

2 r1,r2,rr1,rr2,e1,e2,mp,m1,m2,kp,ki,kd,wp)
3 % Funcion que simula el control del robot utilizando la dinamica directa
4 % como planta y un controlador tipo PID
5 [l(1),l(2),l(3)] = inversa(wi,fi,pi,h,rb,rp);
6 [lf(1),lf(2),lf(3)] = inversa(wf,ff,pf,h,rb,rp);
7
8 w = [wi;fi;pi];
9
10 dt=T/1000;
11 t=0:dt:T;
12 L = zeros(3,length(t));
13 E = L;
14 W = E;
15 F = E;
16 Wp = E;
17 www = ones(1,length(t)) ∗ wf;
18 fff = ones(1,length(t)) ∗ ff;
19 ppp = ones(1,length(t)) ∗ pf;
20 W2 = [www;fff;ppp];
21 e = lf − l;
22 ie = e ∗ 0;
23 for i=0:dt:T
24 k = round(1 + i/dt);
25 W(:,k) = w(:);
26 Wp(:,k) = wp(:);
27 ea = e;
28 E(:,k) = ea(:);
29 [l(1),l(2),l(3)] = inversa(w(1),w(2),w(3),h,rb,rp);
30 e = lf − l;
31 de = (e − ea) / dt;
32 if i>0
33 ie = ie + e ∗ dt;
34 end
35 f = kp ∗ e + kd ∗ de + ki ∗ ie;
36 F(:,k) = f(:);
37 wpp = dinamicaDirecta(f(:),w,wp,h,rb,rp,hp,r1,r2,rr1,rr2,e1,e2,mp,m1,m2);
38 temp=wp;
39 wp = wp + dt ∗ wpp(:);
40 [o,ff,p,op,fp,pp,opp,fpp,ppp] = angulosDirecta(wpp(:),wp,w,dt,temp);
41 w = [o;ff;p];
42 wp = [op;fp;pp];
43 end
A.11.1. Cálculo de ángulos de Euler mediante dinámica directa
1 function [o,f,p,op,fp,pp,opp,fpp,ppp] = angulosDirecta(wwpp,wp,w,ddt,wpi)

2 % Dada la aceleracion angular calculada mediante dinamica directa, se
3 % calculan los nuevos angulos de Euler, asi como la velocidad y aceleracion
4 % de estos.
5
6 wpp = wwpp;
7 dt = ddt;
8 omi = w(1);
9 fii = w(2);
10 psii = w(3);
11
12 Rt=[1 0 sin(fii);0 cos(omi) −cos(fii)∗sin(omi);0 sin(omi) cos(fii)∗cos(omi)];

13 wp2=Rt\wp;
14
15 omp = wp2(1);
16 fip = wp2(2);
17 psip = wp2(3);
18
19 ompi = wpi(1);
20 fipi = wpi(2);
21 psipi = wpi(3);
22
23 r1=fip∗psip∗cos(fii);
24 r2=fip∗psip∗sin(fii)∗sin(omi)−psip∗omp∗cos(fii)∗cos(omi)−fip∗omp∗sin(omi);
25 r3=−fip∗psip∗sin(fii)∗cos(omi)−psip∗omp∗cos(fii)∗sin(omi)+fip∗omp∗cos(omi);
26 R = [r1;r2;r3];
27 wpp2 = Rt \ (wpp − R);
28
29 w = w + wpi ∗ dt + wpp2 ∗ dt^2 / 2;

30 wp2 = wpi + wpp2 ∗ dt;
31 x = [w;wp2;wpp2];
32
33 o = x(1);
34 f = x(2);
35 p = x(3);
36 op = x(4);
37 fp = x(5);
38 pp = x(6);
39 opp = x(7);
40 fpp = x(8);
41 ppp = x(9);
A.12. Control difuso
1 function [E,DE,W,W2,F,t] = controlFuzzy(wi,fi,pi,wf,ff,pf,T,h,rp,rb,hp,...

2 r1,r2,rr1,rr2,e1,e2,mp,m1,m2,fismat,wp)
3 % Funcion que simula el control del robot utilizando la dinamica directa
4 % como planta y un controlador difuso
5
6 [l(1),l(2),l(3)] = inversa(wi,fi,pi,h,rb,rp);
7 [lf(1),lf(2),lf(3)] = inversa(wf,ff,pf,h,rb,rp);
8
9 w = [wi;fi;pi];
10
11 dt=T/400;
12 t=0:dt:T;
13 L = zeros(3,length(t));
14 E = L;
15 W = E;
16 F = E;
17 DE = E;
18 www = ones(1,length(t)) ∗ wf;
19 fff = ones(1,length(t)) ∗ ff;
20 ppp = ones(1,length(t)) ∗ pf;
21 W2 = [www;fff;ppp];
22 e = lf − l;
23 ie = e ∗ 0;
24 warning(’off’)
25 for i=0:dt:T
26 k = round(1 + i/dt);
27 W(:,k) = w(:);
28 ea = e;
29 E(:,k) = ea(:);
30 [l(1),l(2),l(3)] = inversa(w(1),w(2),w(3),h,rb,rp);
31 e = lf − l;
32 de = (e − ea) / dt;
33 DE(:,k) = de;
34 if i>0
35 ie = ie + e ∗ dt;
36 end
37 f = evalfis([e’ de’],fismat); %Evaluar los valores en el sistema difuso
38 F(:,k) = f(:);
39 wpp = dinamicaDirecta(f(:),w,wp,h,rb,rp,hp,r1,r2,rr1,rr2,e1,e2,mp,m1,m2);
40 temp=wp;
41 wp = wp + dt ∗ wpp(:);
42 [o,ff,p,op,fp,pp,opp,fpp,ppp] = angulosDirecta(wpp(:),wp,w,dt,temp);
43 w = [o;ff;p];
44 end
A.13. Cálculo de trayectoria de posición por interpolación

lineal
1 function [longitudes, angulos, angulos2]=trayectoria(h,rb,rp,fia,...

2 psia,oma,fi,psi,om,red,T)
3 % Funcion que recibe los angulos de Euler actuales y los deseados,
4 % los parametros del robot y la red neuronal de cinematica directa y a
5 % partir de ellos calcula los puntos intermedios necesarios para llegar
6 % al angulo deseado. La funcion regresa tanto los angulos de Euler
7 % intermedios como la longitud de los actuadores necesaria para alcanzar
8 % esos angulos. Tambien calcula los angulos mediante cinematica directa
9 % por si se desea comparar. Utiliza el metodo de interpolacion lineal.
10 % El orden de los angulos es angulos=[omega;fi;psi]
11
12 % Numero de puntos
13 puntos = 8;
14
15 %pendientes
16 mfi = (fi − fia) / T;
17 mpsi = (psi − psia) / T;
18 mom = (om − oma) / T;
19
20 Fi = zeros(1,puntos+1);
21 Psi = zeros(1,puntos+1);
22 Om = zeros(1,puntos+1);
23
24 Fi(1) = fia;
25 Psi(1) = psia;
26 Om(1) = oma;
27
28 t=zeros(1,puntos+1);
29
30 longitudes = zeros(3,puntos + 1);

31 angulos = [Om;Fi;Psi];
32 angulos2 = angulos;
33
34 [longitudes(1,1),longitudes(2,1),longitudes(3,1)] = ...
35 inversa(Om(1),Fi(1),Psi(1),h,rb,rp);
36
37 for i=1:puntos − 1
38 t(i+1) = T ∗ i / puntos;
39 Fi(i+1) = mfi ∗ t(i+1) + Fi(1);
40 Psi(i+1) = mpsi ∗ t(i+1) + Psi(1);
41 Om(i+1) = mom ∗ t(i+1) + Om(1);
42
43 angulos(:,i+1) = [Om(i+1);Fi(i+1); Psi(i+1)];

44
45
46 [longitudes(1,i+1),longitudes(2,i+1),longitudes(3,i+1)] = ...
47 inversa(Om(i+1),Fi(i+1),Psi(i+1),h,rb,rp);
48
49 [omn, fin, psin] = ...

50 directa(longitudes(1,i+1),longitudes(2,i+1),longitudes(3,i+1),...
51 Om(i),Fi(i),Psi(i),rp,red);
52
53 angulos2(:,i+1) = [omn;fin;psin];
54
55 end
56
57 Fi(puntos+1) = mfi ∗ T + Fi(1);

58 Psi(puntos+1) = mpsi ∗ T + Psi(1);
59 Om(puntos+1) = mom ∗ T + Om(1);
60
61 angulos(:,puntos+1) = [Om(puntos+1);Fi(puntos+1); Psi(puntos+1)];

62 [longitudes(1,puntos+1),longitudes(2,puntos+1),longitudes(3,puntos+1)] = ...
63 inversa(Om(puntos+1),Fi(puntos+1),Psi(puntos+1),h,rb,rp);
64 [omn,fin, psin] = ...
65 directa(longitudes(1,puntos+1),longitudes(2,puntos+1),...
66 longitudes(3,puntos+1),Om(puntos+1),Fi(puntos+1),Psi(puntos+1),rp,red);
67 angulos2(:,puntos+1) = [omn;fin; psin];
A.14. Cálculo de trayectoria de posición por PRM
1 function [longitudes, angulos, angulos2, posible]=trayectoria2(h,rb,rp,...

2 fia,psia,oma,fi,psi,om,ommin,ommax,psimin,psimax,red,T)
3 % Funcion que recibe los angulos de Euler actuales y los deseados,
4 % los parametros del robot y la red neuronal de cinematica directa y a
5 % partir de ellos calcula los puntos intermedios necesarios para llegar
6 % al angulo deseado. La funcion regresa tanto los angulos de Euler
7 % intermedios como la longitud de los actuadores necesaria para alcanzar
8 % esos angulos. Tambien calcula los angulos mediante cinematica directa
9 % por si se desea comparar. Utiliza el metodo de PRM.
10 % El orden de los angulos es angulos=[omega;fi;psi]
11
12 posible = 1;
13 dmax = .3;
14
15 xi = oma;
16 xf = om;
17 yi = psia;
18 yf = psi;
19
20 dom = ommax − ommin;

21 dpsi = psimax − psimin;
22
23 %Cuadrado con centro en cero

24 obstaculo = [−.01 −.01;−.01 .01;.01 .01;.01 −.01;−.01 −.01];
25
26 nodo = 70; % Numero de puntos a generar

27 vecino= 6; % Conexiones maximas
28 i=1;
29 nodos=zeros(2,nodo);
30
31 % Calcular nodos que esten afuera de todos los obstaculos

32 while i<=nodo
33 xn = dom ∗ rand + ommin;
34 yn = dpsi ∗ rand + psimin;
35 dentro=0; % 0 significa que esta fuera del obstaculo
36 % checar si el nodo esta dentro del cuadrado
37 x = obstaculo(:,1)’;
38 y = obstaculo(:,2)’;
39 xcruza = polyxpoly(x,y,[xn 7],[yn yn]);
40 if ~isempty(xcruza)
41 dentro = 1;
42 end
43 if dentro==0
44 nodos(:,i)=[xn;yn];
45 i=i+1;
46 end
47 end
48
49 m = 1;
50 d = zeros(nodo,nodo);
51 da = d;
52 db = d;
53 vecinos = [];
54 %Calcular distanca entre nodos
55
56 for i=1:nodo
57 menores=100∗ones(1,vecino);
58 for j=1:nodo
59 d(i,j)=norm(nodos(:,i)−nodos(:,j));
60 da(i,j)=norm(nodos(1,i)−nodos(1,j));
61 db(i,j)=norm(nodos(2,i)−nodos(2,j));
62 if da(i,j)<dmax&&db(i,j)<dmax&&d(i,j)<max(menores)&&d(i,j)>0
63 menores(find(menores==max(menores),1))=d(i,j);
64 end
65 end
66 for k=1:vecino
67 if menores(k)<dmax
68 vec=find(d(i,:)==menores(k));
69 if i<vec
70 vecinos(m,1)=i;
71 vecinos(m,2)=vec;
72 else
73 vecinos(m,2)=i;
74 vecinos(m,1)=vec;
75 end
76 m=m+1;
77 end
78 end
79 end
80
81 %Ordenar y quitar duplicados

82 if ~isempty(vecinos)
83 vecinos=sortrows(vecinos,[1 2]);
84 vecinos=unique(vecinos,’rows’);
85 end
86
87 borrar=[];
88 % Checar que la linea entre vecinos no choque con ningun obstaculo
89 for i=1:size(vecinos,1)
92 xcruza = polyxpoly([nodos(1,vecinos(i,1)) nodos(1,vecinos(i,2))],...
93 [nodos(2,vecinos(i,1)) nodos(2,vecinos(i,2))], x, y);

94 if ~isempty(xcruza)
95 % Borrar vecinos que tengan un obstaculo entre ellos
96 borrar=[borrar i];
97 end
98 end
99
100 while ~isempty(borrar)

101 vecinos(borrar(length(borrar)),:)=[];
102 borrar(length(borrar))=[];
103 end
104
105 % Matriz para el calculo de la trayectoria

106 matriz=zeros(nodo);
108 matriz(vecinos(i,1),vecinos(i,2))=1;
109 matriz(vecinos(i,2),vecinos(i,1))=1;
110 end
111
112 %Calcular con que punto se puede conectar xiyi y xfyf

113 for i=1:size(nodos,2)
114 di(i,1)=sqrt((nodos(1,i)−xi)^2+(nodos(2,i)−yi)^2);
115 di(i,2)=i;
116 df(i,1)=sqrt((nodos(1,i)−xf)^2+(nodos(2,i)−yf)^2);
117 df(i,2)=i;
118 end
119
120 di=sortrows(di,1);
121 df=sortrows(df,1);
122
123 %Probar que no choque con obstaculos el punto inicial y su nodo

124 unido=0;
125 r=1;
126
127 while unido==0&&r<=length(di)

130 xcruza = polyxpoly([xi nodos(1,di(r,2))],[yi nodos(2,di(r,2))], x, y);
131 if isempty(xcruza)
132 % No choca
133 unido=1;
134 pin=di(r,2);
135 else
136 r=r+1;
137 end
138 end
139
140 if r>length(di)
141 pin=[];
142 end
143
144 %Lo mismo para el punto final

145 unido=0;
146 r=1;
147
148 while unido==0&&r<=length(df)

151 xcruza = polyxpoly([xf nodos(1,df(r,2))],[yf nodos(1,df(r,2))], x, y);
152 if isempty(xcruza)
153 % No choca
154 unido=1;
155 pf=df(r,2);
156 else
157 r=r+1;
158 end
159 end
160
161 if r>length(df)
162 pf=[];
163 end
164
165 fila=pin;
166 fin=0;
167
168 t=pin;
169 it=1;
170
171 m2=zeros(size(matriz,1));
172
173 while fin==0

174 for i=1:size(matriz,1)
175 if matriz(t,i)==1&&isempty(find(fila==i))
176 fila=[fila i];
177 m2(t,i)=t;
178 if i==pf
179 fin=1;
180 end
181 end
182 end
183 it=it+1;
184 if it>length(fila)
185 fin=1;
186 else
187 t=fila(it);
188 end
189 end
190
191 % Ya que se llego al punto deseado, se busca quienes generaron ese camino
192 q=pf;
193 e=1;
194 tray=q;
195 while e<=it&q~=pin
196 Q=find(m2(:,q)); %punto final
197 q=Q;
198 tray=[q tray];
199 e=e+1;
200 end
201
202 puntos = [];

203
204 if tray(1)==pin
205 % Ya que se tienen los puntos, se ponen las coordenadas
206 puntos(:,1) = [xi yi];
207 for i = 1:size(tray,2)
208 puntos(:,i+1) = nodos(:,tray(i));
209 end
210 puntos = [puntos [xf;yf]];
211 else
212 posible = 1;
213 end
214
215 % Quitar comentarios si se desea graficar la trayectoria.

216 line([xi nodos(1,pin)],[yi nodos(2,pin)])
217 hold on
219 line([nodos(1,vecinos(i,1)) nodos(1,vecinos(i,2))],...
220 [nodos(2,vecinos(i,1)) nodos(2,vecinos(i,2))],...
221 ’Color’,[.2 .2 .2],’LineStyle’,’:’)
222 end
223 xlimit = [−.02 .02];
224 ylimit = [−.02 .02];
225 xbox = xlimit([1 1 2 2 1]);
226 ybox = ylimit([1 2 2 1 1]);
227 mapshow(xbox,ybox,’DisplayType’,’polygon’,...
228 ’LineStyle’,’none’,’FaceColor’,[.4,.4,.4])
229
230
231
232 line([xf nodos(1,pf)],[yf nodos(2,pf)])

233 axis([ommin ommax psimin psimax])
234 line(puntos(1,:),puntos(2,:),’Color’,’k’,’LineWidth’,3)
235 mapshow(xi,yi,’DisplayType’,’point’,’Marker’,’o’,...
236 ’MarkerFaceColor’,[.4 .4 .4],’MarkerSize’,13,’MarkerEdgeColor’,’none’)
237 mapshow(xf,yf,’DisplayType’,’point’,’Marker’,’^’,...
238 ’MarkerFaceColor’,[.4 .4 .4],’MarkerSize’,13,’MarkerEdgeColor’,’none’)
239 mapshow(nodos(1,:),nodos(2,:),’DisplayType’,’point’,’Marker’,’x’,...
240 ’MarkerSize’,8,’MarkerFaceColor’,’k’,’MarkerEdgeColor’,’k’)
241
242 legend(’Trajectory’,’Roadmap’,’Singularity’,’Starting point’,’End point’)

243 title(’Trajectory using probabilistic roadmaps’)
244 xlabel(’\omega (radians)’)
245 ylabel(’\psi (radians)’)
246
247
248 mfi = (fi − fia) / (size(puntos,2)−1);

249 temp = zeros(1,size(puntos,2));
250 temp(1) = fia;
251 for i = 1:size(puntos,2)−1
252 temp(i+1) = temp(i) + mfi;
253 end
254
255 angulos = [puntos(1,:);temp;puntos(2,:)];

256
257 longitudes = zeros(size(angulos));

258 angulos2 = angulos;
259 [longitudes(1,1),longitudes(2,1),longitudes(3,1)] = ...
260 inversa(angulos(1,1),angulos(2,1),angulos(3,1),h,rb,rp);
261
262 for i = 1:size(angulos,2)−1

263 [longitudes(1,i+1),longitudes(2,i+1),longitudes(3,i+1)] = ...
264 inversa(angulos(1,i+1),angulos(2,i+1),angulos(3,i+1),h,rb,rp);
265 [angulos2(1,i+1), angulos2(2,i+1), angulos2(3,i+1)] = ...
266 directa(longitudes(1,i+1),longitudes(2,i+1),longitudes(3,i+1),...
267 angulos(1,i),angulos(2,i),angulos(3,i),rp,red);
268 end
A.15. Cálculo de trayectoria de velocidad y aceleración me-

diante MUA
1 function [r,v,a,t] = movimiento2(puntos,T)

2 % Generacion de trayectoria de velocidad y aceleracion dada una trayectoria
3 % de posicion. Utiliza el metodo de MUA.
4
5 tam = size(puntos);
6 dt = T / (tam(2)−1);
7 t = 0:dt:T;
8
9 r = puntos;
10 v = zeros(tam);
11 a = v;
12
13 for i = 2:tam(2)
14 dt = t(i)−t(i−1);
15 a(:,i) = 2 ∗ (r(:,i) − r(:,i−1) − v(:,i−1) ∗ dt) / dt^2;
16 v(:,i) = v(:,i−1) + a(:,i) ∗ dt;
17 end

diante MUA y función polinomial
1 function [r,v,a,t] = movimiento3(puntos,T)

3 % de posicion. Utiliza el metodo de MUA excepto para el ultimo punto, en
4 % donde se utiliza una funcion polinomial para llevar la velocidad final a
5 % cero
6
8 dt = T / (tam(2)−1);
9 t = 0:dt:T−dt;
10 r = puntos(:,1:tam(2)−1);
11 v = zeros(size(r));
12 a = v;
13 for i = 2:tam(2)−1
14 dt = t(i)−t(i−1);
15 a(:,i) = 2 ∗ (r(:,i) − r(:,i−1) − v(:,i−1) ∗ dt) / dt^2;
16 v(:,i) = v(:,i−1) + a(:,i) ∗ dt;
17 end
18
19 % Ultimos puntos
20 vi=v(:,i);
21 ri=r(:,i);
22 rf=puntos(:,tam(2));
23
24 f = 2 ∗ ri + vi − 2 ∗ rf;
25 g = rf − ri − vi − f;
26 h = vi;
27 i = ri;
28
29
30 t2=dt/5:dt/5:dt;
31
32 rr=zeros(tam(1),length(t2));
33 vv = rr;
34 aa = rr;
35
36 for j=1:length(t2)
37 tt = t2(j) / dt;
38 t2(j) = t2(j) + t(length(t));
39 rr(:,j) = f ∗ tt^3 + g ∗ tt^2 + h ∗ tt + i;
40 vv(:,j) = 3 ∗ f ∗ tt^2 + 2 ∗ g ∗ tt + h;
41 aa(:,j) = 6 ∗ f ∗ tt + 2 ∗ g;
42 end
43
44 r = [r rr];
45 v = [v vv];
46 a = [a aa];
47 t = [t t2];

diante funciones polinomiales
1 function [r,v,a,t] = movimiento(puntos,T,vin,ai)

3 % de posicion. Utiliza el metodo polinomial.
4
6 dt = T / (tam(2)−1);
7 t=0:dt:T−2∗dt;
8 p = zeros(tam(1),5);
9 r = zeros(tam(1),tam(2)−3);
10 v = r;
11 a = r;
12
13
14 % puntos(i,3:tam(2)−1)
15 for i=1:tam(1)
16 p(i,:) = polyfit(t,puntos(i,2:tam(2)−1),4);
17 end
18 for i=1:length(t)
19 r(:,i) = p(:,1) ∗ t(i)^4 + p(:,2) ∗ t(i)^3 + p(:,3) ∗ t(i)^2 ...
20 + p(:,4) ∗ t(i) + p(:,5);
21 v(:,i) = p(:,1) ∗ t(i)^3 ∗ 4 + p(:,2) ∗ t(i)^2 ∗ 3 + p(:,3)...
22 ∗ t(i) ∗ 2 + p(:,4);
23 a(:,i) = p(:,1) ∗ t(i)^2 ∗ 3 ∗ 4 + p(:,2) ∗ t(i) ∗ 2 ∗ 3 + p(:,3) ∗ 2;
24 end
25
26 % Ultimos puntos
27 vi=v(:,i);
28 ri=r(:,i);
29 rf=puntos(:,tam(2));
30
31 f = 2 ∗ ri + vi − 2 ∗ rf;
32 g = rf − ri − vi − f;
33 h = vi;
34 i = ri;
35
36
37 t2=dt/5:dt/5:dt;
38
39 rr=zeros(tam(1),length(t2));
40 vv = rr;
41 aa = rr;
42 t=t+dt;
44 tt = t2(j) / dt;
45 t2(j) = t2(j) + t(length(t));
46 rr(:,j) = f ∗ tt^3 + g ∗ tt^2 + h ∗ tt + i;
47 vv(:,j) = 3 ∗ f ∗ tt^2 + 2 ∗ g ∗ tt + h;
48 aa(:,j) = 6 ∗ f ∗ tt + 2 ∗ g;
49 end
50
51
52 %Primer punto
53 t1=0:dt/5:dt;
54
55 ri=puntos(:,1);
56 rf=r(:,1);
57 vf=v(:,1);
58 af=a(:,1);
59
60 a3 = ai/2;
61 a4 = vin;
62 a5 = ri;
63 a0 = (af − 6 ∗ vf + 12 ∗ rf − 2 ∗ a3 − 6 ∗ a4 − 12 ∗ a5) / 2;
64 a2 = 4 ∗ rf − vf + a0 − 2 ∗ a3 − 3 ∗ a4 − 4 ∗ a5;
65 a1 = rf − a0 − a2 − a3 − a4 − a5;
66
67 rini = zeros(3,length(t1));
68 vini = rini;
69 aini = rini;
70
72 tt = t1(j) / dt;
73 rini(:,j) = a0 ∗ tt^5 + a1 ∗ tt^4 + a2 ∗ tt^3 + a3 ∗ tt^2 +...
74 a4 ∗ tt + a5;
75 vini(:,j) = 5 ∗ a0 ∗ tt^4 + 4 ∗ a1 ∗ tt^3 + 3 ∗ a2 ∗ tt^2 +...
76 2 ∗ a3 ∗ tt + a4;
77 aini(:,j) = 20 ∗ a0 ∗ tt^3 + 12 ∗ a1 ∗ tt^2 + 6 ∗ a2 ∗ tt + 2 ∗ a3;
78 end
79
80 r = [rini r rr];
81 v = [vini v vv];
82 a = [aini a aa];
83 t = [t1 t t2];
A.18. Programa general del robot
1 clear all
2 close all
3 clc
4 %%
5 mens=’Desea calcular el espacio de trabajo? 0 = no, 1 = si ’;
6 calculaEspacio = input(mens);
7 if ~isnumeric(calculaEspacio)||isempty(calculaEspacio)
8 calculaEspacio = 0;
9 end
10 fprintf(’\n’)
11
12 % Parametros del robot

13 global h rp rb
14 % Revisar si existen los parametros
15 if exist(’parametros.mat’,’file’) == 2
16 disp(’Cargando parametros del robot’)
18 load(’parametros’)
19 else
20 disp(’Calculando parametros del robot’)
22 h = 25.17;
23 rb = 10.98;
24 rp = 13.54;
25
26 %longitud en posicion cero

27 l=norm([rp 0 h]−[rb 0 0]);
28 lmin = 2 ∗ l / 3;
29 e1 = lmin / 2;
30 e2 = e1;
31 hp = 1;
32 r1 = 1.5;
33 rr1 = 1.3;
34 r2 = 1.25;
35 rr2 = 1;
36
37 dA = 2700; %kg/m^3 densidad aluminio

38 dAl = dA ∗ 1000 / 1000000;
39 mp = pi ∗ rp^2 ∗ hp ∗ dAl;
40 m1 = pi ∗ (r1^2 − rr1^2) ∗ 2 ∗ e1 ∗ dAl;
41 m2 = pi ∗ (r2^2 − rr2^2) ∗ 2 ∗ e2 ∗ dAl;
42
43 datos = xlsread(’datosSPS.xls’);
44 net = red(datos);
45
46 save(’parametros.mat’,’h’,’rp’,’rb’,’net’,’e1’,’e2’,’hp’,...
47 ’r1’,’rr1’,’r2’,’rr2’,’mp’,’m1’,’m2’)
48 end
49
50 %%
51 %Espacio de trabajo
52 if calculaEspacio == 0 && exist(’eTrabajo.mat’,’file’) == 2
53 disp(’Cargando espacio de trabajo’)
54 load(’eTrabajo.mat’)
55 else
56 disp(’Calculo del espacio de trabajo’)
57 mens=’Cual es el angulo maximo que pueden girar las articulaciones? ’;
58 ang = input(mens);
59 if ~isnumeric(ang)||isempty(ang)
60 ang = 10;
61 else
62 ang = 90 − ang;
63 end
64 D = 2 ∗ r1;
65 mens=’Cual es el paso (en grados) para el cual desea calcular el’;
66 mens1=’ espacio de trabajo? ’;
67 paso = input([mens mens2]);
68 if ~isnumeric(paso)||isempty(paso)
69 paso = 5;
70 else
71 if paso<1
72 disp(’Paso muy pequeno, se cambiara a 5 grados’)
73 paso = 5;
74 end
75 end
77 disp(’Iniciar calculo de puntos posibles’)
78 [puntos,Domega,Dfi,Dpsi] = Wspace(h,rb,rp,ang,D,paso);
79 disp(’Puntos calculados’)
81 disp(’Calculando singularidades’)
82 obstaculos = Qspace(rb,rp,h,puntos);
83 disp(’Espacio de configuraciones calculado’)
85 save(’eTrabajo.mat’,’puntos’,’Domega’,’Dfi’,’Dpsi’,’obstaculos’,’paso’)
86 end
87
88
89
90
91 %%
92 %Posiciones actuales y deseadas
93 mens=’Ingrese el vector de posiciones iniciales [omega,fi,psi] en grados ’;

94 dat = input(mens);
95 if ~isnumeric(dat)||length(dat)~=3
96 fiIni = −55 ∗ pi / 180;
97 psiIni = −35 ∗ pi / 180;
98 omIni = 3 ∗ pi / 180;
99 else
100 omIni = dat(1) ∗ pi / 180;
101 fiIni = dat(2) ∗ pi / 180;
102 psiIni = dat(3) ∗ pi / 180;
103 end
104 mens=’Ingrese el vector de posiciones deseadas [omega,fi,psi] en grados ’;
105 dat = input(mens);
106 if ~isnumeric(dat)||length(dat)~=3
107 fi = 15 ∗ pi / 180;
108 psi = 25 ∗ pi / 180;
109 om = −2 ∗ pi / 180;
110 else
111 om = dat(1) ∗ pi / 180;
112 fi = dat(2) ∗ pi / 180;
113 psi = dat(3) ∗ pi / 180;
114 end
115
116 dat = input(’Ingrese el tiempo deseado del movimiento ’);

118 if ~isnumeric(dat)||isempty(dat)
119 T=1;
120 else
121 T=dat(1);
122 end
123
124 %Velocidad y aceleraci’on inicial

125 vi=[0;0;0];
126 ai=[0;0;0];
127
128 disp(’Calculando trayectoria’)

129 %Calculo de posiciones, velocidades y aceleraciones intermedias
130 %trayectoria−>lineal
131 %trayectoria2−>PRM
132 %[l,an,an2] = trayectoria(h,rb,rp,fiIni,psiIni,omIni,fi,psi,om,net,T);
133 [l,an,an2] = trayectoria2(h,rb,rp,fiIni,psiIni,omIni,fi,
134 psi,om,Domega(1),Domega(2),Dpsi(1),Dpsi(2),net,T);
135 %movimiento−>polinomial
136 %movimiento2−>mua
137 %movimiento3−>mua y polinomial
138 [r,v,a,t] = movimiento(an,T,0,0);
139 %[r,v,a,t] = movimiento2(an,T);
140 traye=1;
141 disp(’Trayectoria calculada’)

143
144
145 %%
146 %Dinamica inversa
147 % angulos=[omega;fi;psi]
148
149 disp(’Calculando fuerzas’)

150
151 f = zeros(3,length(t));
152 Jp = zeros(3,3,length(t));
153 f2=f;
154 Jp2=Jp;
155
156 for i = 1:length(t)

157 [f(:,i),Jp(:,:,i)] = dinamicaInversa(r(1,i),r(2,i),r(3,i),v(1,i),...
158 v(2,i),v(3,i),a(1,i),a(2,i),a(3,i),h,rb,rp,hp,r1,r2,rr1,...
159 rr2,e1,e2,mp,m1,m2);
160 [f2(:,i),Jp2(:,:,i)] = dinamicaInv(r(1,i),r(2,i),r(3,i),v(1,i),...
161 v(2,i),v(3,i),a(1,i),a(2,i),a(3,i),h,rb,rp,hp,r1,r2,rr1,...
162 rr2,e1,e2,mp,m1,m2);
163 end
164 disp(’Fuerzas calculadas’)
166
167 %%
168 % Dinamica directa
169 disp(’Calculando aceleraciones’)
170
171 wpp = zeros(3,length(t));

172 w = [r(1,1);r(2,1);r(3,1)];
173 wp = [0;0;0];
174 r3=zeros(size(r));
175 v3=r3;
176 a3=r3;
177 r3(:,1) = w;
178 for i = 1:length(t)−1
179 dt = (t(i+1)−t(i));
180 wpp(:,i+1) = dinamicaDirecta(f(:,i+1),w,wp,h,rb,rp,hp,r1,r2,...
181 rr1,rr2,e1,e2,mp,m1,m2);
182 temp=wp;
183 wp = wp + dt ∗ wpp(:,i+1);
184 [o,ff,p,op,fp,pp,opp,fpp,ppp] = angulosDirecta(wpp(:,i+1),wp,w,dt,temp);
185 w=[o;ff;p];
186 r3(:,i+1) = w;
187 v3(:,i+1) = [op;fp;pp];
188 a3(:,i+1) = [opp;fpp;ppp];
189 end
190 disp(’Aceleraciones calculadas’)
192
193 %%
194 %Control PID
195 disp(’Calculando control PID’)
196 TT = 2;
197 ffi = fiIni;
198 ppi = psiIni;
199 wi = omIni;
200 ff = fi;
201 pf = psi;
202 wf = om;
203 kp=4;
204 % kd=1;
205 % ki=0.01;
206 kd=0;
207 ki=0;
208 wp=[0;0;0];
209 [E,W,Wp,W2,F,ti] = controlPID(wi,ffi,ppi,wf,ff,pf,TT,h,rp,rb,hp,r1,r2,...
210 rr1,rr2,e1,e2,mp,m1,m2,kp,ki,kd,wp);
211 disp(’Control calculado’)
213
214 %%
215 %Control difuso
216 disp(’Calculando control difuso’)
217 fismat = readfis(’controlDifuso’);
218 [Ef,DEf,Wf,W2f,Ff,tif] = controlFuzzy(wi,ffi,ppi,wf,ff,pf,TT,h,rp,rb,...
219 hp,r1,r2,rr1,rr2,e1,e2,mp,m1,m2,fismat,wp);
220 disp(’Control calculado’)
222 %%
223 %Graficas
224 graficar(rb,rp,h,puntos,r,omIni,om,fiIni,fi,psiIni,psi,obstaculos,an,...
225 an2,t,v,a,f,f2,T,Dfi,wpp,r3,v3,a3,E,W,W2,F,ti,Ef,Wf,W2f,...
226 Ff,tif,DEf,traye,Domega,Dpsi,l)
A.18.1. Gráficas del robot
1 function graficar(rb,rp,h,puntos,r,omIni,om,fiIni,fi,psiIni,psi,...
2 obstaculos,an,an2,t,v,a,f,f2,T,Dfi,wpp,rr,vv,aa,E,W,W2,F,ti,Ef,...
3 Wf,W2f,Ff,tif,DEf,traye,Domega,Dpsi,l)
4

7 an = an ∗ 180 / pi;
8 an2 = an2 ∗ 180 / pi;
9
10
12 graf1 = input(’Desea graficar el espacio de trabajo? 0 = no, 1 = si ’);
13 if ~isnumeric(graf1)||isempty(graf1)
14 graf1 = 0;
15 end
17 mens=’Desea graficar la trayectoria en el espacio de trabajo?’;
18 mens2=’ 0 = no, 1 = si ’;
19 graf2 = input([mens mens2]);
21 graf2 = 0;
22 end
24 graf3 = input(’Desea graficar las singularidades? 0 = no, 1 = si ’);
26 graf3 = 0;
27 end
29 graf11 = input(’Desea graficar la cinematica inversa? 0 = no, 1 = si ’);
31 graf11 = 0;
32 end
34 mens=’Desea graficar el error de la cinematica directa? 0 = no, 1 = si ’;
35 graf4 = input(mens);
37 graf4 = 0;
38 end
40 mens=’Desea graficar las posiciones, velocidades y aceleraciones?’;
41 mens2=’ 0 = no, 1 = si ’;
42 graf5 = input([mens mens2]);
44 graf5 = 0;
45 end
47 graf6 = input(’Desea graficar las fuerzas? 0 = no, 1 = si ’);
49 graf6 = 0;
50 end
52 mens1=’Desea graficar las posiciones, velocidades y’;
53 graf7 = input([mens1 ’ aceleraciones (dinamica directa)? 0 = no, 1 = si ’]);
55 graf7 = 0;
56 end
58 graf8 = input(’Desea graficar las aceleraciones? 0 = no, 1 = si ’);
60 graf8 = 0;
61 end
63 graf9 = input(’Desea graficar el control PID? 0 = no, 1 = si ’);
65 graf9 = 1;
66 end
68 graf10 = input(’Desea graficar el control difuso? 0 = no, 1 = si ’);
70 graf10 = 0;
71 end
73 %% % % % % % % % % % % % % % % % % % % % %
74 % Espacio de Trabajo
75 verts1=[];
76 verts2=[];
77 faces=[];
78 u=1;
79 for tq = 0:0.1:2∗pi;
80 verts1 = [verts1;cos(tq)∗rb sin(tq)∗rb −h];
81 verts2 = [verts2;cos(tq)∗rp sin(tq)∗rp 0];
82 faces=[faces u];
83 u=u+1;
84 end
85
86 if graf1==1
87 figure
88 if length(puntos)<1000000
89 if length(puntos)>100000
90 mens1=’Graficando espacio de trabajo, por favor espere ya’;
91 disp([mens1 ’ que se calcularon mas de cien mil puntos’])
92 end
93 [xb1,yb1,zb1] = ang2xyz(Bi(1,:),puntos);
94 rmax=max(rp,rb);
95 axis([−rmax rmax −rmax rmax −h rp]);
96 scatter3(xb1,yb1,zb1,12,’k’,’filled’)
97 title(’Espacio de Trabajo’)
98 xlabel(’x’)
99 ylabel(’y’)
100 zlabel(’z’)
101 hold on
102 patch(’Faces’,faces,’Vertices’,verts1,’FaceColor’,’b’,...
103 ’FaceAlpha’,0.5);
104 patch(’Faces’,faces,’Vertices’,verts2,’FaceColor’,’g’,...
105 ’FaceAlpha’,0.25);
106 l1=[Ai(1,1) Bi(1,1);Ai(1,2) Bi(1,2);Ai(1,3) Bi(1,3)];
107 l2=[Ai(2,1) Bi(2,1);Ai(2,2) Bi(2,2);Ai(2,3) Bi(2,3)];
108 l3=[Ai(3,1) Bi(3,1);Ai(3,2) Bi(3,2);Ai(3,3) Bi(3,3)];
109 plot3(l1(1,:),l1(2,:),l1(3,:),’LineWidth’,4,’Color’,’r’)
112 scatter3(Ai(:,1),Ai(:,2),Ai(:,3),’filled’,’MarkerFaceColor’,’k’)
113 scatter3(Bi(:,1),Bi(:,2),Bi(:,3),’filled’,’MarkerFaceColor’,’k’)
114 else
115 mens1=’No se ha graficado el espacio de trabajo debido a que se ’;
116 mens2=’calcularon mas de un millon de puntos, intentar’;
117 disp([mens1 mens2 ’ utilizar menos puntos’])
118 end
119 end
120
121
122
123 %%
124 %% % % % % % % % % % % % % % % % % % % % % % % % % % % % % % %
125 %Trayectoria en espacio de trabajo
126 if graf2==1
127 figure
128 [xb,yb,zb] = ang2xyz(Bi(1,:),[r(1,:);r(2,:);r(3,:)]);
129 rmax=max(rp,rb);
130 axis([−rmax rmax −rmax rmax −h rp]);
131 plot3(xb,yb,zb,’LineWidth’,3,’Color’,[1,.1,.85])
132 grid on
133 title(’Trayectoria’)
134 xlabel(’x’)
135 ylabel(’y’)
136 zlabel(’z’)
137 hold on
138 %Plataforma inicial
139 Ra = [1 0 0; 0 cos(omIni) −sin(omIni);0 sin(omIni) cos(omIni)];
140 Rb = [cos(fiIni) 0 sin(fiIni);0 1 0;−sin(fiIni) 0 cos(fiIni)];
141 Rc = [cos(psiIni) −sin(psiIni) 0;sin(psiIni) cos(psiIni) 0;0 0 1];

142 R = Ra ∗ Rb ∗ Rc;
143 B=(R∗Bi’)’;
144 verts3=(R∗verts2’)’;
145 %Plataforma final
146 Ra = [1 0 0; 0 cos(om) −sin(om);0 sin(om) cos(om)];
147 Rb = [cos(fi) 0 sin(fi);0 1 0;−sin(fi) 0 cos(fi)];
148 Rc = [cos(psi) −sin(psi) 0;sin(psi) cos(psi) 0;0 0 1];
149 R = Ra ∗ Rb ∗ Rc;
150 B2=(R∗Bi’)’;
151 verts4=(R∗verts2’)’;
152 patch(’Faces’,faces,’Vertices’,...
153 verts3,’FaceColor’,[.6,.6,.6],’FaceAlpha’,0.2);
154 patch(’Faces’,faces,’Vertices’,...
156 patch(’Faces’,faces,’Vertices’,....
158 l1=[Ai(1,1) B(1,1);Ai(1,2) B(1,2);Ai(1,3) B(1,3)];
159 l2=[Ai(2,1) B(2,1);Ai(2,2) B(2,2);Ai(2,3) B(2,3)];
160 l3=[Ai(3,1) B(3,1);Ai(3,2) B(3,2);Ai(3,3) B(3,3)];
161 plot3(l1(1,:),l1(2,:),l1(3,:),’LineWidth’,4,’Color’,’k’)
165 scatter3(B(:,1),B(:,2),B(:,3),’filled’,’MarkerFaceColor’,’k’)
166 l1=[Ai(1,1) B2(1,1);Ai(1,2) B2(1,2);Ai(1,3) B2(1,3)];
167 l2=[Ai(2,1) B2(2,1);Ai(2,2) B2(2,2);Ai(2,3) B2(2,3)];
168 l3=[Ai(3,1) B2(3,1);Ai(3,2) B2(3,2);Ai(3,3) B2(3,3)];
173 scatter3(B2(:,1),B2(:,2),B2(:,3),’filled’,’MarkerFaceColor’,’k’)
174
175 legend(’Trayectoria’,’Posicion inicial’,’Posicion final’)

176
177 end
178 %%
179 %% % % % % % % % % % % % % % % % % % % % % % % % %
180 %Espacio de Configuraciones
181 if graf3==1
182 if isempty(obstaculos)
183 j=1;
184 for i=Dfi(1):.0175:Dfi(2)
185 obstaculos(:,j) = [0;i;0];
186 j=j+1;
187 end
188 end
189 figure
190 omq=obstaculos(1,:)∗180/pi;
191 fiq=obstaculos(2,:)∗180/pi;
192 psiq=obstaculos(3,:)∗180/pi;
193 scatter3(omq(:),fiq(:),psiq(:),4,[.4,.1,.7],’filled’)
194 title(’Singularities’)
195 % legend(’Posiciones singulares’)
196 xlabel(’\omega (degrees)’)
197 ylabel(’\phi (degrees)’)
198 zlabel(’\psi (degrees)’)
199 axis([Domega(1) Domega(2) Dfi(1) Dfi(2) Dpsi(1) Dpsi(2)]∗180/pi);
200 end
201
202 %%
203 %% % % % % % % % % % % % % % % % % % % % % % %
204 %Cinematica inversa
205 if graf11==1
206 figure
207 subplot(2,1,1)
208 plot(t,an(1,:),’k−’,t,an(2,:),’k−−’,t,an(3,:),’k:’)
209 title(’Platform position’)
210 legend(’\omega’,’\phi’,’\psi’)
211 xlabel(’Time (seconds)’)
212 ylabel(’Angle (degrees)’)
213 subplot(2,1,2)
214 plot(t,l(1,:),’k−’,t,l(2,:),’k−−’,t,l(3,:),’k:’)
215 title(’Length of the limbs’)
216 legend(’Limb 1’,’Limb 2’,’Limb 3’)
218 ylabel(’Length (centimeters)’)
219 end
220
221 %%
222 %% % % % % % % % % % % % % % % % % % % % % % % % % % % % % %
223 % Trayectoria planeada vs trayectoria de cinematica directa
224 if graf4==1
225 figure
226 % subplot(2,1,1)
227 plot(t,an(1,:),’k−’,t,an(2,:),’k−−’,t,an(3,:),’k:’)
228 hold on
229 plot(t,an2(1,:),’kd’,t,an2(2,:),’ko’,t,an2(3,:),’kx’)
230 title(’Angle trajectory’)
231 legend(’\omega’,’\phi’,’\psi’,’calculated \omega’,...
232 ’calculated \phi’,’calculated \psi’,’Location’,’NorthEastOutside’)
235 ylim([min(min(an))−10 max(max(an))+10])
236 % subplot(2,1,2)
237 figure
238 % stem(t,(an−an2)’)
239 an3=(an−an2);
240 plot(t,an3(1,:),’k−’,t,an3(2,:),’k−−’,t,an3(3,:),’k:’)
241 title({’Angle error’})
242 legend(’\omega error’,’\phi error’,’\psi error’,...
243 ’Location’,’NorthEastOutside’)
245 ylabel(’Error (degrees)’)
246 end
247
248 %%
249 %% % % % % % % % % % % % % % % % % % % % % % % % % % % % % % %
250 %Posiciones, velocidades y aceleraciones
251 if graf5==1
252
253 figure
254 green=[0,.41,0];
255 subplot(3,1,1)
256 plot(t,r(1,:),’k−’)
257 hold on
258 plot(t,r(2,:),’k:’)
259 plot(t,r(3,:),’k−−’)
260 title(’Position’)
261 legend(’\omega’,’\phi’,’\psi’,’Location’,’NorthEastOutside’)
263 ylabel(’rad’)
264 ylim([min(min(r))−.05 max(max(r))+.05])
265 subplot(3,1,2)
266 plot(t,v(1,:),’k−’)
267 hold on
268 plot(t,v(2,:),’k:’)
269 plot(t,v(3,:),’k−−’)
270 title(’Velocity’)
271 h=legend(’$\dot{\omega}$’,’$\dot{\phi}$’,...
272 ’$\dot{\psi}$’,’Location’,’NorthEastOutside’);
273 set(h,’Interpreter’,’latex’)
275 ylabel(’rad/s’)
276 ylim([min(min(v))−.05 max(max(v))+.05])
277 subplot(3,1,3)
278 plot(t,a(1,:),’k−’)
279 hold on
280 plot(t,a(2,:),’k:’)
281 plot(t,a(3,:),’k−−’)
282 % stairs(t,[a(1,2:length(a)) 0],’k−’)
283 % hold on
284 % stairs(t,[a(2,2:length(a)) 0],’k:’)
285 % stairs(t,[a(3,2:length(a)) 0],’k−−’)

286 title(’Acceleration’)
287 h=legend(’$\ddot{\omega}$’,’$\ddot{\phi}$’,...
288 ’$\ddot{\psi}$’,’Location’,’NorthEastOutside’);
291 ylabel(’rad/s^2’)
292 ylim([min(min(a))−.05 max(max(a))+.05])
293 end
294
295
296 %%
297 %% % % % % % % % % % % % % % % % % % % % % % % % % % % % % %
298 %Fuerzas
299 if graf6==1
300 figure
301 %subplot(2,1,1)
302 f = [f(:,2:length(f)) [0;0;0]];
303 f2 = [f2(:,2:length(f)) [0;0;0]];
304 stairs(t,f(1,:)’,’k−’)
305 hold on
306 stairs(t,f(2,:)’,’k:’)
307 stairs(t,f(3,:)’,’k−−’)
308 title(’Required forces’)
309 legend(’Actuator 1’,’Actuator 2’,’Actuator 3’,...
312 ylabel(’Force (Newtons)’)
313 figure
314 subplot(2,1,2)
315 stairs(t,f2’)
316 title(’Trayectoria de fuerzas (Mediante el metodo de Merlet)’)
317 legend(’Actuador 1’,’Actuador 2’,’Actuador 3’,...
319 xlabel(’Tiempo (segundos)’)
320 ylabel(’Fuerza (Newtons)’)
321 % subplot(3,1,3)
322 % stairs(t,(f−f2)’)
323 % title(’Diferencia de trayectorias (Trayectoria 1 − Trayecrotia 2)’)
324 % legend(’Actuador 1’,’Actuador 2’,’Actuador 3’)
325 % xlabel(’Tiempo (segundos)’)
326 % ylabel(’Fuerza (Newtons)’)
327 end
328
329 %%
330 %% % % % % % % % % % % % % % % % % % % % % % % % % % % %
331 %Aceleraciones 1
332 if graf7==1
333 figure
334 green=[0,.41,0];
335 subplot(3,1,1)
336 plot(t,rr(1,:),’b’)
337 hold on
338 plot(t,rr(2,:),’Color’,green)
339 plot(t,rr(3,:),’r’)
340 title(’Posicion’)
341 legend(’\omega’,’\phi’,’\psi’,’Location’,’NorthEastOutside’)
343 ylabel(’rad’)
344 subplot(3,1,2)
345 plot(t,vv(1,:),’b’)
346 hold on
347 plot(t,vv(2,:),’Color’,green)
348 plot(t,vv(3,:),’r’)
349 title(’Velocidad’)
350 h=legend(’$\dot{\omega}$’,’$\dot{\phi}$’,...
351 ’$\dot{\psi}$’,’Location’,’NorthEastOutside’);
354 ylabel(’rad/s’)
355 subplot(3,1,3)
356 stairs(t,[aa(1,2:length(aa)) 0],’b’)
357 hold on
358 stairs(t,[aa(2,2:length(aa)) 0],’Color’,green)
359 stairs(t,[aa(3,2:length(aa)) 0],’r’)
360 title(’Aceleracion’)
361 h=legend(’$\ddot{\omega}$’,’$\ddot{\phi}$’,...
362 ’$\ddot{\psi}$’,’Location’,’NorthEastOutside’);
366 end
367
368 %%
369 %% % % % % % % % % % % % % % % % % % % % % % % % % % % %
370 %Aceleraciones 2
371 if graf8==1
372 rt=r(:,1);
373 vt=v(:,1);
374 rr = zeros(3,length(t));
375 vv=rr;
376 rr(:,1) = rt;
377 vv(:,1) = vt;
378 app=zeros(3,length(t));
379 for i = 1:length(t)
380 if i>1
381 dt = t(i)−t(i−1);
382 else
383 dt = 0;
384 end
385 ompp=a(1,i);
386 fipp=a(2,i);
387 psipp=a(3,i);
388 omp=v(1,i);
389 fip=v(2,i);
390 psip=v(3,i);
391 om=r(1,i);
392 fi=r(2,i);
393 app1=ompp + psipp ∗ sin(fi) + psip ∗ fip ∗ cos(fi);
394 app2=fipp ∗ cos(om) − fip ∗ omp ∗ sin(om) − psipp ∗ cos(fi)...
395 ∗ sin(om) + psip ∗ fip ∗ sin(fi) ∗ sin(om) − psip...
396 ∗ omp ∗ cos(fi) ∗ cos(om);
397 app3=fipp ∗ sin(om) − fip ∗ omp ∗ cos(om) + psipp ∗ cos(fi)...
398 ∗ cos(om) − psip ∗ fip ∗ sin(fi) ∗ cos(om) − psip...
399 ∗ omp ∗ cos(fi) ∗ sin(om);
400 app(:,i)=[app1;app2;app3];
401 end
402
403 figure
404 app = [app(:,2:length(app)) [0;0;0]];
405 wpp = [wpp(:,2:length(wpp)) [0;0;0]];
406 stairs(t,app’)
407 hold on
408 stairs(t,wpp’,’−−’)
409 title(’Aceleracion’)
412 h=legend(’$\ddot{\omega}$ real’,’$\ddot{\phi}$ real’,...
413 ’$\ddot{\psi}$ real’,...
414 ’$\ddot{\omega}$ calculada’,’$\ddot{\phi}$ calculada’,...
415 ’$\ddot{\psi}$ calculada’,’Location’,’NorthEastOutside’);
417 end
418
419 %%
420 %% % % % % % % % % % % % % % % % % % % % % % % % % % % %
421 %Controles
422 if graf9==1&&graf10==1
423 figure
424 subplot(2,2,1)
425 plot(ti,E)
426 title(’Error en actuadores (Control PID)’)
428 ylabel(’Error (centimetros)’)
429 legend(’Actuador 1’,’Actuador 2’,’Actuador 3’)

430 subplot(2,2,2)
431 plot(ti,W∗180/3.1416)
432 hold on
433 plot(ti,W2∗180/3.1416,’−−’)
434 title(’Posicion de la plataforma (Control PID)’)
436 ylabel(’Angulo (grados)’)
438 subplot(2,2,3)
439 plot(tif,Ef)
440 title(’Error en actuadores (Control difuso)’)
442 ylabel(’Error (centimetros)’)
443 legend(’Actuador 1’,’Actuador 2’,’Actuador 3’)
444 subplot(2,2,4)
445 plot(tif,Wf∗180/3.1416)
446 hold on
447 plot(tif,W2f∗180/3.1416,’−−’)
448 title(’Posicion de la plataforma (Control difuso)’)
450 ylabel(’Angulo (grados)’)
452 else
453 %Control PID
454 if graf9==1
455 figure
456 subplot(2,1,1)
457 plot(ti,E(1,:),’k−’)
458 hold on
459 plot(ti,E(2,:),’k:’)
460 plot(ti,E(3,:),’k−−’)
461 title(’Error of the actuators’)
463 ylabel(’Error (centimeters)’)
464 legend(’Actuator 1’,’Actuator 2’,’Actuator 3’)
465 subplot(2,1,2)
466 plot(ti,W(1,:)∗180/3.1416,’k−’,’LineWidth’,2)
467 hold on
468 plot(ti,W(2,:)∗180/3.1416,’k:’,’LineWidth’,2)
469 plot(ti,W(3,:)∗180/3.1416,’k−−’,’LineWidth’,2)
470 plot(ti,W2(1,:)∗180/3.1416,’k−’,’LineWidth’,1)
471 plot(ti,W2(2,:)∗180/3.1416,’k:’,’LineWidth’,1)
472 plot(ti,W2(3,:)∗180/3.1416,’k−−’,’LineWidth’,1)
473 title(’Position of the plaform’)
476 legend(’\omega’,’\phi’,’\psi’,’desired \omega’,...
477 ’desired \phi’,’desired \psi’)

478 end
479 %% % % % % % % % % % % % % % % % % % % % % % % % % % % % % % %
480 %Control difuso
481 if graf10==1
482 figure
483 subplot(2,1,1)
484 plot(tif,Ef(1,:),’k−’)
485 hold on
486 plot(tif,Ef(2,:),’k:’)
487 plot(tif,Ef(3,:),’k−−’)
488 title(’Error of the actuators’)
490 ylabel(’Error (centimeters)’)
491 legend(’Actuator 1’,’Actuator 2’,’Actuator 3’)
492 subplot(2,1,2)
493 plot(tif,Wf(1,:)∗180/3.1416,’k−’,’LineWidth’,2)
494 hold on
495 plot(tif,Wf(2,:)∗180/3.1416,’k:’,’LineWidth’,2)
496 plot(tif,Wf(3,:)∗180/3.1416,’k−−’,’LineWidth’,2)
497 plot(tif,W2f(1,:)∗180/3.1416,’k−’,’LineWidth’,1)
498 plot(tif,W2f(2,:)∗180/3.1416,’k:’,’LineWidth’,1)
499 plot(tif,W2f(3,:)∗180/3.1416,’k−−’,’LineWidth’,1)
500 title(’Position of the plaform’)
503 legend(’\omega’,’\phi’,’\psi’,’desired \omega’,...
504 ’desired \phi’,’desired \psi’)
505 end
506 end
A.18.1.1. Cálculo de coordenadas del punto b1
1 function [x,y,z] = ang2xyz(Bi,puntos)

2 % Funcion que recibe la matriz Bi y los angulos de euler. La salida son las
3 % coordenadas del punto b1
4 b = Bi(:);
5 tam = size(puntos,2);
6 x = zeros(1,tam);
7 y = x;
8 z = y;
9
10 for i = 1:size(puntos,2)
11 om = puntos(1,i);
12 fi = puntos(2,i);
13 psi = puntos(3,i);
14 Ra = [1 0 0; 0 cos(om) −sin(om);0 sin(om) cos(om)];
15 Rb = [cos(fi) 0 sin(fi);0 1 0;−sin(fi) 0 cos(fi)];
16 Rc = [cos(psi) −sin(psi) 0;sin(psi) cos(psi) 0;0 0 1];
17 R = Ra ∗ Rb ∗ Rc;
18 Bn = R ∗ b;
19 x(i) = Bn(1);
20 y(i) = Bn(2);
21 z(i) = Bn(3);
22 end
Bibliografía
[1] Bai, S., and Hansen, M. R. Evaluation of Workspace of a Spherical Robotic

Wrist. In 2007 IEEE/ASME international conference on advanced intelligent me-
chatronics (2007), Ieee, pp. 1–6.
[2] Clavel, R. DELTA, a fast robot with parallel geometry. In Proc 18th International
Symposium on Industrial Robots (1988), pp. 91–100.
[3] Craig, J. J. Introduction to Robotics: Mechanics and Control. Prentice Hall, 2004.
[4] Crane, C. D., and Duffy, J. Kinematic analysis of robot manipulators. Cam-
bridge University Press, 1998.
[5] Cui, G., and Zhang, Y. Kinetostatic Modeling and Analysis of a New 3-DOF Pa-
rallel Manipulator. In 2009 International Conference on Computational Intelligence
and Software Engineering (Dec. 2009), IEEE, pp. 1–4.
[6] Dasgupta, B., and Mruthyunjaya, T. Closed-Form Dynamic Equations of the

General Stewart Platform through the Newton–Euler Approach. Mechanism and
Machine Theory 99, 8 (1998), 1135–1152.
[7] Deep, K., and Mebrahtu, H. New Variations of Order Crossover for Travelling
Salesman Problem. International Journal of Combinatorial Optimization Problems
and Informatics 2, 1 (2011), 2–13.
[8] Di Gregorio, R. A new parallel wrist using only revolute pairs: the 3-RUU wrist.
Robotica 19, 03 (2001), 305–309.
[9] Di Gregorio, R. Kinematics of the 3-RSR wrist. IEEE Transactions on Robotics

20, 4 (2004), 750–753.
[10] Di Gregorio, R., and Parenti Castelli, V. Closed-Form solution of the

position analysis of pure translational 3-RUU parallel mechanism. In Proc. of the
8th Symposium on Mechanics and Mechanical Transmissions (2000), pp. 119–124.
169
Bibliografía 170
[11] Gan, D., Dias, J., and Seneviratne, L. Design and Analytical Kinematics of a
Robot Wrist Based on a Parallel Mechanism. In World Automation Congress (2012),
pp. 1–6.
[12] Geng, Z., Haynes, L. S., Lee, J. D., and Carroll, R. L. On the dynamic mo-
del and kinematic analysis of a class of Stewart platforms. Robotics and Autonomous
Systems 9, 4 (1992), 237–254.
[13] Gosselin, C. Kinematic Analysis, Optimization and Programming of Parallel Ro-

botic Manipulators. PhD thesis, McGill University, Montréal, Cánada, 1988.
[14] Hervé, J. M., and Karouia, M. A family of novel orientational 3-DOF parallel
robots. In Proc. RoManSy 14 (Udine, Italy, 2002), pp. 359–368.
[15] Huda, S., and Takeda, Y. Dimensional Synthesis of 3-URU Pure Rotational
Parallel Mechanism with Respect to Singularity and Workspace. In International
Federation for the Theory of Mechanisms and Machines World Congress (Besançon,
2007), pp. 1–6.
[16] Karouia, M., and Hervé, J. M. A Three-dof Tripod for Generating Spherical
Rotation. In Advances in Robot Kinematics. Springer, 2000, pp. 395–402.
[17] Kavraki, L. E., Svestka, P., Latombe, J.-C., and Overmars, M. H. Proba-
bilistic roadmaps for path planning in high-dimensional configuration spaces. IEEE
Transactions on Robotics and Automation 12, 4 (1996), 566–580.
[18] Khan, S., and Andersson, K. Optimal Design of a 6-DoF Haptic device. In
International Conference on mechatronics (Istambul, 2011), IEEE, pp. 713–718.
[19] Kong, X., and Gosselin, C. Type Synthesis of 3-DOF Spherical Parallel Ma-
nipulators Based on Screw Theory. Journal of Mechanical Design 126, 1 (2004),
101.
[20] Li, Y., and Xu, Q. Kinematics and inverse dynamics analysis for a general 3-PRS
spatial parallel mechanism. Robotica 23, 2 (Mar. 2005), 219–229.
[21] Liu, M.-j., Li, C.-X., and Li, C.-n. Dynamics analysis of the Gough-Stewart
platform manipulator. IEEE Transactions on Robotics and Automation 16, 1 (2000),
94–98.
[22] Lou, Y., Liu, G., and Li, Z. Randomized Optimal Design of Parallel Manipu-
lators. IEEE Transactions on Automation Science and Engineering 5, 2 (2008),
223–233.
Bibliografía 171
[23] Merlet, J. P. Parallel Robots. Solid Mechanics and its Applications. Springer,
2006.
[24] Mitchell, M. An Introduction to Genetic Algorithms. The MIT Press, Cambridge,

1996.
[25] Norton, R. L. Design of Machinery: An Introduction to the Synthesis and Analysis

of Mechanisms and Machines. McGraw-Hill, 2011.
[26] Ollero Baturone, A. Robótica: manipuladores y robots móviles. Alfaomega -

Marcombo, 2007.
[27] Ponce Cruz, P. Inteligencia Artificial con Aplicaciones a la Ingeniería, first ed.
Alfaomega, 2010.
[28] Ponce Cruz, P., and Ramírez Figueroa, F. D. Intelligent Control Systems
with LabView, first ed. Springer, 2010.
[29] Ruiz García, J. A. Diseño y análisis de un robot paralelo 6-PUS y su implementa-

ción como una fresadora CNC. Master’s thesis, Instituto Tecnológico y de Estudios
Superiores De Monterrey, 2008.
[30] Shang, Y., and Li, G.-J. New crossover operators in genetic algorithms. In Pro-
ceedings of The Third International Conference on Tools for Artificial Intelligence
(1991), pp. 150–153.
[31] Tsai, K., and Lin, J. Determining the compatible orientation workspace of Ste-
wart–Gough parallel manipulators. Mechanism and Machine Theory 41, 10 (Oct.
2006), 1168–1184.
[32] Tsai, L.-W. Kinematics of A Three-Dof Platform with Three Extensible Limbs.
In Recent Advances in Robot Kinematics. Springer, 1996, ch. 8, pp. 401–410.
[33] Tsai, L.-W. Solving The Inverse Dynamics of Parallel Manipulators by The Prin-
ciple of Virtual Work. In ASME Design Eng. Tech. Conf. (1998), pp. 451–457.
[34] Tsai, L.-W. Robot Analysis: The Mechanics of Serial and Parallel Manipulators.
John Wiley & Sons, Inc., 1999.
[35] Wang, Z., Wang, G., Ji, S., Wan, Y., and Yuan, Q. Optimal design of a linear
Delta robot for the prescribed cuboid dexterous workspace. In 2007 IEEE Interna-
tional Conference on Robotics and Biomimetics ROBIO (2007), Ieee, pp. 2183–2188.
[36] Zadeh, L. A. Fuzzy sets. Information and control (1965), 338–353.

Bibliografía 172
[37] Zhang, L., and Song, Y. Optimal design of the Delta robot based on dynamics.
In 2011 IEEE International Conference on Robotics and Automation (2011), School
of Mechanical Engineering, Tianjin University, 300072, China, IEEE, pp. 336–341.
[38] Zlatanov, D., Bonev, I., and Gosselin, C. Constraint Singularities as C-Space
Singularities. In 8th International Symposium on Advances in Robot Kinematics
(Caldes de Malavella, Spain, 2002).
View publication stats

Robotica

Enviado por

Dados do documento

Direitos autorais

Formatos disponíveis

Compartilhar este documento

Compartilhar ou incorporar documento

Opções de compartilhamento

Você considera este documento útil?

Este conteúdo é inapropriado?

Direitos autorais:

Formatos disponíveis

Robotica

Enviado por

Direitos autorais:

Formatos disponíveis

See discussions, stats, and author profiles for this publication at: https://www.researchgate.

Análisis, Diseño y Control de un Manipulador Paralelo 3-SPS-1S Utilizado en

Thesis · June 2015

Daniel Chaparro Altamirano Ricardo Zavala-Yoe

SEE PROFILE SEE PROFILE

Modelling and control of magnetorheological suspensions in cars View project

The user has requested enhancement of the downloaded file.

Análisis, Diseño y Control de un

Tesis presentada en cumplimiento de los requisitos

Escuela de Diseño, Ingenieniería y Arquitectura

Maestro en Ciencias de la Ingeniería

Análisis, Diseño y Control de un Manipulador Paralelo 3-SPS-1S Utilizado

por Daniel Chaparro Altamirano

La presente tesis presenta un método geométrico para obtener la cinemática directa

Lista de Tablas viii

3. Manipuladores paralelos esféricos 14

A.5.5. Decodificación de cromosomas . . . . . . . . . . . . . . . . . . . . . 123

2.1. a) Unión rotacional y b) unión prismática [4]. . . . . . . . . . . . . . . . . 4

3.1. Manipuladores con tres movimientos rotacionales . . . . . . . . . . . . . . 15

4.1. Manipulador 3-SPS-1-S . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24

4.11. Ángulos calculados mediante cinemática directa . . . . . . . . . . . . . . . 36

5.1. Espacio de trabajo con h = 20 cm, rb = 15 cm y rp = 15 cm . . . . . . . . 43

6.1. Singularidades del robot en el espacio de configuraciones de la plataforma 53

7.1. Ángulos de la pata i [34]. . . . . . . . . . . . . . . . . . . . . . . . . . . . 59

8.1. Posición de la plataforma y error en actuadores con un controlador PID . 73

9.1. Ejemplo de trayectoria mediante interpolación lineal . . . . . . . . . . . . 81

3.1. Combinaciones posibles de articulaciones para tener 3 GDL en la plataforma. 15

4.1. Problemas que se resuelven mediante RNA. . . . . . . . . . . . . . . . . . 34

5.1. Comparación de ángulos máximos y mínimos . . . . . . . . . . . . . . . . 49

8.1. Matriz de asociación difusa del controlador PD . . . . . . . . . . . . . . . 76

La inseguridad es un problema en el mundo y hoy en día en México se ha convertido en

1.1. Trabajo a futuro

2.1. Cadenas cinemáticas, mecanismos y máquinas

Una cadena cinemática es un ensamble de eslabones que se conectan mediante juntas.

Unión universal (U ). Esta unión permite rotación sobre dos ejes.

Unión prismática (P ). Esta unión permite traslación sobre un eje.

Unión cilíndrica (C ). Esta unión permite rotación y traslación sobre el mismo

Un mecanismo es un ensamble de sólidos rígidos o elásticos, que interactúan entre

Figura 2.1: a) Unión rotacional y b) unión prismática [4].

El manipulador mecánico es un mecanismo ya que solamente se encarga de transmitir

El Robot Institute of America define a un robot industrial como “manipulador programa-

2.2. Grados de libertad de un mecanismo

Un concepto muy importante en el estudio de mecanismos es el de grado de libertad

La movilidad es el número de grados de libertad del efector final de un robot, en otras

movimientos en el efector final, por lo que su movilidad es de dos, pero necesita un

Figura 2.2: Robot planar con dos grados de libertad.

Figura 2.3: Robot PUMA.

donde m es la movilidad, n el número de eslabones del mecanismo (incluyendo la base

2.3. Espacio de trabajo

Espacio de trabajo de orientación constante o traslación⇒ Todas las posibles ubi-

Espacio de trabajo de orientación⇒ Todas las posibles orientaciones que puede

Espacio de trabajo de orientación inclusiva⇒ Todas las ubicaciones que puede

Espacio de trabajo diestro⇒ Todas las ubicaciones de C que se pueden alcanzar

Espacio de trabajo de orientación total reducido⇒ Todas las ubicaciones de C que

Figura 2.4: Representación del espacio de trabajo de orientación [23].

2.4. Clasificación de robots manipuladores

2.4.1. Según sus grados de libertad

La primera forma de clasificar a los robots es mediante el número de grados de libertad

Figura 2.5: Robot SCARA [23].

2.4.2. Según su estructura cinemática

2.4.3. Según la geometría del espacio de trabajo

Se mencionó que el espacio de trabajo en robots seriales es el volúmen en el cual se puede

Figura 2.6: Plataforma Stewart-Gough [29].

2.5. Arquitectura y configuración