Descripción, actualización y ensayo de un vehículo radiocontrolado para la medida de irregularidades del terreno mediante sensores inerciales

Equation Chapter 1 Section 1 Trabajo Fin de Grado Grado en Tecnologías en Ingeniería Industriales Descripción, actualización y ensayo de un vehículo radiocontrolado para la medida de irregularidades del terreno mediante sensores inerciales Autor: José Cabello del Río Tutor: José Luis Escalona Franco Andrés Francisco Hidalgo Dep. de Ingeniería Mecánica y de Fabricación Escuela Técnica Superior de Ingeniería Universidad de Sevilla Sevilla, 2015 iii Trabajo Fin de Grado Grado en Ingeniería de Tecnologías Industriales Descripción, actualización y ensayo de un vehículo radiocontrolado para la medida de irregularidades del terreno mediante sensores inerciales Autor: José Cabello del Río Tutor: José Luis Escalona Franco Profesor titular Andrés Francisco Hidalgo Profesor ponente Dep. de Ingeniería Mecánica y de Fabricación Escuela Técnica Superior de Ingeniería Universidad de Sevilla Sevilla, 2015 v Trabajo Fin de Grado: Descripción, actualización y ensayo de un vehículo radiocontrolado para la medida de irregularidades del terreno mediante sensores inerciales Autor: José Cabello del Río Tutor: José Luis Escalona Franco Andrés Francisco Hidalgo El tribunal nombrado para juzgar el Proyecto arriba indicado, compuesto por los siguientes miembros: Presidente: Vocales: Secretario: Acuerdan otorgarle la calificación de: Sevilla, 2015 El Secretario del Tribunal vii A mi familia A mis maestros ix Agradecimientos Gracias a mi hermana María, mi padre Juan Antonio, mi madre María A. y mi pareja María del Mar por sus consejos, su apoyo y su ánimo, que han sido el combustible que me ha dado la energía necesaria para llegar al final de mis estudios. Gracias a mis compañeros por hacer amenos estos años de esfuerzo, sacrificio y estudio. Gracias a mis profesores por su exigencia, sus clases y sus explicaciones. Gracias al departamento de Ingeniería Gráfica por permitirnos el uso de una estación total y a Cristina Torrecillas por facilitarnos su utilización realizando la calibración del aparato. Por todos ellos GRACIAS. Figura 37: Ensayo a 100Hz variando el número de puntos de pérdida a una frecuencia 58 Figura 38: Ensayo a 1000 Hz variando la frecuencia de pérdida de datos 59 Figura 39: Ensayo a 1000Hz variando el número de puntos de pérdida a una frecuencia 59 xvii ÍNDICE DE CÓDIGOS Código 1: Función Encoder 30 Código 2: Función enc 30 Código 3: Función getrpm 30 Código 4: Función getpulses 30 Código 5: Función controlVelocity 31 Código 6: Función speed 32 Código 7: Función readIMU 35 Código 8: Funcion GuardaDatosBuffer 36 Código 9: Programa arduino 37 Código 10: Generación perfiles aleatorios 42 Código 11: Cálculo señales modelo 43 Código 12: Tratado de señales de los ensayos para el uso del filtro de Kalman 50 xix NOTACIÓN TRV Track Recording Vehicles TRC Track Recording Coaches MEMS Micro Electro-Mechanical Systems IMU Inertial Measurement Unit PWM pulse-width modulation SCl Serial Clock SDA Serial Data MSB Most Significant Bit LSB Less Significant Bit PSD Power Spectral Density FFT Fast Fourirer Transform Notación 20 1 INTRODUCCIÓN Este Trabajo de Fin de Grado se desarrolla con la ayuda del departamento de Ingeniería Mecánica de la Escuela Técnica Superior de Ingenieros de Sevilla. Se sitúa en el ámbito de la auscultación de vías ferroviarias, particularmente en la obtención de perfiles de superficies mediante un vehículo teledirigido que toma medidas de las aceleraciones y giros sufridos durante su recorrido con la ayuda de una IMU (Inertial Measurement Unit). El objetivo es conseguir un vehículo auscultador que reproduzca fielmente la geometría de la pista que atraviesa con el fin de conseguir desarrollar tecnologías de auscultación de menor coste que las usadas actualmente, basadas en modelos matemáticos. 1.1 Objetivo y estado del arte La auscultación de vías consiste en evaluar las condiciones en las que se encuentra una vía midiendo diversos parámetros como pueden ser la geometría de los carriles o la catenaria. Estas mediciones se realizan a través de trenes o vagones auscultadores acoplados a trenes convencionales, que son capaces de medir los parámetros de la infraestructura mientras circulan por ella [1]. Existen dos tipos de medidas: geométricas y dinámicas. Las geométricas miden las variaciones de geometría respecto a una geometría de referencia mientras que las dinámicas miden la respuesta del vehículo que circula ante dichas irregularidades [2]. Los métodos de auscultación tradicionales estaban basados únicamente en la realización de medidas geométricas de la vía tales como calibre, superficie, alineación, nivelación transversal giro… y su comparación con los límites de los parámetros predefinidos con el fin de comprobar que la geometría de la vía se encontraba dentro de los márgenes que proporcionaban una marcha segura de vehículos. Estas técnicas se veían obligadas a un análisis off-line después de la recolección en la pista impidiendo así el conocimiento instantáneo de los resultados y por tanto del estado de la pista [3]. De modo que ha habido en la industria un movimiento por evaluar la calidad y rugosidad de la geometría de la vía basada en estimaciones en tiempo real. Algunos estudios como los realizados en las referencias [4] y [5] proponen además un estudio de la respuesta dinámica del vehículo ante dichas irregularidades mediante modelos de contacto entre rueda y carril. Los medios actuales de auscultación ferroviaria como los TRV (Track Recording Vehicles) y los “hauled” TRC (Track Recording Coaches) conllevan costes de operación y mantenimeinto muy elevados [6]. Por el contrario, la auscultación geométrica mediante sensores inerciales (como los MEMS (Micro Electro- Mechanical Systems) de muy bajo coste comparados con otras tecnologías) colocados en distintas partes de vehículos en servicio es una alternativa de más bajo coste y que permite una inspección más regular de las características de la vía. Siendo estas características a su vez más beneficiosas para realizar mantenimiento predictivo, reduciendo así los costes generales de mantenimiento y mejorando las condiciones de seguridad [7]. Dada la relevancia de obtener sistemas auscultadores basados en la información proveniente de sensores inerciales colocados sobre vehículos en servicio, en el año 2013 se comenzó la construcción de un vehículo autónomo dirigido remotamente sobre el cual se colocaría una IMU (Inertial Measurement Unit) con el objetivo de poder estimar las irregularidades geometricas de terrenos con curvas suaves por donde el vehículo circulase. Introducción 22 22 Al inicio de este trabajo se contaba con una versión preliminar (sobre todo desde un punto de vista del software) del vehículo auscultador cuya construcción formó parte del Proyecto de Fin de Carrera “Desarrollo, instrumentación y construcción de un vehículo radio controlado para la medida de irregularidades del terreno mediante sensores inerciales” [2]. Figura 1: Elementos que componen el mando y el Rover Como se observa en la Figura 1 el vehículo constaba de dos motores con sus respectivos decodificadores, un puente H, un microcontrolador mbed, dos antenas XBee (una para el mando y otra para el vehículo), un microcontrolador Arduino FIO y una pantalla de Adafruit para el control remoto, una IMU y un puerto USB para conectar una memoria flash donde almacenar los datos obtenidos dutante los ensayos. En esta versión inicial del vehículo autónomo se lograron conectar todas las partes del hardware interviniente a los microcontroladores y se estableció intercambio de señales entre el mando y el vehículo, pero el control mostraba ciertos problemas para mover el vehículo con una determinada precisión y fluidez, sin interrupciones debidas a diversos fallos y sin retardos entre el envío y la recepción de la señal de aceleración, frenado y giro. Estos problemas con el mando del vehículo a su vez impidieron analizar la lectura de aceleraciones y velocidades angulares desde la IMU, no llegándose a almacenar datos en la memoria flash y no ejecutándose ningún ensayo real con el vehículo. Por este motivo el principal objetivo de este Trabajo de Fin de Grado es el de realizar ensayos reales con el vehículo, obteniéndose así los datos de las mediciones de la IMU para estimar las irregularidades del perfil sobre el que se mueve el vehículo. Con este fin y teniendo en cuenta las mejoras que eran necesarias implementarse principalmente en el software de los microcontroladores del mando y del vehículo se obtendrá una versión mejorada del vehículo auscultador que se permita obtener la geometría de perfiles aleatorios mediante la utilización de sensores inerciales de bajo costo. Si bien la construcción del vehículo se ha mantenido inalterado salvo el cambio del puente H por uno nuevo y la mejora de algunas conexiones puntuales que impedían el correcto funcionamiento de los elementos que componen el aparato. La estructura de este documento consiste en siete secciones, incluyendo las conclusiones y referencias. En la Sección 2 se lleva a cabo una descripción general del funcionamiento del software que controla tanto el control remoto como el vehículo auscultador detallando con mayor profundidad los cambios efectuados en el mismo para mejorar la robustez de la ejecución del programa; en la Sección 3 se muestra la estimación de irregularidades en perfiles aleatorios mediante el Filtro de Kalman; a continuación en la Seccion 4 se describe la realización de los ensayos de auscultación usando el Rover para obtener los datos recogidos por los sensores de inercia de la IMU; las tres secciones restantes recogen los resultados y conclusiones . Finalmente se recoge la documentación usada para la realización de este trabajo. 23 1.2 Fundamento teórico El movimiento del Rover sobre un perfil aleatorio se puede modelar, como se muestra en la Figura 2, como un sistema de dos grados de libertad, desplazamiento vertical x y giro θ, al que se le impone un desplazamiento en la base asociado al perfil. Figura 2: Modelo Rover - Perfil El objetivo es calcular el perfil del terreno que recorre el vehículo, denotado por , a partir de los datos obtenidos del acelerómetro y giróscopo . Para ello se tienen en cuenta las siguientes hipótesis: - La velocidad del vehículo a lo largo del recorrido es constante - Los neumáticos se consideran rígidos - El vehículo no tiene suspensión De la Figura 2 se obtienen las siguientes relaciones: 0 () 2 L zz xt   (1) 0 () L zz tL    (2) Donde x(t) representa la posición de la IMU en función del tiempo, z0 la coordenada vertical del perfil en la rueda delantera, zL la coordenada vertical del perfil de la rueda trasera, θ(t) es el ángulo girado entre el vehículo con respecto a la horizontal y L la longitud de este. La aceleración vertical que sufre el sensor se conoce derivando dos veces respecto al tiempo la coordenada x como se muestra en las siguientes expresiones: dx ds x x v ds dt   (3) 2 dx ds dv x v x x v x v ds dt dt        (4) Donde x y x son la derivada primera y segunda de la Ecuación (1) respecto de s y v es la velocidad de avance del vehículo, siendo 0 z , L z , 0 z y L z las derivadas primeras y segundas de las coordenadas verticales del perfil en la rueda delantera y trasera del vehículo con respecto a s. Introducción 24 24 0 2 L zz x   (5) 0 2 L zz x     (6) De igual forma se conoce la velocidad angular a la que se somete el sensor derivando con respecto al tiempo la coordenada θ: d ds v ds dt     (7) Donde: 0L zz L     (8) Las ecuaciones (4) y (7) representan la relación entre las señales IMU y la coordenada vertical del perfil por el que avanza el vehículo. Suponiendo   zs como una distribución senoidal   2sis z s Ze Ze        donde Z es la amplitud y la frecuencia, se tiene:   is x s Ae    (9) is e    (10) Para el caso de la aceleración, haciendo uso de la Ecuación (6) y (9):       2 22 1 22 i s i L i s L is is Ze e Ze Ze x s Ae            (11) De donde se obtiene:   2 11 2 iL A e Z      (12)     21 11 2 iL AeT Z       (13) Donde 1 T es la función de transferencia entre la aceleración y la irregularidad del perfil. Para la velocidad angular, haciendo uso de la Ecuación (8) y (10):       11 i s L is is i s i L i Ze i Ze e i Ze e LL             (14) De donde se obtienen la amplitud y la función de tranferencia entre la velocidad angular y la irregularidad 2 T , que se muestra en las expresiones (15) y (16):   11iL i e Z L      (15)     2 11iL i e T ZL      (16) 25 En la Figura 3 se observa que las funciones de transferencia (T1 y T2) de cada sensor se anulan para ciertas frecuecias. Esto significa que cada sensor es incapaz de detectar dichas frecuencias y debido a que 2     determinadas longitudes de onda. Por este motivo para obtener una descripción completa de la geometría del perfil se opta por la fusión de sensores mediante el uso del Filtro de Kalman, con el que se obtiene la estimación irregularidades de perfiles como se verá en la sección 3 Estimación de Irregularidades en Perfiles Aleatorios Mediante el Filtro de Kalman. Figura 3: Funciones de Transferencia Descripción y Mejoras del Rover 32 32 2.4 Puente H El Puente H es el elemento encargado de controlar la velocidad y el sentido de giro de los motores. Con el fin de gestionar el funcionamiento del puente H se ha incluido en el programa contenido en la Mbed una clase llamada Motor también disponible en la página oficial de Mbed [16]. En la Figura 7 se muestran las conexiones realizadas entre el procesador, el puente H y los motores. Las conexiones de color amarillo son las asociadas al motor derecho mientras que las de color gris al izquierdo. Mediante los pines 5, 6, 7 y 8 se envían las señales que indican a cada motor el sentido de giro, las variables _fwd indican que el sentido de giro es hacia adelante mientras que las _rev lo indican hacia atrás. Mediante los pines 21 y 22 se envían la señal PWM que indica al motor la velocidad de giro. Figura 7: Conexiones motores - puente H - mbed En el Código 6 se muestra la función speed encargada de la gestión del puente H que pertenece a la clase Motor. Esta recibe el valor de velocidad calculado por el control PID mediante la función compute como se observa en el Código 5. En función del signo del valor que recibe se determina el sentido de giro del motor enviando un uno o un cero a través de las variables DigitalOut _fwd o _rev. La variable PwmOut _pwm envía la señal PWM al motor haciéndolo girar a la velocidad deseada. Código 6: Función speed void Motor::speed(float speed) { _fwd = (speed > 0.0); _rev = (speed < 0.0); _pwm = abs(speed); } 33 2.5 IMU En la Figura 8 se observa la conexión de la IMU con el procesador, esta es una conexión puerto serie I2C [12] realizada mediante una placa Droteck. La configuración I2C se realiza mediante dos conexiones: SCL y SDA realizadas en los pines 27 y 28 de la Mbed denominando a este último como Maestro y al acelerómetro (BMA180) y al giróscopo (IGT3200) como Esclavos. Cada Esclavo tiene una dirección diferente, de modo que para leer la información recogida por la IMU el Maestro envía la dirección de esclavo del cual leerá y este envía la información solicitada. Figura 8: Conexión IMU – Mbed Con el fin de gestionar la lectura de la IMU se usan unas librerías disponibles en la página de Mbed [16]: BMA180 e IGT3200, sin embargo la primera necesitó ser modificada para leer los datos de aceleración adecuadamente ya que se detectó un error. Las señales digitales del acelerómetro están descritas por 2’s complement basado en 14 bits [12], de modo que el rango de medidas del acelerómetro de 2 g se obtiene de forma binaria: -2.00000g : 10 0000 0000 0000 -199975g : 10 0000 0000 0001 … -0.00025g : 11 1111 1111 1111 0.000000g : 00 0000 0000 0000 +0.00025g : 00 0000 0000 0001 … +1.99950g : 01 1111 1111 1110 +1.99975g : 01 1111 1111 1111 Descripción y Mejoras del Rover 34 34 Cada valor esta compuesto por MSB (Most significant Bit) y LSB (Less significant Bit) tal como se observa en la Figura 9 [12]. Figura 9: Mapa de memoria BMA180 La versión de la librería BMA180 descargada de página oficial de Mbed realizaba la lectura del LSB de cada dato de aceleración, debido a que los dos últimos bits de dicha lectura no son datos de aceleración estos eran eliminados y a continuación se leía el MSB almacenándose estos en una única variable que representaba el dato de aceleración. El hecho de eliminar los dos últimos bits antes de combinar el MSB y el LSB implicaba desplazar los bits no eliminados hacia la derecha colocando dos ceros en los bits 6 y 7 del LSB, de modo que al combinar estos para obtener el valor de aceleración se obtenía un dato no real. En la Figura 10 se representan las acciones descritas. Figura 10: Lectura aceleración librería BMA180 Debido a esto, se modificó la librería descargada de modo que se realizaran en primer lugar las lecturas del MSB y LSB, a continuación su combinación y por último la eliminación de los dos últimos bits para obtener el dato real del valor de aceleración, como se representa en la Figura 11. Figura 11: Lectura aceleración librería BMA180 modificada 2.6 Puerto USB Con el objetivo de gestionar las lectuas de la IMU se decidió utilizar un buffer donde almacenar los datos recogidos por los sensores para después escribirlos en el fichero de texto. Para ello se utiliza una librería, llamada RingBuffer, que contiene una clase llamada Buffer con la que se generan los objetos que realizan la escritura y lectura de los datos en el buffer. Se define un buffer de 500 celdas, como el representado en la Figura 12, asociado a cada parámetro que se almacenará en el fichero: tiempo del ensayo en el que se realiza la lectura, velocidad de ambas ruedas y aceleraciones y velocidades angulares en las direcciones X, Y y Z. Figura 12: RingBuffer 35 Como se recuerda del apartado 2.1 Descripción general del funcionamiento del Rover, el hilo uno de programación es el encargado de la lectura de los datos de la IMU mediante la función readIMU que se observa en Código 7. En ella se leen los datos de aceleración en g y las velocidades angulares en º/s de los sensores, además de las velocidades de los motores y a continuación se almacena esta información en el buffer correspondiente mediante la función put que pertene a la clase Buffer, esto se representa en la Figura 12 mediante el índice ESCRITURA. Código 7: Función readIMU void readIMU(void const *n){ Acel3[1] = -(acc3.getX() - a_xBias3)*0.00024417043; Acel3[0] = -(acc3.getY() - a_yBias3)*0.00024417043; Acel3[2] = -(acc3.getZ() - a_zBias3)*0.00024417043; Gyro3[1] = -(gyro3.getGyroX()- w_xBias3) / 14.375; Gyro3[0] = -(gyro3.getGyroY()- w_yBias3) / 14.375; Gyro3[2] = -(gyro3.getGyroZ()- w_zBias3) / 14.375; rpm_R = R_encoder.getrpm(); rpm_L = L_encoder.getrpm(); buff_velocA.put(rpm_R); buff_velocB.put(rpm_L); timeIMU.put(t.read()); buff_aX.put(Acel3[1]); buff_aY.put(Acel3[0]); buff_aZ.put(Acel3[2]); buff_wX.put(Gyro3[1]); buff_wY.put(Gyro3[0]); buff_wZ.put(Gyro3[2]); } Para almacenar la información del buffer en el fichero de texto se utiliza la función GuardaDatosBuffer que se presenta en el Código 8 por el índice LECTURA de la Figura 12. Esta función lee los datos almacenados en cada celda del buffer mediante la función get (perteneciente a la clase Buffer) y los escribe en el fichero creando una tabla como la de la Figura 13. El índice de LECTURA de desplaza siempre detrás del índice de ESCRITURA de modo que cuando llegan al final del buffer vuelven al inicio de este para continuar con el almacenamiento de datos. Teniendo en cuenta que los datos de las celdas en las que se sobresscribirá ya se encuentran almacenados en el fichero de texto. Tiempo ensayo Velocidad rueda derecha Velocidad rueda izquierda Aceleración eje X Aceleración eje Y Aceleracón eje Z Velocidad angular eje X Velocidad angular eje Y Velocidad angular eje Z . . . . . . . . . . . . . . . . . . . . . . . . . . . Figura 13: Tabla formato fichero de texto Descripción y Mejoras del Rover 36 36 Código 8: Funcion GuardaDatosBuffer void GuardaDatosBuffer(){ bool empty = buff_aX.isEmpty(); float tiempo; if(empty == false){ tiempo = ((float)ContadorDatos)*((float)PasoTiempo)/1.0e6; fprintf(fp,"%.2f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f \n", timeIMU.get(), buff_velocA.get(), buff_velocB.get(), buff_aX.get(), buff_aY.get(), buff_aZ.get(),buff_wX.get(), buff_wY.get(), buff_wZ.get()); myled2 = !myled2; ContadorDatos++; } } La memoria interna del Porcesador es de 512 KB [17], de modo que permite almacenar datos de ensayos de tan solo unos segundos de duración. Debido a la necesidad de realizar ensayos más largos el Rover incorpora un puerto USB para la conexión de un dispositivo de almacenamiento masivo externo para recoger la información del proceso de auscultación. 2.7 Mando a distancia El mando a distancia se controla a través del Arduino FIO al que se le conectan una Xbee para la transmisión de datos entre el mando y el Rover, una pantalla LCD donde imprimir información relativa al funcionamiento del vehículo y un joystick con el que dirigir el aparato. En la versión de partida con la que se comenzó a trabajar en este Trabajo de Fin de Grado la transmisión de datos entre el mando y el vehículo era muy lenta y poco fluida. Para mejorar este aspecto hubo que setear correctamente las direcciones de las dos Xbee con el programa X-CTU [18] de forma que cada una de ellas tuviera correctamente la dirección de la otra. Por otra parte el programa encargado de la transmisión de datos contenido en el arduino utilizaba muchos bloques lógicos if para detectar las señales y realizaba las lecturas de la Xbee cada cierto tiempo. Esto ha sido modificado sustituyendo el programa del arduino por el que se muestra en el Código 9 cuyo funcionamiento se describe a continuación. En este la lectura de las Xbee se realiza de forma libre, es decir con un baud rate de 4800, a diferencia del usado en la versión inicial de 9600. Con la introducción de estos cambios se mejoró notablemante la transferencia de datos entre el control remoto y el Rover. En al Figura 14 se observa la secuencia de tareas que realiza el programa contenido en el Arduino además del intercambio de información entre los distintos elementos que componen el control remoto. El joystick genera dos señales analógicas en función del movimiento del vástago que se han llamado VERT y HORIZ y que son recibidas por el arduino que a continuación realiza la escritura en la pantalla LCD de la velocidad real y de si se ejecuta un giro. El dato de velocidad real que se envía desde el Rover se recibe en tres paquetes, esto permite realizar un checksum para comprobar que no se pierde información durante la transmisión de datos y que esta se realiza correctamente. Primero se realizan las lecturas de la Xbee que recibe el dato de velocidad real y a continuación se realiza el checksum. Por último se codifican las señales del joystick para enviarlas hacia el Rover. Las señales generadas por el joystick toman valores de 0 a1023 de modo que si la señal vertical toma valores entre 515 y 535 la variable VERT toma el valor de 70 indicando que se mantiene la misma velocidad de avance; si es mayor que 535, VERT toma el valor de 75 indicando que se reduce la velocidad; mientras que si es menor que 515, la variable VERT toma el valor de 80 indicando un aumento de velocidad. 37 De igual forma sucede con la varible HORIZ tomando los valores 24, 25 y 26 si el joystick se encuentra en su posición central, si se desplaza hacia la derecha o hacia la izquierda respectivamente. Estos valores se envía a través de la Xbee y con ellos se controla el movimiento del vehículo. Código 9: Programa arduino #include <SPI.h> #include <Adafruit_GFX.h> #include <Adafruit_PCD8544.h> Adafruit_PCD8544 display… =Adafruit_PCD8544(8, 9, 10, 11, 12); const int VERT = 5; const int HORIZ = 6; const int SEL = 13; void setup() { pinMode(SEL,INPUT); digitalWrite(SEL,HIGH); Serial.begin(4800); display.begin(); display.setContrast(50); } int vertical_ref, horizontal_ref; int vertical_Accum, horizontal_Accum; int horizontal_big; int entero, rec, rec_aux, error,it, itchck; int rec_accum; int itmax = 100; char chnum1, chnum2, chnum3,e; char rounding, hznum1, hznum2; char hznum3, hznum4; int num1, num2, num3; void loop() { int vertical, horizontal, select, Vel_real; int Vx, vert, check; double Vel_ref; calibrate_joystick(); vertical = analogRead(VERT); horizontal = analogRead(HORIZ); select = digitalRead(SEL); display.clearDisplay(); display.setTextSize(1); display.setTextColor(BLACK); display.setCursor(0,0); display.println("Vreal: "); display.println(rec_accum); display.println("Rounding: "); display.println(horizontal_big); display.println("select: "); if(select == 1){ display.println(select); }else{ display.println("not "); digitalWrite(SEL,HIGH); } display.display(); it = 0; rec_aux = 0; if (Serial.available() > 0){ num1 = Serial.read(); num2 = Serial.read(); num3 = Serial.read(); itchck = 0; if(((num1 & num2) == num3)){ rec_accum = num2; rec_accum = rec_accum << 8; rec_accum = rec_accum + num1; error = 0; } else{ error = 1; rec_accum = e; } } if (515 <=vertical && vertical <= 535){ vert = 70; check = 1; } else if (535 < vertical){ vert = 75; check = 1; } else if(vertical < 515){ vert = 80; check = 1; } if (check ==1){ entero = vert; chnum1 = entero; Serial.write(chnum1); } check = 0; Descripción y Mejoras del Rover 38 38 if (490 <=horizontal && horizontal… <= 530){ horizontal_big = 24; } else if (horizontal > 530){ horizontal_big = 25; } else if(horizontal < 490){ horizontal_big = 26; } entero = horizontal_big; hznum1 = entero; Serial.write(hznum1); } void calibrate_joystick(void){ int i; vertical_Accum = 0; horizontal_Accum = 0; vertical_ref = 0; horizontal_ref = 0; for (i=0; i<100; i++){ vertical_Accum += analogRead(VERT); horizontal_Accum += analogRead(HORIZ); } vertical_Accum /= 100; horizontal_Accum /= 100; vertical_ref = vertical_Accum; horizontal_ref = horizontal_Accum; } Figura 14: Esquema funcionamiento mando a distancia Rover 39 3 ESTIMACIÓN DE IRREGULARIDADES EN PERFILES ALEATORIOS MEDIANTE EL FILTRO DE KALMAN En esta sección se explican los pasos seguidos para la obtención de un algoritmo que simula el proceso de auscultación de un vehículo auscultador de las características del Rover, como el de la Figura 15, que permita predecir la geometría de un perfil aleatorio a partir de señales de la IMU mediante el uso del filtro de Kalman. Para ello nos hemos basado en el algoritmo de auscultación propuesto por Hidalgo, Ros y Escalona en el documento “A Kalman filter-based algorithm for IMU signal fusion applied to track geometry estimation” [19]. Figura 15: Modelo vehículo auscultador De la Figura 15 se obtienen las ecuaciones (17) y (18) que representan la relación entre la geometría del perfil y las señales IMU: 20 ´z z s z s    (17) 0L zz s L     (18) Adicionalmente con un decodificador situado en la rueda delantera podemos conocer en cada instante de tiempo el ángulo rotado por esta mediante la siguiente expresión, donde R es el radio de la rueda: 0() () st tR   (19) De modo que usando la información procedente de los sensores, los parámetros de nuestro sistema y suponiendo los ruidos y errores de las medidas como variables estadísticas podemos estimar la geometría del perfil recorrido por el vehículo usando el filtro de Kalman discreto, descrito por las siguientes ecuaciones: Estimación de Irregularidades en Perfiles Aleatorios Mediante el Filtro de Kalman 40 40 1 1 1 1 1k k k k k k x A x G u w         (20) k k k k z H x v (21) Donde k es el instante de tiempo, x es el vector estado del sistema, u es el control input, z el vector de medida y w y v representan el ruido del proceso y de las medidas. A, H y G son matrices con las dimensiones apropiadas. El desarrollo en mayor detalle de las ecuaciones de estimación del algoritmo se encuentran en el documento anteriormente mencionado de la referencia [19]. El algoritmo se estructura como se muestra en la Figura 16. En primer lugar se genera un perfil aleatorio a partir del cual se calculan las señales teóricas de aceleración y velocidad angular que se obtendrían a través de la IMU para finalmente realizar la estimación del perfil aleatorio generado. Figura 16: Algoritmo de auscultación 3.1 Generación de un perfil aleatorio Para la generación de perfiles aleatorios se ha usado la teoría propuesta por Tyan, Hong, Tu y Jeng [20], en la que aproximan la geometría del perfil mediante una superposición de N funciones senoidales. La norma ISO sugiere determinados valores de longitudes de onda de las ondulaciones de la pista para la determinación de perfiles aleatorios, sin embargo para adaptar esta teoría a nuestro modelo se han tomado valores distintos, obtenidos en función de los rangos de trabajo de los sensores usados y de las dimensiones características de nuestro vehículo. 3.1.1 Definición de las irregularidades del camino aleatorio Para calcular las longitudes de onda máximas y mínimas que permiten detectar las aceleraciones y velocidades angulares con los sensores que se usan en el vehículo se supone que este recorre un camino sinusoidal. El rango de trabajo del acelerómetro es +/-2 g y su sensibilidad de 0.25 mg. La mínima aceleración viene dada por: 2 2min min min max v Az      (22) Donde Amin es la minima amplitud del perfil, vmin la velocidad minima de avance del vehículo y λmax la máxima longitud de onda detectable por el acelerómetro. El motor Pololu del Rover gira a 350 rpm sin carga y la rueda tiene un radio de 0.045 m, luego la velocidad máxima que puede alcanzarse es de 1.65 m/s. Si se establece que la velocidad mínima de los ensayos sea 1 m/s y suponiéndose como λmax 5 m, la amplitud mínima es Amin = 0.00155 m. Para determinar la longitud de onda mínima de las ondulaciones del perfil aleatorio se debe considerar que la mayor rueda del vehículo atraviese sin problemas la parte del perfil con menor radio de curvatura. Considerando que el camino es sinusoidal en función de la coordenada s y teniendo en cuenta la curvatura de una curva definida por una función h se tienen las relaciones (23) y (24), donde Ah representa la amplitud del camino, C la curvatura K el radio de curvatura:   2 sin h h s A s      (23) 3 22 '' 1 (1 ' ) h CK h   (24) 41 Debido a que el mayor radio de las ruedas del vehículo es de 0.045 m, se establece que el menor radio de curvatura del camino sea 0.05 m. De esta forma la máxima curvatura es: 2 21 ()0.05 max h CA    (25) Despejando de esta expresión se obtiene el valor de longitud de onda mínima del camino aleatorio. 2 0.05 0.055 min min Am   (26) El giróscopo usado es un IT 2 con una resolución de ( /sec)/count (0.0012(rad/sec)/count). Teniendo en cuenta que la distancia entre ejes del vehículo es 0.16 m se expresa la velocidad angular mínima como: 2 2 min min min max Av L    (27) Si se consideran las mismas condiciones que con el acelerómetro la amplitud mínima para el giróscopo es 1.08 e-4 m. Esta amplitud es menor que la obtenida con el acelerómetro, por lo que no se podrían detectar las aceleraciones generadas por ella dada la sensibilidad del acelerómetro que se está considerando. Por este motivo, la amplitud mínima usada para la generación de perfiles aleatorios es la obtenida con el acelerómetro y por tanto: min 0.055m   (28) max 5m   (29) 3.1.2 Función del camino aleatorio Suponiendo que el vehículo viaja a velocidad constante, la coordenada vertical z del perfil aleatorio del camino de longitud Lroad en función de la coordenada horizontal s puede aproximarse mediante la siguiente expresión. 1 ( ) sin( ) N i i i i z s A s       (30) Donde i son tratados como variables aleatorias siguiendo una distribución uniforme en el intervalo [0, 2π) y Ai es la amplitud definida como: ( ) , 1,2, ii A i N        (31) Donde   i  es la PSD (Power Spectral Density) para i  y 1 1 N N     y donde las i  se eligen como múltiplos de  . La PSD tiene la siguiente expresión:     2 0 0 1 2 01 0 , 0 () , N                           (32) Donde 01/rad m y   0  es el valor de la PSD para 0  , cuyo valor es función de la calidad de la geometría del perfil que se desea obtener. En nuestro caso se obtendrá una calidad pobre del perfil con el objetivo de que presente irregularidades notables para el vehículo, por lo que toma un valor de 64 e-6 m3. 1  y N  se definen como: 1 22 5 máx      (33) Pruebas Experimentales 48 48 Figura 22: Perfil del terreno donde se realizan los ensayos de auscultación 4.2 Ensayos sobre el tramo medido Los ensayos de auscultación consisten en recorrer con el Rover el tramo de pista de baloncesto de la Escuela medido como se describe en el apartado anterior 4.1. Con el fin de recorrer los 5 m de longitud del terreno a velocidad constante el vehículo arranca dos metros antes del punto inicial del tramo medido durante los cuales acelera hasta alcanzar la velocidad del ensayo y se detiene una vez recorrida la zona medida. En la Figura 23 se representa un esquema de las posiciones inicial y final del vehículo. Figura 23: Posición inicial y final del Rover en los ensayos Como resultado del proceso de auscultación el software que controla el Rover genera un fichero de texto que contiene, como se vió en la Figura 13, las velocidades de cada motor y las aceleraciones y velocidades angulares sufridas por el vehículo en cada instante de tiempo. Sabiendo que la IMU se encuentra orientada como se observa en la Figura 24 se concluye que los datos necesarios para estimar la geometría recorrida son la aceleración en dirección Z y la velocidad angular en dirección X, también son necesarias las velocidades de ambos motores ya que con ellas obtenemos el perfil de velocidad del vehículo durante el proceso de auscultación, la aceleración en dirección y nos da la aceleración en el sentido de avance del vehículo. 49 Figura 24: Orientación IMU Debido a que no se disponía de tiempo suficiente para aplazar la realización de estos ensayos, durante los mismos se tuvo un problema con el buffer encargado del almacenamiento de los datos medidos por el vehículo. Este problema era que después de un tiempo determinado la frecuencia de adquisición de los datos que se almacenaban aumentaba, volviéndose irregular. Debido a la importancia de tener una adquisición de datos a intervalos de tiempo constante que nos permita el uso de la FFT, con el objetivo de evitar este problema con el buffer se estableció una frecuencia de adquisión de datos menor de modo que nos permitiera recorrer el camino medido en un tiempo menor al que la frecuencia de muestreo empezaba a ser irregular. Teniendo en cuenta el tamaño del buffer y que las velocidades que desarrolla el Rover se establecen en torno a 1 m/s se decidió establecer la toma de datos con un periodo de 0.02 s ya que establecer un periodo de adquisición más bajo nos obligaba a realizar los ensayos a velocidades mayores. Estas pruebas a mayores velocidades no pudieron realizarse con éxito debido a que el tramo de camino medido es muy estrecho y esto dificultaba el poder controlar el vehículo para mantenerlo en la trayectoria deseada. Posteriormente a la realización de los ensayos se solucionó parcialmente este problema simplificando al máximo las funciones de lectura y escritura de los datos en el buffer. No obstante, aún habiendo solucionado el problema del buffer, se ha observado que los incrementos de tiempo no siempre son regulares sino que aproximadamente cada 40 datos almacenados aparece aparece un intervalo de tiempo mayor que los establecidos. Esto implica que el valor que debía ser medido en ese instante no se almacena. Debido a este fenómeno en el apartado 4.3 se realiza un pequeño análisis para evaluar como afecta el hecho de la aparición de estos intervalos de tiempo mayores en la aplicación de la FFT. De esta forma se realizaron dos ensayos para predecir la geometría del perfil medido que denominaremos como Ensayo 1 y Ensayo 2. El Código 12 representa el tratado de las señales del Ensayo 2 (siguiéndose una metodología similar para tratar las señales del Ensayo 1). El primer paso es eliminar tanto los datos incorrectos por el llenado del buffer como las primeras lecturas hasta que se empieza a medir el tiempo. A continuación se eliminan los datos no reales de velocidad. El siguiente paso es realizar un filtrado de la velocidad del Rover durante el ensayo con el objetivo de eliminar el ruido en la medida de los encoderes, una vez filtradas se eliminan los datos de reposo del vehículo ya que estos no aportan información al proceso de auscultación teniendo en cuenta que el origen de tiempo es t=0. A continuación se realiza el filtrado de las señales de aceleración y velocidad angular, a las primeras se les realiza un fitrado paso alto mientras que a las segundas uno paso bajo. A pesar de establecer un periodo de adquisición de datos constante incluso durante el correcto funcionamiento del buffer cada cierto numero de medidas se produce una con un periodo mayor siguiendo tras esta con el periodo establecido, de modo que se interpolan los datos para obtener señales con un periodo de muestreo constante, necesario para poder aplicar la FFT para comparar el contenido en frecuencia de los perfiles estimados y real. Una vez interpolados se calcula la longitud recorreida por el Rover durante el ensayo. Por último se obtienen las señales de la IMU en m/s y rad/s. Una vez tratadas las señales procedentes de la IMU y los decodificadores, podemos predecir la geometría del perfil del terreno recorrido por el Rover usando, al igual que de forma teórica, el filtro de Kalman. En la siguiente sección 5 Resultados se muestran los resultados obtenidos de los dos ensayos. Pruebas Experimentales 50 50 Código 12: Tratado de señales de los ensayos para el uso del filtro de Kalman % Cargado de datos fileID=… fopen('MSCIMU_0p02_5_2m.txt','r'); formatSpec = '%f %f %f %f %f %f'; A = fscanf(fileID,formatSpec); j = 1; for i=1:9:length(A) tv(j) = A(i); va(j) = A(i+1); vb(j) = A(i+2); accy(j) = A(i+3); accz(j) = A(i+5); Gyrox(j) = A(i+7); j = j+1; end % Eliminación de datos incorrectos tv(1:7)=[]; tv(654:end)=[]; va(1:7)=[]; va(654:end)=[]; vb(1:7)=[]; vb(654:end)=[]; accy(1:7)=[]; accy(654:end)=[]; accz(1:7)=[]; accz(654:end)=[]; Gyrox(1:7)=[];Gyrox(654:end)=[]; % Eliminación datos incorrectos de velocidad va(254-7) = (va(253-7)+… va(255-7))*0.5; va(252-7) = (va(251-7)+… va(253-7))*0.5; vb(254-7) = (vb(253-7)+… vb(255-7))*0.5; vb(252-7) = (vb(251-7)+… vb(253-7))*0.5; radio_m = 0.045; Va_m = (va*2.0*pi*radio_m)/60.0; Vb_m = (vb*2.0*pi*radio_m)/60.0; vms = (Va_m + Vb_m)*0.5; % Filtrado de las velocidad [b,a]=butter(9,0.1); Vms = filter(b,a,vms); % Eliminación de velocidades nulas for i=1:length(tv) if Vms(i)< 0.0001 ifirst = i; i = length(tv); end end tfinal = tv(end); tini = tv(ifirst-1); tv(1:ifirst-1)=[]; tv(1:end) = tv(1:end) - tini; Vms(1:ifirst-1)=[]; accy(1:ifirst-1)=[]; accz(1:ifirst-1)=[]; Gyrox(1:ifirst-1)=[]; % Filtrado señales fn = 25; fc = 24.7; [b,a]=butter(4,fc/fn,'high'); accz_filt = filter(b,a,accz); accz = accz_filt; [b,a]=butter(4,fc/fn); Gyrox_filt = filter(b,a,Gyrox); Gyrox = Gyrox_filt; fc = 12; [b,a]=butter(4,fc/fn,'high'); accy_filt = filter(b,a,accy); accy = accy_filt; % Interpolación datos delts = 0.02; tint=0:delts:tv(end); Vms_int= spline(tv,Vms,tint); accz_int= spline(tv,accz,tint); Gyrox_int= spline(tv,Gyrox,tint); accy_int= spline(tv,accy,tint); Vms = Vms_int; accz = accz_int; Gyrox = Gyrox_int; accy = accy_int; % Cálculo longitud recorrida s = zeros(1,length(tv)); s(1) = 0; for i=1:length(tv)-1 s(i+1) = s(i)+ Vms(i)*delts; endsini(1)=0; L=0.16; LL = L + L*0.2; Np_max=10; dsini = LL/(Np_max-1); for i=2:Np_max sini(i) = sini(i-1) + dsini; end s = s +sini(end); % Señales para la estimación hpp = (accz)*9.8; ww = -Gyrox*pi/180; Ams = accy*9.8; 51 4.3 Pérdida de datos La aparición aproximadamente regular de intevalos de tiempo mayores a los establecidos implica que los valores que debían ser medido en esos instantes de tiempo no son medidos y por lo tanto implica una perdida de datos en las mediciones realizadas. Con el fin de estudiar el efecto que esta perdida de datos conlleva a la estimación de los perfiles mediante el filtro de Kalman y determinar la validez del uso de la transformada de Fourier frente a intervalos de tiempo no constantes, se simula en este apartado este efecto usando el algoritmo teórico de la sección 3. Para ello a las señales teóricas calculadas a partir del perfil aleatorio generado se le eliminan datos a una cierta frecuencia que simula esta perdida de datos debida a los incrementos de tiempo mayores para, posteriormente, realizar la estimación del perfil con estas señales modificadas. A continuación se describe la metodología usada para modificar las señales teóricas. Dada una señal teórica cualquiera (aceleración o velocidad angular) con intervalos de tiempo constante como la que se muestra en la Figura 25, donde P1, P2, …, P1 son los datos medidos y t1, t2, …, t1 son los instantes de tiempo en los que se miden dichos puntos y definiendo t1=t2-t1, t2=t3-t2, …, t9=t10-t9 siendo todos estos iguales, cada cierto número de datos se elimina el valor en ese instante para generar un intervalo de tiempo mayor, donde ahora ’t1=t2-t1, ’t2=t3-t2, ’t3=t5-t3, ’t4=t5-t6 ’t5=t8-t6, ’t6=t9-t8 y ’t7=t10-t9, donde ’t3 y ’t5 son mayores que el resto. Debido al uso de Fourier que requiere intervalos de tiempo regulares, por lo que se reconstruye la señal haciendo que de nuevo sus intervalos de tiempo sean todos constantes por lo que ahora, como se observa en la Figura 26, P’1=P1, P’2=P2, P’ =P , P’4=P5, P’5=P6, P’6=P8, P’ =P9 y P’8=P1 Figura 25: Señal teórica Pruebas Experimentales 52 52 Figura 26: Señal con pérdida de datos Para evaluar el efecto que esta perdida de datos tiene sobre la estimación del perfil y el uso de la FFT se realiza la estimación de perfiles aleatorios aplicando esta metodología a todas las señales que intervienen en la estimación mediante el filtro de Kalman (vector de tiempos, de aceleraciones, de velocidades angulares y la señal de los decodificadores). 53 5 RESULTADOS En esta sección se presentan los resultados obtenidos aplicando el algoritmo de auscultación descrito en la sección 3 Estimación de Irregularidades en Perfiles Aleatorios Mediante el Filtro de Kalman para estimar las irregularidades de perfiles verticales. En primer lugar se realiza la estimación de un perfil aleatorio teórico. En segundo lugar se realiza la estimación del perfil real sobre el cual se han realizado los ensayos de auscultación con el Rover. 5.1 Estimación de perfiles teóricos En esta sección se muestra el resultado de la simulación del proceso de auscultación teórica de un vehículo auscultador de las características del Rover que avanza a velocidad constante de 1 m/s sobre un perfil aleatorio, de 300 m de longitud. En color rojo se muestra el resultado de la estimación mientras que en azul se muestran los valores teóricos. En la Figura 27 se muestra la evolución de la coordenada vertical de las irregularidades del perfil en función de la distancia recorrida por el vehículo y el perfil aleatorio representado por la expresión (30) de la sección 3.1.2. En la Figura 28 se observa el contenido en frecuencia de los perfiles mostrados en la Figura 27. Se ha supuesto para la aceleración un bias senoidal con una amplitud del 5% de la aceleración máxima con un periodo de 300 s, en la Figura 29 se observa la estimación del filtro de Kalman. Figura 27: Estimación perfil aleatorio Resultados 54 54 Figura 28: Contenido en frecuencia estimación perfil aleatorio Figura 29: Bias acelerómetro 5.2 Estimación perfil real Los resultados de los dos ensayos de auscultación de la geometría vertical de la pista de baloncesto medida en la Escuela usando el Rover como vehículo auscultador se presentan en este apartado. En azul se muestran los resultados obtenidos en el Ensayo 1 y los obtenidos en el Ensayo 2 en Rojo, mientras que en negro se representan los datos de los valores reales del perfil medido con la estación total. En la Figura 30 se muestran en función del tiempo las aceleraciones verticales y las velocidades angulares obtenidas con el Rover al recorrer el camino medido, en la Figura 31 se muestra el contenido en frecuencia de las aceleraciones mostradas en la Figura 30. Se observa que los valores de aceleraciones son muy elevados también a baja frecuencias para las velocidades a las que avanza el vehículo, por lo que se sospecha que estos valores pueden no ser correctos, esto puede estar debido a una mala calibración del acelerómetro, que el acelerómetro tenga algún fallo o que en realidad el vehículo avance dando pequeños botes que no son apreciables a simple vista debido a que no tiene amortiguación y el suelo es relativamente rugoso. No obtante esto no son más que suposiciones y se está trabajando para solucionar este probema. Por este motivo se ha decicido filtrar las señales procedentes de los sensores con el fin de obtener una mejor concordancia entre los perfiles estimados y el perfil real, realizando un filtro paso alto a las aceleraciones y uno paso bajo a las velocidades angulares. En la Figura 30 también se observa que a partir de un determinado tiempo los incrementos de tiempo en la adquisición de los datos de los sensores aunentan volviéndose irregulares, esto se debe al probema con el buffer que se tuvo durante la realización de los ensayos, que se menciona en la sección 4.2. Debido a la necesidad de que los incrementos de tiempo sean constantes para el uso de Fourier de decidió eliminar dichos datos. También se decidió interpolar las señales obtenidas con el fin de solucionar le problema de los puntuales intervalos de tiempo irregulares que introduce la Mbed durante las mediciones. 55 Figura 30: Aceleración verical y velocidad angular x obtenidas en los ensayos Figura 31: Contenido en frecuencia de las aceleraciones verticales de los ensayos En la Figura 32 se muestran los perfiles de velocidad de ambos ensayos obtenidos como la media de las velocidades de cada rueda del vehículo. En ella se observa un pico de velocidad en el Ensayo 2 de unos 25 m/s. debido a que la velocidad máxima que desarrolla el vehiculo es de 1.65 m/s este pico no es un dato real de velocidad por lo que debe ser eliminado. Para ello se aproxima la velocidad en ese instante como la media del instante anterior y posterior. Resultados 56 56 Figura 32: Perfiles de velocidad de los ensayos En la Figura 33 se representan en función de la distancia recorrida por el Rover tanto la evolución de la coordenada z de las irregularidades de los perfiles estimados con el filtro de Kalman como el perfil real sobre el que se realizaron los ensayos, se observa que el perfil estimado en el Ensayo 1 no termina de recorrer el terremo marcado por el ensayo, esto es debido que no se alcanzó la velocidad necesaria para recorrer toda la longitud del terreno antes de que se llenara el buffer. Notese que aunque se ha realizado la estimación de la irregularidades del terreno en toda la longitud recorrida por el vehículo auscultador, el perfil conocido se encuentra entre los metros dos y siete de modo que tanto los dos primeros metros de los ensayos como el tramo final del Ensayo 2 no pueden ser comparados con los valores reales. Por este motivo se encuentra ampliada la zona conocida del perfil real. En la Figura 34 se muestra el contenido en frecuencia de los perfiles mostrados en la Figura 33. Figura 33: Estimación perfiles reales 57 Figura 34: Contenido en frecuencia de los perfiles Con el objetivo de mostrar una mejor representación de las irregularidades de los perfiles obtenidos en los ensayos se ha aplicado un filtro paso bajo a los perfiles de ambos ensayos para eliminar las altas frecuencias obteniendo un perfil más suave como el que se representa en la Figura 35. Figura 35: Estimación perfiles filtrados 5.3 Pérdida de datos Con el objetivo de estudiar el efecto de la pérdida de datos se han realizado diversas simulaciones realizando la estimación de perfiles aleatorios a diferentes frecuencias de muestreo 100Hz y 1000 Hz. Con cada uno de estos perfiles aleatorios se realiza la estimación del perfil variando tanto la frecuencia a la que se pierde un solo dato como variando el número de datos que se pierden a una misma frecuencia de pérdida de datos. Usando una frecuencia de muestreo de 100Hz, en la Figura 36 se muestra en negro la evolución del perfil teórico y en magenta y verde la evolución del perfil con una pérdida de un dato cada 1000 y 500 puntos, también se muestra su contenido en frecuencia que ha sido amplizado para mayor detalle. En la Figura 37 se muestra la misma información con una pérdida de dos y cuatro datos cara 1000 puntos. En las Figura 38 y Figura 39 se representan los mismo resultado usando en este caso una frecuencia de muestreo de 1000Hz. Referencias 64 64 65