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.