martes, 10 de julio de 2007

Desarrollo de un seguidor de paredes inteligente (Parte 7 de 7: Consideraciones finales)

Ahora que tenemos preparado nuestro controlador difuso para el seguimiento de paredes, hace falta un código cliente que haga uso de él y comenzar las pruebas.

Se dieron problemas durante las simulaciones y se debía a que los sensores no eran capaces de trabajar a la velocidad a la que iteraba el programa.

Para solucionar el problema basta con variar el tiempo de iteración con Thread.sleep (300)

El código es:

public class Principal{
PlayerClient robot = null;
Position2DInterface posi = null;
SonarInterface soni = null;
ControladorFuzzy controladorOrientacion = null;


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: &mt; Error connecting to Player: ");
System.err.println (" [ " + e.toString() + " ]");
System.exit (1);
}

controladorOrientacion = new ControladorFuzzy("datosAngular.pwm", "datosAngular.wm");

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;
ArrayList a = new ArrayList();

while (true){
// Hago un barrido de sónar
temp = getSonar();

DD = temp[5]; //Distancia por la derecha
DF = temp[3];
errorActual = DD - DD_REFERENCIA;


AE = errorActual - errorAnterior;


//CÓDIGO DE NAVEGACIÓN CON CONTROLADOR DIFUSO

//Control de la velocidad (no es necesario controlador)
if ((DF < 0.5f))
xSpeed = MIN_XSPEED_VALUE;
else
xSpeed = MAX_XSPEED_VALUE;


//Controlador de giro
a.add(new Float(AE));
a.add(new Float(DF));
a.add(new Float(errorActual));

//Controlador de dirección

yawSpeed = controladorOrientacion.evalua(a);

System.out.println(yawSpeed);

a.clear();

//FIN DEL CÓDIGO DE NAVEGACIÓN CON CONTROLADOR DIFUSO

posi.setSpeed(xSpeed, yawSpeed);

errorAnterior = errorActual;

try { Thread.sleep (300); } catch (Exception e) { System.out.println(e);}
}

}

public float[] getSonar(){
float distancias[];

while (!soni.isDataReady());
distancias = soni.getData().getRanges();

// Filtrado de valores fuera del rango [SONAR_MIN_VALUE; SONAR_MAX_VALUE]
for (int i = 0; i < soni.getData ().getRanges_count (); i++)

if (distancias[i] < SONAR_MIN_VALUE)
distancias[i] = SONAR_MIN_VALUE;
else
if (distancias[i] &mt; SONAR_MAX_VALUE)
distancias[i] = SONAR_MAX_VALUE;

return distancias;
}


public float acota(float valor, float min, float max){
if (valor < min)
valor = min;
else if (valor &mt; max)
valor = max;
return valor;
}


}



public static void main (String[] args) {
SeguidorParedes sp = new SeguidorParedes();
Principal p = sp.new Principal();

}

Desarrollo de un seguidor de paredes inteligente (Parte 6 de 7: Implementación de un controlador difuso)

El controlador consta de varias clases que podrían haberse implementado en distintos ficheros. El motivo de haberlo hecho todo en el mismo .java se debe a facilitar la futura implementación en un Javelin, que sólo admite en principio un fichero.

Aún así analizaré una a una las clases, desde la más interna, al controlador final. Las clases son:
  • Etiqueta
  • Variable
  • Regla
  • Util
  • ControladorFuzzy

La clase Etiqueta representa el valor que puede tomar un componente de una variable difusa. Si por ejemplo tuviéramos la variable Altura con los posibles valores BAJA, MEDIA y ALTA, dichos valores serían etiquetas.
Por convenencia de proceso, representamos las etiquetas como variables triangulares. Dichas etiquetas pueden ser cortadas a una altura determinada durante la inferencia, para calcular los grados de emparejamiento. En ese caso, si al triángulo le cortamos el pico, tenemos un trapecio, por lo que necesitaremos dos valores de pico, para poder almacenar dichos valores.
El código es el siguiente:

private class Etiqueta{
public float izq;
public float pico;
public float pico2;
public float der;

public Etiqueta(float izq,float pico,float der){
this.izq = izq;
this.pico = this.pico2 = pico;
this.der = der;
}

public Etiqueta(float izq,float pico,float pico2, float der){
this.izq = izq;
this.pico = pico;
this.pico2 = pico2;
this.der = der;
}

public boolean igualA(float izq,float pico,float der){

if ((this.izq == izq) && (this.pico == pico) && (this.der == der))
return true;
else
return false;
}

public void imprimir(){
System.out.println("Izq: "+this.izq+" Pico :"+this.pico+" Pico2 :"+this.pico2+" Der :"+this.der);
}
}

La clase Variable sería para almacenar una variable difusa completa. Una variable tiene un valor mínimo, un valor máximo, una salida por defecto y un número de posibles etiquetas:

private class Variable{
public int nEtiquetas;
public float min;
public float max;
public float salidaPorDefecto;
public ArrayList etiquetas = new ArrayList();

public Variable(float min,float max,int nEtiquetas){
this.nEtiquetas = nEtiquetas;
this.min = min;
this.max = max;
this.salidaPorDefecto = (max + min)/2;
}

public void agregarEtiqueta(float izq,float pico,float der){
etiquetas.add(new Etiqueta(izq,pico,der));
}

public Etiqueta get(int i){
return (Etiqueta)etiquetas.get(i);
}

/** Dados unos valores de un triangulo devuelve
* el índice de la etiqueta si está en la variable
* y -1 si no está
* @param a Valor izquierdo
* @param b Valor central
* @param c Valor derecho
* @return El índice correspondiente
*/
public int esEtiqueta(float a,float b,float c){
Etiqueta temp;
for(int i=0;i<this.nEtiquetas;i++){
temp = this.get(i);
if (temp.igualA(a,b,c))
return i;
}
return -1;
}

public void imprimir(){
System.out.println("Número de etiquetas = "+nEtiquetas);
for(int i=0;i<nEtiquetas;i++){
System.out.println("Valor de la etiqueta "+(i+1)+": ");
this.get(i).imprimir();
}
}

}

La clase sería Regla. Una regla está formada por diversas variables, que pueden ser de estado (entrada) o de control (salida):

private class Regla{
int antecedentes[];
int consecuente;

public Regla(ArrayList datos){
antecedentes = new int[datos.size()];
for(int i=0;i<datos.size()-1;i++)
antecedentes[i] = ((Integer)datos.get(i)).intValue();

consecuente = ((Integer)datos.get(datos.size()-1)).intValue();

}

public int getAntecedente(int i){
try{
return antecedentes[i];
}catch(Exception ex){
return -1;
}
}
}


La clase Util, la he utilizado para declarar métodos que uso en diversas partes del código, como una clase accesibles por las demás, para tenerlo todo más ordenado:
private static class Util {

/** Lee un número real de un fichero de texto */
public static float leeFloat(FileInputStream f){
char c;
StringBuffer temp = new StringBuffer();
try{
c = (char)f.read();
while(!Character.isDigit(c) &&amp; (c != '-') && (c != '.')){
c = (char)f.read();
}
while (Character.isDigit(c) || (c == '-') || (c == '.')){
temp.append(c);
c = (char)f.read();
}

return Float.valueOf(new String(temp)).floatValue();

}catch(Exception ex){
System.out.println(ex.toString());
System.exit(1);
return 0;
}
}

/** Lee un número entero de un fichero de texto */
public static int leeEntero(FileInputStream f){
char c;
StringBuffer temp = new StringBuffer();
try{
c = (char)f.read();
while(!Character.isDigit(c) &&amp; (c != '-')){
c = (char)f.read();
}
while (Character.isDigit(c) || (c == '-')){
temp.append(c);
c = (char)f.read();
}
return Integer.valueOf(new String(temp)).intValue();


}catch(Exception ex){
System.out.println(ex.toString());
System.exit(1);
return 0;
}
}

/** Lee una palabra de un fichero de texto */
public static String leePalabra(FileInputStream f){
char c;
StringBuffer temp = new StringBuffer();
try{
c = (char)f.read();
while(!Character.isLetter(c)){
c = (char)f.read();
}
while (Character.isLetter(c)){
temp.append(c);
c = (char)f.read();
}


}catch(Exception ex){
System.out.println(ex.toString());
System.exit(1);
}
return new String(temp);
}

/** Trata de leer una cadena en un fichero */
public static boolean leeCadena(FileInputStream f,String cadena){
char c;
StringBuffer temp = new StringBuffer(cadena);
try{
for(int i=0;i<temp.length();i++){
c = (char)f.read();
if (c != temp.charAt(i))
return false;
}
}catch(Exception ex){
System.out.println(ex.toString());
System.exit(0);
}
return true;

}

}

La clase ControladorFuzzzy contiene todo lo necesario para un controlador difuso. Haciendo uso de las clases anteriores, es capaz de cargar todos los datos a partir de los archivos .wm y .pwm. Una vez cargados todos los valores en las estructuras, provee de métodos para la evaluación de datos externos. Comentaré los métodos uno a uno:
Contructor: Llama a los métidos cargaVariables y cargaReglas para cargar los datos del controlador en los atributos de la clase que son:
1.antecedentes: Una lista con las Variables de entrada del problema.
2.consecuente: La Variable de salida del problema.
3.reglas: Una lista con la base de Reglas del problema

cargaVariables: Toma el fichero .pwm y carga los datos sobre las variables del problema, tales como el universo de discurso, el número de etiquetas y el valor de los puntos de los triángulos para cada una de las etiquetas.

cargaReglas: Cada regla está formada por una etiqueta para cada una de las variables del problema. Este método toma el fichero .wm y lee los valores de los triángulos que conforman las etiquetas de cada regla. Si las etiquetas son valores válidos para las variables cargadas en el método anterior serán aceptadas y almacenadas. En caso contrario, se cancelará la operación.

Fuzzifica: Es el método encargado de tomar un dato de entrada y comprobar el emparejamiento con una etiqueta determinada. Dependiendo del punto en el que corte al triángulo, devolverá un valor entre 0 y 1.

evalua: Recibe como parámetros unos valores de entrada detectados por los sensores y calcula la salida adecuada.
Primero calcula los grados de emparejamiento de los datos con cada una de las reglas por la conjunción mediante la T-Norma del mínimo.
A continuación infiere los consecuentes para cada regla.
Para terminar, calcula el valor desfuzzificado mediante:


private class ControladorFuzzy{

private ArrayList antecedentes = new ArrayList();
private Variable consecuente;
private ArrayList reglas = new ArrayList();

public ControladorFuzzy(String datosVariables,
String datosReglas){

this.cargarVariables(datosVariables);
this.cargarReglas(datosReglas);
//DEBUG
/*
for(int i=0;i<antecedentes.size();i++){
((Variable)antecedentes.get(i)).imprimir();
}
consecuente.imprimir();
*/
}

private void cargarVariables(String fichero){
int nEtiquetas;
float min,max,izq,pico,der;
boolean fin = false;
String cadena;
Variable temp;
FileInputStream f = null;
try{
f = new FileInputStream(fichero);
while(!fin){
while (!Util.leeCadena(f,"Variable de "));
cadena = Util.leePalabra(f);
if (!cadena.equals("estado"))
fin = true;
while (!Util.leeCadena(f,"Numero de etiquetas = "));
nEtiquetas = Util.leeEntero(f);
while(!Util.leeCadena(f,"Universo de discurso = ["));
min = Util.leeFloat(f);
//while(!Util.leeCadena(f,","));
max = Util.leeFloat(f);
//while(!Util.leeCadena(f,"]"));
if (!fin)
antecedentes.add(new Variable(min,max,nEtiquetas));
else
consecuente = new Variable(min,max,nEtiquetas);
}

for(int i=0;i<antecedentes.size();i++){
temp = (Variable)antecedentes.get(i);
for(int j=0;j<temp.nEtiquetas;j++){
while(!Util.leeCadena(f,"("));
izq = Util.leeFloat(f);
pico = Util.leeFloat(f);
der = Util.leeFloat(f);
temp.agregarEtiqueta(izq,pico,der);
}

}
for(int j=0;j<consecuente.nEtiquetas;j++){
while(!Util.leeCadena(f,"("));
izq = Util.leeFloat(f);
pico = Util.leeFloat(f);
der = Util.leeFloat(f);
consecuente.agregarEtiqueta(izq,pico,der);
}



f.close();
}
catch(Exception ex){
System.out.println("Error de lectura"+ex.toString());

}
}

private void cargarReglas(String fichero){
FileInputStream f = null;
int nReglas = 0;
Variable v;
float valorIzquierdo = 0;
float valorCentral = 0;
float valorDerecho = 0;
ArrayList etiqueta = new ArrayList();

try{
f = new FileInputStream(fichero);

}catch(Exception ex){
System.out.println("Ocurrió un error con el fichero de reglas");
System.exit(1);
}

Util.leeCadena(f,"Numero de reglas: ");
nReglas = Util.leeEntero(f);

//Lectura de las reglas
for(int i=0;i<nReglas;i++){
for(int j=0;j<antecedentes.size()+1;j++){

valorIzquierdo = Util.leeFloat(f);
valorCentral = Util.leeFloat(f);
valorDerecho = Util.leeFloat(f);


if (j == antecedentes.size())
v = consecuente;
else{
v = (Variable)antecedentes.get(j);

}
etiqueta.add(new Integer(v.esEtiqueta(valorIzquierdo,valorCentral,valorDerecho)));
if (((Integer)etiqueta.get(j)).intValue() == -1){
System.out.println("Etiqueta no reconocida");
System.exit(1);
}
}

reglas.add(new Regla(etiqueta));
etiqueta.clear();
}

}



private float Fuzzifica(float X,Etiqueta D)
{

if ((X<D.izq) || (X&mt;D.der))
{
return (0);
}

if (X<D.pico)
return ((X-D.izq)*(1/(D.pico-D.izq)));

if (X&mt;D.pico2)
return ((D.der-X)*(1/(D.der-D.pico2)));
return (1);
}


public float evalua(ArrayList dat){

int i,j;
float aux,minimo,num,den,pico,pico2;
float gradoEmparejamiento[] = new float[reglas.size()];
int ndatos = dat.size();
float datos[] = new float[ndatos];
Variable x[] = new Variable[ndatos];
Etiqueta consecuentes[] = new Etiqueta[reglas.size()];
Etiqueta temp;

for(i=0;i<ndatos;i++){
datos[i] = ((Float)dat.get(i)).floatValue();
x[i] = ((Variable)antecedentes.get(i));
}

/* Conjunción mediante T-Norma del mínimo */

for(j=0;j<reglas.size();j++)
{
minimo = Float.MAX_VALUE;
for(i=0;i<ndatos;i++){
aux = this.Fuzzifica(datos[i],x[i].get( ((Regla)reglas.get(j)).getAntecedente(i)) );
if (aux<minimo)
minimo = aux;
}

gradoEmparejamiento[j] = minimo;
}

/* Inferencia de los consecuentes difusos para cada regla */
for(j=0;j<reglas.size();j++)
{
if (gradoEmparejamiento[j] != 0.0)
{
if (gradoEmparejamiento[j] == 1.0)
consecuentes[j] = consecuente.get(((Regla)reglas.get(j)).consecuente);
else
{
temp = consecuente.get(((Regla)reglas.get(j)).consecuente);
pico = temp.izq + (temp.pico - temp.izq)*gradoEmparejamiento[j];
pico2 = temp.der + (temp.pico2 - temp.der)*gradoEmparejamiento[j];
consecuentes[j] = new Etiqueta(temp.izq,pico,pico2,temp.der);
}
}
}

/* Desfuzzifico */
num = 0.0f;
den = 0.0f;
for(j=0;j<reglas.size();j++)
if (gradoEmparejamiento[j] != 0)
{
//System.out.println("Grado de emparejamiento "+ j+ " es "+gradoEmparejamiento[j]+".El valor acumulado ("+num+","+den+")" );
num += gradoEmparejamiento[j] * (consecuentes[j].pico+consecuentes[j].pico2)/2;
den += gradoEmparejamiento[j];
}
if (den!=0.0)
return (num/den);
else
return (consecuente.salidaPorDefecto);




}
}

Desarrollo de un seguidor de paredes inteligente (Parte 5 de 7: Algoritmo de Wang y Mendel)

Para el algoritmo Wang y Mendel utilicé una implementación de la universidad por lo que no puedo poner el código.

Es fácil de encontrar documentación y hacer vuestra propia implementación del algoritmo de Wang y Mendel.

Para que os hagáis una idea del funcionamiento, los ficheros de entrada son así:

  • Los archivo datosAngular.tra y datosAngular.tst tienen el siguiente formato:
1415
4
-2.000000 0.000000 -2.000000 0.500000
-2.000000 0.000000 -1.500000 0.500000
-2.000000 0.000000 -1.000000 0.500000
-2.000000 0.000000 -0.500000 0.500000
(...)

  1. El primer valor es el número de ejemplos contenidos en el fichero.
  2. El segundo es el número de valores de cada ejemplo.
  3. A continuación vienen el resto de ejemplos.
  • El archivo datosAngular.t tiene la siguiente información:
Fichero con datos de entrenamiento = datosAngular.tra
Fichero con datos de prueba = datosAngular.tst
Variable de estado = 1
Numero de etiquetas = 5
Universo de discurso =[-2.000000,2.000000]
Variable de estado = 2
Numero de etiquetas = 5
Universo de discurso =[0.000000,5.000000]
Variable de estado = 3
Numero de etiquetas = 5
Universo de discurso =[-2.00000,5.00000]
Variable de control = 1
Numero de etiquetas = 5
Universo de discurso =[-0.50000,0.500000]

Desarrollo de un seguidor de paredes inteligente (Parte 4 de 7: Los ejemplos de aprendizaje)


Para crear los ejemplos diseñé un script basado en las conclusiones del apartado anterior para generar una serie de ejemplos.



#!/usr/bin/env python
# -*- coding: utf-8 -*-

# #######################################################
# Diego Muñoz Escalante
#
# Diseño de un seguidor de paredes inteligente
#
# ##########################################################


import sys
import math


#########################
# PARÁMETROS NECESARIOS #
#########################

# Distancia de referencia a la pared
D_WALL = 1/math.cos(math.pi/6)

#Distancia a la que empezamos a responder ante una esquina cerrada
D_ESQUINA = 2.5

# Incremento del error
MIN_AE = -2.0
MAX_AE = +2.0
P_AE = 0.5

# Distancia frontal
MIN_DF = 0.0
MAX_DF = 5.0
P_DF = 0.5

# Error (es más completa que la distancia lateral
MIN_ERROR = -2.0
MAX_ERROR = 5.0
P_ERROR = 0.5

# Velocidad angular
MAX_YAW = 0.5 #La positiva marca un giro izquierdo y la negativa a la derecha

# Velocidad lineal
MAX_XSPEED = 0.5


#############################
# Archivos de entrenamiento #
#############################

lista = []
print 'Generando datos de entrenamiento...'
iAE = MIN_AE - P_AE
while (iAE < MAX_AE):
iAE += P_AE
iDF = MIN_DF - P_DF
while (iDF < MAX_DF):
iDF += P_DF
iERROR = MIN_ERROR - P_ERROR
while (iERROR < MAX_ERROR):
iERROR += P_ERROR
regla = False
# Creación de las reglas
xSpeed = MAX_XSPEED
if (iDF <= D_ESQUINA):
if (iDF < 0.5):
xSpeed = 0
yawSpeed = MAX_YAW
regla = True

elif ((iERROR &mt;= -D_WALL) and (iERROR <= D_WALL)):
yawSpeed = - iERROR * MAX_YAW
regla = True

elif (iAE &mt; 1.0) or (iERROR &mt; D_WALL):
yawSpeed = - MAX_YAW
regla = True

if (regla):
lista.append([iAE,iDF,iERROR,yawSpeed])

f = open('datosAngular.tra','w')
f.write(str(len(lista))+'\n'+str(len(lista[0]))+'\n')
for l in lista:
f.write('%f %f %f %f\n' % (l[0],l[1],l[2],l[3]))
f.close()

####################
# Archivos de test #
####################
lista = []
print 'Generando datos de test...'
iAE = MIN_AE - P_AE/2
while (iAE < MAX_AE/2):
iAE += P_AE
iDF = MIN_DF - P_DF/2
while (iDF < MAX_DF/2):
iDF += P_DF
iERROR = MIN_ERROR - P_ERROR/2
while (iERROR < MAX_ERROR/2):
iERROR += P_ERROR
regla = False
# Creación de las reglas
xSpeed = MAX_XSPEED
if (iDF <= D_ESQUINA):
if (iDF < 0.5):
xSpeed = 0
yawSpeed = MAX_YAW
regla = True

elif ((iERROR &mt;= -D_WALL) and (iERROR <= D_WALL)):
yawSpeed = - iERROR * MAX_YAW
regla = True

elif (iAE &mt; 1.0) or (iERROR &mt; D_WALL):
yawSpeed = - MAX_YAW
regla = True

if (regla):
lista.append([iAE,iDF,iERROR,yawSpeed])

f = open('datosAngular.tst','w')
f.write(str(len(lista))+'\n'+str(len(lista[0]))+'\n')
for l in lista:
f.write('%f %f %f %f\n' % (l[0],l[1],l[2],l[3]))
f.close()

print 'Hecho'

Con este script obtenemos el fichero datosAngular.tra, que será el de los datos de entrenamiento, y el datosAngular.tst que será el de las pruebas de test. Estos dos ficheros son necesarios para obtener las reglas por el método de Wang y Mendel que veremos en el próximo apartado.

La explicación del código es sencilla:

  1. Declaramos los valores máximos, mínimos y de resolución de cada variable, y los valores necesarios como la distancia a la pared y a las esquinas.

  2. Hacemos tres bucles anidados recorriendo todos los posibles valores de las variables.

  3. Vemos si la combinación de los tres valores pertenece a uno de los tres casos fundamentales que comentamos en el caso anterior. Si pertenece, calculamos el valor de salida adecuado y lo añadimos a la lista de casos. Si no, lo desechamos.

  4. Hacemos dos veces el proceso, una para los datos de entrenamiento y otro para los datos de test, pero en el caso de los de test, sumamos la mitad de la resolución al valor inicial, para ir obteniendo valores distintos (los mismos valores de test que de entrenamiento, no sirven).

El script es código Python, para ejecutarlo simplemente hacemos:

#python generaDatos.py



miércoles, 16 de mayo de 2007

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