Desarrollo de un seguidor de paredes inteligente (Parte 3 de 7: Decisión de las variables de entrada)

Durante la aproximación reactiva utilicé el incremento del error, la distancia frontal y la distancia lateral. Pero llegué a la conclusión de que la distancia lateral la podía sustituir por el error (Diferencia entre Distancia lateral y Distancia de referencia) que es una información mucho más completa al tener signo.
Por lo que a partir de ahora nuestras variables de entradas serán:

• AF: Incremento del error.

• DF: Distancia frontal.

• ERROR: Diferencia entre distancia lateral y distancia de referencia.

Seguidamente, descompuse el problema en tres casos para los que creé los datos necesarios para el controlador.

1-Seguimiento de pared (la pared derecha)
Si la distancia lateral está entre
[-DISTANCIA_DE_REFERENCIA, +DISTANCIA_DE_REFERENCIA],
el giro que haré es
(-1) * errorActual * Máximo_giro siendo:

• DISTANCIA_DE_REFERENCIA: La distancia lateral a la que intentamos mantenernos de la pared.

• errorActual: Es la diferencia entre la Distancia lateral y la Distancia de referencia.

• Máximo_giro: Es el valor máximo de giro que permitiremos para el vehículo. En nuestro caso el simulador permite valores entre 0 y 1, y tomaremos un valor máximo de 0.5.

• Signo negativo (multiplicar por -1): En el simulador un giro positivo es un giro a la izquierda y un giro negativo a la derecha, si el error es positivo significa que me encuentro más lejos de la DISTANCIA_DE_REFERENCIA y que tengo que girar a la derecha, así que multiplico por -1 para cambiar el signo. En caso contrario funciona de la misma manera.

Resumiendo, la fórmula lo único que hace es un giro suave y proporcional a la magnitud del error cuando se encuentra cerca de la distancia de referencia, logrando eliminar el “zigzagueo” que tenía el navegador reactivo visto en el apartado anterior.


2. Evitar obstáculo cerrado
Para este caso busqué la forma de describir una circunferencia perfecta para que al salir de esquivar el obstáculo no hubiera que volver a buscar la distancia de referencia, y salir directamente al punto correcto.
Para ello, como el controlador no usa coordenadas cartesianas, no era posible hacerlo por trigonometría. Por lo que, teniendo en cuenta la máxima capacidad de giro, y haciendo algunas pruebas, comprobé que la distancia exacta a la que había que empezar a girar era a 2.5 metros del obstáculo.
El único dato necesario para esquivar el obstáculo es la distancia frontal.

3. Evitar obstáculo abierto
Cuando aparece un obstáculo de este tipo se produce un crecimiento anormalmente grande en el incremento del error.
Evidentemente esto se debe a un aumento del errorActual respecto al error en la iteración anterior. Para este caso la solución es sencilla: girar hasta que se restablezca un error aceptable.
Como consecuencia de un error positivo y con un valor absoluto mayor de lo normal, el vehículo comienza a girar a la derecha. Acabará detectando la nueva pared a seguir con el sensor lateral, por lo que a partir de ese momento hará las correcciones siguiendo el comportamiento del seguimiento de la pared.

Estos tres casos son suficientes para definir los ejemplos de aprendizaje del seguidor de pared.

Desarrollo de un seguidor de paredes inteligente (Parte 2 de 7: Una aproximación reactiva)

Antes de empezar recordar el vehículo real al que intentaremos transportar todo. Se trata de un vehículo diferencial, con una 'rueda loca' trasera, dotado de un sensor ultrasónico, montado sobre un servo capaz de orientarlo 180º de derecha a izquierda.

Para el conjunto de sensores elegí un sistema sencillo que luego se pudiera transportar un sistema real, por lo que me basé únicamente en dos medidas ultrasónicas, una frontal y otra perpendicular apuntando a la derecha. Para hacer las pruebas edité un robot del simulador para tener los 7 sensores que cubrirían el espacio de trabajo del robot real que sería. El robot queda así:

Los sensores están numerados para poder acceder a ellos del 0 al 6 de izquierda a derecha, por lo que para usar el frontal y el derecho tomaremos los sensores de índice 3 y 6, orientados a 0 y 90 grados respectivamente. De este modo, simularemos los robots del laboratorio, que como se ha dicho anteriormente, están dotados de un sónar orientable. Si tomáramos la información de los 7 sensores de los que está dotado nuestro robot del simulador todo sería más sencillo, pero estaríamos diseñando un sistema difícil de obtener para un aficionado.

Por lo tanto, de momento tenemos dos variables de entrada:

• Distancia frontal (DF): Es la distancia al objeto detectado en la zona frontal, que tendrá un valor entre cero y cinco metros.

• Distancia lateral (DD): Seguiremos la pared a la derecha del robot, por lo que la distancia derecha o distancia lateral es la distancia a la que se encuentra la pared. Igualmente tendrá valores entre cero y cinco metros.

Necesitaremos otra variable, ya que será necesario encontrar una forma de ubicar la dirección en la que se mueve el vehículo y tomar una decisión u otra. Esta nueva variable será el incremento del error (AE), que será negativo si hemos hecho un movimiento que nos acerca a la pared y positivo si hacemos un movimiento que nos aleja de ella.
Tenemos que tener en cuenta también una constante que será la distancia a la que queremos mantenernos de la pared (D_WALL).

Primer intento

Con todo esto una tabla de comportamientos para la aproximación reactiva al problema sería:














Hecho esto, sólo resta añadir el comportamiento al encontrar un obstáculo de frente (una esquina cerrada). Lo que haremos en esta aproximación inicial es parar y girar a la izquierda hasta que el obstáculo ya no se detecte.

Es fácil resumir todo lo expuesto en un conjunto de reglas IF-THEN y el código resultante es el siguiente:

public Principal(){
float temp[];
float xSpeed, yawSpeed;
try {
// Connect to the Player server and request access to Position and Sonar
robot = new PlayerClient ("localhost", 6665);
posi = robot.requestInterfacePosition2D (0, PlayerConstants.PLAYER_OPEN_MODE);
soni = robot.requestInterfaceSonar (0, PlayerConstants.PLAYER_OPEN_MODE);
} catch (PlayerException e) {
System.err.println ("WallFollowerExample: > Error connecting to Player: ");
System.err.println (" [ " + e.toString() + " ]");
System.exit (1);
}

robot.runThreaded (-1, -1);
xSpeed = yawSpeed = 0;
float DD, DF; // Distancias por la derecha (actual y anterior)
float AE = 0; // Incremento del error
float DD_REFERENCIA = 1f; // Distancia a la que intentamos mantenernos
float errorActual, errorAnterior = 0f;
float IZQ_15 = 0.15f;
float IZQ_30 = 0.3f;
float DER_15 = -0.15f;
float DER_30 = -0.3f;
float DER_45 = -0.5f;
while (true){
// Hago un barrido de sónar
temp = getSonar();
DD = temp[6]; //Distancia por la derecha
DF = temp[3]; //Distancia por la frontal
errorActual = DD - DD_REFERENCIA;
AE = errorActual - errorAnterior;
if (DF<1.5){
xSpeed=0f;
yawSpeed=IZQ_30;
}else {

xSpeed=1f;

if (AE &mt; 0){
if ((DD&mt;=0) && (DD<0.5))
yawSpeed = 0f;
else if ((DD&mt;=0.5) && (DD<1))
yawSpeed = 0f;
else if ((DD&mt;=1) && (DD<1.5))
yawSpeed = DER_15;
else if ((DD&mt;=1.5) && (DD<2))
yawSpeed = DER_15;
else if ((DD&mt;=2) && (DD<3))
yawSpeed = DER_30;
else if ((DD&mt;=3))
yawSpeed = DER_45;

else{
if ((DD&mt;=0) && (DD<0.5))
yawSpeed = IZQ_30;
else if ((DD&mt;=0.5) && (DD<1))
yawSpeed = IZQ_15;
else if (DD&mt;=1)
yawSpeed = 0;

}

posi.setSpeed(xSpeed, yawSpeed);

errorAnterior = errorActual;

try { Thread.sleep (100); } catch (Exception e) { }
}

}

Con este diseño obtuve el siguiente trazado:














En el esquema anterior podemos notar que:

• El seguimiento en rectas es aceptable (a nivel reactivo).

• El comportamiento en esquinas cerradas se realiza correctamente, aunque se podría afinar para que los giros fueran más suaves (trazada de circunferencia).

• El comportamiento en esquinas abiertas es el que peores resultados obtiene, además tras una esquina abierta el vehículo no se ajusta como debiera.

• El vehículo ha chocado, este error no es aceptable.

Por lo tanto nos centraremos en corregir el choque del vehículo.

Segundo intento: Cambio del diseño de sensores

El problema del choque puede deberse al sensor lateral que detecta los cambios en la pared al mismo tiempo que llega a ellos. Sería conveniente detectar los cambios antes de alcanzarlos para tener más tiempo de reacción. Siguiendo el esquema de sensores del robot que se vio anteriormente, podríamos usar el sensor 5 en lugar del 6. Al estar orientado 60º en lugar de 90º vemos los cambios antes de llegar a ellos, consiguiendo lo que queremos.

La distancia deseada para este sensor se puede calcular por simple trigonometría, y hacer el cambio en nuestro código:










float DD_REFERENCIA = new Float(1 / Math.cos(Math.PI/6)).floatValue(); //Distancia a la que intentamos mantenernos


El resultado obtenido es:














En el esquema anterior podemos notar que:

• El seguimiento en rectas es aceptable

• El comportamiento en esquinas cerradas se realiza correctamente, aunque se podría afinar para que los giros fueran más suaves (trazada de circunferencia).

• El comportamiento en esquinas abiertas es el que peores resultados obtiene, además tras una esquina abierta el vehículo no se ajusta como debiera.

• El vehículo ha completado la vuelta sin chocar.

Ahora que hemos conseguido evitar los choques por medio de un aumento del margen de reacción, nos centraremos en mejorar el comportamiento en esquinas abiertas.

Tercer intento: Detección del nivel de incremento de error para detectar cambios bruscos (esquinas abiertas)

El problema de las esquinas abiertas es el siguiente. Al alcanzarlas el error aumenta considerablemente y se reajusta la dirección, pero en la próxima medida ya el error ha mejorado (aunque sea mínimamente) así que el vehículo considera que está mejorando y sigue recto. Una posible solución es modificar el incremento del error para estos casos especiales.

Lo que haré es que cuando el incremento del error sea mayor a 2, a la hora de apuntar el error anterior para la próxima iteración, lo pondré a 0 de modo que mientras que el error no se reduzca drásticamente se seguirá detectando un incremento considerable del error, y se seguirá intentando corregir el rumbo del vehículo hasta enderezarlo, momento en el que con el sensor lateral detectará la pared de la esquina y volveremos al comportamiento normal.

El nuevo código es:

if (AE>2)
errorAnterior = 0;
else
errorAnterior = errorActual;

Y el resultado obtenido es:














En el esquema anterior podemos notar que:

• El seguimiento en rectas es aceptable

• El comportamiento en esquinas cerradas se realiza correctamente, aunque se podría afinar para que los giros fueran más suaves (trazada de circunferencia).

• El comportamiento en esquinas abiertas es el mejor visto hasta ahora y si es brusco a veces, parece consecuencia directa de venir de una esquina cerrada.

• El vehículo ha completado la vuelta sin chocar.

Por lo que ahora es turno de ajustar las esquinas cerradas.


Cuarto intento: Suavizado del giro en esquinas cerradas
Ahora mismo el giro es brusco porque se espera a estar a una distancia de 1.5 metros para detener el vehículo y girar hasta no tener nada delante. Lo que voy a hacer es detectar la pared mucho antes, e ir incrementando el giro conforme me voy acercando de modo que describa una curva (idealmente un cuarto de circunferencia).

Tras algunos intentos he calibrado los valores y modificado el código de la siguiente manera:

public Principal(){
float temp[];
float xSpeed, yawSpeed;
try {
// Connect to the Player server and request access to Position and Sonar
robot = new PlayerClient ("localhost", 6665);
posi = robot.requestInterfacePosition2D (0, PlayerConstants.PLAYER_OPEN_MODE);
soni = robot.requestInterfaceSonar (0, PlayerConstants.PLAYER_OPEN_MODE);
} catch (PlayerException e) {
System.err.println ("WallFollowerExample: > Error connecting to Player: ");
System.err.println (" [ " + e.toString() + " ]");
System.exit (1);
}

robot.runThreaded (-1, -1);
xSpeed = yawSpeed = 0;
float DD, DF; // Distancias por la derecha (actual y anterior)
float AE = 0; // Incremento del error
float DD_REFERENCIA = new Float(1 / Math.cos(Math.PI/6)).floatValue(); // Distancia a la que intentamos mantenernos
float errorActual, errorAnterior = 0f;
float IZQ_15 = 0.15f;
float IZQ_30 = 0.3f;
float DER_15 = -0.15f;
float DER_30 = -0.3f;
float DER_45 = -0.5f;
while (true){
// Hago un barrido de sónar
temp = getSonar();
DD = temp[5]; //Distancia por la derecha (30º)
DF = temp[3]; //Distancia por la frontal
errorActual = DD - DD_REFERENCIA;
AE = errorActual - errorAnterior;


xSpeed=0.5f;


if (DF<4){
if (DF<1){
xSpeed=0f;
yawSpeed=IZQ_45;
}

else if (DF<2)
yawSpeed=IZQ_30;
else
yawSpeed=IZQ_15;

}else {

if (AE &mt; 0){
if ((DD&mt;=0) && (DD<0.5))
yawSpeed = 0f;
else if ((DD&mt;=0.5) && (DD<1))
yawSpeed = 0f;
else if ((DD&mt;=1) && (DD<1.5))
yawSpeed = DER_15;
else if ((DD&mt;=1.5) && (DD<2))
yawSpeed = DER_15;
else if ((DD&mt;=2) && (DD<3))
yawSpeed = DER_30;
else if ((DD&mt;=3))
yawSpeed = DER_45;

else{
if ((DD&mt;=0) && (DD<0.5))
yawSpeed = IZQ_30;
else if ((DD&mt;=0.5) && (DD<1))
yawSpeed = IZQ_15;
else if (DD&mt;=1)
yawSpeed = 0;

}

}posi.setSpeed(xSpeed, yawSpeed);

if (AE>2)
errorAnterior = 0;
else
errorAnterior = errorActual;

try { Thread.sleep (100); } catch (Exception e) { }
}

}


Y los resultados obtenidos son:

Este resultado es más que aceptable para un controlador reactivo, ahora que nos hemos familiarizado con los problemas de un seguidor de paredes podemos comenzar el diseño del modelo inteligente.

Desarrollo de un seguidor de paredes inteligente (Parte 1 de 7: Introducción)

Uno de los primeros ejercicios que se realizan al tratar la navegación es el seguimiento de una pared. El problema consiste en mantenerse paralelo a una pared y avanzar tratando de mantener una distancia de referencia constante.

En este caso, el navegador recorrerá la pared situada a su derecha.

Una solución trivial sería ir mirando la distancia a la pared derecha y si es mayor que la de referencia girar a la derecha y en caso contrario a la izquierda.

En dicha solución habría que solucionar el problema de que también hay que mirar al frente, ya que puede llegar una esquina. Además el comportamiento probablemente provocaría un zigzagueo.

Para llegar a una solución iré proponiendo una serie de mejoras que tratarán de dar un comportamiento final más refinado