martes, 10 de julio de 2007

Desarrollo de un seguidor de paredes inteligente (Resultados)

Para empezar ejecutaremos nuestro controlador difuso sobre el ejemplo con el hicimos las primeras aproximaciones.

El resultado es:

Podemos apreciar claramente la mejora. Las rectas no tienen ningún tipo de “zigzagueo”. Las curvas cerradas se toman con suavidad, y el vehículo detecta perfectamente las curvas abiertas corrigiendo su rumbo.


Veamos otro ejemplo más complicado:


Como podemos ver con curvas con un ángulo más pronunciado, el robot también responde.

Aparentemente, la sección superior con forma de escalera, la hace mal. Pero no es así. El robot detecta la curva abierta y la corrige, pero en el proceso encuentra la cerrada y la corrige suavemente. Esa suavidad es la que hace que parezca que está pasando de largo, pero vimos en las secciones anteriores que era preferible una curva suave, que un giro de 90 grados parando el vehículo.


Analicemos otro ejemplo más:

Nuevamente el resultado es perfecto. Tras los tres ejemplos analizados podemos decir que el comportamiento es adecuado.

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