Simulación cinemática y dinámica de un vehículo controlado por control remoto en tres dimensiones

Equation Chapter 1 Section 1 Proyecto Trabajo Fin de Grado Grado en Ingeniería en Tecnologías Industriales Simulación cinemática y dinámica de un vehículo controlado por control remoto en tres dimensiones Autor: Alberto González González Tutor: Juana María Mayo Nuñez Dpto. Ingeniería Mecánica y Fabricación Escuela Técnica Superior de Ingeniería Universidad de Sevilla Sevilla, 2018 Proyecto Trabajo Fin de Grado Grado en Ingeniería en Tecnologías Industriales Simulación cinemática y dinámica de un vehículo controlado por control remoto en tres dimensiones Autor: Alberto González González Tutor: Juana María Mayo Nuñez ii iii Proyecto Trabajo Fin de Grado Grado en Ingeniería en Tecnologías Industriales Simulación cinemática y dinámica de un vehículo controlado por control remoto en tres dimensiones Autor: Alberto González González Tutor: Juana María Mayo Nuñez Catedrática Dpto. de Ingeniería Mecánica y Fabricación Escuela Técnica Superior de Ingeniería Universidad de Sevilla Sevilla, 2018 iv v Proyecto Trabajo de Fin de Grado: Simulación cinemática y dinámica de un vehículo controlado por control remoto en tres dimensiones Autor: Alberto González González Tutor: Juana María Mayo Nuñ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, 2018 El Secretario del Tribunal vi vii A mi familia A mis maestros viii ix Agradecimientos Me gustaría agradecer a Dª Juana María Mayo Nuñez por darme la oportunidad de realizar el presente trabajo de fin de grado con ella, así como por haberme ayudado y guiado a lo largo de todo el proceso. A la par me gustaría agradecer a mi familia, por el apoyo que he recibido durante toda la carrera ya no habría sido posible si no fuera por ellos. xvi ÍNDICE DE TABLAS Tabla 1. Comparativa coordenadas y ecuaciones en el modelo completo. Tabla 2. Comparativa coordenadas y ecuaciones del modelo simplificado. Tabla 3. Error ángulo trayectoria vertical respecto ángulo carretera. Tabla 4. Errores ángulo cabeceo respecto ángulo carretera. xvii ÍNDICE DE FIGURAS Figura 1. Ángulo de Euler.!! Figura 2. Modelo Rover en 3D.! Figura 3. Dimensiones características del Rover.! Figura 4. Superficie parametrizada.! Figura 5. Trayectorias verticales modelo simplificado. Figura 6. Ángulos de cabeceo modelo simplificado. Figura 7. Trayectorias sobre plano XY modelo simplificado. Figura 8. Trayectorias sobre plano XY modelo completo. Figura 9. Trayectorias verticales modelo completo. Figura 10. Ángulos de cabeceo modelo completo Figura 11. Ángulos de balanceo modelo completo Figura 12. Trayectoria vertical con tres tramos a pendientes diferentes Figura 13. Rover y control remoto Figura 14. Ángulos girados por las ruedas Figura 15. Velocidades de las ruedas Figura 16. Aceleraciones de las ruedas Figura 17. Rampa utilizada para la simulación Figura 18. Trayectoria vertical experimental Figura 19. Ángulo cabeceo experimental Figura 20. Ángulo balanceo experimental Figura 21. Trayectoria sobre el plano XY experimental Figura 22. Velocidades angulares de las ruedas Figura 23. Velocidad angular IMU frente a la calculada Figura 24. Aceleración IMU frente calculada xviii xix Notación XYZ Sistema de coordenadas globales q Coordenadas generalizadas q Velocidades generalizadas q Aceleraciones generalizadas A!! Inversa de la matriz A A! Traspuesta de la matriz A dep dependientes ind independientes C Ecuaciones de restricción Cq Jacobiano de las ecuaciones de restricción holónomas Ct Derivada parcial de las ecuaciones de restricción holónomas con respecto al tiempo B Jacobiano de las ecuaciones de restricción no holónomas D Matriz de los jacobianos de las ecuaciones de restricción no holónomas y holónomas E Matriz de las derivadas parciales de las ecuaciones de restricción no holónomas y holónomas con respecto al tiempo 𝑓(x) Derivada parcial de la función f respecto a x 𝑓(x) Derivada temporal de la función f 𝐹! Fuerzas aplicadas sobre el sólido i 𝑚! Masa del sólido i 𝑟! Posición del centro de gravedad del sólido i 𝑣! Velocidad del centro de gravedad del sólido i a! ! Aceleración del centro de gravedad del sólido i 𝑥 Vector x en coordenadas locales 𝑀! Momentos aplicados sobre el sólido i 𝐿 Momento cinético 𝑤 Velocidad angular 𝛼 Aceleración angular 𝐼! Tensor de inercias aplicado en el centro de gravedad 𝑅! Radio ruedas delanteras 𝑅! Radio rueda trasera 𝑡!! Tangente sobre la superficie de la carretera en dirección x 𝑡!! Tangente sobre la superficie de la carretera en dirección y 𝑥 Módulo de x × Producto escalar sen Función seno xx cos Función coseno 1 1 INTRODUCCIÓN a cinemática y dinámica de vehículos trata con las ecuaciones que permiten describir los movimientos realizados por los vehículos considerando las fuerzas que los producen. Mediante la simulación se pueden obtener estos movimientos en instantes sucesivos de tiempo. El uso de la simulación de vehículos ha ganado importancia a lo largo de los años debido a la gran competencia existente actualmente dentro de la industria automovilística, así como los complejos procesos de calidad que cada vehículo tiene que superar con el objetivo de poder ser puestos a la venta. Dado estas condiciones, la industria automovilística invierte en la creación de sistemas de software para la simulación de prototipos con los que consiguen reducir en gran parte los costes de prueba y error. [1] El origen de este trabajo se sitúa en las prácticas de la asignatura de Cinemática y Dinámica de máquinas en las que realizamos la simulación de un vehículo motorizado, conocido con el nombre de Rover, con la singularidad de ser en el caso plano. El procedimiento propuesto para su desarrollo constaba de tres fases: a) procesado de los datos obtenidos por el conjunto de sensores y la identificación del centro de gravedad del vehículo, b) simulación cinemática del vehículo y c) simulación dinámica inversa junto con una serie de comprobaciones que pusieran en relación los datos obtenidos con los sensores con los valores de la simulación dinámica. En el modelo de dos dimensiones explicado con anterioridad, las únicas ecuaciones de restricción de las que disponía el sistema eran las ecuaciones debido a la condición de no deslizamiento de las ruedas, siendo éstas restricciones no holónomas. Este conocimiento de base nos ha servido para el desarrollo del modelo de tres dimensiones propuesto; incluyendo un mayor número de coordenadas y de ecuaciones de restricción, manteniendo el mismo número de grados de libertad, es decir, los datos de entrada a nuestro problema, siendo en este caso los ángulos girados por las ruedas motoras. Las limitaciones de nuestro trabajo residen en los grados de libertad de los que dispone el Rover, y de las limitaciones que nos proporcionan el número de sólidos y las uniones rígidas entre sólidos, ya que el uso de un vehículo con un mayor número de sólidos y uniones no rígidas entre ruedas y chasis nos permitiría obtener resultados cuyo comportamiento se asimilará mejor al de un vehículo actual. Por ello, para futuros trabajos se propone la elaboración de un Rover con cuatro ruedas y un eje de giro de las ruedas independientes al chasis. 1.1 Objetivos El objetivo principal que se pretende conseguir con este Trabajo de Fin de Grado es la simulación cinemática y dinámica del comportamiento dinámico del rover en tres dimensiones. Para ello, se va a hacer uso de las ecuaciones utilizadas para la simulación de sistemas multicuerpos y se van a analizar los resultados obtenidos en la simulación con los trayectos realizados experimentalmente. Tanto los parámetros dimensionales como los parámetros de masa, inercia y centros de gravedad de cada uno de los sólidos han sido obtenidos del estudio previo realizado en la asignatura descrita con anterioridad, de forma que la explicación de cómo se han obtenido esos resultados no forma parte de los objetivos del presente trabajo. La simulación y los cálculos correspondientes se van a realizar con el programa de cálculo matemático Matlab L Introducción 2 2 versión R2016a. 1.2 Estructura En este trabajo se presenta en primer lugar un bloque teórico, en el que se abordan los conceptos fundamentales sobre sistemas multicuerpos utilizados para la resolución del problema. A continuación, se desarrolla la cinemática y dinámica de los distintos modelos utilizados en el Rover, un modelo completo y un modelo simplificado, basándose en las ecuaciones descritas en el bloque anterior. Con mayor concreción en el tercer capítulo se describen los programas generados para la simulación junto con un análisis de los resultados analíticos obtenidos antes de la experimentación con el Rover que tiene como objetivo servir como comprobación para asegurar los resultados experimentales. Finalmente un último capitulo en el que se presenta el tratamiento de los datos registrados y los resultados de la experimentación realizada con el Rover.Los resultados obtenidos nos permiten establecer las conclusiones del trabajo desarrollado así como plantear en un sentido prospectivo nuevos desarrollos dentro de este campo. 3 2 SISTEMAS MULTICUERPO 2.1 Introducción Un sistema multicuerpo puede definirse como un conjunto de solidos rígidos o deformables unidos entre sí por pares cinemáticos y actuadores, permitiendo el movimiento relativo entre ellos.[2] Esta definición también podría aplicarse a la de un mecanismo, sin embargo, en este caso es utilizada para referirse a sistemas de mayor complejidad como pude ser un propio vehículo.[3] Los sólidos rígidos de un sistema multicuerpo se caracterizan por su masa y momento de inercia, y los puntos característicos son su centro de gravedad y los puntos en los que fuerzas o momentos están aplicados, así como los puntos de unión con el resto de los sólidos. [4] Los algoritmos y ecuaciones utilizados para la resolución están pensados para ser resueltos computacionalmente, en caso de no ser así, éstas ecuaciones cambiarían totalmente. A su vez, a la hora de entender el desarrollo de las ecuaciones es importante conocer que el funcionamiento de un ordenador difiere con el de un cerebro humano y que el éxito de la resolución de este sistema reside en que el proceso sea sistemático.[3] 2.2 Tipos de coordenadas La selección del tipo de coordenadas es un parámetro esencial en términos de la sistematicidad de la que van a disponer nuestros algoritmos. Por ello, podemos definir los siguientes tipos de coordenadas: • Coordenadas Lagrangianas: Son un conjunto de magnitudes geométricas, distancias o ángulos que indican la posición u orientación de cada uno de los sólidos con respecto de la barra fija. [3] • Coordenadas relativas: La posición u orientación de los sólidos están definidos respecto al miembro contiguo, de forma que conocida las coordenadas de uno de ellos respecto al sistema global, podemos obtener la posición del resto de sólidos de nuestro sistema. Por tanto, se puede decir que las coordenadas relativas están asociadas a los pares cinemáticos del sistema.[3] • Coordenadas de referencia: Consiste en la identificación del centro de gravedad de cada uno de los sólidos con respecto al sistema global XYZ y la orientación de cada uno de los ejes locales con respecto al sistema global. De forma que tendríamos 6 coordenadas (3 posiciones y 3 ángulos) por cada sólido en el caso en tres dimensiones, y 3 coordenadas en el caso plano (2 posiciones y 1 ángulo) • Coordenadas naturales: Surgen como evolución de las coordenadas de referencia en las que en lugar de ser introducidas en los centros de gravedad de cada sólido, éstas coordenadas son fijadas con la posición de los puntos característicos del sistema, de forma que queda perfectamente definida la posición y orientación de todos los sólidos. [5] Además de esta clasificación, podemos definir las coordenadas en independientes o dependientes. Por coordenadas independientes se entiende aquellas que no dependen la una de la otra. Habrá tantas coordenadas Sistemas multicuerpo 4 4 independientes como grados de libertad tenga nuestro sistema.[6] En el caso de que se esté utilizando un mayor número de coordenadas que grados de libertad, la diferencia entre estos dos números nos dará la cantidad de coordenadas dependientes, así como el número de restricciones necesarias para definir correctamente el sistema. 2.3 Problema de posición El problema de posición trata de averiguar la posición y orientación de todos los sólidos de un sistema multicuerpo conocidos tantos valores como grados de libertad de los que disponga dicho sistema.[3] Es decir, si se divide el problema en coordenadas independientes y dependientes 𝑞=𝑞!"# 𝑞!"# y un sistema dispone de n grados de libertad y m ecuaciones de restricción, el número de coordenadas independientes viene determinado por : 𝑔=𝑛−𝑚 Si se introducen ahora unos valores determinados a las coordenadas independientes y se diferencian las ecuaciones de restricción en dependientes e independientes, se dispone de un sistema de ecuaciones con m incógnitas y m ecuaciones. De modo que conocidos los valores de las coordenadas independientes, se pueden conocer los valores de las dependientes y de esta forma, queda resuelto el problema de posición. En un sistema de ecuaciones lineales, 𝑨𝒙 =𝒃, si se dispone del mismo número de incógnitas que de ecuaciones y la matriz de coeficientes A no es singular, entonces se obtiene la solución única como 𝒙=𝑨!𝟏𝒃. En un sistema de ecuaciones no lineales, no se puede obtener siguiendo este método y requiere de métodos iterativos como el método de Newton-Raphson.[3] Este método se basa en la estimación inicial de un valor de la variable independiente, que se aproxime al valor buscado, y se calcula tanto el valor de la función como la derivada en ese punto, de modo que la próxima iteración se obtiene como: 𝑥!!!=𝑥!−𝑓(𝑥!) 𝑓′(𝑥!) Esta fórmula es aplicada hasta que la diferencia entre ambos términos de la resta es muy pequeña. Dependiendo de la estimación inicial puede que el sistema no converja o que el número de iteraciones sea mayor del esperado.[3] Adaptando la ecuación del método Newton-Raphson a un sistema múlticuerpo con qdep coordenadas dependientes y C ecuaciones de restricción se llega a la siguiente expresión [3]: 𝑞!!! !"# =𝑞! !"# − 𝜕𝐶 𝜕𝑞!"# −1 𝑞! !"# 𝐶(𝑞! !"#) Iniciándose el método con una iteración inicial de 𝑞! !"#. El jacobiano de las ecuaciones de restricción se suele escribir como 𝐶!!"#, de modo que la ecuación pasa a escribirse de la siguiente forma [3]: 5 5 Simulación cinemática y dinámica de un vehículo controlado por control remoto en tres dimensiones 𝑞!!! !"# =𝑞! !"# −𝐶!!"# !!∙𝐶(𝑞! !"#) 2.4 Simulación cinemática de sistemas multicuerpo La simulación cinemática no es más que el problema de posición en un conjunto sucesivo de instantes de tiempo muy cercanos entre sí dentro de un intervalo especificado.[3] Conocidas las ecuaciones de restricción de un sistema: 𝐶𝑞,𝑡=0 𝑠𝑖𝑒𝑛𝑑𝑜 𝑞=𝑞!"# 𝑞!"# se pueden calcular las aceleraciones y velocidades en la simulación cinemática de un sistema multicuerpo. Para ello, partimos de las siguientes ecuaciones: 𝑑𝐶(𝑞,𝑡) 𝑑𝑡 =0 𝑑𝐶(𝑞,𝑡) 𝑑𝑡 =0 Que aplicando la regla de la cadena, pasan a ser [3]: 𝐶𝑞,𝑡= 𝜕𝐶 𝜕𝑞 𝑑𝑞 𝑑𝑡 + 𝜕𝐶 𝜕𝑡 =𝐶!∙𝑞+𝐶!=0 𝐶𝑞,𝑡=𝐶!∙𝑞+𝐶!∙𝑞+𝐶!=0 Cq representa el jacobiano de las ecuaciones de restricción y Ct la derivada parcial de las ecuaciones de restricción con respecto al tiempo. Separando estas ecuaciones en independientes y dependientes, llegamos a las siguientes expresiones [3]: 𝐶!!"# ∙𝑞!"# +𝐶!!"# ∙𝑞!"# +𝐶!=0 → 𝑞!"# =−𝐶!!"# !!(𝐶!!"# ∙𝑞!"# +𝐶!) 𝐶!!"# ∙𝑞!"# +𝐶!!"# ∙𝑞!"# +𝐶!∙𝑞+𝐶!=0 → 𝑞!"# =−𝐶!!"# !!(𝐶!!"# ∙𝑞!"# + 𝐶!∙𝑞+𝐶!) De modo que conocidos los valores de 𝑞!"# ,𝑞!"# y los valores de Cq, Ct y sus respectivas derivadas temporales, podemos calcular las velocidades y aceleraciones de las coordenadas dependientes, resolviendo de esta forma el problema de velocidades y de aceleraciones.[3] 2.5 Simulación dinámica de sistemas multicuerpo Las ecuaciones del movimiento de un sistema se pueden obtener aplicando las ecuaciones de Newton-Euler a cada uno de los puntos materiales pertenecientes a dicho sistema. Dado que un sólido rígido se puede considerar como un conjunto de puntos materiales cuyas distancias mutuas se mantienen invariables, el movimiento del centro de masa G de un sólido vendrá dado por la segunda ley de Newton.[4] 𝐹!=𝑚!𝑎! ! 𝑠𝑖𝑒𝑛𝑑𝑜 𝐹!=𝐹! ! 𝑦 𝑎! ! 𝑙𝑎 𝑎𝑐𝑒𝑙𝑒𝑟𝑎𝑐𝑖ó𝑛 𝑙𝑖𝑛𝑒𝑎𝑙 𝑑𝑒𝑙 𝑐𝑒𝑛𝑡𝑟𝑜 𝑑𝑒 𝑚𝑎𝑠𝑎. Dicha ecuación se puede descomponer según sus componentes: Cinemática y dinámica del vehículo 12 12 A4 A5 A6) representan los componentes unitarios 𝚤 ,𝚥 ,𝑘 locales con respecto al sistema de referencia global XYZ. 3.1.3 Posición y velocidades de los sólidos La posición del centro de gravedad del chasis (sólido 2) viene determinada por: 𝑟 !=𝑥!𝑦!𝑧! La posición de chasis del rover nos sirve de referencia para calcular la posición del resto de los sólidos. Para ello, tenemos que definir los siguientes vectores locales: 𝑢!!=𝑑𝑥!𝑑𝑦!−𝑑𝑧! 𝑢!!=𝑑𝑥!−𝑑𝑦!−𝑑𝑧! 𝑢!!=𝑑𝑥!− 𝑑𝑥!0(𝑅!+𝑑𝑧!)−(𝑅!+𝑑𝑧!) 𝑢!!=−𝑑𝑥!0−𝑑𝑧! Figura 3. Dimensiones características del Rover. 13 13 Simulación cinemática y dinámica de un vehículo controlado por control remoto en tres dimensiones Una vez que son conocidos la posición en coordenadas locales de dichos puntos, se obtiene en coordenadas globales: 𝑟 != 𝑟 !+ 𝐴!∙𝑢!! 𝑟 != 𝑟 !+ 𝐴!∙𝑢!! 𝑟 != 𝑟 !+ 𝐴!∙𝑢!! 𝑟 != 𝑟 !+ 𝐴!∙𝑢!! Conocida la posición del centro de gravedad de los sólidos, calculamos sus velocidades a partir de los jacobianos de la posición de dichos puntos. 𝑣!=!!! !" !" !" =𝐻!∙𝑞 𝑣!=!!! !" !" !" =𝐻!∙𝑞 𝑣!=!!! !" !" !" =𝐻!∙𝑞 𝑣!=!!! !" !" !" =𝐻!∙𝑞 𝑣!=!!! !" !" !" =𝐻!∙𝑞 Además de estas velocidades, necesitamos calcular las velocidades de rotación de los sólidos. Dichas velocidades se obtienen a partir de las matrices de rotación y sus derivadas temporales. Para ello, se deriva cada matriz de rotación con respecto a todos los ángulos del sistema de coordenadas: 𝐴!= 𝜕𝐴! 𝜕𝜓! 𝜓!+ 𝜕𝐴! 𝜕𝜃! 𝜃!+ 𝜕𝐴! 𝜕𝜑! 𝜑!+ 𝜕𝐴! 𝜕𝜃! 𝜃!+ 𝜕𝐴! 𝜕𝜃! 𝜃!+ 𝜕𝐴! 𝜕𝜓! 𝜓!+ 𝜕𝐴! 𝜕𝜃! 𝜃! Una vez conocidas dichas matrices, se calculan las velocidades angulares en coordenadas locales: 𝑊 !=𝐴! !∙𝐴! La resolución de dicha ecuación proporciona una matriz con las siguientes componentes: 𝑊 != 0−𝑤! !𝑤! ! 𝑤! !0−𝑤! ! −𝑤! !𝑤! !0 𝑑𝑒 𝑓𝑜𝑟𝑚𝑎 𝑞𝑢𝑒 𝑒𝑙 𝑣𝑒𝑐𝑡𝑜𝑟 𝑤!=𝑊𝑖(3,2)𝑊𝑖(1,3)𝑊𝑖(2,1) 𝑊 !=𝐴! !∙𝐴! → 𝑤!=𝑊 !(3,2)𝑊 !(1,3)𝑊 !(2,1) 𝑊 !=𝐴! !∙𝐴! → 𝑤!=𝑊 !(3,2)𝑊 !(1,3)𝑊 !(2,1) 𝑊 !=𝐴! !∙𝐴! → 𝑤!=𝑊 !3,2𝑊 !1,3𝑊 !2,1 𝑊 !=𝐴! !∙𝐴! → 𝑤!=𝑊 !3,2𝑊 !1,3𝑊 !2,1 Cinemática y dinámica del vehículo 14 14 𝑊 !=𝐴! !∙𝐴! → 𝑤!=𝑊 !(3,2)𝑊 !(1,3)𝑊 !(2,1) 3.1.4 Ecuaciones de restricción Tal y como se ha dicho con anterioridad, para hacer que nuestro sistema sea compatible necesitamos 17 ecuaciones, es decir, tantas como coordenadas dependientes dispone nuestro sistema. Se puede diferenciar dos tipos de ecuaciones: a) rodadura sin deslizamiento y b) del contacto rígido entre las ruedas y la superficie de la carretera. 3.1.4.1 Rodadura sin deslizamiento Las ecuaciones de rodadura sin deslizamiento son ecuaciones de restricción no holónomas, es decir, que dependen de la velocidad. A estas ecuaciones se les exige que sean integrables con el objetivo de que sea posible hallar la posición.[7] Estas ecuaciones exigen que las componentes x e y de la velocidad de las ruedas en los puntos de contactos sean nulas en todo momento. La componente z de la velocidad siempre es nula ya que el rover no se despega del suelo. Estas restricciones implican la formulación de 6 ecuaciones: 𝑉 != 𝑉 ! ! 𝑉 ! ! =0 𝑉 != 𝑉 !! 𝑉 !! =0 𝑉 != 𝑉 !! 𝑉 !! =0 Sin embargo, las velocidades 𝑉 !! 𝑦 𝑉 !! son las mismas debido al paralelismo existente entre ambas ruedas, de modo que se tiene una ecuación redundante, quedando en este caso restringida a 5 ecuaciones debido a la rodadura sin deslizamiento. Las velocidades de dichos puntos se obtienen a partir de las velocidades de los centros de gravedad de los sólidos, las posiciones en coordenadas locales de los puntos de contacto y las velocidades angulares de los sólidos. 𝑢!=𝑅!sin (𝜃!)0−cos(𝜃!) 𝑢!=𝑅!sin (𝜃!)0−cos (𝜃!) 𝑢!=𝑅!sin (𝜃!)0−cos(𝜃!) Una vez conocida la posición de estos puntos se calcula la velocidad de los puntos de contacto mediante las siguientes ecuaciones: 𝑣!=𝑣!+𝐴!𝑤!×𝑢!=0 15 15 Simulación cinemática y dinámica de un vehículo controlado por control remoto en tres dimensiones 𝑣!=𝑣!+𝐴!𝑤!×𝑢!=0 𝑣!=𝑣!+𝐴!𝑤!×𝑢!=0 Además, debido a que el rover puede tener las ruedas delanteras en una pendiente, y la rueda trasera en otra pendiente, es necesario proyectar las velocidades con respecto a la tangentes en direcciones x e y de la superficie por la que el rover circula. 𝑉 ! !=𝑉 !∙!"!! !"!! 𝑉 ! !=𝑉 !∙ !"!! !"!! 𝑉 !!=𝑉 !∙!"!! !"!! 𝑉 !!=𝑉 !∙ !"!! !"!! 𝑉 !!=𝑉 !∙!"!! !"!! 𝑉 !!=𝑉 !∙ !"!! !"!! Los cálculos de las tangentes sobre la superficie serán explicados en el apartado siguiente. 3.1.4.2 Contacto rígido El primer paso a realizar es definir dos parámetros superficiales para cada uno de los sólidos en cada punto de contacto, es decir, necesitamos 12 parámetros superficiales.[8] Sin embargo, dado que se ha supuesto que el grosor de las ruedas es despreciable, solo se necesita un parámetro superficial en el caso de las ruedas, de modo que tenemos 9 parámetros superficiales. 𝑆=𝑠! 𝑠!"! 𝑠!"! 𝑠! 𝑠!"! 𝑠!"! 𝑠! 𝑠!"! 𝑠!"! Estos parámetros se consideran coordenadas no generalizadas ya que no tienen ni inercias ni fuerzas externas asociadas a ellas [8]. Se obtendrán resolviendo las ecuaciones diferenciales no lineales de restricción. Una vez definidos los parámetros superficiales, se necesita parametrizar las superficies de contacto. Para ello, se utilizan los siguientes vectores de posición en coordenadas locales para las ruedas: 𝑢! != 𝑅!sin (𝑠!)0−cos(𝑠!) 𝑢!!= 𝑅!sin (𝑠!)0−cos(𝑠!) 𝑢!!= 𝑅!sin (𝑠!)0−cos(𝑠!) y estos vectores para la carretera: 𝑢!!=𝑠!"!𝑠!"!0 Cinemática y dinámica del vehículo 16 16 𝑢!!=𝑠!"!𝑠!"!0 𝑢!!=𝑠!"!𝑠!"!0 Además, se necesitan las tangentes en el punto de contacto, en ambas direcciones, las cuales calculamos como: 𝑡! != !!!! !!! = 𝑅! cos (𝑠!) 0 sin (𝑠!) 𝑡!" != !!!! !!!"! = 1 0 0 𝑡!" != !!!! !!!"! = 0 1 0 𝑡!!=!!!! !!! = 𝑅! cos (𝑠!) 0 sin (𝑠!) 𝑡!"!= !!!! !!!"! = 1 0 0 𝑡!"!= !!!! !!!"! = 0 1 0 𝑡!!= !!!! !!! = 𝑅! cos (𝑠!) 0 sin (𝑠!) 𝑡!"!= !!!! !!!"! = 1 0 0 𝑡!"!= !!!! !!!"! = 0 1 0 Las normales en los puntos se calculan con el producto escalar de las dos componentes tangenciales. En cuanto a las ecuaciones de restricción, se pueden distinguir dos tipos: • Los puntos de contacto coinciden, es decir, la posición en coordenadas globales usando estos parámetros de un sólido respecto al otro debe ser la misma.[8] • Las normales de las dos superficies en el punto de contacto deben ser paralelas.[8] Estas condiciones implican que necesitamos tener tanto la posición de los puntos de contacto, como las tangentes en coordenadas globales. 𝑟 ! !=𝑟 !+𝐴!𝑢! ! 𝑟 !!=𝑟 !+𝐴!𝑢!! 𝑟 !!=𝑟 !+𝐴!𝑢!! 𝑟 !!=𝑟 !+𝐴!𝑢!! 𝑟 !!=𝑟 !+𝐴!𝑢!! 𝑟 !!=𝑟 !+𝐴!𝑢!! Siendo 𝑟 ! la posición en globales del origen de la carretera y 𝐴! la matriz de orientación de la carretera. A su vez, las tangentes en coordenadas globales: 𝑡! !=𝐴!𝑡! ! 𝑡!" !=𝐴!𝑡!" ! 𝑡!" !=𝐴!𝑡!" ! 𝑛!!=𝑡!" !× 𝑡!" ! 𝑡!!=𝐴!𝑡!! 𝑡!"!=𝐴!𝑡!"! 𝑡!"!=𝐴!𝑡!"! 𝑛!!=𝑡!"!× 𝑡!"! 𝑡!!=𝐴!𝑡!! 𝑡!"!=𝐴!𝑡!"! 𝑡!"!=𝐴!𝑡!"! 𝑛!!=𝑡!"!× 𝑡!"! De modo, que conocidos todos estos valores, se pueden plantear las ecuaciones de restricción: • Punto P: o 𝑟 ! !−𝑟 !!=0 17 17 Simulación cinemática y dinámica de un vehículo controlado por control remoto en tres dimensiones o 𝑛!!∙𝑡! !=0 • Punto Q: o 𝑟 !!−𝑟 !!=0 o 𝑛!!∙𝑡!!=0 • Punto S: o 𝑟 !!−𝑟 !!=0 o 𝑛!!∙𝑡!!=0 Estas ecuaciones implican un total de 12 ecuaciones de restricción. A continuación se presenta en la tabla I un resumen del sistema completo. Tabla 1. Comparativa coordenadas y ecuaciones en el modelo completo. MODELO COMPLETO TOTAL Coordenadas Dependientes 𝑥! 𝑦! 𝑧! 𝜓! 𝜃! 𝜑! 𝑠! 𝑠! 𝜓! 𝜃! 𝑠! 𝑠𝑐𝑝! 𝑠𝑐𝑝! 𝑠𝑐𝑞! 𝑠𝑐𝑞! 𝑠𝑐𝑠! 𝑠𝑐𝑠! 19 Independientes 𝜃! 𝜃! Ecuaciones Rodadura sin deslizamiento: 5 ecuaciones 17 Contacto rígido ruedas-carretera: 12 ecuaciones De forma, que tal y como hemos expresado con anterioridad, para poder resolver el sistema se tienen que tener tantas ecuaciones como coordenadas dependientes, y la diferencia entre coordenadas y ecuaciones, son los grados de libertad del sistema, en este caso dos grados de libertad. 3.1.5 Dinámica del vehículo Conviene comenzar con la definición de las matrices que se van a utilizar para el cálculo de las ecuaciones del movimiento del rover. 𝑀!"!!" =𝑚!1!!!0 0𝐼! 𝑖=2,3,4,5,6. 𝑄!"!!=𝐹! 𝑀! 𝑄! !"!!=0 −𝑤!×(𝐼!𝑤!) 𝐿!"!!" =𝐻! 𝐺! 𝑙!"!!" =ℎ! 𝑔! Una vez conocidas estas matrices, se pueden realizar los cálculos necesarios para transformar el sistema de modo que se utilicen coordenadas generalizas y sus derivadas temporales. 𝑀!"!!" =𝐿!"!!" !𝑀!"!!" 𝐿!"!!" Cinemática y dinámica del vehículo 18 18 𝑄!"!!=𝑄!"#!"!!+𝑄!"#$!"!! 𝑄! !"!!=𝐿!"!!" !(𝑄! !"!!−𝑀!"!!"𝑙!"!!"𝑞!"!!) 𝑄!"#!"!!=𝐿!"!!" !𝑄!"#!"!! 𝑄!"#$!"!!= 𝐷!"!!" !𝜆!"!! 𝐷!"!!" = 𝐶!!"!!! 𝐵!!!" Las ecuaciones de movimiento del sistema son las siguientes: 𝑀!"!!"𝑞!"!!+𝐷!"!!" !𝜆!"!! =𝑄!"#!"!!+𝑄! !"!! 𝐷!"!!"𝑞!"!!+𝐷!"!!"𝑞!"!!=0 →𝑀𝐷! 𝐷0 𝑞 𝜆=𝑄!"# +𝑄! −𝐷𝑞 En este caso, el término debido a la derivada respecto al tiempo de las restricciones holónomas es nulo, es decir, E=0. Las fuerzas generalizadas aplicadas sobre el sólido son, las gravitatorias y las provocadas por las ruedas motrices. 𝑄!"#$!"!!=−𝑚!𝑔 0 𝑠𝑖𝑒𝑛𝑑𝑜 𝑔:𝑎𝑐𝑒𝑙𝑒𝑟𝑎𝑐𝑖ó𝑛 𝑑𝑒 𝑙𝑎 𝑔𝑟𝑎𝑣𝑒𝑑𝑎𝑑 𝑄!"#!"!!=𝐺!!"!! ! 0 𝑃𝑎𝑟 !! 0 +𝐺!!"!! ! 0 𝑃𝑎𝑟 !! 0 Sin embargo, dado que se desconocen el valor de los pares en las ruedas, se incluyen estos términos como incógnitas adicionales del problema. Además, dividimos el problema en términos dependientes e independientes, dado que ya se conocen los independientes. 𝑇!!!=𝑃𝑎𝑟 !! 𝑃𝑎𝑟 !! 𝑆!"!!=𝐺!!"!! !(2)𝐺!!"!! !(2) 𝐺!!"!! !2:𝑠𝑒𝑔𝑢𝑛𝑑𝑎 𝑓𝑖𝑙𝑎 𝑚𝑎𝑡𝑟𝑖𝑧 𝑀!"#!"!!" −𝑆!"!!𝐷!"!!" ! 𝐷!"#!"!!" 0!"!!0!"!!" 𝑞!"#!"!! 𝑇!!! 𝜆!"!! = 𝑄!"#$!"!!+𝑄! !"!!−𝑀!"#!"!!𝑞!"#!!! −𝐷!"!!"𝑞!"!!−𝐷!"#!"!!𝑞!"#!!! Sistema que podemos convertir en: 𝐴36𝑥36 =𝑥36𝑥1𝑏36𝑥1 Sistema que se podrá resolver como x=A\b siempre que la matriz A no sea singular. 19 19 Simulación cinemática y dinámica de un vehículo controlado por control remoto en tres dimensiones 3.2 Modelo simplificado 3.2.1 Selección de coordenadas Dado que se ha eliminado el ángulo de balanceo del rover, debido a las características de la carretera por la que va a circular, las coordenadas del rover pasan a ser las siguientes: 𝑞= 𝑥! 𝑦! 𝑧! 𝜓! 𝜃! 𝜃! 𝑠! 𝜃! 𝑠! 𝜓! 𝜃! 𝑠! 𝑠𝑐𝑝! 𝑠𝑐𝑝! 𝑠𝑐𝑞! 𝑠𝑐𝑞! 𝑠𝑐𝑠! 𝑠𝑐𝑠! De modo que en este modelo, disponemos de 18 coordenadas, 2 coordenadas independientes y 16 dependientes. 𝑞!"# = 𝑥! 𝑦! 𝑧! 𝜓! 𝜃! 𝑠! 𝑠! 𝜓! 𝜃! 𝑠! 𝑠𝑐𝑝! 𝑠𝑐𝑝! 𝑠𝑐𝑞! 𝑠𝑐𝑞! 𝑠𝑐𝑠! 𝑠𝑐𝑠! 𝑞!"# = 𝜃! 𝜃! 3.2.2 Matrices de rotación Mismas matrices que en el caso del modelo completo, con la variación de la matriz de rotación del chasis (sólido 2) que interfiere en el resto de matrices. 𝐴!= 𝐴!!∙ 𝐴!! 𝐴!= 𝐴!∙ 𝐴!! 𝐴!= 𝐴!∙ 𝐴!! 𝐴!= 𝐴!∙ 𝐴!! 𝐴!= 𝐴!∙ 𝐴!! 3.2.3 Posición y velocidades de los sólidos Las ecuaciones para obtener la posición de todos los sólidos y sus velocidades son las mismas que en el modelo completo, con la única diferencia de que desaparece el ángulo de balanceo, simplificando de esta forma los cálculos. 𝑟 !=𝑥!𝑦!𝑧! 𝑣!=𝐻!∙𝑞 𝑊 !=𝐴! !∙𝐴! → 𝑤!=𝑊 !(3,2)𝑊 !(1,3)𝑊 !(2,1) 𝑟 != 𝑟 !+ 𝐴!∙𝑢!! 𝑣!=𝐻!∙𝑞 𝑊 !=𝐴! !∙𝐴! → 𝑤!=𝑊 !(3,2)𝑊 !(1,3)𝑊 !(2,1) 𝑟 != 𝑟 !+ 𝐴!∙𝑢!! 𝑣!=𝐻!∙𝑞 𝑊 !=𝐴! !∙𝐴! → 𝑤!=𝑊 !(3,2)𝑊 !(1,3)𝑊 !(2,1) 𝑟 != 𝑟 !+ 𝐴!∙𝑢!! 𝑣!=𝐻!∙𝑞 𝑊 !=𝐴! !∙𝐴! → 𝑤!=𝑊 !(3,2)𝑊 !(1,3)𝑊 !(2,1) 𝑟 != 𝑟 !+ 𝐴!∙𝑢!! 𝑣!=𝐻!∙𝑞 𝑊 !=𝐴! !∙𝐴! → 𝑤!=𝑊 !(3,2)𝑊 !(1,3)𝑊 !(2,1) donde 𝐴! han sido calculados con la siguiente ecuación: Cinemática y dinámica del vehículo 20 20 𝐴!= 𝜕𝐴! 𝜕𝜓! 𝜓!+ 𝜕𝐴! 𝜕𝜃! 𝜃!+ 𝜕𝐴! 𝜕𝜃! 𝜃!+ 𝜕𝐴! 𝜕𝜃! 𝜃!+ 𝜕𝐴! 𝜕𝜓! 𝜓!+ 𝜕𝐴! 𝜕𝜃! 𝜃! 3.2.4 Ecuaciones de restricción Igual que en el modelo completo, se proponen ecuaciones por la rodadura sin deslizamiento y por el contacto rígido existente entre las ruedas y la carretera. 3.2.4.1 Rodadura sin deslizamiento Estas ecuaciones son exactamente igual que en el modelo completo. 𝑉 ! != 𝑉 !∙!"!! !"!! =0 𝑉 ! != 𝑉 !∙ !"!! !"!! =0 𝑉 !!=𝑉 !∙!"!! !"!! =0 𝑉 !!=𝑉 !∙!"!! !"!! =0 𝑉 !!=𝑉 !∙ !"!! !"!! =0 Como se puede observar, en el modelo simplificado también se mantiene el paralelismo de las ruedas delanteras, de modo que la ecuación de la 𝑉 !!=0 es redundante. 3.2.4.2 Contacto rígido En este caso, existe una pequeña variación con respecto al caso del modelo completo, ya que una de las ecuaciones es redundante. Las normales de las ruedas delanteras son las mismas, ya que el radio de ambas ruedas es el mismo, de forma que lo que hace que varíen las ecuaciónes son las tangentes en los puntos de contacto P y Q de la carretera. Dado que la superficie por la que el vehículo va a circular es totalmente plana y va a circular en línea recta, las tangentes en ambos puntos son también las mismas, de modo que ya se conoce la ecuación redundante del sistema. En este caso, las ecuaciones son las siguientes: • Punto P: o 𝑟 ! !−𝑟 !!=0 o 𝑛!!∙𝑡! !=0 • Punto Q: o 𝑟 !!−𝑟 !!=0 • Punto S: o 𝑟 !!−𝑟 !!=0 o 𝑛!!∙𝑡!!=0 En este caso, disponemos de un total de 11 ecuaciones del contacto rígido, siendo esta la principal diferencia con respecto al caso anterior. Nuestro sistema es compatible, tal y como se puede observar en la siguiente 21 21 Simulación cinemática y dinámica de un vehículo controlado por control remoto en tres dimensiones tabla, dado que se ha eliminado una coordenada y una ecuación, manteniendo los dos grados de libertad del sistema. Tabla 2. Comparativa coordenadas y ecuaciones del modelo simplificado. MODELO SIMPLIFICADO TOTAL Coordenadas Dependientes 𝑥! 𝑦! 𝑧! 𝜓! 𝜃! 𝑠! 𝑠! 𝜓! 𝜃! 𝑠! 𝑠𝑐𝑝! 𝑠𝑐𝑝! 𝑠𝑐𝑞! 𝑠𝑐𝑞! 𝑠𝑐𝑠! 𝑠𝑐𝑠! 18 Independientes 𝜃! 𝜃! Ecuaciones Rodadura sin deslizamiento: 5 ecuaciones 16 Contacto rígido ruedas-carretera: 11 ecuaciones 3.2.5 Dinámica del vehículo En este caso, las viariaciones existentes dentro de las ecuaciones del movimiento están provocadas por la coordenada que se ha eliminado y la ecuación redundante existente en el caso del contacto rígido entre los sólidos. En este caso, las ecuaciones del movimiento son las siguientes: 𝑀!"#!"!!" −𝑆!"!!𝐷!"!!" ! 𝐷!"#!"!!" 0!"!!0!"!!" 𝑞!"#!"!! 𝑇!!! 𝜆!"!! = 𝑄!"#$!"!!+𝑄! !"!!−𝑀!"#!"!!𝑞!"#!!! −𝐷!"!!"𝑞!"!!−𝐷!"#!"!!𝑞!"#!!! 𝐴!"!!" 𝑥!"!!=𝑏!"!! Como se puede observar, la diferencia entre las dimensiones del modelo completo y el modelo simplificado coinciden con la reducción de una coordenada generalizada y una ecuación de restricción. 3.3 Superficie parametrizada A la hora de realizar los cálculos necesarios para la simulación dinámica del vehículo, previos a los análisis expermientales, se ha parametrizado la carretera por la que el rover va a circular como un plano inclinado. A pesar de que las dimensiones de estas se suponen infinitas para su cálculo, se ha representado el plano con unas dimensiones acotadas para que fuese posible su representación. Programas utilizados para la simulación 28 28 Figura 5. Trayectorias verticales modelo simplificado. Estos resultados indican que la trayectoria realizada por el rover, medida como la relación existente entre las coordenadas del chasis (x2,z2), concuerdan con los ángulos introducidos de las pendientes. Los errores obtenidos de los ángulos del rover respecto al ángulo de la carretera han sido los siguientes: Tabla 3. Error ángulo trayectoria vertical respecto ángulo carretera. Velocidad [rad/s] Ángulo carretera Error absoluto 10 10º 3.7836 ∙10!!" 10 20º 6.029 ∙10!!" 10 30º 5.2224 ∙10!!" El error que se introduce en la simulación se debe al proceso de integración. Este error depende de la velocidad que sea introducida en las ruedas motoras, el paso de tiempo que se utilice en la integración, y las dimensiones del vector tiempo, es decir, la duración de la simulación. Además de la posición del chasis (x2,z2), se puede utilizar el ángulo de cabeceo (𝜃!) y el recorrido sobre el plano XY como comprobación. En este caso, el recorrido debería de ser una línea recta y este ángulo (𝜃!) debería coincidir con el ángulo de la pendiente de la carretera. Representación gráfica del ángulo de cabeceo de Rover a lo largo de todo el recorrido. 29 29 Simulación cinemática y dinámica de un vehículo controlado por control remoto en tres dimensiones Figura 6. Ángulos de cabeceo modelo simplificado. Se han calculado los errores de éste ángulo con respecto al ángulo de la pendiente, en los tres casos anteriores, y el resultado es el siguiente: Tabla 4. Errores ángulo cabeceo respecto ángulo carretera. Ángulo carretera Error absoltuto 10º 5.1366 ∙10!!" 20º 2.8974 ∙10!!! 30º 1.6645 ∙10!!! Representación gráfica de la trayectoria sobre el plano XY global. Programas utilizados para la simulación 30 30 Figura 7. Trayectorias sobre plano XY modelo simplificado Como podemos observar, los resultados obtenidos tanto con los ángulos de cabeceo del Rover como con la trayectoria sobre el plano XY coinciden con los esperados, manteniendo un error absoluto del mismo orden que el calculado siguiendo la trayectoria (x2,z2). 4.6.2 Modelo completo Este modelo permite que el vehículo gire al subir la pendiente ya que le hemos proporcionado el ángulo de balanceo que anteriormente hemos eliminado. Al igual que en el caso anterior, se ha analizado la trayectoria del vehículo en tres pendientes diferentes, pero con la variación de introducirle velocidades diferentes pero constantes a cada rueda, de forma que debería realizar una trayectoria circular. Dentro de éste análisis, se ha representado gráficamente la trayectoria sobre el plano XY global del sistema, la trayectoria seguida por el chasis (x2,z2), y los respectivos ángulos de cabeceo y balanceo del vehículo. Los resultados obtenidos son los siguientes: 31 31 Simulación cinemática y dinámica de un vehículo controlado por control remoto en tres dimensiones Figura 8. Trayectorias sobre plano XY modelo completo. Figura 9. Trayectorias verticales modelo completo. Estas representaciones nos indican que a priori tanto la trayectoria vertical como la trayectoria sobre el plano XY coincide con lo esperado, ya que la velocidad introducida para la rueda derecha era levemente superior a la izquierda, produciendo de esta forma el giro hacia la izquierda deseado. Programas utilizados para la simulación 32 32 Figura 10. Ángulos de cabeceo modelo completo Figura 11. Ángulos de balanceo modelo completo Los resultados obtenidos en estas representaciones también concuerdan con lo esperado, ya que el ángulo de cabeceo inicial coincide con el de la pendiente de la carretera y este disminuye hasta llegar a ser nulo en el punto en el que la trayectoria es totalmente perpendicular a la dirección de subida de la pendiente. 33 33 Simulación cinemática y dinámica de un vehículo controlado por control remoto en tres dimensiones Simultáneamente, el valor del ángulo de balanceo aumenta desde un valor inicial nulo hasta el máximo en el mismo instante en el que el ángulo de cabeceo es nulo, siendo este instante en torno a t=80 s. Aparte de esta simulación, dado que siempre se ha iniciado la simulación directamente sobre la rampa, hemos decidido dividirlo en tres tramos, los tramos iniciales y finales sin pendiente y el tramo de en medio con una pendiente de 20º. Obteniendo resultados de la figura 12. Estos resultados nos indican que la proyección de las velocidades de los puntos en las tangentes superficiales nos han sido de utilidad para que fuese posible tener las ruedas delanteras en una pendiente y la trasera en otra diferente, de forma que no fuesen paralelos al eje local del chasis. Figura 12. Trayectoria vertical con tres tramos a pendientes diferentes El objetivo de este apartado era la comprobación con diversas situaciones de que los programas generados nos proporcionaban las soluciones adecuadas, de forma que a expensas de realizar la simulación con los datos experimentales parece que los resultados que obtenemos con los diferentes programas coinciden con lo esperado y podemos pasar al siguiente apartado en el que introducimos como valores de entrada los registrados por los sensores del Rover. Resultados experimentales de la simulación 34 34 5 RESULTADOS EXPERIMENTALES DE LA SIMULACIÓN ste capítulo está formado por los resultados obtenidos de la simulación que se ha realizado con los datos extraidos directamente del Rover cuando éste realiza un trayecto por una cuesta con cierta pendiente. 5.1 Registro de señales digitales El trayecto realizado por el Rover queda registrado por el conjunto de sensores que éste incluye, siendo estos sensores dos encoders, un giroscopio y un acelerómetro. Obteniendo de esta forma los siguientes datos: • Columna 1. Instante de tiempo [s]. • Columna 2. Aceleración en dirección X [𝑚𝑠!]. • Columna 3. Aceleración en dirección Y [𝑚𝑠!]. • Columna 4. Aceleración en dirección Z [𝑚𝑠!]. • Columna 5. Velocidad angular en dirección X [grados/s]. • Columna 6. Velocidad angular en dirección Y [grados/s]. • Columna 7. Velocidad angular en dirección Z [grados/s]. • Columna 8. Pulsos de encoder rueda derecha con signo negativo. • Columna 9. Pulsos de encoder rueda izquierda. Figura 13. Rover y control remoto. E 35 35 Simulación cinemática y dinámica de un vehículo controlado por control remoto en tres dimensiones 5.2 Tratamiento de señales digitales Los encoders tienen una resolución de 960 pulsos por revolución, de modo que el paso inicial es convertir los pulsos de los encoders en radianes.[7] Una vez conocidos los ángulos girados por ambas ruedas, en radianes, hay que obtener las velocidades y aceleraciones ángulares de las ruedas. Estos resultados se han obtenidos mediante el método diferencial de diferencias finitas centradas. En cuanto a las señales registradas, con el objetivo de conocer digitalmente el espectro de una señal hasta una determinada frecuencia máxima (𝑓 !"#), se requiere realizar el muestreo a una frecuencia (𝑓 !) al menos el doble de la frecuencia máxima, dado que sino no se podrían conocer los armónicos superiores a la mitad de la frecuencia de muestreo. Además, si existen frecuencias iguales o superiores a 𝑓 !/2, estas frecuencias perturbarían el resultado obtenido. Esta condición es conocida con el nombre de criterio de Nyquist. [9] Como no es siempre realizar el muestreo a la frecuencia anterior, la señal debe filtrarse con el objetivo de eliminar todas las frecuencias superiores a la de Nyquist, es decir, la mitad de la frecuencia de muestreo. Estos filtros son los denominados filtros de paso bajo, que eliminan todas aquellas frecuencias superiores a la de Nyquist y se permite el paso en frecuencias inferiores, garantizando de esta forma que el espectro obtenido es suficientemente aproximado hasta esa frecuencia.[9] Los filtros se caracterizan por su función de transferencia, de forma que el tipo de filtro dependerá de dicha función. En este caso, se han filtrado las señales de los ángulos, velocidades y aceleraciones angulares de las ruedas con un filtro de butterworth de segundo orden a una frecuencia de corte de 4 Hz.[7] Dicho filtro se ha realizado mediante la función de matlab “butter(N,Wn,'low')” siendo N el orden del filtro y Wn la frecuencia de corte. Figura 14. Ángulos girados por las ruedas Resultados experimentales de la simulación 36 36 Figura 15. Velocidades de las ruedas Figura 16. Aceleraciones de las ruedas Estas señales son los datos de entrada para la resolución del sistema. 𝑞!"# =𝜃! 𝜃! 𝑞!"# =𝜃! 𝜃! 𝑞!"# =𝜃! 𝜃! 37 37 Simulación cinemática y dinámica de un vehículo controlado por control remoto en tres dimensiones 5.3 Superficie parametrizada La superficie utilizada para la trayectoria recorrida por el Rover es una de las rampas de la escuela. Para su simulación, necesitamos conocer el valor de su pendiente, por ello, hemos tomado la posición en la dirección X y Z de tres puntos de la rampa y la pendiente obtenida ha sido de 4.5º. Figura 17. Rampa utilizada para la simulación 5.4 Resultados Al igual que en los resultados análiticos optenidos en el apartado 4.5, se han realizado representaciones de una serie de valores que nos proporcionan la información necesaria para poder confirmar que los resultados analíticos obtenidos con los datos registrados experimentalmente coinciden con lo esperado. Por ello, como punto de partida, se ha representado la trayectoria vertical seguida por el Rover (Figura 16). Anexos 44 44 A7(1,:) = [ cos(tetc) 0 -sin(tetc)]; A7(2,:) = [ 0 1 0]; A7(3,:) = [sin(tetc) 0 cos(tetc)]; A2=A2_1*A2_2*A2_3; A3 = A2*A23; A4 = A2*A24; A5 = A2*A25; A6 = A5*A56; %matlabFunction(A2,'file', 'RotMatSolido2'); %matlabFunction(A3,'file', 'RotMatSolido3'); %matlabFunction(A4,'file', 'RotMatSolido4'); %matlabFunction(A5,'file', 'RotMatSolido5'); %matlabFunction(A6,'file', 'RotMatSolido6'); dA2 = diff(A2,psi2)*dpsi2+diff(A2,tet2)*dtet2+diff(A2,phi2)*dphi2+diff(A2,tet3)*dtet3+diff(A2,tet 4)*dtet4+diff(A2,psi5)*dpsi5+diff(A2,tet6)*dtet6; dA3 = diff(A3,psi2)*dpsi2+diff(A3,tet2)*dtet2+diff(A3,phi2)*dphi2+diff(A3,tet3)*dtet3+diff(A3,tet 4)*dtet4+diff(A3,psi5)*dpsi5+diff(A3,tet6)*dtet6; dA4 = diff(A4,psi2)*dpsi2+diff(A4,tet2)*dtet2+diff(A4,phi2)*dphi2+diff(A4,tet3)*dtet3+diff(A4,tet 4)*dtet4+diff(A4,psi5)*dpsi5+diff(A4,tet6)*dtet6; dA5 = diff(A5,psi2)*dpsi2+diff(A5,tet2)*dtet2+diff(A5,phi2)*dphi2+diff(A5,tet3)*dtet3+diff(A5,tet 4)*dtet4+diff(A5,psi5)*dpsi5+diff(A5,tet6)*dtet6; dA6 = diff(A6,psi2)*dpsi2+diff(A6,tet2)*dtet2+diff(A6,phi2)*dphi2+diff(A6,tet3)*dtet3+diff(A6,tet 4)*dtet4+diff(A6,psi5)*dpsi5+diff(A6,tet6)*dtet6; % PosiciÛn en locales de puntos caracterÌsticos uG3 = [dx3 dy3 -dz3]'; uG4 = [dx3 -dy3 -dz3]'; uG5 = [dx3-dx5 0 (Rf+dz3)-(Rr+dz6)]'; uG6 = [-dx6 0 -dz6]'; uP = Rf*[sin(tet3) 0 -cos(tet3)]'; uQ = Rf*[sin(tet4) 0 -cos(tet4)]'; uS = Rr*[sin(tet6) 0 -cos(tet6)]'; %Posiciones de los solidos en globales R2=[x2 y2 z2]'; R3 = R2 + A2*uG3; R4 = R2 + A2*uG4; R5 = R2 + A2*uG5; R6 = R5 + A5*uG6; R7=[0 0 0]'; %Velocidades angulares en locales w4Lsk = simplify(A4'*dA4); w4L = [-w4Lsk(2,3) w4Lsk(1,3) -w4Lsk(1,2)]'; w3Lsk = simplify(A3'*dA3); w3L = [-w3Lsk(2,3) w3Lsk(1,3) -w3Lsk(1,2)]'; w2Lsk = simplify(A2'*dA2); w2L = [-w2Lsk(2,3) w2Lsk(1,3) -w2Lsk(1,2)]'; w5Lsk = simplify(A5'*dA5); w5L = [-w5Lsk(2,3) w5Lsk(1,3) -w5Lsk(1,2)]'; 45 45 Simulación cinemática y dinámica de un vehículo controlado por control remoto en tres dimensiones w6Lsk = simplify(A6'*dA6); w6L = [-w6Lsk(2,3) w6Lsk(1,3) -w6Lsk(1,2)]'; % Traslaciones rotacionales H2 = jacobian(R2,q); h2 = jacobian(H2*dq,q); H3 = jacobian(R3,q); h3 = jacobian(H3*dq,q); H4 = jacobian(R4,q); h4 = jacobian(H4*dq,q); H5 = jacobian(R5,q); h5 = jacobian(H5*dq,q); H6 = jacobian(R6,q); h6 = jacobian(H6*dq,q); G2 = (jacobian(w2L,dq)); g2 = (jacobian(w2L,q)); G3 = (jacobian(w3L,dq)); g3 = (jacobian(w3L,q)); G4 = (jacobian(w4L,dq)); g4 = (jacobian(w4L,q)); G5 = (jacobian(w5L,dq)); g5 = (jacobian(w5L,q)); G6 = (jacobian(w6L,dq)); g6 = (jacobian(w6L,q)); %matlabFunction(G3,'file', 'MatrizG3' ); %matlabFunction(G4,'file', 'MatrizG4' ); %Restricciones contacto posicion ucp=[scpx scpy 0]'; ucq=[scqx scqy 0]'; ucs=[scsx scsy 0]'; Rcp=R7+A7*ucp; Rcq=R7+A7*ucq; Rcs=R7+A7*ucs; u3P = Rf*[sin(s3) 0 -cos(s3)]'; u4Q = Rf*[sin(s4) 0 -cos(s4)]'; u6S = Rr*[sin(s6) 0 -cos(s6)]'; R3p=R3+A3*u3P; R4q=R4+A4*u4Q; R6s=R6+A6*u6S; %restricciones contacto normales tcpx=jacobian(Rcp,scpx); tcpy=jacobian(Rcp,scpy); Ncp=simplify(cross(tcpx,tcpy)); tcqx=jacobian(Rcq,scqx); tcqy=jacobian(Rcq,scqy); Ncq=simplify(cross(tcqx,tcqy)); tcsx=jacobian(Rcs,scsx); tcsy=jacobian(Rcs,scsy); Ncs=simplify(cross(tcsx,tcsy)); t3p=jacobian(R3p,s3); t2=[0 1 0]'; N3p=simplify(cross(t3p,t2)); t4q=jacobian(R4q,s4); t2=[0 1 0]'; N4q=simplify(cross(t4q,t2)); t6s=jacobian(R6s,s6); t2=[0 1 0]'; N6s=simplify(cross(t6s,t2)); Ch=[Rcp(1)-R3p(1) Rcp(2)-R3p(2) Rcp(3)-R3p(3) dot(Ncp,t3p) Rcq(1)-R4q(1) Rcq(2)-R4q(2) Rcq(3)-R4q(3) dot(Ncq,t4q) Rcs(1)-R6s(1) Rcs(2)-R6s(2) Rcs(3)-R6s(3) dot(Ncs,t6s)]'; Cqh=jacobian(Ch,q); Anexos 46 46 dCqh = jacobian(Cqh*dq,q); %matlabFunction(Ch,'file', 'MatrizCh' ); %matlabFunction(Cqh,'file', 'MatrizCqh' ); %matlabFunction(dCqh,'file', 'MatrizdCqh' ); % Velocidades de los centros de gravedad V2 = H2*dq; V3 = H3*dq; V4 = H4*dq; V5 = H5*dq; V6 = H6*dq; % Restricciones de rodadura sin deslizamiento VP = simplify(V3 + A3*cross(w3L,uP)); VQ = simplify(V4 + A4*cross(w4L,uQ)); VS = simplify(V6 + A6*cross(w6L,uS)); VPx=dot(VP,tcpx/norm(tcpx)); VPy=dot(VP,tcpy/norm(tcpy)); VQx=dot(VQ,tcqx/norm(tcqx)); VSx=dot(VS,tcsx/norm(tcsx)); VSy=dot(VS,tcsy/norm(tcsy)); Cnh = [VPx VPy VQx VSx VSy]'; B = jacobian(Cnh,dq); dB = jacobian(B*dq,q); %matlabFunction(Cnh,'file','MatrizCnh'); %matlabFunction(B,'file', 'MatrizB' ); %matlabFunction(dB,'file', 'MatrizdB' ); %Matriz restricciones D=[Cqh;B]; dD=[dCqh;dB]; %matlabFunction(D,'file', 'MatrizD' ); %matlabFunction(dD,'file', 'MatrizdD' ); % Matrices de masas e inercias M2 = m2*eye(3); M3 = m3*eye(3); M4 = m4*eye(3); M5 = m5*eye(3); M6 = m6*eye(3); I2(1,:) = [I2xx I2xy I2xz]; I2(2,:) = [I2xy I2yy I2yz]; I2(3,:) = [I2xz I2yz I2zz]; I3(1,:) = [I3xx I3xy I3xz]; I3(2,:) = [I3xy I3yy I3yz]; I3(3,:) = [I3xz I3yz I3zz]; I4(1,:) = [I4xx I4xy I4xz]; I4(2,:) = [I4xy I4yy I4yz]; I4(3,:) = [I4xz I4yz I4zz]; I5(1,:) = [I5xx I5xy I5xz]; I5(2,:) = [I5xy I5yy I5yz]; I5(3,:) = [I5xz I5yz I5zz]; I6(1,:) = [I6xx I6xy I6xz]; I6(2,:) = [I6xy I6yy I6yz]; I6(3,:) = [I6xz I6yz I6zz]; 47 47 Simulación cinemática y dinámica de un vehículo controlado por control remoto en tres dimensiones Z0 = zeros(3,3); MM(1:3,:) = [M2 Z0 Z0 Z0 Z0 Z0 Z0 Z0 Z0 Z0]; MM(4:6,:) = [Z0 M3 Z0 Z0 Z0 Z0 Z0 Z0 Z0 Z0]; MM(7:9,:) = [Z0 Z0 M4 Z0 Z0 Z0 Z0 Z0 Z0 Z0]; MM(10:12,:) = [Z0 Z0 Z0 M5 Z0 Z0 Z0 Z0 Z0 Z0]; MM(13:15,:) = [Z0 Z0 Z0 Z0 M6 Z0 Z0 Z0 Z0 Z0]; MM(16:18,:) = [Z0 Z0 Z0 Z0 Z0 I2 Z0 Z0 Z0 Z0]; MM(19:21,:) = [Z0 Z0 Z0 Z0 Z0 Z0 I3 Z0 Z0 Z0]; MM(22:24,:) = [Z0 Z0 Z0 Z0 Z0 Z0 Z0 I4 Z0 Z0]; MM(25:27,:) = [Z0 Z0 Z0 Z0 Z0 Z0 Z0 Z0 I5 Z0]; MM(28:30,:) = [Z0 Z0 Z0 Z0 Z0 Z0 Z0 Z0 Z0 I6]; % Fuerzas generalizadas cuadr·ticas en velocidad Z0 = zeros(3,1); QQv(16:18,:) = -cross(w2L,I2*w2L); QQv(19:21,:) = -cross(w3L,I3*w3L); QQv(22:24,:) = -cross(w4L,I4*w4L); QQv(25:27,:) = -cross(w5L,I5*w5L); QQv(28:30,:) = -cross(w6L,I6*w6L); QQv(1:3,:) = Z0; QQv(4:6,:) = Z0; QQv(7:9,:) = Z0; QQv(10:12,:) = Z0; QQv(13:15,:) = Z0; % Fuerzas gravitatorias QQgrav(1:3,1) = [0 0 -m2*g]'; QQgrav(4:6,1) = [0 0 -m3*g]'; QQgrav(7:9,1) = [0 0 -m4*g]'; QQgrav(10:12,1) = [0 0 -m5*g]'; QQgrav(13:15,1) = [0 0 -m6*g]'; QQgrav(16:30,1) = zeros(15,1); % Matrices de transformaciÛn cinem·tica L = [H2' H3' H4' H5' H6' G2' G3' G4' G5' G6']'; l = [h2' h3' h4' h5' h6' g2' g3' g4' g5' g6']'; M = L'*MM*L; Qv = L'*(QQv-MM*l*dq); Qgrav = L'*QQgrav; Qmot = G3'*[0 ParM3 0]' + G4'*[0 ParM4 0]'; MatMasaFuerzas = [M Qv Qgrav Qmot]; %matlabFunction(MatMasaFuerzas,'file', 'MatMasaFuerzas'); toc Anexos 48 48 7.2 Anexo B. Simulación Dinámica Inversa %SIMULACION DINAMICA clear;clc tic global Param Param = ParametrosRover; Ind = Param.Ind; Dep = Param.Dep; nn = Param.nn; tetc= Param.tetc; %EXTRACCION DATOS load DATOS_1.mat; t = DATOS_1(:,1); tet3 = DATOS_1(:,2); dtet3 = DATOS_1(:,3); ddtet3 = DATOS_1(:,4); tet4 = DATOS_1(:,5); dtet4 = DATOS_1(:,6); ddtet4 = DATOS_1(:,7); wx = DATOS_1(:,8); wy = DATOS_1(:,9); wz = DATOS_1(:,10); ax = DATOS_1(:,11); ay = DATOS_1(:,12); az = DATOS_1(:,13); %CONDICIONES INICIALES 1 At = (t(end) - t(end-1)); ci=condicionesIniciales(tetc); q0 = [0 tetc 0 0 0 0 0.2 0.08 0.2 -0.08 0 0]'; [q_0,fval]=fsolve(@MatrizCh,q0,optimoptions('fsolve','Display','off')); x2_0 = ci(1); y2_0 = q_0(1); z2_0 = ci(2); psi2_0 = 0; tet2_0 = tetc; phi2_0 = 0; s3_0 = q_0(3); s4_0 = q_0(4); psi5_0 = 0; tet6_0 = 0; s6_0 = q_0(5); scpx_0 = q_0(6); scpy_0 = q_0(7); scqx_0 = q_0(8); scqy_0 = q_0(9); scsx_0 = q_0(10); scsy_0 = q_0(11); qdep_0 = [x2_0 y2_0 z2_0 psi2_0 tet2_0 phi2_0 s3_0 s4_0 psi5_0 tet6_0 s6_0 scpx_0 scpy_0 scqx_0 scqy_0 scsx_0 scsy_0]'; q = zeros(nn,length(t)); dq = zeros(nn,length(t)); 49 49 Simulación cinemática y dinámica de un vehículo controlado por control remoto en tres dimensiones ddq = zeros(nn,length(t)); ParM3 = zeros(length(t),1); ParM4 = zeros(length(t),1); for i = 1 : length(t) %DATOS COORDENADAS INDEPENDIENTES qind = [tet3(i) tet4(i)]'; dqind = [dtet3(i) dtet4(i)]'; ddqind = [ddtet3(i) ddtet4(i)]'; if t(i) == 0.0 qdep = qdep_0; end q(:,i) = ReordenaCoord(qind,qdep); %CALCULO MATRIZ D D = MatrizD(q(:,i),tetc); Dind = D(:,Ind); Ddep = D(:,Dep); %CALCULO VELOCIDADES DEPENDIENTES dqdep=-Ddep\Dind*dqind; dq(:,i) = ReordenaCoord(dqind,dqdep); %Ch=MatrizCh(q(:,i),tetc); %Cnh=MatrizCnh(q(:,i),dq(:,i),tetc); %CALCULO MATRIZ dD dD = MatrizdD(q(:,i),dq(:,i),tetc); %CALCULO MATRIZ DE MASA Y FUERZAS GENERALIZADAS MMF = MatMasaFuerzas(q(:,i),dq(:,i),0,0); M = MMF(:,1:nn); Qv = MMF(:,nn+1); Qgrav = MMF(:,nn+2); %CALCULO MATRIZ FUERZAS MOTORAS G3 = MatrizG3(q(:,i)); G4 = MatrizG4(q(:,i)); S = [G3(2,:)' G4(2,:)']; M_dep = [M(:,1) M(:,2) M(:,3) M(:,4) M(:,5) M(:,6) M(:,8) M(:,10) M(:,11) M(:,12) M(:,13) M(:,14) M(:,15) M(:,16) M(:,17) M(:,18) M(:,19)]; M_ind = [M(:,7) M(:,9)]; D_dep = [D(:,1) D(:,2) D(:,3) D(:,4) D(:,5) D(:,6) D(:,8) D(:,10) D(:,11) D(:,12) D(:,13) D(:,14) D(:,15) D(:,16) D(:,17) D(:,18) D(:,19)]; D_ind = [D(:,7) D(:,9)]; %RESOLUCION Ax=b AA = zeros(36,36); bb = zeros(36,1); AA(1:19,1:17) = M_dep; AA(1:19,18:19) = -S; AA(1:19,20:36) = D'; AA(20:36,1:17) = D_dep; AA(20:36,18:19) = zeros(17,2); AA(20:36,20:36) = zeros(17,17); bb(1:19) = -M_ind*ddqind + Qgrav + Qv; Anexos 50 50 bb(20:36) = -dD*dq(:,i) - D_ind*ddqind; x=AA\bb; ParM3(i) = x(18); ParM4(i) = x(19); Lambdas(:,i) = x(20:36); ddqdep = x(1:17,1); %VELOCIDADADES Y ACELERACIONES dx2 = dqdep(1); ddx2 = ddqdep(1); dy2 = dqdep(2); ddy2 = ddqdep(2); dz2 = dqdep(3); ddz2 = ddqdep(3); dpsi2 = dqdep(4); ddpsi2 = ddqdep(4); dtet2 = dqdep(5); ddtet2 = ddqdep(5); dphi2 = dqdep(6); ddphi2 = ddqdep(6); ds3 = dqdep(7); dds3 = ddqdep(7); ds4 = dqdep(8); dds4 = ddqdep(8); dpsi5 = dqdep(9); ddpsi5 = ddqdep(9); dtet6 = dqdep(10); ddtet6 = ddqdep(10); ds6 = dqdep(11); dds6 = ddqdep(11); dscpx = dqdep(12); ddscpx = ddqdep(12); dscpy = dqdep(13); ddscpy = ddqdep(13); dscqx = dqdep(14); ddscqx = ddqdep(14); dscqy = dqdep(15); ddscqy = ddqdep(15); dscsx = dqdep(16); ddscsx = ddqdep(16); dscsy = dqdep(17); ddscsy = ddqdep(17); %INTEGRACION NUMERICA x2 = x2_0 + dx2*At + 0.5*At^2*ddx2; y2 = y2_0 + dy2*At + 0.5*At^2*ddy2; z2 = z2_0 + dz2*At + 0.5*At^2*ddz2; psi2 = psi2_0 + dpsi2*At + 0.5*At^2*ddpsi2; tet2 = tet2_0 + dtet2*At + 0.5*At^2*ddtet2; phi2 = phi2_0 + dphi2*At + 0.5*At^2*ddphi2; s3 = s3_0 + ds3*At + 0.5*At^2*dds3; s4 = s4_0 + ds4*At + 0.5*At^2*dds4; psi5 = psi5_0 + dpsi5*At + 0.5*At^2*ddpsi5; tet6 = tet6_0 + dtet6*At + 0.5*At^2*ddtet6; s6 = s6_0 + ds6*At + 0.5*At^2*dds6; scpx = scpx_0 + dscpx*At + 0.5*At^2*ddscpx; scpy = scpy_0 + dscpy*At + 0.5*At^2*ddscpy; scqx = scqx_0 + dscqx*At + 0.5*At^2*ddscqx; scqy = scqy_0 + dscqy*At + 0.5*At^2*ddscqy; scsx = scsx_0 + dscsx*At + 0.5*At^2*ddscsx; scsy = scsy_0 + dscsy*At + 0.5*At^2*ddscsy; %CAMBIO VALORES INICIALES x2_0 = x2; y2_0 = y2; z2_0 = z2; psi2_0 = psi2; tet2_0 = tet2; phi2_0 = phi2; s3_0 = s3; s4_0 = s4; psi5_0 = psi5; tet6_0 = tet6; s6_0 = s6; scpx_0 = scpx; scpy_0 = scpy; scqx_0 = scqx; scqy_0 = scqy; scsx_0 = scsx; scsy_0 = scsy; q(:,i) = [x2 y2 z2 psi2 tet2 phi2 tet3(i) s3 tet4(i) s4 psi5 tet6 s6 scpx scpy 51 51 Simulación cinemática y dinámica de un vehículo controlado por control remoto en tres dimensiones scqx scqy scsx scsy]'; qdep = [x2 y2 z2 psi2 tet2 phi2 s3 s4 psi5 tet6 s6 scpx scpy scqx scqy scsx scsy]'; end %Animacion AnimaRover(q,tetc) Anexos 52 52 7.3 Anexo C. AnimaRover function AnimaRover(q,tetc) global Param Rf = Param.Rf; Rr = Param.Rr; Lar = Param.Lar; Anc = Param.Anc; Alt = Param.Alt; cdgx = Param.cdgx; cdgz = Param.cdgz; C1 = Param.C1; C2 = Param.C2; C3 = Param.C3; C4 = Param.C4; % Chasis Rover uAA1bar = [Lar-cdgx Anc/2 cdgz]'; uAA2bar = [Lar-cdgx -Anc/2 cdgz]'; uAA3bar = [-cdgx -Anc/2 cdgz]'; uAA4bar = [-cdgx Anc/2 cdgz]'; uBB1bar = [-cdgx C3/2 cdgz]'; uBB2bar = [-cdgx -C3/2 cdgz]'; uBB3bar = [-cdgx-C3 -C3/2 cdgz]'; uBB4bar = [-cdgx-C3 C3/2 cdgz]'; uDD1bar = [Lar-cdgx Anc/2 -cdgz]'; uDD2bar = [Lar-cdgx -Anc/2 -cdgz]'; uDD3bar = [-cdgx -Anc/2 -cdgz]'; uDD4bar = [-cdgx Anc/2 -cdgz]'; uEE1bar = [Lar-C1 -Anc/2 -cdgz]'; uEE2bar = [Lar-C1 -Anc/2-C2 -cdgz]'; uFF1bar = [Lar-C1 Anc/2 -cdgz]'; uFF2bar = [Lar-C1 Anc/2+C2 -cdgz]'; uGG2bar = [0 0 -(Alt+Rf-Rr)]'; alfa = 0 : pi/8 : 2*pi; [~,col] = size(q); for i = 1 : col A2 = RotMatSolido2(q(:,i)); R2 = [q(1,i) q(2,i) q(3,i)]'; % Rueda derecha u3bar = [Lar-C1 -Anc/2-C2 -cdgz]'; R3 = R2 + A2*u3bar; A3 = RotMatSolido3(q(:,i)); RP = zeros(3,18); for j = 1 : length(alfa) RP(:,j) = R3 + Rf*A3*[cos(alfa(j)) 0 sin(alfa(j))]'; 53 53 Simulación cinemática y dinámica de un vehículo controlado por control remoto en tres dimensiones end RP(:,18) = R3; % Rueda izquierda u4bar = [Lar-C1 Anc/2+C2 -cdgz]'; R4 = R2 + A2*u4bar; A4 = RotMatSolido4(q(:,i)); RQ = zeros(3,18); for j = 1 : length(alfa) RQ(:,j) = R4 + Rf*A4*[cos(alfa(j)) 0 sin(alfa(j))]'; end RQ(:,18) = R4; % Rueda trasera A5 = RotMatSolido5(q(:,i)); u5bar = [-cdgx-C3/2 0 cdgz]'; R5 = R2 + A2*u5bar; u6bar = [-C4 0 -(Alt+Rf-Rr)]'; R6 = R5 + A5*u6bar; A6 = RotMatSolido6(q(:,i)); RS = zeros(3,18); for j = 1 : length(alfa) RS(:,j) = R6 + Rr*A6*[cos(alfa(j)) 0 sin(alfa(j))]'; end RS(:,18) = R6; %Ploteo Chasis Rover RAA1 = R2 + A2*uAA1bar; RAA2 = R2 + A2*uAA2bar; RAA3 = R2 + A2*uAA3bar; RAA4 = R2 + A2*uAA4bar; RBB1 = R2 + A2*uBB1bar; RBB2 = R2 + A2*uBB2bar; RBB3 = R2 + A2*uBB3bar; RBB4 = R2 + A2*uBB4bar; RDD1 = R2 + A2*uDD1bar; RDD2 = R2 + A2*uDD2bar; RDD3 = R2 + A2*uDD3bar; RDD4 = R2 + A2*uDD4bar; REE1 = R2 + A2*uEE1bar; REE2 = R2 + A2*uEE2bar; RFF1 = R2 + A2*uFF1bar; RFF2 = R2 + A2*uFF2bar; RGG2 = R5 + A5*uGG2bar; Chasis = [RAA1 RAA2 RAA3 RAA4 RAA1 RDD1 RDD2 RAA2 RDD2 RDD3 RAA3... RDD3 RDD4 RAA4 RDD4 RDD1]; x = Chasis(1,:); y = Chasis(2,:); z = Chasis(3,:); figure(1000) grid on plot3(x,y,z,'y'); hold on