Instituto Politécnico Nacional

Escuela Superior de Ingeniería Mecánica y Eléctrica

Sección de Estudios de Posgrado e Investigación

Álgebra dual en el análisis cinemático y dinámico

del manipulador industrial robótico (MIRH) y
validación por matrices de transformación
homogénea

T E S I S

QUE PARA OBTENER EL GRADO DE

MAESTRO EN CIENCIAS
CON ESPECIALIDAD EN INGENIERÍA MECÁNICA

PRESENTA:
ING. RICARDO TAPIA HERRERA

DIRIGIDA POR:
M. EN C. CANDIDO PALACIOS MONTUFAR
DR. JUAN ALEJANDRO FLORES CAMPOS

México, D.F. 2009

 Agradecimientos

A Dios por brindarme la oportunidad de terminar una etapa más en vida.

Al Instituto Politécnico Nacional, a la Sección de Estudios de Posgrado e Investigación, a

todos mis profesores, a la Comisión Revisora y a mis directores de tesis, M. en C. Cándido
Palacios Montufar, Dr. Juan Alejandro Flores Campos por la formación académica y los
conocimientos proporcionados para la elaboración de esta tesis.

A mi familia, mis hermanos Rodrigo, Roberto, Fernando y Andrea por ser una fuente de
inspiración y en especial a mis padres Ma. Del Refugio Herrera Pérez y Ricardo Tapia
Rodríguez por apoyarme siempre incondicionalmente durante toda mi formación
académica.

A mis compañeros y amigos que siempre me apoyaron y brindaron sus consejos

A todos ustedes mis más grande y sincero agradecimiento.

Resumen
Este trabajo presenta y desarrolla un estudio en forma explícita de la aplicación de
los números duales y de las matrices de transformación homogénea, en el
modelado cinemático y dinámico de un manipulador robótico industrial de 5 grados
de libertad con articulaciones de tipo revolución (topología 5R).

La cinemática directa resuelta por algebra dual es establecida por la ecuación de

cadena cerrada formada por matrices duales de dimensión 3x3, mismas que
proporcionan simultáneamente las rotaciones y translaciones de un sistema de
cuerpos rígidos en el espacio. Por otra parte las matrices de transformación
homogénea utilizadas regularmente para solucionar el problema cinemático
directo, son aplicadas para validar los resultados, analizar ventajas y desventajas
entre ambas metodologías.

También es establecida la matriz Jacobiana dual generalizada que se obtiene a

partir de las matrices duales, esta matriz Jacobiana permite realizar un análisis
generalizado de velocidades en manipuladores con diferentes topologías.

La dinámica es desarrollada aplicando el momento dual y el principio de

D’Alembert, con estas herramientas es posible calcular las reacciones y torques
en cada una de las articulaciones.

Los momentos o torques de cada articulación también son obtenidos por el

método energético de Lagrange-Euler, con la finalidad de poder comparar los
resultados obtenidos del algebra dual y poder determinar la eficiencia de esta
herramienta.

Como complemento de este trabajo se desarrolla una interface gráfica que permite
visualizar el movimiento del manipulador MIRH en un espacio de trabajo, con los
resultados obtenidos por la formulación dual.

I
Abstract
This work presents and develops a study in explicit form of dual numbers and
homogeneous transformation matrices application in the kinematic and dynamic
modeling of an industrial robotic manipulator with 5 degrees of freedom and
revolute joins (5R topology)

The forward kinematic is solved by dual algebra establishing the closed chain
equation composted with 3x3 dual matrices. They give the rotation and
displacement of a set of links in the space.

The homogeneous transformation matrices are used to validate the results and
analyze the advantages and disadvantages between both methodologies.

The generalized dual Jacobian Matrix is obtained from the dual matrices; this
Jacobian matrix allows a generalized analysis of velocities in manipulators with
different range of topologies.

The dynamics is developed applying the dual momentum and D’Alembert principle,
using these tools is possible to calculate the reactions and torques in each joint.

The momentums or torques of each joint are obtained by the energetic method of
Lagrange-Euler too. With the finality to compare the calculated results of dual
algebra and to be able for establish the efficiency of this tool.

As complement of this work, is develop a graphic interface that allows visualize the
MIRH movements in a work space, with the results obtained by the dual
formulation.

II
Índice general
Resumen .................................................................................................................. I
Abstract ................................................................................................................... II
Índice general ......................................................................................................... III
Índice de figuras ..................................................................................................... VI
Índice de tablas .................................................................................................... VIII
Simbología ............................................................................................................. IX
Justificación ............................................................................................................ XI
Introducción ........................................................................................................... XII
Capítulo 1 Estado del Arte....................................................................................... 1
1.1. Antecedentes de los números duales. .......................................................... 2
1.2. Cinemática espacial. ..................................................................................... 2
1.3. El álgebra dual en el análisis de manipuladores robóticos. .......................... 3
1.4. Dinámica de cuerpos rígidos ........................................................................ 3
Capítulo 2 Marco Teórico ........................................................................................ 5
2.1. Generalidades del álgebra dual .................................................................... 5
2.2. Operaciones básicas del álgebra dual .......................................................... 5
2.3. Ángulo dual ................................................................................................... 6
2.4. Funciones trigonométricas de los ángulos duales. ....................................... 8
2.5. Velocidad dual .............................................................................................. 9
2.5.1. Velocidades en diferentes puntos de un cuerpo rígido .............................. 9
2.5.2. Velocidades expresadas en diferentes sistemas de referencia ................. 9
2.5.3. Teorema de velocidades relativas ........................................................... 10
2.6. Fuerza dual ................................................................................................. 10
2.7. Momento dual ............................................................................................. 10
2.8. Fuerza dual de inercia ................................................................................ 12
2.9. Equilibrio dinámico de un eslabón .............................................................. 12
Capítulo 3. Solución de las ecuaciones cinemáticas para generar una trayectoria
de trabajo definida. ................................................................................................ 13
3.1. Cinemática directa del MIRH aplicando algebra dual ................................. 13
3.2. Cinemática inversa por números duales ..................................................... 17

III
 3.3. Análisis de velocidades por números duales .............................................. 18
3.4. Análisis inverso de velocidades por números duales ................................. 21
3.5. Análisis de Aceleraciones ........................................................................... 24
3.6. Análisis inverso de aceleraciones ............................................................... 25
3.7. Cinemática directa del MIRH por matrices de transformación homogénea. 25
3.8. Solución de la cinemática inversa ............................................................... 28
3.9. Análisis de Velocidades .............................................................................. 29
3.10. Análisis inverso de velocidades por algebra matricial ............................... 33
3.11. Análisis inverso de aceleraciones por algebra matricial............................ 34
Capítulo 4 Solución de las ecuaciones dinámicas del manipulador. ..................... 35
4.1. Análisis de fuerzas dinámicas por números duales .................................... 35

4.1.1. Velocidades PVˆi ,Pj de cada eslabón .......................................................... 35

4.1.2. Matrices de inercia de los eslabones del manipulador en estudio. .......... 37

4.1.3. Cálculo del Momento dual de cada eslabón. ........................................... 40
4.1.4. Cálculo de las fuerzas duales de inercia .................................................. 41
4.1.5. Principio de D’Alembert ........................................................................... 42
4.2. Solución dinámica por el método de Lagrange – Euler............................... 44
4.2.1 Matrices de inercia de cada eslabón ........................................................ 45
4.2.2. Matriz simétrica de inercia ....................................................................... 45
4.2.3. Vector de fuerza de Coriolis y centrífuga no lineal h(q, q ) . ..................... 48
4.2.4. Vector de carga gravitacional .................................................................. 56
4.3. Generación de Trayectorias ........................................................................ 56
4.3.1. Polinomios de quinto orden ..................................................................... 56
4.4. Interface MATLAB – SIMULINK – VISUAL NASTRAN ............................... 58
Capítulo 5 Análisis de resultados. ......................................................................... 61
5.1. Análisis de la cinemática directa ................................................................. 61
5.2. Análisis de la cinemática inversa ................................................................ 64
5.3. Análisis de velocidades y aceleraciones ..................................................... 66
5.4. Generación de trayectorias ......................................................................... 66
5.5. Análisis dinámico ........................................................................................ 75

IV
Conclusiones ......................................................................................................... 79
Recomendaciones para trabajos futuros ............................................................... 81
Anexo A.- Programa para solucionar la cinemática directa por álgebra dual ........ 82
Anexo B.- Programa para solucionar la cinemática inversa por álgebra dual ....... 84
Anexo C.- Función del sistema de ecuaciones de diseño duales.......................... 85
Anexo D.- Programa para solucionar la cinemática directa por matrices de
transformación homogénea ................................................................................... 87
Anexo E.- Programa para solucionar la cinemática inversa por matrices de
transformación homogénea ................................................................................... 89
Anexo F.- Función del sistema de ecuaciones de diseño por matrices de
transformación homogénea ................................................................................... 90
Anexo G.- Programa para solucionar la dinámica por algebra dual ...................... 91
Anexo H.- Programa para solucionar la dinámica por matrices de transformación
homogénea ......................................................................................................... 103
Referencias ......................................................................................................... 119

V
Índice de figuras
Figura 2.1. Descripción Geométrica de un ángulo dual (Fisher, 1999) ................... 7

Figura 2.2. Ángulos duales ˆn y ˆ n para un eslabonamiento (E. Pennestri, 2007) 8

Figura 3.1. Esquema del algoritmo de transformación del espacio articulado al

espacio cartesiano. ............................................................................................... 13
Figura 3.2. Descripción geométrica del Manipulador Industrial Robótico Hidráulico
(MIRH) ................................................................................................................... 14
Figura 3.2. Condiciones iniciales para resolver la cinemática inversa .................. 17
Figura 4.1. Localización del centro de masas del eslabón 1 respecto al sistema de
coordenadas asignado. ......................................................................................... 37
Figura 4.2. Localización del centro de masas del eslabón 2 respecto al sistema de
coordenadas asignado. ......................................................................................... 38
Figura 4.3. Localización del centro de masas del eslabón 3 respecto al sistema de
coordenadas asignado. ......................................................................................... 38
Figura 4.4. Localización del centro de masas del eslabón 4 respecto al sistema de
coordenadas asignado. ......................................................................................... 39
Figura 4.5. Localización del centro de masas del eslabón 5 respecto al sistema de
coordenadas asignado. ......................................................................................... 39
Figura 4.19. Esquema de funcionamiento de la simulación ................................. 59
Figura 4.22. Modelo 3D con controles de rotación. .............................................. 59
Figura 4.21. Programa en SIMULINK para conectar MATLAB con el modelo 3D.60
Figura 5.1. Posición y orientación iniciales ........................................................... 61
Figura 5.2. Grafica comparativa de tiempo requerido para solucionar las
ecuaciones de lazo cerrado................................................................................... 63
Figura 5.3. Configuración final deseada ............................................................... 62
Figura 5.4. Gráfica comparativa de tiempo de solución del sistema de ecuaciones
por el método Newton-Raphson ............................................................................ 66
Figura 5.5. Posición articulación 1 ........................................................................ 68
Figura 5.6. Velocidad angular de la articulación 1 ................................................ 69
Figura 5.7. Aceleración angular articulación 1 ...................................................... 69
Figura 5.8. Posición articulación 2 ........................................................................ 69
Figura 5.9. Velocidad angular articulación 2 ......................................................... 70
Figura 5.10. Aceleración angular articulación 2 .................................................... 70

VI
Figura 5.11. Posición articulación 3 ...................................................................... 71
Figura 5.12. Velocidad angular articulación 3 ....................................................... 71
Figura 5.13. Aceleración angular articulación 3 .................................................... 72
Figura 5.14. Posición articulación 4 ...................................................................... 72
Figura 5.15. Velocidad angular articulación 4 ....................................................... 73
Figura 5.16. Aceleración angular articulación 4 .................................................... 74
Figura 5.17. Posición articulación 5 ...................................................................... 74
Figura 5.18. Velocidad angular articulación 5 ....................................................... 75
Figura 5.19. Aceleración angular articulación 5 .................................................... 75
Figura 5.20. Torque de la articulación 1 ............................................................... 76
Figura. 5.21. Torque articulación 2 ....................................................................... 76
Figura 5.22. Torque articulación 3 ........................................................................ 76
Figura 5.23. Torque articulación 4 ........................................................................ 77
Figura 5.24. Torque articulación 5 ........................................................................ 77

VII
Índice de tablas
Tabla 3.1. Parámetros de Denavit – Hartemberg para el MIRH............................ 15
Tabla 5.1. Ángulos propuestos ............................................................................. 62
Tabla 5.2. Vector de posición del efector final para los ángulos de entrada
establecidos .......................................................................................................... 63
Tabla 5.3. Vectores de posición deseados ........................................................... 64
Tabla 5.4. Vectores normal, de orientación y de aproximación deseados ............ 65
Tabla 5.5. Ángulos calculados por algebra dual. .................................................. 65
Tabla 5.6. Ángulos calculados por matrices de transformación homogénea. ....... 66

VIII
Simbología
aˆ.  Número dual

IX
Objetivo
Obtener el modelo cinemático y dinámico de un manipulador robótico industrial
aplicando la formulación del álgebra dual y validarla con la formulación de matrices
de transformación homogénea.

Objetivos Particulares

 Determinar las matrices duales eslabón-articulación

 Obtener las ecuaciones de diseño duales del manipulador para formar un
sistema de ecuaciones sobredeterminado no lineal.
 Obtener las velocidades duales del efector final.
 Calcular las velocidades duales de cada articulación a partir de la velocidad
dual del efector final.
 Calcular las aceleraciones duales de cada articulación.
 Determinar las fuerzas duales requeridas en el manipulador para realizar
una trayectoria deseada.
 Establecer las matrices de transformación homogénea.
 Resolver la cinemática directa e inversa por matrices de 4x4
 Desarrollar la dinámica del manipulador por el algoritmo de Langrange –
Euler.
 Resolver el problema de la generación de trayectorias punto a punto
mediante polinomios de quinto orden.
 Aplicar las ecuaciones duales para la simulación del movimiento del
manipulador robótico industrial en un software comercial.

X
Justificación
Una gran cantidad de algoritmos se han desarrollado para la solución de los
problemas de la cinemática y dinámica de manipuladores robóticos. El costo
computacional, facilidad de programación, facilidad de manipulación y
simplificación algebraica, son algunas de las características principales que debe
tener un algoritmo. Esto puede resumirse en que el mejor algoritmo para gobernar
el movimiento de un manipulador es aquel que tenga rapidez y precisión a un bajo
costo.

Los métodos geométricos para la solución de la cinemática y dinámica de robots

manipuladores, presentan ventajas esencialmente en velocidad de solución,
precisión y consumo de recursos computacionales.

En particular los números duales han sido una herramienta matemática

ampliamente utilizada por investigadores para representar movimientos en el
espacio de sistemas complejos de cuerpos rígidos. Por lo que el algebra dual
presenta una opción compacta para solucionar la cinemática y dinámica de
mecanismos espaciales de forma generalizada.

El algebra dual a pesar de ser una herramienta analítica con más de un siglo de
antigüedad no es hasta la década de los 80’s que comienza su aplicación y tras
una exhaustiva investigación literaria pocos son los trabajos enfocados a la
solución cinemática y dinámica de robots industriales, dejando aún un amplio
campo abierto en la investigación de rotaciones infinitesimales de cuerpos rígidos.

Específicamente en la Sección de Posgrado de la E.S.I.M.E. Zacatenco no se han

realizado investigaciones y aplicaciones de los números duales, como alternativa a
la solución de la cinemática y dinámica de manipuladores robóticos industriales.

XI
Introducción
La necesidad de aumentar la productividad, conseguir productos con una calidad
uniforme, realizar exploraciones en lugares peligrosos, manejo de materiales
radioactivos, entre otras actividades más, ha hecho que se busquen alternativas
de automatización basadas en máquinas que operen bajo algoritmos
computacionales eficientes, dando como resultado que los robots representen una
alternativa flexible para realizar diversos trabajos, ya que actualmente son
maquinas fácilmente de reprogramar por lo que hay miles de robots alrededor del
mundo, especialmente en el sector industrial usados para realizar trabajos con una
gran precisión.

La RIA (Robot Institute of America) define a un robot como un manipulador

reprogramable multifuncional diseñado para mover materiales, partes,
herramientas o dispositivos especializados a través de movimientos variables
programados para el desempeño de una variedad de tareas.

La aplicación y correcto funcionamiento de un manipulador robótico dentro del

sector industrial o en el campo de la investigación depende del estudio de su
movimiento, de las fuerzas que en él intervienen y de los sistemas de control que
lo gobiernan. Así mismo se ha hecho necesario desarrollar herramientas que
permitan emular el comportamiento de un robot para un óptimo desempeño en las
tareas a realizar.

Las matrices de transformación homogénea son una de las herramientas con

mayor difusión en la literatura, por otra parte a pesar de ser una herramienta
analítica con más de 100 años, el álgebra dual no ha tenido una profunda
aplicación en la robótica.

En el capítulo 1 se hace una semblanza, de algunos de los trabajos más

representativos realizados hasta la actualidad, que tratan la aplicación del algebra
dual en el análisis de manipuladores robóticos.

Las reglas y conceptos que forma el algebra dual son presentados en el capítulo
2.

El capitulo 3 aborda el tema de la posición y orientación del efector final, así como
la configuración geométrica o valores de articulación requeridos para alcanzar una
posición y orientación deseada, tomando como base las ecuaciones de diseño
duales y matriciales, para formar un sistema de ecuaciones el cual debe satisfacer
las condiciones iniciales.

XII
También el análisis de velocidades y de aceleraciones tanto en el espacio
cartesiano como en el espacio articulado, es planteando y solucionado por la
matriz Jacobiana convencional y por la matriz Jacobiana dual generalizada.

En el capítulo 4 se establece el modelo que gobierna el movimiento dinámico del

manipulador, que es resuelto aplicando el principio de D’Alembert y la metodología
de Lagrange Euler, basadas en los números duales y en matrices de
transformación homogénea respectivamente, además es desarrollado un
generador de trayectorias para obtener los valores en el espacio articulado para
los puntos a satisfacer.

Finalmente se realiza un análisis comparativo de los resultados obtenidos por

ambas metodologías.

XIII
Capítulo 1 Estado del Arte.

1
El estudio del movimiento de manipuladores ha sido tratado por una gran cantidad
de investigadores alrededor del planeta. Dando como resultado una gran
diversidad de herramientas analíticas y computacionales para la solución de los
diferentes problemas que surgen de la robótica.

Dentro del campo de las herramientas analíticas para la representación del

movimiento en el espacio de cuerpos rígidos se encuentran (Seilig, 1996):

 Algebra matricial
 Algebra de Lie
 Geometría diferencial
 Teoría de tornillos
 Algebra dual
 Cuaterniones
 Cuaterniones duales

Un manipulador robótico, es un sistema múltiple de cuerpos rígidos de cadena

abierta, por lo que diversas herramientas que se aplican al estudio de mecanismos
espaciales o tridimensionales, son aplicadas a los manipuladores robóticos. Tal es
el caso de los números duales en comparación con los números reales,
representan simultáneamente rotaciones y traslaciones infinitesimales de cuerpos
rígidos en el espacio (movimiento de tornillo), que resultan en ecuaciones y
sistemas de ecuaciones que proporcionan mayor información del sistema o
mecanismo en estudio.

Partiendo del principio de transferencia, el cual establece que si se reemplazan los

números reales por números duales, las leyes del algebra vectorial que describen
la cinemática de un cuerpo rígido con un punto fijo, también serán aplicables al
algebra de motores (Bayro-Corrochano, 2000), la cual describe a un cuerpo rígido
libre (Brodsky y Shoham, 1999).
 Capítulo 1

El algebra dual puede ser una herramienta con un manejo matemático elegante
para calcular fuerzas, momentos, movimientos de traslación lineal y angular,
velocidades y aceleraciones, los números duales brindan una gran eficiencia para
transferir del plano al espacio los sistemas de cuerpos rígidos, por lo que los
programas basados en el algebra dual pueden ser generalizados a una diversidad
de mecanismos con diferentes topologías (Keler, 2000).

1.1. Antecedentes de los números duales.

El concepto de número dual fue introducido por primera vez en 1873 por el
matemático inglés William Kingdon Clifford (1845-1878), en 1903 el matemático
alemán Edgard Study (1862-1930) les denominó dicho nombre, y sus primeras
aplicaciones en el campo de la mecánica del cuerpo rígido específicamente en
mecanismos datan de 1964, realizadas por Yang y Freudenstein.

La manipulación de los algoritmos que emplean vectores y matrices duales son

objetivos de diversos autores tal es el caso de Pennestrì y Valentini (2008)
quienes realizan un estudio detallado del empleo de los números duales en la
cinemática del cuerpo rígido, su trabajo es basado en la recopilación de toda la
información que sustenta al algebra dual destacando la definición de la matriz
Pseudo-inversa de Moore Pen Rose planteada en forma dual.

De igual forma Pennestri y Stefanelli (2007) establecen de forma sistematizada los

algoritmos que se emplean en las trasformaciones de cuerpos rígidos en el
espacio, establecen el costo computacional de cada una de las operaciones
duales definidas hasta la actualidad, proponen los métodos QR y SVD para
descomponer matrices duales, también se plantean los algoritmos para obtener la
pseudoinversa, eigenvalores y eigenvectores de las matrices duales y la propuesta
de solución a sistemas lineales y no lineales ofreciendo estos últimos una
reducción de tiempo en el procesamiento de las operaciones duales.

1.2. Cinemática espacial.

La transformación de coordenadas en un espacio cartesiano tridimensional

comúnmente es realizada por la matriz de transformación homogénea, sin
embargo, la transformación homogénea es una transformación de punto
(transformación esférica). En contraste, en una transformación de línea
(transformación espacial) el elemento transformado es una línea en tres
dimensiones, esto significa que cantidades angulares y lineales pueden ser
representadas en una sola (Gu y Yang, 1987). Bajo este concepto se propone un
enfoque simplificado para la formulación de la cinemática y dinámica de
manipuladores roboticos, así como para el modelado de sistemas de control.

2
 Capítulo 1

1.3. El álgebra dual en el análisis de manipuladores robóticos.

Los desplazamientos angulares y lineales requeridos por cada una de las

articulaciones de un robot manipulador para alcanzar una posición y orientación
específica, son obtenidos resolviendo las ecuaciones de lazo cerrado. Estas
ecuaciones expresan el hecho de que la secuencia de transformación de
coordenadas entre cada eslabón del manipulador desde el efector final hasta la
base debe ser igual que la transformación directa desde la base al efector final.
Dichas transformaciones pueden ser realizadas empleando matrices ortogonales
de 3x3 cuyos elementos son los números duales, produciendo una forma dual de
la matriz de Denavit-Hartenberg y de la matriz Jacobiana característica de un
manipulador (Mc Carthy, 1986).

Funda (1988) realiza un análisis y comparación computacional entre los diferentes

métodos que existen para representar el movimiento de tornillo (rotación y
traslación) de un cuerpo rígido. Así mismo establece que las matrices ortogonales
duales representan una alternativa para la transformación de coordenadas de un
cuerpo rígido sujeto a transformaciones espaciales y desarrolla la programación
de números duales produciendo algoritmos computacionales con gran eficiencia
para resolver la cinemática directa e inversa. No obstante Funda concluye que el
alto costo de composición de las matrices duales ortogonales es su principal
desventaja.

Harry H. Cheng (1994) realiza un extenso estudio enfocado a la programación de

los números duales, debido a que por su naturaleza son difíciles de programar en
lenguajes convencionales. Cheng establece que los números duales son de gran
utilidad para un tratado analítico de la cinemática y dinámica de mecanismos
espaciales, sin embargo debido a la estructura de sus fórmulas no pueden ser
implementados como tal en lenguajes de programación comunes.

1.4. Dinámica de cuerpos rígidos

El modelo dinámico que representa el comportamiento de sistemas de cuerpos

rígidos en el espacio, ha sido una de las áreas que presentan mayor dificultad de
análisis debido a que en muchos de los casos las ecuaciones son demasiado
complejas, por lo que expresar cantidades lineales y angulares en una sola hace
que el algebra dual sea una alternativa para obtener el modelo dinámico de
sistemas de cuerpos rígidos complejos.

Brodsky y Shoham (1994), aplican el binor inercial (Featherstone, 1987), el cual

permite la relación directa del momento dual y la velocidad dual, por lo que la
ecuación del momento de movimiento lineal y su derivada con respecto al tiempo,

3
 Capítulo 1

es reemplazada en cada una de sus literales por la fuerza, velocidad e inercia

dual, lo que proporcionará los cambios de movimiento angular y lineal de un
cuerpo rígido. Por lo que las ecuaciones de las formulaciones de Newton- Euler y
Langrange son obtenidas en forma dual 3-dimensional, como resultado de la
aplicación sistemática del concepto de inercia dual para derivar el momento y la
energía dual.

Brodsky y Shoham (1996) también establecen la matriz Jacobiana en forma

generalizada, la cual se forma de las matrices de transformación duales en
contraste con la matriz Jacobiana convencional que está conformada por sólo
columnas específicas de las matrices de transformación homogéneas. La matriz
Jacobiana generalizada relaciona las fuerzas y momentos en el efector final, hacia
las fuerzas y momentos en todas las direcciones en las articulaciones. La matriz
Jacobiana generalizada permite relacionar el movimiento en todas las direcciones
de la articulación, hacia el movimiento del efector final.

En Pennock (1982) y en Backlouti (1993) se realizan estudios explícitos de los

modelos cinemático y dinámico inversos, de sistemas múltiples de cuerpos rígidos
de cadena abierta. Estos estudios aplican el principio de transferencia, algebra de
tornillos y matrices duales de transformación, en el cálculo de los modelos que
gobiernan el movimiento de un manipulador con la finalidad de realizar el mínimo
de operaciones aritméticas y así sintetizar el control en tiempo real.

4
Capítulo 2 Marco Teórico

2.1. Generalidades del álgebra dual

2
Los números duales en analogía con los números complejos son definidos por
(Fisher, 1999):

â  a   a0 (2.1)

Donde:

a  Componente primaria del número dual â

a0  Componente dual de â

De igual forma quedará definida una matriz dual  como:

  A   A0 (2.2)

En las ecuaciones (2.1) y (2.2)  es el operador de tornillo de Clifford, el cual

denota la parte dual de â y  . Así mismo se define la regla fundamental de los
números duales, la cual está dada por las siguientes expresiones:

 0 (2.3)

 2 ,  3 ,  4 ... n  0 (2.4)

2.2. Operaciones básicas del álgebra dual

Los números duales no corresponden a un espacio Euclidiano por lo que se han

establecido una serie de reglas para definir el algebra de los números duales.

Sean â y b̂ números duales y cˆ  aˆ  bˆ por lo tanto:

ĉ   a  b     a0  b0  (2.5)
 Capítulo 2

ˆ ˆ , la multiplicación estará dada

Así mismo si â y b̂ son números duales y cˆ  ab
por:

âbˆ   a   a0  b   b0  (2.6)

ĉ  ab    ab0  a0b  (2.7)

La división de dos números duales â y b̂ :

a a b  ab
cˆ   0 2 0 (2.8)
b b

Un número dual â elevado una potencia n será:

aˆ n  a n   na0a n1 (2.9)

La raíz cuadrada de un número dual â es:

a0
aˆ  a   (2.10)
2 a

La expansión de la serie de Taylor para un número dual â se expresa como:

f  aˆ   f  a   a0  (2.11)

df (a)
f  aˆ   f  a    a0 (2.12)
da

2.3. Ángulo dual

EL ángulo dual representa el desplazamiento relativo y la orientación entre dos

líneas en el espacio. Un ángulo dual ˆ será representado de la siguiente forma:

ˆ     S (2.13)

La figura 2.1 muestra geométricamente la definición del ángulo dual. Donde se

observa que la parte primaria  es el ángulo proyectado entre las líneas A y B, y
la parte dual S es la distancia más corta entre las dos líneas.

6
 Capítulo 2

Figura 2.1. Descripción Geométrica de un ángulo dual (Fisher, 1999)

En los mecanismos se establecen los ángulos duales ˆi y ˆ i , los cuales describen
las dimensiones de un eslabón así como su desplazamiento angular y lineal
respetivamente, por lo que ˆi y ˆ i son los ángulos duales que caracterizan
geométricamente al manipulador o mecanismo en estudio (Figura 2.2).

ˆi   i   ai (2.14)

ai  Distancia entre articulaciones de un eslabón (Longitud del eslabón)

 i  Angulo entre articulaciones de un eslabón (Giro del eslabón)

ˆi  i   Si (2.15)

i  Giro de la articulación (Movimiento angular)

Si  Offset de la articulación (Movimiento prismático, de deslizamiento o de

traslación).

7
 Capítulo 2

Figura 2.2. Parámetros

i ,  i , ai y Si para un eslabonamiento (Pennestri, 2007)

2.4. Funciones trigonométricas de los ángulos duales.

Las funciones trigonométricas de un ángulo dual son definidas a partir de los

conceptos de ángulo dual y de la expansión de la serie de Taylor.

sin ˆ  sin    S cos (2.16)

cosˆ  cos   S sin  (2.17)

tan ˆ  tan    S sec2  (2.18)

En el algebra dual de acuerdo a las ecuaciones antes mencionadas también es

necesario definir las identidades trigonométricas en forma dual. Por el principio de
transferencia (Fisher, 1999) las identidades trigonométricas aplicadas a los
números reales también son aplicables a los números duales.

sin 2 ˆ  cos 2 ˆ  1 (2.19)

sin 2ˆ  2sin ˆ cosˆ (2.20)

cos 2ˆ  cos 2 ˆ  sin 2 ˆ (2.21)

8
 Capítulo 2

 ˆ  1  cos ˆ
tan    (2.22)
2 sin ˆ

2.5. Velocidad dual

La velocidad dual en un movimiento de tornillo de un cuerpo rígido implica un

movimiento de traslación a lo largo del eje del tornillo y un movimiento de rotación
alrededor del eje antes mencionado. Estos movimientos en el algebra dual son
combinados en un vector dual de velocidades denominado motor.

Vˆ     V  uˆ (2.23)

De la expresión (2.23) Vˆ y û son vectores duales mientras que  y V son

escalares.

Para expresar la velocidad dual de cualquier punto sobre un cuerpo rígido dada la
velocidad dual en el eje sobre el que se realiza el movimiento de tornillo, se adopta
una notación para la velocidad dual representada en diferentes sistemas de
referencia.
R
Vˆi ,Pj  Velocidad dual del punto P del eslabón i relativo al eslabón j en términos
del sistema inercial R.

2.5.1. Velocidades en diferentes puntos de un cuerpo rígido

La velocidad dual de cualquier punto P de un eslabón o cuerpo rígido expresado

en un marco inercial P y con origen P se puede obtener si es conocida la
velocidad dual en términos del sistema S y con origen en S sobre el eje de
movimiento dual del mismo eslabón.
P
Vˆi ,Pj  PTˆS SVˆi ,Sj (2.24)

2.5.2. Velocidades expresadas en diferentes sistemas de referencia

Para expresar la ecuación (2.24) en términos de los vectores unitarios del sistema
Q, solamente es necesario considerar una rotación entre los dos sistemas de
referencia.
Q
Vˆi ,Pj  QTP PVˆi ,Pj (2.25)

9
 Capítulo 2

Q
El término TP es la parte primaria de la matriz dual de transformación de
coordenadas Q Mˆ .
P

2.5.3. Teorema de velocidades relativas

La velocidad relativa del eslabón m relativo al eslabón n (Vˆm,n ) se encuentra

establecida en forma dual por la siguiente ecuación.

Vˆk ,i  Vˆj ,i  Vˆk , j (2.26)

2.6. Fuerza dual

El algebra dual establece que si una fuerza y un torque actúan o están

representados en un sistema de coordenadas, se pueden representar éstos dos
conceptos en uno solo, llamado fuerza dual.
 
F̂  F   T (2.27)

Si la fuerza dual es aplicada en un punto “B”, diferente al punto de origen “A” de la

fuerza dual, el efecto que esta tendrá sobre el punto “B” estará determinada por
una trasformación de coordenadas. Por lo tanto una fuerza dual aplicada en un
sistema de referencia con origen en el punto “A” pero expresada en otro sistema
de coordenadas con origen en el punto “B” será:
 
B
FˆA  B FA   BTA (2.28)

2.7. Momento dual

El concepto de “Momento dual” se introduce como resultado de que la aceleración

de un cuerpo rígido es un pseudo-vector dual, es decir no puede ser establecida
como un vector en forma dual (Yang ,1966), por lo tanto el análisis dinámico
basado en aceleraciones no representa una alternativa para obtener las fuerzas
que actúan en un cuerpo, de tal forma que el estudio de la dinámica de cuerpos
rígidos por números duales se desarrolla a partir del momento dual de un cuerpo
rígido “A” (Fisher, 1999).
 
B
Hˆ A  
A
 B
Pp   B H p  (2.29)

10
 Capítulo 2

B
 B

Donde el vector Pp y H p representan el momento lineal y angular de una
partícula " p " del cuerpo “A” respectivamente, y ambos vectores se encuentran
expresados en términos del sistema de referencia “B”.
B
PA  mA  BVAB    B S AB   B AB  (2.30)

B
H A   B S AB   BVAB    B J AB   B AB  (2.31)

B
El término S AB es el primer momento de inercia de un cuerpo “A” respecto a un
punto “B” en términos de los vectores unitarios del sistema de referencia “B”.
B B
S Ax    x dm (2.32)
A

B B
S Ay    y dm (2.33)
A

B B
S Az    z dm (2.34)
A

O en forma matricial:

 0  B S Az
B B B
S Ay 
 B B 
S A   B S Az
B B
0  B S Ax  (2.35)
  B S Ay
B B B
S Ax 0 


Por lo tanto el término B J AB representa los segundos momentos de inercia.

B B
J Axx     y2   z2 dm (2.36)
A

B B
J Ayy     x2   z2 dm (2.37)
A

B B
J Azz     x2   y2 dm (2.38)
A

B B
J Axy    x  y dm (2.39)
A

B B
J Ayz    y  z dm (2.40)
A

B B
J Axz    x  z dm (2.41)
A

11
 Capítulo 2

Las ecuaciones (2.36) - (2.41) pueden ser combinadas en una sola matriz.

 B J Axx
B
 B J Axy
B
 B J Axz
B

 B 
J A    B J Axy
B B B B B
J Ayy  B J Ayz  (2.42)
  B J Axz
B
 B J Azy
B B B 
J Azz 


2.8. Fuerza dual de inercia

La fuerza dual de inercia de un cuerpo rígido “A” en movimiento y con respecto a

un punto “B”, es la derivada de su momento dual.

B
fˆA 
d
dt
 B
Hˆ A  (2.43)

 B Hˆ  
 Ai   0  BVˆAkB VˆAjB   B Hˆ Ai 
B

  
f A   B Hˆ Aj    BVˆAkB
B ˆ
0  BVˆAiB   B Hˆ Aj  (2.44)
    
 B Hˆ    BVˆAjB B ˆB
V 0   B Hˆ Ak 
 Ak  
Ai

Así cada una de las componentes de la fuerza dual de inercia a lo largo de los ejes
x , y y z serán:


 B fˆAi   B Hˆ Ai  BVˆAkB B Hˆ Aj  BVˆAjB B Hˆ Ak 
 B  B  
ˆ  ˆ B ˆB B ˆ
 f Aj   H Aj  VAi H Ak  VAk H Ai B ˆB B ˆ 
(2.45)
B ˆ   B  
 f Ak   Hˆ Ak  VˆAj Hˆ Ai  VˆAi Hˆ Aj 
 B B B B B B
 

2.9. Equilibrio dinámico de un eslabón

El principio de D’Alembert puede ser aplicado de tal forma que las fuerzas
aplicadas y el momento dual formen un conjunto de ecuaciones que describan el
estado de equilibrio en un cuerpo rígido, puesto que la combinación de las fuerzas
duales de inercia así como las fuerzas duales aplicadas al cuerpo producen un
equilibrio dinámico.
B
fˆA  B Mˆ A A FˆA  B FˆB (2.46)

12
Capítulo 3. Solución de las ecuaciones
cinemáticas para generar una trayectoria de
trabajo definida.

3
3.1. Cinemática directa del MIRH aplicando algebra dual

La cinemática directa trata el problema de la posición y orientación del efector final

dados cada uno de los valores de las variables de la articulación 1 , 2 ... n .

X 1 , 2 ... n 

1 , 2 ... n Cinemática Directa Y 1 , 2 ... n 

Z 1 , 2 ... n 

Figura 3.1. Esquema del algoritmo de transformación del espacio articulado al

espacio cartesiano.

Las rotaciones y translaciones de un cuerpo rígido son representadas a través de

la denominada ecuación articulación – eslabón generalizada para articulaciones
prismáticas, cilíndricas o de revolución (Fisher, 1999).

cos ˆn  cos ˆ n sin ˆn sin ˆ n sin ˆn 

 
M n 1   sin ˆn
n ˆ
cos ˆ cos ˆ
n n  sin ˆ n cos ˆn  (3.1)
 0 sin ˆ n cos ˆ n 
 

En donde ˆn y ˆ n son los ángulos duales representados en las ecuaciones (2.14)
y (2.15), que definen la geometría de la articulación y del eslabón de un
mecanismo, es posible observar que los elementos que forman a ˆn y ˆ n , son los
parámetros de Denavit-Hartenberg, los cuales se designan a partir de la siguiente
convención:

:
 Capítulo 3

 n : Ángulo de la articulación entre el eje xn1 y xn con respecto al eje zn1 .

Sn : Distancia de separación entre los orígenes del sistema n y el sistema n  1 a

lo largo del eje zn1 .

 n : Ángulo de separación del eje zn1 al eje z n respecto del eje xn .

an : Distancia de separación entre los orígenes del sistema n y el sistema n  1 a

lo largo del eje xn 1 .

Para el caso de estudio del MIRH, los parámetros de Denavit-Hartenberg se

obtienen a partir de la designación de los sistemas de coordenadas (Fu, 1988),
mostrados en la Figura 3.2 y en la Tabla 3.1 se resumen los parámetros de
Denavit-Hartenberg, obtenidos a partir de la configuración y asignación de ejes
mostrada en la Figura 3.2.

Figura 3.2. Descripción geométrica del Manipulador Industrial Robótico Hidráulico

(MIRH)

Aplicando las ecuaciones (2.14) y (2.15) para cada uno de los valores de la Tabla
3.1, de forma que para cada eslabón y articulación se establecen los ángulos
duales característicos del MIHR.

14
 Capítulo 3

Articulación n Sn n an
1 1 + 90º 0 mm 90º 0 mm
2 2 0 mm 0º 450 mm
3 3 0 mm 0º 450 mm
4 4 - 90º 0 mm -90º 0 mm
5 5 680 mm 0º 0 mm
Tabla 3.1. Parámetros de Denavit-Hartenberg para el MIRH

ˆ1  1  90º  0 (3.2)

ˆ2   2   0 (3.3)

ˆ3  3   0 (3.4)

ˆ4   4  90   0 (3.5)

ˆ5  5   680 (3.6)

ˆ1  90º  0 (3.7)

ˆ 2  0º  450 (3.8)

ˆ3  0º  450 (3.9)

ˆ 4  90º  0 (3.10)

ˆ5  0   0 (3.11)

Sustituyendo estos ángulos duales en la ecuación (3.1), se obtienen las 5 matrices

duales que relacionan los movimientos de rotación y traslación (en este caso
solamente rotación) de dos eslabones adyacentes, cabe señalar que se descarta
en las articulaciones 1 y 4 el ángulo de 90º sumado y restado respectivamente a
cada articulación, con el propósito de reducir las ecuaciones de diseño, sin
embargo posteriormente debe de compensarse dicho valor.

cos 1 0 sin 1 
0
Mˆ 1   sin 1 0 cos 1  (3.12)
 0 1 0 

15
 Capítulo 3

cos  2  sin  2 450 sin  2 

1
Mˆ 2   sin  2 cos  2 450 cos  2  (3.13)
 0 450 1 

cos 3  sin 3 450 sin 3 

M 3   sin 3
2 ˆ
cos 3 450 cos 3  (3.14)
 0 450 1 

cos  4 0 sin  4 
M 4   sin  4
3 ˆ
0 cos  4  (3.15)
 0 1 0 

 cos 5  680 sin 5  sin 5  680 cos 5 0

M 5  sin 5  680 cos 5
4 ˆ
cos 5  680 sin 5 0  (3.16)
 0 0 1 

Para relacionar la orientación y posición del efector final (sistema inercial 5) en

términos de la base (sistema inercial 0), se utiliza la condición de lazo cerrado
(Funda, 1988).
0
Mˆ 1 1Mˆ 2 2 Mˆ 3 3Mˆ 4 4 Mˆ 5 5 Mˆ 0  Iˆ (3.17)

0
Mˆ 1 1Mˆ 2 2 Mˆ 3 3Mˆ 4 4 Mˆ 5  0 Mˆ 5 (3.18)

La matriz dual 0 M̂ 5 contiene en la parte primaria la orientación del efector final, y

la parte dual establece la relación entre la orientación y la posición del efector final
(Funda, 1988).
0
M̂ 5  0T5   0 D5 (3.19)

Donde:
0
T5 : Matriz primaria

0
D5 : Matriz con la parte dual de 0 M̂ 5
0
D5  P 0T5 (3.20)

16
 Capítulo 3

El vector de posición del efector final es calculado a partir del teorema de Cayley
(Ángeles, 2002), ya que P es una matriz cuadrada simétrica.

 0  pz py 
 
P   pz 0  px  (3.21)
 py px 0 


Por lo tanto la matriz establecida en la ecuación (3.21) puede ser transformada en

el vector de posición del efector final.

 px 
  
p   py  (3.22)
 p z 

3.2. Cinemática inversa por números duales

Un manipulador generalmente es programado para que el efector final tenga una

posición y orientación definida. Dada esta posición y orientación, el manipulador
debe ser capaz de configurar su geometría y satisfacer dichas condiciones.

Figura 3.2. Condiciones iniciales para resolver la cinemática inversa

17
 Capítulo 3

De la ecuación (3.20) se puede establecer el sistema de ecuaciones a resolver, ya

que esta matriz contiene toda la información requerida para la configuración
geométrica a satisfacer. Por lo tanto es establecido el siguiente sistema:

 0  pz p y   nx ox ax 
 
F 1 , 2 ,3 , 4 ,5    pz 0  px   n y oy a y   0 D5 (3.23)
 py px 0   nz oz az 


En donde F 1 , 2 ,3 , 4 ,5  es un sistema sobre-determinado no lineal de 9

ecuaciones. Para resolverlo se recurre al método de Newton-Raphson.

1  1   F 1 

     
 2  2  . 
3   3    J   . 

(3.24)
     
 4   4   . 
5  5   F  9
C  

Donde el término J  es la matriz pseudoinversa de Moore Pen Rose de la matriz

Jacobiana del sistema de ecuaciones.
1
  F (1) F (1) 
T
 F (1) F (1)    F (1) F (1) 
T

 ... ...    ...

  1 5    5   5 
  1
  1

 .   .   . 
     
J 

  .   .   . 
 .   .   . 
     
  F (9) F (9)   F (9) F (9)   F (9) ... F (9) 
   ...    1
...
5  
 1 5    1 5 
(3.25)

3.3. Análisis de velocidades por números duales

Dentro del problema de la cinemática es necesario realizar un estudio de las

velocidades del efector final. Particularmente en la cinemática directa se aborda el
efecto que tiene la velocidad angular de cada una de las articulaciones del
manipulador, sobre el efector final. Es decir dadas 1 , 2 ...n se obtendrán la
velocidad lineal y angular del efector final vx , vy , vz , x , y , z , en términos del
sistema fijo a la base del manipulador.

18
 Capítulo 3

Partiendo de las matrices duales (3.12), (3.13), (3.14), (3.15), (3.16), las
velocidades duales de cada una de las articulaciones son conocidas, por lo que se
procede a obtener las velocidades relativas duales por medio de las ecuaciones
(2.24), (2.25) y (2.26).

0 0 0 0 0

0
Vˆ   0 
0
1,0 Vˆ   0 
1 1
2,1
2
Vˆ   0 
2
3,2
3
Vˆ   0 
3
4,3 Vˆ   0 
4 4
5,4

1  2  3  4  5 


V̂1,00  1
0
Velocidad dual de la articulación 1 en términos del sistema 0

Vˆ1,0
1 1
 1Mˆ 0 0Vˆ1,00 Velocidad dual del final del eslabón 1 en términos del sistema 1

Vˆ2,0
1 1
 1Mˆ 0 0Vˆ1,00  1Vˆ2,1
1
Velocidad dual de la articulación 2 en términos del sistema 1

Vˆ2,0
2 2
 2 Mˆ 1 1Vˆ2,0
1
Velocidad dual del final del eslabón 2 en términos del sistema 2

Vˆ3,0
2 2
 2Mˆ 1 1Vˆ2,0
1
 2Vˆ3,2
2
Velocidad dual de la articulación 3 en términos del sistema 2

Vˆ3,0
3 3
 3Mˆ 2 2Vˆ3,0
2
Velocidad dual del final del eslabón 3 en términos del sistema 3

Vˆ4,0
3 3
 3Mˆ 2 2Vˆ3,0
2
 3Vˆ4,3
3
Velocidad dual de la articulación 4 en términos del sistema 3

Vˆ4,0
4 4
 4 Mˆ 3 3Vˆ4,0
3
Velocidad dual del final del eslabón 4 en términos del sistema 4

Vˆ5,0
4 4
 4Mˆ 3 3Vˆ4,0
3
 4Vˆ5,4
4
Velocidad dual de la articulación 5 en términos del sistema 4

Vˆ5,0
5 5
 5Mˆ 4 4Vˆ5,0
4
Velocidad dual del final de eslabón 5 en términos del sistema 5

Sustituyendo cada uno de los valores, la velocidad relativa dual del efector final
con respecto a la base y en términos del sistema inercial fijo al efector final es:
i
i
Vˆi ,0i   i Mˆ j 1 j 1Vˆj j, j 11 (3.26)
j 1

19
 Capítulo 3

Sin embargo se busca que la velocidad dual del efector final sea expresada en
términos del sistema fijo a la base, por lo tanto la ecuación (3.26) será necesario
transformarla a las coordenadas del sistema inercial 0:
i
0
Vˆi ,0i  0Ti  i Mˆ j 1 j 1Vˆj j, j 11 (3.27)
j 1

Donde 0Ti es la parte primaria de la matriz 0 Mˆ i  0Ti   0 Di .

Desarrollando para el MIRH:

0 0 0 0 0

0
Vˆ  T5 Mˆ 0  0   T5 Mˆ 1  0   T5 Mˆ 2  0   T5 Mˆ 3  0   T5 Mˆ 4  0 
5
5,0
0 
5  0 5   0 5   0 5   0 5

1  2  3  4  5 

(3.28)

La ecuación (3.28) para efectos de programación se puede establecer como un

sistema matricial en bloques.

 0Vˆ1,00 
 
 1Vˆ2,0
1 

 
Vˆ5,0
0 5
  0T5   5 Mˆ 0 5
Mˆ 1 5
Mˆ 2 5
Mˆ 3 5 ˆ  2 ˆ2 
M 4  V3,0 (3.29)
 
 3Vˆ 3 
 4,0 
 4Vˆ 4 
 5,0 

En donde la velocidad dual 0V̂5,0

5
expresa la velocidad angular y lineal del efector
final en términos de la base.

 x  Vx 
0
Vˆ   y  Vy 
5
5,0 (3.30)
  z  Vz 

Cabe señalar que de la ecuación (3.29), la matriz en bloques formada por las
matrices de rotación duales, es la matriz Jacobiana dual generalizada
característica del manipulador (Brodsky & Shoham, 1998).

20
 Capítulo 3

Yˆ   5Mˆ 0 5
Mˆ 1 5
Mˆ 2 5
Mˆ 3 5
Mˆ 4  (3.31)

3.4. Análisis inverso de velocidades por números duales

Debido a que el manipulador en estudio sólo contiene pares de revolución, y

multiplicando la matriz de orientación por la matriz Jacobiana dual generalizada
( 0T5 y Yˆ ) , la ecuación (3.29) se puede reducir al siguiente sistema de ecuaciones.

x  0.51 sin 2  3  4  5   sin 2  3   4  5   2  3  4  sin 5 (3.32)

 y  0.51 cos 2  3  4  5   cos 2  3  4  5   2  3  4  cos5 (3.33)

z  1 cos  2  3   4   5 (3.34)

Vx  2251 sin  2  5   sin  2  5   sin  2  3  5   sin  2  3  5  

 3401 cos  2  3   4  5   cos  2  3   4  5   6802 cos 5 
(3.35)
 2252 sin 3   4  5   sin 3   4  5   sin  4  5   sin  4  5  
 2253 sin  4  5   sin  4  5   6803 cos5  6804 cos 5

Vy  2251 cos  2  5   cos  2  5   cos  2  3  5   cos  2  3  5  

 3401 sin  2  3   4  5   sin  2  3   4  5   6802 sin 5 
(3.36)
 2252 cos 3   4  5   cos 3   4  5   cos  4  5   cos  4  5  
 2253 cos  4  5   cos  4  5   6803 sin 5  6804 sin 5

Vz  4502 cos 3   4   4502 cos 4  4503 cos 4 (3.37)

Este sistema de ecuaciones es expresado en forma matricial por:

 x 
   1 
  y  
 z   2
   W  3  (3.38)
 Vx   
Vy  4 
  5 
 Vz 

21
 Capítulo 3

Donde W es una matriz rectangular de 6x5 y cuyos elementos son:

W1,1  0

W12  sin 1

W13  sin 1

W14  sin 1

W15  12 sin 1   2  3   4   12 sin 1   2  3   4 

W21  0

W22   cos1

W23   cos1

W24   cos1

W25  12 cos 1   2  3   4   12 cos 1   2  3   4 

W31  1

W32  0

W33  0

W34  0

W35  cos  2  3   4 

W41  340cos 1   2  3   4   340cos 1   2  3   4   225sin 1   2  3  

 225sin 1   2  3   225sin 1   2   225sin 1   2 

W42  340cos 1   2  3   4   340cos 1   2  3   4   225sin 1   2  3  

 225sin 1   2  3   225sin 1   2   225sin 1   2 

W43  340cos 1   2  3   4   340cos 1   2  3   4   225sin 1   2  3  

 225sin 1   2  3 

22
 Capítulo 3

W44  340cos 1   2  3   4   340cos 1   2  3   4 

W45  0

W51  340sin 1   2  3   4   340sin 1   2  3   4   225cos 1   2  3  

 225sin 1   2  3   225sin 1   2   225cos 1   2 

W52  340sin 1   2  3   4   340sin 1   2  3   4   225cos 1   2  3  

 225cos 1   2  3   225cos 1   2   225cos 1   2 

W53  340sin 1   2  3   4   340sin 1   2  3   4   225cos 1   2  3  

 225cos 1   2  3 

W54  340sin 1   2  3   4   340sin 1   2  3   4 

W55  0

W61  0

W62  680sin  2  3   4   450cos  2  3   450cos  2 

W63  680sin  2  3   4   450  2  3 

W64  680sin  2  3   4 

W65  0

Para el caso de un manipulador de 5 grados de libertad, la matriz W no es

cuadrada, por lo que para obtener los valores de la velocidad dual en el espacio
articulado, se recurre nuevamente a la matriz pseudoinversa de Moore-Pen Rose
(Pennestrí & Stefanelli ,2007).

 A   AT A  AT
1
m  n 

 A  AT  AAT 
1
m  n 

Por lo que tomando en cuenta estas dos últimas consideraciones, las velocidades
angulares de cada articulación a partir de la ecuación (3.38) serán:

23
 Capítulo 3

 x 
 1   
   y
 2   
3   W   z  (3.39)
   Vx 

  4
Vy 
5   
 Vz 

3.5. Análisis de Aceleraciones

Las aceleraciones lineal y angular del efector final, se obtendrán a partir de la

aceleración angular de cada una de las articulaciones. Por lo tanto derivando con
respecto al tiempo la ecuación (3.29) y expresada en términos del sistema de
referencia 0 se obtiene:

 0Vˆ1,00 
 
 1Vˆ2,0
1 


d 5Vˆ5,0
5
 
d 0 ˆ 2 ˆ2 
 
  T5Y  V3,0

(3.40)
dt dt  
 3Vˆ 3 
 4,0 
 4Vˆ 4 
 5,0 

Tanto la matriz Jacobiana generalizada como la matriz en bloques de velocidades

duales de las articulaciones son funciones de  , por que la ecuación (3.40) será:

 0Vˆ1,00   0Vˆ1,00 
   
 1Vˆ2,0
1   1Vˆ2,0
1 
 x   vx  0 ˆ    
   v   d  T5Y   2Vˆ 2    0T Yˆ  d  2Vˆ 2  (3.41)
 y y
dt  3,0   5  dt  3,0 
  z   vz   3Vˆ 3   3Vˆ 3 
 4,0   4,0 
 4Vˆ 4   4Vˆ 4 
 5,0   5,0 

Sin embargo esta última ecuación resulta complicada de invertir y programar, por
lo que se recurre a la ecuación (3.38), la cual derivando con respecto al tiempo
proporciona las aceleraciones del efector final respecto al sistema inercial base.

24
 Capítulo 3

 x 
   1   1 
  y  2   
 z  d W    2
   3   W   3  (3.42)
 Vx  dt    
 4   4 
Vy 

  5   5 
 Vz 

3.6. Análisis inverso de aceleraciones

El análisis inverso de aceleraciones consiste en encontrar las aceleraciones

angulares de cada una de las articulaciones del manipulador a partir de las
aceleraciones lineal y angular del efector final.

Despejando de la ecuación (3.42) la matriz que contiene las aceleraciones

angulares, se establece:

 x 
 1     1 
    y  
 2   z   d
 2
 3   W     W  W  3  (3.43)
  V dt  
  x
  4
Vy  4 
 5    5 
 Vz 

3.7. Cinemática directa del MIRH por matrices de transformación

homogénea.

Denavit y Hartenberg en 1955 propusieron una convención, para relacionar la

orientación y posición del efector final de un manipulador hacia la base del mismo.
También proponen una matriz de transformación homogénea, que relaciona cada
uno de los eslabones del manipulador con su eslabón adyacente inmediato.

cos  n  cos  n sin  n sin  n an cos  n 

 sin  cos  n cos  n  sin  n cos  n an sin  n 
n 1
Mn   n
(3.44)
 0 sin  n cos  n dn 
 
 0 0 0 1 

A partir de los datos de la Tabla 3.1 se obtienen las siguientes matrices de

transformación homogénea:

25
 Capítulo 3

cos1 0 sin 1 0
 sin  0  cos 1 0 
0
M1   1
(3.45)
 0 1 0 0
 
 0 0 0 1

cos  2  sin  2 0 450cos  2 

 sin  cos  2 0 450sin  2 
1
M2   2
(3.46)
 0 0 1 0 
 
 0 0 0 1 

cos3  sin 3 0 450cos3 

 sin  cos3 0 450sin 3 
2
M3   3
(3.47)
 0 0 1 0 
 
 0 0 0 1 

cos  4 0  sin  4 0
 sin  0 cos  4 0 
3
M4   4
(3.48)
 0 1 0 0
 
 0 0 0 1

cos 5  sin 5 0 0 
 sin  cos5 0 0 
4
M5   5
(3.49)
 0 0 1 680 
 
 0 0 0 1 

Para relacionar el marco de referencia ubicado en el efector final a la base del

manipulador, se utiliza la matriz de transformación homogénea que establece la
relación del sistema inercial 5 al sistema inercial 0.
0
T5  0 M1 1M 2 2 M 3 3M 4 4 M 5 (3.50)

La ecuación (3.50) (matriz de transformación homogénea) proporciona los

vectores: normal, de orientación, de aproximación y de posición del efector final
con respecto al sistema de referencia base.

26
 Capítulo 3

 nx ox ax px 
n oy ay p y 
0
T5   y (3.51)
 nz oz az pz 
 
0 0 0 1

De la ecuación (3.51) para el MIRH y para la configuración mostrada en la figura

3.1, se obtienen las siguientes ecuaciones de diseño en función de 1 ,  2 , 3 ,  4
y 5 .

nx  0.25cos 1   2  3   4  5   0.25cos 1   2   3   4   5 

 0.25cos 1   2  3   4  5   0.25cos 1   2   3   4   5  (3.52)
 0.50cos 1  5   0.50cos 1  5 

n y  0.25sin 1   2  3   4  5   0.25sin 1   2   3   4   5 

 0.25sin 1   2  3   4  5   0.25sin 1   2  3   4   5  (3.53)
 0.50sin 1   2  3   4  5   0.50sin 1  5 

nz  0.50sin  2  3   4  5   0.50sin  2  3   4  5  (3.54)

ox  0.25sin 1   2  3   4  5   0.25sin 1   2   3   4   5 

 0.25sin 1   2  3   4   5   0.25sin 1   2   3   4   5  (3.55)
 0.50sin 1  5   0.5sin 1  5 

o y  0.25cos 1   2  3   4  5   0.25cos 1   2   3   4   5 

 0.25cos 1   2  3   4   5   0.25cos 1   2   3   4   5  (3.56)
 0.50cos 1  5   0.50cos 1  5 

oz  0.50cos  2  3   4  5   0.50cos  2  3   4  5  (3.57)

ax  0.50sin 1   2  3   4   0.50sin 1   2  3   4  (3.58)

a y  0.50cos 1   2  3   4   0.50cos 1   2  3   4  (3.59)

az  cos  2  3   4  (3.60)

27
 Capítulo 3

px  340sin 1   2  3   4   225cos 1   2  3   225cos 1   2 

(3.61)
 340sin 1   2  3   4   225cos 1   2  3   225cos 1   2 

p y  340cos 1   2  3   4   225sin 1   2  3   225sin 1   2 

(3.62)
 340cos 1   2  3   4   225sin 1   2  3   225sin 1   2 

pz  680cos  2  3   4   450sin  2  3   450sin  2  (3.63)

3.8. Solución de la cinemática inversa

Como se mencionó anteriormente se requieren calcular el valor de cada una de

las articulaciones dados los vectores de orientación, aproximación, orientación y
posición del efector final.

 nx ox ax px   nx 1 , 2 ... n  ox 1 , 2 ... n  ax 1 , 2 ... n  px 1 , 2 ... n  

n  
 y oy ay p y   ny 1 , 2 ... n  oy 1 , 2 ... n  a y 1 , 2 ... n  p y 1 , 2 ... n  

 nz oz az pz   nz 1 , 2 ... n  oz 1 , 2 ... n  az 1 , 2 ... n  pz 1 , 2 ... n  
   
0 0 0 1  0 0 0 1 
(3.64)

De la expresión (3.64) se establece un sistema de 12 ecuaciones con 5 incógnitas

(sistema sobre-determinado no lineal), que numéricamente puede ser resuelto por
el método iterativo de Newton-Raphson.

1  1   F 1 

     
 2  2  . 
3   3    J   . 

(3.65)
     
 4   4   . 
5  5   F 12  
C  

Donde:
1
  F (1) F (1) 
T
 F (1) F (1)    F (1) F (1) 
T

 ... ...    ...

  1 5    5   5 
  1
  1

J    
 (3.66)
      
     
  F (12) F (12)   F (12) ... F (12)    F (12) ... F (12) 
   ...
5   1 5    1 5 
 1


28
 Capítulo 3

3.9. Análisis de Velocidades

La relación entre las velocidades angulares de cada una de las articulaciones, la

velocidad lineal y angular del efector final, está determinada por la matriz
Jacobiana del manipulador (Spong & Vidyasagar, 1989).

La matriz Jacobiana (Niku, 2001) es una representación de la geometría de los

elementos de un mecanismo en el tiempo. Está matriz permite la conversión de
movimientos diferenciales o velocidades de cada una de las articulaciones en
movimientos diferenciales o velocidades de algún punto en especifico. También
relaciona los movimientos diferenciales de cada una de las articulaciones con los
movimientos totales del mecanismo.

 vx 
v 
 y  1 
 vz   
    J n    
0
(3.67)
x  n 
 y 
 
z 

Para el manipulador MIRH la matriz Jacobiana será calculada a partir de la

siguiente ecuación (Sponk, 1989):
              
 z0   p5  p0  z1   p5  p1  z2   p5  p2  z3   p5  p3  z4   p5  p4  
0
Jn        
 z0 z1 z2 z3 z4 

(3.68)
  
De la expresión (3.68) los vectores z j 1 , p j 1 y pn , son extraídos de las matrices
de transformación homogénea 0 M n , n  1  5 (Ecuaciones (3.45), (3.46), (3.47),
(3.48), (3.49)).

0 
  
z0   0  (3.69)
1 

 sin 1 
 
z1    cos 1  (3.70)
 0 

29
 Capítulo 3

 sin 1 
 
z2    cos 1  (3.71)
 0 

 sin 1 
 
z3    cos 1  (3.72)
 0 

 12 sin 1   2  3   4   12 sin 1   2  3   4  

  
z4   12 cos 1   2  3   4   12 cos 1   2  3   4   (3.73)
 cos  2  3   4  

 12 sin 1   2  3   4   12 sin 1   2  3   4  

  
z5   12 cos 1   2  3   4   12 cos 1   2  3   4   (3.74)
 cos  2  3   4  

0
  
p0  0  (3.75)
0 

0 
  
p1  0 (3.76)
 
 0 

 450 cos 1 cos  2 

p2   450 sin 1 cos 2 

(3.77)
 
 450 sin  2 

 225cos 1   2  3   225cos 1   2  3   225cos 1   2   225cos 1   2  

  
p3   225sin 1   2  3   225sin 1   2  3   225sin 1   2   225sin 1   2  
 450sin  2  3   450sin  2 

(3.78)

30
 Capítulo 3

 225cos 1   2  3   225cos 1   2   3   225cos 1   2   225cos 1   2  

  
p4   225sin 1   2  3   225sin 1   2  3   225sin 1   2   225sin 1   2  
 450sin  2  3   450sin  2 
(3.79)

 340sin 1   2  3   4   340sin 1   2  3   4   225cos 1   2  3   

 
 225cos 1   2  3   225cos 1   2   225cos 1   2  
 
 340cos 1   2  3   4   340cos 1   2  3   4   225sin 1   2  3   
p5 
 225sin        225sin      225sin     
 1 2 3 1 2 1 2

 
 680cos  2  3   4   450sin  2  3   450sin  2  
 

(3.80)

Sustituyendo las ecuaciones (3.69)-(3.80) en la expresión (3.68) la matriz

Jacobiana será:

 J11 J12 J13 J14 J15 

J J 22 J 23 J 24 J 25 
 21
J J 32 J 33 J 34 J 35 
J   31  (3.81)
 J 41 J 42 J 43 J 44 J 45 
 J 51 J 52 J 53 J 54 J 55 
 
 J 61 J 62 J 63 J 64 J 65 

J11  340cos 1   2  3   4   340cos 1   2  3   4   225sin 1   2  3  

 225sin 1   2  3   225sin 1   2   225sin 1   2 

J12  340cos 1   2  3   4   340cos 1   2  3   4   225sin 1   2  3  

 225sin 1   2  3   225sin 1   2   225sin 1   2 

J13  340cos 1   2  3   4   340cos 1   2  3   4   225sin 1   2  3  

 225sin 1   2  3 

J14  340cos 1   2  3   4   340cos 1   2  3   4 

31
 Capítulo 3

J15  0

J 21  340sin 1   2  3   4   340sin 1   2  3   4   225cos 1   2  3  

 225sin 1   2  3   225sin 1   2   225cos 1   2 

J 22  340sin 1   2  3   4   340sin 1   2  3   4   225cos 1   2  3  

 225cos 1   2  3   225cos 1   2   225cos 1   2 

J 23  340sin 1   2  3   4   340sin 1   2  3   4   225cos 1   2  3  

 225cos 1   2  3 

J 24  340sin 1   2  3   4   340sin 1   2  3   4 

J 25  0

J 31  0

J 32  680sin  2  3   4   450cos  2  3   450cos  2 

J 33  680sin  2  3   4   450  2  3 

J 34  680sin  2  3   4 

J 35  0

J 41  0

J 42  sin 1

J 43  sin 1

J 44  sin 1

J 45  12 sin 1   2  3   4   12 sin 1   2  3   4 

J 51  0

J 52   cos 1

J 53   cos 1

32
 Capítulo 3

J 54   cos 1

J 55  12 cos 1   2  3   4   12 cos 1   2  3   4 

J 61  1

J 62  0

J 63  0

J 64  0

J 65  cos  2  3   4 

Si se proponen las velocidades angulares de cada una de las articulaciones del

manipulador en la ecuación (3.67), se obtendrán las velocidades angular y lineal
del efector final.

3.10. Análisis inverso de velocidades por algebra matricial

En los manipuladores industriales como se ha mencionado anteriormente y por lo

general, las velocidades del efector final son parte de las condiciones iniciales de
la trayectoria deseada. Por consiguiente se procede a calcular cada una de las
velocidades angulares de las articulaciones.

 vx 
v 
 1   y
     0 J  1  vz 
   n   
(3.82)
n   x
 y 
 
z 

La ecuación (3.82) para el caso del MIRH será:

 vx 
 1  v 
   y
 2  v 
3    0 J 5   z  (3.83)
 
  x 
  4
 y 
5   
 z 

33
 Capítulo 3

3.11. Análisis inverso de aceleraciones por algebra matricial.

Al igual que en el análisis de velocidades es necesario encontrar las aceleraciones

en términos del espacio articulado por lo que si (3.67) es derivada con respecto al
tiempo y se despeja el vector de aceleraciones angulares, se obtiene:

 ax 
1  a   1 
   y  
 2  a   d  2
 3    0 J 5   z    0 J 5   0 J 5  3  (3.84)
 
  x  dt  
  4
 y  4 
 5    5 
 z 

Se observa que (3.84) es análoga a (3.43) por lo tanto las aceleraciones angulares
para ambos métodos serán iguales.

34
Capítulo 4 Solución de las ecuaciones
dinámicas del manipulador.

4
El análisis de fuerzas dinámicas, proporciona la relación que existe entre las
fuerzas que actúan sobre un cuerpo, y el movimiento que provocan en el mismo.

En el análisis de fuerzas dinámicas de manipuladores robóticos, se determinan los

valores de torque o de fuerza (dependiendo del tipo de articulación), requeridos
por los actuadores en cada una de las articulaciones para alcanzar una posición ó
realizar una trayectoria de trabajo definida.

4.1. Análisis de fuerzas dinámicas por números duales

En (Fisher, 1999) se propone una metodología sistemática, para obtener las

fuerzas o torques de los actuadores en un manipulador, empleando la ecuación
dual de Euler y el principio de D’Alembert.

La metodología propuesta por Fisher, está basada en el siguiente algoritmo:

 Obtener la velocidad dual PVˆi ,Pj de cada eslabón.

 Establecer las matrices de inercia de cada eslabón en movimiento.
 Calcular el momento dual de cada eslabón.
 Derivar el momento dual.
 Con los valores obtenidos deducir las fuerzas duales de inercia.
 Aplicar el principio de D’Alembert.
 Definir el sistema de ecuaciones a resolver.

4.1.1. Velocidades PVˆi ,Pj de cada eslabón

Las velocidades duales PVˆi ,Pj en cada articulación, son obtenidas de acuerdo a
(3.26) como:
 Capítulo 4

0
1
Vˆ  Mˆ 0 Vˆ  1 
1
1,0
1 0 0
1,0 (4.1)
 0 

 1 sin  2   0 
Vˆ2,0
2 2
 2 Mˆ 1 1Vˆ2,0
1   
 1 cos 2     0.452  (4.2)

 2   0.452 cos 2 

 1 sin( 2  3 )   0.452 sin 3 

3 ˆ3 3 ˆ 2 ˆ2   
V3,0  M 2 V3,0  1 cos( 2  3 )     0.452 (cos3  1)  0.453  (4.3)

 2  3   0.451 cos 2  0.451 cos( 2  3 ) 

1 sin( 2  3   4 )   0.452 sin(3   4 )  sin  4  3 sin  4  

 
Vˆ4,0
4 4
 4 Mˆ 3 3Vˆ4,0
3
  2  3  4     0.451 cos 2  cos( 2  3 ) 
 cos( 2  3   4 )  0.452 cos( 3   4 )  cos 4  3 cos 4 

(4.4)
5 5
5
Vˆ5,0
5
 5Mˆ 4 4Vˆ5,0
4
 55,0   5V5,0 (4.5)

De la expresión (4.5) sus correspondientes componentes son:

x  0.51 sin( 2  3   4  5 )  sin( 2  3   4  5 )  2  3  4  sin 5

 y  0.51 cos( 2  3   4  5 )  cos( 2  3   4  5 )  2  3  4  cos5

z  1 cos( 2  3   4 )  5

Vx  0.2251 sin( 2  5 )  sin( 2  5 )  sin( 2  3  5 )  sin( 2  3  5 ) 

 0.2252 sin(3   4  5 )  sin(3   4  5 )  sin( 4  5 )  sin( 4  5 ) 
 0.2253 sin( 4  5 )  sin( 4  5 )  (2  3  4 )0.68cos 5 
 0.341 cos( 2  3   4  5 )  cos( 2  3   4  5 )

36
 Capítulo 4

Vy  0.2251 cos( 2  5 )  cos( 2  5 )  cos( 2   3   5 )  cos( 2   3   5 ) 

 0.2252 cos(3   4   5 )  cos( 3   4   5 )  cos( 4   5 )  cos( 4   5 ) 
 0.2253 cos( 4  5 )  cos( 4  5 )  (2  3  4 )0.68sin  5 
 0.341 sin( 2  3   4   5 )  sin( 2   3   4  5 )

Vz  0.452 cos(3   4 )  2 cos 4  3 cos 4 

4.1.2. Matrices de inercia de los eslabones del manipulador en estudio.

B
Las matrices de los primeros S A y segundos momentos B I A de inercia de los
eslabones, son tomados de la tesis del diseño mecánico del manipulador robótico
industrial hidráulico (R. Vázquez, 2005), los cuales se muestran a continuación.

 Matriz de inercia, masa y coordenadas del centro de masas del eslabón 1

(Figura 4.1), en términos del sistema de referencia 1.

 I xx  0.437272 I xy  0.055516 I xz  0.000024 

 
1
I1  kg  m 2    I yx  I xy I yy  0.7438689 I yz  0.0000012 
 I zx  I xz I zy  I yz I zz  1.04214 


m1  22.085kg ; cm1x  0.11136m; cm1y  0.03464m; cmz  0;

Figura 4.1. Centro de masas del eslabón 1 respecto al sistema de coordenadas 1.

37
 Capítulo 4

 Matriz de inercia, masa y coordenadas del centro de masas del eslabón 2

(Figura 4.2), en términos del sistema de referencia 2.

 I xx  0.065755 I xy  0.08367 I xz  0.000046 

 
2
I 2  kg  m 2    I yx  I xy I yy  0.823166 I yz  0.000091
 I zx  I xz I zy  I yz I zz  0.836873 

m2  10.94kg ; cm2 x  0.2325m; cm2 y  0.02757m; cm2 z  0m;

Figura 4.2. Centro de masas del eslabón 2 respecto al sistema de coordenadas 2.

 Matriz de inercia, masa y coordenadas del centro de masas del eslabón 3

(Figura 4.3), en términos del sistema de referencia 3.

 I xx  0.03418 I xy  0.00045 I xz  0.0019451 

 
3
I 3  kg  m 2    I yx  I xy I yy  0.711573 I yz  0.0000003
 I zx  I xz I zy  I yz I zz  0.69911249 

m3  8.05kg ; cm3x  0.242m; cm3 y  0.0008m; cmz  0.0005m;

Figura 4.3. Centro de masas del eslabón 3 respecto al sistema de coordenadas 3.

38
 Capítulo 4

 Matriz de inercia, masa y coordenadas del centro de masas del eslabón 4

(Figura 4.4), en términos del sistema de referencia 4.

 I xx  0.01765 I xy  0.000545 I xz  0.000209 

 
4
I 4  kg  m 2    I yx  I xy I yy  0.501474 I yz  0.0000002 
 I zx  I xz I zy  I yz I zz  0.494986 

m4  10.58kg ; cm4 x  0.15446m; cm4 y  0.00013m; cm4 z  0.0015m;

Figura 4.4. Centro de masas del eslabón 4 respecto al sistema de coordenadas 4.

 Matriz de inercia, masa y coordenadas del centro de masas del eslabón 5

(Figura 4.5), en términos del sistema de referencia 5.

 I xx  1.228036 I xy  0.0000306 I xz  0.0023565

 
5
I 5  kg  m 2    I yx  I xy I yy  1.227358 I yz  0.015329 
 I zx  I xz I zy  I yz I zz  0.009164 

m5  4.98kg ; cm5x  0.00097m; cm5 y  0.00624m; cm5z  0.4923m;

Figura 4.5. Centro de masas del eslabón 5 respecto al sistema de coordenadas 5.

39
 Capítulo 4

4.1.3. Cálculo del Momento dual de cada eslabón.

El momento dual de cada uno de los eslabones del manipulador se obtendrán a

partir de la ecuación (2.29) en su forma extendida.
 
B 
 
Hˆ A  mA  BVAB    B S AB   B AB     B S AB   BVAB    B I A   B AB   (4.6)

1 1
1
Ĥ 0  m1  1V1,0
1
   1S1   11,0 
    1S1   1V1,0
1
   1I1   11,0 
 (4.7)

2 2
2
Ĥ1  m2  2V2,0
2
   2 S2   22,0 
    2 S2   2V2,0
2
   2 I 2   22,0   (4.8)

3 3
3
Ĥ 2  m3  3V3,0
3
   3S3   33,0 
    3S3   3V3,0
3
   3 I 3   33,0   (4.9)

4 4
4
Ĥ 3  m4  4V4,0
4
   4 S4   44,0 
    4 S4   4V4,0
4
   4 I 4   44,0   (4.10)

5 5
5
Ĥ 4  m5  5V5,0
5
   5 S5   55,0 
    5 S5   5V5,0
5
   5 I 5   55,0   (4.11)

Dado que la masa, el primer y segundo momento de inercia son constantes la

derivada del momento dual será:

 d 1 1   1   d 1  1    d 1 1   1   d 1  1  
1 
Hˆ 0  m1 
 dt 
 
V1,0    S1    1,0       1S1  
 dt   dt   dt

V1,0    I1    1,0  




(4.12)

 d 2 2   2   d 2 2    d 2 2   2   d 2 2 
2 
Hˆ 1  m2 
 dt 
 
V2,0    S 2    2,0       2 S 2  
 dt   dt   dt

V2,0    I 2    2,0  

 (4.13)


 d 3 3   3   d 3 3    d 3 3   3   d 3 3  
3 
Hˆ 2  m3 
 dt 
 
V3,0    S3    3,0       3S3  
 dt   dt   dt

V3,0    I 3    3,0   




(4.14)

 d 4 4   4   d 4 4    d 4 4   4   d 4 4 
4 
Hˆ 3  m4 
 dt 
 
V4,0    S4    4,0       4 S4  
 dt   dt   dt

V4,0    I 4    4,0  



(4.15)

40
 Capítulo 4

 d 5 5   5   d 5 5    d 5 5   5   d 5 5  
5 
Hˆ 4  m5 
 dt
 

V5,0    S5    5,0       5S5  
 dt   dt 
 
V5,0    I 5    5,0  
 dt 


(4.16)

4.1.4. Cálculo de las fuerzas duales de inercia

Las fuerzas de inercia se obtienen a partir de la ecuación (2.45), para los fines de
estudio en este trabajo, se recurre a la forma extendida de la ecuación de la fuerza
dual de inercia.

f x   t x  Pi  k Pj   j Pk    H i  k H j  Vk Pj   j H k  V j Pk  (4.17)

f y   t y  Pj  i Pk  k Pi    H j  i H k  Vi Pk  k H i  Vk Pi  (4.18)

f z   t z  Pk   j Pi  i Pj    H k   j H i  V j Pi  i H j  Vi Pj  (4.19)

Por lo que para cada articulación se calcularán las fuerzas duales de inercia a lo
largo y alrededor de cada uno de sus ejes de referencia.

 1 f 0i   1P0i  11,0 k P0 j  1,0 j P0 k

1 1 1 1 1

1   1  
 f 0 j    P0 j  1,0i P0 k  1,0 k P0i 
1 1 1 1 1 1
(4.20)
 1 f 0 k   1P0 k  11,0
1 1 
j P0 i  1,0 i P0 j 
1 1 1
  

 1t0i   1H 0i  11,0

1 1
k H 0 j  V1,0 k P0 j  1,0 j H 0 k  V1,0 j P0 k
1 1 1 1 1 1 1 1 1

1   1  
t
  0 j  H 0 j  
1 1 1
1,0 i H 0 k  1 1 1
V1,0 i P0 k  
1 1 1
1,0 k H 0 i  1 1 1
V1,0 k P0 i  (4.21)
 1t0 k   1H 0 k  11,0
1 1
H  1 1 1
V P  
1 1 1
H  1 1 1
V P 
   j 0i 1,0 j 0i 1,0 i 0j 1,0 i 0j 

 2 f1i   2 P1i  22,0

2
k P1 j  2,0 j P1k
2 2 2 2

2   2  
 f1 j    P1 j  2,0i P1k  2,0 k P1i 
2 2 2 2 2 2
(4.22)
 2 f1k   2 P1k  22,0
2 2 2 2 2 
j P1i  2,0 i P1 j 
  

 2t1i   2 H 1i  22,0
2
k H1 j  V2,0 k P1 j  2,0 j H1k  V2,0 j P1k
2 2 2 2 2 2 2 2 2 2

2   2  
 t1 j    H1 j  2,0i H1k  V2,0i P1k  2,0 k H1i  V2,0 k P1i 
2 2 2 2 2 2 2 2 2 2 2 2
(4.23)
 2t1k   2 H 1k  22,02 2 2 2 2 2 2 2 2 2 2 
j H1i  V2,0 j P1i  2,0 i H1 j  V2,0 i P1 j 
  

41
 Capítulo 4

 3 f 2i   3 P2i  33,0
3
k P2 j  3,0 j P2 k
3 3 3 3

3   3  
 f 2 j    P2 j  3,0i P2 k  3,0 k P2i 
3 3 3 3 3 3
(4.24)
 3 f 2 k   3 P2 k  33,0
3 3 3 3 3 
j P2 i  3,0 i P2 j 
  

 3t2i   3 H 2i  33,0
3
k H 2 j  V3,0 k P2 j  3,0 j H 2 k  V3,0 j P2 k
3 3 3 3 3 3 3 3 3 3

3   3  
 t2 j    H 2 j  3,0i H 2 k  V3,0i P2 k  3,0 k H 2i  V3,0 k P2i 
3 3 3 3 3 3 3 3 3 3 3 3
(4.25)
 3t2 k   3 H 2 k  33,0
3 3 3 3 3 3 3 3 3 3 3 
j H 2 i  V3,0 j P2 i  3,0 i H 2 j  V3,0 i P2 j 
  

 4 f3i   4 P3i  44,0

4
k P3 j  4,0 j P3 k
4 4 4 4

4   4  
 f3 j    P3 j  4,0i P3k  4,0 k P3i 
4 4 4 4 4 4
(4.26)
 4 f3k   4 P3k  44,0
4 4 4 4 4 
j P3i  4,0 i P3 j 
  

 4t3i   4 H 3i  44,0
4
k H 3 j  V4,0 k P3 j  4,0 j H 3 k  V4,0 j P3 k
4 4 4 4 4 4 4 4 4 4

4   4  
t
  
3 j  H 3 j  
4 4 4
4,0 i H 3 k  4 4 4
V4,0 i P3 k  
4 4
4,0 k
4
H 3 i  4 4 4
V4,0 k P3 i  (4.27)
 4t3k   4 H 3k  44,0 4 4
H  4 4 4
V P  
4 4 4
H  4 4 4
V P 
   j 3i 4,0 j 3i 4,0 i 3j 4,0 i 3j 

 5 f 4i   5 P4i  55,0
5
k P4 j  5,0 j P4 k
5 5 5 5

5   5  
 f 4 j    P4 j  5,0i P4 k  5,0 k P4i 
5 5 5 5 5 5
(4.28)
 5 f 4 k   5 P4 k  55,0
5 5 5 5 5 
j P4 i  5,0 i P4 j 
  

 5t4i   5 H 4i  55,0
5
k H 4 j  V5,0 k P4 j  5,0 j H 4 k  V5,0 j P4 k
5 5 5 5 5 5 5 5 5 5

5   5  
 t4 j    H 4 j  5,0i H 4 k  V5,0i P4 k  5,0 k H 4i  V5,0 k P4 i 
5 5 5 5 5 5 5 5 5 5 5 5
(4.29)
 5t4 k   5 H 4 k  55,0
5 5 5 5 5 5 5 5 5 5 5 
j H 4 i  V5,0 j P4 i  5,0 i H 4 j  V5,0 i P4 j 
  

Las ecuaciones (4.20) - (4.29) resultan prácticas para ser programadas en

lenguajes de programación tales como Basic ó C++, sin embargo y dado que en
este trabajo se programa en MATLAB® y dadas las características de este
programa basado en vectores y matrices, es conveniente usar la siguiente
expresión de la fuerza dual de inercia (Fisher,1999).


B
fˆA  B Hˆ A  BVAB,0  B Hˆ A (4.30)

4.1.5. Principio de D’Alembert

El principio de D’Alembert establece que un cuerpo en movimiento puede ser

considerado en equilibrio en un instante de tiempo, este concepto permite crear un

42
 Capítulo 4

sistema de ecuaciones con las fuerzas duales de inercia para cada uno de los
eslabones del manipulador.
B
Mˆ A A FˆA  B FˆB  B fˆA  0 (4.31)

La ecuación (4.31) puede ser descompuesta en su correspondiente parte primaria

y parte dual, produciendo un sistema de ecuaciones, que involucran los vectores
de fuerzas y torques tanto intrínsecos  fˆ  , así como extrínsecos 
B
A
A
FA , B FB  del
manipulador.
     
 B
M A   B DA  A

FA   ATA  B FB   BTB  B f A   Bt A  0 (4.32)

De la expresión (4.32) se definen las fuerzas de reacción en cada uno de los ejes
de las articulaciones del manipulador.
      
B
M A AFA  B FB  B f A    B
M A ATA  B DA AFA  BTB  Bt A  0  (4.33)

Por lo tanto descomponiendo en parte primaria y dual se obtiene un arreglo

matricial de ecuaciones.
  
B
M A A FA  B FB  B f A  0 (4.34)
   
B
M A ATA  B DA A FA  BTB  Bt A  0 (4.35)

Así las fuerzas y torques en cada una de las articulaciones serán:

  
 
1
1
F1   1M 0  2
F2  1 f 0 (4.36)

  
 
1
2
F2   2 M 1  3 F3  2 f1 (4.37)

  
F f
1
3
F3   3M 2  4
4
3
2 (4.38)

  
 
1
4
F4   4 M 4  5 F5  4 f3 (4.39)

 
 
1
5
F5   4 M 5  5
f4 (4.40)

   
 
1
1
T1   1M 0  2T2  1t0  1D0 1F1 (4.41)

43
 Capítulo 4

   
 
1
2
T2   2 M 1  3T3  2t1  2 D1 2 F2 (4.42)

   
 
1
T3   3M 2  4T4  3t2  3 D2 3 F3
3
(4.43)

   
 
1
T4   4 M 3  5T5  4t3  4 D3 4 F4
4
(4.44)

  
 
1
5
T5   5 M 4  5t4  5 D4 5 F5 (4.45)

4.2. Solución dinámica por el método de Lagrange – Euler.

El método de Lagrange – Euler es un método con el que se obtienen los torques

alrededor de los ejes de giro de los eslabones de un manipulador. Básicamente el
algoritmo de Lagrange – Euler se basa en el cálculo de la función Lagrangiana que
depende de la energía cinética y potencial de un sistema (Fu,1990).

d  L  L
   i (4.46)
dt  qi  qi

La ecuación (4.46) en variables de estado será:

  t   D(q)q  h(q, q )  c(q) (4.47)

D(q)  Matriz simétrica de inercia.

Tr U jk J jU Tji 
n
Dik  
j  max( i ,k )
i, k  1, 2...n (4.48)

q  Vector de aceleración de cada una de las articulaciones.

h(q, q )  Vector de fuerza de Coriolis y centrífuga no lineal.

n n
hi   hikm qk qm  q T H i q i  1, 2...n (4.49)
k 1 m 1

Tr U jkm J jU Tji 
n
hikm  
j  max( i ,k ,m )
i, k , m  1, 2...n (4.50)

c(q)  Vector de fuerza gravitacional

44
 Capítulo 4

n

ci   m j gU ji j rj i  1, 2...n (4.51)
j i

Cabe señalar que las variables generalizadas q, q , q

 dependen del tiempo.

4.2.1 Matrices de inercia de cada eslabón

Las matrices de inercia de cada eslabón son tomadas de la sección 4.1.2.

4.2.2. Matriz simétrica de inercia

La matriz simétrica de inercia definida para un manipulador con 5 grados de

libertad será:

 D11 D12 D13 D14 D15 

D D22 D23 D24 D25 
 12
D     D13 D23 D33 D34 D35  (4.52)
 
 D14 D24 D34 D44 D45 
 D15 D25 D35 D45 D55 

Por lo tanto cada término Dik es:

D11  Tr U11I1U11T   Tr U 21I 2U 21

T
  Tr U 31I 3U 31T   Tr U 41I 4U 41T   Tr U 51I 5U 51T 
D12  Tr U 22 I 2U 21
T
  Tr U 32 I3U 31T   Tr U 42 I 4U 41T   Tr U 52 I 5U 51T 
D13  Tr U 33 I 3U 31
T
  Tr U 43I 4U 41T   Tr U 53I5U 51T 
D14  Tr U 44 I 4U 41
T
  Tr U 54 I5U 51T 
D15  Tr U 55 I 5U 51
T

D21  D12

D22  Tr U 22 I 2U 22
T
  Tr U 32 I3U 32T   Tr U 42 I 4U 42T   Tr U 52 I 5U 52T 
D23  Tr U 33 I 3U 32
T
  Tr U 43I 4U 42T   Tr U 53I3U 52T 

45
 Capítulo 4

D24  Tr U 44 I 4U 42
T
  Tr U 54 I5U 52T 
D25  Tr U 55 I 5U 52
T

D31  D13

D32  D23

D33  Tr U 33 I 3U 33
T
  Tr U 43I 4U 43T   Tr U 53I 5U 53T 
D34  Tr U 44 I 4U 43
T
  Tr U 54 I5U 53T 
D35  Tr U 55 I 5U 53
T

D41  D14

D42  D24

D43  D34

D44  Tr U 44 I 4U 44
T
  Tr U 54 I5U 54T 
D45  Tr U 55 I 5U 54
T

D51  D15

D52  D25

D53  D35

D54  D45

D55  Tr U 55 I 5U 55
T

Donde las matrices U ij , U ijk son las derivadas parciales de las matrices de
transformación homogénea:

 0Ti ( A1... Aj ... Ai )

U ij    A1...Q j Aj ... Ai j i (4.53)
q j q j

46
 Capítulo 4

U ij
U ijk  (4.54)
qk

Por lo tanto aplicando las ecuaciones (4.53) y (4.54) se obtienen las siguientes
expresiones:

U11  QA1 U111  QQA1 U 21  QA1 A2

U 211  QQA1 A2 U 212  QAQA

1 2 U 22  A1QA2

U 221  QAQA
1 2 U 222  A1QQA2 U 31  QA1 A2 A3

U 311  QQA1 A2 A3 U 312  QA1QA2 A3 U 313  QA1 A2QA3

U 32  AQA
1 2 A3 U 321  QAQA
1 2 A3 U 322  A1QQA2 A3

U 323  A1QA2QA3 U 33  A1 A2QA3 U 331  QA1 A2QA3

U 332  A1QA2QA3 U 333  A1 A2QQA3 U 41  QA1 A2 A3 A4

U 411  QQA1 A2 A3 A4 U 412  QA1QA2 A3 A4 U 413  QA1 A2QA3 A4

U 414  QA1 A2 A3QA4 U 42  A1QA2 A3 A4 U 421  QA1QA2 A3 A4

U 422  AQQA
1 2 A3 A4 U 423  A1QA2QA3 A4 U 424  A1QA2 A3QA4

U 43  A1 A2QA3 A4 U 431  QA1 A2QA3 A4 U 432  A1QA2QA3 A4

U 433  A1 A2QQA3 A4 U 434  A1 A2QA3QA4 U 44  A1 A2 A3QA4

U 441  QA1 A2 A3QA4 U 442  A1QA2 A3QA4 U 443  A1 A2QA3QA4

U 444  A1 A2 A3QQA4 U 51  QA1 A2 A3 A4 A5 U 511  QQA1 A2 A3 A4 A5

U 512  QA1QA2 A3 A4 A5 U 513  QA1 A2QA3 A4 A5 U 514  QA1 A2 A3QA4 A5

U 515  QA1 A2 A3 A4QA5 U 52  A1QA2 A3 A4 A5 U 521  QA1QA2 A3 A4 A5

U 522  AQQA
1 2 A3 A4 A5 U 523  A1QA2QA3 A4 A5 U 524  A1QA2 A3QA4 A5

U 525  A1QA2 A3 A4QA5 U 53  A1 A2QA3 A4 A5 U 531  QA1 A2QA3 A4 A5

47
 Capítulo 4

U 532  A1QA2QA3 A4 A5 U 533  A1 A2QQA3 A4 A5 U 534  A1 A2QA3QA4 A5

U 535  A1 A2QA3 A4QA5 U 54  A1 A2 A3QA4 A5 U 541  QA1 A2 A3QA4 A5

U 542  A1QA2 A3QA4 A5 U 543  A1 A2QA3QA4 A5 U 544  A1 A2 A3QQA4 A5

U 545  A1 A2 A3QA4QA5 U 55  A1 A2 A3 A4QA5 U 551  QA1 A2 A3 A4QA5

U 552  A1QA2 A3 A4QA5 U 553  A1 A2QA3 A4QA5 U 554  A1 A2 A3QA4QA5

U 555  A1 A2 A3 A4QQA5

4.2.3. Vector de fuerza de Coriolis y centrífuga no lineal h(q, q ) .

h111  tr U111 J1U11T   tr U 211J 2U 21

T
  tr U 311J 3U 31T   tr U 411J 4U 41T   tr U 511J 5U 51T 
h112  tr U 212 J 2U 21
T
  tr U 312 J 3U 31T   tr U 412 J 4U 41T   tr U 512 J 5U 51T 
h113  tr U 313 J 3U 31
T
  tr U 413 J 4U 41T   tr U513 J5U51T 
h114  tr U 414 J 4U 41
T
  tr U514 J5U51T 
h115  tr U 515 J 5U 51
T

h121  h112

h122  tr U 222 J 2U 21
T
  tr U 322 J 3U 31T   tr U 422 J 4U 41T   tr U 522 J 5U 51T 
h123  tr U 323 J 3U 31
T
  tr U 423 J 4U 41T   tr U523 J5U51T 
h124  tr U 424 J 4U 41
T
  tr U524 J5U51T 
h125  tr U 525 J 5U 51
T

h131  h113

h132  h123

48
 Capítulo 4

h133  tr U 333 J 3U 31
T
  tr U 433 J 4U 41T   tr U533 J5U51T 
h134  tr U 434 J 4U 41
T
  tr U534 J5U51T 
h135  tr U 535 J 5U 51
T

h141  h114

h142  h124

h143  h134

h144  tr U 444 J 4U 41
T
  tr U544 J5U51T 
h145  tr U 545 J 5U 51
T

h151  h115

h152  h125

h153  h135

h154  h145

h155  tr U 555 J 5U 51
T

 h111 h121 h131 h141 h151 
h h122 h132 h142 h152 
 112
H1   h113 h123 h133 h143 h153  ; h1   T H1
 
 h114 h124 h134 h144 h154 
 h115 h125 h135 h145 h155 

h211  tr U 211 J 2U 22
T
  tr U 311J 3U32T   tr U 411J 4U 42T   tr U511J5U52T 
h212  tr U 212 J 2U 22
T
  tr U 312 J 3U 32T   tr U 412 J 4U 42T   tr U 512 J 5U 52T 
h213  tr U 313 J 3U 32
T
  tr U 413 J 4U 42T   tr U 513 J 5U 52T 

49
 Capítulo 4

h214  tr U 414 J 4U 42
T
  tr U 514 J 5U 52T 
h215  tr U 515 J 5U 52
T

h221  h212

h222  tr U 222 J 2U 22
T
  tr U 322 J 3U 32T   tr U 422 J 4U 42T   tr U 522 J 5U 52T 
h223  tr U 323 J 3U 32
T
  tr U 423 J 4U 42T   tr U 523 J 5U 52T 
h224  tr U 424 J 4U 42
T
  tr U 524 J 5U 52T 
h225  tr U 525 J 5U 52
T

h231  h13

h232  h223

h233  tr U 333 J 3U 32
T
  tr U 433 J 4U 42T   tr U 533 J 5U 52T 
h234  tr U 434 J 4U 42
T
  tr U 534 J 5U 52T 
h235  tr U 535 J 5U 52
T

h241  h214

h242  h224

h243  h234

h244  tr U 444 J 4U 42
T
  tr U 544 J 5U 52T 
h245  tr U 545 J 5U 52
T

h251  h215

h252  h225

50
 Capítulo 4

h253  h235

h254  h245

h255  tr U 555 J 5U 52
T

 h211 h221 h231 h241 h251 
h h22 h232 h242 h252 
 212
H 2   h213 h223 h233 h243 h253  ; h2   T H 2
 
 h214 h224 h234 h244 h254 
 h215 h225 h235 h245 h255 

h311  Tr U 311 J 3U 33
T
  Tr U 411J 4U 43T   Tr U 511J 5U 53T 
h312  Tr U 312 J 3U 33
T
  Tr U 412 J 4U 43T   Tr U 512 J 5U 53T 
h313  Tr U 313 J 3U 33
T
  Tr U 413 J 4U 43T   Tr U 513 J 5U 53T 
h314  Tr U 414 J 4U 43
T
  Tr U 514 J 5U 53T 
h315  Tr U 515 J 5U 53
T

h321  h312

h322  Tr U 322 J 3U 33
T
  Tr U 422 J 4U 43T   Tr U 522 J 5U 53T 

h323  Tr U 323 J 3U 33
T
  Tr U 423 J 4U 43T   Tr U 523 J 5U 53T 

h324  Tr U 424 J 4U 43
T
  Tr U 524 J 5U 53T 
h325  Tr U 525 J 5U 53
T

h331  h313

h332  h323

51
 Capítulo 4

h333  Tr U 333 J 3U 33
T
  Tr U 433 J 4U 43T   Tr U 533 J 5U 53T 
h334  Tr U 434 J 4U 43
T
  Tr U 534 J 5U 53T 
h335  Tr U 535 J 5U 53
T

h341  h314

h342  h324

h343  h334

h344  Tr U 444 J 4U 43
T
  Tr U 544 J 5U 53T 
h345  Tr U 545 J 5U 53
T

h351  h315

h352  h325

h353  h335

h354  h345

h355  Tr U 555 J 5U 53
T

 h311 h321 h331 h341 h351 
h h322 h332 h342 h352 
 312
H 3   h313 h323 h333 h343 h353  ; h3  wT H 3 w
 
 h314 h324 h334 h344 h354 
 h315 h325 h335 h345 h355 

h415  Tr U 515 J 5U 54
T

h425  Tr U 525 J 5U 54
T

h435  Tr U 535 J 5U 54
T


52
 Capítulo 4

h445  Tr U 545 J 5U 54
T

h455  Tr U 555 J 5U 54
T

h414  Tr U 414 J 4U 44
T
  Tr U 514 J5U54T 
h424  Tr U 424 J 4U 44
T
  Tr U 524 J5U54T 
h434  Tr U 434 J 4U 44
T
  Tr U 534 J5U54T 
h444  Tr U 444 J 4U 44
T
  Tr U 544 J5U54T 
h454  h445

h413  Tr U 413 J 4U 44
T
  Tr U 513 J 5U 54T 
h423  Tr U 423 J 4U 44
T
  Tr U 523 J 5U 54T 
h433  Tr U 433 J 4U 44
T
  Tr U 533 J 5U 54T 
h443  h434

h453  h435

h412  Tr U 412 J 4U 44
T
  Tr U 512 J5U54T 
h422  Tr U 422 J 4U 44
T
  Tr U 522 J5U54T 
h432  h423

h442  h424

h452  h425

h411  Tr U 411 J 4U 44
T
  Tr U 511J 5U 54T 
h421  h412

53
 Capítulo 4

h431  h413

h441  h414

h451  h415

 h411 h421 h431 h441 h451 

h h422 h432 h442 h452 
 412
H 4   h413 h423 h433 h443 h453  ; h4  wT H 4 w
 
 h414 h424 h434 h444 h454 
 h415 h425 h435 h445 h455 

h511  Tr U 511 J 5U 55
T

h512  Tr U 512 J 5U 55
T

h513  Tr U 513 J 5U 55
T

h514  Tr U 514 J 5U 55
T

h515  Tr U 515 J 5U 55
T

h521  h512

h522  Tr U 522 J 5U 55
T

h523  Tr U 523 J 5U 55
T

h524  Tr U 524 J 5U 55
T

h525  Tr U 525 J 5U 55
T

h531  h513

h532  h523

h533  Tr U 533 J 5U 55
T

54
 Capítulo 4

h534  Tr U 534 J 5U 55
T

h535  Tr U 535 J 5U 55
T

h541  h514

h542  h524

h543  h534

h544  Tr U 544 J 5U 55
T

h545  Tr U 545 J 5U 55
T

h551  h515

h552  h525

h553  h535

h554  h545

h555  Tr U 555 J 5U 55
T

 h511 h521 h531 h541 h551 
h h522 h532 h542 h552 
 512
H 5   h513 h523 h533 h543 h553  ; h5   T H 5
 
 h514 h524 h534 h544 h554 
 h515 h525 h535 h545 h555 

Por lo tanto:

 h1 
h 
 2
h   h3 
 
 h4 
 h5 

55
 Capítulo 4

4.2.4. Vector de carga gravitacional

El vector de carga o de fuerza gravitacional es aquel que proporciona los torques

con respecto al tiempo, que actúan en las articulaciones.

 m1 g1U11r11  m2 g 2U 22 r22  m3 g 3U 33r33  m4 g 4U 44r44  m5 g 5U 55r55 

 m2 g 2U 22 r22  m3 g3U 33r33  m4 g 4U 44 r44  m5 g 5U 55r55 
 
c   m3 g3U 33r33  m4 g 4U 44 r44  m5 g 5U 55r55 
 
 m4 g 4U 44 r44  m5 g5U 55r55 
 m5 g5U 55r55 

4.3. Generación de Trayectorias

El generador de trayectoria es una herramienta que facilita la descripción del

movimiento de un manipulador para el usuario, permitiendo que el sistema
resuelva a detalle cada uno de los cálculos (Craig, 2006). Por lo tanto la
implementación de un generador de trayectorias tiene por objetivo calcular los
valores de cada articulación, dadas las condiciones iniciales de posición, velocidad
y aceleración a satisfacer en un intervalo de tiempo determinado.

4.3.1. Polinomios de quinto orden

La cinemática inversa permite calcular el conjunto de ángulos de cada una de las

articulaciones que corresponden a la posición, orientación, velocidad y aceleración
deseadas.

 (t )  a0  a1t  a2t 2  a3t 3  a4t 4  a5t 5 (4.55)

(t )  a1  2a2t  3a3t 2  4a4t 3  5a5t 4 (4.56)

(t )  2a2  6a3t  12a4t 2  20a5t 3 (4.57)

Con las condiciones iniciales se establece el siguiente conjunto de ecuaciones:

56
 Capítulo 4

i   (0)  a0
i  (0)  a1
i  (0)  2a2
(4.58)
 f   (t f )  a0  a1t f  a2t 2f  a3t 3f  a4t 4f  a5t 5f
 f  (t f )  a1  2a2t f  3a3t 2f  4a4t 3f  5a5t 4f
f  (t f )  2a2  6a3t f  12a4t 2f  20a5t 3f

Del conjunto de ecuaciones (4.58) se define un arreglo matricial para calcular cada
uno de los coeficientes de los polinomios de posición, velocidad y aceleración.
1
 a0  1 0 0 0 0 0   i 
 a  0 1 0 0 0 0   i 
 1   
 a2  0 0 1 0 0 0   0.5i  
      2  (4.59)
 f  i  i t f  0.5i t f 
3
 a3  0 0 0 tf t 4f t 5f 
 a4  0 0 0 3t 2f 4t 3f 5t 4f   f  i  it f 
     
 a5  0 0 0 6t f 12t 2f 40t 3f   f  i 

Partiendo de las expresiones definidas anteriormente, también es posible expresar

un polinomio de quinto orden en forma dual.

 (t )   S (t )  a0  a1t  a2t 2  a3t 3  a4t 4  a5t 5    b0  b1t  b2t 2  b3t 3  b4t 4  b5t 5 

(4.60)

(t )   S (t )  a1  2a2t  3a3t 2  4a4t 3  5a5t 4    b1  2b2t  3b3t 2  4b4t 3  5b5t 4 

(4.61)

(t )   S(t )  2a2  6a3t  12a4t 2  20a5t 3    2b2  6b3t  12b4t 2  20b5t 3  (4.62)

 a0  b0   i   Si 
a  b       S  
  1  1  i   i 
 a2  b2  1  0.5i  1  0.5Si 
       t         t      (4.63)
 f  i  i t f  0.5i t f   S f  Si  Si t f  0.5Si t f 
2 2
 a3  b3 
 a4  b4   f  i  i t f   S f  Si  Si t f 
       
 a5  b5   f  i   Sf  Si 

57
 Capítulo 4

Donde:

1 0 0 0 0 0 
0 1 0 0 0 0 

0 0 1 0 0 0 
t    0 0 3
0 tf t 4f t 5f 
 (4.64)

0 0 0 3t 2f 4t 3f 5t 4f 
 
0 0 0 6t f 12t 2f 40t 3f 

Por lo tanto la ecuación (4.63) establece de forma generalizada el conjunto de

desplazamientos, velocidades y aceleraciones angulares y lineales de un
manipulador robótico con articulaciones de revolución, cilíndricas o prismáticas.

4.4. Interface MATLAB – SIMULINK – VISUAL NASTRAN

Como parte complementaria de este trabajo se desarrolló una simulación, que a

partir de las ecuaciones duales y matriciales, fuera capaz de emular el movimiento
del manipulador. Se estableció una interface visual, que aprovecha las
características del modelo en CAD por medio de VISUAL NASTRAN®, así como
también la potencia de solución de las ecuaciones por medio de MATLAB®. Dicha
conexión es realizada por la herramienta de SIMULINK®.

El algoritmo a través del cual opera la interface es el siguiente:

La posición, velocidad y aceleración inicial y final en el espacio cartesiano son

propuestos en MATLAB.

Se transforman las variables de posición, velocidad y aceleración al espacio

articulado por medio de la cinemática inversa.

Los valores inicial y final de las variables de articulación son introducidas al

algoritmo de generación de trayectorias para calcular los valores intermedios de
las variables de articulación para que el movimiento del manipulador sea suave y
continuo con respecto al tiempo.

Del espacio de trabajo de MATLAB son enviados a SIMULINK, las matrices con
los valores de los ángulos para cada articulación, definidos para cada instante de
tiempo.

SIMULINK a través de la función vNPLANT envía a NASTRAN, los ángulos y el

tiempo en que cada articulación debe moverse.

58
 Capítulo 4

Figura 4.19. Esquema de funcionamiento de la simulación

Figura 4.21. Modelo 3D con controles de rotación.

59
 Capítulo 4

Figura 4.22. Programa en SIMULINK para conectar MATLAB con el modelo 3D.

60
Capítulo 5 Análisis de resultados.

5
A lo largo de este trabajo se han desarrollado los algoritmos basados en matrices
de transformación homogénea y en matrices duales ortogonales, para obtener el
modelo que describe el movimiento cinemático y dinámico de manipuladores
seriales, en específico el MIRH, no obstante como objetivo de este trabajo es
necesario realizar un análisis comparativo entre ambos métodos.

5.1. Análisis de la cinemática directa

De la configuración geométrica del MIRH mostrada en la Figura 5.1, la cual es la

posición inicial elegida arbitrariamente y es la distancia máxima de alcance del
manipulador.

Figura 5.1. Posición y orientación iniciales

Por lo que de la configuración geométrica mostrada en la Figura 5.1, es la posición

máxima de alcance del MIRH la cual genera una circunferencia de 1580 mm.
 Capítulo 5

Las ecuaciones de lazo cerrado obtenidas a partir de las matrices duales y de

transformación homogénea son evaluadas con los ángulos de entrada mostrados
en la Tabla 5.1, por lo tanto el MIRH pasará a la posición deseada, mostrada en la
Figura 5.3.

1 2 3 4 5
0 0 0 0 0
9 4.5 -9 4.5 18
18 9 -18 9 36
27 13.5 -27 13.5 54
36 18 -36 18 72
45 22.5 -45 22.5 90
54 27 -54 27 108
63 31.5 -63 31.5 126
72 36 -72 36 144
81 40.5 -81 40.5 162
90 45 -90 45 180
Tabla 5.1. Ángulos propuestos

Figura 5.3. Configuración final deseada

En la Tabla 5.2 se observa de manera clara que los valores del vector de posición
del efector final tanto para las matrices homogéneas como para las matrices
duales el resultado es similar, por lo que el error entre ambas es nulo, sin embargo
durante la ejecución de los programas se pudo observar que el tiempo de
procesamiento es menor para algebra matricial que para las matrices duales, esto
62
 Capítulo 5

se debe principalmente a que se optó utilizar la forma expandida de las matrices

duales lo cual aumenta significativamente las operaciones a realizar, la grafica 5.2
muestra el tiempo requerido para resolver la cinemática directa.

Matrices Duales Matrices Homogéneas

X Y Z X Y Z
0 1580 0 0 1580 0
-246.73 1557.8 0 -246.73 1557.8 0
-484.82 1492.1 0 -484.82 1492.1 0
-706.02 1385.6 0 -706.02 1385.6 0
-902.81 1242.6 0 -902.81 1242.6 0
-1068.8 1068.8 0 -1068.8 1068.8 0
-1198.9 871.04 0 -1198.9 871.04 0
-1289.6 657.1 0 -1289.6 657.1 0
-1339.2 435.13 0 -1339.2 435.13 0
-1347.6 213.43 0 -1347.6 213.43 0
-1316.4 0 0 -1316.4 0 0
Tabla 5.2. Vector de posición del efector final para los ángulos de entrada
establecidos

0.02

0.015
Tiempo de procesamiento

0.01
Matrices Duales

0.005 Matrices
Homogéneas

0
1 2 3 4 5 6 7 8 9 10

-0.005
Puntos

Figura 5.2. Grafica comparativa de tiempo requerido para solucionar las ecuaciones
de lazo cerrado

63
 Capítulo 5

La Figura 5.3 muestra la posición deseada, esta es la posición mínima en el plano

X-Y del efector final, la cual dibuja una circunferencia de 1316 mm.

5.2. Análisis de la cinemática inversa

Partiendo de los valores de posición y orientación del efector final mostrados en

las Tablas 5.3 y 5.4, se requiere conocer los valores de los ángulos en las
articulaciones para que el manipulador logre satisfacer dicha posición.

X y z
deseado deseado deseado
1 0 1580 0
2 0 450 -230
3 1316.4 0 0
4 -526.0909 0 872.8617
5 0 680 900
6 0 680 900
7 0 680 900
8 0 680 900
9 0 680 900
10 0 820 1000
11 0 820 900
12 0 680 900
13 -1000 1000 150
14 -700 700 600
15 0 450 -230
Tabla 5.3. Vectores de posición deseados

nx ny nz ox oy oz ax ay az
1 0 0 -1 -1 0 0 0 1 0
2 0 -1 0 -1 0 0 0 0 1
3 0 1 0 0 0 1 1 0 0
4 0 0 -1 0 -1 0 -1 0 0
5 0 0 -1 -1 0 0 0 1 0
6 0 0.4226 -0.9063 -1 0 0 0 0.9063 0.4226
7 0 0.7071 -0.7071 -1 0 0 0 0.7071 0.7071
8 0 0.9397 -0.342 -1 0 0 0 0.342 0.9397
9 0 1 0 -1 0 0 0 0 1
10 0 1 0 -1 0 0 0 0 1
11 0 1 0 -1 0 0 0 0 1
12 0 1 0 -1 0 0 0 0 1
13 -0.7071 -0.7071 0 0 0 1 -0.7071 0.7071 0
14 -0.7071 -0.7071 0 0 0 1 -0.7071 0.7071 0

64
 Capítulo 5

15 0 -1 0 -1 0 0 0 0 -1
Tabla 5.4. Vectores normal, de orientación y de aproximación deseados

Los ángulos calculados a partir de las ecuaciones duales, que satisfacen las
condiciones de orientación y de posición son presentados en la Tabla 5.5.

t1 t2 t3 t4 t5
0 0 0 0 0
0 90 -90 -90 0
-90 44.9996 -89.9993 44.9997 90
90 110 -20 -90 0
0 44.9999 0.0002 -90.0001 0
0 130.8783 -93.6294 -12.2496 0
0 123.5454 -117.9199 39.3746 0
0 85.1176 -109.7227 94.6064 0
0 55.3563 -74.8567 109.5004 0
0 33.3502 -24.0646 80.7144 0
0 34.3994 -38.762 94.3627 0
0 55.3563 -74.8567 109.5004 0
45 45.1762 -67.3 22.0829 90
45 104.0589 -82.7576 -21.3011 90
0 90 -90 -90 0
Tabla 5.5. Ángulos calculados por algebra dual.

Por matrices de transformación homogénea los ángulos de las articulaciones que

satisfacen las configuraciones deseadas del manipulador se dan en la Tabla 5.6.

t1 t2 t3 t4 t5
0 0 0 0 0
0 90 -90 -90 0
-90 44.9997 -89.9993 44.9996 90
90 110 -20 -90 0
0 90.0005 -0.0011 -89.9995 0
0 130.8772 -93.6281 -12.2499 0
0 123.5448 -117.9191 39.3743 0
0 85.1175 -109.7227 94.6064 0
0 55.3563 -74.8567 109.5004 0
0 33.3502 -24.0646 80.7144 0
0 34.3994 -38.762 94.3627 0
0 55.3563 -74.8567 109.5004 0
45 45.1762 -67.3 22.0829 90
45 104.0589 -82.7576 -21.3011 90

65
 Capítulo 5

0 90 -90 -90 0
Tabla 5.6. Ángulos calculados por matrices de transformación homogénea.

Para ambos casos los resultados son muy idénticos y ambos satisfacen las
condiciones iniciales con un error promedio de 0.000089º.

En cuanto a tiempo de procesamiento (Figura 5.4) el sistema de ecuaciones

formado a partir de las matrices duales, converge más rápido aunque no de
manera significativa.

Figura 5.4. Tiempo de solución de los sistemas de ecuaciones generados por matrices
de transformación homogénea y matrices duales.

5.3. Análisis de velocidades y aceleraciones

Del capítulo 3 desarrollado en este trabajo de tesis, las ecuaciones de velocidad y

aceleración son idénticos, tanto por la matriz Jacobiana dual generalizada como
por la matriz Jacobiana característica del manipulador.

5.4. Generación de trayectorias

A partir de la información proporcionada por las tablas 5.3, 5.4, 5.5 y 5.6 con
velocidades y aceleraciones nulas para el inicio y final de cada intervalo, es
posible establecer un generador de trayectoria, el cual producirá un movimiento
suave y continuo en cada una de las articulaciones, por lo tanto cada articulación
para los intervalos deseados tendrá el siguiente comportamiento.

66
 Capítulo 5

1) 2) 3)

4) 5) 6)

7) 8) 9)

10) 11) 12)

13) 14) 15)

67
 Capítulo 5

Figura 5.??. Secuencia de puntos deseados de 0 a 70 segundos.

Partiendo de las configuraciones deseadas en la figura, la posición, velocidad y

aceleración angular, así como el torque para cada una de las articulaciones son
mostradas en las siguientes gráficas.

Figura 5.5. Posición articulación 1

68
 Capítulo 5

Figura 5.6. Velocidad angular de la articulación 1

Figura 5.7. Aceleración angular articulación 1

Para la articulación 2 se desean los desplazamientos angulares mostrados en la

figura 5.8.

Figura 5.8. Posición articulación 2

69
 Capítulo 5

Figura 5.9. Velocidad angular articulación 2

Figura 5.10. Aceleración angular articulación 2

70
 Capítulo 5

Figura 5.11. Posición articulación 3

Figura 5.12. Velocidad angular articulación 3

71
 Capítulo 5

Figura 5.13. Aceleración angular articulación 3

Figura 5.14. Posición articulación 4

72
 Capítulo 5

Figura 5.15. Velocidad angular articulación 4

73
 Capítulo 5

Figura 5.16. Aceleración angular articulación 4

Figura 5.17. Posición articulación 5

74
 Capítulo 5

Figura 5.18. Velocidad angular articulación 5

Figura 5.19. Aceleración angular articulación 5

5.5. Análisis dinámico

En el análisis dinámico como se ha mencionado anteriormente se determinaron

los valores de torque requerido para cada una de las articulaciones tomando como
entradas cada uno de los datos de posición, velocidad y aceleración angular
obtenidos del generador de trayectorias. En las siguientes figuras se muestran los
torques obtenidos por el algebra dual y por algebra matricial convencional.

75
 Capítulo 5

Figura 5.20. Torque de la articulación 1

Figura. 5.21. Torque articulación 2

Figura 5.22. Torque articulación 3

76
 Capítulo 5

Figura 5.23. Torque articulación 4

Figura 5.24. Torque articulación 5

Se observa de la figuras 5.20 a 5.24 el torque para ambas metodologías no varía

de manera significativa, sin embargo en la articulación 5 se observa que en ciertos
puntos los resultados son diferentes.

77
Capítulo 5

78
Conclusiones
Los números duales han demostrado ser una herramienta con un amplio poder de
solución a problemas que implican translaciones y rotaciones de cuerpos rígidos
en el espacio. Además proporcionan un método capaz de ser generalizado a
articulaciones prismáticas o cilíndricas, lo cual reduce el tiempo de programación
para cada uno de los diferentes problemas derivados de la diversidad de
topologías en los manipuladores industriales.

A partir de los conceptos de ángulo dual y matriz eslabón-articulación, fue posible

determinar matrices duales de dimensión 3x3, que proporcionan simultáneamente
las traslaciones y rotaciones de los eslabones del MIRH.

De las matrices duales se estableció la ecuación de lazo cerrado y las ecuaciones

de diseño duales del manipulador, así que el algebra dual aplicada a la cinemática
directa e inversa resulta viable, debido a que lo resultados obtenidos son muy
semejantes a los del algebra matricial. Sin embargo, a pesar de que el sistema de
ecuaciones calculado a partir de la matriz dual resulta más complejo, el tiempo de
solución es menor.

Las velocidades y aceleraciones angulares de cada articulación, fueron calculadas

con ayuda de la matriz Jacobiana dual generalizada, que presenta como principal
ventaja poder ser usada para articulaciones, cilíndricas prismáticas o de revolución
ya que es formada a partir de las matrices duales eslabón- articulación.

En dinámica el álgebra dual es una herramienta con una mayor capacidad de

análisis debido a que proporciona mayor información acerca del sistema es decir
Lagrange obtiene 5 torques mientras que el algebra dual obtiene 15 fuerzas
duales (15 reacciones y 15 torques)

En comparación con la metodología de Lagrange el algebra dual resulta poco útil

para su empleo en sistemas de control en tiempo real, a pesar de que la
formulación dinámica dual es sistemática y con un fácil entendimiento, tiene un
alto costo computacional al tener que derivar el momento dual lo que se traduce
un mayor tiempo de solución.

Usar un generador de trayectorias, es de substancial ayuda para obtener los

puntos intermedios entre dos puntos deseados, produciendo un movimiento
continuo y suave del efector final. Por lo tanto un polinomio dual de 5º orden
puede ser de gran utilidad en sistemas de cadena abierta con topologías
complejas.

79
Simular los movimientos del MIRH en una interface gráfica basada en las
ecuaciones de movimiento dual, permite observar el comportamiento y las
posibles colisiones de algún eslabón o del efector final.

Además de los aspectos antes mencionados durante este trabajo también se pudo
concluir que agregar un grado de libertad más al MIRH, sería de gran utilidad para
incrementar su movilidad y volumen de trabajo ya que del análisis hecho en el
simulador se observó la deficiencia que tiene el MIRH para realizar diferentes
trayectorias. Por lo que el MIRH con una topología 6R, permitiría que la matriz
Jacobiana característica del manipulador sea cuadrada lo que se traduce en una
mayor facilidad para invertir dicha matriz.

80
Recomendaciones para trabajos futuros
Diversos son los trabajos que pueden surgir de esta tesis, tales temas sugeridos
son:

Optimizar la programación del algebra dual, con el propósito de que las

ecuaciones sean mas rápidas de resolver y puedan usarse con fines de de
programación en tiempo real y realizar programas en C++, Builder o VBasic para
fines prácticos.

Establecer en forma dual modelos dinámicos que tomen en cuenta el efecto de la

fricción.

Realizar un software de simulación basado en números duales cuya principal

característica sea la generalización del algoritmo para diversas topologías de
robots manipuladores.

De acuerdo a la simulación realizada sería indicado anexar un grado más de

libertad al MIRH, así como considerar un rediseño para obtener mayor movilidad y
por consiguiente un mayor volumen de trabajo.

81
Anexo A.- Programa para solucionar la cinemática directa
por álgebra dual
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%CINEMATICA DIRECTA DE MIRH POR NUMEROS DUALES METODOLOGIA DEL FISHER %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clc
clear all
syms E t1 t2 t3 t4 t5
alpha1=90; a1=0; S1=0;
alpha2=0; a2=450; S2=0;
alpha3=0; a3=450; S3=0;
alpha4=-90; a4=0; S4=0;
alpha5=0; a5=0; S5=680;
tic
P01=[cos(t1), -cosd(alpha1)*sin(t1), sind(alpha1)*sin(t1);
sin(t1), cosd(alpha1)*cos(t1), -sind(alpha1)*cos(t1);
0, sind(alpha1), cosd(alpha1);];
P12=[cos(t2), -cosd(alpha2)*sin(t2), sind(alpha2)*sin(t2);
sin(t2), cosd(alpha2)*cos(t2), -sind(alpha2)*cos(t2);
0, sind(alpha2), cosd(alpha2);];
P23=[cos(t3), -cosd(alpha3)*sin(t3), sind(alpha3)*sin(t3);
sin(t3), cosd(alpha3)*cos(t3), -sind(alpha3)*cos(t3);
0, sind(alpha3), cosd(alpha3);];
P34=[cos(t4), -cosd(alpha4)*sin(t4), sind(alpha4)*sin(t4);
sin(t4), cosd(alpha4)*cos(t4), -sind(alpha4)*cos(t4);
0, sind(alpha4), cosd(alpha4);];
P45=[cos(t5), -cosd(alpha5)*sin(t5), sind(alpha5)*sin(t5);
sin(t5), cosd(alpha5)*cos(t5), -sind(alpha5)*cos(t5);
0, sind(alpha5), cosd(alpha5);];
P05=P01*P12*P23*P34*P45;
D01=[-S1*sin(t1),a1*sind(alpha1)*sin(t1)-S1*cosd(alpha1)*cos(t1),
a1*cosd(alpha1)*sin(t1)+S1*sind(alpha1)*cos(t1);
S1*cos(t1),-a1*sind(alpha1)*cos(t1)-S1*cosd(alpha1)*sin(t1),
-a1*cosd(alpha1)*cos(t1)+S1*sind(alpha1)*sin(t1);
0, a1*cosd(alpha1),
-a1*sind(alpha1);];
D12=[-S2*sin(t2), a2*sind(alpha2)*sin(t2)-S2*cosd(alpha2)*cos(t2),
a2*cosd(alpha2)*sin(t2)+S2*sind(alpha2)*cos(t2);
S2*cos(t2), -a2*sind(alpha2)*cos(t2)-S2*cosd(alpha2)*sin(t2), -
a2*cosd(alpha2)*cos(t2)+S2*sind(alpha2)*sin(t2);

82
 0, a2*cosd(alpha2),
-a2*sind(alpha2);];
D23=[-S3*sin(t3), a3*sind(alpha3)*sin(t3)-S3*cosd(alpha3)*cos(t3),
a3*cosd(alpha3)*sin(t3)+S3*sind(alpha3)*cos(t3);
S3*cos(t3), -a3*sind(alpha3)*cos(t3)-S3*cosd(alpha3)*sin(t3), -
a3*cosd(alpha3)*cos(t3)+S3*sind(alpha3)*sin(t3);
0, a3*cosd(alpha3),
-a3*sind(alpha3);];
D34=[-S4*sin(t4), a4*sind(alpha4)*sin(t4)-S4*cosd(alpha4)*cos(t4),
a4*cosd(alpha4)*sin(t4)+S4*sind(alpha4)*cos(t4);
S4*cos(t4), -a4*sind(alpha4)*cos(t4)-S4*cosd(alpha4)*sin(t4), -
a4*cosd(alpha4)*cos(t4)+S4*sind(alpha4)*sin(t4);
0, a4*cosd(alpha4),
-a4*sind(alpha4);];
D45=[-S5*sin(t5), a5*sind(alpha5)*sin(t5)-S5*cosd(alpha5)*cos(t5),
a5*cosd(alpha5)*sin(t5)+S5*sind(alpha5)*cos(t5);
S5*cos(t5), -a5*sind(alpha5)*cos(t5)-S5*cosd(alpha5)*sin(t5), -
a5*cosd(alpha5)*cos(t5)+S5*sind(alpha5)*sin(t5);
0, a5*cosd(alpha5),
-a5*sind(alpha5);];

D05 = D01*P12*P23*P34*P45 + P01*D12*P23*P34*P45 + P01*P12*D23*P34*P45 +

P01*P12*P23*D34*P45 + P01*P12*P23*P34*D45;
j1=input('theta 1: '); t1=(pi*(j1+90))/180;
j2=input('theta 2: '); t2=(j2*pi)/180;
j3=input('theta 3: '); t3=(j3*pi)/180;
j4=input('theta 4: '); t4=((j4-90)*pi)/180;
j5=input('theta 5: '); t5=(j5*pi)/180;
tic
TP=eval(P05); %MATRIZ CON LA PARTE PRIMARIA
TD=eval(D05); %MATRIZ DUAL
%SEPARACION DEL TEOREMA DEL PRODUCTO CRUZ Y LA MATRIZ DE ORIENTACION
POSICION=TD*TP';
Mat_orientacion=TP
Vec_posicion=[POSICION(3,2);
POSICION(1,3);
POSICION(2,1);]
toc

83
Anexo B.- Programa para solucionar la cinemática
inversa por álgebra dual
%%%%Programa para resolver la cinematica inversa y directa por algebra
clc
clear all
syms E t1 t2 t3 t4 t5 t6
global MP px py pz
tic
%%%%Vector fila de valores propuestos!!!
%MAtriz de orientacion
nx=0 ny=-1 nz=0
ox=-1 oy=0 oz=0
ax=0 ay=0 az=-1

px=0 py=450 pz=-230

MP=[nx ox ax
ny oy ay
nz oz az];

%Matriz de posición
t0=[1.5,.5,-1.1,.1,.1];
%Función para resolver el sistema de ecuaciones
t=fsolve(@ecua_dual,t0);
toc

84
Anexo C.- Función del sistema de ecuaciones de diseño
duales
function F=ecua_dual(t)
global MP px py pz
P=[0 -pz py;
pz 0 -px;
-py px 0;];
MD=P*MP;
D11=MD(1,1);D21=MD(2,1);D31=MD(3,1);D12=MD(1,2);D22=MD(2,2);D32=MD(3,2);
D13=MD(1,3);D23=MD(2,3);D33=MD(3,3);

%Matriz de funciones

F=[D11-
((450*sin(t(1))*sin(t(3))*cos(t(4))+450*sin(t(1))*cos(t(3))*sin(t(4)))*co
s(t(5))-
450*cos(t(1))*sin(t(2))*sin(t(5))+450*sin(t(1))*sin(t(4))*cos(t(5))+(-
450*cos(t(1))*cos(t(2))*sin(t(3))-
450*cos(t(1))*sin(t(2))*cos(t(3)))*sin(t(5))-
680*((cos(t(1))*cos(t(2))*cos(t(3))-
cos(t(1))*sin(t(2))*sin(t(3)))*cos(t(4))+(-cos(t(1))*cos(t(2))*sin(t(3))-
cos(t(1))*sin(t(2))*cos(t(3)))*sin(t(4)))*sin(t(5))-
680*sin(t(1))*cos(t(5)))
D21-((-450*cos(t(1))*sin(t(3))*cos(t(4))-
450*cos(t(1))*cos(t(3))*sin(t(4)))*cos(t(5))-
450*sin(t(1))*sin(t(2))*sin(t(5))-450*cos(t(1))*sin(t(4))*cos(t(5))+(-
450*sin(t(1))*cos(t(2))*sin(t(3))-
450*sin(t(1))*sin(t(2))*cos(t(3)))*sin(t(5))-
680*((sin(t(1))*cos(t(2))*cos(t(3))-
sin(t(1))*sin(t(2))*sin(t(3)))*cos(t(4))+(-sin(t(1))*cos(t(2))*sin(t(3))-
sin(t(1))*sin(t(2))*cos(t(3)))*sin(t(4)))*sin(t(5))+680*cos(t(1))*cos(t(5
)))
D31-(450*cos(t(2))*sin(t(5))+(-
450*sin(t(2))*sin(t(3))+450*cos(t(2))*cos(t(3)))*sin(t(5))-
680*((sin(t(2))*cos(t(3))+cos(t(2))*sin(t(3)))*cos(t(4))+(-
sin(t(2))*sin(t(3))+cos(t(2))*cos(t(3)))*sin(t(4)))*sin(t(5)))
D12-(-
(450*sin(t(1))*sin(t(3))*cos(t(4))+450*sin(t(1))*cos(t(3))*sin(t(4)))*sin
(t(5))-450*cos(t(1))*sin(t(2))*cos(t(5))-
450*sin(t(1))*sin(t(4))*sin(t(5))+(-450*cos(t(1))*cos(t(2))*sin(t(3))-
450*cos(t(1))*sin(t(2))*cos(t(3)))*cos(t(5))-
680*((cos(t(1))*cos(t(2))*cos(t(3))-
cos(t(1))*sin(t(2))*sin(t(3)))*cos(t(4))+(-cos(t(1))*cos(t(2))*sin(t(3))-
cos(t(1))*sin(t(2))*cos(t(3)))*sin(t(4)))*cos(t(5))+680*sin(t(1))*sin(t(5
)))

85
 D22-(-(-450*cos(t(1))*sin(t(3))*cos(t(4))-
450*cos(t(1))*cos(t(3))*sin(t(4)))*sin(t(5))-
450*sin(t(1))*sin(t(2))*cos(t(5))+450*cos(t(1))*sin(t(4))*sin(t(5))+(-
450*sin(t(1))*cos(t(2))*sin(t(3))-
450*sin(t(1))*sin(t(2))*cos(t(3)))*cos(t(5))-
680*((sin(t(1))*cos(t(2))*cos(t(3))-
sin(t(1))*sin(t(2))*sin(t(3)))*cos(t(4))+(-sin(t(1))*cos(t(2))*sin(t(3))-
sin(t(1))*sin(t(2))*cos(t(3)))*sin(t(4)))*cos(t(5))-
680*cos(t(1))*sin(t(5)))
D32-(450*cos(t(2))*cos(t(5))+(-
450*sin(t(2))*sin(t(3))+450*cos(t(2))*cos(t(3)))*cos(t(5))-
680*((sin(t(2))*cos(t(3))+cos(t(2))*sin(t(3)))*cos(t(4))+(-
sin(t(2))*sin(t(3))+cos(t(2))*cos(t(3)))*sin(t(4)))*cos(t(5)))
D13-(-
450*sin(t(1))*sin(t(3))*sin(t(4))+450*sin(t(1))*cos(t(3))*cos(t(4))+450*s
in(t(1))*cos(t(4)))
D23-(450*cos(t(1))*sin(t(3))*sin(t(4))-
450*cos(t(1))*cos(t(3))*cos(t(4))-450*cos(t(1))*cos(t(4)))
D33];

86
Anexo D.- Programa para solucionar la cinemática directa
por matrices de transformación homogénea
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%CINEMATICA DIRECTA POR ALGEBRA MATRICIAL%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clc
clear all
syms t1 t2 t3 t4 t5
tic
%Parametros de Denavith y Hartemberg
alpha1=90; a1=0; d1=0;
alpha2=0; a2=450; d2=0;
alpha3=0; a3=450; d3=0;
alpha4=-90; a4=0; d4=0;
alpha5=0; a5=0; d5=680;
A01=[cos(t1), -cosd(alpha1)*sin(t1), sind(alpha1)*sin(t1), a1*cos(t1);
sin(t1), cosd(alpha1)*cos(t1), -sind(alpha1)*cos(t1), a1*sin(t1);
0, sind(alpha1), cosd(alpha1), d1;
0, 0, 0, 1;];

A12=[cos(t2), -cosd(alpha2)*sin(t2), sind(alpha2)*sin(t2), a2*cos(t2);

sin(t2), cosd(alpha2)*cos(t2), -sind(alpha2)*cos(t2), a2*sin(t2);
0, sind(alpha2), cosd(alpha2), d2;
0, 0, 0, 1;];

A23=[cos(t3), -cosd(alpha3)*sin(t3), sind(alpha3)*sin(t3), a3*cos(t3);

sin(t3), cosd(alpha3)*cos(t3), -sind(alpha3)*cos(t3), a3*sin(t3);
0, sind(alpha3), cosd(alpha3), d3;
0, 0, 0, 1;];

A34=[cos(t4), -cosd(alpha4)*sin(t4), sind(alpha4)*sin(t4), a4*cos(t4);

sin(t4), cosd(alpha4)*cos(t4), -sind(alpha4)*cos(t4), a4*sin(t4);
0, sind(alpha4), cosd(alpha4), d4;
0, 0, 0, 1;];

A45=[cos(t5), -cosd(alpha5)*sin(t5), sind(alpha5)*sin(t5), a5*cos(t5);

sin(t5), cosd(alpha5)*cos(t5), -sind(alpha5)*cos(t5), a5*sin(t5);
0, sind(alpha5), cosd(alpha5), d5;
0, 0, 0, 1;];

87
T=A01*A12*A23*A34*A45;
T05=simple(T);
toc
h1=input('theta1: '); t1=deg2rad(h1+90);
h2=input('theta2: '); t2=deg2rad(h2);
h3=input('theta3: '); t3=deg2rad(h3);
h4=input('theta4: '); t4=deg2rad(h4-90);
h5=input('theta5: '); t5=deg2rad(h5);
tic
M_05=Eval(T05)
Toc
%%%Gráfica de Resultados
grid on
%%Conversion de polar a Cartesiano
[x2,y2]=pol2cart(t2,450);
[x3,y3]=pol2cart(t3+t2,450);
[x4,y4]=pol2cart(t2+t3+t4+(pi/2),680);

line([0 x2],[0 y2])

line([x2 x2+x3],[y2 y2+y3])
line([x2+x3 x2+x3+x4],[y2+y3 y2+y3+y4])

88
Anexo E.- Programa para solucionar la cinemática
inversa por matrices de transformación homogénea
%%%%Programa para resolver la cinematica inversa y directa por algebra
clc
clear all
global A
tic
%%%%Vector fila de valores propuestos
t0=[1.5,.5,-1.1,.1,.1];
%Función para resolver el sistema de ecuaciones

nx=0 ny=-1 nz=0

ox=-1 oy=0 oz=0
ax=0 ay=0 az=-1

px=0 py=450 pz=-230

MP=[nx ox ax
ny oy ay
nz oz az];

A=[nx ox ax px
ny oy ay py
nz oz az pz
0 0 0 1];

t = fsolve(@Ecua_mat,t0);

toc
%t=lsqnonlin(@myfun,t0);

89
Anexo F.- Función del sistema de ecuaciones de diseño
por matrices de transformación homogénea
function F=Ecua_mat(t)

global A
nx=A(1,1);ny=A(2,1);nz=A(3,1);ox=A(1,2);oy=A(2,2);oz=A(3,2);ax=A(1,3);
ay=A(2,3);az=A(3,3);px=A(1,4);py=A(2,4);pz=A(3,4);

%Matriz de funciones
F=[nx-(1/4*cos(-t(5)-t(4)-t(3)+t(1)-t(2))+1/4*cos(t(5)-t(4)-t(3)+t(1)-
t(2))+1/4*cos(-
t(5)+t(4)+t(3)+t(1)+t(2))+1/4*cos(t(5)+t(4)+t(3)+t(1)+t(2))-1/2*cos(t(1)-
t(5))+1/2*cos(t(1)+t(5)))
ny-(1/4*sin(t(5)+t(4)+t(3)+t(1)+t(2))+1/4*sin(-
t(5)+t(4)+t(3)+t(1)+t(2))+1/4*sin(t(5)-t(4)-t(3)+t(1)-t(2))+1/4*sin(-
t(5)-t(4)-t(3)+t(1)-t(2))+1/2*sin(t(1)+t(5))-1/2*sin(t(1)-t(5)))
nz-(1/2*sin(t(2)+t(3)+t(4)+t(5))+1/2*sin(t(2)+t(3)+t(4)-t(5)))
ox-(-1/4*sin(t(5)-t(4)-t(3)+t(1)-t(2))+1/4*sin(-t(5)-t(4)-t(3)+t(1)-
t(2))-1/4*sin(t(5)+t(4)+t(3)+t(1)+t(2))+1/4*sin(-
t(5)+t(4)+t(3)+t(1)+t(2))-1/2*sin(t(1)+t(5))-1/2*sin(t(1)-t(5)))
oy-(-1/4*cos(-
t(5)+t(4)+t(3)+t(1)+t(2))+1/4*cos(t(5)+t(4)+t(3)+t(1)+t(2))-1/4*cos(-
t(5)-t(4)-t(3)+t(1)-t(2))+1/4*cos(t(5)-t(4)-t(3)+t(1)-t(2))+1/2*cos(t(1)-
t(5))+1/2*cos(t(1)+t(5)))
oz-(-1/2*cos(t(2)+t(3)+t(4)-t(5))+1/2*cos(t(2)+t(3)+t(4)+t(5)))
ax-(1/2*sin(-t(4)-t(3)+t(1)-t(2))-1/2*sin(t(4)+t(3)+t(1)+t(2)))
ay-(1/2*cos(t(4)+t(3)+t(1)+t(2))-1/2*cos(-t(4)-t(3)+t(1)-t(2)))
az-(cos(t(2)+t(3)+t(4)))
px-(340*sin(-t(4)-t(3)+t(1)-t(2))-
340*sin(t(4)+t(3)+t(1)+t(2))+225*cos(-t(3)+t(1)-
t(2))+225*cos(t(3)+t(1)+t(2))+225*cos(t(1)-t(2))+225*cos(t(1)+t(2)))
py-(340*cos(t(4)+t(3)+t(1)+t(2))-340*cos(-t(4)-t(3)+t(1)-
t(2))+225*sin(t(3)+t(1)+t(2))+225*sin(-t(3)+t(1)-
t(2))+225*sin(t(1)+t(2))+225*sin(t(1)-t(2)))
pz-(680*cos(t(2)+t(3)+t(4))+450*sin(t(2)+t(3))+450*sin(t(2)))];

90
Anexo G.- Programa para solucionar la dinámica por
algebra dual
% ANALISIS DINAMICO CON NUMEROS DUALES
% Ricardo Tapia Herrera
clc
clear all
syms E t1 t2 t3 t4 t5 w1 w2 w3 w4 w5 aa1 aa2 aa3 aa4 aa5
tic

ti_1=0; wi_1=0; ai_1=0;

tf_1=0; wf_1=0; af_1=0;
ti_2=0; wi_2=0; ai_2=0;
tf_2=0; wf_2=0; af_2=0;
ti_3=0; wi_3=0; ai_3=0;
tf_3=0; wf_3=0; af_3=0;
ti_4=0; wi_4=0; ai_4=0;
tf_4=0; wf_4=0; af_4=0;
ti_5=0; wi_5=0; ai_5=0;
tf_5=0; wf_5=0; af_5=0;

tiempo_f=10;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Matriz de tiempo para el generador de trayectorias

L=[1 0 0 0 0 0;
0 1 0 0 0 0;
0 0 1 0 0 0;
0 0 0 tiempo_f^3 tiempo_f^4 tiempo_f^5;
0 0 0 3*tiempo_f^2 4*tiempo_f^3 5*tiempo_f^4;
0 0 0 6*tiempo_f 12*tiempo_f^2 20*tiempo_f^3;];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%GENERADOR ARTICULACION 1
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
C=[ti_1
wi_1
ai_1/2
tf_1-ti_1-wi_1*tiempo_f-(ai_1/2)*tiempo_f^2
wf_1-wi_1-ai_1*tiempo_f
af_1-ai_1];

91
%Calculo de lo coeficientes Cn
Cn=inv(L)*C;
c0=Cn(1,1); c1=Cn(2,1); c2=Cn(3,1); c3=Cn(4,1); c4=Cn(5,1); c5=Cn(6,1);

for tiempo=0:tiempo_f
angulos_1(tiempo+1,1)=c0 + c1*tiempo + c2*tiempo^2 + c3*tiempo^3 +
c4*tiempo^4 + c5*tiempo^5;
vel_ang_1(tiempo+1,1)=c1 + 2*c2*tiempo + 3*c3*tiempo^2 +
4*c4*tiempo^3 + 5*c5*tiempo^4;
ace_ang_1(tiempo+1,1)=2*c2 + 6*c3*tiempo + 12*c4*tiempo^2 +
20*c5*tiempo^3;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%GENERADOR ARTICULACION 2
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
D=[ti_2
wi_2
ai_2/2
tf_2-ti_2-wi_2*tiempo_f-(ai_2/2)*tiempo_f^2
wf_2-wi_2-ai_2*tiempo_f
af_2-ai_2];
%Calculo de lo coeficientes Dn
Dn=inv(L)*D;

d0=Dn(1,1); d1=Dn(2,1); d2=Dn(3,1); d3=Dn(4,1); d4=Dn(5,1); d5=Dn(6,1);

for tiempo=0:tiempo_f
angulos_2(tiempo+1,1)=d0 + d1*tiempo + d2*tiempo^2 + d3*tiempo^3 +
d4*tiempo^4 + d5*tiempo^5;
vel_ang_2(tiempo+1,1)=d1 + 2*d2*tiempo + 3*d3*tiempo^2 +
4*d4*tiempo^3 + 5*d5*tiempo^4;
ace_ang_2(tiempo+1,1)=2*d2 + 6*d3*tiempo + 12*d4*tiempo^2 +
20*d5*tiempo^3;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%GENERADOR ARTICULACION 3
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
E=[ti_3
wi_3
ai_3/2

92
 tf_3-ti_3-wi_3*tiempo_f-(ai_3/2)*tiempo_f^2
wf_3-wi_3-ai_3*tiempo_f
af_3-ai_3];
%Calculo de lo coeficientes En
En=inv(L)*E;

e0=En(1,1); e1=En(2,1); e2=En(3,1); e3=En(4,1); e4=En(5,1); e5=En(6,1);

for tiempo=0:tiempo_f
angulos_3(tiempo+1,1)=e0 + e1*tiempo + e2*tiempo^2 + e3*tiempo^3 +
e4*tiempo^4 + e5*tiempo^5;
vel_ang_3(tiempo+1,1)=e1 + 2*e2*tiempo + 3*e3*tiempo^2 +
4*e4*tiempo^3 + 5*e5*tiempo^4;
ace_ang_3(tiempo+1,1)=2*e2 + 6*e3*tiempo + 12*e4*tiempo^2 +
20*e5*tiempo^3;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%GENERADOR ARTICULACION 4
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
F=[ti_4
wi_4
ai_4/2
tf_4-ti_4-wi_4*tiempo_f-(ai_4/2)*tiempo_f^2
wf_4-wi_4-ai_4*tiempo_f
af_4-ai_4];
%Calculo de lo coeficientes Fn
Fn=inv(L)*F;

f0=Fn(1,1); f1=Fn(2,1); f2=Fn(3,1); f3=Fn(4,1); f4=Fn(5,1); f5=Fn(6,1);

for tiempo=0:tiempo_f
angulos_4(tiempo+1,1)=f0 + f1*tiempo + f2*tiempo^2 + f3*tiempo^3 +
f4*tiempo^4 + f5*tiempo^5;
vel_ang_4(tiempo+1,1)=f1 + 2*f2*tiempo + 3*f3*tiempo^2 +
4*f4*tiempo^3 + 5*f5*tiempo^4;
ace_ang_4(tiempo+1,1)=2*f2 + 6*f3*tiempo + 12*f4*tiempo^2 +
20*f5*tiempo^3;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%GENERADOR ARTICULACION 5

93
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
G=[ti_5
wi_5
ai_5/2
tf_5-ti_5-wi_5*tiempo_f-(ai_5/2)*tiempo_f^2
wf_5-wi_5-ai_5*tiempo_f
af_5-ai_5];
%Calculo de lo coeficientes Gn
Gn=inv(L)*G;

g0=Gn(1,1); g1=Gn(2,1); g2=Gn(3,1); g3=Gn(4,1); g4=Gn(5,1); g5=Gn(6,1);

for tiempo=0:tiempo_f
angulos_5(tiempo+1,1)=g0 + g1*tiempo + g2*tiempo^2 + g3*tiempo^3 +
g4*tiempo^4 + g5*tiempo^5;
vel_ang_5(tiempo+1,1)=g1 + 2*g2*tiempo + 3*g3*tiempo^2 +
4*g4*tiempo^3 + 5*g5*tiempo^4;
ace_ang_5(tiempo+1,1)=2*g2 + 6*g3*tiempo + 12*g4*tiempo^2 +
20*g5*tiempo^3;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% PROPIEDADES ESLABON 1
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
x1=-.11136;
y1=-.03464;
z1=0;
Ixx1=0.43727255323; Ixy1=-0.055516687; Ixz1=0.00002317469;
Iyx1=Ixy1; Iyy1=0.74386897156; Iyz1=0.00000123833;
Izx1=Ixz1; Izy1=Iyz1; Izz1=1.042139809

m1=22.085;
r11=[x1;y1;z1;1];
% Radio de giro
k1x=Ixx1/m1; k1y=Iyy1/m1; k1z=Izz1/m1;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% PROPIEDADES ESLABON 2
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%desde el sistema i
x2=-.2175;
y2=-.02757;

94
z2=0;
Ixx2=0.06575521538; Ixy2=-0.083668376; Ixz2=0.0000911082;
Iyx2=Ixy2; Iyy2=0.82316593214; Iyz2=0.0000465806;
Izx2=Ixz2; Izy2=Iyz2; Izz2=0.83687356747;

m2=10.94;
r22=[x2;y2;z2;1];
% Radio de giro
k2x=Ixx2/m2; k2y=Iyy2/m2; k2z=Izz2/m2;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% PROPIEDADES ESLABON 3
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%desde el sistema i
x3=-.2075;
y3=-.0008;
z3=.00052;
Ixx3=0.03418690464; Ixy3=-0.045107043; Ixz3=0.00194510992;
Iyx3=Ixy3; Iyy3=0.71157307907; Iyz3=0.00000031207;
Izx3=Ixz3; Izy3=Iyz3; Izz3=0.69911249196;

m3=8.05;
r33=[x3;y3;z3;1];
% Radio de giro
k3x=Ixx3/m3; k3y=Iyy3/m3; k3z=Izz3/m3;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% PROPIEDADES ESLABON 4
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
x4=0.00013;
y4=-0.00152;
z4=.15446;
Ixx4=0.501473; Ixy4=0.0000002; Ixz4=-0.000505;
Iyx4=Ixy4; Iyy4=0.494986; Iyz4=0.000209;
Izx4=Ixz4; Izy4=Iyz4; Izz4=0.01765;

m4=10.58; %KG
r44=[x4;y4;z4;1];
% Radio de giro
k4x=Ixx4/m4; k4y=Iyy4/m4; k4z=Izz4/m4;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% PROPIEDADES ESLABON 5

95
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%desde sistema i
x5=0.00097;
y5=0.00607;
z5=-0.173;
Ixx5=0.171885; Ixy5=0.000029; Ixz5=-0.00085;
Iyx5=Ixy5; Iyy5=0.1712118; Iyz5=-0.005214;
Izx5=Ixz5; Izy5=Iyz5; Izz5=0.009164;

m5=4.98;
r55=[x5;y5;z5;1];
% Radio de giro
k5x=Ixx5/m5; k5y=Iyy5/m5; k5z=Izz5/m5;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%
% PRIMEROS MOMENTO DE INERCIA
S1=m1*[0, -z1, y1;
z1, 0, -x1;
-y1, x1, 0;];

S2=m2*[0, -z2, y2;

z2, 0, -x2;
-y2, x2, 0;];

S3=m3*[0, -z3, y3;

z3, 0, -x3;
-y3, x3, 0;];

S4=m4*[0, -z4, y4;

z4, 0, -x4;
-y4, x4, 0;];

S5=m5*[0, -z5, y5;

z5, 0, -x5;
-y5, x5, 0;];
%% SEGUNDOS MOMENTOS DE INERCIA
J1=m1*[k1x, 0, 0;
0, k1y, 0;
0, 0, k1z];

J2=m2*[k2x, 0, 0;

96
 0, k2y, 0;
0, 0, k2z];

J3=m3*[k3x, 0, 0;
0, k3y, 0;
0, 0, k3z];

J4=m4*[k4x, 0, 0;
0, k4y, 0;
0, 0, k4z];

J5=m5*[k5x, 0, 0;
0, k5y, 0;
0, 0, k5z];
%% Velocidades duales en terminos del final de cada eslabon
VP1 =[0
w1
0];

VP2=[sin(t2)*w1
cos(t2)*w1
w2];

VP3=[sin(t3+t2)*w1
cos(t3+t2)*w1
w2+w3];

VP4=[sin(t4+t3+t2)*w1
-w2-w3-w4
cos(t4+t3+t2)*w1];

VP5=[1/2*w1*sin(t2+t3-t5+t4)+1/2*w1*sin(t2+t3+t5+t4)-sin(t5)*w2-
sin(t5)*w3-sin(t5)*w4
1/2*w1*cos(t2+t3+t5+t4)-1/2*w1*cos(t2+t3-t5+t4)-cos(t5)*w2-
cos(t5)*w3-cos(t5)*w4
cos(t4+t3+t2)*w1+w5];

VD1=[0
0
0];

97
VD2=[0
9/20*w2
-9/20*cos(t2)*w1];

VD3=[9/20*sin(t3)*w2
9/20*w2+9/20*w2*cos(t3)+9/20*w3
-9/20*w1*cos(t3+t2)-9/20*cos(t2)*w1];

VD4=[9/20*w2*sin(t4)+9/20*w2*sin(t4+t3)+9/20*sin(t4)*w3
9/20*w1*cos(t3+t2)+9/20*w1*cos(t2)
9/20*w2*cos(t4)+9/20*w2*cos(t4+t3)+9/20*cos(t4)*w3];

VD5=[17/50*w1*cos(t2+t3+t5+t4)-17/50*w1*cos(t2+t3-t5+t4)-9/40*w1*sin(t2-
t5+t3)+9/40*w1*sin(t2+t5+t3)+9/40*w1*sin(t5+t2)+9/40*w1*sin(t5-t2)-
17/25*w2*cos(t5)+9/40*w2*sin(t5+t4)+9/40*w2*sin(-t5+t4)+9/40*w2*sin(t3-
t5+t4)+9/40*w2*sin(t3+t5+t4)-
17/25*w3*cos(t5)+9/40*w3*sin(t5+t4)+9/40*w3*sin(-t5+t4)-17/25*cos(t5)*w4
-17/50*w1*sin(t2+t3-t5+t4)-
17/50*w1*sin(t2+t3+t5+t4)+9/40*w1*cos(t2+t5+t3)+9/40*w1*cos(t2-
t5+t3)+9/40*w1*cos(t5-t2)+9/40*w1*cos(t5+t2)+17/25*w2*sin(t5)-
9/40*w2*cos(-t5+t4)+9/40*w2*cos(t5+t4)+9/40*w2*cos(t3+t5+t4)-
9/40*w2*cos(t3-t5+t4)+17/25*w3*sin(t5)-9/40*w3*cos(-
t5+t4)+9/40*w3*cos(t5+t4)+17/25*sin(t5)*w4
9/20*w2*cos(t4)+9/20*w2*cos(t4+t3)+9/20*cos(t4)*w3];

%% CALCULO DEL MOMENTO DUAL SEPARANDO PARTE PRIMARIA Y PARTE DUAL

H=HP+E*HD
HP1=m1*VD1 - S1*VP1; HD1=S2*VD1 + J1*VP1;
HP2=m2*VD2 - S2*VP2; HD2=S2*VD2 + J2*VP2;
HP3=m3*VD3 - S3*VP3; HD3=S3*VD3 + J3*VP3;
HP4=m4*VD4 - S4*VP4; HD4=S4*VD4 + J4*VP4;
HP5=m5*VD5 - S5*VP5; HD5=S5*VD5 + J5*VP5;

%% DERIVADAS DE LAS VELOCIDADES DUALES

d_HP1=diff(HP1,t1)*w1+diff(HP1,w1)*aa1;
d_HP2=diff(HP2,t1)*w1+diff(HP2,w1)*aa1 +
diff(HP2,t2)*w2+diff(HP2,w2)*aa2;
d_HP3=diff(HP3,t1)*w1+diff(HP3,w1)*aa1 + diff(HP3,t2)*w2+diff(HP3,w2)*aa2
+ diff(HP3,t3)*w3+diff(HP3,w3)*aa3;
d_HP4=diff(HP4,t1)*w1+diff(HP4,w1)*aa1 + diff(HP4,t2)*w2+diff(HP4,w2)*aa2
+ diff(HP4,t3)*w3+diff(HP4,w3)*aa3 + diff(HP4,t4)*w4+diff(HP4,w4)*aa4;
d_HP5=diff(HP5,t1)*w1+diff(HP5,w1)*aa1 + diff(HP5,t2)*w2+diff(HP5,w2)*aa2
+ diff(HP5,t3)*w3+diff(HP5,w3)*aa3 + diff(HP5,t4)*w4+diff(HP5,w4)*aa4 +
diff(HP5,t5)*w5+diff(HP5,w5)*aa5;

98
d_HD1=diff(HD1,t1)*w1+diff(HD1,w1)*aa1;
d_HD2=diff(HD2,t1)*w1+diff(HD2,w1)*aa1 +
diff(HD2,t2)*w2+diff(HD2,w2)*aa2;
d_HD3=diff(HD3,t1)*w1+diff(HD3,w1)*aa1 + diff(HD3,t2)*w2+diff(HD3,w2)*aa2
+ diff(HD3,t3)*w3+diff(HD3,w3)*aa3;
d_HD4=diff(HD4,t1)*w1+diff(HD4,w1)*aa1 + diff(HD4,t2)*w2+diff(HD4,w2)*aa2
+ diff(HD4,t3)*w3+diff(HD4,w3)*aa3 + diff(HD4,t4)*w4+diff(HD4,w4)*aa4;
d_HD5=diff(HD5,t1)*w1+diff(HD5,w1)*aa1 + diff(HD5,t2)*w2+diff(HD5,w2)*aa2
+ diff(HD5,t3)*w3+diff(HD5,w3)*aa3 + diff(HD5,t4)*w4+diff(HD5,w4)*aa4 +
diff(HD5,t5)*w5+diff(HD5,w5)*aa5;

for r=1:(tiempo_f+1)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% DATOS DE ENTRADA
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Angulos GRADOS Angulos en radianes
j1=angulos_1(r,1); t1=deg2rad(j1+90);
j2=angulos_2(r,1); t2=deg2rad(j2);
j3=angulos_3(r,1); t3=deg2rad(j3);
j4=angulos_4(r,1); t4=deg2rad(j4-90);
j5=angulos_5(r,1); t5=deg2rad(j5);

%Velocidades angulares RAD/SEG Aceleraciones angulares

w1=vel_ang_1(r,1);
w2=vel_ang_2(r,1);
w3=vel_ang_3(r,1);
w4=vel_ang_4(r,1);
w5=vel_ang_5(r,1);
aa1=ace_ang_1(r,1);
aa2=ace_ang_2(r,1);
aa3=ace_ang_3(r,1);
aa4=ace_ang_4(r,1);
aa5=ace_ang_5(r,1);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% MATRICES DE ROTACION DUALES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Patametros de Denavith y Hartemberg
alpha1=90; a1=0; S1=0;
alpha2=0; a2=.450; S2=0;
alpha3=0; a3=.450; S3=0;
alpha4=-90; a4=0; S4=0;
alpha5=0; a5=0; S5=.680;

P1=[cos(t1), -cosd(alpha1)*sin(t1), sind(alpha1)*sin(t1);

sin(t1), cosd(alpha1)*cos(t1), -sind(alpha1)*cos(t1);

99
 0, sind(alpha1), cosd(alpha1)];

P2=[cos(t2), -cosd(alpha2)*sin(t2), sind(alpha2)*sin(t2);

sin(t2), cosd(alpha2)*cos(t2), -sind(alpha2)*cos(t2);
0, sind(alpha2), cosd(alpha2)];

P3=[cos(t3), -cosd(alpha3)*sin(t3), sind(alpha3)*sin(t3);

sin(t3), cosd(alpha3)*cos(t3), -sind(alpha3)*cos(t3);
0, sind(alpha3), cosd(alpha3)];

P4=[cos(t4), -cosd(alpha4)*sin(t4), sind(alpha4)*sin(t4);

sin(t4), cosd(alpha4)*cos(t4), -sind(alpha4)*cos(t4);
0, sind(alpha4), cosd(alpha4)];

P5=[cos(t5), -cosd(alpha5)*sin(t5), sind(alpha5)*sin(t5);

sin(t5), cosd(alpha5)*cos(t5), -sind(alpha5)*cos(t5);
0, sind(alpha5), cosd(alpha5)];

D1=[-S1*sin(t1), a1*sind(alpha1)*sin(t1)-S1*cosd(alpha1)*cos(t1),
a1*cosd(alpha1)*sin(t1)+S1*sind(alpha1)*cos(t1);
S1*cos(t1), -a1*sind(alpha1)*cos(t1)-S1*cosd(alpha1)*sin(t1), -
a1*cosd(alpha1)*cos(t1)+S1*sind(alpha1)*sin(t1);
0, a1*cosd(alpha1), -
a1*sind(alpha1)];

D2=[-S2*sin(t2), a2*sind(alpha2)*sin(t2)-S2*cosd(alpha2)*cos(t2),
a2*cosd(alpha2)*sin(t2)+S2*sind(alpha2)*cos(t2);
S2*cos(t2), -a2*sind(alpha2)*cos(t2)-S2*cosd(alpha2)*sin(t2), -
a2*cosd(alpha2)*cos(t2)+S2*sind(alpha2)*sin(t2);
0, a2*cosd(alpha2), -
a2*sind(alpha2)];

D3=[-S3*sin(t3), a3*sind(alpha3)*sin(t3)-S3*cosd(alpha3)*cos(t3),
a3*cosd(alpha3)*sin(t3)+S3*sind(alpha3)*cos(t3);
S3*cos(t3), -a3*sind(alpha3)*cos(t3)-S3*cosd(alpha3)*sin(t3), -
a3*cosd(alpha3)*cos(t3)+S3*sind(alpha3)*sin(t3);
0, a3*cosd(alpha3), -
a3*sind(alpha3)];

D4=[-S4*sin(t4), a4*sind(alpha4)*sin(t4)-S4*cosd(alpha4)*cos(t4),
a4*cosd(alpha4)*sin(t4)+S4*sind(alpha4)*cos(t4);

100
 S4*cos(t4), -a4*sind(alpha4)*cos(t4)-S4*cosd(alpha4)*sin(t4), -
a4*cosd(alpha4)*cos(t4)+S4*sind(alpha4)*sin(t4);
0, a4*cosd(alpha4), -
a4*sind(alpha4)];

D5=[-S5*sin(t5), a5*sind(alpha5)*sin(t5)-S5*cosd(alpha5)*cos(t5),
a5*cosd(alpha5)*sin(t5)+S5*sind(alpha5)*cos(t5);
S5*cos(t5), -a5*sind(alpha5)*cos(t5)-S5*cosd(alpha5)*sin(t5), -
a5*cosd(alpha5)*cos(t5)+S5*sind(alpha5)*sin(t5);
0, a5*cosd(alpha5), -
a5*sind(alpha5)];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% ECUACIONES duales DE EULER
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
f1=eval(d_HP1+cross(VP1,HP1));
f2=eval(d_HP2+cross(VP2,HP2));
f3=eval(d_HP3+cross(VP3,HP3));
f4=eval(d_HP4+cross(VP4,HP4));
f5=eval(d_HP5+cross(VP5,HP5));

t_1=eval(d_HD1+cross(VP1,HD1)+cross(VD1,HP1));
t_2=eval(d_HD2+cross(VP2,HD2)+cross(VD2,HP2));
t_3=eval(d_HD3+cross(VP3,HD3)+cross(VD3,HP3));
t_4=eval(d_HD4+cross(VP4,HD4)+cross(VD4,HP4));
t_5=eval(d_HD5+cross(VP5,HD5)+cross(VD5,HP5));
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% ANALISIS DE FUERZAS Y TORQUES EN LAS ARTICULACIONES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
F5=(P5)*(f5);
F4=(P4)*(F5+f4);
F3=(P3)*(F4+f3);
F2=(P2)*(F3+f2);
F1=(P1)*(F2+f1);

T5=(P5)*(t_5-D5'*F5);
T4=(P4)*(t_4+T5-D4'*F4);
T3=(P3)*(t_3+T4-D3'*F3);
T2=(P2)*(t_2+T3-D2'*F2);
T1=(P1)*(t_1+T2-D1'*F1);

%TORQUE EN EL EJE DE GIRO DEL ESLABON

101
 torques_1(r,1)=T1(3,1);
torques_2(r,1)=T2(3,1);
torques_3(r,1)=T3(3,1);
torques_4(r,1)=T4(3,1);
torques_5(r,1)=T5(3,1);

end
toc

torques_1
torques_2
torques_3
torques_4
torques_5

subplot(1,5,1); plot(torques_1)
subplot(1,5,2); plot(torques_2)
subplot(1,5,3); plot(torques_3)
subplot(1,5,4); plot(torques_4)
subplot(1,5,5); plot(torques_5)

102
Anexo H.- Programa para solucionar la dinámica por
matrices de transformación homogénea
% Formulación de%Langrage-Euler
% 17-julio-2009
% Ing. Ricardo Tapia Herrera
clc
clear all
tic

ti_1=0; wi_1=0; ai_1=0;

tf_1=0; wf_1=0; af_1=0;
ti_2=0; wi_2=0; ai_2=0;
tf_2=0; wf_2=0; af_2=0;
ti_3=0; wi_3=0; ai_3=0;
tf_3=0; wf_3=0; af_3=0;
ti_4=0; wi_4=0; ai_4=0;
tf_4=0; wf_4=0; af_4=0;
ti_5=0; wi_5=0; ai_5=0;
tf_5=0; wf_5=0; af_5=0;

tiempo_f=10;

L=[1 0 0 0 0 0;
0 1 0 0 0 0;
0 0 1 0 0 0;
0 0 0 tiempo_f^3 tiempo_f^4 tiempo_f^5;
0 0 0 3*tiempo_f^2 4*tiempo_f^3 5*tiempo_f^4;
0 0 0 6*tiempo_f 12*tiempo_f^2 20*tiempo_f^3;];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%
%GENERADOR ARTICULACION 1
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%
C=[ti_1
wi_1
ai_1/2
tf_1-ti_1-wi_1*tiempo_f-(ai_1/2)*tiempo_f^2
wf_1-wi_1-ai_1*tiempo_f
af_1-ai_1];

103
%Calculo de lo coeficientes Cn
Cn=inv(L)*C;

c0=Cn(1,1);
c1=Cn(2,1);
c2=Cn(3,1);
c3=Cn(4,1);
c4=Cn(5,1);
c5=Cn(6,1);

for tiempo=0:tiempo_f
angulos_1(tiempo+1,1)=c0 + c1*tiempo + c2*tiempo^2 + c3*tiempo^3 +
c4*tiempo^4 + c5*tiempo^5;
vel_ang_1(tiempo+1,1)=c1 + 2*c2*tiempo + 3*c3*tiempo^2 +
4*c4*tiempo^3 + 5*c5*tiempo^4;
ace_ang_1(tiempo+1,1)=2*c2 + 6*c3*tiempo + 12*c4*tiempo^2 +
20*c5*tiempo^3;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%GENERADOR ARTICULACION 2
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
D=[ti_2
wi_2
ai_2/2
tf_2-ti_2-wi_2*tiempo_f-(ai_2/2)*tiempo_f^2
wf_2-wi_2-ai_2*tiempo_f
af_2-ai_2];

Dn=inv(L)*D;

d0=Dn(1,1);
d1=Dn(2,1);
d2=Dn(3,1);
d3=Dn(4,1);
d4=Dn(5,1);
d5=Dn(6,1);

for tiempo=0:tiempo_f
angulos_2(tiempo+1,1)=d0 + d1*tiempo + d2*tiempo^2 + d3*tiempo^3 +
d4*tiempo^4 + d5*tiempo^5;

104
 vel_ang_2(tiempo+1,1)=d1 + 2*d2*tiempo + 3*d3*tiempo^2 +
4*d4*tiempo^3 + 5*d5*tiempo^4;
ace_ang_2(tiempo+1,1)=2*d2 + 6*d3*tiempo + 12*d4*tiempo^2 +
20*d5*tiempo^3;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%GENERADOR ARTICULACION 3
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
E=[ti_3
wi_3
ai_3/2
tf_3-ti_3-wi_3*tiempo_f-(ai_3/2)*tiempo_f^2
wf_3-wi_3-ai_3*tiempo_f
af_3-ai_3];

En=inv(L)*E;

e0=En(1,1);
e1=En(2,1);
e2=En(3,1);
e3=En(4,1);
e4=En(5,1);
e5=En(6,1);

for tiempo=0:tiempo_f
angulos_3(tiempo+1,1)=e0 + e1*tiempo + e2*tiempo^2 + e3*tiempo^3 +
e4*tiempo^4 + e5*tiempo^5;
vel_ang_3(tiempo+1,1)=e1 + 2*e2*tiempo + 3*e3*tiempo^2 +
4*e4*tiempo^3 + 5*e5*tiempo^4;
ace_ang_3(tiempo+1,1)=2*e2 + 6*e3*tiempo + 12*e4*tiempo^2 +
20*e5*tiempo^3;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%GENERADOR ARTICULACION 4
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
F=[ti_4
wi_4
ai_4/2
tf_4-ti_4-wi_4*tiempo_f-(ai_4/2)*tiempo_f^2
wf_4-wi_4-ai_4*tiempo_f

105
 af_4-ai_4];

Fn=inv(L)*F;

f0=Fn(1,1);
f1=Fn(2,1);
f2=Fn(3,1);
f3=Fn(4,1);
f4=Fn(5,1);
f5=Fn(6,1);

for tiempo=0:tiempo_f
angulos_4(tiempo+1,1)=f0 + f1*tiempo + f2*tiempo^2 + f3*tiempo^3 +
f4*tiempo^4 + f5*tiempo^5;
vel_ang_4(tiempo+1,1)=f1 + 2*f2*tiempo + 3*f3*tiempo^2 +
4*f4*tiempo^3 + 5*f5*tiempo^4;
ace_ang_4(tiempo+1,1)=2*f2 + 6*f3*tiempo + 12*f4*tiempo^2 +
20*f5*tiempo^3;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%GENERADOR ARTICULACION 5
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
G=[ti_5
wi_5
ai_5/2
tf_5-ti_5-wi_5*tiempo_f-(ai_5/2)*tiempo_f^2
wf_5-wi_5-ai_5*tiempo_f
af_5-ai_5];

Gn=inv(L)*G;

g0=Gn(1,1);
g1=Gn(2,1);
g2=Gn(3,1);
g3=Gn(4,1);
g4=Gn(5,1);
g5=Gn(6,1);

for tiempo=0:tiempo_f

106
 angulos_5(tiempo+1,1)=g0 + g1*tiempo + g2*tiempo^2 + g3*tiempo^3 +
g4*tiempo^4 + g5*tiempo^5;
vel_ang_5(tiempo+1,1)=g1 + 2*g2*tiempo + 3*g3*tiempo^2 +
4*g4*tiempo^3 + 5*g5*tiempo^4;
ace_ang_5(tiempo+1,1)=2*g2 + 6*g3*tiempo + 12*g4*tiempo^2 +
20*g5*tiempo^3;
end

for r=1:(tiempo_f+1)
%%%%%Angulos GRADOS
j1=angulos_1(r,1);
j2=angulos_2(r,1);
j3=angulos_3(r,1);
j4=angulos_4(r,1);
j5=angulos_5(r,1);
t1=deg2rad(j1+90);
t2=deg2rad(j2);
t3=deg2rad(j3);
t4=deg2rad(j4-90);
t5=deg2rad(j5);
%Velocidades angulares RAD/SEG
w1=vel_ang_1(r,1);
w2=vel_ang_2(r,1);
w3=vel_ang_3(r,1);
w4=vel_ang_4(r,1);
w5=vel_ang_5(r,1);
%Aceleraciones angulares RAD/SEG^2
aa1=ace_ang_1(r,1);
aa2=ace_ang_2(r,1);
aa3=ace_ang_3(r,1);
aa4=ace_ang_4(r,1);
aa5=ace_ang_5(r,1);

w=[w1;w2;w3;w4;w5];
aa=[aa1;aa2;aa3;aa4;aa5];

%% MATRIZ 0A1
A1=[cos(t1), 0, sin(t1), 0;
sin(t1), 0, -cos(t1), 0;
0, 1, 0, 0;
0, 0, 0, 1];

107
%% MATRIZ 1A2
A2=[cos(t2), -sin(t2), 0, .450*cos(t2);
sin(t2), cos(t2), 0, .450*sin(t2);
0, 0, 1, 0;
0, 0, 0, 1];

%% MATRIZ 2A3
A3=[cos(t3), -sin(t3), 0, .450*cos(t3);
sin(t3), cos(t3), 0, .450*sin(t3);
0, 0, 1, 0;
0, 0, 0, 1];

%% MATRIZ 3A4
A4=[cos(t4), 0, -sin(t4), 0;
sin(t4), 0, cos(t4), 0;
0, -1, 0, 0;
0, 0, 0, 1];

%% MATRIZ 4A5
A5=[cos(t5), -sin(t5), 0, 0;
sin(t5), cos(t5), 0, 0;
0, 0, 1, .680;
0, 0, 0, 1];

Q=[0 -1 0 0;
1 0 0 0;
0 0 0 0;
0 0 0 0];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% PROPIEDADES ESLABON 1
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%desde el sistema i
x1=-.11136;
y1=-.03464;
z1=0;
Ixx1=0.43727255323; Ixy1=-0.055516687; Ixz1=0.00002317469;

108
Iyx1=Ixy1; Iyy1=0.74386897156; Iyz1=0.00000123833;
Izx1=Ixz1; Izy1=Iyz1; Izz1=1.042139809

m1=22.085;
r11=[x1;y1;z1;1];
% Radio de giro
k1x=Ixx1/m1; k1y=Iyy1/m1; k1z=Izz1/m1;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% PROPIEDADES ESLABON 2
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%desde el sistema i
x2=-.2175;
y2=-.02757;
z2=0;
Ixx2=0.06575521538; Ixy2=-0.083668376; Ixz2=0.0000911082;
Iyx2=Ixy2; Iyy2=0.82316593214; Iyz2=0.0000465806;
Izx2=Ixz2; Izy2=Iyz2; Izz2=0.83687356747;

m2=10.94;
r22=[x2;y2;z2;1];
% Radio de giro
k2x=Ixx2/m2; k2y=Iyy2/m2; k2z=Izz2/m2;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% PROPIEDADES ESLABON 3
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%desde el sistema i
x3=-.2075;
y3=-.0008;
z3=.00052;
Ixx3=0.03418690464; Ixy3=-0.045107043; Ixz3=0.00194510992;
Iyx3=Ixy3; Iyy3=0.71157307907; Iyz3=0.00000031207;
Izx3=Ixz3; Izy3=Iyz3; Izz3=0.69911249196;

m3=8.05;
r33=[x3;y3;z3;1];
% Radio de giro
k3x=Ixx3/m3; k3y=Iyy3/m3; k3z=Izz3/m3;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% PROPIEDADES ESLABON 4
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

109
x4=0.00013;
y4=-0.00152;
z4=.15446;
Ixx4=0.501473; Ixy4=0.0000002; Ixz4=-0.000505;
Iyx4=Ixy4; Iyy4=0.494986; Iyz4=0.000209;
Izx4=Ixz4; Izy4=Iyz4; Izz4=0.01765;

m4=10.58; %KG
r44=[x4;y4;z4;1];
% Radio de giro
k4x=Ixx4/m4; k4y=Iyy4/m4; k4z=Izz4/m4;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% PROPIEDADES ESLABON 5
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%desde sistema i
x5=0.00097;
y5=0.00607;
z5=-0.173;
Ixx5=0.171885; Ixy5=0.000029; Ixz5=-0.00085;
Iyx5=Ixy5; Iyy5=0.1712118; Iyz5=-0.005214;
Izx5=Ixz5; Izy5=Iyz5; Izz5=0.009164;

m5=4.98;
r55=[x5;y5;z5;1];
% Radio de giro
k5x=Ixx5/m5; k5y=Iyy5/m5; k5z=Izz5/m5;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% TENSORES DE INERCIA
J1=[(-Ixx1+Iyy1+Izz1)/2, Ixy1, Ixz1, m1*x1;
Iyx1, (Ixx1-Iyy1+Izz1)/2, Iyz1, m1*y1;
Izx1, Izy1, (Ixx1+Iyy1-Izz1)/2, m1*z1;
m1*x1, m1*y1, m1*z1, m1];

J2=[(-Ixx2+Iyy2+Izz2)/2, Ixy2, Ixz2, m2*x2;

Iyx2, (Ixx2-Iyy2+Izz2)/2, Iyz2, m2*y2;
Izx2, Izy2, (Ixx2+Iyy2-Izz2)/2, m2*z2;
m2*x2, m2*y2, m2*z2, m2];

J3=[(-Ixx3+Iyy3+Izz3)/2, Ixy3, Ixz3, m3*x3;

Iyx3, (Ixx3-Iyy3+Izz3)/2, Iyz3, m3*y3;

110
 Izx3, Izy3, (Ixx3+Iyy3-Izz3)/2, m3*z3;
m3*x3, m3*y3, m3*z3, m3];

J4=[(-Ixx4+Iyy4+Izz4)/2, Ixy4, Ixz4, m4*x4;

Iyx4, (Ixx4-Iyy4+Izz4)/2, Iyz4, m4*y4;
Izx4, Izy4, (Ixx4+Iyy4-Izz4)/2, m4*z4;
m4*x4, m4*y4, m4*z4, m4];

J5=[(-Ixx5+Iyy5+Izz5)/2, Ixy5, Ixz5, m5*x5;

Iyx5, (Ixx5-Iyy5+Izz5)/2, Iyz5, m5*y5;
Izx5, Izy5, (Ixx5+Iyy5-Izz5)/2, m5*z5;
m5*x5, m5*y5, m5*z5, m5];

%DERIVADAS (U) DE LAS MATRICES DE ROTACION

U11=Q*A1;U111=Q*U11;

U21=Q*A1*A2; U211=Q*Q*A1*A2;
U212=Q*A1*Q*A2; U22=A1*Q*A2;
U221=Q*A1*Q*A2; U222=A1*Q*Q*A2;

U31=Q*A1*A2*A3; U311=Q*Q*A1*A2*A3;
U312=Q*A1*Q*A2*A3; U313=Q*A1*A2*Q*A3;
U32=A1*Q*A2*A3; U321=Q*A1*Q*A2*A3;
U322=A1*Q*Q*A2*A3; U323=A1*Q*A2*Q*A3;
U33=A1*A2*Q*A3; U331=Q*A1*A2*Q*A3;
U332=A1*Q*A2*Q*A3; U333=A1*A2*Q*Q*A3;

U41=Q*A1*A2*A3*A4;
U412=Q*A1*Q*A2*A3*A4;
U414=Q*A1*A2*A3*Q*A4;
U421=Q*A1*Q*A2*A3*A4;
U423=A1*Q*A2*Q*A3*A4;
U43=A1*A2*Q*A3*A4;
U432=A1*Q*A2*Q*A3*A4;
U434=A1*A2*Q*A3*Q*A4;
U441=Q*A1*A2*A3*Q*A4;
U443=A1*A2*Q*A3*Q*A4;
U411=Q*Q*A1*A2*A3*A4;
U413=Q*A1*A2*Q*A3*A4;
U42=A1*Q*A2*A3*A4;
U422=A1*Q*Q*A2*A3*A4;
U424=A1*Q*A2*A3*Q*A4;
U431=Q*A1*A2*Q*A3*A4;
U433=A1*A2*Q*Q*A3*A4;
U44=A1*A2*A3*Q*A4;
U442=A1*Q*A2*A3*Q*A4;
U444=A1*A2*A3*Q*Q*A4;

U51=Q*A1*A2*A3*A4*A5; U511=Q*Q*A1*A2*A3*A4*A5;
U512=Q*A1*Q*A2*A3*A4*A5; U513=Q*A1*A2*Q*A3*A4*A5;

111
U514=Q*A1*A2*A3*Q*A4*A5;
U52=A1*Q*A2*A3*A4*A5;
U522=A1*Q*Q*A2*A3*A4*A5;
U524=A1*Q*A2*A3*Q*A4*A5;
U53=A1*A2*Q*A3*A4*A5;
U532=A1*Q*A2*Q*A3*A4*A5;
U534=A1*A2*Q*A3*Q*A4*A5;
U515=Q*A1*A2*A3*A4*Q*A5;
U521=Q*A1*Q*A2*A3*A4*A5;
U523=A1*Q*A2*Q*A3*A4*A5;
U525=A1*Q*A2*A3*A4*Q*A5;
U531=Q*A1*A2*Q*A3*A4*A5;
U533=A1*A2*Q*Q*A3*A4*A5;
U535=A1*A2*Q*A3*A4*Q*A5;

U54=A1*A2*A3*Q*A4*A5; U541=Q*A1*A2*A3*Q*A4*A5;
U542=A1*Q*A2*A3*Q*A4*A5; U543=A1*A2*Q*A3*Q*A4*A5;
U544=A1*A2*A3*Q*Q*A4*A5; U545=A1*A2*A3*Q*A4*Q*A5;

U55=A1*A2*A3*A4*Q*A5; U551=Q*A1*A2*A3*A4*Q*A5;
U552=A1*Q*A2*A3*A4*Q*A5; U553=A1*A2*Q*A3*A4*Q*A5;
U554=A1*A2*A3*Q*A4*Q*A5; U555=A1*A2*A3*A4*Q*Q*A5;

%MATRIZ DE ACELERACION
MD(1,1)=trace(U11*J1*U11')+trace(U21*J2*U21')+trace(U31*J3*U31')+trace(U4
1*J4*U41')+trace(U51*J5*U51');
MD(1,2)=trace(U22*J2*U21')+trace(U32*J3*U31')+trace(U42*J4*U41')+trace(U5
2*J5*U51');
MD(1,3)=trace(U33*J3*U31')+trace(U43*J4*U41')+trace(U53*J5*U51');
MD(1,4)=trace(U44*J4*U41')+trace(U54*J5*U51');
MD(1,5)=trace(U55*J5*U51');

MD(2,1)=MD(1,2);
MD(2,2)=trace(U22*J2*U22')+trace(U32*J3*U32')+trace(U42*J4*U42')+trace(U5
2*J5*U52');
MD(2,3)=trace(U33*J3*U32')+trace(U43*J4*U42')+trace(U53*J5*U52');
MD(2,4)=trace(U44*J4*U42')+trace(U54*J5*U52');
MD(2,5)=trace(U55*J5*U52');

MD(3,1)=MD(1,3);
MD(3,2)=MD(2,3);
MD(3,3)=trace(U33*J3*U33')+trace(U43*J4*U43')+trace(U53*J5*U53');
MD(3,4)=trace(U44*J4*U43')+trace(U54*J5*U53');
MD(3,5)=trace(U55*J5*U53');

MD(4,1)=MD(1,4);
MD(4,2)=MD(2,4);
MD(4,3)=MD(3,4);

112
MD(4,4)=trace(U44*J4*U44')+trace(U54*J5*U54');
MD(4,5)=trace(U55*J5*U54');

MD(5,1)=MD(1,5);
MD(5,2)=MD(2,5);
MD(5,3)=MD(3,5);
MD(5,4)=MD(4,5);
MD(5,5)=trace(U55*J5*U55');

%terminos de coriolis y centrifugos

h111=trace(U111*J1*U11')+trace(U211*J2*U21')+trace(U311*J3*U31')+trace(U4
11*J4*U41')+trace(U511*J5*U51');
h112=trace(U212*J2*U21')+trace(U312*J3*U31')+trace(U412*J4*U41')+trace(U5
12*J5*U51');
h113=trace(U313*J3*U31')+trace(U413*J4*U41')+trace(U513*J5*U51');
h114=trace(U414*J4*U41')+trace(U514*J5*U51');
h115=trace(U515*J5*U51');

h121=h112;
h122=trace(U222*J2*U21')+trace(U322*J3*U31')+trace(U422*J4*U41')+trace(U5
22*J5*U51');
h123=trace(U323*J3*U31')+trace(U423*J4*U41')+trace(U523*J5*U51');
h124=trace(U424*J4*U41')+trace(U524*J5*U51');
h125=trace(U525*J5*U51');

h131=h113;
h132=h123;
h133=trace(U333*J3*U31')+trace(U433*J4*U41')+trace(U533*J5*U51');
h134=trace(U434*J4*U41')+trace(U534*J5*U51');
h135=trace(U535*J5*U51');

h141=h114;
h142=h124;
h143=h134;
h144=trace(U444*J4*U41')+trace(U544*J5*U51');
h145=trace(U545*J5*U51');

h151=h115;
h152=h125;
h153=h135;
h154=h145;

113
h155=trace(U555*J5*U51');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%H1
H1=[h111 h121 h131 h141 h151;
h112 h122 h132 h142 h152;
h113 h123 h133 h143 h153;
h114 h124 h134 h144 h154;
h115 h125 h135 h145 h155];
h1=w'*H1*w
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

h211=trace(U211*J2*U22')+trace(U311*J3*U32')+trace(U411*J4*U42')+trace(U5
11*J5*U52');
h212=trace(U212*J2*U22')+trace(U312*J3*U32')+trace(U412*J4*U42')+trace(U5
12*J5*U52');
h213=trace(U313*J3*U32')+trace(U413*J4*U42')+trace(U513*J5*U52');
h214=trace(U414*J4*U42')+trace(U514*J5*U52');
h215=trace(U515*J5*U52');

h221=h212;
h222=trace(U222*J2*U22')+trace(U322*J3*U32')+trace(U422*J4*U42')+trace(U5
22*J5*U52');
h223=trace(U323*J3*U32')+trace(U423*J4*U42')+trace(U523*J5*U52');
h224=trace(U424*J4*U42')+trace(U524*J5*U52');
h225=trace(U525*J5*U52');

h231=h213;
h232=h223;
h233=trace(U333*J3*U32')+trace(U433*J4*U42')+trace(U533*J5*U52');
h234=trace(U434*J4*U42')+trace(U534*J5*U52');
h235=trace(U535*J5*U52');

h241=h214;
h242=h224;
h243=h234;
h244=trace(U444*J4*U42')+trace(U544*J5*U52');
h245=trace(U545*J5*U52');

h251=h215;
h252=h225;
h253=h235;
h254=h245;

114
h255=trace(U555*J5*U52');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%H2
H2=[h211 h221 h231 h241 h251;
h212 h222 h232 h242 h252;
h213 h223 h233 h243 h253;
h214 h224 h234 h244 h254;
h215 h225 h235 h245 h255];
h2=w'*H2*w
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

h311=trace(U311*J3*U33')+trace(U411*J4*U43')+trace(U511*J5*U53');
h312=trace(U312*J3*U33')+trace(U412*J4*U43')+trace(U512*J5*U53');
h313=trace(U313*J3*U33')+trace(U413*J4*U43')+trace(U513*J5*U53');
h314=trace(U414*J4*U43')+trace(U514*J5*U53');
h315=trace(U515*J5*U53');

h321=h312;
h322=trace(U322*J3*U33')+trace(U422*J4*U43')+trace(U522*J5*U53');
h323=trace(U323*J3*U33')+trace(U423*J4*U43')+trace(U523*J5*U53');
h324=trace(U424*J4*U43')+trace(U524*J5*U53');
h325=trace(U525*J5*U53');

h331=h313;
h332=h323;
h333=trace(U333*J3*U33')+trace(U433*J4*U43')+trace(U533*J5*U53');
h334=trace(U434*J4*U43')+trace(U534*J5*U53');
h335=trace(U535*J5*U53');

h341=h314;
h342=h324;
h343=h334;
h344=trace(U444*J4*U43')+trace(U544*J5*U53');
h345=trace(U545*J5*U53');

h351=h315;
h352=h325;
h353=h335;
h354=h345;
h355=trace(U555*J5*U53');

115
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%H3
H3=[h311 h321 h331 h341 h351;
h312 h322 h332 h342 h352;
h313 h323 h333 h343 h353;
h314 h324 h334 h344 h354;
h315 h325 h335 h345 h355];
h3=w'*H3*w
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
h415=trace(U515*J5*U54');
h425=trace(U525*J5*U54');
h435=trace(U535*J5*U54');
h445=trace(U545*J5*U54');
h455=trace(U555*J5*U54');

h414=trace(U414*J4*U44')+trace(U514*J5*U54');
h424=trace(U424*J4*U44')+trace(U524*J5*U54');
h434=trace(U434*J4*U44')+trace(U534*J5*U54');
h444=trace(U444*J4*U44')+trace(U544*J5*U54');
h454=h445;

h413=trace(U413*J4*U44')+trace(U513*J5*U54');
h423=trace(U423*J4*U44')+trace(U523*J5*U54');
h433=trace(U433*J4*U44')+trace(U533*J5*U54');
h443=h434;
h453=h435;

h412=trace(U412*J4*U44')+trace(U512*J5*U54');
h422=trace(U422*J4*U44')+trace(U522*J5*U54');
h432=h423;
h442=h424;
h452=h425;

h411=trace(U411*J4*U44')+trace(U511*J5*U54');
h421=h412;
h431=h413;
h441=h414;
h451=h415;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%H4
H4=[h411 h421 h431 h441 h451;

116
 h412 h422 h432 h442 h452;
h413 h423 h433 h443 h453;
h414 h424 h434 h444 h454;
h415 h425 h435 h445 h455];
h4=w'*H4*w
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

h511=trace(U511*J5*U55');
h512=trace(U512*J5*U55');
h513=trace(U513*J5*U55');
h514=trace(U514*J5*U55');
h515=trace(U515*J5*U55');

h521=h512;
h522=trace(U522*J5*U55');
h523=trace(U523*J5*U55');
h524=trace(U524*J5*U55');
h525=trace(U525*J5*U55');

h531=h513;
h532=h523;
h533=trace(U533*J5*U55');
h534=trace(U534*J5*U55');
h535=trace(U535*J5*U55');

h541=h514;
h542=h524;
h543=h534;
h544=trace(U544*J5*U55');
h545=trace(U545*J5*U55');

h551=h515;
h552=h525;
h553=h535;
h554=h545;
h555=trace(U555*J5*U55');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%H5
H5=[h511 h521 h531 h541 h551;
h512 h522 h532 h542 h552;
h513 h523 h533 h543 h553;

117
 h514 h524 h534 h544 h554;
h515 h525 h535 h545 h555];
h5=w'*H5*w
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%MATRIZ DE FUERZA DE CORIOLIS Y CENTRIFUGA

h=[h1;h2;h3;h4;h5];
%Calculo de la matriz h y c
g1=[0,0,-9.81,0];
g2=[0,0,-9.81,0];
g3=[0,0,-9.81,0];
g4=[0,0,-9.81,0];
g5=[0,0,-9.81,0];

%MATRIZ DE FUERZA DE LA CARGA GRAVITATORIA "C"

c(1,1)=-(m1*g1*U11*r11 + m2*g2*U21*r22 + m3*g3*U31*r33 + m4*g4*U41*r44 +
m5*g5*U51*r55)
c(2,1)=-(m2*g2*U22*r22 + m3*g3*U32*r33 + m4*g4*U42*r44 + m5*g5*U52*r55)
c(3,1)=-(m3*g3*U33*r33 + m4*g4*U43*r44 + m5*g5*U53*r55)
c(4,1)=-(m4*g4*U44*r44 + m5*g5*U54*r55)
c(5,1)=-(m5*g5*U55*r55)

torque=MD*aa+h+c

torques_1(r,1)=torque(1,1);
torques_2(r,1)=torque(2,1);
torques_3(r,1)=torque(3,1);
torques_4(r,1)=torque(4,1);
torques_5(r,1)=torque(5,1);
end
toc

subplot(1,5,1); plot(torques_1)
subplot(1,5,2); plot(torques_2)
subplot(1,5,3); plot(torques_3)
subplot(1,5,4); plot(torques_4)
subplot(1,5,5); plot(torques_5)

118
Referencias
Angeles, J. (2002), “Fundamentals of Robotic Mechanical Systems: Theory,
methods and algorithms”, Second Edition.

Backlouti, M. and Castelain J.M. (1993), “Dynamic model of robot manipulator in

explicit form formulation with dual vectors and the notion of augmented body”,
Journal of Robotic Systems

Bayro-Corrochano, E. (2000), "Motor Algebra Approach for Computing the

Kinematics of Robot Manipulators", Journal of Robotic Systems 17(9): 495-516

Brodsky V. and Shoham, M. (1994), "The Dual Inertia Operator and its Application
to Robot Dynamics", Journal of Mechanical Design 116(4)

Brodsky, V, and Shoham, M. (1998), "Derivation of dual forces in robot

manipulators", Journal of Mechanism and Machine Theory: 1241-1248

Brodsky, V. and Shoham, M. (1999), “Dual Numbers Representation of Rigid

Bodies Dynamics”, Journal of Mechanism and Machine Theory, Elsevier: 693-718

Cheng, H. H. (1994), “Programming with Dual Numbers and its application in

Mechanism Design”, Conference of Engineering with Computers, Universidad de
California

Craig, J.J. (2006), “Robótica”, Tercera Edición

Featherstone, R. (1987), "Robot Dynamics Algorithms", Kluwer academic

Publishers: 48-50

Fisher, I. S. (1999), “Dual number methods in kinematics, statics and dynamics”,

Florida, United States of America, CRC Press LLC

Funda, J. (1988), “A Computational Analysis of Line - Oriented Screw

Transformations in Robotics”

Gu, Y. L. and Yang, L. (1987), “Dual number transformation and its applications to
robotics. Journal of Robotics an Automation”, IEEE 615-623

Illescas, G. (2007), “Utilización de las algebras de Lie en Cinemática Paralela”, 8º

Congreso Iberoamericano de Ingeniería Mecánica

Keler, M. L. (2000), “On the theory of screws and the dual method”, Treatise on the
Theory of Screws, University of Cambridge, Trinity College

119
McCarthy, J. M. (1986), "Dual Orthogonal Matrices in Manipulator Kinematics", The
International Journal of Robotics Research

Niku, S. B. (2001), “Introduction to Robotics Analysis, Systems, Applications”

Pennestri, E. and Stefanelli, R. (2007), “Linear Algebra and Numerical Algorithms

Using Dual Numbers”, Universidad de Roma, Italia

Pennestrì, E. and Valentini, P.P. (2008), “Linear Dual Algebra Algorithms and their
Application to Kinematics”, Journal of Multi-body Dynamics, Universidad de Roma,
Italia, Springer

Pennock, G. R. (1982), “Kinematics and dynamics of multi-rigid-body open-chain

systems: application to robot manipulators”, University of California. Thesis for Ph.
Doctor Degree

Selig, J. M. (1996), “Geometrical Methods in Robotics”, Ithaca, N. Y.

Sponk, M.W. and Vidyasagar M. (1989), “Robot Dynamics an control”

Stachel, H. (2005), “Teaching Spatial Kinematics for Engineers”, International

Conference on Engineering Education.

Vázquez, R. V. (2005), “Diseño Mecánico de un Brazo Manipulador Robótico

Industrial Hidráulico de 5 G. D. L.”, Escuela Superior de Ingeniería Mecánica y
Eléctrica. Tesis de grado para M. en C. en Ingeniería Mecánica.

Walker, M. W. (1987), “Manipulator Kinematics and the Epsilon Algebra”, The

University of Michigan

120