Tesis: IMPLEMENTACIÓN DE UN ALGORITMO PARA LA GENERACIÓN DE TRAYECTORIAS DE UN BRAZO ROBÓTICO DE 5 GDL UTILIZANDO CUATERNIONES

Universidad Nacional de Trujillo
Facultad de Ingeniería
Escuela Profesional de Ingeniería
Mecatrónica
IMPLEMENTACIÓN DE UN ALGORITMO PARA LA GENERACIÓN

DE TRAYECTORIAS DE UN BRAZO ROBÓTICO DE 5 GRADOS DE
LIBERTAD UTILIZANDO CUATERNIONES
TESIS
Para optar el Título Profesional de
INGENIERO MECATRÓNICO
Autor:
Barreto Vásquez, Maximino Víctor
Asesor:
Alva Alcántara, Josmell Henry
1
DEDICATORIA
Dedico este trabajo a mi familia.

A mis padres, por guiarme y orientarme en cada paso que doy. Papá, gracias por
encaminarme, por enseñarme valores, el trabajo honesto y a actuar pensando en
trascender. Mamá, gracias por ser mi amiga, confidente y consejera, por tu apoyo
incondicional. Gracias Víctor y Haydeé por su amor infinito, por estar conmigo en todos
mis logros y por aguardar mis caídas para levantarme y seguir andando.
A mi abuela Mercedes. Cómo ha pasado el tiempo querida abuela, te observo con ternura.
Eres una mujer admirable, luchadora y exitosa; reúnes tantas cualidades, me inspiro en ti
al forjar mi destino. Gracias por estar conmigo desde que era un niño, por todo el amor
que me has dado. Tengo en mi memoria tantos momentos felices junto a ti.
A mi querido abuelo Oscar. Papá Oscar, lamento que no estés conmigo en el inicio de mi
vida adulta. Te agradezco por todas las risas que me arrancaste, por mostrarme tu lado
más humano, por permitirme acompañarte en tu oficio y tus travesías. Recuerdo
alegremente los juegos que ideabas para mí, tu cantar, tu carisma. Te tengo presente a
cada momento.
A mi hermana Jackelyn. Hermana, somos compañeros de vida, hemos reído y llorado,

nos hemos peleado y reconciliado, y en cada vivencia hemos fortalecido el lazo que nos
une. Estaremos juntos en este camino.
A mis tíos, Antero, Optaciano, Irma, Ángel, Isabel y muy especialmente a Alberto, mi
segundo padre y maestro; Juan, mi entrañable amigo y Anselmo, mi compañero
inseparable.
A mis abuelos Maximino, el hombre más sabio que conozco, un ejemplo a seguir y
Graciela, tan alegre e inteligente, como pocas.
2
RESUMEN
Este trabajo presenta todos los desarrollos realizados para generar trayectorias en línea
recta para un brazo robótico de 5 grados de libertad diseñado en base a piezas pre-
fabricadas, obteniéndose un prototipo que emula las prestaciones de un manipulador
industrial y puede ser utilizado para fines didácticos.
La cinemática directa se realizó utilizando el algoritmo de Denavit Hartenberg y

utilizando cuaterniones duales, la cinemática inversa, utilizando los métodos de desacoplo
cinemático, geométrico y algebraico; además de conceptos vectoriales básicos. Los
cálculos cinemáticos fueron verificados utilizando Solidworks y Matlab, obteniendo
errores ínfimos a frecuencias mucho mayores que las recomendadas por la literatura
especializada
La generación de trayectorias se obtuvo utilizando cuaterniones, debido a la facilidad que

presenta para la manipulación de la orientación y la ventaja en cuanto a requerimientos
computacionales con respecto a otros métodos. Los errores de desviación fueron bastante
pequeños, con tiempos de cálculo en el orden de centésimas de segundo.
3
INDICE
CAPÍTULO I: INTRODUCCIÓN ................................................................................................ 1

1.1. Realidad problemática ...................................................................................................... 1
1.2. ANTECEDENTES ........................................................................................................... 1
1.2.1. KUKA ................................................................................................................. 1
1.2.2. UNIVERSAL ROBOTS ...................................................................................... 3
1.2.3. Tesis “DISEÑO Y CONSTRUCCIÓN DE UN BRAZO ROBOT DE 6
GRADOS DE LIBERTAD CON FINES EDUCATIVOS PARA
APLICACIONES EN NIVEL MEDIO SUPERIOR”. ........................................ 4
1.2.4. Tesis “DISEÑO DE UNA INTERFAZ DE USUARIO Y CONTROL
CINEMÁTICO DE UN BRAZO DE 6 GRADOS DE LIBERTAD PARA LA
PLANIFICACIÓN DE TRAYECTORIAS EN SOFTWARE MATLAB Y
SIMULINK” ........................................................................................................ 4
1.2.5. Tesis “METODOLOGÍA PARA GENERACIÓN DE TRAYECTORIAS DE
MANIPULADORES ROBÓTICOS, SU CINEMÁTICA Y DINÁMICA” ....... 4
1.2.6. Tesis “Control de posición/fuerza para manipuladores rígidos basado en
cuaterniones unitarios” ........................................................................................ 5
1.3. Marco Teórico .................................................................................................................. 6
1.3.1. Robots manipuladores: Definición y estructura. ................................................. 6
1.3.2. Descripciones espaciales y transformaciones ...................................................... 8
1.3.3. Cuaterniones ...................................................................................................... 15
1.3.4. Cinemática directa ............................................................................................. 19
1.3.5. Cinemática Inversa ............................................................................................ 21
1.3.6. Ecuaciones de decisión y verificación de modelos cinemáticos ....................... 32
1.3.7. Generación de trayectorias ................................................................................ 35
1.4. Formulación del Problema ............................................................................................. 48
1.5. Justificación del Estudio:................................................................................................ 48
1.6. Hipótesis ......................................................................................................................... 48
1.7. Objetivos ........................................................................................................................ 49
1.7.1. Objetivo General ............................................................................................... 49
1.7.2. Objetivos Específicos ........................................................................................ 49
CAPÍTULO II: MÉTODO .......................................................................................................... 50
2.1. Diseño de investigación: ................................................................................................ 50
2.2. Variables......................................................................................................................... 50
2.2.1. Variables independientes. .................................................................................. 50
2.2.2. Variables dependientes ...................................................................................... 50
2.3. Población y Muestra ....................................................................................................... 50
4
2.3.1. Población: .......................................................................................................... 50
2.3.2. Muestra: ............................................................................................................. 50
2.4. Técnicas e Instrumentos ................................................................................................. 50
CAPÍTULO III: DESARROLLO Y RESULTADOS ................................................................. 51
3.1. ESTRUCTURA MECÁNICA DEL ROBOT MANIPULADOR .................................. 51
3.2. CINEMÁTICA DIRECTA............................................................................................. 52
3.2.1. Asignación de tramas a las articulaciones del robot y obtención de parámetros
de Denavit-Hatenberg ........................................................................................ 52
3.2.2. Obtención de Matrices de Transformación Homogénea y cinemática directa. . 53
3.2.3. Cinemática directa utilizando cuaterniones ....................................................... 54
3.3. CINEMÁTICA INVERSA DEL MANIPULADOR ..................................................... 55
3.3.1. DESACOPLO CINEMÁTICO.......................................................................... 56
3.3.2. OBTENCIÓN DE LAS TRES PRIMERAS VARIABLES DE
ARTICULACIÓN. ............................................................................................ 62
3.3.3. CÁLCULO DE Q4 Y Q5 UTILIZANDO EL MÉTODO ALGEBRÁICO ...... 68
3.3.4. PROGRAMACIÓN EN MATLAB Y TIEMPO DE CÁLCULO ..................... 69
3.4. VERIFICACIÓN DE LOS ALGORITMOS DE CINEMÁTICA DIRECTA E
INVERSA. .............................................................................................................................. 69
3.4.1. Ecuaciones de decisión para los indicadores de configuración del brazo ......... 70
3.4.2. Calculo de errores e implementación del algoritmo de verificación ................. 71
3.5. GENERACIÓN DE TRAYECTORIA LINEAL USANDO CUATERNIONES .......... 72
3.6. VERIFICACIÓN DE ERRORES MÁXIMOS .............................................................. 80
3.7. AHORRO COMPUTACIONAL Y TIEMPO DE CALCULO ...................................... 82
3.7.1. CUANTIFICACIÓN OPERACIONAL ............................................................ 82
3.7.2. TIEMPO DE CÁLCULO .................................................................................. 84
CAPÍTULO IV: DISCUSIÓN DE RESULTADOS. .................................................................. 85
4.1. ESTRUCTURA DEL BRAZO ROBÓTICO ................................................................. 85
4.2. CINEMÁTICA DIRECTA............................................................................................. 85
4.3. CINEMÁTICA INVERSA............................................................................................. 85
4.4. GENERACIÓN DE TRAYECTORIAS ........................................................................ 86
CAPITULO V: CONCLUSIONES ............................................................................................. 88
CAPITULO VI: REFERENCIAS BIBLIOGRÁFICAS ............................................................. 89
ANEXOS..................................................................................................................................... 91
A. CODIGOS EN MATLAB .............................................................................................. 91
A.1 FUNCIONES PARA IMPLEMENTAR OPERACIONES BÁSICAS
USANDO CUATERNIONES. .......................................................................... 91
A.2 CINEMÁTICA DIRECTA USANDO MTH ................................................................. 93
5
A.3 CINEMÁTICA DIRECTA USANDO CUATERNIONES ........................................... 94
A.4 COMPARACIÓN DE ALGORITMOS DE CINEMÁTICA DIRECTA USANDO
CUATERNIONES Y MTH .................................................................................................... 95
A.5 TIEMPOS DE CÁLCULO PARA ALGORITMOS DE CINEMÁTICA DIRECTA ... 96
A.6 CINEMÁTICA INVERSA............................................................................................. 97
A.7 VERIFICACIÓN DE CINEMÁTICA INVERSA Y DIRECTA ................................. 101
A.8 GENERACIÓN DE TRAYECTORIAS ...................................................................... 103
6
CAPÍTULO I: INTRODUCCIÓN
1.1. Realidad problemática
En los últimos años, los desarrollos llevados a cabo en el área de la Robótica, son cada
vez más impresionantes en todas las áreas en las que ésta se divide, podemos dar cuenta
de robots humanoides que son capaces de caminar por una cuerda, de trasladar objetos
pesados, incluso de manejar una bicicleta. La robótica, conjugada con la visión e
inteligencia artificial, el control y otras ciencias involucradas, seguirán sorprendiéndonos
con el paso del tiempo.
La robótica está cada vez más cerca de nosotros, en el ámbito industrial está siendo de
mucha utilidad. Un ejemplo de ello es la inclusión de Brazos Robóticos o Robots
Manipuladores en las plantas industriales, que realizan trabajos que pueden ser
peligrosos, engorrosos y cansados para el ser humano, como el caso del transporte de
objetos pesados, manipulación de sustancias peligrosas y desarrollo de labores repetitivas.
Actualmente este tipo de Robots se han convertido en la parte más importante de los
sistemas de Paletizado, convirtiéndolos en sistemas más eficientes que permiten a las
empresas registrar cifras de ahorro significativas. En el Perú, hoy en día, ya muchas
empresas cuentan con esta tecnología.
Por ello, es importante que incursionemos en esta área y desarrollemos el marco teórico
que nos permita entender el fundamento que está detrás del funcionamiento de los
mencionados Robots manipuladores y estar preparados para tratar con ellos en nuestra
vida profesional.
1.2. ANTECEDENTES
La Robótica es un área de mucho interés en el entorno científico y tecnológico, sobre todo

al descubrirse el potencial que tenía en la industria, lo cual ha sido motivo de muchos
desarrollos ya disponibles en el mercado y muchas publicaciones científicas e
investigaciones. A continuación detallamos una serie antecedentes, empezando por el
ámbito mundial, hasta llegar a los avances, aportes o investigaciones realizadas en nuestra
región.
1.2.1. KUKA
“KUKA AG es una empresa presente a nivel internacional con un volumen de ventas de

3 mil millones de euros aproximadamente. Los cerca de 12.300 trabajadores de KUKA
se encargan diariamente de que sigamos siendo uno de los líderes globales en soluciones
1
de automatización inteligentes, tanto en la sede de Augsburgo como en el resto del
mundo. Nuestros clientes internacionales proceden, entre otras, de las industrias
automovilística y general. En KUKA somos proveedores totales: ponemos a su
disposición tanto componentes individuales como instalaciones completamente
automatizadas” [1].
Robots industriales de KUKA
En su página web [2], KUKA, nos menciona sus Manipuladores:
“KUKA le ofrece una gama muy amplia de robots industriales. En KUKA encontrará el
robot adecuado sin importar la exigencia de la tarea:
• Robots de seis ejes en todas las dimensiones, con distintas capacidades de carga,
de alcances y en las más diversas variantes
• Robots ligeros preparados para el trabajo con humanos, para una directa
colaboración entre personas y máquinas
• Robots resistentes al calor y a la suciedad para entornos especialmente adversos
• Robots industriales para salas blancas, para cumplir con las máximas exigencias
de higiene
• Robots pequeños con equipamiento Waterproof, adecuados para su uso en

máquinas-herramienta
• Robots pequeños equipados como "Hygienic Machine (HM)" para el contacto

directo con alimentos y sustancias farmacéuticas
• Concatenadores de prensas con un alcance enorme para cargar y descargar piezas

grandes en concatenaciones de prensas
• Robots de paletizado para tareas de manipulación de cualquier tipo
• Robots de soldadura orientados a la precisión y la máxima movilidad
• Robots de consola en todas las variantes
• Robots de alta precisión para una precisión máxima”
2
Fig. 1.1 Gama de Robots Industriales KUKA
(Fuente: https://www.kuka.com/es-es/productos-servicios/sistemas-de-robot/robot-
industrial)
1.2.2. UNIVERSAL ROBOTS
“Lo automatizamos prácticamente todo” [3], nos dice Universal Robots en la primera
imagen que podemos observar en su página web.
“Cuando decimos que los robots de UR pueden automatizarlo prácticamente todo,

queremos decir prácticamente todo: desde el montaje hasta la pintura, el atornillado y el
etiquetado, pasando por el proceso de inyección en molde y soldadura, hasta el
empaquetado y el pulido” [3], se menciona a continuación
“Los brazos robóticos de Universal Robots se integran fácilmente en los entornos de

producción existentes. Con seis ejes y una gran flexibilidad, se han diseñado para imitar
el rango de movimiento del brazo humano”. [4]
Fig. 1.2 Robots industriales de Universal Robots

(Fuente: https://www.universal-robots.com/es/productos/)
3
1.2.3. Tesis “DISEÑO Y CONSTRUCCIÓN DE UN BRAZO ROBOT DE 6
GRADOS DE LIBERTAD CON FINES EDUCATIVOS PARA APLICACIONES
EN NIVEL MEDIO SUPERIOR”.
“Este trabajo presenta el diseño y desarrollo de un brazo robot de 6 grados de libertad que será
utilizado para difundir de una manera más abierta y fácil la cultura de la robótica en las aulas de
los jóvenes estudiante que se interesan por la ingeniería, teniendo como bases la programación
y la electrónica para ayudarnos a comprender el rol tan importante de la automatización de los
procesos cotidianos en nuestros días y que en algún momento se aplicarán en los medios de
transporte así como en nuestro hogar para orientarlos al mejoramiento de la calidad de vida del
ser humano.”
1.2.4. Tesis “DISEÑO DE UNA INTERFAZ DE USUARIO Y CONTROL

CINEMÁTICO DE UN BRAZO DE 6 GRADOS DE LIBERTAD PARA LA
PLANIFICACIÓN DE TRAYECTORIAS EN SOFTWARE MATLAB Y
SIMULINK”
“En este proyecto de tesis se diseñó una interfaz de usuario con cinco prácticas de cálculo
cinemático para determinar los desplazamientos que debe tener un brazo robótico mediante
interfaces gráficas de usuario realizadas en Matlab. Los movimientos del brazo robótico, la
orientación, se basa en un conjunto de parámetros que se hallan por los ángulos de las
articulaciones y la posición en el espacio, utilizando sistemas de referencia definidos. Estas
trayectorias se generan como secuencia de configuraciones aproximadas, y el análisis del cálculo
respectivo es complejo, tomando en cuenta esto, surge la necesidad de crear las interfaces como
herramienta para el estudiante, con un brazo robótico de 6 grados de libertad y así crear otro
método de enseñanza para mejorar el aprendizaje de los estudiantes en la materia de robótica,
practicando la relación directa entre la robótica y las matemáticas”.
1.2.5. Tesis “METODOLOGÍA PARA GENERACIÓN DE TRAYECTORIAS DE

MANIPULADORES ROBÓTICOS, SU CINEMÁTICA Y DINÁMICA”
“En este trabajo se presenta una metodología que permite obtener los parámetros
necesarios para la generación de trayectorias de manipuladores robóticos fuera de línea,
como son: los vectores de posición angular que resulten en la configuración espacial del
manipulador que se requiere durante la trayectoria deseada, y los vectores de fuerza o
torque necesarios para que se cumplan las condiciones dinámicas de movimiento.
4
Partiendo de un análisis cinemático y dinámico del manipulador y de la tarea a
desempeñar por este. Desarrollando también las herramientas de computo necesarias para
este fin.”
1.2.6. Tesis “Control de posición/fuerza para manipuladores rígidos basado en
cuaterniones unitarios”
“En esta tesis se presenta un esquema controlador-observador de posición-fuerza usando

cuaterniones unitarios, la teoría desarrollada fue probada en el Robot A465 de CRS
Robotics de seis grados de libertad que se encuentra ubicado en el laboratorio de robótica
del Edificio de Posgrado de la Facultad de Ingeniería de la Universidad Nacional
Autónoma de México (UNAM), el cual no cuenta con transductores de velocidad; pero
si con sensores de posición y fuerza. El robot realiza tareas en movimiento restringido,
consisten en el seguimiento de trayectorias deseadas tanto de posición como de fuerza
sobre dos tipos de superficies: plana y esférica.”
5
1.3. Marco Teórico
1.3.1. Robots manipuladores: Definición y estructura.
1.3.1.1. Definición
Buena parte de las definiciones y clasificaciones de robots existentes responde al robot

ampliamente utilizado hasta la fecha, el robot industrial, robot de producción o robot
manipulador. Frente a esto, están también los robots especiales, también denominados
robots de servicio (Barrientos 1997), que en los últimos años están alcanzando un nivel
de desarrollo importante, sin embargo el término robot generalmente hace referencia a
los robots industriales.
Fu, Gonzales y Lee (1988) consideran que la definición más precisa de los robots
industriales, es la dada por el Robot Institute of America: “un robot es un manipulador
reprogramable multifuncional diseñado para mover materiales, piezas o dispositivos
especializados, a través de movimientos programados variables para la realización de una
diversidad de tareas”.
Saha (2010, p. 5), sin embargo, prefiere la definición brindada por la Organización
Internacional para la Estandarización (ISO), que adopta la definición anterior, con una
ligera modificación y nos dice: Un robot es un manipulador multifuncional
reprogramable, capaz de mover materiales, piezas, herramientas o dispositivos especiales,
a través de movimientos variables programados, para el desempeño de tareas diversas.
“Existen otras definiciones dadas por otras asociaciones, como por ejemplo, el Robotics
Institute of America (RIA), la Japan Industrial Robot Association (JIRA), la British Robot
Association (BRA) y otras. Todas ellas coinciden en dos puntos: la capacidad de
reprogramación y la multifuncionalidad de los robots”, concluye Saha (2010, p. 5)
Para Barrientos (1997), debido a que las anteriores no contemplan el hecho de que el
robot tenga varios grados de libertad, la definición más completa es la brindada por la
Asociación Francesa de Normalización (AFNOR), que define primero el manipulador y
luego, basándose en dicha definición, define el robot:
• Manipulador: mecanismo formado generalmente por los elementos en serie,

articulados entre sí, destinado al agarre y desplazamiento de objetos. Es
multifuncional y puede ser gobernado directamente por un operador humano o
mediante dispositivo lógico.
6
• Robot: es un manipulador automático servocontrolado, reprogramable, polivalente,
capaz de posicionar y orientar piezas, útiles o dispositivos especiales, siguiendo
trayectorias variables reprogramables, para la ejecución de tareas variadas.
Normalmente tiene la forma de uno o varios brazos terminados en una muñeca. Su
unidad de control incluye un dispositivo de memoria y ocasionalmente de
precepción del entorno. Normalmente su uso es el de realizar una tarea de manera
cíclica, pudiéndose adaptar a otra sin cambios permanentes en su material.
1.3.1.2. Estructura mecánica de un robot manipulador
“Un robot está formado por una serie de elementos o eslabones unidos mediante
articulaciones que permiten un movimiento relativo entre cada dos eslabones
consecutivos. La constitución física de la mayor parte de los robots industriales guarda
cierta similitud con la anatomía del brazo humano, por lo que en ocasiones, para hacer
referencia a los distintos elementos que componen el robot, se usan términos como
cuerpo, brazo, codo y muñeca”, menciona Barrientos (1997, p. 16)
Barrientos (1997) menciona también que el movimiento de cada articulación puede ser
de desplazamiento, de giro o una combinación de ambos. Dando esto como resultado 6
tipos de articulaciones que se muestran en la Figura 1.3, sin embargo, en la práctica, en
los robots solo se emplean la de rotación y la prismática.
Fig. 1.3 Distintos tipos de articulaciones para robots

(Fuente: Barrientos (1997, p. 17))
7
La Figura 1.4 nos muestra un Robot Manipulador de 6 grados de libertad, cada
articulación representa un grado de libertad y todas son de tipo giratorio. Esta es una
configuración típica, y la más usada para el diseño de manipuladores en la industria.
Fig. 1.4 Robot Manipulador de 6 Grados de Libertad.

(Fuente: Saha (2010, p. 5))
1.3.2. Descripciones espaciales y transformaciones

1.3.2.1. Definición
Una descripción se utiliza para especificar los atributos de varios objetos con los que
trata un sistema de manipulación.
A) Descripción de una posición
Una vez que se establece un sistema de coordenadas, podemos ubicar cualquier punto
en el universo con un vector de posición de orden 3 × 1.
Como es común que definamos muchos sistemas de coordenadas, además del sistema
de coordenadas universal, los vectores deben etiquetarse con información que
identifique en cuál sistema están definidos; por ejemplo, 𝐴𝑃. Esto significa que los
componentes de 𝐴𝑃 tienen valores numéricos que indican distancias sobre los ejes del
8
sistema de coordenadas {A}, como se ve en la Fig. 1.5. Dados los subíndices 𝑥, 𝑦 y 𝑧,
los elementos individuales de un vector son:
𝑝𝑥
𝐴
𝑃 = [𝑝𝑦 ] (1.1)
𝑝𝑧
Fig. 1.5 Vector Relativo a una trama

(Fuente: Craig (2006 p. 20))
B) Descripción de una Orientación

Para describir la orientación de un cuerpo, adjuntaremos un sistema de coordenadas
al cuerpo y luego daremos una descripción de este sistema de coordenadas relativo al
sistema de referencia. En la Figura 1.6 se ha adjuntado el sistema de coordenadas {B}
al cuerpo de una manera conocida. Ahora basta con una descripción de {B} relativo a
{A} para dar la orientación del cuerpo.
Fig. 1.6 Ubicación de un objeto en posición y orientación

(Fuente: Craig (2006, p. 21))
Por ende, las posiciones de los puntos se describen con vectores, y las orientaciones
de los cuerpos con un sistema de coordenadas adjunto.
9
Para denotar los vectores unitarios proporcionamos las direcciones principales del
sistema de coordenadas {B} como 𝑋⃗𝐵 , 𝑌
⃗⃗𝐵 , 𝑍⃗𝐵 . Al escribirse en términos del sistema de
coordenadas {A}, se llaman 𝐴𝑋⃗𝐵 , 𝐴𝑌

⃗⃗𝐵 y 𝐴𝑍⃗𝐵 . Es conveniente si apilamos estos tres
vectores unitarios como columnas de una matriz de 3𝑥3, en el orden 𝐴𝑋⃗𝐵 , 𝐴𝑌

⃗⃗𝐵 , 𝐴𝑍⃗𝐵 . A
esta matriz la llamaremos matriz de rotación y, dado que esta matriz de rotación
específica describe a {B} en forma relativa a {A}, la representamos con la notación 𝐵𝐴𝑅 :
𝑟11 𝑟12 𝑟13

𝐴
𝐵𝑅 =[ 𝐴
𝑋⃗𝐵 𝐴𝑌
⃗⃗𝐵 𝐴𝑍⃗𝐵 ] = [𝑟21 𝑟22 𝑟23 ]. (1.2)
𝑟31 𝑟32 𝑟33
En consecuencia, siempre que se represente la posición de un punto con un vector, la
orientación de un cuerpo se representará con una matriz
Podemos ofrecer expresiones para los escalares 𝑟𝑖𝑗 en la ecuación (1.2) teniendo en
cuenta que los componentes de cualquier vector son simplemente las proyecciones de
ese vector en las direcciones unitarias de su trama de referencia. Por lo tanto, cada
componente puede escribirse como el producto punto de un par de vectores unitarios:
𝑋⃗𝐵 . 𝑋⃗𝐴 ⃗⃗𝐵 . 𝑋⃗𝐴
𝑌 𝑍⃗𝐵 . 𝑋⃗𝐴
𝐴 𝐴 ⃗ 𝐴 ⃗⃗ 𝐴 ⃗ ⃗𝐵 . 𝑌⃗⃗𝐴 ⃗⃗𝐵 . 𝑌
⃗⃗𝐴
𝐵𝑅 = [ 𝑋𝐵 𝑌𝐵 𝑍𝐵 ] = [ 𝑋 𝑌 𝑍⃗𝐵 . 𝑌⃗⃗𝐴 ]. (1.3)
𝑋⃗𝐵 . 𝑍⃗𝐴 ⃗⃗𝐵 . 𝑍⃗𝐴
𝑌 𝑍⃗𝐵 . 𝑍⃗𝐴
Esta perspectiva de una matriz de rotación es importante debido a que nos permite
calcular de manera genérica la orientación de cualquier trama respecto de otra, sin
necesidad de usar ningún algoritmo o método práctico que a veces, aún que muy
pocas, pueden fallar.
Una inspección más detallada de la ecuación (1.3), en cuanto a los productos vectoriales,
nos muestra que las filas de la matriz son los vectores unitarios de {A} expresados en
{B}; es decir,
𝑇
𝐵
𝑋⃗𝐴
𝑇
𝐴
𝐵𝑅 = [ 𝐴𝑋⃗𝐵 𝐴𝑌
⃗⃗𝐵 𝐴𝑍⃗𝐵 ] = 𝐵 ⃗⃗
𝑌𝐴 . (1.4)
𝑇
[ 𝐵𝑍⃗𝐴 ]
Así, 𝐵𝐴𝑅 , la descripción de la trama {A} relativa a {B}, se da mediante la transpuesta de

(1.3); esto es,
𝐴
𝐵𝑅 = 𝐵𝐴𝑅 𝑇 . (1.5)
10
Esto sugiere que la inversa de una matriz de rotación es igual a su transpuesta; lo cual
puede verificarse fácilmente de la siguiente manera:
𝑇
𝐵
𝑋⃗𝐴
𝑇
𝐵 𝑇 𝐴
𝐴𝑅 𝐵 𝑅 = 𝐵 ⃗⃗
𝑌𝐴 [ 𝐴𝑋⃗𝐵 𝐴𝑌
⃗⃗𝐵 𝐴𝑍⃗𝐵 ] = 𝐼3 . (1.6)
𝑇
[ 𝐵𝑍⃗𝐴 ]
Donde 𝐼3 es la matriz identidad 3𝑥3. Por lo que:

𝐴
𝐵𝑅 = 𝐵𝐴𝑅 −1 = 𝐵𝐴𝑅 𝑇 (1.7)
En efecto, mediante el álgebra lineal, sabemos que el inverso de una matriz con
columnas ortonormales es igual a su transpuesta.
C) Descripción de una trama

La información necesaria para especificar completamente en donde se encuentra la
mano del manipulador de la Figura 1.6 es una posición y una orientación. Por
conveniencia, el punto cuya posición describiremos se elige como el origen de la trama
adjunta al cuerpo.
La situación de un par posición-orientación surge tan a menudo en robótica que
definimos una entidad llamada trama, la cual es un conjunto de cuatro vectores que
proporcionan información sobre la posición y orientación. Por ejemplo, en la Figura 1.6
un vector ubica la posición de la punta de los dedos y tres más describen su orientación.
De manera equivalente, la descripción de una trama puede definirse como un vector
posición y una matriz de rotación. Observe que una trama es un sistema de coordenadas
en donde además de la orientación damos un vector de posición que ubica su origen de
manera relativa a alguna otra trama fija. Por ejemplo, la trama {B} se describe mediante
𝐴
𝐵𝑅 y 𝐴𝑃𝐵𝑂𝑅𝐺 , en donde 𝐴𝑃𝐵𝑂𝑅𝐺 es el vector que ubica el origen de la trama {B}:
{𝐵} = { 𝐵𝐴𝑅 , 𝐴𝑃𝐵𝑂𝑅𝐺 } (1.8)
11
Fig. 1.7 Ejemplo de varias tramas.
(Fuente: Craig (2006, p. 23))
1.3.2.2. Asignaciones: Como cambiar posiciones de trama a trama
En este apartado se trata el problema de cómo expresar la misma cantidad en diferentes

sistemas de coordenadas de referencia. Siendo más específicos, cómo expresar un vector
o un ente que está expresado respecto de un sistema de referencia, respecto de otro
distinto. Para esto se utilizan las llamadas Matrices de Transformación Homogénea.
Fig. 1.8 Transformada general de un vector.

(Fuente: Craig (2006. p. 27))
Las Matrices de Transformación Homogénea se utilizan como operadores. Por ejemplo

si deseamos expresar el vector 𝐵𝑃, respecto del sistema de la trama {𝐴}, realizamos la
siguiente operación
12
𝐴 𝐴 𝐵
𝑃= 𝐵𝑇 𝑃 . (1.9)
Es importante también darnos cuenta que la matriz 𝐵𝐴𝑇 representa la relación que existe
entre la trama {𝐵} y la trama {𝐴}; es decir, expresa la trama {𝑩} respecto de la trama
{𝑨}.
La matriz de transformación homogénea, en adelante MTH, se construye de la siguiente

manera:
(1.10)
1.3.2.3. Aritmética de transformaciones

En la Figura 1.9, la trama {𝐶} se conoce en términos de la trama {𝐵}, y la trama {𝐵} se
conoce en términos de la trama {𝐴}, además conocemos 𝐶𝑃. Deseamos encontrar 𝐴𝑃 .
Craig (2006, p. 35), demuestra que:
Conocidos 𝐵𝐴𝑇 y 𝐵𝐶𝑇 calculamos sencillamente la matriz que describe {𝐶} en relación a la
trama {𝐴}, con una simple multiplicación de matrices:
𝐴
𝐶𝑇 = 𝐵𝐴𝑇 𝐵𝐶𝑇 (1.11)
Con ello calculamos 𝐴𝑃 de la siguiente manera:
𝐴 𝐴 𝐶
𝑃= 𝐶𝑇 𝑃 (1.12)
Fig. 1.9 Tramas compuestas: cada una se conoce de forma relativa a la anterior
(Fuente: Craig (2006. p. 35))
13
Invertir una transformada
Si conocemos 𝐴
𝑃 y la matriz 𝐵𝐴𝑇 y deseamos conocer 𝐵𝑃 , necesitamos calcular la matriz
𝐵 𝐴
𝐴𝑇 , a la cual llamaremos matriz inversa de 𝐵𝑇 . Craig (2006, p. 36), demuestra lo siguiente:
𝐵
𝐴𝑇 = 𝐵𝐴𝑇 −1 (1.13)
−1
Donde, el operador representa la operación que nos permite calcular la inversa de
cualquier matriz.
1.3.2.4. Resumen de interpretaciones
Las matrices de transformación homogénea son la herramienta principal en los

desarrollos realizados en robótica; por ello es importante comprender a cabalidad todas
las interpretaciones que se les puede dar y así aprovecharlas en todos los contextos en los
que se las puede utilizar. Craig (2006) resume las tres interpretaciones que se les da a las
MTH de la siguiente manera:
1. Es la descripción de una trama. 𝐵𝐴𝑇 describe la trama {𝐵} en relación con la trama
𝐴
{𝐴}. Específicamente, las columnas 𝐵𝑅 son vectores unitarios que definen las
direcciones de los ejes principales de {𝐵} y 𝐵𝐴𝑃𝐵𝑂𝑅𝐺 ubica la posición del origen de
{𝐵}, ambos respecto de {𝐴}.
2. Es la asignación de una transformada. 𝐵𝐴𝑇 asigna 𝐵𝑃 → 𝐴𝑃.
3. Es un operador de transformación. 𝑇 opera en 𝐴𝑃1 para crear 𝐴𝑃2 .
1.3.2.5. Cinemática de Manipuladores
Barrientos (1997), nos brinda los conceptos necesarios para entender fácilmente este
apartado.
La cinemática del robot manipulador, estudia el movimiento del mismo con respecto a un
sistema de referencia. Así, la cinemática se interesa por la descripción analítica del
movimiento espacial del robot como una función del tiempo, y en particular por las
relaciones entre la posición y orientación del extremo final del robot con los valores que
toman sus coordenadas articulares.
14
Existen dos problemas fundamentales a resolver en la cinemática del robot (Fig. 1.10); el
primero de ellos se conoce como el problema cinemático directo, y consiste en
determinar cuál es la posición y orientación del extremo final del robot, con respecto a un
sistema de coordenadas que se toma como referencia, conocidos los valores de las
articulaciones y los parámetros geométricos de los elementos del robot; el segundo,
denominado problema cinemático inverso, resuelve la configuración que debe adoptar
el robot para una posición y orientación del extremo conocidas.
Fig. 1.10 Diagrama de relación entre cinemática directa e inversa

(Fuente: Barrientos (1997. p. 94))
1.3.3. Cuaterniones
En, tal vez, “el más famoso acto de vandalismo matemático”, el 16 de octubre de 1843,
Sir William Rowan Hamilton escribió la siguiente ecuación en el Puente Brougham,
(ahora Puente Broom) en Dublín:
𝑖 2 = 𝑗 2 = 𝑘 2 = 𝑖𝑗𝑘 = −1. (1.14)
La formulación de estas leyes fue un acto decisivo en la historia del álgebra.
Los cuaterniones son un concepto que generaliza el concepto de números complejos, se
podría decir también que son una extensión de los números reales. Los números
complejos se generan con la adición de la unidad imaginaria 𝑖, mientras que los
cuaterniones se generan con la adición de tres unidades imaginarias (𝑖, 𝑗, 𝑘).
El concepto de cuaternión ha sido aplicado satisfactoriamente en el análisis de
mecanismos espaciales durante las últimas décadas. Utilizaremos los cuaterniones para
facilitar la representación de la orientación de la mano del manipulador para planificar
una trayectoria en línea recta.
De acuerdo a la ecuación 1.51, se puede particularizar:
𝒊𝒋 = 𝒌 𝒋𝒌 = 𝒊 𝒌𝒊 = 𝒋
𝒋𝒊 = −𝒌 𝒌𝒋 = −𝒊 𝒊𝒌 = −𝒋 (1.15)
15
Las unidades 𝒊, 𝒋, 𝒌 del cuaternión se pueden interpretar como los tres vectores básicos
de un conjunto de ejes cartesianos. Así, un cuaternión 𝑸 se puede escribir como una parte
escalar 𝑠 y una parte vectorial 𝒗.
𝑄 = [𝑠 + 𝒗] = 𝑠 + 𝑎𝒊 + 𝑏𝒋 + 𝑐𝒌 = (𝑠, 𝑎, 𝑏, 𝑐) (1.16)
Las siguientes propiedades del álgebra de cuaterniones son básicas:

Parte escalar de 𝑄: 𝑠
Parte vectorial de 𝑄: 𝑎𝒊 + 𝑏𝒋 + 𝑐𝒌
Conjugado de 𝑄: 𝑠 − (𝑎𝒊 + 𝑏𝒋 + 𝑐𝒌)
Normal de 𝑄: 𝑠 2 + 𝑎2 + 𝑏 2 + 𝑐 2
𝑠−(𝑎𝒊+𝑏𝒋+𝑐𝒌)
Recíproco de 𝑄: 𝑠2 +𝑎2 +𝑏2 +𝑐 2
Cuaternión unidad: 𝑠 − (𝑎𝒊 + 𝑏𝒋 + 𝑐𝒌), donde 𝑠 2 + 𝑎2 + 𝑏 2 + 𝑐 2 = 1
Es importante observar que los cuaterniones incluyen a los números reales (𝑠, 0,0,0); a
los números complejos (s,a,0,0) y los vectores (0,a,b,c)
Aritmética básica de cuaterniones

Sean los cuaterniones
𝑞 = 𝑞 0 + 𝑞 1𝒊 + 𝑞 2𝒋 + 𝑞 3𝒌
y
𝑟 = 𝑟 0 + 𝑟 1𝒊 + 𝑟 2𝒋 + 𝑟 3𝒌
La adición se realiza análogamente a como se hace con los números complejos, es decir,
término a término:
𝑞 + 𝑟 = (𝑞0 + 𝑟0 ) + (𝑞1 + 𝑟1)𝒊 + (𝑞2 + 𝑟2)𝒋 + (𝑞3 + 𝑟3)𝒌 (1.17)
El producto tiene la forma de:

𝑛 = 𝑞 × 𝑟 = 𝑛0 + 𝑛1𝒊 + 𝑛2𝒋 + 𝑛3𝒌 (1.18)
Donde:
𝑛0 = (𝑟0𝑞0 − 𝑟1𝑞1 − 𝑟2𝑞2 − 𝑟3𝑞3)
𝑛1 = (𝑟0𝑞1 + 𝑟1𝑞0 − 𝑟2𝑞3 + 𝑟3𝑞2)
𝑛2 = (𝑟0𝑞2 + 𝑟1𝑞3 + 𝑟2𝑞0 − 𝑟3𝑞1)
𝑛3 = (𝑟0𝑞3 − 𝑟1𝑞2 + 𝑟2𝑞1 + 𝑟3𝑞0)
16
1.3.3.1. Los cuaterniones y las rotaciones.
Las rotaciones finitas en el espacio se pueden tratar de una manera simple y eficiente con
la ayuda del álgebra de cuaterniones.
1.3.3.1.1. Equivalencia entre Par de Rotación ( Representación eje-ángulo) y

cuaternion
Convertir par de rotación a cuaternion:

Podemos representar una rotación 𝑅𝑜𝑡(𝒏, 𝜃) de ángulo 𝜃 respecto de un eje 𝒏 mediante
un cuaternión
𝜃 𝜃
𝑅𝑜𝑡(𝒏, 𝜃) = [cos ( ) + sin ( ) 𝒏] (1.19)
2 2
Convertir cuaternion a par de rotación

El Par de rotación que se obtiene a partir del cuaternión 𝑞 = 𝑞0 + 𝑞1𝒊 + 𝑞2𝒋 + 𝑞3𝒌 es:
(𝑞1,𝑞2,𝑞3)
𝒏= (1.20)
√𝑞1 2 +𝑞2 2 +𝑞3 2
𝜃 = 2𝑎𝑡𝑎𝑛2(√𝑞1 2 + 𝑞2 2 + 𝑞3 2 , 𝑞0) (1.21)

La rotación nula estaría dada por:
𝑅𝑜𝑡(𝒏, 𝜃) = [1 + 0𝒏] = [1 + 𝟎] (1.22)
1.3.3.1.2. Equivalencia entre Matrices de rotación y cuaterniones
Convertir matriz de rotación a cuaternión

𝑟11 𝑟12 𝑟13
𝑟
Sea el cuaternión 𝑞 = 𝑞0 + 𝑞1𝒊 + 𝑞2𝒋 + 𝑞3𝒌 y la matriz de rotación 𝑅 = [ 21 𝑟22 𝑟23 ]
𝑟31 𝑟32 𝑟33
√𝑟11 +𝑟22 +𝑟33 +1
𝑞0 = 2
(1.23)
𝑠𝑖𝑔𝑛(𝑟32 −𝑟23 )√𝑟11 −𝑟22 −𝑟33 +1
𝑞1 = 2
(1.24)
𝑠𝑖𝑔𝑛(𝑟13 −𝑟31 )√𝑟22 −𝑟33 −𝑟11 +1
𝑞2 = 2
(1.25)
𝑠𝑖𝑔𝑛(𝑟21 −𝑟12 )√𝑟33 −𝑟11 −𝑟22 +1
𝑞3 = 2
(1.26)
Convertir cuaterniones a matrices de rotación

Sea el cuaternión 𝑞 = 𝑞0 + 𝑞1𝒊 + 𝑞2𝒋 + 𝑞3𝒌 y la matriz de rotación 𝑅
17
2(𝑞0 2 + 𝑞1 2 ) − 1 2(𝑞1 𝑞2 − 𝑞0 𝑞3 ) 2(𝑞1 𝑞3 + 𝑞0 𝑞2 )
𝑅 = [ 2(𝑞1 𝑞2 + 𝑞0 𝑞3 ) 2(𝑞0 2 + 𝑞2 2 ) − 1 2(𝑞2 𝑞3 − 𝑞0 𝑞1 ) ] (1.27)
2(𝑞1 𝑞3 − 𝑞0 𝑞2 ) 2(𝑞2 𝑞3 + 𝑞0 𝑞1 ) 2(𝑞0 2 + 𝑞3 2 ) − 1
Nótese que si una rotación 𝑅 es representada por un cuaternión Q, luego el cuaternión

correspondiente a 𝑅 −1 puede ser obtenido fácilmente negando la parte vectorial de 𝑄, es
decir calculando su conjugada. De manera similar, es fácil ver que si las rotaciones 𝑅1 y
𝑅2 son representadas por los cuaternios 𝑄1y 𝑄2 , respectivamente, luego, la rotación
resultante de 𝑅1 𝑅2 , será representada por el cuaternión resultante de 𝑄1 𝑄2.
Los requerimientos computacionales de algunas operaciones comunes que involucran
rotaciones, usando representaciones de cuaterniones y matrices, se dan en la Tabla 1.2.
Además de requerir generalmente menos operaciones, los cuaterniones (4 números)
tienen la ventaja de ser una representación menos redundante que las matrices de rotación
(9 números), simplificando así los problemas asociados con los errores de redondeo de la
máquina.
Tabla 1.2 Requerimientos computacionales para operaciones de rotación comunes
Operación Representación mediante Representación matricial

cuaterniones
𝑅1 𝑅2 9 sumas, 15 sumas,
16 multiplicaciones 24 multiplicaciones
𝑅 → (𝒏, 𝜃) 4 multiplicaciones, 8 sumas, 10 multiplicaciones
1 raíz cuadrada, 2 raíz cuadrada,
1 arco tangente 1 arco tangente
(𝒏, 𝜃) → 𝑅 4 multiplicaciones, 10 adiciones,
1 par seno-coseno 15 multiplicaciones,
1 par seno-coseno.
1.3.3.2. Cuaterniones duales
Los cuaternios unitarios pueden ser utilizados para representar orientaciones en el

espacio. Sin embargo, es necesario contar con un elemento compacto análogo a una
Matriz de Transformación Homogénea, que contenga tanto la orientación como la
posición de un cuerpo rígido. Es así que nos introducimos en el estudio de los
Cuaterniones Duales o Pares Cuaternion-Vector, que se definen de la siguiente manera:
18
𝑄(𝑞, 𝑝) = ([cos(𝜃⁄2) , sin(𝜃⁄2) < 𝑘𝑥 , 𝑘𝑦 , 𝑘𝑧 >], < 𝑝𝑥 , 𝑝𝑦 , 𝑝𝑧 >) (1.28)
Donde, el cuaternion 𝑞 representa la rotación y el vector 𝑝, el correspondiente

desplazamiento traslacional.
Para una articulación prismática, la formula anterior toma la siguiente forma:
𝑄(𝑞, 𝑝) = ([1, < 0,0,0 >], < 𝑝𝑥 , 𝑝𝑦 , 𝑝𝑧 >) (1.29)
La multiplicación de dos cuaterniones duales se realiza de la según:
𝑄1 𝑄2 = (𝑞1 , 𝑝1 ) ∗ (𝑞2 , 𝑝2 ) = 𝑞1 ∗ 𝑞2 , 𝑞1 ∗ 𝑝2 ∗ 𝑞1 −1 + 𝑝1 (1.30)
Donde, 𝑞1 ∗ 𝑝2 ∗ 𝑞1 −1 = 𝑝2 + 2𝑠1 (𝑣1 𝑥𝑝2 ) + 2𝑣1 𝑥(𝑣1 𝑥𝑝2 ).
Finalmente, el inverso de un cuaternion dual sería:
𝑄1 −1 = (𝑞1 −1 , −𝑞1 −1 ∗ 𝑝 ∗ 𝑞) (1.31)
Donde, −𝑞1 −1 ∗ 𝑝 ∗ 𝑞 = −𝑝 + [2𝑠(𝑣𝑥(−𝑝)) + 2𝑣𝑥(𝑣𝑥(−𝑝))]
1.3.4. Cinemática directa
Dado que un robot se puede considerar como una cadena cinemática formada por objetos
rígidos o eslabones unidos entre sí mediante articulaciones, se puede establecer un
sistema de referencia fijo situado en la base del robot y describir la localización de cada
uno de los eslabones con respecto a dicho sistema de referencia. De esta forma, el
problema cinemático directo se reduce a encontrar una matriz de transformación
homogénea T que relacione la posición y orientación del extremo del robot respecto del
sistema de referencia fijo situado en la base del mismo. Ésta matriz será función de las
coordenadas articulares.
En general, un robot de n grados de libertad está formado por n eslabones unidos por n
articulaciones, de forma que cada par articulación-eslabón constituye un grado de
libertad. A cada eslabón se le puede asociar un sistema de coordenadas solidario a él y,
utilizando las matrices de transformación homogénea, es posible representar las
rotaciones y traslaciones relativas entre los distintos eslabones que componen el robot.
Normalmente, la MTH que representa la posición y orientación relativa entre los sistemas
asociados a dos eslabones consecutivos del robot se suele denominar matriz 𝑖−1𝐴𝑖 , según
𝑖−1
la notación utilizada por Barrientos (1997), o 𝑖𝑇, según la notación utilizada por Craig
19
(2006). Como lo venimos haciendo, en la presente investigación utilizamos la segunda
notación.
Los ejes de coordenadas de cada eslabón se asignan según el Algoritmo de Denavit-

Hartenberg, expuesto en Barrientos (1997, p. 97), con ello se obtienen los parámetros de
Denavit-Hartenberg, en adelante parámetros D-H, para luego con ellos, construir las
MTH que definan la estructura del robot. Es importante tener en cuenta un detalle
importante, los ejes de cada sistema de coordenadas deben ser dispuestos de tal manera
que éste sea dextrógiro.
Parámetros D-H.
Barrientos (1997, p. 99), nos dice:
Los cuatro parámetros D-H, dependen únicamente de las características geométricas de

cada eslabón y de las articulaciones que le unen con el anterior y siguiente. En concreto,
estos representan (Fig. 1.11):
𝜃𝑖 : Es el ángulo que forman los ejes 𝑋𝑖−1 y 𝑋𝑖 medido en un plano perpendicular al

eje 𝑍𝑖−1 , utilizando la regla de la mano derecha. Se trata de un parámetro variable en
articulaciones giratorias.
𝑑𝑖 : Es la distancia a lo largo del eje 𝑍𝑖−1 desde el origen del sistema de coordenadas
(𝑖 − 1) − é𝑠𝑖𝑚𝑜 hasta la intersección del eje 𝑍𝑖−1 con el eje 𝑋𝑖 . Se trata de un
parámetro variable en articulaciones prismáticas.
𝑎𝑖 : Es la distancia a lo largo del eje 𝑋𝑖 que va desde la intersección del eje 𝑍𝑖−1 con
el eje 𝑋𝑖 hasta el origen del sistema 𝑖 − é𝑠𝑖𝑚𝑜, en el caso de articulaciones
giratorias. En el caso de articulaciones prismáticas, se calcula como la distancia más
corta entre los ejes 𝑍𝑖−1 y 𝑍𝑖 .
∝𝑖 : Es el ángulo de separación del eje 𝑍𝑖−1 y el eje 𝑍𝑖 , medido en un plano
perpendicular al eje 𝑋𝑖 , utilizando la regla de la mano derecha.
Una vez obtenidos los parámetros D-H, el cálculo de las relaciones entre los eslabones
consecutivos del robot es inmediato, ya que vienen dadas por las matrices 𝑖−1𝑖𝐴, que se
calculan según la expresión general (1.14).
𝑐𝑜𝑠 𝜃𝑖 − cos ∝𝑖 𝑠𝑒𝑛 𝜃𝑖 𝑠𝑒𝑛 ∝𝑖 sen 𝜃𝑖 𝑎𝑖 𝑐𝑜𝑠 𝜃𝑖
𝑖−1 𝑠𝑒𝑛 𝜃𝑖 cos ∝𝑖 𝑐𝑜𝑠 𝜃𝑖 −𝑠𝑒𝑛 ∝𝑖 cos 𝜃𝑖 𝑎𝑖 𝑠𝑒𝑛 𝜃𝑖
𝑖𝐴 = ( 𝑠𝑒𝑛 ∝𝑖 𝑐𝑜𝑠 ∝𝑖
) (1.32)
0 𝑑𝑖
0 0 0 1
20
Las relaciones entre eslabones no consecutivos, es decir, entre cualquier par de sistemas
de coordenadas del brazo, se obtiene como producto de un conjunto de matrices (1.15),
utilizando los conceptos de operaciones con MTH vistos anteriormente.
0
6𝐴 = 01𝐴 12𝐴 23𝐴 34𝐴 45𝐴 56𝐴 (1.33)
1.3.5. Cinemática Inversa
El objetivo del problema cinemático inverso consiste en encontrar los valores que deben
adoptar las coordenadas articulares del robot 𝑞 = [𝑞1 , 𝑞2 , 𝑞3 , … , 𝑞𝑛 ]𝑇 para que su
extremo se posicione y oriente según una determinada localización espacial.
Los valores ingresados serán la posición y orientación final del efector final del
manipulador, información contenida en la MTH que relaciona este elemento con la
base, si 𝑛 es el número de grados de libertad de un manipulador, entonces, para obtener
los valores de las articulaciones, debemos conocer 𝑛0𝐴
0 𝒏 𝒐 𝒂 𝒑
𝑛𝐴 =(
0 0 0 1
) (1.34)
Donde, el vector 𝒏, 𝒐, 𝒂 representan la orientación del vector 𝑥, 𝑦, z del sistema de

coordenadas 𝑆𝑛 respecto del sistema de coordenadas de la base 𝑆0 , es decir, 0𝑋⃗𝑛 0𝑌
⃗⃗𝑛 0𝑍⃗𝑛
y 𝒑, la posición del sistema de coordenadas 𝑆𝑛 respecto de 𝑆0 .
Tenemos dos posibilidades para resolver la cinemática inversa de un robot, utilizar

métodos numéricos o buscar una solución cerrada. Todos los autores consultados nos
recomiendan la segunda, debido a que la primera alternativa no siempre nos garantiza una
solución rápida y en algunos casos ni siquiera se llega a la solución.
“Se han desarrollado procedimientos genéricos susceptibles de ser programados de como

que un computador pueda, a partir del conocimiento de la cinemática del robot (con sus
parámetros D-H, por ejemplo), obtener la n-upla de valores articulares que posicionan y
orientan su extremo. El inconveniente de estos procedimientos es que se trata de métodos
numéricos, cuya velocidad de convergencia e incluso su convergencia en sí no está
siempre garantizada”, concluye, por ejemplo, Barrientos (1997, p. 63). Además nos
enumera dos ventajas del uso de una solución cerrada:
21
1. En aplicaciones en las que se solicite resolver en tiempo real el problema
cinemático inverso en tiempo real. Una solución de tipo iterativo no garantiza tener
la solución en el momento adecuado.
2. La solución del problema cinemático inverso, con cierta frecuencia, no es única;
existiendo diferentes n-uplas [𝑞1 , 𝑞2 , 𝑞3 , … , 𝑞𝑛 ]𝑇 que posicionan y orientan el
extremo del robot del mismo modo. En estos casos, una solución cerrada permite
incluir determinadas reglas o restricciones que aseguren que la solución obtenida
sea la más adecuada de entre las posibles.
“La mayor parte de los robots poseen estructuras o eslabones dispuestos de tal manera
que facilitan en cierta medida la resolución de su problema cinemático inverso. Por
ejemplo, si solo se consideran los primeros grados de libertad de muchos robots, estos
tienen una estructura planar, esto es, los tres primeros elementos quedan contenidos en
un plano. Esta circunstancia facilita la resolución del problema. Asimismo, en muchos
robots se da la circunstancia de que los tres grados de libertad últimos, dedicados
fundamentalmente a orientar el extremo del robot, corresponden a giros sobre ejes que se
cortan en un solo punto. Este otro criterio facilita también el cálculo de la n-upla
[𝑞1 , 𝑞2 , 𝑞3 , … , 𝑞𝑛 ]𝑇 correspondiente a la posición y orientación deseadas. Por lo tanto, para
los casos citados y otros, es posible establecer ciertas pautas generales que permitan
plantear y resolver el problema cinemático inverso”, Barrientos (1997, p. 108).
Los métodos más utilizados para el cálculo de la cinemática inversa, son: el método
geométrico, método algebraico o matricial y el método de desacoplo cinemático. La
forma más sencilla de llegar al resultado es combinando el método de desacoplo
cinemático con cualquiera de los otros dos.
Definición de las diversas configuraciones de brazo
“Para un robot PUMA mostrado en la Figura 1.11 (y otros robots giratorios), se definen
diversas configuraciones de brazo de acuerdo con la geometría del brazo humano y el
sistema de coordenadas de elementos que se estableció utilizando el algoritmo 2.1 como
(Fig. 12):
BRAZO DERECHO (hombro): 𝜃2 positivo mueve la muñeca en la dirección 𝑧0 positivo

mientras la articulación tres no se activa.
22
BRAZO IZQUIERDO (hombro) 𝜃2 positivo mueve la muñeca en la dirección 𝑧0 negativa
mientras la articulación tres no se activa.
BRAZO ARRIBA (codo por encima de la muñeca): Posición de la muñeca del brazo
{DERECHO, IZQUIERDO} con respecto al sistema de coordenadas del hombro, tiene
valor de coordenada {negativo, positivo} a lo largo del eje 𝑦2 .
BRAZO ABAJO (codo por debajo de la muñeca): posición de la muñeca del brazo
{DERECHO, IZQUIERDO} con respecto al sistema de coordenadas del hombro, tiene
valor de coordenada {positivo, negativo} a lo largo del eje 𝑦2
MUÑECA ABAJO: El vector unitario s del sistema de coordenadas de la mano y el vector

unitario 𝑦5 del sistema de coordenadas (𝑋5 , 𝑌5 , 𝑍5 ) tienen un producto escalar positivo.
MUÑECA ARRIBA: El vector unitario s del sistema de coordenadas de la mano y el

vector unitario 𝑦5 del sistema de coordenadas (𝑋5 , 𝑌5 , 𝑍5 ) tiene un producto escalar
negativo.
(Obsérvese que la definición de las configuraciones del brazo con respecto al sistema de
coordenadas del elemento pueden tener que ser ligeramente modificadas si se utilizan
diferentes sistemas de coordenadas para los elementos.)”, Fu, Gonzales y Lee (1988, p.
64),
Fig. 1.11 Sistema de coordenadas para un robot PUMA

(Fuente: Fu, Gonzales y Lee (1988, p. 39))
“Con respecto a la definición anterior de las diversas configuraciones del brazo, se

definen dos indicadores de configuraciones de brazo (BRAZO y CODO) para cada
23
configuración. Estos dos indicadores se combinan para dar una solución de las cuatro
posibles para las tres primeras articulaciones. Para cada una de las cuatro configuraciones
del brazo (Figura 1.12) el tercer indicador (MUÑECA) da una de las dos posibles
soluciones de articulación para las tres últimas articulaciones. Estos tres indicadores se
pueden definir como:
(1.35)
(1.36)
(1.37)
Además de estos indicadores, el usuario puede definir un conmutador <FLIP> como:
(1.38)
24
Fig. 1.12 Definición de diversas configuraciones de un brazo
Los valores señalados por estos indicadores y el conmutador se especifican por el usuario
para encontrar la solución cinemática inversa”, Fu, Gonzales y Lee (1988, pp. 64-65).
El uso de estos parámetros, nos permiten seleccionar una de las 16 soluciones posibles
que tiene un brazo robótico de seis grados de libertad con articulaciones giratorias. El
indicador al que Fu, Gonzales y Lee llaman CONMUTADOR, selecciona una de las dos
posiciones posibles para el último grado de libertad.
1.3.5.1. Método de Desacoplo Cinemático
Como sabemos, los tres primeros grados de libertad posicionan el extremo final del robot
en un lugar deseado y las articulaciones siguientes, si bien es cierto varían un poco la
posición del robot, se utilizan para determinar la orientación del extremo final.
25
Valiéndose de lo anterior; este método consiste, como se mencionó antes, en separar el
problema cinemático inverso en dos: posición y orientación. Las articulaciones finales,
generalmente se cortan en un solo punto, el cual se calcula fácilmente con una resta de
vectores. Una vez hecho esto, se procede a calcular los tres primeros grados de libertad:
[𝑞1 , 𝑞2 , 𝑞3 ], utilizando el método algebraico o geométrico. Con estos valores, es sencillo calcular
los valores de los ángulos de articulación restantes.
Barrientos (1997, p.118), nos explica el método de desacoplo cinemático para un robot
de 6 grados de libertad que cumple con la condición de que se intersecten grados de
libertad de orientación en un solo punto (Fig. 1.11).
Dadas la posición y orientación deseadas para el manipulador de la Figura 1.11 ( 06𝐴), de donde
obtenemos 𝑂6, que es el vector posición del sistema de coordenadas del eslabón final con
respecto a la base; encontramos el punto 𝑂5 realizando una resta de vectores, éste punto
es la posición de la intersección de los ejes de las articulaciones de orientación con
respecto a la base. Con esto podemos fácilmente calcular los ángulos de las articulaciones
como se mencionó líneas arriba.
Fig. 1.13 Manipulador utilizado para explicar el método de desacoplo cinemático

(Fuente: Barrientos (1997. p.119))
26
1.3.5.2. Método geométrico
Fu, Gonzales y Lee (1988, p. 66), nos explican un método geométrico, muy interesante,
el cual posteriormente utilizaremos en la investigación, luego de aplicar el método de
desacoplo cinemático, que divide el problema en dos etapas. Éste método nos permite
elegir entre los diferentes conjuntos de ángulos que solucionan la cinemática inversa.
Un manipulador puede llegar a tener hasta 16 soluciones posibles, Craig (2006, p. 105).
Fig. 1.14 Solución geométrica para la articulación 1 de un brazo robótico de

6 grados de libertad.
(Fuente: Fu, Gonzales y Lee (1988, p.67))
1.3.5.3. Método Algebráico
El método algebraico utiliza las MTH para resolver el problema cinemático inverso.
Aplicarlo para resolver los 6 grados de libertad a partir del conocimiento de la matriz
0
𝑛𝐴 (ecuación 1.15), es muy complicado y tedioso, sin embargo, si utilizamos
previamente el método de desacoplo cinemático, el método algebraico se vuelve viable
27
debido a que se resuelven primero las articulaciones [𝑞1 , 𝑞2 , 𝑞3 ] y luego, una vez
calculados esos valores, las articulaciones siguientes.
Barrientos (1997, p. 112), nos explica la utilización de este método para el manipulador
de tres grados de libertad de la Figura 13.
Fig. 1.15 Robot polar de 3-GDL.

(Fuente: Barrientos (1997, p. 113))
“El primer paso a dar para resolver el problema cinemático es obtener la expresión (1.14)
correspondiente a este robot. Es decir, obtener la matriz que relaciona el sistema de
referencia {𝑆0 } asociado a la base con el sistema de referencia {𝑆3 } asociado con su extremo.
La figura 1.15 representa la asignación de sistemas de referencia según los criterios de Denavit-
Hartenberg, con el robot situado en su posición de partida (𝑞1 = 𝑞2 = 𝑞3 = 0 ),y la Tabla 1
muestra los valores de los parámetros D-H”.
Tabla 1.1: Parámetros D-H del robot de la figura 13
Articulación 𝜃 𝑑 𝑎 𝛼
1 𝑞1 𝑙1 0 90
2 𝑞2 0 0 −90
3 0 𝑞3 0 0
A partir de estos parámetros, de inmediato, se pueden obtener las matrices que relacionan
cada eslabón con el eslabón siguiente y la matriz que relaciona el eslabón final con la
base del robot. Estas son:
28
(1.39)
“Obtenida la expresión 03𝐴 en función de las coordenadas articulares (𝑞1 , 𝑞2 , 𝑞3 ) y su

localización de destino para el extremo del robot definida por los vectores 𝒏, 𝒐, 𝒂 y 𝒑 se puede
intentar manipular directamente las dos ecuaciones resultantes de 03𝐴 a fin de despejar 𝑞1 , 𝑞2 y
𝑞3 en función de 𝒏, 𝒐, 𝒂 y 𝒑.
Sin embargo, este procedimiento directo es complicado, apareciendo ecuaciones trascendentes.

En lugar de ello, suele ser más adecuado es aplicar el siguiente procedimiento:
Puesto que 03𝐴 = 01𝐴 12𝐴 23𝐴, se tendrá que:
0 −1 0
1𝐴 3𝐴 = 12𝐴 23𝐴 (1.40)
1 −1 0 −1 0
2𝐴 1𝐴 3𝐴 = 23𝐴 (1.41)
𝒏 𝒐 𝒂 𝒑
Puesto que 03𝐴 = ( ) es conocida, los miembros a la izquierda en las
0 0 0 1
expresiones (1.22) y (1.23) son funciones de las variables (𝑞1 , … , 𝑞𝑘 ) mientras que los
miembros de la derecha lo son de las variables articulares (𝑞𝑘+1 , … , 𝑞𝑛 ).
De este modo, de la primera de las expresiones (1.22) y (1.23) se tendrá 𝑞1 aislado del resto de
las variables articulares y tal vez será posible obtener su valor sin la complejidad que se tendría
abordando directamente la manipulación de la expresión (1.16). A su vez, una vez obtenida 𝑞1 , la
expresión (1.23) permitirá tener el valor de 𝑞2 aislado y respecto de 𝑞3 . Por último, conocidos 𝑞1
y 𝑞2 se podrá obtener 𝑞3 de la expresión (1.16).”, Barrientos (1997, pp. 115-117).
29
Fig. 1.16 Asignación de sistemas de referencia de robot polar de la Figura 13
(Fuente: Barrientos (1997, p.114))
Las matrices inversas pueden calcularse manualmente o utilizando MATLAB o cualquier

software que nos permita realizar operaciones matriciales.
De la ecuación resultante al desarrollar (1.22), podremos obtener 12 igualdades, al

comparar los elementos de las matrices resultantes a cada lado del signo igual, debemos
escoger aquella que exprese 𝑞1 en función de constantes. Para este ejemplo se tomó el elemento
(3,4) y obtuvimos:
(1.42)
Encontrar los valores de los dos siguientes grados de libertad es un poco más complejo,
debido a que se presentan ecuaciones trascendentes [9], sin embargo, Craig (2006) aporta
un método que utiliza cambios de variables para convertir estas ecuaciones en ecuaciones
algebraicas sencillas de resolver.
Solución algebraica por reducción a polinomio
“Las ecuaciones trascendentales son a menudo difíciles de resolver porque, aun cuando
solamente hay una variable (digamos, 𝜃), generalmente aparece como 𝑠𝑒𝑛 𝜃 y 𝑐𝑜𝑠 𝜃. No
30
obstante, si hacemos las siguientes sustituciones se produce una expresión en términos de
una sola variable 𝑢:
(1.43)
Ésta es una sustitución geométrica muy importante que se utiliza a menudo para resolver
ecuaciones cinemáticas. Estas sustituciones convierten ecuaciones trascendentales en
ecuaciones polinomiales en 𝑢”, Craig (2006, pp. 113-114).
Al resolver la cinemática de un robot de tres grados de libertad, mediante el método

algebraico, nos encontraremos, generalmente, con ecuaciones de la forma:
𝑎 𝑐𝑜𝑠 𝜃 + 𝑏 𝑠𝑒𝑛 𝜃 = 𝑐 (1.44)
Luego de aplicar la sustitución por una variable 𝑢, la igualdad tomará una forma
polinomial:
𝑎(1 − 𝑢2 ) + 2𝑏𝑢 = 𝑐(1 + 𝑢2 ) (1.45)
Al desarrollar la ecuación y ordenarla convenientemente tenemos que:
(𝑎 + 𝑐)𝑢2 − 2𝑏𝑢 + (𝑐 − 𝑎) = 0, (1.46)
que se resuelve mediante la fórmula cuadrática:
𝑏±√𝑏 2 +𝑎2 −𝑐 2
𝑢= (1.47)
𝑎+𝑐
Por lo tanto,
𝑏±√𝑏2 +𝑎2 −𝑐 2
𝜃 = 2𝑡𝑎𝑛−1 ( ). (1.48)
𝑎+𝑐
“En caso de que la solución para u en base a la ecuación (1.29) fuera compleja, no hay
solución real para la ecuación trascendental original. Observe que si a + c = 0, el
argumento del arco tangente se vuelve infinito y por ende θ = 180°. En una
implementación por computadora debe comprobarse antes esta división potencial entre
cero. Esta situación se produce cuando el término cuadrático de (1.28) desaparece, de
31
manera que la ecuación cuadrática se degenera en una ecuación lineal”, Craig (2006, p.
114).
Función arco tangente de cuatro cuadrantes atan2(x,y)

𝑦
“𝑎𝑡𝑎𝑛2(𝑦, 𝑥) calcula 𝑡𝑎𝑛−1 (𝑥 ) pero utiliza los signos tanto de 𝑥 como de 𝑦 para
identificar el cuadrante en el que se encuentra el ángulo resultante. Por ejemplo,

𝑎𝑡𝑎𝑛2(−2.0, −2.0) = −135°, mientras que 𝑎𝑡𝑎𝑛2(2.0, 2.0) = 45°, una distinción que
se perdería con una función arco tangente de un solo argumento. Frecuentemente estamos
calculando ángulos que pueden variar sobre un giro completo de 360°, por lo que
utilizaremos comúnmente la función 𝑎𝑡𝑎𝑛2. Observe que 𝑎𝑡𝑎𝑛2 se vuelve indefinida
cuando ambos argumentos son iguales a cero. Algunas veces se le llama “arco tangente
de 4 cuadrantes” y algunas bibliotecas de lenguajes de programación la tienen
predefinida.” Craig (2006, p. 43).
1.3.6. Ecuaciones de decisión y verificación de modelos cinemáticos

Como vimos anteriormente, la cinemática inversa no tiene una solución única, sino varias.
Es por eso que se requiere el uso de los indicadores vistos en la sección anterior (BRAZO,
CODO Y MUÑECA), los cuales, para fines de verificar los algoritmos, podremos
calcularlos una vez conocidos los ángulos de las articulaciones, mediante el uso de las
llamadas ecuaciones de decisión. El algoritmo a usar se describe en la Figura 1.17.
Fu, Gonzales y Lee (1998, pp. 76-78) nos muestra como plantear las ecuaciones de
decisión para el robot manipulador de seis grados de libertad de la Figura 1.11, utilizando
las definiciones dadas en la sección 1.3.4.
Fig. 1.17 Algoritmo para verificación de la cinemática de un manipulador.

(Fuente: Fu, Gonzales y Lee (1988, p.77))
32
Para el indicador BRAZO
Sean:
𝒁𝟎 : Eje del sistema de coordenadas de la base.
𝒁𝟏 : Eje del sistema de coordenadas 𝑆1, respecto de 𝑆0 .
𝒑′ = (𝑝𝒙 , 𝑝𝒚 , 0): La proyección de 𝑷𝟎𝟓 sobre el plano 𝑿𝟎 𝒀𝟎 .
𝑖 𝒋 𝒌
𝒁 𝑥 𝒑′ 1
𝑔= 𝒁𝟎 . |𝒁𝟏 𝑥 𝒑′| = 𝒁𝟎 . |− sin 𝑞1 cos 𝑞1 0| |𝒁 𝑥 𝒑′|
𝟏 𝟏
𝑝𝑥 𝑝𝑦 0
−𝑝𝒚 sin 𝑞1−𝑝𝒙 cos 𝑞1
𝑔= |𝒁𝟏 𝑥 𝒑′|
(1.49)
Si 𝑔 > 0, entonces el brazo está en la configuración BRAZO DERECHO.

Si 𝑔 < 0, el brazo está en la configuración BRAZO IZQUIERDO.
Si g=0, no existe solución unívoca, por defecto elegiremos la configuración BRAZO
DERECHO.
Para el indicador Codo

Para calcular este indicador, seguimos la definición CODO ARRIBA/CODO ABAJO en
la sección 1.3.4. Analizando la figura 1.18, nos podemos dar cuenta que un parámetro que
puede definir el indicador CODO, es 𝑷𝟐𝟑 𝒚 ; es decir la componente en el eje 𝑌 del sistema
de coordenadas 𝑆2 , de la posición del origen del sistema 𝑆3 respecto del sistema 𝑆2 .

También podemos percatarnos que la configuración BRAZO DERECHO/BRAZO
IZQUIERDO invierte el signo del parámetro CODO, para sus dos configuraciones
posibles.
33
Figura 1.18 Configuraciones codo arriba y codo abajo según la configuración brazo
derecho y brazo izquierdo.
(Fuente: Elaboración propia)
La ecuación para hallar el parámetro CODO, para el manipulador de la figura 1.11, tendría
la siguiente forma (Fu, Gonzales y Lee, 1988):
𝐶𝑂𝐷𝑂 = 𝐵𝑅𝐴𝑍𝑂. 𝑠𝑖𝑔𝑛(𝑃24 𝑦 )
+1 → 𝐶𝑜𝑑𝑜 𝑒𝑛𝑐𝑖𝑚𝑎 𝑑𝑒 𝑙𝑎 𝑀𝑢ñ𝑒𝑐𝑎
𝐶𝑂𝐷𝑂 = 𝐵𝑅𝐴𝑍𝑂. 𝑠𝑖𝑔𝑛(𝑑4 𝐶3 − 𝑎𝟑 𝑆3 ) = {
−1 → 𝐶𝑜𝑑𝑜 𝑑𝑒𝑏𝑎𝑗𝑜 𝑑𝑒 𝑙𝑎 𝑀𝑢ñ𝑒𝑐𝑎
(1.50)
En esta ecuación se utiliza 𝑷𝟐𝟒 ya que, en el análisis de las primeras tres articulaciones
se encontró conveniente, debido a la coincidencia de los Sistemas de coordenadas 𝑆𝟎𝟑 y
𝑆04 .
34
1.3.7. Generación de trayectorias
La planificación de trayectorias consiste en generar una función que satisfaga, según las
exigencias del usuario, una posición inicial y final, o bien una posición inicial y final y
algunos puntos intermedios definidos, o bien una posición inicial y final y un camino
específico deseado (generalmente un camino en línea recta).
Las dos primeras opciones se pueden conseguir mediante métodos de interpolación, que
actúan en el espacio de las variables articulares, es decir, unimos los puntos inicial y final,
o los puntos inicial y final más los puntos intermedios (Craig (2006) los llama Puntos
Vía; Fu , Gonzales y Lee los llaman “ligaduras de camino”) mediante funciones
polinomiales de grado 𝑛.
La tercera opción se consigue de una manera un poco más complicada, y se tiene que
hacer un trabajo que involucra al espacio de las coordenadas cartesianas.
Así, algunos autores clasifican a los métodos de generación de trayectorias en dos grupos,
los que trabajan en el espacio de las variables de articulación y los que trabajan en el
espacio de las coordenadas cartesianas.
1.3.7.1. Generación de trayectorias en el espacio articular (interpolación).
Paul (1972) demostró que son de interés las siguientes consideraciones, para la
generación de trayectorias de este tipo:
1. Cuando se toma un objeto, el movimiento de la mano debe dirigirse hacia fuera
del objeto; en otro caso, mano puede golpear la superficie soporte del objeto.
2. Si especificamos una posición de partida (punto de despegue) a lo largo del vector
normal a la superficie desde la posición inicial y si queremos que la mano (es
decir, el origen del sistema de coordenadas de la mano) pase a través de esta
posición, entonces tenemos un movimiento de partida admisible. Si especificamos
más aún el tiempo necesitado para alcanzar esta posición, podríamos controlar la
velocidad con la cual despega el objeto.
3. El mismo conjunto de requisitos de despegue para el movimiento del brazo es
también válido cuando nos acercamos a la posición final (es decir, debemos
movernos en una dirección normal a la superficie y frenándonos cuando nos
aproximamos a la posición final) de manera que se pueda obtener y controlar la
dirección de aproximación correcta.
35
4. De lo anterior, tenemos cuatro posiciones para cada movimiento del brazo: inicial,
despegue, asentamiento y final (Figura 1.18)
5. Ligaduras de posición:
a. Posición inicial: serán la velocidad y aceleración (normalmente nula)
b. Posición de despegue: movimiento continuo a puntos intermedios.
c. Posición de asentamiento: lo mismo que para la posición de despegue.
d. Posición final: serán la velocidad y aceleración (normalmente nulas)
6. Además de estas ligaduras, los extremos de todas las trayectorias de la
articulación deben estar dentro de sus límites físicos y geométricos.
Fig. 1.18 Condiciones de posición para una trayectoria de articulación

Las ligaduras para la planificación de trayectorias de articulación interpoladas son:
Posición inicial
1. Posición
2. Velocidad inicial (normalmente nula)
3. Aceleración inicial (normalmente nula)
Posición intermedia
4. Posición de despegue
5. Continuidad en posición
36
6. Continuidad en velocidad
7. Continuidad en aceleración
8. Posición de asentamiento
9. Continuidad en posición
10. Continuidad en velocidad
11. Continuidad en aceleración
Posición final
12. Posición
13. Velocidad final (normalmente nula)
14. Aceleración final (normalmente nula)
1.3.7.1.1. Calculo de una trayectoria de articulación 4-3-4.

Este método consiste en dividir la trayectoria en tres tramos, el primer segmento es un
polinomio de cuarto grado que especifica la trayectoria desde la posición inicial hasta la
de despegue. El segundo segmento de trayectoria (o segmento medio de trayectoria) es
un polinomio de tercer grado que especifica la trayectoria desde la posición de despegue
hasta la posición de asentamiento. El último segmento de trayectoria es un polinomio de
cuarto grado que especifica la trayectoria desde la posición de asentamiento hasta la
posición final.
Los polinomios usados tienen la siguiente forma
ℎ1 = 𝑎14 𝑡 4 + 𝑎13 𝑡 3 + 𝑎12 𝑡 2 + 𝑎11 𝑡 + 𝑎10 (primer segmento) (1.51)

ℎ2 = 𝑎23 𝑡 3 + 𝑎22 𝑡 2 + 𝑎21 𝑡 + 𝑎20 (segundo segmento) (1.52)
ℎ1 = 𝑎𝑛4 𝑡 4 + 𝑎𝑛3 𝑡 3 + 𝑎𝑛2 𝑡 2 + 𝑎𝑛1 𝑡 + 𝑎𝑛0 (tercer segmento) (1.53)
Los subíndices de cada ecuación polinomial indican el número de segmento, y 𝑛 indica

el último segmento de la trayectoria. El coeficiente 𝑎𝑗𝑖 indica el coeficiente 𝑖-ésimo para
el segmento de trayectoria 𝑗 de una trayectoria de articulación. Las condiciones fronteras
que este conjunto de polinomios para los segmentos de trayectoria de la articulación
deben satisfacer son:
1. Posición inicial = 𝜃0 = 𝜃(𝑡0 ).
2. Magnitud de la velocidad inicial = 𝑣0 (normalmente nulo).
3. Magnitud de la aceleración inicial = 𝑎0 (normalmente nulo).
37
4. Posición de despegue = 𝜃1 = 𝜃(𝑡1 ).
5. Continuidad en posición en 𝑡1 .
6. Continuidad en velocidad en 𝑡1 .
7. Continuidad en aceleración en 𝑡1 .
8. Posición de asentamiento = 𝜃2 = 𝜃(𝑡2 )
9. Continuidad en posición en 𝑡2
10. Continuidad en velocidad en 𝑡2
11. Continuidad en aceleración en 𝑡2 .
12. Posición final = 𝜃𝑓 = 𝜃(𝑡𝑓 ).
13. Magnitud de la velocidad final = 𝑣𝑓 (normalmente nula).
14. Magnitud de la aceleración final = 𝑎𝑓 (normalmente nula).
Fu, Gonzales y Lee (1988) detallan el desarrollo y la obtención de las expresiones

correspondientes a los coeficientes de las ecuaciones polinomiales correspondientes a los
tres segmentos, los resultados se detallan a continuación:
Primer segmento de trayectoria:
𝑎0 𝑡1 2 𝑎0 𝑡1 2
ℎ1 (𝑡) = (𝛿1 − 𝑣0 𝑡1 − − 𝜎) 𝑡 4 + 𝜎𝑡 3 + ( ) 𝑡 2 + (𝑣0 𝑡1 )𝑡 + 𝜃0 (1.54)
2 2
ℎ1 (1) 4𝛿 𝜎
𝑣1 = = ( 𝑡 1 − 3𝑣0 − 𝑎0 𝑡1 − 𝑡 ) (1.55)
𝑡1 1 1
ℎ1 (1) 12𝛿1 12𝑣0 6𝜎

𝑎1 = =(𝑡 − − 5𝑎0 − 𝑡 2 ) (1.56)
𝑡1 2 1
2 𝑡1 1
Segundo segmento de trayectoria:
𝑎1 𝑡2 2 𝑎1 𝑡2 2
ℎ2 (𝑡) = (𝛿2 − 𝑣1 𝑡2 − ) 𝑡3 + ( ) 𝑡 2 + (𝑣1 𝑡2 )𝑡 + 𝜃1 (1.57)
2 2
ℎ2 (1) 3𝛿 𝑎1 𝑡2
𝑣2 = = ( 𝑡 2 − 2𝑣1 − ) (1.58)
𝑡2 2 2
ℎ2 (1) 6𝛿2 6𝑣1

𝑎2 = = (𝑡 − − 2𝑎1 𝑡2 ) (1.59)
𝑡1 2 2
2 𝑡2
Ultimo segmento de trayectoria:
38
𝑎2 𝑡𝑛 2 𝑎𝑓 𝑡𝑛 2
ℎ𝑛 (𝑡) = (9𝛿𝑛 − 4𝑣2 𝑡𝑛 − − 5𝑣𝑓 𝑡𝑛 + ) 𝑡 4 + (−8𝛿𝑛 + 5𝑣𝑓 𝑡𝑛 −
2 2
𝑎𝑓 𝑡𝑛 2 𝑎2 𝑡𝑛 2
+ 3𝑣2 𝑡𝑛 ) 𝑡 3 + ( ) 𝑡 2 + (𝑣2 𝑡𝑛 )𝑡 + 𝜃2 (1.60)
2 2
donde 𝜎 = 𝑓/𝑔 y
2𝑡𝑛 2𝑡𝑛 3𝑡2 𝛿2 𝑡1 𝑡𝑛 2𝛿𝑛 𝑡1

𝑓 = 2𝛿1 (4 + + + )− (3 + ) +
𝑡2 𝑡1 𝑡1 𝑡2 𝑡2 𝑡𝑛
6𝑡2 4𝑡𝑛 3𝑡𝑛 5 𝑡1 2𝑡1 5𝑡2
− 𝑣0 𝑡1 (6 + + + ) − 𝑣𝑓 𝑡1 − 𝑎0 𝑡1 𝑡𝑛 ( + + + )
𝑡1 𝑡1 𝑡2 3 𝑡2 𝑡𝑛 𝑡𝑛
− 𝑎𝑓 𝑡1 𝑡𝑛
𝑡𝑛 2𝑡𝑛 3𝑡2
𝑔= + +2+
𝑡2 𝑡1 𝑡1
1.3.7.1.2. Cálculo de trayectorias 3-5-3

Lo mismo que la trayectoria 4-3-4, pero utiliza polinomios de grados diferentes para cada
segmento: un polinomio de tercer grado para el primer segmento, un polinomio de quinto
grado para el segundo segmento, y un polinomio de tercer grado para el último segmento.
Aplicando las condiciones explicadas anteriormente, se obtienen las siguientes
ecuaciones:
Primer segmento de trayectoria:
𝑎0 𝑡1 2 𝑎0 𝑡1 2 2
ℎ1 (𝑡) = (𝛿1 − 𝑣0 𝑡1 − ) 𝑡3 + 𝑡 + (𝑣0 𝑡1 )𝑡 + 𝜃0 (1.61)
2 2
ℎ1 (1) 3𝛿 𝑎0 𝑡1
𝑣1 = = ( 𝑡 1 − 2𝑣0 − ) (1.62)
𝑡1 1 2
ℎ1 (1) 6𝛿1 6𝑣0

𝑎1 = = (𝑡 − − 2𝑎0 ) (1.63)
𝑡1 2 1
2 𝑡1
Segundo segmento de trayectoria:
𝑎1 𝑡2 2 𝑎2 𝑡2 2
ℎ2 (𝑡) = (6𝛿2 − 3𝑣1 𝑡2 − 3𝑣2 𝑡2 − + ) 𝑡 5 + (−15𝛿2 − 8𝑣1 𝑡2 +
2 2
3𝑎1 𝑡2 2 3𝑎1 𝑡2 2
7𝑣2 𝑡2 + − 𝑎2 𝑡2 2 ) 𝑡 4 + 6𝛿2 − (10𝛿2 − 6𝑣1 𝑡2 − 4𝑣2 𝑡2 − +
2 2
𝑎2 𝑡2 2 𝑎1 𝑡2 2 2
2
) 𝑡3 + 2
𝑡 + (𝑣1 𝑡2 )𝑡 + 𝜃1 (1.64)
39
ℎ2 (1) 3𝛿𝑛 𝑎𝑓 𝑡𝑛
𝑣2 = =( − 2𝑣𝑓 + ) (1.65)
𝑡2 𝑡𝑛 2
ℎ2 (1) −6𝛿𝑛 6𝑣𝑓

𝑎2 = 2 =( 2 + − 2𝑎𝑓 ) (1.66)
𝑡1 𝑡𝑛 𝑡𝑛
Último segmento de trayectoria
𝑎𝑓 𝑡𝑛 2
ℎ𝑛 (𝑡) = (𝛿𝑛 − 𝑣𝑓 𝑡𝑛 + ) 𝑡 3 + (−3𝛿𝑛 + 3𝑣𝑓 𝑡𝑛 + 𝑎𝑓 𝑡𝑛 2 )𝑡 2 + (3𝛿𝑛 − 2𝑣𝑓 𝑡𝑛 +
2
𝑎𝑓 𝑡𝑛 2
) 𝑡 + 𝜃0 (1.67)
2
ℎ1 (1) 3𝛿 𝑎0 𝑡1
𝑣1 = = ( 𝑡 1 − 2𝑣0 − ) (1.68)
𝑡1 1 2
ℎ1 (1) 6𝛿1 6𝑣0

𝑎1 = 2 = (𝑡 2 − − 2𝑎0 ) (1.69)
𝑡1 1 𝑡1
1.3.7.2. Generación de trayectorias en el espacio cartesiano utilizando cuaterniones

En la sección anterior describimos funciones polinomiales de bajo orden para generar
puntos de consignas de la trayectoria de la articulación interpolada para el control de un
manipulador. Aunque las coordenadas de articulación del manipulador especifican
completamente la posición y orientación de su mano, no son apropiadas para especificar
una tarea porque la mayoría de las coordenadas de articulación del manipulador no son
ortogonales y no separan la posición de la orientación. Para los robots más sofisticados
se han desarrollado lenguajes de programación que controlan el manipulador para cumplir
una determinada tarea. En tales sistemas, una tarea se suele especificar como secuencias
de puntos nudos cartesianos a través de los cuales debe pasar la mano del manipulador o
el efector final. Así, al describir los movimientos del manipulador en una tarea, estamos
más interesados por el formalismo de especificar las posiciones finales por las cuales tiene
que moverse la mano del manipulador, así como la curva en el espacio (o camino) que
recorre.
Paul (1979) describe el diseño de las trayectorias de manipuladores constituidas por
segmentos de línea recta para el movimiento de la mano. La velocidad y aceleración de
la mano entre estos segmentos se controla convirtiéndolas en coordenadas de articulación
y se suavizan mediante una rutina de interpolación cuadrática. Taylor (1979) extendió y
refinó el método de Paul utilizando la representación de cuaterniones de números duales
para describir la localización de la mano. A causa de las propiedades de los cuaterniones,
40
las transiciones entre posiciones de la mano debidos a operaciones rotacionales necesitan
menos cálculo, mientras que las operaciones traslacionales no dan ninguna ventaja.
El esquema de trayectoria de movimiento en línea recta de Paul utiliza el método de la
matriz de transformación homogénea para representar las posiciones finales. Esta
representación es fácil de comprender y de utilizar. Sin embargo, las matrices son
moderadamente costosas de almacenar y los cálculos sobre ellas requieren más
operaciones que para algunas otras representaciones. Taylor (1979) observó que
utilizando un cuaternión para representar la rotación hacía el movimiento más uniforme
y eficaz. Propuso dos métodos al problema de planificación de movimientos en línea recta
entre puntos nudos. El primero, llamado control del camino cartesiano, es un
refinamiento de la técnica de Pul, pero utilizando una representación de cuaterniones
para las rotaciones. Este método es simple y proporciona un movimiento rotacional más
uniforme. Sin embargo, necesita considerables cálculos en tiempo real y es vulnerable a
configuraciones del manipulador degeneradas. El segundo método, llamado camino de la
articulación de desviación acotada, necesita una fase de pre-planificación del
movimiento que selecciona suficientes puntos nudos de manera que el manipulador se
pueda controlar mediante interpolación lineal de los valores de la articulación sin permitir
que la mano del manipulador se desvíe de la trayectoria en línea recta más de una cantidad
preespecificada. Este método reduce grandemente la cantidad de cálculos que se deben
hacer en cada intervalo de muestreo.
El método de generación de trayectorias utilizado por Paul, puede ser revisado en Fu,
Gonzales y Lee (1988). En el presente texto profundizaremos en los dos métodos
planteados por Taylor; para lo cual, primero abordaremos el concepto de cuaterniones y
el álgebra que los gobierna.
1.3.7.2.1. Primer método: Esquema de control del camino cartesiano
Se requiere mover el sistema de coordenadas de la mano del manipulador a lo largo de

una línea recta entre dos puntos nudos especificador por 𝑭𝟎 y 𝑭𝟏 en el tiempo 𝑇, donde
cada sistema de coordenadas se representaba mediante una matriz de transformación
homogénea
𝑹 𝑷𝒊
𝑭𝒊 = ( 𝒊 ) (1.70)
0 0
41
El movimiento a lo largo del camino consiste en traslaciones del origen del sistema de la
herramienta desde 𝒑𝟎 hasta 𝒑𝟏 acomplado con rotaciones de la parte de orientación desde
la estructura de la herramienta desde 𝑹𝟎 hasta 𝑹𝟏 . Sea λ(𝑡) la fracción que queda todavía
del movimiento para ser recorrida en el tiempo 𝑡. Para un movimiento uniforme tenemos:
𝑇−𝑡
λ(𝑡) = (1.71)
𝑇
donde 𝑇 es el tiempo total necesitado para recorrer el segmento y 𝑡 es el tiempo

comenzando desde el principio del recorrido del segmento. La posición y orientación de
la herramienta en el tiempo 𝑡 se dan, respectivamente, por:
𝒑(𝒕) = 𝒑𝟏 − 𝜆(𝑡)(𝒑𝟏 − 𝒑𝟎 ) (1.72)
𝑹(𝒕) = 𝑹𝟏 𝑅𝑜𝑡[𝒏, −𝜃𝜆(𝑡)] (1.73)
donde Rot(𝐧, θ) es una rotación de θ respecto de un eje 𝐧 para reorientar 𝑹𝟎 en 𝑹𝟏 ,
𝑅𝑜𝑡(𝒏, 𝜃) = 𝑹𝟎 −1 𝑹𝟏 (1.74)
donde 𝑅𝑜𝑡(𝒏, 𝜃) representa la retoación resultante de 𝑹𝟎 −1 𝑹𝟏 en forma de cuaternión.

Es conveniente observar que 𝒑𝟏 − 𝒑𝟎 en la ecuación (1.60) y 𝒏 y 𝜃 en la ecuación (1.61)
necesitan evaluarse solamente una vez por segmento si el segmento 𝑭𝟏 está fijo. Por otra
parte, si el punto de destino está cambiando, entonces 𝑭𝟏 estará modificándose también.
En este caso, 𝒑𝟏 − 𝒑𝟎 , 𝒏 y 𝜃 se deberían evaluar en cada paso.
Si se desea que la mano del manipulador se mueva de un segmento a otro manteniendo la

aceleración constante, se debe acelerar o desacelerar desde un segmento hasta el
siguiente. Para lograr esto, la transición debe comenzar un tiempo 𝜏 antes que el
manipulador alcance la intersección constante, se debe acelerar o desacelerar desde un
segmento hasta el siguiente. Para lograr esto, la transición debe comenzar un tiempo 𝜏
antes que el manipulador alcance la intersección de los dos segmentos y complete la
transición al nuevo segmento en el tiempo 𝜏 después que ha sido alcanzada la intersección
con el nuevo segmento. De este requisito, las condiciones fronteras para la transición de
segmentos son
𝜏∆𝒑𝟏
𝒑(𝑇1 − 𝜏) = 𝒑𝟏 − (1.75)
𝑇1
𝜏∆𝒑𝟐
𝒑(𝑇1 + 𝜏) = 𝒑𝟏 + (1.76)
𝑇2
42
𝑑 ∆𝒑𝟏
𝒑(𝑡)|𝑡=𝑇1−𝜏 = (1.77)
𝑑𝑡 𝑇1
𝑑 ∆𝒑𝟐
𝒑(𝑡)|𝑡=𝑇1+𝜏 = (1.78)
𝑑𝑡 𝑇2
donde ∆𝒑𝟏 = 𝒑𝟏 − 𝒑𝟐 , ∆𝒑𝟐 = 𝒑𝟐 − 𝒑𝟏 , y 𝑇1 y 𝑇2 son los tiempos recorridos por los dos
segmentos. Si aplicamos una aceleración constante a la transición,
𝑑2
𝒑(𝑡) = 𝒂𝒑 (1.79)
𝑑𝑡 2
Integrando la ecuación anterior dos veces y aplicando las condiciones fronteras,

obtenemos la ecuación de posición de la herramienta para el segmento de transición,
2 2
(𝜏−𝑡 ′ ) (𝜏+𝑡 ′ )
𝒑(𝑡′) = 𝒑𝟏 − ∆𝒑𝟏 + ∆𝒑𝟐 (1.80)
4𝜏𝑇1 4𝜏𝑇2
donde 𝑡 ′ = 𝑡 − 𝑇1 es el tiempo de la intersección de los dos segmentos; es decir, cuando

𝑡 ′ = 0 nos ubicamos en la intersección de los dos segmentos, cuando 𝑡 ′ = −𝜏 y 𝑡 ′ = 𝜏,
nos encontramos en el inicio y final de la transición, respectivamente.
Análogamente, la ecuación de orientación de la herramienta se obtiene como

2 2
(𝜏−𝑡 ′ ) (𝜏−𝑡 ′ )
𝑹(𝑡′) = 𝑹𝟏 𝑅𝑜𝑡 [𝒏𝟏 , − 𝜃1 ] 𝑅𝑜𝑡 [𝒏𝟐 , − 𝜃2 ] (1.81)
Donde
𝑅𝑜𝑡(𝒏𝟏 , 𝜃1 ) = 𝑹𝟎 −1 𝑹𝟏 y 𝑅𝑜𝑡(𝒏𝟐 , 𝜃2 ) = 𝑹𝟏 −1 𝑹𝟐
Las ecuaciones anteriores para la posición y orientación de la estructura de la herramienta

a lo largo del camino en línea recta producen una transición suave entre los dos
segmentos.
1.3.7.2.2. Segundo método: Trayectoria de la articulación con desviación acotada
El sistema de control del camino cartesiano descrito anteriormente necesita una cantidad
considerable de tiempo de cálculo y es difícil de tratar con las restricciones sobre la
conducta espacial de las variables de articulación del manipulador en tiempo real. Existen
algunas formas posibles que están disponibles para tratar este problema. Una podría ser
pre-calcular y almacenar la solución de la articulación mediante la simulación del
algoritmo en tiempo real, antes de la ejecución del movimiento. En este caso, la ejecución
43
del movimiento sería trivial, ya que los puntos de consigna del servo se podrían leer
rápidamente de memoria. Otra forma posible es pre-calcular la solución de la articulación
para cada intervalo de muestreo y luego realizar interpolaciones de articulación utilizando
polinomios de bajo grado para ajustarlo a través de estos puntos intermedios para generar
los puntos de consigna del servo. La dificultad de este método es que el número de puntos
intermedios necesitados para mantener la mano del manipulador aceptablemente próxima
a la trayectoria en línea recta cartesiana depende del movimiento particular que se realice.
Cualquier intervalo predeterminado suficientemente pequeño para garantizar pequeñas
desviaciones necesitará una cantidad superflua de tiempo y de espacio de memoria.
A la vista de esto, Taylor (1979) propuso una estrategia de movimiento en el espacio de

la variable de articulación, llamada trayectoria de la articulación de desviación acotada,
que selecciona suficientes puntos intermedios durante la fase de pre-planificación para
garantizar que la desviación de la mano del manipulador de la trayectoria cartesiana en
línea recta sobre cada segmento de movimiento permanece dentro de acotaciones de error
pre-especificadas.
El esquema comienza pre-calculando todos los vectores solución de las articulaciones 𝒒𝑖

correspondientes a los puntos nudos 𝑭𝑖 a lo largo de la línea recta deseada. Los vectores
𝒒𝑖 del espacio de articulación se utilizan como puntos nudos para una estrategia de
interpolación en el espacio de las variables de articulación análoga a la que se utiliza para
la ecuación de posición del camino del control cartesiano. Esto es, para el movimiento
desde el punto nudo 𝒒0 hasta 𝒒1 , tenemos
𝑇 −𝑡
𝒒(𝒕) = 𝒒𝟏 − 𝑇1 ∆𝒒𝟏 (1.82)
1
y, para la transición entre 𝒒𝟎 a 𝒒𝟏 y 𝒒𝟏 a 𝒒𝟐 , tenemos:

2 2
(𝜏−𝑡 ′ ) (𝜏+𝑡 ′ )
𝒒(𝑡′) = 𝒒𝟏 − ∆𝒒𝟏 + ∆𝒒𝟐 (1.83)
donde, ∆𝒒𝟏 = 𝒒𝟏 − 𝒒𝟎 , ∆𝒒𝟐 = 𝒒𝟐 − 𝒒𝟏 y 𝑇1 , 𝑇2 , 𝜏 y 𝑡′ tienen el mismo sentido que en el

esquema de control del camino cartesiano. Estas ecuaciones consiguen velocidad
uniforme entre los puntos nudos de la articulación y efectúan transiciones suaves con una
desaceleración y una aceleración contantes entre los segmentos. Sin embargo, la
herramienta se puede desviar sustancialmente del camino en línea recta deseado. El error
de desviación se puede ver a partir de la diferencia entre las 𝑭𝒋 (𝒕), que corresponde a la
44
mano del manipulador en el punto nudo de la articulación 𝒒𝒋 (𝒕) y 𝑭𝒄 (𝒕), que corresponde
a la mano del manipulador en el correspondiente camino cartesiano, en el punto medio
del segmento. Luego, definimos las desviaciones en desplazamiento y rotación, de la
siguiente manera:
𝛿𝑝 = |𝒑𝒋 (𝒕) − 𝒑𝒄 (𝒕)| (1.84)
𝛿𝑅 = |𝑝𝑎𝑟𝑡𝑒 𝑎𝑛𝑔𝑢𝑙𝑎𝑟 𝑑𝑒 𝑅𝑜𝑡(𝒏, ∅) = 𝑹𝒄 (𝒕)−𝟏 𝑹𝒇 (𝒕)| (1.85)
Especificando las desviaciones máximas, 𝛿𝑝 𝑚á𝑥 y 𝛿𝑅 𝑚á𝑥 , para las partes de

desplazamiento y orientación, respectivamente, desearíamos acotar los errores de
desviación como:
𝛿𝑝 < 𝛿𝑝 𝑚á𝑥 y 𝛿𝑅 < 𝛿𝑅 𝑚á𝑥 (1.86)
Con estas acotaciones sobre el error de desviación, necesitamos seleccionar suficientes

puntos intermedios entre dos puntos nudos de articulación consecutivos de forma tal que
se satisfaga la ecuación (1.74). Taylor (1979) presentó una trayectoria de articulación de
desviación acotada que es básicamente un método bisector recursivo para determinar los
puntos intermedios de forma tal que se verifique la ecuación (1.74). El algoritmo converge
bastante rápidamente para producir un buen conjunto de puntos intermedios, aunque no
son un conjunto mínimo. Su algoritmo es como sigue.
Algoritmo BDJP: Dadas las acotaciones de desviaciones de error máximo 𝛿𝑝 𝑚á𝑥 y
𝛿𝑅 𝑚á𝑥 para la posición y orientación de la herramienta, respectivamente, y los puntos

nudos cartesianos 𝑭𝒊 a lo largo de la línea recta deseada, este algoritmo selecciona
suficientes puntos nudos de la articulación de manera que la mano del manipulador no se
desviará de las acotaciones de error especificadas a lo largo de la línea recta deseada, este
algoritmo selecciona suficientes puntos nudos de la articulación de manera que la mano
del manipulador no se desviará de las acotaciones de error especificadas a lo largo de la
línea recta deseada.
Paso 1. Calcular la solución de la articulación. Calcular los vectores solución de la

articulación 𝒒𝟎 y 𝒒𝟏 que corresponde, respectivamente, a 𝑭𝟎 y a 𝑭𝟏 .
Paso 2. Determinar punto medios en el espacio de la articulación. Calcular puntos medios

en el espacio de las variables de articulación.
45
𝒒𝒎 = 𝒒𝟏 − 1⁄2 ∆𝒒𝟏 (1.87)
donde ∆𝒒𝟏 = 𝒒𝟏 − 𝒒𝟎 y utilizar 𝒒𝒎 para calcular el correspondiente sistema de la mano

𝑭𝒎 de los valores de articulación 𝒒𝒎 .
Paso 3: Determinar puntos medios en el espacio cartesiano. Calcular el punto medio 𝑭𝑪

del camino cartesiano correspondiente:
𝒑𝟎 +𝒑𝟏 𝜃
𝒑𝒄 = y 𝑹𝒄 = 𝑹𝟏 𝑅𝑜𝑡(𝒏𝟏 , − 2) (1.88)
𝟐
donde 𝑅𝑜𝑡(𝒏, 𝜃) = 𝑹𝒐 −𝟏 𝑹𝟏 .
Paso 4: Determinar los errores de desviación. Calcular el error de desviación entre 𝑭𝒎 y

𝑭𝒄
𝛿𝑝 = |𝒑𝒎 − 𝒑𝒄 | (1.89)
𝛿𝑅 = |𝑝𝑎𝑟𝑡𝑒 𝑎𝑛𝑔𝑢𝑙𝑎𝑟 𝑑𝑒 𝑅𝑜𝑡(𝒏, ∅) = 𝑹𝒄 −𝟏 𝑹𝒎 | (1.90)
Paso 5: Comprobación de las cotas de error. Si 𝛿𝑝 < 𝛿𝑝 𝑚á𝑥 y 𝛿𝑅 < 𝛿𝑅 𝑚á𝑥 , entonces
parar. De lo contrario, calcular el vector solución de la articulación 𝑞𝑐 corresponde al
punto medio del espacio cartesiano 𝑭𝒄 , y aplicar los pasos S2 a S5 recursivamente para
los dos subsegmentos sustituyendo 𝑭𝟏 por 𝑭𝒄 y 𝑭𝒄 por 𝑭𝟎 .
La convergencia del algoritmo anterior es bastante rápida. El error de desviación máximo

se suele reducir aproximadamente por un factor de 4 para cada iteración recursiva. Taylor
(1979) investigó la velocidad de convergencia del algoritmo para un robot cilíndrico (dos
articulaciones prismáticas acopladas con una articulación giratoria) y encontró que va
desde un factor de 2 hasta un factor de 4, dependiendo de las posiciones de la mano del
manipulador.
En resumen, el esquema de la trayectoria de la articulación con desviación acotada

descansa sobre una fase de pre-planificación para interpolar suficientes puntos
intermedios en el espacio de la variable de articulación de forma que el manipulador se
pueda mover en el espacio de las variables de articulación sin desviarse más que un error
pre-especificado de la línea recta deseada.
La figura 1.19 muestra una comparación entre los dos métodos expuestos en esta sección,
como vemos el método de desviación acotada reduce rápidamente el error de desviación
46
a medida que incrementa el número de segmentos utilizados, la trayectoria no es exacta,
pero el error de desviación es bastante pequeño y el tiempo de cálculo, mucho más rápido.
Fig. 1.19. Comparación entre el esquema de control de camino cartesiano (cartesian

path montion) y de desviación acotada (joint-space motion).
47
1.4. Formulación del Problema
¿Es posible desarrollar un algoritmo que nos permita realizar la generación de

trayectorias para un Brazo Robótico de 5 grados de libertad, utilizando cuaterniones
unitarios?
1.5. Justificación del Estudio:

El presente proyecto busca realizar una investigación que permita recopilar la
información necesaria para sentar las bases teóricas que nos permitan entender el
comportamiento de los Robots Manipuladores, y con ello poder desarrollar una aplicación
tan importante y usada en la industria, como la generación de trayectorias utilizando
cuaterniones unitarios, un concepto matemático que permite reducir el costo
computacional y controlar la posición y orientación del efector final de manera
independiente. Esta investigación nos brinda los siguientes beneficios:
- Nos presenta un marco teórico que recopila información de distintas fuentes y aportes
propios, referentes a los robots manipuladores, lo cual nos servirá para adentrarnos
más en este tema.
- Abordaremos un problema real de un brazo robótico con un diseño poco
convencional, lo cual generará una dificultad que deberemos solucionar utilizando
con inteligencia los conceptos vectoriales.
- Con los conocimientos adquiridos, estaremos más preparados para enfrentarnos a
estos sistemas en nuestra vida profesional, ya que cada vez son más las empresas que
cuentan con esta tecnología en sus plantas.
- El uso de cuaterniones unitarios nos permite reducir el tiempo requerido para generar
trayectorias en robots manipuladores, así como controlar independientemente la
posición y orientación del efector final
- La escuela de ingeniería Mecatrónica de la Universidad Nacional de Trujillo, cuenta
con las piezas pre-fabricadas que se utilizaron para generar el modelo del brazo
robótico logrado. Los cálculos, algoritomos, códigos y simulaciones pueden ser
utilizados en investigaciones futuras.
1.6. Hipótesis
Aplicando el método de Denavit-Hartenberg, la cinemática directa; la cinemática
inversa, mediante el método de desacoplo cinemático y la generación de trayectorias,
utilizando cuaterniones unitarios, se podrá conseguir un algoritmo que realice el cálculo
48
necesario para generar las trayectorias de un Brazo Robótico de 5 grados de libertad en
tiempo real.
1.7. Objetivos
1.7.1. Objetivo General
Desarrollar, un algoritmo que nos permita generar trayectorias para un brazo
robótico de 5 grados de libertad utilizando cuaterniones unitarios
1.7.2. Objetivos Específicos
- Realizar el ensamblaje del brazo robótico en Solidworks, para determinar su

forma y dimensiones.
- Desarrollar la cinemática directa del brazo robótico.
- Desarrollar la cinemática inversa del brazo robótico.
- Verificar los algoritmos de cinemática inversa y directa de tal manera que se
obtenga un error menor que el 1%.
- Desarrollar el algoritmo de generación de trayectorias, utilizando
cuaterniones unitarios.
- Verificar el algoritmo de tal manera que nos arroje un error menor a 1 cm.
para la posición y 5° para la orientación.
49
CAPÍTULO II: MÉTODO
2.1. Diseño de investigación:
Investigación aplicada.
2.2.Variables
2.2.1. Variables independientes.
 Trayectoria lineal deseada para el efector final.
 Error máximo de desviación para la trayectoria
 Tiempo de paso.
 Estructura y dimensiones del brazo robótico.
2.2.2. Variables dependientes

 Trayectoria real obtenida.
 Desviación de la trayectoria lograda respecto de la trayectoria
deseada.
 Tiempo de cálculo.
 Desplazamiento angular de cada articulación.
2.3. Población y Muestra

2.3.1. Población:
Robots manipuladores de 5 grados de libertad para aplicaciones de traslado
de objetos
2.3.2. Muestra:
Robot manipulador de 5 grados de libertad con cinco articulaciones giratorias.
Cuya estructura se basa en el ensamblaje de piezas de aluminio pre-fabricadas
y servomotores del modelo MG-996.
2.4. Técnicas e Instrumentos

 Se utilizará el programa MATLAB, para realizar los cálculos necesarios y
analizar el comportamiento del sistema.
 Se utilizará el programa SOLIDWORKS, para el diseño de las piezas y
ensamblaje.
 Se recurrirá a diferentes fuentes bibliográficas, libros, artículos, publicaciones,
etc. que cuenten con la información necesaria para realizar la investigación.
50
CAPÍTULO III: DESARROLLO Y RESULTADOS
3.1.ESTRUCTURA MECÁNICA DEL ROBOT MANIPULADOR
La estructura mecánica del brazo robótico está basada en piezas pre-fabricadas de
aluminio, las cuales vienen siendo utilizadas últimamente en los centros de estudio, para
aplicaciones prácticas que involucran áreas como la robótica y electrónica. En este
apartado describiremos la estructura que se logró ensamblar.
Además se utilizan servomotores del modelo MG-996, los cuales pueden generar un
torque máximo de 12 Kg-cm.
En el Anexo A, presentamos los planos detallados de cada una de las piezas utilizadas.
Fig. 3.1. Estructura del brazo robot en Solidworks

(Fuente: Elaboración Propia)
Los motivos por los cuales utilizamos piezas pre-fabricadas son:

- Construcción de sistemas precisos a bajo costo: Las piezas provienen de un proceso
de mecanizado computarizado.
- Fácil montaje.
- Se logra construir un brazo robótico que nos permite realizar aplicaciones similares
a las vistas en la industria.
51
3.2. CINEMÁTICA DIRECTA
3.2.1. Asignación de tramas a las articulaciones del robot y obtención de
parámetros de Denavit-Hatenberg
De acuerdo al algoritmo de Denavit-Hartenberg asignamos los sistemas de coordenadas
a las articulaciones y efector final del brazo robótico.
Fig. 3.2 Sistemas de coordenadas para el brazo robótico, según Algoritmo D-H
Tabla 3.1 Parámetros de Denavit-Hartenberg

Art 𝜽 𝒅 𝒂 𝜶 Recorrido
1 𝜃1 − 90 𝐿1 −𝑑1 90° −90° … 90°
2 𝜃2 + 90 0 𝐿2 0° −90° … 90°
3 𝜃3 0 𝐿3 0° −90° … 90°
4 𝜃4 𝑑4 𝐿4 −90° −180° … 0°
5 𝜃5 𝐿5 0 0° −90° … 90°
52
El brazo en la Figura, se encuentra en la posición:
𝜃1 = 𝜃2 = 𝜃3 = 𝜃4 = 𝜃5 = 0°. (3.1)
3.2.2. Obtención de Matrices de Transformación Homogénea y cinemática directa.

En esta etapa de la investigación se procedió a calcular las Matrices de Transformación
Homogénea que nos permitieran relacionar la trama del efector final del robot, con el
sistema de coordenadas de la base.
Para ello utilizamos el algoritmo de Denavit-Hartenberg, el cual nos permite fácilmente
encontrar las matrices de transformación homogénea del manipulador.
Luego de realizar las verificaciones correspondientes, utilizando Solidworks, se concluye
que estas son las expresiones que definen correctamente el brazo robótico:
𝑠𝑒𝑛𝑞1 0 −𝑐𝑜𝑠 𝑞1 −𝑑1 𝑠𝑒𝑛 𝑞1
0 −𝑐𝑜𝑠𝑞1 0 −𝑠𝑒𝑛 𝑞1 𝑑1 𝑐𝑜𝑠 𝑞1
1𝐴 = ( 0 1 0
) (3.2)
𝐿1
0 0 0 1
−𝑠𝑒𝑛𝑞2 −𝑐𝑜𝑠 𝑞2 0 −𝐿2 𝑠𝑒𝑛 𝑞2

1 𝑐𝑜𝑠𝑞2 −𝑠𝑒𝑛 𝑞2 0 𝐿2 𝑐𝑜𝑠 𝑞2
2𝐴 = ( 0 0 1
) (3.3)
0
0 0 0 1
𝑐𝑜𝑠𝑞3 −𝑠𝑒𝑛𝑞3 0 𝐿3 𝑐𝑜𝑠𝑞3

2 𝑠𝑒𝑛𝑞3 𝑐𝑜𝑠𝑞3 0 𝐿2 𝑠𝑒𝑛𝑞3
3𝐴 = ( 0 0 1
) (3.4)
0
0 0 0 1
𝑐𝑜𝑠𝑞4 0 −𝑠𝑒𝑛𝑞4 𝐿4 𝑐𝑜𝑠𝑞4

3 𝑠𝑒𝑛𝑞4 0 𝑐𝑜𝑠𝑞4 𝐿4 𝑠𝑒𝑛𝑞4
4𝐴 = ( 0 −1 0
) (3.5)
𝑑4
0 0 0 1
𝑐𝑜𝑠𝑞5 −𝑠𝑒𝑛𝑞5 0 0
4 𝑠𝑒𝑛𝑞5 𝑐𝑜𝑠𝑞5 0 0)
5𝐴 = ( 0 0 1 𝐿5
(3.6)
0 0 0 1
La cinemática directa consiste calcular la matriz 05𝐴 que relacione el efector final con la base
del robot y lo hacemos realizando la siguiente operación:
0
5𝐴 = 01𝐴 12𝐴 23𝐴 34𝐴 45𝐴 (3.7)
La expresión resultante es muy extensa, por lo cual, en la ecuación
53
3.8 presentamos solamente su estructura, en el anexo A podemos encontrar su desarrollo
completo, obtenido utilizando Matlab. El código del programa lo presentamos en el anexo
A2.
0 0 0 0
5𝐴11 5𝐴12 5𝐴13 5𝐴14
0 0 0 0
0 5𝐴21 5𝐴22 5𝐴23 5𝐴24
5𝐴 = 0 0 0 0 (3.8)
5𝐴31 5𝐴32 5𝐴33 5𝐴34
0 0 0 0
( 5𝐴41 5𝐴42 5𝐴43 5𝐴44 )
3.2.3. Cinemática directa utilizando cuaterniones
Basándonos en el modelo de cuateriniones, podemos obtener la cinemática directa del

manipulador. Para lo cual, tomaremos en cuenta los ejes de giro de cada articulación,
tomando como referencia el sistema de coordenadas de la base. También tomamos en
cuenta las relaciones de posición para cada articulación. Con esta información
construimos los siguientes pares cuaternion-vector, correspondientes a cada grado de
libertad.
𝜃 𝜃
𝑄1 = ([cos ( 21 ) , sin ( 21 ) < 0,0,1 > ] , < −𝑑1 sin 𝜃1 , 𝑑1 cos 𝜃1 , 𝐿1 >) (3.9)
𝜃 𝜃
𝑄2 = ([cos ( 22 ) , − sin ( 22 ) < 1,0,0 > ] , < 0 , 𝐿2 sin 𝜃2 , 𝐿2 cos 𝜃2 >) (3.10)
𝜃 𝜃
𝑄3 = ([cos ( 23 ) , − sin ( 23 ) < 1,0,0 > ] , < 0 , 𝐿3 sin 𝜃3 , 𝐿3 cos 𝜃3 >) (3.11)
𝜃 𝜃
𝑄4 = ([cos ( 24 ) , − sin ( 24 ) < 1,0,0 > ] , < −𝑑4 , 𝐿4 sin 𝜃4 , 𝐿4 cos 𝜃4 >)(3.12)
𝜃 𝜃
𝑄5 = ([cos ( 25 ) , sin ( 25 ) < 0,1,0 > ] , < 0 , 𝐿5 , 0 >) (3.13)
Multiplicando consecutivamente los cuaterniones duales, obtenemos la expresión

equivalente a la cinemática directa del manipulador. La cual nos brindará la orientación
del efector final mediante un cuaternion 𝑞 y la posición, mediante un vector 𝑝 para valores
definidos de 𝜃1 , 𝜃2 , 𝜃3 , 𝜃4 y 𝜃5 .
𝑄𝑡 (𝑞, 𝑝) = 𝑄1 𝑄2 𝑄3 𝑄4 𝑄5 (3.14)
Además, debemos considerar que, la orientación inicial del efector final del manipulador
respecto de la base, para valores de 𝜃1 = 𝜃2 = 𝜃3 = 𝜃4 = 𝜃5 = 0, no es nula. Por lo cual
54
se debe considerar en el cálculo. Dicha orientación se representa mediante el cuaternion
siguiente:
𝑄𝑖 = ([0.5 − 0.5 − 0.5 − 0.5], < 0,0,0 >) (3.15)
Con lo cual la expresión final para la cinemática directa sería:
𝑄𝑓 = 𝑄𝑡 𝑄𝑖 (3.16)
3.2.4. Verificación de algoritmos de cinemática directa
Para verificar nuestros algoritmos de cinemática directa usando cuaterniones y matrices

de transformación homogénea, corroboraremos que nos dan el mismo resultado. Como
sabemos, la cinemática directa usando cuaterniones nos dará como resultado un
cuaternion v y un vector p, que representarán la orientación y la posición del efector final
del manipulador, respectivamente; además, la cinemática directa convencional nos
brindará una matriz R que representará la orientación y un vector P que representará la
posición del efector final.
Para comparar ambos resultados, luego de obtener el vector v, lo convertiremos a su

expresión equivalente r1, en forma de matriz de rotación, para luego compararla con la
matriz de rotación r2 obtenida mediante la cinemática directa convencional.
El anexo A.4 contiene el programa de Matlab utilizando para este fin, dicho programa
almacena las matrices r1 y r2 correspondientes a un conjunto de ángulos de articulación
dados en vectores en vectores en los vectores R1 y R2, respectivamente. Para obtener el
error, debemos comprar cada elemento de los vectores obtenidos, según:
𝐸𝑟 = |𝑅1(𝑖, 𝑗) − 𝑅2(𝑖, 𝑗)| (3.17)
Para 50 grupos diferentes de ángulos de articulación 𝜃1 , 𝜃2 , 𝜃3 , 𝜃4 y 𝜃5 ; i=3 y j=150.
El error obtenido luego de ejecutar el programa del Anexo A.4, es prácticamente cero,
𝑒 = 4,0134 𝑥 10−16 (3.18)
Por lo tanto nuestros algoritmos de cinemática directa nos brindan el mismo resultado, la
verificación de la cinemática directa, relacionándola con los resultados obtenidos por
cinemática inversa, se realiza más adelante.
3.3.CINEMÁTICA INVERSA DEL MANIPULADOR
55
Como se explicó en la sección 1.3.5, para resolver este problema es necesario interpretar
la estructura del manipulador, de tal manera que los tres primeros grados de libertad
tengan la finalidad de determinar la posición del extremo y los siguientes, la orientación.
El método más utilizado para resolver la cinemática de los manipuladores es el desacoplo
cinemático, el cual utiliza esta “división en dos partes” de la estructura, para simplificar
el cálculo de los ángulos requeridos para una determinada posición y orientación.
Cuando diseñamos un sistema, se busca que los cálculos sean tan sencillos como sea
posible, un criterio que siempre se toma en cuenta es el que para el diseño de
manipuladores consiste en hacer que se intersecten en un solo punto los ejes de las
articulaciones de los grados de libertad que determinan la orientación (como se explica
en la sección 1.3.4.2); en el caso de los manipuladores de seis grados, serían los tres
últimos; en el caso de manipuladores de cinco grados, como el que estamos usando para
esta investigación, serían los dos últimos.
Los robots industriales consideran este criterio, el cual es expuesto por la mayoría de
autores en su discusión sobre cinemática inversa, como Barrientos (1998); Fu, Gonzales
y Lee (1988) y Craig (2006). Sin embargo la presente investigación está enfocada en el
desarrollo de un modelo para un brazo robótico construido a partir de piezas pre-
fabricadas, que pueda realizar las aplicaciones vistas en la industria, el cual no nos permite
cumplir con el criterio expuesto anteriormente. Esto nos dificulta un poco el cálculo de
su cinemática, sin embargo, esto nos sirvió para aplicar recursos distintos a los que
generalmente se utilizan en la literatura tradicional.
Adicionalmente a los conocidos métodos de desacoplo, geométrico y algebraico,

aplicamos conceptos de vectores.
La cinemática inversa consiste en encontrar los valores de todos los ángulos de las
articulaciones del robot, para que alcancen una posición y orientación dadas. Es decir,
dados todos los elementos de la matriz 05𝐴 (Ec. 1.16), debemos encontrar [𝑞1 , 𝑞2 , 𝑞3 , 𝑞4 , 𝑞5 ]
3.3.1. DESACOPLO CINEMÁTICO.
El método de desacoplo cinemático consiste en “dividir el problema cinemático inverso

en dos”, utilizando el hecho de que los tres primeros grados determinan la posición y los
siguientes, la orientación del efector final. Así, cuando las articulaciones de orientación
se intersectan en un solo punto, se calculan sus coordenadas y con ello los valores de las
56
tres primeras articulaciones (𝑞1 , 𝑞2 y 𝑞3 ), para luego, con estos valores y los datos de
orientación, calcular los valores de las articulaciones restantes.
Sin, embargo, debido a la particular estructura de nuestro prototipo, no existe dicho punto
de intersección, por lo cual procederemos a utilizar conceptos sobre vectores para
encontrar la posición del extremo del eslabón 3 (𝑃03 𝑋 , 𝑃03 𝑌 y , 𝑃03 𝑍 ) respecto del sistema
de coordenadas de la base.
CALCULO DE 𝑷𝟎𝟑 𝑿 , 𝑷𝟎𝟑 𝒀 y , 𝑷𝟎𝟑 𝒁
Una vez conocidas la posición y orientación deseadas para el efector final del robot, con
respecto a su base; es decir, todos los elementos de la matriz 05𝐴 (Ec. 1.16), procederemos a
calcular las componentes de 𝑃03 ( 𝑃03 𝑋 , 𝑃03 𝑌 y , 𝑃03 𝑍 ), que es origen del sistema de
coordenadas 𝑆3 (Fig. 3.3), el cual está solidario al extremo del tercer eslabón del
manipulador.
Fig. 3.3 Sistemas de coordenadas para los dos últimos grados de libertad, según
Algoritmo D-H.
⃗⃗𝟒 ,
Según disposición de los sistemas de coordenadas del robot necesitamos conocer 𝟎𝒁
𝟎⃗⃗ ⃗⃗⃗𝟒 , para lo cual realizaremos el siguiente procedimiento:
𝒀𝟒 y 𝟎𝑿
1. Cálculo de 𝟎⃗𝒁⃗𝟒 :
Debido a la estructura del robot:
𝟎⃗⃗
𝒁𝟒 = 𝟎⃗𝒁⃗𝟓 (3.19)
Por lo tanto su valor ya es conocido, debido a que 𝟎⃗𝒁⃗𝟓 = 𝒏, según las ecuaciones 1.4
y 1.16, es un parámetro de entrada para el cálculo de la cinemática inversa.
57
2. Cálculo de 𝟎⃗𝒀⃗𝟒
Para definir completamente este vector, necesitamos conocer sus tres componentes
𝟎⃗⃗
𝒀𝟒 𝒙 , 𝟎⃗𝒀⃗𝟒 𝒚 y 𝟎⃗𝒀⃗𝟒 𝒛 .
Haciendo un análisis del movimiento del brazo robótico, podemos deducir fácilmente
que el vector 𝟎⃗𝒀⃗𝟒 jamás tendrá una componente en la dirección 𝑍⃗ del sistema de
coordenadas de la base 𝑺𝟎 . Por lo tanto:
𝟎⃗⃗
𝒀𝟒 𝒛 = 0. (3.20)
De la estructura del robot, sabemos que 𝟎⃗𝒀⃗𝟒 y 𝟎⃗𝒁⃗𝟓 siempre van a ser perpendiculares.
Por lo tanto, su producto escalar debe ser igual a cero, esto es:
𝟎⃗⃗
𝒀𝟒 . 𝟎⃗𝒁⃗𝟓 = 0. (3.21)
0
𝑌4 𝒙 0𝑍4 𝒙 + 0𝑌4 𝒚 0𝑍4 𝒚 + 0𝑌4 𝒛 0𝑍4 𝑧 = 0, (3.22)
de donde podemos obtener:

− 0𝑌4 𝑦 0𝑍4 𝒚
0
𝑌4 𝒙 = 0𝑍
(3.23)
4𝒙
Del módulo del vector unitario:

0 2 2 2
𝑌4 𝒙 + 0𝑌4 𝒚 + 0𝑌4 𝒛 = 1 (3.24)
0 2 2
𝑌4 𝒙 = 1 − 0𝑌4 𝒚 (3.25)
Relacionando las ecuaciones 3.23 y 3.25, obtenemos

− 0𝑌4 𝑦 0𝑍4 𝒚 2
0𝑍 = 1 − 0𝑌4 𝒚 (3.26)
4𝒙
Despejando:
0 1
𝑌4 𝒚 = (3.27)
√1+(0𝑍4𝒚 )2
0𝑍
4𝒙
La presencia de 0𝑍4 𝒙 en el denominador es peligrosa, debido a que cuando esta

componente sea igual a cero, nos generaría una inconsistencia en el cálculo, debido
a la división entre cero. Por lo cual, seguimos operando y le damos la siguiente forma:
0𝑍
0 4𝒙
𝑌4 𝒚 = (3.28)
𝑅
2 2
donde, 𝑅 = √ 0𝑍4 𝒙 + 0𝑍4 𝒚
58
Con esta forma, si analizamos las posibilidades geométricas, la división de una
𝐶
constante entre cero (0) no sucederá.
Luego, reemplazando 3.28 en 3.25, obtenemos:

− 0𝑍4 𝒚
0
𝑌4 𝒙 = 2 2
(3.29)
√ 0𝑍4 𝒙 + 0𝑍4 𝒚
2 2
donde, 𝑅 = √ 0𝑍4 𝒙 + 0𝑍4 𝒚 .
3. Cálculo de 𝟎⃗𝑿
⃗⃗𝟒
- Debido a que 𝟎⃗𝑿

⃗⃗𝟒 y 𝟎⃗𝒁⃗𝟒 siempre serán perpendiculares, entonces:
𝟎⃗⃗⃗
𝑿𝟒 . 𝟎⃗𝒁⃗𝟒 = 0 (3.30)
De ello se obtiene:
0𝑋 0𝑍 0𝑌
0 4𝒚 4𝒙 4𝒚
𝑋4 𝒛 = 0𝑍
( 0𝑌
− 0𝑍4 𝒚 ) (3.31)
4𝒛 4𝒙
Tomando en cuenta la ecuación 3.14:

0𝑋 0𝑍 2 + 0𝑍 2
0 4𝒚 4𝒙 4𝒚
𝑋4 𝒛 = 0𝑍
( ) (3.32)
4𝒛 − 0𝑍4 𝒚
- ⃗⃗⃗𝟒 y 𝟎⃗𝒀⃗𝟒 siempre serán perpendiculares, entonces:

Debido a que 𝟎𝑿
𝟎⃗⃗⃗
𝑿𝟒 . 𝟎⃗𝒀⃗𝟒 = 0 (3.33)
Despejando,
− 0𝑋4 𝒚 0𝑌4 𝑦
0
𝑋4 𝒙 = 0𝑌
(3.34)
4𝒙
Utilizando la ecuación 3.23, tenemos:

0𝑋 0𝑍
0 4𝒚 4𝑥
𝑋4 𝒙 = 0𝑍 (3.35)
4𝑦
- De la definición de vector unitario
0 2 2 2
𝑋4 𝒙 + 0𝑋4 𝒚 + 0𝑋4 𝒛 = 1 (3.36)
Reemplazando, según las ecuaciones 3.32 y 3.35 y desarrollando, obtenemos:

0𝑍 0𝑍
0 4𝒛 4𝑦
𝑋4 𝒚 = (3.37)
𝑅2
2 2 2 2 2 2
donde: 𝑅𝟐 = √ 0𝑍4 𝒙 0𝑋4 𝒛 + 0𝑍4 𝒛 0𝑍4 𝒚 + ( 0𝑍4 𝒙 + 0𝑍4 𝒚 )2
59
- Reemplazando 3.37 en 3.31 y 3.35, obtenemos:
0𝑍 2 + 0𝑍 2
0 4𝒙 4𝒚
𝑋4 𝒛 = ( ) (3.38)
−𝑅𝟐
0𝑍 0𝑍
0 4𝒙 4𝑧
𝑋4 𝒙 = (3.39)
𝑅2
Combinando los valores de nuestras tres variables, obtenemos cuatro soluciones

posibles, teniendo en cuenta lo siguiente:
0
- 𝑌4 𝒚 tiene dos soluciones
0
- 𝑌4 𝒙 , depende de 0𝑌4 𝒚 .
0
- 𝑋4 𝒚 depende de 0𝑌4 𝒙 y 0𝑌4 𝒚 .
0
- 𝑋4 𝒚 tiene dos soluciones
0
- 𝑋4 𝑧 y 0𝑋4 𝑥 dependen de 0𝑋4 𝒚
La figura 3.4 nos muestra un esquema de las posibles soluciones que tenemos hasta
el momento
Fig. 3.4 Esquema que muestra las cuatro posibles soluciones para encontrar el
punto 𝑃03
4. Reducción de soluciones utilizando condición de sistemas de coordenadas dextrógiro

Barrientos (1997, p. 98), en su exposición sobre el algoritmo D-H, nos indica que los
sistemas de coordenadas solidarios a los eslabones deben ser dextrógiros, estos cumplen
la siguiente condición
𝑋⃗𝑥𝑌
⃗⃗ = 𝑍⃗
60
⃗⃗𝑥 𝑍⃗ = 𝑋⃗
𝑌
𝑍⃗𝑥𝑋⃗ = 𝑌
⃗⃗ (3.40)
En donde, 𝑋⃗, 𝑌
⃗⃗ 𝑦 𝑍⃗ son los vectores unitarios correspondientes a cada sistema de
coordenadas del robot.
Por lo tanto, si aplicamos esta condición a los ejes del sistema de coordenadas 𝑺𝟒 ,
obtenemos:
0
𝑋4 𝑥 0𝑌4 = 0𝑍4
0
𝑌4 𝑥 0𝑍4 = 0𝑋4
0
𝑍4 𝑥 0𝑋4 = 0𝑌4 (3.41)
Donde 0𝑋4 = [ 0𝑋4 𝑥 , 0𝑋4 𝑦 , 0𝑋4 𝑧 ], 0𝑌4 = [ 0𝑌4 𝑥 , 0𝑌4 𝑦 , 0𝑍4 𝑧 ] y 0𝑍4 = [ 0𝑍4 𝑥 , 0𝑍4 𝑦 , 0𝑍4 𝑧 ]
El sistema de coordenadas que elijamos debe cumplir con las tres ecuaciones de (3.31)
Esta condición nos permite reducir a dos nuestras posibles soluciones
ELECCIÓN DE LA SOLUCIÓN CORRECTA
Para seleccionar una solución de las dos posibles que tenemos hasta el momento,
realizaremos un análisis del movimiento del brazo robótico, con lo cual implementaremos
ciertas condiciones, observando y relacionando el valor de la articulación 𝑞1 y la dirección
del vector 0𝑌4 .
- Si −90° < 𝜃1 < 90°, entonces: 0𝑌4 𝑥 > 0°

- Si 𝜃1 = −90, entonces: 0𝑌4 𝑦 < 0°
- Si 𝜃1 = 90, entonces: 0𝑌4 𝑦 > 0°
Adicionalmente:
- Si −180° < 𝜃1 < −90° ó 90° < 𝜃1 < 180°, entonces 0𝑌4 𝑥 < 0°
Para la aplicación nuestra, debido a que los servomotores tienen un recorrido de 180°,
solo utilizaremos las tres primeras condiciones, la cuarta nos sirve para cubrir un rango
de 360°.
61
3.3.2. OBTENCIÓN DE LAS TRES PRIMERAS VARIABLES DE
ARTICULACIÓN.
3.3.2.1. SOLUCIÓN PARA Q1 UTILIZANDO UN MÉTODO GEOMÉTRICO

Aplicaremos un método geométrico, basándonos en lo expuesto en Fu, Gonzales y Lee
(1998, pp. 66-67).
Haciendo un análisis de la estructura del robot, nos damos cuenta que para el método
geométrico que se aplicará para calcular este parámetro, no es necesario conocer 𝑃03 , con
𝑃04 será suficiente. Intentamos también resolver 𝑞2 y 𝑞3 de la misma forma, pero como
será expuesto más adelante, no fue posible, por lo cual, tuvimos que calcular 𝑃03 de todas
maneras.
La articulación 𝑞1 determina el parámetro BRAZO, el cual, como se mencionó en la
sección 1.3.5, tiene dos posibilidades, {IZQUIERDO, DERECHO},
CONFIGURACIÓN BRAZO IZQUIERDO
Fig. 3.5 Desarrollo geométrico para calcular 𝑞1 en la configuración BRAZO

IZQUIERDO.
Según se definió mediante el algoritmo D-H, en la posición dada en la figura 3.5, 𝜃1
debería ser un ángulo positivo. Por lo cual, trazamos dos ejes auxiliares que nos servirán
para darle el signo que deseamos.
Hacemos:
62
⃗⃗⃗⃗⃗
𝑋′ = 𝑌⃗⃗⃗⃗𝑜
⃗⃗ ′ = −𝑋⃗𝑜
𝑌 (3.42)
De la Figura 3.5, geométricamente:
𝜃1 = ∅ − 𝛼 (3.43)
𝐴𝐵 = 𝑑4
𝑂𝐵 = 𝑅 = √ 𝑃𝑥 2 + 𝑃𝑦 2
𝑂𝐴 = 𝑟 = √𝑃𝑥 2 + 𝑃𝑦 2 − 𝑑4 2
Sabemos que:
sin 𝜃1 𝐿 = sin(∅ − 𝛼) = sin ∅ cos 𝛼 − cos ∅ sin 𝛼 (3.35)
Calculamos los valores para las funciones trigonométricas de acuerdo al sistema de
coordenadas auxiliar 𝑋 ′ 𝑌 ′ . Para el cual, 𝑃′𝑦 = −𝑃𝑥 y 𝑃′𝑥 = 𝑃𝑦 , por tanto:
−𝑃𝑥 𝑟 𝑃𝑦 𝑑4
sin 𝜃1 𝐿 = −
𝑅 𝑅 𝑅 𝑅
−𝑃𝑥 𝑟−𝑃𝑦 𝑑4
sin 𝜃1 𝐿 = (3.44)
𝑅
Además
cos 𝜃1 𝐿 = cos(∅ − 𝛼) = cos ∅ cos 𝛼 + sin ∅ sin 𝛼 (3.45)
Entonces:
𝑃𝑦 𝑟−𝑃𝑥 𝑑4
cos 𝜃1 𝐿 = (3.46)
𝑅2
CONFIGURACIÓN BRAZO DERECHO
Fig. 3.6 Desarrollo geométrico para calcular 𝑞1 en la configuración BRAZO

DERECHO.
63
De la figura 3.6, debido a que ∅ es un ángulo positivo; 𝜃1 , un ángulo negativo y 𝛼 no
tiene signo,
𝜃1 − ∅ = 𝜋 + 𝛼 (3.47)
Despejando
𝜃1 = 𝜋 + 𝛼 + ∅ (3.48)
Además, sabemos que,

sin 𝜃1 𝑅 = sin(𝜋 + 𝛼 + ∅) = − sin(𝛼 + ∅) (3.49)
sin 𝜃1 𝑅 = −[sin ∅ cos 𝛼 + cos ∅ sin 𝛼] (3.50)
Reemplazando, de manera análoga al desarrollo de la configuración BRAZO
IZQUIERDO, obtenemos:
𝑃𝑥 𝑟−𝑃𝑦 𝑑4
sin 𝜃1 𝑅 = (3.51)
𝑅2
De la misma forma:
−𝑃𝑦 𝑟−𝑃𝑥 𝑑4
cos 𝜃1 𝑅 = (3.52)
𝑅2
UNIFICACIÓN DE FORMULAS PARA AMBAS CONFIGURACIONES
Para la función sin 𝜃1 , de las ecuaciones 3.44 y 3.51 y tomando en cuenta la ecuación
1.17, podemos escribir:
𝐵𝑅𝐴𝑍𝑂.𝑃𝑥 𝑟−𝑃𝑦 𝑑4
sin 𝜃1 = (3.53)
𝑅2
En donde el operador BRAZO, toma los valores de 1 o −1, según la configuración del
robot (BRAZO IZQUIERDO o BRAZO DERECHO).
Para la función cos 𝜃1 , de las ecuaciones 3.45 y 3.52 y tomando en cuenta la ecuación
1.17, podemos escribir
−𝐵𝑅𝐴𝑍𝑂.𝑃𝑦 𝑟−𝑃𝑥 𝑑4
cos 𝜃1 = (3.54)
𝑅2
Con lo cual,
𝐵𝑅𝐴𝑍𝑂.𝑃𝑥 𝑟−𝑃𝑦 𝑑4
𝜃1 = tan−1 (−𝐵𝑅𝐴𝑍𝑂.𝑃 ) (3.55)
𝑦 𝑟−𝑃𝑥 𝑑4
Para solucionar esta ecuación en Matlab, utilizaremos a función atan2 (sección 1.3.5.3|).
64
3.3.2.2. CÁLCULO DE Q2 UTILIZANDO EL MÉTODO ALGEBRÁICO.
Una vez obtenido el valor de 𝑃03 (sección 3.3.1), utilizando las MTH, calculamos 03𝐴
0 1 2
1𝐴 2𝐴 3𝐴 = 03𝐴 (3.56)
Luego operamos de la siguiente manera (Sección 1.3.5.2)
1 −1 0 −1 0
2𝐴 1𝐴 3𝐴 = 23𝐴 (3.57)
Del resultado de 3.57, ayudándonos con Matlab, tomamos los elementos (1,4) y (2,4) de
la igualdad, obteniendo dos ecuaciones:
cos 𝑞2 (𝑃03 𝑧 − 𝐿1 ) + sin 𝑞2 (𝑃03 𝑦 cos 𝑞1 − 𝑑1 − 𝑃03 𝑥 sin 𝑞1 ) − 𝐿2 = 𝐿3 𝑐𝑜𝑠𝑞3 (3.58)
sin 𝑞2 (𝐿1 − 𝑃03 𝑧 ) + cos 𝑞2 (𝑃03 𝑦 cos 𝑞1 − 𝑑1 − 𝑃03 𝑥 sin 𝑞1 ) = 𝐿3 𝑠𝑖𝑛𝑞3 (3.59)
Definimos las constantes:
𝐾1 = (𝑃03 𝑧 − 𝐿1 ) (3.60)
𝐾2 = 𝑃03 𝑦 cos 𝑞1 − 𝑑1 − 𝑃03 𝑥 sin 𝑞1 (3.61)
Con lo cual podemos escribir las ecuaciones 3.58 y 3.59 de la siguiente manera:
cos 𝑞2 𝐾1 + sin 𝑞2 𝐾2 − 𝐿2 = 𝐿3 𝑐𝑜𝑠𝑞3 (3.62)
cos 𝑞2 𝐾2 − sin 𝑞2 𝐾1 = 𝐿3 sin 𝑞3 (3.63)
Elevamos al cuadrado 3.62 y 3.63 y luego de sumar ambos resultados se obtiene:
2𝐿2 𝐾1 cos 𝑞2 + 2𝐿2 𝐾2 sin 𝑞2 = 𝐾1 2 + 𝐾2 2 + 𝐿2 2 − 𝐿3 2 (3.64)
Para solucionar esta ecuación, utilizamos el método de solución algebraica por reducción
a polinomio, explicado en la sección 1.3.5.3. Hacemos:
𝑞
𝜇 = tan ( 22 ),
1−𝑢2
cos 𝑞2 = 1+𝑢2,
2𝜇
sin 𝑞2 = 1+𝑢2 (3.65)
65
También,
𝑎 = 2𝐿2 𝐾1 ,
𝑏 = 2𝐿2 𝐾2 ,
𝑐 = 𝐾1 2 + 𝐾2 2 + 𝐿2 2 − 𝐿3 2 (3.66)
Por último, la ecuación 3.64, toma la siguiente forma
𝑎 cos 𝑞2 + 𝑏 sin 𝑞2 = 𝑐 (3.67)
Y 𝑞2 se calcula de la siguiente manera:
𝑏±√𝑏 2 +𝑎2 −𝑐 2
𝑞2 = 2𝑡𝑎𝑛−1 ( ). (3.68)
𝑎+𝑐
CONFIGURACIÓN CODO ARRIBA Y CODO ABAJO
Como vemos, la ecuación 3.68 nos brinda 2 soluciones, las cuales corresponderán a la
configuración CODO ARRIBA y CODO ABAJO que adoptará el manipulador.
La figura 3.6 nos muestra la disposición CODO ARRIBA y CODO ABAJO para los
grados de libertad 𝑞2 y 𝑞3 de nuestro manipulador; según la disposición de los sistemas
de coordenadas del robot y el sentido de giro de la articulación 𝑞2 (Figura 3.2), realizando un
pequeño análisis, nos damos cuenta que la configuración CODO ARRIBA siempre tendrá
el ángulo menor, cuando se utiliza la configuración BRAZO IZQUIERDO; sin embargo,
cuando la configuración es BRAZO DERECHO, la configuración CODO ARRIBA
corresponderá al menor ángulo de las dos posibles soluciones de 𝑞2
Utilizaremos los parámetros BRAZO y CODO, para que la formula de cinemática inversa
(Ecuación 3.59) que nos permite calcular la variable de articulación 𝑞2 , también nos
permita seleccionar la posición deseada (CODO ARRIBA O CODO ABAJO). Para ello
tomamos en cuenta las ecuaciones 1.35 y 1.36 y con ello, la Ecuación 3.59 queda
expresada de la siguiente manera:
𝑏+𝐵𝑅𝐴𝑍𝑂.𝐶𝑂𝐷𝑂√𝑏 2 +𝑎2 −𝑐 2
𝑞2 = 2𝑡𝑎𝑛−1 ( ) (3.69)
𝑎+𝑐
66
Fig. 3.7 Las líneas punteadas indican la configuración codo abajo.
(Fuente: Craig 2006, p. 103)
Más adelante mostraremos como se determina matemáticamente el valor de los

parámetros BRAZO Y CODO, mediante las llamadas Ecuaciones de Decisión (Fu,
Gonzales y Lee, 1998)
3.3.2.3. CÁLCULO DE Q3 UTILIZANDO EL MÉTODO ALGEBRÁICO.
Podemos encontrar el valor de 𝑞3 de dos maneras. La primera es trabajando sobre 3.56 y

3.57, de la misma forma que encontramos 𝑞2 , así 𝑞3 solo dependerá de 𝑞1 . Sin embargo,
con este método, tendremos dos soluciones para 𝑞3 , lo cual hará más tedioso el trabajo de
selección de una solución única, para fines de programación. El segundo método es
simplemente despejar las ecuaciones 3.62 y 3.63, con lo cual obtenemos 𝑞3 en función de
𝑞2 y tendrá una única solución, la cual corresponderá al valor de 𝑞2 para la configuración
67
deseada ([CODO ARRIBA, CODO ABAJO] y [BRAZO DERECHO, BRAZO
IZQUIERDO]).
Despejando 3.62:
𝑐𝑜𝑠𝑞2 𝐾1 +sin 𝑞2 𝐾2 −𝐿2

cos 𝑞3 = (3.70)
𝐿3
y despejando 3.63:
𝑐𝑜𝑠𝑞2 𝐾2 −sin 𝑞2 𝐾1
sin 𝑞3 = (3.71)
𝐿3
Con esto, en Matlab, calcularemos 𝑞3 con la función 𝑎𝑡𝑎𝑛2.
𝑞3 = 𝑎𝑡𝑎𝑛2(sin 𝑞3 , cos 𝑞3 ) (3.72)
3.3.3. CÁLCULO DE Q4 Y Q5 UTILIZANDO EL MÉTODO ALGEBRÁICO
Conocidos los valores de los primeros grados de libertad, procedemos a calcular los dos
últimos, que determinarán la orientación de nuestro brazo robótico. Nos basamos en el
procedimiento que realizó Barrientos (1997, p. 120).
0 −1 0
3𝑅 . 5𝑅 = 35𝑅 (3.73)
lo cual es lo mismo que:
( 03𝑅 )−1 [𝑛 𝑜 𝑎] = 35𝑅 , (3.74)
con la ayuda de Matlab en la realización de los cálculos, obtendremos una igualdad de

matrices.
Para calcular 𝑞4 , tomamos los elementos (1,3) y (2,3) de cada lado de la ecuación
resultante, con lo cual obtenemos las siguientes ecuaciones:
cos 𝑞4 = 𝑎𝑦 cos(𝑞2 + 𝑞3 ) cos 𝑞1 − 𝑎𝑧 sin(𝑞2 + 𝑞3 ) − 𝑎𝑥 cos(𝑞2 + 𝑞3 ) sin 𝑞1 (3.75)
sin 𝑞4 = −𝑎𝑦 sin(𝑞2 + 𝑞3 ) cos 𝑞1 − 𝑎𝑧 cos(𝑞2 + 𝑞3 ) + 𝑎𝑥 sin(𝑞2 + 𝑞3 ) sin 𝑞1 (3.76)
Con esto, utilizando nuevamente la función 𝑎𝑡𝑎𝑛2:
𝑞4 = 𝑎𝑡𝑎𝑛2(sin 𝑞4 , cos 𝑞4 ) (3.77)
68
Ahora tomamos los elementos (3,1) y (3,2) de cada lado de la ecuación matricial
resultante de 3.74, obtenemos:
cos 𝑞5 = 𝑜𝑥 cos 𝑞1 + 𝑜𝑦 sin 𝑞1 (3.78)
sin 𝑞5 = 𝑛𝑥 cos 𝑞1 + 𝑛𝑦 sin 𝑞1 (3.79)
De la misma forma, calculamos 𝑞5 , usando 𝑎𝑡𝑎𝑛2:
𝑞5 = 𝑎𝑡𝑎𝑛2(sin 𝑞5 , cos 𝑞5 ) (3.80)
3.3.4. PROGRAMACIÓN EN MATLAB Y TIEMPO DE CÁLCULO
En el Anexo A6 presentamos el código de la función utilizada para calcular la cinemática

inversa del robot manipulador investigado.
El tiempo y frecuencia requeridos para calcular la cinemática inversa, se conocen

utilizando comandos de Matlab. El programa presentado en el Anexo A7, nos brinda el
tiempo promedio de cálculo para 50 configuraciones aleatorias de ángulos de articulación,
con el cual hallamos la frecuencia
𝑓𝑐𝑖 = 3𝑥103 (3.81)
donde 𝑓𝑐𝑖 es la frecuencia de cálculo de la cinemática inversa.
3.3.5. DETERMINACIÓN DE PARÁMETROS DE ENTRADA PARA EL

ALGORITMO DE CINEMÁTICA INVERSA
Elegir la matriz 05𝐴 que relaciona el efector final con la base como parámetro de entrada
no es lo más conveniente; debido a que, obtener la matriz de rotación 05𝑅 que determina
su orientación requiere un cálculo previo. Por lo cual optaremos por ingresar los
3.4. VERIFICACIÓN DE LOS ALGORITMOS DE CINEMÁTICA DIRECTA E

INVERSA.
Para verificar y validar nuestros algoritmos de cinemática directa e inversa utilizamos un
método propuesto por Fu, Gonzales y Lee (1998, pp.76-77), el cual se vale de “las
ecuaciones de decisión” para obtener el error que nos sirva para cuantificar la veracidad
de nuestros cálculos.
69
3.4.1. Ecuaciones de decisión para los indicadores de configuración del brazo
En la sección 1.3.6 presentamos el marco teórico referente a las ecuaciones de decisión.

Basándonos en ello, encontramos las ecuaciones de decisión para el manipulador que
estamos estudiando.
Para el indicador Brazo:

Utilizando la definición BRAZO DERECHO/BRAZO IZQUIERDO de la Sección 1.3.4
y la ecuación 1.31, efectuamos y obtenemos, según el álgebra que define nuestro robot,
tendremos que:
𝑖 𝒋 𝒌
𝒁 𝑥𝑃 ′ 1
𝑔= 𝒁𝟎 . |𝒁𝟏 𝑥 𝑃𝟎𝟒 ′| = 𝒁𝟎 . |− cos 𝑞1 − sin 𝑞1 0| |𝒁 𝑥 𝑃 ′|
𝟏 𝟎𝟒 𝟏 𝟎𝟒
𝑃04 𝑥 𝑃04 𝑦 0
𝑃04 𝑥 sin 𝑞1−𝑃04 𝑦 cos 𝑞1
𝑔= |𝒁𝟏 𝑥 𝑝𝟎𝟒 ′|
(3.82)
Como |𝒁𝟏 𝑥 𝑝𝟎𝟒 ′|, siempre es positivo solo debemos analizar el numerador, por lo tanto,
a ecuación que define el indicador brazo sería:
+1 → 𝑏𝑟𝑎𝑧𝑜 𝐷𝐸𝑅𝐸𝐶𝐻𝑂
𝐵𝑅𝐴𝑍𝑂 = 𝑠𝑖𝑔𝑛 (𝑃04 𝑥 sin(𝑞1 ) − 𝑃04 𝑦 cos(𝑞1 )) = {
−1 → 𝑏𝑟𝑎𝑧𝑜 𝐼𝑍𝑄𝑈𝐼𝐸𝑅𝐷𝑂
(3.83)
Para el indicador Codo:
Analizando la figura 3.6, nos podemos dar cuenta que un parámetro que puede definir el
indicador CODO, es 𝑷𝟐𝟑 𝒚 ; es decir la componente en el eje 𝑌 del sistema de coordenadas
𝑆2 , de la posición del origen del sistema 𝑆3 respecto del sistema 𝑆2 . También podemos
percatarnos que la configuración BRAZO DERECHO/BRAZO IZQUIERDO invierte el
signo del parámetro CODO, para sus dos configuraciones posibles.
De todo lo anterior:
𝐶𝑂𝐷𝑂 = 𝐵𝑅𝐴𝑍𝑂. 𝑠𝑖𝑔𝑛(𝑃24 𝑦 )
+1 → 𝐶𝑜𝑑𝑜 𝑒𝑛𝑐𝑖𝑚𝑎 𝑑𝑒 𝑙𝑎 𝑀𝑢ñ𝑒𝑐𝑎
𝐶𝑂𝐷𝑂 = 𝐵𝑅𝐴𝑍𝑂. 𝑠𝑖𝑔𝑛(𝐿𝟑 sin(𝑞3 )) = {
−1 → 𝐶𝑜𝑑𝑜 𝑑𝑒𝑏𝑎𝑗𝑜 𝑑𝑒 𝑙𝑎 𝑀𝑢ñ𝑒𝑐𝑎
(3.84)
Utilizamos 𝑃24 𝑦 , debido a que el análisis se hace respecto del sistema 𝑆4 y no de 𝑆3 , ya
que es indiferente en el desarrollo.
Para efectos de programación, cuando 𝑃23 𝑦 = 0, no tendremos un signo definido (+1 ó −
1) para la ecuación 3.84, elegiremos CODO ARRIBA, por defecto.
70
3.4.2. Calculo de errores e implementación del algoritmo de verificación
Se implementó en Matlab el algoritmo mostrado en la Figura 1.17, y se realizó la
verificación para 50 conjuntos de variables de articulación (𝑞1 , 𝑞2 , 𝑞3 , 𝑞4 , 𝑞5 ) elegidos al
azar, utilizando la función “rand”, según la formula brindada por la Ayuda de Matlab
[11].
𝑟 = 𝑎 + (𝑏 − 𝑎).∗ 𝑟𝑎𝑛𝑑(𝑁, 1) (3.85)
donde, 𝑎 y 𝑏 son los extremos del intervalo en el cual se generan los valores aleatorios y
el operador “.*”, se refiere a la multiplicación de un escalar por un vector, en el lenguaje
de programación de Matlab.
Los valores de 𝑎 y 𝑏 serán tomados considerando los rangos de recorrido angular de las
articulaciones del manipulador, consignados en la Tabla 3.1 y 𝑟 será el vector que
almacene los 50 valores generados para cada articulación.
Calculo del error
Calculamos el error según la figura 1.17, el cual representa la diferencia entre los valores
de los ángulos ingresado para realizar cinemática directa y los ángulos calculados
utilizando cinemática inversa.
Debido a que los valores calculados pueden ser mayores o menores que los ángulos
ingresados, para cuantificar correctamente la desviación, utilizamos el error absoluto;
luego promediaremos los errores de las cinco articulaciones. Así, obtenemos:
𝑒 = (|𝑞1 𝑖 − 𝑞1 𝑐 | + |𝑞2 𝑖 − 𝑞2 𝑐 | + |𝑞3 𝑖 − 𝑞3 𝑐 | + |𝑞4 𝑖 − 𝑞4 𝑐 | + |𝑞5 𝑖 − 𝑞5 𝑐 |)/5 (3.86)
donde los índices 𝑖 y 𝑐 hacen referencia a valores de ángulos de articulación ingresados y

calculados, respectivamente; y 𝑒 es el error promedio para cada articulación,
El error promedio para los 50 arreglos aleatorios de configuraciones para el manipulador,

se calcula de la siguiente manera:
1
𝑒𝑝𝑟𝑜𝑚 = 50 ∑50
𝑖=1 𝑒𝑖 (3.87)
Utilizando el programa en Matlab del Anexo A7 se obtuvo un error muy cercano a cero,
del orden de 10−13, o menor. Lo cual nos indica que nuestras ecuaciones de cinemática
directa e inversa están correctamente formuladas.
71
3.5. GENERACIÓN DE TRAYECTORIA LINEAL USANDO CUATERNIONES
Utilizando el algoritmo BDJP y el método de desviación acotada de Taylor, expuestos en

la sección 1.3.7.2.2, se desarrolló un programa en Matlab que calcula la trayectoria en
línea recta para un punto inicial y final.
La Figura 3.8 muestra el diagrama de flujo utilizado para conseguir la trayectoria deseada.
Cuando se programe un robot, no se toma en cuenta el último paso, el cual sólo nos sirve
para trazar y evaluar la trayectoria obtenida.
Fig. 3.8 Diagrama de flujo para el cálculo de trayectoria lineal.

Datos de entrada
Las posiciones inicial y final se ingresan utilizando matrices de transformación

homogénea:
0 1 0 −21.9 0 1 0 −21.9
𝐹0 = (0 0 1 113.9 ) y −0.3907
𝐹1 = ( 0.9205 0 0.9205 221.4621) (3.88)
1 0 0 240.9 0 0.3907 150.8336
0 0 0 1 0 0 0 1
Los errores de desviación para la posición y orientación tuvieron los siguientes valores:
𝛿𝑝 𝑚á𝑥 = 3 𝑚𝑚. y 𝛿𝑅 𝑚á𝑥 = 5° (3.89)
72
Etapa de pre-planificación
La etapa de pre-planificación se realiza mediante el Algoritmo BDJP y se calculan los

puntos nudos que determinarán la trayectoria; que, para el brazo robótico estudiado, se
muestran de color azul en la Figura 3.9, graficados respecto de un tiempo normalizado.
Paso 0.
- Convertimos
𝑹𝟎 → 𝒄𝟎
- Convertimos
𝑹𝟏 → 𝒄𝟏
Paso 1. Calcular la solución de la articulación. Calcular los vectores solución de la

articulación y 𝒒𝟏 que corresponde, respectivamente, a 𝑭𝟎 y a 𝑭𝟏 . Para esto utilizamos la
función Cinemática Inversa, Anexo B.2.
- Cinemática Inversa
𝑭𝟏 → 𝒒𝟎
- Cinemática Inversa
𝑭𝟏 → 𝒒𝟏
Paso 2. Determinar puntos medios en el espacio de articulación. Calcular puntos medios

en el espacio de las variables de articulación, utilizando la ecuación 1.81
- Calculamos 𝒒𝒎 , según:
𝒒𝒎 = 𝒒𝟏 − 1⁄2 ∆𝒒𝟏 , donde ∆𝒒𝟏 = 𝒒𝟏 − 𝒒𝟎
- Cinemática directa usando cuaterniones
𝒒𝒎 → 𝑭𝒎 (𝒑𝒎 , 𝒄𝒎 )
Paso 3. Determinar puntos medios en el espacio cartesiano. Calcular el punto medio 𝑭𝑪

del camino cartesiano correspondiente. En este paso utilizamos el álgebra de cuaterniones
para calcular rápidamente el punto medio de la rotación 𝑹𝒄 .
- Calculamos 𝒑𝒄
𝒑𝟎 +𝒑𝟏
𝒑𝒄 = 𝟐
73
𝜃
Ahora calculamos 𝑹𝒄 = 𝑹𝟏 𝑅𝑜𝑡(𝒏𝟏 , − 2) en forma de cuaternion
- Aplicando conjugado de un cuaternión, calculamos 𝒄𝒐 −𝟏 , el conjugado de 𝒄𝒐

- Usando multiplicación de cuaterniones, calculamos 𝒄𝟎𝟏 ,
𝒄𝟎𝟏 = 𝒄𝒐 −𝟏 𝒄𝟏
- Convertimos 𝒄𝟎𝟏 a su expresión equivalente en Par Eje Ángulo 𝑅𝑜𝑡(𝒏𝟏 , 𝜃)
𝒄𝟎𝟏 → 𝑅𝑜𝑡(𝒏𝟏 , 𝜃)
𝜃
- Una vez conocidos 𝒏 y 𝜃 podremos calcular 𝑅𝑜𝑡 (𝒏𝟏 , − ) para luego convertirlo a
2
cuaterniones utilizando la ecuación 1.56.

𝜃
𝑅𝑜𝑡 (𝒏𝟏 , − ) → 𝒄𝒙
2
- Usando multiplicación de cuaterniones, procedemos a calcular 𝒄𝒄 que expresa la
rotación en la mitad de la trayectoria, 𝑭𝒄 .
𝒄𝒄 = 𝒄𝟏 𝒄𝒙
- Finalmente expresamos 𝒄𝒄 como la matriz de rotación 𝑹𝒄 .
𝒄𝒄 → 𝑹𝒄
- Utilizando 𝒑𝒄 y 𝑹𝒄 , construimos 𝑭𝒄
Paso 4: Determinar los errores de desviación. Calcular el error de desviación entre 𝑭𝒎 y

𝑭𝒄 utilizando las ecuaciones 1.78 y 179.
- El error de posición, se calcula fácilmente, según:
𝛿𝑝 = |𝒑𝒎 − 𝒑𝒄 |
Para calcular 𝛿𝑅 = |𝑝𝑎𝑟𝑡𝑒 𝑎𝑛𝑔𝑢𝑙𝑎𝑟 𝑑𝑒 𝑅𝑜𝑡(𝒏, ∅) = 𝑹𝒄 −𝟏 𝑹𝒎 |, procedemos así:
- Multiplicando cuaterniones:
𝒄𝒄 −𝟏 𝒄𝒎
- Convertimos la expresión anterior a Par de Rotación
𝒄𝒄 −𝟏 𝒄𝒎 → 𝑅𝑜𝑡(𝒏, ∅)
74
El error en rotación será igual al módulo de ∅.
𝛿𝑅 = |∅|
Paso 5: Comprobación de las cotas de error. Si 𝛿𝑝 < 𝛿𝑝 𝑚á𝑥 y 𝛿𝑅 < 𝛿𝑅 𝑚á𝑥 , entonces
parar. De lo contrario, calcular el vector solución de la articulación 𝑞𝑐 corresponde al
punto medio del espacio cartesiano 𝑭𝒄 , y aplicar los pasos 2 a 5 recursivamente para los
dos subsegmentos sustituyendo 𝑭𝟏 por 𝑭𝒄 y 𝑭𝒄 por 𝑭𝟎 .
Fig. 3.9 Coordenada Y de los puntos nudos generados por el Algoritmo BDJP.
La figura 3.10 nos muestra la secuencia realizada para la obtención de los puntos nudos
mostrados en la Figura 3.9. La parte iterativa del algoritmo BDJP (Desde el paso 2 hasta
el paso 5) se realiza en total 15 veces para obtener los 9 puntos graficados como se
muestra en la Tabla 3.2.
Fig. 3.10 Esquema del algoritmo BDJP

75
Tabla 3.2. Deducción de la fórmula de iteraciones totales para el algoritmo BDJP
Número de Bucles Iteraciones Totales
1 1
2 3
3 7
4 15
. .
. .
. .
𝑛
n 2 −1
La fórmula para calcular el número de iteraciones totales sería la siguiente
𝐼𝑡 = 2𝑛 − 1 (3.90)
Interpolación en el espacio de articulación y generación de trayectoria
Utilizando el método de desviación acotada, se realiza la interpolación en el espacio de

articulación, aclarando que la ecuación 1.77 se utiliza para la transición entre segmentos
adyacentes y la ecuación 1.76 para el tramo intermedio de cada segmento.
𝑇1 − 𝑡
𝒒(𝒕) = 𝒒𝟏 − ∆𝒒𝟏
𝑇1
(𝜏 − 𝑡 ′ )2 (𝜏 + 𝑡 ′ )2
𝒒(𝑡′) = 𝒒𝟏 − ∆𝒒𝟏 + ∆𝒒𝟐
Fig. 3.11 Trayectoria final obtenida para un error de 3 mm y 5°

76
Luego, con los valores de articulación conocidos, mediante la función de Cinemática
Directa, podemos trazar la trayectoria. Las figura 3.11 muestran la trayectoria obtenida
en el espacio tridimensional, respecto de un tiempo normalizado; como se aprecia, la
trayectoria es bastante similar a una línea recta, más adelante cuantificaremos esta
similitud mediante el cálculo del error de desviación.
La figura 3.12, grafica el comportamiento de la aceleración en la trayectoria. Como se

mencionó en el marco teórico, se utilizan pequeñas aceleraciones en las transiciones de
un segmento a otro, para suavizar el cambio de velocidades.
Fig. 3.12 Aceleración de la articulación 𝑞2 durante la trayectoria

El desplazamiento de las articulaciones es suave y continuo, como se muestra en la Figura

3.13.
77
Fig. 3.13 Desplazamiento angular de las articulaciones del brazo robótico
Programación en Matlab
Para realizar la generación de trayectorias, primero se implementaron funciones que

puedan desarrollar las operaciones básicas utilizando cuaterniones (Sección 1.3.3). Los
códigos se presentan en el anexo A1
78
Luego de eso se realizó la generación de trayectorias, que además de utilizar las funciones
antes mencionadas, utiliza la rutina de cinemática directa e inversa, mostradas en el anexo
A3 y A6 y el algoritmo BDJT.
Las trayectorias generadas se exportaron en un archivo “.txt”, para ser simuladas en

Solidworks.
79
3.6. VERIFICACIÓN DE ERRORES MÁXIMOS
Los errores promedio obtenidos para la posición y orientación son los siguientes:
1. Ingresando 𝛿𝑝 𝑚á𝑥 = 3 𝑚𝑚. y 𝛿𝑅 𝑚á𝑥 = 5°

𝛿𝑝 𝑝𝑟𝑜𝑚 = 1.0378 y 𝛿𝑅 𝑝𝑟𝑜𝑚 = 2.5𝑥10−5 (3.91)
2. Ingresando 𝛿𝑝 𝑚á𝑥 = 1 𝑚𝑚. y 𝛿𝑅 𝑚á𝑥 = 5°
𝛿𝑝 𝑝𝑟𝑜𝑚 = 0.283 y 𝛿𝑅 𝑝𝑟𝑜𝑚 = 1.26𝑥10−5 (3.92)
Como vemos, se satisface con holgura los errores máximos requeridos, siendo el error en
orientación prácticamente cero.
La figura 3.12 muestra la trayectoria final obtenida para 𝛿𝑝 𝑚á𝑥 = 1 𝑚𝑚. y 𝛿𝑅 𝑚á𝑥 = 5°,
como se observa, la línea recta es más precisa que la mostrada en la figura 3.10
Fig. 3.14 Trayectoria final obtenida para un error de 1 mm y 5°

Simulación utilizando Solidworks

La simulación en Solidworks se utiliza para corroborar los cálculos realizados. En el
complemento Solidworks Montion se realiza un Análisis de Movimiento que permite
apreciar la trayectoria generada, la figura 3.14 muestra una secuencia de imágenes
capturadas durante la simulación. Además, la figura 3.15 muestra la posición final
alcanzada por el efector final, las medidas coinciden con las componentes del vector
posición de la matriz 𝐹1 (Ecuación 3.79).
80
Fig. 3.15 Imágenes del recorrido realizado por la simulación del manipulador en
Solidworks.
Fig. 3.16 Posición final del ensamblaje en Solidworks

81
3.7. AHORRO COMPUTACIONAL Y TIEMPO DE CALCULO
3.7.1. CUANTIFICACIÓN OPERACIONAL

Cuantificaremos el costo computacional para nuestro algoritmo de generación de
trayectorias comparándolo con el cálculo que se hubiese requerido, utilizando matrices
de transformación homogénea. Para ello contabilizamos todas las operaciones requeridas
paso a paso en el algoritmo, estas se muestran en la Tabla 3.2.
Tabla 3.2. Operaciones realizadas en algoritmos usando cuaterinones y MTH
ALGORITMO USANDO CUATERNIONES ALGORITMO USANDO MTH

OPERACIÓN CANT. OPERACIÓN CANT.
Paso 0 Cálculo Previo
Matriz de Rotación a cuaternión 2
Paso 1 Paso 1
Función cinemática inversa 2 Func. Cin. Inv 2
Paso 2 Paso 2
Func. Cin.Dir. Cuat. 1 Función Cin. Dir. Mrot 1
Paso 3 Paso 3
Rotación Inversa (iC) 1 Rotación inversa (iMR) 1
I
T Multiplicación de Rotaciones (mulC) 1 Multiplicación de Rotaciones (mulMR) 1
E Cuaternio a Eje-Angulo (C2EA) 1 Mat Rot a Eje-Ang (MR2EA) 1
R Eje-Ang a Cuaternio (EA2C) 1 Eje-Ang a Mat Rot (EA2MR) 1
A Multiplicación de Rotaciones (mulC) 1 Multiplicación de Rotaciones (mulMR) 1
C Cuaternión a Mat. de Rot. (C2MR) 1
I
Paso 4 Paso 4
Ó
Rotación Inversa (iC) 1 Rotación inversa (iMR) 1
N
Multiplicación de Rotaciones (mulC) 1 Multiplicación de Rotaciones (mulMR) 1
Cuaternio a Eje-Angulo (C2EA) 1 Mat. de Rot. a Eje-Angulo (MR2EA) 1
Paso 5 Paso 5
Función Cin Inv 1 Función Cin Inv 1
Utilizando la información disponible en la Tabla 1.2, realizamos una sumatoria de todos

los cálculos comparables requeridos en la parte del algoritmo que corresponde a la
iteración.
Las tablas 3.3 y 3.4 registran estos datos para el algoritmo usando Matrices de
Transformación Homogénea y cuaterniones, respectivamente. Es importante aclarar que
en estas tablas no se considera el paso 5, debido a que se utiliza la misma función de
cinemática inversa para ambos casos.
82
Tabla 3.3. Operaciones realizadas en iteración usando MTH
OPERACIONES REALIZADAS EN ITERACIÓN USANDO MTH

Paso 2 Paso 3 Paso4
CALCULOS TOTAL
Cindir mulMR MR2EA EA2MR mulMR mulMR MR2EA
Adiciones 11 15 8 10 15 15 8 82
Multiplicaciones 13 24 10 15 24 24 10 120
Raiz Cuadrada 1 1 2
Arc Tan 1 1 2
Seno 3 1 4
Coseno 3 1 4
CALCULOS TOTALES 214
Tabla 3.4. Operaciones realizadas en iteración usando cuaterniones
OPERACIONES REALIZADAS EN ITERACIÓN

Paso 2 Paso 3 Paso4
CALCULOS TOTAL
CinDir mulC C2EA EA2C mulC C2MR mulC C2EA
Adiciones 5 9 0 9 12 9 0 44
Multiplicaciones 16 16 4 4 16 13 16 4 89
Raiz Cuadrada 1 1 2
Arc Tan 1 1 2
seno 3 1 4
coseno 3 1 4
CALCULOS TOTALES 145
Tomando en cuenta la fórmula de iteraciones totales (Ecuación 3.85) y considerando que

el “Paso 0” involucra dos operaciones de conversión de Matriz de Rotación a cuaternión,
las cuales involucran 16 adiciones y 7 multiplicaciones cada una (46 operaciones en total),
que se realizan antes de iniciar el algoritmo. La proporción de operaciones totales
requeridas para el cálculo con cuaterniones y matrices de rotación se expresa de la
siguiente manera
(2𝑛 −1)145+46
𝑃𝑜 = (2𝑛 −1)214
𝑥100% (3.93)
Para el ejemplo mostrado en la figura 3.9, donde 𝑛 = 4, el porcentaje de operaciones

requeridas utilizando cuaterniones es 𝑃𝑜 = 68%, respecto de la cantidad de operaciones
requeridas utilizando matrices de transformación homogénea. Siendo más específicos,
según las tablas 3.3 y 3.4, hemos prescindido de 15𝑥(82 − 44) − 32 = 538 adiciones y
15𝑥(120 − 89) − 14 = 451 multiplicaciones.
83
Las fórmulas generales para calcular la cantidad adiciones y multiplicaciones de las que
se prescinde en el cálculo son:
𝐴𝑡 = 38(2𝑛 − 1) − 32 (3.94)
𝑀𝑡 = 31(2𝑛 − 1) − 14 (3.95)
3.7.2. TIEMPO DE CÁLCULO
El tiempo de cálculo lo contabilizamos desde el inicio del algoritmo hasta que culmina la
interpolación en el espacio de articulación, debido a que, esos serán los valores que se
ingresen a un controlador para generar la trayectoria.
La cantidad de puntos necesarios para generar la trayectoria es elegida de la siguiente

manera: Por cada milímetro de trayectoria, usaremos 10 puntos. Esto es básicamente para
poder generar una trayectoria suave al momento de graficar, si en el futuro, se desea
generar trayectorias utilizando servomotores, la cantidad de pasos o punto generados
requerirá un análisis diferente.
Para este ejemplo, generamos un total de 1104 puntos
𝑃𝑡𝑜𝑡 = 1104 (3.96)
El tiempo de cálculo es el siguiente:
𝑇𝑐𝑎𝑙 = 0.0227 s. (3.97)
84
CAPÍTULO IV: DISCUSIÓN DE RESULTADOS.
4.1. ESTRUCTURA DEL BRAZO ROBÓTICO
La estructura mecánica del brazo robótico está basada en piezas pre-fabricadas de

aluminio, las cuales vienen siendo utilizadas últimamente para aplicaciones prácticas que
involucran áreas como la robótica y electrónica. Se consideró usar estas piezas, debido a
su bajo costo y a la precisión con las que son construidas, debido a que generalmente se
fabrican en máquinas CNC o impresoras 3D; esto nos permite tener un modelo que no
desvirtúe los cálculos y simulaciones realizadas.
El modelo desarrollado, es una maqueta de los manipuladores utilizados en la industria,

puede realizar aplicaciones similares, con la salvedad de que tiene sólo 5 grados de
libertad. Un robot industrial con el que se puede comparar es el Mitsubishi Movemaster
EX, analizado por Merchan (2000).
No se logra ensamblar el brazo robótico de tal manera que los ejes de giro de las
articulaciones de orientación se intersecten en un punto común. Esto genera
complicaciones adicionales en la solución de la cinemática del robot.
4.2. CINEMÁTICA DIRECTA
La cinemática directa se desarrolló utilizando el algoritmo de Denavit-Hartenberg, debido

a que es el más aceptado y su forma de asignar los ejes y parámetros es bastante simple.
Para verificar la cinemática directa, ingresamos distintos valores para las variables de
articulación y comparamos los resultados obtenidos en Matlab, con la posición alcanzada
por el efector final en el ensamblaje realizado en Solidworks. Los valores calculados nos
coincidieron con los valores que arrojaba la simulación.
Como menciona Craig (2007, p. 103), “cuando un manipulador tiene menos de seis
grados de libertad, no puede alcanzar posiciones y orientaciones para un destino arbitrario
en espacio 3D”. En efecto, nuestro manipulador está impedido, debido a que solo cuenta
con dos grados de libertad para la orientación (Articulación 4 y Articulación 5), de
acceder a ciertas posiciones y orientaciones definidas.
4.3. CINEMÁTICA INVERSA
Para resolver el problema de cinemática inversa; a diferencia de Merchán (2000), quien

utilizó métodos numéricos; elegimos utilizar una solución cerrada debido a que es más
85
rápida, exacta y garantiza siempre encontrar una solución, como mencionan Craig (2006,
p.106) y Barrientos (1997, p. 108).
Debido a la dificultad adicional que conlleva el no cumplir con la condición de que los
ejes sobre los cuales giran las articulaciones de orientación se intersecten en un punto
(para nuestro caso 𝑍4 y 𝑍5 ); para poder solucionar el problema cinemático inverso,
debimos incluir conceptos vectoriales, los cuales nos brindaron condiciones que nos
sirvieron para llegar a la solución. Este aporte se considera importante porque amplia
nuestra visión sobre la configuración espacial de un manipulador.
Aydin y Kucuk (2006), logran resolver la cinemática inversa para robots manipuladores
utilizando cuaterniones, pero solo abarca robots manipuladores que cuentan con la
configuración “Muñeca de Euler”, es decir cuyos ejes de giro de las articulaciones de
orientación se intersectan en un punto común. Se intentó utilizar este método para resolver
el brazo robótico estudiado, pero las ecuaciones obtenidas fueron bastante complejas y
no se logró obtener una solución. Esto se debe a que, como se explicó líneas arriba, el
prototipo estudiado no cuenta con una muñeca de Euler, lo cual dificulta los cálculos.
La velocidad y frecuencia de cálculo para la cinemática inversa se encuentra en un valor
muy aceptable, tomando como referencia la consideración de Craig (2006, p. 127), quien
sugiere frecuencias de cálculo mayores a 30 Hz. La función que resuelve la cinemática
inversa para el manipulador considerado en éste estudio se ejecuta con una frecuencia del
orden de 103 .
4.4. GENERACIÓN DE TRAYECTORIAS

La generación de trayectorias se realizó utilizando el concepto de cuaterniones, el cual
nos brinda las ventajas descritas en la Tabla 1.2, con esto, conseguimos obtener una
trayectoria bastante similar a una línea recta, con desviaciones de error de menos de 3
mm en la posición y en orientación, bastante cercanos a cero.
Se cuantificaron los requerimientos de cálculo para el algoritmo BDJP utilizando

cuaterniones y se compararon con los cáculos requeridos para desarrollar el algoritmo
utilizando Matrices de Transformación homogénea. Se cuantifica el ahorro
computacional y la cantidad de operaciones de las que se prescinde mediante las formulas
3.93, 3.94 y 3.95.
86
El tiempo de cálculo se encuentra en el orden de las centésimas de segundo, según las
ecuaciones 3.97 y 3.98, por ejemplo, para un total de 1104 puntos generados el tiempo
necesario fue de 0.0227 segundos.
El concepto de cuaternión, es poco utilizado aún en la región, por lo cual esta

investigación nos permite incursionar en el uso de esta herramienta, que nos puede brindar
cálculos sencillos, rápidos y bastante precisos.
Taylor (1979, p. 428), para la ecuación (4) de su investigación, considera 𝑡 ′ = 𝑇1 − 𝑡,

para nuestro cálculo, consideramos 𝑡 ′ = 𝑡 − 𝑇1 (ecuación 1.80), con lo cual obtuvimos
los resultados esperados.
87
CAPITULO V: CONCLUSIONES
5.1. Se modeló un brazo robótico utilizando piezas pre-fabricadas de bajo costo, que nos
brindan la facilidad de obtener un prototipo preciso, de fácil montaje y con el que
podemos emular aplicaciones parecidas a las realizadas por los robots manipuladores
en la industria, para fines ilustrativos.
La dificultad generada fue que no se pudo realizar un diseño que cumpla con la
intersección de los ejes de giro de las articulaciones de orientación en un solo punto,
usado para facilitar el cálculo.
Debido a que el manipulador cuenta con solo cinco grados de libertad, no se pueden
alcanzar todas las posiciones y orientaciones posibles.
5.2. Se logró obtener la cinemática directa con dos métodos, utilizando el algoritmo de
Denavit-Hartenberg y utilizando cuaterniones. Se verificaron las ecuaciones
obtenidas utilizando la simulación en Solidworks y Matlab. Se cuantificaron los
requerimientos computacionales para ambos métodos, obteniendo como resultado
que la cinemática directa usando cuaternios requiere mayor cantidad de operaciones
y por ende mayor tiempo de cálculo. Se implementaron ambos algoritmos en Matlab
y se calcularon los tiempos de cálculo obteniendo que la relación entre el tiempo de
cálculo de la cinemática directa usando cuaterniones respecto del tiempo de cálculo
usando el algoritmo D-H es, en promedio, de 1.1; para 100 muestras aleatorias de
conjuntos de ángulos de articulación.
5.3. La cinemática inversa se obtuvo utilizando los métodos geométrico y algebraico,
además del uso de conceptos vectoriales básicos como recursos adicionales para
solucionar las complicaciones matemáticas que causó la estructura atípica del
prototipo estudiado.
El algoritmo generado en Matlab, nos brinda resultados precisos con errores muy
cercanos a cero y con una frecuencia de cálculo bastante más grande que la
recomendada por la literatura especializada.
5.4. Se logró implementar un algoritmo de generación de trayectorias en línea recta
utilizando cuaterniones, obteniéndose un algoritmo eficiente. Se cuantificó el ahorro
computacional respecto del uso de matrices de transformación homogénea mediante
las ecuaciones 3.93, 3.94 y 3.95. Las trayectorias generadas presentan errores
bastante pequeños con respecto a las trayectorias deseadas, además se calculan en
tiempos que están en el orden de 10−2 .
88
CAPITULO VI: REFERENCIAS BIBLIOGRÁFICAS
[1] KUKA. 2016. [Consulta: 20 octubre 2016]. Disponible en:
https://www.kuka.com/es-es/acerca-de-kuka.
[2] KUKA. 2016. [Consulta: 20 octubre 2016]. Disponible en:
https://www.kuka.com/es-mx/productos-servicios/sistemas-de-robot/robot-
industrial.
[3] UNIVERSAL ROBOTS. 2016. [Consulta: 20 octubre 2016]. Disponible en:
https://www.universal-robots.com/es/.
[4] UNIVERSAL ROBOTS. 2016. [Consulta: 20 octubre 2016]. Disponible en:
https://www.universal-robots.com/es/productos/
[5] BARRIENTOS, Antonio [et al.]. Fundamentos de Robótica. 1a. ed. España:
McGraw Hill, 1997.
[6] CRAIG, Jhon. Robótica. 3a. Ed. México: PERSON EDUACTION, 2006
[7] SAHA, Subir. Introducción a la robótica. 1a. Ed. México: McGraw Hill, 2010.
[8] FU, K.S.; GONZALES, R. C. Y LEE, C. S. G. Robótica: Control, Detección, Visión
e Inteligencia. McGraw Hill, 1998.
[9] WIKIPEDIA. 2017. [Consulta: 19 agosto 2017]. Disponible en:
https://es.wikipedia.org/wiki/Ecuaci%C3%B3n_trascendente.
[10] WIKIPEDIA. 2017. [Consulta: 19 setiembre 2017]. Disponible en:
[11] WIKIPEDIA. 2017. [Consulta: 12 octubre 2017]. Disponible en:
https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation.
[12] MathWorks. 2017. [Consulta: 13 octubre 2017]. Disponible en:
https://es.mathworks.com/help/matlab/ref/rand.html
[13] MERCHAN,Emmanuel. Metodología para generación de trayectorias de
manipuladores robóticos, su cinemática y dinámica. Trabajo de Titulación
(Maestro en ciencias con especialidad en Ingeniería Mecánica). México, D.F.:
Instituto Politécnico Nacional, Escuela de Ingeniería Mecánica y Eléctrica, 2000.
[14] RIVERA, Juan. Control de posición/fuerza para manipuladores rígidos basado en
cuaterniones unitarios. Trabajo de Titulación (Maestría en Ingeniería Área:
Eléctrica, Campo: Control). México, D.F.: Universidad Nacional Autónoma de
México.
[15] VOIGHT, John. Quaternion algebras. V0.9.8. 2017.
[16] TAYLOR, Rusell. Planning and Executiono f Straight Line Manipulator
Trajectories. IBM, J. Res. Devel., vol. 23, núm. 4, pp. 19-41
89
[16] Aydin, Yavuz; Kucuk, Serdar. Quaternion Based Inverse Kinematics for Industrial
Robot Manipulators with Euler Wrist. IEEE 3rd International Conference on
Mechatronics, 2006, pp. 581-586
90
ANEXOS
A. CODIGOS EN MATLAB
A.1 FUNCIONES PARA IMPLEMENTAR OPERACIONES BÁSICAS
USANDO CUATERNIONES.
A.1.1 MULTIPLICACIÓN DE CUATERNIONES
function [ c1xc2 ] = quatmul( c1,c2 )
c1xc2(1)=(c2(1)*c1(1)-c2(2)*c1(2)-c2(3)*c1(3)-c2(4)*c1(4));
c1xc2(2)=(c2(1)*c1(2)+c2(2)*c1(1)-c2(3)*c1(4)+c2(4)*c1(3));
c1xc2(3)=(c2(1)*c1(3)+c2(2)*c1(4)+c2(3)*c1(1)-c2(4)*c1(2));
c1xc2(4)=(c2(1)*c1(4)-c2(2)*c1(3)+c2(3)*c1(2)+c2(4)*c1(1));
end
A.1.2 CONJUGADO DE UN CUATERNION
function [ Cconj ] = conjquat( C )
Cconj=[C(1) -C(2) -C(3) -C(4)];
end
A.1.3 NORMA DE UN CUATERNION
function [ n ] = normaquat( c )
n=c(1)^2+c(2)^2+c(3)^2+c(4)^2;
end
A.1.4 CONVERSIÓN EJE-ANGULO A CUATERNIÓN
function [ c ] = Rot( n,ang )
c=[cos(ang/2) sin(ang/2)*n(1) sin(ang/2)*n(2) sin(ang/2)*n(3)];
end
A.1.5 CONVERSIÓN DE CUATERNION A EJE-ANGULO
function [ n,ang ] = Rot_eje_ang( c )

%UNTITLED9 Summary of this function goes here
% Detailed explanation goes here
n=[c(2),c(3),c(4)]./sqrt(c(2)^2+c(3)^2+c(4)^2);
ang=2*atan2(sqrt(c(2)^2+c(3)^2+c(4)^2),c(1));
end
91
A.1.6 CONVERSIÓN DE CUATERNIÓN A MATRIZ DE ROTACIÓN.
function [ R ] = quat2mrot( c )
%UNTITLED4 Summary of this function goes here
R=[2*(c(1)^2+c(2)^2)-1 2*(c(2)*c(3)-c(1)*c(4))
2*(c(2)*c(4)+c(1)*c(3));
2*(c(2)*c(3)+c(1)*c(4)) 2*(c(1)^2+c(3)^2)-1 2*(c(3)*c(4)-
c(1)*c(2));
2*(c(2)*c(4)-c(1)*c(3)) 2*(c(3)*c(4)+c(1)*c(2)) 2*(c(1)^2+c(4)^2)-
1 ];
end
A.1.7 CONVERSIÓN DE MATRIZ DE ROTACIÓN A CUATERNIÓN
function [ C ] = mrot2quat( R )
%Función que nos entrega el quaternion
% C=zeros(1,4)
C(1)=(1/2)*sqrt(R(1,1)+R(2,2)+R(3,3)+1);
C(2:4)=(1/2).*[sign(R(3,2)-R(2,3))*sqrt(R(1,1)-R(2,2)-R(3,3)+1)
sign(R(1,3)-R(3,1))*sqrt(R(2,2)-R(3,3)-R(1,1)+1) sign(R(2,1)-
R(1,2))*sqrt(R(3,3)-R(1,1)-R(2,2)+1)];
end
92
A.2 CINEMÁTICA DIRECTA USANDO MTH
%CINEMÁTICA DIRECTA PARA BRAZO ROBÓTICO DE 5GDL

%Elaborado por: Víctor Barreto Vásquez
%MATRICES DE TRANSFORMACIÓN
syms q1 q2 q3 q4 q5 L1 d1 d4 L1 L2 L3 L4 L5 h4
A01=[ sin(q1) 0 -cos(q1) -d1*sin(q1);

-cos(q1) 0 -sin(q1) d1*cos(q1);
0 1 0 L1;
0 0 0 1];
A12=[ -sin(q2), -cos(q2), 0, -L2*sin(q2);

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

sin(q3), cos(q3), 0, L3*sin(q3);
0, 0, 1, 0;
0, 0, 0, 1];
A34=[cos(q4) 0 -sin(q4) L4*cos(q4);

sin(q4) 0 cos(q4) L4*sin(q4);
0 -1 0 d4;
0 0 0 1];
A45=[ cos(q5), -sin(q5), 0, 0;

sin(q5), cos(q5), 0, 0;
0, 0, 1, L5;
0, 0, 0, 1];
A05=A01*A12*A23*A34*A45
Px=A05(1,4)
Py=A05(2,4)
Pz=A05(3,4)
ax=A05(1,3)
ay=A05(2,3)
az=A05(3,3)
nx=A05(1,1)
ny=A05(2,1)
nz=A05(3,1)
ox=A05(1,2)
oy=A05(2,2)
oz=A05(3,2)
93
A.3 CINEMÁTICA DIRECTA USANDO CUATERNIONES
%Algoritmo para obtener la expresión final para la cinemática directa

%utilizando cuaterniones
%Elaborado por:
%Víctor Barreto Vásquez
syms q1 q2 q3 q4 q5
syms L1 d1 L2 L3 L4 d4 L5
Q1=[cos(q1/2) 0 0 sin(q1/2)];
p1=[-d1*sin(q1) d1*cos(q1) L1];
Q2=[cos(q2/2) -sin(q2/2) 0 0];
p2=[ 0 L2*sin(q2) L2*cos(q2)];
Q3=[cos(q3/2) -sin(q3/2) 0 0];
p3=[ 0 L3*sin(q3) L3*cos(q3)];
Q4=[cos(q4/2) -sin(q4/2) 0 0];
p4=[-d4 L4*sin(q4) L4*cos(q4) ];
Q5=[cos(q5/2) 0 sin(q5/2) 0];
p5=[0 L5 0 ];
Qi=[0.5000 -0.5000 -0.5000 -0.5000];
[Qaux1 paux1]=MulTransCuat(Q1,p1, Q2, p2);

[Qaux2 paux2]=MulTransCuat( Qaux1,paux1,Q3, p3);
[Qaux3 paux3]=MulTransCuat( Qaux2,paux2,Q4, p4)
[ct pf]= MulTransCuat(Qaux3,paux3 ,Q5, p5);

cf=quatmul(ct,Qi);
94
A.4 COMPARACIÓN DE ALGORITMOS DE CINEMÁTICA DIRECTA
USANDO CUATERNIONES Y MTH
%Programa para verificación cinemática directa

%2017
Q1=-pi/2+(pi/2+pi/2).*rand(50,1);
Q2=-pi/2+(pi/2+pi/2).*rand(50,1);
Q3=-pi/2+(pi/2+pi/2).*rand(50,1);
Q4=-pi+(0+pi).*rand(50,1);
Q5=-pi/2+(pi/2+pi/2).*rand(50,1);
a=0;
k=1;
fault=zeros(1,50);
numero=zeros(1,15);
for i=1:50
q1=Q1(i);
q2=Q2(i);
q3=Q3(i);
q4=Q4(i);
q5=Q5(i);
[c,p]=CinDirCuat(q1,q2,q3,q4,q5);
c1(i,:)=c;
r1=quat2mrot(c);
R1(:,3*i-2:3*i)=r1;
p1(i,:)=p;
[P,R]=Cin_Dir(q1,q2,q3,q4,q5);
R2(:,3*i-2:3*i)=R;
c2(i,:)=mrot2quat(R);
p2(i,:)=P;
if abs(c1(i,1)-c2(i,1))<0.01 && abs(c1(i,2)-c2(i,2))<0.01 &&

abs(c1(i,3)-c2(i,3))<0.01 && abs(c1(i,4)-c2(i,4))<0.01
a=a+1;
fault(1,i)=1;
else
numero(1,k)=i;
k=k+1;
end
end
95
A.5 TIEMPOS DE CÁLCULO PARA ALGORITMOS DE CINEMÁTICA
DIRECTA
%PROGRAMA PARA RELACIONAR LOS TIEMPOS DE CÁLCULO PARA LA CINEMÁTICA

DIRECTA
%USANDO CUATERNIONES Y MTH.
Q1=-pi/2+(pi/2+pi/2).*rand(100,1);
Q2=-pi/2+(pi/2+pi/2).*rand(100,1);
Q3=-pi/2+(pi/2+pi/2).*rand(100,1);
Q4=-pi+(0+pi).*rand(100,1);
Q5=-pi/2+(pi/2+pi/2).*rand(100,1);
pt=0;
for i=1:100
q1=Q1(i);
q2=Q2(i);
q3=Q3(i);
q4=Q4(i);
q5=Q5(i);
[ Qf, P,tiempo] = CinDirCuat( q1,q2,q3,q4,q5 );
t1(i)=tiempo;
[P,R,Tiempo] = Cin_Dir(q1, q2, q3, q4, q5 )
t2(i)=Tiempo;
Po(i)=t1(i)/t2(i);
pt=pt+Po(i);
end
Prom=pt/100
96
A.6 CINEMÁTICA INVERSA
function [ angulos ] = Cin_Inv( A05,Brazo, Codo)

%FUNCIÓN QUE CALCULA LA CINEMÁTICA INVERSA DE UN ROBOT MANIPULADOR DE
%5DOF.
%Este programa permite ingresar los parámetros BRAZO y CODO, que nos
%permitirán calcular, de todas las soluciones posibles, la adecuada,
segun
%la configuración del brazo deseada ([BRAZO DERECHO,BRAZO
IZQUIERDO],[CODO
%ARRIBA,CODO A BAJO]).
tic;
%Parámetros D-H.
L1=16.4;d1=10.5; L2=104;L3=94.5;L4=26;d4=21.9;L5=103.4;
%Si el valor ingresado Codo=1, se pedirá la configuración codo Arriba
%Si es Codo=-1, será codo abajo. Y si es Codo=0, escojeremos por
defecto, la configuración Codo arriba
if Codo==0
Codo=1;
end
%CINEMÁTICA DIRECTA:
P05x=A05(1,4);
P05y=A05(2,4);
P05z=A05(3,4);
a05x=A05(1,3);
a05y=A05(2,3);
a05z=A05(3,3);
n05x=A05(1,1);
n05y=A05(2,1);
n05z=A05(3,1);
o05x=A05(1,2);
o05y=A05(2,2);
o05z=A05(3,2);
%%%%---CINEMÁTICA INVERSA---%%%%%%%%%%
%Desacoplo Cinemático
%Calcularemos Posicionamiento del extremo del eslabón 3, según

posición y
%orientacion del extremo final (P05x, P05y, P05z)
%Necesitamos el valor de los ejes coordenados X e Y del sistema S4
%Para el eje Y04:

%Componente de Y en el Y0
o04y_1=sqrt(1/((a05y/a05x)^2+1));
o04y_2=-sqrt(1/((a05y/a05x)^2+1));
o04x_1=-o04y_1*(a05y/a05x);
o04x_2=-o04y_2*(a05y/a05x);
o04z=0;
%Para el eje X04:

P04x=P05x-(L5)*a05x;
97
P04y=P05y-(L5)*a05y;
P04z=P05z-(L5)*a05z;
u_1=(o04y_1/o04x_1)
u_2=(o04y_2/o04x_2)
v_1=(a05x*o04y_1-o04x_1*a05y)/(a05z*o04x_1)
v_2=(a05x*o04y_2-o04x_2*a05y)/(a05z*o04x_2)
n04y_1=sqrt(1/(u_1^2+v_1^2+1))
n04y_2=-sqrt(1/(u_1^2+v_1^2+1))
n04y_3=sqrt(1/(u_2^2+v_2^2+1))
n04y_4=-sqrt(1/(u_2^2+v_2^2+1))
n04x_1=-n04y_1*u_1;
n04x_2=-n04y_2*u_1;
n04x_3=-n04y_3*u_2;
n04x_4=-n04y_4*u_2;
n04z_1=n04y_1*v_1;
n04z_2=n04y_2*v_1;
n04z_3=n04y_3*v_2;
n04z_4=n04y_4*v_2;
n04x=[n04x_1,n04x_2,n04x_3,n04x_4];
n04y=[n04y_1,n04y_2,n04y_3,n04y_4];
n04z=[n04z_1,n04z_2,n04z_3,n04z_4];
O04Z=0;
o04x=[o04x_1,o04x_2];
o04y=[o04y_1,o04y_2];
j=0;
%Antes de continuar, calculamos q1, porque lo necesitaremos para
%seleccionar la solución deseada.
Q1_u=atan2((Brazo*P04x*sqrt(P04x^2+P04y^2-d4^2)-P04y*d4),(-
Brazo*P04y*sqrt(P04x^2+P04y^2-d4^2)-P04x*d4))
%Reducimos, de 4 soluciones posibles, a 2, utilizando la condicion de

%Sistema de Coordenadas dextrógiro, para S04.
for i=1:4
if i<3
XxY=cross([n04x(i),n04y(i),n04z(i)],[o04x(1),o04y(1),0]);
% YxZxoyo=cross([o04x(1),o04y(1),0],[a05x,a05y,0]);
if abs(XxY(1)-a05x)<0.0001 && abs(XxY(2)-a05y)<0.0001 &&
abs(XxY(3)-a05z)<0.0001
if (-pi/2<Q1_u && Q1_u<pi/2 && o04x(1)>0)||(Q1_u==-pi/2
&& o04y(1)<0)||(Q1_u==pi/2 && o04y(1)>0 )
j=j+1;
P03xv(j)=P05x-(L5)*a05x-L4*n04x(i)+d4*o04x(1);
P03yv(j)=P05y-(L5)*a05y-L4*n04y(i)+d4*o04y(1);
P03zv(j)=P05z-(L5)*a05z-L4*n04z(i)+d4*o04z;
end
end
else
XxY=cross([n04x(i),n04y(i),n04z(i)],[o04x(2),o04y(2),0]);
98
% YxZxoyo=cross([o04x(2),o04y(2),0],[a05x,a05y,0]);
if abs(XxY(1)-a05x)<0.0001 && abs(XxY(2)-a05y)<0.0001 &&
abs(XxY(3)-a05z)<0.0001
if (-pi/2<Q1_u && Q1_u<pi/2 && o04x(2)>0)||(Q1_u==-pi/2
&& o04y(2)<0)||(Q1_u==pi/2 && o04y(2)>0)
j=j+1;
P03xv(j)=P05x-(L5)*a05x-L4*n04x(i)+d4*o04x(2);
P03yv(j)=P05y-(L5)*a05y-L4*n04y(i)+d4*o04y(2);
P03zv(j)=P05z-(L5)*a05z-L4*n04z(i)+d4*o04z;
end
end
end
end
nsol=length(P03xv);
sol=ones(nsol,6);
for i=1:nsol
fprintf('ANGULOS OBTENIDOS POR CINEMATICA INVERSA');
fprintf('\n');
%de nuevo q1
% Q1(i)=atan2(-P03xv(i),P03yv(i))
% Q1(i)=atan2(-Brazo*P04y*sqrt(P04x^2+P04y^2-d4^2)-P04x*d4,-
Brazo*P04x*sqrt(P04x^2+P04y^2-d4^2)-P04y*d4);
Q1(i)=Q1_u
% Q1_2(i)=atan2(P03xv(i),-P03yv(i))
%trabajar esta condicion para encontrar el correcto Q1
if Q1(i)>pi/2 && Q1(i)<pi

Q1(i)=Q1(i)-pi
end
if Q1(i)>-pi && Q1(i)<-pi/2

Q1(i)=Q1(i)+pi
end
%calculamos q2
K1=(P03zv(i)-L1);
K2=(P03yv(i)*cos(Q1(i))-P03xv(i)*sin(Q1(i))-d1);
b=2*K2*L2;
a=2*K1*L2;
c=-L3^2+K1^2+K2^2+L2^2;
Aux=b^2+a^2-c^2;
%agregamos esta parte de código, por que a veces en algunas
%configuraciones, debido a los redondeos q realiza matlab, al
calcular q2,
%el determinante de la ecuación cuadrática llega a presentar una
pequeña
%parte imaginaria del orden de 10^-6 en adelante, con lo cual se
nos genera
%una solución imaginaria que no debería generarse, esto lo
solucionamos
%asi:
if abs(Aux-0)<0.00001
Aux=0
end
if Aux < 0
fprintf('No solucion real');

fprintf('\n');
99
Q2(i)=100;
sol(i,1)=0;
continue
end
Q2(i)=2*atan((b+Brazo*Codo*sqrt(Aux))/(a+c))
if a+c==0
Q2(i)=pi;
end
% Calculamos q3 de otra manera

cosq3=(cos(Q2(i))*K1+sin(Q2(i))*K2-L2)/L3;
sinq3=(cos(Q2(i))*K2-sin(Q2(i))*K1)/L3;
Q3(i)=atan2(sinq3,cosq3)
%Calculamos Q4
%Agregar condición porque algunos Q2 y Q3 son complejos, por tanto
el
%programa no avanzará por q atan no admite solucinoes complejas
cosq4=a05y*cos(Q2(i)+Q3(i))*cos(Q1(i))-a05z*sin(Q2(i)+Q3(i))-
a05x*cos(Q2(i)+Q3(i))*sin(Q1(i));
sinq4=-a05z*cos(Q2(i)+Q3(i))-
a05y*sin(Q2(i)+Q3(i))*cos(Q1(i))+a05x*sin(Q2(i)+Q3(i))*sin(Q1(i));
Q4(i)=atan2(sinq4,cosq4)
%Calculamos Q5
cosq5=o05x*cos(Q1(i))+o05y*sin(Q1(i));
sinq5=n05x*cos(Q1(i))+n05y*sin(Q1(i));
Q5(i)=atan2(sinq5,cosq5);
Q1s(i)=Q1(i)*180/pi
Q2s(i)=Q2(i)*180/pi
Q3s(i)=Q3(i)*180/pi
Q4s(i)=Q4(i)*180/pi
Q5s(i)=Q5(i)*180/pi
sol(i,:)=[1 Q1s(i) Q2s(i) Q3s(i) Q4s(i) Q5s(i)]
end
%LA SOLUCIÓN ES LA SIGUIENTE:

for i=1:nsol
if sol(i,1)==1
angulos=sol(i,2:6);
end
end
tiempo=toc
end
100
A.7 VERIFICACIÓN DE CINEMÁTICA INVERSA Y DIRECTA
%PROGRAMA PARA VERIFICAR EL ALGORITMO DE CINEMÁTICA DIRECTA E INVERSA

%UTILIZANDO ECUACIONES DE DECISIÓN
%Parámetros del Robot

L1=16.4;d1=10.5; L2=104;L3=94.5;L4=26;
d4=21.9;
L5=103.4;
Q1=-pi/2+(pi/2+pi/2).*rand(50,1);
Q2=-pi/2+(pi/2+pi/2).*rand(50,1);
Q3=-pi/2+(pi/2+pi/2).*rand(50,1);
Q4=-pi+(0+pi).*rand(50,1);
Q5=-pi/2+(pi/2+pi/2).*rand(50,1);
Q1(1)=0*pi/180;
Q2(1)=20*pi/180;
Q3(1)=87*pi/180;
Q4(1)=-130*pi/180;
Q5(1)=0*pi/180;
e_tot=0
tiempo_tot=0
for i=1:50
q1=Q1(i);
q2=Q2(i);
q3=Q3(i);
q4=Q4(i);
q5=Q5(i);
% q1s=q1*180/pi;
% q2s=q2*180/pi;
% q3s=q3*180/pi;
% q4s=q4*180/pi;
% q5s=q5*180/pi;
angulos_ing(i,:)=[q1 q2 q3 q4 q5];
A05 = [ cos(q1)*sin(q5) - sin(q2 + q3 + q4)*cos(q5)*sin(q1),
cos(q1)*cos(q5) + sin(q2 + q3 + q4)*sin(q1)*sin(q5), -cos(q2 + q3 +
q4)*sin(q1), - d4*cos(q1) - d1*sin(q1) - L2*sin(q1)*sin(q2) -
L5*cos(q2 + q3 + q4)*sin(q1) - L4*cos(q2 + q3)*sin(q1)*sin(q4) -
L4*sin(q2 + q3)*cos(q4)*sin(q1) - L3*cos(q2)*sin(q1)*sin(q3) -
L3*cos(q3)*sin(q1)*sin(q2);
sin(q1)*sin(q5) + sin(q2 + q3 + q4)*cos(q1)*cos(q5),
cos(q5)*sin(q1) - sin(q2 + q3 + q4)*cos(q1)*sin(q5), cos(q2 + q3 +
q4)*cos(q1), d1*cos(q1) - d4*sin(q1) + L2*cos(q1)*sin(q2) +
L5*cos(q2 + q3 + q4)*cos(q1) + L4*cos(q2 + q3)*cos(q1)*sin(q4) +
L4*sin(q2 + q3)*cos(q1)*cos(q4) + L3*cos(q1)*cos(q2)*sin(q3) +
L3*cos(q1)*cos(q3)*sin(q2);
cos(q2 + q3 + q4)*cos(q5),
-cos(q2 + q3 + q4)*sin(q5), -sin(q2 + q3 + q4),
L1 + L3*cos(q2 + q3) + L2*cos(q2) + L4*cos(q2 + q3 + q4) - L5*sin(q2 +
q3 + q4);
0,
0, 0,
1];
P05x=A05(1,4);
P05y=A05(2,4);
P05z=A05(3,4);
a05x=A05(1,3);
101
a05y=A05(2,3);
a05z=A05(3,3);
n05x=A05(1,1);
n05y=A05(2,1);
n05z=A05(3,1);
o05x=A05(1,2);
o05y=A05(2,2);
o05z=A05(3,2);
P04x=P05x-(L5)*a05x;
P04y=P05y-(L5)*a05y;
P04z=P05z-(L5)*a05z;
Brazo=sign(P04x*sin(q1) - P04y*cos(q1));
if Brazo==0
Brazo=1;
end
Codo=-Brazo*sign(L3*sin(q3));
[angulos,tiempo]=Cin_Inv(A05,Brazo,Codo);
angulos_cal(i,:)=angulos;
e=(abs(q1-angulos(1))+abs(q2-angulos(2))+abs(q3-
angulos(3))+abs(q4-angulos(4))+abs(q5-angulos(5)))/5;
if e>1
verf(i)=1;
end
e_tot=e_tot+e;
Brazov(i)=Brazo;
Codov(i)=Codo;
P05xv(i)=P05x;
P05yv(i)=P05y;
P05zv(i)=P05z;
tiempo_tot=tiempo_tot+tiempo;
end
tiempo_prom=tiempo_tot/50;
e_tot=e_tot/50
102
A.8 GENERACIÓN DE TRAYECTORIAS
%PROGRAMA PARA GENERAR UNA TRAYECTORIA LINEAL PARA UN BRAZO ROBÓTICO

DE 5
%GDL.
clc
clear all
close all
%%
%Paso 1: Datos de entrada
%Errores máximos deseados
deltar_max=5*pi/180;
deltap_max=1;
%%%%%%
% POSICIÓN INICIAL
p0=[ -21.9000;
113.9000;
240.9000];
R0=[0 1 0;
0 0 1;
1 0 0];
% p0=[ -21.9000;
% 221.4621;
% 150.8336];
%
% R0=[ 0 1.0000 0;
% -0.3907 0 0.9205;
% 0.9205 0 0.3907];
F0=[R0(1,1) R0(1,2) R0(1,3) p0(1);

R0(2,1) R0(2,2) R0(2,3) p0(2);
R0(3,1) R0(3,2) R0(3,3) p0(3);
0 0 0 1 ];
%%%%%%%%%%%%%%%%%%%%%%%
%POSICIÓN FINAL
p1=[ -21.9000;
221.4621;
150.8336];
%
% R1=[ 0 1.0000 0;
% -0.3907 0 0.9205;
% 0.9205 0 0.3907];
%
% p1=[ -21.9000;
% 221.4621;
% 50];
%
R1=[0 1 0;
0 0 1;
1 0 0];
F1=[R1(1,1) R1(1,2) R1(1,3) p1(1);
R1(2,1) R1(2,2) R1(2,3) p1(2);
R1(3,1) R1(3,2) R1(3,3) p1(3);
0 0 0 1 ];
%%%%%%%%%%%%%%%%%%%%%%%%%%5
103
Brazo=-1;
Codo=1;
tic;
q0=Cin_Inv(F0,Brazo,Codo);
%
q1=Cin_Inv(F1,Brazo,Codo);
%Conversión de matriz de rotación a cuaternión.
c0=mrot2quat(R0);
c1=mrot2quat(R1);
%Almacenamos los primeros puntos nudos segun Algoritmo BDJP.
c(1:4,1)=c0;
c(1:4,3)=c1;
q(1:5,1)=q0;
q(1:5,3)=q1;
p(1:3,1)=p0;
p(1:3,3)=p1;
logic=0;
iter=0;
noconv=0;
while logic==0
% for op=1:2
deltar_t=0;
deltap_t=0;
[m,n]=size(q);
iter=iter+1;
for i=1:(n-1)/2
%Paso 1: jalamos las variables para el nuevo bucle
q0=q(1:5,2*i-1);
q1=q(1:5,2*i+1);
c0=c(1:4,2*i-1);
c1=c(1:4,2*i+1);
p0=p(1:3,2*i-1);
p1=p(1:3,2*i+1);
%Paso 2: Determinar puntos medios en el espacio de

articulación
deltaq=q1-q0;
qm=q1-1/2.*deltaq;
[pm,cm]=CinDirCuat(qm(1),qm(2),qm(3),qm(4),qm(5));
%Paso 3: Determinar puntos medios en espacio cartesiano
pc=(p0+p1)./2;
c01=quatmul(conjquat(c0),c1);
[n1,ang]=Rot_eje_ang(c01);
cc=quatmul(c1,Rot(n1,-ang/2));
Rc=quat2mrot(cc);
Fc=[Rc(1,1) Rc(1,2) Rc(1,3) pc(1);
Rc(2,1) Rc(2,2) Rc(2,3) pc(2);
Rc(3,1) Rc(3,2) Rc(3,3) pc(3);
0 0 0 1 ];
%Paso 4: Determinar errores de desviación
104
%Rc=quat2mrot(cc);
% cm=mrot2quat(Rm);
cc_conj=conjquat(cc);
%calculo de error
[nmn,deltar]=Rot_eje_ang(quatmul(cc_conj,cm));
deltap=norm(pm-pc);
%Rc=quat2mrot(cc);
%Almacenamos
deltap_t=deltap_t+deltap;
deltar_t=deltar_t+deltar;
%%%pc y Rc
p(1:3,2*i)=pc;
%R(1:3,6*i-2:6*i)=Rc;
%cc
c(1:4,2*i)=cc;
% q(1:5,2*i)=qc;
Fc_v(1:4,(4*i-3):4*i)=Fc;
end
deltap_prom=deltap_t/i;
deltar_prom=deltar_t/i;
if deltar_prom>=deltar_max || deltap_prom>=deltap_max
for k=1:i
q(1:5,2*k)=Cin_Inv(Fc_v(1:4,4*k-3:4*k),Brazo,Codo);
end
[m,n]=size(q);
q_2=zeros(5,2*n-1);
p_2=zeros(3,2*n-1);
%R_2=zeros(1:3,3*n+3*(n-1));
c_2=zeros(4,2*n-1);
for j=1:n
q_2(1:5,2*j-1)=q(1:5,j);
p_2(1:3,2*j-1)=p(1:3,j);
%R_2(1:3,(6*n-2):6*n)=R(1:3,(3*n-2):3*n);
c_2(1:4,2*j-1)=c(1:4,j);
end
q=q_2;
p=p_2;
c=c_2;
else
logic=1;
[u,v]=size(q);
for i=1:((v+1)/2)
q_f(1:5,i)=q(1:5,2*i-1);%
p_final(1:3,i)=p(1:3,2*i-1);
end
end
if iter>=15
105
fprintf('no convergencia')
noconv=1;
break
end
end
%
if noconv==0
%%%%%% Interpolación de trayectorias metodo de Taylor
[u,v]=size(q_f);
l_est=norm(p_final(1:3,2)-p_final(1:3,1));
pasos=l_est*8;
pasos=round(pasos);
tao=pasos/3;
tao=round(tao);
o=1;
%Calculo de periodos
for i=1:v-1
l=norm(p_final(1:3,i+1)-p_final(1:3,i));%longitud del segmento en
unidades métricas
T(i)=pasos*(l/l_est);%periodo de segmento
T(i)=round(T(i))
end
for i=1:v-1
t=0:1:T(i);%definimos un paso de 1 unidad, debido a que luego se

normalizará
for j=1:T(i)
lamda=(T(i)-t(j))/T(i);
if t(j)<=tao
lamda=(T(i)-t(j))/T(i);
if i==1%inicio del primer segmento de la trayectoria
path_q(1:5,o)=q_f(1:5,i+1)-lamda*(q_f(1:5,i+1)-
q_f(1:5,i));
o=o+1;
else
%ant t_int=T(i-1)-1+t(j)
t_int=T(i-1)+t(j);%mdf:T(i-1)-1+t(j) %tiempo de
recorrido desde el inicio del periodo
t_pri=t_int-T(i-1);
path_q(1:5,o)=q_f(1:5,i)-(tao-t_pri)^2/(4*tao*T(i-
1))*(q_f(1:5,i)-q_f(1:5,i-
1))+(tao+t_pri)^2/(4*tao*T(i))*(q_f(1:5,i+1)-q_f(1:5,i));
o=o+1;
end
elseif t(j)>tao && t(j)<T(i)-tao
q_f(1:5,i));
o=o+1;
else
if i==v-1%final del ultimo segmento de la trayectoria

q_f(1:5,i));
o=o+1;
106
else
t_int=t(j);%tiempo de recorrido en la niterseccion de
segmentos
t_pri=t_int-T(i);
path_q(1:5,o)=q_f(1:5,i+1)-(tao-
t_pri)^2/(4*tao*T(i))*(q_f(1:5,i+1)-
q_f(1:5,i))+(tao+t_pri)^2/(4*tao*T(i+1))*(q_f(1:5,i+2)-q_f(1:5,i+1));
o=o+1;
end
end
end
end
tiempo=toc;
%Encontramos trayectoria
for i=1:o-1
[path_p(1:3,i),path_R(1:3,i:i+2)]=Cin_Dir(path_q(1,i),
path_q(2,i),path_q(3,i),path_q(4,i),path_q(5,i));
path_x(i)=path_p(1,i);
path_y(i)=path_p(2,i);
path_z(i)=path_p(3,i);
end
[m n]=size(q_f)
for i=1:n
p_f_x(i)=p_final(1,i);
p_f_y(i)=p_final(2,i);
p_f_z(i)=p_final(3,i);
end
length(path_z);
t=0:1:length(path_z)-1;
tn=t./(length(path_z));
t_p=0:pasos-1:length(path_z);
t_p_n=t_p./length(path_z);
figure
plot3(path_x,path_y,path_z)
grid on
xlabel('Coordenada x')
ylabel('Coordenada y')
zlabel('Coordenada Z')
%% exportación de datos a SolidWorks

tn2=1:0.01:1.25;
tnf=[tn,tn2];
q3_sig(1,1:length(tn2))=path_q(3,length(path_y));
q3_solid=[path_q(3,:) q3_sig];
q3_vector=[10.*tnf' q3_solid'.*(180/pi)];
107
dlmwrite('q3_data.txt',q3_vector)
q2_vector=[10.*tn' path_q(2,:)'.*(180/pi)];
108
B. PLANOS DEL ROBOT MANIPULADOR
109
C.
110

Tesis: IMPLEMENTACIÓN DE UN ALGORITMO PARA LA GENERACIÓN DE TRAYECTORIAS DE UN BRAZO ROBÓTICO DE 5 GDL UTILIZANDO CUATERNIONES

Cargado por

Información del documento

Título original

Derechos de autor

Formatos disponibles

Compartir este documento

Compartir o incrustar documentos

Opciones para compartir

¿Le pareció útil este documento?

¿Este contenido es inapropiado?

Copyright:

Formatos disponibles

Tesis: IMPLEMENTACIÓN DE UN ALGORITMO PARA LA GENERACIÓN DE TRAYECTORIAS DE UN BRAZO ROBÓTICO DE 5 GDL UTILIZANDO CUATERNIONES

Cargado por

Copyright:

Formatos disponibles

Universidad Nacional de Trujillo

IMPLEMENTACIÓN DE UN ALGORITMO PARA LA GENERACIÓN

Alva Alcántara, Josmell Henry

Dedico este trabajo a mi familia.

A mi hermana Jackelyn. Hermana, somos compañeros de vida, hemos reído y llorado,

La cinemática directa se realizó utilizando el algoritmo de Denavit Hartenberg y

La generación de trayectorias se obtuvo utilizando cuaterniones, debido a la facilidad que

CAPÍTULO I: INTRODUCCIÓN ................................................................................................ 1

La Robótica es un área de mucho interés en el entorno científico y tecnológico, sobre todo

“KUKA AG es una empresa presente a nivel internacional con un volumen de ventas de

Robots industriales de KUKA

En su página web [2], KUKA, nos menciona sus Manipuladores:

• Robots resistentes al calor y a la suciedad para entornos especialmente adversos

• Robots pequeños con equipamiento Waterproof, adecuados para su uso en

• Robots pequeños equipados como "Hygienic Machine (HM)" para el contacto

• Concatenadores de prensas con un alcance enorme para cargar y descargar piezas

• Robots de paletizado para tareas de manipulación de cualquier tipo

• Robots de soldadura orientados a la precisión y la máxima movilidad

• Robots de consola en todas las variantes

• Robots de alta precisión para una precisión máxima”

1.2.2. UNIVERSAL ROBOTS

“Cuando decimos que los robots de UR pueden automatizarlo prácticamente todo,

“Los brazos robóticos de Universal Robots se integran fácilmente en los entornos de

Fig. 1.2 Robots industriales de Universal Robots

1.2.4. Tesis “DISEÑO DE UNA INTERFAZ DE USUARIO Y CONTROL

1.2.5. Tesis “METODOLOGÍA PARA GENERACIÓN DE TRAYECTORIAS DE

“En esta tesis se presenta un esquema controlador-observador de posición-fuerza usando

Buena parte de las definiciones y clasificaciones de robots existentes responde al robot

• Manipulador: mecanismo formado generalmente por los elementos en serie,

1.3.1.2. Estructura mecánica de un robot manipulador

Fig. 1.3 Distintos tipos de articulaciones para robots

Fig. 1.4 Robot Manipulador de 6 Grados de Libertad.

1.3.2. Descripciones espaciales y transformaciones

Fig. 1.5 Vector Relativo a una trama

B) Descripción de una Orientación

Fig. 1.6 Ubicación de un objeto en posición y orientación

coordenadas {A}, se llaman 𝐴𝑋⃗𝐵 , 𝐴𝑌

vectores unitarios como columnas de una matriz de 3𝑥3, en el orden 𝐴𝑋⃗𝐵 , 𝐴𝑌

𝑟11 𝑟12 𝑟13

Así, 𝐵𝐴𝑅 , la descripción de la trama {A} relativa a {B}, se da mediante la transpuesta de

Donde 𝐼3 es la matriz identidad 3𝑥3. Por lo que:

C) Descripción de una trama

{𝐵} = { 𝐵𝐴𝑅 , 𝐴𝑃𝐵𝑂𝑅𝐺 } (1.8)

1.3.2.2. Asignaciones: Como cambiar posiciones de trama a trama

En este apartado se trata el problema de cómo expresar la misma cantidad en diferentes

Fig. 1.8 Transformada general de un vector.

Las Matrices de Transformación Homogénea se utilizan como operadores. Por ejemplo

La matriz de transformación homogénea, en adelante MTH, se construye de la siguiente

1.3.2.3. Aritmética de transformaciones

Con ello calculamos 𝐴𝑃 de la siguiente manera:

(Fuente: Craig (2006. p. 35))

1.3.2.4. Resumen de interpretaciones

Las matrices de transformación homogénea son la herramienta principal en los

1.3.2.5. Cinemática de Manipuladores

Fig. 1.10 Diagrama de relación entre cinemática directa e inversa

Las siguientes propiedades del álgebra de cuaterniones son básicas:

Cuaternión unidad: 𝑠 − (𝑎𝒊 + 𝑏𝒋 + 𝑐𝒌), donde 𝑠 2 + 𝑎2 + 𝑏 2 + 𝑐 2 = 1

Aritmética básica de cuaterniones

El producto tiene la forma de:

1.3.3.1.1. Equivalencia entre Par de Rotación ( Representación eje-ángulo) y

Convertir par de rotación a cuaternion:

Convertir cuaternion a par de rotación

𝜃 = 2𝑎𝑡𝑎𝑛2(√𝑞1 2 + 𝑞2 2 + 𝑞3 2 , 𝑞0) (1.21)