cinemÁtica y dinÁmica del manipulador industrial …

CINEMÁTICA Y D INÁMICA DEL MANIPULADOR INDUSTRIA L ROBÓTICO HIDRÁULICO (MIRH1)


NSTITUTO POLlTECNICO NACIONAL SECRETARIA DE INVESTIGACION y POSGRADO

CARTA CESION DE DERECHOS

En la Ciudad de México, D. F., el día 19 del mes de Junio del año 2006 .

el(la) que suscribe Jose Reyes Garcia alumno (a) del Programa de

Maestría en Ciencias de Ingenieria Mecánica .

con número de registro B 031550 adscrito a la Sección de Estudios de Posgrado e Investigación de la

E.S.I.M.E. Unidad Zacatenco, manifiesta que es autor(a) intelectual del presente Trabajo de Tesis

bajo la dirección del Dr. Samuel Alcántara Montes y cede los derechos del

trabajo intitulado: Cinemática Y Dinámica Del Manipulador Industrial Robótico Hidráulico (MIRH1) al

Instituto Politécnico Nacional para su difusión, con fines Académicos y de Investigación.

Los usuarios de la información no deben reproducir el contenido textual, graficas o datos del trabajo sin el

permiso expreso del autor y/o director del trabajo. Este puede ser obtenido escribiendo a la siguiente

dirección: [email protected] .

Si el permiso se otorga, el usuario deberá dar el agradecimiento correspondiente y citar la fuente

del mismo.


RESUMEN

En el presente trabajo se desarrollan las ecuaciones de movimiento cinemático y dinámico de un manipulador de cinco grados de libertad, se obtienen las ecuaciones cinemáticas de movimiento, se resuelve la cinemática directa e inversa del manipulador, empleando la metodología Craigh aplicada a los parámetros de Denavit-Hartenberg, mismos que describen la geometría del manipulador de forma matricial, y a partir de los cuales se obtienen las matrices de transformación de los eslabones que componen el MIRH1, las matrices de transformación, al multiplicarse, relacionan entre si los eslabones obteniendo una matriz del brazo manipulador, que describe las orientaciones y posiciones del extremo libre del MIRH1, se calculan las coordenadas generalizadas, se calculan las velocidades y aceleraciones de los vectores del extremo libre del manipulador. Una vez que son resueltas las ecuaciones cinemáticas del manipulador, se desarrolla la dinámica a partir de la metodología de Lagrange – Euler, con la que se obtienen las ecuaciones de movimiento dinámico del MIRH1.


ABSTRACT

In this work, the cinematic and dynamic movement equations of a five freedom degrees manipulator are developed, the cinematic equations are obtained by solving the direct and inverse cinematic of the system, employing a methodology that cases the Denavit–Hartenberg parameters. The Denavit-Hartenberg parameters describe the manipulator geometry in a matrix way, and starting from them the transformation link matrices for the MIRH1. once the transformation matrices are multiplied the MIRH1 links are related to each other getting the arm manipulator matrix, which describes the free side’s positions and orientations, and the generalized coordinates are calculated. The speed and acceleration vectors of the free side are calculated. When the cinematic equations are solved, the dynamic is developed using the Lagrange-Euler methodology to obtain the MIRH1 dynamic movement equations.


INDICE

RESUMEN ...................................................................................................................... 4

ABSTRACT .................................................................................................................... 5

OBJETIVOS.................................................................................................................. 10

Objetivo general......................................................................................................... 10

Objetivos específicos ................................................................................................. 10

INTRODUCCIÓN......................................................................................................... 11

GLOSARIO. .................................................................................................................. 14

CAPÍTULO 1. ESTADO DEL ARTE DE LOS MANIPULADORES ROBÓTICOS INDUSTRIALES.................................................................................................................. 16

1.1 Clasificación de los robots de acuerdo a su configuración mecánica.................. 16

1.1.1. Geometría cartesiana ................................................................................... 16

1.1.2. Geometría cilíndrica .................................................................................... 16

1.1.3. Geometría esférica ....................................................................................... 17

1.1.4. Geometría articulada.................................................................................... 17

1.2. Clasificación de los robots de acuerdo a su fuente de poder .............................. 18

1.2.1. Potencia hidráulica....................................................................................... 18

1.2.2. Potencia neumática ...................................................................................... 18

1.2.3. Potencia electromagnética ........................................................................... 18

1.3. Clasificación de acuerdo a la organización internacional de estándares (ISO).. 19

1.3.1. Secuencia ..................................................................................................... 19

1.3.2. Trayectoria ................................................................................................... 19

1.3.3. Adaptables ................................................................................................... 19

1.3.4. Teleoperados ................................................................................................ 19


1.4. Clasificación de acuerdo a su empleo................................................................. 19

1.4.1 Robot de tomar y colocar.............................................................................. 19

1.4.2. Servorobot.................................................................................................... 19

1.4.3. Robot programable. ..................................................................................... 20

1.4.4. Robot controlado por computadora. ............................................................ 20

1.4.5. Robot sensorial. ........................................................................................... 20

1.4.6. Robot para línea de ensamble. ..................................................................... 20

1.5. Beneficios y desventajas de la robótica .............................................................. 20

CAPÍTULO 2. ASPECTOS FÍSICOS DEL MANIPULADOR ................................... 23

2.1 Clasificación MIRH1 ........................................................................................... 23

2.2 Capacidades del MIRH1 ...................................................................................... 23

2.2.1. Volumen de trabajo...................................................................................... 24

2.2.1.1 Alcance longitudinal.................................................................................. 24

2.2.1.2 Alcance vertical ......................................................................................... 25

2.2.2 Capacidades de carga máxima. ..................................................................... 26

2.2.2 Capacidades de carga máxima. ..................................................................... 27

2.3. Obtención de los parámetros de Denavit – Hartenberg...................................... 27

2.4. Momentos de inercia de masa del MIRH1 ......................................................... 30

2.5 Jacobiano del manipulador .................................................................................. 33

2.5.1. Cálculo del movimiento diferencial del manipulador ................................. 34

2.5.2 La transformación de traslación y rotación diferencial ∆ ............................ 35

2.5.3 Cambios de transformación diferencial entre marcos relacionados ............. 38

2.5.4 Las invariaciones lineales de una matriz de 3 3× ........................................ 41

CAPÍTULO 3. CÁLCULO DE LA CINEMÁTICA DEL MANIPULADOR INDUSTRIAL ROBÓTICO HIDRÁULICO MIRH1.......................................................... 54


3.1. Obtención de las matrices de transformación de los eslabones del manipulador MIRH1 .............................................................................................................................. 54

3.2. Ecuaciones cinemáticas del MIRH1 ................................................................... 57

3.3. Cinemática directa del manipulador MIRH1...................................................... 59

3.4. Cinemática Inversa del manipulador MIRH1 ..................................................... 61

3.5. Cálculo de las velocidades angulares de las articulaciones del manipulador MIRH1 .............................................................................................................................. 67

3.6. Cálculo de las aceleraciones angulares de las articulaciones del MIRH 1 ......... 71

CAPÍTULO 4. CÁLCULO DE LA DINAMICA DEL MANIPULADOR INDUSTRIAL ROBÓTICO HIDRÁULICO (MIRH1) ....................................................... 87

4.1 Cálculo de la energía cinética del manipulador................................................... 88

4.2. Cálculo de la energía potencial del manipulador................................................ 92

4.3. Ecuaciones de movimiento del manipulador ...................................................... 93

4.4. Cálculo del vector de fuerza de Coriolis y centrífuga ........................................ 96

CAPITULO 5. ANALISIS DE RESULTADOS........... ¡Error! Marcador no definido.

CONCLUSIONES ......................................................... ¡Error! Marcador no definido.

RECOMENDACIONES................................................ ¡Error! Marcador no definido.

ANEXOS ....................................................................... ¡Error! Marcador no definido.

Programa para calcular la matriz de transformación del brazo ¡Error! Marcador no definido.

Programa para calcular las ecuaciones cinemáticas del manipulador. ..............¡Error! Marcador no definido.

Programa para calcular la cinemática directa del manipulador.¡Error! Marcador no definido.

Programa para calcular las cinemática inversa del manipulador.¡Error! Marcador no definido.

Programa para calcular las velocidades angulares de los eslabones del manipulador........................................................................................... ¡Error! Marcador no definido.


Programa para calcular las aceleraciones angulares de los eslabones del manipulador. ..................................................................... ¡Error! Marcador no definido.

Funciones de los programas....................................... ¡Error! Marcador no definido.

Programa para calcular la dinámica utilizando el método de Lagrange-Euler..¡Error! Marcador no definido.

BIBLIOGRAFIA. .......................................................... ¡Error! Marcador no definido.


OBJETIVOS

Objetivo general

Obtener las fuerzas generalizadas que deberán tener los motores de las articulaciones, para mover los eslabones del manipulador a una velocidad angular determinada, en el extremo libre del manipulador.

Objetivos específicos

1. Obtener los parámetros de Denavit-Hartenberg del manipulador, a partir de la geometría y el número de grados de libertad del manipulador.

2. Obtener la matriz Jacobiana del manipulador.

3. Obtener las matrices de transformación de los eslabones

4. Obtener las ecuaciones de movimiento del manipulador, a través de la correlación matricial de los eslabones del manipulador, referidos a un eje de referencia inercial.

5. Calcular la cinemática directa del manipulador

6. Calcular las coordenadas generalizadas del manipulador para una trayectoria dada de trabajo, mediante la aplicación del método numérico de Newton-Raphson.

7. Calcular las velocidades angulares de los motores de cada articulación, para una velocidad dada de trabajo.

8. Calcular las aceleraciones angulares de los motores de cada articulación

9. Calcular la energía cinética del manipulador

10. Calcular la energía potencial del manipulador

11. Obtener las ecuaciones de movimiento dinámico del manipulador.


INTRODUCCIÓN

Un robot es un aparato programable destinado a mover materiales, piezas, herramientas y componentes especializados, en una gran variedad de movimientos, para una gran variedad de tareas. Las máquinas automáticas son máquinas destinadas a una sola tarea, que bien pueden ser reprogramadas, pero siempre con el fin de realizar una tarea específica.

La diferencia entre un robot y una máquina automática radica en que el robot se puede volver a programar las veces que sean necesarias para ejecutar las diferentes tareas que le son asignadas, a diferencia de las máquinas automáticas que sólo pueden ejecutar la tarea específica para la cual fueron diseñadas.

Un robot puede constar de dos o más eslabones, entre los cuales destacan el cuerpo del manipulador, que es el conjunto de eslabones que están unidos de manera consecutiva, y un efector final, que será el eslabón que realizará la acción que se requiere del robot, estos pueden ser para sujetar piezas, soldar, pintar, atornillar, o bien la combinación de todas las anteriores.

La robótica ha tenido un desarrollo importante a lo largo de los años, incrementándose de manera exponencial en las últimas décadas, al hacer uso de la tecnología de punta, obteniendo como resultado robots autónomos, con inteligencia artificial capaces de resolver problemas cotidianos, sin necesidad de ser reprogramados.

En la sección de estudios de posgrado de la Escuela Superior de Ingeniería Mecánica y Eléctrica, unidad profesional Zacatenco, se esta diseñando un manipulador robótico de cinco grados de libertad, este proyecto tiene el nombre “Manipulador Robótico Industrial Hidráulico MIRH1”. Es un tema multidisciplinario, que se desarrolla en varias etapas. La primer etapa fue el diseño mecánico del mismo, realizado en una tesis de maestría por el M. en C. Raymundo Vázquez, haciendo el diseño estático del manipulador, este trabajo es la segunda etapa del proyecto, y es el diseño cinemático y dinámico del MIRH1, donde se desarrollan las ecuaciones de movimiento cinemático y dinámico, que serán utilizadas en las etapas posteriores del proyecto, cuando se diseñe el control del manipulador.

A continuación se presenta un breve resumen del desarrollo recorrido por la robótica a través de los años.

En 1801, Joseph Jacquard inventó la máquina textil que era operada por medio de tarjetas perforadas, está máquina fue llamada telar programable y fue usada para la producción en masa.

En 1892, en los Estados Unidos, Seward Babbit desarrolla una grúa motorizada, equipada con un efector final para remover lingotes de una caldera.

En 1921, se da la primera referencia de la palabra robot por el checoslovaco Karen Capek, la cual aparece en Londres, está palabra tiene sus raíces en el checo robota, la cual significa servir en una labor de sirviente, a partir de está fecha se toma el concepto de robot.


En 1938, es diseñado un mecanismo de pintura en spray programable para la compañía DeVilbiss, por los americanos William Pollard y Harold Roselund

En 1946, George Devol patenta el aparato para controlar máquinas de propósito general, el cual usa un proceso de grabación magnética.

En 1948, Norbert Wiener un profesor del Instituto Tecnológico de Mássachussets, publica el libro Cybernetics, el cual describe el concepto de comunicación y control en electrónica, mecánica, y sistemas biológicos.

En 1951, es diseñado un brazo articulado equipado con un teleoperador por Raymond Goertz para la Comisión de Energía Atómica.

En 1954, es diseñado el primer robot programable por George C. Devol, quien utiliza el término automatización universal.

En 1959, la compañía Planet corporation introduce en el mercado el primer robot comercial disponible.

En 1960, la compañía Condec Corporation compra Unimation y comienza el desarrollo del Unimate Robot system.

En 1961, es instalado el primer robot Unimate.

En 1962, General Motors instala el primer robot industrial en una línea de producción, el robot seleccionado es de la marca Unimate.

En 1968, el instituto de investigación de Stanford, construye y prueba un robot con capacidad de visión, el cual es nombrado Shakeys.

En 1970, es desarrollado, en la universidad de Stanford, un brazo robot, el cual se convierte en la base para los proyectos de investigación. La fuente de potencia del brazo es eléctrica y es conocido como el brazo de Stanford.

En 1971, la Asociación de Robots Industriales Japonesa (JIRA por sus siglas en inglés) comienza la promoción del uso de robots en las industrias japonesas.

En 1973, es desarrollado el primer robot controlado por mini computadora, comercialmente disponible, por Richard Honh para la compañía Cincinnati Milacron. El robot es llamado el T3, la herramienta del mañana.

En 1975, se forma el Instituto de Robots Americano (RIA) para ayudar efectivamente a las industrias norteamericanas a implementar robots, en la automatización de las fábricas.

En 1976, El robot de la NASA “Viking II” aterriza en Marte. El cual disponía de un brazo robótico articulado.

En 1977, ASEA Brown Boberi Robotics Inc, una compañía europea de robots, ofrece dos tamaños de robots industriales eléctricos controlados por microcomputadora.


En 1978, con apoyo de la General Motors, Unimation desarrolla la máquina universal de ensamble programable (PUMA), usando tecnología de la empresa Vircarm.

En 1984, son introducidos los robots Direct-drive por la compañía Adept, con motores eléctricos conectados directamente en los brazos, eliminando la necesidad de engranes y cadenas intermediarias.

En 1990, ASEA Brown Boberi Robotics Inc. compra la división de robótica de la compañía Cincinnati Milacron, y todos los robots futuros serán ASEA machines

Los robots industriales pueden clasificarse de diversas maneras, entre las que destacan la geometría, la fuente de poder y las aplicaciones, temas que serán tratados en el capítulo uno del presente trabajo

El capítulo dos, trata el cálculo de las coordenadas generalizadas del manipulador, al obtener por principio, la matriz de datos de los parámetros de Denavit-Hartenberg, obtenidos al posicionar los ejes de referencia de cada uno de los eslabones del manipulador, para después obtener las matrices de transformación que los relacionan con respecto a un sistema de ejes de referencia inercial y así obtener las ecuaciones de movimiento del manipulador, ecuaciones que serán resueltas usando el método iterativo de Newton – Raphson, debido a que se tiene un sistema no lineal sobredeterminado

En el capítulo tres, se obtienen los aspectos físicos del manipulador como son determinación de la matriz de inercia de cada eslabón al referir el mismo a sus ejes de revolución, así como a los centros de masa y los vectores que relacionan los eslabones con el sistema inercial en la base del manipulador.

En el capítulo cuarto, se obtienen los pares de fuerzas para mover los eslabones del manipulador, estos se obtienen a partir de la metodología de Lagrange, la cual se divide en dos partes principales, el cálculo de la energía cinética total y el cálculo de la energía potencial total del manipulador, que al sumarse dan como resultado la energía total necesaria, para que el manipulador pueda cambiar de posición, misma que es transformada en energía mecánica, obteniendo así las fuerzas y momentos necesarios, para que el manipulador recorra una distancia en la punta de su extremo libre.

Las conclusiones y recomendaciones se dan en el capítulo cinco, estás que pueden ser útiles en el desarrollo de las demás áreas necesarias futuras para el desempeño total del manipulador, obteniéndose los puntos críticos del movimiento, las limitaciones del funcionamiento, y los posibles errores generados por la disposición mecánica.


GLOSARIO.

Cinemática Rama de la mecánica que estudia los movimientos de los cuerpos, con independencia de las fuerzas que los producen.

Cadena cinemática Sucesión de eslabones relacionados entre si para transmitir movimiento.

Coordenada Cada una de las líneas que sirven para determinar la posición de un punto, y de los ejes y planos a que se refieren dichas líneas.

Coordenada generalizada Son cada uno de los parámetros, independientes entre si, que determinan la configuración del sistema respecto a un eslabón fijo.

Dinámica Rama de la mecánica que estudia las relaciones de las fuerzas y los movimientos.

Efector final Último eslabón de un manipulador robótico, diseñado para realizar una tarea especifica.

Eslabón Todos y cada uno de los elementos que forman la cadena cinemática de un manipulador robótico.

Fuerza acción o influencia capaz de modificar el estado de movimiento de un cuerpo.

Geometría Disciplina matemática que tiene por objeto el estudio riguroso del espacio y de las formas.

Grados de libertad Cantidad de coordenadas generalizadas que determinan completamente la configuración del mecanismo.

Hidráulica Ciencia y técnica que trata las leyes de la estabilidad y circulación de los líquidos.

Jacobiano Representación de la geometría de los elementos de un mecanismo en el tiempo.

Manipulador robótico Mecanismo reprogramable.

Máquina Conjunto de mecanismos combinados para recibir una forma determinada de energía, transformarla y restituirla en otra más apropiada.

Marcos de referencia Ejes de coordenadas con respecto a los cuales se describen los eslabones de un manipulador.


Matriz de inercia Matriz que contiene los momentos de inercia y los productos de inercia de un cuerpo.

Matriz de transformación Descripción de un par cinemático de acuerdo a la metodología de Denavit-Hartenberg.

Mecánica Ciencia que tiene por objeto el estudio de las fuerzas y de sus acciones.

Mecanismo Conjunto de eslabones articulados, diseñados para trazar una trayectoria, generar una función o mover un cuerpo por ángulos y puntos dados.

Momento Es la acción de una fuerza referenciada a un punto distante de su aplicación.

Robot Conjunto de eslabones seriados automáticos y programables, capaces de realizar una tarea preestablecida.

Robótica Conjunto de técnicas utilizadas para el diseño y construcción de robots industriales y la puesta en práctica de sus aplicaciones.

Trayectoria Línea descrita por un punto material en movimiento.

Vector de posición Vector que ubica la posición del centro de masa de un eslabón.


CAPÍTULO 1. ESTADO DEL ARTE DE LOS

MANIPULADORES ROBÓTICOS INDUSTRIALES

En el presente capítulo se revisarán las diferentes clasificaciones en que pueden ser catalogados los manipuladores robóticos industriales, tomando en cuenta: la morfología, la energía primaria para inducir el movimiento, la clasificación de acuerdo a los lineamientos de la Organización Internacional de Estandarización y el uso final al que estén destinados.

1.1 Clasificación de los robots de acuerdo a su configuración mecánica

De acuerdo a la geometría o a la configuración mecánica básica del manipulador robótico, estos pueden clasificarse como cartesianos, cilíndricos, esféricos y articulados.

1.1.1. Geometría cartesiana

Un robot con geometría cartesiana, puede mover su efector final a cualquier posición dentro de un cubo o un rectángulo definido como su área de trabajo. Esta configuración está formada por dos categorías, transversal y longitudinal.

Este tipo de coordenadas geométricas tienen las siguientes ventajas:

Áreas muy largas de trabajo, ya que el desplazamiento en el eje “X” puede ser incrementado fácilmente.

El montaje de cabezales deja grandes áreas de manufactura libres para otros usos.

Pueden ser usados sistemas de control simples.

Las desventajas que presentan estos tipos de coordenadas son:

El acceso al área de trabajo, por medio del cabezal con carga de otros materiales o equipos, puede desequilibrar la estructura.

En algunos modelos la posición de los mecanismos de manejo, así como el control eléctrico, pueden causar dificultades de mantenimiento. [1]

1.1.2. Geometría cilíndrica

Un robot de geometría cilíndrica puede mover su efector final dentro del volumen descrito por un cilindro. El brazo de geometría cilíndrica está posicionado, en el área de trabajo, por dos movimientos lineales, uno a lo largo del eje “Z”, otro en la dirección del radio ”R”, y uno de rotación angular alrededor del eje Z.


Algunas de las ventajas en la geometría cilíndrica son las siguientes:

Un profundo alcance horizontal en las máquinas de producción.

La estructura vertical de la máquina ahorra espacio en la planta.

Es necesaria una estructura muy rígida para soportar grandes cargas y una buena repetibilidad.

La gran desventaja de está geometría, es el alcance limitado a la izquierda y a la derecha, debido a las deformaciones mecánicas.[1]

1.1.3. Geometría esférica

Los brazos de geometría esférica, también llamados polares, son aquellos que pueden mover su efector final dentro del volumen descrito por una esfera, requieren un movimiento coordenado en todos los ejes de posición para movimiento en las direcciones X, Y y Z.

Los brazos de geometría esférica, posicionan al robot en dos rotaciones y un desplazamiento lineal. La orientación de la herramienta está dada por medio de tres rotaciones en la muñeca (rotación, cabeceo y alabeo).

Las ventajas y desventajas son las mismas que las de los brazos de geometría cilíndrica, con la excepción de que los robots de geometría cilíndrica poseen una estructura más vertical, y los de geometría esférica son más bajos y alargados en tamaño. [1]

1.1.4. Geometría articulada

Los robots industriales articulados, también llamados máquinas antropomórficas, tienen un área de trabajo irregular, tienen dos grandes variantes: verticalmente articulado y horizontalmente articulado.

Los robots verticalmente articulados, son también llamados juntas esféricas y tienen tres movimientos principales angulares, la base de rotación (eje 1), el hombro (eje 2) y antebrazo (eje 3).

Ventajas de los robots verticalmente articulados:

Aunque ocupan un mínimo de espacio en el piso, tienen una gran distancia de alcance horizontal.

Tienen un buen radio de alcance, resultado de la habilidad de contraer el brazo cuando se encuentra en su posición de nido.

Una alta movilidad y posicionamiento del brazo les permiten tener alcance en espacios cerrados, y alrededor de obstrucciones.


Existen dos posibles variaciones de la geometría verticalmente articulada, descritas como:

Un eje adicional de movimiento rotacional (eje 4) en el antebrazo, que le permite rotar a este eslabón.

Un eje de movimiento adicional lineal (eje 4) en el antebrazo, que le permite extenderse y expandirse.

El robot horizontalmente articulado, tiene dos movimientos angulares que consisten en: una rotación en el brazo y el antebrazo, y un movimiento de posición lineal para un posicionamiento vertical. Los brazos horizontalmente articulados están divididos por dos configuraciones mecánicas:

El brazo de robot articulado.

El robot articulado de base horizontal.[1]

1.2. Clasificación de los robots de acuerdo a su fuente de poder

Existen tres fuentes primarias para la alimentación de energía en la manufactura de los sistemas de potencia, energía hidráulica, energía neumática y fuerza electromagnética, usadas como generadores de movimiento en los robots actuales. La clasificación de los robots en base a la fuente de energía utilizada es la siguiente:

1.2.1. Potencia hidráulica

Los robots que utilizan como fuente primaria un generador hidráulico, son diseñados para trabajos en los que la fuerza necesaria para ejecutar la tarea es muy grande, estos robots obtienen la fuerza requerida a través de una bomba hidráulica que alimenta los actuadores hidráulicos que a su vez generan el movimiento de los eslabones del manipulador.

1.2.2. Potencia neumática

Los robots que utilizan como fuente primaria el aire comprimido, son diseñados para trabajos en los que la fuerza que se requerirá es pequeña, pero a velocidades considerables, estos robots obtienen la fuerza requerida a través de un compresor de aire, que alimenta los actuadores neumáticos, que a su vez generan el movimiento de los eslabones del manipulador.

1.2.3. Potencia electromagnética

Los robots que utilizan la potencia electromagnética, son los de menor capacidad en cuanto a fuerza de trabajo se refiere, pueden llegar a ser muy precisos, dependiendo el número de pasos de los motores que moverán las articulaciones, estos motores son alimentados a través de una corriente eléctrica a diferentes voltajes.


1.3. Clasificación de acuerdo a la organización internacional de estándares (ISO)

La ISO ha establecido muchos documentos de estándares para ayudar en la colección de datos válidos de un robot, siendo cuatro áreas principales: secuencia, trayectoria, adaptabilidad y tele operación. La operación del controlador del robot provee la principal diferencia en la clasificación de categorías dadas por el estándar.[1]

1.3.1. Secuencia

El robot neumático no servo-controlado con control de línea de paro a paro, ya sea en geometría cartesiana o cilíndrica, es el que mejor describe a está categoría. La naturaleza binaria de encendido y apagado del controlador de salida, maneja los ejes secuencialmente, para una buena definición de los puntos finales. La trayectoria sin embargo, no está controlada o definida. El controlador más frecuentemente usado para está categoría de robots es el PLC o controlador lógico programable.[1]

1.3.2. Trayectoria

Está categoría incluye todas las geometrías con servomotores eléctricos o con ejes hidráulicos y operaciones de trayectoria controlada. Está clasificación se caracteriza por el movimiento de multiejes y movimientos en línea recta generados internamente.[1]

1.3.3. Adaptables

Está es una nueva categoría que incluye las “máquinas pensantes”. Ejemplos puros de está categoría no existen actualmente, sin embargo, robots de trayectoria operados con sensores adaptables, o controles con funciones de autoaprendizaje, tipifican estos sistemas.

1.3.4. Teleoperados

Robots teleoperados que extienden las funciones motrices humanas y robots en otros planetas y lugares remotos, han sido usados por muchos años para manipular material radioactivo. Está categoría incluye una nueva clase de máquinas operadas a distancia, que pueden programarse para responder a las acciones del operador.[1]

1.4. Clasificación de acuerdo a su empleo

1.4.1 Robot de tomar y colocar.

Es el robot más sencillo, este robot toma un objeto y lo coloca en otro lugar. La libertad del movimiento suele estar limitada a los grados de libertad del manipulador.

1.4.2. Servorobot.

En este robot se emplean servomecanismos para los brazos y manos, a fin de modificar su sentido de movimiento cuando están en el aire.


1.4.3. Robot programable.

Se acciona con un controlador programable, en el cual se almacena una secuencia de movimientos en una memoria y se estos se repiten en forma continua.

1.4.4. Robot controlado por computadora.

Este tipo de robot se programa mediante instrucciones electrónicas al controlador.

1.4.5. Robot sensorial.

Es un robot controlado por computadora, que tiene uno o más sentidos artificiales para detectar las zonas de trabajo y retroalimentar información al controlador.

1.4.6. Robot para línea de ensamble.

Es un robot controlado por computadora, que puede o no tener sensores; está destinado a trabajos en la línea de ensamble. [2]

1.5. Beneficios y desventajas de la robótica

La ventaja que presenta en la industria, el invertir dinero en un robot, es que, aunque es una inversión a largo plazo, por el alto costo inicial que representa el comprar un robot, la productividad es aumentada considerablemente, además de una mejor eficiencia comparada con la que podría tener un operario calificado, ya que al ser humano le afectan muchos factores de su vida cotidiana, como son las desveladas, la mala alimentación, y conflictos emocionales que se le pueden llegar a presentar, teniendo estos un gran peso en la calidad del producto que se está elaborando, a diferencia de un manipulador robótico, el cual, al no poseer sentimientos, no tiene ningún tipo de problema emocional, además de que pueden trabajar turnos seguidos sin cansarse, manteniendo la misma calidad en su trabajo y con la misma eficiencia.

En cuanto a seguridad se refiere, es más barato tener a un robot para realizar tareas de alto riesgo, que a un operador calificado, ya que primero, en caso de que sucediera un accidente, el robot no necesita de un hospital para ser atendido, por lo tanto no se necesita pagar una póliza de seguro para resguardar su salud.

Tomando como ejemplo el proceso de soldadura con arco eléctrico, el cual es un proceso en el cual se produce mucho calor, lo que produce mas cansancio en el soldador, además de ser riesgoso y tedioso por lo repetitivo, existe riesgo de que el operador sufra un accidente, consecuencia de las condiciones de trabajo, a diferencia del manipulador, que está diseñado específicamente para realizar tareas repetitivas.

Cabe mencionar que los robots actuales no van a sustituir a las personas, ya que los robots no pueden actuar ante situaciones imprevistas, ni con cambios de condiciones, por lo que en la mayoría de las plantas manufactureras se emplean personas y robots en las líneas de producción, en donde las personas elaboran actividades que requieren de la capacidad


motora y perceptiva humana, la coordinación de los ojos con las manos, la planeación, decisiones y evaluación.

De lo anterior se pueden resumir las razones por las cuales se instala un robot en una industria.

La reducción de costos de mano de obra.

Mejorar la calidad del producto.

Eliminar trabajos peligrosos y monótonos.

Aumentar el volumen de producción.

Aumentar la flexibilidad en los productos.

Reducir el desperdicio de materiales.

Cumplir con los reglamentos de seguridad industrial.

Disminuir la rotación de personal.

Reducir el costo de inversiones en equipo.

La clasificación de los manipuladores industriales ayuda a poder seleccionar el robot más adecuado para realizar una tarea determinada, al tener parámetros de selección como es el área o volumen de trabajo, la fuerza necesaria para realizar una tarea, el sistema de control necesario, en virtud de los ciclos que serán llevadas a cabo y la necesidad de reprogramarlos.

Ya se han presentado las diferentes clasificaciones de los manipuladores robóticos que existen actualmente en la industria, por lo que en el siguiente capitulo, será el MIRH1, el objeto de estudio, clasificándolo, describiendo sus capacidades como son el volumen de trabajo, además de comenzar el estudio de sus eslabones al obtener los parámetros de Denavit-Hartenberg, y los momentos de inercia del manipulador.


[1] REHG, James A. INTRODUCTION TO ROBOTICS IN CIM SYSTEMS. New Jersey. Prentice – Hall 2000.

[2] FEIRER, John L. METALISTERIA ARTE Y CIENCIA DEL TRABAJO CON METALES. México. McGraw-Hill 1990.

CINEMÁTICA Y D INÁMICA DEL MANIPULADOR INDUSTRIAL ROBÓTICO HIDRÁULICO (MIRH1)

CAPÍTULO 2. ASPECTOS FÍSICOS DEL

MANIPULADOR

Dentro del presente capítulo, se hablará de la conformación del manipulador, pudiendo así posicionarlo dentro de las diferentes clasificaciones de manipuladores robóticos, también se hablará de la capacidad de trabajo, el volumen de trabajo, los momentos de inercia de sus eslabones y la obtención de la matriz Jacobiana del manipulador.

Para clasificar el MIRH1, primero se darán a conocer sus capacidades y su geometría, además de la energía primaria con la que opera.

Para poder situar el MIRH1 en un área de trabajo, es necesario catalogarlo, de acuerdo a las clasificaciones del capitulo 1, para después comenzar el estudio cinemático obteniendo sus parámetros geométricos en una matriz numérica que los describa, para así desarrollar las ecuaciones cinemáticas de movimiento, esto se hace haciendo uso de los parámetros de Denavit-Hartenberg, mismos que serán desarrollados posteriormente.

2.1 Clasificación MIRH1

De acuerdo a la geometría del manipulador, el MIRH1 se clasifica dentro de los robots industriales de geometría articulada, verticalmente articulado, ya que posee tres movimientos principales, que son una rotación en la base, un hombro y un antebrazo, además de poseer un eje adicional de movimiento rotacional, que le permite rotar el antebrazo.

Con respecto a la fuente de poder, el MIRH1 se sitúa dentro de los robots hidráulicos, de donde viene su nombre, debido a que el movimiento esta generado por la fuerza que le transmite una bomba hidráulica, repartida mediante tubería a sus diferentes actuadores para así lograr movimiento en sus eslabones.

El MIRH1 es un robot de trayectoria, de acuerdo a la clasificación de la Organización Internacional de Estándares, ya que es la que contiene a los robots hidráulicos con operaciones de trayectorias controladas.

2.2 Capacidades del MIRH1

Las capacidades del MIRH1 están enfocadas básicamente al volumen de trabajo, y a la capacidad de carga que puede tener el manipulador, éstas capacidades fueron obtenidas directamente de la tesis de maestria “Diseño mecánico de un brazo Manipulador Industrial Robótico Hidráulico (MIRH1) de 5 grados de libertad” del M. En C. Raymundo Vázquez, trabajo que precede al actual y en el que se definió la estructura y diseño mecánico del MIRH1. [1]


2.2.1. Volumen de trabajo

El MIRH1 tiene un volumen de trabajo determinado por la geometría articulada de sus eslabones, la capacidad de sus pistones y motores hidráulicos. Este volumen comprende un alcance máximo y mínimo longitudinal, tomado a partir del eje de giro del motor de la base al extremo libre del brazo, y un alcance vertical superior e inferior, medidos desde de la base sobre la cual está montado, hasta el extremo libre del manipulador.

2.2.1.1 Alcance longitudinal

El alcance máximo horizontal del MIRH1 es de 1178.60 mm a una altura de 571.70 mm tomando como referencia el eje de rotación del motor de la base, como se muestra en la figura 2.1

El alcance mínimo horizontal del MIRH1 es de 840.40 mm a una altura de 850.0 mm tomando como referencia el eje de rotación del motor de la base, como se muestra en la figura 2.2

Figura 2.1 Alcance máximo horizontal del MIRH1

Figura 2.2 Alcance mínimo horizontal del MIRH1


2.2.1.2 Alcance vertical

El alcance máximo vertical del MIRH1 es una altura de 1540.9 mm, a una distancia longitudinal de 302.10 mm, tomando como referencia el eje de rotación del motor del eslabón 1 y la base de anclaje, como se muestra en la figura 2.3

El alcance inferior vertical del MIRH1 está situado a una altura de -80.30 mm, por debajo de la base de anclaje del manipulador, a una distancia de 560.20 mm, tomando como referencia el eje de rotación del motor del eslabón 1, como se muestra en la figura 2.4

Figura 2.3 Alcance máximo vertical de MIRH1

Figura 2.4 Alcance mínimo vertical del MIRH1


VISTA LATERAL

VISTA FRONTAL

VISTA SUPERIOR

Figura 2.4b Volumen de trabajo del MIRH1


2.2.2 Capacidades de carga máxima.

El MIRH1 tiene dentro de su configuración dos motores hidráulicos, ubicados de tal manera que uno de ellos genera movimiento al eslabón 5, y el otro da movimiento al efector final, una vez que este sea instalado, estos motores son de capacidades limitadas teniendo torques máximos de 80N-m y 38 N-m respectivamente.

El movimiento de los eslabones dos y tres, es generado a partir de cilindros hidráulicos de doble efecto, ya que es en estos eslabones en los que se requiere la mayor fuerza para mover el manipulador.[1]

2.3. Obtención de los parámetros de Denavit – Hartenberg

Los parámetros de Denavit-Hartenberg, permiten describir a partir de matrices, la geometría de los eslabones del MIRH1, este método matricial, establece un sistema coordenado para cada elemento de una cadena articulada. La representación de Denavit-Hartenberg, resulta en una matriz de transformación homogénea de 4x4, que representa cada uno de los sistemas de coordenadas del elemento previo, así mediante transformaciones secuenciales, el extremo libre del manipulador, se puede transformar y expresar en coordenadas de la base, que constituyen el sistema inercial.

La convención a usar para localizar los marcos de coordenadas de los eslabones como se ilustra en la figura 2.5 es la siguiente:

De acuerdo con la metodología Craig[2] el eje Z del marco {i}, llamado Zi, debe coincidir con el eje de rotación i. El origen del marco {i} deberá estar localizado donde la perpendicular ai intersecte el eje de revolución i. Xi deberá apuntar a lo largo de ai en del dirección del eje de revolución i al eje de revolución i+1.

Cuando ai= 0, Xi será normal al plano Zi y Zi+1. αi será medida de acuerdo a la regla de la mano derecha respecto a Xi, teniéndose la libertad de escoger el signo de αi en dos opciones de dirección Xi y Yi, debiendo tener siempre en cuenta la regla de la mano derecha para completar el i-ésimo marco.


Figura 2.5 Marcos de referencia.

0a = Es la distancia de Z0 a Z1 medida a lo largo de X0 igual a 0.

0α = Es el ángulo entre Z0 y Z1 medido sobre X0 igual a 0°.

0d = Es la distancia de X0-1 a X0 medida a lo largo de Z0 igual a 0.

0θ = Es el ángulo entre X0 y X1 medido sobre Z0 y es variable.



1d = Es la distancia de X0 a X1 medida a lo largo de Z1 igual a 0.

1θ = Es el ángulo entre X0 y X1 medido sobre Z1 y es variable

2a = Es la distancia de Z2 a Z3 medida a lo largo de X2 igual a 450mm.




3a = Es la distancia de Z3 a Z4 medida a lo largo de X3 igual a 450mm.











5d = Es la distancia de X4 a X5 medida a lo largo de Z5 igual a –442mm.


En la figura 2.5 se muestran únicamente los ejes de referencia 0 a 5, por lo que los elementos que hacen referenc ia a los ejes -1 y 6 serán cero.

En la tabla 2.3.1, se resumen los parámetros de Denavit-Hartenberg. [2], que describen las medidas geométricas de cada par cinemático.

Tabla 2.3.1

j jθ ja jα jd Valor de θ

0 0θ 0 0 0 0

1 1θ 0 2π

0

2π

2 2θ 450 0 0 -

2π

3 3θ 450 0 0 0

4 4θ 0 2π

0 0

5 5θ 0 0 -442 0


2.4. Momentos de inercia de masa del MIRH1

Cuando un cuerpo rígido esta sometido a fuerzas y pares de fuerza, el movimiento rotacional resultante depende no solo de su masa, sino también de cómo esta distribuida la masa[8].

Para calcular los momentos de inercia de un cuerpo, es conveniente modelarlos como distribuciones continuas de masa, y expresar el momento de inercia de masa respecto a un eje, de la siguiente manera:

2

m

I r dm= ∫

Donde

r es la distancia perpendicular del eje del centro de masa, al elemento diferencial de masa dm

Los momentos de inercia de cuerpos complejos se pueden determinar sumando los momentos de inercia de sus partes individuales. Esto es factible de hacer utilizando el teorema de los ejes paralelos, que relaciona los momentos de inercia de cuerpos compuestos, por combinaciones de partes sencillas.

20I I d m= +

Donde

m es la masa del cuerpo

d es la distancia perpendicular entre el eje al que se referencia el momento de inercia de la figura, y el eje al que se quiere encontrar el momento de inercia

I es el momento de inercia del cuerpo referenciado a su centro de masa.

Para determinar el momento de inercia de una pieza compuesta se recomienda lo siguiente:

• Dividir el cuerpo en partes cuyos momentos de inercia de masa se reconozcan o se puedan determinar con facilidad.

• Determinar el momento de inercia de esas partes, primero determinando el momento de inercia que pasa por el centro de masa, y después utilizar el teorema de los ejes paralelos para reverenciarlo al eje que se requiere.

• Sumar los momentos de inercia de todas las partes que componen el cuerpo.


Los momentos de inercia de los eslabones, fueron obtenidos de la tesis “Diseño mecánico de un manipulador industrial robótico hidráulico (MIRH1) de cinco grados de libertad”, siendo estos como a continuación se describen.[1]

Propiedades de masa del eslabón 0

Densidad: 30.01gmm

Masa: 21843.61g

Volumen: 33091840.00mm

Área: 21029577.47m

Centro de masa: 112.70

31.74

31.74

X mm

Y mm

Z mm

= −= −= −

Matriz de inercia:

-05 -06 2

-06

0.391781677 3.71897E-05 -0.1330075243.71897x10 0.719094618 -2.64877x10

-0.133007524 -2.64877x10 0.465967557

kg m −

Figura 2.6 Eslabón 0 del MIRH1

Z0

X0


Densidad: 30.01gmm

Masa: 14109.13g


Área: 2738104.05m

Centro de masa: 250.1118.75

0.09

X mmY mmZ mm

== −=

Matriz de inercia:

-05

2

-05

0.062009812 -0.006557103 6.9287x10-0.006557103 0.247236143 0.0001289696.9287x10 0.000128969 0.256148909

kg m −


Y1

X1





Densidad: 30.01gmm

Masa: 8088.14g


Área: 2685170.32m

Centro de masa: 242.430.800.52

X mmY mmZ mm

===

Matriz de inercia:

-06 2

-06

0.034488457 -0.002005642 0.000929828-0.002005642 0.239412077 -3.67819x100.000929828 -3.67819x10 0.226877156

kg m −

Y2

X2


Densidad: 30.01gmm

Masa: 6005.60g


Área: 2481104.99m

Centro de masa: 264.570.39

0.00

X mmY mmZ mm

== −=

Matriz de inercia:

-08

-07 2 -07

-08 -07

0.012120202 -0.000315008 6.439x10-0.000315008 0.109259555 2.1664x10 x106.439x10 2.1664x10 0.104761902

kg m −

Y3

X3

X4

Z4



2.5 Jacobiano del manipulador

El jacobiano es la representación de los elementos geométricos de un mecanismo en el tiempo. Permite la conversión de movimientos o velocidades diferenciales de articulaciones individuales, en los movimientos o velocidades diferenciales de los puntos de interés. También contiene la manera en que afectan las articulaciones individuales a todo el movimiento del mecanismo.

El jacobiano es relativo al tiempo, ya que los valores de los ángulos varían en el tiempo, los elementos del jacobiano también variarán de la misma manera.

Los cambios diferenciales de posición y orientación en la matriz de transformación, están dados por los cambios de los ángulos de sus articulaciones de rotación dθ .

Se definirán las transformaciones diferenciales de cambio ∆ , que corresponden a una unidad de rotación diferencial alrededor del eje z, siendo este aplicado al eslabón i-1, teniendo la nomenclatura 1i

i− ∆ .


Densidad: 30.01gmm

Masa: 429.50g


Área: 224565.71m

Centro de masa: 0.000.02562122.17

X mmY mmZ mm

===

Matriz de inercia:

-07 2

-07

0.000200549 0 00 0.000298324 -1.98813x100 -1.98813x10 0.000200423

kg m −

X5

Z5


2.5.1. Cálculo del movimiento diferencial del manipulador

A partir del teorema de Chasles que dice “Dadas dos posiciones distintas de un sólido, siempre se puede pasar de una a otra aplicando una traslación seguida de un giro de infinitas formas posibles. Entre ellas hay una en la que el eje de rotación es paralelo a la traslación, por lo que el movimiento resultante es de tipo helicoidal o de tornillo” podemos aplicar lo mismo para las relaciones diferenciales, que son de gran importancia para la manipulación. La mas obvia es el caso de los movimientos de acomodo, por ejemplo cuando una cámara observa el efector final del manipulador y calcula los cambios diferenciales de posición y orientación. Las relaciones diferenciales también son empleadas para encontrar los cambios correspondientes en las articulaciones de un cambio diferencial especifico. En este caso se quiere transformar los cambios diferenciales de un marco especifico a otro marco.

Dada una matriz de transformación cuyos elementos son funciones de algunas variables, la transformación diferencial con respecto a esas variables es la transformación cuyos elementos son las derivadas de los elementos de la transformación original. Ahora bien si se restringe solo a las transformaciones que representan traslación y rotación, se pueden expresar las derivadas de la traslación y rotación como una traslación y una rotación diferencial. Mas aun se puede expresar la traslación y la rotación diferencial en términos ya sea de las coordenadas dadas de un marco especifico, o en el marco de coordenadas de la base. Lo que es, dado un marco especifico G, podemos expresar G+dG como:

( ) ( ), , ,G dG Traslación dx dy dz Rotación k d Gθ+ = (2.1)

Donde:

( ), ,Traslacion dx dy dz

Es la transformación que representa la traslación de dx, dy y dz, en las coordenadas de la base.

( ),Rotacion k dθ

Es la transformación que representa la rotación diferencial dθ , alrededor de un vector k también en las coordenadas de la base.

De lo anterior se deduce que

( ) ( )( ), , ,dG Traslación dx dy dz Rotación k d I Gθ= − (2.2)

Alternativamente se puede expresar el cambio diferencial en términos de una traslación y una rotación diferencial en el marco especifico G

( ) ( ), , ,G dG G Traslación dx dy dz Rotación k dθ+ = ⋅ (2.3)


Donde:

( ), ,Traslacion dx dy dz

Es ahora una transformación que representa la traslación diferencial con respecto al marco de referencia G.

( ),Rotacion k dθ

Representa la rotación diferencial dθ , alrededor de un vector k descrito en el marco de referencia G.

dG estará dada entonces por:

( ) ( )( ), , ,dG G Traslación dx dy dz Rotación k d Iθ= − (2.4)

2.5.2 La transformación de traslación y rotación diferencial ∆

La expresión común que aparece en las ecuaciones (2.2) y (2.4)

( ) ( ), , ,Traslación dx dy dz Rotación k d Iθ −

Representa la traslación y rotación diferencial, que será denotada por el símbolo ∆

( ) ( ), , , 1Traslacion dx dy dz Rotacion k dθ∆ = − (2.5)

La ecuación (2.2), se puede escribir como:

dG G= ∆ (2.6)

Siendo ésta el cambio con respecto al marco de coordenadas de la base. Para el cambio con respecto a un marco de referencia T se indica con el superíndice T por delante como se indica en (2.7):

TdG G= ∆ (2.7)

La traslación y la rotación alrededor de un vector k son:

( )

1 0 00 1 00 0 10 0 0 1

x

y

z

dd

Tras dd

=

(2.8)


( )

cos 0cos 0

,cos 0

0 0 0 1

x x y x z z x y

x y z y y z y x

x z y y z x z z

k k vers k k vers k sen k k vers k senk k vers k sen k k vers k k vers k sen

Rot kk k vers k sen k k vers k sen k k vers

θ θ θ θ θ θθ θ θ θ θ θ

θθ θ θ θ θ θ

+ − − + + − = − − +

(2.9)

La ecuación (2.9), representa la matriz de rotación respecto a un eje arbitrario, en la que xk , yk y zk , son los vectores unitarios que representan el marco inercial de coordenadas y θ , es el ángulo de rotación respecto al eje Z[4]

Para tener un cambio diferencial dθ , las funciones trigonométricas cambian a:

0

0

0

lim

limcos 1

lim 0

sen d

vers

θ

θ

θ

θ θ

θ

θ

→

→

→

→

→

→

Por lo que, la ecuación (2.9) se reduce a:

( )

1 01 0

,1 0

0 0 0 1

z y

z x

y x

k d k dk d k d

Rot kk d k d

θ θθ θ

θθ θ

− − = −

(2.10)

Por lo tanto, la ecuación (2.5) se define como:

1 0 0 1 0 1 0 0 00 1 0 1 0 0 1 0 00 0 1 1 0 0 0 1 00 0 0 1 0 0 0 1 0 0 0 1

z y

z x

y x

dx k d k ddy k d k ddz k d k d

θ θθ θθ θ

− − ∆ = − −

00

00 0 0 0

z y

z x

y x

k d k d dxk d k d dyk d k d dz

θ θθ θθ θ

− − ∆ = −

(2.11)

Ahora bien, estableciendo una relación entre las transformaciones de rotación alrededor de los ejes x, y, z, se tiene que:

( )

1 0 0 00 cos 0

,0 cos 00 0 0 1

senRot x

senθ θ

θθ θ

− =

(2.12)


( )

cos 0 00 1 0 0

,0 cos 0

0 0 0 1

sen

Rot ysen

θ θ

θθ θ

= −

(2.13)

( )

cos 0 0cos 0 0

,0 0 1 00 0 0 1

sensen

Rot z

θ θθ θ

θ

=

(2.14)

Para el caso de cambios diferenciales en los que, sen dθ θ→ y cos 1θ → , las rotaciones (2.12) (2.13) y (2.14) quedan de la siguiente manera:

( )

1 0 0 00 1 0

,0 1 00 0 0 1

xx

x

Rot xδ

δδ

− =

(2.15)

( )1 0 00 1 0 0

,0 1 0

0 0 0 1

y

yy

Rot y

δ

δδ

= −

(2.16)

( )

1 0 01 0 0

,0 0 1 00 0 0 1

z

zzRot z

δδ

δ

− =

(2.17)

Por lo que, al relacionar los giros en los tres ejes, e ignorando los términos de segundo orden se tiene:

( ) ( ) ( )

1 01 0

, , ,1 0

0 0 0 1

z y

z xx y z

y x

Rot x Rot y Rot z

δ δδ δ

δ δ δδ δ

− − = −

(2.18)

Así pues, al comparar las ecuaciones (2.18) y (2.10), se nota que las diferenciales de rotación dθ , alrededor de un eje k, son equivalentes a las rotaciones diferenciales ,x yδ δ y

zδ , por lo que, la ecuación (2.11) queda de la siguiente manera:


00

00 0 0 0

z y

z x

y x

dxdydz

δ δδ δδ δ

− − ∆ = −

(2.19)

La transformación de traslación y rotación diferencial ∆ , puede ser considerada compuesta por dos vectores d y δ , conocidos como los vectores de traslación y rotación diferencial respectivamente:

x y z

d dxi dyj dzki j kδ δ δ δ

= + += + +

(2.20)

Estos dos vectores son conocidos también como vector de movimiento diferencial, teniendo para este caso que la parte superior del mismo está dada por la traslación diferencial y la parte baja, por la rotación diferencial, por lo que se tiene un vector columna de la siguiente forma:

x

y

z

dx

dydz

Dδδδ

=

(2.21)

Este vector de movimiento diferencial será usado posteriormente, en la obtención del jacobiano del manipulador, ya que el jacobiano permite la conversión de movimientos diferenciales en movimientos de un punto de interés.

2.5.3 Cambios de transformación diferencial entre marcos relacionados

Aquí se analizará la transformación de cambios diferenciales entre marcos de referencia relacionados, esto es, dada una ∆ , encontrar la T ∆

Las ecuaciones (2.6) y (2.7), dan una expresión para dG, por lo que se pueden igualar de manera que se obtenga una relación entre ellas de la siguiente manera:

TG G∆ = ∆ (2.22)

De la ecuación (2.22) se puede obtener directamente T ∆ de la siguiente manera:

1T G G−∆ = ∆ (2.23)

Al expandir está ecuación matricialmente, ocurre una gran simplificación, dando relaciones directas entre los elementos de los vectores diferenciales de cambio d y δ . La


transformación G en la ecuación (2.23), es conocida como la transformación de coordenadas diferenciales.

Si se representan los elementos de la transformación de coordenadas diferenciales G, en una matriz T dada en términos de los vectores n, s, a, p como sigue:

0 0 0 1

nx sx ax pxnx sy ay py

Tnz sz az pz

=

(2.24)

Donde:

n Es el vector normal del extremo libre del manipulador

s Es el vector de deslizamiento del extremo libre del manipulador

a Es el vector de aproximación

p Es el vector de posición del extremo libre del manipulador

Se puede expresar el producto cruz del lado derecho de las dos transformaciones de la ecuación (2.23) como:

( ) ( ) ( ) ( )( )( ) ( ) ( ) ( )( )( ) ( ) ( ) ( )( )

0 0 0 0

x x x x

y y y y

z z z z

n s a p d

n s a p dT

n s a p d

δ δ δ δ

δ δ δ δ

δ δ δ δ

× × × × +

× × × × + ∆ =

× × × × +

(2.25)

δ Y d son los vectores de rotación y traslación diferencial como se vio anteriormente. Premultiplicando por T-1 se obtiene:

( ) ( ) ( ) ( )( )( ) ( ) ( ) ( )( )( ) ( ) ( ) ( )( )

1

0 0 0 0

n n n s n a n p d

s n s s s a s p dT T

a n a s a a a p d

δ δ δ δ

δ δ δ δ

δ δ δ δ−

⋅ × ⋅ × ⋅ × ⋅ × +

⋅ × ⋅ × ⋅ × ⋅ × + ∆ = ⋅ × ⋅ × ⋅ × ⋅ × +

(2.26)

Ahora bien, de las propiedades del triple producto, se tiene que los pares de vectores pueden ser intercambiados si a su vez se cambia el signo del producto por cada intercambio, y también, si dos de los vectores son iguales, el valor del triple producto será cero, se tiene:


( ) ( ) ( )( ) ( ) ( )( ) ( ) ( )

1

00

0

0 0 0 0

n s a n p n d nn s s a p s d s

T Ta n s a p a d a

δ δ δδ δ δδ δ δ

−

− ⋅ × ⋅ × ⋅ × + ⋅ ⋅ × − ⋅ × ⋅ × + ⋅ ∆ = − ⋅ × ⋅ × ⋅ × + ⋅

(2.27)

Dedido a que los vectores n, s, y a forman un marco de referencia tridimensional, se tiene que:

n s aa n ss a n

× =× =× =

(2.28)

Por lo que la expresión (2.26), se reduce a

( )( )( )

00

0

0 0 0 0

T

a s p n d na n p s d ss n p a d a

δ δ δδ δ δδ δ δ

− ⋅ ⋅ ⋅ × + ⋅ ⋅ − ⋅ ⋅ × + ⋅ ∆ = − ⋅ ⋅ ⋅ × + ⋅

(2.29)

Y T ∆ , quedará definida de la siguiente manera

00

00 0 0 0

T T Tz y x

T T TT z x y

T T Ty x z

ddd

δ δδ δδ δ

−

− ∆ = −

(2.30)

Al igualar las ecuaciones (2.29) y (2.30) se obtienen los vectores diferenciales de traslación y rotación descritos con relación a un marco de coordenadas ( ),T TT dδ , en

términos de los vectores de traslación y rotación diferencial descritos con respecto a la base de coordenadas ( ),dδ

( )( )( )

Tx

Ty

Tz

d p n d n

d p s d sd p a d a

δ

δδ

= ⋅ × + ⋅

= ⋅ × + ⋅= ⋅ × + ⋅

(2.31)

Tx

Ty

Tz

nsa

δ δδ δδ δ

= ⋅= ⋅= ⋅

(2.32)


Simplificando las ecuaciones se tiene que:

( )( )( )( )( )( )

Tx

Ty

Tz

d n p d

d s p d

d a p d

δ

δ

δ

= ⋅ × +

= ⋅ × +

= ⋅ × +

(2.33)

Tx

Ty

Tz

nsa

δ δδ δδ δ

= ⋅= ⋅= ⋅

(2.34)

2.5.4 Las invariaciones lineales de una matriz de 3 3×

Ahora introduciremos dos matrices invariantes de 3 3× . Dada cualquier matriz A de 3 3× elementos, su descomposición cartesiana, representada en números complejos[3], consiste en la suma de su parte simétrica sA , y su proyección antisimétrica ssA está definida por:

( ) ( )1 1,

2 2T T

s SSA A A A A A= + = − (2.35)

El vector axial, o para abreviar el vector de A , es el vector con la propiedad a ssv A v× ≡ , para cualquier vector v de tres dimensiones. La traza de A , es la suma de los valores principales de sA , que pertenecen a los números reales. Partiendo de que no existe ningún marco de referencia en las definiciones anteriores, estas son invariaciones. Cuando se calculan estás invariaciones por supuesto que deberá de usarse un marco de referencia. Asumamos que las entradas de la matriz A , en un marco de referencia dado, están dadas por un arreglo de números reales ija , para , 1,2,3i j = , más aún, dejemos que a tenga

componentes ia para 1,2,3i = en el mismo marco, por lo tanto las invariaciones arriba mencionadas son calculadas como:

( )32 23

13 31

21 12

1a

2

a a

vect A a aa a

− ≡ ≡ − −

; ( ) 11 22 33tr A a a a≡ + +

Para las definiciones consecuentes lo siguiente es ahora aparente:

Teorema[3]: el vector de una matriz de 3 3× tiende a cero, si y sólo si es simétrica, lo que significa que la traza de una matriz de n n× tiende a cero si la matriz es antisimétrica.

Otras relaciones útiles son dadas aquí, para cualquier vector tridimensional a y b


( ) 12

Tvect ab a b= − × Y ( )T Ttr ab a b=

La relación ( )T Ttr ab a b= es casi directa pero ( ) 12

Tvect ab a b= − × no lo es tanto, una

prueba de la primer relación está dada aquí, dejemos que w sea ( )Tvect ab , ahora bien, de

la definición ssa v A v× ≡ , para cualquier vector tridimensional v se tiene:

w v Wv× = , donde W es el componente antisimétrico de Tab , por lo tanto

( )12

T TW ab ba≡ − y así ( ) ( )12

T TWv w v b v a a v b = × = − ahora comparando esta

expresión con el doble producto cruz, se tiene ( ) ( ) ( )T Tb a v b v a a v b× × = − de donde se

deduce que 12

w b a= ×

De lo anterior, se puede escribir una ecuación para los cambios diferenciales de la matriz de transformación dT, en términos de los cambios diferenciales de las coordenadas de la matriz de transformación, y en términos de los cambios diferenciales de cualquier articulación.

55 5

Ti idT T dq= ∆ (2.36)

Por lo tanto

555

Ti

i

TT

dq∂

= ∆ (2.37)

Donde

( ) ( )51 1

1 5 1 5... ...T ii i i i iA A A A A A

− −+ +∆ = ∆ (2.38)

Así pues, la transformación diferencial será

( ) 11 5 5... i

i iT A A A T−+= = (2.39)

Donde iA representa la matriz de transformación de cada uno de los eslabones de acuerdo con los parámetros de Denavit-Hartenberg.

A partir de que el manipulador MIRH-1 está diseñado únicamente de articulaciones de rotación, las ecuaciones (2.25) y (2.26) se simplifican de la siguiente manera:


( )( )( )( )( )( )

Tx

Ty

Tz

d n p

d s p

d a p

δ

δ

δ

= ⋅ ×

= ⋅ ×

= ⋅ ×

(2.40)

Tx

Ty

Tz

nsa

δ δδ δδ δ

= ⋅= ⋅= ⋅

(2.41)

Sin embargo 1 0 0 1i j kδ = + + debido a que solo presenta rotación en el eje Z, por lo tanto las ecuaciones se simplifican a:

( ) ( ) ( )Ti x y y x x y y x x y y xd n p n p i s p s p j a p a p k= − + + − + − − + (2.42)

Ti z z zd n i s j a k= + + (2.43)

El cambio diferencial de posición y orientación de T , como una función de sus articulaciones, estará dado por una matriz de n n× que consiste en los vectores diferenciales de rotación y traslación. Cada columna del Jacobiano consiste en la rotación y traslación diferencial de cada una de las articulaciones del manipulador. [3]

De lo anterior, de acuerdo con Paul (1986)[5][6], el jacobiano puede ser resumido a que el movimiento diferencial de la articulación, premultiplicado por la matriz de transformación relativa al último marco de referencia, se tendrán referenciados los movimientos diferenciales del extremo libre del manipulador relativos, al sistema de coordenadas inerciales, por lo que el jacobiano se puede calcular de la siguiente forma:

( )( )( )

15 1

15 2

15 3

15 4

15 5

15 6

ii x y y x

ii x y y x

ii x y y x

ii z

ii z

ii z

T J n p n p

T J s p s p

T J a p a p

T J n

T J s

T J a

−

−

−

−

−

−

= − +

= − +

= − +

=

=

=

(2.44)

De la cual, para el caso de estudio se tiene:

05 1 2 3 4 5T A A A A A= (2.45)

15 2 3 4 5T A A A A= (2.46)

25 3 4 5T A A A= (2.47)


35 4 5T A A= (2.48)

45 5T A= (2.49)

Así pues, para el MIRH1, se calculará el jacobiano comenzando del eslabón 5 al 1, como se describe a continuación:

La matriz de transformación del eslabón 5 es:

5 5

45

5 5

cos 0 00 0 1 442

cos 0 00 0 0 1

sen

Tsen

θ θ

θ θ

− − =

Sustituyendo en la ecuación (2.36), se obtiene la quinta columna del jacobiano, recordando que se está calculando desde el último hasta el primer eslabón, para i=5:

45 15 5

45 25 5

45 35

45 45 5

45 55 5

45 65

442cos

442cos

0

cos

0

T J

T J

T J

T J sen

T J

T J

θ

θ

θ

θ

= −

= −

=

=

=

=

Por lo que la quinta columna de la matriz jacobiana será:

5

5

55

5

442cos442cos

0

cos0

dsen

θθ

θθ

− −

=

Después, la matriz de transformación del eslabón 4 es:

4 4

4 434

cos 0 450cos 0 0

0 0 1 00 0 0 1

sensen

A

θ θθ θ

− =

Al multiplicarla por la matriz de transformación del eslabón 5 se tiene:


( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )

( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )

5 4 5 4 5 4 5 4 5 3

3 55

5 4 5 4 5 4 5 4 5 3

cos cos cos cos 0 cos0 0 -1 -d

cos cos cos cos 00 0 0 1

sen sen sen sen a

Tsen sen sen sen sen a

θ θ θ θ θ θ θ θ θ


− − − = + −

De la cual, al sustituir en la ecuación (2.36), se obtiene para i = 4:

( ) ( ) ( ) ( )( ) ( )( ) ( ) ( ) ( )( ) ( )

( )( ) ( )( )( ) ( ) ( ) ( )( ) ( ) ( ) ( )

35 14 5 4 5 4 5

35 24 5 4 5 4 5

35 34 5 3

35 44 5 4 5 4

35 54 5 4 5 4

35 64

cos cos -d

cos cos -d

1 cos

cos cos

cos cos

0

T J sen sen

T J sen sen

T J a

T J sen sen

T J sen sen

T J

θ θ θ θ

θ θ θ θ

θ

θ θ θ θ

θ θ θ θ

= − −

= − −

= − −

= +

= −

=

Por lo que la cuarta columna de la matriz jacobiana estará dada por el vector:

( ) ( ) ( ) ( )( )( ) ( ) ( ) ( )( )

( ) ( ) ( ) ( )( ) ( ) ( ) ( )

5 4 5 4 5

5 4 5 4 5

4

5 4 5 4

5 4 5 4

cos cos d

cos cos d

0cos cos

cos cos0

sen sen

sen sen

dsen sen

sen sen

θ θ θ θ

θ θ θ θ

θ θ θ θθ θ θ θ

− −

+ = +

−

Para el eslabón 3, se tiene que su matriz de transformación es:

3 3

3 323

cos 0 450cos 0 0

0 0 1 00 0 0 1

sensen

A

θ θθ θ

− =

Que al multiplicarla por las matrices de transformación de los eslabones 5 y 4, da como resultado:


( ) ( ) ( ) ( )( ) ( )( ) ( ) ( ) ( )( ) ( )

( ) ( ) ( ) ( )( ) ( )( ) ( ) ( ) ( )( ) ( )

( ) ( ) ( ) ( )( )( )

( ) ( ) ( ) ( )( ) ( )( ) ( ) ( ) ( )( ) ( )

( ) ( ) ( ) ( )( ) ( )( ) ( ) ( ) ( )( ) ( )

( ) ( ) ( ) ( )( )( )

5 4 5 4 3 5 4 5 4 3 5 4 5 4 2

4 5 4 3 5 4 5 4 3 5 3

525

5 4 5 4 3 5 4 5 4 3 5 4 5 4 2

5 4 5 4 3 5 4 5 4 3 5 3

c c c c c c c0

c c c c c c c

0 0 1

c c c - c c c c0

c c c c c

0 0 0 1

s s s s s s s a

s s s s a

dT

s s s s s s s a

s s s s s s a

θ θ θ θ θ θ θ θ θ θ θ θ θ θ

θ θ θ θ θ θ θ θ θ θ θ



− + − − −

− − + − − +

− −=+ + +

+ − + − +

De la cual, al sustituir en las ecuaciones (2.36), se obtiene para i = 3:

( ) ( ) ( ) ( )( ) ( )( ) ( ) ( ) ( )( ) ( )

( )

( ) ( ) ( ) ( )( ) ( )( ) ( ) ( ) ( )( ) ( )

( )

( )( ) ( ) ( ) ( )( )( )

( ) ( ) ( ) ( )( ) ( ) ( ) ( ) ( ) ( )( ) ( )( ) ( ) ( )

5 4 5 4 325 13 5

4 5 4 3

5 4 5 4 325 23 5

5 4 5 4 3

5 4 5 4 225 33

5 3

25 43 5 4 5 4 3 5 4 5 4 3

25 53 5 4 5

c c c

c c

c c

c c c c

c c1

c

c c c c c

- c c

s sT J d

s s s

s s sT J d

s

s s aT J

a

T J s s s s s

T J s

θ θ θ θ θ

θ θ θ θ θ

θ θ θ θ θ

θ θ θ θ θ

θ θ θ θ

θ

θ θ θ θ θ θ θ θ θ θ

θ θ θ

− − + = − − − − = − + − −

−= −

+ = + + −

= + ( )( ) ( ) ( ) ( ) ( ) ( )( ) ( )4 3 5 4 5 4 3

25 63

c c c

0

s s s s

T J

θ θ θ θ θ θ θ+ −

=

Por lo que la tercera columna de la matriz jacobiana estará dada por el vector:

( ) ( ) ( ) ( )( ) ( ) ( ) ( ) ( ) ( )( ) ( )( )( ) ( ) ( ) ( )( ) ( ) ( ) ( ) ( ) ( )( ) ( )( )

( ) ( ) ( ) ( )( ) ( )( ) ( ) ( ) ( )( ) ( ) ( ) ( ) ( ) ( )( ) ( )( ) ( ) ( ) ( )( ) ( ) ( ) ( ) ( ) ( )( ) ( )

5 4 5 4 3 4 5 4 3 5

5 4 5 4 3 5 4 5 4 3 5

5 4 5 4 2 5 33

5 4 5 4 3 5 4 5 4 3

5 4 5 4 3 5 4 5 4 3

c c c c c

c c c c c c

c c c

c c c c c

- c c c c c

0

s s s s s d

s s s s d

s s a ads s s s s

s s s s s



θ θ θ θ θ



− + − − − − + + − − −=

+ + −

+ + −

Para el eslabón 2, se tiene la matriz:

2 2

12

2 2

cos 0 00 0 1 0

cos 0 00 0 0 1

sen

Asen

θ θ

θ θ

− − =

Que, al multiplicarla por las matrices de transformación de los eslabones 3, 4 y 5, da como resultado:


( ) ( )( ) ( )

( )

( ) ( )( ) ( )

( )( )

( ) ( )( ) ( )

( )

( ) ( )( ) ( )

( )( )

( ) ( )( ) ( )

( )

( ) ( )( ) ( )

( )

( ) ( )( ) ( )

5 4 5 4 5 43 3 3 5 4

5 4 5 4 4 4

2 2 5 4

5 4 5 4 5 43 3 3

5 4 5 4 5 4

15

c c c c c cc c c c

cc c c

cc c c

ss s s s s s

s s ss s s

s ss s s

T

θ θ θ θ θ θθ θ θ θ θ

θ θ θ θ θ θθ θ θ θ

θ θ θ θ θ θθ θ θ

θ θ θ θ θ θ

− + − + + −

− − − + +

+

=

( )

( ) ( )( ) ( )( ) ( )

( )

( ) ( )( ) ( ) ( )

( )

( ) ( )( ) ( )

( )

( ) ( )( ) ( )

( )( )

( ) ( )( ) ( )

( )

( ) ( )( ) ( )

2

5 3

2 2 5

5 4 5 4 5 43 3 3

5 4 5 4 5 4

2 3

5 4 4 5 43

5 4 3 5 4 5 4

c

c 0

c c cc c

c c cc -

c c c c c c

a

a

s d

s s ss

s s ss

ss s s s s s s

θ

θ θ


θ θ θ θ θ θθ θ

θ θ θ θ θ θθ

θ θ θ θ θ θ θ

+

− − −

+ + + + + −

− − −

( )

( ) ( )( ) ( )( )

5 42

5 4

3 5 3

c

c

c

0 0 0 1

sa

s

s a

θ θ

θ θ

θ θ

+

+


Sustituyendo en la ecuación (2.36), para i = 2:

( ) ( )( ) ( )

( )

( ) ( )( ) ( )

( )( ) ( )

( ) ( )( ) ( )( )

( ) ( )( ) ( )

( )

( ) ( )( ) ( )

( )( ) ( )

5 43 5 4

5 4 215 12 2 5 2 5 4

5 43 5 3

5 4

5 43

5 415 22 2 5 2

5 43

5 4

c cc c c

cc

cc

c cc

cc

c

s s aT J d s s s

ss a

s

s sT J s d

ss

s

θ θθ θ θ

θ θθ θ θ θ

θ θθ θ

θ θ

θ θθ

θ θθ θ

θ θθ

θ θ

− + − = − − − + + + = −

+

( ) ( )( ) ( )( )

( ) ( )( ) ( )

( )( ) ( )( ) ( )

( )

( ) ( )( ) ( )

( )

( ) ( )( ) ( ) ( )

( )

( ) ( )( )

5 42

5 4

5 3

5 4 5 415 32 3 3 5

4 4 5 4

5 43

5 415 42 2

5 4

5 4 3

5 415 52

5

c c

c

c c cc

c

cc

cc

c c

c-

c

as s

a

sT J s d

s s s

s

sT J

s s s

sT J

s

θ θ

θ θ

θ

θ θ θ θθ θ

θ θ θ θ

θ θθ

θ θθ

θ θ

θ θ θ

θ θ

θ θ

− +

− + = +

+ + =

−

+=

( )( )

( ) ( )( ) ( )

( ) ( )

( ) ( )( ) ( )

( )( ) ( )( ) ( )

( )

43 3 3

4 5 4

5 4 5 415 62 3 3

5 4 5 4

c cc

c c cc

c

s ss s

sT J s

s s s

θ θθ θ θ

θ θ

θ θ θ θθ θ

θ θ θ θ

− +

+ − = −

Así pues, su vector de movimiento diferencial es:


( ) ( )( ) ( )

( )

( ) ( )( ) ( )

( )( ) ( )

( ) ( )( ) ( )( )

( ) ( )( ) ( )

( )

( ) ( )( ) ( )

( )( ) ( )

( ) ( )( )

5 43 5 4

5 4 22 5 2 5 4

5 43 5 3

5 4

5 43 5 4

5 4

2 5 2 5

5 43

5 4

2

c cc c c

cc

cc

c cc c c

cc

c

s s ad s s s

ss a

s

s ss d s s

ss

s

d

θ θθ θ θ

θ θθ θ θ θ

θ θθ θ

θ θ

θ θθ θ θ

θ θθ θ θ

θ θθ

θ θ

− + − − − − +

+ + − −

+

=

( )( )

( ) ( )( ) ( )

( )( ) ( )( ) ( )

( )

( ) ( )( ) ( )

( )

( ) ( )( ) ( ) ( )

( )

( ) ( )( ) ( )

( )( ) ( )( ) ( )

24

5 3

5 4 5 43 3 5

4 4 5 4

5 43

5 4

2

5 4

5 4 3

5 4 43

5 4 5 4

c

c c cc

c

cc

cc

c c

c c c- c

c

a

a

ss d

s s s

s

s

s s s

s

s s s

θ

θ

θ θ θ θθ θ

θ θ θ θ

θ θθ

θ θθ

θ θ

θ θ θ

θ θ θ θθ

θ θ θ θ

+

− + +

+ +

−

+ − +

( ) ( )

( ) ( )( ) ( )

( )( ) ( )( ) ( )

( )

3 3

5 4 5 43 3

5 4 5 4

c c cc

c

s s

ss

s s s

θ θ

θ θ θ θθ θ

θ θ θ θ

+ − −

El vector diferencial del primer eslabón estará dado por la multiplicación de las matrices de transformación de todos los eslabones, como se describe a continuación:

La matriz de transformación del eslabón 1 es:

1 1

1 101

cos 0 0cos 0 0

0 0 1 00 0 0 1

sensen

A

θ θθ θ

− =


Misma que al multiplicar por las anteriores da como resultado

( ) ( )( ) ( ) ( )

( ) ( )( ) ( ) ( )

( ) ( )

( ) ( )( ) ( ) ( )

( ) ( )( ) ( ) ( )

( ) ( )

( ) ( )( ) ( ) ( )

( ) ( )( ) ( ) ( )

5 4 5 4

3 3

5 4 5 4

2 1

5 4 5 4

3 3

5 4 5 4

5 4

35 4

2 1

5 4

35 4

05

c c c cc

c c -c c

c c

c cc

c

c

cs s s s

s ss s

s s

s ss s

ss

s

T

θ θ θ θθ θ

θ θ θ θθ θ

θ θ θ θθ θ

θ θ θ θ

θ θθ

θ θθ θ

θ θθ

θ θ

− − + +

− − − − −

− −

+ −

=

( ) ( )

( ) ( )( ) ( ) ( )

( ) ( )( ) ( ) ( )

( ) ( )

( ) ( )( ) ( ) ( )

( ) ( )( ) ( ) ( )

( ) ( )( ) ( )( )

( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )( )

2 1

5 4

3 5 45 4 2

5 4

5 45 43 5 33

5 45 4

2 1

5 4

35 4

2 1 2 1 2 1 2 1 5

5

cc c

c c

cc cc cc

cc

c

c

c c c c 0

c

s

ss s a

s ss

ass s

ss

ss

s s s s d

s

θ θθ θ

θ θ θθ θ

θ θθ θθ θ

θ θθθ θθ θ

θ θθ θ

θθ θ

θ θ θ θ θ θ θ θ

θ

+ − − + − + +

− − −

− − − −

( )( ) ( ) ( )

( ) ( )( ) ( ) ( )

( ) ( )

( ) ( )( ) ( )

( )

( ) ( )( ) ( ) ( )

( ) ( )

( ) ( )( ) ( ) ( )

( ) ( )( ) ( ) ( )

4 5 4

3 35 4 5 4

2 1

5 4 5 4

3 35 4 5 4

5 4

35 4

2 1

5 4

35 4

cc c

c cc c - c

c c c c

cc

c

c c

s

s s

s ss s s s

s

ss s

ss s

θ θ θθ θ

θ θ θ θθ θ

θ θ θ θθ θ

θ θ θ θ

θ θθ

θ θθ θ

θ θθ

θ θ

+ + + +

− − + +

− −

( ) ( )

( ) ( )( ) ( )

( )

( ) ( )( ) ( ) ( )

( ) ( )

( ) ( )( ) ( ) ( )

( ) ( )( ) ( ) ( )

( ) ( )( ) ( )( )

2 1

5 4

3 5 45 4 2

5 4

5 45 43 5 33

5 45 4

2 1

5 4

35 4

cc

cc

c ccc

cc

c c

0 0 0 1

ss

s ss a

ss c

s as ss

s

ss s

θ θθ θ

θ θ θθ θ

θ θθ θθ θ

θ θθθ θθ θ

θ θθ θ

θθ θ

+ − +

− + + +

− −

Matriz de la cual, su vector diferencial de movimiento es:


( ) ( )( ) ( )

( )

( ) ( )( ) ( ) ( )

( ) ( )

( ) ( )( ) ( )

( )

( ) ( )( ) ( ) ( )

( ) ( ) ( ) ( )( ) ( )

( ) ( )( )

5 4 5 43 3 5 4

5 4 5 4 2 105 11 2 1 2 1 5 5

2 15 4 5 43 3

5 4 5 4

c c c cc c c

cc c c

cc c

c c

ss s s s s

T J s s d sss s

s ss s


θ θ θ θ θ θθ θ θ θ θ

θ θθ θ θ θθ θ

θ θ θ θ

− − + + − = − − − − − − + −

( )( )

( ) ( )( ) ( )

( )

( ) ( )( ) ( )

( )( ) ( )

( ) ( )( ) ( )

( )

( ) ( )( ) ( )

( )( ) ( )

( )

24

5 3

5 4 5 43 3

5 4 5 4 205 21 2 1 2 1 5

5 4 5 43 3

5 4 5 4

c c c cc

c cc c

c c

a

s a

cs s s s s

T J s s ds s

s ss s

θ

θ

θ θ θ θθ θ

θ θ θ θ θθ θ θ θ

θ θ θ θθ θ

θ θ θ θ

+

− − + + = + +

− − − −

( )( ) ( )

( ) ( )( ) ( )( )

( ) ( )( ) ( ) ( )

( ) ( )( ) ( ) ( )

( ) ( )( ) ( )

( )

( ) ( )( ) ( ) ( )

( )

5 41 2

5 42 1

5 3

5 4 5 405 31 3 3 5

5 4 5 4

5 43

5 405 41 2

5 43

5 4

c

cc c

c c cc

c

cc

cc c

c c

ss a

s

s a

sT J s d

s s s

s

sT J

ss s

θ θθ

θ θθ θ

θ

θ θ θ θθ θ

θ θ θ θ

θ θθ

θ θθ

θ θθ

θ θ

+ − +

+ = − + −

+ + =

−

( )

( ) ( )( ) ( )

( )

( ) ( )( ) ( ) ( )

( ) ( )

( ) ( )( ) ( )

( )

( ) ( )( ) ( ) ( )

( ) ( )

( ) ( )( ) ( )

( )

( ) ( )( ) ( ) ( )

5 43

5 4

1 2 15 4

35 4

5 4 5 43 3

5 4 5 405 51 2 1

5 4 5 43 3

5 4 5 4

cc

c

c c

cc c

c c- c

c c c c

s

ss s

ss s

s s c

s sT J s

s ss s s s

θ θθ

θ θθ θ θ

θ θθ

θ θ

θ θ θ θθ θ

θ θ θ θθ θ

θ θ θ θθ θ

θ θ θ θ

+ + −

−

+ + + + = −

− −

( ) ( )

( ) ( )( ) ( )

( ) ( ) ( )( ) ( )

( )

2 1

5 4 5 405 61 3 3

5 4 5 4

c

c c cc

c

s

sT J s

s s s

θ θ

θ θ θ θθ θ

θ θ θ θ

+ − = −

Sustituyendo en la ecuación (2.36) se tiene para i = 1:

( ) ( )( ) ( )

( )

( ) ( )( ) ( )

( )( ) ( )

( ) ( )( ) ( )

( )

( ) ( )( ) ( )

( )( ) ( )

( ) ( )( ) ( )

( ) ( )( ) ( )

5 4 5 43 3 5 4

5 4 5 4 2 1

2 1 2 1 5 5 42 15 4 5 4

3 3

5 4 5 4

1

c c c cc c c

cc c c

cc c

c c

ss s s s s

s s d sss s

s ss s

d




θ θ θ θ

− − + + − − − − − − − + −

=

( )

( ) ( )( ) ( )

( )

( ) ( )( ) ( )

( )( ) ( )

( ) ( )( ) ( )

( )

( ) ( )( ) ( )

( )( ) ( )

( ) ( )( ) ( )

2

5 3

5 4 5 43 3

5 4 5 4 2 1

2 1 2 1 52 15 4 5 4

3 3

5 4 5 4

c c c cc

c cc cc c

c c

a

s a

cs s s s s s

s s ds s

s ss s

θ

θ θ θ θθ θ

θ θ θ θ θ θθ θ θ θ


θ θ θ θ

+

− − + + + +

−− − − −

( ) ( )( ) ( )( )

( ) ( )( ) ( )

( )( ) ( )( ) ( )

( )

( ) ( )( ) ( )

( )

( ) ( )( ) ( )

( )( ) ( )

( ) ( )( ) ( )

5 42

5 4

5 3

5 4 5 4

3 3 55 4 5 4

5 4 5 43

5 4 5 4

2 1

5 43

5 4

c

c

c c cc

c

c cc c

c cc c

c c

sa

s

s a

ss d

s s s

s s

s s

ss s

θ θ

θ θ

θ

θ θ θ θθ θ

θ θ θ θ

θ θ θ θθ

θ θ θ θθ θ

θ θθ

θ θ

+ +

+ − + −

+ + + −

−

( )

( ) ( )( ) ( )

( )( ) ( )

( ) ( )( ) ( )

( )

( ) ( )( ) ( )

( )( ) ( )

( ) ( )( ) ( )

( )

( ) ( )( ) ( )

( )( ) ( )

( ) ( )( ) ( )

3

2 1

5 43

5 4

5 4 5 4

3 35 4 5 4

2 1 2 1

5 4 5 4

3 35 4 5 4

5 4

5 4

c c

cc c

c c- c c

c c c c

c

c

s s

ss s

s s c

s ss s

s ss s s s

s

s

θ

θ θθ θ

θθ θ

θ θ θ θθ θ


θ θ θ θθ θ

θ θ θ θ

θ θ

θ θ

+

−

+ + + + −

− −

+( )

( ) ( )( ) ( )

( )5 43 3

5 4

c ccs

s s

θ θθ θ

θ θ

− −


Por lo tanto la matriz Jacobiana del manipulador estará dada de la siguiente manera:

[ ]1 1 2 3 4 5MIRHJ d d d d d= (2.50)

Al finalizar este capitulo se ha obtenido una descripción matricial del MIRH1, a través de los parámetros de Denavit-Hartenberg, se saben cuales son las capacidades de movimiento del MIRH1, las capacidades de carga, y demás parámetros físicos necesarios para el calculo de la cinemática y dinámica del manipulador, por lo que el capitulo 3, tratara el calculo de la cinemática, obteniendo las ecuaciones que controlaran las traslaciones y rotaciones del extremo libre del manipulador.


[1] VAZQUEZ Raymundo A. DISEÑO MECÁNICO DE UN MANIPULADOR INDUSTRIAL ROBÓTICO HIDRÁULICO (MIRH1) DE CINCO GRADOS DE LIBERTAD. México. 2005

[2] CRAIG John J. INTRODUCTION TO ROBOTICS MECHANICS AND CONTROL. EUA. Adison Wesley 1989.

[3] ANGELES, Jorge FUNDAMENTALS OF ROBOTIC MECHANICAL SYSTEMS. Canada. Springer 1997.

[4] FU, K.S. GONZALEZ, LEE ROBÓTICA: CONTROL DETECCION, VISIÓN E INTELIGENCIA. España. Mc Graw Hill 1990.

[5] RICHARD P. Paul. ROBOT MANIPULATOR MATHEMATICS PROGRAMMING AND CONTROL. England. MIT Cambridge 1986.

[6] SAEED, B. Niku INTRODUCTION TO ROBOTICS. ANALYSIS, SYSTEMS, APLICATIONS. New Jersey. Prentice hall 2001.

[7] SHOICHIRO Nakamura. ANALISIS Y VISUALIZACION GRAFICA CON MATLAB. México. Parson educación 1997.

[8] BEDFORD, A; WALLACE F. MECÁNICA PARA INGENIERIA, DINÁMICA. México. Addison Weasley Iberoamericana 2000.


CAPÍTULO 3. CÁLCULO DE LA CINEMÁTICA DEL

MANIPULADOR INDUSTRIAL ROBÓTICO

HIDRÁULICO MIRH1

La cinemática es la rama de la física que estudia el movimiento sin importar las causas que lo generan. En este capítulo se desarrollarán las ecuaciones que gobiernan el los desplazamientos del MIRH1, la velocidad lineal y la aceleración lineal en el extremo libre del manipulador, sin tomar en cuenta las fuerzas que los generan, desarrollando un total de 36 ecuaciones cinemáticas. Comenzando con la cinemática directa, en la que, a partir de valores dados en las articulaciones, se obtiene el valor de la matriz que describe la posición y orientación del extremo libre del manipulador. Después se calcula la cinemática inversa, en la que a partir de una coordenada en el extremo libre del manipulador, se calculan los valores angulares que deberán tener los motores de las articulaciones, para esto se creara una trayectoria de once puntos, a los cuales se les calcularán, los valores angulares de las articulaciones, la velocidad angular y la aceleración angular para recorrerla.

3.1. Obtención de las matrices de transformación de los eslabones del manipulador MIRH1

Los parámetros obtenidos a partir de la matriz de Denavit-Hartenberg[1], son sustituidos en la matriz general de rotación-traslación, para obtener así la matriz que describirá cada eslabón, de acuerdo a las características geométricas del mismo, en está matriz están incluidas la longitud del eslabón, la posición y la rotación en relación con los eslabones que se acopla. De acuerdo con la metodología Craigh, la matriz general será:

1

1 1 1 1

1 1 1 1

cos 0cos cos cos

cos cos cos0 0 0 1

i i i

i i i i i i ii

i i i i i i i

sen asen sen sen d

Asen sen sen d

θ θθ α θ α α αθ α θ α α α

−

− − − −

− − − −

− − − =

(3.1)

Matriz que define el marco {i}, relativo al marco {i-1}, de acuerdo con la figura 2.5, se relacionan tres marcos de referencia intermediarios {P}, {Q} y {R}, situados a lo largo de cada eslabón. [1]

Hay que denotar que sólo se muestran los ejes X y Z para cada marco de referencia, esto con el fin de no saturar la imagen.

El marco {R} difiere del marco i-1 sólo por la rotación del ángulo 1iα − . El marco de referencia {Q} difiere del marco {R} por una traslación 1ia − . El marco {P} difiere del {Q} por la rotación del ángulo iθ y el marco {i} difiere del {P} por la traslación id , de lo


anterior, para transformar los vectores definidos en el marco de referencia {i}, a su descripción en el {i-1}, se tiene:

1 1i i R Q P iR Q P iP T T T T P− −= (3.2)

La ecuación (3.2) puede escribirse también como

1 1i i iiP T P− −= (3.3)

Donde

1 1i i R Q Pi R Q P iT T T T T− −= (3.4)

Al considerar cada una de estás transformaciones, podemos ver, de la ecuación (3.4), que existen dos rotaciones y dos traslaciones, por lo que se puede escribir como:

( ) ( ) ( ) ( )11 1

ii x i x i z i z iT R D a R D dα θ−

− −= (3.5)

O bien

( ) ( )11 1, ,i

i x i i z i iT Tornillo a tornillo dα θ−− −= (3.6)

Donde la notación ( )1 1,x i iTornillo a α− − , representa la traslación a lo largo del eje X de

una distancia 1ia − , y una rotación alrededor del mismo eje con un ángulo 1iα − . Realizando las multiplicaciones contenidas en la ecuación (3.5), se obtiene la matriz general de transformación (3.1).

. Figura 3.1Sistemas de coordenadas de elementos y sus parámetros. 1

Al sustituir los datos de cada eslabón en la matriz (3.1), se obtienen las matrices de cada uno de los eslabones, así pues se tiene:


Para el eslabón 1, su matriz general de transformación será:

1 1 1

1 0 1 0 0 0 101

1 0 1 0 0 0 1

cos 0cos cos cos

cos cos cos0 0 0 1

sen asen sen sen d

Asen sen sen d


− − − =

Simplificando términos se tiene, de acuerdo con la tabla 2.3.1:

1 1

1 101

cos 0 0cos 0 0

0 0 1 00 0 0 1

sensen

A

θ θθ θ

− =


2 2 2

2 1 2 1 1 1 212

2 1 2 1 1 1 2

cos 0cos cos cos

cos cos cos0 0 0 1

sen asen sen sen d

Asen sen sen d


− − − =

simplificando términos se tiene, de acuerdo con la tabla 2.3.1:

2 2

12

2 2

cos 0 00 0 1 0

cos 0 00 0 0 1

sen

Asen

θ θ

θ θ

− − =


3 3 3

3 2 3 2 2 2 323

3 2 3 2 2 2 3

cos 0cos cos cos

cos cos cos0 0 0 1

sen asen sen sen d

Asen sen sen d


− − − =


3 3

3 323

cos 0 450cos 0 0

0 0 1 00 0 0 1

sensen

A

θ θθ θ

− =



4 4 4

4 3 4 3 3 3 434

4 3 4 3 3 3 4

cos 0cos cos cos

cos cos cos0 0 0 1

sen asen sen sen d

Asen sen sen d


− − − =


4 4

4 434

cos 0 450cos 0 0

0 0 1 00 0 0 1

sensen

A

θ θθ θ

− =


5 5 5

5 4 5 4 4 4 545

5 4 5 4 4 4 5

cos 0cos cos cos

cos cos cos0 0 0 1

sen asen sen sen d

Asen sen sen d


− − − =


5 5

45

5 5

cos 0 00 0 0 442

cos 1 00 0 0 1

sen

Asen

θ θ

θ θ

− =

3.2. Ecuaciones cinemáticas del MIRH1

Una vez obtenidas las matrices de transformación de cada uno de los eslabones, el siguiente paso es relacionarlas para así obtener la matriz de transformación del brazo T [2], está matriz especifica la orientación y posición del extremo libre del manipulador con respecto a la base de coordenadas, resultado de la multiplicación de las matrices de transformación en orden ascendente. La matriz del manipulador estará dada por:

0 0 1 2 3 45 1 2 3 4 5T A A A A A= i i i i (3.7)

Desarrollando la ecuación (3.7), se tiene:

( ) ( ) ( ) ( ) ( )( ) ( ) ( ) ( ) ( )

( ) ( )

1 2 1 2 1

0 0 1 1 2 1 2 12 1 2

2 2

cos cos -cos sin sin 0sin cos -sin sin -cos 0

sin cos 0 00 0 0 1

T A A

θ θ θ θ θθ θ θ θ θ

θ θ

= =

i


( ) ( ) ( )( ) ( ) ( )

( ) ( ) ( )( ) ( ) ( )

( ) ( ) ( )

( ) ( ) ( )( ) ( ) ( )

( ) ( ) ( )( ) ( ) ( )

( ) ( ) ( )

( ) ( )( ) ( )

( )

1 2 3 1 2 31 1 2 2

1 2 3 1 2 3

1 2 3 1 2 31 1 2 20 0 2

3 2 3 1 2 3 1 2 3

2 3 2

2 3

cos cos cos -cos cos sensen cos cos

-cos sen sen -cos sen cos

sen cos cos -sen cos sen-cos sen cos

-sen sen sen -sen sen cos

sen cos -sen

+cos sen

a

aT T A


θ θ θ θ θ θ


θ θ θ θ θ θ

θ θ θ

θ θ

= =i( )

( ) ( )( )3

2 22 3

sen0 sen

+cos cos

0 0 0 1

aθ

θθ θ

( ) ( ) ( ) ( ) ( ) ( )( ) ( ) ( ) ( ) ( ) ( )

( ) ( )

4 5 4 5 4 5 4 3

3 3 4 4 5 4 5 4 5 45 4 5

5 5

cos cos -cos sen sen d sensen cos -sin sen -cos -dcos

sen cos 0 00 0 0 1

a

T A A


θ θ

+ = =

i

0 0 35 3 5T T T= i

( ) ( ) ( )( ) ( )

( ) ( ) ( )( ) ( )

( ) ( ) ( )( )

( ) ( )( ) ( ) ( )

( ) ( )( ) ( ) ( )( ) ( )

( )

1 2 3 4 5 1 2 3 4 5 2 3 4 51 2 3 4 1

1 5 1 5 2 3 3 2 2

1 2 3 4 5 1 2 3 4 50 1

5 1 5 1 5

cos cos cos cos coscos cos

cos cos cos

cos cos cos

cos cos cos

sen sen dsen

sen sen sen a a

sen sen sensen

T sen

θ θ θ θ θ θ θ θ θ θ θ θ θθ θ θ θ θ


θ θ θ θ θ θ θ θ θ θθ

θ θ θ θ

+ + − + + + + + + + + + + +

+ + − + += − −

( ) ( )( )

( ) ( )

( ) ( ) ( ) ( ) ( )( ) ( )( )

2 3 4 52 3 4 1

2 3 3 2 2

2 3 4 5 2 3 32 3 4 5 2 3 4 5 2 3 4

2 2

cos cos

coscos cos

0 0 0 1

sen dsen sen

a a

d sen asen sen sen

sen a

θ θ θθ θ θ θ

θ θ θ

θ θ θ θ θθ θ θ θ θ θ θ θ θ θ θ

θ

+ + + + + + +

− + + + + + + − + + − + + + (3.8)

La matriz de transformación homogénea compuesta del manipulador, está dada de la siguiente manera:

0 0 0 10 0 0 1

x x x x

y y y y

z z z z

n s a pn s a p n s a p

Tn s a p

= =

(3.9)

Donde:

n Es el vector normal del extremo libre del manipulador

s Es el vector de deslizamiento del extremo libre del manipulador

a Es el vector de aproximación

p Es el vector de posición del extremo libre del manipulador

Igualando (3.7) y (3.9), se obtienen las componentes rectangulares de los vectores de orientación y de posición, por lo que se definen así las ecuaciones cinemáticas, que controlan los movimientos del manipulador.

( ) ( ) ( ) ( ) ( )1 2 3 4 5 1 5cos cos cosxn sen senθ θ θ θ θ θ θ= + + + (3.10)


( ) ( ) ( ) ( ) ( )1 2 3 4 5 1 5cos cos cosyn sen senθ θ θ θ θ θ θ= + + − (3.11)

( ) ( )2 3 4 5coszn sen θ θ θ θ= + + (3.12)

( ) ( ) ( ) ( ) ( )1 2 3 4 5 1 5cos cos cosxs sen senθ θ θ θ θ θ θ= − + + + (3.13)

( ) ( ) ( ) ( ) ( )1 2 3 4 5 1 5cos cos cosys sen senθ θ θ θ θ θ θ= − + + − (3.14)

( ) ( )2 3 4 5zs sen senθ θ θ θ= − + + (3.15)

( ) ( )1 2 3 4cosxa senθ θ θ θ= + + (3.16)

( ) ( )1 2 3 4ya sen senθ θ θ θ= + + (3.17)

( )2 3 4cosza θ θ θ= − + + (3.18)

( ) ( ) ( ) ( )( )1 2 3 4 5 2 3 3 2 2cos cos cosxp sen d a aθ θ θ θ θ θ θ= + + + + + (3.19)

( ) ( ) ( ) ( )( )1 2 3 4 5 2 3 3 2 2cos cosyp sen sen d a aθ θ θ θ θ θ θ= + + + + + (3.20)

( ) ( ) ( )2 3 4 5 2 3 3 2 2coszp d sen a sen aθ θ θ θ θ θ= − + + + + + (3.21)

Estas ecuaciones son las que controlan los movimientos de los vectores de posición y orientación del extremo libre del manipulador. Estas serán las ecuaciones a manipular para el control de los movimientos deseados del manipulador, como se demuestra en el siguiente apartado, donde se desarrolla la cinemática directa del manipulador.

3.3. Cinemática directa del manipulador MIRH1

A partir de la obtención de las ecuaciones cinemáticas del manipulador, se obtiene la cinemática directa de este, como se menciono anteriormente, esta consiste en dar los valores angulares que deberán tener las articulaciones del manipulador, para saber cual es la posición y orientación del extremo libre del manipulador.

Para calcular la cinemática directa del manipulador, se sustituyen los valores de los parámetros de Denavit-Hartenberg, por lo que se deberán obtener los valores de la siguiente matriz.

05

1 0 0 4500 1 0 00 0 1 8920 0 0 1

T

− = −

(3.22)


Por lo tanto, los valores angulares de las articulaciones serán:

0

1

2

3

4

5

0

2

2

0

0 0

rad

rad

rad

rad

radrad

θ

πθ

πθ

θ

θθ

=

=

= −

=

==

Por lo que al sustituir en las

( ) ( )cos cos 0 0 cos 0 02 2 2xn sen senπ π π = − + + +

( ) ( )cos 0 0 cos 0 cos 02 2 2yn sen senπ π π = − + + −

( )0 0 cos 02zn senπ = − + +

( ) ( )cos cos 0 0 0 cos 02 2 2xs sen senπ π π = − − + + +

( ) ( )cos 0 0 0 cos cos 02 2 2ys sen senπ π π = − − + + −

( )0 0 02zs sen senπ = − − + +

cos 0 02 2xa senπ π = − + +

0 02 2ya sen senπ π = − + +

cos 0 02zaπ = − − + +

( ) ( ) ( )cos 0 0 442 cos 0 450 cos 4502 2 2 2xp senπ π π π = − + + − + − + + −


( ) ( ) ( )0 0 442 cos 0 450 cos 4502 2 2 2yp sen senπ π π π = − + + − + − + + −

( ) ( ) ( )cos 0 0 442 0 450 4502 2 2zp sen senπ π π = − − + + − + − + + −

Realizando las operaciones, se obtienen la orientación y posición del extremo libre del manipulador, siendo estas iguales a la matriz (3.22)

1xn =

0yn =

0zn =

0xs =

1ys = −

0zs =

0xa =

0ya =

1za = −

450xp =

0yp =

892zp =

Con lo que se comprueba que las ecuaciones cinemáticas del manipulador han sido definidas correctamente.

3.4. Cinemática Inversa del manipulador MIRH1

La cinemática inversa del manipulador es la solución de las coordenadas generalizadas, esto es, encontrar los ángulos que deben tener las articulaciones del manipulador para una posición especifica, para resolver este problema se creó una trayectoria dentro del volumen de trabajo del manipulador. Esta es una línea recta, variando únicamente el eje “X” y manteniendo constante los ejes “Y” y “Z”, la trayectoria dada es de 20 cm, dados desde la


coordenada (0.400,0.100,0.100), hasta la coordenada (0.600,0.100,0.100) con incrementos de 2cm, obteniéndose por lo tanto 11 puntos, de los cuales se calcularán los valores angulares de las articulaciones haciendo uso del método iterativo de Newton-Raphson, como a continuación se describe:

La primer coordenada es (0.400,0.100,0.100), por lo que la matriz que la identifica será:

1 0 0 0.4000 1 0 0.1000 0 1 0.1000 0 0 1

TG

=

Esta matriz contiene los valores a los que se les calcularán los valores angulares de las articulaciones del manipulador, por lo que se iguala a la matriz T del brazo, ya que son estos los valores que deberán tener los vectores que la conforman.

1 0 0 0.4000 1 0 0.1000 0 1 0.1000 0 0 1 0 0 0 1

x x x x

y y y y

z z z z

n s a pn s a pn s a p

=

(3.22)

Ahora bien, cada elemento de la matriz T del brazo, representa una de las ecuaciones cinemáticas de la (3.10) a (3.21), ecuaciones que controlan el movimiento del extremo libre, así que igualando estas ecuaciones a los valores numéricos, asignados anteriormente en (3.22), se tiene:

( ) ( ) ( ) ( ) ( )( ) ( ) ( ) ( ) ( )( ) ( )

( ) ( ) ( ) ( ) ( )( ) ( ) ( ) ( ) ( )( ) ( )

( ) ( )( )

1 2 3 4 5 1 5

1 2 3 4 5 1 5

2 3 4 5

1 2 3 4 5 1 5

1 2 3 4 5 1 5

2 3 4 5

1 2 3 4

1

1=cos cos cos +sen sen

0 cos cos cos

0 cos

0 cos cos cos

1 cos cos cos

0

0 cos

0

sen sen

sen

sen sen

sen sen

sen sen

sen

sen



θ θ θ θ



θ θ θ θ

θ θ θ θ

θ

+ +

= + + −

= + +

= − + + +

= − + + −

= − + +

= + +

= ( )( )

( ) ( ) ( ) ( )( )( ) ( ) ( ) ( )( )

( ) ( ) ( )

2 3 4

2 3 4

1 2 3 4 5 2 3 3 2 2

1 2 3 4 5 2 3 3 2 2

2 3 4 2 3 3 2 2

1 cos

0.400 cos cos cos

0.100 cos cos

0.100 cos

sen

sen d a a

sen sen d a a

sen a sen a

θ θ θ

θ θ θ



θ θ θ θ θ θ

+ +

= − + +

= + + + + +

= + + + + +

= − + + + + +

(3.23)


El método de newton Raphson, pide valores iniciales, para iniciar las iteraciones. Estos

valores son arbitrarios, por lo que es propuesto el vector

1

2

3

4

5

11111

θθ

θθθ

=

Sustituyendo los valores iniciales en las ecuaciones (3.10) a (3.21), se tiene:

( ) ( ) ( ) ( )( ) ( ) ( ) ( ) ( )( ) ( )

( ) ( ) ( ) ( ) ( )( ) ( ) ( ) ( ) ( )( ) ( )

( ) ( )( )

cos 1 cos 1 1 1 cos+sen 1 sen 1 =0.4191

1 cos 1 1 1 cos 1 cos 1 1 -0.9047

1 1 1 cos 1 0.0762

cos 1 cos 1 1 1 1 1 cos 1 0.9047

1 cos 1 1 1 1 cos 1 cos 1 0.4091

1 1 1 1 -0.1187

cos 1 1 1 1 0.0762

1 1 1

sen sen

sen

sen sen

sen sen

sen sen

sen

sen sen

+ +

+ + − =

+ + =

− + + + =

− + + − =

− + + =

+ + =

+ +( )( )

( ) ( ) ( ) ( )( )( ) ( ) ( ) ( )( )

( ) ( ) ( )

5 3 2

5 3 2

3 2

1 0.1187

cos 1 1 1 0.9900

cos 1 1 1 1 cos 1 1 cos 1 -0.0035

1 1 1 1 cos 1 1 cos 1 -0.0055

cos 1 1 1 1 1 1 0.3503

sen d a a

sen sen d a a

sen a sen a

=

− + + =

+ + + + + =

+ + + + + =

− + + + + + =

Expresados de forma matricial se tiene:

0.4191 0.9047 0.0762 -0.0035-0.9047 0.4091 0.1187 -0.00550.0762 -0.1187 0.9900 0.3503

0 0 0 1

T

=

Entre las matrices TG y T existe una distancia la cual es calculada en dos partes, una que corresponde a la traslación del vector p y otra que corresponde a la rotación de los vectores , ,n s a .

Como el manipulador no cuenta con un efector final, no interesa cuales serán las orientaciones del efector final, por lo que únicamente se trabajará, con los vectores de posición, de manera que se utiliza un vector invariante par mantener constante la distancia entre los cambios de los vectores de orientación.


Para encontrar la distancia entre ambos puntos, se restan las coordenadas de traslación, y para mantener el sistema se aplica un vector invariante a los vectores de orientación.

La distancia entre ambos puntos estará dada por la ecuación:

( ) ( ) ( )1 1 12 2 2

TG T

TG T TG T TG T

p pdist

n n s s a a

− = × + × ×

(3.24)

12

TGx Tx

TGy Ty

TGz Tz

TGx Tx TGx Tx TGx Tx

TGy Ty TGy Ty TGy Ty

TGz Tz TGz Tz TGz Tz

p pp pp p

distn n s s a an n s s a an n s s a a

− − −

= × × × × + × + × × × ×

(3.25)

Por lo que, al sustituir valores se tiene:

( )( )( )

0.400 0.00350.100 0.00550.100 0.3503

1 0.4191 0 0.9047 0 0.07621

0 0.9047 1 0.4091 0 0.11872

0 0.0762 0 0.1187 1 0.9900

dist

− − − − − =

× × × × − + × + × × × − ×

0.40350.10550.2503

0.11870

0.9047

dist

−

=

Una vez obtenida la distancia que existe entre el punto de la trayectoria y el punto calculado, se obtiene el jacobiano del manipulador sustituyendo los valores iniciales en la ecuacion (2.50).


0.0055 -0.1893 0.0153 0.2364 0-0.0035 -0.2947 0.0239 0.3682 0

0 -0.0065 -0.2496, -0.0624 0

0 0.8415 0.8415 0.8415 0.07620 -0.5403 -0.5403 -0.5403 0.11871 0 0 0 0.9900

J

=

Para optimizar el sistema, se hace uso de la matriz pseudo inversa, encontrando así la solución mas optima, que satisfaga las ecuaciones (3.10) a (3.21) con el menor error posible. Después aplicando esta optimizacion a la distancia que existe entre el punto de la trayectoria, y el punto calculado resultante de la sustitución los valores iniciales, se obtienen los nuevos valores angulares de las articulaciones, valores que deberán satisfacer la matriz de trayectoria TG, al sustituir en las ecuaciones (3.10) a (3.21).

Si al evaluar las ecuaciones (3.10) a (3.21) con los valores obtenidos, no se satisface la matriz TG, se toman estos valores como valores iniciales y se comienza una nueva iteración, hasta que se satisfaga la matriz TG.

Los pasos a seguir para calcular la cinemática inversa del manipulador, están representados en el diagrama de bloques 3.2.

Para realizar está tarea cíclica, se desarrolló un programa de cómputo utilizando el software Matlab 7.0, el cual se encuentra en los anexos bajo el nombre cininversa.m. y los resultados se presentan en la tabla III.4.

Tabla III.4

X Y Z rad rad rad rad rad1 0.400 0.100 0.100 9.6698 1.5085 1.4252 0.2079 -6.52822 0.420 0.100 0.100 9.6585 1.5514 1.3841 0.2061 -6.51693 0.440 0.100 0.100 9.6483 1.5947 1.3407 0.2062 -6.50674 0.460 0.100 0.100 9.6388 1.6385 1.2948 0.2082 -6.49725 0.480 0.100 0.100 9.6302 1.6831 1.2462 0.2123 -6.48866 0.500 0.100 0.100 9.6222 1.7284 1.1947 0.2186 -6.48067 0.520 0.100 0.100 9.6148 1.7746 1.1399 0.2271 -6.47328 0.540 0.100 0.100 9.6079 1.822 1.0815 0.2381 -6.46639 0.560 0.100 0.100 9.6015 1.8709 1.0188 0.2518 -6.4599

10 0.580 0.100 0.100 9.5955 1.9217 0.9513 0.2686 -6.453911 0.600 0.100 0.100 9.5899 1.9749 0.8777 0.289 -6.4483

punto coordenadas(mm) 1θ 2θ 3θ 4θ 5θ


Figura 3.2 diagrama de flujo para calcular la cinemática inversa.

INICIO

Dar el primer punto de la trayectoria deseada

Dar valores iniciales a los ángulos de las articulaciones

Comparar la norma del vector

diferencial con la tolerancia

especificada

Calcular la cinemática directa para los valores dados.

Calcular la distancia entre el punto calculado y el punto de la

trayectoria.

Calcular el jacobiano para los valores calculados

Calcular la pseudo inversa de los valores calculados

Multiplicar la pseudos inversa por la distancia entre el punto calculado y el punto dado.

Sumar el vector diferencial a los valores iniciales

Calcular la norma del vector diferencial

FIN


3.5. Cálculo de las velocidades angulares de las articulaciones del manipulador MIRH1

El cálculo de la velocidad angular a la cual se moverán las articulaciones, es por mucho un cálculo más sencillo que la obtención de las coordenadas generalizadas del manipulador ya que, a diferencia del sistema anterior, este es un sistema lineal. Este sistema se obtiene al derivar con respecto al tiempo las ecuaciones (3.10) a la (3.21), con lo que se obtienen las ecuaciones para el control de la velocidad lineal en el extremo libre del manipulador, en función de las velocidades angulares de los eslabones, como se describen a continuación:

( ) ( ) ( ) ( ) ( ) ( ) ( )( ) ( ) ( ) ( ) ( ) ( ) ( )

1 1 2 3 4 5 1 2 3 4 2 3 4 5

1 2 3 4 5 5 1 1 5 1 5 5

cos cos cos cos

cos cos s cos cos

xn sen sen

en sen sen


θ θ θ θ θ θ θ θ θ θ θ θ

= − + + − + + + + −

+ + + +

& & & &&& & &

(3.26)

( ) ( ) ( ) ( ) ( )( ) ( )( ) ( ) ( ) ( ) ( ) ( ) ( )

1 1 2 3 4 5 1 2 3 4 2 3 4 5

1 2 3 4 5 5 1 1 5 1 5 5

cos cos cos cos

cos cos cos

yn sen sen

sen sen sen sen



= + + − + + + + −

+ + + −

& & & &&& & &

(3.27)

( ) ( ) ( ) ( ) ( )2 3 4 2 3 4 5 2 3 4 5 5cos coszn sen senθ θ θ θ θ θ θ θ θ θ θ θ= + + + + − + +& & & && (3.28)

( ) ( ) ( ) ( ) ( ) ( ) ( )( ) ( ) ( ) ( ) ( ) ( ) ( )

1 1 2 3 4 5 1 2 3 4 2 3 4 5

1 2 3 4 5 5 1 1 5 1 5 5

cos cos

cos cos cos cos

xs sen sen sen sen

sen sen sen



= + + + + + + + −

+ + + −

& & & &&& & &

(3.29)

( ) ( ) ( ) ( ) ( ) ( ) ( )( ) ( ) ( ) ( ) ( ) ( ) ( )

1 1 2 3 4 5 1 2 3 4 2 3 4 5

1 2 3 4 5 5 1 1 5 1 5 5

cos cos

cos cos cos cos

ys sen sen sen sen

sen sen sen



= − + + + + + + + −

+ + + +

& & & &&& & &

(3.30)

( ) ( ) ( ) ( ) ( )2 3 4 2 3 4 5 2 3 4 5 5cos coszs sen senθ θ θ θ θ θ θ θ θ θ θ θ= + + + + + + +& & & && (3.31)

( ) ( ) ( ) ( ) ( )1 1 2 3 4 1 2 3 4 2 3 4cos cosxa sen senθ θ θ θ θ θ θ θ θ θ θ θ= − + + + + + + +& & & && (3.32)

( ) ( ) ( ) ( ) ( )1 1 2 3 4 1 2 3 4 2 3 4cos cosya sen senθ θ θ θ θ θ θ θ θ θ θ θ= + + + + + + +& & & && (3.33)

( ) ( )2 3 4 2 3 4za sen θ θ θ θ θ θ= + + + +& & & (3.34)


( ) ( ) ( ) ( )( )( ) ( ) ( ) ( ) ( ) ( )( )

1 1 2 3 4 5 2 3 3 2 2

1 2 3 4 2 3 4 5 2 3 2 3 3 2 2 2

cos cos

cos cos

xp sen sen d a a

d sen a sen a


θ θ θ θ θ θ θ θ θ θ θ θ θ

= − + + + + + +

+ + + + − + + −

&&& & & & & &

(3.35)

( ) ( ) ( ) ( )( )( ) ( ) ( ) ( ) ( ) ( )( )

1 1 2 3 4 5 2 3 3 2 2

1 2 3 4 2 3 4 5 2 3 2 3 3 2 2 2

cos cos cos

cos

yp sen d a a

sen d sen a sen a



= + + + + + +

+ + + + − + + +

&&& & & & & &

(3.36)

( ) ( ) ( ) ( ) ( )2 3 4 2 3 4 5 2 3 2 3 3 2 2 2cos coszp sen d a aθ θ θ θ θ θ θ θ θ θ θ θ= + + + + + + + +& & & & & && (3.37)

Este sistema se resuelve de una manera muy sencilla ya que su solución es directa haciendo uso de cualquier método de resolución de sistemas de ecuaciones lineales, por lo que el sistema de 12 ecuaciones con 5 incógnitas puede ser resuelto por medio de la ecuación:

A x B=i (3.38)

Donde:

A Es el sistema de ecuaciones que, para el caso de estudio, es de 12 x 5

x Es el vector de incógnitas o velocidades angulares a calcular

B Es el vector de datos resultante

Como datos se tiene la matriz A, que es formada al acomodar matricialmente las ecuaciones (3.26) a (3.37), de manera que esta quedara en función de los valores angulares únicamente.

El vector B, es la velocidad lineal para la cual se requieren encontrar las velocidades angulares contenidas en el vector x. Como el sistema de ecuaciones lineales (3.38), es sobredeterminado, y no se puede resolver como se presenta, es necesario optimizarlo con la matriz pseudo inversa, como se describe a continuación.

Para un sistema n x n se tiene:

0A Bθ − =&i

Pero el MIRH1, presenta un sistema lineal de m x n, entonces se tiene:

A B eθ − =&i

Que significa que este sistema no es exacto, es igual a un error, y para minimizar ese error se eleva al cuadrado la ecuación, para así tener el error mas pequeño, como se realiza a continuación:


( ) ( )( ) ( ) ( )( )

2

2

22T T T T

A B A B e

A B A B e

A A A B B B e

θ θ

φ θ θ θ

φ θ θ θ θ

− − =

= − − =

= − + =

& &i i& & &i i& & & &

(3.39)

Derivando con respecto a θ& , se tiene:

( )( )

' 2 2

'

T T

T T

A A A B

A A A B

φ θ θ

φ θ θ

= −

= −

& && &

Igualando a cero, para obtener los puntos críticos:

( ) 1= T TA A A Bθ

−& (3.40)

De (3.40), el termino ( ) 1T TA A A−

se conoce como la matriz pseudo inversa, utilizada

para optimizar sistemas de ecuaciones.

Ahora, como velocidad deseada en el extremo libre es de 0.1 m/seg, se tiene que el

vector

x

y

z

x

y

z

x

y

z

x

y

z

nn

nsss

Baaappp

=

&&&&&&&&&&&&

,

Discriminando los valores de las velocidades de orientación y asignando los valores necesarios en los vectores de posición se tiene la siguiente matriz de datos

0 0 0 0.05770 0 0 0.05770 0 0 0.05770 0 0 1


Que al sustituir en el vector de datos B, se obtiene:

000000000

0.05770.05770.0577

B

=

m/seg

Al sustituir en (3.40), y hacer las operaciones se tiene que las velocidades en las articulaciones serán de:

1.01821.80523.0008

1.19561.0182

radsegθ

= − −

&

Para el cálculo de las velocidades angulares de los siguientes puntos de la trayectoria, y con objeto de simplificar la tarea, se desarrolló un programa utilizando el software Matlab 7.0, que está incluido en los anexos de la tesis con el nombre de velocidades.m, y con el que se obtiene los resultados expresados en la tabla III.5.:

Tabla III.5

X Y Z rad/seg rad/seg rad/seg rad/seg rad/seg1 0.400 0.100 0.100 1.0182 1.8052 -3.0008 1.1956 -1.01822 0.420 0.100 0.100 0.9906 1.8054 -3.0795 1.2741 -0.99063 0.440 0.100 0.100 0.9636 1.8123 -3.1665 1.3542 -0.96364 0.460 0.100 0.100 0.9374 1.8266 -3.2635 1.4369 -0.93745 0.480 0.100 0.100 0.9121 1.8495 -3.3732 1.5236 -0.91216 0.500 0.100 0.100 0.8877 1.8825 -3.4986 1.616 -0.88777 0.520 0.100 0.100 0.8643 1.9276 -3.6441 1.7165 -0.86438 0.540 0.100 0.100 0.8418 1.9878 -3.8158 1.828 -0.84189 0.560 0.100 0.100 0.8202 2.0672 -4.0223 1.9551 -0.820210 0.580 0.100 0.100 0.7995 2.1723 -4.2768 2.1045 -0.799511 0.600 0.100 0.100 0.7797 2.3136 -4.6004 2.2868 -0.7797

coordenadas(mm)punto 1θ& 2θ& 3θ& 4θ& 5θ&1θ& 2θ& 3θ& 4θ& 5θ&


3.6. Cálculo de las aceleraciones angulares de las articulaciones del MIRH 1

El cálculo de las aceleraciones angulares del manipulador es muy similar al cálculo de las velocidades angulares, debido a que nuevamente el sistema a resolver es un sistema lineal sobredeterminado.

El primer paso para el cálculo de las aceleraciones angulares, es derivar las ecuaciones de movimiento del manipulador dos veces con respecto al tiempo, para obtener el sistema matricial, que se resolverá para una aceleración lineal en el extremo libre del manipulador.

Al derivar las ecuaciones cinemáticas del manipulador, de la (3.26) a la (3.37) con respecto al tiempo, se obtienen las ecuaciones de control para la aceleración lineal en el extremo libre del manipulador, en función de las aceleraciones angulares de las articulaciones, como se presentan a continuación:

( ) ( ) ( ) ( ) ( )( )( ) ( ) ( ) ( ) ( )( ) ( )( ) ( ) ( ) ( ) ( ) ( ) ( )

( ) ( ) ( )( )( )

1 2 3 4 5 1 5 1

2 1 2 3 4 5 1 2 3 4 2 3 4 51

1 2 3 4 5 5 1 1 5 1 5 5

1 2 3 4 5 2

1

xn = -sen cos cos +cos sen

-cos cos cos +sen sen cos+

+sen cos sen -sen sen +cos cos

+ -cos sen cos

sen+


θ θ θ θ θ θ θ θ θ θ θ θ θ θθ


θ θ θ θ θ θ

θ θ

+ +

+ + + + + + + +

+ +

&&&&& & & &

&& & &&&

& ( ) ( ) ( ) ( ) ( ) ( )( ) ( ) ( )

( ) ( ) ( )( )( ) ( ) ( ) ( ) ( )( ) ( )( ) ( ) ( )

1 2 3 4 5 1 2 3 4 2 3 4 5

2

1 2 3 4 5 5

1 2 3 4 5 3

1 1 2 3 4 5 1 2 3 4 2 3 4 5

1 2 3 4 5 5

sen cos -cos cos cos

+cos sen sen

+ -cos sen cos

+sen sen cos -cos cos cos+

+cos sen sen

θ θ θ θ θ θ θ θ θ θ θ θθ

θ θ θ θ θ θ

θ θ θ θ θ θ


θ θ θ θ θ θ

+ + + + + + + +

+ +

+ + + + + +

+ +

& & &&

&&&

& & & &&

( ) ( ) ( )( )( ) ( ) ( ) ( ) ( )( ) ( )( ) ( ) ( )

( ) ( ) ( ) ( ) ( )( )( ) ( ) ( ) ( )

3

1 2 3 4 5 4

1 1 2 3 4 5 1 2 3 4 2 3 4 54

1 2 3 4 5 5

1 2 3 4 5 1 5 5

1 1 2 3 4 5 1

+ -cos sen cos


+cos sen sen

+ -cos cos sen +sen cos

+sen cos sen +cos+

θ

θ θ θ θ θ θ


θ θ θ θ θ θ



+ +

+ + + + + + + +

+ +

+ +

&

&&& & & &

&&

&&& ( )( ) ( )

( ) ( ) ( ) ( ) ( ) ( ) ( )2 3 4 2 3 4 5

5

1 2 3 4 5 5 1 1 5 1 5 5

sen sen

-cos cos cos +cos cos -sen sen

θ θ θ θ θ θ θθ


+ + + + + +

& & &&

& & &

(3.41)


( ) ( ) ( ) ( ) ( )( )( ) ( ) ( ) ( ) ( )( ) ( )( ) ( ) ( ) ( ) ( ) ( ) ( )( ) ( ) ( )( )( )

y 1 2 3 4 5 1 5 1

1 1 2 3 4 5 1 2 3 4 2 3 4 51

1 2 3 4 5 5 1 1 5 1 5 5

1 2 3 4 5 2

1

n = cos cos cos +sen sen

-sen cos cos -cos sen cos+

-cos cos sen +cos sen +sen cos

+ -sen sen cos

-cos+




θ θ θ θ θ θ

θ θ

+ +

+ + + + + + + +

+ +

&&&&& & & &

&& & &&&

& ( ) ( ) ( ) ( )( ) ( )( ) ( ) ( )

( ) ( ) ( )( )( ) ( ) ( ) ( ) ( )( ) ( )( ) ( ) ( )

1 2 3 4 5 1 2 3 4 2 3 4 5

2

1 2 3 4 5 5

1 2 3 4 5 3

1 1 2 3 4 5 1 2 3 4 2 3 4 5

1 2 3 4 5

sen cos -sen cos cos

+sen sen sen

+ -sen sen cos

-cos sen cos -sen cos cos+

+sen sen sen


θ θ θ θ θ θ

θ θ θ θ θ θ


θ θ θ θ θ θ

+ + + + + + + +

+ +

+ + + + + +

+ +

& & & &&&&

& & & & & & &&

( ) ( ) ( )( )( ) ( ) ( ) ( ) ( ) ( )( ) ( ) ( )

( ) ( ) ( ) ( ) ( )( )( ) ( ) ( ) ( )

3

5

1 2 3 4 5 4

2

1 1 2 3 4 5 1 2 3 4 54

1 2 3 4 5

1 2 3 4 5 1 5 5

1 1 2 3 4 5 1 2

+ -sen sen cos


+sen sen cos

+ -sen cos sen -cos cos

-cos cos sen +sen sen+

θ

θ θ θ θ θ θ

θ θ θ θ θ θ θ θ θ θ θθ

θ θ θ θ θ



+ +

+ + + + + +

+ +

+ + +

&

&&& &

&&& ( )( )

( ) ( ) ( ) ( ) ( ) ( ) ( )3 4 2 3 4 5

5

1 2 3 4 5 5 1 1 5 1 5 5

sen

-sen cos cos +sen cos +cos sen )

θ θ θ θ θθ


+ + + + +

& & & &&

& & &

(3.42)

( ) ( )( )( ) ( ) ( ) ( ) ( )( )

( ) ( )( )( ) ( ) ( ) ( ) ( )( )

( ) ( )( )( ) ( )

2 3 4 5 2

2 3 4 2 3 4 5 2 3 4 5 5 2

2 3 4 5 3

2 3 4 2 3 4 5 2 3 4 5 5 3

2 3 4 5 4

2 3 4 2 3 4

zn = cos cos

+ -sen cos -cos sen

+ cos cos

+ -sen cos -cos sen

+ cos cos

+ -sen cos

θ θ θ θ θ


θ θ θ θ θ


θ θ θ θ θ


+ +

+ + + + + +

+ +

+ + + + + +

+ +

+ + + +

&&&&& & & & &

&&& & & & &

&&& & & ( ) ( ) ( )( )

( ) ( )( )( ) ( ) ( ) ( ) ( )( )

5 2 3 4 5 5 4

2 3 4 5 5

2 3 4 2 3 4 5 2 3 4 5 5 5

-cos sen

+ -sen sen

+ -cos sen -sen cos

θ θ θ θ θ θ

θ θ θ θ θ


+ +

+ +

+ + + + + +

& &&&

& & & & &

(3.43)


( ) ( ) ( ) ( ) ( )( )( ) ( ) ( ) ( ) ( ) ( ) ( )

( ) ( ) ( ) ( ) ( ) ( ) ( )( ) ( ) ( )( )( )

1 2 3 4 5 1 5 1

1 1 2 3 4 5 1 2 3 4 2 3 4 5

1

1 2 3 4 5 5 1 1 5 1 5 5

1 2 3 4 5 2

1 1

xs = sen cos sen +cos cos

cos cos sen -sen cos sen+

+sen cos cos -sen cos -cos sen

+ cos sen sen

-sen s+




θ θ θ θ θ θ

θ θ

+ +

+ + + + + + + +

+ +

&&&&& & & &

&& & &

&&& ( ) ( ) ( ) ( )( ) ( )

( ) ( ) ( )( ) ( ) ( )( )

( ) ( ) ( ) ( ) ( ) ( ) ( )( ) ( ) ( )

2 3 4 5 1 2 3 4 2 3 4 5

2

1 2 3 4 5 5

1 2 3 4 5 3

1 1 2 3 4 5 1 2 3 4 2 3 4 5

1 2 3 4 5 5

en sen +cos cos sen

+cos sen cos

+ cos sen sen

(-sen sen sen +cos cos sen+

+cos sen cos


θ θ θ θ θ θ

θ θ θ θ θ θ


θ θ θ θ θ θ

+ + + + + + + +

+ +

+ + + + + + + +

& & &&

&&&

& & & &&

( ) ( ) ( )( )( ) ( ) ( ) ( ) ( )( ) ( )( ) ( ) ( )

( ) ( ) ( ) ( ) ( )( )( ) ( ) ( )

3

1 2 3 4 5 4

1 1 2 3 4 5 1 2 3 4 2 3 4 54

1 2 3 4 5 5

1 2 3 4 5 1 5 5

1 1 2 3 4 5

+ cos sen sen

-sen sen sen +cos cos sen+

+cos sen cos

+ -cos cos cos -sen sen

(sen cos cos +cos+

θ

θ θ θ θ θ θ


θ θ θ θ θ θ



+ +

+ + + + + + + +

+ +

+ +

&&

&&& & & &

&&

&&& ( ) ( )( ) ( )

( ) ( ) ( ) ( ) ( ) ( ) ( )1 2 3 4 2 3 4 5

5

1 2 3 4 5 5 1 1 5 1 5 5

sen cos

+cos cos sen -cos sen -sen cos



+ + + + + +

& & & && & &

(3.44)


( ) ( ) ( ) ( ) ( )( )( ) ( ) ( ) ( ) ( )( ) ( )( ) ( ) ( ) ( ) ( ) ( ) ( )

( ) ( ) ( )( )( )

y 1 2 3 4 5 1 5 1

1 1 2 3 4 5 1 2 3 4 2 3 4 51

1 2 3 4 5 5 1 1 5 1 5 5

1 2 3 4 5 2

1 1

s = -cos cos sen +sen cos

sen cos sen +cos sen sen+


+ sen sen sen

cos s+




θ θ θ θ θ θ

θ θ

+ +

+ + + + + + + +

+ +

&&&&& & & &

&& & &&&

& ( ) ( ) ( ) ( ) ( ) ( )( ) ( ) ( )

( ) ( ) ( )( )( ) ( ) ( ) ( ) ( ) ( ) ( )

( ) ( ) ( )

2 3 4 5 1 2 3 4 2 3 4 52

1 2 3 4 5 5

1 2 3 4 5 3

1 1 2 3 4 5 1 2 3 4 2 3 4 5

1 2 3 4 5 5

en sen +sen cos sen

+sen sen cos

+ sen sen sen

cos sen sen +sen cos sen+

+sen sen cos


θ θ θ θ θ θ

θ θ θ θ θ θ


θ θ θ θ θ θ

+ + + + + + + +

+ +

+ + + + + + + +

& & &&

&&&

& & & &&

( ) ( ) ( )( )( ) ( ) ( ) ( ) ( ) ( ) ( )

( ) ( ) ( )( ) ( ) ( ) ( ) ( )( )( ) ( ) ( ) ( )

3

1 2 3 4 5 4

1 1 2 3 4 5 1 2 3 4 2 3 4 54

1 2 3 4 5 5

1 2 3 4 5 1 5 5

1 1 2 3 4 5 1 2

+ sen sen sen


+sen sen cos

+ -sen cos cos +cos sen

-cos cos cos +sen sen+

θ

θ θ θ θ θ θ


θ θ θ θ θ θ



+ +

+ + + + + + + +

+ +

+ + +

&

&&& & & &

&&

&&& ( )( ) ( )

( ) ( ) ( ) ( ) ( ) ( ) ( )3 4 2 3 4 5

5

1 2 3 4 5 5 1 1 5 1 5 5

cos


θ θ θ θ θ θθ


+ + + + +

& & &&

& & & (3.45)

( ) ( )( )( )( ) ( ) ( ) ( )( )( ) ( )( )

( )( ) ( ) ( ) ( )( )( ) ( )( )

( )( ) ( )

2 3 4 5 2

2 3 4 2 3 4 5 2 3 4 5 5 2

2 3 4 5 3

2 3 4 2 3 4 5 2 3 4 5 5 3

2 3 4 5 4

2 3 4 2 3 4 5

zs = -cos sen

+ sen sen -cos cos

+ -cos sen

+ sen sen -cos cos d

+ -cos sen

+ sen sen

θ θ θ θ θ


θ θ θ θ θ


θ θ θ θ θ


+ +

+ + + + + +

+ +

+ + + + + +

+ +

+ + + +

&&&&& & & & &

&&& & & &

&&& & & ( )( )

( ) ( )( )( )( ) ( ) ( ) ( )( )

2 3 4 1 5 4

2 3 4 5 5

2 3 4 2 3 4 5 2 3 4 5 5 5

-cos cos

+ -sen cos

+ -cos cos +sen sen

θ θ θ θ θ θ

θ θ θ θ θ


+ +

+ +

+ + + + + +

& & &&&

& & & & &

(3.46)


( ) ( )( )( ) ( ) ( ) ( ) ( )( )

( ) ( )( )( ) ( ) ( ) ( ) ( )( )

( ) ( )( )( ) ( ) ( )

1 2 3 4 1

1 1 2 3 4 1 2 3 4 2 3 4 1

1 2 3 4 2

1 1 2 3 4 1 2 3 4 2 3 4 2

1 2 3 4 3

1 1 2 3 4 1

xa = -sen sen

+ -cos sen -sen cos

+ cos cos

+ -sen cos -cos sen

+ cos cos

+ -sen cos -cos s

θ θ θ θ θ


θ θ θ θ θ


θ θ θ θ θ

θ θ θ θ θ θ

+ +

+ + + + + +

+ +

+ + + + + +

+ +

+ +

&&&&& & & & &

&&& & & & &

&&& ( ) ( )( )

( ) ( )( )( ) ( ) ( ) ( ) ( )( )

2 3 4 2 3 4 3

1 2 3 4 4

1 1 2 3 4 1 2 3 4 2 3 4 4

en

+ cos cos

+ -sen cos -cos sen


θ θ θ θ θ


+ + + +

+ +

+ + + + + +

& & & &&&

& & & & &&

(3.47)

( ) ( )( )( ) ( ) ( ) ( ) ( )( )

( ) ( )( )( ) ( ) ( ) ( ) ( )( )( ) ( )( )( ) ( ) ( )

1 2 3 4 1

1 1 2 3 4 1 2 3 4 2 3 4 1

1 2 3 4 2

1 1 2 3 4 1 2 3 4 2 3 4 2

1 2 3 4 3

1 1 2 3 4 1

ya = cos sen

+ -sen sen +cos cos

+ sen cos

+ cos cos -sen sen

+ sen cos

+ cos cos -sen sen

θ θ θ θ θ


θ θ θ θ θ


θ θ θ θ θ


+ +

+ + + + + +

+ +

+ + + + + +

+ +

+ +

&&&&& & & & &

&&& & & & &

&&& ( ) ( )( )

( ) ( )( )( ) ( ) ( ) ( ) ( )( )

2 3 4 2 3 4 3

1 2 3 4 4

1 1 2 3 4 1 2 3 4 2 3 4 4

+ sen cos

+ cos cos -sen sen

θ θ θ θ θ θ

θ θ θ θ θ


+ + + +

+ +

+ + + + + +

& & & &&&

& & & & &&

(3.48)

( )( ) ( ) ( )( )( )( ) ( ) ( )( )( )( ) ( ) ( )( )

2 3 4 2 2 3 4 2 3 4 2

2 3 4 3 2 3 4 2 3 4 3

2 3 4 4 2 3 4 2 3 4 4

za = sen + cos

+ sen + cos

+ sen + cos




+ + + + + +

+ + + + + +

+ + + + + +

&& & & & &&&&& & & & &&& & & & &

(3.49)


( ) ( ) ( ) ( )( )( )( ) ( ) ( ) ( )( )( ) ( ) ( ) ( ) ( ) ( )( )

( ) ( ) ( ) ( )( )( )

1 2 3 4 5 2 3 2 1

1 1 2 3 4 5 2 3 2

1

1 2 3 4 2 3 4 5 2 3 2 3 2 2

1 2 3 4 5 2 3 2

x 3 2

3 2

3 2

3 2

p = -sen sen d +cos a +cos a

-cos sen d +cos a +cos a+

-sen cos d -sen a -sen a

+ cos cos d -sen a -sen a


θ θ θ θ θ θ θ θθ



+ + +

+ + + + + + + + +

+ + +

&&&&&

&& & & & & &

&&

( ) ( ) ( ) ( )( )( ) ( )( ) ( ) ( ) ( )( )

( ) ( ) ( )( )( )( ) ( ) ( )( )( ) ( )

2

1 1 2 3 4 5 2 3 2

2

1 2 3 4 2 3 4 5 2 3 2 3 2 3

1 2 3 4 5 2 3 3

1 1 2 3 4 5 2 3

1 2 3 4

3 2

3 2

3

3

-sen cos d -sen a -sen a+

+cos -sen d -cos a -cos a

cos cos d -sen a

-sen cos d -sen a+

+cos -sen





θ θ θ θ

+ + + + + + + + +

+ + + +

+ + +

+ +

&&

& & & &&&

&&( ) ( ) ( )( )

( ) ( )( )( ) ( ) ( ) ( )( )( )

3

2 3 4 5 2 3 2 3

1 2 3 4 5 4

1 1 2 3 4 5 1 2 3 4 2 3 4 5 4

3d -cos a

+ cos cos d

+ -sen cos d -cos sen d

θθ θ θ θ θ θ θ

θ θ θ θ θ


+ + + +

+ +

+ + + + + +

&& & & &

&&& & & & &

(3.50)

( ) ( ) ( ) ( )( )( )( ) ( )( ) ( )( ) ( )( ) ( )( )

( ) ( ) ( )

1 2 3 4 5 2 3 2 1

1 1

11 2 3 4 2 3 4 5 2 3 2 3 2 2

1 2 3 4 5 2 3

y 3 2

3 2

3

p = cos sen d +cos a +cos a

-sen sen(t2+t3+t4)*d5+cos(t2+t3)*a3+cos(t2)*a2+

+cos cos d -sen a -sen a

+ sen cos d -sen a -se


θ θθ


θ θ θ θ θ θ

+ + +

+ + + + + +

+ + +

&&&&&

&& & & & & &

( )( )( )( ) ( ) ( ) ( )( )

( ) ( ) ( ) ( ) ( ) ( )( )( ) ( ) ( )( )( )( ) ( ) ( )( )( )

2 2

1 1 2 3 4 5 2 3 2

2

1 2 3 4 2 3 4 5 2 3 2 3 2 2

1 2 3 4 5 2 3 3

1 1 2 3 4 5 2 3

1

2

3 2

3 2

3

3

n a

cos cos d -sen a -sen a+

+sen -sen d -cos a -cos a

+ sen cos d -sen a

(cos cos d -sen a+

+sen -sen

θ θ





θ

+ + + + + + + + +

+ + +

+ + +

&&&

&& & & & & &

&&&

( ) ( ) ( ) ( )( )( ) ( )( )( ) ( ) ( ) ( ) ( )( )

3

2 3 4 2 3 4 5 2 3 2 3

1 2 3 4 5 4

1 1 2 3 4 5 1 2 3 4 2 3 4 5 4

3d -cos a

+ sen cos d

+ cos cos d -sen sen d

θθ θ θ θ θ θ θ θ θ θ

θ θ θ θ θ


+ + + + + +

+ +

+ + + + + +

&& & & & &

&&& & & & &

(3.51)


( ) ( ) ( )( )( ) ( ) ( ) ( )( )( ) ( )( )( ) ( ) ( )( )( )( )( )( )

2 3 4 5 2 3 2 2

22 3 4 5 2 3 2 3 2 2 2

2 3 4 5 2 3 3

2 3 4 2 3 4 5 2 3 2 3 3

2 3 4 5 4

2 3 4 2 3

z 3 2

3 2

3

3

p = sen d +cos a +cos a

+ cos d -sen a -sen a

+ sen d +cos a

+ cos d -sen a

+ sen d

+ cos



θ θ θ θ θ θ


θ θ θ θ

θ θ θ θ θ

+ + +

+ + + +

+ + +

+ + + + + +

+ +

+ + +

&&&&& & & &

&&& & & & & &

&&& &( )( )4 5 4dθ θ+ & &

(3.52)

Este sistema se resuelve de la misma manera que se resolvieron las velocidades angulares. Por lo que el sistema de 12 ecuaciones con 5 incógnitas puede ser resuelto por medio de la fórmula (3.40):

La matriz A estará formada por los elementos de las ecuaciones (3.41) a la (3.52), representadas matricialmente y despejando los valores conocidos como son, los valores angulares de las articulaciones y los valores de las velocidades angulares, pasando estos a ser parte del vector B.

El vector B, será la aceleración lineal para la cual se requieren encontrar las aceleraciones angulares contenidas en el vector θ&& , nuevamente se vuelve a tener un sistema sobredeterminado, de 12 ecuaciones con 5 incógnitas, por lo que se deberá optimizar haciendo uso de la matriz pseudo inversa, así que de la ecuación (3.40) se tiene:

( ) 1T TA A A Bθ−

=&& i i (3.53)

Se desea una aceleración en el extremo libre del manipulador de 10 2m seg , por lo tanto esta aceleración, deberá descomponerse en sus componentes ortogonales, siendo de 0.0577 m/seg2, en cada una.

Como el vector B esta formado por los elementos ya conocidos de las ecuaciones (3.41) a (3.52); los valores angulares de las articulaciones, y los cambios de los mismos con respecto al tiempo, estos serán restados al valor de la aceleración lineal deseado, por lo que el vector B, quedara de la siguiente manera:


( ) ( ) ( ) ( ) ( ) ( ) ( )( ) ( ) ( ) ( ) ( ) ( ) ( )

( ) ( ) ( ) ( ) ( )( ) ( )( )

1

2 1 2 3 4 5 1 2 3 4 2 3 4 5

1

1 2 3 4 5 5 1 1 5 1 5 5

1 1 2 3 4 5 1 2 3 4 2 3 4 5

1 2

xB n



sen sen cos -cos cos cos+

+cos sen




θ θ

= −

+ + + + + + + +

+ + + + + +

&&& & & &

&& & &

& & & &

( ) ( )

( ) ( ) ( ) ( ) ( )( ) ( )( ) ( ) ( )( ) ( ) ( ) ( ) ( )( ) ( )( ) ( )

2

3 4 5 5

1 1 2 3 4 5 1 2 3 4 2 3 4 53

1 2 3 4 5 5

1 1 2 3 4 5 1 2 3 4 2 3 4 5

1 2 3 4

sen


+cos sen sen


+cos sen sen

θθ θ θ θ


θ θ θ θ θ θ


θ θ θ θ θ

+ + + + + + + + + +

+ + + + + +

+ +

&&

& & & &&

&& & & &

( )( ) ( ) ( ) ( ) ( ) ( ) ( )

( ) ( ) ( ) ( ) ( ) ( ) ( )

4

5 5

1 1 2 3 4 5 1 2 3 4 2 3 4 55

1 2 3 4 5 5 1 1 5 1 5 5

+sen cos sen +cos sen sen+


θθ



+ + + + + + + +

&&

& & & &&

& & &

( ) ( ) ( ) ( ) ( )( ) ( )( ) ( ) ( ) ( ) ( ) ( ) ( )( ) ( ) ( ) ( ) ( )( ) ( )( )

2 y

1 1 2 3 4 5 1 2 3 4 2 3 4 51

1 2 3 4 5 5 1 1 5 1 5 5

1 1 2 3 4 5 1 2 3 4 2 3 4 5

1

n -

-sen cos cos -cos sen cos+

-cos cos sen +cos sen +sen cos


+sen sen

B




θ θ

=

+ + + + + + + +

+ + + + + +

&&& & & & &

& & && & & &

( ) ( )

( ) ( ) ( ) ( ) ( )( ) ( )( ) ( ) ( )

( ) ( ) ( ) ( ) ( ) ( )( ) ( ) ( )

2

2 3 4 5 5

1 1 2 3 4 5 1 2 3 4 2 3 4 5

3

1 2 3 4 5 5

21 1 2 3 4 5 1 2 3 4 5

1 2 3 4 5

sen


+sen sen sen


+sen sen cos

θθ θ θ θ


θ θ θ θ θ θ


θ θ θ θ θ

+ + + + + + + + + +

+ + + + + +

&&

& & & & & & & &&

&

( ) ( ) ( ) ( ) ( )( )( ) ( ) ( ) ( ) ( ) ( ) ( )

4

1 1 2 3 4 5 1 2 3 4 2 3 4 55

51 2 3 4 5 5 1 1 5 1 5 5

-cos cos sen +sen sen sen+

-sen cos cos +sen cos +cos sen )

θ



+ + + + + + + +

&

& & & & & && & &

( ) ( ) ( ) ( ) ( )( )( ) ( ) ( ) ( ) ( )( )( ) ( ) ( ) ( ) ( )( )( )( ) ( )

3

2 3 4 2 3 4 5 2 3 4 5 5 2

2 3 4 2 3 4 5 2 3 4 5 5 3

2 3 4 2 3 4 5 2 3 4 5 5 4

2 3 4 2 3 4 5 2

zB n

+ -sen cos -cos sen

+ -sen cos -cos sen

+ -sen cos -cos sen

+ -cos sen -sen





= −

+ + + + + +

+ + + + + +

+ + + + + +

+ + + + +

&&& & & & && & & & && & & & &

& & & ( ) ( )( )3 4 5 5 5cosθ θ θ θ θ+ & &


( ) ( ) ( ) ( ) ( ) ( ) ( )( ) ( ) ( ) ( ) ( ) ( ) ( )

( ) ( ) ( ) ( ) ( )( ) ( )( )

4

1 1 2 3 4 5 1 2 3 4 2 3 4 5

1

1 2 3 4 5 5 1 1 5 1 5 5

1 1 2 3 4 5 1 2 3 4 2 3 4 5

1 2

xB s

cos cos sen -sen cos sen+

+sen cos cos -sen cos -cos sen


+cos sen




θ θ

= −

+ + + + + + + +

+ + + + + +

&&& & & &

&& & &

& & & &

( ) ( )( ) ( ) ( ) ( ) ( ) ( ) ( )( ) ( ) ( )

( ) ( ) ( ) ( ) ( )( ) ( )( ) ( )

2

3 4 5 5

1 1 2 3 4 5 1 2 3 4 2 3 4 5

3

1 2 3 4 5 5

1 1 2 3 4 5 1 2 3 4 2 3 4 5

1 2 3 4

cos

(-sen sen sen +cos cos sen+

+cos sen cos


+cos sen co

θθ θ θ θ


θ θ θ θ θ θ


θ θ θ θ

+ + + + + + + + + +

+ + + + + +

+ +

&&

& & & &&&

&& & & &

( )( ) ( ) ( ) ( ) ( )( ) ( )( ) ( ) ( ) ( ) ( ) ( ) ( )

4

5 5

1 1 2 3 4 5 1 2 3 4 2 3 4 5

55

1 2 3 4 5 5 1 1 5 1 5 5

s

(sen cos cos +cos sen cos+

+cos cos sen -cos sen -sen cos

θθ θ



+ + + + + + + +

&&

& & & &&

& & &

( ) ( ) ( ) ( ) ( )( ) ( )( ) ( ) ( ) ( ) ( ) ( ) ( )

( ) ( ) ( ) ( ) ( ) ( ) ( )( )

5 y

1 1 2 3 4 5 1 2 3 4 2 3 4 51

1 2 3 4 5 5 1 1 5 1 5 5

1 1 2 3 4 5 1 2 3 4 2 3 4 5

1 2

s -

sen cos sen +cos sen sen+



+sen sen

B




θ θ

=

+ + + + + + + +

+ + + + + +

+

&&& & & &

&& & &

& & & &

( ) ( )( ) ( ) ( ) ( ) ( ) ( ) ( )

( ) ( ) ( )( ) ( ) ( ) ( ) ( ) ( ) ( )

( ) ( ) ( )

2

3 4 5 5

1 1 2 3 4 5 1 2 3 4 2 3 4 53

1 2 3 4 5 5

1 1 2 3 4 5 1 2 3 4 2 3 4 5

1 2 3 4 5

cos


+sen sen cos


+sen sen cos

θθ θ θ θ


θ θ θ θ θ θ


θ θ θ θ θ θ

+ + + + + + + + +

+ + + + + +

+ +

&&

& & & &&

&& & & &

&

( ) ( ) ( ) ( ) ( )( ) ( )( ) ( ) ( ) ( ) ( ) ( ) ( )

4

5

1 1 2 3 4 5 1 2 3 4 2 3 4 55

1 2 3 4 5 5 1 1 5 1 5 5



θ



+ + + + + + + +

&

& & & &&

& & &


( )( ) ( ) ( ) ( )( )( )( ) ( ) ( ) ( )( )( )( ) ( ) ( )( )( )( ) ( )

6

2 3 4 2 3 4 5 2 3 4 5 5 2

2 3 4 2 3 4 5 2 3 4 5 5 3

2 3 4 2 3 4 5 2 3 4 1 5 4

2 3 4 2 3 4 5 2 3

zB s

+ sen sen -cos cos

+ sen sen -cos cos d

+ sen sen -cos cos

+ -cos cos +sen





= −

+ + + + + +

+ + + + + +

+ + + + + +

+ + + + + +

&&& & & & && & & && & & & & &

& & & ( ) ( )( )4 5 5 5senθ θ θ θ& &

( ) ( ) ( ) ( ) ( )( )( ) ( ) ( ) ( ) ( )( )( ) ( ) ( ) ( ) ( )( )( ) ( ) ( )

7

1 1 2 3 4 1 2 3 4 2 3 4 1

1 1 2 3 4 1 2 3 4 2 3 4 2

1 1 2 3 4 1 2 3 4 2 3 4 3

1 1 2 3 4 1 2 3

xB a

+ -cos sen -sen cos

+ -sen cos -cos sen

+ -sen cos -cos sen

+ -sen cos -cos sen





= −

+ + + + + +

+ + + + + +

+ + + + + +

+ + + +

&&& & & & && & & & && & & & &

& ( ) ( )( )4 2 3 4 4θ θ θ θ θ+ +& & & &&

( ) ( ) ( ) ( ) ( )( )( ) ( ) ( ) ( ) ( )( )( ) ( ) ( ) ( ) ( )( )( ) ( ) ( ) ( )

8

1 1 2 3 4 1 2 3 4 2 3 4 1

1 1 2 3 4 1 2 3 4 2 3 4 2

1 1 2 3 4 1 2 3 4 2 3 4 3

1 1 2 3 4 1 2 3 4

yB a

+ -sen sen +cos cos

+ cos cos -sen sen

+ cos cos -sen sen

+ cos cos -sen sen





= −

+ + + + + +

+ + + + + +

+ + + + + +

+ + + +

&&& & & & &

& & & & && & & & &

& &( )( )2 3 4 4θ θ θ θ+ +& & &&

( )( )( )( ) ( )( )( ) ( )( )

9 2 3 4 2 3 4 2

2 3 4 2 3 4 3

2 3 4 2 3 4 4

zB a cos

+ cos

+ cos




= − + + + +

+ + + +

+ + + +

& & & &&&& & & && & & &


( ) ( ) ( ) ( )( )( ) ( )( ) ( ) ( ) ( )( )( ) ( ) ( ) ( )( )( ) ( ) ( )

10

1 1 2 3 4 5 2 3 2

1

1 2 3 4 2 3 4 5 2 3 2 3 2 2

1 1 2 3 4 5 2 3 2

1 2 3 4 2 3 4 5 2

x

3 2

3 2

3 2

B p

-cos sen d +cos a +cos a+

-sen cos d -sen a -sen a

-sen cos d -sen a -sen a+

+cos -sen d -cos





= −

+ + + + + + + + +

+ + +

+ + + +

&&&

&& & & & & &

&& & & ( ) ( ) ( )( )

( ) ( ) ( )( )( ) ( ) ( ) ( ) ( )( )

( ) ( ) ( ) ( )( )( )

2

3 2 3 2 3

1 1 2 3 4 5 2 3

3

1 2 3 4 2 3 4 5 2 3 2 3

1 1 2 3 4 5 1 2 3 4 2 3 4 5 4

3 2

3

3

a -cos a

-sen cos d -sen a+

+cos -sen d -cos a

+ -sen cos d -cos sen d

θθ θ θ θ θ




+ + + + + + + + + + +

+ + + + + +

&&

&&

& & & & &

& & & & &

( ) ( )( ) ( )( ) ( )( ) ( )( )

( ) ( ) ( ) ( )( )( ) ( ) ( )

11

1 1

1

1 2 3 4 2 3 4 5 2 3 2 3 2 2

1 1 2 3 4 5 2 3 2

1 2 3 4 2 3 4

y

3 2

3 2

B p

-sen sen(t2+t3+t4)*d5+cos(t2+t3)*a3+cos(t2)*a2+

+cos cos d -sen a -sen a

cos cos d -sen a -sen a+

+sen -sen

θ θθ




= −

+ + + + + +

+ + +

+ + + +

&&&

&& & & & & &

&& & & ( ) ( ) ( )( )

( ) ( ) ( )( )( ) ( ) ( ) ( ) ( )( )

( ) ( ) ( ) ( ) ( )( )

2

5 2 3 2 3 2 2

1 1 2 3 4 5 2 3

3

1 2 3 4 2 3 4 5 2 3 2 3

1 1 2 3 4 5 1 2 3 4 2 3 4 5 4

3 2

3

3

d -cos a -cos a

(cos cos d -sen a+

+sen -sen d -cos a

+ cos cos d -sen sen d

θθ θ θ θ θ θ




+ + + + + + + + + + +

+ + + + + +

&& & &

&&

& & & & &

& & & & &

( ) ( ) ( ) ( )( )( ) ( ) ( )( )( )( ) ( )( )

12

22 3 4 5 2 3 2 3 2 2 2

2 3 4 2 3 4 5 2 3 2 3 3

2 3 4 2 3 4 5 4

z

3 2

3

B p

+ cos d -sen a -sen a

+ cos d -sen a

+ cos d




= −

+ + + +

+ + + + + +

+ + + +

&&& & & &

& & & & & &

& & & &


1

2

3

4

5

6

7

8

9

10

11

12

BB

BBBB

BBBBBBB

=

(3.54)

La matriz A, queda entonces en función de las coordenadas generalizadas únicamente, como se presenta a continuación:

( ) ( ) ( ) ( ) ( )( ) ( ) ( ) ( ) ( )

( ) ( ) ( ) ( ) ( )( ) ( ) ( ) ( ) ( )

( ) ( )

1 2 3 4 5 1 5

1 2 3 4 5 1 5

1 2 3 4 5 1 5

1 2 3 4 5 1 5

1 2 3 4

A(1,1)=-sen cos cos +cos sen

A(2,1)=cos cos cos +sen sen

A(3,1)=0

A(4,1)=sen cos sen +cos cos

A(5,1)=-cos cos sen +sen cos

A(6,1)=0

A(7,1)=-sen sen

A(





θ θ θ θ

+ +

+ +

+ +

+ +

+ +

( ) ( )

( ) ( ) ( ) ( )( )( ) ( ) ( ) ( )( )

1 2 3 4

1 2 3 4 5 2 3 3 2 2

1 2 3 4 5 2 3 3 2 2

8,1)=cos sen

A(9,1)=0

A(10,1)=-sen sen d +cos a +cos a

A(11,1)=cos sen d +cos a +cos a

A(12,1)=0

θ θ θ θ



+ +

+ + +

+ + +


( ) ( ) ( )( ) ( ) ( )

( ) ( )( ) ( ) ( )( ) ( ) ( )( ) ( )

( ) ( )( )

1 2 3 4 5

1 2 3 4 5

2 3 4 5

1 2 3 4 5

1 2 3 4 5

2 3 4 5

1 2 3 4

1

A(1,2)=-cos sen cos

A(2,2)=-sen sen cos

A(3,2)=cos cos

A(4,2)=cos sen sen

A(5,2)=sen sen sen

A(6,2)=-cos sen

A(7,2)=cos cos

A(8,2)=sen cos

θ θ θ θ θ

θ θ θ θ θ

θ θ θ θ

θ θ θ θ θ

θ θ θ θ θ

θ θ θ θ

θ θ θ θ

θ θ

+ +

+ +

+ +

+ +

+ +

+ +

+ +

( )( )

( ) ( ) ( ) ( )( )( ) ( ) ( ) ( )( )( ) ( ) ( )

2 3 4

2 3 4

1 2 3 4 5 2 3 3 2 2

1 2 3 4 5 2 3 3 2 2

2 3 4 5 2 3 3 2 2

A(9,2)=sen

A(10,2)=cos cos d -sen a -sen a

A(11,2)=sen cos d -sen a -sen a

A(12,2)=sen d +cos a +cos a

θ θ

θ θ θ



θ θ θ θ θ θ

+ +

+ +

+ + +

+ + +

+ + +

( ) ( ) ( )( ) ( ) ( )

( ) ( )( ) ( ) ( )( ) ( ) ( )( ) ( )

( ) ( )( )

1 2 3 4 5

1 2 3 4 5

2 3 4 5

1 2 3 4 5

1 2 3 4 5

2 3 4 5

1 2 3 4

1

A(1,3)=-cos sen cos

A(2,3)=-sen sen cos

A(3,3)=cos cos

A(4,3)=cos sen sen

A(5,3)=sen sen sen

A(6,3)=-cos sen

A(7,3)=cos cos

A(8,3)=sen cos

θ θ θ θ θ

θ θ θ θ θ

θ θ θ θ

θ θ θ θ θ

θ θ θ θ θ

θ θ θ θ

θ θ θ θ

θ θ

+ +

+ +

+ +

+ +

+ +

+ +

+ +

( )( )

( ) ( ) ( )( )( ) ( ) ( )( )( ) ( )

2 3 4

2 3 4

1 2 3 4 5 2 3 3

1 2 3 4 5 2 3 3

2 3 4 5 2 3 3

A(9,3)=sen

A(10,3)=cos cos d -sen a

A(11,3)=sen cos d -sen a

A(12,3)=sen d +cos a

θ θ

θ θ θ

θ θ θ θ θ θ

θ θ θ θ θ θ

θ θ θ θ θ

+ +

+ +

+ + +

+ + +

+ + +


( ) ( ) ( )( ) ( ) ( )

( ) ( )( ) ( ) ( )( ) ( ) ( )( ) ( )

( ) ( )( )

1 2 3 4 5

1 2 3 4 5

2 3 4 5

1 2 3 4 5

1 2 3 4 5

2 3 4 5

1 2 3 4

1

A(1,4)=-cos sen cos

A(2,4)=-sen sen cos

A(3,4)=cos cos

A(4,4)=cos sen sen

A(5,4)=sen sen sen

A(6,4)=-cos sen

A(7,4)=cos cos

A(8,4)=sen cos

θ θ θ θ θ

θ θ θ θ θ

θ θ θ θ

θ θ θ θ θ

θ θ θ θ θ

θ θ θ θ

θ θ θ θ

θ θ

+ +

+ +

+ +

+ +

+ +

+ +

+ +

( )( )

( ) ( )( ) ( )( )

2 3 4

2 3 4

1 2 3 4 5

1 2 3 4 5

2 3 4 5

A(9,4)=sen

A(10,4)=cos cos d

A(11,4)=sen cos d

A(12,4)=sen d

θ θ

θ θ θ

θ θ θ θ

θ θ θ θ

θ θ θ

+ +

+ +

+ +

+ +

+ +

( ) ( ) ( ) ( ) ( )( ) ( ) ( ) ( ) ( )( ) ( )( ) ( ) ( ) ( ) ( )( ) ( ) ( ) ( ) ( )

1 2 3 4 5 1 5

1 2 3 4 5 1 5

2 3 4 5

1 2 3 4 5 1 5

1 2 3 4 5 1 5

2 3

A(1,5)=-cos cos sen +sen cos

A(2,5)=-sen cos sen -cos cos

A(3,5)=-sen sen

A(4,5)=-cos cos cos -sen sen

A(5,5)=-sen cos cos +cos sen

A(6,5)=-sen



θ θ θ θ



θ θ

+ +

+ +

+ +

+ +

+ +

+( ) ( )4 5cos

A(7,5)=0A(8,5)=0A(9,5)=0A(10,5)=0A(11,5)=0A(12,5)=0

θ θ+


Al sustituir los valores conocidos, y resolviendo la ecuación (3.53), se obtienen los valores de las aceleraciones angulares para el punto en cuestión de la trayectoria creada, para realizar este proceso se hizo un programa utilizando el software Matlab 7.0, en el que se resuelven las aceleraciones para los once puntos de la trayectoria deseada, y está incluido en los anexos de la presente tesis con el nombre de aceleraciones.m.

Los valores de las aceleraciones angulares para cada uno de los puntos de la trayectoria, se presentan en la tabla III.6.

Tabla III.6

X Y Z

1 0.400 0.100 0.100 -2.4378 3.5054 -2.5255 -0.9799 2.87972 0.420 0.100 0.100 -2.1984 3.8507 -3.0616 -0.7892 2.64933 0.440 0.100 0.100 -1.9856 4.2251 -3.663 -0.5621 2.44534 0.460 0.100 0.100 -1.7962 4.6411 -4.3505 -0.2906 2.26485 0.480 0.100 0.100 -1.6273 5.1158 -5.1532 0.0374 2.10526 0.500 0.100 0.100 -1.4763 5.6729 -6.113 0.4401 1.96457 0.520 0.100 0.100 -1.341 6.3471 -7.2922 0.9452 1.8418 0.540 0.100 0.100 -1.2196 7.1909 -8.7874 1.5965 1.73379 0.560 0.100 0.100 -1.1103 8.2884 -10.7547 2.4663 1.6417

10 0.580 0.100 0.100 -1.0117 9.7826 -13.4633 3.6806 1.565211 0.600 0.100 0.100 -0.9226 11.9363 -17.4116 5.4753 1.5051

coordenadas(mm)punto

1θ&& 2θ&& 4θ&& 5θ&&2rad/seg 2rad/seg 2rad/seg 2rad/seg 2rad/seg

3θ&&

Se ha completado el cálculo de la cinemática, calculando primero la cinemática directa, tomando como datos, los valores angulares de las articulaciones, y como incógnitas las coordenadas en el extremo libre del manipulador. Después se calculo la cinemática inversa, en la que se toman como datos las coordenadas del extremo libre del manipulador, y como incógnitas los valores angulares de las articulaciones.

Se obtuvieron los valores numéricos de los ángulos de las articulaciones para una trayectoria de once puntos, así como las velocidades angulares para que esta trayectoria se recorriera a una velocidad de 1 m/seg y a una aceleración de 10 m/seg2.

Estos valores servirán para el calculo de las fuerzas generalizadas del manipulador, mismo que se llevara a cabo en el capitulo 4, donde se trata el calculo de la dinámica del manipulador.


[1] CRAIG John J. INTRODUCTION TO ROBOTICS MECHANICS AND CONTROL. EUA. Adison Wesley 1989.

[2] FU, K.S. GONZALEZ, LEE ROBÓTICA: CONTROL DETECCION, VISIÓN E INTELIGENCIA. España. Mc Graw Hill 1990.

[3] OLLERO, INTRODUCCIÓN A LA ROBÓTICA, Madrid.Marcombo 1998

[4] SHOICHIRO Nakamura. ANALISIS Y VISUALIZACION GRAFICA CON MATLAB. México. Parson educación 1997.

[5] ANGELES, Jorge FUNDAMENTALS OF ROBOTIC MECHANICAL SYSTEMS. Canada. Springer 1997.

[6] RICHARD P. Paul. ROBOT MANIPULATOR MATHEMATICS PROGRAMMING AND CONTROL. England. MIT Cambridge 1986.


Página 87

CAPÍTULO 4. CÁLCULO DE LA DINAMICA DEL

MANIPULADOR INDUSTRIAL ROBÓTICO

HIDRÁULICO (MIRH1)

La dinámica es la rama de la física que estudia el movimiento y las causas que lo generan. En el capitulo anterior se obtuvieron las ecuaciones que controlan los parámetros cinemáticas del manipulador, necesarios para el calculo de las fuerzas generalizadas.

En el presente capitulo se obtendrán las ecuaciones dinámicas del manipulador, ecuaciones que controlan las fuerzas generalizadas en las articulaciones para mover los eslabones del robot. Estas ecuaciones se obtienen al emplear la metodología de Lagrange- Euler [1], metodología que se desarrolla a partir del cálculo total de la energía necesaria para cambiar de posición, está se dividide fundamentalmente en dos: energía cinética y energía potencial.

La metodología de Lagrange-Euler, es simple y sistemática, debido a que supone movimientos de cuerpos rígidos, sin considerar las fuerzas internas que puede haber entre ellos, y considerando un sistema conservativo, genera un conjunto de ecuaciones diferenciales no lineales, acopladas de segundo orden.

Las ecuaciones diferenciales de movimiento, proporcionan ecuaciones de estado explícitas para la dinámica del manipulador, y pueden ser utilizadas para analizar y diseñar estrategias de control avanzadas, en el espacio de las variables de articulación. En una menor medida, se están utilizando para resolver el problema dinámico directo, esto es, dadas las fuerzas o pares deseados, se utilizan las ecuaciones dinámicas para resolver las aceleraciones de las articulaciones, que se integran a continuación para obtener las velocidades y coordenadas generalizadas, o para el problema dinámico inverso, que es, a partir de las coordenadas deseadas y sus primeras dos derivadas respecto al tiempo, se calculan las fuerzas generalizadas.

La derivación de las ecuaciones dinámicas de un manipulador de n grados de libertad, se basa en la comprensión de dos factores:

El primero es la matriz de transformación de coordenadas homogénea A, que describe la relación espacial entre los sistemas de coordenadas de dos elementos, relacionando así un punto fijo en el elemento i expresado en coordenadas homogéneas con respecto al sistema de coordenadas i-ésimo, en el sistema de coordenadas (i-1)-ésimo.

El segundo es la ecuación de Lagrange-Euler

1,5,3,4,5ii i

d L Li

dt q qτ

∂ ∂− = = ∂ ∂ & (4.1)


Página 88

Donde:

L Función lagrangiana

L K P= − (4.1b)

K Energía cinética

P Energía potencial

iq Coordenada generalizada del brazo

iq& Primera derivada con respecto al tiempo de la coordenada generalizada iq

iτ Fuerza generalizada aplicada al sistema en la articulación i, para mover el elemento i

Para el caso de articulaciones giratorias, i iq θ= , que es el margen del ángulo de articulación.

4.1 Cálculo de la energía cinética del manipulador

El cálculo de la energía cinética requiere conocer el valor de la velocidad de cada articulación, por lo que el primer paso será deducir la velocidad de un punto fijo del elemento en cuestión, así como los efectos que provocarán los movimientos de otra articulación, sobre todos los puntos de este elemento.

De lo anterior, se requiere fijar un punto en reposo del elemento de estudio i, expresado en coordenadas homogéneas de la base, con respecto al sistema de coordenadas del mismo elemento, obteniendo un vector de posición de este punto, con respecto al marco de referencia del mismo eslabón i

ir y, un vector de posición con respecto al sistema de

coordenadas de la base 0ir , está relación se hace utilizando las matriz de transformación del

manipulador, así pues se tendrá:

0 0 ii i ir A r= (4.2)

Al derivar el vector de posición 0ir , con respecto al tiempo, se obtiene la velocidad de

ese eslabón con respecto al sistema de coordenadas de la base:

( ) ( )0

0 0 0

1

ii ii

i i i i i j ij j

Ad dv v r A r q r

dt dt q=

∂≡ = = =

∑ & (4.3)

La derivada del vector de posición iir con respecto al tiempo, es igual a cero al ser de

dimensiones constantes.


Página 89

La derivada parcial de 0iA con respecto a jq , se puede obtener fácilmente con ayuda

de una matriz iQ , que para el caso de articulaciones de revolución se define como:

0 1 0 01 0 0 00 0 0 00 0 0 0

iQ

− =

(4.4)

Entonces, se tiene que

11

iii

i ij

AQ A

q

−−∂

=∂

(4.5)

De la ecuación (4.3), se deduce que:

00 1 -2 -1 -1

1 2 1... ...j j jij j j j

j

AA A A Q A A

q −∂

=∂

(4.6)

Para simplificar notaciones se define:

0 -11

jij j j iU A Q A−= (4.7)

Por lo tanto, al sustituir en la ecuación (4.2) se tiene:

1

ii

i ij j ij

v U q r=

=

∑ & (4.8)

La derivada parcial de -1iiA , con respecto a iq& , resulta en una matriz sin estructura, esto

es que el pre multiplicar -1iiA , por iQ , es equivalente a intercambiar los elementos de las

dos primeras filas de -1iiA , negando todos los elementos de la primera fila, y anulando todos

los elementos de las filas tercera y cuarta.

De la ecuación (4.8) se obtienen las velocidades de los cinco eslabones del manipulador, por lo que las ecuaciones serán:

( )( )( )( )

11 11 1 1

22 11 1 21 1 12 2 22 2 2

33 11 1 21 1 31 1 22 2 32 2 33 3 3

44 11 1 21 1 31 1 41 1 22 2 32 2 42 2 33 3 43 3 44 4 4

5 11 1 21 1 31 1 41 1 5

v U q r

v U q U q U q U q r

v U q U q U q U q U q U q r

v U q U q U q U q U q U q U q U q U q U q r

v U q U q U q U q U

=

= + + +

= + + + + +

= + + + + + + + + +

= + + + +

&& & & && & & & & && & & & & & & & & && & & &( ) 5

1 1 22 2 32 2 42 2 52 2 33 3 43 3 53 3 53 3 44 4 54 4 55 5 5q U q U q U q U q U q U q U q U q U q U q U q r+ + + + + + + + + + +& & & & & & & & & & & &


Página 90

Para encontrar los efectos de interacción entre las articulaciones, se tiene que:

0 -1 -11 1

ij j kijk j j k k i

k

UU A Q A Q A

q − −

∂= =

∂ (4.9)

La ecuación (4.9) se interpreta como los efectos de interacción del movimiento de la articulación j y k, sobre todos los puntos en el elemento i

La energía cinética del manipulador estará dada por la ecuación:

( )2 2 21 1 1

12

dK x y z dm= + +& & & (4.10)

( )12

Ti idK Tr v v dm= (4.11)

La traza en la ecuación anterior, es utilizada en lugar de un producto escalar de vectores para formar el tensor de inercia.

Al sustituir las velocidades en la ecuación (4.11), se tiene:

1 1

12

Ti ii i

ip p i ir r ip r

dK Tr U q r U q r dm= =

=

∑ ∑& & (4.12)

1 1

12

i ii i T T

ip i i ir p rp r

dK Tr U r r U q q dm= =

=

∑∑ & & (4.13)

( )1 1

12

i ii i T T

ip i i ir p rp r

dK Tr U rdm r U q q= =

=

∑∑ & & (4.14)

La matriz ipU es la velocidad de cambio de los puntos. ( )iir sobre el elemento i,

relativo al sistema de coordenadas de la base cuando jq cambia. Es constante para todos los puntos del elemento i e independientemente de la distribución de masa del elemento i. También, iq& son independientes de la distribución de masa del elemento i, así que, sumando, todas las energías cinéticas de todos los elementos y poniendo la integral dentro de los corchetes se tiene:

( )1 1

12

i ii i T T

i i ip i i ir p rp r

K dK Tr U rdm r U q q= =

= =

∑∑∫ ∫ & & (4.15)

El término i i Ti irdm r∫ es mejor conocido como la matriz de inercia de todos los puntos

del elemento i, por lo que:


Página 91

2

2

2

i i i i i i

i i i i i ii i Ti i i

i i i i i i

i i i

x dm x y dm x z dm x dm

x y dm y dm y z dm y dmJ rdm r

x z dm y z dm z dm z dm

x dm y dm z dm dm

= =

∫ ∫ ∫ ∫∫ ∫ ∫ ∫

∫∫ ∫ ∫ ∫∫ ∫ ∫ ∫

(4.16)

Expresando iJ como un tensor de inercia, se tiene:

2

2

2

xx yy zzxy xz i i

xx yy zzxy yz i i

i

xx yy zzxz yz i i

i i i i i i i

I I II I m x

I I II I m y

JI I I

I I m z

m x m y m z m

− + +

− + =

+ −

(4.17)

Así pues, la energía cinética total del manipulador K es:

( )1 1 1

12

n i iT

ip i ir p ri p r

K Tr U J U q q= = =

=

∑ ∑∑ & & (4.18)

Desarrollando la ecuación (4.18), se tiene:

( )1 1 1

12

n i iT

ip i ir p ri p r

K Tr U J U q q= = =

= ∑∑∑ & & (4.19)

Para n = 1,2,3,4,5


Página 92

( ) ( ) ( ) ( )( ) ( ) ( ) ( )( ) ( ) ( )

11 1 11 1 1 21 2 21 1 1 21 2 22 1 2 22 2 21 2 1

22 2 22 2 2 31 3 31 1 1 31 3 32 1 2 31 3 33 1 3

32 3 31 2 1 32 3 32 2 2 32 3 33 2

1K=

2

T T T T

T T T T

T T T

Tr U J U Tr U J U Tr U J U Tr U J U


Tr U J U Tr U J U Tr U J U



θ θ θ θ θ θ

+ + +

+ + + +

+ + +

& & & & & & & && & & & & & & && & & & & & ( )

( ) ( ) ( ) ( )( ) ( ) ( ) ( )( ) ( )

3 33 3 31 3 1

33 3 32 3 2 33 3 33 3 3 41 4 41 1 1 41 4 42 1 2

41 4 43 1 3 41 4 44 1 4 42 4 41 4 1 42 4 42 2 2

42 4 43 2 3 42 4 44 2 4

T

T T T T

T T T T

T T

Tr U J U



Tr U J U Tr U J U

θ θ



θ θ θ θ

+

+ + + +

+ + + +

+ + +

& && & & & & & & && & & & & & & && & & & ( ) ( )

( ) ( ) ( ) ( )( ) ( ) ( ) ( )( )

43 4 41 3 1 43 4 42 3 2

43 4 43 3 3 43 4 44 3 4 44 4 41 4 1 44 4 42 4 2

44 4 43 4 3 44 4 44 4 4 51 5 51 1 1 51 5 52 1 2

51 5 53 1 3

T T

T T T T

T T T T

T

Tr U J U Tr U J U



Tr U J U Tr

θ θ θ θ



θ θ

+

+ + + +

+ + + +

+ +

& & & && & & & & & & && & & & & & & && & ( ) ( ) ( )

( ) ( ) ( ) ( )( ) ( ) ( ) ( )

51 5 54 1 4 51 5 55 1 5 52 5 51 2 1

52 5 52 2 2 52 5 53 2 3 52 5 54 2 4 52 5 55 2 5

53 5 51 3 1 53 5 52 3 2 53 5 53 3 3 53 5 54 3 4

5

T T T

T T T T

T T T T

U J U Tr U J U Tr U J U



Tr U

θ θ θ θ θ θ



+ +

+ + + +

+ + + +

+

& & & & & && & & & & & & && & & & & & & &

( ) ( ) ( ) ( )( ) ( ) ( ) ( )( ) ( ) ( )

3 5 55 3 5 54 5 51 4 1 54 5 52 4 2 54 5 53 4 3

54 5 54 4 4 54 5 55 4 5 55 5 51 5 1 55 5 52 5 2

55 5 53 5 3 55 5 54 5 4 55 5 55 5 5

T T T T

T T T T

T T T

J U Tr U J U Tr U J U Tr U J U





θ θ θ θ θ θ

+ + +

+ + + +

+ + +

& & & & & & & && & & & & & & && & & & & &

(4.20)

4.2. Cálculo de la energía potencial del manipulador

La energía potencial del manipulador en cada uno de sus componentes, está dada de la siguiente manera:

( )0 0 ii i i i i iP m g r m g A r= − = − (4.21)

La energía potencial del conjunto total del brazo manipulador se obtendrá al sumar las energías potenciales de cada elemento, por lo que será calculada por medio de la fórmula:

( )0

1 1

n ni

i i i ii i

P P m g A r= =

= = −∑ ∑ (4.22)

Donde:

g es un vector columna de gravedad, expresado en el sistema de coordenadas de la base;

2

0 00 0

9.80620 0

mgsegg

= = −


Página 93

La ecuación para calcular la energía potencial del manipulador será:

( ) ( ) ( ) ( ) ( )0 1 0 2 0 3 0 4 0 51 1 1 2 2 2 3 3 3 4 4 4 5 5 5P m g A r m g A r m g A r m g A r m g A r= − − − − − (4.23)

4.3. Ecuaciones de movimiento del manipulador

Al sustituir las ecuaciones (4.20) y (4.23), en la ecuación (4.1b), reobtiene la función lagrangiana como se describe a continuación:

( ) ( ) ( ) ( )( ) ( ) ( ) ( )( ) ( ) ( )

11 1 11 1 1 21 2 21 1 1 21 2 22 1 2 22 2 21 2 1

22 2 22 2 2 31 3 31 1 1 31 3 32 1 2 31 3 33 1 3

32 3 31 2 1 32 3 32 2 2 32 3 33 2

12L=

T T T T

T T T T

T T T






θ θ θ θ θ θ

+ + +

+ + + +

+ + +

& & & & & & & && & & & & & & && & & & & & ( )

( ) ( ) ( ) ( )( ) ( ) ( ) ( )( ) ( )

3 33 3 31 3 1

33 3 32 3 2 33 3 33 3 3 41 4 41 1 1 41 4 42 1 2

41 4 43 1 3 41 4 44 1 4 42 4 41 4 1 42 4 42 2 2

42 4 43 2 3 42 4 44 2 4

T

T T T T

T T T T

T T

Tr U J U



Tr U J U Tr U J U

θ θ



θ θ θ θ

+

+ + + +

+ + + +

+ + +

& && & & & & & & && & & & & & & && & & & ( ) ( )

( ) ( ) ( ) ( )( ) ( ) ( ) ( )( )

43 4 41 3 1 43 4 42 3 2

43 4 43 3 3 43 4 44 3 4 44 4 41 4 1 44 4 42 4 2

44 4 43 4 3 44 4 44 4 4 51 5 51 1 1 51 5 52 1 2

51 5 53 1 3

T T

T T T T

T T T T

T

Tr U J U Tr U J U



Tr U J U Tr

θ θ θ θ



θ θ

+

+ + + +

+ + + +

+ +

& & & && & & & & & & && & & & & & & && & ( ) ( ) ( )

( ) ( ) ( ) ( )( ) ( ) ( ) ( )

51 5 54 1 4 51 5 55 1 5 52 5 51 2 1

52 5 52 2 2 52 5 53 2 3 52 5 54 2 4 52 5 55 2 5

53 5 51 3 1 53 5 52 3 2 53 5 53 3 3 53 5 54 3 4

5

T T T

T T T T

T T T T

U J U Tr U J U Tr U J U



Tr U

θ θ θ θ θ θ



+ +

+ + + +

+ + + +

+

& & & & & && & & & & & & && & & & & & & &

( ) ( ) ( ) ( )( ) ( ) ( ) ( )( ) ( ) ( )

3 5 55 3 5 54 5 51 4 1 54 5 52 4 2 54 5 53 4 3

54 5 54 4 4 54 5 55 4 5 55 5 51 5 1 55 5 52 5 2

55 5 53 5 3 55 5 54 5 4 55 5 55 5 5

T T T T

T T T T

T T T

J U Tr U J U Tr U J U Tr U J U





θ θ θ θ θ θ

+ + +

+ + + +

+ + +

& & & & & & & && & & & & & & && & & & & &

( ) ( ) ( ) ( ) ( )0 1 0 2 0 3 0 4 0 51 1 1 2 2 2 3 3 3 4 4 4 5 5 5m g A r m g A r m g A r m g A r m g A r

+ + + + +

La aplicación de la formulación de Lagrange-Euler a la función lagrangiana del manipulador, da el par generalizado necesario.

( ) ( )1 1 1 1

j j jn n nT T i

i jk j ji k jkm j ji k m j ji jj i k j i k m ji i

d L LTr U J U Tr U J U m gU r

dt q qτ θ θ θ

= = = = = =

∂ ∂= − = + − ∂ ∂

∑∑ ∑∑∑ ∑&& & &&

(4.24)

Expresando la ecuación (4.24) de forma matricial para simplificarla, se tiene:

1 1 1

j j j

i ik k ikm k m ik k m

D h cτ θ θ θ= = =

= + +∑ ∑∑&& & & (4.25)


Página 94

Donde:

ikD es una matriz simétrica inercial relacionada con la aceleración, su tamaño es de 5 5× elementos, y estos se desarrollan a partir de la formula:

( )max(, )

nT

ik jk j jij i j

D Tr U J U=

= ∑ (4.26)

( ),h q q& es el vector de fuerza de Coriolis y centrífuga no lineal, su tamaño es de 5 1× elementos, y estos se desarrollan a partir de la formula:

1 1

n n

i ikm k mk m

h h θ θ= =

= ∑ ∑ & & (4.27)

( )( )max , ,

nT

ikm jkm j jij i k m

h Tr U J U=

= ∑ (4.28)

( )c q Es el vector de la fuerza gravitatoria, su tamaño es de 5 1× elementos, y estos se desarrollan a partir de la formula:

( )1

nj

i j ji jj

c m gU r=

= −∑ (4.29)

Las ecuaciones de las fuerzas de la carga gravitatoria serán entonces:

1 1 11 1 2 21 2 3 31 3 4 41 4 5 51 5c m gU r m gU r m gU r m gU r m gU r= − − − − −

2 2 22 2 3 32 3 4 42 4 5 52 5c m gU r m gU r m gU r m gU r= − − − −

3 3 33 3 4 43 4 5 53 5c m gU r m gU r m gU r= − − −

4 4 44 4 5 54 5c m gU r m gU r= − −

5 5 55 5c m gU r= −

La matriz inercial relacionada con la aceleración será:

( ) ( ) ( ) ( ) ( )11 11 1 11 21 2 21 31 3 31 41 4 41 51 5 51T T T T TD Tr U J U Tr U J U Tr U J U Tr U J U Tr U J U= + + + +

( ) ( ) ( ) ( )12 22 2 21 32 3 31 42 4 41 52 5 51T T T TD Tr U J U Tr U J U Tr U J U Tr U J U= + + +

( ) ( ) ( )13 33 3 31 43 4 41 53 5 51T T TD Tr U J U Tr U J U Tr U J U= + +


Página 95

( ) ( )14 44 4 41 54 5 51T TD Tr U J U Tr U J U= +

( )15 55 5 51TD Tr U J U=




( ) ( )24 44 4 42 54 5 52T TD Tr U J U Tr U J U= +

( )25 55 5 52TD Tr U J U=




( ) ( )34 44 4 43 54 5 53T TD Tr U J U Tr U J U= +

( )35 53 5 55TD Tr U J U=

( ) ( )41 41 4 44 51 5 54T TD Tr U J U Tr U J U= +

( ) ( )42 42 4 44 52 5 54T TD Tr U J U Tr U J U= +

( ) ( )43 43 4 44 53 5 54T TD Tr U J U Tr U J U= +

( ) ( )44 44 4 44 54 5 54T TD Tr U J U Tr U J U= +

( )45 55 5 54TD Tr U J U=

( )51 51 5 55TD Tr U J U=

( )52 52 5 55TD Tr U J U=


Página 96

( )53 53 5 55TD Tr U J U=

( )54 54 5 55TD Tr U J U=

( )55 55 5 55TD Tr U J U=

4.4. Cálculo del vector de fuerza de Coriolis y centrífuga

( ) ( )1 2 3 4 5, , , , , Th h h h h hθ θ= =&

De las ecuaciones (4.27) y (4.28), se obtienen los vectores de fuerza de Coriolis y centrífuga, por lo que al desarrollarlas se tiene:

( ) ( ) ( ) ( ) ( )111 111 1 11 211 2 21 311 3 31 411 4 41 511 5 51T T T T Th Tr U J U Tr U J U Tr U J U Tr U J U Tr U J U= + + + +

( ) ( ) ( ) ( )112 212 2 21 312 3 31 412 4 41 512 5 51T T T Th Tr U J U Tr U J U Tr U J U Tr U J U= + + +

( ) ( ) ( )113 313 3 31 413 4 41 513 5 51T T Th Tr U J U Tr U J U Tr U J U= + +

( ) ( )114 414 4 41 514 5 51T Th Tr U J U Tr U J U= +

( )115 515 5 51Th Tr U J U=




( ) ( )124 424 4 41 524 5 51T Th Tr U J U Tr U J U= +

( )125 525 5 51Th Tr U J U=





Página 97

( ) ( )134 434 4 41 534 5 51T Th Tr U J U Tr U J U= +

( )135 535 5 51Th Tr U J U=

( ) ( )141 441 4 41 541 5 51T Th Tr U J U Tr U J U= +

( ) ( )142 442 4 41 542 5 51T Th Tr U J U Tr U J U= +

( ) ( )143 443 4 41 543 5 51T Th Tr U J U Tr U J U= +

( ) ( )144 444 4 41 544 5 51T Th Tr U J U Tr U J U=

( )145 545 5 51Th Tr U J U=

( )151 551 5 51Th Tr U J U=

( )152 552 5 51Th Tr U J U=

( )153 553 5 51Th Tr U J U=

( )154 554 5 51Th Tr U J U=

( )155 555 5 51Th Tr U J U=




( ) ( )214 515 5 42 514 5 52T Th Tr U J U Tr U J U= +

( )215 515 5 52Th Tr U J U=




Página 98


( ) ( )224 424 4 42 524 5 52T Th Tr U J U Tr U J U= +

( )225 525 5 52Th Tr U J U=




( ) ( )234 434 4 42 534 5 52T Th Tr U J U Tr U J U= +

( )235 535 5 52Th Tr U J U=

( ) ( )241 441 4 42 541 5 52T Th Tr U J U Tr U J U= +

( ) ( )242 442 4 42 542 5 52T Th Tr U J U Tr U J U= +

( ) ( )243 443 4 42 543 5 52T Th Tr U J U Tr U J U= +

( ) ( )244 444 4 42 544 4 52T Th Tr U J U Tr U J U= +

( )245 545 5 52Th Tr U J U=

( )251 551 5 52Th Tr U J U=

( )252 552 5 52Th Tr U J U=

( )253 553 5 52Th Tr U J U=

( )254 554 5 52Th Tr U J U=

( )255 555 55 52Th Tr U J U=



Página 99



( ) ( )314 414 4 43 514 5 53T Th Tr U J U Tr U J U= +

( )315 515 5 53Th Tr U J U=




( ) ( )324 424 4 43 524 5 53T Th Tr U J U Tr U J U= +

( )325 525 5 53Th Tr U J U=




( ) ( )334 434 4 43 534 5 53T Th Tr U J U Tr U J U= +

( )335 335 5 53Th Tr U J U=

( ) ( )341 441 4 43 541 5 53T Th Tr U J U Tr U J U= +

( ) ( )342 442 4 43 542 5 53T Th Tr U J U Tr U J U= +

( ) ( )343 443 4 43 543 5 53T Th Tr U J U Tr U J U= +

( ) ( )344 444 4 43 544 5 53T Th Tr U J U Tr U J U= +

( )345 545 5 53Th Tr U J U=


Página 100

( )351 551 5 53Th Tr U J U=

( )352 552 5 53Th Tr U J U=

( )353 553 5 53Th Tr U J U=

( )354 554 5 53Th Tr U J U=

( )355 555 5 53Th Tr U J U=

( ) ( )411 411 4 44 511 5 54T Th Tr U J U Tr U J U= +

( ) ( )412 412 4 44 512 5 54T Th Tr U J U Tr U J U= +

( ) ( )413 413 4 44 513 5 54T Th Tr U J U Tr U J U= +

( ) ( )414 414 4 44 541 5 54T Th Tr U J U Tr U J U= +

( )415 515 5 54Th Tr U J U=

( ) ( )421 421 4 44 521 5 54T Th Tr U J U Tr U J U= +

( ) ( )422 422 4 44 522 5 54T Th Tr U J U Tr U J U= +

( ) ( )423 423 4 44 523 5 54T Th Tr U J U Tr U J U= +

( ) ( )424 424 4 44 524 5 54T Th Tr U J U Tr U J U= +

( )425 525 5 54Th Tr U J U=

( ) ( )431 431 4 44 531 5 54T Th Tr U J U Tr U J U= +

( ) ( )432 432 4 44 532 5 54T Th Tr U J U Tr U J U= +

( ) ( )433 433 4 44 533 5 54T Th Tr U J U Tr U J U= +

( ) ( )434 434 4 44 534 5 54T Th Tr U J U Tr U J U= +


Página 101

( )435 535 5 54Th Tr U J U=

( ) ( )441 441 4 44 541 5 54T Th Tr U J U Tr U J U= +

( ) ( )442 442 4 44 542 5 54T Th Tr U J U Tr U J U= +

( ) ( )443 443 4 44 543 5 54T Th Tr U J U Tr U J U= +

( ) ( )444 444 4 44 544 5 54T Th Tr U J U Tr U J U= +

( )445 545 5 54Th Tr U J U=

( )451 551 5 54Th Tr U J U=

( )452 552 5 54Th Tr U J U=

( )453 553 5 54Th Tr U J U=

( )454 554 5 54Th Tr U J U=

( )455 555 5 54Th Tr U J U=

( )511 511 5 55Th Tr U J U=

( )512 512 5 55Th Tr U J U=

( )513 513 5 55Th Tr U J U=

( )514 514 5 55Th Tr U J U=

( )515 515 5 55Th Tr U J U=

( )521 521 5 55Th Tr U J U=

( )522 522 5 55Th Tr U J U=

( )523 523 5 55Th Tr U J U=


Página 102

( )524 524 5 55Th Tr U J U=

( )525 525 5 55Th Tr U J U=

( )531 531 5 55Th Tr U J U=

( )532 532 5 55Th Tr U J U=

( )533 533 5 55Th Tr U J U=

( )534 534 5 55Th Tr U J U=

( )5535 535 5 55Th Tr U J U=

( )541 541 5 55Th Tr U J U=

( )542 542 5 55Th Tr U J U=

( )543 543 5 55Th Tr U J U=

( )544 544 5 55Th Tr U J U=

( )545 545 5 55Th Tr U J U=

( )551 551 5 55Th Tr U J U=

( )552 552 5 55Th Tr U J U=

( )553 553 5 55Th Tr U J U=

( )554 554 5 55Th Tr U J U=

( )555 555 5 55Th Tr U J U=


Página 103

Por tanto, las ecuaciones de los vectores de fuerza de Coriolis y centrífuga no lineal serán:

1 111 1 1 112 1 2 113 1 3 114 1 4 115 1 5 121 2 1 122 2 2 123 2 3 124 2 4

125 2 5 131 3 1 132 3 2 133 3 3 134 3 4 135 3 5 141 4 5 142 4 2 143 4 3

h h h h h h h h h h

h h h h h h h h h

θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ


= + + + + + + + +

+ + + + + + + + +

& & & & & & & & & & & & & & & & & && & & & & & & & & & & & & & & & & &

144 4 4 145 4 5 151 5 1 152 5 2 153 5 3 154 5 4 155 5 5h h h h h h hθ θ θ θ θ θ θ θ θ θ θ θ θ θ

+

+ + + + + +& & & & & & & & & & & & & &

2 211 1 1 212 1 2 213 1 3 214 1 4 215 1 5 221 2 1 222 2 2 223 2 3 224 2 4

225 2 5 231 3 1 232 3 2 233 3 3 234 3 4 235 3 5 241 4 1 242 4 2 243 4 3

h h h h h h h h h h

h h h h h h h h h



= + + + + + + + + +

+ + + + + + + +

& & & & & & & & & & & & & & & & & && & & & & & & & & & & & & & & & & &

244 4 4 245 4 5 251 5 252 5 2 253 5 3 254 5 4 255 5 5h h h h h h hθ θ θ θ θ θ θ θ θ θ θ θ θ

+

+ + + + + +& & & & & & & & & & & & &

3 311 1 1 312 1 2 313 1 3 314 1 4 315 1 5 321 2 1 322 2 2 323 2 3 324 2 4

325 2 5 331 3 1 332 3 2 333 3 3 334 3 4 335 3 5 341 4 1 342 4 2 343 4 3

h h h h h h h h h h

h h h h h h h h h



= + + + + + + + +

+ + + + + + + + +

& & & & & & & & & & & & & & & & & && & & & & & & & & & & & & & & & & &


+

+ + + + + +& & & & & & & & & & & & & &

4 411 1 1 412 1 2 413 1 3 414 1 4 415 1 5 421 2 1 422 2 2 423 2 3 424 2 4

425 2 5 431 3 1 432 3 2 433 3 3 434 3 4 435 3 5 441 4 1 442 4 2 443 4 3

h h h h h h h h h h

h h h h h h h h h



= + + + + + + + +

+ + + + + + + + +

& & & & & & & & & & & & & & & & & && & & & & & & & & & & & & & & & & &


+

+ + + + + +& & & & & & & & & & & & & &

5 511 1 1 512 1 2 513 1 3 514 1 4 515 1 5 521 2 1 522 2 2 523 2 3 524 2 4

525 2 5 531 3 1 532 3 2 533 3 3 534 3 4 535 3 5 541 4 1 542 4 2 543 4 3

h h h h h h h h h h

h h h h h h h h h



= + + + + + + + +

+ + + + + + + + +

& & & & & & & & & & & & & & & & & && & & & & & & & & & & & & & & & & &


+

+ + + + + +& & & & & & & & & & & & & &

Los pares necesarios para el movimiento de los eslabones del manipulador serán entonces dados por las siguientes ecuaciones:

1 11 1 12 2 13 3 14 4 15 5 111 1 1 112 1 2 113 1 3 114 1 4 115 1 5

121 2 1 122 2 2 123 2 3 124 2 4 125 2 5 131 3 1 132 3 2 133 3 3 134 3 4

13

D D D D D h h h h h

h h h h h h h h h

h

τ θ θ θ θ θ θ θ θ θ θ θ θ θ θ θ


= + + + + + + + + + +

+ + + + + + + + +

&& && && && && & & & & & & & & & && & & & & & & & & & & & & & & & & &

5 3 5 141 4 1 142 4 2 143 4 3 144 4 4 145 4 5 151 5 1 152 5 2 153 5 3

154 5 4 155 5 5 1

h h h h h h h h

h h c


θ θ θ θ

+ + + + + + + + +

+ +

& & & & & & & & & & & & & & & & & && & & &

(4.30)

2 21 1 22 2 23 3 24 4 25 5 211 1 1 212 1 2 213 1 3 214 1 4 215 1 5

221 2 1 222 2 2 223 2 3 224 2 4 225 2 5 231 3 1 232 3 2 233 3 3 234 3 4

23

D D D D D h h h h h

h h h h h h h h h

h



= + + + + + + + + + +

+ + + + + + + + +

&& && && && && & & & & & & & & & && & & & & & & & & & & & & & & & & &

5 3 5 241 4 1 242 4 2 243 4 3 244 4 4 245 4 5 251 5 1 252 5 2 253 5 3

254 5 4 255 5 5 2

h h h h h h h h

h h c


θ θ θ θ

+ + + + + + + + +

+ +

& & & & & & & & & & & & & & & & & && & & &

(4.31)


Página 104

3 31 1 32 2 33 3 34 4 35 5 311 1 1 312 1 2 313 1 3 314 1 4 315 1 5

321 2 1 322 2 2 323 2 3 324 2 4 325 2 5 331 3 1 332 3 2 333 3 3 334 3 4

33

D D D D D h h h h h

h h h h h h h h h

h



= + + + + + + + + + +

+ + + + + + + + +

&& && && && && & & & & & & & & & && & & & & & & & & & & & & & & & & &

5 3 5 341 4 1 342 4 2 343 4 3 344 4 4 345 4 5 351 5 1 352 5 2 353 5 3

354 5 4 355 5 5 3

h h h h h h h h

h h c


θ θ θ θ

+ + + + + + + + +

+ +

& & & & & & & & & & & & & & & & & && & & &

(4.32)

4 41 1 42 2 43 3 44 4 45 5 411 1 1 412 1 2 413 1 3 414 1 4 415 1 5

421 2 1 422 2 2 423 2 3 424 2 4 425 2 5 431 3 1 432 3 2 433 3 3 434 3 4

45

D D D D D h h h h h

h h h h h h h h h

h



= + + + + + + + + + +

+ + + + + + + + +

&& && && && && & & & & & & & & & && & & & & & & & & & & & & & & & & &

4 3 5 441 4 1 442 4 2 443 4 3 444 4 4 445 4 5 451 5 1 452 5 2 453 5 3

454 5 4 455 5 5 4

h h h h h h h h

h h c


θ θ θ θ

+ + + + + + + + +

+ +

& & & & & & & & & & & & & & & & & && & & &

(4.33)

5 51 1 52 2 53 3 54 4 55 5 511 1 1 512 1 2 513 1 3 514 1 4 515 1 5

521 2 1 522 2 2 523 2 3 524 2 4 525 2 5 531 3 1 532 3 2 533 3 3 534 3 4

55

D D D D D h h h h h

h h h h h h h h h

h



= + + + + + + + + + +

+ + + + + + + + +

&& && && && && & & & & & & & & & && & & & & & & & & & & & & & & & & &

4 3 5 541 4 1 542 4 2 543 4 3 544 4 4 545 4 5 551 5 1 552 5 2 553 5 3

554 5 4 555 5 5 5

h h h h h h h h

h h c


θ θ θ θ

+ + + + + + + + +

+ +

& & & & & & & & & & & & & & & & & && & & &

(4.34)

Debido a la cantidad de operaciones, que requiere el cálculo de las ecuaciones de movimiento dinámico del manipulador, se desarrolló un programa utilizando el software Matlab 7.0, el cual está incluido en los anexos de la presente tesis, con el nombre de dinámica.m. este programa calcula los vectores de fuerza de Coriolis y centrífuga, los valores de las fuerzas de carga gravitatorias y los valores de los pares de fuerza en cada eslabón requeridos para que este cambie de posición.

Los resultados que arroja este programa tomando como dato, los valores angulares de las articulaciones, las velocidades angulares y las aceleraciones angulares, calculados en el capitulo 3, se presentan en la tabla 4.1.


Página 105

Tabla 4.1

X Y Z kg-m kg-m kg-m kg-m kg-m1 0.400 0.100 0.100 -32.4844 113.2964 -21.0878 -23.4157 -6.68612 0.420 0.100 0.100 -28.4238 94.4775 -7.3208 -35.8025 -6.02473 0.440 0.100 0.100 -25.5206 81.3238 2.0708 -44.1562 -5.52924 0.460 0.100 0.100 -23.3683 71.6348 8.8593 -50.1204 -5.14175 0.480 0.100 0.100 -21.734 64.2003 13.9952 -54.57 -4.82746 0.500 0.100 0.100 -20.4755 58.3028 18.0283 -58.0062 -4.56457 0.520 0.100 0.100 -19.5014 53.4911 21.2957 -60.7326 -4.3388 0.540 0.100 0.100 -18.7515 49.4673 24.0141 -62.9405 -4.13749 0.560 0.100 0.100 -18.1849 46.0269 26.3281 -64.7542 -3.954910 0.580 0.100 0.100 -17.7736 43.0243 28.3373 -66.2562 -3.784411 0.600 0.100 0.100 -17.4988 40.3524 30.1124 -67.5007 -3.6209

coordenadas(mm)punto 1τ 2τ 3τ 4τ 5τ

Con la obtención de las fuerzas generalizadas, se da por terminado el cálculo de la dinámica, y con ello se engloban los parámetros cinemáticos que gobiernan el manipulador, así como las ecuaciones que los controlan, para poder así, en trabajos posteriores, diseñar el sistema de control del manipulador.


Página 106

CAPITULO 5. ANALISIS DE RESULTADOS

Ya se tienen las ecuaciones para controlar el movimiento cinemático y dinámico del manipulador, así como los resultados obtenidos del cálculo de los valores angulares, las velocidades angulares, las aceleraciones angulares y las fuerzas generalizadas, pero hasta el momento no se sabe que tan bruscos han sido los movimientos del manipulador, recordando que para llegar a un punto son posibles diferentes trayectorias.

En el presente capitulo se analizarán los resultados obtenidos al manejar las ecuaciones de movimiento del manipulador, a partir de graficas de comportamiento de sus articulaciones.

A continuación se muestra la grafica 5.1, en la que se ilustra el movimiento angular (eje vertical) de la articulación 1 del manipulador, grafica en la que se aprecia un movimiento continuo, sin cambios bruscos, lo que deduce un movimiento constante, esto es que continua la trayectoria, con la menor energía gastada. En todos los casos que siguen el eje horizontal contiene los once puntos de la trayectoria.

Grafica 5.1 Comportamiento angular de la articulación 1.

La grafica 5.2, representa el desplazamiento de la articulación 2 del manipulador, teniendo un movimiento continuo a lo largo de la trayectoria, sin cambios bruscos de dirección, con lo que se tiene el desplazamiento mas corto con la menor energía.


La grafica 5.3, representa el movimiento que tuvo la articulación 3, a lo largo de toda la trayectoria. En esta grafica se observa un movimiento continuo, sin cambios bruscos de


Página 107

dirección, lo que representa que consigue seguir la trayectoria con la menor distancia, y la menor energía gastada.


La grafica 5.4, representa el desplazamiento de la articulación 4 del manipulador, teniendo un movimiento continuo a lo largo de la trayectoria, sin cambios bruscos de dirección, con lo que se tiene el desplazamiento mas corto con la menor energía.


La grafica 5.5, representa el movimiento que tuvo la articulación 5, a lo largo de toda la trayectoria. En esta grafica se observa un movimiento continuo, sin cambios bruscos de dirección, lo que representa que consigue seguir la trayectoria con la menor distancia, y la menor energía gastada.


La grafica 5.6, ilustra el comportamiento que tuvo la velocidad angular (eje vertical) de la articulación 1. En ella se nota una velocidad que aunque varia, no tiene aceleraciones bruscas, manteniendo un movimiento uniforme en la articulación 1.


Página 108

Grafica 5.6 Comportamiento de la velocidad angular de la articulación 1.

La grafica 5.7, ilustra el comportamiento que tuvo la velocidad angular de la articulación 2. Esta grafica muestra una velocidad sin aceleraciones bruscas, manteniendo un movimiento uniforme en la articulación 2.


La grafica 5.8, ilustra el comportamiento que tuvo la velocidad angular de la articulación 3. Esta grafica muestra una velocidad sin aceleraciones bruscas, manteniendo un movimiento uniforme en la articulación 3.


La grafica 5.9, ilustra el comportamiento que tuvo la velocidad angular de la articulación 4. En ella se ve que la velocidad no tiene aceleraciones bruscas, manteniendo un movimiento uniforme en la articulación 4.


Página 109


La grafica 5.10, ilustra el comportamiento que tuvo la velocidad angular de la articulación 5. Esta grafica, muestra una velocidad sin aceleraciones bruscas, manteniendo un movimiento uniforme en la articulación 5.


La grafica 5.11, muestra el comportamiento de la aceleración angular de la articulación 1, esta grafica muestra una desaceleración uniforme en la articulación, con una variación de 2 rad/seg2 entre el primer y el ultimo punto de la trayectoria, manteniendo una variación casi lineal, lo que explica la suavidad con que se comporto la velocidad angular 1.

Grafica 5.11 Comportamiento de la aceleración angular de la articulación 1.

La grafica 5.12, muestra el comportamiento de la aceleración angular (eje vertical) de la articulación 2, en ella se ve una aceleración ascendente, lo que explica el incremento que sufrio la velocidad de la articulación. Muestra una curva en la que la aceleración crece 8 rad/seg2 del punto uno al punto once de la trayectoria.


Página 110


La grafica 5.13, muestra el comportamiento de la aceleración angular de la articulación 3, en ella se ve una aceleración descendente, comportamiento que explica la reduccion de velocidad que sufre la velocidad de la articulación, la grafica muestra una curva, en la que la aceleración decrece 15 rad/seg2 del punto uno al punto once de la trayectoria.


La grafica 5.14, muestra el comportamiento de la aceleración angular de la articulación 4, en ella se ve una curva de aceleración ascendente, con un diferencial de 8 rad/seg2 entre el primer punto y el ultimo de la trayectoria.


La grafica 5.15, muestra el comportamiento de la aceleración angular de la articulación 5, en ella se ve una aceleración desciende 1 rad/seg2, lo que explica el comportamiento casi constante de la velocidad de la articulación.


Página 111


La grafica 5.16, muestra el comportamiento del momento angular de la articulación 1. en ella se ve el incremento que tiene el momento, como consecuencia de la aceleración sufrida, disminuyendo gradualmente desde el primer punto hasta el ultimo punto de la trayectoria.

Grafica 5.16 Comportamiento del momento angular de la articulación 1.

La grafica 5.17, muestra el comportamiento del momento angular de la articulación 2. en esta articulación se encuentra el momento mas grande generado por cualquiera de las articulaciones del manipulador, lo que va acorde a la geometría del manipulador. Ya que es la articulación sobre la que apoyan la mayor parte de los eslabones, a diferencia de la primer articulación, en la que el rodamiento y el eje de rotación, ayudan a distribuir el peso de los eslabones, necesitando un menor momento angular para desplazar el eslabón de un punto a otro.


La grafica 5.18, muestra el comportamiento del momento angular de la articulación 3. aquí se ve el cambio del momento angular, notandose como incrementa un 30% desde el primer punto, hasta el ultimo punto de latrayectoria.


Página 112


La grafica 5.19, muestra el comportamiento del momento angular de la articulación 4. en ella se ve el incremento del momento, siendo un momento casi tres veces mayor el momento del ultimo punto, con respecto al primer punto de la trayectoria, debido al incremento tan grande de la aceleración angular de la articulación.


La grafica 5.20, muestra el comportamiento del momento angular de la articulación 5. en la que se ve el momento mas pequeño que sufre cualquiera de las articulaciones del manipulador, como es de esperar, ya que este no presenta ningún otro eslabón acoplado a el, hasta sea diseñado un efector final.


La grafica 5.21, muestra el comportamiento de los términos dinámicos hijk del manipulador, estos se relacionan con la velocidad de las variables de la articulación, específicamente con la fuerza de coriolis y la fuerza centrifuga. En ellos los subíndices k y m relacionan las articulaciones k y m, en la articulación i, así que el subíndice i se relaciona siempre con la articulación donde se perciben los pares de reacción inducidos por la velocidad. Estos elementos varían con respecto al diseño mecánico del manipulador, y a la


Página 113

posición en la que este se encuentre, debido a que la trayectoria creada es una línea recta, con incrementos pequeños, esto hace que los eslabones cambien muy poco su posición y orientación, lo que resulta que los términos dinámicos se mantengan constantes durante los once puntos de la trayectoria, razón por la que solo se grafica el comportamiento del primer punto, ya que este comportamiento es el mismo en los demás.

Grafica 5.21 Comportamiento de los términos dinámicos.

La grafica 5.22, muestra el comportamiento de el vector que representa la fuerza de coriolis y la fuerza centrifuga no lineal h, para cada uno de los eslabones del manipulador, en ella, como se menciona anteriormente existen variaciones muy pequeñas debido a los incrementos tan pequeños de la trayectoria creada, generando líneas casi rectas.

Grafica 5.22 Comportamiento de fuerza de coriolis y centrifuga no línea.

La grafica 5.23, muestra el comportamiento de el vector de fuerza gravitatoria c, en el que al ser muy pequeños los desplazamientos del manipulador, se mantiene constante para cada punto.


Página 114

Grafica 5.23 Comportamiento del vector de fuerza gravitatoria.

La grafica 5.24, muestra el comportamiento de las energías potencial y cinética del manipulador, que permanecen constantes, a lo largo de toda la trayectoria, debido al movimiento tan pequeño que tienen los eslabones del manipulador.

Grafica 5.24 Comportamiento de la energía cinética y potencial.


Página 115

CONCLUSIONES

Se han desarrollado las ecuaciones que controlan la cinemática y la dinámica del manipulador. Ecuaciones que servirán posteriormente para el diseño del control.

Se obtuvieron:

• 12 ecuaciones para el control del desplazamiento, del extremo libre del manipulador.

• 12 ecuaciones para el control de la velocidad lineal, en el extremo libre del manipulador.

• 12 ecuaciones para el control de la aceleración lineal, en el extremo libre del manipulador.

• 5 ecuaciones para el control de las fuerzas de coriolis y centrifugas no lineales

• 5 ecuaciones para el control de los momentos angulares en las articulaciones del manipulador.

Estas ecuaciones se obtuvieron al cumplir con los objetivos específicos de la tesis.

La obtención de los parámetros de Denavit-Hartenberg, se llevó a cabo utilizando la metodología Craigh. Al comienzo de la investigación, se trabaja con la metodología Fu, teniendo pequeñas pero significativas variaciones, debido a las relaciones que establece entre los eslabones. Esta metodología, genero ecuaciones que no fueron posibles resolver por el método iterativo desarrollado en esta tesis. Debido a lo anterior es importante resaltar que las metodologías no son universales, además de que son solo recomendaciones a seguir para el establecimiento de los ejes de coordenadas y con ellos obtener los parámetros de Denavit-Hartenberg, ya que no existe una regla general para ello.

Se obtuvo la matriz jacobiana del manipulador, teniendo que utilizar la metodología de Paúl para su desarrollo, ya que la matriz resultante de derivar parcialmente las ecuaciones cinemáticas de movimiento con respecto a sus ángulos, para obtener la matriz jacobiana desarrollada por Carl Gustav Jacobi, que requiere de un método numérico mas poderoso, para resolver la cinemática inversa, a diferencia de la matriz generada con la metodología de Paúl, en la que se genera una matriz mas pequeña, para calcular los cambios diferenciales del manipulador.

Se obtuvo la matriz de transformación de cada eslabón, para relacionar el extremo libre del manipulador con el sistema de la base, para descomponerla en los cuatro vectores que forman el sistema de coordenadas del extremo libre del manipulador, que serán igualados a la matriz del brazo para obtener las ecuaciones de movimiento, estas ecuaciones son la base de la cinemática de un manipulador, ya que a partir de su complejidad, será la facilidad con


Página 116

que puedan resolverse, estando ellas en función del acomodo de los ejes de coordenadas de los eslabones, por lo que es importante buscar una buena selección de los ejes coordenados de los eslabones.

El cálculo de las coordenadas generalizadas, es probablemente la parte medular del presente trabajo, ya que es una de las manera en que se puede controlar un manipulador, al decirle a que punto debe de ir, y el software decide cuales serán los valores que adopten las articulaciones. Existen muchas metodologías para esta solución, pero no todas logran resolver el problema cinemático inverso, para el presente trabajo se estudiaron tres metodologías, una de ellas fue la igualación de términos en las matrices de transformación, para que por medio de sustituciones e igualaciones, se fueran obteniendo los valores angulares, en la segunda se dividió el manipulador en dos partes para así tener ecuaciones menos acopladas, trabajando con menos eslabones, pero no se logró solucionar el problema, siendo la metodología expuesta en el capitulo 3, la única que logró un resultado optimo para las doce ecuaciones. De ahí que la solución de las coordenadas generalizadas, se puede volver una tarea de ardua investigación, para encontrar un método que pueda resolver las ecuaciones cinemáticas no lineales y sobredeterminadas.

El cálculo de las velocidades y aceleraciones angulares, se llevo a cabo de forma muy similar, en donde a partir de una optimización de las ecuaciones, debido a que son sistemas lineales sobredeterminados, se encuentra la solución mas óptima, para las doce ecuaciones en cuestión.

Se obtuvieron las fuerzas generalizadas para mover las articulaciones del manipulador a velocidades y aceleraciones específicas.

De lo anterior se resume lo siguiente:

• Es importante tener en cuenta la metodología que se emplea para la obtención de los parámetros de Denavit –Hartenberg, ya que de ello depende la facilidad con que se resolverá la cinemática inversa.

• La obtención de la matriz jacobiana, depende del método numérico con el que se vaya a resolver la cinemática inversa. Siendo la metodología de Paúl, la que genera una matriz jacobiana de mayor simplicidad en su solución.

• Para grandes aceleraciones en el extremo libre del manipulador, se deberá tener cuidado en las fuerzas de coriolis y centrifuga, ya que de ellas dependerán los movimientos inerciales de los eslabones pudiendo generar movimientos inesperados en la trayectoria.

• Los pares de fuerza necesarios para el movimiento de los eslabones están relacionados directamente con la geometría del manipulador, por lo que al realizar el diseño mecánico de un brazo, se recomienda se hagan piezas de geometría tal, que tenga momentos de inercia de masa, pequeños.


Página 117

RECOMENDACIONES

El diseño de un manipulador abarca muchas áreas de investigación, y por lo mismo muchas etapas de trabajo, el presente estudio es la segunda etapa del total necesario para obtener un manipulador industrial, que funcione y que pueda tener alguna aplicación, razón por la cual, se recomienda seguir el trabajo realizado durante el diseño mecánico, cinemático y dinámico, desarrollando el control y la instrumentación del manipulador, además del diseño y la fabricación de efectores finales.

Así pues se recomiendan los siguientes puntos.

• Terminar de ensamblar y fabricar las piezas faltantes del MIRH1.

• Calcular la estabilidad y los puntos críticos que tendrá el manipulador, para ser tomados como datos durante el diseño del control.

• Diseñar el sistema de control del manipulador, para lograr controlar los movimientos del MIRH1.

• Diseñar efectores finales para darle una aplicación mas práctica al manipulador dándole las herramientas necesarias para realizar una tarea.


ANEXOS

Programa para calcular la matriz de transformación del brazo

ectrans.m


%%PROGRAMA QUE CALCULA LA MATRIZ DE TRANSFORMACION A PARTIR DE LA MATRIZ DE DENAVIT-HARTENBER clear all %borra las variables previamente utilizadas clc %borra la pantalla syms t1 t2 t3 t4 t5 a2 a3 d5% indica que el programa trabajara % simbolicamente %asignacion de los parametros de Denavit-Hartenberg a0=0; a1=0; %a2=450; %a3=450; a4=0; alpha0= 0; alpha1= pi/2; alpha2= 0; alpha3= 0; alpha4= pi/2; d1=0; d2=0; d3=0; d4=0; %sustituye en la forma general de la matriz de transformacion A01 = [ cos(t1) , -sin(t1), 0 , a0 ; sin(t1)*cos(alpha0), cos(t1)*cos(alpha0), -sin(alpha0) , -sin(alpha0)*d1 ; sin(t1)*sin(alpha0), cos(t1)*sin(alpha0), cos(alpha0) , cos(alpha0)*d1 ; 0 0 0 1 ] A12 = [ cos(t2) , -sin(t2), 0 , a1 ; sin(t2)*0, cos(t2)*0, -1 , -1*d2 ; sin(t2)*1, cos(t2)*1, 0 , 0*d2 ; 0 , 0 , 0 , 1 ] A23 = [ cos(t3) , -sin(t3), 0 , a2 ; sin(t3)*cos(alpha2), cos(t3)*cos(alpha2), -sin(alpha2) , -sin(alpha2)*d3 ; sin(t3)*sin(alpha2), cos(t3)*sin(alpha2), cos(alpha2) , cos(alpha2)*d3 ; 0 0 0 1 ] A34 = [ cos(t4) , -sin(t4), 0 , a3 ; sin(t4)*cos(alpha3), cos(t4)*cos(alpha3), -sin(alpha3) , -sin(alpha3)*d4 ; sin(t4)*sin(alpha3), cos(t4)*sin(alpha3), cos(alpha3) , cos(alpha3)*d4 ; 0 0 0 1 ] A45 = [ cos(t5) , -sin(t5), 0 , a4 ; sin(t5)*0, cos(t5)*0, -1 , -1*d5 ; sin(t5)*1, cos(t5)*1, 0 , 0*d5 ; 0 0 0 1 ] %multiplica las matrices de los eslabones para obtener la matriz del brazo A05=A45*A34*A23*A12*A01


Programa para calcular las ecuaciones cinemáticas del manipulador.

eccinem.m


%PROGRAMA QUE CALCULA LAS ECUACIONES CINEMATICAS DEL MANIPULADOR MIRH 1

ectrans %llama al programa para obtener las matrices de transformacion %obtiene la matriz del brazo y la simplifica T02=A01*A12 T03=T02*A23 T04=T03*A34 T05=T04*A45 T14= simple( A12*A23*A34); T05 =simple(A01*T14*A45) %muestra en la pantalla las ecuaciones cinematicas del manipulador nx=T05(1,1) ny=T05(2,1) nz=T05(3,1) sx=T05(1,2) sy=T05(2,2) sz=T05(3,2) ax=T05(1,3) ay=T05(2,3) az=T05(3,3) px=T05(1,4); py=T05(2,4); pz=T05(3,4);


Programa para calcular la cinemática directa del manipulador.

cindirecta.m


%PROGRAMA QUE CALCULA LA CINEMATICA DIRECTA DEL MANIPULADOR MIRH 1 clear all clc %asigna los valores de Denavit-Hartenberg t1=0; t2=pi/2; t3=-pi/2; t4=0; t5=0; a0=0; a1=0; a2=450; a3=450; a4=0; alpha0= 0; alpha1= pi/2; alpha2= 0; alpha3= 0; alpha4= pi/2; d1=0; d2=0; d3=0; d4=0; d5=-442; %sustituye los valores de los parametros de Denavit-Hartenberg en las %ecuaciones cinematicas del manipulador nx = cos(t1)*cos(t2+t3+t4)*cos(t5)+sin(t1)*sin(t5); ny = sin(t1)*cos(t2+t3+t4)*cos(t5)-cos(t1)*sin(t5); nz = sin(t2+t3+t4)*cos(t5); sx = -cos(t1)*cos(t2+t3+t4)*sin(t5)+sin(t1)*cos(t5); sy = -sin(t1)*cos(t2+t3+t4)*sin(t5)-cos(t1)*cos(t5); sz = -sin(t2+t3+t4)*sin(t5); ax = cos(t1)*sin(t2+t3+t4); ay = sin(t1)*sin(t2+t3+t4); az = -cos(t2+t3+t4); px = cos(t1)*(sin(t2+t3+t4)*d5+cos(t2+t3)*a3+cos(t2)*a2) py = sin(t1)*(sin(t2+t3+t4)*d5+cos(t2+t3)*a3+cos(t2)*a2) pz = -cos(t2+t3+t4)*d5+sin(t2+t3)*a3+sin(t2)*a2


Programa para calcular las cinemática inversa del manipulador.

cininv.m


%PROGRAMA QUE CALCULA LA CINEMATICA INVERZA DEL MANIPULADOR MIRH1 clear all %borra las variables previamente utilizadas clc %borra la pantalla %ectrans %Llama al archivo para calcular las ecuaciones de transformacion qt=[]; %inicializa el vector donde seran almacenados los resultados qi=[]; dp= []; ddp= []; pe=[]; dq=[]; ddq=[]; acelx=1; acely=1; acelz=1; a2=0.450; a3=0.450; d5=-0.442; q = [1 1 1 1 1]'; %valores iniciales propuestos x= [.400:.02:.600];%genera una trayectoria de 11 puntos en la coordenada x y =[0.1 0.1 0.1 0.1 0.1 0.1 0.1 0.1 0.1 0.1 0.1]; % mantiene constante la % coordenada y para % generar una linea % horizontal z = .100;% mantiene constante la coordenada z %genera las matrices de los puntos de la trayectoria generada for k = 1:11; TG (:,:,k)=[ 1 0 0 x(k); 0 1 0 y(k); 0 0 1 z; 0 0 0 1]; end %asigna el numero maximo de iteraciones que hara el programa y la presicion %de los resultados stol = 0.000000001 ; ilimit = 1000; %matriz de Denavit-Hartenberg dh = [ 0, 0, 0, 0, 0; pi/2, 0, 0, 0, 0; 0, 0.450, 0, 0, 0; 0, 0.450, 0, 0, 0; pi/2, 0, 0, -0.442, 0]; %propone los valores iniciales a los angulos de las articulaciones de los %eslabones tcount = 0; %inicializa el contador tcount for i=1:11;%inicializa el contador i, para trabajar los 11 puntos de la %trayectoria


nm = 1;%inicializa el vector nm que despues sera utilizado para comparar %la norma del vector de datos calculado T = TG(:,:,i);%asigna el valor de la matriz del primer punto al que %seran calculados los valores de sus articulaciones count = 0;%inicializa el contador count while nm > stol;%inicializa el proceso indicando que no pare hasta que %la norma del vector de datos calculados sea menor que %la tolerancia establecida T2=mattot(q');%genera las matrices de transformación %asigna valores a los vectores de orientacion y posicion n=T(1:3,1); n2=T2(1:3,1); s=T(1:3,2); s2=T2(1:3,2); a=T(1:3,3); a2=T2(1:3,3); p=T(1:3,4); p2=T2(1:3,4); %calcula la distancia entre el punto de la trayectoria y el punto %previamente calculado en la cinematica directa d=[p-p2;(cross(n2,n)+cross(s2,s)+cross(a2,a))/2]; %calcula el jacobiano de los valores de lasarticulaciones w= jacob01(dh, q'); %calcula la pseudoinverza del jacobiano dqs = pinv(w)*d; %suma el incremento al valor previo de los angulos de las %articulaciones q = q + dqs; %calcula la norma del vector que contiene los resultados obtenidos %de los valores de las articulaciones del manipulador nm = norm(dqs); end qi = [qi; q'];%genera el vector que contendra los valores obtenidos de %las articulaciones de los eslabones tcount = tcount + count;%incrementa el contador tcount para que el %programa pare si llega al numero de iteraciones %previamente establecido end q=qi; %valores numericos de la geometria del manipulador a2=0.450; a3=0.450; d5=-0.442; %%%%%comprobacion de los valores angulares for n=1:11 t1 =[q(n,1)]; t2 =[q(n,2)]; t3 =[q(n,3)]; t4 =[q(n,4)]; t5 =[q(n,5)]; T=[t1,t2,t3,t4,t5];


px=cos(t1)*(sin(t2+t3+t4)*d5+cos(t2+t3)*a3+cos(t2)*a2); py=sin(t1)*(sin(t2+t3+t4)*d5+cos(t2+t3)*a3+cos(t2)*a2); pz=-cos(t2+t3+t4)*d5+sin(t2+t3)*a3+sin(t2)*a2; pes=[px py pz]; pe=[pe;pes]; end angulos=[q,pe]


Programa para calcular las velocidades angulares de los eslabones del manipulador.

velocidades.m


%PROGRAMA QUE CALCULA LAS VELOCIDADES ANGULARES DE LAS %ARTICULACIONES PARA UNA VELOCIDAD DETERMINADA, EN EL %EXTREMO LIBRE DEL MIRH1 for i=1:n; t1 =[q(i,1)];%% t1 valor del angulo theta1 en la trayectoria deseada t2 =[q(i,2)];%% t2 valor del angulo theta1 en la trayectoria deseada t3 =[q(i,3)];%% t3 valor del angulo theta1 en la trayectoria deseada t4 =[q(i,4)];%% t4 valor del angulo theta1 en la trayectoria deseada t5 =[q(i,5)];%% t5 valor del angulo theta1 en la trayectoria deseada T=[t1,t2,t3,t4,t5]; A=vel(T); B = [0 0 0 0 0 0 0 0 0 0.577 0.577 0.577]'; X = (A'*A)^-1*A'*B; dt1 = X(1,1); dt2 = X(2,1); dt3 = X(3,1); dt4 = X(4,1); dt5 = X(5,1); dqi=[dt1 dt2 dt3 dt4 dt5]; dq=[dq;dqi]; qt1 =[dq(i,1)]; qt2 =[dq(i,2)]; qt3 =[dq(i,3)]; qt4 =[dq(i,4)]; qt5 =[dq(i,5)]; dpx= -sin(t1)*dt1*(sin(t2+t3+t4)*d5+cos(t2+t3)*a3+cos(t2)*a2)+cos(t1)*(cos(t2+t3+t4)*(dt2+dt3+dt4)*d5-sin(t2+t3)*(dt2+dt3)*a3-sin(t2)*dt2*a2); dpy= cos(t1)*dt1*(sin(t2+t3+t4)*d5+cos(t2+t3)*a3+cos(t2)*a2)+sin(t1)*(cos(t2+t3+t4)*(dt2+dt3+dt4)*d5-sin(t2+t3)*(dt2+dt3)*a3-sin(t2)*dt2*a2); dpz= sin(t2+t3+t4)*(dt2+dt3+dt4)*d5+cos(t2+t3)*(dt2+dt3)*a3+cos(t2)*dt2*a2; dpi=[dpx dpy dpz]; dp=[dp;dpi]; end velocidades=[dq,dp]


Programa para calcular las aceleraciones angulares de los eslabones del manipulador.

aceleraciones.m


Funciones de los programas

mattot.m

%FUNCION QUE EVALUA OBTIENE EL VALOR NUMERICO DE LA MATRIZ DE TRANSFORMACION DEL BRAZO function T2=mattot(q) %asigna los valores del vector q a su respectiva articulacion t1=q(1,1); t2=q(1,2); t3=q(1,3); t4=q(1,4); t5=q(1,5); a2=450; a3=450; d5=-442; %sustituye el valor de las articulaciones en la matriz general T2 =[ cos(t1)*cos(t2+t3+t4)*cos(t5)+sin(t1)*sin(t5), -cos(t1)*cos(t2+t3+t4)*sin(t5)+sin(t1)*cos(t5), cos(t1)*sin(t2+t3+t4), (cos(t1)*(sin(t2+t3+t4)*d5+cos(t2+t3)*a3+cos(t2)*a2))*0.001; sin(t1)*cos(t2+t3+t4)*cos(t5)-cos(t1)*sin(t5), -sin(t1)*cos(t2+t3+t4)*sin(t5)-cos(t1)*cos(t5), sin(t1)*sin(t2+t3+t4), (sin(t1)*(sin(t2+t3+t4)*d5+cos(t2+t3)*a3+cos(t2)*a2))*0.001; sin(t2+t3+t4)*cos(t5), -sin(t2+t3+t4)*sin(t5), -cos(t2+t3+t4), (-cos(t2+t3+t4)*d5+sin(t2+t3)*a3+sin(t2)*a2)*0.001; 0, 0, 0, 1];


Jaco01.m

%FUNCION PARA OBTENER EL VALOR NUMERICO DEL JACOBIANO DEL MANIPULADOR function J1 = jaco(dh, q) t1=q(1,1); t2=q(2,1); t3=q(3,1); t4=q(4,1); t5=q(5,1); d1=0; d2=0; d3=0; d4=0; d5=-442; a0=0; a1=0; a4=0;


a2=450; a3=450; alpha0= 0; alpha1= pi/2; alpha2= 0; alpha3= 0; alpha4= pi/2; A45 = [ cos(t5) , -sin(t5), 0 , a4 ; sin(t5)*0, cos(t5)*0, -1 , -1*d5 ; sin(t5)*1, cos(t5)*1, 0 , 0*d5 ; 0 0 0 1 ] A34 = [ cos(t4) , -sin(t4), 0 , a3 ; sin(t4)*cos(alpha3), cos(t4)*cos(alpha3), -sin(alpha3) , -sin(alpha3)*d4 ; sin(t4)*sin(alpha3), cos(t4)*sin(alpha3), cos(alpha3) , cos(alpha3)*d4 ; 0 0 0 1 ] A23 = [ cos(t3) , -sin(t3), 0 , a2 ; sin(t3)*cos(alpha2), cos(t3)*cos(alpha2), -sin(alpha2) , -sin(alpha2)*d3 ; sin(t3)*sin(alpha2), cos(t3)*sin(alpha2), cos(alpha2) , cos(alpha2)*d3 ; 0 0 0 1 ] A12 = [ cos(t2) , -sin(t2), 0 , a1 ; sin(t2)*0, cos(t2)*0, -1 , -1*d2 ; sin(t2)*1, cos(t2)*1, 0 , 0*d2 ; 0 , 0 , 0 , 1 ] A01 = [ cos(t1) , -sin(t1), 0 , a0 ; sin(t1)*cos(alpha0), cos(t1)*cos(alpha0), -sin(alpha0) , -sin(alpha0)*d1 ; sin(t1)*sin(alpha0), cos(t1)*sin(alpha0), cos(alpha0) , cos(alpha0)*d1 ; 0 0 0 1 ] D5=[-A45(1,1)*A45(2,4)+A45(2,1)*A45(1,4); -A45(1,2)*A45(2,4)+A45(2,2)*A45(1,4); -A45(1,3)*A45(2,4)+A45(2,3)*A45(1,4); A45(3,1); A45(3,2); A45(3,3)] A35=A45*A34 D4=[-A35(1,1)*A35(2,4)+A35(2,1)*A35(1,4); -A35(1,2)*A35(2,4)+A35(2,2)*A35(1,4); -A35(1,3)*A35(2,4)+A35(2,3)*A35(1,4); A35(3,1); A35(3,2); A35(3,3)] A25=A35*A23 D3=[-A25(1,1)*A25(2,4)+A25(2,1)*A25(1,4); -A25(1,2)*A25(2,4)+A25(2,2)*A25(1,4); -A25(1,3)*A25(2,4)+A25(2,3)*A25(1,4); A25(3,1); A25(3,2); A25(3,3)]


A15=A25*A12 D2=[-A15(1,1)*A15(2,4)+A15(2,1)*A15(1,4); -A15(1,2)*A15(2,4)+A15(2,2)*A15(1,4); -A15(1,3)*A15(2,4)+A15(2,3)*A15(1,4); A15(3,1); A15(3,2); A15(3,3)] A05=A15*A01 D1=[-A05(1,1)*A05(2,4)+A05(2,1)*A05(1,4); -A05(1,2)*A05(2,4)+A05(2,2)*A05(1,4); -A05(1,3)*A05(2,4)+A05(2,3)*A05(1,4); A05(3,1); A05(3,2); A05(3,3)] J=[D5 D4 D3 D2 D1] J1=[A05(1:3,1:3) zeros(3,3); zeros(3,3) A05(1:3,1:3)] * J


Vel.m

%FUNCION PARA OBTENER VALOR NUMERICO DE LA MATRIZ A DE LAS ECUACIONES 3. 40 Y 3.53 function A=vel(T) a2=0.450; a3=0.450; d5=-0.442; t1 =[T(1,1)];%% t1 valor del angulo theta1 en la trayectoria deseada t2 =[T(1,2)];%% t2 valor del angulo theta1 en la trayectoria deseada t3 =[T(1,3)];%% t3 valor del angulo theta1 en la trayectoria deseada t4 =[T(1,4)];%% t4 valor del angulo theta1 en la trayectoria deseada t5 =[T(1,5)]; % syms t1 t2 t3 t4 t5 % syms a2 a3 d5 % syms dt1 dt2 dt3 dt4 dt5 %syms t1 t2 t3 t4 t5 a2 a3 d5 dt1 dt2 dt3 dt4 dt5 A(1,1)=-sin(t1)*cos(t2+t3+t4)*cos(t5)+cos(t1)*sin(t5); A(2,1)=cos(t1)*cos(t2+t3+t4)*cos(t5)+sin(t1)*sin(t5); A(3,1)=0; A(4,1)=sin(t1)*cos(t2+t3+t4)*sin(t5)+cos(t1)*cos(t5); A(5,1)=-cos(t1)*cos(t2+t3+t4)*sin(t5)+sin(t1)*cos(t5); A(6,1)=0; A(7,1)=-sin(t1)*sin(t2+t3+t4);


A(8,1)=cos(t1)*sin(t2+t3+t4); A(9,1)=0; A(10,1)=-sin(t1)*(sin(t2+t3+t4)*d5+cos(t2+t3)*a3+cos(t2)*a2); A(11,1)=cos(t1)*(sin(t2+t3+t4)*d5+cos(t2+t3)*a3+cos(t2)*a2); A(12,1)=0; A(1,2)=-cos(t1)*sin(t2+t3+t4)*cos(t5); A(2,2)=-sin(t1)*sin(t2+t3+t4)*cos(t5); A(3,2)=cos(t2+t3+t4)*cos(t5); A(4,2)=cos(t1)*sin(t2+t3+t4)*sin(t5); A(5,2)=sin(t1)*sin(t2+t3+t4)*sin(t5); A(6,2)=-cos(t2+t3+t4)*sin(t5); A(7,2)=cos(t1)*cos(t2+t3+t4); A(8,2)=sin(t1)*cos(t2+t3+t4); A(9,2)=sin(t2+t3+t4); A(10,2)=cos(t1)*(cos(t2+t3+t4)*d5-sin(t2+t3)*a3-sin(t2)*a2); A(11,2)=sin(t1)*(cos(t2+t3+t4)*d5-sin(t2+t3)*a3-sin(t2)*a2); A(12,2)=sin(t2+t3+t4)*d5+cos(t2+t3)*a3+cos(t2)*a2; A(1,3)=-cos(t1)*sin(t2+t3+t4)*cos(t5); A(2,3)=-sin(t1)*sin(t2+t3+t4)*cos(t5); A(3,3)=cos(t2+t3+t4)*cos(t5); A(4,3)=cos(t1)*sin(t2+t3+t4)*sin(t5); A(5,3)=sin(t1)*sin(t2+t3+t4)*sin(t5); A(6,3)=-cos(t2+t3+t4)*sin(t5); A(7,3)=cos(t1)*cos(t2+t3+t4); A(8,3)=sin(t1)*cos(t2+t3+t4); A(9,3)=sin(t2+t3+t4); A(10,3)=cos(t1)*(cos(t2+t3+t4)*d5-sin(t2+t3)*a3); A(11,3)=sin(t1)*(cos(t2+t3+t4)*d5-sin(t2+t3)*a3); A(12,3)=sin(t2+t3+t4)*d5+cos(t2+t3)*a3; A(1,4)=-cos(t1)*sin(t2+t3+t4)*cos(t5); A(2,4)=-sin(t1)*sin(t2+t3+t4)*cos(t5); A(3,4)=cos(t2+t3+t4)*cos(t5); A(4,4)=cos(t1)*sin(t2+t3+t4)*sin(t5); A(5,4)=sin(t1)*sin(t2+t3+t4)*sin(t5); A(6,4)=-cos(t2+t3+t4)*sin(t5); A(7,4)=cos(t1)*cos(t2+t3+t4); A(8,4)=sin(t1)*cos(t2+t3+t4); A(9,4)=sin(t2+t3+t4); A(10,4)=cos(t1)*cos(t2+t3+t4)*d5; A(11,4)=sin(t1)*cos(t2+t3+t4)*d5; A(12,4)=sin(t2+t3+t4)*d5; A(1,5)=-cos(t1)*cos(t2+t3+t4)*sin(t5)+sin(t1)*cos(t5); A(2,5)=-sin(t1)*cos(t2+t3+t4)*sin(t5)-cos(t1)*cos(t5);


A(3,5)=-sin(t2+t3+t4)*sin(t5); A(4,5)=-cos(t1)*cos(t2+t3+t4)*cos(t5)-sin(t1)*sin(t5); A(5,5)=-sin(t1)*cos(t2+t3+t4)*cos(t5)+cos(t1)*sin(t5); A(6,5)=-sin(t2+t3+t4)*cos(t5); A(7,5)=0; A(8,5)=0; A(9,5)=0; A(10,5)=0; A(11,5)=0; A(12,5)=0; vel=A;


matdatos.m

%FUNCION QUE OBTIENE Y EVALUA EL VECTOR B DE DATOS, EN LA ECUACION 3.54 function c=matdatos(T,dT) %valores angulares de las articulaciones t1=T(1,1); t2=T(1,2); t3=T(1,3); t4=T(1,4); t5=T(1,5); %valores de aceleraciones angulares dt1=dT(1,1); dt2=dT(1,2); dt3=dT(1,3); dt4=dT(1,4); dt5=dT(1,5); %datos geometricos del brazo robot a2=0.450; a3=0.450; d5=-0.442; ddnx=(-cos(t1)*dt1*cos(t2+t3+t4)*cos(t5)+sin(t1)*sin(t2+t3+t4)*(dt2+dt3+dt4)*cos(t5)+sin(t1)*co


s(t2+t3+t4)*sin(t5)*dt5-sin(t1)*dt1*sin(t5)+cos(t1)*cos(t5)*dt5)*dt1+(+sin(t1)*dt1*sin(t2+t3+t4)*cos(t5)-cos(t1)*cos(t2+t3+t4)*(dt2+dt3+dt4)*cos(t5)+cos(t1)*sin(t2+t3+t4)*sin(t5)*dt5)*dt2+(+sin(t1)*dt1*sin(t2+t3+t4)*cos(t5)-cos(t1)*cos(t2+t3+t4)* (dt2+dt3+dt4)*cos(t5)+cos(t1)*sin(t2+t3+t4)*sin(t5)*dt5)*dt3+(+sin(t1)*dt1*sin(t2+t3+t4)*cos(t5)-cos(t1)*cos(t2+t3+t4)* (dt2+dt3+dt4)*cos(t5)+cos(t1)*sin(t2+t3+t4)*sin(t5)*dt5)*dt4+(+sin(t1)*dt1*cos(t2+t3+t4)*sin(t5)+cos(t1)*sin(t2+t3+t4)*(dt2+dt3+dt4)*sin(t5)-cos(t1)*cos(t2+t3+t4)*cos(t5)*dt5+cos(t1)*dt1*cos(t5)-sin(t1)*sin(t5)*dt5)*dt5; ddny=(-sin(t1)*dt1*cos(t2+t3+t4)*cos(t5)-cos(t1)*sin(t2+t3+t4)*(dt2+dt3+dt4)*cos(t5)-cos(t1)*cos(t2+t3+t4)*sin(t5)*dt5+cos(t1)*dt1*sin(t5)+sin(t1)*cos(t5)*dt5)*dt1+(-cos(t1)*dt1*sin(t2+t3+t4)*cos(t5)-sin(t1)*cos(t2+t3+t4)*(dt2+dt3+dt4)*cos(t5)+sin(t1)*sin(t2+t3+t4)*sin(t5)*dt5)*dt2+(-cos(t1)*dt1*sin(t2+t3+t4)*cos(t5)-sin(t1)*cos(t2+t3+t4)* (dt2+dt3+dt4)*cos(t5)+sin(t1)*sin(t2+t3+t4)*sin(t5)*dt5)*dt3+(-cos(t1)*dt1*sin(t2+t3+t4)*cos(t5)-sin(t1)*cos(t2+t3+t4)* (t2+t3+t4)*cos(t5)+sin(t1)*sin(t2+t3+t4)*cos(t5))*dt4+(-cos(t1)*dt1*cos(t2+t3+t4)*sin(t5)+sin(t1)*sin(t2+t3+t4)*(dt2+dt3+dt4)*sin(t5)-sin(t1)*cos(t2+t3+t4)*cos(t5)*dt5+sin(t1)*dt1*cos(t5)+cos(t1)*sin(t5)*dt5)*dt5; ddnz=(-sin(t2+t3+t4)*(dt2+dt3+dt4)*cos(t5)-cos(t2+t3+t4)*sin(t5)*dt5)*dt2+(-sin(t2+t3+t4)*(dt2+dt3+dt4)*cos(t5)-cos(t2+t3+t4)*sin(t5)*dt5)*dt3+(-sin(t2+t3+t4)* (dt2+dt3+dt4)*cos(t5)-cos(t2+t3+t4)*sin(t5)*dt5)*dt4+(-cos(t2+t3+t4)*(dt2+dt3+dt4)*sin(t5)-sin(t2+t3+t4)*cos(t5)*dt5)*dt5; ddsx=(+cos(t1)*dt1*cos(t2+t3+t4)*sin(t5)-sin(t1)*cos(t2+t3+t4)* (dt2+dt3+dt4)*sin(t5)+sin(t1)*cos(t2+t3+t4)*cos(t5)*dt5-sin(t1)*dt1*cos(t5)-cos(t1)*sin(t5)*dt5)*dt1+(-sin(t1)*dt1*sin(t2+t3+t4)*sin(t5)+cos(t1)*cos(t2+t3+t4)*(dt2+dt3+dt4)*sin(t5)+cos(t1)*sin(t2+t3+t4)*cos(t5)*dt5)*dt2+(-sin(t1)*dt1*sin(t2+t3+t4)*sin(t5)+cos(t1)*cos(t2+t3+t4)*(dt2+dt3+dt4)*sin(t5)+cos(t1)*sin(t2+t3+t4)*cos(t5)*dt5)*dt3+(-sin(t1)*dt1*sin(t2+t3+t4)*sin(t5)+cos(t1)*cos(t2+t3+t4)* (dt2+dt3+dt4)*sin(t5)+cos(t1)*sin(t2+t3+t4)*cos(t5)*dt5)*dt4+(+sin(t1)*dt1*cos(t2+t3+t4)*cos(t5)+cos(t1)*sin(t2+t3+t4)* (dt2+dt3+dt4)*cos(t5)+cos(t1)*cos(t2+t3+t4)*sin(t5)*dt5-cos(t1)*dt1*sin(t5)-sin(t1)*cos(t5)*dt5)*dt5; ddsy=(+sin(t1)*dt1*cos(t2+t3+t4)*sin(t5)+cos(t1)*sin(t2+t3+t4)* (dt2+dt3+dt4)*sin(t5)-cos(t1)*cos(t2+t3+t4)*cos(t5)*dt5+cos(t1)*dt1*cos(t5)-sin(t1)*sin(t5)*dt5)*dt1+(cos(t1)*dt1*sin(t2+t3+t4)*sin(t5)+sin(t1)*cos(t2+t3+t4)*(dt2+dt3+dt4)*sin(t5)+sin(t1)*sin(t2+t3+t4)*cos(t5)*dt5)*dt2+(+cos(t1)*dt1*sin(t2+t3+t4)*sin(t5)+sin(t1)*cos(t2+t3+t4)*(dt2+dt3+dt4)*sin(t5)+sin(t1)*sin(t2+t3+t4)*cos(t5)*dt5)*dt3+(+cos(t1)*dt1*sin(t2+t3+t4)*sin(t5)+sin(t1)*cos(t2+t3+t4)*(dt2+dt3+dt4)*sin(t5)+sin(t1)*sin(t2+t3+t4)*cos(t5)*dt5)*dt4+(-cos(t1)*dt1*cos(t2+t3+t4)*cos(t5)+sin(t1)*sin(t2+t3+t4)* (dt2+dt3+dt4)*cos(t5)+sin(t1)*cos(t2+t3+t4)*sin(t5)*dt5-sin(t1)*dt1*sin(t5)+cos(t1)*cos(t5)*dt5)*dt5; ddsz=(+sin(t2+t3+t4)* (dt2+dt3+dt4)*sin(t5)-cos(t2+t3+t4)*cos(t5)*dt5)*dt2+(+sin(t2+t3+t4)* (dt2+dt3+dt4)*sin(t5)-cos(t2+t3+t4)*cos(t5)*dt5)*dt3+(+sin(t2+t3+t4)* (dt2+dt3+dt4)*sin(t5)-


cos(t2+t3+t4)*cos(t5)*dt5)*dt4+(-cos(t2+t3+t4)*(dt2+dt3+dt4)*cos(t5)+sin(t2+t3+t4)*sin(t5)*dt5)*dt5; ddax=(-cos(t1)*dt1*sin(t2+t3+t4)-sin(t1)*cos(t2+t3+t4)*(dt2+dt3+dt4))*dt1+(-sin(t1)*dt1*cos(t2+t3+t4)-cos(t1)*sin(t2+t3+t4)* (dt2+dt3+dt4))*dt2+(-sin(t1)*dt1*cos(t2+t3+t4)-cos(t1)*sin(t2+t3+t4)* (dt2+dt3+dt4))*dt3+(-sin(t1)*dt1*cos(t2+t3+t4)-cos(t1)*sin(t2+t3+t4)*(dt2+dt3+dt4))*dt4; dday=(-sin(t1)*dt1*sin(t2+t3+t4)+cos(t1)*cos(t2+t3+t4)*(dt2+dt3+dt4))*dt1+(+cos(t1)*dt1*cos(t2+t3+t4)-sin(t1)*sin(t2+t3+t4)* (dt2+dt3+dt4))*dt2+(+cos(t1)*dt1*cos(t2+t3+t4)-sin(t1)*sin(t2+t3+t4)*(dt2+dt3+dt4))*dt3+(cos(t1)*dt1*cos(t2+t3+t4)-sin(t1)*sin(t2+t3+t4)* (dt2+dt3+dt4))*dt4; ddaz=+(cos(t2+t3+t4)* (dt2+dt3+dt4))*dt2+(cos(t2+t3+t4)* (dt2+dt3+dt4))*dt3+(+cos(t2+t3+t4)*(dt2+dt3+dt4))*dt4; ddpx=(-cos(t1)*dt1*(sin(t2+t3+t4)*d5+cos(t2+t3)*a3+cos(t2)*a2)-sin(t1)*(cos(t2+t3+t4)*(dt2+dt3+dt4)*d5-sin(t2+t3)*(dt2+dt3)*a3-sin(t2)*dt2*a2))*dt1+(-sin(t1)*dt1*(cos(t2+t3+t4)*d5-sin(t2+t3)*a3-sin(t2)*a2)+cos(t1)*(-sin(t2+t3+t4)* (dt2+dt3+dt4)*d5-cos(t2+t3)* (dt2+dt3)*a3-cos(t2)*dt2*a2))*dt2+(-sin(t1)*dt1*(cos(t2+t3+t4)*d5-sin(t2+t3)*a3)+cos(t1)*(-sin(t2+t3+t4)* (dt2+dt3+dt4)*d5-cos(t2+t3)* (dt2+dt3)*a3))*dt3+(-sin(t1)*dt1*cos(t2+t3+t4)*d5-cos(t1)*sin(t2+t3+t4)*(dt2+dt3+dt4)*d5)*dt4; ddpy=(-sin(t1)*dt1*(sin(t2+t3+t4)*d5+cos(t2+t3)*a3+cos(t2)*a2)+cos(t1)*(cos(t2+t3+t4)*(dt2+dt3+dt4)*d5-sin(t2+t3)*(dt2+dt3)*a3-sin(t2)*dt2*a2))*dt1+(+cos(t1)*dt1*(cos(t2+t3+t4)*d5-sin(t2+t3)*a3-sin(t2)*a2)+sin(t1)*(-sin(t2+t3+t4)* (dt2+dt3+dt4)*d5-cos(t2+t3)* (dt2+dt3)*a3-cos(t2)*dt2*a2))*dt2+(+cos(t1)*dt1*(cos(t2+t3+t4)*d5-sin(t2+t3)*a3)+sin(t1)*(-sin(t2+t3+t4)*(dt2+dt3+dt4)*d5-cos(t2+t3)* (dt2+dt3)*a3))*dt3+(+cos(t1)*dt1*cos(t2+t3+t4)*d5-sin(t1)*sin(t2+t3+t4)* (dt2+dt3+dt4)*d5)*dt4; ddpz=(+cos(t2+t3+t4)* (t2+t3+t4)*d5-sin(t2+t3)*(dt2+dt3)*a3-sin(t2)*dt2*a2)*dt2+(+cos(t2+t3+t4)* (dt2+dt3+dt4)*d5-sin(t2+t3)*(dt2+dt3)*a3)*dt3+(cos(t2+t3+t4)* (dt2+dt3+dt4)*d5)*dt4; c=[ddnx, ddny, ddnz, ddsx, ddsy, ddsz, ddax, dday, ddaz, ddpx, ddpy, ddpz]'; matdatos=c;


Programa para calcular la dinámica utilizando el método de Lagrange-Euler

dinamica.m


%PROGRAMA QUE CALCULA LAS FUERZAS GENERALIZADAS DEL MIRH1 mom=[]; h=[]; C=[]; enerk=[]; enerp=[]; for i=1:11 a =[q(n,1)]; b =[q(n,2)]; c =[q(n,3)]; d =[q(n,4)]; e =[q(n,5)]; da =[dq(i,1)]; db =[dq(i,2)]; dc =[dq(i,3)]; dd =[dq(i,4)]; de =[dq(i,5)]; dda =[ddq(i,1)]; ddb =[ddq(i,2)]; ddc =[ddq(i,3)]; ddd =[ddq(i,4)]; dde =[ddq(i,5)]; a0=0; a1=0; a2=0.450; a3=0.450; a4=0; a5=0; alpha0= 0; alpha1= pi/2; alpha2= 0; alpha3= 0; alpha4= pi/2; alpha5= 0; d1=0; d2=0; d3=0; d4=0; d5=-0.442;


A01 = [ cos(a) , -sin(a), 0 , a0 ; sin(a)*cos(alpha0), cos(a)*cos(alpha0), -sin(alpha0) , -sin(alpha0)*d1 ; sin(a)*sin(alpha0), cos(a)*sin(alpha0), cos(alpha0) , cos(alpha0)*d1 ; 0 0 0 1 ]; A12 = [ cos(b) , -sin(b), 0 , a1 ; sin(b)*0, cos(b)*0, -1 , -1*d2 ; sin(b)*1, cos(b)*1, 0 , 0*d2 ; 0 , 0 , 0 , 1 ]; A23 = [ cos(c) , -sin(c), 0 , a2 ; sin(c)*cos(alpha2), cos(c)*cos(alpha2), -sin(alpha2) , -sin(alpha2)*d3 ; sin(c)*sin(alpha2), cos(c)*sin(alpha2), cos(alpha2) , cos(alpha2)*d3 ; 0 0 0 1 ]; A34 = [ cos(d) , -sin(d), 0 , a3 ; sin(d)*cos(alpha3), cos(d)*cos(alpha3), -sin(alpha3) , -sin(alpha3)*d4 ; sin(d)*sin(alpha3), cos(d)*sin(alpha3), cos(alpha3) , cos(alpha3)*d4 ; 0 0 0 1 ]; A45 = [ cos(e) , -sin(e), 0 , a4 ; sin(e)*0, cos(e)*0, -1 , -1*d5 ; sin(e)*1, cos(e)*1, 0 , 0*d5 ; 0 0 0 1 ]; %disp('......................................................................................................................') %disp('......................................................................................................................') %disp('......................................................................................................................') %disp(' CALCULO DE LA DINAMICA.') %disp('......................................................................................................................') m1= 21843.61;%g; m1=m1/1000;%kg x1=-112.70; x1=x1/1000;%mm y1=-0.01; y1=y1/1000;%mm z1=-31.74; z1=z1/1000;%mm m2=14109.13;%g; x2=250.11; y2=-18.75; z2=-0.09; m2=m2/1000;%kg x2=x2/1000;%mm y2=y2/1000;%mm


z2=z2/1000;%mm m3=8088.14;%g; x3=242.43; y3=0.8; z3=0.52; m3=m3/1000;%kg x3=x3/1000;%mm y3=y3/1000;%mm z3=z3/1000;%mm m4 =6005.60;%g; x4 =264.57; y4 =-0.39; z4 = 0; m4=m4/1000;%kg x4=x4/1000;%mm y4=y4/1000;%mm z4=z4/1000;%mm m5 = 429.5; x5 = 0.02562; y5 = 0.0; z5 = 122.17; m5=m5/1000;%kg x5=x5/1000;%mm y5=y5/1000;%mm z5=z5/1000;%mm % x1=1;y1=1;z1=1; % x5=1;y5=1;z5=1; % x4=1;y4=1;z4=1; % x3=1;y3=1;z3=1; % x2=1;y2=1;z2=1; r1=[x1,y1,z1,1]'; r2=[x2,y2,z2,1]'; r3=[x3,y3,z3,1]'; r4=[x4,y4,z4,1]'; r5=[x5,y5,z5,1]'; A02=A01*A12; A03=A02*A23; A04=A03*A34; A05=A04*A45;


r01=A01*r1; r02=A02*r2; r03=A03*r3; r04=A04*r4; r05=A05*r5; A02=A01*A12; A03=A02*A23; A04=A03*A34; A05=A04*A45; A1=A01; A2=A12; A3=A23; A4=A34; A5=A45; A21=A2*A1; A31=A3*A21; A41=A4*A31; A51=A5*A41; A12=A1*A2; A32=A3*A2; A42=A4*A3*A2; A13=A1*A2*A3; A23=A2*A3; A43=A4*A3; A14=A1*A2*A3*A4; A24=A2*A3*A4; A34=A3*A4; A15=A1*A2*A3*A4*A5; A25=A2*A3*A4*A5; A35=A3*A4*A5; A45=A4*A5; Q1=[0 -1 0 0;1 0 0 0;0 0 0 0;0 0 0 0]; Q2=Q1; Q3=Q1; Q4=Q1; Q5=Q1; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %CALCULO DE LA VELOCIDAD LINEAL


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% U11=Q1*A01; U12=0; U13=A02*Q1*A21; U14=A03*Q1*A31; U15=A04*Q1*A41; U21=Q2*A02; U22=A01*Q2*A12; U23=0; U24=A03*Q2*A32; U25=A04*Q2*A42; U31=Q3*A03; U32=A01*Q3*A13; U33=A02*Q3*A23; U34=0; U35=A04*Q3*A43; U41=Q4*A01; U42=A01*Q4*A14; U43=A02*Q4*A24; U44=A03*Q4*A34; U45=0; U51=Q5*A05; U52=A01*Q5*A15; U53=A02*Q5*A25; U54=A03*Q5*A35; U55=A04*Q5*A45; v1=U11*da*r1; v2=(U11*da+U12*da+U22*db)*r2; v3=(U11*da+U12*da+U13*da+U22*db+U23*db+U33*dc)*r3; v4=(U11*da+U12*da+U13*da+U14*da+U22*db+U23*db+U24*db+U33*dc+U34*dc+U44*dd)*r4; v5=(U11*da+U12*da+U13*da+U14*da+U15*da+U22*db+U23*db+U24*db+U25*db+U33*dc+U34*dc+U44*dd+U45*dd+U55*de)*r5; %disp('las velocidades de los eslabones son:') v1; v2;


v3; v4; v5; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %CALCULO DE LA ENERGIA CINETICA %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%tensor de inercia5%%%%%%%%%%%%%%%%%%%%%%%% %%%%unidades gr*mm^2 Ixx1=391781677.14; Ixy1=37189.73; Ixz1=-133007524.40; Iyx1=37189.73; Iyy1=719094618.17; Iyz1=-2648.77; Izx1=-133007524.4; Izy=-2648.77; Izz1=465967556.7; %%%%unidades kg*m^2 Ixx1=0.391781677; Ixy1=3.71897E-05; Ixz1=-0.133007524; Iyx1=3.71897E-05; Iyy1=0.719094618; Iyz1=-2.64877E-06; Izx1=-0.133007524; Izy1=-2.64877E-06; Izz1=0.465967557; A1=(-Ixx1+Iyy1+Izz1)/2; B1=(Ixx1-Iyy1+Izz1)/2; C1=(Ixx1+Iyy1-Izz1)/2; J1=[ A1, Ixy1 , Ixz1 , m1*x1 ; Ixy1, B1 , Iyz1 , m1*y1 ;Ixz1, Iyz1 , C1 , m1*z1 ; m1*x1, m1*y1 , m1*z1 , m1 ]; %%%%unidades gr*mm^2 Ixx2=62009811.53; Ixy2=-6557103.11; Ixz2=69286.95; Iyx2=-6557103.11;


Iyy2=247236143.2; Iyz2=128968.86; Izx2=69286.95; Izy2=128968.86; Izz2=256148909.4; %%%%unidades kg*m^2 Ixx2=0.06200981; Ixy2=-0.0065571; Ixz2=6.9287E-05; Iyx2=-0.0065571; Iyy2=0.24723614; Iyz2=0.00012897; Izx2=6.9287E-05; Izy2=0.00012897; Izz2=0.25614891; A2=(-Ixx2+Iyy2+Izz2)/2; B2=(Ixx2 -Iyy2+Izz2)/2; C2=(Ixx2+Iyy2-Izz2)/2; J2=[ A2, Ixy2 , Ixz2 , m2*x2 ; Ixy2, B2 , Iyz2 , m2*y2 ;Ixz2, Iyz2 , C2 , m2*z2 ; m2*x2, m2*y2 , m2*z2 , m2 ]; %%%%unidades gr*mm^2 Ixx3=34488456.64; Ixy3=-2005641.77; Ixz3=929828.4; Iyx3=-2005641.77; Iyy3=239412076.7; Iyz3=-3678.19; Izx3=929828.4; Izy3=-3678.19; Izz3=226877155.9; %%%%unidades kg*m^2 Ixx3=0.03448846; Ixy3=-0.00200564; Ixz3=0.00092983; Iyx3=-0.00200564; Iyy3=0.23941208; Iyz3=-3.6782E-06; Izx3=0.00092983; Izy3=-3.6782E-06; Izz3=0.22687716; A3=(-Ixx3 + Iyy3 +Izz3)/2; B3=(Ixx3 -Iyy3 +Izz3)/2; C3=(Ixx3+Iyy3-Izz3)/2;


J3=[ A3, Ixy3 , Ixz3 , m3*x3 ; Ixy3, B3 , Iyz3 , m3*y3 ;Ixz3, Iyz3 , C3 , m3*z3 ; m3*x3, m3*y3 , m3*z3 , m3 ]; %%%%unidades gr*mm^2 Ixx4=12120201.7; Ixy4=-315008.03; Ixz4=64.39; Iyx4=-315008.03; Iyy4=109259555; Iyz4=216.64; Izx4=64.39; Izy4=216.64; Izz4=104761902; %%%%unidades kg*m^2 Ixx4=0.0121202; Ixy4=-0.00031501; Ixz4=6.439E-08; Iyx4=-0.00031501; Iyy4=0.10925956; Iyz4=2.1664E-07; Izx4=6.439E-08; Izy4=2.1664E-07; Izz4=0.1047619; A4=(-Ixx4 +Iyy4 +Izz4)/2; B4=(Ixx4 -Iyy4 +Izz4)/2; C4=(Ixx4 +Iyy4 -Izz4)/2; J4=[ A4, Ixy4 , Ixz4 , m4*x4 ; Ixy4, B4 , Iyz4 , m4*y4 ;Ixz4, Iyz4 , C4 , m4*z4 ; m4*x4, m4*y4 , m4*z4 , m4 ]; %%%%unidades gr*mm^2 Ixx5=200549.276; Ixy5=0; Ixz5=0; Iyx5=0; Iyy5=298323.5902; Iyz5=-198.81271; Izx5=0; Izy5=-198.81271; Izz5=200422.7442; %%%%unidades kg*m^2 Ixx5=0.00020055; Ixy5=0; Ixz5=0; Iyx5=0; Iyy5=0.00029832; Iyz5=-1.9881E-07; Izx5=0;


Izy5=-1.9881E-07; Izz5=0.00020042; A5=(-Ixx5 +Iyy5 +Izz5)/2; B5=(Ixx5 -Iyy5 +Izz5)/2; C5=(Ixx5 +Iyy5 -Izz5)/2; J5=[ A5, Ixy5 , Ixz5 , m5*x5 ; Ixy5, B5 , Iyz5 , m5*y5 ;Ixz5, Iyz5 , C5 , m5*z5 ; m5*x5, m5*y5 , m5*z5 , m5 ]; %disp('......................................................................................................................') %disp('Los tensores de inercia son:') J1; J2; J3; J4; J5; %disp('......................................................................................................................') K=1/2*(TRACE(U11*J1*U11')*da*da+TRACE(U21*J2*U21')*da*da+TRACE(U21*J2*U22')*da*db+TRACE(U22*J2*U21')*db*da+TRACE(U22*J2*U22')*db*db+TRACE(U31*J3*U31')*da*da+TRACE(U31*J3*U32')*da*db+TRACE(U31*J3*U33')*da*dc+TRACE(U32*J3*U31')*db*da+TRACE(U32*J3*U32')*db*db+TRACE(U32*J3*U33')*db*dc+TRACE(U33*J3*U31')*dc*da+TRACE(U33*J3*U32')*dc*db+TRACE(U33*J3*U33')*dc*dc+TRACE(U41*J4*U41')*da*da+TRACE(U41*J4*U42')*da*db+TRACE(U41*J4*U43')*da*dc+TRACE(U41*J4*U44')*da*dd+TRACE(U42*J4*U41')*db*da+TRACE(U42*J4*U42')*db*db+TRACE(U42*J4*U43')*db*dc+TRACE(U42*J4*U44')*db*dd+TRACE(U43*J4*U41')*dc*da+TRACE(U43*J4*U42')*dc*db+TRACE(U43*J4*U43')*dc*dc+TRACE(U43*J4*U44')*dc*dd+TRACE(U44*J4*U41')*dd*da+TRACE(U44*J4*U42')*dd*db+TRACE(U44*J4*U43')*dd*dc+TRACE(U44*J4*U44')*dd*dd+TRACE(U51*J5*U51')*da*da+TRACE(U51*J5*U52')*da*db+TRACE(U51*J5*U53')*da*dc+TRACE(U51*J5*U54')*da*dd+TRACE(U51*J5*U55')*da*de+TRACE(U52*J5*U51')*db*da+TRACE(U52*J5*U52')*db*db+TRACE(U52*J5*U53')*db*dc+TRACE(U52*J5*U54')*db*dd+TRACE(U52*J5*U55')*db*de+TRACE(U53*J5*U51')*dc*da+TRACE(U53*J5*U52')*dc*db+TRACE(U53*J5*U53')*dc*dc+TRACE(U53*J5*U54')*dc*dd+TRACE(U53*J5*U55')*dc*de+TRACE(U54*J5*U51')*dd*da+TRACE(U54*J5*U52')*dd*db+TRACE(U54*J5*U53')*dd*dc+TRACE(U54*J5*U54')*dd*dd+TRACE(U54*J5*U55')*dd*de+TRACE(U55*J5*U51')*de*da+TRACE(U55*J5*U52')*de*db+TRACE(U55*J5*U53')*de*dc+TRACE(U55*J5*U54')*de*dd+TRACE(U55*J5*U55')*de*de); %fprintf('La energia cinetica total del brazo es de :%7.40f\n',K); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %CALCULO DE LA ENERGIA POTENCIAL %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% g=[0,0,-9.81,0]; P=-m1*g*(A01*r1)-m2*g*(A02*r2)-m3*g*(A03*r3)-m4*g*(A04*r4)-m5*g*(A05*r5); %disp('......................................................................................................................') %fprintf('La energia potencial total del brazo es de :%7.20f\n',P); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %CALCULO DE LA LAGRANGIANA %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% L=K+P; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %CALCULO DEL VECTOR DE FUERZA DE CORIOLIS Y CENTRIFUGA %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% D11=TRACE(U11*J1*U11')+TRACE(U21*J2*U21')+TRACE(U31*J3*U31')+TRACE(U41*J4*U41')+TRACE(U51*J5*U51'); D12=TRACE(U22*J2*U21')+TRACE(U32*J3*U31')+TRACE(U42*J4*U41')+TRACE(U52*J5*U51'); D13=TRACE(U33*J3*U31')+TRACE(U43*J4*U41')+TRACE(U53*J5*U51'); D14=TRACE(U44*J4*U41')+TRACE(U54*J5*U51'); D15=TRACE(U55*J5*U51'); D21=TRACE(U21*J2*U22')+TRACE(U31*J3*U32')+TRACE(U41*J4*U42')+TRACE(U51*J5*U52'); D22=TRACE(U22*J2*U22')+TRACE(U32*J3*U32')+TRACE(U42*J4*U42')+TRACE(U52*J5*U52'); D23=TRACE(U33*J3*U32')+TRACE(U43*J4*U42')+TRACE(U53*J5*U52');


D24=TRACE(U44*J4*U42')+TRACE(U54*J5*U52'); D25=TRACE(U55*J5*U52'); D31=TRACE(U31*J3*U33')+TRACE(U41*J4*U43')+TRACE(U51*J5*U53'); D32=TRACE(U32*J3*U33')+TRACE(U43*J4*U43')+TRACE(U53*J5*U53'); D33=TRACE(U33*J3*U33')+TRACE(U43*J4*U43')+TRACE(U53*J5*U53'); D34=TRACE(U44*J4*U43')+TRACE(U54*J5*U53); D35=TRACE(U53*J5*U55'); D41=TRACE(U41*J4*U44')+TRACE(U51*J5*U54'); D42=TRACE(U42*J4*U44')+TRACE(U52*J5*U54'); D43=TRACE(U43*J4*U44')+TRACE(U53*J5*U54'); D44=TRACE(U44*J4*U44')+TRACE(U54*J5*U54'); D45=TRACE(U55*J5*U54'); D51=TRACE(U51*J5*U55'); D52=TRACE(U52*J5*U55'); D53=TRACE(U53*J5*U55'); D54=TRACE(U54*J5*U55'); D55=TRACE(U55*J5*U55'); Q=Q1; U111=Q*Q1*A01; U112=0; U113=Q*A02*Q1*A21; U114=Q*A03*Q1*A31; U115=Q*A04*Q1*A41; U121=Q*Q2*A02; U122=Q*A01*Q2*A12; U123=0; U124=Q*A03*Q2*A32; U125=Q*A04*Q2*A42; U131=Q*Q3*A03; U132=Q*A01*Q3*A13; U133=Q*A02*Q3*A23; U134=0; U135=Q*A04*Q3*A43; U141=Q*Q4*A01; U142=Q*A01*Q4*A14; U143=Q*A02*Q4*A24; U144=Q*A03*Q4*A34; U145=0; U151=Q*Q5*A05;


U152=Q*A01*Q5*A15; U153=Q*A02*Q5*A25; U154=Q*A03*Q5*A35; U155=Q*A04*Q5*A45; U211=Q*Q1*A01; U212=0; U213=Q*A02*Q1*A21; U214=Q*A03*Q1*A31; U215=Q*A04*Q1*A41; U221=Q*Q2*A02; U222=Q*A01*Q2*A12; U223=0; U224=Q*A03*Q2*A32; U225=Q*A04*Q2*A42; U231=Q*Q3*A03; U232=Q*A01*Q3*A13; U233=Q*A02*Q3*A23; U234=0; U235=Q*A04*Q3*A43; U241=Q*Q4*A01; U242=Q*A01*Q4*A14; U243=Q*A02*Q4*A24; U244=Q*A03*Q4*A34; U245=0; U251=Q*Q5*A05; U252=Q*A01*Q5*A15; U253=Q*A02*Q5*A25; U254=Q*A03*Q5*A35; U255=Q*A04*Q5*A45; U311=Q*Q1*A01; U312=0; U313=Q*A02*Q1*A21; U314=Q*A03*Q1*A31; U315=Q*A04*Q1*A41; U321=Q*Q2*A02; U322=Q*A01*Q2*A12; U323=0; U324=Q*A03*Q2*A32; U325=Q*A04*Q2*A42;


U331=Q*Q3*A03; U332=Q*A01*Q3*A13; U333=Q*A02*Q3*A23; U334=0; U335=Q*A04*Q3*A43; U341=Q*Q4*A01; U342=Q*A01*Q4*A14; U343=Q*A02*Q4*A24; U344=Q*A03*Q4*A34; U345=0; U351=Q*Q5*A05; U352=Q*A01*Q5*A15; U353=Q*A02*Q5*A25; U354=Q*A03*Q5*A35; U355=Q*A04*Q5*A45; U411=Q*Q1*A01; U412=0; U413=Q*A02*Q1*A21; U414=Q*A03*Q1*A31; U415=Q*A04*Q1*A41; U421=Q*Q2*A02; U422=Q*A01*Q2*A12; U423=0; U424=Q*A03*Q2*A32; U425=Q*A04*Q2*A42; U431=Q*Q3*A03; U432=Q*A01*Q3*A13; U433=Q*A02*Q3*A23; U434=0; U435=Q*A04*Q3*A43; U441=Q*Q4*A01; U442=Q*A01*Q4*A14; U443=Q*A02*Q4*A24; U444=Q*A03*Q4*A34; U445=0; U451=Q*Q5*A05; U452=Q*A01*Q5*A15; U453=Q*A02*Q5*A25; U454=Q*A03*Q5*A35; U455=Q*A04*Q5*A45;


U511=Q*Q1*A01; U512=0; U513=Q*A02*Q1*A21; U514=Q*A03*Q1*A31; U515=Q*A04*Q1*A41; U521=Q*Q2*A02; U522=Q*A01*Q2*A12; U523=0; U524=Q*A03*Q2*A32; U525=Q*A04*Q2*A42; U531=Q*Q3*A03; U532=Q*A01*Q3*A13; U533=Q*A02*Q3*A23; U534=0; U535=Q*A04*Q3*A43; U541=Q*Q4*A01; U542=Q*A01*Q4*A14; U543=Q*A02*Q4*A24; U544=Q*A03*Q4*A34; U545=0; U551=Q*Q5*A05; U552=Q*A01*Q5*A15; U553=Q*A02*Q5*A25; U554=Q*A03*Q5*A35; U555=Q*A04*Q5*A45; h111=TRACE(U111*J1*U11')+TRACE(U211*J2*U21')+TRACE(U311*J3*U31')+TRACE(U411*J4*U41')+TRACE(U511*J5*U51'); h112=TRACE(U212*J2*U21')+TRACE(U312*J3*U31')+TRACE(U412*J4*U41')+TRACE(U512*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=TRACE(U221*J2*U21')+TRACE(U321*J3*U31')+TRACE(U421*J4*U41')+TRACE(U521*J5*U51');


h122=TRACE(U222*J2*U21')+TRACE(U322*J3*U31')+TRACE(U422*J4*U41')+TRACE(U522*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=TRACE(U331*J3*U31')+TRACE(U431*J4*U41')+TRACE(U531*J5*U51'); h132=TRACE(U332*J3*U31')+TRACE(U432*J4*U41')+TRACE(U532*J5*U51'); 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=TRACE(U441*J4*U41')+TRACE(U541*J5*U51'); h142=TRACE(U442*J4*U41')+TRACE(U542*J5*U51'); h143=TRACE(U443*J4*U41')+TRACE(U543*J5*U51'); h144=TRACE(U444*J4*U41')+TRACE(U544*J5*U51'); h145=TRACE(U545*J5*U51'); h151=TRACE(U551*J5*U51'); h152=TRACE(U552*J5*U51'); h153=TRACE(U553*J5*U51'); h154=TRACE(U554*J5*U51'); h155=TRACE(U555*J5*U51'); h211=TRACE(U211*J2*U22')+TRACE(U311*J3*U32')+TRACE(U411*J4*U42')+TRACE(U511*J5*U52'); h212=TRACE(U212*J2*U22')+TRACE(U312*J3*U32')+TRACE(U412*J4*U42')+TRACE(U512*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=TRACE(U221*J2*U22')+TRACE(U321*J3*U32')+TRACE(U421*J4*U42')+TRACE(U521*J5*U52'); h222=TRACE(U222*J2*U22')+TRACE(U322*J3*U32')+TRACE(U422*J4*U42')+TRACE(U522*J5*U52'); h223=TRACE(U322*J3*U32')+TRACE(U422*J4*U42')+TRACE(U522*J5*U52'); h224=TRACE(U424*J4*U42')+TRACE(U524*J5*U52'); h225=TRACE(U525*J5*U52'); h231=TRACE(U331*J3*U32')+TRACE(U431*J4*U42')+TRACE(U531*J5*U52'); h232=TRACE(U332*J3*U32')+TRACE(U432*J4*U42')+TRACE(U532*J5*U52');


h233=TRACE(U333*J3*U32')+TRACE(U433*J4*U42')+TRACE(U533*J5*U52'); h234=TRACE(U434*J4*U42')+TRACE(U534*J4*U52'); h235=TRACE(U535*J5*U52'); h241=TRACE(U441*J4*U42')+TRACE(U541*J5*U52'); h242=TRACE(U442*J4*U42')+TRACE(U542*J5*U52'); h243=TRACE(U443*J4*U42')+TRACE(U543*J5*U52'); h244=TRACE(U444*J4*U42')+TRACE(U544*J4*U52'); h245=TRACE(U545*J5*U52'); h251=TRACE(U551*J5*U52'); h252=TRACE(U552*J5*U52'); h253=TRACE(U553*J5*U52'); h254=TRACE(U554*J5*U52'); h255=TRACE(U555*J5*U52'); 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=TRACE(U321*J3*U33')+TRACE(U421*J4*U43')+TRACE(U521*J5*U53'); 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=TRACE(U331*J3*U33')+TRACE(U431*J4*U43')+TRACE(U531*J5*U53'); h332=TRACE(U332*J3*U33')+TRACE(U432*J4*U43')+TRACE(U532*J5*U53'); 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=TRACE(U441*J4*U43')+TRACE(U541*J5*U53'); h342=TRACE(U442*J4*U43')+TRACE(U542*J5*U53'); h343=TRACE(U443*J4*U43')+TRACE(U543*J5*U53'); h344=TRACE(U444*J4*U43')+TRACE(U544*J5*U53'); h345=TRACE(U545*J5*U53'); h351=TRACE(U551*J5*U53'); h352=TRACE(U552*J5*U53'); h353=TRACE(U553*J5*U53'); h354=TRACE(U554*J5*U53'); h355=TRACE(U555*J5*U53'); h411=TRACE(U411*J4*U44')+TRACE(U511*J5*U54');


h412=TRACE(U412*J4*U44')+TRACE(U512*J5*U54'); h413=TRACE(U413*J4*U44')+TRACE(U513*J5*U54'); h414=TRACE(U414*J4*U44')+TRACE(U514*J5*U54'); h415=TRACE(U515*J5*U54'); h421=TRACE(U421*J4*U44')+TRACE(U521*J5*U54'); h422=TRACE(U422*J4*U44')+TRACE(U522*J5*U54'); h423=TRACE(U423*J4*U44')+TRACE(U523*J5*U54'); h424=TRACE(U424*J4*U44')+TRACE(U524*J5*U54'); h425=TRACE(U525*J5*U54'); h431=TRACE(U431*J4*U44')+TRACE(U531*J5*U54'); h432=TRACE(U432*J4*U44')+TRACE(U532*J5*U54'); h433=TRACE(U433*J4*U44')+TRACE(U533*J5*U54'); h434=TRACE(U434*J4*U44')+TRACE(U534*J5*U54'); h435=TRACE(U535*J5*U54'); h441=TRACE(U441*J4*U44')+TRACE(U541*J5*U54'); h442=TRACE(U442*J4*U44')+TRACE(U542*J5*U54'); h443=TRACE(U443*J4*U44')+TRACE(U543*J5*U54'); h444=TRACE(U444*J4*U44')+TRACE(U544*J5*U54'); h445=TRACE(U545*J5*U54'); h451=TRACE(U551*J5*U54'); h452=TRACE(U552*J5*U54'); h453=TRACE(U553*J5*U54'); h454=TRACE(U554*J5*U54'); h455=TRACE(U555*J5*U54'); 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=TRACE(U521*J5*U55'); h522=TRACE(U522*J5*U55'); h523=TRACE(U523*J5*U55'); h524=TRACE(U524*J5*U55'); h525=TRACE(U525*J5*U55'); h531=TRACE(U531*J5*U55'); h532=TRACE(U532*J5*U55'); h533=TRACE(U533*J5*U55'); h534=TRACE(U534*J5*U55'); h535=TRACE(U535*J5*U55');


h541=TRACE(U541*J5*U55'); h542=TRACE(U542*J5*U55'); h543=TRACE(U543*J5*U55'); h544=TRACE(U544*J5*U55'); h545=TRACE(U545*J5*U55'); h551=TRACE(U551*J5*U55'); h552=TRACE(U552*J5*U55'); h553=TRACE(U553*J5*U55'); h554=TRACE(U554*J5*U55'); h555=TRACE(U555*J5*U55'); h1=h111*da*da+h112*da*db+h113*da*dc+h114*da*dd+h115*da*de+h121*db*da+h122*db*db+h123*db*dc+h124*db*dd+h125*db*de+h131*dc*da+h132*dc*db+h133*dc*dc+h134*dc*dd+h135*dc*de+h141*dd*da+h142*dd*db+h143*dd*dc+h144*dd*dd+h145*dd*de+h151*de*da+h152*de*db+h153*de*dc+h154*de*dd+h155*de*de; h2=h211*da*da+h212*da*db+h213*da*dc+h214*da*dd+h215*da*de+h221*db*da+h222*db*db+h223*db*dc+h224*db*dd+h225*db*de+h231*dc*da+h232*dc*db+h233*dc*dc+h234*dc*dd+h235*dc*de+h241*dd*da+h242*dd*db+h243*dd*dc+h244*dd*dd+h245*dd*de+h251*de*da+h252*de*db+h253*de*dc+h254*de*dd+h255*de*de; h3=h311*da*da+h312*da*db+h313*da*dc+h314*da*dd+h315*da*de+h321*db*da+h322*db*db+h323*db*dc+h324*db*dd+h325*db*de+h331*dc*da+h332*dc*db+h333*dc*dc+h334*dc*dd+h335*dc*de+h341*dd*da+h342*dd*db+h343*dd*dc+h344*dd*dd+h345*dd*de+h351*de*da+h352*de*db+h353*de*dc+h354*de*dd+h355*de*de; h4=h411*da*da+h412*da*db+h413*da*dc+h414*da*dd+h415*da*de+h421*db*da+h422*db*db+h423*db*dc+h424*db*dd+h425*db*de+h431*dc*da+h432*dc*db+h433*dc*dc+h434*dc*dd+h435*dc*de+h441*dd*da+h442*dd*db+h443*dd*dc+h444*dd*dd+h445*dd*de+h451*de*da+h452*de*db+h453*de*dc+h454*de*dd+h455*de*de; h5=h511*da*da+h512*da*db+h513*da*dc+h514*da*dd+h515*da*de+h521*db*da+h522*db*db+h523*db*dc+h524*db*dd+h525*db*de+h531*dc*da+h532*dc*db+h533*dc*dc+h534*dc*dd+h535*dc*de+h541*dd*da+h542*dd*db+h543*dd*dc+h544*dd*dd+h545*dd*de+h551*de*da+h552*de*db+h553*de*dc+h554*de*dd+h555*de*de; c1=-m1*g*U11*r1-m2*g*U21*r3-m3*g*U31*r3-m4*g*U41*r4-m5*g*U51*r5; c2=-m2*g*U22*r2-m3*g*U32*r3-m4*g*U42*r4-m5*g*U52*r5; c3=-m3*g*U33*r3-m4*g*U43*r4-m5*g*U53*r5; c4=-m4*g*U44*r4-m5*g*U54*r5; c5=-m5*g*U55*r5; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %CALCULO DE LOS MOMENTOS EN LAS ARTICULACIONES %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% THAO1=D11*dda+D12*ddb+D13*ddc+D14*ddd+D15*dde+h111*da*da+h112*da*db+h113*da*dc+h114*da*dd+h115*da*de+h121*db*da+h122*db*db+h123*db*dc+h124*db*dd+h125*db*de+h131*dc*da+h132*dc*db+h133*dc*dc+h134*dc*dd+h135*dc*de+h141*dd*da+h142*dd*db+h143*dd*dc+h144*dd*dd+h145*dd*de+h151*de*da+h152*de*db+h153*de*dc+h154*de*dd+h155*de*de+c1; THAO2=D21*dda+D22*ddb+D23*ddc+D24*ddd+D25*dde+h211*da*da+h212*da*db+h213*da*dc+h214*da*dd+h215*da*de+h221*db*da+h222*db*db+h223*db*dc+h224*db*dd+h225*db*de+h231*dc*da+h232*dc*db+h233*dc*dc+h234*dc*dd+h235*dc*de+h241*dd*da+h242*dd*db+h243*dd*dc+h244*dd*dd+h245*dd*de+h251*de*da+h252*de*db+h253*de*dc+h254*de*dd+h255*de*de+c2; THAO3=D31*dda+D32*ddb+D33*ddc+D34*ddd+D35*dde+h311*da*da+h312*da*db+h313*da*dc+h314*da*dd+h315*da*de+h321*db*da+h322*db*db+h323*db*dc+h324*db*dd+h325*db*de+h331*dc*da+h332*dc*db+h333*dc*dc+h334*dc*dd+h335*dc*de+h341*dd*da+h342*dd*db+h343*dd*dc+h344*dd*dd+h345*dd*de+h351*de*da+h352*de*db+h353*de*dc+h354*de*dd+h355*de*de+c3; THAO4=D41*dda+D42*ddb+D43*ddc+D44*ddd+D45*dde+h411*da*da+h412*da*db+h413*da*dc+h414*da*dd+h415*da*de+h421*db*da+h422*db*db+h423*db*dc+h424*db*dd+h425*db*de+h431*dc*da+h432*dc*db+h433*dc*dc+h434*dc*dd+h435*dc*de+h441*dd*da+h442*dd*db+h443*dd*dc+h444*dd*dd+h445*dd*de+h451*de*da+h452*de*db+h453*de*dc+h454*de*dd+h455*de*de+c4; THAO5=D51*dda+D52*ddb+D53*ddc+D54*ddd+D55*dde+h511*da*da+h512*da*db+h513*da*dc+h514*da*dd+h515*da*de+h521*db*da+h522*db*db+h523*db*dc+h224*db*dd+h525*db*de+h531*dc*da+h532*dc*db+h533*dc*dc+h534*dc*dd+h535*dc*de+h541*dd*da+h542*dd*db+h543*dd*dc+h544*dd*dd+h545*dd*de+h551*de*da+h552*de*db+h553*de*dc+h554*de*dd+h555*de*de+c5; %disp('......................................................................................................................') momi=[THAO1,THAO2,THAO3,THAO4,THAO5]; mom=[momi;mom]; enerki=k; enerk=[enerk;enerki]; enerpi=P; enerp=[enerp;enerpi]; hijk=[h111,h112,h113,h114,h115; h121,h122,h123,h124,h125;


h131,h132,h133,h134,h135; h141,h142,h143,h144,h145; h151,h152,h153,h154,h155; h211,h212,h213,h214,h215; h221,h222,h223,h224,h225; h231,h232,h233,h234,h235; h241,h242,h243,h244,h245; h251,h252,h253,h254,h255; h311,h312,h313,h314,h315; h321,h322,h323,h324,h325; h331,h332,h333,h334,h335; h341,h342,h343,h344,h345; h351,h352,h353,h354,h355; h411,h412,h413,h414,h415; h421,h422,h423,h424,h425; h431,h432,h433,h434,h435; h441,h442,h443,h444,h445; h451,h452,h453,h454,h455; h511,h512,h513,h514,h515; h521,h522,h523,h524,h525; h531,h532,h533,h534,h535; h541,h542,h543,h544,h545; h551,h552,h553,h554,h555]; hi=[h1,h2,h3,h4,h5]; h=[h;hi] ; Ci=[c1,c2,c3,c4,c5]; C=[C;Ci]; end momentos=[mom,enerk,enerp]


BIBLIOGRAFIA.

ANGELES, Jorge FUNDAMENTALS OF ROBOTIC MECHANICAL SYSTEMS. Canada. Springer 1997.

BEDFORD, A., WALLACE F. MECÁNICA PARA INGENIERIA, DINÁMICA. México. Addison Weasley Iberoamericana 2000.

BEER P., RUSSEL J, MECANICA VECTORIAL PARA INGENIEROS ESTÁTICA, Quinta edición, Mexico, Mc Graw Hill 1990.

BEER P., RUSSEL J, MECANICA VECTORIAL PARA INGENIEROS DINAMICA, Quinta edición, Mexico, Mc Graw Hill 1990.

CRAIG John J. INTRODUCTION TO ROBOTICS MECHANICS AND CONTROL. EUA. Adison Wesley 1989.

FEIRER, John L. METALISTERIA ARTE Y CIENCIA DEL TRABAJO CON METALES. México. McGraw-Hill 1990.

FU, K.S. GONZALEZ, LEE ROBÓTICA: CONTROL DETECCION, VISIÓN E INTELIGENCIA. España. Mc Graw Hill 1990.

McCLOY Don, ROBOTICA UNA INTRODUCCIÓN. México. Limusa 1993

OLLERO, INTRODUCCIÓN A LA ROBÓTICA, Madrid.Marcombo 1998

PALACIOS, M. Candido, ANALISIS Y SINTESIS DE MECANISMOS TOMO I, Mexico, Instituto Politecnico Nacional 1998.

REHG, James A. INTRODUCTION TO ROBOTICS IN CIM SYSTEMS. New Jersey. Prentice – Hall 2000.

RICHARD P. Paul. ROBOT MANIPULATOR MATHEMATICS PROGRAMMING AND CONTROL. England. MIT Cambridge 1986.

SAEED, B. Niku INTRODUCTION TO ROBOTICS. ANALYSIS, SYSTEMS, APLICATIONS. New Jersey. Prentice hall 2001.

SHOICHIRO Nakamura. ANALISIS Y VISUALIZACION GRAFICA CON MATLAB.. México. Parson educación 1997.

VAZQUEZ Raymundo A. DISEÑO MECÁNICO DE UN MANIPULADOR INDUSTRIAL ROBÓTICO HIDRÁULICO (MIRH1) DE CINCO GRADOS DE LIBERTAD. México. 2005

cinemÁtica y dinÁmica del manipulador industrial …

Documents