En esta ultima practica con el NXT hemos usado un mapa en el que hemos situado particulas y Hans se ha podido autolocalizar.Aunque al principio estabamos mas perdidos que el propio robot hemos conseguido sacar las tres primeras partes.
Visualizacion del mapa y de las particulas
El objetivo de este programa es mostrar el mapa cargado y las particulas inicializadas en el. Para conseguirlo hemos transformado los datos en cm del mapa real a una escala que la pantalla fuera capaz de mostrar.
Diseño del radar
En este caso se trataba de crear una clase que se encargue de englobar los aspectos necesarios para la orientacion por ultrasonidos. Su unico metodo consiste en tomar varias medidas desde diferentes angulos (segun los parametros de entrada) y devolverlas.
Reconocimiento de posicion y orientacion
En esta ultima parte usamos la clase Radar implementada anteriormente. En primer lugar es necesario establecer algunos puntos de referencia. La clase TomarMedidas se encarga de hacer un barrido de 360 grados y almacenar seguidamente la firma. Cada vez que se pulse enter se almacena un numero de firma distinta. Esta firma es concretamente el histograma de las medidas obtenidas por el ultrasonidos por cada angulo al que esta mirando.
Ahora con las posiciones posibles que puede ocupar el robot se puede autolocalizar. Se toma un nuevo barrido de 360 grados donde situemos el robot (con cualquier orientacion). Posteriormente el histograma se transforma en otro histograma que muestra la cantidad de veces que ha aparecido una medida del ultrasonido. Hacemos lo mismo con cada firma almacenada y se va comparando. La diferencia mas pequeña entre la medida capturada y una de las firmas sera la que nos indique en el punto en el que esta.Lo mas complicado en este proceso fue conseguir que el motor consiguiera suficiente precision como para ser fiable al hacer arcos muy cortos con el fin de tener los histogramas lo mas ricos posibles.
Para calcular la diferencia en grados en esta nueva posicion hemos intentado hallar el angulo en el que se situa comparamos la firma capturada con las firma guardada a la que pertenece el punto en que nos situamos. El histograma capturado va rotando los valores y calculando la diferencia con la firma del punto en que nos situamos. Finalmente la menor diferencia nos indicara la cantidad de posiciones que hemos tenido que rotar el histograma para encontrar la firma aproximada. Segun ese numero y la resolucion podemos obtener los grados de diferencia.
HansF10 ha vuelto a sus modestos orígenes en apariencia. Volvemos al diseño con el que comenzo. Pero aunque ya no pueda ser aspirante a míster robot 2010 lo ha compensado con un interior particularmente decente.
Auto-localización mediante filtros de partículas
El primer reto al que se enfrentaba era manejar un filtro de partículas utilizando el método de Monte Carlo. No hemos necesitado creárselo nosotros, ya que las librerías del NXJ ya nos lo proporcionaban. Las utilidades de este filtro son proporcionar la posición del robot aproximada y la posición de objetos ajenos a el. En este caso nos interesa la primera de ellas.
*Ejemplo de robot situado dentro de un mapa y usando un filtro de partículas y ultrasonidos para navegar.
En la practica 1 se implementó una forma de que el robot recorriera un cuadrado. No tenía la exactitud que se esperaba por las limitaciones concretas del robot con el entorno. Con la ayuda del filtro de partículas se podría conseguir corregir los pequeños errores.
Resultados de la ejecución del cuadrado auto localizándose con partículas
La prueba de concepto esta en el programa Particulas.java. Dentro de un mapa 100cm x 100cm sin obstáculos se sitúa el robot en las coordenadas (30 cm, 30 cm, 0 º). En ese punto se sitúan las 100 partículas. El siguiente paso es que el robot empieza a moverse en linea recta, mientras lo hace aplica el movimiento a las partículas usando la odometría de las ruedas.El movimiento aplicado a las partículas también incluirán el ruido en distancia que tenga asignado. Hemos elegido un ruido muy bajo para minimizar la dispersión al máximo, ya que el error en el recorrido era poco mas del 1%.
Al trazar el primer lado del cuadrado el robot gira 90 º y se lo aplica asimismo a las partículas junto con el ruido en ángulo. De aquí en adelante las partículas se dispersan en arco. El ruido para distancia recorrida y giro muy bajo por consiguiente el arco es mínimo.En las dos últimas iteraciones se repite el proceso aumentándose la dispersión en arco cada vez. Para la representación en la pantalla LCD del nxt hemos tenido que establecer una proporción de 0`6 píxeles por cada cm del mapa. Para clarificar el recorrido incluimos también el mapa para que se observara claramente sus limites.
Cálculo de trayectorias
En esta segunda parte se nos pedía implementar un método de navegación para el robot. Siguiendo un eje de coordenadas global, nos manejamos en el espacio de (x,y, rotación). Una primera idea consiste en navegar siguiendo 3 pasos o etapas:
Orientación al punto de destino
Desplazamiento al punto de destino
Obtención de la orientación final
Aprovechando la clase vector implementada para la práctica anterior pudimos desenvolvernos con soltura en el mundo de los ángulos y las distancias.
Una segunda parte nos llevó a plantear el desplazamiento del robot en 2 etapas:
Movimiento en arco hasta el punto de destino
Obtención de la orientación final.
Si bien no pudimos conseguir una limpia consecución del objetivo, al menos lo intentamos. Conseguimos que el robot recorriese en arco la distancia deseada, pero no con la orientación que debería:
Ante esta nueva prueba de esfuerzo (y pensabamos que el cubo de rubick se nos quedaba grande....) hemos tenido que modificar el diseño del robot.
Consultando con personalidades doctoradas en la estetica del robot (aflelou?Llongueras?) conseguimos lo siguiente:
Incorporamos dos sensores infrarrojos del modelo anterior (RCX) formando 45º con su eje, al igual que un sensor de ultrasonidos montando sobre un motor para poder obtener lecturas en distintos angulos.
Sin escatimar esfuerzo y derrochando pruebas hemos conseguido lo siguiente.
COMPORTAMIENTO DE EVITACION DE OBSTACULOS USANDO SENSORES DE CONTACTO
Este caso consta de los comportamientos IrHaciaDelante y EsquivarObstaculos.
El comportamiento IrHaciaDelante es simplemente moverse en linea recta continuamente.
static TachoPilot tp = new TachoPilot(5.6f,11.25f,Motor.C,Motor.B);
public boolean takeControl() { return true; }
public void suppress() { tp.stop(); }
public void action() { tp.forward(); } }
El comportamiento EsquivarObstaculos consiste en que cada vez que alguno de los sensores de contacto sea presionado el metodo action se activa. Como consecuencia Hansf10 retrocederia, rodearia el objeto trazando un arco y pararia en el mismo sentido en el que empezo.
El programa que los utiliza es Ejercicio1. El comportamiento IrHaciaDelante siempre se esta ejecutando hasta que un sensor de contacto sea pulsado. Por prioridad pasa a ejecutarse EsquivarObstaculo y una vez esquivado el obstaculo devolvuelve el control a IrHaciaDelante.
import lejos.robotics.subsumption.*;
public class Ejercicio1 { public static void main(String [] args) { Behavior b1 = new IrHaciaDelante(); Behavior b2 = new EsquivarObstaculo(); Behavior [] bArray = {b1, b2}; Arbitrator arby = new Arbitrator(bArray); arby.start(); } }
COMPORTAMIENTO DE EVITACION DE OBSTACULOS USANDO EL SENSOR DE ULTRASONIDOS
INCOMPLETO. No creemos en la derrota(normalmente) pero esta vez si que hemos admitido no haber superado este obstaculo. El ejercicio planteado, a nuestro juicio, hubiese requerido de más tiempo para completarlo. No obstante, tenemos la sensación de habernos faltado bien poco.
Colgamos las clases implementadas, las cuales creemos están a punto de caramelo de funcionar.
El problema se estructura en 3 clases.
-Clase Vector, para implementar VFF's
-Clase OrientaRobot, para describir el comportamiento.
-Clase Ejercicio2, que inicializa el objeto arbitrator y que gestiona el (buen?) funcionamiento del robot.
CLASE VECTOR
public class Vector{ private float coordX=0; private float coordY=0;
import lejos.nxt.*; import lejos.robotics.navigation.TachoPilot; import lejos.robotics.subsumption.Behavior; import java.util.*; public class OrientaRobot implements Behavior{ public final int MEDIDA_MINIMA=30;//cm. Es la medida con la que se activa el comportamiento public final int MEDIDA_DETECTABLE=100;//cm. Es la medida a partir de la cual empieza a tener en cuenta obstaculos private TachoPilot tp=new TachoPilot(5.6f, 11.2f, Motor.C, Motor.B); private ArrayList distancias=new ArrayList(); private Vector destinoRobot; private Vector vectorResultante=new Vector(0,0,true);
private float distanciaRecorrida=0;
public OrientaRobot(Vector destino){ destinoRobot=new Vector(destino.getX(),destino.getY(),true); tp.reset(); tp.setSpeed(90); Motor.A.resetTachoCount(); Motor.A.setSpeed(90); } //volver sobre la idea de "hayObstaculo".... private void escanear(){ UltrasonicSensor US=new UltrasonicSensor(SensorPort.S1); int actual; US.continuous(); Motor.A.rotate(46); actual=US.getDistance(); for(int i=0;i<4+1;i++){ actual="US.getDistance();"> it=distancias.iterator(); if(dist!=0){ while(it.hasNext()){ Vector actual=it.next(); actual.setX(actual.getX()-dist); } destinoRobot.setX(destinoRobot.getX()-dist); } if(ang!=0){ it=distancias.iterator(); while(it.hasNext()){ Vector actual=it.next(); actual.RotarVector(ang); } destinoRobot.RotarVector(ang); } } private void HallarResultante(){ vectorResultante=new Vector(0,0,true); Iterator it=distancias.iterator(); while(it.hasNext()){ Vector actual=it.next();
public boolean takeControl(){ ActualizarVectores(tp.getTravelDistance()-distanciaRecorrida,0); distanciaRecorrida+=tp.getTravelDistance()-distanciaRecorrida;
public static void main(String[] args) { Behavior b1 = new OrientaRobot(new Vector(100,0,true)); Behavior [] comportamientos = {b1}; Arbitrator ar = new Arbitrator(comportamientos); ar.start(); }
}
COMPORTAMIENTO IR HACIA LA LUZ
Lo realiza el comportamiento Orientar luz con alma de polilla suicida. Se utilizan los dos sensores de luz del rcx para ello. Antes de iniciarlo hemos creido oportuno que se pudiese calibrar segun la luz disponible para conseguir un funcionamiento correcto en cualquier situacion.El comportamiento es muy sencillo. Si alguno de los dos sensores de luz detecta una cantidad diferente que su homologo significara que la luz esta a uno de los lados. La reaccion a dicho estimulo sera girar hasta que la luz vuelva a ser la misma para ambos.Hemos añadido un margen a los valores leidos para evitar que las minimas variaciones de luz del ambiente pudiesen afectar.La inclusion de @SuppressWarnings("deprecation") esta solo para evitar los warnings resultantes por haber utilizado clases obsoletas para leer de los sensores del RCX.
import lejos.nxt.*;
import lejos.nxt.addon.RCXLightSensor;
import lejos.robotics.navigation.*;
import lejos.robotics.subsumption.Behavior;
public class OrientarALuz implements Behavior{
static TachoPilot tp = new TachoPilot(5.6f,11.25f,Motor.C,Motor.B);
public class Ejercicio3 { static RCXLightSensor luzIzq=new RCXLightSensor(SensorPort.S3); static RCXLightSensor luzDer=new RCXLightSensor(SensorPort.S4); @SuppressWarnings("deprecation") public static void main(String [] args) throws Exception { int min; int max; luzIzq.activate(); luzDer.activate(); Thread.sleep(150); LCD.drawString("MinValue:", 2, 2); while(!Button.ENTER.isPressed()){} min=luzIzq.readValue(); Thread.sleep(150); LCD.drawString("MaxValue:", 2, 2); while(!Button.ENTER.isPressed()){} max=luzIzq.readValue(); Behavior b1 = new IrHaciaDelante(); Behavior b2 = new OrientarALuz(min,max); Behavior [] bArray = {b1, b2}; Arbitrator arby = new Arbitrator(bArray); arby.start(); } }
COMPORTAMIENTO DE IR HACIA LA LUZ EVITANDO OBSTACULOS
Por razones evidentes, al no poder completar el segundo ejercicio, el 4º nos pareció imposible :S
Problemas encontrados
En general los mayores problemas los hemos tenido con el comportamiento de esquivar un obstaculo con el ultrasonido.
Conseguimos una posible solucion usando una clase Vector auxiliar para que almacenase vectores y sus metodos asociados.
Los problemas vinieron cuando el robot actuaba como si no detectase el obstaculo.Barajamos varias posibilidades, como que la velocidad de giro del ultrasonido impidiera una lectura correcta,que calculase mal las resultantes, etc. Descubrimos que los metodos estaticos de Vector tenian parte de culpa pero no era suficiente.
Una vez arreglado no tenia en cuenta los obstaculos fuera del alcance de vision de ultrasonidos, asique rediseñamos parte del codigo.
Seguimos intentando mejorar en nuestros avances con HansF10, pero es inevitable echar mano de aquel mítico concursante de gran hermano, para describir lo que sentimos: "¿Quién me pone la mano encima para que no levante cabeza?".
1. Obteniendo información...
El programa en esencia debía mostrar la información obtenida mediante los distintos sensores conectados a Hans (ultrasonidos e Infrarojos).
El resultado fue el esperado, o incluso mejor ya que hicimos que el programa se actualizase en tiempo real.
En este ejercicio se nos pedía un programa que hiciese que el robot, atendiendo a una palmada o sonido brusco, avanzase o se detuviera.
En un principio pensamos abordar el problema captando una medida, y en caso de de ser superior a X dB, activar o no el avance del robot. Sin embargo no teníamos en cuenta el ruido ambiente. Si no tenemos en cuenta que el ruido ambiente puede estar cerca del umbral de los X dB que nosotros marcamos deliberadamente, el robot puede comportarse de manera errática.
Así pues decidimos que lo mejor era tomar una medida, esperar una fracción de tiempo, y volver a medir, y si la diferencia entre ambas era muy grande es porque se había producido un ruido brusco.
Aplicando estos criterios obtuvimos el siguiente comportamiento:
El código que implementamos fue el siguiente:
import lejos.nxt.*;
import lejos.robotics.navigation.TachoPilot;
public class ClapControl {
static TachoPilot tp=new TachoPilot(5.6f, 11.0f, Motor.C, Motor.B);
static SoundSensor SS=new SoundSensor(SensorPort.S3);
static boolean moviendose=false;
static final int SONIDO_MAX=30;
public static void main(String[] args)throws Exception {
int sonido1;
int sonido2;
while (!Button.ESCAPE.isPressed()) {
sonido1= SS.readValue();
Thread.sleep(50);
sonido2=SS.readValue();
LCD.drawInt(Math.abs(sonido2-sonido1),2,2);
if (Math.abs(sonido2-sonido1)>=SONIDO_MAX){
if (!moviendose){
tp.forward();
}else{
tp.stop();
}
moviendose=!moviendose;
}
}
}
}
3. Bump & Go! usando sensores de contacto
La práctica de Bump&Go! implementada consiste en poner el robot a avanzar en una dirección, y en cuanto detecte un obstaculo(esta vez mediante sensores de contacto) retrocede, rota un número arbitrario de ángulos y repite los mismos pasos.El ejercicio ha sido sencillo, siendo lo más tedioso tener que demostrar nuestro avanzado control sobre las matemáticas (¡¿?!) al tener que controlar que los giros no fuesen mayores de 180 º, y en caso de serlo usar el ángulo complementario.Lo que conseguimos fue esto:
public class BumpAndGo {
static TachoPilot tp = new TachoPilot(5.6f,12f,Motor.C,Motor.B);
static TouchSensor touch1=new TouchSensor(SensorPort.S1);
static TouchSensor touch2=new TouchSensor(SensorPort.S2);
public static void main(String[] args) throws Exception{
int giro;
while(!Button.ESCAPE.isPressed()){
tp.forward();
while(!touch1.isPressed() && !touch2.isPressed()){}
tp.travel(-15);
giro=(int)(Math.random()*360);
if (giro>180){
giro-=360;
}
tp.rotate(giro);
}
}
}
4. Bump & Go! usando sensores de ultrasonido
El comportamiento a sequir debía ser el mismo que el del ejercicio anterior, solo que esta vez usando ultrasonidos. Una vez que la distancia obtenida fuese inferior a X cm, el robot debía retroceder, elegir otra dirección y repetir el proceso.
static TachoPilot tp = new TachoPilot(5.6f,11f,Motor.C,Motor.B);
static UltrasonicSensor us = new UltrasonicSensor(SensorPort.S1);
public static void main(String[] args) throws InterruptedException {
int distancia;
int giro;
while(!Button.ESCAPE.isPressed()){
tp.forward();
do{
distancia = us.getDistance();
}while(distancia > 30);
tp.travel(-15);
giro=(int)(Math.random()*360);
if (giro>180){
giro-=360;
}
tp.rotate(giro);
}
}
}
5. Comportamiento sigue-pared para salir de un laberinto
Este ejercicio es el que más problemas nos dio, de hecho, no conseguimos terminarlo.En primer lugar nos costaba que el robot hiciese correctamente el giro de una esquina de 90º. Mediante el método rotate, en cuanto conseguía llegar a los 45º, perdía el contacto con la pared y no se "decidía" si debía girar en sentido horario o antihorario para encontrar de nuevo la pared.Probamos más tarde llamando a "steer(100, giro *5);", es decir, girando, no sobre sí mismo sino sobre la rueda interior, y haciendo un giro de 5º, donde la variable "giro" que puede valer 1 o -1 indica la dirección.El resultado obtenido fue que el robot despues de hacer steer se quedaba parado.Finalmente por falta de tiempo acabamos deshechando la idea de completar este ejercicio.Código con el cual trabajamos:
En este ejercicio se nos pedía determinar el margen de error y limitaciones del sensor de ultrasonidos.
Apartado1
Distancia real maxima = 23distancia real minima = 215
Apartado 2
Para ayudarnos en la tarea hemos usado este pequeño programa :
import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.UltrasonicSensor;
import lejos.robotics.navigation.TachoPilot;
public class PruebaRotando {
private static TachoPilot tp = new TachoPilot(5.6f,12f,Motor.C,Motor.B);
private static UltrasonicSensor us = new UltrasonicSensor(SensorPort.S1);
public static void main(String[] args) throws InterruptedException {
int i,boton,suma,sentido;
sentido = 1;
suma = 0;
boton = 0;
while (!(boton==1)){
boton = Button.waitForPress();
if (boton == 8){
sentido = -1;
}
if (boton == 2){
tp.rotate(sentido*10);
LCD.drawInt(us.getDistance(), 2, 2);
suma += 10*sentido;
LCD.drawInt(suma, 3, 3);
}
else if (boton == 4){
for(i=1;i<=9;i++){
tp.rotate(sentido*1);
LCD.drawInt(us.getDistance(), 2, 2);
suma += 1*sentido;
LCD.drawInt(suma, 3, 3);
Thread.sleep(1000);
}
}
LCD.drawInt(suma, 3, 3);
}
}
}
Estos son los resultados obtenidos:
Grados positivos
10
20
30
40
50
60
70
80
90
medida ultrasonidos
40
40
40
41
255
255
255
255
255
Grados positivos
41
42
43
44
45
46
47
48
49
medida ultrasonidos
41
41
40
41
40
41
40
41
255
Grados negativos
-10
-20
-30
-40
-50
-60
-70
-80
-90
medida ultrasonidos
40
40
41
41
255
255
255
255
255
Grados negativos
-41
-42
-43
-44
-45
-46
-47
-48
-49
medida ultrasonidos
41
40
41
41
40
40
255
255
255
Por tanto el máximo valor negativo para los que los valores son validos es -46 grados, y el positivo 48 grados.
Apartado 3
Medida real
20
30
40
50
60
70
80
90
100
Medida US
22
31
40
50
61
71
81
89
101
La media de los errores entre la medida real y la del ultrasonido es 0,66.
Apartado 4
RealUltrasonido (10 observaciones)
40
40
40
40
40
40
40
40
40
40
40
50
50
50
50
50
50
50
50
50
50
50
60
61
60
60
60
61
61
60
60
60
60
70
71
71
71
71
71
71
71
71
71
71
80
81
82
81
82
81
81
82
81
81
82
90
90
91
91
91
90
91
91
91
91
91
100
100
101
101
101
100
100
101
101
101
101
110
111
113
112
113
111
112
113
113
112
113
120
120
120
121
120
120
121
120
120
121
120
En vista de los resultados obtenidos no tenemos ninguna evidencia que indique que la distancia influya en el error de medicion del sensor de ultrasonidos.
