Observación de estados de un vehículo a control remoto mediante modelo de sistema multicuerpo, medidas inerciales y visión artificial

Equation Chapter 1 Section 1 Trabajo Fin de Máster Máster en Diseño Avanzado en Ingeniería Mecánica Observación de Estados de un Vehículo a Control Remoto Mediante Modelo de Sistema Multicuerpo, Medidas Inerciales y Visión Artificial Autor: Pedro Urda Gómez Tutor: Sergio Muñoz Moreno Dpto. Ingeniería Mecánica y Fabricación Escuela Técnica Superior de Ingeniería Universidad de Sevilla Sevilla, 201 8 2 3 Trabajo Fin de Máster Máster en Diseño Avanzado en Ingeniería Mecánica Observación de Estados de un Vehículo a Control Remoto Mediante Modelo de Sistema Multicuerpo, Medidas Inerciales y Visión Artificial Autor: Pedro Urda Gómez Tutor: Sergio Muñoz Moreno Profesor titular Dpto. de Teoría Ingeniería Mecánica y Fabricación Escuela Técnica Superior de Ingeniería Universidad de Sevilla Sevilla, 2018 4 5 Trabajo Fin de Máster: Observación de Estados de un Vehículo a Control Remoto Mediante Modelo de Sistema Multicuerpo, Medidas Inerciales y Visión Artificial Autor: Pedro Urda Gómez Tutor: Sergio Muñoz Moreno 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 6 7 Índice Índice de Figuras ........................................................................................................................ 9 Índice de Tablas ........................................................................................................................ 11 Introducción .............................................................................................................................. 13 1.1. Antecedentes ............................................................................................................. 13 1.2. Descripción general de las modificaciones realizadas en el Róver ................... 15 Modelo multicuerpo del vehículo .......................................................................................... 17 2.1 Identificación paramétrica ...................................................................................... 17 2.1.1 Modelo CAD del vehículo.............................................................................. 17 2.1.2 Determinación experimental del centro de masas del vehículo ............... 19 2.1.3 Determinación experimental del tensor de inercias del vehículo ............. 21 2.2 Modelo cinemático ................................................................................................... 25 2.3 Modelo dinámico. ..................................................................................................... 31 2.4 Simulación cinemática y dinámica inversa en Matlab del modelo empleando datos experimentales. .......................................................................................................... 32 Sistema de visión computacional para la reconstrucción de la trayectoria ..................... 40 3.1 Reconstrucción de una trayectoria 2D mediante visión artificial ...................... 40 3.1.1 Cinemática de la Visión Artificial .................................................................. 42 3.1.2 Simplificación para el caso 2D ........................................................................ 43 3.2 Calibración del sistema de visión computacional ............................................... 45 3.3 Puesta a punto del sistema de visión artificial ..................................................... 47 Electrónica del vehículo........................................................................................................... 52 4.1 Rediseño de la circuitería del vehículo y del mando a distancia ...................... 52 4.2 Algoritmo de control del vehículo ......................................................................... 56 4.3 Sistema de adquisición de datos ............................................................................ 58 4.4 Puesta a punto del sistema de adquisición ........................................................... 60 Conclusiones y trabajos futuros ............................................................................................. 62 Bibliografía ................................................................................................................................ 64 Anexo 1 ...................................................................................................................................... 65 Esquemas eléctricos ................................................................................................................. 65 Anexo 2 ...................................................................................................................................... 68 8 Algoritmo de control del vehículo ......................................................................................... 68 Anexo 3 ...................................................................................................................................... 72 Programación en C++ de la Mbed ......................................................................................... 72 Anexo 4 ...................................................................................................................................... 78 Simulación cinemática con Maltab ........................................................................................ 78 Anexo 5 ...................................................................................................................................... 80 Simulación dinámica inversa con Maltab ............................................................................. 80 9 Índice de Figuras Figura 1. Construcción original del Róver 14 Figura 2. Controladores Arduino Fio (Izquierda) y Mbed (Derecha) 15 Figura 3. Modelo CAD del Róver 18 Figura 4. Determinación del cdg sobre el primer eje 19 Figura 5. Determinación del cdg sobre el segundo eje 20 Figura 6. Determinación experimental del cdg 21 Figura 7. Determinación experimental de los momentos principales de inercia 22 Figura 8. Oscilación en torno al eje Z 23 Figura 9. Respuesta en frecuencia de la señal oscilatoria 23 Figura 10. Desalineamiento entre los ejes principales de inercia y los de oscilación durante el experimento 25 Figura 11. Modelización del vehículo. 26 Figura 12. Simulación cinemática de la trayectoria seguida por el vehículo 34 Figura 13. Velocidades angulares en z medidas y simuladas 35 Figura 14. Aceleraciones lineales en y simuladas y medidas 36 Figura 15. Aceleraciones lineales en x simuladas y medias 36 Figura 16. Pares motores en las ruedas 38 Figura 17. Trayectoria integrada en la simulación dinámica inversa 39 Figura 18. Esquema de la cinemática del problema 41 Figura 19. Cinemática de la visión artificial 42 Figura 20. Sistema de visión artificial. 46 Figura 21. Elemento fotosensible cámara de fotos. 46 Figura 22. Montaje del sistema de visión artificial 48 Figura 23. Fotograma captado durante el experimento de reconstrucción de trayectorias 49 Figura 24. Trayectoria reconstruida por el algoritmo de visión artificial 49 Figura 25. Trayectoria simulada 50 Figura 26. Velocidades angulares reales y simuladas 51 Figura 27. Vista en planta del Róver 53 Figura 28. Placa base del Róver en Eagle® 54 Figura 29. Ubicación de la IMU en el vehículo 55 16 Finalmente, el mando también ha sido rediseñado. Fundamentalmente se ha intentado simplificar al máximo su funcionamiento, suprimiendo la pantalla LCD que montaba originalmente y que lo único que hacía era complicar el sistema. El mando sí que tenía de origen un Arduino Fio con módulo inalámbrico Xbee, el cual obviamente se ha mantenido. Como innovaciones en el diseño del mando se han introducido cuatro botones para el control del vehículo y un Joystick de mayor funcionalidad que el original. Con respecto a la alimentación se ha pasado de las dos baterías Lipo a una pila recargable de 9V. Una vez conseguido un sistema lo suficientemente fiable se ha construido un modelo del vehículo en MATLAB, con el que se puede simular el comportamiento dinámico del vehículo. Dicho trabajo aparte de ser utilizado en las prácticas de la asignatura Cinemática y Dinámica de Máquinas, pretende ser también útil con fines investigadores. En las prácticas de Cinemática y Dinámica de Máquinas, los alumnos realizan un experimento de campo con el vehículo en el que recogen datos que luego utilizarán para la validación de un modelo matemático del Róver que ellos mismos deben construir durante las prácticas. Es también de gran interés para el Grupo de Ingeniería Mecánica de la Universidad de Sevilla la auscultación de superficies a partir de medidas de sensores inerciales. Gracias al trabajo desarrollado en este proyecto se dispone ahora de un vehículo equipado con los sensores necesarios para llevar a cabo dicha tarea. La idea es poder utilizar el modelo desarrollado en este TFM en combinación con algoritmos de fusión se sensores y filtrado Kalman para conseguir una lectura precisa de la geometría del terreno por el cual es vehículo se va desplazando. 17 Capítulo 2 Modelo multicuerpo del vehículo En este segundo capítulo desarrollaremos el modelo multicuerpo del Róver que nos permitirá simular su comportamiento cinemático y dinámico a partir de unos inputs sintetizados u obtenidos directamente de los sensores. Para ello se debe primero caracterizar el vehículo identificando todos sus parámetros geométricos másicos e inerciales. Una vez hecho eso se puede proceder a generar el modelo simbólico del vehículo y a partir de él simular su cinemática y su dinámica. 2.1 Identificación paramétrica 2.1.1 Modelo CAD del vehículo. Para poder identificar los parámetros másicos e inerciales, en un primer paso se ha procedido a generar un modelo 3D del vehículo en Solidworks a partir del cual obtendremos la posición del centro de gravedad y valores del tensor de inercia del vehículo. En la figura 3 vemos dicho modelo . 18 Tabla 1. Masas de los elementos del vehículo Elemento Masa Motor 0.178 kg Rueda 0.022 kg Chasis 0.328 kg Placa base 0.170 kg Eje jockey 0.323 kg Rueda jockey 0.090 kg Con respecto a la posición del centro de gravedad del vehículo, suponiendo que tomamos como referencia el centro de la placa inferior del chasis (color morado) con el eje X alineado en la dirección de avance del vehículo, el eje Z perpendicular al chasis y hacia arriba y el eje Y formando un sistema a derechas con los anteriores, según Solidworks el centro de gravedad del vehículo está ubicado a -32.32 mm en X y a 39.84 mm en Z. Está posición parece a priori totalmente razonable al estar la masa del vehículo mayormente concentrada en la parte trasera del mismo. Figura 3. Modelo CAD del Róver 19 El tensor de inercias calculado por Solidworks resulta ser: 2 0.0015 0 0 0 0.001 0.0001 0 0.0001 0.002 I kg m     =      i 2.1.2 Determinación experimental del centro de masas del vehículo Para cerciorarnos de que los resultados obtenidos son correctos, alternativamente a los cálculos numéricos se han realizado una serie de experimentos para determinar de forma empírica la posición del centro de gravedad del Róver y sus momentos principales de inercia. Supongamos que tenemos un objeto plano con geometría irregular. Si colgado dicho objeto de un punto cualquiera de su superficie este tenderá a orientarse de forma que su centro de gravedad quede alineado con el punto desde el que se ha colgado. Si repetimos dicho experimento colgando el sólido desde un punto distinto volverá a producirse el mismo fenómeno. La intersección de ambas líneas marcará la posición del centro de gravedad. En las figuras siguientes se resume el proceso seguido. Figura 4. Determinación del cdg sobre el primer eje 20 Figura 5. Determinación del cdg sobre el segundo eje Superponiendo ambas imágenes con la escala adecuada llegamos a la construcción geométrica siguiente, donde la intersección de las líneas verde y amarilla determinan la posición del cdg. Tomando como referencia el centro de la rueda y midiendo paralela y perpendicularmente al chasis se obtienen las cotas dx y dz. Aplicando el factor de escala del dibujo se llega a que los valores obtenidos son: 37.67 37.8 dx dz =− = Como puede observarse los valores obtenidos son aproximadamente iguales a los calculador por Solidworks. 21 dx dz Figura 6. Determinación experimental del cdg 2.1.3 Determinación experimental del tensor de inercias del vehículo La determinación experimental del tensor de inercias del vehículo requiere de un proceso algo más sofisticado que el simple cálculo de la posición del centro de masas de un sólido. Para obtenerlo se debe colgar el vehículo de distintos puntos, hacerlo oscilar en torno a cada uno de los ejes donde se quiere calcular el momento y registrar con el giróscopo la velocidad angular medida en torno a dicho eje. Teniendo en cuenta que durante el experimento la única fuerza que actúa sobre el sistema y que lo hacen pendulear es la gravedad se puede llegar a determinar la inercia polar en torno a ese eje. En la figura 7 vemos como se debe colocar el vehículo para obtener los momentos de inercia polar respecto a cada uno de los ejes. Es fundamental que cuando el vehículo está colgado quede perfectamente alineado con los puntos de colgado, ya que de lo contrario al poner en movimiento el sistema esté se comportaría como un péndulo doble. Recordar que el fenómeno que estamos intentando provocar es un movimiento de péndulo simple en torno a cada uno de los ejes. Para las dos primeras posiciones representadas en la figura 7, dadas las simetrías del vehículo, se puede conseguir el alineamiento antes comentado, pero sin embargo, para el tercer caso no es posible hacer oscilar el vehículo directamente en torno al eje X del sistema. En este caso el vehículo una vez 22 colgado queda formando un cierto ángulo respecto al eje X sobre el que nos interesa conocer la medida. Para corregir esto deberemos aplicar la transformación geométrica conveniente desde el eje de rotación hasta el eje X deseado. Figura 7. Determinación experimental de los momentos principales de inercia En la figura 8 se muestra el resultado de la adquisición realizada con el giróscopo mientras se ha hecho oscilar el vehículo en torno al eje Z. Como puede observarse la oscilación se va amortiguando con el tiempo y no se ve contaminada por ninguna frecuencia rara. Ello quiere decir que efectivamente el sólido ha oscilado como péndulo simple. La figura 9 se corresponde con respuesta en frecuencia calcula a la señal del giróscopo. A partir de las frecuencias naturales de oscilación obtenidas en cada eje se pueden calcular los momentos principales de inercia con el procedimiento que se describe a continuación. 23 Figura 8. Oscilación en torno al eje Z Figura 9. Respuesta en frecuencia de la señal oscilatoria Las ecuaciones siguientes describen en detalle el procedimiento seguido para obtener los momentos polares de inercia en cada eje y a partir de ellos calcular los momentos principales de inercia del vehículo. Nótese que por simplicidad se está asumiendo que todos los productos cruzados de inercia son nulos. Con este desarrollo se pretende obtener una estimación de la magnitud del tensor de inercias del vehículo para poder así comparar con el resultado generado por el software de CAD donde se ha realizado el modelo. Vel.Angular(º/s) 24 ( ) 2 G n I m l m l g ω + ⋅ =⋅ ⋅ (1) 2 P G z n g I I m l l ω   = = ⋅ ⋅ −     (2) P z xx yy I I I = + (3) Procediendo de forma análoga, P y xx zz I I I = + (4) Para el caso del eje X, al no producirse la oscilación exactamente en torno a él, es preciso realizar una transformación geométrica desde el eje X 1 de oscilación hasta el eje X deseado mediante la matriz de orientación , 1 x x A indicada en la ecuación 5 . ( ) , 1 cos 0 sin( ) 0 1 0 sin( ) 0 cos( ) x x a a A a a −     =       (5) En la figura 10 se observa el desalineamiento entre los ejes principales de inercia y los ejes en torno a los cuales se está produciendo la oscilación en este último experimento. Procediendo según se indica con las ecuaciones siguientes se obtiene: ( ) , 1 , 1 1 T x x x x I A I A= ⋅ ⋅ (6) ( ) 2 2 1 1 1 sin ( ) sin ( ) xx xx zz I I a I a = ⋅ − + ⋅ (7) ( ) 2 2 1 1 1 sin ( ) sin ( ) zz zz xx I I a I a = ⋅ − + ⋅ (8) 25 Figura 10. Desalineamiento entre los ejes principales de inercia y los de oscilación durante el experimento Si a las ecuaciones (3), (4), (7) y (8), añadimos la ecuación (9) llegamos a un sistema determinado de 5 ecuaciones con 5 incógnitas del cual obtenemos los momentos principales de inercia buscados. Se ha tenido en cuenta que los ejes yy e 1 yy se mantienen paralelos. 1 1 1 1 P x zz yy zz yy I I I I I = + = + (9) Los resultados obtenidos de resolver el sistema de ecuaciones anterior resultan ser I xx = 0.0024, I yy = 0.00083 e I zz = 0.0021. Lo cual viene a corroborar muy aproximadamente lo obtenido del modelo CAD. 2.2 Modelo cinemático Para llevar a cabo la confección del modelo cinemático se debe comenzar por realizar una modelización del vehículo real. De esta manera se pasa desde un sistema real complejo a una simplificación teórica que puede ser modelada computacionalmente. En la figura 11 se muestra el conjunto de sólidos original y su versión simplificada. 32 Las fuerzas gravitatorias: 2 6 0 0 ... 0 0 0 ... 0 grav m g QQ m g       − ⋅         =       − ⋅             (49) Y las fuerzas motrices: 3 4 0 0 3 4 0 0 T T mot Q G ParM G ParM         = ⋅ + ⋅             (50) Las matrices de transformación cinemáticas calculadas anteriormente se ensamblan de la siguiente forma: 2 6 2 6 ... ... T T T T L H H G G   =   (51) 2 6 2 6 ... ... T T T T l h h g g   =   (52) Combinando las expresiones anteriores llegamos a la expresión de las ecuaciones del movimiento que en el próximo epígrafe serán manipuladas y ensambladas. ( ) T T T v T grav grav M L MML Q L QQv MM l q Q L QQ = = − ⋅ ⋅ = ɺ (53) 2.4 Simulación cinemática y dinámica inversa en Matlab del modelo empleando datos experimentales. 33 En el anexo 4 de este trabajo aparecen los códigos de Matlab programados para llevar a cabo la simulación cinemática. A continuación se describe brevemente cómo funciona el algoritmo y se muestran los resultados obtenidos de la simulación cinemática. Lo primero que se hace es cargar los parámetros del modelos, es decir: masas, inercias y dimensiones que previamente han sido determinadas. A continuación se cargan los datos obtenidos en un experimento realizado con el vehículo. Dichos datos han sido previamente pre-procesados para introducirlos en un formato adecuado al programa de simulación. Los inputs al modelo serán exclusivamente los giros de ambas ruedas captados por los encoder y sus derivadas primera y segunda. Los demás datos de aceleraciones y velocidades angulares se usan exclusivamente para la validación del modelo. Decir que el experimento que se ha incluido en este trabajo consiste en un viaje del vehículo donde describe una trayectoria en L de ida y vuelta. El siguiente paso es hacer una división de las coordenadas en dependientes e independientes. Teniendo en cuenta que el número total de coordenadas del problema es según (10) 7 n = y que el número de ecuaciones de restricción según (43) es 5 m = , el número de grado de libertad del sistema será 2 g n m = − = . Esto tiene sentido ya que recordemos que las entradas físicas a nuestro sistema son las señales de control a los dos motores del vehículo. Teniendo en cuenta todo lo anterior tomaremos la siguiente división de coordenadas. [ ] [ ] 3 4 2 2 2 5 6 ind dep q q x y θ θ ψ ψ θ = = (54) La simulación cinemática consistirá en ir resolviendo el modelo en cada instante de tiempo e ir determinando las coordenadas dependientes a partir de los valores de las independientes según las ecuaciones siguientes. ( ) ( ) 1 dep ind dep Rod Rod ind q B B q − = − ⋅ ɺ ɺ (55) ( ) ( ) 1 dep ind dep Rod Rod ind Rod q B B q B q − = − ⋅ + ⋅ ɺ ɺɺ ɺɺ ɺ (56) Una vez conocidas las velocidades y aceleraciones independientes aplicando una simple regla de integración obtenemos los valores de las coordenadas dependientes. 34 0 2 2 2 2 2 1 2 x x x t t x = + ⋅∆ + ⋅∆ ⋅ ɺ ɺɺ (57) 0 2 2 2 2 2 1 2 y y y t t y = + ⋅∆ + ⋅∆ ⋅ ɺ ɺɺ (58) 0 2 2 2 2 2 1 2 t t ψ ψ ψ ψ = + ⋅∆ + ⋅∆ ⋅ ɺ ɺɺ (59) 0 2 5 5 5 5 1 2 t t ψ ψ ψ ψ = + ⋅∆ + ⋅∆ ⋅ ɺ ɺɺ (60) 0 2 6 6 6 6 1 2 t t θ θ θ θ = + ⋅∆ + ⋅∆ ⋅ ɺ ɺɺ (61) La figura siguiente representa la trayectoria seguida por el vehículo, como puede observarse no se trata de una L perfecta de ida y vuelta. Ello se debe a que el modelo presenta el denominado drift y que los resultados obtenidos difieren de la realidad. El drift aparece debido a que al tener un modelo con restricciones de tipo no-holónomo no se puede ir resolviendo la posición (es decir los valores de las coordenadas) en cada iteración, sino que se están resolviendo unas restricciones función de las derivadas de las coordenadas. Ello implica que se vayan acumulando errores en cada iteración. Figura 12. Simulación cinemática de la trayectoria seguida por el vehículo Una vez calculados los valores de las coordenadas se puede validar el resultado de la simulación comparando las aceleraciones que experimentaría una IMU virtual situada en el mismo lugar del vehículo donde se haya colocado el sensor x2(m) 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 -2.5 -2 -1.5 -1 -0.5 0 0.5 1 Trayectoria Rover 35 físico. Para ello es preciso realizar un sencillo post-proceso a los datos obtenidos. La velocidad angular experimentada por el cdg del chasis del vehículo vendrá dada por la ecuación (62). En la figura 14 se observa como la señal de velocidad angular en el eje z medida por la IMU y la simulada con el modelo casan casi a la perfección. Figura 13. Velocidades angulares en z medidas y simuladas Para hacer una comparación similar con las aceleraciones medidas y simuladas es preciso hacer una serie de manipulaciones previas. El proceso seguido es el descrito en las ecuaciones de la (62) a la (65). Donde IMU u es la posición de colocación de la IMU expresada en el sistema de referencia del chasis. ( ) 2 2 2 IMU T IMU R A R A u= ⋅ + ⋅ ɺɺ ɺɺ ɺɺ (62) ( ) 2 2 A A α ω ω = + ⋅ ⋅ ɺɺ ɶ ɶ ɶ (63) Gq gq α = + ɺɺ ɺ (64) 2 0 0 IMU T IMU a R A g     = + ⋅       ɺɺ (65) En las figuras 15 y 16 se observa la comparación entre la aceleración lineal en x e y medidas por la IMU y generadas por el modelo. Cómo puede observase, la t(s) 0 5 10 15 20 25 30 35 40 -3 -2 -1 0 1 2 3Velocidad angular en Z (rad/s) wzMOD wzIMU 36 aceleración en y es calculada razonablemente bien, se aprecian perfectamente los giros efectuados por el vehículo para seguir la trayectoria en L. No ocurre lo mismo con la aceleración lineal en x donde aunque siendo ambas señales iguales en magnitud no se capta totalmente su forma a partir de la simulación. Figura 14. Aceleraciones lineales en y simuladas y medidas Figura 15. Aceleraciones lineales en x simuladas y medias t(s) 0 5 10 15 20 25 30 35 40 -1 -0.8 -0.6 -0.4 -0.2 0 0.2 0.4 0.6 0.8 1Aceleración en y (m/s2) ayMOD ayIMU t(s) 0 5 10 15 20 25 30 35 40 -0.8 -0.6 -0.4 -0.2 0 0.2 0.4 0.6 Aceleración en x (m/s2) axMOD axIMU 37 Para realizar la simulación dinámica inversa procedemos de forma similar. De dicha simulación obtendremos las fuerzas que hacen posible el movimiento a partir de los datos de entrada procedentes de los encoder. Al igual que en la simulación cinemática se mantiene la división en coordenadas dependientes e independientes indicada en (54). Las velocidades dependientes se obtiene a partir de las velocidades independientes a partir de la ecuación (55). Las aceleraciones se obtendrán mediante la resolución de las ecuaciones de Newton-Euler expresadas en (66) y (67). Teniendo en cuenta que del experimento conocemos los valores de aceleración de las coordenadas independientes obtenidos mediante derivación, dichos valores deben ser introducidos en las ecuaciones de Newton-Euler como inputs para a partir de ellos obtener el valor de las fuerzas motrices que ponen al vehículo en movimiento. Dichas fuerzas serán los pares motores aplicados a cada una de las ruedas. Las ecuaciones de Newton-Euler vienen dadas por: T ap v Mq B Q Q λ + = + ɺɺ (65) 0 Bq Bq + = ɺ ɺɺ ɺ (66) Separando los términos dependientes de los independientes llegamos a: 3 4 ¨ T dep dep ind ind M M grav v M q M q B Q Q Q Q λ + + = + + + ɺɺ ɺɺ (67) 0 dep dep ind ind B q B q Bq + + = ɺ ɺɺ ɺɺ ɺ (68) Despejando términos conocidos a la derecha e incógnitas a la izquierda se llega a: 0 0 dep T ind ind grav v dep ind ind dep q M q Q Q M S B TBq B q B λ     − ⋅ + +−     ⋅ =       − −         ɺɺ ɺɺ ɺɺ (69) Donde: 3 4 M M Par T Par   =       (70) 3 4 (2,:) (2,:) T T S G G   =   (71) 38 Resolviendo el sistema de ecuaciones de (69) se obtienen las aceleraciones dependientes. Conocidas ya las velocidades y aceleraciones dependientes, emulando el proceso de integración seguido en (57) a (61) se obtienen los valores de las coordenadas dependientes. En las gráficas siguientes se muestran la trayectoria simulada seguida por el vehículo y los pares motores en cada rueda. Como puede observarse la trayectoria sigue presentando el mismo drift que con la simulación cinemática. Figura 16. Pares motores en las ruedas t(s) 0 5 10 15 20 25 30 35 40 Par(Nm) -20 -15 -10 -5 0 5 10 Pares en las ruedas 39 Figura 17. Trayectoria integrada en la simulación dinámica inversa x2(m) 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 y2(m) -2.5 -2 -1.5 -1 -0.5 0 0.5 1 Trayectoria Rover 40 Capítulo 3 Sistema de visión computacional para la reconstrucción de la trayectoria En este capítulo se describe el proceso de creación de un sistema de visión computacional para la reconstrucción de la trayectoria seguida por el vehículo. Se ha desarrollado un algoritmo capaz de identificar una trayectoria bidimensional a partir de las imágenes captadas por una video cámara durante el movimiento del vehículo. Finalmente se explican los detalles de la calibración y puesta a punto del sistema de visión artificial. 3.1 Reconstrucción de una trayectoria 2D mediante visión artificial Se pretende reconstruir la trayectoria descrita por un punto P que se desplaza sobre el plano definido por los puntos G1, G2, G3 y G4 a partir de las imágenes tomadas por una cámara web ubicada en una posición arbitraria del espacio. Supondremos que el punto P, cuya trayectoria queremos reconstruir, se moverá siempre sobre el plano de trabajo, cuyo sistema de referencia vendrá dado por el triedro < X, Y, Z >. De aquí en adelante lo denominaremos sistema de referencia global. 41 P P´X CAM Y CAM Z CAM f r P CAM r P´ CAM X Y Z G1 G2 G4 G3 Ψ CAM ϴ CAM ϕ CAM R P R CAM R G 3 P Plano Sensor Plano de Trabajo Cámara Video Figura 18. Esquema de la cinemática del problema Las coordenadas de los puntos G1, G2, G3 y G4 respecto al sistema de referencia global se suponen conocidas y vendrán dadas por las expresiones (1) a (4). [ ] 1 1 1 0 T G G G R X Y= (72) [ ] 2 2 2 0 T G G G R X Y= (73) [ ] 3 3 3 0 T G G G R X Y= (74) [ ] 4 4 4 0 T G G G R X Y= (75) Por otra parte, supondremos que la cámara de vídeo encargada de ir tomando los distintos fotogramas de la trayectoria, estará ubicada en una posición arbitraria y desconocida del espacio, dada por el vector de la expresión (76). [ ] T CAM CAM CAM CAM R X Y Z= (76) Al mismo tiempo, el sistema de referencia de la cámara dado por el triedro < X CAM , Y CAM , Z CAM > tendrá una determinada orientación respecto del sistema de referencia global, y estará dada por los ángulos de Euler φ CAM , θ CAM y ψ CAM . La matriz de orientación del sistema de referencia de la cámara respecto del sistema de referencia global vendrá dada por la siguiente expresión. 48 Figura 22. Montaje del sistema de visión artificial Una vez calibrado el sistema se puede dar inicio a la experimentación. Para ello se hace rodar el vehículo dentro del área de control y se grava su movimiento con la video cámara. Existen múltiples algoritmos que permiten detectar el movimiento de un objeto en un conjunto de imágenes. En este trabajo no se ha utilizado ninguno de esos algoritmos sino que se ha hecho un seguimiento a un punto fijo del vehículo. Ese punto es un led blanco que va montado sobre el chasis del vehículo. Con un ajuste correcto de parámetros en la cámara, puede hacerse esta menos insensible a otras tonalidades y hacerla captar perfectamete la luz del diodo. En la figura 23 se observa un fotograma del video grabado durante el experimento. El algoritmo programado en Matlab va localizando y almacenando en cada iteración la posición del led blanco en cada fotograma. 49 Figura 23. Fotograma captado durante el experimento de reconstrucción de trayectorias Una vez procesado el vídeo y obtenidas las posiciones en píxeles que el vehículo ha ido ocupando se introducen esos datos en el algoritmo de visión artificial. Tras resolver las ecuaciones (87) a (90) devolverá las posiciones reales en el plano por donde el vehículo a pasado. En la figura 24 se muestra la reconstrucción de la trayectoria realizada por el algoritmo. Figura 24. Trayectoria reconstruida por el algoritmo de visión artificial Y(m) 50 Si tomamos los datos adquiridos por la IMU y los encoder de las ruedas y los introducimos en el modelo cinemático o dinámico utilizado en el capítulo anterior podemos ver la contrastación entre la trayectoria real seguida por el vehículo (a partir del algoritmo de visión artificial) y la simulada por el programa. Figura 25. Trayectoria simulada En la figura 26 se representan las velocidades angulares reales y simuladas, como puede observarse hay un alto grado de acuerdo entre ambas señales. La figura 25 muestra la trayectoria simulada y como puede observarse no llega a ser cerrada como la real. Ello se debe al igual que antes al drift presentado por el modelo. y2(m) 51 Figura 26. Velocidades angulares reales y simuladas wz(rad/s) 52 Capítulo 4 Electrónica del vehículo En este segundo capítulo se explican los detalles del proceso de rediseño y fabricación de la circuitería del vehículo y del mando a distancia. También se recoge todo lo relativo a nuevo sistema de control del vehículo y al nuevo sistema de adquisición de datos. Finalmente se presentan algunos resultados obtenidos durante la puesta a punto del sistema. 4.1 Rediseño de la circuitería del vehículo y del mando a distancia Para dar solución a todos los problemas electrónicos mencionados en el capítulo primero, se ha llevado a cabo un rediseño y construcción del hardware del vehículo y de su mando a distancia. En la figura 27 y tabla 3 se muestran e indican los distintos elementos que componen la placa base del vehículo. Como puede observarse, las diferencias respecto al diseño original son notables. Uno de los principales avances tecnológicos introducidos ha sido pasar de una placa base soldada sobre una baquelita perforada estándar, a un circuito impreso diseñado y construido a medida para esta aplicación. El diseño del circuito integrado principal se ha diseñado con Eagle®, software específico para el diseño de este tipo de aplicaciones electrónicas. En la figura 28 se muestra el diseño de pistas resultante de Eagle®. Una vez validado el diseño se procedió a su fabricación mediante el método convencional de 53 insolación, revelado y ataque con ácido. El resultado obtenido fue altamente satisfactorio. Figura 27. Vista en planta del Róver Una vez fabricada la placa, se realizaron los taladros de los pines y se soldaron los componentes. Todos los componentes se han soldado a la placa y no son removibles con excepción de los microcontroladores y el puente H que están instalados sobre racks de pines soldados a la placa. La alimentación de la placa se realiza a partir del paquete de tres baterías Lipo conectadas en serie, que suministran una tensión nominal de aproximadamente 12 V. Con este voltaje sólo puede ser alimentado directamente el puente H que lleva integrado su propio regulador de corriente. Para dar energía a los microcontroladores y los encoder se han instalado limitadores de tensión a 5 y 9 voltios del tipo LM78xx acompañados de disipador de calor. 54 Tabla 2. Componentes electrónicos de la placa base del Róver Número Nombre del componente 1 Motor DC con encoder modeloPololu 30:1 Metal Gearmotor 37Dx52L mm 64 CPR Encoder. 2 Puente H modelo estándar con 2 salidas. 3 Microcontrolador Mbed LPC1768. 4 Microcontrolador Arduino Fio + Xbee RF. 5 Dispositivo de almacenamiento externo formato FAT32. 6 Limitador de tensión LM78xx con disipador de calor. 7 Antena receptora. Figura 28. Placa base del Róver en Eagle® Por último, la IMU utilizada en el vehículo es una unidad inercial compuesta de dos sensores. Un acelerómetro BMA180 y un giróscopo ITG3200. El dispositivo se conecta a través de protocolo I 2 C a la Mbed. Se ha instalado dentro del chasis del vehículo en una posición muy próxima al centro de masas del vehículo como puede observarse en la figura 29. La IMU, junto con los encoders y la Mbed constituyen el sistema de adquisición de datos del vehículo. 55 Figura 29. Ubicación de la IMU en el vehículo De igual forma que para la placa base del vehículo se describen a continuación las características del mando a distancia y sus diferentes funciones. En la figura 30 y tabla 4 se muestran y describen los componentes del dispositivo. Figura 30. Componentes del mando a distancia 56 Tabla 3. Componentes electrónicos del mando a distancia. Número Nombre del componente 1 Microcontrolador Arduino Fio + Xbee RF. 2 Batería 9V. 3 Interruptor On/Off. 4 Limitadores de tensión tipo LM7805sin disipador. 5 Joystick analógico 5V. 6 Botón STOP. 7 Botón LOG. 8 Botón corregir deriva a izquierda. 9 Botón corregir deriva a derecha. La tecnología de fabricación del mando a distancia ha sido idéntica a la de la placa base del vehículo. La alimentación se realiza a partir de una pila convencional de 9V cuya tensión es reducida y estabilizada a 5V por dos limitadores tipo LM7805 para suministrar energía al microcontrolador y al joystick. La botonera está enrutada con el microcontrolador. Su misión es la de enviar señales discretas a una serie de pines del mismo y en función de su valor producir diferentes acciones de control. El botón STOP se utiliza para detener completamente los motores del vehículo, el botón LOG inicia y finaliza la adquisición de datos. Los botones R y L corrigen la deriva lateral del vehículo como se explicará en el siguiente epígrafe. El joystick como era de esperar, se utiliza para controlar los movimientos del vehículo. En el anexo 1 se muestra el esquema eléctrico simplificado de la placa base del vehículo y del mando a distancia. 4.2 Algoritmo de control del vehículo Como se ha comentado anteriormente, el control de los motores del vehículo se realiza por medio del controlador Arduino Fio. El algoritmo de control programado es un control en bucle abierto, es decir, no se retroalimenta de la información de los encoder para saber en cada instante la velocidad de cada motor. El motivo de haber elegido esta forma de control se debe a que el microcontrolador Arduino no tiene suficiente potencia como para gestionar el cálculo de velocidad de ambos motores, la ejecución del algoritmo PID de control de la velocidad y el control remoto del vehículo simultáneamente. 57 Inicialmente se realizaron diversas pruebas pero el comportamiento del vehículo no fue en absoluto el esperado. Figura 31.Control lazo cerrado Figura 32. Control lazo abierto Con el control en lazo abierto lo que se hace es enviar directamente a ambos motores una señal tipo PWM proporcional a la velocidad de avance que se quiere conseguir. Supuesto que ambos motores son idénticos, si la señal de control es la misma en ambos, el vehículo debe avanzar en línea recta. La realidad es que ni los motores son exactamente iguales ni tampoco lo son las señales de control que salen del puente H. Como resultado aparece una leve discrepancia entre la velocidad de giro de ambos motores respecto de la consigna establecida. Para solucionar este problema se determina experimentalmente un factor que multiplicará la señal de control de uno de los motores de manera que la velocidad resultante de ambos sea la misma. 64 Bibliografía [1] Apuntes de la asignatura Cinemática y Dinámica de Máquinas. [2] Manual online Mbed: https://developer.mbed.org/handbook/Homepage [3] Manual online Arduino: https://playground.arduino.cc/Es/Es [4] J. García de Jalón, E. Bayo, Kinematic and Dynamic Simulation of Multibody Systems. [5] A. Shabana, Dynamics of Multibody Systems. [6] A. Shabana, E. Zaazaa, H. Sugiyama, RailRoad Vehicle Dynamics. [7] D. Gross, W. Huger, J. Schröder, A. Wall, S. Govindjee, Engineering Mechanics 3. [8] Sen M. Kuo, Real-Time Digital Signal Processing. [9] H. Press, A. Teukilshy, Numerical Recipes, The art of scientific computing. 65 Anexo 1 Esquemas eléctricos 66 A1.1 Esquema eléctrico del mando 67 A1.2 Esquema eléctrico de la placa base 68 Anexo 2 Algoritmo de control del vehículo A2.1 Algoritmo del mando a distancia int analogPin4 = 4; int analogPin5 = 5; intParar = 11; int Grabar = 10; int Izquierda = 9; int Derecha = 8; int v4 = 0; int v5 = 0; void setup() { Serial.begin(9600); pinMode(Parar,INPUT); pinMode(Grabar,INPUT); pinMode(Izquierda,INPUT); pinMode(Derecha,INPUT); } void loop() { v4 = analogRead(analogPin4); v5 = analogRead(analogPin5); if ( v4 > 900 && (v5 > 600 && v5 < 900)){ Serial.println("A"); } if ( v4 < 600 && (v5 > 600 && v5 < 900)){ Serial.println("B"); } if ( (v4 > 600 && v4 < 900) && v5 < 600){ Serial.println("D"); 69 } if ( (v4 > 600 && v4 < 900) && v5 > 900){ Serial.println("I"); } if( (v4 > 600 && v4 < 900) && (v5 > 600 && v5 < 900)){ Serial.println("C"); } if(digitalRead(Parar) == HIGH){ Serial.println("S"); } if(digitalRead(Grabar) == HIGH){ Serial.println("G"); } if( digitalRead(Derecha) == HIGH){ Serial.println("R"); } if( digitalRead(Izquierda) == HIGH){ Serial.println("L"); } delay(10); } A2.2 Algoritmo del vehículo intVel; float Vel1; float Vel2; floatGiroD; floatGiroI; int P1D = 8; int P2D = 11; int P1I = 12; int P2I = 13; int Mot1 = 9; int Mot2 = 10; intestado = 0; char letter0 = 'X'; intmaximo = 300; int logging = 6; booleangrabar = false; float factor = 0.87; floatcorreccion = 0.003; void setup() { pinMode(P1D,OUTPUT); pinMode(P2D,OUTPUT); 70 pinMode(P1I,OUTPUT); pinMode(P2I,OUTPUT); digitalWrite(P1D,HIGH); digitalWrite(P2D,LOW); digitalWrite(P1I,HIGH); digitalWrite(P2I,LOW); analogWrite(Mot1,0); analogWrite(Mot2,0); Serial.begin(9600); pinMode(logging,OUTPUT); digitalWrite(logging,LOW); } void loop() { if(Serial.available()>0){ char letter = Serial.read(); if (letter == 'A'){ if (estado<maximo){ estado++; letter0 = letter; } } if (letter == 'B'){ if (estado> 0){ estado--; letter0 = letter; } } if (letter == 'C'){ letter0 = letter; GiroD = 1; GiroI = 1; } if (letter == 'I'){ GiroI = 0.4; GiroD = 1.4; letter0 = letter; } if (letter == 'D'){ GiroD = 0.4; GiroI = 1.4; letter0 = letter; } if (letter == 'S'){ estado = 0; 71 } if (letter == 'G'){ grabar = !grabar; digitalWrite(logging,grabar); } if (letter == 'R'){ factor = factor + correccion; } if (letter == 'L'){ factor = factor - correccion; } Vel = map(estado,0,maximo,0,200); Vel1 = Vel*GiroD; Vel2 = Vel*GiroI; } analogWrite(Mot1,Vel2); analogWrite(Mot2,Vel1*factor); } 72 Anexo 3 Programación en C++ de la Mbed A3.1 Algoritmo del mando a distancia #include "mbed.h" #include "RingBuffer.h" #include "BMA180_mod.h" #include "ITG3200.h" #include "QEI.h" #include "USBHostMSD.h" intboton = 0; intbotonantes = 0; intbotonahora = 0; inttipoAdquisicion = 0; float tiempo = 0; intContadorEnsayos = 0; int16_taX, aY, aZ; intwX, wY, wZ; float Acel3[3] = {0,0,0}; float Gyro3[3] = {0,0,0}; float a_xBias3, a_yBias3, a_zBias3; int w_xBias3, w_yBias3, w_zBias3; intpprev = 32; intpulsos_R = 0, pulsos_L = 0; QEI wheel_R (p21, p22, NC, pprev); QEI wheel_L (p23, p24, NC, pprev); ITG3200 gyro3(p28,p27); BMA180 acc3(p28,p27); /// PEN DRIVE /// USBHostMSD *msc; 73 LocalFileSystemlocal("mbedFlash"); DigitalOutled(LED1); DigitalOutled2(LED2); DigitalOutled3(LED3); DigitalOutled4(LED4); DigitalInboton_Pin(p17); Timer timer; FILE * fp; FILE * fp2; voidadquisition(void); voidGuardaDatosBuffer(void); voidcalibracion(void); voidinicializaIMU(void); Ticker tickerLectura; intsizeBuff = 100; intPasoTiempo = 10000; unsignedintContadorDatos = 0; Buffer buff_aX(sizeBuff); Buffer buff_aY(sizeBuff); Buffer buff_aZ(sizeBuff); Buffer buff_wX(sizeBuff); Buffer buff_wY(sizeBuff); Buffer buff_wZ(sizeBuff); Buffer buff_wheel_R(sizeBuff); Buffer buff_wheel_L(sizeBuff); int main() { msc = new USBHostMSD("local"); led4 = 1; if(!msc->connected()){ while(!msc->connect()) { } } led4 = 0; inicializaIMU(); led3=1; calibracion(); led3=0; tickerLectura.attach_us(&adquisition,PasoTiempo); timer.start(); led = 0; 80 Anexo 5 Simulación dinámica inversa con Maltab % Simulación Dinámica Inversa % Simulación Dinámica Inversa Róver 7 coordenadas clc clear closeall global Param Param = ParametrosRóver; Ind = Param.Ind; Dep = Param.Dep; nn = Param.nn; inputs = importdata('V62_b.mat'); t = inputs(:,1); tet3 = inputs(:,2); dtet3 = inputs(:,3); ddtet3 = inputs(:,4); tet4 = inputs(:,5); dtet4 = inputs(:,6); ddtet4 = inputs(:,7); ax = inputs(:,8); ay = inputs(:,9); az = inputs(:,10); wx = inputs(:,11); wy = inputs(:,12); wz = inputs(:,13); At = t(end) - t(end-1); x2_0 = 0; y2_0 = 0; psi2_0 = 0; psi5_0 = 0; tet6_0 = 0; qdep_0 = [x2_0 y2_0 psi2_0 psi5_0 tet6_0]'; q = zeros(nn,length(t)); dq = zeros(nn,length(t)); ddq = zeros(nn,length(t)); ParM3 = zeros(length(t),1); ParM4 = zeros(length(t),1); for i = 1 : length(t) 81 t(i) 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) = ReordenaCoordenadas(qind,qdep); BRod = MatrizB_Rodadura(q(:,i)); BRodind = BRod(:,Ind); BRoddep = BRod(:,Dep); dqdep = -BRoddep\(BRodind*dqind); dq(:,i) = ReordenaCoordenadas(dqind,dqdep); dBRod = dMatrizB_Rodadura(q(:,i),dq(:,i)); MMF = MatMasaFuerzas(q(:,i),dq(:,i),0,0); M = MMF(:,1:nn); Qv = MMF(:,nn+1); Qgrav = MMF(:,nn+2); G3 = MatrizG3(q(:,i)); G4 = MatrizG4(q(:,i)); S = [G3(2,:)' G4(2,:)']; M_dep = [M(:,1) M(:,2) M(:,3) M(:,6) M(:,7)]; M_ind = [M(:,4) M(:,5)]; BRod_dep = [BRod(:,1) BRod(:,2) BRod(:,3) BRod(:,6) BRod(:,7)]; BRod_ind = [BRod(:,4) BRod(:,5)]; AA = zeros(12,12); bb = zeros(12,1); AA(1:7,1:5) = M_dep; AA(1:7,6:7) = -S; AA(1:7,8:12) = BRod'; AA(8:12,1:5) = BRod_dep; AA(8:12,6:7) = zeros(5,2); AA(8:12,8:12) = zeros(5,5); bb(1:7) = -M_ind*ddqind + Qgrav + Qv; bb(8:12) = -dBRod*dq(:,i) - BRod_ind*ddqind; x = AA\bb; ParM3(i) = x(6); ParM4(i) = x(7); Lambdas(:,i) = x(8:12); ddqdep = x(1:5,1); 82 dx2 = dqdep(1); ddx2 = ddqdep(1); dy2 = dqdep(2); ddy2 = ddqdep(2); dpsi2 = dqdep(3); ddpsi2 = ddqdep(3); dpsi5 = dqdep(4); ddpsi5 = ddqdep(4); dtet6 = dqdep(5); ddtet6 = ddqdep(5); x2 = x2_0 + dx2*At + 0.5*At^2*ddx2; y2 = y2_0 + dy2*At + 0.5*At^2*ddy2; psi2 = psi2_0 + dpsi2*At + 0.5*At^2*ddpsi2; psi5 = psi5_0 + dpsi5*At + 0.5*At^2*ddpsi5; tet6 = tet6_0 + dtet6*At + 0.5*At^2*ddtet6; x2_0 = x2; y2_0 = y2; psi2_0 = psi2; psi5_0 = psi5; tet6_0 = tet6; q(:,i) = [x2 y2 psi2 tet3(i) tet4(i) psi5 tet6]'; qdep = [x2 y2 psi2 psi5 tet6]'; end