UNIVERSIDAD NACIONAL AUTÓNOMA DE MÉXICO PROGRAMA DE MAESTRÍA Y DOCTORADO EN INGENIERÍA MECÁNICA MECATRÓNICA

Transcripción

1 UNIVERSIDAD NACIONAL AUTÓNOMA DE MÉXICO PROGRAMA DE MAESTRÍA Y DOCTORADO EN INGENIERÍA MECÁNICA MECATRÓNICA ANÁLISIS DE UN ROBOT PARALELO DE SEIS GRADOS DE LIBERTAD ORIENTADO A LA SIMULACIÓN DE SISMOS TESIS QUE PARA OPTAR POR EL GRADO DE: MAESTRO EN INGENIERÍA PRESENTA: CRISTIAN GIOVANNY RANGEL LARA TUTOR PRINCIPAL DR. FRANCISCO CUENCA JIMÉNEZ FACULTAD DE INGENIERÍA MÉXICO, D. F. ENERO 215

2 JURADO ASIGNADO: Presidente: Dr. González Villela Víctor Javier Secretario: Dr. Ramírez Reivich Alejandro C. Vocal: Dr. Cuenca Jiménez Francisco 1 er. Suplente: Dr. Velázquez Villegas Fernando 2 d o. Suplente: M.I. Díaz Hernández Octavio Lugar o lugares donde se realizó la tesis: FACULTAD DE INGENIERÍA TUTOR DE TESIS: DR. FRANCISCO CUENCA JIMÉNEZ FIRMA (Seg u n d a ho j a )

3 Agradecimientos A la Universidad Nacional Autónoma de México, por permitirme el tan merecido orgullo de ser parte de su gran comunidad estudiantil. A los docentes de la Facultad de Ingeniería, por sus invaluables enseñanzas e inspiraciones que me acompañan día a día. Al Dr. Francisco Cuenca Jiménez, por su orientación y apoyo incondicional para la realización de esta tesis, y sobre todo, por su gran sentido de responsabilidad y enseñanza para con sus alumnos. Así también, se extiende un agradecimiento al programa de becas de CONACYT por el apoyo económico que permitió dirigir la atención directamente a la realización de este proyecto. 3

4 Resumen En este trabajo se presenta el análisis de un robot paralelo de 6 grados libertad cuya aplicación final está orientada a la simulación y réplica de movimientos sísmicos. La primera parte del trabajo, consistió en calcular la posición, velocidad y aceleración de las variables de las articulaciones del sistema, a través de las técnicas de la Cinemática Inversa. Para ello, se propuso una trayectoria oscilatoria basada en los movimientos armónicos producidos por las ondas sísmicas. La segunda parte del trabajo, consistió en encontrar las fuerzas necesaria que deben ejercer los actuadores para mover una carga C a través de la trayectoria anteriormente propuesta, utilizando el método de Euler-Lagrange y el enfoque de la Dinámica Inversa. Finalmente, se obtuvo el modelo dinámico del robot y se realizaron simulaciones para observar el comportamiento del sistema, verificando que la estructura pudiera seguir la trayectoria propuesta sin perder de vista la evolución de las variables de interés a través del tiempo. 4

5 Contenido Capítulo 1 Generalidades Introducción Objetivos Objetivo General Objetivos Particulares Justificación La problemática La propuesta Metodología Configuración del robot Capítulo 2 Análisis Cinemático Introducción Grados de Libertad Solución Algebraica Cálculo de la Posición Solución del desplazamiento x 32i Solución del ángulo ψ 65i Solución del ángulo φ 76i Solución del ángulo φ 111i Solución del ángulo ψ 19i Solución del ángulo φ 98i Cálculo de la Velocidad Velocidad del desplazamiento x 32i Velocidad del ángulo ψ 65i

6 2.5.3 Velocidad del ángulo φ 76i Velocidad del ángulo φ 111i Velocidad del ángulo ψ 19i Velocidad del ángulo φ 98i Cálculo de la Aceleración Aceleración del desplazamiento x 32i Aceleración del ángulo ψ 65i Aceleración del ángulo φ 76i Aceleración del ángulo φ 111i Aceleración del ángulo ψ 19i Aceleración del ángulo φ 98i Capítulo 3 Análisis Dinámico Formulación Euler-Lagrange Introducción Análisis de Posición y Velocidad de Centros de Gravedad Posición y Velocidad de Centro de Gravedad del Cuerpo 1 i Posición y Velocidad de Centro de Gravedad del Cuerpo 2 i Velocidad Angular del Centro de Gravedad del Cuerpo 2 i Posición y Velocidad de Centro de Gravedad del Plato Móvil Velocidad Angular del Centro de Gravedad del Plato Móvil Posición y Velocidad de Centro de Gravedad de la Carga Velocidad Angular del Centro de Gravedad de la Carga Función Lagrangiana Desarrollo del Primer Término de la Ecuación de Lagrange Desarrollo del Segundo Término de la Ecuación de Lagrange Fuerzas Generalizadas Sustitución de los términos en la Ecuación de Lagrange Capítulo 4 Resultados

7 4.1. Gráficas de Posición Gráficas de Velocidad Gráficas de Aceleración Solución del método Euler-Lagrange Capítulo 5 Conclusiones Bibliografía Apéndice A Generación de Trayectoria Apéndice B Coeficientes de Ecuaciones de Velocidad Apéndice C Coeficientes de Ecuaciones de Aceleración Apéndice D Desarrollo de la Ecuación de Lazo Apéndice E Matriz Jacobiana de Velocidad para x 32i

8 Índice de Ilustraciones Fig. 1.1 Mesa Vibradora y Simulador de Vuelo Fig. 1.2 Manipulador Híbrido Cheope Fig. 1.3 Configuración del Robot Paralelo Fig. 2.1 Sistemas de referencia del inercial al 1i Fig. 2.2 Sistemas de referencia del 1i al 5i Fig. 2.3 Sistemas de referencia del 5i al 7i Fig. 2.4 Sistemas de referencia del 7i al 8i Fig. 2.5 Sistemas de referencia del 8i al 11i... 2 Fig. 2.6 Sistemas de referencia del 11i al 14i... 2 Fig. 2.7 Ángulos de Euler Fig. 2.8 Notación propuesta para denotar claridad en el análisis cinemática Fig. 3.1 Cuerpos del robot Fig. 3.2 Vectores de centro de gravedad Fig. 3.3 Centro de Gravedad del Cuerpo 1i Fig. 3.4 Centro de Gravedad del Cuerpo 2i Fig. 3.5 Centro de Gravedad de la Plataforma Fig. 3.6 Centro de Gravedad de la Carga Fig. 4.1 Gráfica para x 32i Fig. 4.2 Gráfica para ψ 65i Fig. 4.3 Gráfica para φ 76i Fig. 4.4 Gráfica para φ 98i Fig. 4.5 Gráfica para ψ 19i Fig. 4.6 Gráfica para φ 111i Fig. 4.7 Gráfica para x 32i... 9 Fig. 4.8 Gráfica para ψ 65i... 9 Fig. 4.9 Gráfica para φ 76i... 9 Fig. 4.1 Gráfica para φ 98i Fig Gráfica para ψ 19i Fig Gráfica para φ 111i Fig Gráfica para x 32i Fig Gráfica para ψ 65i

9 Fig Gráfica para φ 76i Fig Gráfica para φ 98i Fig Gráfica para ψ 19i Fig Gráfica para φ 111i Fig Gráfica de fuerzas de los actuadores Fig. 4.2 Gráfica de trabajo mecánico efectuado por la fuerza Fi Fig Gráfica de potencia necesaria en los actuadores Fig Gráfica de potencia necesaria en los actuadores Fig Gráfica de potencia necesaria en los actuadores Fig Gráfica de potencia necesaria en los actuadores Fig. A.1 Trayectoria propuesta

10 Nomenclatura GDL cθ sθ r b a T z1 (x) T z2 (y) T z3 (z) T z4 (θ x ) T z5 (θ y ) T z6 (θ z ) T xy R n m ω j i α j i I ij K U L M ij qq Q j Grados de Libertad Cos θ Sen θ Vector de posición del cuerpo a proyectado en la base b Matriz de transformación homogénea, para traslación en el eje x Matriz de transformación homogénea, para traslación en el eje y Matriz de transformación homogénea, para traslación en el eje z Matriz de transformación homogénea, para rotación en el eje x Matriz de transformación homogénea, para rotación en el eje y Matriz de transformación homogénea, para rotación en el eje z Matriz de transformación de la base x a la base y Matriz de transformación que proyecta de la base m a la n Velocidad angular del cuerpo i proyectada en la base j Aceleración angular del cuerpo i proyectada en la base j Matriz de inercia del cuerpo i, de la cadena j Energía cinética del sistema mecánico. Energía potencial del sistema mecánico. Función Lagrangiana Matriz de elementos de masa del cuerpo i, de la cadena j Vector de coordenadas generalizadas Vector de fuerzas generalizadas 1

11 Capítulo 1 Generalidades 1.1. Introducción En este capítulo se presentan los objetivos del proyecto de tesis, así como la justificación de los temas a desarrollar, estado del arte y metodología empleada. Los temas desarrollados en este trabajo de tesis son: Análisis Cinemático Inverso y Análisis Dinámico Inverso a partir de la formulación Euler-Lagrange Objetivos Objetivo General Obtener un modelo dinámico de una estructura cuya aplicación pueda orientarse a la simulación de sismos característicos de la Ciudad de México. Además, el modelo obtenido debe representar las generalidades del comportamiento del sistema, y que además, permita la aplicación de algún algoritmo de control, de tal manera que se pueda dar continuidad al proyecto, y con esto, obtener una aplicación real que beneficie a la sociedad Objetivos Particulares 1. Realizar el Análisis Cinemático utilizando las técnicas de la Cinemática Inversa. 2. Realizar el Análisis Dinámico, obteniendo un modelo dinámico cuya ecuación general permita implementar un algoritmo de control a futuro Justificación La problemática Si bien es cierto que el territorio nacional se encuentra sobre una zona sísmica con cinco placas tectónicas que interactúan entre sí (la placa de Norteamérica, placa de Cocos, placa del Pacífico, placa de Rivera y placa del Caribe), es preocupante que en los últimos 14 años la tasa de eventos 11

12 sísmicos por año ha ido en aumento. Según los datos del Servicio Sismológico Nacional (SNN), se indica que desde el año 2 hasta el 213 se han registrado 129 movimientos de intensidad considerable, de los cuales 51 han sido arriba de los 6 grados Richter. Los sismos con mayor intensidad registrados durante el siglo XXI han llegado inclusive a los 7.6 grados Richter. Aunque las autoridades hacen todo lo posible para minimizar los daños materiales y humanos, es necesaria una cultura de prevención que permita educar para crear consciencia, adoptando nuevas conductas para lograr una actitud responsable y de respeto por la protección de las vidas. Actualmente, la única cultura de prevención sísmica instaurada en México se basa en la planificación y ejecución de simulacros, los cuales son ensayos de actuación en caso de emergencia, siguiendo un plan previamente establecido basado en procedimientos de seguridad y protección. Sin embargo, únicamente sirven para acostumbrar a la población a adoptar rutinas de acción convenientes, sin que sea sometida a verdaderas situaciones con afectaciones sísmicas La propuesta Sabemos que hoy en día han surgido diferentes variedades de robots cuya finalidad es solventar una tarea en específico. Éste arranque se debe en gran medida a los avances tecnológicos de las últimas décadas, especialmente en el campo de la electrónica y la mecánica computacional, que han podido arrojar métodos, herramientas y dispositivos que permiten desarrollar sistemas complejos con gran precisión y poca probabilidad de falla. Actualmente existen robots de todo tipo, orientados a la medicina, industria, producción en serie, etc., los cuales permiten tener gran calidad y precisión en el producto o servicio del que se trate. Algunos de ellos inclusive se dedican realizar simulaciones de situaciones físicas, permitiendo realizar estudios a fondo bajo ciertas condiciones externas. Tal es el caso de las mesas vibratorias y los simuladores de vuelo (Fig. 1.1). El proyecto sobre el cuál se basa éste trabajo, es conocido como Cheope, un robot 2-en-1 cuya estructura principal consiste de un robot paralelo, la cual le permite realizar la mayoría de los movimientos relativamente amplios; por otra parte, la estructura secundaria es un robot serial, colocado a manera de efector final, el cual está orientado a realizar los movimientos más sutiles y precisos del sistema (Fig. 1.2). 12

13 Fig. 1.1 Mesa Vibradora y Simulador de Vuelo Fig. 1.2 Manipulador Híbrido Cheope Este trabajo presenta el análisis de un robot paralelo de seis grados de libertad cuyo objetivo radica en realizar simulaciones de sismos, que permitan a su vez fomentar la cultura de prevención permitiendo que la sociedad experimente réplicas de sismos con diferentes magnitudes Metodología 1) Análisis Cinemático a) Análisis de Posición b) Análisis de Velocidad c) Análisis de Aceleración 2) Análisis Dinámico a) Formulación Euler-Lagrange a.1) Energía Cinética a.2) Energía Potencial a.3) Función Lagrangiana a.4) Fuerzas Generalizadas b) Simulación por Software 1.5. Configuración del robot El robot paralelo sobre el cual se realizará el análisis se encuentra definido por seis cadenas cinemáticas, las cuales constan cada una de un actuador lineal y un brazo unido al efector final. El actuador lineal se desplaza a lo largo de un riel fijo instalado sobre la base del sistema, y se encuentra unido al brazo por medio de una junta esférica. El efector final consiste en una plataforma móvil cuya posición dependerá directamente de la ubicación y orientación de cada 13

14 uno de los seis brazos. Dichos brazos se encuentran unidos a la plataforma por medio de juntas esféricas (Fig. 1.3). Debido a que las seis cadenas cinemáticas son idénticas, sólo será descrita a detalle una de ellas, identificándola por medio del iterador i. Para éste análisis, se considera que todos los eslabones, así como la base y la plataforma móvil son cuerpos rígidos. Plataforma Móvil Junta Esférica Actuador Lineal Brazo i Riel Junta Esférica Fig. 1.3 Configuración del Robot Paralelo 14

15 Capítulo 2 Análisis Cinemático 2.1. Introducción La cinemática es la parte de la mecánica que estudia las leyes del movimiento de los cuerpos sin tener en cuenta los efectos extremos, fuerzas y/o torques que lo causan, limitándose, esencialmente, al estudio de la trayectoria en función del tiempo, es decir, trata la posición, velocidad y aceleración de los cuerpos. Sistemas de referencia son empleados para definir la cinemática inversa de un cuerpo. En la descripción del presente trabajo de investigación de emplean dos sistemas de referencia cartesianos, sistema de referencia fijos o marcos inerciales y sistemas de referencia relativos o marcos locales. En el presente capítulo se desarrolla el análisis de posición, velocidad y aceleración de los ángulos encontrados en las juntas del robot Grados de Libertad Los grados de libertad de un mecanismo son el número de parámetros independientes o entradas necesarias para especificar la configuración de un mecanismo completamente. Los grados de libertad de un mecanismo paralelo pueden ser determinados con la aplicación de la fórmula Chebyshev-Grübler-Kutzbach: L = 6(b g 1) + f k b = Número de cuerpos del sistema. g = Número de juntas del mecanismo. f k = Número de grados de libertad de la junta k. Por lo tanto, para la plataforma se tiene: k 15

16 b = 14 g = 18 f k = 36 k Sustituyendo los valores se tiene: L = 6( ) + 36 L = 6 Por lo tanto el sistema robótico tiene 6 grados de libertad Solución Algebraica En ésta sección se obtendrán de forma cerrada las variables actuadas y no actuadas. En una ecuación de forma cerrada se expresan las variables incógnitas como una función de las variables de entrada conocidas, en este caso particular, las variables incógnitas son las posiciones, velocidades y aceleraciones angulares en cada una de las juntas que conformas el manipulador; y las variables de entrada conocidas son las que definen la posición y orientación del efector final en el espacio. De ésta manera se tendrá un sistema de ecuaciones escalares de nxn, dentro del cual se despejarán cada una de las incógnitas Cálculo de la Posición En el problema cinemático inverso de la posición, se tiene que, dada la posición (x p, y p, z p, θ p, ψ p, φ p ) del plato móvil, hallar la posición de las variables (x 32i, ψ 65i, φ 76i, φ 98i, ψ 19i, φ 111i ) que se encuentran en las juntas del robot. Dicho análisis es esencial para el control de posición de robots paralelos. Para hacer el análisis del robot se tomaron como herramienta las matrices de transformación homogéneas, las cuales nos proporcionan desplazamiento y rotación de un cuerpo en el espacio. La matriz de transformación homogénea (Stejskal, y otros, 1996) es una matriz de 4x4 que tiene la siguiente definición: T = [ R d 1 ] (2.1) R = Matriz de Rotación (3x3). d = Vector de Desplazamiento (3x1). = Vector cero (1x3). 16

17 La matriz R de 3x3 denota la orientación de una base móvil respecto a una base de referencia, el vector d de 3x1 denota la posición del origen de la base móvil relativa a la base de referencia, el vector de 1x3 denota el vector cero. Las matrices de transformación de traslación en los ejes x, y y z respectivamente son: 1 x y 1 T z1 (x) = [ ] T 1 z2 (x) = [ ] T 1 z3 (z) = [ ] 1 z Las matrices de transformación de rotación en los ejes x, y y z respectivamente son: 1 x cθ y sθ y cθ z sθ z cθ T z4 (θ x ) = [ x sθ x 1 y ] T sθ x cθ x z5 (θ y ) = [ ] T sθ y cθ y z6 (θ z ) sθ = [ z cθ z ] 1 z Debido a la simetría entre las cadenas cinemáticas, se describirá a detalle sólo una de las seis que conforman el robot, y se podrán diferenciar con ayuda del iterador i. En las figuras 2.1, 2.2, 2.3, 2.4, 2.5 y 2.6 se muestra la secuencia en la cual se fueron generando, a partir de la base inercial (i, j, k ) las bases locales. Aplicando una matriz de transformación de giro en el eje z, y una transformación de giro en el eje y, se forman respectivamente las bases 1i, 2i, es decir: T 2i = T z6 (γ 1i )T z5 (β 2i ) Para encontrar las bases 3i y 4i, es necesario aplicar una transformación de traslación a lo largo del eje x, la cual representa el movimiento del actuador lineal; posteriormente, basta con realizar una transformación de traslación a través del eje y para seleccionar la cadena cinemática por la que llegaremos al efector final, esto es: T 24i = T z1 (x 32i )T z2 (b 43i ) De la misma forma, para obtener la base 5i, basta con emplear una transformación a lo largo del eje z. Para las bases 6i y 7i, encontradas en la primera junta esférica, es necesario usar dos transformaciones de rotación continuas, la primera utilizando el eje y y la segunda usando al eje z. Se tiene entonces: T 47i = T z3 (c 54i )T z5 (ψ 65i )T z6 (φ 76i ) 17

18 Fig. 2.1 Sistemas de referencia del inercial al 1i Fig. 2.2 Sistemas de referencia del 1i al 5i 18

19 Fig. 2.3 Sistemas de referencia del 5i al 7i En la figura 2.4 y 2.5, se muestra como fueron generadas las bases 8i, 9i 1i y 11i, las últimas tres localizadas en la segunda junta esférica: T 711i = T z1 (a 87i )T z6 (φ 98i )T z5 (ψ 19i )T z6 (φ 111i ) Fig. 2.4 Sistemas de referencia del 7i al 8i 19

20 Fig. 2.5 Sistemas de referencia del 8i al 11i Para las bases 12i, 13i y 14i, las cuales se encuentran en el plato móvil (fig. 2.6), se tiene: T 1114i = T z6 (γ 1211i )T z1 (a 1312i )T z6 (γ 1413i ) Fig. 2.6 Sistemas de referencia del 11i al 14i Por último, para generar una ecuación de lazo se requiere generar las bases que nos permitan llegar de la base inercial a la base 14i sin pasar por los brazos del robot, esto es, tomando como datos la posición y orientación deseados del efector final. En la figura 2.7 se muestran las bases generadas para la orientación del plato, donde las transformaciones requeridas son las siguientes: T p = T z1 (x p )T z2 (y p )T z3 (z p )T z4 (θ p )T z5 (ψ p )T z6 (φ p ) 2

21 Fig. 2.7 Ángulos de Euler En la figura 2.7 no se muestran las traslaciones x, y y z ya que son paralelas a la inercial, por lo que únicamente se muestran las bases para la orientación de la plataforma. La notación propuesta se expone en las siguientes tablas (Fig. 2.8): Fig. 2.8 Notación propuesta para denotar claridad en el análisis cinemática 21

22 De ésta manera, podemos observar que las incógnitas a determinar son tanto las posiciones de los actuadores lineales, como los ángulos de las juntas esféricas, las cuales en conjunto nos permiten orientar la plataforma en el espacio, siendo estas: (x 32i, ψ 65i, φ 76i, φ 98i, ψ 19i, φ 111i ) donde: i = 1,2,3,4,5,6. A partir de éste resultado, se tienen en total 36 variables por calcular, de las cuales 6 son actuadas tal y como se determinó gracias a los grados de libertad. Se establece además, a las variables x 32i como las variables actuadas del mecanismo, ya que la base en la que se encuentran definidas está adherida directamente al actuador lineal Solución del desplazamiento x 32i Para la obtención de dicho desplazamiento se requiere obtener una ecuación escalar que se encuentre sólo en función de las longitudes y ángulos conocidos (propios de la geometría del mecanismo). Por esta razón, se optó por obtener el vector de posición de la segunda junta esférica, ya que éste nos ayudaría a encontrar una ecuación que nos permitiría determinar el valor del desplazamiento del actuador. Se plantearon dos recorridos: el primero a través del actuador y del brazo i, y el segundo a través de la plataforma móvil. A continuación, se describirá el proceso para obtener dicho vector. Partiendo de la ecuación generada a través de las matrices de transformación homogéneas en la que se llegó a la plataforma del robot, tenemos: T 2i T 24i T 47i T 711i T 1114i = T p (2.2) Si deseamos obtener una ecuación similar, pero esta vez llegando a la posición de la segunda junta 1 esférica, basta con postmultiplicar T 1114i en ambos miembros de la ecuación 2.2: 1 T 2i T 24i T 47i T 711i = T p T 1114i (2.3) Por otra parte, sabemos que: T 711i = T z1 (a 87i )T z6 (φ 98i )T z5 (ψ 19i )T z6 (φ 111i ) (2.4) 1 T 1114i = (T z6 (γ 1211i )T z1 (a 1312i )T z6 (γ 1413i )) 1 = T 1 z6 (γ 1413i )T 1 z1 (a 1312i )T 1 z6 (γ 1211i ) 22

23 Tras sustituir las ecuaciones 2.4 en 2.3, tenemos: T 2i T 24i T 47i (T z1 (a 87i )T z6 (φ 98i )T z5 (ψ 19i )T z6 (φ 111i )) = T p (T 1 z6 (γ 1413i )T 1 z1 (a 1312i )T 1 z6 (γ 1211i )) (2.5) Puesto que únicamente nos interesa extraer el vector de posición de la segunda junta esférica, al recorrer la cadena cinemática a través del actuador, podemos observar que es posible prescindir de las transformaciones T z6 (φ 98i ), T z5 (ψ 19i ) y T z6 (φ 111i ), ya que únicamente orientan los marcos de referencia de la junta, sin modificar su posición final. Aplicando la misma lógica, al recorrer la cadena cinemática a través de la plataforma, nos damos cuenta que también es posible omitir la transformación T 1 z6 (γ 1211i ), puesto que ya nos encontramos en la posición de la junta esférica. Aplicando dichos cambios a la ecuación 2.5: T 2i T 24i T 47i T z1 (a 87i ) = T p T 1 z6 (γ 1413i )T 1 z1 (a 1312i ) Despejando las incógnitas de la primera junta esférica: T 47i T z1 (a 87i ) = (T 2i T 24i ) 1 T p T 1 z6 (γ 1413i )T 1 z1 (a 1312i ) Desarrollando T 47i y reacomodando la expresión: T z3 (c 54i )T z5 (ψ 65i )T z6 (φ 76i )T z1 (a 87i ) = T 1 24i T 1 2i T p T 1 z6 (γ 1413i )T 1 z1 (a 1312i ) T z5 (ψ 65i )T z6 (φ 76i )T z1 (a 87i ) = (T z3 (c 54i )) 1 T 1 24i T 1 2i T p T 1 z6 (γ 1413i )T 1 z1 (a 1312i ) (2.6) Se define la siguiente matriz homogénea con elementos constantes: Sustituyendo la expresión 2.7 en 2.6: A B C D T cte = T 1 2i T p T 1 z6 (γ 1413i )T 1 E F G H z1 (a 1312i ) = [ ] (2.7) I J K L 1 T z5 (ψ 65i )T z6 (φ 76i )T z1 (a 87i ) = (T z3 (c 54i )) 1 1 T 24i T cte (2.8) Se sabe que la cuarta columna de toda matriz de transformación homogénea contiene la información relacionada con la posición, sin tomar en cuenta la orientación del sistema de referencia. Para extraer la información de la posición de la segunda junta esférica, basta con expresar la ecuación 2.8 de la siguiente manera: 23

24 T z5 (ψ 65i )T z6 (φ 76i )T z1 (a 87i )n = (T z3 (c 54i )) 1 1 T 24i T cte n (2.9) n = [ 1] T Luego entonces, tenemos: D x 32i H b [ 43i ] = [ c 54i + L 1 a 87i cφ 76i cψ 65i a 87i sφ 76i a 87i cφ 76i sψ 65i 1 Es posible extraer los vectores de posición aplicando la siguiente transformación: ] (2.1) T 3D (u) = u (2.11) Dónde: u = [ u x u y u z 1 u x ] u = [ u y ] u z Aplicando la transformación 2.11 a la ecuación 2.1, obtenemos lo siguientes vectores: D x 32i a 87i cφ 76i cψ 65i p 1 = [ H b 43i ] c 54i + L p 2 = [ a 87i sφ 76i ] a 87i cφ 76i sψ 65i Igualando los módulos de los vectores p 1 y p 2 obtenemos: p 1 T p 1 = p 2 T p 2 p 1 T p 1 = p 2 T p 2 Sustituyendo los valores de p 1 y p 2, y desarrollando: Renombrando, tenemos: a 87i = D 2 + ( c 54i + L) 2 + (H b 43i ) 2 2 2Dx 32i + x 32i 2 2 A 1i x 32i + B 1i x 32i + D 1i = a 87i (2.12) A 1i = 1 B 1i = 2D D 1i = D 2 + ( c 54i + L) 2 + (H b 43i ) 2 24

25 Finalmente, hemos obtenido una expresión cuadrática cuya única variable es el desplazamiento del actuador lineal, es decir: 2 A 1i x 32i + B 1i x 32i + (D 1i a 2 87i ) = (2.13) A 1i = 1 B 1i = 2(sβ 21i (z p + a 1312i s (γ 1413i φ p )sθ p + a 1312i c (γ 1413i φ p )cθ p sψ p ) cβ 21i (cγ 1i (x p a 1312i c (γ 1413i φ p )cψ p ) + sγ 1i (y p + a 1312i s (γ 1413i φ p )cθ p a 1312i c (γ 1413i φ p )sθ p sψ p ))) D 1i = ( c 54i + cγ 1i (x p a 1312i c (γ 1413i φ p )cψ p )sβ 21i + (y p + a 1312i s (γ 1413i φ p )cθ p )sβ 21i sγ 1i + cβ 21i (z p + a 1312i s (γ 1413i φ p )sθ p ) + a 1312i c (γ 1413i φ p )(cβ 21i cθ p sβ 21i sγ 1i sθ p )sψ p ) 2 + ( b 43i + ( x p + a 1312i c (γ 1413i φ p )cψ p )sγ 1i + cγ 1i (y p + a 1312i s (γ 1413i φ p )cθ p a 1312i c (γ 1413i φ p )sθ p sψ p )) 2 + (sβ 21i (z p + a 1312i s (γ 1413i φ p )sθ p + a 1312i c (γ 1413i φ p )cθ p sψ p ) cβ 21i (cγ 1i (x p a 1312i c (γ 1413i φ p )cψ p ) + sγ 1i (y p + a 1312i s (γ 1413i φ p )cθ p a 1312i c (γ 1413i φ p )sθ p sψ p ))) 2 (2.14) La solución de la ecuación 2.13 se muestra a continuación: Solución del ángulo ψ 65i x 32i = B 2 1i + 4 A 1i a 87i + B 2 1i 4 A 1i D 1i (2.15) 2 A 1i Para la obtención de ésta variable, requiere obtener nuevamente una ecuación escalar que se encuentre sólo en función de parámetros y variables conocidas. Para lograr éste objetivo, es posible utilizar de nueva cuenta las transformaciones definidas en el sistema. Partimos de la siguiente ecuación matricial para llegar a la segunda junta esférica: T 47i T z1 (a 87i ) = (T 2i T 24i ) 1 T p (T z1 (a 1312i )T z6 (γ 1413i )) 1 (2.16) En ésta ecuación encontramos las siguientes transformaciones conocidas: T 2i, T 24i, T z1 (a 1312i ), T z6 (γ 1413i ) Ya que además de contener distancias y ángulos constantes, contienen el desplazamiento variable x 32i calculado en el apartado anterior. Por otra parte, el miembro izquierdo de la ecuación 2.16 contiene la información de los ángulos de la junta universal, por lo que es posible utilizarla para despejar las variables. Dicho esto, se procede a obtener la información de la posición de la junta universal de la siguiente manera: T 47i T z1 (a 87i )n = (T 2i T 24i ) 1 T p (T z1 (a 1312i )T z6 (γ 1413i )) 1 n (2.17) 25

26 Siendo que el lado derecho de la ecuación 2.17 es conocido, se puede renombrar de la siguiente forma: T 47i T z1 (a 87i )n = T total X i Y i T total = [ ] Z i 1 Tras realizar las operaciones del miembro izquierdo, se obtiene el siguiente resultado: [ a 87i cφ 76i cψ 65i a 87i sφ 76i c 54i a 87i cφ 76i sψ 65i 1 ] = [ X i Y i Z i 1 ] (2.18) Para resolver el ángulo ψ 65i, se toman las componentes 1 y 3 de cada lado de la ecuación Posteriormente, se despejan sin(ψ 65i ) y cos(ψ 65i ), y empleando la función tangente se obtiene: tan ψ 65i = sψ 65i cψ 65i = c 54i Z i X i (2.19) Despejando ψ 65i : ψ 65i = arctan ( c 54i Z i X i ) (2.2) X i = x 32i sβ 21i (z p + a 1312i s(γ 1413i φ p ) sθ p + a 1312i c(γ 1413i φ p ) cθ p sψ p ) + cβ 21i (cγ 1i (x p a 1312i c(γ 1413i φ p ) cψ p ) + sγ 1i (y p + a 1312i s(γ 1413i φ p ) cθ p a 1312i c(γ 1413i φ p ) sθ p sψ p )) (2.21) Z i = sβ 21i (cγ 1i (x p a 1312i c (γ 1413i φ p )cψ p ) + (y p + a 1312i s (γ 1413i φ p )cθ p )sγ 1i ) + cβ 21i (z p + a 1312i s (γ 1413i φ p )sθ p ) + a 1312i c (γ 1413i φ p )(cβ 21i cθ p sβ 21i sγ 1i sθ p )sψ p 26

27 2.4.3 Solución del ángulo φ 76i Para el cálculo de ésta variable, se partirá de la ec. 2.18, donde en este caso, la única incógnita es φ 76i, ya que ψ 65i se calculó en la sección anterior. Para resolver el ángulo φ 76i se toman las componentes 1 y 2 de cada lado de la ecuación. Posteriormente, se despejan sin(φ 76i ) y cos(φ 76i ), y empleando la función tangente se obtiene: tan φ 76i = sφ 76i cφ 76i = Y i cψ 65i X i (2.22) Despejando φ 76i : φ 76i = arctan ( Y i cψ 65i X i ) (2.23) Donde X i se encuentra en las ecuaciones 2.21 y: Y i = b 43i + ( x p + a 1312i c (γ 1413i φ p )cψ p )sγ 1i + cγ 1i (y p + a 1312i s (γ 1413i φ p )cθ p a 1312i c (γ 1413i φ p )sθ p sψ p ) (2.24) Solución del ángulo φ 111i Los ángulos φ 98i, ψ 19i y φ 111i conforman las juntas esféricas del sistema. Dichos ángulos se obtienen mediante una formulación de lazos matriciales, es decir, por medio de giros y desplazamientos definidos por transformaciones homogéneas, se generan lazos que partan del sistema de referencia inercial o absoluto y terminen en un sistema relativo deseado. Para el cálculo de φ 111i se partirá de la ec. matricial 2.2: T 2i T 24i T 47i T 711i T 1114i = T p (2.2) Donde las transformaciones T 2i, T 24i, T 47i y T 1114i son conocidas, ya que además de contener distancias y ángulos constantes, también contiene el desplazamiento x 32i y los ángulos ψ 65i y φ 76i, los cuales se calcularon en las secciones anteriores y son todos ellos variables. Por lo tanto, para poder calcular los ángulos de la junta esférica, despejamos, dejando de lado izquierdo la transformación T 711i, la cual contiene los ángulos φ 98i, ψ 19i y φ 111i T 711i = T 47i T 24i T 2i T p T 1114i (2.25) 27

28 Renombrando el lado derecho de la ecuación 2.25, se tiene: T 711i = T ai (2.26) cφ 98i cφ 111i cψ 19i sφ 98i sφ 111i cφ 111i sφ 98i cφ 98i cψ 19i sφ 111i cφ 98i sψ 19i a 87i cφ T 711i = [ 111i cψ 19i sφ 98i + cφ 98i sφ 111i cφ 98i cφ 111i cψ 19i sφ 98i sφ 111i sφ 98i sψ 19i ] cφ 111i sψ 19i sφ 111i sψ 19i cψ 19i 1 a 11i a 12i a 13i a 14i a T ai = [ 21i a 31i a 22i a 32i a 23i a 33i a 24i ] a 34i 1 Para resolver el ángulo φ 111i se toman las componentes (3,1) y (3,2) de cada lado de la ecuación Posteriormente, se despejan sin(φ 111i ) y cos(φ 111i ), y empleando la función tangente se obtiene: Despejando φ 111i : tan φ 111i = sφ 111i cφ 111i = a 32i a 31i (2.27) φ 111i = arctan ( a 32i a 31i ) (2.28) a 31i = cφ p (c(γ 1211i + γ 1413i ) s(β 21i + ψ 65i ) cγ 1i cψ p s(γ 1211i + γ 1413i ) (s(β 21i + ψ 65i ) cθ p sγ 1i + c(β 21i + ψ 65i ) sθ p )) + s(γ 1211i + γ 1413i ) s(β 21i + ψ 65i ) cγ 1i cψ p sφ p + c(γ 1211i + γ 1413i ) (s(β 21i + ψ 65i ) cθ p sγ 1i + c(β 21i + ψ 65i ) sθ p )sφ p + c(γ 1211i + γ 1413i φ p ) ( c(β 21i + ψ 65i ) cθ p + s(β 21i + ψ 65i ) sγ 1i sθ p )sψ p a 32i = c (γ 1211i + γ 1413i )(c (β 21i + ψ 65i )cφ p sθ p + s (β 21i + ψ 65i )(cθ p cφ p sγ 1i cγ 1i cψ p sφ p )) + s (γ 1211i + γ 1413i )(c (β 21i + ψ 65i )sθ p sφ p + s (β 21i + ψ 65i )(cγ 1i cφ p cψ p + cθ p sγ 1i sφ p )) + s (γ 1211i + γ 1413i φ p )( c (β 21i + ψ 65i )cθ p + s (β 21i + ψ 65i )sγ 1i sθ p )sψ p (2.29) Solución del ángulo ψ 19i Para el cálculo de ésta variable, se partirá de la ec. 2.26, donde en éste caso, la única incógnita es ψ 19i, ya que φ 111i se calculó en la sección anterior. Para resolver el ángulo ψ 19i se toman las componentes (3,2) y (3,3) de cada lado de la ecuación. Posteriormente, se despejan sin(ψ 19i ) y cos(ψ 19i ), y empleando la función tangente se obtiene: tan ψ 19i = sψ 19i cψ 19i = a 32i csc φ 111i a 33i (2.3) 28

29 Despejando ψ 19i : ψ 19i = arctan ( a 32i csc φ 111i a 33i ) (2.31) Donde a 32i se encuentra en las ecuaciones 2.29 y: a 33i = c (β 21i + ψ 65i )cθ p cψ p + s (β 21i + ψ 65i )( cψ p sγ 1i sθ p + cγ 1i sψ p ) (2.32) Solución del ángulo φ 98i Para resolver el ángulo φ 98i, se trabajará con la ec. 2.26, donde en éste caso, la única incógnita es φ 98i, ya que φ 111i y ψ 19i se calcularon en las últimas dos secciones anteriores. Para resolver el ángulo φ 98i se toman las componentes (1,1) y (1,2) de cada lado de la ecuación. Posteriormente, se despejan sin(φ 98i ) y cos(φ 98i ), y empleando la función tangente se obtiene: Despejando φ 98i : tan φ 98i = sφ 98i cφ 98i = cψ 19i( a 12i cφ 111i a 11i sφ 111i ) a 11i cφ 111i a 12i sφ 111i (2.33) φ 98i = arctan ( cψ 19i( a 12i cφ 111i a 11i sφ 111i ) a 11i cφ 111i a 12i sφ 111i ) (2.34) a 11i = c(β 21i + ψ 65i ) s(γ 1211i + γ 1413i φ p ) cθ p cφ 76i sγ 1i + s(γ 1211i + γ 1413i ) s(β 21i + ψ 65i ) cφ 76i cφ p sθ p s (γ 1211i + γ 1413i )cψ p sγ 1i sφ 76i sφ p c (γ 1211i + γ 1413i )(cφ p cψ p sγ 1i sφ 76i + s (β 21i + ψ 65i )cφ 76i sθ p sφ p ) + c (γ 1211i + γ 1413i φ p )cφ 76i (s (β 21i + ψ 65i )cθ p + c (β 21i + ψ 65i )sγ 1i sθ p )sψ p + cγ 1i ( s (γ 1211i + γ 1413i φ p )cθ p sφ 76i + c (γ 1211i + γ 1413i φ p )(c (β 21i + ψ 65i )cφ 76i cψ p + sθ p sφ 76i sψ p )) (2.35) a 12i = s (γ 1211i + γ 1413i )(cφ p cψ p (c (β 21i + ψ 65i )cγ 1i cφ 76i sγ 1i sφ 76i ) + s (β 21i + ψ 65i )cφ 76i ( sθ p sφ p + cθ p cφ p sψ p ) + (c (β 21i + ψ 65i )cφ 76i sγ 1i + cγ 1i sφ 76i )(cθ p sφ p + cφ p sθ p sψ p )) + c (γ 1211i + γ 1413i )(cψ p ( c (β 21i + ψ 65i )cγ 1i cφ 76i + sγ 1i sφ 76i )sφ p s (β 21i + ψ 65i )cφ 76i (cφ p sθ p + cθ p sφ p sψ p ) + (c (β 21i + ψ 65i )cφ 76i sγ 1i + cγ 1i sφ 76i )(cθ p cφ p sθ p sφ p sψ p )) (2.36) 2.5. Cálculo de la Velocidad En el problema cinemático inverso de la velocidad, se tiene que, dada la velocidad (x p, y p, z p, θ p, ψ p, φ p) del plato móvil, hallar la velocidad de las variables (x 32i, ψ 65i, φ 76i, φ 98i, ψ 19i, φ 111i) que se encuentran en las juntas del robot. 29

30 La velocidad de un punto o un cuerpo rígido que experimenta movimiento, puede ser obtenida por la derivada respecto al tiempo de su función de posición. Se asume en ésta sección que la posición y orientación de los cuerpos son totalmente conocidas, ya que son resultado del análisis de la posición. Por lo tanto, con base en las ecuaciones obtenidas en la sección anterior, se obtendrá la velocidad al derivar con respecto al tiempo para cada una de ellas Velocidad del desplazamiento x 32i Tomando la ecuación 2.13 y derivando con respecto al tiempo, obtenemos: A 1ix 32i 2 A 1i x 32i + B 1i x 32i + (D 1i a 2 87i ) = (2.13) 2 + B 1ix 32i + D 1i + B 1i x 32i + 2A 1i x 32i x 32i = (2.37) Despejando x 32i y simplificando, se tiene: x 32i = A 1ix 2 32i B 1ix 32i D 1i (2.38) B 1i + 2A 1i x 32i Donde A 1i, B 1i y D 1i se encuentran en el análisis de posición, y además: A 1i = B 1i = 2(sβ 21i (z. p a 1312i c (γ 1413i φ p )sθ p (φ. p + θ. psψ p ) + a 1312i cθ p (ψ. pc (γ 1413i φ p )cψ p + s (γ 1413i φ p )(θ. p + φ. psψ p ))) + cβ 21i ( cγ 1i (x. p a 1312i φ. ps (γ 1413i φ p )cψ p + a 1312i ψ. pc (γ 1413i φ p )sψ p ) + sγ 1i ( y. p + a 1312i c (γ 1413i φ p )cθ p (φ. p + θ. psψ p ) + a 1312i sθ p (ψ. pc (γ 1413i φ p )cψ p + s (γ 1413i φ p )(θ. p + φ. psψ p ))))) (2.39) D 1i = 2(( c 54i + cγ 1i (x p a 1312i c (γ 1413i φ p )cψ p )sβ 21i + (y p + a 1312i s (γ 1413i φ p )cθ p )sβ 21i sγ 1i + cβ 21i (z p + a 1312i s (γ 1413i φ p )sθ p ) + a 1312i c (γ 1413i φ p )(cβ 21i cθ p sβ 21i sγ 1i sθ p )sψ p )(sβ 21i (x. pcγ 1i + y. psγ 1i ) + cβ 21i (z. p a 1312i c (γ 1413i φ p )sθ p (φ. p + θ. psψ p ) + a 1312i cθ p (ψ. pc (γ 1413i φ p )cψ p + s (γ 1413i φ p )(θ. p + φ. psψ p ))) a 1312i sβ 21i (c (γ 1413i φ p )cθ p sγ 1i (φ. p + θ. psψ p ) + ψ. pc (γ 1413i φ p )(cψ p sγ 1i sθ p cγ 1i sψ p ) + s (γ 1413i φ p )(φ. pcγ 1i cψ p + sγ 1i sθ p (θ. p + φ. psψ p )))) + ( b 43i + ( x p + a 1312i c (γ 1413i φ p )cψ p )sγ 1i + cγ 1i (y p + a 1312i s (γ 1413i φ p )cθ p a 1312i c (γ 1413i φ p )sθ p sψ p ))( sγ 1i (x. p a 1312i φ. ps (γ 1413i φ p )cψ p + a 1312i ψ. pc (γ 1413i φ p )sψ p ) + cγ 1i (y. p a 1312i (c (γ 1413i φ p )cθ p (φ. p + θ. psψ p ) + sθ p (ψ. pc (γ 1413i φ p )cψ p + s (γ 1413i φ p )(θ. p + φ. psψ p ))))) + (sβ 21i (z p + a 1312i s (γ 1413i φ p )sθ p + a 1312i c (γ 1413i φ p )cθ p sψ p ) cβ 21i (cγ 1i (x p a 1312i c (γ 1413i φ p )cψ p ) + sγ 1i (y p + a 1312i s (γ 1413i φ p )cθ p a 1312i c (γ 1413i φ p )sθ p sψ p )))(sβ 21i (z. p a 1312i c (γ 1413i φ p )sθ p (φ. p + θ. psψ p ) (2.4) + a 1312i cθ p (ψ. pc (γ 1413i φ p )cψ p + s (γ 1413i φ p )(θ. p + φ. psψ p ))) + cβ 21i ( cγ 1i (x. p a 1312i φ. ps (γ 1413i φ p )cψ p + a 1312i ψ. pc (γ 1413i φ p )sψ p ) + sγ 1i ( y. p + a 1312i c (γ 1413i φ p )cθ p (φ. p + θ. psψ p ) + a 1312i sθ p (ψ. pc (γ 1413i φ p )cψ p + s (γ 1413i φ p )(θ. p + φ. psψ p )))))) 3

31 Sustituyendo las ecuaciones 2.14, 2.39 y 2.4 en la ecuación 2.38, para después agrupar en x p, y p, z p, θ p, ψ p, φ p, se tiene: x 32i = 1 (V V 2i x p + V 3i y p + V 4i z p + V 5i θ p + V 6i ψ p + V 7i φ p) 1i Donde los coeficientes son: (2.41) V 1i = B 1i + 2A 1i + x 32i V 2i = 2( x p + x 32i cβ 21i cγ 1i + a 1312i c(γ 1413i φ p ) cψ p + c 54i cγ 1i sβ 21i b 43i sγ 1i ) V 31 = 2( y p + b 43i cγ 1i a 1312i s(γ 1413i φ p ) cθ p + x 32i cβ 21i sγ 1i + c 54i sβ 21i sγ 1i + a 1312i c(γ 1413i φ p ) sθ p sψ p ) V 4i = 2(z p c 54i cβ 21i + x 32i sβ 21i + a 1312i s(γ 1413i φ p ) sθ p + a 1312i c(γ 1413i φ p ) cθ p sψ p ) V 5i = 2a 1312i s(γ 1413i φ p ) (cθ p (z p c 54i cβ 21i + x 32i sβ 21i ) + ( y p + b 43i cγ 1i + x 32i cβ 21i sγ 1i + c 54i sβ 21i sγ 1i )sθ p ) + 2a 1312i c(γ 1413i φ p ) (cθ p (y p b 43i cγ 1i (x 32i cβ 21i + c 54i sβ 21i )sγ 1i ) + (z p c 54i cβ 21i + x 32i sβ 21i )sθ p )sψ p V 6i = 2a 1312i c (γ 1413i φ p )(cθ p cψ p (z p c 54i cβ 21i + x 32i sβ 21i ) + cψ p ( y p + b 43i cγ 1i + x 32i cβ 21i sγ 1i + c 54i sβ 21i sγ 1i )sθ p + (x p cγ 1i (x 32i cβ 21i + c 54i sβ 21i ) + b 43i sγ 1i )sψ p ) V 7i = 2a 1312i (cφ p cψ p (x p cγ 1i (x 32i cβ 21i + c 54i sβ 21i ) + b 43i sγ 1i )sγ 1413i + sγ 1413i (cθ p (y p b 43i cγ 1i (x 32i cβ 21i + c 54i sβ 21i )sγ 1i ) + (z p c 54i cβ 21i + x 32i sβ 21i )sθ p )sφ p + cγ 1413i (cθ p cφ p (y p b 43i cγ 1i (x 32i cβ 21i + c 54i sβ 21i )sγ 1i ) + cφ p (z p c 54i cβ 21i + x 32i sβ 21i )sθ p + cψ p ( x p + x 32i cβ 21i cγ 1i + c 54i cγ 1i sβ 21i b 43i sγ 1i )sφ p ) s (γ 1413i φ p )(cθ p (z p c 54i cβ 21i + x 32i sβ 21i ) + ( y p + b 43i cγ 1i + x 32i cβ 21i sγ 1i + c 54i sβ 21i sγ 1i )sθ p )sψ p ) (2.42) Velocidad del ángulo ψ 65i De la ecuación 2.19 se tiene: tan (ψ 65i ) = sψ 65i cψ 65i = c 54i Z i X i (2.19) Derivando ésta última expresión: ψ. 65isec 2 (ψ 65i ) = Z. i + X. i(c 54i Z i ) X 2 i X i Despejando ψ 65i de la ecuación 2.43: ψ. 65i = (c 54iX. i + Z. ix i X. iz i )cos 2 (ψ 65i ) X i 2 (2.43) (2.44) Derivando las ecuaciones 2.21 para obtener (X i, Z i), sustituirlas en 2.44, simplificando y agrupando, tenemos: 31

32 ψ 65i = 1 V 8i (V 9i x p + V 1i y p + V 11i z p + V 12i θ p + V 13i ψ p + V 14i φ p + V 15i x 32i) (2.45) Los términos de la ecuación anterior se muestran en el apéndice B. Finalmente, sustituyendo la ec en 2.45 y agrupando: ψ 65i = 1 V 8i (E 1i x p + E 2i y p + E 3i z p + E 4i θ p + E 5i ψ p + E 6i φ p) (2.46) E 1i = V 9i + V 15iV 2i V 1i E 2i = V 1i + V 15iV 3i V 1i E 3i = V 11i + V 15iV 4i V 1i E 4i = V 12i + V 15iV 5i V 1i E 5i = V 13i + V 15iV 6i V 1i E 6i = V 14i + V 15iV 7i V 1i Velocidad del ángulo φ 76i De la ecuación 2.22 se tiene: tan φ 76i = sφ 76i cφ 76i = Y i cψ 65i X i (2.22) Derivando ésta última expresión: φ. 76isec 2 (φ 76i ) = Y. icψ 65i X i ψ. 65iY i sψ 65i X i X. iy i cψ 65i 2 X i (2.47) Despejando φ 76i de la ecuación 2.46: φ. 76i = cos2 (φ 76i ) ((Y. ix i X. iy i )cψ 65i ψ. 65iX i Y i sψ 65i ) X i 2 (2.48) Derivando las ecuaciones 2.21 y 2.24 para obtener (X i, Y i), sustituirlas en 2.47, simplificando y agrupando, tenemos: φ 76i = 1 V 16i (V 17i x p + V 18i y p + V 19i z p + V 2i θ p + V 21i ψ p + V 22i φ p + V 23i x 32i + V 24i ψ 65i) (2.49) 32

33 Los términos de la ecuación anterior se muestran en el apéndice B. Finalmente, sustituyendo la ec y 2.46 en 2.49 y agrupando: φ 76i = 1 V 16i (E 7i x p + E 8i y p + E 9i z p + E 1i θ p + E 11i ψ p + E 12i φ p) (2.5) E 7i = V 17i + V 23iV 2i V 1i E 8i = V 18i + V 23iV 3i V 1i E 9i = V 19i + V 23iV 4i V 1i + E 1iV 24i V 8i + E 2iV 24i V 8i + E 3iV 24i V 8i E 1i = V 2i + V 23iV 5i V 1i E 11i = V 21i + V 23iV 6i V 1i E 12i = V 22i + V 23iV 7i V 1i + E 4iV 24i V 8i + E 5iV 24i V 8i + E 6iV 24i V 8i Velocidad del ángulo φ 111i De la ecuación 2.27 se tiene: Derivando ésta última expresión: tan φ 111i = sφ 111i cφ 111i = a 32i a 31i (2.27) a. 32i + a 31i φ. 111isec 2 (φ 111i ) = a 32ia. 31i a 31i (2.51) Despejando φ 111i de la ecuación 2.51: φ. 111i = (a 32ia. 31i a 31i a. 32i)cos 2 (φ 111i ) a 31i 2 (2.52) Derivando las ecuaciones 2.29 para obtener (a 31i, a 32i), sustituirlas en 2.52, simplificando y agrupando, tenemos: φ 111i = 1 (V V 26i x p + V 27i y p + V 28i z p + V 29i θ p + V 3i ψ 25i + V 34i φ 76i) p + V 31i φ p + V 32i x 32i + V 33i ψ 65i (2.53) Los términos de la ecuación anterior se muestran en el apéndice C. Finalmente, sustituyendo la ec. 2.41, 2.46 y 2.5 en 2.53 y agrupando: φ 111i = 1 V 25i (E 13i x p + E 14i y p + E 15i z p + E 16i θ p + E 17i ψ p + E 18i φ p) (2.54) 33

34 E 13i = E 1iV 32i V 8i E 14i = E 2iV 32i V 8i E 15i = E 3iV 32i V 8i E 16i = V 29i + E 4iV 32i V 8i E 17i = V 3i + E 5iV 32i V 8i E 18i = V 31i + E 6iV 32i V 8i Velocidad del ángulo ψ 19i De la ecuación 2.3 se tiene: tan ψ 19i = sψ 19i cψ 19i = a 32i csc φ 111i a 33i (2.3) Derivando ésta última expresión: ψ. 19isec 2 (ψ 19i ) = a. 32icsc (φ 111i ) a 33i a. 32ia 33icsc (φ 111i ) a 33i 2 a. 32iφ111icot (φ 111i )csc(φ 111i ) (2.55) a 33i Despejando ψ 19i de la ecuación 2.55: ψ. 19i = cos2 (ψ 19i )(a 33i a. 32i a 32i a. 33i a 32i a 33i φ. 111i cot(φ 111i )) csc(φ 111i ) 2 a 33i (2.56) Derivando las ecuaciones 2.29 y 2.32 para obtener (a 32i, a 33i), sustituirlas en 2.56, simplificando y agrupando, tenemos: ψ 19i = 1 (V V 36i x p + V 37i y p + V 38i z p + V 39i θ 35i + V 45i φ 111i) p + V 4i ψ p + V 41i φ p + V 42i x 32i + V 43i ψ 65i + V 44i φ 76i (2.57) Los términos de la ecuación anterior se muestran en el apéndice C. Finalmente, sustituyendo la ec. 2.41, 2.46, 2.5 y 2.54 en 2.57 y agrupando: ψ 19i = 1 V 35i (E 19i x p + E 2i y p + E 21i z p + E 22i θ p + E 23i ψ p + E 24i φ p) (2.58) 34

35 E 7i = E 13iV 45i V 25i E 8i = E 14iV 45i V 25i E 9i = E 15iV 45i V 25i + E 1iV 43i V 8i + E 2iV 43i V 8i + E 3iV 43i V 8i E 1i = V 39i + E 16iV 45i V 25i E 11i = V 4i + E 17iV 45i V 25i E 12i = V 41i + E 18iV 45i V 25i + E 4iV 43i V 8i + E 5iV 43i V 8i + E 6iV 43i V 8i Velocidad del ángulo φ 98i De la ecuación 2.33 se tiene: tan φ 98i = sφ 98i cφ 98i = cψ 19i( a 12i cφ 111i a 11i sφ 111i ) a 11i cφ 111i a 12i sφ 111i (2.33) Derivando ésta última expresión: φ. 98isec 2 (φ 98i ) = a 12ia. 11icos 2 (φ 111i )cψ 19i (a 11i cφ 111i a 12i sφ 111i ) 2 a 12i 2 φ. 111icos 2 (φ 111i )cψ 19i (a 11i cφ 111i a 12i sφ 111i ) 2 a 11ia. 12isin 2 (φ 111i )cψ 19i (a 11i cφ 111i a 12i sφ 111i ) 2 a 11i 2 φ. 111isin 2 (φ 111i )cψ 19i (a 11i cφ 111i a 12i sφ 111i ) 2 + a 11ia. 11icφ 111i cψ 19i sφ 111i (a 11i cφ 111i a 12i sφ 111i ) 2 a 12ia12icφ 111i cψ 19i sφ 111i (a 11i cφ 111i a 12i sφ 111i ) 2. (2.59) 2a 11ia 12i φ. 111icφ 111i cψ 19i sφ 111i (a 11i cφ 111i a 12i sφ 111i ) 2 Despejando φ 98i de la ecuación 2.59: φ. a 12i a. 11icos 2 (φ 98i )cψ 19i 98i = (a 11i cφ 111i a 12i sφ 111i ) 2 a. 11ia12icos 2 (φ 98i )cψ 19i (a 11i cφ 111i a 12i sφ 111i ) 2 a 11i 2 φ. 111icos 2 (φ 98i )cψ 19i (a 11i cφ 111i a 12i sφ 111i ) 2 a 12i 2 φ. 111icos 2 (φ 98i )cψ 19i (a 11i cφ 111i a 12i sφ 111i ) 2 (2.6) + a 11ia 12i ψ. 19icos 2 (φ 98i ) c(2φ 111i ) sψ 19i (a 11i cφ 111i a 12i sφ 111i ) 2 + a 11i 2 ψ. 19icos 2 (φ 98i )cφ 111i sφ 111i sψ 19i (a 11i cφ 111i a 12i sφ 111i ) 2 a 12i 2 ψ. 19icos 2 (φ 98i )cφ 111i sφ 111i sψ 19i (a 11i cφ 111i a 12i sφ 111i ) 2 Derivando las ecuaciones 2.35 y 2.36 para obtener (a 11i, a 12i), sustituirlas en 2.6, simplificando y agrupando, tenemos: 35

36 φ 98i = 1 (V V 47i x p + V 48i y p + V 49i z p + V 5i θ 46i + V 56i φ 111i + V 57i ψ 19i) p + V 51i ψ p + V 52i φ p + V 53i x 32i + V 54i ψ 65i + V 55i φ 76i (2.61) Los términos de la ecuación anterior se muestran en el apéndice B. Finalmente, sustituyendo la ec. 2.41, 2.46, 2.5, 2.54 y 2.58 en 2.61 y agrupando: φ 98i = 1 V 46i (E 25i x p + E 26i y p + E 27i z p + E 28i θ p + E 29i ψ p + E 3i φ p) (2.62) E 25i = E 7iV 55i V 16i E 26i = E 8iV 55i V 16i E 27i = E 9iV 55i V 16i E 28i = V 5i + E 1iV 55i V 16i E 29i = V 51i + E 11iV 55i V 16i E 3i = V 52i + E 12iV 55i V 16i 2.6. Cálculo de la Aceleración + E 13iV 56i V 25i + E 14iV 56i V 25i + E 15iV 56i V 25i + E 16iV 56i V 25i + E 17iV 56i V 25i + E 18iV 56i V 25i + E 19iV 57i V 35i + E 2iV 57i V 35i + E 21iV 57i V 35i + E 22iV 57i V 35i + E 23iV 57i V 35i + E 24iV 57i V 35i + E 1iV 54i V 8i + E 2iV 54i V 8i + E 3iV 54i V 8i + E 4iV 54i V 8i + E 5iV 54i V 8i + E 6iV 54i V 8i En el problema cinemático inverso de la aceleración, se tiene que, dada la aceleración (x p, y p, z p, θ p, ψ p, φ p) del plato móvil, hallar la aceleración de las variables (x 32i, ψ 65i, φ 76i, φ 98i, ψ 19i, φ 111i) que se encuentran en las juntas del robot. La aceleración de un punto o un cuerpo rígido que experimenta movimiento, puede ser obtenida por la derivada respecto al tiempo de su función de velocidad. Se asume en ésta sección que la posición, orientación y velocidad de los cuerpos son totalmente conocidas, ya que son resultado del análisis de posición y velocidad. Por lo tanto, con base en las ecuaciones obtenidas en la sección anterior, se obtendrá la aceleración al derivar con respecto al tiempo para cada una de ellas. 36

37 2.6.1 Aceleración del desplazamiento x 32i Tomando la ecuación X.X y obteniendo la segunda derivada con respecto al tiempo, obtenemos 2 A 1i x 32i + B 1i x 32i + (D 1i a 2 87i ) = (2.13) D.. 1i + B 1i x.. 32i + 2x. 32i(B. 1i + A 1i x. 32i) + (B.. 1i + 2A 1i x.. 32i + 4A. 1ix. 32i)x 32i + A.. 2 1ix 32i = (2.63) Despejando x 32i y simplificando, se tiene: x.. 32i = D.. 1i + 2x. 32i(B. 1i + A 1i x. 32i) + (B.. 1i + 4A. 1ix. 32i)x 32i + A.. 2 1ix 32i (2.64) B 1i + 2A 1i x 32i Donde A 1i, B 1i, D 1i, A 1i, B 1i y D 1i se encuentran en el análisis de posición y velocidad, además: A 1i = B 1i = 2(sβ 21i (z.. p a 1312i sθ p (c(γ 1413i φ p )(φ.. p + 2θ. pψ. pcψ p + θ.. psψ p ) + s (γ 1413i φ p )(2θ. pφ. psψ p + θ. p 2 + φ. p 2 )) + a 1312i cθ p (s(γ 1413i φ p )(θ.. p + 2φ. pψ. pcψ p + φ.. psψ p ) c (γ 1413i φ p )(2θ. pφ. p ψ.. pcψ p + sψ p (θ. p 2 + φ. p 2 + ψ. p 2 )))) cβ 21i (cγ 1i (x.. p + a 1312i s (γ 1413i φ p )( φ.. pcψ p + 2φ. pψ. psψ p ) + a 1312i c (γ 1413i φ p )(ψ.. psψ p + cψ p (φ. p 2 + ψ. p 2 ))) + sγ 1i (y.. p a 1312i cθ p (c (γ 1413i (2.65) φ p )(φ.. p + 2θ. pψ. pcψ p + θ.. psψ p ) + s (γ 1413i φ p )(2θ. pφ. psψ p + θ. p 2 + φ. p 2 )) + a 1312i sθ p ( s (γ 1413i φ p )(θ.. p + 2φ. pψ. pcψ p + φ.. psψ p ) + c (γ 1413i φ p )(2θ. pφ. p ψ.. pcψ p + sψ p (θ. p 2 + φ. p 2 + ψ. p 2 )))))) 37

38 38

39 Sustituyendo A 1i, B 1i, D 1i, A 1i, B 1i, D 1i, A 1i, B 1i y D 1i, en la ecuación 2.64 para después agrupar en x 32i, ψ 65i, φ 76i, φ 98i, ψ 19i, φ 111i, se tiene: x 32i = 1 V 58i (G 1i x p + G 2i y p + G 3i z p + G 4i θ p + G 5i ψ p + G 6i φ p + G 7i ) (2.67) Los coeficientes de la ecuación 2.67 se muestran en el apéndice C Aceleración del ángulo ψ 65i Tomando la ecuación 2.19 y obteniendo la segunda derivada con respecto al tiempo, obtenemos: tan (ψ 65i ) = sψ 65i cψ 65i = c 54i Z i X i (2.19) ψ.. 65isec 2 (ψ 65i ) + 2sec 2 (ψ 65i )ψ. 2 65i tan(ψ 65i ) = Z.. i 2X. X i iz. i 2 X i + (c 54i Z i ) ( X.. i. i 2 2 X 2X 3 i X ) (2.68) i Despejando ψ 65i y simplificando, se tiene: ψ.. 65i = Z.. icos 2 (ψ 65i ) X i + c.. 54iX + 2Z icos 2 (ψ 65i )X. i 2 X i 3 icos 2 (ψ 65i ) X i 2 2X. 2ψ. 65i 2 tan (ψ 65i ) iz. icos 2 (ψ 65i ) X i 2 X.. iz i cos 2 (ψ 65i ) X i 2 2c 54icos 2. (ψ 65i )Xi 2 3 X i (2.69) Donde X i, Z i, X i y Z i se encuentran en el análisis de posición y velocidad. Además, derivando las ecuaciones 2.21 para obtener (X i, Z i), sustituirlas en 2.69, simplificando y agrupando, tenemos: ψ 65i = 1 V 59i (G 8i x p + G 9i y p + G 1i z p + G 11i θ p + G 12i ψ p + G 13i φ p + G 14i ) (2.7) Los coeficientes de la ecuación 2.71 se muestran en el apéndice C Aceleración del ángulo φ 76i Tomando la ecuación 2.22 y obteniendo la segunda derivada con respecto al tiempo, obtenemos: tan (φ 76i ) = Y icψ 65i X i (2.22) 39

40 φ.. 76isec 2 (φ 76i ) + 2φ. 2 76i sec 2 (φ 76i ) tan(φ 76i ) = Y.. icψ 65i X i cψ 65i ( X.. i + 2Y. i ( ψ. X 2 + 2X. i 65isψ 65i X i i 2 X i 3 ) + ψ X. icψ 65i X 2.. i 65isψ 65i cψ 65i ψ. 2 65i) X i ) + Y i ( 2X. iψ. 65isψ 65i X 2 + i (2.71) Despejando φ 76i y simplificando, se tiene: φ.. 76i = Y.. icos 2 (φ 76i )cψ 65i X i X.. iy i cos 2 (φ 76i )cψ 65i 2 X i Y icos 2 (φ 76i )cψ 65i ψ. 65i X i 2Y. iψ. 65icos 2 (φ 76i )sψ 65i 2 X i + 2X. iψ. 65iY i cos 2 (φ 76i )sψ 65i X i 2 2φ. 2 76i tan (φ76i ) ψ.. 65iY i cos 2 (φ 76i )sψ 65i X i + 2Y icos 2. (φ 76i )cψ 65i X X i 3 2X. iy. icos 2 (φ 76i )cψ 65i i 2 X i 2 (2.72) Donde X i, Y i, X i y Y i se encuentran en el análisis de posición y velocidad. Además, Derivando las ecuaciones 2.21 y 2.24 para obtener (X i, Y i), sustituirlas en 2.72, simplificando y agrupando, tenemos: φ 76i = 1 V 6i (G 15i x p + G 16i y p + G 17i z p + G 18i θ p + G 19i ψ p + G 2i φ p + G 21i ) (2.73) Los coeficientes de la ecuación 2.73 se muestran en el apéndice C Aceleración del ángulo φ 111i Tomando la ecuación 2.27 y obteniendo la segunda derivada con respecto al tiempo, obtenemos: tan φ 111i = a 32i a 31i (2.27) φ.. 111isec 2 (φ 111i ) + 2sec 2 (φ 111i )φ itan (φ 111i ) = 2a. 31ia. 32i a 2 31i a.. 32i.. 31i + a a 32i ( a 31i a 2 2a 31i. 2 31i 3 ) a 31i (2.74) Despejando φ 111i y simplificando, se tiene: φ.. 111i = 2a. 31ia. 32icos 2 (φ 111i ) a 31i 2 2φ itan (φ 111i ) + a.. 32ia 31icos 2 (φ 111i ) a 31i 2 a.. 32icos 2 (φ 111i ) a 31i 2a 32icos 2 (φ 111i )a 3 a 31i. 2 31i (2.75) Donde a 31i, a 32i, a 31i y a 32i se encuentran en el análisis de posición y velocidad. Además, Derivando dos veces las ecuaciones 2.29 para obtener (a 31i, a 32i), sustituirlas en 2.75, simplificando y agrupando, tenemos: 4

41 φ 111i = 1 V 61i (G 22i x p + G 23i y p + G 24i z p + G 25i θ p + G 26i ψ p + G 27i φ p + G 28i ) (2.76) Los coeficientes de la ecuación 2.75 se muestran en el apéndice C Aceleración del ángulo ψ 19i Tomando la ecuación 2.3 y obteniendo la segunda derivada con respecto al tiempo, obtenemos: tan(ψ 19i ) = a 32i csc(φ 111i ) a 33i (2.3) ψ.. 19isec 2 (ψ 19i ) = 2sec 2 (ψ 19i )ψ. 2 19i tan(ψ 19i ) + a.. 32i csc(φ 111i ) a 33i a 32iφ.. 111i cot(φ 111i ) csc(φ 111i ) 2a. 32iφ. 111i cot(φ 111i ) csc(φ 111i ) a 33i a 33i 2a. 32ia. 33i csc(φ 111i ) 2 a 32ia.. 33i csc(φ 111i ) 2 a 33i a 33i + 2a 32ia. 33iφ. 111i cot(φ 111i ) csc(φ 111i ) 2 + 2a32i csc(φ ) a. 111i 33i 2 3 a 33i a 33i. + a 32icot 2 (φ 111i ) csc(φ 111i ) φ. 111i + a 32icsc 3 (φ 111i )φ a 33i 2 a 33i 2 111i (2.77) Despejando ψ 19i y simplificando, se tiene: ψ.. 19i = 1 a 3 (cos 2 (ψ 19i ) csc(φ 111i ) ( a 33i (2a. 32ia. 33i + a 32i a.. 33i) + 2a 32i a. 2 33i + a.. 32ia. 2 33i + 33i a 33i (a 32i a 33i csc 2 (φ 111i )φ i + cot(φ 111i ) ( a 32i a 33i φ.. 111i 2a 33i a. 32iφ. 111i + (2.78) 2a 32i a. 33iφ. 111i + a 32i a 33i cot(φ 111i ) φ i))) 2a 3 33i ψ. 2 19i tan(ψ 19i )) Donde a 32i, a 32i, a 32i y a 33i se encuentran en el análisis de posición y velocidad. Además, Derivando dos veces las ecuaciones 2.29 y 2.32 para obtener (a 32i, a 33i), sustituirlas en 2.78, simplificando y agrupando, tenemos ψ 19i = 1 V 62i (G 29i x p + G 3i y p + G 31i z p + G 32i θ p + G 33i ψ p + G 34i φ p + G 35i ) (2.79) Los coeficientes de la ecuación 2.79 se muestran en el apéndice C. 41

42 2.6.6 Aceleración del ángulo φ 98i Tomando la ecuación 2.98 y obteniendo la segunda derivada con respecto al tiempo, obtenemos: tan (φ 98i ) = cψ 19i( a 12i cφ 111i a 11i sφ 111i ) a 11i cφ 111i a 12i sφ 111i (2.33) (φ.. 98i + 2φ. 98i 2 1 tan (φ 98i )) = (a 11i cφ 111i a 12i sφ 111i ) 3 ( 2(a 11icφ 111i a 12i sφ 111i )(( a. 11i + a 12i φ. 111i)cφ 111i + (a. 12i + a 11i φ. 111i)sφ 111i )(sφ 111i ((a. 11i a 12i φ. 111i)cψ 19i a 11i ψ. 19isψ 19i ) + cφ 111i ((a. 12i + a 11i φ. 111i)cψ 19i a 12i ψ. 19isψ 19i )) + cψ 19i ( a 12i cφ 111i a 11i sφ 111i )(2((a. 11i a 12i φ. 111i)cφ 111i (a. 12i (2.8) + a 11i φ. 111i)sφ 111i ) 2 + (a 11i cφ 111i a 12i sφ 111i )(cφ 111i ( a.. 11i + a 12i φ.. 111i + 2a. 12iφ. 111i + a 11i φ i) + sφ 111i (a.. 12i + a 11i φ.. 111i + 2a. 11iφ. 111i a 12i φ i))) + (a 11i cφ 111i a 12i sφ 111i ) 2 (sφ 111i ((a 11i ψ.. 19i + 2a. 11iψ. 19i 2a 12i φ. 111iψ. 19i)sψ 19i + cψ 19i ( a.. 11i + a 12i φ.. 111i + 2a. 12iφ. 111i + a 11i φ i + a 11i ψ. 2 19i)) + cφ 111i ((a 12i ψ.. 19i + 2(a. 12i + a 11i φ. 111i)ψ. 19i)sψ 19i cψ 19i (a.. 12i + a 11i φ.. 111i + 2a. 11iφ. 111i a 12i φ i a 12i ψ. 2 19i)))) Despejando φ.. 98i y simplificando, se tiene: φ.. 98i = cos 2 1 (φ 98i )( 4(a 11i cφ 111i a 12i sφ 111i ) 3 (2(2a 11ia 12i ψ.. 19ic (2φ 111i ) + (a 11i a 12i )(a 11i + a 12i )ψ.. 19is (2φ 111i ) + 4ψ. 19i( a 12i a. 11i + a 11i (a. 12i + a 11i φ. 111i) + φ. 111ia i ))(a 11i cφ 111i a 12i sφ 111i )sψ 19i + cψ 19i (a 11i s (3φ 111i )(a 11i 3a 12i 2 a 12i c (3φ 111i )( 3a 11i + a 2 12i )ψ. 2 19i + sφ 111i (4(a 3 12i φ.. 111i 2a 11i (a. 12i + a 11i φ. 111i) 2 + a 12i (a 11i (a.. 12i + a 11i φ.. 111i) + 2a. 11i(a. 12i + 2a 11i φ. 111i)) a 2 12i (a.. 11i + 2a 11i φ i)) 2 + a 11i (a 11i + a 2 12i )ψ. 2 19i) + cφ 111i ( 4a 11i ( 2a. 11ia. 12i + a 11i (a.. 12i + a 11i φ.. 111i)) 2 )ψ. 19i (2.81) 4(a 11i φ.. 111i 4a. 11iφ i)a 12i + a 3 12i ( 8φ i + ψ. 2 19i) + a 12i ( 8a. 2 11i + a 11i (4a.. 11i 8φ. 111i(2a. 12i + a 11i φ. 111i) + a 11i ψ. 2 19i))))) 2φ. 2 98i sec 2 (φ 98i )tan(φ 98i )) Donde a 11i, a 12i, a 11i y a 12i se encuentran en el análisis de posición y velocidad. Además, Derivando dos veces las ecuaciones 2.35 y 2.36 para obtener (a 11i, a 12i), sustituirlas en 2.81, simplificando y agrupando, tenemos: φ 98i = 1 V 63i (G 36i x p + G 37i y p + G 38i z p + G 39i θ p + G 4i ψ p + G 41i φ p + G 42i ) (2.82) Los coeficientes de la ecuación 2.82 se muestran en el apéndice C. 42

43 Capítulo 3 Análisis Dinámico Formulación Euler-Lagrange 3.1. Introducción En éste capítulo, se considera la dinámica del robot, con el fin de poder determinar los torques aplicados por los actuadores en los eslabones de entrada para que el efector final alcance una trayectoria dada. El método de Euler-Lagrange formula ecuaciones de movimiento usando un conjunto de coordenadas generalizadas (Spong, y otros, 1989). Esto permite eliminar todas o algunas de las fuerzas de restricción y permite manejar desplazamientos tanto desplazamientos lineales como angulares con un solo tipo de coordenadas. Con el entendimiento de la dinámica del manipulador, es posible diseñar un controlador con mejores características de ejecución que las realizadas con los típicos encontrados usando métodos heurísticos después de que el manipulador ha sido construido. En este capítulo se empleará la siguiente notación: I ij Matriz de inercia del cuerpo i cadena j K Energía cinética del sistema mecánico L Función Lagrangiana M ij Matriz de elementos de masa del cuerpo i, cadena j q j Coordenada j-ésima generalizada q Vector de coordenadas generalizadas U Energía potencial del sistema mecánico Q j Vector de fuerzas generalizadas La función Lagrangiana es definida como la diferencia entre la energía cinética y la energía potencial de un sistema como: 43

44 L = K U (3.1) Donde K es la energía cinética definida como: K = 1 2 mvt v + ω T Iω (3.2) Y la energía potencial como: U = mg T r G (3.3) La energía cinética depende de la velocidad de los eslabones del manipulador, mientras que la energía potencial depende únicamente de la localización de los eslabones. La ecuación de Lagrange de movimiento es formulada en términos de la función Lagrangiana como: d dt ( L ) L = Q j (3.4) El término Q j es conocido como el vector de fuerzas generalizadas y se obtendrá a partir de expresiones, que involucren los torques y las coordenadas generalizadas Análisis de Posición y Velocidad de Centros de Gravedad Para el análisis del robot paralelo se tomaron en cuenta los siguientes cuerpos: Carga Plato móvil Cuerpo 2i Cuerpo 1i Fig. 3.1 Cuerpos del robot 44

45 Las siguientes matrices de rotación, nos representan la rotación alrededor de los ejes x,y,z respectivamente: 1 cθ sθ cθ sθ R x (θ) = [ cθ sθ] R y (θ) = [ 1 ] R z (θ) = [ sθ cθ ] sθ cθ sθ cθ 1 Se puede observar que en la ec. (3.2) aparece la velocidad de centro de gravedad y la velocidad angular de cada cuerpo., por lo que tendrán que ser formuladas a lo largo de éste sección. A continuación, se procederá a obtener los vectores de centro de gravedad definidos en la base inercial, mostrados en la figura 3.2: Fig. 3.2 Vectores de centro de gravedad 45

46 Posición y Velocidad de Centro de Gravedad del Cuerpo 1i El objetivo de estas secciones, es poder encontrar ecuaciones que definan las posiciones y velocidades necesarias para las ecuaciones de energía cinética (3.2) y potencial (3.3). Dicho proceso será realizado para cada uno de los cuerpos tomados en cuenta. Utilizando la representación vectorial, se tiene: Fig. 3.3 Centro de Gravedad del Cuerpo 1i Con el fin de obtener la velocidad del centro de gravedad del cuerpo 1 i, y con base en la figura anterior, se tiene la siguiente ecuación de posición definida en la base inercial: r G1i = r 32i + r G1i (3.5) r 32i = x 32i i 2i i 2i = R 2 2i i 2i 2 = [1,, ] T i 2i r G1i 2 r G1i 2 = R 2i r G1i = [x G1i, y G1i, z G1i ] T La matriz de rotación antes definida es: R 2i = R z (γ 1i ) R y (β 21i ) 46

47 Derivando respecto al tiempo la ecuación (3.5) se obtiene la velocidad del centro de gravedad: v G1i = v 32i + v G1i = v 32i v G1i = x 32i i 2i (3.6) Donde v G1i =, ya que es un vector de magnitud y orientación constante. Además: x 32i = 1 V 1i (V 2i x p + V 3i y p + V 4i z p + V 5i θ p + V 6i ψ p + V 7i φ p) Haciendo cambio de variable, tenemos: T x 32i = k 1i q (3.7) k T 1i = 1 [V V 2i, V 3i, V 4i, V 5i, V 6i, V 7i ] 1i q = [x p, y p, z p, θ p, ψ p, φ p] T Sustituyendo la ec. (3.7) en (3.6) con el fin de poner ésta última en términos de las coordenadas generalizadas, se tiene: Renombrando: v G1i = i 2i x 32i T v G1i = i 2i (k 1i q ) v G1i = (i 2i k T 1i ) q v G1i = M 1i q (3.8) T M 1i = i 2i k 1i (3.9) Es importante destacar que el cuerpo 1i no realiza desplazamientos angulares, por lo que su velocidad angular es cero: ω 3i = (3.1) 47

48 Posición y Velocidad de Centro de Gravedad del Cuerpo 2i Utilizando la representación vectorial, se tiene: Fig. 3.4 Centro de Gravedad del Cuerpo 2i Con el fin de obtener la velocidad del centro de gravedad del cuerpo 2 i, y con base en la figura anterior, se tiene la siguiente ecuación de posición definida en la base inercial: r G2i = r 32i + r 43i + r 54i + r G2i (3.11) 3 r 43i = R 2i r 43i 3 r 43i = [, ±b 43i, ] T r G2i 7 r G2i 7 = R 7i r G2i = [x G2i,, ] T 4 r 54i = R 2i r 54i 4 r 54i = [,, c 54i ] T La matriz de rotación antes definida es: R 7i = R z (γ 1i ) R y (β 21i )R y (ψ 65i ) R z (φ 76i ) Derivando respecto al tiempo la ecuación (3.11) se obtiene la velocidad del centro de gravedad: 48

49 v G2i = v 32i + v 43i + v 54i + v G2i = x 32i i 2i ω 7i r G2i (3.12) Donde v 43i = v 54i =, ya que es un vector de magnitud y orientación constante, y: v G2i = ω 7i r G2i Debido a que el cuerpo 2 i únicamente realiza movimientos rotacionales con respecto a la base móvil 7 i. Además, el vector de velocidad angular inercial ω 7i para el cuerpo 2 i respecto a la base inercial, se define como: ω 7i = ω 65i + ω 76i (3.13) En donde: = ψ = φ ω 65i ω 76i 65i j 5i 76i k 6i Sustituyendo la ecuación (3.13) en la ecuación (3.12), y desarrollando: v G2i = x 32i i 2i + (ω 65i + ω 76i ) r G2i = x 32i i 2i + (ψ 65i j 5i + φ 76i k 6i ) r G2i = i 2i x 32i + ( j 5i r G2i )ψ 65i + ( k 6i r G2i )φ 76i (3.14) En donde: j 5 5i = R 5i j 5i 6 = R 6i j 6i = R z (γ 1i ) R y (β 21i ) = R z (γ 1i ) R y (β 21i )R y (ψ 65i ) k 6i R 5i R 6i j 5i j 6i 5 = [, 1, ] T 6 = [, 1, ] T Realizando cambio de variables en la ecuación (3.14), tenemos: v G2i = i 2i x 32i + k 2i ψ 65i + k 3i φ 76i (3.15) k 2i = j 5i r G2i k 3i = k 6i r G2i 49

50 Además: ψ 65i = 1 V 8i (E 1i x p + E 2i y p + E 3i z p + E 4i θ p + E 5i ψ p + E 6i φ p) φ 76i = 1 V 16i (E 7i x p + E 8i y p + E 9i z p + E 1i θ p + E 11i ψ p + E 12i φ p) Realizando cambio de variable en las últimas dos ecuaciones: T ψ 65i = k 4i q T φ 76i = k 5i q (3.16) (3.17) k T 4i = 1 [E V 1i, E 2i, E 3i, E 4i, E 5i, E 6i ] 8i k T 5i = 1 [E V 7i, E 8i, E 9i, E 1i, E 11i, E 12i ] 16i Sustituyendo las ecs. (3.7), (3.16) y (3.17) en (3.15), con el fin de poner ésta última en términos de las coordenadas generalizadas, se tiene: T T T v G2i = i 2i k 1i q + k 2i k 4i q + k 3i k 5i q v G2i = (i 2i k T 1i + k 2i k T 4i + k 3i k T 5i ) q (3.18) Renombrando la ec. (3.18): v G2i = M 2i q (3.19) M 2i = i 2i k T 1i + k 2i k T T 4i + k 3i k 5i (3.2) Velocidad Angular del Centro de Gravedad del Cuerpo 2i Desarrollando la ec. (3.13), se tiene: ω 7i = ω 65i + ω 76i ψ ω 7i = j 5i ω 7i = j 5i ω 7i = (j 5i 65i + k 6i φ 76i T T q + k 6i k 5i q + k 6i k T 5i ) q k 4i k T 4i 5

51 Finalmente: ω 7i = M 3i q (3.21) M 3i = j 5i k T T 4i + k 6i k 5i (3.22) Posición y Velocidad de Centro de Gravedad del Plato Móvil Utilizando la representación vectorial, se tiene: Fig. 3.5 Centro de Gravedad de la Plataforma Con el fin de obtener la velocidad del centro de gravedad del plato móvil, y con base en la figura anterior, se tiene la siguiente ecuación de posición definida en la base inercial: r Gp = r p + r Gp (3.23) Sin embargo, si consideramos que la plataforma es una superficie de forma regular y uniforme, en la cual coincide su centroide con su centro de masa, podemos prescindir del vector r Gp y llegar directamente al centro de gravedad utilizando únicamente el vector r p, esto es: r Gp = r p (3.24) 51

52 r p = [x p, y p, z p ] T Derivando respecto al tiempo la ecuación (3.24) se obtiene la velocidad del centro de gravedad: En donde: v Gp = v p v Gp = [x p, y p, z p] T Haciendo un cambio de variable, tenemos: v Gp = M 4 q (3.25) 1 M 4 = [ 1 ] 1 (3.26) Velocidad Angular del Centro de Gravedad del Plato Móvil El vector de velocidad angular inercial ω p del plato móvil se define como: ω p = ω θ + ω ψ + ω φ ω p = i θ p + j 15 ψ p + k 16 φ p (3.27) j = R 15 j = R 16 k 16 k 16 i = [1,, ] T 15 = [, 1, ] T j 15 k = [,, 1] T Las matrices de rotación anteriormente definidas, son: = R x (θ p ) = R x (θ p ) R y (ψ p ) R 15 R 16 52

53 Se define entonces: θ p = k 6 T q ψ p = k 7 T q φ p = k 8 T q (3.28) (3.29) (3.3) k 6 T = [,,, 1,, ] k 7 T = [,,,,1, ] k 8 T = [,,,,, 1] Sustituyendo las ecuaciones (3.28), (3.29) y (3.3) en (3.27): Finalmente: ω p = i k T 6 q + j 15 k T 7 q + k 16 k T 8 q ω p = (i k T 6 + j 15 k T 7 + k 16 k T 8 ) q ω p = M 5 q (3.31) Se define M 5 como sigue: M 5 = i k T 6 + j 15 k T T 7 + k 16 k 8 (3.32) Posición y Velocidad de Centro de Gravedad de la Carga Utilizando la representación vectorial, se tiene: Fig. 3.6 Centro de Gravedad de la Carga 53

54 Con el fin de obtener la velocidad del centro de gravedad de la carga, y con base en la figura anterior, se tiene la siguiente ecuación de posición definida en la base inercial: r Gc = r p + r Gc (3.33) r Gc r p Gc p = R p r Gc = [x Gc, y Gc, z Gc ] T La matriz de rotación anteriormente definida, es: R p = R x (θ p ) R y (ψ p )R z (φ p ) Derivando respecto al tiempo la ecuación (3.33), se obtiene la velocidad del centro de gravedad de la carga: v Gc = v p + v Gc v Gc = v p + ω c r Gc (3.34) v Gc = ω c r Gc Debido a que la carga únicamente realiza movimientos rotacionales con respecto a la base móvil 17 i. Además, el vector de velocidad angular inercial ω c para la carga respecto a la base inercial, se define como: ω c = ω θp + ω ψp + ω φp (3.35) Tras sustituir la ec. (3.35) en la ec. (3.34), tenemos: Finalmente: v Gc = v p + (ω θp + ω ψp = v p + (θ p i + ψ v Gc v Gc + ω φp ) r Gc + φ p j 15 = v p + ( i r Gc )θ p + (j 15 p k 16 ) r Gc r Gc )ψ p + (k 16 r Gc )φ p v Gc = v p + k 9 θ p + k 1 ψ p + k 11 φ p (3.35) 54

55 k 9 = i r Gc k 1 = j 15 r Gc k 11 = k 16 r Gc Sustituyendo ec. (3.25), (3.28), (3.29) y (3.3) en (3.35), obtenemos: = M 4 q + k 9 k T 6 q + k 1 k T 7 q + k 11 k T 8 q = (M 4 + k 9 k T 6 + k 1 k T 7 + k 11 k T 8 )q v Gc v Gc (3.36) Finalmente, tras realizar un cambio de variable en la ec. 3.36, obtenemos: v Gc = M 6 q (3.37) M 6 = M 4 + k 9 k 6 T + k 1 k 7 T + k 11 k 8 T (3.38) Velocidad Angular del Centro de Gravedad de la Carga Debido a que la carga se encuentra fija al plato móvil, se observa que la velocidad angular de la carga y del plato son ambas la misma, es decir: 3.3. Función Lagrangiana ω c = ω p = M 5 q (3.39) Aplicando la ec. (3.1) se consigue de manera general la siguiente expresión: 6 2 L = ( (K ki U ki )) + L p + L c i=1 k=1 i = Número de la cadena k = Número de cuerpos en la cadena i Expandiendo los términos del primer paréntesis: 6 L = ((K 1i U 1i ) + (K 2i U 2i )) + (K p U p ) + (K c U c ) i=1 6 L = (L 1i + L 2i ) + L p + L c (3.4) i=1 55

56 Donde L ki = K ki U ki L 1i = 1 2 (m 1i(v G1i ) T v G1i ) + m 1i g T r G1i L 2i = 1 2 (m 2i(v G2i ) T v G2i + (ω 7i ) T J G1i ω 7i ) + m 2i g T r G2i L p = 1 2 (m p(v Gp ) T v Gp + (ω p ) T J Gp ω p ) + m p g T r Gp L c = 1 2 (m c(v Gc ) T v Gc + (ω c ) T J Gc ω c ) + m c g T r Gc Donde g = [,, 9.81] T Desarrollo del Primer Término de la Ecuación de Lagrange Desarrollando el término d ( L ) dt q j Se tiene la siguiente ecuación definida al principio del capítulo: d dt ( L ) L = Q j (3.4) Desarrollando el primer término de la ecuación (3.4) a partir de la ecuación (3.4): 6 L = ( L 1i i=1 + L 2i ) + L p + L c (3.41) Para j=1,2,3,4,5,6, donde: q 1 = x p q 2 = y p q 3 = z p q 4 = θ p q 5 = ψ p q 6 = φ p Desarrollando L 1i q j A partir de la definición del Lagrangiano en la ec. 3.1, se procede a desarrollar el primer término de la ec. (3.41): L 1i = 1 2 (m 1i(v G1i ) T v G1i ) + m 1i g T r G1i = 1 2 (m 1i(M 1i q ) T M 1i q ) + m 1i g T r G1i 56

57 = 1 2 (m 1iq T (M T 1i M 1i ) q ) + m 1i g T r G1i = 1 2 q T (m 1i M T 1i M 1i ) q + m 1i g T r G1i L 1i = 1 2 q T N 1i q + m 1i g T r (3.42) G1i N 1i = m 1i M 1i T M 1i Aplicando la derivada parcial respecto a q j a la ec. (3.42): L 1i L 1i = ( 1 2 q T N 1i q + m 1i g T r G1i ) = 1 2 ( q T = 1 2 ( q T N 1i q + q T N 1i q q + q T N 1i ) N 1i q + q T N 1i q ) (3.43) Por otra parte, tenemos la siguiente identidad: ( q T N 1i ) q = A T q = q T A = q T T (N q 1i ) (3.44) Aplicando dicha identidad a la ec. (3.43), obtenemos la siguiente expresión: L 1i L 1i = 1 2 (q T T N q q 1i + q T N 1i ) = 1 2 (q T (N T 1i + N 1i ) q ) = 1 2 (q T (2N 1i ) q ) q = q T N 1i (3.45) Donde N 1i es una matriz simétrica, es decir N 1i = N 1i T, comprobando lo anterior: N 1i T = (m 1i M 1i T M 1i ) T = m 1i (M 1i T M 1i ) T = m 1i M 1i T M 1i = N 1i 57

58 Desarrollando L 2i q j A partir de la definición del Lagrangiano en la ec. 3.1, se procede a desarrollar el segundo término de la ec. (3.41): L 2i = 1 2 (m 2i(v G2i ) T v G2i + (ω 7i ) T J G2i ω 7i ) + m 2i g T r G2i = 1 2 (m 2i(M 2i q ) T M 2i q + (M 3i q ) T J G2i M 3i q ) + m 2i g T r G2i = 1 2 (m 2iq T (M T 2i M 2i ) q + q T (M T 3i J G2i M 3i ) q ) + m 2i g T r G2i (3.46) = 1 2 q T (m 2i M T 2i M 2i + M T 3i J G2i M 3i ) q + m 2i g T r G2i L 2i = 1 2 q T N 2i q + m 2i g T r G2i N 2i = m 2i M T 2i M 2i + M T 3i J G2i M 3i 7 J G2i = R 7i J G2i R T 7i Realizando la derivada parcial respecto a q j a la ec. (3.46): L 2i L 2i = ( 1 2 q T N 2i q + m 2i g T r G2i ) = 1 ( q T T N 2 2i q + q N 2i q q + q T N 2i ) = 1 ( q T q N 2 2i q + q T N 2i ) (3.47) Aplicando la identidad (3.44) a la ec. (3.47), obtenemos: L 2i = 1 ( q T q N 2 2i q + q T N 2i ) = 1 2 (q T T N q q 2i + q T N 2i ) = 1 2 (q T (N T 2i + N 2i ) q ) = 1 2 (q T (2N 2i ) q ) L 2i q = q T N 2i (3.48) 58

59 Donde N 2i es una matriz simétrica, es decir N 2i = N 2i T, comprobando lo anterior: N T 2i = (m 2i M T 2i M 2i + M T 3i J G2i M 3i ) T = (m 2i M T 2i M 2i ) T + (M T 3i J G2i M 3i ) T = m 2i (M T 2i M 2i ) T + (M T 3i J G2i M 3i ) T N T 2i = m 2i M T 2i M 2i + M T 3i J T G2i M 3i (3.49) Finalmente, como el tensor de Inercia J T G2i es, a su vez, una matriz simétrica también, se cumple que: J T G2i = J G2i Aplicando este resultado a la ecuación anterior (3.49), concluimos que: N T 2i = m 2i M T 2i M 2i + M T 3i J G2i M 3i = N 2i Desarrollando L p q j A partir de la definición del Lagrangiano en la ec. 3.1, se procede a desarrollar el tercer término de la ec. (3.41): L p = 1 2 (m p(v Gp ) T v Gp + (ω p ) T J Gp ω p ) + m p g T r Gp = 1 2 (m p(m 4 q ) T M 4 q + (M 5 q ) T J Gp M 5 q ) + m p g T r Gp = 1 2 (m p q T (M T 4 M 4 ) q + q T (M T 5 J Gp M 5 ) q ) + m p g T r Gp = 1 2 q T (m p M T 4 M 4 + M T 5 J Gp M 5 ) q + m p g T r Gp L p = 1 2 q T N 3 q + m p g T r (3.5) Gp N 3 = m p M T 4 M 4 + M T 5 J Gp M 5 p J Gp = R p J Gp R p T Realizando la derivada parcial respecto a q j a la ec. (3.5): L p L p = ( 1 2 q T N 3 q + m p g T r Gp ) = 1 ( q T N 2 3 q + q N T 3 q q + q T N 3 ) = 1 ( q T q N 2 3 q + q T N 3 ) (3.51) 59

60 Aplicando la identidad (3.44) a la ec. (3.51), obtenemos: L p = 1 ( q T q N 2 3 q + q T N 3 ) = 1 2 (q T T N q q 3 + q T N 3 ) = 1 2 (q T (N T 3 + N 3 ) q ) = 1 2 (q T (2N 3 ) q ) L p q = q T N 3 (3.52) Donde N 3 es una matriz simétrica, es decir N 3 = N 3 T. Desarrollando L c q j A partir de la definición del Lagrangiano en la ec. 3.1, se procede a desarrollar el cuarto término de la ec. (3.41): L c = 1 2 (m c(v Gc ) T v Gc + (ω c ) T J Gc ω c ) + m c g T r Gc = 1 2 (m c(m 6 q ) T M 6 q + (M 5 q ) T J Gc M 5 q ) + m c g T r Gc = 1 2 (m c q T (M T 6 M 6 ) q + q T (M T 5 J Gc M 5 ) q ) + m c g T r Gc = 1 2 q T (m c M T 6 M 6 + M T 5 J Gc M 5 ) q + m c g T r Gc L c = 1 2 q T N 4 q + m c g T r (3.53) Gc N 4 = m c M T 6 M 6 + M T 5 J Gc M 5 J Gc = R p J p Gc R T p Realizando la derivada parcial respecto a q j a la ec. (3.53): L c L c = ( 1 2 q T N 4 q + m c g T r Gc ) = 1 ( q T T N 2 4 q + q N 4 q q + q T N 4 ) = 1 ( q T q N 2 4 q + q T N 4 ) (3.54) 6

61 Aplicando la identidad (3.44) a la ec. (3.54), obtenemos: L c = 1 ( q T q N 2 4 q + q T N 4 ) = 1 2 (q T T N q q 4 + q T N 4 ) = 1 2 (q T (N T 4 + N 4 ) q ) = 1 2 (q T (2N 4 ) q ) L c q = q T N 4 (3.55) Donde N 4 es una matriz simétrica, es decir N 4 = N 4 T. Al evaluar el término q, dependerá de qué valor tome j, de tal manera que se tienen los q j siguientes resultados para los diferentes valores del iterador. De esta forma, para: j = 1 j = 2 j = 3 j = 4 q = [x p, y p, z p, θ p, ψ p, φ p] T = q 1 q 1 x p q = [1,,,,, ] T q 1 q = [x p, y p, z p, θ p, ψ p, φ p] T = q 2 q 2 y p q = [, 1,,,, ] T q 2 q = [x p, y p, z p, θ p, ψ p, φ p] T = q 3 q 3 z p q = [,, 1,,, ] T q 3 q = [x p, y p, z p, θ p, ψ p, φ p] T = q 4 q 4 θ p q = [,,, 1,, ] T q 4 [x p, y p, z p, θ p, ψ p, φ p] T [x p, y p, z p, θ p, ψ p, φ p] T [x p, y p, z p, θ p, ψ p, φ p] T [x p, y p, z p, θ p, ψ p, φ p] T 61

62 j = 5 j = 6 q = [x p, y p, z p, θ p, ψ p, φ p] T = q 5 q 5 ψ p q = [,,,, 1, ] T q 5 q = [x p, y p, z p, θ p, ψ p, φ p] T = q 6 q 1 x p q = [,,,,, 1] T q 6 [x p, y p, z p, θ p, ψ p, φ p] T [x p, y p, z p, θ p, ψ p, φ p] T Por lo tanto, se hace notar que al derivar el término q respecto al tiempo, se tiene: q j d dt ( q ) = Finalmente, tomando la ecuación (3.41) y derivando respecto al tiempo, se tiene: d dt ( L ) = d dt ( ( L 1i 6 i=1 + L 2i ) + L p + L c ) (3.56) El desarrollo de la derivada con respecto al tiempo de cada término se muestra a continuación: Desarrollando d dt ( L 1i ) q j Derivando respecto al tiempo a la ec. (3.45) y agrupando, se tiene: d dt ( L 1i ) = d ( q T N dt 1i q ) = d T q N dt 1i q + q T dn 1i dt q + q T N 1i dq dt d dt ( L 1i ) = q T N 1i q + q T N 1i q (3.57) Finalmente, realizando cambios de variable a la ec. (3.57): d dt ( L 1i ) = D 1ij q + V 1ij q (3.58) 62

63 D 1ij = q T V 1ij = q T N 1i N 1i (3.59) Además: N 1i = m 1i (M 1iT M1i + M T 1i M 1i) M 1i = i 2i k T T 1i + i 2i k 1i i 2i = k T 1i = 1 [V V 2i, V 3i, V 4i, V 5i, V 6i, V 7i ] 1i k 1i T = d dt k T 1i Desarrollando d dt ( L 2i ) q j Derivando respecto al tiempo a la ec. (3.48) y agrupando, se tiene: d dt ( L 2i ) = d ( q T N dt 2i q ) = d T q N dt 2i q + q T dn 2i dt q + q T N 2i dq dt d dt ( L 2i ) = q T N 2i q + q T N 2i q (3.6) Finalmente, realizando cambios de variable a la ec. (3.6): d dt ( L 2i ) = D 2ij q + V 2ij q (3.61) D 2ij = q T V 2ij = q T N 2i N 2i (3.62) 63

64 Además: M 2i = i 2i k T 1i M 3i = j 5i k T 4i + k 2i k T 4i T + k 6i k 5i 7 J G2i = R 7i J G2i R T 7i k 6 6i = R 6i j 6i T + k 3i k 5i k 2i = j 5i r G2i k 3i = k 6i r G2i k T 4i = 1 [E V 1i, E 2i, E 3i, E 4i, E 5i, E 6i ] 8i k T 5i = 1 [E V 7i, E 8i, E 9i, E 1i, E 11i, E 12i ] 16i j 5 5i = R 5i j 5i = R 7 7i r G2i R 5i = R z (γ 1i ) R y (β 21i ) R 6i = R z (γ 1i ) R y (β 21i )R y (ψ 65i ) = R z (γ 1i ) R y (β 21i )R y (ψ 65i ) R z (φ 76i ) r G2i R 7i Derivando: N 2i = m 2i (M 2iT M2i + M T 2i M 2i) + (M 3iT JG2i M 3i + M T 3i J G2i M 3i + M T 3i J G2i M 2i = i 2i k 1i T + k 2i k T 4i + k 2i k 4i T + k 3i k T T 5i + k 3i k 5i M 3i = j 5i k T 4i + j 5i k 4i T + k 6i k T T 5i + k 6i k 5i 7 J G2i = R 7i J G2i R T 7 7i + R 7i J G2i R T 7i k 2i = j 5i r G2i + j 5i r G2i k 3i = k 6i r G2i + k 6i r G2i k 4i T k 5i T = d dt k T 4i = d dt k T 5i M 3i) A su vez: r G2i k 6i 7 = R 7i r G2i 6 = R 6i j 6i = R 5i R y(ψ 65i ) R 6i R 7i = R 5i B y (ψ 65i ) ψ 65i R y(ψ 65i ) R z (φ 76i ) + R 5i = R 5i = R 5i B y (ψ 65i ) R z (φ 76i ) ψ R y (ψ 65i ) R z(φ 76i ) 65i + R 5i R y (ψ 65i ) B z (φ 76i ) φ 76i Donde j5 7 5i, r G2i j6 6i y j 5i son iguales al vector cero, ya que dependen de valores constantes que no cambian respecto al tiempo. Por otra parte, R 5i = 3 3, ya que sus elementos también son constantes a través del tiempo. 64

65 Desarrollando d dt ( L p ) q j Derivando respecto al tiempo a la ec. (3.52) y agrupando, se tiene: d dt ( L p ) = d ( q T N dt 3 q ) = d T q N dt 3 q + q T dn 3 q + q T N dq dt 3 dt d dt ( L p ) = q T N 3 q + q T N 3 q (3.63) Finalmente, realizando cambios de variable a la ec. (3.63): d dt ( L p ) = D 3j q + V 3j q (3.64) D 3j = q T N 3 V 3j = q T N 3 (3.65) Además: 1 M 4 = [ 1 ] 1 M 5 = i k T 6 + j 15 k T T 7 + k 16 k 8 J p = R p J p 12 R p T k 6 T = [,,, 1,, ] k 7 T = [,,,,1, ] k 8 T = [,,,,, 1] i = [1,, ] T j = R 15 j 15i k = R 16 k 16 j 15 15i = [, 1, ] T k 16 16i = [,, 1] T R 15 = R x (θ p ) R 16 = R x (θ p ) R y (ψ p ) 65

66 R p = R x (θ p ) R y (ψ p )R z (φ p ) Derivando: N 3 = m p (M 4T M4 + M 4 T M 4) + (M 5T Jp M 5 + M 5 T J p M 5 + M 5 T J p M 5) = M 5T Jp M 5 + M T 5 J p M 5 + M T 5 J p M 5 M 4 = M 5 = j 15 k T 7 + k 16 T k 8 j 15 = R j 15i k 16 = R k 16 = B x (θ p ) θ p = R x(θ p ) R y (ψ p ) + R x (θ p ) R y(ψ p ) R 15i R 16i = B x (θ p ) R y (ψ p )θ p + R x (θ p ) B y (ψ p )ψ p R p = R x(θ p ) R y (ψ p )R z (φ p ) + R x (θ p ) R y(ψ p )R z (φ p ) + R x (θ p ) R y (ψ p )R z(φ p ) = B x (θ p ) R y (ψ p )R z (φ p )θ p + R x (θ p ) B y (ψ p )R z (φ p )ψ p + R x (θ p ) R y (ψ p )B z (φ p )φ p J p = R p J p 12 R p T + R p J p 12 R p T Desarrollando d dt ( L c ) q j Derivando respecto al tiempo a la ec. (3.55) y agrupando, se tiene: d dt ( L c ) = d ( q T N dt 4 q ) = d T q N dt 4 q + q T dn 4 q + q T N dq dt 4 dt d dt ( L c ) = q T N 4 q + q T N 4 q (3.66) Finalmente, realizando cambios de variable a la ec. (3.66): d dt ( L c ) = D 4j q + V 4j q (3.67) D 4j = q T N 4 V 4j = q T N 4 (3.68) 66

67 Además: M 6 = M 4 + k 9 k T 6 + k 1 k T T 7 + k 11 k 8 J Gc = R p J p Gc R T p k 9 = i r Gc k 1 = j 15 r Gc k 11 = k 16 r Gc r Gc r p Gc p = R p r Gc = [x Gc, y Gc, z Gc ] T Derivando: N 4 = m c (M 6 T M 6 + M T 6 M 6) + (M 5T JGc M 5 + M T 5 J Gc M 5 + M T 5 J Gc M 5) M 6 = M 4 + k 9k T 6 + k 1k T T 7 + k 11k 8 k 9 = i r Gc k 1 = j 15 r Gc + j 15 r Gc k 11 = k 16 r Gc + k 16 r Gc r Gc = R p p r Gc J Gc = R p J Gc R T p + R p J Gc R p T Desarrollo del Segundo Término de la Ecuación de Lagrange Tomando la ec. (3.4) y aplicando la derivada parcial con respecto a la variable de coordenadas generalizadas: 6 L = ( L 1i + L 2i i=1 ) + L p + L c (3.69) Desarrollando el término L 1i L 1i se define como: L 1i = 1 2 q T N 1i q + m 1i g T r G1i N 1i = m 1i M 1i T M 1i T M 1i = i 2i k 1i i 2 2i = R 2i i 2i 67

68 R 2i = R z (γ 1i ) R y (β 21i ) i 2 2i = [1,, ] T k T 1i = 1 [V V 2i, V 3i, V 4i, V 5i, V 6i, V 7i ] 1i Derivando respecto a q j : L 1i L 1i = 1 ( q T T N 1i N 2 q 1i q + q q + q T q N j q 1i ) + m r j q 1i g T G1i j = 1 2 q T N 1i q + m r 1i g T G1i Renombrando: L 1i = V 1ij q + C 1ij (3.7) V 1ij = 1 2 q T N 1i C 1ij = m 1i g T r G1i (3.71) A su vez: N 1i M 1i T M 1i = m 1i T = i k 1i 2i M 1i + m 1i M 1i T M 1i k T 1i = 1 [V V 2i, V 3i, V 4i, V 5i, V 6i, V 7i ] 1i A continuación, se procederá a derivar los vectores k i (los cuales han sido resultado de la simplificación de algunas expresiones) respecto a q j. A lo largo del procedimiento se presentarán dos tipos de vectores k i, aquellos que contienen tres y los que contienen seis elementos, siendo ambos funciones de los ángulos x 32i, ψ 65i, φ 76i. Con el fin de obtener las derivadas parciales de ambos vectores, se hará uso de la regla de la cadena. A continuación, se procederá a derivar los vectores de tres elementos: 68

69 k m = k m x 32i + k m ψ 65i + k m φ 76i x 32i ψ 65i φ 76i Agrupando de forma matricial, tenemos: k m = J n,3x3 θ i θ i = [ x 32i ψ 65i T φ 76i ] (3.72) Donde J n,3x3 es una matriz de 3x3 y se obtiene factorizando los términos x 32i, ψ 65i, φ 76i. Para obtener las derivadas de los vectores de seis componentes, se hará uso de la regla de la cadena, tal como sigue: k m k m = k m x 32i + k m ψ 65i + k m φ 76i x 32i ψ 65i φ 76i θ i = J n,6x3 + k, m + km, Donde J n,6x3 es una matriz de 6x3, θ i es un vector de 3x1 y k, m es un vector de 6x1 y es la derivada parcial del vector k m, respecto a x p, y p, z p, θ p, ψ p y φ p según corresponda para j (no confundir con k m que es la derivada parcial completa del vector k m respecto a q j ): J n = [ k m x 32i θ i = [ x 32i k m ψ 65i ψ 65i k m φ 76i ] T φ 76i ] A partir de estos resultados, como k T 1i es un vector de 6 componentes, tenemos que: T k 1i θ i = J 1i + k, 1i (3.73) J 1i = [ k 1i x 32i ] 69

70 k 1i = ( 1 [V x 32i x 32i V 2i, V 3i, V 4i, V 5i, V 6i, V 7i ] T ) 1i, k 1i = ( 1 [V V 2i, V 3i, V 4i, V 5i, V 6i, V 7i ] T ) 1i Para obtener r G1i se parte de la ec. 3.5: r G1i = r 32i + r G1i r G1i = r 32i + r G1i (3.5) Sabemos que: r 32i = x 32i i 2i 2 = R 2i r G1i = R z (γ 1i ) R y (β 21i ) r G1i R 2i Luego entonces: r G1i r G1i = (x 32i i 2i ) = i x 32i 2 2i = R q 2i i x 32i 2i j Escribiendo matricialmente la ecuación anterior: r G1i = J 2i θ i (3.74) J 2i = [R 2 2i i 2i ] El cálculo del término θ i se muestra en el apéndice D (Desarrollo de la ecuación de lazo). 7

71 Desarrollando el término L 2i L 2i se define como: L 2i = 1 2 q T N 2i q + m 2i g T r G2i N 2i = m 2i M T 2i M 2i + M T 3i J G2i M 3i M 2i = i 2i k T 1i + k 2i k T T 4i + k 3i k 5i M 3i = j 5i J G2i k T 4i = R 7 7i J G2i + k 6i R T 7i T k 5i k T 1i = 1 [V V 2i, V 3i, V 4i, V 5i, V 6i, V 7i ] 1i k 2i = j 5i r G2i k 3i = k 6i r G2i k T 4i = 1 [E V 1i, E 2i, E 3i, E 4i, E 5i, E 6i ] 8i T = 1 [E V 7i, E 8i, E 9i, E 1i, E 11i, E 12i ] 16i k 5i Derivando respecto a q j : L 2i L 2i = 1 2 ( q T N 2i q + q T N 2i = 1 2 q T N 2i q + m 2i g r T G2i q q + q T N 2i ) + m q 2i g r T G2i j Renombrando: L 2i = V 2ij q + C 2ij (3.75) V 2ij = 1 2 q T N 2i C 2ij = m 2i g T r G2i (3.76) 71

72 A su vez: N 2i = (m q 2i M T 2i M 2i ) + (M T j q 3i J G2i M 3i ) j N 2i M 2i T = M 2i T = m 2i ( M 2i T M q 2i + M T M 2i 2i j (i q 2i k T 1i j = ( i 2i k T 1i + k 2i k T 4i + k 3i k T 5i ) + i k T 1i 2i + k 2i k T 4i ) + ( M3i T J q G2i j + k k T 4i 2i J G2i M 3i + M 3i + k 3i k T q 5i j + k k T 5i 3i ) M 3i + M T 3i J M 3i G2i ) Por otra parte, observamos que i 2i no depende de alguna de las coordenadas generalizadas, ya que: por lo tanto: i 2 2i = R 2i i 2i = R z (γ 1i ) R y (β 21i ) R 2i i 2 2i = [1,, ] T M 2i T = (i k T 1i 2i + k 2i k T 4i + k k T 4i 2i + k 3i k T q 5i j + k k T 5i 3i ) Para obtener el término k 2i, se sabe que: k 2i = j 5i r q G2i + j 5i r G2i j Además, observamos que las siguientes variables no dependen de alguna de las coordenadas generalizadas: Por lo tanto: j 5 5i = R 5i j 5i = R z (γ 1i ) R y (β 21i ) R 5i j 5 5i = [, 1, ] T k 2i = j 5i r G2i Además: r G2i R 7i 7 r G2i 7 = R 7i r G2i = R z (γ 1i ) R y (β 21i )R y (ψ 65i ) R z (φ 76i ) = [x G2i,, ] T 72

73 Luego entonces: k 2i k 2i = j 5i R 7 7i r G2i = j 5i [R q z (γ 1i ) R y (β 21i )R y (ψ 65i ) R z (φ 76i ) r G2i ] j 7 Desarrollando y simplificando, tenemos: k 2i k 2i k 2i k 2i = j 5i [R z (γ 1i ) R y (β 21i ) R y(ψ 65i ) 7 R q z (φ 76i ) r G2i j + R z (γ 1i ) R y (β 21i )R y (ψ 65i ) R z(φ 76i ) 7 r q G2i ] j = j 5i [R z (γ 1i ) R y (β 21i ) R y(ψ 65i ) ψ 65i 7 R ψ 65i q z (φ 76i ) r G2i j + R z (γ 1i ) R y (β 21i )R y (ψ 65i ) R z(φ 76i ) φ 76i 7 r φ 76i q G2i ] j = j 7 ψ 65i 5i [R z (γ 1i ) R y (β 21i )B y (ψ 65i ) R z (φ 76i ) r G2i 7 φ 76i + R z (γ 1i ) R y (β 21i )R y (ψ 65i )B z (φ 76i ) r G2i ] = [j 5i (R z (γ 1i ) R y (β 21i )B y (ψ 65i ) R z (φ 76i ) r G2i 7 )] ψ 65i + [j 7 5i (R z (γ 1i ) R y (β 21i )R y (ψ 65i )B z (φ 76i ) r G2i )] φ 76i Escribiendo matricialmente la ecuación anterior: k 2i = J 3i θ i (3.77) J 2i = [ j 7 5i R z (γ 1i ) R y (β 21i )B y (ψ 65i ) R z (φ 76i ) r G2i R z (γ 1i ) R y (β 21i )R y (ψ 65i )B z (φ 76i ) r G2i ] j 5i 7 Para obtener el término k 3i, se tiene: k 3i = k 6i r q G2i + k 6i r G2i (3.78) j k 6 6i = R 6i j 6i = R z (γ 1i ) R y (β 21i )R y (ψ 65i ) R 6i j 6 6i = [, 1, ] T 73

74 Obteniendo k 6i: k 6i = R 6 6i j 6i = [R q z (γ 1i ) R y (β 21i )R y (ψ 65i )j 6 6i ] j k 6i = R q z (γ 1i ) R y (β 21i ) R y(ψ 65i ) j k 6i = R q z (γ 1i ) R y (β 21i ) R y(ψ 65i ) j ψ 65i k 6i 6 j 6i ψ 65i 6 j q 6i j = [R z (γ 1i ) R y (β 21i )B y (ψ 65i )j 6 6i ] ψ 65i Sustituyendo este resultado en 3.78: k 3 = k 6i r q G2i j k 3i k 3i k 3i + k 6i r G2i = [R z (γ 1i ) R y (β 21i )B y (ψ 65i )j 6 6i ] ψ 65i r q G2i j + k 6i 7 ψ 65i [R z (γ 1i ) R y (β 21i )B y (ψ 65i ) R z (φ 76i ) r G2i 7 φ 76i + R z (γ 1i ) R y (β 21i )R y (ψ 65i )B z (φ 76i ) r G2i ] = [R z (γ 1i ) R y (β 21i )B y (ψ 65i )j 6 6i r G2i ] ψ 65i + [k 7 6i (R z (γ 1i ) R y (β 21i )B y (ψ 65i ) R z (φ 76i ) r G2i )] ψ 65i + [k 7 6i R z (γ 1i ) R y (β 21i )R y (ψ 65i )B z (φ 76i ) r G2i ] φ 76i = [R z (γ 1i ) R y (β 21i )B y (ψ 65i )j 6 6i r G2i + k 6i 7 (R z (γ 1i ) R y (β 21i )B y (ψ 65i ) R z (φ 76i ) r G2i )] ψ 65i + [k 7 6i R z (γ 1i ) R y (β 21i )R y (ψ 65i )B z (φ 76i ) r G2i ] φ 76i Escribiendo matricialmente la ecuación anterior: k 3i = J 4i θ i (3.79) J 4i = [ R z (γ 1i ) R y (β 21i )B y (ψ 65i )j 6 6i r G2i + k 7 6i (R z (γ 1i ) R y (β 21i )B y (ψ 65i ) R z (φ 76i ) r G2i )] k 7 6i R z (γ 1i ) R y (β 21i )R y (ψ 65i )B z (φ 76i ) r G2i T 74

75 Desarrollando L p L p se define como: Simplificando: L p = 1 2 (m p(v Gp ) T v Gp + (ω p ) T J Gp ω p ) + m p g T r Gp L p = 1 2 q T N 3 q + m p g T r Gp Derivando con respecto a q j : N 3 = m p M T 4 M 4 + M T 5 J Gp M 5 1 M 4 = [ 1 ] 1 M 5 = i k T 6 + j 15 k T T 7 + k 16 k 8 J Gp = R p J p Gp R T p k 6 T = [,,, 1,, ] k 7 T = [,,,,1, ] k 8 T = [,,,,, 1] k = R 16 k 16 k 16 16i = [,, 1] T r Gp = r p r p = [x p, y p, z p ] T R 16 = R x (θ p ) R y (ψ p ) R p = R x (θ p ) R y (ψ p )R z (φ p ) L p = 1 (q T N 2 q 3 q ) + L p (m j q p g T r Gp j L p = 1 2 q T N 3 q + m g r T Gp p ) Renombrando: L p = V q 3j q + C 3j j (3.8) 75

76 A su vez: V 3j = 1 2 q T N 3 C 3j = m p g r T Gp (3.81) N 3 = (m q p M T 4 M 4 ) + j N 3 = m q p ( M 4 T M j q 4 + M M 4 4 j Desarrollando cada derivada parcial: T (M T q 5 J Gp M 5 ) j ) + ( M T 5 T J Gp M 5 + M J Gp 5 M 5 + M T 5 J M 5 Gp ) M 4 = J Gp = (R q p J p Gp R T p ) = R p j J p Gp R T p p + R p J Gp R p T Obteniendo R p : R p = R x(θ p ) R y (ψ p )R z (φ p ) + R x (θ p ) R y(ψ p ) R z (φ p ) + R x (θ p ) R y (ψ p ) R z(φ p ) Además: R x (θ p ) = R x(θ p ) θ p = B θ p q x (θ p ) θ p j R y (ψ p ) = R y(ψ p ) ψ p = B ψ p q y (ψ p ) ψ p j R z (φ p ) = R z(φ p ) φ p = B φ p q z (φ p ) φ p j T M 5 = i k 6 q + j 15 k T j q 7 + j 15 + k 16 k T j q 8 + k 16 j k 7 T k 8 T 76

77 Puesto que k 6 T, k 7 T y k 8 T son vectores constantes, se tiene que: Obteniendo j 15 : = [,,, 1,, ] = T k 7 = [,,,, 1, ] = T k 8 = [,,,,, 1] = k 6 T j 15 = R 15 j q 15 j R 15 = R x(θ p ) 15 j 15 = 15 + R j = R x(θ p ) θ p θ p = B q x (θ p ) θ p j Obteniendo k 16 : k 16 = R 16 k q 16 j 16 + R k R 16 = R x(θ p ) R q y (ψ p ) + R x (θ p ) R y(ψ p ) j R k 16 = = R x(θ p ) θ p θ p R q y (ψ p ) + R x (θ p ) R y(ψ p ) ψ p j ψ p = B x (θ p ) R y (ψ p ) θ p + R x (θ p ) B y (ψ p ) ψ p Por otra parte: r Gp = r p = [x q p, y p, z p ] T j Desarrollando L c L c se define como: L c = 1 2 (m c(v Gc ) T v Gc + (ω c ) T J Gc ω c ) + m c g T r Gc (3.82) 77

78 Simplificando: L c = 1 2 (q T N 4 q ) + m c g T r Gc N 4 = m c M T 6 M 6 + M T 5 J Gc M 5 M 6 = M 4 + k 9 k T 6 + k 1 k T T 7 + k 11 k 8 J Gc = R p J p Gc R T p k 9 = i r Gc k 1 = j 15 r Gc k 11 = k 16 r Gc Derivando con respecto a q j : L c = 1 (q T N 2 q 4 q ) + L p (m j q p g T r Gc j L c = 1 2 q T N 4 q q + m gt rgc p j ) Finalmente: L c = V q 4j q + C 4j j (3.83) A su vez: V 4j = 1 2 q N T 4 C 4j = m p g T r Gc (3.84) N 4 = (m q c M T 6 M 6 ) + j N 4 = m q c ( M 6 T M j q 6 + M M 6 6 j Desarrollando cada derivada parcial: M 6 = T (M T q 5 J Gc M 5 ) j ) + ( M T 5 (M 4 + k 9 k 6 T + k 1 k 7 T + k 11 k 8 T ) M 6 = M 4 + k T 9 k k T 6 q 6 + k 9 j + k 1 T J Gc M 5 + M J Gc 5 T k k T 7 q 7 + k 1 j + k 11 M 5 + M T 5 J M 5 Gc ) T k k T 8 q 8 + k 11 j 78

79 M 6 = k 9 k T q 6 + k 1 k T j q 7 + k 11 j k 8 T Obteniendo k 9 : Obteniendo k 1 : k 9 = (i q r Gc ) j k 9 = i r q Gc j k 1 = (j q 15 r Gc ) j k 1 = j 15 r q Gc j + i r Gc + j 15 r Gc Obteniendo k 11 : k 11 = (k q 16 r Gc ) j k 11 = k 16 r Gc + k 16 r Gc Por otra parte: r Gc = (R q p r p Gc ) j r Gc = ( R p r p Gc + R r p Gc p ) Además: r Gc = ( r q p + r Gc ) j r Gc = r p + r Gc Finalmente: J Gc = (R q p J p Gc R T p ) j J Gc = R p J p Gc R p T + R p J Gc p R p T 79

80 3.4. Fuerzas Generalizadas La formulación de la ecuación de Lagrange considera el uso de fuerzas generalizadas contemplando las fuerzas aplicadas externamente, fuerzas y torques de actuadores, de modo que es necesario desarrollar estas expresiones para que sean compatibles con el Lagrangiano, y además sean consistentes con las restricciones mecánicas. Las fuerzas generalizadas se obtienen a partir de la expresión de trabajo virtual. Primero, consideremos el caso el caso en el cual los actuadores ejercen una fuerza o torque en las juntas y fuerzas y momentos externos son aplicados al efector final. Definamos F = [f 1 f 2 f 3 f 4 f 5 f 6 ] T como un vector que representa el torque generado en las juntas y F e = [f e n e ] T, el vector de seis coordenadas de las fuerzas y momentos resultantes en el efector final. Por lo tanto, el trabajo virtual producido por estas fuerzas y momentos es: 6 δw = F T i δr i + M T i δq i i=1 Aplicado al robot, donde i, j representan el número de cadena y cuerpo respectivamente: 6 2 δw = (f T ij δr i + m ij i=1 j=1 6 = f i T δr i i=1 + f T ext T δq ij ) + f T ext δr ext + n T ext δq ext δr ext + n T ext δq ext (3.85) Obteniendo los términos f i : f i = R 2i (f i i 2i 2i ) R 2i = R z3 (γ 1i )R z2 (β 21i ) (3.86) Las velocidades lineales se relacionan con los desplazamientos virtuales, esto es: δr = δv δx δx Se plantean los desplazamientos virtuales que están relacionados con las fuerzas externas: δr i = δv 32i δx 32i δx 32i (3.87) 8

81 v 32i = x 32i i 2i i 2i = R 2i 2i i 2i (3.88) Sustituyendo la ec en 3.87: δr i = δr 2i 2i i 2i x 32i δx 32i 2i = R 2i i 2i δx 32i δx 32i (3.89) De la ec tenemos: 6 f T i δr i = f T 1 δr 1 + f T 2 δr 2 + f T 3 δr 3 + f T 4 δr 4 + f T 5 δr 5 + f T 6 δr 6 i=1 (3.9) Sustituyendo los términos de las ec y 3.89 en la ec. 3.9 se tiene: 6 f i T δr i i=1 6 = (R 2i f i i 2i 2i ) T 2i (R 2i i 2i δx 32i ) i=1 6 2i T = f i (i 2i R T 2i )(R 2i i 2i 2i )δx 32i i=1 6 2i T 2i = f i i 2i i 2i δx 32i i=1 6 = f i δx 32i i=1 6 f T i δr i = f 1 δx f 2 δx f 3 δx f 4 δx f 5 δx f 6 δx 326 i=1 (3.91) Escribiendo matricialmente la ec. 3.91: 6 f T i δr i = f T δr i=1 (3.92) f T = [f 1 f 2 f 3 f 4 f 5 f 6 ] δr = [ δr 321 δr 322 δr 323 δr 324 δr 325 δr 326 ] T 81

82 Ahora, con el fin de poner la ec en función de δq, se tiene la ec. D.3: J R R = J q q (D.3) Dividiendo ambos lados de la ecuación anterior por δt y despejando δr, tenemos: δr = J R 1 J q δq (3.93) Finalmente, sustituyendo la ec 3.93 en 3.92: 6 f T i δr i = f T 1 J R J q δq i=1 (3.94) Para obtener el término δr ext, se tiene: r ext = r p p + R p r e r p = [ x p y p z p] T r e = [ x e y e z e] T Obteniendo ahora el cambio virtual en el vector r ext : δr e = δr p + δr p r e p = δr p + ( δr p = δr p + δr p δθ δθ p + δr p δψ p δψ p + δr p p δφ p δφ p ) r e p r δθ p e δθ p + δr p r p δψ p e δψ p + δr p r p δφ p e δφ p p Escribiendo matricialmente la ecuación anterior: δr ext = J 26 δq (3.95) J 26 = [I 3x3 J 26,1 J 26,2 J 26,3 ] δq = [δr p δθ p δψ p δφ p ] T 82

83 J 26,1 = δr p δθ p r e p J 26,2 = δr p δψ p r e p J 26,3 = δr p δφ p r e p Para obtener el término δq ext : = δω Op δθ δq ext δθ + δω Op δψ δψ + δωop δφ δφ (3.96) ω p = ω θ + ω ψ + ω φ ω θ = θ pi ω 15 ψ = ψ pr 15 j 15 ω φ = φ pr k 16 R 15 = R x (θ p ) R 16 = R x (θ p ) R y (ψ p ) Sustituyendo los términos anteriores en la ec. 3.96: δq ext = δ (θ pi δθ + ψ pr 15 j φ pr 16 k )δθ + δ (θ pi δψ + ψ pr 15 j φ pr 16 k )δψ + δ (θ pi δφ + ψ pr 15 j φ pr 16 k )δφ = i 15 δθ + R 15 j 15 δψ + R k 16 δφ Escribiendo matricialmente la ecuación anterior: δq ext = J 27 δq (3.97) J 27 = [ J 27,1 J 27,2 J 27,3 ] δq = [δr p δψ δθ δφ] T 83

84 J 27,1 = i 15 J 27,2 = R 15 j 15 J 27,3 = R k 16 Por último, sustituyendo las ecuaciones 3.94, 3.95 y 3.97 en 3.85: 6 δw = f i T δr i i=1 + f T ext δr ext + n T ext δq ext = f T 1 T T J R J q δq + f ext J 26 δq + n ext J 27 δq = (f T 1 T T J R J q + f ext J 26 + n ext J 27 ) δq = Q T δq (3.98) Las fuerzas generalizadas obtenidas son: Q T = f T 1 T T J R J q + f ext J 26 + n ext J Sustitución de los términos en la Ecuación de Lagrange Por último, se tiene la ec 3.4: d dt ( L ) L = Q j (3.4) d dt ( L ) = d dt ( ( L 1i 6 6 i=1 + L 2i ) + L p + L c ) = (D 1ij q + V 1ij q + D 2ij q + V 2ij q ) + D 3j q + V 3j q + D 4j q + V 4j q i=1 6 = [ (D 1ij + D 2ij ) + D 3j + D 4j ] q + [ (V 1ij + V 2ij ) + V 3j + V 4j ] q i=1 6 L = i=1 6 (L 1i + L 2i ) + L p + L c = (V 1ij q + C 1ij + V 2ij q + C 2ij ) + V 3j q + C 3j + V 4j q + C 4j i=1 6 i=1 84

85 6 = [ (V 1ij + V 2ij ) + V 3j + V 4j ] q + [ (C 1ij + C 2ij ) + C 3j + C 4j ] i=1 6 i=1 Q T = f T 1 T T J R J q + f ext J 26 + n ext J 27 1 Q = (J R J q ) T T T f + J 26 f ext + J 27 n ext Sustituyendo en la ec. 3.4: Finalmente: 6 [ (D 1ij + D 2ij ) + D 3j + D 4j ] q + [ (V 1ij + V 2ij ) + V 3j + V 4j ] q i=1 6 i=1 6 i=1 6 [ (V 1ij + V 2ij ) + V 3j + V 4j ] q [ (C 1ij + C 2ij ) + C 3j + C 4j ] = Q j 6 i=1 [ (D 1ij + D 2ij ) + D 3j + D 4j ] q + i=1 6 [ (V 1ij + V 2ij ) + V 3j + V 4j (V 1ij + V 2ij ) V 3j V 4j ] q + i=1 6 6 i=1 [ ( C 1ij C 2ij ) C 3j C 4j ] = Q j i=1 D j q + V j q + C j = Q j (3.99) 6 D j = [ (D 1ij + D 2ij ) + D 3j + D 4j ] i=1 6 V j = [ (V 1ij + V 2ij ) + V 3j + V 4j i=1 6 C j = [ ( C 1ij C 2ij ) C 3j C 4j ] i=1 6 (V 1ij i=1 + V 2ij ) V 3j V 4j Escribiendo la ec seis veces, una para cada j=1,2,3,4,5,6, obtenemos seis ecuaciones escalares, las cuales se pueden ordenar de la siguiente forma: ] 85

86 D 1 D 2 D 3 D 4 D 5 [ D 6 ] q + V 1 V 2 V 3 V 4 V 5 [ V 6 ] q + C 1 C 2 C 3 C 4 C 5 [ C 6 ] = Q 1 Q 2 Q 3 Q 4 Q 5 [ Q 6 ] D q + V q + C = Q D q + V q + C = (J 1 R J q ) T f + J T 26 f ext + J T 27 n ext D q + V q + C + ( J T 26 f ext J T 27 n ext ) = (J 1 R J q ) T f 1 (J R J q ) T D 1 q + (J R J q ) T V 1 q + (J R J q ) T C 1 + (J R J q ) T T T ( J 26 f ext J 27 n ext ) = f Finalmente: Dq + Vq + C + E = f (3.1) D = (J R 1 J q ) T D V = (J R 1 J q ) T V C = (J R 1 J q ) T C E = (J 1 R J q ) T ( J T 26 f ext J T 27 n ext ) 86

87 Capítulo 4 Resultados A continuación, se muestran los resultados del análisis cinemático y dinámico del robot paralelo para la trayectoria descrita en el Apéndice A. 4.1 Gráficas de Posición Los parámetros del robot relacionados con la geometría están basados son similares a la estructura en la que se basó éste trabajo, y se presentan de la siguiente manera debido a que se repiten para cada una de las cadenas cinemáticas: γ 11 = β 211 = 3 b 431 =.127 m c 541 =.85 m a 871 =.8 m γ = 33 a = 35 γ = 21 γ 11 = 12 β 211 = 3 b 431 =.127 m c 541 =.85 m a 871 =.8 m γ = 3 a = 35 γ = 3 γ 11 = β 211 = 3 b 431 =.127 m c 541 =.85 m a 871 =.8 m γ = 3 a = 35 γ = 15 γ 11 = 24 β 211 = 3 b 431 =.127 m c 541 =.85 m a 871 =.8 m γ = 33 a = 35 γ = 33 γ 11 = 12 β 211 = 3 b 431 =.127 m c 541 =.85 m a 871 =.8 m γ = 33 a = 35 γ = 9 γ 11 = 24 β 211 = 3 b 431 =.127 m c 541 =.85 m a 871 =.8 m γ = 3 a = 35 γ = 27 87

88 Fig. 4.1 Gráfica para x 32i Fig. 4.2 Gráfica para ψ 65i Fig. 4.3 Gráfica para φ 76i 88

89 Fig. 4.4 Gráfica para φ 98i Fig. 4.5 Gráfica para ψ 19i Fig. 4.6 Gráfica para φ 111i 89

90 4.2 Gráficas de Velocidad Fig. 4.7 Gráfica para x 32i Fig. 4.8 Gráfica para ψ 65i Fig. 4.9 Gráfica para φ 76i 9

91 Fig. 4.1 Gráfica para φ 98i Fig Gráfica para ψ 19i Fig Gráfica para φ 111i 91

92 4.3 Gráficas de Aceleración Fig Gráfica para x 32i Fig Gráfica para ψ 65i Fig Gráfica para φ 76i 92

93 Fig Gráfica para φ 98i Fig Gráfica para ψ 19i Fig Gráfica para φ 111i 93

94 4.4. Solución del método Euler-Lagrange La solución del método de Euler-Lagrange, se obtuvo con ayuda del software Mathematica, y consistió en programar la ecuación 3.1 con cada uno de los términos que la componen. Los datos de masas, inercias y fuerzas externas son obtenidos a partir del CAD y del material que constituirá al sistema. En la figura 4.19 se muestra la gráfica de torques obtenida para el análisis dinámico, correspondiente a la trayectoria trazada en el apéndice A: Los parámetros de masa e inercia para cadena se muestran a continuación: m 1i = 1.76 Kg m 2i = Kg J G2i = [..5.] Kg m Para i = 1,2,3,4,5,6 Por otra parte, los datos de masa e inercia de la plataforma y la carga son: m p = Kg J G2 = [ ] Kg m m c = 3 Kg J G2 = [ ] Kg m Fig Gráfica de fuerzas de los actuadores A partir de los datos obtenidos de la posición x 32i, y con la información de las fuerzas necesarias en los actuadores para mover una carga de 8 [N], fue posible obtener las curvas de trabajo y potencia necesarias para cumplir con la trayectoria descrita en el apéndice A. 94

95 Fig. 4.2 Gráfica de trabajo mecánico efectuado por la fuerza Fi Fig Gráfica de potencia necesaria en los actuadores 95

96 Capítulo 5 Conclusiones En el presente trabajo se realizó el estudio de la cinemática y dinámica de un robot paralelo de 6 grados de libertad utilizando el método de transformaciones homogéneas y el método de Euler- Lagrange, respectivamente. Ambos métodos permitieron la obtención de variables de gran interés para verificar que la estructura pueda cumplir con su principal propósito, sea en este caso, simular movimientos armónicos que representen en gran medida el comportamiento de los sismos comunes de la Ciudad de México. El análisis a partir del método de transformaciones homogéneas permitió un desarrollo sencillo de la cinemática del robot, tomando en cuenta la geometría y los distintos tipos de juntas de la estructura. Una de las grandes ventajas de éste método es que, sin importar que tan complicada pueda ser la configuración de un robot, es posible aplicar una serie de transformaciones convenientemente definidas a través de la estructura del robot para representar la geometría y los movimientos posibles entre cada eslabón que lo conforman. Utilizando las técnicas de la cinemática inversa, fue posible proponer una trayectoria y un conjunto de orientaciones basadas en el tipo de ondas provocadas por movimientos telúricos. Con esto, fue posible obtener el movimiento necesario de los actuadores lineales, así como las orientaciones de las juntas esféricas. En total, se lograron obtener seis variables por cadena cinemática (36 en total por todo el sistema). Las simulaciones realizadas en el sistema para la trayectoria propuesta, nos permitieron demostrar que las velocidades y aceleraciones no generan picos considerables, y por ende, los actuadores no necesitan ejercer fuerzas excesivamente grandes para vencer y contrarrestar las inercia de los eslabones y la carga. A partir del análisis por medio del método de Euler-Lagrange, se logró obtener el conjunto de fuerzas necesarias para mover una carga de 85 [Kg] (aprox. 83 [N]). Por otra parte, al conocer los desplazamientos lineales de los actuadores, fue posible calcular el trabajo y la potencia requerida por el sistema, siendo estas variables importantes en la etapa de selección de los actuadores más adecuados al sistema. A diferencia de las mesas vibradoras, éste modelo permite contemplar todos los movimientos posibles en un espacio tridimensional, lo cual permite una representación más fiel a la realidad de los sismos ocurridos en cualquier parte del mundo. 96

97 Bibliografía [1] Angeles, Jorge Fundamentals of Robotic Mechanical Systems. Montreal Quebec. Spinger, [2] Arai, T, et al Development of Parallel Link Manipulator for Underground Excavation Task. Proc International Symposyum on Advanced Robot Technology pp [3] Clavel, R A Fast Robot with Parallel Geometry. Proc. 18 th International Symposium on Industrial Robots pp [4] Codourey, Alain Dynamic Modelling of Parallel Robots for Computed-Torque Control Implementation. Sagepublications, 1998, Vol. The International Journal of Robotics Research 1998; 17; [5] Flores, Shair Mendoza. 26. Análisis Cinemático y Dinámico de un Robot Delta de 3 Grados de Libertad. México. D.F.: s.n., 26. [6] Giddings &Lewis Gidding and Lewis Machine Tool. Fond du Lac [7] Ahmadi, Mahdi, et al. 28. Inverse Dynamics of Hexa Parallel Robot Using Lagrangian Dynamics Formulation. Florida : IEEE, 28, Vols. 12 th International Conference on Intelligent Engineering Systems. February [8] McGill, David J. and King, Wilton Mecánica para ingeniería y sus aplicaciones. Dinámica. S.l. : Grupo Editorial Iberoamérica, [9] Merlet, Jean Pierre. 2. Parallel Robots. S.l. : Kluwer Academic Publishers, 2. [1] Pierrot, F., Fournier, A. and Dauchez, P Toward a Fully Parallel 6-DOF Robot for High-Speed Applications. Proc IEEE International Conference on Robotics and Automation pp [11] Reinholtz, C and Gahkale, D. Design an Analysis of Variable Geometry Truss Robot. Proc. 9 th Applied Mechanisms Conference. Oklahoma State University, Stillwater. OK : s.n. [12] Spong, Mark W. and Vidyasagar, M Robot Dynamics and Control. S.l. : John Wiley & Sons, pp [13] Stejskal, Vladimir and Valásek, Michael. 1996, Kinematics and Dynamics of Machinery. s.l. : Marcel Dekker, Inc., [14] Stewart, D A platform with 6 degrees of freedom. Proc. Institution of Mechanical Engineers Vol. 18, pp

98 [15] Tsai, Lun Wen Robot Analysis The mechanics of serial and parallel manipulators. s.l. : John Wiley & Sons,

99 Apéndice A Generación de Trayectoria El movimiento de un cuerpo en el espacio tridimensional puede definirse a través de dos movimientos separados. En el primero de ellos, se puede establecer por medio de una trayectoria a través de un conjunto de rectas y /o curvas, la cual debe seguir algún punto del cuerpo de interés (por ejemplo, el centro de gravedad del órgano terminal). El segundo movimiento, se encarga de describir la orientación angular. Ambas ecuaciones deben cumplir con condiciones de posición, velocidad y aceleración linear y angular, y se realizadas durante un tiempo definido. A continuación, se plantean las trayectorias propuestas para el centro de gravedad de la plataforma. Trayectoria La función aquí definida, representa una superposición de movimientos armónicos simples, acorde con el Teorema de Fourier. Dicha trayectoria se compone de las siguientes ecuaciones paramétricas en el espacio tridimensional: f(t) = { x p = R cos (t) y p = R sin (t) z p = A sin(ωt) Además, se define: R = radio de la circunferencia en el plano XY [m] A = Amplitud del movimiento armónico simple [m] ω = Frecuencia angular del movimiento armónico simple [rad/s] ω = 2πf f = Frecuencia del movimiento armónico simple [Hz] Para obtener las ecuaciones de velocidad y aceleración, bastó con derivar una y dos veces las ecuaciones paramétricas: 99

100 v(t) = { x p = R sin (t) y p = R cos(t) z p = ωa cos(ωt) a(t) = { x p = R sin (t) y p = R sin (t) z p = ω 2 A sin(ωt) En la figura A.1 se muestra la trayectoria aquí descrita, para los siguientes parámetros: R =.1 [m] A =.1 [m] f = 1 [Hz] Orientación Angular Fig. A.1 Trayectoria Propuesta Para la orientación angular, también se definieron movimientos que representaran la superposición de ondas a través de los tres grados de libertad disponibles. Las ecuaciones se muestran a continuación: θ p [t] = A 1 cos (f 1 t) ψ p [t] = A 2 cos (f 2 t) φ p [t] = A 3 cos (f 3 t) Donde A i = Amplitud del movimiento armónico simple para la orientación i f i = Frecuencia del movimiento armónico simple para la orientación i 1

101 Para obtener las ecuaciones de velocidad y aceleración, bastó con derivar una y dos veces las ecuaciones paramétricas, lo cual condujo a las siguientes ecuaciones: θ p[t] = f 1 A 1 sin (f 1 t) ψ p[t] = f 2 A 2 sin (f 2 t) φ p [t] = f 3A 3 sin (f 3 t) θ p [t] = f 1 2 A 1 cos (f 1 t) ψ p [t] = f 2 2 A 2 cos (f 2 t) φ p [t] = f 3 2 A 3 cos (f 3 t) 11

102 Apéndice B Coeficientes de la ecuación V 8i = 2c 54i Z i + c 54i + X 2 2 i + Z i V 9i = cγ 1i (z p c 54i cβ 21i + x 32i sβ 21i + a 1312i s (γ 1413i φ p )sθ p + a 1312i c (γ 1413i φ p )cθ p sψ p ) V 1i = sγ 1i (z p c 54i cβ 21i + x 32i sβ 21i + a 1312i s (γ 1413i φ p )sθ p + a 1312i c (γ 1413i φ p )cθ p sψ p ) V 11i = x 32i cβ 21i + cγ 1i ( x p + a 1312i c (γ 1413i φ p )cψ p ) + c 54i sβ 21i sγ 1i (y p + a 1312i s (γ 1413i φ p )cθ p a 1312i c (γ 1413i φ p )sθ p sψ p ) V 12i =.5a 1312i (2a 1312i cos 2 (θ p )(c(γ 1413i φ p ) 2 sin 2 (ψ p ) + s (γ 1413i φ p ) 2 )sβ 2 21i sγ 1i + cθ p sβ 21i ( a 1312i s (2(γ 1413i φ p ))cγ 1i cψ p sβ 21i + 2s (γ 1413i φ p )( c 54i + x p cγ 1i sβ 21i + y p sβ 21i sγ 1i ) + 2c (γ 1413i φ p )(x 32i + z p sβ 21i )sγ 1i sψ p ) + 2cβ 21i ( s (γ 1413i φ p )(x 32i cθ p + c 54i sγ 1i sθ p ) + c (γ 1413i φ p )( c 54i cθ p sγ 1i + x 32i sθ p )sψ p ) + sθ p (a 1312i c (γ 1413i φ p ) 2 s (2ψ p )cγ 1i + 2sβ 21i (a 1312i c (γ 1413i φ p ) 2 sin 2 (ψ p )sβ 21i sγ 1i sθ p + s (γ 1413i φ p )sγ 1i (x 32i + sβ 21i (z p + a 1312i s (γ 1413i φ p )sθ p )) + c (γ 1413i φ p )(c 54i sβ 21i (x p cγ 1i + y p sγ 1i ))sψ p )) + cβ 2 21i (cγ 1i (2x p s (γ 1413i φ p )cθ p a 1312i s (2(γ 1413i φ p ))cθ p cψ p 2x p c (γ 1413i φ p )sθ p sψ p ) + 2sγ 1i (a 1312i c (γ 1413i φ p ) 2 sin 2 (ψ p ) + s (γ 1413i φ p )(a 1312i s (γ 1413i φ p ) + y p cθ p + z p sθ p ) + c (γ 1413i φ p )(z p cθ p y p sθ p )sψ p ))) V 13i = a 1312i c (γ 1413i φ p )(cψ p (cθ p (c 54i sβ 21i y p sγ 1i ) sγ 1i (a 1312i s (γ 1413i φ p ) + (z p + x 32i sβ 21i )sθ p ) + cβ 21i (x 32i cθ p + c 54i sγ 1i sθ p )) + cγ 1i (cθ p (a 1312i c (γ 1413i φ p ) x p cψ p ) + (z p c 54i cβ 21i + x 32i sβ 21i + a 1312i s (γ 1413i φ p )sθ p )sψ p )) V 14i = a 1312i (sγ 1413i (cθ p (z p c 54i cβ 21i + x 32i sβ 21i )sγ 1i + (x 32i cβ 21i + c 54i sβ 21i y p sγ 1i )sθ p )sφ p + cγ 1413i (cθ p cφ p (z p c 54i cβ 21i + x 32i sβ 21i )sγ 1i + cφ p (x 32i cβ 21i x p cγ 1i + c 54i sβ 21i y p sγ 1i )sθ p cγ 1i cψ p (z p c 54i cβ 21i + x 32i sβ 21i )sφ p ) + (a 1312i sγ 1i + s (γ 1413i φ p )( cθ p (x 32i cβ 21i + c 54i sβ 21i y p sγ 1i ) + (z p c 54i cβ 21i + x 32i sβ 21i )sγ 1i sθ p ))sψ p + cγ 1i (cφ p cψ p (z p c 54i cβ 21i + x 32i sβ 21i )sγ 1413i + a 1312i cψ p sθ p x p sγ 1413i sθ p sφ p + x p s (γ 1413i φ p )cθ p sψ p )) V 15i = c 54i sβ 21i (cγ 1i (x p a 1312i c (γ 1413i φ p )cψ p ) + (y p + a 1312i s (γ 1413i φ p )cθ p )sγ 1i ) cβ 21i (z p + a 1312i s (γ 1413i φ p )sθ p ) + a 1312i c (γ 1413i φ p )( cβ 21i cθ p + sβ 21i sγ 1i sθ p )sψ p Coeficientes de la ecuación 2.49 V 16i = Y i 2 cos 2 (ψ 65i ) + X i 2 V 17i = cψ 65i (cβ 21i ( y p + b 43i cγ 1i a 1312i s (γ 1413i φ p )cθ p + a 1312i c (γ 1413i φ p )sθ p sψ p ) + sγ 1i (x 32i + sβ 21i (z p + a 1312i s (γ 1413i φ p )sθ p + a 1312i c (γ 1413i φ p )cθ p sψ p ))) V 18i = cψ 65i ( cβ 21i (x p a 1312i c (γ 1413i φ p )cψ p + b 43i sγ 1i ) + cγ 1i (x 32i + sβ 21i (z p + a 1312i s (γ 1413i φ p )sθ p + a 1312i c (γ 1413i φ p )cθ p sψ p ))) V 19i = cψ 65i sβ 21i ( b 43i + ( x p + a 1312i c (γ 1413i φ p )cψ p )sγ 1i + cγ 1i (y p + a 1312i s (γ 1413i φ p )cθ p a 1312i c (γ 1413i φ p )sθ p sψ p )) 12

103 V 2i =.5a 1312i cψ 65i (2a 1312i c (γ 1413i φ p ) 2 sin 2 (ψ p )cγ 1i sβ 21i + 2a 1312i cos 2 (θ p )s (γ 1413i φ p ) 2 cγ 1i sβ 21i + sθ p (a 1312i s (2(γ 1413i φ p ))cβ 21i cψ p + 2s (γ 1413i φ p )( cβ 21i (x p + b 43i sγ 1i ) + cγ 1i (x 32i + sβ 21i (z p + a 1312i s (γ 1413i φ p )sθ p )))) + 2c (γ 1413i φ p )sβ 21i (b 43i y p cγ 1i + (x p a 1312i c (γ 1413i φ p )cψ p )sγ 1i )sθ p sψ p + cθ p (sβ 21i (a 1312i s (2(γ 1413i φ p ))cψ p sγ 1i 2s (γ 1413i φ p )(b 43i y p cγ 1i + x p sγ 1i )) + 2c (γ 1413i φ p )(cγ 1i (x 32i + z p sβ 21i ) cβ 21i (x p a 1312i c (γ 1413i φ p )cψ p + b 43i sγ 1i ))sψ p )) V 21i = a 1312i c (γ 1413i φ p )cψ 65i (cγ 1i cψ p (a 1312i s (γ 1413i φ p )sβ 21i + (x 32i + z p sβ 21i )sθ p ) + sγ 1i (x 32i + sβ 21i (z p + a 1312i s (γ 1413i φ p )sθ p ))sψ p + cθ p (sβ 21i (a 1312i c (γ 1413i φ p )sγ 1i cψ p (b 43i y p cγ 1i + x p sγ 1i )) a 1312i s (γ 1413i φ p )cβ 21i sψ p ) + cβ 21i ((a 1312i c (γ 1413i φ p ) cψ p (x p + b 43i sγ 1i ))sθ p + ( y p + b 43i cγ 1i )sψ p )) V 22i = a 1312i cψ 65i ( s (γ 1413i φ p )cψ p (x 32i + z p sβ 21i )sγ 1i + sβ 21i ( a 1312i cψ p sγ 1i + c (γ 1413i φ p )(b 43i + x p sγ 1i ))sθ p s (γ 1413i φ p )cθ p sβ 21i (b 43i + x p sγ 1i )sψ p + cγ 1i (c (γ 1413i φ p )(cθ p (x 32i + z p sβ 21i ) y p sβ 21i sθ p ) + (a 1312i sβ 21i + s (γ 1413i φ p )(y p cθ p sβ 21i + (x 32i + z p sβ 21i )sθ p ))sψ p ) + cβ 21i ( cθ p ( a 1312i cψ p + c (γ 1413i φ p )(x p + b 43i sγ 1i )) + s (γ 1413i φ p )((y p b 43i cγ 1i )cψ p (x p + b 43i sγ 1i )sθ p sψ p ))) V 23i = cψ 65i ( b 43i + ( x p + a 1312i c (γ 1413i φ p )cψ p )sγ 1i + cγ 1i (y p + a 1312i s (γ 1413i φ p )cθ p a 1312i c (γ 1413i φ p )sθ p sψ p )) V 24i = sψ 65i ( b 43i + ( x p + a 1312i c (γ 1413i φ p )cψ p )sγ 1i + cγ 1i (y p + a 1312i s (γ 1413i φ p )cθ p a 1312i c (γ 1413i φ p )sθ p sψ p ))( x 32i sβ 21i (z p + a 1312i s (γ 1413i φ p )sθ p + a 1312i c (γ 1413i φ p )cθ p sψ p ) + cβ 21i (cγ 1i (x p a 1312i c (γ 1413i φ p )cψ p ) + sγ 1i (y p + a 1312i s (γ 1413i φ p )cθ p a 1312i c (γ 1413i φ p )sθ p sψ p ))) Coeficientes de la ecuación V 25i = a 31i + a 32i V 26i = V 27i = V 28i = V 29i =.125(4cψ p ( s(2(β 21i + ψ 65i ))cγ 1i cθ p + s (2γ 1i )s(β 21i + ψ 65i ) 2 sθ p ) + (6 2c (2γ 1i ) + 2c (2(β 21i + ψ 65i )) + c (2(β 21i γ 1i + ψ 65i )) + c (2(β 21i + γ 1i + ψ 65i )))sψ p ) V 3i = (s (β 21i + ψ 65i )cθ p sγ 1i + c (β 21i + ψ 65i )sθ p )(c (β 21i + ψ 65i )cθ p cψ p + s (β 21i + ψ 65i )( cψ p sγ 1i sθ p + cγ 1i sψ p )) V 31i = 1 64 (2c (2(β 21i γ 1i + ψ 65i )) + 2c (2(β 21i + γ 1i + ψ 65i )) 6c (2(β 21i θ p + ψ 65i )) + c (2(β 21i γ 1i θ p + ψ 65i )) + c (2(β 21i + γ 1i θ p + ψ 65i )) 6c (2(β 21i + θ p + ψ 65i )) + c (2(β 21i V 32i = γ 1i + θ p + ψ 65i )) + c (2(β 21i + γ 1i + θ p + ψ 65i )) 4c (2θ p )(1 + (1 + 3c (2(β 21i + ψ 65i )))c (2ψ p )) + 4c (2γ 1i )( 1 2cos 2 (ψ p )c (2θ p ) + (3 + c (2θ p )c (2(β 21i + ψ 65i )))c (2ψ p )) + 4(c (2(β 21i + ψ 65i ))(1 6c (2ψ p )cγ 2 1i ) + 4s (2(β 21i + ψ 65i ))( 2s(2ψ p )cγ 1i cθ p + c (2ψ p )s (2θ p )sγ 1i )) 4( 11 + c (2ψ p ) + s (2β 21i γ 1i 2θ p + 2ψ 65i ) s (2β 21i + γ 1i 2θ p + 2ψ 65i ) s (2β 21i γ 1i + 2(θ p + ψ 65i )) + s (2β 21i + γ 1i + 2(θ p + ψ 65i )) 8s (2γ 1i )s (β 21i + ψ 65i ) 2 s (2ψ p )sθ p )) 13

104 V 33i =.15(2c (2(β 21i γ 1i + ψ 65i )) + 2c (2(β 21i + γ 1i + ψ 65i )) 6c (2(β 21i θ p + ψ 65i )) V 34i = + c (2(β 21i γ 1i θ p + ψ 65i )) + c (2(β 21i + γ 1i θ p + ψ 65i )) 6c (2(β 21i + θ p + ψ 65i )) + c (2(β 21i γ 1i + θ p + ψ 65i )) + c (2(β 21i + γ 1i + θ p + ψ 65i )) 4c (2θ p )(1 + (1 + 3c (2(β 21i + ψ 65i )))c (2ψ p )) + 4c (2γ 1i )( 1 2cos 2 (ψ p )c (2θ p ) + (3 + c (2θ p )c (2(β 21i + ψ 65i )))c (2ψ p )) + 4(c (2(β 21i + ψ 65i ))(1 6c (2ψ p )cγ 2 1i ) + 4s (2(β 21i + ψ 65i ))( 2s(2ψ p )cγ 1i cθ p + c (2ψ p )s (2θ p )sγ 1i )) 4( 11 + c (2ψ p ) + s (2β 21i γ 1i 2θ p + 2ψ 65i ) s (2β 21i + γ 1i 2θ p + 2ψ 65i ) s (2β 21i γ 1i + 2(θ p + ψ 65i )) + s (2β 21i + γ 1i + 2(θ p + ψ 65i )) 8s (2γ 1i )s (β 21i + ψ 65i ) 2 s (2ψ p )sθ p )) Coeficientes de la ecuación 2.57 V 35i = cos2 (ψ 19i )csc (φ 111i ) 2 a 33i V 36i = V 37i = V 38i = V 39i =.125(c (γ 1211i + γ 1413i θ p φ p ψ p )s (2γ 1i )s (β 21i + ψ 65i ) 2 + 2c (γ 1211i + γ 1413i φ p )(3 + c (2(β 21i + ψ 65i )) 2c (2γ 1i )s (β 21i + ψ 65i ) 2 )cψ p + s (2γ 1i )s (β 21i + ψ 65i ) 2 (4s (γ 1211i + γ 1413i φ p )cθ p c (γ 1211i + γ 1413i θ p φ p )cψ p + (s (γ 1211i + γ 1413i θ p φ p ) 2s (γ 1211i + γ 1413i + θ p φ p ))sψ p ) + 4s (2(β 21i + ψ 65i ))cγ 1i (s (γ 1211i + γ 1413i φ p )sθ p + c (γ 1211i + γ 1413i φ p )cθ p sψ p )) V 4i = cos 2 (θ p )c(β 21i + ψ 65i ) 2 s (γ 1211i + γ 1413i φ p ) s (γ 1211i + γ 1413i φ p )s(β 21i + ψ 65i ) 2 2 cγ 1i c (γ 1211i + γ 1413i φ p )s (β 21i + ψ 65i ) 2 cγ 1i cθ p cψ p sγ 1i sin 2 (θ p )s (γ 1211i + γ 1413i φ p )s(β 21i + ψ 65i ) 2 2 sγ 1i c (γ 1211i + γ 1413i φ p )c(β 21i + ψ 65i )s(β 21i + ψ 65i )cγ 1i cψ p sθ p + s (γ 1211i + γ 1413i φ p )s (2(β 21i + ψ 65i ))cθ p sγ 1i sθ p + c (γ 1211i + γ 1413i φ p )(s (β 21i + ψ 65i )cθ p sγ 1i + c (β 21i + ψ 65i )sθ p )(c (β 21i + ψ 65i )cθ p s (β 21i + ψ 65i )sγ 1i sθ p )sψ p V 41i = (cφ p (s (β 21i + ψ 65i )( c (γ 1211i + γ 1413i )cγ 1i cψ p + s (γ 1211i + γ 1413i )cθ p sγ 1i ) + c (β 21i + ψ 65i )s (γ 1211i + γ 1413i )sθ p ) (s (γ 1211i + γ 1413i )s (β 21i + ψ 65i )cγ 1i cψ p + c (γ 1211i + γ 1413i )(s (β 21i + ψ 65i )cθ p sγ 1i + c (β 21i + ψ 65i )sθ p ))sφ p + c (γ 1211i + γ 1413i φ p )(c (β 21i + ψ 65i )cθ p s (β 21i + ψ 65i )sγ 1i sθ p )sψ p )(c (β 21i + ψ 65i )cθ p cψ p + s (β 21i + ψ 65i )( cψ p sγ 1i sθ p + cγ 1i sψ p )) V 42i = V 43i = s (γ 1211i + γ 1413i φ p )cγ 1i cθ p + c (γ 1211i + γ 1413i φ p )(cψ p sγ 1i cγ 1i sθ p sψ p ) V 44i = V 45i = cot (φ 111i )(c (γ 1211i + γ 1413i )(c (β 21i + ψ 65i )cφ p sθ p + s (β 21i + ψ 65i )(cθ p cφ p sγ 1i cγ 1i cψ p sφ p )) + s (γ 1211i + γ 1413i )(c (β 21i + ψ 65i )sθ p sφ p + s (β 21i + ψ 65i )(cγ 1i cφ p cψ p + cθ p sγ 1i sφ p )) + s (γ 1211i + γ 1413i φ p )( c (β 21i + ψ 65i )cθ p + s (β 21i + ψ 65i )sγ 1i sθ p )sψ p )(c (β 21i + ψ 65i )cθ p cψ p + s (β 21i + ψ 65i )( cψ p sγ 1i sθ p + cγ 1i sψ p )) 14

105 Coeficientes de la ecuación 2.61 cos 2 (φ 98i ) V 46i = (a 11i cφ 111i a 12i sφ 111i ) 2 V 47i = V 48i = V 49i = V 5i =.125cψ 19i (8cψ p (c(β 21i + ψ 65i )cγ 1i cφ 76i sγ 1i sφ 76i )(cφ 76i (s(β 21i + ψ 65i )cθ p + c (β 21i + ψ 65i )sγ 1i sθ p ) + cγ 1i sθ p sφ 76i ) + (5 + c (2γ 1i )(1 3c (2φ 76i )) + c (2φ 76i ) + 4c (β 21i + ψ 65i )s(2γ 1i )s(2φ 76i ) 4cos 2 (φ 76i )c(2(β 21i + ψ 65i ))cγ 2 1i )sψ p ) V 51i = cψ 19i (s (β 21i + ψ 65i )cφ 76i sθ p cθ p (c (β 21i + ψ 65i )cφ 76i sγ 1i + cγ 1i sφ 76i ))(sφ 76i (cγ 1i cψ p sθ p + sγ 1i sψ p ) + cφ 76i (s (β 21i + ψ 65i )cθ p cψ p + c (β 21i + ψ 65i )(cψ p sγ 1i sθ p cγ 1i sψ p ))) V 52i =.625cψ 19i ((8 + 4cos 2 (ψ p )c (2θ p ) c (2(θ p φ 76i )) c (2(θ p + φ 76i )) + c (2(β 21i φ 76i + ψ 65i )) + c (2(β 21i + φ 76i + ψ 65i )) + c (2(β 21i + ψ 65i ))(2 + 4cos 2 (φ 76i )c (2ψ p )) 2c (2φ 76i )(2 2 + ( 2 + c (2θ p ))c(2ψ p )))cγ 1i + cγ 1i ( 8cos 2 (ψ p )s(2θ p )s(2φ 76i )s(β 21i + ψ 65i ) + 8cos 2 (φ 76i )s(2(β 21i + ψ 65i ))s(2ψ p )cθ p ) + 2(cos 2 (θ p )cos 2 (φ 76i )( ( 4 + 2c (2γ 1i ) + c (2(β 21i γ 1i + ψ 65i )) + c (2(β 21i + γ 1i + ψ 65i )))s(γ 1211i + γ 1413i ) 2 4c (2ψ p )s(β 21i + ψ 65i ) 2 ) 4s (2φ 76i )s(β 21i + ψ 65i )s(2ψ p )cθ p sγ 1i c (γ 1211i + γ 1413i ) 2 ( 8cos 2 (ψ p )sin 2 2 (φ 76i )sγ 1i + cos 2 (φ 76i )(cos 2 (θ p )( 4 + 2c (2γ 1i ) + c (2(β 21i γ 1i + ψ 65i )) + c (2(β 21i + γ 1i + ψ 65i ))) 8sin 2 (θ p )s(β 21i + ψ 65i ) 2 + 4cos 2 (ψ p )s (2θ p )s (2(β 21i + ψ 65i ))sγ 1i )) + 4(2cos 2 (φ p )s(γ 1211i + γ 1413i ) 2 (cos 2 (φ 76i )sin 2 (θ p )s(β 21i + ψ 65i ) 2 + cos 2 (ψ p )sin 2 (φ 76i )sγ 2 1i ) + s (γ 1211i + γ 1413i ) 2 (2cos 2 (φ 76i )sin 2 (θ p )sin 2 (φ p )s(β 21i + ψ 65i ) 2 + cos 2 (ψ p )sγ 1i ( cos 2 (φ 76i )s (2θ p )s (2(β 21i + ψ 65i )) + 2sin 2 (φ 76i )sin 2 (φ p )sγ 1i )) sin 2 (φ 76i )s (2γ 1i )s (2ψ p )sθ p + c (β 21i + ψ 65i )s (2φ 76i )( cos 2 (ψ p )sin 2 (θ p )s (2γ 1i ) + sin 2 (ψ p )s (2γ 1i ) + c (2γ 1i )s (2ψ p )sθ p ) + cos 2 (φ 76i )c (β 21i + ψ 65i ) 2 sθ p (s (2γ 1i )s (2ψ p ) + 2sin 2 (ψ p )sγ 2 1i sθ p )))) V 53i = V 54i = cφ 76i cψ 19i ( cψ p (s(β 21i + ψ 65i )cθ p + c (β 21i + ψ 65i )sγ 1i sθ p )sφ 76i + cφ 76i sγ 1i sψ p + cγ 1i (cφ 76i cψ p sθ p + c (β 21i + ψ 65i )sφ 76i sψ p )) V 55i = cψ 19i (c (β 21i + ψ 65i )cθ p cψ p + s (β 21i + ψ 65i )( cψ p sγ 1i sθ p + cγ 1i sψ p )) V 56i = cψ 19i ((c (β 21i + ψ 65i )s (γ 1211i + γ 1413i φ p )cθ p cφ 76i sγ 1i s (γ 1211i + γ 1413i )s (β 21i + ψ 65i )cφ 76i cφ p sθ p + s (γ 1211i + γ 1413i )cψ p sγ 1i sφ 76i sφ p + c (γ 1211i + γ 1413i )(cφ p cψ p sγ 1i sφ 76i + s (β 21i + ψ 65i )cφ 76i sθ p sφ p ) c (γ 1211i + γ 1413i φ p )cφ 76i (s (β 21i + ψ 65i )cθ p + c (β 21i + ψ 65i )sγ 1i sθ p )sψ p cγ 1i ( s (γ 1211i + γ 1413i φ p )cθ p sφ 76i + c (γ 1211i + γ 1413i φ p )(c (β 21i + ψ 65i )cφ 76i cψ p + sθ p sφ 76i sψ p ))) 2 + (s (γ 1211i + γ 1413i )(cφ p cψ p (c (β 21i + ψ 65i )cγ 1i cφ 76i sγ 1i sφ 76i ) + s (β 21i + ψ 65i )cφ 76i ( sθ p sφ p + cθ p cφ p sψ p ) + (c (β 21i + ψ 65i )cφ 76i sγ 1i + cγ 1i sφ 76i )(cθ p sφ p + cφ p sθ p sψ p )) + c (γ 1211i + γ 1413i )(cψ p ( c (β 21i + ψ 65i )cγ 1i cφ 76i + sγ 1i sφ 76i )sφ p s (β 21i + ψ 65i )cφ 76i (cφ p sθ p + cθ p sφ p sψ p ) + (c(β 21i + ψ 65i )cφ 76i sγ 1i + cγ 1i sφ 76i )(cθ p cφ p sθ p sφ p sψ p ))) 2 ) 15

106 V 57i = sψ 19i (c (2φ 111i )( c (β 21i + ψ 65i )s (γ 1211i + γ 1413i φ p )cθ p cφ 76i sγ 1i + s (γ 1211i + γ 1413i )s (β 21i + ψ 65i )cφ 76i cφ p sθ p s (γ 1211i + γ 1413i )cψ p sγ 1i sφ 76i sφ p c (γ 1211i + γ 1413i )(cφ p cψ p sγ 1i sφ 76i + s (β 21i + ψ 65i )cφ 76i sθ p sφ p ) + c (γ 1211i + γ 1413i φ p )cφ 76i (s (β 21i + ψ 65i )cθ p + c (β 21i + ψ 65i )sγ 1i sθ p )sψ p + cγ 1i ( s (γ 1211i + γ 1413i φ p )cθ p sφ 76i + c (γ 1211i + γ 1413i φ p )(c (β 21i + ψ 65i )cφ 76i cψ p + sθ p sφ 76i sψ p )))(s (γ 1211i + γ 1413i )(cφ p cψ p (c (β 21i + ψ 65i )cγ 1i cφ 76i sγ 1i sφ 76i ) + s (β 21i + ψ 65i )cφ 76i ( sθ p sφ p + cθ p cφ p sψ p ) + (c (β 21i + ψ 65i )cφ 76i sγ 1i + cγ 1i sφ 76i )(cθ p sφ p + cφ p sθ p sψ p )) + c (γ 1211i + γ 1413i )(cψ p ( c (β 21i + ψ 65i )cγ 1i cφ 76i + sγ 1i sφ 76i )sφ p s (β 21i + ψ 65i )cφ 76i (cφ p sθ p + cθ p sφ p sψ p ) + (c (β 21i + ψ 65i )cφ 76i sγ 1i + cγ 1i sφ 76i )(cθ p cφ p sθ p sφ p sψ p ))) + cφ 111i sφ 111i ((c (β 21i + ψ 65i )s (γ 1211i + γ 1413i φ p )cθ p cφ 76i sγ 1i s (γ 1211i + γ 1413i )s (β 21i + ψ 65i )cφ 76i cφ p sθ p + s (γ 1211i + γ 1413i )cψ p sγ 1i sφ 76i sφ p + c (γ 1211i + γ 1413i )(cφ p cψ p sγ 1i sφ 76i + s (β 21i + ψ 65i )cφ 76i sθ p sφ p ) c (γ 1211i + γ 1413i φ p )cφ 76i (s (β 21i + ψ 65i )cθ p + c (β 21i + ψ 65i )sγ 1i sθ p )sψ p cγ 1i ( s (γ 1211i + γ 1413i φ p )cθ p sφ 76i + c (γ 1211i + γ 1413i φ p )(c (β 21i + ψ 65i )cφ 76i cψ p + sθ p sφ 76i sψ p ))) 2 (s (γ 1211i + γ 1413i )(cφ p cψ p (c (β 21i + ψ 65i )cγ 1i cφ 76i sγ 1i sφ 76i ) + s (β 21i + ψ 65i )cφ 76i ( sθ p sφ p + cθ p cφ p sψ p ) + (c (β 21i + ψ 65i )cφ 76i sγ 1i + cγ 1i sφ 76i )(cθ p sφ p + cφ p sθ p sψ p )) + c (γ 1211i + γ 1413i )(cψ p ( c (β 21i + ψ 65i )cγ 1i cφ 76i + sγ 1i sφ 76i )sφ p s (β 21i + ψ 65i )cφ 76i (cφ p sθ p + cθ p sφ p sψ p ) + (c (β 21i + ψ 65i )cφ 76i sγ 1i + cγ 1i sφ 76i )(cθ p cφ p sθ p sφ p sψ p ))) 2 )) 16

107 Apéndice C Coeficientes de la ecuación X.XX 2 V 58i = 2c 54i Z i + c 54i + X 2 2 i + Z i G 1i = 2( x p + x 32i cβ 21i cγ 1i + a 1312i c (γ 1413i φ p )cψ p + c 54i cγ 1i sβ 21i b 43i sγ 1i ) G 2i = 2( y p + b 43i cγ 1i a 1312i s (γ 1413i φ p )cθ p + x 32i cβ 21i sγ 1i + c 54i sβ 21i sγ 1i + a 1312i c (γ 1413i φ p )sθ p sψ p ) G 3i = 2(z p c 54i cβ 21i + x 32i sβ 21i + a 1312i s (γ 1413i φ p )sθ p + a 1312i c (γ 1413i φ p )cθ p sψ p ) G 4i = 2a 1312i s (γ 1413i φ p )(cθ p (z p c 54i cβ 21i + x 32i sβ 21i ) + ( y p + b 43i cγ 1i + x 32i cβ 21i sγ 1i + c 54i sβ 21i sγ 1i )sθ p ) + 2a 1312i c (γ 1413i φ p )(cθ p (y p b 43i cγ 1i (x 32i cβ 21i + c 54i sβ 21i )sγ 1i ) + (z p c 54i cβ 21i + x 32i sβ 21i )sθ p )sψ p G 5i = 2a 1312i c (γ 1413i φ p )(cθ p cψ p (z p c 54i cβ 21i + x 32i sβ 21i ) + cψ p ( y p + b 43i cγ 1i + x 32i cβ 21i sγ 1i + c 54i sβ 21i sγ 1i )sθ p + (x p cγ 1i (x 32i cβ 21i + c 54i sβ 21i ) + b 43i sγ 1i )sψ p ) G 6i = 2a 1312i (cφ p cψ p (x p cγ 1i (x 32i cβ 21i + c 54i sβ 21i ) + b 43i sγ 1i )sγ 1413i + sγ 1413i (cθ p (y p b 43i cγ 1i (x 32i cβ 21i + c 54i sβ 21i )sγ 1i ) + (z p c 54i cβ 21i + x 32i sβ 21i )sθ p )sφ p + cγ 1413i (cθ p cφ p (y p G 7i = b 43i cγ 1i (x 32i cβ 21i + c 54i sβ 21i )sγ 1i ) + cφ p (z p c 54i cβ 21i + x 32i sβ 21i )sθ p + cψ p ( x p + x 32i cβ 21i cγ 1i + c 54i cγ 1i sβ 21i b 43i sγ 1i )sφ p ) s (γ 1413i φ p )(cθ p (z p c 54i cβ 21i + x 32i sβ 21i ) + ( y p + b 43i cγ 1i + x 32i cβ 21i sγ 1i + c 54i sβ 21i sγ 1i )sθ p )sψ p ) 17

108 Coeficientes de la ecuación X.XX V 59i = cos2 (ψ 65i ) X 3 G 8i = X i(sγ 1i V 58i X i + (G i cβ 21i cγ 1i V 58i )(c 54i Z i )) V 58i G 9i = X i(cγ 1i V 58i X i (G 2i cβ 21i sγ 1i V 58i )(c 54i Z i )) V 58i G 1i = (G 3i + sβ 21i V 58i )X i (c 54i Z i ) V 58i G 11i = 1 V 58i X i (a 1312i V 58i (c (γ 1413i φ p )sψ p (cγ 1i cθ p X i + (cβ 21i cθ p sγ 1i sβ 21i sθ p )(c 54i Z i )) + s (γ 1413i φ p )(cγ 1i sθ p X i + (cθ p sβ 21i + cβ 21i sγ 1i sθ p )(c 54i Z i ))) + G 4i (c 54i Z i ))G 12i = 1 V 58i X1i((c 54i Z1i)G 5i + a 1312i c (γ 1413i φ p )(cψ p (X1icγ 1i sθ p + (c 54i Z1i)(cθ p sβ 21i + cβ 21i sγ 1i sθ p )) + (( c 54i + Z1i)cβ 21i cγ 1i + X1isγ 1i )sψ p )V 58i ) G 12i = 1 V 58i X i (G 5i (c 54i Z i ) + a 1312i c (γ 1413i φ p )V 58i (cψ p (cγ 1i sθ p X i + (cθ p sβ 21i + cβ 21i sγ 1i sθ p )(c 54i Z i )) + sψ p (sγ 1i X i + cβ 21i cγ 1i ( c 54i + Z i )))) G 13i = 1 V 58i X i (a 1312i V 58i ( s (γ 1413i φ p )cψ p sγ 1i X i + cγ 1i (c (γ 1413i φ p )cθ p X i + s (γ 1413i φ p )(sθ p sψ p X i + cβ 21i cψ p (c 54i Z i ))) + sβ 21i ( c (γ 1413i φ p )sθ p + s (γ 1413i φ p )cθ p sψ p )(c 54i Z i ) + cβ 21i sγ 1i (c (γ 1413i φ p )cθ p + s (γ 1413i φ p )sθ p sψ p )(c 54i Z i )) + G 6i (c 54i Z i )) G 14i = 1 V 58i (c 54i X i G 7i + 2X. iz. iv 58i X i + 2c 54i V 58i X. i 2 2V 58i Z i X. i 2 + a 1312i cγ 1i V 58i X i 2 (cθ p (2θ. pψ. pc (γ 1413i φ p )cψ p + s (γ 1413i φ p )(2θ. pφ. psψ p + θ. p 2 + φ. p 2 )) + sθ p (2φ. pψ. ps (γ 1413i φ p )cψ p c (γ 1413i φ p )(2θ. pφ. p + sψ p (θ. p 2 + φ. p 2 + ψ. p 2 )))) a 1312i cβ 21i V 58i X i (c 54i Z i )(cγ 1i (2φ. pψ. ps (γ 1413i φ p )sψ p + c (γ 1413i φ p )cψ p (φ. p 2 + ψ. p 2 )) + sγ 1i (cθ p ( 2θ. pψ. pc (γ 1413i φ p )cψ p s (γ 1413i φ p )(2θ. pφ. psψ p + θ. p 2 + φ. p 2 )) + sθ p ( 2φ. pψ. ps (γ 1413i φ p )cψ p + c (γ 1413i φ p )(2θ. pφ. p + sψ p (θ. p 2 + φ. p 2 + ψ. p 2 ))))) + 2X i Z i (x. 32i(B. 1i + A 1i x. 32i + 2A. 1ix 32i ) + x. p 2 + y. p 2 + z. p 2 a 1312i (cγ 1413i (sφ p (2φ. pcψ p ( x. p + ψ. p( y p + b 43i cγ 1i + +x 32i cβ 21i sγ 1i + c 54i sβ 21i sγ 1i )sθ p ) G 14i = +2φ. pψ. p(x p cγ 1i (x 32i cβ 21i + c 54i sβ 21i ) + b 43i sγ 1i )sψ p + sθ p ( 2y. pθ. p 2φ. p(y. p + θ. pz p c 54i θ. pcβ 21i + θ. px 32i sβ 21i )sψ p z p (θ. p 2 + φ. p 2 ) + (c 54i cβ 21i x 32i sβ 21i )(θ. p 2 + φ. p 2 ))) + cθ p (sφ p (2z. pθ. p + 2φ. p(ψ. pcψ p (z p c 54i cβ 21i + x 32i sβ 21i ) + (z. p θ. py p )sψ p ) y p (θ. p 2 + φ. p 2 ) + b 43i cγ 1i (2θ. pφ. psψ p + θ. p 2 + φ. p 2 ) + (x 32i cβ 21i + c 54i sβ 21i )sγ 1i (2θ. pφ. psψ p + θ. p 2 + φ. p 2 )) + cφ p (2φ. p(y. p + θ. pz p ) 2z. pψ. pcψ p 2θ. p( φ. px 32i sβ 21i + cβ 21i (c 54i φ. p + ψ. px 32i cψ p sγ 1i ) + ψ. pcψ p ( y p + b 43i cγ 1i + c 54i sβ 21i sγ 1i )) + sψ p (2y. pθ. p + z p (θ. p 2 + φ. p 2 + ψ. p 2 ) (c 54i cβ 21i x 32i sβ 21i )(θ. p 2 + φ. p 2 + ψ. p 2 )))) + cφ p ( 2x. pψ. psψ p + cψ p (2ψ. p(y. p + θ. pz p c 54i θ. pcβ 21i + θ. px 32i sβ 21i )sθ p (x p cγ 1i (x 32i cβ 21i + c 54i sβ 21i ) + b 43i sγ 1i )(φ. p 2 + ψ. p 2 )) + sθ p (2φ. p(z. p 18