Análisis cinemático y dinámico de una plataforma de Stewart

Equation Chapter 1 Section 1 Trabajo Fin de Grado Grado en Ingeniería Aeroespacial Análisis cinemático y dinámico de una plataforma de Stewart. Escuela Técnica Superior de Ingeniería de Sevilla Auto r: Juan Carlos Sánchez Palomino Tutor a: Juana María Mayo Nuñez Dpto. de Ingeniería Mecánica Escuela Técnica Superior de Ingeniería Universidad de Sevilla Sevilla, 2020 2 Trabajo Fin de Grado Ingeniería Aeroespacial Escuela Técnica Superior de Ingeniería de Sevilla Autor: Juan Carlos Sánchez Palomino Tutora: Juana María Mayo Núñez Catedrática de Universidad Dpto. de Ingeniería Mecánica y Fabricación Escuela Técnica Superior de Ingeniería Universidad de Sevilla Sevilla, 2020 4 Trabajo Fin de Grado: Escuela Técnica Superior de Ingeniería de Sevilla Autor: Juan Carlos Sánchez Palomino Tutora: Juana María Mayo Núñez El tribunal nombrado para juzgar el Proyecto arriba indicado, compuesto por los siguientes miembros: Presidente: Vocales: Secretario: Acuerdan otorgarle la calificación de: Sevilla, 2020 El Secretario del Tribunal 6 A Pilar y Juan Carlos, os quiero. 8 Agradecimientos Es curioso como un duro camino como este ablanda a las personas. Jamás me imaginaba abriéndome de esta manera para resaltar a todas aquellas personas que han hecho posible que hoy esté aquí, bien por su apoyo o bien por su “no eres capaz”. En primer lugar, mi infinita gratitud a mi madre, Pilar, por ser la principal fuente de amor, cariño y apoyo que siempre he tenido y que siempre tendré. Eres mi razón de ser, un referente en mi vida que me ha enseñado lo más importante: a ser humano. No existen palabras suficientes en el mundo para expresarte cuánto te quiero y cómo de necesaria eres para mí, ni existen años suficientes en un milenio para hacerte un agradecimiento justo. No me faltes nunca. Te quiero. En segundo lugar, agradecer a mi padre, Juan Carlos, su tesón, esfuerzo y sacrificio diario para que consiga mis objetivos. La vida no se te ha presentado fácil y sin embargo eso nunca te ha impedido dárnoslo todo. Por tu profesionalidad y tu buen hacer, esta victoria es tan mía como tuya. Agradecer también a mi hermana, Cristina, todo su cariño y confidencia. Fuiste una segunda madre para mí y tu orgulloso hermano te dedica sus logros por ser parte esencial de ellos. Tampoco puedo dejar de agradecer a mis amigos, mi segunda familia, todo lo que, sin ellos saberlo, han hecho por mí. Sois otra razón más por la que levantarme feliz cada día. Me gustaría hacer una mención especial a mis dos pilares, Celia y Antonio. Para toda la vida. A aquellos del “no eres capaz”: gracias también por enseñarme que vida sólo hay una, y que no podemos vivirla al gusto de los demás. Por último, gracias a mi tutora Juana por su ayuda, dedicación, paciencia y profesionalidad, virtudes que forman a una gran docente y referente en su profesión. Gracias también a la Universidad Pública, que tantos sueños hace realidad. 1 1 INTRODUCCIÓN ste proyecto tiene como fin profundizar en el estudio de la cinemática y dinámica de sistemas multicuerpos. En particular, se analizará la cinemática y la dinámica inversa, además de realizar la simulación cinemática del mecanismo de una Plataforma de Stewart. La definición académica de un sistema multicuerpo es: “una colección de sólidos rígidos o deformables conectados por pares cinemáticos (pares de revolución, prismáticos, …), suspensiones (muelles y amortiguadores) y actuadores (motores eléctricos, cilindros hidráulicos, …)”. Como se expresa anteriormente, en el presente proyecto tiene como objetivo principal el análisis cinemático y dinámico de dicha plataforma, entendiéndose el análisis cinemático como el estudio de la posición y el movimiento de sistemas, independientemente de las fuerzas que puedan actuar sobre dicho mecanismo. Por el contrario, el análisis dinámico inverso consiste en, conocido el movimiento, calcular las fuerzas que son necesarias para reproducir dicho movimiento. Se estructuran a continuación un total de 4 capítulos. En el primer capítulo se expone una introducción para poner al lector en contexto, introduciéndolo en el mundo de los manipuladores paralelos, familia a la cual pertenece la plataforma de Stewart. Se explicará en que consiste exactamente un manipulador en serie y uno paralelo, explicando las características de cada uno y las diferencias entre ellos, además de algunas aplicaciones y ventajas de uno frente a otro. Posteriormente se da paso a un capítulo extenso de fundamentos teóricos, donde el primer paso será dar una breve introducción a la plataforma de Stewart, mostrando algunas partes de su historia y origen, así como algunos ingenieros y científicos cuya aportación fue importante para su creación. Seguidamente se desarrollan dos subapartados de cinemática 2D y 3D, donde se detallan los aspectos más importantes y necesarios para conseguir la implementación de este mecanismo con éxito, mediante el uso de coordenadas de referencia, explicadas más adelante. Además, se detallará también el estudio de la dinámica inversa, tanto en 2D como en 3D, y estos conocimientos serán aplicados en el mecanismo biela manivela de ejemplo y en la propia plataforma de Stewart. Por último, se expone un tercer capítulo con los resultados obtenidos de los diferentes análisis llevados a cabo y un último capítulo con los códigos creados. 1.1 Manipuladores. 1.1.1 Manipuladores en serie y paralelos. Los robots se pueden clasificar, según su estructura, en robots tipo serie y robots o manipuladores paralelos. Los robots tipo serie están formados por una cadena cinemática abierta, similar a la del brazo humano. Por el contrario, los robots o manipuladores paralelos están compuestos por dos plataformas, una fija y otra móvil, unidas en varias cadenas cinemáticas en paralelo, y por ello, formando cadenas cinemáticas cerradas. Los robots en serie han sido utilizados a lo largo de la historia en numerosas aplicaciones, y raro es el proceso industrial automatizado que no lleve relacionado algún proceso de ensamblaje o automatización con robots en serie. Por el contrario, los robots paralelos son más modernos y usados en menor medida. El primer enfoque dado a los robots tipo serie es el de simular un brazo humano, para así automatizar con mayor facilidad tareas que anteriormente eran manuales. Estos manipuladores tienen la ventaja E Introducción 2 de tener un amplio espacio de trabajo, mientras que los robots paralelos no. Sin embargo, los robots tipo serie no son capaces de soportar altos pesos de trabajo, provocando vibraciones cuando trabajan a grandes velocidades y pesos. Por tanto, para tareas cuyo objetivo sea la precisión, rapidez y el trabajo con una carga mayor los robots paralelos resultan ideales. Los actuadores paralelos frecuentemente se sitúan en la base fija del mecanismo, y por ello, son fijos, mientras que en los robots en serie dichos actuadores se sitúan en las articulaciones del mecanismo, lo que aumenta a su vez el riesgo de sufrir averías, debido al desplazamiento de los motores y el aumento de las fuerzas de inercia. La creciente demanda de la industria a la hora de realizar tareas de mayor complejidad y flexibilidad ha ido dando lugar al estudio de mecanismos y manipuladores de más de un grado de libertad que permitan simular cualquier tipo de movimiento en el espacio. Sin ir más lejos, la plataforma de Stewart tratada en este proyecto, consta de 6 grados de libertad, lo que permite el libre movimiento de su plataforma móvil. 1.1.2 Manipuladores paralelos. Un mecanismo robótico es un sistema compuesto por diferentes solidos rígidos unidos entre sí mediante articulaciones. Cuando estos sólidos están unidos entre sí siguiendo un único camino se denominan manipuladores en serie, mientras que si estos están conectados mediante dos o más caminos se denominan en paralelo. De este modo, se puede definir un manipulador paralelo como un mecanismo conformado por una base fija y otra móvil, unidas entre sí mediante varios actuadores o cadenas cinemáticas en serie. El número máximo de grados de libertad de este tipo de manipuladores ha de ser seis ya que se necesitan tres coordenadas de translación y tres de rotación independientes para definir la posición del efector final. Un manipulador paralelo que se ha demostrado con potencial a lo largo de los últimos años es la Plataforma de Stewart: formada a partir de seis actuadores lineales que soportan una base móvil y la separan de la fija. Aunque los manipuladores paralelos son relativamente modernos, estos llevan siendo estudiados desde hace ya décadas. Su historia comienza en los años 30, concretamente en 1931, cuando Grwinnet patentó una plataforma para ubicar los asientos de un teatro, con el fin de proporcionar una sensación más realista al espectáculo; sin embargo, de acuerdo con la información disponible, esta nunca llegó a construirse. Ya en 1942, Gough diseñó y construyó un manipulador paralelo con 6 grados de libertad, utilizado por la empresa Dunlop para el ensayo de neumáticos. Tras esto, se han seguido implementando diseños de manipuladores paralelos hasta que, en 1965, Stewart patentó una plataforma de 6 grados de libertad, a la cual le dio su nombre, utilizada en principio para simuladores de vuelo[5]. 1.1.3 Aplicaciones. Dadas las ventajas económicas y de operatividad que este tipo de manipuladores tenía y sigue teniendo, no se tardó en implantar dicha idea de la plataforma de Stewart en diferentes sectores de la industria. Por ejemplo, en la empresa Volkswagen se diseñó un simulador de automóvil de 3 grados de libertad a principios de los años setenta. Posteriormente, la empresa Ford, introdujo el Virttex, con 6 grados de libertad. Posteriormente la NADS mostró en la Universidad de Iowa su simulador de 6 gdl. También la empresa Renault desarrolló un robot con 6 GDL sobre una base que se desplaza en los ejes XY. Uno de los últimos desarrollados en el ámbito industrial fue el elaborado por Toyota, con un diseño similar al de la NADS, pero más largo. [4] En el área de la manufactura y el mecanizado de piezas, se tiene el caso de robots como el Variax, VOH-1000, el robot Tornado, así como fresadoras, taladradoras y otras máquinas. 3 3 Escuela Técnica Superior de Ingeniería de Sevilla En soldadura y ensamble, destaca la empresa FANUC en la industria automotriz. En electrónica, la empresa Physik Instrumente creó el robot F-206, usado para el ensamble de dispositivos electrónicos, manipulación de semiconductores y prueba de elementos ópticos. También en medicina, particularmente en el área de cirugía. Uno de los más conocidos es el SurgiScope, que se ha usado en el Laboratorio de Robots Quirúrgicos de dicha universidad y fue desarrollado para neurocirugía. Otro robot empleado en el área de medicina es el MARS, usado como posicionador de alta precisión para hacer taladrados en cirugía a nivel intramedular. [4] Imagen 1.1: Simulador NADS, Universidad de Iowa. Todo este despliegue tecnológico también ha sido utilizado, como no, por la industria aeronáutica, donde se han creado simuladores de vuelo sofisticados basados en los mecanismos de la plataforma. Un ejemplo lo tenemos en el Airbus Military Training Center de Sevilla, donde conviven los simuladores de vuelo del A400M y el C295, dos aviones militares ensamblados también en Sevilla. 1.1.4 Ventajas y desventajas de los manipuladores paralelos frente a manipuladores en serie. Como ya se explicó anteriormente, un manipulador paralelo está compuesto por una plataforma móvil y otra fija, unidas entre sí por cadenas cinemáticas. Normalmente, estas cadenas tienen algún tipo de accionamiento (eléctrico, hidráulico…). También se puede dar el caso de que dichos accionamientos se encuentren en la base fija, todo dependiendo de la aplicación futura de dicho mecanismo. En el presente proyecto, se hará uso de una plataforma de Stewart cuyos actuadores son lineales. Una de las primeras ventajas de los manipuladores paralelos frente a los manipuladores en serie es que permiten trabajar con un mayor número de grados de libertad, normalmente 5 o 6, aunque existen también mecanismos de 3 grados de libertad para otorgarles mayor ligereza. Además, para tareas correspondientes a manipulación de objetos, los actuadores paralelos poseen una mayor precisión, velocidad y rigidez. La relación carga/peso de un manipulador paralelo es también muy superior al de un manipulador en serie. Sin embargo, también poseen fuertes desventajas. Los manipuladores paralelos poseen una cinemática compleja, unos requisitos de computación mayores y una dificultad de control añadida respecto a los manipuladores en serie. El uso de los manipuladores paralelos es relativamente moderno si lo comparamos con los manipuladores en serie, lo que implica que la tecnología de estos últimos está más desarrollada e implementada que la de los robots paralelos, los cuales requieren una mayor inversión para seguir desarrollándolos. Introducción 4 1.2 Objetivos del Proyecto. El objetivo fundamental de este proyecto es la implementación y modelado de una plataforma de Stewart con actuadores lineales mediante un sistema de referencia definido previamente, y su posterior análisis cinemático y dinámico. Para entender los fundamentos de la cinemática y dinámica 3D es necesario tener destreza en el estudio cinemático y dinámico 2D utilizando coordenadas de referencia. Para ello se realizarán cálculos y análisis a un mecanismo simple de biela manivela, con el objetivo de profundizar en la cinemática y dinámica de sistemas multicuerpos. 5 2 FUNDAMENTOS TEÓRICOS n este apartado se desarrollan todos los fundamentos teóricos necesarios y utilizados para la realización del presente proyecto, desde un breve recorrido por la historia de la Plataforma de Stewart, su diseño conceptual, aplicaciones que actualmente tiene, pasando por la definición del modelo. Por último, se explicarán los fundamentos teóricos en los que se ha basado dicho proyecto en cuanto a análisis computacional: se definirá los diferentes pares que este mecanismo posee y cómo se modelan dichos pares en función del sistema de referencia escogido. 2.1. Plataforma de Stewart. 2.1.1 Historia de la Plataforma de Stewart Como se ha explicado en anteriores apartados, la plataforma de Stewart es un mecanismo de 6 grados de libertad compuesto por dos plataformas, una móvil y otra fija, unidas entre sí por 6 actuadores que forman entre ellos una estructura triangular, y que están unidos a las plataformas mediante pares Cardan. Para mayor claridad, se muestra la figura a continuación. Imagen 2.1: Plataforma de Stewart. Imagen extraída de Wikipedia. Este mecanismo resulta ideal a la hora de intentar imitar movimientos de diferentes transportes, por ejemplo, un avión o un automóvil ya que los actuadores, dependiendo de cómo se extiendan y contraigan, pueden otorgar cualquier posición a la plataforma móvil, situada en la parte superior. En 1965, Stewart publicó un documento en el cual describía un mecanismo capaz de realizar simulaciones de vuelo para el entreno de los pilotos. Como se nombra en capítulos anteriores, Stewart no es el pionero en manipuladores paralelos. Por ejemplo, Gaugh ya presentó uno en 1947, y fue este en persona uno de los evaluadores del artículo de Stewart. E “Aquel que ama la práctica sin la teoría es como un marinero que navega sin timón ni brújula y nunca sabe a dónde ir.” Leonardo Da Vinci. Fundamentos teóricos 6 Tampoco se considera que Gaugh sea el pionero en este tipo de mecanismos. Dicha invención se otorga a Cauchy, quien hablaba de un “octaedro articulado” [5]. 2.1.2 Diseño conceptual de la Plataforma de Stewart. La plataforma de Stewart puede adoptar diferentes formas dependiendo de la función que esta vaya a desempeñar. Entran en juego aspectos como con qué cargas se va a trabajar, cual es el espacio de trabajo necesario, qué grado de rigidez demanda, la precisión requerida… Dependiendo de todos estos aspectos la geometría de dicha plataforma adopta configuraciones de lo más diversas. Este diseño podría complicarse tanto como se quisiese, pero ya que este documento tiene principalmente un fin académico se opta por realizar una serie de simplificaciones, de manera que facilite su futura implementación numérica, así como su modelaje. Se opta así por una plataforma de Stewart compuesta por dos bases circulares, cuyos eslabones se distribuyen equidistantes en su borde, separados entre sí 120 grados, y desfasados entre ambas plataformas 60 grados. Por lo tanto, los eslabones que se encuentran más cerca están separados un ángulo de 30º, mientras que entre los eslabones consecutivos que están más separados existen 90º de ángulo. Así, entre eslabones no consecutivos existe un ángulo total de 120º. Por último, la plataforma móvil será idéntica a la fija, pero desfasada con respecto a esta un ángulo de 60º, lo que provocará que las barras se encuentren inclinadas. Una vez explicada la configuración geométrica de la plataforma, se pasa al desarrollo de los fundamentos teóricos de cinemática 2D necesarios para poder entender la implementación cinemática de cualquier mecanismo cinemático. Se comienza explicando los diferentes sistemas de referencias con los que se puede trabajar, así como los pares de restricción existentes y como modelar estas restricciones. Posteriormente se explican cómo resolver el problema de posición, velocidad y aceleración de manera numérica. Una vez explicada y desarrollada la cinemática 2D, se expondrá la analogía en mecanismos 3D, destacando aquellas peculiaridades y herramientas que sólo se usan a la hora de la cinemática 3D. Por último, para cada caso se hará un ejemplo. Para el 2D se realizará un ejemplo de un mecanismo biela-manivela y para el 3D, la plataforma de Stewart. Cabe destacar que se explicará cómo modelar las ecuaciones de restricción de los pares involucrados en dichos ejemplos. 2.2. Cinemática 2D El primer paso realizado en este proyecto es entender profundamente los conceptos del análisis cinemático de sistemas multicuerpos en 2D, es decir, movimiento plano. [3] La primera decisión que se tuvo que tomar al comenzar el proyecto fue decidir qué sistema de referencia sería el idóneo para la simulación cinemática de una Plataforma de Stewart. Para ello, se exponen más adelante y brevemente las opciones planteadas. Dichas coordenadas serán elegidas no de manera arbitraria, sino que tendrán que estar establecidas de una manera concreta para que posteriormente satisfagan las ecuaciones de restricción. Es por esto que se dice que dichas coordenadas son dependientes. Además de dichas coordenadas existen otras magnitudes denominadas parámetros, que se mantienen constantes a diferencia de las coordenadas, que van variando durante el movimiento del mecanismo. Ejemplos de parámetros son, por ejemplo, las longitudes de las barras en un mecanismo. 7 Escuela Técnica Superior de Ingeniería de Sevilla Una vez explicado esto, se procede a exponer los diferentes sistemas de coordenadas planteados. 2.2.1 Selección de coordenadas. 2.2.1.1 Coordenadas Lagrangianas. Las coordenadas lagrangianas son un conjunto de magnitudes, distancias o ángulos, que indican la posición y orientación de cada uno de los sólidos con respecto a la barra fija. En este sentido, se dicen que son coordenadas absolutas. Dicho conjunto de coordenadas se agrupa en una matriz columna denominada q. Ejemplo: Sea un mecanismo biela-manivela definido por sus ángulos 𝜑𝜑,𝜃𝜃 𝑦𝑦 𝑠𝑠 según se muestra en la siguiente figura: Imagen 2.2: Mecanismo biela-manivela con coordenadas Lagrangianas. Imagen extraída de [3]. El conjunto de coordenadas quedaría definido como: 𝐪𝐪=�φ θ s� Estas coordenadas deben cumplir las condiciones de la Teoría de Máquinas para poder establecer las ecuaciones de lazo, que son las siguientes: l2cos φ+ l3cos θ−s = 0 ( 1) l 2 sen φ+ l 3 sen θ= 0 ( 2) 2.2.1.2 Coordenadas relativas. Las coordenadas relativas son coordenadas más libres de escoger en el caso de mecanismos de Fundamentos teóricos 8 cadena abierta. Por el contrario, esto produce que la definición de las ecuaciones de restricción sea más compleja. En el ejemplo anterior escogemos estas coordenadas: Imagen 2.3: Mecanismo biela-manivela con coordenadas relativas. Imagen extraída de [3]. En este caso tenemos cuatro coordenadas definidas porque en este tipo de coordenadas se escoge una coordenada por cada grado de libertad relativo que cada sólido tiene con el anterior, considerando la barra fija como parte del mecanismo. Por tanto, dado que el número de grados de libertad está definido y es 1, el número de ecuaciones de restricción necesariamente ha de ser 3. Las dos primeras definen la misma condición que en el apartado anterior, mientras que la tercera expresa que la suma de los tres ángulos ha de ser 360º: l2cos φ+ l3cos(φ+α)−s = 0 ( 3) l2sin φ+ l3cos(φ+α)= 0 ( 4) φ+α+β−2π= 0 ( 5) Este tipo de coordenadas son especialmente útiles cuando se trabaja con mecanismos robots de cadena abierta. 2.2.1.3 Coordenadas de referencia. Este tipo de coordenadas son las que se utilizarán en el presente proyecto debido a que son las más sistemáticas de implementar numéricamente a la hora de modelar un sistema multicuerpo. Utilizando el mismo ejemplo anterior, en este caso sus coordenadas serían las presentadas en la siguiente figura: 9 Escuela Técnica Superior de Ingeniería de Sevilla Imagen 2.4: Mecanismo biela-manivela en coordenadas de referencia. Imagen extraída de [3]. Como se aprecia en la imagen, este mecanismo consta de un sistema de referencia global y tres sistemas de referencia locales, uno para cada barra móvil. Las coordenadas de referencia son 3: x, y el ángulo, medido en sentido anti horario, desde la horizontal hasta el eje x, como muestra la imagen 2.4. En el caso tridimensional se contará no con 3, sino mínimo con 6 coordenadas. Existen coordenadas (como el ángulo de la corredera) cuyos valores no varían, pero esto se hace así para que la tarea de implementación numérica sea sistemática. Por tanto, tendríamos en total 3*n coordenadas, siendo n el número de barras móviles, y como este mecanismo es de un grado de libertad serán necesarias 8 ecuaciones de restricción. Claramente, este tipo de coordenadas aumenta considerablemente el número de restricciones a imponer para un mecanismo, por lo que resulta tedioso para resolver a mano. Sin embargo, a la hora del modelado computacional de sistemas multicuerpos esto permite una sistematización que hace posible el estudio de mecanismos más complejos. Por último, es esencial definir la diferencia que existe entre el sistema de referencia local y el global. Todas las coordenadas q de un sólido deberán estar según el sistema de referencia global establecido, de modo que para pasar del sistema de referencia local de cada barra al global es necesario una matriz de rotación que relacione ambos sistemas de coordenadas. En el caso 2D esta matriz de rotación es la siguiente: 𝐀𝐀=�cos (θ) −sen(θ) sen(θ) cos (θ)� Siendo 𝜃𝜃 el ángulo que forman ambos sistemas de referencia, medido desde el eje X del sistema global al eje x del sistema local en sentido antihorario. 2.2.1.4 Coordenadas naturales. Las coordenadas naturales constituyen una herramienta que se encuentra a medio camino entre las relativas y las de referencia. Por tanto, nos permiten ser más sistemáticos que con las coordenadas relativas, pero menos que con las de referencia. En el ejemplo de la biela-manivela, un sistema de coordenadas útil sería: Fundamentos teóricos 16 y el vector v es el vector unitario que indica la dirección del eje de giro. La matriz de rotación de estos parámetros es: 𝐀𝐀=𝐈𝐈+2𝛉𝛉(𝐈𝐈θ0+𝛉𝛉) ( 21) Con 𝛉𝛉: 𝛉𝛉= ⎣ ⎢ ⎡ 0 θ3−θ3θ2 0 −θ1 −θ2θ10 ⎦ ⎥ ⎤ Los parámetros de Euler añaden lo que se llaman restricciones cinemáticas de orientación, que para este caso concreto no es más que imponer que este vector tiene módulo unidad: 𝛉𝛉T𝛉𝛉−1=0 ( 22) Cada sólido tendrá asociado, además de sus tres coordenadas cartesianas, sus 4 parámetros de Euler. En principio puede parecer que tendremos tantas restricciones de orientación como sólidos tenga el mecanismo, pero esto no será así, ya que numéricamente se le da un trato especial a la barra fija, como se detallará en capítulos posteriores. Una vez definidos los parámetros de Euler, se sigue con la explicación de las restricciones debidas a pares Cardan y cilíndricos, que son los dos pares que tendrá el mecanismo de este proyecto. 2.3.2 Ecuaciones de restricción debidas a un par Cardan. En la siguiente figura se muestra un par Cardan, también llamado junta universal. Este par permite dos giros, restringiendo los otros 4 grados de libertad. Para establecer las ecuaciones de restricción, obsérvese la siguiente figura, extraída del libro “Computer-Aided Analysis of Mechanical Systems”, de Parviz E. Nikravesh: Imagen 2.8: Representación general de un par Cardan o junta universal. Imagen extraída de [2]. 17 Escuela Técnica Superior de Ingeniería de Sevilla Como se puede observar, se tienen que definir dos vectores 𝐬𝐬𝐢𝐢 y 𝐬𝐬𝐣𝐣 perpendiculares entre sí, cada uno en la dirección de una de las crucetas del par, así como dos vectores 𝐮𝐮𝐩𝐩 𝒊𝒊 y 𝐮𝐮𝐩𝐩 𝒋𝒋 (en la figura 𝐬𝐬𝐢𝐢𝑷𝑷 y 𝐬𝐬𝐣𝐣𝑷𝑷) que van desde el origen de coordenadas hasta el punto P común a ambos sólidos. Debido a que este par permite sólo dos giros, serán necesarias cuatro ecuaciones de restricción, que corresponden a: 1) Imponer que el punto P pertenece permanentemente a ambos sólidos (3 ecuaciones) 2) Imponer que los vectores s permanecen perpendiculares entre sí siempre. Estos dos requerimientos se traducen matemáticamente como: 𝐫𝐫𝐢𝐢+𝐀𝐀𝐢𝐢𝐮𝐮𝐩𝐩 𝐢𝐢=𝐫𝐫𝐣𝐣+𝐀𝐀𝐣𝐣𝐮𝐮𝐩𝐩 𝐣𝐣 ( 23) (𝐀𝐀𝐢𝐢𝒔𝒔𝒊𝒊)𝑇𝑇�𝑨𝑨𝒋𝒋𝒔𝒔𝒋𝒋�=𝟎𝟎 ( 24) Siendo r los vectores correspondientes a las tres coordenadas cartesianas en globales de cada sólido i y j. Como en cinemática 2D, los vectores u quedan vistos desde cada sistema de referencia local de cada sólido, igual que los vectores s. 2.3.3 Ecuaciones de restricción debidas a un par cilíndrico. El par cilíndrico permite una tanslación relativa entre los dos sólidos a través de una línea de deslizamiento, y un giro alrededor de este eje, tal y como se muestra en la figura: Imagen 2.9: Representación general de un par cilíndrico. Imagen extraída de [3]. Dicho par restringe cuatro grados de libertad, por tanto, se necesitan cuatro ecuaciones de restricción. Se introducen los vectores v, que tienen módulo unidad y ambos la misma dirección del eje de deslizamiento, y los vectores u, que indican la posición de los puntos P y Q en locales. Dichos puntos pertenecen cada uno a un sólido diferente y deben estar alineados en el eje de giro del par. Las condiciones que hay que imponer en este caso son: 1) Los vectores v son paralelos entre sí. 2) El vector que conecta P y Q también es paralelo a v. Para establecer estas condiciones de paralelismos, se recurre a imponer una doble perpendicularidad. Para ello, se crean dos vectores 𝐯𝐯𝟏𝟏 𝐢𝐢 y 𝐯𝐯𝟐𝟐 𝐢𝐢 de la siguiente manera: Fundamentos teóricos 18 𝐯𝐯𝟏𝟏 𝐢𝐢=�1 0 0�x 𝐯𝐯𝐢𝐢 ,𝐯𝐯𝟐𝟐 𝒊𝒊=𝐯𝐯𝐢𝐢 𝐱𝐱 𝐯𝐯𝟏𝟏 𝐢𝐢 Si el vector 𝐯𝐯𝐢𝐢 estuviera en la dirección del eje x se escoge otro vector para evitar que el producto vectorial sea nulo. Las restricciones se expresan matemáticamente como: (𝐀𝐀𝐢𝐢𝐯𝐯𝟏𝟏 𝐢𝐢)𝑻𝑻𝐀𝐀𝐣𝐣𝐯𝐯𝐣𝐣=0 ( 25) (𝐀𝐀𝐢𝐢𝐯𝐯𝟐𝟐 𝐢𝐢)𝑻𝑻𝐀𝐀𝐣𝐣𝐯𝐯𝐣𝐣=0 ( 26) (𝐀𝐀𝐢𝐢𝐯𝐯𝟏𝟏 𝐢𝐢)𝐓𝐓(𝐫𝐫𝐢𝐢+𝐀𝐀𝐢𝐢𝐮𝐮𝐏𝐏 𝐢𝐢−𝐫𝐫𝐣𝐣−𝐀𝐀𝐣𝐣𝐮𝐮𝐐𝐐 𝐣𝐣)=0 ( 27) (𝐀𝐀𝐢𝐢𝐯𝐯𝟐𝟐 𝐢𝐢)𝐓𝐓�𝐫𝐫𝐢𝐢+𝐀𝐀𝐢𝐢𝐮𝐮𝐏𝐏 𝐢𝐢−𝐫𝐫𝐣𝐣−𝐀𝐀𝐣𝐣𝐮𝐮𝐐𝐐 𝐣𝐣�=0 ( 28) Se tratan de cuatro ecuaciones algebraicas de restricción. 2.3.4 Jacobianos del par Cardan y cilíndricos. Jacobiano de las restricciones de orientación. Hallar los jacobianos de las ecuaciones de restricción de ambos pares en mecanismos 3D es completamente análogo al 2D, pero más laborioso debido al aumento de coordenadas. A continuación, se expresan las submatrices de cada sólido, tal y como se expusieron en el 2D. Estas submatrices deberán incorporarse correspondientemente en el jacobiano, en la fila correspondiente a la ecuación de restricción que estemos tratando y en la columna correspondiente a la variable con respecto a la cual estamos derivando. La submatriz del jacobiano debido a un par Cardan para el sólido i es: ⎣ ⎢ ⎢ ⎡ 100 010 001 𝐀𝐀𝛉𝛉𝟎𝟎 𝐢𝐢𝐮𝐮𝐏𝐏 𝐢𝐢 𝐀𝐀𝛉𝛉𝟏𝟏 𝐢𝐢𝐮𝐮𝐏𝐏 𝐢𝐢 𝐀𝐀𝛉𝛉𝟐𝟐 𝐢𝐢𝐮𝐮𝐏𝐏 𝐢𝐢 𝐀𝐀𝛉𝛉𝟑𝟑 𝐢𝐢𝐮𝐮𝐏𝐏 𝐢𝐢 000�𝐀𝐀𝛉𝛉𝟎𝟎 𝐢𝐢𝐬𝐬𝐢𝐢�𝐓𝐓�𝐀𝐀𝐣𝐣𝐬𝐬𝐢𝐢� �𝐀𝐀𝛉𝛉𝟏𝟏 𝐢𝐢𝐬𝐬𝐢𝐢�𝐓𝐓�𝐀𝐀𝐣𝐣𝐬𝐬𝐢𝐢��𝐀𝐀𝛉𝛉𝟐𝟐 𝐢𝐢𝐬𝐬𝐢𝐢�𝐓𝐓�𝐀𝐀𝐣𝐣𝐬𝐬𝐢𝐢� �𝐀𝐀𝛉𝛉𝟑𝟑 𝐢𝐢𝐬𝐬𝐢𝐢�𝐓𝐓�𝐀𝐀𝐣𝐣𝐬𝐬𝐢𝐢� ⎦ ⎥ ⎥ ⎤ Y para el sólido j: ⎣ ⎢ ⎢ ⎡ −1 0 0 0 −1 0 0 0 −1 −𝐀𝐀𝛉𝛉𝟎𝟎 𝐣𝐣𝐮𝐮𝐏𝐏 𝐣𝐣 −𝐀𝐀𝛉𝛉𝟏𝟏 𝐣𝐣𝐮𝐮𝐏𝐏 𝐣𝐣 −𝐀𝐀𝛉𝛉𝟐𝟐 𝐣𝐣𝐮𝐮𝐏𝐏 𝐣𝐣 −𝐀𝐀𝛉𝛉𝟑𝟑 𝐣𝐣𝐮𝐮𝐏𝐏 𝐣𝐣 0 0 0 (𝐀𝐀𝐢𝐢𝐬𝐬𝐢𝐢)𝐓𝐓�𝐀𝐀𝛉𝛉𝟎𝟎 𝐣𝐣𝐬𝐬𝐢𝐢� (𝐀𝐀𝐢𝐢𝐬𝐬𝐢𝐢)𝐓𝐓�𝐀𝐀𝛉𝛉𝟏𝟏 𝐣𝐣𝐬𝐬𝐢𝐢�(𝐀𝐀𝐢𝐢𝐬𝐬𝐢𝐢)𝐓𝐓�𝐀𝐀𝛉𝛉𝟐𝟐 𝐣𝐣𝐬𝐬𝐢𝐢� (𝐀𝐀𝐢𝐢𝐬𝐬𝐢𝐢)𝐓𝐓�𝐀𝐀𝛉𝛉𝟑𝟑 𝐣𝐣𝐬𝐬𝐢𝐢� ⎦ ⎥ ⎥ ⎤ Cada submatriz es de 4x7, correspondientes a las cuatro ecuaciones de restricción y a las siete coordenadas de cada sólido. Las matrices 𝑨𝑨𝜽𝜽 corresponden a las matrices cuyos componentes son 19 Escuela Técnica Superior de Ingeniería de Sevilla las derivadas parciales con respecto al parámetro de Euler que corresponda. La implementación de la matriz jacobiana debida al par cilíndrico resulta algo más laboriosa, debido a que algunas derivadas parciales requieren aplicar la regla de la cadena, lo que hace que dicha expresión se alargue. Para el sólido i se tiene: ⎣ ⎢ ⎢ ⎢ ⎢ ⎡ 000 000�𝐀𝐀𝛉𝛉𝟎𝟎 i𝐯𝐯𝟏𝟏 i�T𝐀𝐀𝐣𝐣𝐯𝐯𝐣𝐣�𝐀𝐀𝛉𝛉𝟏𝟏 i𝐯𝐯𝟏𝟏 i�T𝐀𝐀𝐣𝐣𝐯𝐯𝐣𝐣�𝐀𝐀𝛉𝛉𝟐𝟐 i𝐯𝐯𝟏𝟏 i�T𝐀𝐀𝐣𝐣𝐯𝐯𝐣𝐣�𝐀𝐀𝛉𝛉𝟑𝟑 i𝐯𝐯𝟏𝟏 i�T𝐀𝐀𝐣𝐣𝐯𝐯𝐣𝐣 �𝐀𝐀𝛉𝛉𝟎𝟎 i𝐯𝐯𝟐𝟐 i�T𝐀𝐀𝐣𝐣𝐯𝐯𝐣𝐣�𝐀𝐀𝛉𝛉𝟏𝟏 i𝐯𝐯𝟐𝟐 i�T𝐀𝐀𝐣𝐣𝐯𝐯𝐣𝐣�𝐀𝐀𝛉𝛉𝟐𝟐 i𝐯𝐯𝟐𝟐 i�T𝐀𝐀𝐣𝐣𝐯𝐯𝐣𝐣�𝐀𝐀𝛉𝛉𝟑𝟑 i𝐯𝐯𝟐𝟐 i�T𝐀𝐀𝐣𝐣𝐯𝐯𝐣𝐣 (𝐀𝐀𝐢𝐢𝐯𝐯𝟏𝟏 𝐢𝐢)T (𝐀𝐀𝐢𝐢𝐯𝐯𝟐𝟐 𝐢𝐢)T𝐃𝐃𝐪𝐪,𝟏𝟏 𝛉𝛉𝟎𝟎 𝐃𝐃𝐪𝐪,𝟏𝟏 𝛉𝛉𝟏𝟏 𝐃𝐃𝐪𝐪,𝟏𝟏 𝛉𝛉𝟐𝟐 𝐃𝐃𝐪𝐪,𝟏𝟏 𝛉𝛉𝟑𝟑 𝐃𝐃𝐪𝐪,𝟐𝟐 𝛉𝛉𝟎𝟎 𝐃𝐃𝐪𝐪,𝟐𝟐 𝛉𝛉𝟏𝟏 𝐃𝐃𝐪𝐪,𝟐𝟐 𝛉𝛉𝟐𝟐 𝐃𝐃𝐪𝐪,𝟐𝟐 𝛉𝛉𝟑𝟑 ⎦ ⎥ ⎥ ⎥ ⎥ ⎤ Donde: 𝐃𝐃𝐪𝐪,𝟏𝟏 𝛉𝛉 𝐤𝐤 =�𝐀𝐀𝛉𝛉𝐤𝐤𝐯𝐯𝟏𝟏 i�T�𝐫𝐫𝐢𝐢+𝐀𝐀𝐢𝐢𝐮𝐮𝐏𝐏 i−𝐫𝐫𝐣𝐣−𝐀𝐀𝐣𝐣𝐮𝐮𝐐𝐐 j�+(𝐀𝐀𝐢𝐢𝐯𝐯𝟏𝟏 i)T(𝐀𝐀𝛉𝛉𝐤𝐤 i𝐮𝐮𝐏𝐏 i) ( 29) 𝐃𝐃𝐪𝐪,𝟐𝟐 𝛉𝛉𝐤𝐤=�𝐀𝐀𝛉𝛉 𝐤𝐤 𝐯𝐯𝟐𝟐 i�T�𝐫𝐫𝐢𝐢+𝐀𝐀𝐢𝐢𝐮𝐮𝐏𝐏 i−𝐫𝐫𝐣𝐣−𝐀𝐀𝐣𝐣𝐮𝐮𝐐𝐐 j�+(𝐀𝐀𝐢𝐢𝐯𝐯𝟐𝟐 i)T(𝐀𝐀𝛉𝛉 𝐤𝐤 i𝐮𝐮𝐏𝐏 i) ( 30) Y, por último, para el sólido j: ⎣ ⎢ ⎢ ⎢ ⎢ ⎡ 000 000(𝐀𝐀𝐢𝐢𝐯𝐯𝟏𝟏 i)T𝐀𝐀𝛉𝛉𝟎𝟎 𝐣𝐣𝐯𝐯𝐣𝐣(𝐀𝐀𝐢𝐢𝐯𝐯𝟏𝟏 i)T𝐀𝐀𝛉𝛉𝟏𝟏 𝐣𝐣𝐯𝐯𝐣𝐣(𝐀𝐀𝐢𝐢𝐯𝐯𝟏𝟏 i)T𝐀𝐀𝛉𝛉𝟐𝟐 𝐣𝐣𝐯𝐯𝐣𝐣(𝐀𝐀𝐢𝐢𝐯𝐯𝟏𝟏 i)T𝐀𝐀𝛉𝛉𝟑𝟑 𝐣𝐣𝐯𝐯𝐣𝐣 (𝐀𝐀𝐢𝐢𝐯𝐯𝟐𝟐 i)T𝐀𝐀𝛉𝛉𝟎𝟎 𝐣𝐣𝐯𝐯𝐣𝐣(𝐀𝐀𝐢𝐢𝐯𝐯𝟐𝟐 i)T𝐀𝐀𝛉𝛉𝟏𝟏 𝐣𝐣𝐯𝐯𝐣𝐣(𝐀𝐀𝐢𝐢𝐯𝐯𝟐𝟐 i)T𝐀𝐀𝛉𝛉𝟐𝟐 𝐣𝐣𝐯𝐯𝐣𝐣(𝐀𝐀𝐢𝐢𝐯𝐯𝟐𝟐 i)T𝐀𝐀𝛉𝛉𝟑𝟑 𝐣𝐣𝐯𝐯𝐣𝐣 (𝐀𝐀𝐢𝐢𝐯𝐯𝟏𝟏 𝐢𝐢)T (𝐀𝐀𝐢𝐢𝐯𝐯𝟐𝟐 𝐢𝐢)T𝐃𝐃𝐪𝐪,𝟏𝟏 𝛉𝛉𝟎𝟎 𝐃𝐃𝐪𝐪,𝟏𝟏 𝛉𝛉𝟏𝟏 𝐃𝐃𝐪𝐪,𝟏𝟏 𝛉𝛉𝟐𝟐 𝐃𝐃𝐪𝐪,𝟏𝟏 𝛉𝛉𝟑𝟑 𝐃𝐃𝐪𝐪,𝟐𝟐 𝛉𝛉𝟎𝟎 𝐃𝐃𝐪𝐪,𝟐𝟐 𝛉𝛉𝟏𝟏 𝐃𝐃𝐪𝐪,𝟐𝟐 𝛉𝛉𝟐𝟐 𝐃𝐃𝐪𝐪,𝟐𝟐 𝛉𝛉𝟑𝟑 ⎦ ⎥ ⎥ ⎥ ⎥ ⎤ Donde: 𝐃𝐃𝐪𝐪,𝟏𝟏 𝛉𝛉𝐤𝐤=−(𝐀𝐀𝐢𝐢𝐯𝐯𝟏𝟏 i)T(𝐀𝐀𝛉𝛉 𝐤𝐤 j𝐮𝐮𝐐𝐐 j) ( 31) 𝐃𝐃𝐪𝐪,𝟐𝟐 𝛉𝛉𝐤𝐤=−(𝐀𝐀𝐢𝐢𝐯𝐯𝟐𝟐 i)T(𝐀𝐀𝛉𝛉 𝐤𝐤 j𝐮𝐮𝐐𝐐 j) ( 32) Ya se tienen, por tanto, los jacobianos debidos a las ecuaciones de restricción cinemáticas. Faltarían por añadir la parte del jacobiano correspondiente a las restricciones de orientación y las restricciones de movilidad que el usuario impone según el movimiento que quiera simular en el mecanismo, por lo que estas últimas pueden tener cualquier forma. En el caso del presente proyecto, y debido al uso de los parámetros de Euler, las restricciones de orientación que se tienen son todas de la forma 𝛉𝛉𝐓𝐓𝛉𝛉= 1. La fila correspondiente a dicha restricción tendrá la siguiente forma: [000 2θ02θ12θ22θ3] Fundamentos teóricos 20 2.3.5 Matriz de la derivada del jacobiano 𝐃𝐃𝐃𝐃𝐪𝐪. Para poder resolver el problema de aceleración se procede igual que en el caso del 2D. La matriz de la derivada del jacobiano debido al par Cardan, para el sólido i, es: ⎣ ⎢ ⎢ ⎡ 000 000 000 𝐀𝐀𝛉𝛉𝟎𝟎𝟎𝟎 𝐢𝐢𝐮𝐮𝐏𝐏 𝐢𝐢 𝐀𝐀𝛉𝛉𝟏𝟏𝟏𝟏 𝐢𝐢𝐮𝐮𝐏𝐏 𝐢𝐢 𝐀𝐀𝛉𝛉𝟐𝟐𝟐𝟐 𝐢𝐢𝐮𝐮𝐏𝐏 𝐢𝐢 𝐀𝐀𝛉𝛉𝟑𝟑𝟑𝟑 𝐢𝐢𝐮𝐮𝐏𝐏 𝐢𝐢 000�𝐀𝐀𝛉𝛉𝟎𝟎𝟎𝟎 𝐢𝐢𝐬𝐬𝐢𝐢�𝐓𝐓�𝐀𝐀𝐣𝐣𝐬𝐬𝐢𝐢� �𝐀𝐀𝛉𝛉𝟏𝟏𝟏𝟏 𝐢𝐢𝐬𝐬𝐢𝐢�𝐓𝐓�𝐀𝐀𝐣𝐣𝐬𝐬𝐢𝐢��𝐀𝐀𝛉𝛉𝟐𝟐𝟐𝟐 𝐢𝐢𝐬𝐬𝐢𝐢�𝐓𝐓�𝐀𝐀𝐣𝐣𝐬𝐬𝐢𝐢� �𝐀𝐀𝛉𝛉𝟑𝟑𝟑𝟑 𝐢𝐢𝐬𝐬𝐢𝐢�𝐓𝐓�𝐀𝐀𝐣𝐣𝐬𝐬𝐢𝐢� ⎦ ⎥ ⎥ ⎤ Donde 𝐀𝐀𝛉𝛉𝐤𝐤𝐤𝐤es la derivada con respecto al parámetro de Euler θk de la matriz 𝐀𝐀𝛉𝛉𝐤𝐤 Y para el sólido j: ⎣ ⎢ ⎢ ⎡ 000 000 000−𝐀𝐀𝛉𝛉𝟎𝟎𝟎𝟎 𝐣𝐣𝐮𝐮𝐏𝐏 𝐣𝐣 −𝐀𝐀𝛉𝛉𝟏𝟏𝟏𝟏 𝐣𝐣𝐮𝐮𝐏𝐏 𝐣𝐣 −𝐀𝐀𝛉𝛉𝟐𝟐𝟐𝟐 𝐣𝐣𝐮𝐮𝐏𝐏 𝐣𝐣 −𝐀𝐀𝛉𝛉𝟑𝟑𝟑𝟑 𝐣𝐣𝐮𝐮𝐏𝐏 𝐣𝐣 000 (𝐀𝐀𝐢𝐢𝐬𝐬𝐢𝐢)𝐓𝐓�𝐀𝐀𝛉𝛉𝟎𝟎𝟎𝟎 𝐣𝐣𝐬𝐬𝐢𝐢� (𝐀𝐀𝐢𝐢𝐬𝐬𝐢𝐢)𝐓𝐓�𝐀𝐀𝛉𝛉𝟏𝟏𝟏𝟏 𝐣𝐣𝐬𝐬𝐢𝐢�(𝐀𝐀𝐢𝐢𝐬𝐬𝐢𝐢)𝐓𝐓�𝐀𝐀𝛉𝛉𝟐𝟐𝟐𝟐 𝐣𝐣𝐬𝐬𝐢𝐢� (𝐀𝐀𝐢𝐢𝐬𝐬𝐢𝐢)𝐓𝐓�𝐀𝐀𝛉𝛉𝟑𝟑𝟑𝟑 𝐣𝐣𝐬𝐬𝐢𝐢� ⎦ ⎥ ⎥ ⎤ Con respecto al par prismático, se tiene, para el sólido i: ⎣ ⎢ ⎢ ⎢ ⎢ ⎡ 000 000�𝐀𝐀𝛉𝛉𝟎𝟎𝟎𝟎 i𝐯𝐯𝟏𝟏 i�T𝐀𝐀𝐣𝐣𝐯𝐯𝐣𝐣�𝐀𝐀𝛉𝛉𝟏𝟏𝟏𝟏 i𝐯𝐯𝟏𝟏 i�T𝐀𝐀𝐣𝐣𝐯𝐯𝐣𝐣�𝐀𝐀𝛉𝛉𝟐𝟐𝟐𝟐 i𝐯𝐯𝟏𝟏 i�T𝐀𝐀𝐣𝐣𝐯𝐯𝐣𝐣�𝐀𝐀𝛉𝛉𝟑𝟑𝟑𝟑 i𝐯𝐯𝟏𝟏 i�T𝐀𝐀𝐣𝐣𝐯𝐯𝐣𝐣 �𝐀𝐀𝛉𝛉𝟎𝟎𝟎𝟎 i𝐯𝐯𝟐𝟐 i�T𝐀𝐀𝐣𝐣𝐯𝐯𝐣𝐣�𝐀𝐀𝛉𝛉𝟏𝟏𝟏𝟏 i𝐯𝐯𝟐𝟐 i�T𝐀𝐀𝐣𝐣𝐯𝐯𝐣𝐣�𝐀𝐀𝛉𝛉𝟐𝟐𝟐𝟐 i𝐯𝐯𝟐𝟐 i�T𝐀𝐀𝐣𝐣𝐯𝐯𝐣𝐣�𝐀𝐀𝛉𝛉𝟑𝟑𝟑𝟑 i𝐯𝐯𝟐𝟐 i�T𝐀𝐀𝐣𝐣𝐯𝐯𝐣𝐣 000 000 𝐃𝐃𝐪𝐪,𝟏𝟏 𝛉𝛉𝟎𝟎𝟎𝟎 𝐃𝐃𝐪𝐪,𝟏𝟏 𝛉𝛉𝟏𝟏𝟏𝟏 𝐃𝐃𝐪𝐪,𝟏𝟏 𝛉𝛉𝟐𝟐𝟐𝟐 𝐃𝐃𝐪𝐪,𝟏𝟏 𝛉𝛉𝟑𝟑𝟑𝟑 𝐃𝐃𝐪𝐪,𝟐𝟐 𝛉𝛉𝟎𝟎𝟎𝟎 𝐃𝐃𝐪𝐪,𝟐𝟐 𝛉𝛉𝟏𝟏𝟏𝟏 𝐃𝐃𝐪𝐪,𝟐𝟐 𝛉𝛉𝟐𝟐𝟐𝟐 𝐃𝐃𝐪𝐪,𝟐𝟐 𝛉𝛉𝟑𝟑𝟑𝟑 ⎦ ⎥ ⎥ ⎥ ⎥ ⎤ Donde: 𝐃𝐃𝐪𝐪,𝟏𝟏 𝛉𝛉 𝐤𝐤𝐤𝐤 =�𝐀𝐀𝛉𝛉𝐤𝐤𝐤𝐤𝐯𝐯𝟏𝟏 i�T�𝐫𝐫𝐢𝐢+𝐀𝐀𝐢𝐢𝐮𝐮𝐏𝐏 i−𝐫𝐫𝐣𝐣−𝐀𝐀𝐣𝐣𝐮𝐮𝐐𝐐 j�+2�𝐀𝐀𝛉𝛉𝐤𝐤𝐯𝐯𝟏𝟏 i�T�𝐀𝐀θk𝐮𝐮𝐏𝐏 i�+(𝐀𝐀𝐢𝐢𝐯𝐯𝟏𝟏 i)T(𝐀𝐀𝛉𝛉𝐤𝐤𝐤𝐤 i𝐮𝐮𝐏𝐏 i) ( 33) 𝐃𝐃𝐪𝐪,𝟐𝟐 𝛉𝛉𝐤𝐤𝐤𝐤 =�𝐀𝐀𝛉𝛉 𝐤𝐤𝐤𝐤 𝐯𝐯𝟐𝟐 i�T�𝐫𝐫𝐢𝐢+𝐀𝐀𝐢𝐢𝐮𝐮𝐏𝐏 i−𝐫𝐫𝐣𝐣−𝐀𝐀𝐣𝐣𝐮𝐮𝐐𝐐 j�+2�𝐀𝐀𝛉𝛉 𝐤𝐤 𝐯𝐯𝟐𝟐 i�T�𝐀𝐀θ k 𝐮𝐮𝐏𝐏 i�+(𝐀𝐀𝐢𝐢𝐯𝐯𝟐𝟐 i)T(𝐀𝐀𝛉𝛉 𝐤𝐤𝐤𝐤 i𝐮𝐮𝐏𝐏 i) ( 34) Por último, para el sólido j: ⎣ ⎢ ⎢ ⎢ ⎢ ⎡ 000 000(𝐀𝐀𝐢𝐢𝐯𝐯𝟏𝟏 i)T𝐀𝐀𝛉𝛉𝟎𝟎𝟎𝟎 𝐣𝐣𝐯𝐯𝐣𝐣(𝐀𝐀𝐢𝐢𝐯𝐯𝟏𝟏 i)T𝐀𝐀𝛉𝛉𝟏𝟏𝟏𝟏 𝐣𝐣𝐯𝐯𝐣𝐣(𝐀𝐀𝐢𝐢𝐯𝐯𝟏𝟏 i)T𝐀𝐀𝛉𝛉𝟐𝟐𝟐𝟐 𝐣𝐣𝐯𝐯𝐣𝐣(𝐀𝐀𝐢𝐢𝐯𝐯𝟏𝟏 i)T𝐀𝐀𝛉𝛉𝟑𝟑𝟑𝟑 𝐣𝐣𝐯𝐯𝐣𝐣 (𝐀𝐀𝐢𝐢𝐯𝐯𝟐𝟐 i)T𝐀𝐀𝛉𝛉𝟎𝟎𝟎𝟎 𝐣𝐣𝐯𝐯𝐣𝐣(𝐀𝐀𝐢𝐢𝐯𝐯𝟐𝟐 i)T𝐀𝐀𝛉𝛉𝟏𝟏𝟏𝟏 𝐣𝐣𝐯𝐯𝐣𝐣(𝐀𝐀𝐢𝐢𝐯𝐯𝟐𝟐 i)T𝐀𝐀𝛉𝛉𝟐𝟐𝟐𝟐 𝐣𝐣𝐯𝐯𝐣𝐣(𝐀𝐀𝐢𝐢𝐯𝐯𝟐𝟐 i)T𝐀𝐀𝛉𝛉𝟑𝟑𝟑𝟑 𝐣𝐣𝐯𝐯𝐣𝐣 000 000 𝐃𝐃𝐪𝐪,𝟏𝟏 𝛉𝛉𝟎𝟎 𝐃𝐃𝐪𝐪,𝟏𝟏 𝛉𝛉𝟏𝟏 𝐃𝐃𝐪𝐪,𝟏𝟏 𝛉𝛉𝟐𝟐 𝐃𝐃𝐪𝐪,𝟏𝟏 𝛉𝛉𝟑𝟑 𝐃𝐃𝐪𝐪,𝟐𝟐 𝛉𝛉𝟎𝟎 𝐃𝐃𝐪𝐪,𝟐𝟐 𝛉𝛉𝟏𝟏 𝐃𝐃𝐪𝐪,𝟐𝟐 𝛉𝛉𝟐𝟐 𝐃𝐃𝐪𝐪,𝟐𝟐 𝛉𝛉𝟑𝟑 ⎦ ⎥ ⎥ ⎥ ⎥ ⎤ Con: 𝐃𝐃 𝐪𝐪,𝟏𝟏 𝛉𝛉𝐤𝐤 =−(𝐀𝐀 𝐢𝐢 𝐯𝐯 𝟏𝟏 i ) T (𝐀𝐀 𝛉𝛉𝐤𝐤𝐤𝐤 j 𝐮𝐮 𝐐𝐐 j ) ( 35) 𝐃𝐃𝐪𝐪,𝟐𝟐 𝛉𝛉𝐤𝐤=−(𝐀𝐀𝐢𝐢𝐯𝐯𝟐𝟐 i)T(𝐀𝐀𝛉𝛉𝐤𝐤𝐤𝐤 j𝐮𝐮𝐐𝐐 j) ( 36) 21 Escuela Técnica Superior de Ingeniería de Sevilla 2.3.6 Creación de la estructura de la plataforma de Stewart. El primer paso de la modelización de la plataforma es establecer su configuración geométrica, así como los sistemas de referencias global y locales de cada sólido. Por simplicidad, el sistema de referencia global se sitúa en el centro de la plataforma fija, con el plano x-y perteneciente a dicha plataforma y el eje z perpendicular y apuntando a la plataforma móvil. Por motivos de comodidad a la hora de crear el fichero de estructura los sistemas de referencias locales de cada una de las 12 barras que unen las dos plataformas se han situado en los extremos de las barras, pegados a las plataformas, de manera que el eje z pertenece siempre al eje de las barras y el plano x-y permanece perpendicular a dicho eje. Por último, el sistema de coordenadas local de la plataforma móvil se sitúa en el centro de esta y con su plano x-y paralelo al plano de la plataforma fija, pero girado con respecto a estos 60 grados. De esta forma, cuando se modelen los pares cilíndricos, los vectores 𝐯𝐯𝐢𝐢,𝐯𝐯𝐣𝐣 tendrán la dirección y sentido del eje z. Y los vectores 𝐮𝐮𝐏𝐏 𝐢𝐢 y 𝐮𝐮𝐐𝐐 𝐣𝐣 serán nulos debido a que los puntos P y Q se sitúan en los orígenes del sistema. Por último, los pares Cardan se sitúan en ambas plataformas de manera que la parte de la cruceta perteneciente a dicha plataforma sea perpendicular a la dirección que une el centro de la plataforma con el par, quedando así definidos los vectores 𝐬𝐬𝐢𝐢,𝐬𝐬𝐣𝐣,𝐮𝐮𝐏𝐏 𝐢𝐢 y 𝐮𝐮𝐏𝐏 𝐣𝐣 tal y como se aprecia en el programa de Matlab StewartPlatform.m, expuesto al final de este documento, con anotaciones que tienen como objetivo facilitar su comprensión. Cabe destacar que, con el objetivo de realizar luego un estudio dinámico del mecanismo, estos sistemas de referencias de las barras se trasladan a su centro de gravedad de forma que sus ejes sean ejes principales de inercia. Esto resulta útil a la hora de montar la matriz de masa para que esta sea diagonal y como se explica en el apartado de dinámica inversa. 2.4. Dinámica Inversa 2D Tras el estudio de la cinemática de los mecanismos 2D y 3D, se procede al estudio de la dinámica inversa, donde también se trabajará primero en 2D, y posteriormente se explicarán los nuevos detalles que atañen al caso 3D. Para empezar la dinámica inversa consiste en, conocido el movimiento de un mecanismo, averiguar las fuerzas que surgen en el para producir dichos movimientos. La dinámica directa, por el contrario, consiste en dadas unas fuerzas, calcular cómo será el movimiento del mecanismo. Para realizar el estudio dinámico del mecanismo lo primero que se ha tenido en cuenta es la posición de los sistemas de referencia. Estos podrían estar donde mismo que estaban a la hora de la resolución del problema cinemático, sin embargo, para facilitar el cálculo se procede a desplazar dichos sistemas de referencias a los centros de inercia de las barras, de esta manera el cálculo se simplificará considerablemente, como se verá a continuación. Fundamentos teóricos 22 Las ecuaciones del movimiento, para un sólido no sometido a restricciones, se expresa de la siguiente manera: mixı=fx ( 37) miyı=fy ( 38) μ i ϕ ı =n i ( 39) O lo que es lo mismo, en forma matricial: 𝐌𝐌𝐢𝐢𝐃𝐃𝐢𝐢 =𝐠𝐠𝐢𝐢 ( 40) Donde la matriz 𝐌𝐌𝐢𝐢 es la matriz de masas que, al tener los sistemas de referencia en ejes principales de inercia, es diagonal con los dos primeros componentes la masa de la barra y el tercer componente la inercia respecto al eje perpendicular al plano. El vector 𝐃𝐃𝐢𝐢 está compuesto por las aceleraciones del sólido i en x, y y la aceleración angular. Por último, el vector 𝐠𝐠𝐢𝐢 es el vector de fuerzas externas que actúan sobre el sólido (como por ejemplo, la gravedad). Quedaría: ⎣ ⎢ ⎡ mi0 0 0 m i 0 0 0 μ i ⎦ ⎥ ⎤ ⎣ ⎢ ⎡ xı y ı  ϕ ı  ⎦ ⎥ ⎤ = ⎣ ⎢ ⎡ fx f y n i ⎦ ⎥ ⎤ ( 41) Para un sólido con restricciones habrá que añadir al sistema anterior otro vector de reacciones debido a esas restricciones. Quedaría, por tanto: 𝐌𝐌𝐃𝐃=𝐠𝐠+𝐠𝐠(𝐜𝐜) ( 42) Donde 𝐠𝐠(𝐜𝐜) es el vector de reacciones, el cual resulta ser: 𝐠𝐠(𝐜𝐜) =𝚽𝚽𝐪𝐪 𝑻𝑻𝝀𝝀 ( 43) Donde la matriz 𝚽𝚽𝐪𝐪 𝐓𝐓 es la matriz jacobiana traspuesta calculada en el problema cinemático y el vector 𝛌𝛌 consistiría en el vector incógnita a calcular. Despejando dicho vector, el sistema a resolver sería: 𝛌𝛌=�𝚽𝚽𝐪𝐪 𝐓𝐓�−𝟏𝟏(𝐌𝐌𝐃𝐃−𝐠𝐠) ( 44) 23 Escuela Técnica Superior de Ingeniería de Sevilla Este vector 𝛌𝛌 es llamado vector de reacciones, y cada uno de sus componentes está asociado a las ecuación de restricción correspondiente que se halla establecido en el problema cinemático. 2.5. Dinámica Inversa 3D. La dinámica inversa en el caso 3D comparte la misma base que el caso 2D, pero resulta más laborioso y se añaden conceptos y cálculos nuevos que hay que realizar debido a la inclusión de los parámetros de Euler. El primer paso a realizar, como en el caso 2D es el de poner todos los sistemas de coordenadas en ejes principales de inercia para cada sólido, para así conseguir una matriz de masas diagonal. Lo primero que cambia es que a la hora de plantear las ecuaciones de movimiento hay que distinguir entre ecuaciones de movimiento debido a las traslaciones (a lo largo de los ejes cartesianos) y las ecuaciones del movimiento debido a las rotaciones (alrededor de los ejes cartesianos). Estas últimas están relacionadas con las velocidades angulares de los sólidos alrededor de los ejes cartesianos. Sin embargo, a lo largo de toda la cinemática en este proyecto se ha trabajado con los llamados ángulos de Euler, con lo que será necesario el desarrollo de una formulación específica para pasar de trabajar con los parámetros de Euler a trabajar con velocidades angulares. Yendo parte por parte, las ecuaciones del movimiento traslacional son, como en el caso 2D: 𝐍𝐍𝐢𝐢𝐫𝐫𝐢𝐢=𝐟𝐟𝐢𝐢 ( 45) Donde la primera matriz es una matriz diagonal (porque se trabaja con ejes de coordenadas en ejes principales de inercia) de masa de los sólidos de dimensión 3 y con la masa del sólido en cada componente de la diagonal. Esta matriz se encuentra multiplicada por el vector de aceleraciones, y se iguala dicho producto a las fuerzas externas. Para el caso de las ecuaciones de movimiento rotacionales se va a llevar a cabo la siguiente formulación extraída de la literatura, concretamente del libro de Parviz E.Nikravesh “Computer- Aided Analysis of Mechanical Systems”: 𝐉𝐉𝐢𝐢′𝛚𝛚𝐢𝐢′+𝛚𝛚𝐢𝐢 �𝐉𝐉𝐢𝐢′𝛚𝛚𝐢𝐢′=𝐧𝐧𝐢𝐢′ ( 46) Donde 𝝎𝝎𝒊𝒊 ′ 𝒚𝒚 𝝎𝝎𝒊𝒊′ son la velocidad y aceleración angular del sólido i, que se calculan a partir de los parámetros de Euler del sólido i de la siguiente forma: 𝛚𝛚 𝐢𝐢′ =2�−e1e0e3−e2 −e 2 −e 3 e 0 e 1 −e3e2−e1e0 � ⎣ ⎢ ⎢ ⎡ e0 e 1  e2 e 3  ⎦ ⎥ ⎥ ⎤ ( 47) Fundamentos teóricos 24 𝛚𝛚𝐢𝐢′=2𝐋𝐋𝐩𝐩 ( 48) Y la derivada se calcula como: 𝛚𝛚𝐢𝐢′=2𝐋𝐋𝐩𝐩 ( 49) Y, por último: 𝛚𝛚𝐢𝐢 �=2𝐆𝐆𝐋𝐋𝐓𝐓𝐋𝐋𝐆𝐆𝐓𝐓 ( 50) Donde: 𝐆𝐆=�−𝑒𝑒1𝑒𝑒0−𝑒𝑒3𝑒𝑒2 −𝑒𝑒 2 𝑒𝑒 3 𝑒𝑒 0 −𝑒𝑒 1 −𝑒𝑒 3 −𝑒𝑒 2 𝑒𝑒 1 𝑒𝑒 0 � ( 51) Posteriormente, se expresan las ecuaciones de movimiento para sólidos sometidos a restricciones. Para las ecuaciones traslacionales se tiene, al igual que en el caso 2D: 𝐍𝐍𝐢𝐢𝐫𝐫𝐢𝐢=𝐟𝐟𝐢𝐢+𝐟𝐟𝐢𝐢(𝐜𝐜) ( 52) A la cual se les ha añadido las reacciones debidas a dichas restricciones. Dichas reacciones son: 𝐟𝐟𝐢𝐢(𝐜𝐜) =𝚽𝚽𝒓𝒓𝒊𝒊 𝑻𝑻𝝀𝝀 ( 53) Por tanto, se tiene: 𝐍𝐍𝐢𝐢𝐫𝐫𝐢𝐢−𝚽𝚽𝒓𝒓𝒊𝒊 𝑻𝑻𝝀𝝀=𝐟𝐟𝐢𝐢 ( 54) Como en el caso bidimensional, la incógnita es 𝝀𝝀. Para las ecuaciones rotacionales, las ecuaciones de la formulación presentada anteriormente quedan: 𝐉𝐉𝐢𝐢′𝛚𝛚𝐢𝐢′+𝛚𝛚𝐢𝐢 �𝐉𝐉𝐢𝐢′𝛚𝛚𝐢𝐢′−1 2𝐋𝐋𝐢𝐢𝚽𝚽𝐩𝐩 𝐢𝐢 𝐓𝐓𝛌𝛌=𝐧𝐧𝐢𝐢′ ( 55) Para unificar el problema y unificar las ecuaciones de movimiento translacionales y rotacionales habría que resolver el siguiente sistema: 25 Escuela Técnica Superior de Ingeniería de Sevilla �𝐌𝐌 𝐁𝐁𝐓𝐓 𝐁𝐁 𝟎𝟎 ��𝐡𝐡 −𝛌𝛌�=�𝐠𝐠 𝛄𝛄#� ( 56) Donde M es la matriz de masa, diagonal (por estar los sistemas de referencia en ejes principales de inercia), compuesta por submatrices 6x6 (una por cada sólido del mecanismo) donde los tres primeros elementos de la diagonal son la masa del sólido y los 3 siguientes las inercias con respecto a x, y y z respectivamente. La matriz B es: 𝐁𝐁=�𝚽𝚽𝐫𝐫 𝟏𝟏 ,1 2𝚽𝚽𝒑𝒑 𝟏𝟏 𝑳𝑳𝟏𝟏 𝑻𝑻,…, 𝚽𝚽𝐫𝐫 𝐛𝐛 ,1 2𝚽𝚽𝒑𝒑 𝒃𝒃 𝑳𝑳𝒃𝒃 𝑻𝑻,� ( 57) La matriz 𝐡𝐡 y b: 𝐡𝐡= ⎣ ⎢ ⎢ ⎢ ⎡ 𝒓𝒓𝟏𝟏 𝝎𝝎𝟏𝟏′  … 𝒓𝒓𝒃𝒃 𝝎𝝎𝒃𝒃′  ⎦ ⎥ ⎥ ⎥ ⎤ 𝐛𝐛= ⎣ ⎢ ⎢ ⎡ 𝟎𝟎 𝛚𝛚𝟏𝟏 �𝐉𝐉𝟏𝟏′𝛚𝛚𝟏𝟏′ … 𝟎𝟎 𝛚𝛚𝐛𝐛 �𝐉𝐉𝐛𝐛′𝛚𝛚𝐛𝐛′ ⎦ ⎥ ⎥ ⎤ Para entender estos fundamentos en mayor profundidad el lector puede recurrir al libro de Nikravesh, expuesto en la bibliografía. Como se puede observar la matriz B no es cuadrada, lo que implica que para poder resolver el sistema se necesita completar dicho sistema con una artificialidad, que en este caso es añadir el vector 𝛄𝛄# como se puede ver en la expresión matricial anterior. Al resolver el sistema, se obtendrán los valores de 𝐡𝐡 que ya eran conocidos, pero además el vector 𝛌𝛌, que nos servirá de manera análoga al caso 2D para conocer fuerzas que se producen en el mecanismo cuando imponemos algún movimiento. Una vez implementado este procedimiento en la plataforma de Stewart aparece una singularidad en el que la matriz compuesta por la matriz de masa M y la matriz B. Por ello se opta por otro procedimiento, extraído del libro de Edward J. Haug: “Computer Aided Kinematics and Dynamics of Mechanical Systems”. Dicho procedimiento consiste en resolver la siguiente expresión. 𝐌𝐌𝐃𝐃+𝚽𝚽𝐪𝐪 𝐓𝐓𝛌𝛌=𝐅𝐅 Donde las matrices 𝐃𝐃, 𝚽𝚽𝒒𝒒 𝑻𝑻 y 𝝀𝝀 no varían respecto al procedimiento anterior. Sin embargo, las matrices M y F sufren una ligera modificación: 𝐌𝐌=�𝐦𝐦𝐢𝐢𝟎𝟎 𝟎𝟎 𝟒𝟒𝐋𝐋𝐢𝐢𝐓𝐓𝐉𝐉𝐢𝐢𝐋𝐋𝐢𝐢� 𝐅𝐅=� 𝐅𝐅𝐢𝐢 𝟐𝟐𝐋𝐋𝐢𝐢𝐓𝐓𝐧𝐧𝐢𝐢+𝟖𝟖𝐋𝐋𝐢𝐢𝐓𝐓 𝐉𝐉𝐢𝐢𝐋𝐋𝐢𝐢𝐩𝐩𝐢𝐢� La matriz M es una matriz de dimensión 7n, con n el número de sólidos del mecanismo. Las tres Resultados. 32 Realizado este procedimiento numéricamente, que puede observarse en el apartado códigos del presente documento, se obtiene el siguiente resultado: Imagen 3.5: Representación del par motor con respecto al tiempo. 4.2. Resultados de la cinemática y dinámica de la plataforma de Stewart. Primeramente, se ha optado por representar movimientos sencillos para ver si la implementación era correcta. Para ello, se ha modelado una plataforma de Stewart cuyas barras son todas rectas y no están inclinadas respecto a la plataforma. De esta forma, todas las velocidades y aceleraciones de todas las barras pegadas a la plataforma móvil deberían ser iguales. Las velocidades y aceleraciones en las barras pegadas a la plataforma fija deben de ser nulas. Efectivamente, esto es así y el resultado es el siguiente expuesto. El sólido 1 es la plataforma fija, el 2 una de las barras pegadas a la fija mediante el par Cardan, el 13 una de las barras pegadas a la plataforma móvil y el 14 la plataforma móvil. Por tanto, las barras de la 3 a la 7 tienen el mismo resultado que la 2 y las barras de la 8 a las 12 tienen el mismo resultado que la 13. 33 Escuela Técnica Superior de Ingeniería de Sevilla Resultados. 34 Imagen 3.6: Representación cinemática plataforma de Stewart. Posteriormente se representa la cinemática de la plataforma de Stewart real, con sus barras inclinadas. Se representan los tres movimientos básicos de una aeronave: guiñada, cabeceo y alabeo. Para cada actuación, se va a representar la velocidad y aceleración de alguna de las barras pegadas a la plataforma: 35 Escuela Técnica Superior de Ingeniería de Sevilla Imagen 3.7: Resultados guiñada. Resultados. 36 37 Escuela Técnica Superior de Ingeniería de Sevilla Imagen 3.8: Resultados alabeo. Resultados. 38 39 Escuela Técnica Superior de Ingeniería de Sevilla Imagen 3.9: Resultados cabeceo. Una vez expresados algunos de los resultados de la cinemática, se procede a la dinámica, donde los resultados que se expondrán a continuación serán, la fuerza que es necesario aplicar en el centro de la plataforma móvil para permitir estos movimientos básicos de una aeronave. Como se expuso en el capítulo anterior, los sistemas de coordenadas de las barras han cambiado a su centro de gravedad y formando ejes principales de inercia. Imagen 3.10: Resultados de fuerzas en guiñada. Resultados. 40 Imagen 3.11: Resultados de fuerzas en alabeo. Imagen 3.12: Resultado de fuerzas en cabeceo. 41 Escuela Técnica Superior de Ingeniería de Sevilla 4.3. Resultados fuerza en los pistones. Se muestran a continuación los resultados de las fuerzas en los pistones para las diferentes maniobras del avión (guiñada, alabeo y cabeceo): Resultados. 48 49 Escuela Técnica Superior de Ingeniería de Sevilla Imagen 3.15: Resultados fuerzas de los pistones en cabeceo. Resultados. 50 4.4. Conclusiones. Referente a la plataforma de Stewart: se han podido corroborar algunas de las características que al principio del presente documento se exponían: - Gran utilidad a la hora de simular movimientos de 6 grados de libertad, permitiendo un gran rango de desplazamientos. En particular, se ha demostrado la facilidad de dicho mecanismo para representar el movimiento de aeronaves a partir de los tres movimientos básicos de alabeo, cabeceo y guiñada. - Permite trabajar a altas velocidades. Por ejemplo, se han simulado movimientos de alabeo, guiñada y cabeceo a altas frecuencias y el mecanismo a funcionado correctamente. - La relación carga/peso efectivamente es alta. Los seis pistones permiten repartirse la carga de manera que las fuerzas ejercidas no son excesivamente altas para las cargas que está soportando la plataforma. - En definitiva, la gran utilidad de este mecanismo a la hora de aplicarlo a simuladores. - A pesar del mayor coste computacional y la complejidad del mecanismo, las grandes ventajas que este proporciona hacen de este mal un mal menor. Referente a la dinámica de sistemas multicuerpos: se ha puesto de manifiesto en este trabajo el beneficio de usar el análisis de sistemas multicuerpos asistidos por ordenador, ya que ha demostrado ser una técnica que puede ser utilizada de manera general y sistemática para cualquier mecanismo. Además, permite evaluar previamente mecanismos antes de que estos se fabriquen, para estudiar su viabilidad, lo que se traduce en una mayor eficiencia y reducción de costes. En definitiva, todos estos conocimientos adquiridos al profundizar en dicha disciplina han servido al autor de este trabajo para tener una visión más amplia del mundo de la teoría de máquinas. 4.5. Mejoras futuras. Puesto que esto es solo el principio de algo que podría llevar a un estudio mucho más exhaustivo, las posibles mejoras futuras son numerosas: - Resolución del problema dinámico directo. - Representación de movimientos curiosos y más complejos de aeronaves, combinando los tres movimientos básicos de alabeo, guiñada y cabeceo. - Estudio de singularidades del mecanismo - Aplicación de control automático a la plataforma de Stewart. 51 Escuela Técnica Superior de Ingeniería de Sevilla 4 CÓDIGOS. 4.1. Códigos mecanismo Biela-Manivela. 4.1.1 Estructura %% Programa que monta la estructura de la Biela-Manivela. MBody.nombre = "BielaManivela"; %Datos enteros básicos del mecanismo MBody.nb = 4; %Número de sólidos MBody.nr = 3; %Número de pares de revolución MBody.np = 1; %Número de pares prismáticos %Vector de estructuras de sólidos MBody.Solidos(1).nombre = "Fija"; MBody.Solidos(2).nombre = "Manivela"; MBody.Solidos(3).nombre = "Biela"; MBody.Solidos(4).nombre = "Corredera"; %Vector de estructuras de pares de revolución MBody.Pares.Rev(1).I = 1; MBody.Pares.Rev(1).J = 2; MBody.Pares.Rev(1).vi = [0 0]'; MBody.Pares.Rev(1).vj = [0 0]'; MBody.Pares.Rev(2).I = 2; MBody.Pares.Rev(2).J = 3; MBody.Pares.Rev(2).vi = [1 0]'; MBody.Pares.Rev(2).vj = [0 0]'; MBody.Pares.Rev(3).I = 3; MBody.Pares.Rev(3).J = 4; MBody.Pares.Rev(3).vi = [2 0]'; MBody.Pares.Rev(3).vj = [0 0]'; %Vector de estructuras de pares prismáticos MBody.Pares.Prism(1).I = 1; MBody.Pares.Prism(1).J = 4; MBody.Pares.Prism(1).vi = [0 0]'; MBody.Pares.Prism(1).vj = [0 0]'; MBody.Pares.Prism(1).hi = [0 1]'; MBody.Pares.Prism(1).bet = 0.0; 4.1.2 Restricciones. %% Función que monta la matriz de ecuaciones de restricción. function C = RestriccionesParesCinematicos(q) global MBody global tiempo Códigos. 52 %% Inicializa vector de restricciones nrestr = 3 + 2*(MBody.nr + MBody.np); C = zeros(nrestr,1); %% Barra fija C(1,1) = q(1); C(2,1) = q(2); C(3,1) = q(3); %% Pares de revolución for k = 1:MBody.nr; i = MBody.Pares.Rev(k).I; j = MBody.Pares.Rev(k).J; qi = q(3*(i-1)+1:3*(i-1)+3); qj = q(3*(j-1)+1:3*(j-1)+3); Cr = RestriccionParRevolucion(k,qi,qj); C(3+2*(k-1)+1,1) = Cr(1); C(3+2*(k-1)+2,1) = Cr(2); end %% Pares prismáticos for k=1:MBody.np; i = MBody.Pares.Prism(k).I; j = MBody.Pares.Prism(k).J; qi = q(3*(i-1)+1:3*(i-1)+3); qj = q(3*(j-1)+1:3*(j-1)+3); Cp = RestriccionParPrismatico(k,qi,qj); C(3+2*MBody.nr+2*(k-1)+1,1) = Cp(1); C(3+2*MBody.nr+2*(k-1)+2,1) = Cp(2); end %% Restricción de movilidad. Hay que modificar en el caso de otro mecanismo, ya que las restricciónes serán diferentes. C(3+2*(MBody.nr+MBody.np)+1,1)=q(6)-MBody.w*tiempo; end %% Función que monta las ecuaciones del par de revolución que une el sólido i con el j function Cr = RestriccionParRevolucion(k,qi,qj) global MBody ui = MBody.Pares.Rev(k).vi; uj = MBody.Pares.Rev(k).vj; Ai = RotMat(qi(3)); Aj = RotMat(qj(3)); Cr = qi(1:2)+Ai*ui-qj(1:2)-Aj*uj; end %% Función que monta las ecuaciones del par de prismático que une el sólido i con el j 53 Escuela Técnica Superior de Ingeniería de Sevilla function Cp = RestriccionParPrismatico(k,qi,qj) global MBody ui = MBody.Pares.Prism(k).vi; uj = MBody.Pares.Prism(k).vj; hi = MBody.Pares.Prism(k).hi; bet = MBody.Pares.Prism(k).bet; Ai = RotMat(qi(3)); Aj = RotMat(qj(3)); Cp(1) = qi(3)-qj(3)-bet; Cp(2) = (Ai*hi)'*(qi(1:2,1)+Ai*ui-qj(1:2,1)-Aj*uj); end 4.1.3 Jacobiano, matriz DCq y derivadas con respecto al tiempo de las restricciones. %% Función que monta la matriz jacobiana de las restricciones. Cada fila corresponde a una ecuación de restricción, y cada columna a la variable con respecto a la cual se está derivando. Se introducen las submatrices en bloques, primero la del sólido i, y luego la del j. function Cq = Jacobiano(q,nrestr) global MBody %% Inicializa jacobiano con dimensión (nrest+ngl)x(nrest+ngl) porque añadimos a las ecuaciones de restricción tantas ecuaciones de movilidad como gdl tenga el mecanismo. ngl=1; Cq=zeros(nrestr+ngl); %% Barra fija C=eye(3); Cq(1:3,1:3)=C; %% Pares de revolución for k = 1:MBody.nr; i = MBody.Pares.Rev(k).I; j = MBody.Pares.Rev(k).J; ui = MBody.Pares.Rev(k).vi; uj = MBody.Pares.Rev(k).vj; qi = q(3*(i-1)+1:3*(i-1)+3,1); qj = q(3*(j-1)+1:3*(j-1)+3,1); Cq(2*k+2:2*k+3 , 3*i-2:3*i)=[ 1 0 -ui(1)*sin(qi(3))- ui(2)*cos(qi(3)); 0 1 ui(1)*cos(qi(3))-ui(2)*sin(qi(3))]; Cq(2*k+2:2*k+3 , 3*j-2:3*j)=[-1 0 uj(1)*sin(qj(3))+uj(2)*cos(qj(3)); 0 -1 - uj(1)*cos(qj(3))+uj(2)*sin(qj(3))]; end %% Pares prismáticos for k=1:MBody.np; i = MBody.Pares.Prism(k).I; j = MBody.Pares.Prism(k).J; Códigos. 54 ui = MBody.Pares.Prism(k).vi; uj = MBody.Pares.Prism(k).vj; hi = MBody.Pares.Prism(k).hi; bet = MBody.Pares.Prism(k).bet; qi = q(3*(i-1)+1:3*(i-1)+3,1); qj = q(3*(j-1)+1:3*(j-1)+3,1); Ai = RotMat(qi(3)); Aj = RotMat(qj(3)); Athetai= RotMatTheta(qi(3)); Athetaj= RotMatTheta(qj(3)); Cq(3+2*MBody.nr+(2*k-1):3+2*MBody.nr+2*k , 3*i-2:3*i)=[0 0 1;(Ai*hi)' (Athetai*hi)'*(qi(1:2)+Ai*ui-qj(1:2)- Aj*uj)+(Ai*hi)'*(Athetai*ui)]; Cq(3+2*MBody.nr+(2*k-1):3+2*MBody.nr+2*k , 3*j-2:3*j)=[0 0 -1;- (Ai*hi)' (Ai*hi)'*(-Athetaj*uj)]; end %% Ecuación de movilidad. Nos queda meter en el jacobiano la restricción de movilidad que hayamos establecido. Cq(3+2*MBody.nr+2*MBody.np+1,6)=1; end %% Derivada de las ecuaciones de restricción con respecto al tiempo. function Ct = dtRestr(q,tspan); global MBody ngl=1; nrestr = 3 + 2*(MBody.nr + MBody.np); Ct=zeros(nrestr+ngl,1); Ct(end,1)=-MBody.w; End %% Derivada del jacobiano respecto a las coordenadas. No es más que derivar los jacobianos en cada columna por su variable correspondiente. function DCq = DtJacobiano(q,v) global MBody %% Inicializa derivada del jacobiano con dimensión (nrest+ngl)x(nrest+ngl) porque añadimos a las ecuaciones de restricción tantas ecuaciones de movilidad como gdl tenga el mecanismo. ngl=1; nrestr = 3 + 2*(MBody.nr + MBody.np); DCq=zeros(nrestr+ngl); %% Barra fija C=zeros(3); 55 Escuela Técnica Superior de Ingeniería de Sevilla DCq(1:3,1:3)=C; %% Pares de revolución for k = 1:MBody.nr; i = MBody.Pares.Rev(k).I; j = MBody.Pares.Rev(k).J; ui = MBody.Pares.Rev(k).vi; uj = MBody.Pares.Rev(k).vj; qi = q(3*(i-1)+1:3*(i-1)+3,1); qj = q(3*(j-1)+1:3*(j-1)+3,1); DCq(2*k+2:2*k+3 , 3*i-2:3*i)=[0 0 - ui(1)*cos(qi(3))+ui(2)*sin(qi(3)); 0 0 -ui(1)*sin(qi(3))- ui(2)*cos(qi(3))]; DCq(2*k+2:2*k+3 , 3*j-2:3*j)=[0 0 uj(1)*cos(qj(3))- uj(2)*sin(qj(3)); 0 0 uj(1)*sin(qj(3))+uj(2)*cos(qj(3))]; end %% Pares prismáticos for k=1:MBody.np; i = MBody.Pares.Prism(k).I; j = MBody.Pares.Prism(k).J; ui = MBody.Pares.Prism(k).vi; uj = MBody.Pares.Prism(k).vj; hi = MBody.Pares.Prism(k).hi; bet = MBody.Pares.Prism(k).bet; qi = q(3*(i-1)+1:3*(i-1)+3,1); qj = q(3*(j-1)+1:3*(j-1)+3,1); Ai = RotMat(qi(3)); Aj = RotMat(qj(3)); Athetai= RotMatTheta(qi(3)); Athetaj= RotMatTheta(qj(3)); Atheta2i= RotMatTheta2(qi(3)); Atheta2j= RotMatTheta2(qj(3)); DCq(3+2*MBody.nr+(2*k-1):3+2*MBody.nr+2*k , 3*i-2:3*i)=[0 0 0;0 0 (Atheta2i*hi)'*(qi(1:2)+Ai*ui-qj(1:2)- Aj*uj)+(Athetai*hi)'*(Athetai*ui)+(Athetai*hi)'*(Athetai*ui)+(Ai *hi)'*(Atheta2i*ui)]; DCq(3+2*MBody.nr+(2*k-1):3+2*MBody.nr+2*k , 3*j-2:3*j)=[0 0 0;0 0 (Ai*hi)'*(-Atheta2j*uj)]; end %% Ecuación de movilidad. Nos queda meter en el jacobiano la restricción de movilidad que hayamos establecido. DCq(3+2*(MBody.nr+MBody.np)+1,6)=0; End %% Derivada segunda de las restricciones respecto al tiempo. En este caso, todo es 0. function DCt = DtdtRestr(q,v,tspan) global MBody Códigos. 56 ngl=1; nrestr = 3 + 2*(MBody.nr + MBody.np); DCt=zeros(nrestr+ngl,1); End 4.1.4 Programa principal. Simulación cinemática. %% Programa principal. En el se resuelven las ecuaciones de restricción para resolver el problema de posición, y posteriormente se calculan las velocidades y aceleraciones. close all clear all clc global MBody global tiempo BielaManivela; %%%%%%%%%%%%%%%%%%%%%%%%% % SIMULACIÓN CINEMÁTICA % %%%%%%%%%%%%%%%%%%%%%%%%% MBody.w = 10*(2*pi/60); %VELOCIDAD ANGULAR (10 rpm) tspan = 0:0.1:(4*pi/MBody.w); %TIEMPO DE INTEGRACIÓN (2 vueltas) %Estimacion inicial q0 = [0 0 0 0 0 0 2 0 0 3 0 0]'; nrestr = 3 + 2*(MBody.nr + MBody.np); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % BUCLE DE SOLUCIÓN DEL PROBLEMA DE POSICIÓN % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% for i = 1:length(tspan), %PROBLEMA DE POSICION tiempo=tspan(i); q(:,i) = fsolve(@RestriccionesParesCinematicos,q0,optimset('Display','off ')); Cq = Jacobiano(q(:,i),nrestr); Ct = dtRestr(q(:,i),tspan(i)); %CALCULO DE VELOCIDADES v(:,i) = -Cq\Ct; DCq = DtJacobiano(q(:,i),v(:,i)); DCt = DtdtRestr(q(:,i),v(:,i),tspan(i)); %CALCULO DE ACELERACIONES a(:,i) = -Cq\(DCq*v(:,i).^2+DCt); % a(:,i) = -Cq\(DCq*v(:,i)+DCt); q0 = q(:,i); end figure(1) plot(tspan,q(10,:)) 57 Escuela Técnica Superior de Ingeniería de Sevilla hold on plot(tspan,q(11,:)) plot(tspan,q(12,:)) xlabel('Tiempo (s)') ylabel('x, y, \theta') title('Coordenadas de la corredera.') legend('x(m)','y(m)','\theta(rad)') filename=['Pos_Corredera.png']; saveas([1],filename) figure(2) plot(tspan,v(10,:)) hold on plot(tspan,v(11,:)) plot(tspan,v(12,:)) xlabel('Tiempo (s)') ylabel('V_x, V_y,\omega_\theta') title('Velocidades de la corredera.') legend('V_x (m/s)','V_y(m/s)','\omega_\theta(rad/s)') filename=['Vel_Corredera.png']; saveas([2],filename) figure(3) plot(tspan,a(10,:)) hold on plot(tspan,a(11,:)) plot(tspan,a(12,:)) xlabel('Tiempo (s)') ylabel('V_x, V_y,\omega_\theta') title('Aceleraciones de la corredera.') legend('a_x (m/s)','a_y(m/s)','\omega_\theta(rad/s^2)') filename=['Ac_Corredera.png']; saveas([2],filename) figure(4) plot(tspan,v(10,:)) hold on plot(tspan,a(10,:)) xlabel('Tiempo (s)') ylabel('x, y, \theta') title('Velocidad VS Aceleración corredera.') legend('V_x(m/s)','a_x(m/s^2)') filename=['Vel_Ac_Corredera.png']; saveas([4],filename) % figure(4) % anima %% Animador del mecanismo. Realiza una representación en el plano de cómo se va moviendo el mecanismo. Códigos. 64 end %% Pares Cilíndricos. for k=1:MBody.ncil i = MBody.Pares.Cil(k).I; j = MBody.Pares.Cil(k).J; qi = q(7*(i-1)+1:7*(i-1)+7); qj = q(7*(j-1)+1:7*(j-1)+7); Ccil = RestriccionParCilindrico(k,qi,qj); C(7+4*MBody.ncar+4*(k-1)+1,1) = Ccil(1); C(7+4*MBody.ncar+4*(k-1)+2,1) = Ccil(2); C(7+4*MBody.ncar+4*(k-1)+3,1) = Ccil(3); C(7+4*MBody.ncar+4*(k-1)+4,1) = Ccil(4); end %% Restricciones de orientación. for k=2:MBody.nb-1 C(7+4*(MBody.ncar+MBody.ncil)+(k-1),1)=q(7*k-3:7*k)'*q(7*k- 3:7*k)-1; %No entra ni la plataforma fija, ni la móvil. end %% Restricciones de movilidad. Aquí deberían ir las 7 ecuaciones de movilidad. % Cuatro de ellas deben ser los parametros de Euler de la plataforma móvil, % que que hemos eliminado la condición de módulo unitario en esta % plataforma. % Comentar los que no procedan y descomentar el que se quiera. % if Ac==1 % Prueba C(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+8,1)= q(92); %Primera ecuación C(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+9,1)= q(93); %Segunda ecuación C(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+10,1)= q(94)-50- 4*sin(5*tiempo); %Tercera ecuación C(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+11,1)= q(95)-1; %Cuarta ecuación C(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+12,1)= q(96); %Quinta ecuación C(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+13,1)= q(97); %Sexta ecuación C(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+14,1)= q(98); %Séptima ecuación % elseif Ac==2 % % Guiñada 65 Escuela Técnica Superior de Ingeniería de Sevilla % C(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+8,1)= q(92); %Primera ecuación % C(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+9,1)= q(93); %Segunda ecuación % C(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+10,1)= q(94)-50; %Tercera ecuación % C(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+11,1)= q(95)- cos(pi/18/2*sin(5*tiempo)); %Cuarta ecuación % C(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+12,1)= q(96); %Quinta ecuación % C(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+13,1)= q(97); %Sexta ecuación % C(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+14,1)= q(98)- sin(pi/18/2*sin(5*tiempo)); %Séptima ecuación % elseif Ac==3 % Alabeo. Giro alrededor de eje x % C(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+8,1)= q(92); %Primera ecuación % C(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+9,1)= q(93); %Segunda ecuación % C(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+10,1)= q(94)-50; %Tercera ecuación % C(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+11,1)= q(95)- cos(pi/18/2*sin(5*tiempo)); %Cuarta ecuación % C(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+12,1)= q(96)- sin(pi/18/2*sin(5*tiempo)); %Quinta ecuación % C(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+13,1)= q(97); %Sexta ecuación % C(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+14,1)= q(98); %Séptima ecuación % elseif Ac==4 % Cabeceo. Giro alrededor de eje y % C(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+8,1)= q(92); %Primera ecuación % C(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+9,1)= q(93); %Segunda ecuación % C(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+10,1)= q(94)-50; %Tercera ecuación % C(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+11,1)= q(95)- cos(pi/18/2*sin(5*tiempo)); %Cuarta ecuación % C(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+12,1)= q(96); %Quinta ecuación % C(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+13,1)= q(97)- sin(pi/18/2*sin(5*tiempo)); %Sexta ecuación % C(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+14,1)= q(98); %Séptima ecuación % end Códigos. 66 End function Ccil=RestriccionParCilindrico(k,qi,qj) global MBody vi=MBody.Pares.Cil(k).vi; vj=MBody.Pares.Cil(k).vj; ui=MBody.Pares.Cil(k).ui; uj=MBody.Pares.Cil(k).uj; Ai=RotMat(qi(4:7)); Aj=RotMat(qj(4:7)); %% Restricción de movilidad. vec=[1 0 0]'; vi1=cross(vec,vi); vi2=cross(vi,vi1); Ccil(1,1)=(Ai*vi1)'*Aj*vj; Ccil(2,1)=(Ai*vi2)'*Aj*vj; Ccil(3,1)=(Ai*vi1)'*(qi(1:3)+Ai*ui-qj(1:3)-Aj*uj); Ccil(4,1)=(Ai*vi2)'*(qi(1:3)+Ai*ui-qj(1:3)-Aj*uj); %% Restricción de orientación. % Ccil(5,1)=qi(4:7)' End function Ccar=RestriccionParCardan(k,qi,qj) global MBody uPi=MBody.Pares.Car(k).uPi; uPj=MBody.Pares.Car(k).uPj; si=MBody.Pares.Car(k).si; sj=MBody.Pares.Car(k).sj; Ai=RotMat(qi(4:7)); Aj=RotMat(qj(4:7)); %% Restricción de movilidad Ccar(1:3,1)=qi(1:3)+Ai*uPi-qj(1:3)-Aj*uPj; Ccar(4,1)=(Ai*si)'*(Aj*sj); %% Restricción de orientación % Ccar(5,1)=q(4:7)'*q(4:7)-1; End 4.2.3 Jacobiano, DCq y derivadas de las restricciones respecto al tiempo. function Cq = Jacobiano(q,nrestr,Ac) global MBody 67 Escuela Técnica Superior de Ingeniería de Sevilla %% Inicializa jacobiano con dimensión (nrest+ngl)x(nrest+ngl) porque añadimos a las ecuaciones de restricción tantas ecuaciones de movilidad como gdl tenga el mecanismo. ngl=6; Cq=zeros(nrestr+ngl); %% Barra fija C=eye(7); Cq(1:7,1:7)=C; %% Pares Cardan for k=1:MBody.ncar i=MBody.Pares.Car(k).I; j=MBody.Pares.Car(k).J; uPi=MBody.Pares.Car(k).uPi; uPj=MBody.Pares.Car(k).uPj; si=MBody.Pares.Car(k).si; sj=MBody.Pares.Car(k).sj; qi = q(7*(i-1)+1:7*(i-1)+7,1); qj = q(7*(j-1)+1:7*(j-1)+7,1); Ai =RotMat(qi(4:7)); Aj =RotMat(qj(4:7)); Ae0i = RotMat0(qi(4:7)); Ae1i = RotMat1(qi(4:7)); Ae2i = RotMat2(qi(4:7)); Ae3i = RotMat3(qi(4:7)); Ae0j = RotMat0(qj(4:7)); Ae1j = RotMat1(qj(4:7)); Ae2j = RotMat2(qj(4:7)); Ae3j = RotMat3(qj(4:7)); Cq(4*(k-1)+8:4*(k-1)+8+3,7*(i-1)+1:7*(i-1)+7)=[eye(3) Ae0i*uPi Ae1i*uPi Ae2i*uPi Ae3i*uPi; zeros(1,3) (Ae0i*si)'*(Aj*sj) (Ae1i*si)'*(Aj*sj) (Ae2i*si)'*(Aj*sj) (Ae3i*si)'*(Aj*sj)]; Cq(4*(k-1)+8:4*(k-1)+8+3,7*(j-1)+1:7*(j-1)+7)=[-eye(3) - Ae0j*uPj -Ae1j*uPj -Ae2j*uPj - Ae3j*uPj; zeros(1,3) (Ai*si)'*(Ae0j*sj) (Ai*si)'*(Ae1j*sj) (Ai*si)'*(Ae2j*sj) (Ai*si)'*(Ae3j*sj)]; end %% Pares Cilíndricos. for k=1:MBody.ncil i=MBody.Pares.Cil(k).I; Códigos. 68 j=MBody.Pares.Cil(k).J; ui=MBody.Pares.Cil(k).ui; uj=MBody.Pares.Cil(k).uj; vi=MBody.Pares.Cil(k).vi; vj=MBody.Pares.Cil(k).vj; vec=[1 0 0]'; vi1=cross(vec,vi); vi2=cross(vi,vi1); qi = q(7*(i-1)+1:7*(i-1)+7,1); qj = q(7*(j-1)+1:7*(j-1)+7,1); Ai =RotMat(qi(4:7,1)); Aj =RotMat(qj(4:7,1)); Ae0i = RotMat0(qi(4:7,1)); Ae1i = RotMat1(qi(4:7,1)); Ae2i = RotMat2(qi(4:7,1)); Ae3i = RotMat3(qi(4:7,1)); Ae0j = RotMat0(qj(4:7,1)); Ae1j = RotMat1(qj(4:7,1)); Ae2j = RotMat2(qj(4:7,1)); Ae3j = RotMat3(qj(4:7,1)); Cq(4*MBody.ncar+4*(k-1)+8:4*MBody.ncar+4*(k-1)+8+3,7*(i- 1)+1:7*(i-1)+7)=[zeros(1,3), (Ae0i*vi1)'*(Aj*vj), (Ae1i*vi1)'*(Aj*vj), (Ae2i*vi1)'*(Aj*vj), (Ae3i*vi1)'*(Aj*vj); zeros(1,3), (Ae0i*vi2)'*(Aj*vj), (Ae1i*vi2)'*(Aj*vj), (Ae2i*vi2)'*(Aj*vj), (Ae3i*vi2)'*(Aj*vj); (Ai*vi1)', ((Ae0i*vi1)'*(qi(1:3)+Ai*ui-qj(1:3)- Aj*uj))+((Ai*vi1)'*(Ae0i*ui)), ((Ae1i*vi1)'*(qi(1:3)+Ai*ui- qj(1:3)-Aj*uj))+((Ai*vi1)'*(Ae1i*ui)), ((Ae2i*vi1)'*(qi(1:3)+Ai*ui-qj(1:3)- Aj*uj))+((Ai*vi1)'*(Ae2i*ui)), ((Ae3i*vi1)'*(qi(1:3)+Ai*ui- qj(1:3)-Aj*uj))+((Ai*vi1)'*(Ae3i*ui)); (Ai*vi2)', ((Ae0i*vi2)'*(qi(1:3)+Ai*ui-qj(1:3)- Aj*uj))+((Ai*vi2)'*(Ae0i*ui)), ((Ae1i*vi2)'*(qi(1:3)+Ai*ui- qj(1:3)-Aj*uj))+((Ai*vi2)'*(Ae1i*ui)), ((Ae2i*vi2)'*(qi(1:3)+Ai*ui-qj(1:3)- Aj*uj))+((Ai*vi2)'*(Ae2i*ui)), ((Ae3i*vi2)'*(qi(1:3)+Ai*ui- qj(1:3)-Aj*uj))+((Ai*vi2)'*(Ae3i*ui))]; Cq(4*MBody.ncar+4*(k-1)+8:4*MBody.ncar+4*(k-1)+8+3,7*(j- 1)+1:7*(j-1)+7)=[zeros(1,3) (Ai*vi1)'*(Ae0j*vj) (Ai*vi1)'*(Ae1j*vj) (Ai*vi1)'*(Ae2j*vj) (Ai*vi1)'*(Ae3j*vj); zeros(1,3) (Ai*vi2)'*(Ae0j*vj) (Ai*vi2)'*(Ae1j*vj) (Ai*vi2)'*(Ae2j*vj) (Ai*vi2)'*(Ae3j*vj); 69 Escuela Técnica Superior de Ingeniería de Sevilla -(Ai*vi1)' (Ai*vi1)'*(-Ae0j*uj) (Ai*vi1)'*(-Ae1j*uj) (Ai*vi1)'*(-Ae2j*uj) (Ai*vi1)'*(-Ae3j*uj); -(Ai*vi2)' (Ai*vi2)'*(-Ae0j*uj) (Ai*vi2)'*(-Ae1j*uj) (Ai*vi2)'*(-Ae2j*uj) (Ai*vi2)'*(-Ae3j*uj)]; end %% Añadimos el jacobiano de las restricciones de orientación. for k=2:MBody.nb-1 Cq(4*(MBody.ncar+MBody.ncil)+(k-2)+7+1,7*(k-1)+1:7*(k- 1)+7)=[zeros(1,3) 2*q(7+4+(k-2)*7:7+4+(k-2)*7+3,1)']; end %% Por último se añaden las ecuaciones de movilidad. if Ac==1 % % Prueba Cq(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+8,92)= 1; %Primera ecuación Cq(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+9,93)= 1; %Segunda ecuación Cq(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+10,94)= 1; %Tercera ecuación Cq(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+11,95)= 1; %Cuarta ecuación Cq(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+12,96)= 1; %Quinta ecuación Cq(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+13,97)= 1; %Sexta ecuación Cq(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+14,98)= 1; %Séptima ecuación elseif Ac==2 % Guiñada Cq(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+8,92)= 1; %Primera ecuación Cq(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+9,93)= 1; %Segunda ecuación Cq(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+10,94)= 1; %Tercera ecuación Cq(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+11,95)= 1; %Cuarta ecuación Cq(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+12,96)= 1; %Quinta ecuación Cq(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+13,97)= 1; %Sexta ecuación Cq(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+14,98)= 1; %Séptima ecuación Códigos. 70 elseif Ac==3 % Alabeo Cq(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+8,92)= 1; %Primera ecuación Cq(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+9,93)= 1; %Segunda ecuación Cq(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+10,94)= 1; %Tercera ecuación Cq(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+11,95)= 1; %Cuarta ecuación Cq(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+12,96)= 1; %Quinta ecuación Cq(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+13,97)= 1; %Sexta ecuación Cq(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+14,98)= 1; %Séptima ecuación elseif Ac==4 % Cabeceo Cq(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+8,92)= 1; %Primera ecuación Cq(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+9,93)= 1; %Segunda ecuación Cq(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+10,94)= 1; %Tercera ecuación Cq(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+11,95)= 1; %Cuarta ecuación Cq(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+12,96)= 1; %Quinta ecuación Cq(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+13,97)= 1; %Sexta ecuación Cq(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+14,98)= 1; %Séptima ecuación end end function Ct = dtRestr(q,Ac); global tiempo global MBody ngl=6; nrestr = 7 + 4*(MBody.ncil + MBody.ncar) + (MBody.nb-1); Ct=zeros(ngl+nrestr,1); if Ac==1 % Prueba Ct(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+8,1)= 0; %Primera ecuación Ct(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+9,1)= 0; %Segunda ecuación 71 Escuela Técnica Superior de Ingeniería de Sevilla Ct(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+10,1)= - 20*cos(5*tiempo); %Tercera ecuación Ct(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+11,1)= 0; %Cuarta ecuación Ct(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+12,1)= 0; %Quinta ecuación Ct(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+13,1)= 0; %Sexta ecuación Ct(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+14,1)= 0; %Séptima ecuación elseif Ac==2 % Guiñada Ct(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+8,1)= 0; %Primera ecuación Ct(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+9,1)= 0; %Segunda ecuación Ct(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+10,1)= 0; %Tercera ecuación Ct(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+11,1)= sin(pi/18/2*sin(5*tiempo))*pi/18/2*cos(5*tiempo)*5; %Cuarta ecuación Ct(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+12,1)= 0; %Quinta ecuación Ct(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+13,1)= 0; %Sexta ecuación Ct(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+14,1)= - cos(pi/18/2*sin(5*tiempo))*pi/18/2*cos(5*tiempo)*5; %Séptima ecuación elseif Ac==3 % Alabeo Ct(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+8,1)= 0; %Primera ecuación Ct(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+9,1)= 0; %Segunda ecuación Ct(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+10,1)= 0; %Tercera ecuación Ct(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+11,1)= sin(pi/18/2*sin(5*tiempo))*pi/18/2*cos(5*tiempo)*5; %Cuarta ecuación Ct(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+12,1)= - cos(pi/18/2*sin(5*tiempo))*pi/18/2*cos(5*tiempo)*5; %Quinta ecuación Ct(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+13,1)= 0; %Sexta ecuación Ct(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+14,1)= 0; %Séptima ecuación elseif Ac==4 Códigos. 72 % Cabeceo Ct(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+8,1)= 0; %Primera ecuación Ct(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+9,1)= 0; %Segunda ecuación Ct(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+10,1)= 0; %Tercera ecuación Ct(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+11,1)= sin(pi/18/2*sin(5*tiempo))*pi/18/2*cos(5*tiempo)*5; %Cuarta ecuación Ct(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+12,1)= 0; %Quinta ecuación Ct(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+13,1)= - cos(pi/18/2*sin(5*tiempo))*pi/18/2*cos(5*tiempo)*5; %Sexta ecuación Ct(4*(MBody.ncar+MBody.ncil)+(MBody.nb-2)+14,1)= 0; %Séptima ecuación end end function DCq = DtJacobiano(q,v,nrestr,Ac); global MBody %% Inicializa jacobiano con dimensión (nrest+ngl)x(nrest+ngl) porque añadimos a las ecuaciones de restricción tantas ecuaciones de movilidad como gdl tenga el mecanismo. ngl=6; DCq=zeros(nrestr+ngl); %% Barra fija C=zeros(7); DCq(1:7,1:7)=C; %% Pares Cardan for k=1:MBody.ncar i=MBody.Pares.Car(k).I; j=MBody.Pares.Car(k).J; uPi=MBody.Pares.Car(k).uPi; uPj=MBody.Pares.Car(k).uPj; si=MBody.Pares.Car(k).si; sj=MBody.Pares.Car(k).sj; qi = q(7*(i-1)+1:7*(i-1)+7,1); qj = q(7*(j-1)+1:7*(j-1)+7,1); Ai =RotMat(qi(4:7)); Aj =RotMat(qj(4:7)); Ae0i = RotMat0(qi(4:7)); 73 Escuela Técnica Superior de Ingeniería de Sevilla Ae1i = RotMat1(qi(4:7)); Ae2i = RotMat2(qi(4:7)); Ae3i = RotMat3(qi(4:7)); Ae0j = RotMat0(qj(4:7)); Ae1j = RotMat1(qj(4:7)); Ae2j = RotMat2(qj(4:7)); Ae3j = RotMat3(qj(4:7)); Ae00i = RotMat00(qi(4:7)); Ae11i = RotMat11(qi(4:7)); Ae22i = RotMat22(qi(4:7)); Ae33i = RotMat33(qi(4:7)); Ae00j = RotMat00(qj(4:7)); Ae11j = RotMat11(qj(4:7)); Ae22j = RotMat22(qj(4:7)); Ae33j = RotMat33(qj(4:7)); DCq(4*(k-1)+8:4*(k-1)+8+3,7*(i-1)+1:7*(i-1)+7)=[zeros(3) Ae00i*uPi Ae11i*uPi Ae22i*uPi Ae33i*uPi; zeros(1,3) (Ae00i*si)'*(Aj*sj) (Ae11i*si)'*(Aj*sj) (Ae22i*si)'*(Aj*sj) (Ae33i*si)'*(Aj*sj)]; DCq(4*(k-1)+8:4*(k-1)+8+3,7*(j-1)+1:7*(j-1)+7)=[-zeros(3) - Ae00j*uPj -Ae11j*uPj -Ae22j*uPj - Ae33j*uPj; zeros(1,3) (Ai*si)'*(Ae00j*sj) (Ai*si)'*(Ae11j*sj) (Ai*si)'*(Ae22j*sj) (Ai*si)'*(Ae33j*sj)]; end %% Pares Cilíndricos. for k=1:MBody.ncil i=MBody.Pares.Cil(k).I; j=MBody.Pares.Cil(k).J; ui=MBody.Pares.Cil(k).ui; uj=MBody.Pares.Cil(k).uj; vi=MBody.Pares.Cil(k).vi; vj=MBody.Pares.Cil(k).vj; vec=[1 0 0]'; vi1=cross(vec,vi); vi2=cross(vi,vi1); qi = q(7*(i-1)+1:7*(i-1)+7,1); qj = q(7*(j-1)+1:7*(j-1)+7,1); Ai =RotMat(qi(4:7,1)); Aj =RotMat(qj(4:7,1)); Ae0i = RotMat0(qi(4:7,1)); Ae1i = RotMat1(qi(4:7,1)); Ae2i = RotMat2(qi(4:7,1)); Códigos. 80 for i = 1:length(tspan) %PROBLEMA DE POSICION tiempo=tspan(i); q(:,i) = fsolve(@Restricciones,q0,optimset('Display','on')); Cq = Jacobiano(q(:,i),nrestr,Ac); Ct = dtRestr(q(:,i),Ac);%,tspan(i)); %CALCULO DE VELOCIDADES v(:,i) = -Cq\Ct; DCq = DtJacobiano(q(:,i),v(:,i),nrestr,Ac); DCt = DtdtRestr(q(:,i),v(:,i),nrestr,Ac); % %CALCULO DE ACELERACIONES a(:,i) = -Cq\(DCq*v(:,i)+DCt); q0 = q(:,i); end p=1; for k=1:MBody.nb figure(p) plot(tspan,v(1+7*(k-1),:),'g') hold on plot(tspan,v(2+7*(k-1),:),'b') plot(tspan,v(3+7*(k-1),:),'r') xlabel('t(s)') ylabel('v(cm/s)') title(['Velocidades solido ', num2str(k)]) legend('Eje x','Eje y','Eje z') % filename=['Vel_' num2str(k) '.png']; % saveas([p],filename) p=p+1; end for k=1:MBody.nb figure(p) plot(tspan,a(1+7*(k-1),:),'g') hold on plot(tspan,a(2+7*(k-1),:),'b') plot(tspan,a(3+7*(k-1),:),'r') xlabel('t(s)') ylabel('a(cm/s^2)') title(['Aceleraciones solido ', num2str(k)]) legend('Eje x','Eje y','Eje z') % filename=['Ac_' num2str(k) '.png']; % saveas([p],filename) p=p+1; end for k=8:MBody.nb %Para ver el desfase de aceleración y velocidad. figure(p) plot(tspan,v(3+7*(k-1),:),'r') hold on 81 Escuela Técnica Superior de Ingeniería de Sevilla plot(tspan,a(3+7*(k-1),:),'b') xlabel('t(s)') ylabel('a(cm/s^2),v(cm/s)') title(['Desfases solido ', num2str(k)]) legend('Velocidad (cm/s)','Aceleración (cm/s^2)') % filename=['Des_' num2str(k) '.png']; % saveas([p],filename) p=p+1; end % figure % AnimaStewart(q) 4.2.6 Animación del mecanismo. %% Programa de animación del mecanismo. function AnimaStewart(q) global tspan %% Animación Plataforma de Stewart. np=length(tspan); nc=1; %% Definición de los puntos notables del mecanismo. % Barras pegadas a la plataforma fija x02 = q(8,:); y02 = q(9,:); z02 = q(10,:); x03 = q(15,:); y03 = q(16,:); z03 = q(17,:); x04 = q(22,:); y04 = q(23,:); z04 = q(24,:); x05 = q(29,:); y05 = q(30,:); z05 = q(31,:); x06 = q(36,:); y06 = q(37,:); z06 = q(38,:); x07 = q(43,:); y07 = q(44,:); z07 = q(45,:); % Barras pegadas a la plataforma móvil. x08 = q(50,:); y08 = q(51,:); z08 = q(52,:); x09 = q(57,:); Códigos. 82 y09 = q(58,:); z09 = q(59,:); x10 = q(64,:); y10 = q(65,:); z10 = q(66,:); x11 = q(71,:); y11 = q(72,:); z11 = q(73,:); x12 = q(78,:); y12 = q(79,:); z12 = q(80,:); x13 = q(85,:); y13 = q(86,:); z13 = q(87,:); %% Definicion de las lineas a dibujar. % Barras. x1 = [x03; x08]; y1 = [y03; y08]; z1 = [z03; z08]; x2 = [x04; x09]; y2 = [y04; y09]; z2 = [z04; z09]; x3 = [x05; x10]; y3 = [y05; y10]; z3 = [z05; z10]; x4 = [x06; x11]; y4 = [y06; y11]; z4 = [z06; z11]; x5 = [x07; x12]; y5 = [y07; y12]; z5 = [z07; z12]; x6 = [x02; x13]; y6 = [y02; y13]; z6 = [z02; z13]; % Plataforma fija. x7 = [x02; x03; x04; x05; x06; x07; x02]; y7 = [y02; y03; y04; y05; y06; y07; y02]; z7 = [z02; z03; z04; z05; z06; z07; z02]; % Plataforma móvil. x8 = [x08; x09; x10; x11; x12; x13; x08]; y8 = [y08; y09; y10; y11; y12; y13; y08]; 83 Escuela Técnica Superior de Ingeniería de Sevilla z8 = [z08; z09; z10; z11; z12; z13; z08]; %************************************* for j=1:nc, for i=2:(np), % Se pintan las seis barras plot3(x1(:,i),y1(:,i),z1(:,i),'b') hold on plot3(x2(:,i),y2(:,i),z2(:,i),'b') plot3(x3(:,i),y3(:,i),z3(:,i),'b') plot3(x4(:,i),y4(:,i),z4(:,i),'b') plot3(x5(:,i),y5(:,i),z5(:,i),'b') plot3(x6(:,i),y6(:,i),z6(:,i),'b') % Se pintan la plataforma fija y móvil plot3(x7(:,i),y7(:,i),z7(:,i),'k') plot3(x8(:,i),y8(:,i),z8(:,i),'r') hold off % Fijamos los ejes para evitar la distorsión y establecemos un pause % para que de tiempo a verlo. Poner el mismo rango para que no se % distorsione figura (circulo y no elipse) axis([-75 75 -75 75 0 150]) axis square off pause(.1) end end end 4.2.7 Matrices de rotación. function A=RotMat(x) I=eye(3); theta=[0 -x(4) x(3);x(4) 0 -x(2);-x(3) x(2) 0]; A=I+2*theta*(I*x(1)+theta); end function Ae0=RotMat0(x) Ae0=[0 -2*x(4) 2*x(3); 2*x(4) 0 -2*x(2); -2*x(3) 2*x(2) 0]; end Códigos. 84 function Ae1=RotMat1(x) Ae1=[0 2*x(3) 2*x(4); 2*x(3) -4*x(2) -2*x(1); 2*x(4) 2*x(1) -4*x(2)]; end function Ae2=RotMat2(x) Ae2=[-4*x(3) 2*x(2) 2*x(1); 2*x(2) 0 2*x(4); -2*x(1) 2*x(4) -4*x(3)]; end function Ae3=RotMat3(x) Ae3=[-4*x(4) -2*x(1) 2*x(2); 2*x(1) -4*x(4) 2*x(3); 2*x(2) 2*x(3) 0]; end function Ae00=RotMat00(x) Ae00=zeros(3); end function Ae11=RotMat11(x) Ae11=[0 0 0; 0 -4 0; 0 0 -4]; end function Ae22=RotMat22(x) Ae22=[-4 0 0; 0 0 0; 0 0 -4]; end function Ae33=RotMat33(x) 85 Escuela Técnica Superior de Ingeniería de Sevilla Ae33=[-4 0 0; 0 -4 0; 0 0 0]; end 4.2.8 Dinámica inversa plataforma de Stewart. %% Programa que calcula la dinámica inversa. Método 2, sin cambiar los 4 parámetros de Euler por los 3 ángulos. global MBody tspan ngl=6; nrestr =7+4*(MBody.ncil+MBody.ncar)+(MBody.nb-1); rho=2700; %Densidad aluminio (Kg/m^3) h=0.05; %Espesor de la plataforma (m) d=0.015; %Diámetro de las barras (m) g=9.81; %Gravedad (m/s^2) Ac=input('¿En qué actuación estamos?: '); %% Longitudes de las barras. (En m) D1=1; L2=0.3; L3=0.3; L4=0.3; L5=0.3; L6=0.3; L7=0.3; L8=0.3; L9=0.3; L10=0.3;L11=0.3;L12=0.3;L13=0.3;D14=1; %% Cálculos de masas (En kg) M1=pi*D1^2/4*h*rho; M2=pi*d^2/4*L2*rho; M3=pi*d^2/4*L3*rho; M4=pi*d^2/4*L4*rho; M5=pi*d^2/4*L5*rho; M6=pi*d^2/4*L6*rho; M7=pi*d^2/4*L7*rho; M8=pi*d^2/4*L8*rho; M9=pi*d^2/4*L9*rho; M10=pi*d^2/4*L10*rho; M11=pi*d^2/4*L11*rho; M12=pi*d^2/4*L12*rho; M13=pi*d^2/4*L13*rho; M14=pi*D14^2/4*h*rho; %% Cálculos de inercias. (Kg m^2 ) I1_x=1/4*M1*(D1/2)^2; I1_y=1/4*M1*(D1/2)^2; I1_z=1/2*M1*(D1/2)^2; I2_x=1/12*M2*(3*(d/2)^2+L2^2); I2_y=1/12*M2*(3*(d/2)^2+L2^2); I2_z=1/2*M2*(d/2)^2; I3_x=1/12*M3*(3*(d/2)^2+L3^2); I3_y=1/12*M3*(3*(d/2)^2+L3^2); I3_z=1/2*M3*(d/2)^2; I4_x=1/12*M4*(3*(d/2)^2+L4^2); I4_y=1/12*M4*(3*(d/2)^2+L4^2); I4_z=1/2*M4*(d/2)^2; I5_x=1/12*M5*(3*(d/2)^2+L5^2); I5_y=1/12*M5*(3*(d/2)^2+L5^2); I5_z=1/2*M5*(d/2)^2; I6_x=1/12*M6*(3*(d/2)^2+L6^2); I6_y=1/12*M6*(3*(d/2)^2+L6^2); I6_z=1/2*M6*(d/2)^2; I7_x=1/12*M7*(3*(d/2)^2+L7^2); I7_y=1/12*M7*(3*(d/2)^2+L7^2); I7_z=1/2*M7*(d/2)^2; I8_x=1/12*M8*(3*(d/2)^2+L8^2); I8_y=1/12*M8*(3*(d/2)^2+L8^2); I8_z=1/2*M8*(d/2)^2; I9_x=1/12*M9*(3*(d/2)^2+L9^2); I9_y=1/12*M9*(3*(d/2)^2+L9^2); I9_z=1/2*M9*(d/2)^2; I10_x=1/12*M10*(3*(d/2)^2+L10^2); I10_y=1/12*M10*(3*(d/2)^2+L10^2); I10_z=1/2*M10*(d/2)^2; Códigos. 86 I11_x=1/12*M11*(3*(d/2)^2+L11^2); I11_y=1/12*M11*(3*(d/2)^2+L11^2); I11_z=1/2*M11*(d/2)^2; I12_x=1/12*M12*(3*(d/2)^2+L12^2); I12_y=1/12*M12*(3*(d/2)^2+L12^2); I12_z=1/2*M12*(d/2)^2; I13_x=1/12*M13*(3*(d/2)^2+L13^2); I13_y=1/12*M13*(3*(d/2)^2+L13^2); I13_z=1/2*M13*(d/2)^2; I14_x=1/4*M14*(D14/2)^2; I14_y=1/4*M14*(D14/2)^2; I14_z=1/2*M14*(D14/2)^2; I=eye(3); %Matriz identidad de orden 3 %% Vector con todas las masas e inercias del mecanismo. En orden. Las 6 primeras corresponden al sólido 1, 3 masas y 3 inercias x, y y z MI=[M1 M1 M1 I1_x I1_y I1_z M2 M2 M2 I2_x I2_y I2_z M3 M3 M3 I3_x I3_y I3_z M4 M4 M4 I4_x I4_y I4_z M5 M5 M5 I5_x I5_y I5_z M6 M6 M6 I6_x I6_y I6_z M7 M7 M7 I7_x I7_y I7_z M8 M8 M8 I8_x I8_y I8_z M9 M9 M9 I9_x I9_y I9_z M10 M10 M10 I10_x I10_y I10_z M11 M11 M11 I11_x I11_y I11_z M12 M12 M12 I12_x I12_y I12_z M13 M13 M13 I13_x I13_y I13_z M14 M14 M14 I14_x I14_y I14_z]'; %% Una vez se tiene el vector, se monta la matriz M y la g. M=zeros(7*MBody.nb); %La matriz tendrá 7 filas y columnas por cada sólido. g0=zeros(7*MBody.nb,1); %% Comenzamos el bucle del tiempo, donde para cada instante de tiempo se calculan las matrices B,h,b y gamma. lambda=zeros(98,length(tspan)); lambda1=zeros(98,length(tspan)); F_x=zeros(98,length(tspan)); F_y=zeros(98,length(tspan)); F_z=zeros(98,length(tspan)); F_e0=zeros(98,length(tspan)); F_e1=zeros(98,length(tspan)); F_e2=zeros(98,length(tspan)); F_e3=zeros(98,length(tspan)); for i=1:length(tspan) for k=1:MBody.nb M(1+7*(k-1):3+7*(k-1),1+7*(k-1):3+7*(k-1))=MI(1+6*(k-1):3+6*(k- 1)).*I; J=(MI(4+6*(k-1):6+6*(k-1)).*I); L=MatL(q(4+7*(k-1):7+7*(k-1),1)); M(4+7*(k-1):7+7*(k-1),4+7*(k-1):7+7*(k-1))=4*L'*J*L; if k==1 || k==14 % Este if se puede hacer así porque las masas de todas las barras son iguales, y las de la plataforma también. g0(3+7*(k-1),1)=-M1*g; else g0(3+7*(k-1),1)=-M2*g; end L_punto=MatL(v(4+7*(k-1):7+7*(k-1),1)); g0(4+7*(k-1):7+7*(k-1),1)=8*L_punto'*J*L_punto*q(4+7*(k-1):7+7*(k- 1),1); end 87 Escuela Técnica Superior de Ingeniería de Sevilla Cq=Jacobiano(q(:,i),nrestr,Ac); lambda(:,i)=Cq'\(g0-M*a(:,i)); %% Ahora se va a calcular la fuerza en los pistones aplicando el TPV. % Sacar las fuerzas de la plataforma móvil (Fx,Fy,Fz,Fe0,Fe1,Fe2,Fe3). % Fx lambda1(92,i)=lambda(92,i); F_x(:,i)=Cq'*lambda1(:,i); lambda1(92,i)=0; % Fy lambda1(93,i)=lambda(93,i); F_y(:,i)=Cq'*lambda1(:,i); lambda1(93,i)=0; % Fz lambda1(94,i)=lambda(94,i); F_z(:,i)=Cq'*lambda1(:,i); lambda1(94,i)=0; % Fe0 lambda1(95,i)=lambda(95,i); F_e0(:,i)=Cq'*lambda1(:,i); lambda1(95,i)=0; % Fe1 lambda1(96,i)=lambda(96,i); F_e1(:,i)=Cq'*lambda1(:,i); lambda1(96,i)=0; % Fe2 lambda1(97,i)=lambda(97,i); F_e2(:,i)=Cq'*lambda1(:,i); lambda1(97,i)=0; % Fe3 lambda1(98,i)=lambda(98,i); F_e3(:,i)=Cq'*lambda1(:,i); lambda1(98,i)=0; end % figure % plot(tspan,F_x(92,:)) % hold on % plot(tspan,F_y(93,:)) % plot(tspan,F_z(94,:)) % xlabel('t(s)') % ylabel('F(N)') % title('Fuerzas plataforma móvil.') % legend('F_x (N)','F_y (N)','F_z (N)') % filename=['Fuerza_Fija.png']; % saveas([1],filename) % plot(tspan,F_e0(95,:)) % plot(tspan,F_e1(96,:)) % plot(tspan,F_e2(97,:)) % plot(tspan,F_e3(98,:)) % % title('Fuerza') % xlabel('Tiempo (s)') % ylabel('Fuerza (N)') % legend('F_x','F_y','F_z','F_e0','F_e1','F_e2','F_e3') Códigos. 88 % % filename=['Fuerza_Z.png']; % % saveas([1],filename) % Calculamos ahora los "deltas" que multiplican a las fuerzas de los pistones. F213=zeros(1,91); F38=zeros(1,91); F49=zeros(1,91); F510=zeros(1,91); F611=zeros(1,91); F712=zeros(1,91); Res_213=zeros(7,length(tspan)); Res_38=zeros(7,length(tspan)); Res_49=zeros(7,length(tspan)); Res_510=zeros(7,length(tspan)); Res_611=zeros(7,length(tspan)); Res_712=zeros(7,length(tspan)); for i=1:length(tspan) Cq=Jacobiano(q(:,i),nrestr,Ac); Cqd=Cq(1:91,1:91); Cqi=Cq(1:91,92:98); deltaqd=-(Cqd)\Cqi; %Pistón 2-13 At=RotMat(q(11:14,i)); r1=q(8,i); r2=q(9,i); r3=q(10,i); e0=q(11,i); e1=q(12,i); e2=q(13,i); e3=q(14,i); p1=q(85,i); p2=q(86,i); p3=q(87,i); F213(1,8:10)=[At(3,1) At(3,2) At(3,3)]; F213(1,85:87)=[-At(3,1) -At(3,2) -At(3,3)]; F213(1,11:14)=2*[At(3,1)*(-e2*(p3-r3)+e3*(p2-r2))+At(3,2)*(e1*(p3- r3)-e3*(p1-r1))+At(3,3)*(-e1*(p2-r2)+e2*(p1-r1)); At(3,1)*(e3*(p3-r3)-e2*(p2-r2))+At(3,2)*(-e0*(p3- r3)-e2*(p1-r1))+At(3,3)*(e0*(p2-r2)-e3*(p1-r1)); At(3,1)*(e0*(p3-r3)-e1*(p2-r2))+At(3,2)*(-e3*(p3- r3)+e1*(p1-r1))+At(3,3)*(-e3*(p2-r2)+e0*(p1-r1)); At(3,1)*(-e1*(p3-r3)+e0*(p2-r2))+At(3,2)*(-e2*(p3- r3)+e0*(p1-r1))+At(3,3)*(e2*(p2-r2)+e1*(p1-r1))]'; Res_213(:,i)=(F213*deltaqd)'; %Vector de 7*1 que multiplica a las "deltas" independientes. Sale de multiplicar 1*91 x 91*7 y trasponer %Pistón 3-8 At=RotMat(q(18:21,i)); r1=q(15,i); r2=q(16,i); r3=q(17,i); e0=q(18,i); 89 Escuela Técnica Superior de Ingeniería de Sevilla e1=q(19,i); e2=q(20,i); e3=q(21,i); p1=q(50,i); p2=q(51,i); p3=q(52,i); F38(1,15:17)=[At(3,1) At(3,2) At(3,3)]; F38(1,50:52)=[-At(3,1) -At(3,2) -At(3,3)]; F38(1,18:21)=2*[At(3,1)*(-e2*(p3-r3)+e3*(p2-r2))+At(3,2)*(e1*(p3- r3)-e3*(p1-r1))+At(3,3)*(-e1*(p2-r2)+e2*(p1-r1)); At(3,1)*(e3*(p3-r3)-e2*(p2-r2))+At(3,2)*(-e0*(p3- r3)-e2*(p1-r1))+At(3,3)*(e0*(p2-r2)-e3*(p1-r1)); At(3,1)*(e0*(p3-r3)-e1*(p2-r2))+At(3,2)*(-e3*(p3- r3)+e1*(p1-r1))+At(3,3)*(-e3*(p2-r2)+e0*(p1-r1)); At(3,1)*(-e1*(p3-r3)+e0*(p2-r2))+At(3,2)*(-e2*(p3- r3)+e0*(p1-r1))+At(3,3)*(e2*(p2-r2)+e1*(p1-r1))]'; Res_38(:,i)=(F38*deltaqd)'; %Pistón 4-9 At=RotMat(q(25:28,i)); r1=q(22,i); r2=q(23,i); r3=q(24,i); e0=q(25,i); e1=q(26,i); e2=q(27,i); e3=q(28,i); p1=q(57,i); p2=q(58,i); p3=q(59,i); F49(1,22:24)=[At(3,1) At(3,2) At(3,3)]; F49(1,57:59)=[-At(3,1) -At(3,2) -At(3,3)]; F49(1,25:28)=2*[At(3,1)*(-e2*(p3-r3)+e3*(p2-r2))+At(3,2)*(e1*(p3- r3)-e3*(p1-r1))+At(3,3)*(-e1*(p2-r2)+e2*(p1-r1)); At(3,1)*(e3*(p3-r3)-e2*(p2-r2))+At(3,2)*(-e0*(p3- r3)-e2*(p1-r1))+At(3,3)*(e0*(p2-r2)-e3*(p1-r1)); At(3,1)*(e0*(p3-r3)-e1*(p2-r2))+At(3,2)*(-e3*(p3- r3)+e1*(p1-r1))+At(3,3)*(-e3*(p2-r2)+e0*(p1-r1)); At(3,1)*(-e1*(p3-r3)+e0*(p2-r2))+At(3,2)*(-e2*(p3- r3)+e0*(p1-r1))+At(3,3)*(e2*(p2-r2)+e1*(p1-r1))]'; Res_49(:,i)=(F49*deltaqd)'; %Pistón 5-10 At=RotMat(q(32:35,i)); r1=q(29,i); r2=q(30,i); r3=q(31,i); e0=q(32,i); e1=q(33,i); e2=q(34,i); e3=q(35,i); p1=q(64,i); p2=q(65,i); p3=q(66,i); F510(1,29:31)=[At(3,1) At(3,2) At(3,3)]; F510(1,64:66)=[-At(3,1) -At(3,2) -At(3,3)];