Integración de sensores doppler para el cálculo de odometría en vehículos aéreos

i Equation Chapter 1 Section 1 Trabajo Fin de Grado Grado en Ingeniería Aeroespacial Integración de sensores doppler para el cálculo de odometría en vehículos aéreos Autor: Juan Carlos Ortiz Ronda Tutores: Jesús Capitán Fernández Fernando Caballero Benítez Dep. de Ingeniería de Sistemas y Automática Escuela Técnica Superior de Ingeniería Universidad de Sevilla Sevilla, 2016 ii iii Trabajo Fin de Grado Grado en Ingeniería Aeroespacial Integración de sensores doppler para el cálculo de odometría en vehículos aéreos Autor: Juan Carlos Ortiz Ronda Tutores: Jesús Capitán Fernández Profesor Ayudante Doctor Fernando Caballero Benítez Profesor Contratado Doctor Dep. de Ingeniería de Sistemas y Automática Escuela Técnica Superior de Ingeniería Universidad de Sevilla Sevilla, 2016 iv v Trabajo Fin de Grado: Integración de sensores doppler para el cálculo de odometría en vehículos aéreos Autor: Juan Carlos Ortiz Ronda Tutores: Jesús Capitán Fernández Fernando Caballero Benítez El tribunal nombrado para juzgar el Proyecto arriba indicado, compuesto por los siguientes miembros: Presidente: Vocales: Secretario: Acuerdan otorgarle la calificación de: Sevilla, 2016 El Secretario del Tribunal vi vii A mi familia A mis maestros viii ix Agradecimientos En primer lugar me gustaría agradecer a mi tutor, Jesús, por haberme dado la oportunidad de trabajar en este proyecto, como también a Fernando, así como a todos los maestros y profesores que me han ido formando desde que era pequeño hasta el día de hoy. En segundo lugar agradecer a mis padres por apoyarme en todo momento, aconsejarme lo mejor posible y depositar una enorme confianza en mí, brindándome la oportunidad de estudiar lo que me gusta. También agradecer a mi hermana, que seguro estará orgullosa de su hermano. A mi pareja por su paciencia y cariño, animándome y apoyándome cada vez que lo he necesitado. Y por último, a aquellas personas que he conocido a lo largo de estos años de carrera, con quien he compartido buenos y malos momentos. Todos formáis parte de este trabajo y sin vosotros no hubiera sido posible, muchas gracias. Juan Carlos Ortiz Ronda Sevilla, 2016 xvi ÍNDICE DE FIGURAS Figura 1-1: Mapa realizado con tecnología LIDAR 1 Figura 2-1: Observación del efecto Doppler en la sirena de una ambulancia 3 Figura 2-2: Haz del radar Doppler 4 Figura 2-3: Radar primario (izquierda) y radar secundario (derecha). 5 Figura 2-4: Transmisión de onda continua a una cierta frecuencia 5 Figura 2-5: Transmisión de un radar de impulsos coherente 6 Figura 2-6: Transmisión y recepción de radar CW-FM 7 Figura 2-7: Transmisión y recepción en radar CW-FM con desviación Doppler 7 Figura 4-1: Sensor X-Band Motion de Parallax 9 Figura 4-2: Patrón de radiación horizontal y vertical de la antena PCB 10 Figura 4-3: Diagrama de bloques de las placas del sensor Doppler 10 Figura 4-4: Espectro típico de una señal Doppler 11 Figura 4-5: Ángulo 𝜽 dependiendo de la dirección de la propagación de la señal y el movimiento 11 Figura 4-6: Arduino Leonardo 12 Figura 4-7: Parte superior del montaje 12 Figura 4-8: Parte inferior del montaje 13 Figura 4-9: Interfaz del IDE de Arduino 13 Figura 4-10: Interfaz de RealTerm con visualización del puerto serie 14 Figura 5-1: Tren de pulsos en la salida del sensor Doppler 15 Figura 5-2: Diagrama de flujo del funcionamiento del programa 15 Figura 5-3: Diferencia entre FreqCount y FreqMeasure 19 Figura 5-4: Parte frontal de la placa de desarrollo Teensy 3.2 22 Figura 5-5: Parte trasera de la placa de desarrollo Teensy 3.2 22 Figura 6-1: Triángulo de velocidades 24 Figura 6-2: Ejes cuerpo de un vehículo aéreo 24 Figura 6-3: Descomposición de velocidad 24 Figura 6-4: Configuración de Doppler con dos haces 25 Figura 6-5: Tipos de formación Janus 25 Figura 6-6: Configuración JANUS de tres haces, la más óptima 26 Figura 6-7: Orientación de la aeronave 28 Figura 7-1: Representación de velocidades mediante FreqCounter 29 Figura 7-2: Representación velocidades mediante FreqCounter con reducción de muestras 30 Figura 7-3: Representación del error con el método FreqCounter 31 Figura 7-4: Representación de velocidades mediante FreqCount 32 xvii Figura 7-5: Representación velocidades mediante FreqCount con reducción de muestras 32 Figura 7-6: Representación del error con el método FreqCount 33 Figura 7-7: Representación de velocidades mediante FreqMeasure 34 Figura 7-8: Representación velocidades mediante FreqMeasure con reducción de muestras 34 Figura 7-9: Representación del error con el método FreqMeasure 35 Figura 7-10: Representación de velocidades mediante PulseIn 36 Figura 9-1: Diagrama de flujo de programa básico en Arduino 38 xviii 1 1 INTRODUCCIÓN oy en día existen diferentes métodos de navegación para los vehículos aéreos, de los cuales los más usados son el GPS y la unidad de medidas inerciales. Para estos dos, se suele realizar una fusión mediante un filtro, ya sea de Kalman u otro, de forma que la posición sea estimada de una manera más precisa. Sin embargo, cuando en un pequeño vehículo se desea estimar la posición en el interior de edificios, la navegación por GPS es imposible debido a la gran pérdida de potencia de la señal GPS cuando atraviesa paredes, suelos y otros objetos. Es posible a partir de un punto conocido utilizar los acelerómetros y giróscopos que incluye la IMU para estimar la posición, pero esto no es favorable ya que comprende la doble integración de las medidas de aceleración con el consiguiente aumento del error, cuyo valor crece conforme sea mayor el tiempo que se realice la integración. Es debido a que se suele aunar junto con otro sistema, de los cuales hablaremos a continuación. Se denominan IPS (Indoor Positioning System), y son sistemas de posicionamiento para interior de edificios que permiten localizar un objeto mediante tecnología óptica, de radiofrecuencia, e incluso ultrasónica. Estos pueden ser integrados junto con la IMU para obtener una mayor precisión. [1] Cabe destacar, que dentro de los sistemas de navegación, estos pueden ser autónomos o no autómos, siendo los autónomos aquellos que no dependen de ningún sensor exterior que mande información hacia el móvil para poder posicionarse y los no autónomos aquellos que no necesitan de ningún sensor externo ya que les es suficiente con los equipados a bordo. De entre los sistemas no autónomos podemos encontrar el posicionamiento por Wi-Fi que necesita de diferentes puntos de acceso para medir la intensidad de la señal recibida y localizar el objeto mediante triangulación. Otro de ellos es la navegación por visión, la cuál necesita de unas referencias o marcadores visuales, fundamento en el que se basa la cámara de “Motion-tracking” que utilizaremos para nuestras pruebas experimentales. En cuanto a los sistemas autónomos encontramos la navegación mediante campo magnético. Sin embargo esta necesita de un mapa realizado previamente del campo magnético del área y es muy susceptible a interferencias. Actualmente se están desarrollando diversas técnicas distintas de las anteriores comentadas utlizando tecnología basada en láser. Es el caso de la tecnología LIDAR, que permite determinar la distancia desde un emisor láser a un objeto o superficie utilizando un haz láser pulsado. Esta distancia al objeto se determina midiendo el tiempo de retraso entre la emisión del pulso y su detección a través de la señal reflejada. Es así como es posible crear mapas del entorno, como por ejemplo, mapas de interiores de edificios. Si se realizan varias mediciones de distancia en un intervalo de tiempo, es posible calcular la velocidad de movimiento del vehículo, que es el objetivo final del estudio. Sin embargo, su alta precisión se traduce en un alto coste. Figura 1-1: Mapa realizado con tecnología LIDAR H 2 Es por ello, que se quiere buscar otro método que permita el cálculo de la odometría, es decir, la posición del vehículo, de modo que sea más barato y menos complejo de implementar que la tecnología LIDAR. Esto es posible a través del uso del efecto Doppler mediante un sensor que permita calcular la velocidad del móvil. En esto es en lo que se fundamenta el presente trabajo y es lo que abordaremos en los siguientes capítulos. Por tanto, si se cumple la posibilidad de utilizar el sensor Doppler para la navegación interior, será útil su implementación en UAV’s de bajo coste, permitiendo abaratar su construcción por parte de las empresas obteniendo así de un sistema de navegación autónomo cuya precisión puede mejorarse mediante su unión junto con otros sensores mediante un filtro de Kalman. Una vez obtenida la velocidad, si esta es bastante precisa es posible utilizar la integración para obtener el desplazamiento del móvil y permitir así el posicionamiento en interiores. 3 2 FUNDAMENTOS DEL EFECTO DOPPLER a forma por la que se va a calcular la velocidad es mediante la aplicación del efecto Doppler. Existen distintas maneras de utilizar esta técnica dependiendo del modo en el que el emisor transmita la onda. Un sensor Doppler es un tipo de radar, por tanto, a continuación se explicará el fundamento del mismo y los distintos tipos de radar que pueden hallarse. 2.1. Efecto Doppler En primer lugar, ya que el trabajo se basa en la técnica Doppler, explicaremos los fundamentos de la misma. [2] El efecto Doppler es un fenómeno producido durante la observación física de las ondas. Se produce cuando la fuente emisora de ondas y el observador se encuentran en movimiento relativo con respecto al medio material en el que se propaga; la frecuencia de las ondas observadas es distinta a la frecuencia de las ondas emitidas por la fuente. Esta variación es lo que produce el llamada efecto Doppler y la desviación en la frecuencia suele llamarse frecuencia Doppler. Esta relación entre la frecuencia observada y la frecuencia emitida viene dada por la siguiente ecuación: 𝑓=(𝑐+𝑣𝑟 𝑐+𝑣𝑠)𝑓0 (2-1) Siendo: - 𝑓 la frecuencia emitida. - 𝑓0 la frecuencia observada. - 𝑐 la velocidad de la luz: 3·108 𝑚/𝑠. - 𝑣𝑟 es la velocidad del receptor en relación con el medio, positiva si el receptor se mueve hacia el emisor. - 𝑣𝑠 es la velocidad de la fuente con respecto al medio, positiva si la fuente se aleja del receptor. Este argumento es aplicable a cualquier tipo de ondas, ya sean ondas magnéticas, sonoras… Por ejemplo, para el caso de las ondas sonoras, podemos poner como ejemplo la sirena de una ambulancia [3] que pasa por delante de nosotros. Si la ambulancia se acerca a cierta velocidad, la escucharemos cada vez más con un tono más agudo que el real, debido a que la frecuencia va aumentando. En cambio, si se aleja de nosotros, la escucharemos más grave y por tanto la frecuencia de las ondas sonoras disminuye, tal y como se muestra en la siguiente imagen. Figura 2-1: Observación del efecto Doppler en la sirena de una ambulancia L 4 En este caso, como el receptor y el emisor son los mismos ya que se encuentran ambos en el vehículo, y además, como la señal transmitida hace el mismo camino que la recibida por ser un radar primario, término que se explicará en el siguiente subapartado, 𝑣𝑟= −𝑣𝑠. La ecuación queda entonces de la siguiente forma. 𝑓=(1+𝑣𝑟 𝑐 1−𝑣𝑟 𝑐)𝑓0 (2-2) ∆𝑓= 𝑓− 𝑓0= (1+𝑣𝑟 𝑐 1−𝑣𝑟 𝑐)𝑓0−𝑓0= 𝑓0(1+𝑣𝑟 𝑐 1−𝑣𝑟 𝑐−1) (2-3) ∆𝑓=2𝑣𝑟 𝑐 1−𝑣𝑟 𝑐𝑓0 (2-4) Siendo ∆𝑓=𝑓−𝑓0, la desviación de frecuencia o frecuencia Doppler. Como 𝑣𝑟 𝑐≪1: ∆𝑓=2𝑣𝑟 𝑐𝑓0 (2-5) Ahora bien, si el movimiento no es colineal con la trayectoria de las ondas del sensor, la velocidad varía en función del ángulo que forman las ondas con el movimiento del vehículo. Figura 2-2: Haz del radar Doppler 𝑣𝑟=𝑣·cos𝛾 (2-6) De esta forma, la ecuación quedaría: ∆𝑓=2𝑣𝑐𝑓0cos𝛾 (2-7) Por otro lado, como 𝑐𝑓 es igual a la longitud de onda λ, esta última ecuación se podrá escribir como: ∆𝑓=2𝑣 𝜆cos𝛾 (2-8) Donde λ es la longitud de onda de la señal transmitida. 5 Resumiendo, cuando montemos el sensor en el robot, este transmitirá una señal en una frecuencia. Estas ondas rebotarán en las paredes y volverán al sensor, que recibirá la señal. Si el robot se encuentra parado, la frecuencia transmitida y recibida serán la misma por lo que no se detecta movimiento y la velocidad es nula. No obstante, si el robot se encuentra en movimiento, la frecuencia recibida será de unos herzios mayor o menor de la transmitida, y por tanto se detecta que se ha movido y es posible calcular la velocidad. A continuación, se explicará más en detalle en qué consiste el radar [4] y los diferentes tipos que existen, donde incluiremos nuestro sensor. 2.1 Tipos de Radar En primer lugar, hay que diferenciar entre los radares primarios y secundarios. Un radar primario es aquel que funciona con independencia del blanco, en el que su funcionamiento se basa en el eco de la señal. En cambio, un radar secundario es aquel que manda una señal al blanco, lo interroga y este responde con otra señal. Figura 2-3: Radar primario (izquierda) y radar secundario (derecha). Se pueden encontrar distintos tipos de radar dependiendo de cómo se transmita la onda. En función de cómo sea, este permite calcular la velocidad y/o la distancia. Tras el siguiente párrafo se explicará detalladamente como es cada uno de ellos. Otro de los conceptos importantes es el RCS (Radar Cross Section) o Corte Transversal de Radar e indica en un blanco cuál es el área efectiva de éste que permite reflejar parte de la potencia de la señal incidente hacia el transmisor. Es una medida que permite saber cuán de detectable es un objeto mediante radar. Por tanto, un RCS mayor indica que el objeto es más fácil de detectar. En el caso del sensor Doppler, se necesita que se tenga un RCS alto a la hora de reflejar la señal. Como este estará orientado hacia el suelo, no habrá problema en cumplir este requisito. 2.2.1. Radar de Onda Continua (CW) Este es el más simple de todos y el más barato de implementar ya que el Tx, es decir, el transmisor, envía continuamente y el Rx, el receptor, recibe continuamente. Una forma de onda continua pura solamente sirve para medir velocidad radial por efecto Doppler, por lo que tiene sus limitaciones a la hora de medir distancia. Para poder medirlas con un radar CW es necesario modularlo, por ejemplo, mediante modulación FM lineal con la que pueda realizarse una marca de pulso. Aunque de esto se hablará en el radar CW-FM que explicaremos en breves. Figura 2-4: Transmisión de onda continua a una cierta frecuencia 6 De este tipo es nuestro sensor Doppler, es un radar de onda continua que transmite una señal a una frecuencia especifica sin interrupciones a la vez que recibe el eco. 2.2.2. Radar de Impulsos Este tipo de radar emite un tren periódico de pulsos y detecta los ecos reflejados por los blancos. El tiempo transcurrido entre la emision y la recepción de un pulso resuelve la distancia al blanco según la formula: 𝑅=𝑐𝑇 2 (2-9) Siendo c la velocidad de la luz y T el tiempo transcurrido entre emisión y recepción. Si la emisión y la recepción son coherentes, es decir, conservan la fase de portadora, es posible determinar la frecuencia Doppler y medir la velocidad relativa del blanco. Para ello aplican el efecto Doppler tal y como lo conocemos, aplicando la formula 2-7. Figura 2-5: Transmisión de un radar de impulsos coherente 2.2.3. Radar CW-FM Este es un tipo especial de radar que envía una señal continua como el radar CW. Sin embargo, a diferencia de este ultimo, esta se envía con modulación lineal en frecuencia durante un intervalo 𝑇𝑚. Los ecos se reciben de modo continuo, sin interrumpir la transmisión. La señal recibida se mezcla en el receptor con la señal transmitida y el resultado pasa por un filtro paso bajo para producir una superposición de frecuencias 𝑓𝑏= 𝑓𝑇𝑋−𝑓𝑅𝑋, denominado frecuencia de batido. A partir de esta, es posible considerando el resto de características de la señal como parámetros, calcular la distancia al blanco, aplicando la siguiente formula: 𝑅= 𝑐𝑇𝑚 2𝑓𝑏 ∆𝑓 (2-10) Un radar FMCW es la forma más simple de radar de impulsos doppler. El “impulso” en este caso va desde el inicio de la modulación hasta su final, es decir, pulsos de Tm, sin separación entre ellos. 7 Figura 2-6: Transmisión y recepción de radar CW-FM Como 𝑇𝑚 es el tiempo entre pulsos triangulares, la máxima distancia a un blanco que un radar FM-CW puede medir sin ambigüedades es: 𝑅𝑚á𝑥=𝑐𝑇𝑚 2 (2-11) Es decir, la máxima distancia es aquella que permite a la señal la ida y vuelta a la velocidad de la luz antes de que se comience a transmitir el siguiente pulso modulado. Si el blanco se encuentra en movimiento, la forma de onda detectada tendrá un desplazamiento Doppler en frecuencia. A partir de esto, sería posible calcular la velocidad. Figura 2-7: Transmisión y recepción en radar CW-FM con desviación Doppler Por tanto, con un radar de onda continua modulado en FM es posible detectar tanto la velocidad como la distancia al blanco. 14 Por otro lado, para la visualización de datos se ha considerado mejor el software denominado RealTerm, ya que permite leer el puerto serie del Arduino así como poder capturar los datos para posteriormente guardarlos en un archivo de texto y procesarlos en MatLab. Figura 4-10: Interfaz de RealTerm con visualización del puerto serie 15 5 ESTUDIO DEL PROBLEMA vez presentados los componentes de los que se dispone, es necesario realizar la programación del microcontrolador para que éste permita obtener la frecuencia del pin de salida del sensor Doppler, y a partir de ahí calcular la velocidad. Como se ha comentado anteriormente, nuestro sensor muestra por el pin de salida un tren de pulsos con frecuencia variable que oscila con valores HIGH y LOW. Por tanto, tendremos que desarrollar un código que permita observar esa oscilación y mostrarnos el número de veces que cambia en un intervalo de tiempo prefijado. Para ello hemos investigado y hemos dado con diferentes librerías y funciones que son aplicables a nuestro problema. Figura 5-1: Tren de pulsos en la salida del sensor Doppler Antes de explicar en qué consiste el código, se recomienda ver en el Anexo A el funcionamiento y la estructura de un programa en Arduino con el objetivo de entender mejor lo que viene a priori. [Ver Anexo A] Éste está compuesto principalmente de cuatro bloques, de los cuales sólo uno de ellos cambia dependiendo de la librería o función utilizada para el cálculo de la frecuencia. Se presenta un diagrama de flujo que puede aclarar las ideas sobre este apartado, el cual quiere explicar que una vez se inicia la comunicación con el Arduino, este espera a que se detecte movimiento para continuar con el programa. Figura 5-2: Diagrama de flujo del funcionamiento del programa U 16 El funcionamiento de cada bloque lo detallaremos a continuación: 1) Bloque de Inicio: Se inicia la comunicación con el Arduino a una tasa de 115200 baudios. Además, se establecen que pines son los de entrada y salida de datos al sensor. También se crean las variables que van a ser utilizadas en el programa. Un ejemplo de este bloque sería el siguiente: 2) Bloque de Espera: Se inicia el sensor escribiendo en el pin EN a HIGH. Se espera a que detecte movimiento para ponerse a funcionar y detectar la frecuencia. Se hace mediante un bucle while que no deja avanzar hasta que el sensor no modifique el pin de salida. Para más inri, se muestra un ejemplo a continuación. 3) Bloque de Cálculo: Este bloque es diferente para cada uno de los códigos. Se calcula la frecuencia del pulso y se guarda en una variable. Se detallará su funcionamiento en los apartados posteriores. 4) Bloque de Velocidad: Se calcula la velocidad aplicando la siguiente fórmula, que resulta de despejar la velocidad de la ecuación 4-1. 𝑉= 𝐹𝑑·𝑐 2·𝐹𝑡·cos𝜃 (5-1) #include <FreqMeasure.h> const int PinEN = 12; const int PinOUT = 13; int estado_actual = 0; double sum = 0; int count = 0; float frecuencia; float tiempo; unsigned long tiempo0 = 0; void setup() { Serial.begin(115200); pinMode(PinOUT, INPUT); pinMode(PinEN, OUTPUT); enable(); FreqMeasure.begin(); } void wait() { //Espera a que el sensor devuelva un valor de 1 while (digitalRead(PinOUT) != 1) { } estado_actual = 1; //Ahora el sensor se encuentra activado y saca un tren de pulsos //Serial.println("Movimiento detectado!"); 17 Como se ha comentado en apartados anteriores, en nuestro caso el valor de 𝜃 será nulo ya que la dirección de movimiento del sensor será colineal con la dirección de la normal a la superficie de la antena. Por otro lado, existe ambigüedad, ya que si 𝜃=90 la ecuación no podría resolverse. Aún así, este ángulo debe procurarse que sea lo más agudo y pequeño posible para mejorar la precisión. Finalmente se muestran los valores de velocidad tanto en m/s como en km/h por el puerto serie del Arduino, además del instante de tiempo en el que se han capturado. A continuación comentamos cada uno de las librerías o funciones utilizadas, que son cuatro. Por tanto, tenemos cuatro formas de calcular la frecuencia, de las cuales elegiremos la mejor de ellas tras las pruebas de ensayo. 5.1 FreqCounter En este caso se ha tomado una de las librerías realizadas por Martin Nawrath [7] diseñada para medir frecuencias con una resolución alta y precisa. Esta librería hace uso del TIMER1 del microcontrolador del Arduino para provocar una interrupción y poder contar cuántos pulsos se producen en un intervalo prefijado denominado “gatetime”. En nuestro caso hemos fijado este valor para 500 milisegundos. Además de ello, contiene un factor de compensación que permite corregir errores en el gatetime. Sin embargo, no hemos tocado este valor y lo hemos dejado por defecto ya que para calibrarlo es necesario una comparación con un contador profesional que no disponemos. Cabe decir que esta librería en un principio no era posible utilizarla para nuestro microcontrolador. Es por ello que se ha tenido que modificar para que fuese funcional con el Arduino Leonardo. La desvestaja de utilización de ésta reside en que afecta a las salidas PWM de los micros ATMega, incluido en la tarjeta microcontroladora. Las funciones de está libreria las comentamos en las siguientes lineas:  FreqCounter::f_comp=8  Este es el valor de calibración para el gatetime que no hemos tocado y lo hemos dejado por defecto.  FreqCounter::start(gatetime)  Comienza a contar la frecuencia para cada interval del gatetime.  FreqCounter::f_ready  Devuelve “true” cuando una nueva medida está disponible.  FreqCounter::f_freq  Devuelve el valor de flancos de subida vistos en un intervalo del gatetime. void print_speed() { Serial.print(" "); Serial.print(micros()-tiempo0); Serial.print(" "); Serial.print(frecuencia); Serial.print(" "); float ratio = 299792458.0 / (2 * 10525000000); //Razón calculada por c/(2*freq_emitida) Serial.print(frecuencia * ratio); Serial.print(" "); Serial.println(frecuencia * ratio / 3.6); } 18 Un ejemplo de programa donde se utilizan estas funciones sería el que se presenta a continuación: 5.2 FreqCount Esta librería según su autor [8] es muy similar a la anterior pero contiene mejoras pues no necesita de un factor de compensación para el gatetime. Funciona de igual forma, provocando una interrupción cada intervalo de tiempo prefijado que en nuestro caso será de 500 milisegundos. Recomienda utilizarse para lectura de frecuencias comprendidas entre 1 kHz a 8 Mhz. No es nuestro caso pues los valores rondan en un interval de [0, 50] Hz. Aún así se ha querido probar esta librería para comprobar su funcionamiento. Al estar basada en la librería FreqCounter, la funcionalidad PWM queda inutilizada mientras está iniciado el contador de frecuencia. Las funciones que esta incluye son las siguientes:  FreqCount.Begin(gatetime)  Comienza a contar la frecuencia cada intervalo del gatetime, expresado en milisegundos.  FreqCount.available()  Devuelve “true” cuando una nueva medida está disponible. Sólo una medida será guardada en caché por lo que debe ser leida antes del siguiente intervalo del gatetime.  FreqCount.read()  Devuelve la medida más reciente, el número de flancos ascendentes que se han visto dentro de un intervalo del gatetime.  FreqCount.end()  Detiene el contador de frecuencia. Ahora sí es posible utilizar la funcionalidad PWM. Se muestra un código que implementa esta librería como ejemplo: #include <FreqCounter.h> void setup() { Serial.begin(112500); } long int freq; Void loop() { FreqCounter::f_comp= 8; FreqCounter::start(100); while (FreqCounter::f_ready == 0) freq=FreqCounter::f_freq; } #include <FreqCount.h> void setup() { Serial.begin(112500); FreqCount.begin(1000); } void loop() { if (FreqCount.available()) { unsigned long count = FreqCount.read(); } } 19 5.3 FreqMeasure Es del mismo autor que freqcount [9]. Su uso está recomendado para frecuencias comprendidas entre 0.1 Hz y 1 kHz, que es nuestro caso por lo que se espera que este sea el método de los más precisos para cumplir nuestro objetivo. La diferencia con la anterior se establece en que en vez de contar el número de pulsos que ocurren en un intervalo, esta librería calcula cuál es el tiempo que transcurre en un solo ciclo. Esta diferencia podemos observarla en la siguiente figura. Figura 5-3: Diferencia entre FreqCount y FreqMeasure Tiene una desventaja y es que el costo computacional aumenta conforme la frecuencia es mayor. Sin embargo, como en nuestro caso manejaremos frecuencias bajas, esta desventaja no nos concierne. Por otro lado, no es capaz de leer frecuencia nula por lo que se ha implementado un “timeout” que muestre frecuencia cero si no se ha detectado ninguna en un intervalo establecido. Al igual que las funciones anteriores, la funcionalidad PWM es inservible mientras está disponible el contador de frecuencia. Las funciones que ésta incluye se describen a continuación:  FreqMeasure.begin()  Comienza el contador de frecuencia.  FreqMeasure.available()  Devuelve el número de medidas disponibles para leer, o 0 si no hay ninguna sin leer.  FreqMeasure.read()  Lee una medida, el número de ciclos de reloj de la CPU que han transcurrido durante un ciclo del tren de pulsos. Cada medida comienza inmediatamente después de la anterior, sin retardo ninguno, por lo que varias medidas pueden promediarse para una mejor resolución.  FreqMeasure.countToFrequency(count)  Convierte la medida de la función anterior a la frecuencia actual.  FreqMeasure.end()  Termina el contador de frecuencia. Ahora sí es posible utilizar la funcionalidad PWM. 20 Como ejemplo de implementación de esta librería tenemos el siguiente programa: Como vemos, y aprovechando que la función FreqMeasure.read() no tiene latencia al leer las medidas, se hace una media durante 30 ciclos del pulso, y luego ya se decide guardar el valor en la variable en freq. 5.4 PulseIn Para este script no hemos utilizado ninguna librería, sino que hemos usado una función incluida en Arduino que permite leer el pulso en un pin prefijado. Cuando el pin de entrada de datos procedentes del sensor está en HIGH, se comienza a contar y termina cuando el mismo pin vuelve al estado LOW. El resultado es la longitud del pulso en microsegundos o cero si el pulso no se ha completado dentro del “timeout” fijado. Una vez obtenido este tiempo, como tenemos el duty cycle del pulso, es decir, el porcentaje que el pulso toma valor unidad en un ciclo entero, tan sólo es necesario aplicar la siguiente ecuación para obtener el tiempo total del ciclo: 𝑇𝑐𝑖𝑐𝑙𝑜=𝑇𝑝𝑢𝑙𝑠𝑜 𝐷𝑐𝑦𝑐𝑙𝑒∗106 (5-2) Donde: - 𝑇𝑐𝑖𝑐𝑙𝑜 es el tiempo en el que transcurre el ciclo entero. - 𝑇𝑝𝑢𝑙𝑠𝑜 es el tiempo en el que se produce el pulso, es decir, la entrada se encuentra a valor 1. - 𝐷𝑐𝑦𝑐𝑙𝑒 es el valor del duty cycle. En nuestro caso es 0.04 ya que es de un 4%. Tras tener el tiempo del ciclo ya sólo es necesario realizar la inversa de este para conseguir la frecuencia, lo que ya se podría calcular la velocidad. #include <FreqMeasure.h> double sum=0; int cont=0; void setup() { Serial.begin(112500); FreqMeasure.begin(); } void loop() { if (FreqMeasure.available()) { sum = sum + FreqMeasure.read(); cont = cont + 1; if (cont > 30) { float freq FreqMeasure.countToFrequency(sum / cont); sum = 0; cont = 0; } } } 21 𝑓= 1 𝑇𝑐𝑖𝑐𝑙𝑜 (5-3) Un ejemplo donde se aplica esta función es el siguiente: Cabe mencionar que FreqCount y FreqCounter pueden calcular sólo la frecuencia en números enteros. En cambio, FreqMeasure y PulseIn son capaces de calcular las frecuencias con una precisión de centésimas. 5.5 Incorporación de varios sensores Se ha decidido separar este capítulo pues se considera importante la posibilidad de incluir varios sensores en un mismo Arduino, ya que para el objetivo propuesto, al fin y al cabo, lo que se debe es integrar varios sobre un UAV. Sin embargo, al hacer uso del TIMER del microcontrolador, y tal y cómo está desarrollada las librerías, sólo es posible calcular la frecuencia para un sensor conectado a un PIN. Por lo que si se quiere hacer uso de varios sensores, habría que utilizar un Arduino para cada sensor. Quizás esto se vea un poco ilógico ya que para su montaje en un UAV el peso es muy influyente, y la inclusión de como mínimo tres tarjetas microcontroladores va a suponer un aumento de peso considerable. Por tanto, se ha dado con una librería de uno de los mismos autores, denominada FreqMeasureMulti [10] que utiliza el mismo principio que la librería FreqMeasure, es decir, cuenta el número de ciclos de reloj que transcurren en un ciclo del tren de pulsos. Ahora bien, mediante esta librería es posible medir hasta ocho frecuencias simultáneamente. Pero ocurre un problema, y es que no es válida para nuestro Arduino, sino para Teensy. Teensy es una plataforma de desarrollo de bajo coste, sobre unos ~25€, basada en un procesador ARM CortexM4 de 32 bits. Es muy parecida a Arduino ya que es posible programarla con el lenguaje Arduino y C, y además de ser muy potente, es compatible con la mayoría de las librerías de este último. Por otro lado, tiene la ventaja de que tiene un menor peso (3 gramos respecto 20 del Arduino), por lo que es favorable para su montaje en vehículos aéreos. Existen varios modelos, Teensy 2.0, Teensy++2.0, Teensy LC y Teensy 3.2, donde este último es la versión más actual y más potente del dispositivo. Se muestra a continuación: float dur; float tiempo; float frecuencia; void setup() { Serial.begin(115200); pinMode(PinOUT, INPUT); } void loop() { dur = pulseIn(PinOUT, HIGH, 6000000); //Duracion de la activación de un pulso if (dur != 0) { tiempo = dur / (0.04 * 1000000); //D.Cycle=4% frecuencia = 1 / tiempo; } } 22 Figura 5-4: Parte frontal de la placa de desarrollo Teensy 3.2 Figura 5-5: Parte trasera de la placa de desarrollo Teensy 3.2 5.5.1 Funcionamiento de FreqMeasureMulti A continuación se explicará el funcionamiento de esta librería para facilitar la posible integración en trabajos futuros. FreqMeasureMulti utiliza las mismas funciones que FreqMeasure, excepto begin() ya que se debe especificar en la misma el PIN en el que se desea leer la frecuencia. Con la lectura de un ejemplo de código para esta librería en el que se calculan tres frecuencias procedentes de tres pines distintos y después de haber entendido el funcionamiento de FreqMeasure se entiende fácilmente la librería. Para ello se muestra el siguiente. #include <FreqMeasureMulti.h> FreqMeasureMulti freq1; FreqMeasureMulti freq2; FreqMeasureMulti freq3; void setup() { Serial.begin(115200); freq1.begin(6); freq2.begin(9); freq3.begin(10); } float sum1=0, sum2=0, sum3=0; int cont1=0, cont2=0, cont3=0; elapsedMillis timeout; 23 void loop() { if (freq1.available()) { sum1 = sum1 + freq1.read(); cont1 = cont1 + 1; } if (freq2.available()) { sum2 = sum2 + freq2.read(); cont2 = cont2 + 1; } if (freq3.available()) { sum3 = sum3 + freq3.read(); cont3 = cont3 + 1; } //Guarda los resultados en una variable cada 500 milisegundos if (timeout > 500) { if (cont1 > 0) { frecuencia_1 = freq1.contToFrequency(sum1 / cont1); } else { frecuencia_1 = 0; } if (cont2 > 0) { frecuencia_2 = freq2.contToFrequency(sum2 / cont2); } else { frecuencia_2 = 0; } if (cont3 > 0) { frecuencia_3 = freq3.contToFrequency(sum3 / cont3); } else { frecuencia_3 = 0; } sum1 = 0; sum2 = 0; sum3 = 0; cont1 = 0; cont2 = 0; cont3 = 0; timeout = 0; } } 30 Como podemos observar el sensor Doppler sigue bastante los cambios de velocidad, sin embargo, no permite dar valores precisos del valor en sí. Cabe mencionar que durante alrededor de los primeros 40 segundos se hicieron bastantes giros y es por ello que las velocidades no coinciden demasiado. Se han tomado unas 2000 muestras mediante la cámara de seguimiento y unas 212 con el sensor Doppler en el mismo tiempo de ensayo. Por lo que los valores más precisos contienen unas 10 veces más muestras que los tomados con el dispositivo disponible. Por tanto, para poder calcular el error con mayor precisión, se han modificado los datos obtenidos de forma que sea posible reducir el número más elevado de ellos al menor número. Como los datos están sacados en tiempos diferentes en ambas formas, se ha tomado el tiempo de referencia de la adquisición del sensor Doppler. De esta forma, si los datos se recogen por la cámara de seguimiento cada décima de segundo y el sensor lo hace cada segundo, se ha realizado una media de los valores obtenidos por la cámara antes de llegar al tiempo de referencia del Doppler, es decir, la media de los valores que adquiere la cámara en cada décima, y se ha hecho que ese valor medio sea la velocidad recogida por el seguidor de movimiento en la unidad de segundo. De este modo, la representación de las velocidades quedaría de la siguiente forma: Figura 7-2: Representación velocidades mediante FreqCounter con reducción de muestras Como observamos, se han reducido el número de muestras de los datos recogidos por el sensor. Aunque se falsean algo los datos, es una manera eficaz para comparar los datos obtenidos de ambos sensores. Se muestra a continuación la representación del error durante el tiempo de ensayo. 31 Figura 7-3: Representación del error con el método FreqCounter Para los errores se obtienen los siguientes valores: Error medio (m/s) Desviación típica del error (m/s) 0.0516 0.0430 Tabla 1: Valores estadísticos de los errores de FreqCounter 7.2 Método FreqCount En este caso, seguimos representando las velocidades, donde podemos observar que al igual que el método anterior el sensor Doppler sigue bien los cambios en la velocidad, pero no permite calcularla de forma precisa. Cabe mencionar que las velocidades que aparecen tras el segundo 130 en la gráfica es debido al movimiento de personas en la habitación tras la prueba ya que el sensor Doppler es muy sensible a interferencias. 32 Figura 7-4: Representación de velocidades mediante FreqCount Procediendo de la misma forma que explicamos en el método anterior, reducimos el número de muestras quedando de la siguiente forma. Figura 7-5: Representación velocidades mediante FreqCount con reducción de muestras Para el cálculo del error se han eliminado las interferencias finales comentadas. 33 Figura 7-6: Representación del error con el método FreqCount Para los errores se obtienen los valores de la siguiente tabla: Error medio (m/s) Desviación típica del error (m/s) 0.0663 0.0629 Tabla 2: Valores estadísticos de los errores de FreqCount 7.3 Método FreqMeasure Este es el método que se supone debería ser el más exacto. Como podemos ver en la siguiente representación, no es lo suficientemente preciso tal y como esperábamos. Por tanto quizás haya que desechar la idea de utilización de este método. 34 Figura 7-7: Representación de velocidades mediante FreqMeasure Procediendo de la misma forma anterior y eliminando las interferencias finales en los errores, se obtienen las siguientes gráficas. Figura 7-8: Representación velocidades mediante FreqMeasure con reducción de muestras 35 Figura 7-9: Representación del error con el método FreqMeasure Para los errores se obtienen los siguientes valores: Error medio (m/s) Desviación típica del error (m/s) 0.0647 0.0694 Tabla 3: Valores estadísticos de los errores de FreqMeasure 36 7.4 Método PulseIn Para este método se aprecia que las medidas contienen mucho ruido, por lo que se rechaza la idea directamente de utilización de esta forma de adquirir la velocidad y no se procede a calcular los errores. Figura 7-10: Representación de velocidades mediante PulseIn 37 8 CONCLUSIONES Una vez realizadas las pruebas y tras haber recogido la información necesaria de ambos sensores y todos los métodos, se obtiene la siguiente tabla de errores. MÉTODO ERROR MEDIO (M/S) DESVIACIÓN TIPICA (M/S) FREQCOUNTER 0.0516 0.0430 FREQCOUNT 0.0663 0.0629 FREQMEASURE 0.0647 0.0694 PULSEIN NO PROCEDE NO PROCEDE Tabla 4: Recopilación de medias y desviaciones típicas de errores Se llega a la conclusión de que la forma con la que se permite estimar la velocidad con mayor precisión es con la librería FreqCounter. A pesar de que FreqMeasure era la esperada para ser la de mejor precisión, no ha sido así. Tras la presentación de los resultados, tal y cómo se encuentra programado el sensor y vistas las velocidades en las que se ha manejado el robot, se llega a la conclusión de que no se podría utilizar para la navegación en interiores ya que es necesario de unos requerimientos más exigentes de lo que se ha conseguido. Un error de 5cm/s en un movimiento en el que como máximo se ha llegado sobre los 60cm/s es demasiado grande para el objetivo que se busca ya que implica una imprecisión de aproximadamente un 8%, que crece al ser integrado para calcular el desplazamiento. Esto podría haber sido previsto al inicio, ya que como comentamos anteriormente, el sensor del que se disponía no estaba diseñado para cálculo de velocidad sino para detección de movimiento, tal y como nos especificaba el datasheet. Aún así, el error obtenido no está mal si tenemos en cuenta el bajo peso y precio del sensor, además del poco procesamiento añadido que ha necesitado. Por otro lado, la sensibilidad del mismo podría haber sido ajustada con el potenciómetro que incluye pero por falta de disponibilidad de tiempo en la cámara de detección y seguimiento esto no se ha llegado a hacer y se ha dejado a un valor medio. Se había mencionado anteriormente la posibilidad de integrar varios sensores en un mismo controlador Teensy, permitiendo así aligerar peso en el UAV y medir varias frecuencias en la misma placa. La librería que posibilitaría esto funcionaba exactamente de la misma forma que FreqMeasure. Sin embargo, visto los resultados obtenidos no merecería la pena integrarlos ya que no se conseguiría la precisión exigida. Aún así, sería posible en un futuro desarrollo de este trabajo en el que se implemente el sensor Doppler junto con una unidad de medidas inerciales. De esta forma, primero, sería posible mejorar la precisión de la estimación de la posición mediante implementación con un filtro de Kalman de ambos sensores de manera que disminuya el error, y segundo, la posibilidad de calcular el signo de la velocidad, lo cuál es una incógnita tal y como se comentaba en capítulos anteriores. Si los resultados no mejoraran, sería interesante también intentar utilizar FreqMeasure junto con Teensy en otros sensores Doppler que si que estén diseñados para el cálculo de la velocidad. Si se obtienen buenos resultados de una u otra forma se podría implementar una configuración Janus en un UAV tanto λ como T, dependiendo de si se trataría de un UAV de ala fija o de ala rotatoria. 38 9 ANEXO A: PROGRAMACIÓN EN ARDUINO lenguaje de programación de Arduino está basado en lenguaje C, por lo que muchas de las funciones de este mismo son utilizadas en Arduino. En primer lugar, es necesario de la utilización del IDE de Arduino, que es posible descargar en su página oficial, pues es este software el que va a permitir cargar el programa que escribamos en la placa. Ahora bien, este programa posee una estructura básica que es bastante simple y divide la ejecución en dos partes: setup y loop. Por otro lado, cada instrucción debe acabar con un ; .  La función setup() incluye la declaración de las variables, y en ella se debe escribir las funciones de configuración, pues esta sólo se ejecutará una única vez al inicio del programa. Es empleada para configurar el pinMode.  La función loop() incluye el código que debe ser ejecutado continuamente, pues como su propio nombre indica repite todas las funciones que existan dentro de esta en un bucle infinito. Figura 9-1: Diagrama de flujo de programa básico en Arduino Cada una de estas debe ser establecida entre llaves {}. Por otro lado, es posible crear funciones independientes de estas, fuera de la estructura básica, que puedan ser llamadas en las funciones setup o loop. A continuación se muestra un ejemplo de esto: E 39 En la tabla siguiente se recogen las funciones utilizadas en nuestro programa: Función Descripción de la función #include <librería> Incluye al programa la librería especificada, donde librería es el nombre de la misma. Hay que tener en cuenta que debe estar instalada previamente a la ejecución de esta llamada. Normalmente es utilizada al inicio del código. pinMode(pin, mode) Usada en la función setup() para configurar un pin dado para comportarse como INPUT o OUTPUT Serial.begin(rate) Establece la tasa de datos en bits por segundo (baudios) para la transmisión de datos por el puerto serie, donde rate es dicha tasa. digitalWrite(pin, value) Introduce un nivel alto (HIGH) o bajo (LOW) en el pin digital especificado. delayMicroseconds(time) Realiza una pausa del programa la cantidad de tiempo especificado en microsegundos. millis() Devuelve la cantidad en milisegundos que lleva al placa Arduino ejecutando el programa actual como un valor long unsigned. micros() Devuelve la cantidad en microsegundos que lleva al placa Arduino ejecutando el programa actual como un valor long unsigned. Serial.print() Imprime datos al puerto serie Serial.println() Imprime datos al puerto serie, retornando a principio de línea una vez ejecutado. Tabla 5: Relación de funciones utilizadas en el código de Arduino Además de estas funciones también es posible la utilización de los operadores suma, resta, división y multiplicación. Una vez entendido lo anterior, se explicarán los diferentes tipos de variables que existen, lo cual se asemeja completamente al lenguaje C. Una variable debe ser declarada antes de su utilización y manejo. void setup() { inMode(pin, OUTPUT); // Establece 'pin' como salida } void loop() { digitalWrite(pin, HIGH); // Activa 'pin' delay(1000); // Pausa un segundo digitalWrite(pin, LOW); // Desactiva 'pin' delay(1000); } 46 10.5 Código interpretación datos y visualización en Matlab 10.6 Código disminución del número de datos y representación de errores en Matlab Serial.print(" "); float ratio = 299792458.0 / (2 * 10525000000); //Razón calculada por c/(2*freq_emitida) Serial.print(frecuencia * ratio); Serial.print(" "); Serial.println(frecuencia * ratio / 3.6); } close all; clear all; % Carga datos d1 = load('freq_measure_odom.txt'); %Sensor óptico d2 = load('freq_measure_vel.txt'); %Sensor Doppler % Pasamos tiempo a segundos d1(:,3) = d1(:,3)/1000000000; d1(:,3) = d1(:,3)-d1(1,3); d2(:,3) = d2(:,3)/1000000000; d2(:,3) = d2(:,3)-d2(1,3); % Calculamos el modulo de la velocidad del pose v1(1) = 0; for i=2:length(d1(:,1)) v1(i) = norm(d1(i,5:7)-d1(i-1,5:7))/(d1(i,3)-d1(i-1,3)); end % Ploteamos velocidades figure(1); plot(d1(:,3), v1, 'r', d2(:,3), d2(:,5), 'b'); grid on; xlabel('Tiempo [s]'); ylabel('Velocidad [m/s]'); legend('Velocidad Cámara','Velocidad Doppler'); n = length(d1(:,3)); m = length(d2(:,3)); aux = zeros(m,1); sum = 0; k=1; a = d1(k,3); cont=0; %% Disminución de datos a través de medias for i=2:m b = d2(i,3); while a<=b sum = sum + v1(k); k = k+1; cont=cont+1; a = d1(k,3); end aux(i,1) = sum/cont; cont=0; sum = 0; end 47 %% Representación de velocidades con menor nº de datos figure(2); plot(d2(:,3), aux, 'r-*', d2(:,3), d2(:,5), 'b-o'); grid on; xlabel('Tiempo [s]'); ylabel('Velocidad [m/s]'); legend('Velocidad Cámara','Velocidad Doppler'); %% Representación de errores figure(3); err_abs = abs(aux-d2(:,5)); plot(d2(:,3),err_abs) xlabel('Tiempo [s]'); ylabel('Error [m/s]'); media_error = mean(err_abs) desv_error = std(err_abs) 48 REFERENCIAS [1] C. R. &. H. Muller, «Low Cost Indoor Positioning System,» de Ubicomp 2001: Ubiquitous Computing, Springer Berlin Heidelberg, 2001. [2] J. Rosen y L. Q. Gothard, de Encyclopedia of Physical Science, Infobase Publishing, 2009, p. 155. [3] C. Herrera Martínez, «Integración de Sensor Doppler en Móvil Romeo 4,» [En línea]. Disponible: http://0-bibing.us.es.fama.us.es/proyectos/use/abreproy/11087/direccion/Memoriapdf%252F. [4] E. J. Barlow, «Doppler Radar,» Proceedings of the IEEE. [5] Parallax, «DataSheet X-Band Motion Sensor,» [En línea]. Disponible: http://simplytronics.com/products/ST-00018. [6] «Arduino,» [En línea]. Disponible: https://www.arduino.cc/. [7] M. Nawrath. [En línea]. Disponible: http://interface.khm.de/index.php/lab/interfaces-advanced/arduinofrequency-counter-library/. [8] R. &. Paul, «PJRC,» [En línea]. Disponible: https://www.pjrc.com/teensy/td_libs_FreqMeasure.html. [9] P. &. Robin, «PJRC,» [En línea]. Disponible: https://www.pjrc.com/teensy/td_libs_FreqCount.html. [10] P. Stoffregen, «Github,» [En línea]. Disponible: https://github.com/PaulStoffregen/FreqMeasureMulti. [11] W. R. Fried, «Principles and Performance Analysis of Doppler Navigation Systems,» IEEE Transactions on Aerospace and Electronic Systems. [12] U. F. T. F. Cary Spitzer, Digital Avionics Handbook, Third Edition, CRC PRESS, 2014. [13] R. W. Vopat, «Terrain Bias Compensator for Doppler Navigation Systems». Estados Unidos Patente 5923281, 13 Julio 1999. [14] «Wikipedia,» [En línea]. Available: https://en.wikipedia.org/. [15] Apuntes de la carrera. [16] J. Dybedal, «Doppler Radar Speed Measurement Based On A 24 GHz Radar Sensor,» Noruega, 2013.