Práctica 5 - docencia de la ETSIT-URJC - Universidad Rey Juan ...

Prácticas con Lego NXTRobóticaUniversidad Rey Juan CarlosPráctica 5 - Auto-localización basada en filtro departículasEl objetivo de esta práctica es que apliques los conocimientos sobre filtros departículas aprendidos en la práctica 4 al problema de la auto-localización de unrobot móvil en un entorno conocido (mapa).Esta práctica será evaluada por los profesores de la asignatura en un plazo detres semanas desde su presentación en clase. Podrás conseguir hasta un total de18 puntos.Visualización del mapa y las partículas (2 puntos)El mapa del entorno con el que vamos a trabajar se muestra en la figura inferior.La clase LineMap permite crear un mapa utilizando un conjunto de segmentos y unrectángulo exterior (como hicimos en la fase 4). Adicionalmente, es posible volcarla información de un mapa a un fichero binario, para luego cargarlo sin necesidadde definir sus segmentos. Para esta práctica os proporcionamos el fichero map1.bincon el mapa que utilizaremos en esta práctica y que coincide con el mapa realinstalado en el laboratorio.A continuación os mostramos el fragmento de código necesario para cargar elmapa:try {FileInputStream fis = new FileInputStream(new File("map1.bin"));DataInputStream data = new DataInputStream(fis);map = new LineMap();map.loadMap(data);fis.close();} catch (Exception e) {System.out.println("File error");}Escribe un programa denominado Localization.nxt que cargue el mapa bin1.bine instancie un filtro de partículas como el de la fase anterior. Inicializa todas las

partículas de manera que se repartan uniformemente por el mapa. Escribe unmétodo showParticles() que dibuje en la pantalla del robot el mapa cargado, juntocon la posición de todas las partículas (si te resulta útil para depurar tambiénpuedes dibujar la orientación de cada partícula mediante un pequeño segmento).Punto X YO 0 0A 0 189B 89 189C 89 69D 95 69E 95 123F 189 120G 188 0Diseño del radar (2 puntos)En este ejercicio tendrás que crear una nueva clase Radar.java que contengalos siguientes métodos públicos:public Radar (int vel, int resol, int amp, UltrasonicSensor sonar, Motor motor)Éste será el constructor de la clase y queremos que acepte como parámetrosla velocidad de escaneo, la resolución en grados del radar, la amplitud en gradosque es capaz de sensar, el objeto de tipo UltrasonicSensor que vamos a usar comosensor y el motor que va a controlar el movimiento de escaneo.public RangeReadings newScan()Este método provocará un nuevo movimiento de escaneo. Se deberá hacer unbarrido completo con los parámetros apropiados y se deberán registrar las medidaspara ser devueltas en forma de objeto RangeReadings.La clase RangeReadings se encuentra declarada en el paquete lejos.robotics ysu finalidad es encapsular toda la funcionalidad de un escaneo a varias distancias.2

Esta clase mantiene como atributo una lista de objetos de tipo RangeReading,que permiten almacenar una medida concreta con su distancia y ángulo desde laque se realizó. A continuación os mostramos el constructor de la clase RangeReadingsy su método setRange() para especificar una medida concreta. Consulta ladocumentación de la clase para ver todos los métodos disponibles.public RangeReadings(int numReadings)/*** Set the range reading** @param index the index of the reading in the set* @param angle the angle of the reading relative to the robot heading* @param range the range reading*/public void setRange(int index, float angle, float range)Reconocimiento de posición y orientación (4 puntos)Existe un mecanismo de auto-localización consistente en aprender las característicasde determinados puntos del mapa, para luego compararlos con las medidasreales. Esta técnica requiere realizar una fase de aprendizaje para almacenarlas características de cada punto del mapa. Por ejemplo, podría tomarse una seriede medidas del sensor de ultrasonidos mientras se rota una vuelta completa. Elresultado obtenido podría ser similar al mostrado por la figura 1.El histograma real obtenido junto con uno aprendido puede ser comparadorealizando un test de correlación, calculado como suma de diferencias al cuadrado.La diferencia D k entre el nuevo histograma H m (i) y el histograma del descriptorde posición almacenado H k (i) es:D k = ∑ i(H m (i) − H k (i)) 2 (1)La firma que obtenga menor valor D k se corresponderá con el punto más probable.También podría considerarse disponer de un umbral mínimo, que si no essuperado se concluye que se estará en otro lugar.Un detalle a tener en cuenta es que para que está técnica funcione adecuadamenteel histograma real y el aprendido deben realizarse partiendo de la mismaorientación. En caso de no ser así, los dos histogramas se parecerán pero uno de3

Figura 1: Medidas de distancia obtenidas durante la fase de aprendizaje de undeterminado puntoellos estará desplazado. Este valor de desplazamiento puede usarse para calcularla orientación obtenida.Puesto que el cálculo de la orientación requiere gran cantidad de operacionespor cada histograma almacenado, es posible almacenar una firma de cada posiciónde manera invariante a la orientación. Una vez localizada nuestra posición podemosrealizar el test de correlación para conocer nuestra orientación, pero ya sobreun único punto candidato. El histograma invariante a la orientación almacena elnúmero de veces que hemos recibido cada distancia durante la prueba. La figura2 muestra un ejemplo.Escribe un programa que sea capaz de reconocer los cinco puntos que se encuentranmarcados en el mapa. El robot deberá ser capaz de identificar en cuál delos cinco puntos se encuentra y especificar cuál es su posible orientación. Deberásindicar mediante algún mensaje por el LCD el resultado.Puedes utilizar el método dumpReadings() de la clase RangeReadings para volcaren un fichero el resultado de un histograma. De esta manera podrás reutilizarlocuando tu programa termine. Para cargarlo deberás usar el método loadReadings().A continuación mostramos un fragmento de código para almacenar en un ficherosignature1.bin el estado actual del objeto signature de tipo RangeReadings.4

Figura 2: Histograma de medidas invariante a la orientacióntry {File file = new File ("signature1.bin");if(file.exists()){file.delete();}file.createNewFile();FileOutputStream fos = new FileOutputStream(file);DataOutputStream dos = new DataOutputStream(fos);signature.dumpReadings(dos);fos.close();} catch (Exception e) {System.out.println("File error");}Reto de la auto-localización (8 puntos)En este ejercicio deberás combinar toda la potencia de los filtros de partículasaplicados al problema de la auto-localización de nuestros robots. La prueba consistiráen colocar tu robot en uno de los cinco puntos marcados en el mapa, con unaorientación arbitraria. El robot deberá ser capaz de navegar hasta los cuatro puntosrestantes en el orden que se considere oportuno, deteniéndose durante un segundoen cada punto (opcionalmente puede emitir un sonido además de detenerse), parafinalmente volver a su posición de salida. La orientación con la que se alcance los5

puntos intermedios y el punto de finalización de la prueba no será relevante.Para superar este verdadero reto robótico tendrás que activar todas las etapasde la auto-localización mediante filtros de partículas que explicamos en la práctica4. La clase TachoLocalizer del paquete lejos.robotics.localization nos será de granayuda. Esta clase nos permitirá navegar por el mapa, mientras mantiene una estimaciónde nuestra posición en el mismo. Veamos los métodos más importantes deesta clase:public TachoLocalizer(RangeMap map, int numParticles, int numReadings,float wheelDiameter, float trackWidth,Motor leftMotor, Motor rightMotor, float projection,boolean reverse)Éste es el constructor de la clase y acepta varios parámetros: un mapa delentorno, el número de partículas que deseamos utilizar, el número de lecturas quecompondrá nuestra observación (esto valor se utilizará para instanciar un objetode tipo RangeReadings), el motor al que conectaremos nuestra rueda izquierda, elmotor al que conectaremos nuestra rueda derecha, un valor que indica la distanciadesde el sensor de ultrasonidos hasta la parte delantera del robot y un valor parainvertir el sentido de la marcha.A la hora de navegar es posible utilizar los métodos rotate() y travel(). Esimportante invocar el método updatePosition() para desplazar automáticamentelas partículas después cada movimiento (paso de actualización).Las observaciones obtenidas del sensor de ultrasonidos también nos ayudarána que nuestra estimación converja hacia la zona donde nos encontramos. La claseTachoLocalizer es una clase abstracta. Esto obliga a que tengamos que realizarnuestra propia clase heredando de TachoLocalizer e implementando el método takeReadings().En este método deberemos generar un nuevo escaneo de nuestroradar y actualizar el filtro de partículas con esta nueva observación (paso de corrección).El método calculateWeights() de la clase MCLParticleSet se encarga dematerializar este paso actualizando los pesos de cada hipótesis en función de laobservación./*** Calculate the weight for each particle** @param rr the robot range readings*/public void calculateWeights(RangeReadings rr, RangeMap map)Es posible obtener el mapa mediante la llamada getMap() de la clase Tacho-Localizer.6

Finalmente, tras cada movimiento nuevo u observación es necesario generaruna nueva población de partículas (paso de remuestreo). El método resample() dela clase MCLParticleSet se encarga de esta tarea.public boolean resample()Para realizar los pasos de corrección y de remuestreo, el método getParticles()nos devuelve el objeto de tipo MCLParticleSet asociado a nuestro objeto de tipoTachoLocalizer. A su vez, la llamada getEstimatedPose() retorna la posición estimadapor nuestro algoritmo de auto-localización. Cabe destacar que este métodono devuelve ninguna información acerca de la incertidumbre de la estimación. Siqueremos conocer el grado de incertidumbre consulta la documentación de la claseMCLParticleSet. Esta información podría ser muy valiosa para conocer el grado defiabilidad de la estimación y si continuamos con la navegación o, por el contrario,tomamos nuevas medidas hasta estar más seguros de la posición que ocupamosdentro del mapa.Actualización semanal del blog (2 puntos)Actualiza la bitácora de tu grupo de prácticas con todas las curiosidades, problemas,soluciones o aspectos interesantes que hayas extraído de todas las prácticasrealizadas. Se valorará positivamente la inclusión de material multimedia (fotos yvídeos), así como diagramas esquemáticos del código realizado. Recuerda que espreferible una entrada corta pero valiosa que una entrada larga y farragosa peroaburrida y sin aportes.7