C A P T U L O 7

Navegacin

7.0 Introduccin

La navegacin es uno de los santos griales de la robtica. De todos los desafos de la robtica, la

navegacin tiene la mayora de las aplicaciones. Si quieres un robot que pueda limpiar una

alfombra, trapear el piso, cortar el csped, o entregar la medicina, todo lo que se necesita es un

robot que puede navegar.

Usted encontrar poco despus de comenzar la navegacin con su robot muchos imprevistos,

las cosas no planificadas sucedern. Ocurren porque sus sensores no interpretaron el medio

ambiente con la precisin que le hubiera gustado. Llamo estos datos pobres o pobres de

interpretacin de "Ruido". En este captulo, vamos a empezar por tener la navegacin del robot

en un entorno que tiene poco ruido y luego aumentar la complejidad y el ruido del medio

ambiente a medida que avanzamos.

Antes de comenzar, sin embargo, me gustara introducir un poco de terminologa. Los trminos

provienen de una rama de las matemticas llamada teora de grfica.

Un vrtice es un sinnimo de punto o nodo y representa un solo elemento de un conjunto.

Una arista es una conexin entre los vrtices.

Un grfico es el conjunto de vrtices V, y los bordes de V.

Un grfico simple es un grfico no ponderado, no dirigido.

Un grfico dirigido indica los viajes entre dos vrtices slo en una direccin especfica.

Un grfico ponderado es un grfico con los pesos en los bordes.

La Figura 7-1 muestra los diferentes tipos de grficos.

Figura 7-1. Un grfico simple, un grafo dirigido, y una grfica ponderada

Para ilustrar cmo se pueden utilizar los grficos con la navegacin, vamos a tomar un viaje a la

tienda de comestibles. Usted comienza en el vrtice A, y termina en el vrtice B. Para la

diversin, vamos a aadir un viaje ala estacin de gas, que ser el vrtice C, y un viaje a la

mquina de cajero automtico (ATM) por algo de dinero, en el vrtice D. Si agrega millas o el

tiempo que se tarda en ir y venir de cada uno de estos vrtices, el grafico ahora se convierte en

un grfico ponderado (como se muestra en la Figura 7-1).

El grfico de la Figura 7-2 tambin tiene otras cualidades; no se puede llegar desde el cajero

automtico o la estacin de gas desde la casa sin ir a la tienda de comestibles. As que su

programa de robot slo necesita saber cmo ir de A a B. Luego de B slo tiene que saber cmo

llegar a C o D.

Figura 7-2. El grfico de viaje

Para Representar vrtices y aristas, voy a crear dos clases: un vrtice con un nombre campo, y

un borde con un campo de nombre y dos vrtices. Ms tarde, voy a extender estas clases para

que los problemas de navegacin se pueden desglosar en el anlisis de un camino a travs de

un grfico. Ver Ejemplos 7-1 y 7-2.

Example 7-1. Vertex.java

package com.scottpreston.javarobot.chapter7; public class Vertex { public String name; public Vertex() {} public Vertex(String n) { name = n; } } Example 7-2. Edge.java

package com.scottpreston.javarobot.chapter7; public class Edge { public String name; public Vertex v1; public Vertex v2; public int w; public Edge() {} // constructs with two vertices and a weight public Edge(Vertex v1, Vertex v2, int w) { this.v1 = v1; this.v2 = v2; this.w = w; } public String toString() { return "{v1=" + v1.name +",v2=" + v2.name + ",w=" + w +"}"; } }

En este captulo, voy a crear 19 clases y un programa bsico Stamp. Habr cinco clases de

navegacin:

Navegacin: Realiza navegacin bsica (mejor en las regiones ideales)

Localizacin: proporciona un punto de inicio para un robot y le da la posibilidad de navegar a

otras coordenadas

Navegacin Obstculo: Provee para evitar obstculos durante la navegacin

Navegacin Interior: Muestra cmo crear mapas en interiores y cmo navegar

Navegacin al aire libre: Muestra cmo utilizar el GPS para navegar por el robot

La Figura 7-3 muestra un diagrama de clases de todos estos juntos.

Figura 7-3. Todas las clases de navegacin

Antes de comenzar la navegacin con estas clases, tengo que decirte sobre el robot utilizado,

Feynman5 (ver Figura 7-4), ya que tendrs que hacer ajustes en algunas de las clases en este

captulo, segn la configuracin de su robot.

Figura 7-4. El robot Feynman5

En primer lugar, algunos antecedentes sobre el nombre. Empec a construir robots hace mucho

tiempo, y mi primer Robot PC fue nombrado Feynman. Llam as porque nombr todos los PC

de mi casa despus como fsicos famosos. Tuve computadoras nombradas Einstein, Hawking,

Newton, y Feynman. La uno que se convirti posteriormente en un robot era Feynman. El robot

se ilustra en la Figura 7-4 es la quinta generacin de la original, de ah el nombre Feynman5.

Feynman5 fue construido durante la redaccin de este libro en la primavera de 2005. Su chasis

es de 80-20 extrusiones de aluminio y PVC negro, que me dieron en McMaster.Com. Para el

cerebro, tiene un VIA EPOA M1000, Mini-ITX con 256 MB de RAM y un disco duro de 3,5 "de 20

GB. El microcontrolador es un BASIC Stamp 2 en un Parallax de Educacin.

Para el poder, yo uso dos pilas-una hora 33 amperios para los motores y una para el ordenador

y perifricos. Para el movimiento, yo uso una de Scott Edwards MiniSSC-II, dos Victor 833

controles de velocidad de IFI Robtica y dos NPC-41250 motores de sillas de ruedas. He

conectado el MiniSSC en paralelo con mi BASIC Stamp 2 al puerto serie del PC.

Para la deteccin, utilizo una brjula digital Devantech CMPS03, dos GP2D02 de Sharp sensores

infrarrojos, tres SRF04 dispositivos de sonar, y dos Pyro 1394 webcams. Para mover las

webcams, he usado el Kit de Pan & Tilt de Lynxmotion. Para conectar los sensores, he usado una

investigacin Sonar Preston Mdulo de distribucin y una placa portadora CMPS03.

Tabla 7-1 muestra lo que todos los 16 pins E / S del sello estn conectados.

Ahora que usted sabe lo que mi robot tiene, vamos a ver lo que lo hace navegar, a partir del

microcontrolador. Figura 7-5 muestra un diagrama de clases de las estructuras de datos y la

clase NavStamp utilizando esas estructuras.

Figura 7-5. Un diagrama de clases de estructuras NavStamp y Datos.

Antes de entrar datos al microcontrolador, he decidido crear algunas estructuras de datos para

mantener esta informacin. Estas estructuras incluyen los siguientes:

Las lecturas del sonar: realizar los datos del sonar

Lecturas IR: realizar datos infrarrojos

Lecturas Distancia: Para contener datos combinados

Lecturas GPS: realizar longitud GPS y datos de latitud

Todas las lecturas discutidas prximo ser un ~ (tilde) cadena - delimitada. Aunque tener un byte

array es tan til para estos nmeros, los retornos proceden de un puerto serial web o un puerto

serie estndar. Si se acaba de venir de un puerto serie, me vendra bien un byte [], pero byte

corrientes a travs de Internet son menos convenientes. Todas las lecturas tambin tienen

campos pblicos. Evito el setter Java / convencin tpica getter porque quiero acceder a estas

estructuras de datos como puedo acceder a java.awt.Point.

En SonarReadings (vase el Ejemplo 7-3), el constructor toma una cadena de valor Sonar1 ~

Sonar2 Sonar3 ~.

Ejemplo 7-3. SonarReadings.java

package com.scottpreston.javarobot.chapter7; import java.io.Serializable; public class SonarReadings implements Serializable { public int left = 0; public int center = 0; public int right = 0; public SonarReadings() { // default } public SonarReadings(String readings) { // sample input "11~22~33" String[] values = readings.split("~"); left = new Integer(values[0]).intValue(); center = new Integer(values[1]).intValue(); right = new Integer(values[2]).intValue(); } public String toString() { return "left=" + left + ",center=" + center + ",right=" + right; } }

La segunda estructura de datos es para los dos detectores de infrarrojos afilados anteriores y en

frente de las ruedas. El constructor toma una cadena de valor Ir1 ~ Ir2. Vase el ejemplo 7-4.

Ejemplo 7-4. IRReadings.java

package com.scottpreston.javarobot.chapter7; import java.io.Serializable; public class IRReadings implements Serializable { public int left = 0; public int right = 0; public IRReadings() { // default }

public IRReadings(String readings) { String[] values = readings.split("~"); left = new Integer(values[0]).intValue(); right = new Integer(values[1]).intValue(); } public String toString() { return "left=" + left + ",right=" + right; } }

La lectura final es de todos los detectores de infrarrojos de sonar y al mismo tiempo. El

constructor toma una cadena de valor Ir1 ~ ~ Ir2 Sonar1 ~ ~ Sonar2 Sonar3. Vase el ejemplo 7-

5.

Ejemplo 7-5. DistanceReadings.java

package com.scottpreston.javarobot.chapter7; import java.io.Serializable; public class DistanceReadings implements Serializable { public SonarReadings sonar = new SonarReadings(); public IRReadings ir = new IRReadings(); public DistanceReadings(String readings) throws Exception { String[] values = readings.split("~"); ir.left = new Integer(values[0]).intValue(); ir.right = new Integer(values[1]).intValue(); sonar.left = new Integer(values[2]).intValue(); sonar.center = new Integer(values[3]).intValue(); sonar.right = new Integer(values[4]).intValue(); } public String toString() { return ir.toString() + "," + sonar.toString(); } }

Voy a dejar la discusin de GPSReadings.java hasta la seccin 7.5, "Navegacin al aire libre." Para

la clase NavStamp en el Ejemplo 7-6, esto debe parecer muy familiar para las clases que he

creado en Captulo 2. Los bytes de comando en la parte superior coincidan con los bytes

esperados en el programa BASIC Stamp.

El otro PING_CYCLE_TIME primitiva esttica ser utilizada por las clases de navegacin que

necesitan saber cunto tiempo debe esperar hasta que el microcontrolador termine

consiguiendo datos del sensor. El constructor utiliza la interfaz JSerialPort he creado en el

Captulo 2. Los otros mtodos corresponden a la obtencin de datos especficos del

microcontrolador, por ejemplo:

getCompass (): Obtiene un int atrs como un encabezado. Esto le dir el robot qu direccin

es frente relativa al norte magntico.

getIr (): Obtiene los sensores de infrarrojos en la base del robot.

getSonar (): Obtiene el sonar en la parte superior del robot.

getSonarIR (): Obtiene tanto el sonar y la informacin infrarroja del robot.

getGpsLongitude (), getGpsLatitude (), y getGps (): Estas sern discutidas ms adelante en

seccin 7.5.

Ejemplo 7-6. NavStamp.java

package com.scottpreston.javarobot.chapter7; import com.scottpreston.javarobot.chapter2.Controller; import com.scottpreston.javarobot.chapter2.JSerialPort; import com.scottpreston.javarobot.chapter2.Utils; import com.scottpreston.javarobot.chapter2.WebSerialClient; public class NavStamp extends Controller { // command bytes to microcontroller public static byte CMD_INIT = 100; public static byte CMD_COMPASS = 101; public static byte CMD_SONAR = 102; public static byte CMD_IR = 103;

public static byte CMD_IR_SONAR = 104; public static byte CMD_GPS_LAT = 105; public static byte CMD_GPS_LON = 106; public static byte CMD_DIAG = 107; public static int PING_CYCLE_TIME = 200; public NavStamp(JSerialPort port) throws Exception { super(port); } // get compass reading public int getCompass() throws Exception { String heading = execute(new byte[] { CMD_INIT, CMD_COMPASS }, 175); String[] h2 = heading.split("~"); String heading2 = ""; for (int h = 0; h < h2.length; h++) { heading2 = heading2 + (char) new Integer(h2[h]).intValue(); } return new Integer(heading2).intValue(); }

// get ir public IRReadings getIR() throws Exception { String readings = execute(new byte[] { CMD_INIT, CMD_IR }, 75); return new IRReadings(readings); } // get sonar public SonarReadings getSonar() throws Exception { String readings = execute(new byte[] { CMD_INIT, CMD_SONAR }, 75); return new SonarReadings(readings); } // get both ir and sonar public DistanceReadings getSonarIR() throws Exception { String readings = execute(new byte[] { CMD_INIT, CMD_IR_SONAR }, 200); return new DistanceReadings(readings); } // get gps longitude public String getGpsLongitude() throws Exception { byte[] readings = execute2(new byte[] { CMD_INIT, CMD_GPS_LON }, 5000); return Utils.toAscii(readings); } // get gps latitude public String getGpsLatitude() throws Exception { byte[] readings = execute2(new byte[] { CMD_INIT, CMD_GPS_LAT }, 5000); return Utils.toAscii(readings); }

// get both longitude and latitude public GpsReading getGps() throws Exception { String lon = getGpsLongitude(); String lat = getGpsLatitude(); return new GpsReading(lon, lat); } // get diagnostic signal public boolean diagnostic() throws Exception { String s = execute(new byte[] { CMD_INIT, CMD_DIAG }, 80); if (s.equals("1~2~3")) { return true; } return false; }

// test all methods public static void main(String[] args) { try { WebSerialClient com = new WebSerialClient("10.10.10.99", "8080", "1"); NavStamp s = new NavStamp(com); System.out.println("diag=" + s.diagnostic()); Utils.pause(500); System.out.println("compass=" + s.getCompass());

Utils.pause(500); System.out.println("ir=" + s.getIR().toString()); Utils.pause(500); System.out.println("diag=" + s.getSonar().toString()); Utils.pause(500); System.out.println("all dist=" + s.getSonarIR()); s.close(); System.out.println("done"); } catch (Exception e) { e.printStackTrace(); System.exit(1); } } }

El siguiente es el programa para el BASIC Stamp. En la etiqueta principal, espera un byte inicial

de 100, despus de lo cual se espera para los prximos comandos.

Nota: Debido a que el SSC se engancha a la misma conexin de serie que el BASIC Stamp, el

SSC podra enviar un byte de 100 al SSC para una posicin. Sin embargo, debido a que el sello

est en busca de dos bytes en el 100s, que ignorar el segundo byte de la SSC, si es que existe,

ya que ser un byte de sincronizacin 255 (ver SSCProtocol.java en el Ejemplo 3-3).

La primera seccin de este programa inicializa las variables para todas las constantes, las

variables de trabajo, y volver variables. Se puede ver que las constantes se definen a

continuacin corresponden a la BASIC Sello 2 pines en la Tabla 7-1.

La segunda seccin consta de la zona del programa principal, donde se ve y se espera un entrada

de solicitud byte [] de la clase NavStamp y luego las ramas a la subrutina en funcin en el

comando. La tercera seccin consta de subrutinas especficamente diseados para obtener

infrarrojos, sonar, y lecturas de la brjula y, a continuacin, devolver la salida a la clase

NavStamp en forma de una serie byte []. Vase el ejemplo 7-7.

Ejemplo 7-7. nav1.bs2

' {$STAMP BS2} ' {$PBASIC 2.5} ' {$PORT COM1} ' cmd variable cmd VAR Byte N9600 CON 16468 ' GPD02 IR dout1 CON 2 'output to the DIRRS. (green) din1 CON 3 'input from the DIRRS. (yellow) dout2 CON 4 'output to the DIRRS. (green) din2 CON 5 'input from the DIRRS. (yellow) dout3 CON 6 'output to the DIRRS. (green) din3 CON 7 'input from the DIRRS. (yellow) dist1 VAR Byte 'ir dist 1 dist2 VAR Byte 'ir dist 2 dist3 VAR Byte 'ir dist 3 ' CMPS03 COMPASS cin CON 14 'serial data out GREEN (a) heading VAR Word 'heading ' srf04 sonar convfac CON 74 '74 inches, 29 cm ECHO1 CON 9 'input from the SRF04. (red) INIT1 CON 8 'output to the SRF04. (gry) ECHO2 CON 11 'input from the SRF04. (yel) INIT2 CON 10 'output to the SRF04. (grn)

ECHO3 CON 13 'input from the SRF04. (blu) INIT3 CON 12 'output to the SRF04. (pur)

wDist1 VAR Word 'sonar1 wDist2 VAR Word 'sonar2 wDist3 VAR Word 'sonar3 status VAR Byte gpsData1 VAR Byte(5) gpsData2 VAR Byte(4) N4800 CON 16572 'GPS baudrate (4800) main: cmd = 0 SERIN 16,16468,main,[WAIT(100), cmd]

IF cmd = 101 THEN get_compass ' gets compass reading (READ-ms) - return after a time

IF cmd = 102 THEN ping_sonar ' pings the sonar (READ-ms) - return after a time

IF cmd = 103 THEN ping_ir ' pings the sonar (READ-ms) - return after a time

IF cmd = 104 THEN ping_all ' pings the sonar (READ-ms) - return after a time IF cmd = 105 THEN get_lat ' gets gps latitude IF cmd = 106 THEN get_lon ' gets gps longitude IF cmd = 107 THEN get_diag ' gets diagnostic PAUSE 5 GOTO main get_compass: PULSIN cin, 1, heading ' Get reading

heading = (heading-500)/50 ' BS2(e) - Calculate Bearing in degrees SEROUT 16,N9600,[DEC heading] ' out to PC GOTO main ping_sonar: GOSUB sonar1 GOSUB sonar2 GOSUB sonar3 ' output is s1~s2~s3 SEROUT 16,N9600,[wDist1,wDist2,wDist3] GOTO main ping_ir: GOSUB ir1 GOSUB ir2 ' output is ir1~ir2 SEROUT 16,N9600,[dist1,dist2] GOTO main

ping_all: GOSUB ir1 GOSUB ir2 GOSUB sonar1 GOSUB sonar2 GOSUB sonar3 ' output is ir1~ir2~s1~s2~s3 SEROUT 16,N9600,[dist1,dist2,wDist1,wDist2,wDist3] GOTO main ir1: LOW dout1 ir1b: IF IN3=0 THEN ir1b SHIFTIN din1,dout1,2,[dist1\8] HIGH dout1 RETURN ir2: LOW dout2

ir2b:

IF IN5=0 THEN ir2b SHIFTIN din2,dout2,2,[dist2\8] HIGH dout2 RETURN sonar1: PULSOUT INIT1,5 ' 10us init pulse OUTPUT INIT1 ' (delay) RCTIME ECHO1,1,wDist1 ' measure echo time wDist1=wDist1/convfac ' convert to inches RETURN sonar2: PULSOUT INIT2,5 ' 10us init pulse OUTPUT INIT2 ' (delay) RCTIME ECHO2,1,wDist2 ' measure echo time wDist2=wDist2/convfac ' convert to inches RETURN sonar3: PULSOUT INIT3,5 ' 10us init pulse OUTPUT INIT3 ' (delay) RCTIME ECHO3,1,wDist3 ' measure echo time wDist3=wDist3/convfac ' convert to inches RETURN

get_lat:

*SERIN GPS,N4800,2000,get_lat,[WAIT("GPRMC,"),SKIP 7,status,SKIP 1, STR gpsData1\4,SKIP 1,STR gpsData2\4]

*SEROUT 16,N9600,[status,",0",gpsData1(0),gpsData1(1),":",gpsData1(2), gpsData1(3),".",gpsData2(0),gpsData2(1),gpsData2(2),gpsData2(3)] *GOTO main get_lon:

*SERIN GPS,N4800,2000,get_lon,[WAIT("GPRMC,"),SKIP 7,status,SKIP 13, STR gpsData1\5,SKIP 1,STR gpsData2\4]

*SEROUT 16,N9600,[status,",",gpsData1(0),gpsData1(1),gpsData1(2), ":",gpsData1(3), gpsData1(4),".",gpsData2(0),gpsData2(1),gpsData2(2),gpsData2(3)] *GOTO main get_diag: SEROUT 16,N9600,["1","2","3"] goto main

Resumen de la seccin

Ahora estoy listo para navegar el robot. Aunque la seccin anterior era slo una introduccin,

Cubr varios temas fundamentales, tales como:

Bsico Teora de Grafos: Describir escenarios ideales de navegacin

Configuracin del robot: describir los componentes y la estructura del robot se utiliza para

demostrar la navegacin

Estructuras de datos: Describen la realizacin de la informacin procedente de la

microcontrolador

Recuperacin de datos del sensor: Describen los mecanismos exactos para solicitar datos del

microcontrolador para mi configuracin del robot

La siguiente seccin demostrar el proceso de navegacin ms bsico: la navegacin a estima.

7.1 Conceptos bsicos de navegacin

El tipo ms bsico de proceso de navegacin de estimacin. Navegacin a estimarse es el

proceso de deducir la posicin de un vehculo o robot basado en curso y la distancia. Para realizar

este tipo de navegacin se necesita una manera de medir tanto el rumbo y la distancia.

Para demostrar esto, voy a navegar en un espacio definido por las coordenadas de 100 pulgadas

100 pulgadas. Voy a llamar a este espacio de un mundo perfecto (vase la Figura 7-6) y desde

ella se puede ver que si el robot parte de un punto, y necesita moverse al punto B, es un clculo

trigonomtrico basado en la distancia recorrida en la direccin y, y la distancia recorrida en la

direccin x, o ngulo theta a travs de una distancia c.

Figura 7-6. El diagrama del mundo perfecto

Para que un robot pueda hacer esto, se debe cumplir con lo siguiente:

Moverse en una lnea recta.

Enfrentarse a una direccin especfica.

Moverse una distancia especificada.

Preparar

Nuestro robot requerir lo siguiente para realizar la navegacin bsica estimada:

SpeedDiffDrive.java (captulo 4)

Una lectura de la brjula (Captulo 5)

Para actualizar la clase de unidad diferencial y cmo se relaciona con el movimiento, por favor

vea el diagrama de la Figura 7-7.

Figura 7-7. El diagrama de clases SpeedDiffDrive del Captulo 2

Siguiente, me decid a calibrar que nuestro robot se mueva en lnea recta desde el control de la

velocidad y que servo controladores enven tensiones analgicas precisas a los motores. Siempre

y cuando sus motores tengan caractersticas de rotacin idnticas, que debe estar bien con el

establecimiento de ellos a la misma velocidad. Sin embargo, asegrese de tomar algunas

mediciones de la velocidad de los robot se mueve para un determinado velocidad en una unidad

de tiempo determinada. Como yo no estoy usando codificadores, tuve que calibrar Feynman5

en dos superficies de cemento y la alfombra-para velocidades especficas.

Cdigo Objetivo

El objetivo cdigo aqu es crear una clase de navegacin que le da al robot la capacidad de morir

Cdigo Discusin

Figura 7-8 muestra las tres clases que se encargar de la navegacin bsica.

Figura 7-8. Las tres clases que se encargan de la navegacin bsica: navegacin, MotionVector,

y DistanceVector

De las dos clases necesarias para conseguir nuestro robot para realizar la navegacin se estima,

primero lo que vamos a discutir es MotionVector. MotionVector representa el ttulo y la hora

de cualquier movimiento del robot que va a querer hacer. Su clase hija, DistanceVector, se usa

para ayudar al programador a diferenciar entre un movimiento con unidades de distancia o

unidades de tiempo. Los dos campos pblicos son dirigir y magnitud. El ttulo que eleg para esto

es un int para grados y la magnitud puede ser cualquier valor doble. Vase el ejemplo 7-8.

Ejemplo 7-8. MotionVector

package com.scottpreston.javarobot.chapter7; public class MotionVector extends Edge{ public int heading = 0; public double magnitude = 0; public MotionVector(int h, double seconds) { heading = h; magnitude = seconds; weight= (int)seconds; } public MotionVector(String h, String seconds) throws Exception { heading = new Integer(h).intValue(); magnitude = new Double(seconds).doubleValue(); weight= (int)magnitude; } public String toString() { return "Heading: " + heading + " Seconds: " + magnitude; } }

La siguiente clase, DistanceVector, es bsicamente el mismo que el MotionVector excepto que

en esta clase de navegacin puedo convertir centmetros a segundo a travs de una conversin

basada en una calibracin de la velocidad y la superficie de robot. Vase el ejemplo 7-9.

Ejemplo 7-9. DistanceVector

package com.scottpreston.javarobot.chapter7; public class DistanceVector extends MotionVector { public DistanceVector(int h, double inches) { super(h, inches); }

public DistanceVector(String h, String inches) throws Exception { super(h, inches); } public String toString() { return "Heading: " + heading + " Inches: " + magnitude; } }

La prxima cosa que quiero hacer es crear la clase de navegacin. Los tres campos de instancia

en esta clase son para la unidad, el microcontrolador, y el tipo de superficie actual, ya que la

superficie podra cambiar durante el viaje de un robot. La primera de las constantes estticas

son cuatro enumeraciones para especificar que cuando se le da una orden al robot deba

moverse en una direccin especfica durante un tiempo especfico. El segundo set se compone

de relacionar y coordinar las lecturas tomadas de la brjula mientras el robot se enfrentaba a

una direccin especfica. En este nuevo sistema de coordenadas, el norte est en la parte

delantera de la casa, este es el lado derecho, el sur es en la parte trasera, y el oeste es hacia el

lado izquierdo. La constante esttica final es la velocidad por defecto, que yo pongo hoy a 25.

NOTA: modifiqu la clase SpeedDiffDrive del Captulo 3 para tomar el tiempo de 1 a 10 a 1

para 100 para mayor precisin.

Los constructores de esta clase son los mismos: JSerialPort. Con esta JSerialPort, creo una

instancia de la SpeedDiffDrive, NavStamp, y SonarServos. En este momento, todo lo que

necesito es el NavStamp y la SpeedDiffDrive. Asimismo, establecer la velocidad por defecto al

valor constante DEFAULT_SPEED; esto puede ser cualquier valor de 1 a 100.

El siguiente mtodo es changeHeading () con un parmetro de entrada de un int que

representar el encabezamiento objetivo del robot. El encabezamiento meta ser de 0 a 360,

donde 0 es el norte, 90 es este, 180 es el sur, y 270 es oeste. Sin embargo, estos nmeros son

ideales y no coinciden con la relacin de partidas tomadas a travs de la experimentacin. Para

conseguir el objetivo del robot de partida que coincida con rbricas del mundo real, creamos

un mtodo llamado getRealAngle () para realizar la conversin.

Ahora, debido a la velocidad del robot que tena que frenar mi robot de forma considerable

durante el proceso de girar. De lo contrario, se mover muy rpido y tardan ms en encontrar

el rumbo correcto, porque de rebasamiento. Un rebasamiento sucede cuando el robot est

tratando de ir desde, por ejemplo, de 90 a 100 grados y se mueve demasiado, tal vez 130 grados.

Sobre impulso sucede debido al tiempo de la vuelta y la velocidad del giro.

Para evitar el exceso, he creado un booleano llamado alternancia. Cuando el mtodo est en el

estado toggle, le dice al mtodo que ya se sali de una vez y que es el momento de reducir la

velocidad de el tamao a su vez por 250 milsimas de segundo.

He encontrado a travs de experimentacin que el robot funciona mejor con una precisin de

ms o menos 2 grados, una velocidad de 12, y un tamao de giro de 1 segundo.

Para el cambio real rbrica parte del algoritmo, que quera asegurarse de que el robot tom

lecturas y refinaron su posicin hasta que fue dentro de la precisin definida ~ 4 grados. Para

esto, Lo tena continuamente a travs de un while (true) condicional.

Mientras que en el interior del bucle, el robot comprueba su rumbo actual respecto a la direccin

que quiere a cara. Yo llamo a esto relHeading lectura (abreviatura de partida relativa), y para

mantener a los grados siempre entre 0 y 360, aad 360 para estas partidas relativas inferior a

cero. Una vez que el robot conoce su rumbo relativo, puede comenzar de inflexin. Si el

encabezamiento relativo es entre 0 y 180, entonces yo digo que gire a la izquierda. Si en el ttulo

relativo es entre 180 y 360, entonces yo digo que para pivotar a derecha. Dependiendo de lo

lejos que el robot es a partir de su posicin de destino, puedo reducir el tiempo de vuelta.

Luego, una vez que se alcanza la precisin, me aseguro de que la unidad se detiene. Rompo fuera

de onda y Puedo restablecer la velocidad a 2.

Los siguientes mtodos de la clase dos mtodos move (). Uno toma un DistanceVector y el otro

toma un MotionVector. Para el mtodo de tomar la DistanceVector como un parmetro,

pulgadas se convierten a segundos utilizando el mtodo getSurfaceRate (). Es importante tomar

mediciones para esto si no est utilizando un codificador. Si est utilizando un codificador,

entonces su clase unidad ya tendr un mecanismo para que para a una distancia especificada,

as que aqu exigira que el mtodo de su clase de coche en lugar de hacer una conversin.

Por ltimo, en el mtodo main () de prueba, el robot se mueve en una caja cuadrada de 3 pies

en las direcciones este, norte, oeste y sur. Al final, debe ser de vuelta donde empez, siempre

que las calibraciones sean correctas. (Vase el Ejemplo 7-10.)

Ejemplo 7-10. Navigation.java

package com.scottpreston.javarobot.chapter7; import com.scottpreston.javarobot.chapter2.JSerialPort; import com.scottpreston.javarobot.chapter2.Utils; import com.scottpreston.javarobot.chapter2.WebSerialClient; import com.scottpreston.javarobot.chapter3.JMotion; import com.scottpreston.javarobot.chapter3.SpeedDiffDrive; public class Navigation { // movement constants for raw movement public static final int RAW_FWD = 0; public static final int RAW_REV = 1; public static final int RAW_RGT = 2; public static final int RAW_LFT = 3; // relative readings for 4 coordinate axes public static final int REL_NORTH = 40; public static final int REL_EAST = 100; public static final int REL_SOUTH = 160; public static final int REL_WEST = 255; // surface constants public static final int SURFACE_CEMENT = 1; public static final int SURFACE_CARPET = 2; // default speed public static final int DEFAULT_SPEED = 25;

// instance variables public int surface = SURFACE_CEMENT; private JMotion drive; private NavStamp navStamp; public Navigation(JSerialPort serialPort) throws Exception { // drive with default speed drive = new SpeedDiffDrive(serialPort); drive.setSpeed(DEFAULT_SPEED); // stamp for sensors

navStamp = new NavStamp(serialPort); } // change heading public void changeHeading(int newHeading) throws Exception { // this will calculate a real angle from a relative measure of // the coord axis. newHeading = getRealAngle(newHeading); int accuracy = 2; // degrees // autoadjust speed depending on the surface if (surface == SURFACE_CEMENT) { // slow so don't overshoot 15 degrees at 1sec intervals drive.setSpeed(12); } else { // moves slower on carpet

drive.setSpeed(20); } // used to record lats turn int lastTurn = 0; boolean toggle = false; int turnSize = 1000; while (true) { // get compass int currentHeading = navStamp.getCompass(); // get relative heading from compass to where you want to go int relHeading = currentHeading - newHeading; // adjust for negative if (relHeading < 0) { relHeading = 360 + relHeading; } // if within bounds, stop if (relHeading = 360 - accuracy) { drive.stop(); break; }

// in case it overshoots direction twice if (toggle) { // reset toggle = false; // reduce turn time by 250ms turnSize = turnSize - 250; } // turn for a second left if (relHeading < 180 && relHeading > 15) { if (lastTurn == 'R') { toggle = true; } drive.pivotLeft(turnSize); // record what turn lastTurn = 'L'; // turn for a second right } else if (relHeading >= 180 && relHeading < 345) { // records toggle if (lastTurn == 'L') { toggle = true; } drive.pivotRight(turnSize); lastTurn = 'R'; } else if (relHeading >= 345) { drive.pivotRight(250); } else if (relHeading

}

// adjust for angle measured to absolute angle public static int getRealAngle(int theta) { int phi = 0; double ratio = 0.0; // if in 1st quadrant if (theta > 0 && theta < 90) { // 1. get % of the total range // 2. get range // 3. multiply range by percentage, add it to current north reading. phi = (int) ((theta / 90.0) * (REL_EAST - REL_NORTH)) + REL_NORTH; }

if (theta > 90 && theta < 180) { theta = theta - 90; phi = (int) ((theta / 90.0) * (REL_SOUTH - REL_EAST)) + REL_EAST; } if (theta > 180 && theta < 270) { theta = theta - 180; phi = (int) ((theta / 90.0) * (REL_WEST - REL_SOUTH)) + REL_SOUTH; } if (theta > 270 && theta < 360) { theta = theta - 270;

phi = (int) ((theta / 90.0) * ((360 + REL_NORTH) - REL_WEST)) + REL_WEST; } // in case actual directions if (theta == 0) { phi = REL_NORTH; } if (theta == 90) { phi = REL_EAST; } if (theta == 180) { phi = REL_SOUTH; } if (theta == 270) { phi = REL_WEST; } if (phi > 360) { phi = phi - 360; } return phi; }

// setter for drive speed public void setSpeed(int s) throws Exception { drive.setSpeed(s); } // getter for drive speed public int getSpeed() { return drive.getSpeed(); } // distance vector is in inches public void move(DistanceVector dVect) throws Exception { // convert since in inches dVect.magnitude = getSurfaceRate(dVect.magnitude);

// converted to MotionVector move(dVect); } // motion vector is in inches public void move(MotionVector vect) throws Exception { // change heading Utils.log("MV=" + vect.toString()); changeHeading(vect.heading); // move fwd or reverse

if (vect.magnitude > 0) { drive.forward((int) (vect.magnitude * 1000)); } else if (vect.magnitude < 0) { drive.reverse((int) (-vect.magnitude * 1000)); } } public void moveRaw(int dir, int ms) throws Exception { if (dir == RAW_FWD) { drive.forward(ms); } if (dir == RAW_REV) { drive.reverse(ms); } if (dir == RAW_RGT) { drive.pivotRight(ms); } if (dir == RAW_LFT) { drive.pivotLeft(ms); } }

// surface rate when adjusting inches to seconds public int getSurfaceRate(double inches) { if (surface == SURFACE_CARPET) { return getMillisecondsCarpet(inches); } if (surface == SURFACE_CEMENT) { return getMillisecondsCement(inches); } return 0; } // surface rate when adjusting inches to seconds private int getMillisecondsCement(double inches) {

double convFactor = 0.0; // this will be second/inches switch (drive.getSpeed()) { case 10: convFactor = 1 / 4.0; break; case 20: convFactor = 1 / 7.0; break; case DEFAULT_SPEED: convFactor = 1 / 14.0; break; case 30: convFactor = 1 / 20.0; break; } // will return seconds return (int) (inches * convFactor); } // surface rate when adjusting inches to seconds private int getMillisecondsCarpet(double inches) { double convFactor = 0.0; // this will be second/inches switch (drive.getSpeed()) { case 10: convFactor = 1 / 16.0; case 20: convFactor = 1 / 36.0; case 30: convFactor = 1 / 48.0; } return (int) (inches * convFactor); }

// call to stop since in case of emergency public void stop() throws Exception {

drive.stop(); } // move for multiple vectors public void move(MotionVector[] path) throws Exception { for (int i = 0; i < path.length; i++) { move(path[i]); } }

public JMotion getDrive() { return drive; } public NavStamp getNavStamp() { return navStamp; } public static void main(String[] args) { try { WebSerialClient sPort = new WebSerialClient("10.10.10.99", "8080", "1"); Navigation nav = new Navigation(sPort); // move east 36 inches nav.move(new DistanceVector(90, 36)); // move north 36 inches nav.move(new DistanceVector(0, 36)); // move west 36 inches nav.move(new DistanceVector(270, 36)); // move south 36 inches nav.move(new DistanceVector(180, 36)); } catch (Exception e) { e.printStackTrace(); System.exit(1); } } }

Resumen de la seccin

Con el cdigo de esta seccin, usted debera ser capaz de realizar la navegacin estimada usando Java

con una accionamiento diferencial y una brjula. Las tres clases que he creado en esta seccin fueron:

MotionVector: Marcacin en grados y la magnitud en cuestin de segundos

DistanceVector: Marcacin en grados y la magnitud en pulgadas

Navegacin: Clase que implementa la navegacin a estima para ambas DistanceVectors

y MotionVectors

Lo que usted notar como usted comienza a experimentar con este tipo de navegacin son los tipos de los

errores que usted conseguir. Experiment los siguientes tipos de errores:

Errores en los factores de conversin relativos a las tasas de la superficie y de las partidas.

Posicin precisin disminuy a medida que el nmero de movimientos aument.

El robot no evit obstculos.

Las lecturas de la brjula no fueron consistentes en diferentes lugares en el entorno de prueba.

Deslizamiento de ruedas, rampas, obstculos y caus grandes imprecisiones en la navegacin.

En la siguiente seccin, voy a discutir cmo puedo reducir algunos de los errores relativos a la posicin

inexactitud al dar al robot la capacidad de saber dnde est absolutamente en un entorno. Este proceso se

denomina localizacin.

7.2 Localizacin

La localizacin es el proceso de dar al robot la capacidad de posicionarse en su entorno. En el

ltimo captulo, el robot fue capaz de moverse en un vector, pero este vector no tena el punto

de partida, y su punto final tena grandes cantidades de error debido a factores ambientales

como el deslizamiento de las ruedas, errores en la conversin, y as sucesivamente. En esta

seccin, te voy a mostrar cmo reducir este error mediante el uso de sonar para calcular un

comienzo y una posicin final.

Para empezar, voy a establecer el entorno del robot de estar en un mundo perfecto como se

define en la ltima seccin. Esta es una de 100 pulgadas x 100 pulgadas rejilla sin obstculos.

(Ver Figura 7-9.)

Figura 7-9. Mundo perfecto

Una vez ms, para moverse con xito en esta rea, el robot tendr la siguiente informacin:

Un punto de partida (a)

Un punto final (b)

Una manera de orientarnos (encontrar un punto) (theta)

Una manera de saber hasta dnde tenemos que ir (c)

En la seccin anterior, el robot se movi en el camino de vector c con un ttulo y un tiempo (o

distancia), pero sin un punto de partida o de saber si era o no en su punto extremo. Se mueve

con respecto a la nada, que no es tan til en la navegacin. As que, cmo puede el robot

encontrar su punto de partida? Hay dos maneras: Me di cuenta de que el robot donde empieza,

o el robot podra averiguarlo por s mismo.

A decir al robot donde debe empezar, he aadido un solo mtodo esttico para la Localizacin

clase. Este mtodo tiene dos puntos y devuelve un DistanceVector. Aunque los clculos hablan

por s mismos, lo que necesitaba para convertir los ngulos desde el plano cartesiano al plano

de lecturas de la brjula. Para cartesianas, las lecturas (hacia la derecha desde arriba) son 90, 0,

270, y 180, con lecturas de la brjula de 0, 90, 180, y 270. He realizado esta conversin mediante

la comparacin de puntos con respecto a la otra y el arco tangente de la pendiente dos puntos.

Vase el ejemplo 7-11.

Ejemplo 7-11. Localization.getDistanceVector ()

// calculate vector from 2 points.

public static DistanceVector getDistanceVector(Point a, Point b) throws Exception { int d; int dx = a.x - b.x; int dy = a.y - b.y; System.out.println(a.toString()); System.out.println(b.toString()); // get distance double mag = Math.sqrt(dx * dx + dy * dy); // get angle if ((dx) == 0) { d = 90; } else { double slope = (double) (dy) / (double) (dx); d = (int) Math.toDegrees(Math.atan(slope)); } // adjust angle to coordinate system of N,E,S,W if (a.y b.x) { // if 1st point(X) is more to right d = 360 - (90 + d); } else { d = 90 - d; }

} else { if (a.x < b.x) { d = 90 - d; } else { d = 180 + (90 - d); } } return new DistanceVector(d, mag); }

A continuacin, quiero crear una estructura de datos con un nombre que poda almacenar y

recuperar en un momento posterior. No puedo hacer esto con un punto, pero puedo volver a

utilizar un punto con slo extender y dar mi nueva clase una variable llamada nombre. Llam a

esta clase NavPoint y agregu el nombre de la cadena a su constructor. Vase el ejemplo 7-12.

Ejemplo 7-12. NavPoint.java

package com.scottpreston.javarobot.chapter7; import java.awt.Point; public class NavPoint extends Point { public static final String START_POINT = "start"; public static final String EXIT_POINT = "exit"; public static final String CENTER_POINT = "center"; public String name = null; public NavPoint(String name) { super(); this.name = name; }

public NavPoint(String name, int x, int y) { super(x, y); this.name = name; } public NavPoint(String name, Point p) { super(p); this.name = name; } }

A continuacin, es el momento para que el robot para averiguar su posicin de inicio por s solo.

Cdigo Objetivo

El cdigo objetivo aqu es darle al robot la capacidad de encontrar su posicin de inicio, y luego

para navegar con estima.

Cdigo Discusin

Dado que mi robot tiene dos sonares en movimiento, uno de sonar fijo, y un entorno conocido

(un perfecto Mundial), que tendr slo para realizar dos tareas. En primer lugar, se enfrentar

al norte para que pueda alinearse con paredes de su medio ambiente. En segundo lugar, tendr

que tomar las lecturas de las paredes sur y oeste determinar mi ubicacin. Para realizar esta

tarea, voy a tener que crear una clase para mi sonar mvil llamados SonarServos.

La clase tiene tres campos de instancia, dos para mantener las posiciones de los servos y otro

para el MiniSsc. Los otros campos estticos de la clase son especficos para Feynman5 y se

obtuvieron a travs de la experimentacin.

El constructor de la clase toma un JSerialPort y se utiliza para construir la clase MiniSsc

responsable de mover el sonar. El mtodo move () toma dos argumentos: la izquierda y la

derecha. Estas posiciones primas mover el sonar. Usted puede utilizar este mtodo para calibrar

el robot para sus posiciones AFT y FORE. Los moveLeft () y moveRight () mtodos toman ngulos

en grados. Una vez ms, tengo que convertir los ngulos para que coincida con el N, E, S, W

sistema de coordenadas. As es 0 en frente del robot, es 90 a su derecha, y as sucesivamente.

El sonar de la izquierda slo tiene ngulos vlidos 180 a 360, mientras que el sonar derecho slo

tiene validez en ngulos de 0 a 180. Los mtodos tienen en cuenta la FORE y posiciones de popa

del sonar de modo que el robot se mueve al sonar izquierda o derecha para la mejor

aproximacin del ngulo de la resolucin byte posicin del servo.

En main (), los sonares se mueven hacia la parte delantera, a la parte de atrs, y al lado del robot.

esta voluntad de validar que ha configurado correctamente las constantes. Entonces la

programacion los mueve a travs de ngulos de 0 a 360. Aqu se puede observar que el robot se

mueve uno de sonar a la vez desde el sonar son cada uno slo capaz de moverse a travs de 180

grados. Vase el ejemplo 7-13.

Ejemplo 7-13. SonarServos.java

package com.scottpreston.javarobot.chapter7; import com.scottpreston.javarobot.chapter2.JSerialPort; import com.scottpreston.javarobot.chapter2.Utils; import com.scottpreston.javarobot.chapter2.WebSerialClient; import com.scottpreston.javarobot.chapter3.MiniSsc; public class SonarServos { public static final int LEFT_SONAR = 2;

public static final int RIGHT_SONAR = 3; public static final int LEFT_AFT = 60; public static final int LEFT_NEUTRAL = 150; public static final int RIGHT_NEUTRAL = 110; public static final int LEFT_FORE = 245;

public static final int RIGHT_AFT = 200; public static final int RIGHT_FORE = 20; private int leftPos = 127; private int rightPos = 127; private MiniSsc ssc; public SonarServos(JSerialPort serialPort) throws Exception { ssc = new MiniSsc(serialPort); } public void move(int left, int right) throws Exception { Utils.pause(250); // wait for servo settle ssc.move(LEFT_SONAR, left, RIGHT_SONAR, right); Utils.pause(250); // wait for servo settle } // this will be from 180 to 360 of the robot. public void moveLeft(int angle) throws Exception { if (angle > 360) { angle = angle - 360; } if (angle < 0) { angle = angle + 360; }

double thirdQuad = (LEFT_FORE - LEFT_NEUTRAL); // > 127 double fourthQuad = (LEFT_NEUTRAL - LEFT_AFT); // < 127 int pos = LEFT_NEUTRAL; if (angle < 270 && angle > 180) { angle = 270 - angle; pos = (int) ((angle / 90.0) * thirdQuad) + LEFT_NEUTRAL; } else if (angle > 270) { angle = 360 - angle; pos = LEFT_NEUTRAL - (int) ((angle / 90.0) * fourthQuad); } else if (angle < 180) { pos = LEFT_AFT; } ssc.move(LEFT_SONAR, pos); } // this will be from 0 to 180 of the robot. public void moveRight(int angle) throws Exception { if (angle > 360) { angle = angle - 360; }

if (angle < 0) { angle = angle + 360; } double firstQuad = (RIGHT_NEUTRAL - RIGHT_FORE); // < 127 double secondQuad = (RIGHT_AFT - RIGHT_NEUTRAL); // > 127 int pos = RIGHT_NEUTRAL; if (angle < 90) { pos = RIGHT_NEUTRAL - (int) ((angle / 90.0) * firstQuad); } else if (angle > 90 && angle > 180) { angle = 180 - angle; pos = (int) ((angle / 90.0) * secondQuad) + RIGHT_NEUTRAL; } else if (angle > 180) { pos = RIGHT_AFT; } ssc.move(RIGHT_SONAR, pos); } public void lookSide() throws Exception { move(LEFT_NEUTRAL, RIGHT_NEUTRAL); } public void lookFore() throws Exception {

move(LEFT_FORE, RIGHT_FORE); } public void lookAft() throws Exception { move(LEFT_AFT, RIGHT_AFT); } public static void main(String[] args) throws Exception {

try { WebSerialClient com = new WebSerialClient("10.10.10.99", "8080", "1"); SonarServos ss = new SonarServos(com); ss.lookFore(); Utils.pause(1000); ss.lookAft(); Utils.pause(1000); ss.lookSide(); // get 360 readings from sonar for (int a = 0; a < 360; a = a + 10) { ss.moveLeft(a); ss.moveRight(a); Utils.pause(1000); } com.close();

} catch (Exception e) { e.printStackTrace(); System.exit(1); } } }

Ahora que el robot puede mover su sonar, hay que mirar a los cuatro ejes de coordenadas (N, E, S y W), y

debido a que el robot se encuentra en un mundo perfecto y sabe dnde se encuentran las paredes, tan slo

necesita tomar las lecturas para el oeste y el sur, dar al robot su posicin inicial.

La clase de localizacin se extiende de navegacin. Hay un campo para la clase SonarServos y uno para

el radio de robot durante las mediciones de la posicin del robot. Esto es necesario porque los servos de

sonar son 12 pulgadas desde el centro del robot y la posicin del robot siempre sera relativa a su centro.

El siguiente mtodo, GETSTART (), cambia el ttulo del robot a norte, y luego se mueve los servos sonar a

un lado para que el robot puede obtener la lectura sonar a su izquierda (oeste). A continuacin, el servo

sonar mueve AFT (sur) y, a continuacin, toma el promedio de las dos lecturas ya que ambos son mirando

en la misma direccin.

El siguiente procedimiento describe la forma de calcular la posicin de inicio si el robot se enfrenta a algunas

direccines que no sea hacia el norte. En este caso, el programa tendr que saber cmo buscar en su

partida, y luego mover los servos sonar para su mejor aproximacin de los ejes de coordenadas N, E, S y

W.

Para determinar su partida, el mtodo llama primero a sus padres de acceso a la clase NavStamp y luego

llama getCompass (). A continuacin, las posiciones de los cuatro ejes deben basarse calculado en el rumbo

del robot. Aqu, los cuatro ejes de coordenadas se calculan restando el punto partida de los cuatro ejes

coordenados valores angulares (0, 90, 180, y 270). Por ejemplo, si el robot est orientado al este, a

continuacin, su posicin al este se encuentra en frente de ella en 0 grados. Si el robot se enfrenta sureste,

es -90 grados, o se deja en 270, y as sucesivamente. No te preocupes por los nmeros negativos de los

grados, ya que los SonarServos ajustarn esta lectura para el grado correspondiente de 0 a 360 grados

Nota: Las funciones trigonomtricas se pueden utilizar ya sea -90 o 270 para producir el nmero correcto;

sin embargo, es ms fcil de explicar cuando se habla del rango 0-360.

A continuacin, segn el rumbo del robot, tendr que mover su sonar para lacorrespondiente posicin ms

cercana. As, de 0 a 90 grados, sus mejores posiciones estn al sur y al oeste.

Pero mientras se enfrenta a de 90 a 180 grados, la posicin oeste est fuera del alcance del sonar izquierda,

y mientras podra hacer dos lecturas para el sur y oeste con el sonar bien, no es tan eficiente. As en vez

voy a tomar un norte lectura con el sonar de izquierda, y medir el oeste con la derecha. Yo Contine

alternando el sonar toma lo lecturas registrando los "bestReadings" para un rumbo dado a 180 a 270 grados,

y 270 a 360 grados.

Por ltimo, al final del procedimiento, en funcin de las mediciones efectuadas, o bien restar el norte o la

lectura al este de 100, ya que la habitacin es una cuadrcula de 100 100. Luego, al final yo ajustar las

lecturas basadas en el radio de robot.

Para este ajuste, digamos que el robot se enfrenta a 30 grados: westPos = 270-30 o 240

grados, y los southPos = 180-30 o 150 grados. El sonar izquierdo mueve en sentido contrario

un poco, mientras que el sonar derecho mueve un poco en sentido de las manesillas del reloj.

Las mejores lecturas son uno al oeste de 48 y uno al sur de 36. Debido a que el radio de la

robot es de 12 pulgadas, la medicin es x cos (30) * radio o alrededor 10 pulgadas, mientras

que la medicin Y es pecado (30) o 6 pulgadas. Las coordenadas finales son 48 + 10, 36 + 6, o

58,42.

En otro ejemplo, si el robot se voltea en, digamos, 225 grados o suroeste, las lecturas ser para

el norte y el este. Para este ngulo, cos (225) y el sin (225) = -8, por lo que las lecturas sern

restada en 8, que tiene sentido porque el centro del robot est lejos de las lecturas. ver

Ejemplo 7-14

Ejemplo 7-14. Localization.java

package com.scottpreston.javarobot.chapter7; import java.awt.Point; import com.scottpreston.javarobot.chapter2.JSerialPort; import com.scottpreston.javarobot.chapter2.Utils; import com.scottpreston.javarobot.chapter2.WebSerialClient; public class Localization extends Navigation { private SonarServos sonarServos; public static final int ROBOT_RADIUS = 12; public Localization(JSerialPort serialPort) throws Exception { super(serialPort); sonarServos = new SonarServos(serialPort); }

// calculate vector from 2 points.

public static DistanceVector getDistanceVector(Point a, Point b) throws Exception { int d; int dx = a.x - b.x; int dy = a.y - b.y; // get distance double mag = Math.sqrt(dx * dx + dy * dy); // get angle if ((dx) == 0) { d = 90; } else { double slope = (double) (dy) / (double) (dx); d = (int) Math.toDegrees(Math.atan(slope)); }

// adjust angle to coordinate system of N,E,S,W if (a.y

if (a.x > b.x) { // if 1st point(X) is more to right d = 360 - (90 + d); } else { d = 90 - d; } } else { if (a.x < b.x) { d = 90 - d; } else { d = 180 + (90 - d); } } return new DistanceVector(d, mag); }

// this uses sonarServos, add your own sensors here if needed public NavPoint getStart() throws Exception { int[] nesw = getFourCoordinates(); return new NavPoint(NavPoint.START_POINT, nesw[3], nesw[2]); } public int[] getFourCoordinates() throws Exception { // first face north. changeHeading(0); sonarServos.lookSide(); Utils.pause(500); SonarReadings sonarReadings = getNavStamp().getSonar(); int north = sonarReadings.center; int east = sonarReadings.right - ROBOT_RADIUS; int west = sonarReadings.left + ROBOT_RADIUS; sonarServos.lookAft(); Utils.pause(500); sonarReadings = getNavStamp().getSonar(); // average of two readings

int south = (int) ((sonarReadings.left + sonarReadings.right) / 2.0); return new int[] {north,east,south,west}; } // this uses sonarServos, add your own sensors here if needed public NavPoint getStart2() throws Exception { int heading = getNavStamp().getCompass(); int north = 0, south = 0, east = 0, west = 0; int eastPos = 90 - heading; int southPos = 180 - heading; int westPos = 270 - heading; int northPos = 360 - heading; SonarReadings sonarReadings = null;

int bestReadings[] = null; // order x,y if (heading >= 0 && heading < 91) { //1st quad sonarServos.moveLeft(westPos); sonarServos.moveRight(southPos); Utils.pause(500); sonarReadings = getNavStamp().getSonar(); west = sonarReadings.left; south = sonarReadings.right; bestReadings = new int[] { REL_WEST, REL_SOUTH }; } else if (heading > 90 && heading < 181) { sonarServos.moveLeft(northPos); sonarServos.moveRight(westPos); Utils.pause(500); sonarReadings = getNavStamp().getSonar(); north = sonarReadings.left; west = sonarReadings.right; bestReadings = new int[] { REL_WEST, REL_NORTH };

} else if (heading > 180 && heading < 271) { sonarServos.moveLeft(eastPos); sonarServos.moveRight(northPos); Utils.pause(500); sonarReadings = getNavStamp().getSonar(); east = sonarReadings.left; north = sonarReadings.right; bestReadings = new int[] { REL_EAST, REL_NORTH }; } else if (heading > 270 && heading < 360) { sonarServos.moveLeft(southPos); sonarServos.moveRight(eastPos); Utils.pause(500); sonarReadings = getNavStamp().getSonar(); south = sonarReadings.left; east = sonarReadings.right; bestReadings = new int[] { REL_EAST, REL_SOUTH }; }

NavPoint navPoint = new NavPoint(NavPoint.START_POINT, 0, 0); int xOffset = 0; int yOffset = 0; if (bestReadings[0] == REL_EAST) { xOffset = (int)(ROBOT_RADIUS * Math.cos(Math.toRadians(eastPos))); navPoint.x = 100 - east; } else { xOffset = (int)(ROBOT_RADIUS * Math.cos(Math.toRadians(westPos))); navPoint.x = west; } if (bestReadings[1] == REL_NORTH) { yOffset = (int)(ROBOT_RADIUS * Math.sin(Math.toRadians(northPos))); navPoint.y = 100 - north; } else { yOffset = (int)(ROBOT_RADIUS * Math.sin(Math.toRadians(southPos))); navPoint.y = south ; } navPoint.x = navPoint.x + xOffset; navPoint.y = navPoint.y + yOffset; return navPoint; }

// move from a to b public void move(Point a, Point b) throws Exception { MotionVector v = getDistanceVector(a, b); move(v); } public void move(Point b) throws Exception { move(getStart(), b); } public SonarServos getSonarServos() { return sonarServos; } public static void main(String[] args) { try { WebSerialClient sPort = new WebSerialClient("10.10.10.99", "8080", "1"); Localization local = new Localization(sPort); local.move(new Point(36, 36)); } catch (Exception e) { e.printStackTrace();

System.exit(1); } } }

Resumen de la Seccin

Con la localizacin, el robot debe ser capaz de averiguar su posicin inicial con relativa

facilidad, que proporciona el entorno no es tan ruidoso. Repitiendo este proceso al final de su

algoritmo, el robot puede determinar si est lo suficientemente cerca a su meta a moverse de

nuevo o salir.

Las clases creadas en esta seccin fueron

NavPoint: Una clase que extiende java.awt.Point pero ofrece un campo de nombre

SonarServos: Una clase que controla los servos de sonar en la parte superior del robot

Localizacin: Una clase que extiende de navegacin que ofrece para el robot para estimar su

posicin de inicio

A continuacin, es el momento para que el robot se mueva en un entorno real con obstculos

y evitarlos.

Deteccin de obstculos

En las dos ltimas secciones, hablamos de navegacin a estima y la localizacin en un entorno

conocido finito. Cmo un Robot habla sobre los obstculos en el entorno finito se traduce

directamente en la forma que trata el ambientes ruidosos y obstculos. Para empezar, voy a

clasificar obstculos en seis tipos: Esttica til, til Semi-esttico, dinmico til, intil esttico,

Useless semi-esttica, y Dinmica intil. Estn muestran en la Tabla 7-2.

Table 7-2. Clasificacin de obstculos

Tipo de obstaculo Utilidad Inutil

Esttico Las paredes, los muebles, los monumentos histricos,

aceras, csped

Lmparas de pie, elementos muy prximos entre s,

rboles, arroyos, baches

Semi-Estatico Las latas de bebidas, juguetes?

Juguetes en el piso, cuadros, csped

aspersores

Dinmico Rostros, las manos, los cachorros

persona caminando, juguete o caja en el suelo, rbol

Los artculos tiles son las cosas que nos ayudan en la navegacin. Elementos estticos como

paredes, monumentos, aceras son todos los elementos que no se mueven, pero se pueden

utilizar para darnos un punto de inicio, punto de paso, o punto final. Los artculos semi-esttica

no ayudan a la navegacin de robots porque a pesar de que no lo hacen se mueven, no son

siempre en el mismo lugar. Sin embargo, podran ser objetivos para un robot (por ejemplo, la

limpieza de la planta de artculos antes de la aspiradora o conseguir latas de la nevera). Los

finales no ayudan a la navegacin del robot, pero como obstculos semi-estticas que pueden

ser extremo puntos u objetivos para un robot (por ejemplo, a raz de las seales de mano, o

despus de una persona especfica en una habitacin llena de gente).

Los artculos intiles son las cosas que crean ruido para el robot. El ruido puede causar grandes

o pequeos errores en la forma en que el robot determina sus rutas o si se le pega nada.

Elementos estticos que son intiles son las cosas que son muy difciles para que el robot

detecta con sus sensores. Estos son obstculos que puede golpear y que podra daar el robot

(o el obstculo, tambin). Obstculos semi-estticas son artculos que realmente no se

preocupan por, pero no se mueven y tienen que ser ajustados durante la navegacin.

Por ltimo, los obstculos dinmicos son aquellos que normalmente hacen salir del camino del

robot si se les da tiempo suficiente, por lo que en este caso, el robot slo tiene que ser

paciente y luego reanudar su movimiento. Actualmente, slo he hablado de un tipo de

obstculo: la, clase til esttica. Ahora voy a le mostrar cmo crear una clase para manejar el

tipo dinmico semi-esttica e intil intil de obstculos mientras que nuestro robot est

navegando. En la figura 7-10, la ruta original del robot era del punto a al punto b. Sin embargo,

en La figura 7-10 se encuentra un obstculo en la trayectoria entre a y b. As que para que el

robot para llegar a su objetivo (punto b), se debe calcular una ruta alternativa alrededor del

obstculo.

Para calcular el punto D, toma un punto perpendicular a la derecha o a la izquierda del

obstculo. para elegir derecha o izquierda depende de qu lado tiene la mayora del espacio

en frente del robot. Si se trata de la derecha, el ngulo ser el epgrafe 45 grados, mientras que

el camino a la izquierda del ngulo ser el rbrica -45 grados.

Objetivos del cdigo

El objetivo aqu es dar al robot la capacidad de navegar alrededor de obstculos.

Discusin del cdigo

La clase ObstacleNavigation se basa en la clase de localizacin del ltimo captulo. Tiene una

sola variable, offsetDistance, que siempre ser el dimetro del robot lejos de la ruta original. El

constructor pasa JSerialPort a la localizacin de la clase padre, y entonces cualquier referencias

necesarias para la SonarServos, NavStamp y clases SpeedDiffDrive pueden utilizarse de

mtodos de acceso de los padres.

El nico mtodo pblico en esta clase es move (), que reemplaza el mtodo move () en el Clase

de navegacin. La primera cosa que hago en este mtodo es ingrese la MotionVector enviado

al mtodo. Esto fue til para m durante la depuracin, y tambin es bueno tener un registro

de los movimientos del robot. A continuacin, llama changeHeading () al igual que su padre. En

segundo lugar, en el caso de una magnitud negativa, una excepcin se produce porque los

sensores estn configurados para la deteccin adelante. Si tuviera ms sensores, que se

ajustaran esto para cualquier direccin, hacia adelante o hacia atrs.

La siguiente variable en el mtodo, totalMs, sostiene el clculo de la distancia total en

milisegundos para todo el movimiento. Esto es necesario de modo que si el robot encuentra

un obstculo que se puede calcular dnde se ha encontrado el obstculo, permitiendo que los

vectores de derivacin para determinar la posicin de las coordenadas originales. En la figura

7-10, los totalMs representar el longitud de la lnea c.

La siguiente variable es offsetTime. Este calcula el tiempo sobre la base de la tasa de superficie

y la offsetDistance actual. Para el robot actual, esto es 34 pulgadas dividido por 14 pulgadas

por segundo, que se traduce en unos 2.400 milisegundos.

El primer clculo que necesita ser hecho es el nmero de veces que los sensores pueden tomar

lecturas durante el rango total de movimiento. Voy a llamar a estas variables barridos. Al

conocer el nmero de barridos calculados, el nmero de llamadas de intervalo se puede hacer

que los mtodos isObstacleFwd (). si un obstculo no en el camino del robot, se mantiene el

recuento de intervalo. El uso de este nmero, el tiempo restante se puede calcular en el

MotionVector actual. La variable utilizada para calcular el tiempo restante es remainingMS,

que es el recuento multiplicado por el tiempo de ciclo de los sensores. El bit final de la lgica

en este mtodo es la comprobacin para ver si se encuentra un obstculo. Si se era, entonces

me detengo el robot por un segundo antes de pasar a la inversa durante 1 segundo. esto borra

la trayectoria del robot para que pueda girar y avanzar por el camino del vector de derivacin

sin golpear el obstculo. A continuacin, lo que necesito para aumentar los remainingMS

totales en el segundo se traslad a la inversa. Por ltimo, antes de llamar moveBypass (), reviso

para ver si los remainingMS es mayor que la offsetTime. Esto se debe a que todava es posible

que el robot es lo suficientemente cerca de su posicin deseada que cualquier derivacin no lo

conseguira ms cerca de su objetivo que si simplemente se qued donde estaba. Por ltimo, si

no se detecta obstculo, el robot slo se mueve hacia adelante para los milisegundos

restantes.

El primero de los dos mtodos privados es ObsicalForward (), que determina si hay una

obstculo en el camino del robot. Una vez ms, desea ajustar los valores de este mtodo para

su la configuracin del robot. Aqu tengo todas las lecturas de sonda del NavStaqmp, y en

funcin de las lecturas, me devuelven un verdadero obstculo para una falsa y sin obstculo. El

segundo mtodo, moveBypass (), crea dos vectores de derivacin. Estos son los vectores E y F

en la figura 7-10. Para calcular ellos, primero el robot necesita determinar la direccin de la

obstculo. Para calcular la direccin, he usado un enfoque de probabilidad para cada una de

las lecturas.

Las primeras mediciones son los detectores de infrarrojos. Si el lado izquierdo es mayor que el

derecho, I aumentar la probabilidad de la izquierda de que tiene un obstculo ms estrechas

(medios superiores ms estrechas para el infrarrojo sensores). Si la lectura de sonar a la

izquierda es ms pequea, puedo aumentar la probabilidad de la izquierda de un obstculo

porque no hay ms espacio a la derecha.

En la siguiente serie de clculos, lo que necesito para determinar la longitud final del vector f.

Este es el segundo vector en el recorrido de derivacin. Como s que la longitud del primer

vector, voy a tener que calcular el segundo vector de modo que el robot se mover a su punto

objetivo original. El clculo primero mira a la distancia el robot ha recorrido paralelo a su

rumbo original. Esta es la clase de servicio de el ngulo multiplicado por el offsetDistance.

Ahora puedo calcular la distancia que queda en el direccin del rumbo original restando el

tiempo restante de este valor.

Finalmente, el robot tiene una nueva distancia que tiene que viajar tal que el ngulo que se

calcula por el tringulo derecho de uno de los lados es la distancia restante en el camino

original, y el segundo lado es la distancia recorrida a partir de la ruta original. Puedo calcular el

ngulo al tomar el arco tangente de estas dos relaciones, y luego puedo calcular la distancia al

elevar al cuadrado los lados, aadiendo ellos y, a continuacin, tomando la raz cuadrada.

Ahora el robot va a llamar de movimiento () con dos nuevos ttulos. Debido a esta recursin, es

tericamente posible que el robot se mueva en un crculo hasta que se evita el obstculo (s).

Esto se maneja mejor con la cartografa (que ser discutido en la prxima seccin), por lo que

una excepcin se lanza si se requiere ms de un bypass. Vase el ejemplo 7-15.

Ejemplo 7-15. ObstacleNavigation.java

package com.scottpreston.javarobot.chapter7; import com.scottpreston.javarobot.chapter2.JSerialPort; import com.scottpreston.javarobot.chapter2.Utils; import com.scottpreston.javarobot.chapter2.WebSerialClient; public class ObstacleNavigation extends Localization { private double offsetDistance = 0; private boolean inBypass = false; public ObstacleNavigation(JSerialPort serialPort) throws Exception { super(serialPort); offsetDistance = Math.sin(Math.toRadians(45)) * ROBOT_RADIUS * 2; } public void move(MotionVector vect) throws Exception { Utils.log("MV=" + vect.toString()); if (vect.magnitude < 0) { throw new Exception("Only avoids obstacles in forward direction"); } changeHeading(vect.heading);

// get total time in MS for motion (vector length) int totalMS = (int) Math.abs(vect.magnitude) * 1000; int offsetTime = (int) getSurfaceRate(offsetDistance) * 1000; // this will be minimum bypass distance // get number of sonar scans for range of motion int sweeps = (int) (totalMS / NavStamp.PING_CYCLE_TIME); // this will start motion getSonarServos().lookFore(); Utils.pause(2000); // time to move sonar getDrive().forward(); int count = 0; boolean Obstacle = false; while (count < sweeps) { // moves until it hits something or is done. if (isObstacleFwd()) { Utils.log("***fwd Obstacle***"); getDrive().stop(); Obstacle = true; break; } count++; }

getDrive().stop(); // get remaining time in vector int remainingMS = totalMS - (count * NavStamp.PING_CYCLE_TIME); if (Obstacle) {

if (inBypass) { throw new Exception("Already in bypass find another route."); } Utils.pause(1000); // so not rough change of direction moveRaw(RAW_REV, 1000); remainingMS = remainingMS + 1000; // since both an Obstacle and it can be bypassed if (remainingMS > offsetTime) { inBypass = true; moveBypass(new MotionVector(vect.heading, remainingMS), offsetTime); inBypass = false; } } else { // since can't detect this distance anyway getDrive().forward(remainingMS); } }

private void moveBypass(MotionVector remainingVect, int offsetTime) throws Exception {

// since readings in milliseconds remainingVect.magnitude = remainingVect.magnitude; DistanceReadings readings = getNavStamp().getSonarIR(); // to move around obstacle to the left or to the right int newHeading = remainingVect.heading; double sq2 = (Math.sqrt(2) / 2.0); double leftProb = 0; double rightProb = 0; // ir is more important use this first // ir high means close, low means far if (readings.ir.left - 20 > readings.ir.right) { // since something closer on left, then turn right leftProb = leftProb + 0.15; // if so close turning will cause hit if (readings.ir.left > 100) leftProb = leftProb + 0.1; } else {

rightProb = rightProb + 0.15; // if so close not turning will cause hit if (readings.ir.right > 120) rightProb = rightProb + 0.1; } // checking sonar if left < right more room to right so turn right by // increasing prob. if (readings.sonar.left < readings.sonar.right) { leftProb = leftProb + 0.1; // if close if (readings.sonar.left < 24) leftProb = leftProb + 0.1; // if so close not turning will cause hit if (readings.sonar.left < 12) leftProb = leftProb + 0.1; } else { rightProb = rightProb + 0.1; if (readings.sonar.right < 24) rightProb = rightProb + 0.1; if (readings.sonar.right < 12) rightProb = rightProb + 0.1; } int headingOne = 0; int headingTwo = 0; // int offset distance double offsetAdjacent = Math.cos(Math.toRadians(45)) * offsetDistance; double offsetOpposite = Math.sin(Math.toRadians(45)) * offsetDistance;

// remaining time for original heading

double remainingTime = remainingVect.magnitude - offsetAdjacent;

int finalAngle = (int) Math.toDegrees(Math.atan(offsetOpposite / remainingTime));

double finalMagnitude = Math.sqrt(offsetAdjacent * offsetAdjacent + remainingTime * remainingTime); Utils.log("Obstacle prob=" + rightProb + "," + leftProb); if (rightProb < leftProb) { // turn right headingOne = newHeading + 45; headingTwo = newHeading - finalAngle; } else { headingOne = newHeading - 45; headingTwo = newHeading + finalAngle; } MotionVector bypassOne = new DistanceVector(headingOne, offsetTime); move(bypassOne); MotionVector bypassTwo = new MotionVector(headingTwo, finalMagnitude); move(bypassTwo); }

} private boolean isObstacleFwd() throws Exception { DistanceReadings dist = getNavStamp().getSonarIR(); if (dist.ir.left > 100 || dist.ir.right > 120 || dist.sonar.left < 12 || dist.sonar.center < 12 || dist.sonar.right < 12) { return true; } else { return false; } } public static void main(String[] args) { try { WebSerialClient com = new WebSerialClient("10.10.10.99", "8080", "1"); ObstacleNavigation nav = new ObstacleNavigation(com); // in seconds MotionVector[] v = new MotionVector[] { new MotionVector(90, 10) }; nav.move(v); } catch (Exception e) { e.printStackTrace(); System.exit(1); } } }

Resumen de Seccin

Detectar los obstculos pueden ser tan sofisticado como sean sus sensores. He demostrado

una forma usando a travs de infrarrojos y el sonar, pero se puede usar otros mtodos como

su presupuesto lo permite. La clase creada en esta seccin fue ObstacleNavigation y logra esto

mediante la construccin de un camino alrededor de un obstculo.

Actualmente, el algoritmo slo funciona bien para un solo obstculo, o si tiene cosas

temporalmente en el camino de su robot mientras se mueve. Si cree que su robot se

encontrar con mltiples obstculos, que pueden ahorrarle tiempo para construir un mapa

con una ruta en la que el robot no tiene que tratar con ms de un objeto a la vez. Para ello,

tendremos que usar un poco de Teora de Grafos crear un mapa de software de nuestro medio

ambiente.

7.4 Navegacin interior

Hasta ahora, toda nuestra navegacin ha estado en el mundo perfecto de una pulgada 100

cuadrcula de 100 pulgadas. Las habitaciones en una casa o una oficina no encajan muy bien en

este entorno porque hay cosas como mesas, pasillos, puertas, y as sucesivamente. Pero

cuando comenc a experimentar con la navegacin, lo encontr ms fcil para unirse a unos

ambientes idealizados juntos (como la red de 100 100) en un grfico de era para modelar

una habitacin entera con todas sus peculiaridades.

Por ejemplo, utilizando el grfico de la introduccin, puedo crear cuatro vrtices que cada

representar a 100 100 regiones. Si me mudo de A a B, no necesito saber nada de C y D. Del

mismo modo, si me mudo de C a D, no necesito saber nada de A, aunque poda mover a travs

de B si quera tomar un camino ms largo. (Ver Figura 7-11.)

La pregunta es cmo puedo modelar esto en Java? Anteriormente he creado dos clases

llamadas Vertex y Edge. Ahora voy a extender la clase Vertex para crear un objeto que modela

el mundo perfecto en una rejilla de 100 100. Voy a llamar a esta clase de una regin.

Una regin tiene cuatro campos. El primero se hereda de Vertex y se llaman as, mientras que el

segundo es un tamao int que da la distancia desde el punto ms exterior de la regin de su

centro. El tercero es un ArrayList, que almacena NavPoints denotan lugares especficos en la

regin a la que el robot puede ser que desee navegar. El cuarto campo es un [] llamado

caracterstico int. este campo representa los cuatro ejes de coordenadas, ya que se mediran en

esta regin. la matriz {1,0,0,1} representara lecturas en el norte y en direccin oeste, pero no

mucho tiempo o fuera de rango lecturas en el este y el sur.

La regin Tiene Un constructor de nombre de la ONU y la ONU Tamao. En el constructor, aado

Also El Centro waypoint ya Que se necesitar Ms tarde prr la navegacin.

Fuera de mtodos getter y descriptores de acceso estndar, tengo tres mtodos, uno que har

que el punto de la regin por su nombre, y otros dos mtodos que hacen traducciones

puntuales. el mtodo getScaledPoint () convierte puntos tomados en esta regin, por ejemplo,

de un sonar a un punto de escalado en el regin dentro de los lmites de 100 100. La

getScaledMagnitude mtodo () convierte la final vector de nuevo la distancia en pulgadas reales

para cualquier movimiento. Vase el ejemplo 7-16.

Example 7-16. Region.java

package com.scottpreston.javarobot.chapter7; import java.awt.Point; import java.util.ArrayList; public class Region extends Vertex{ //list of way points in center private ArrayList wayPoints = new ArrayList(); // start point in region absolute coordinates private int size = 0; // used to determine position within region N,E,S,W readings private int[] characteristic = new int[]{0,0,0,0}; // constructor public Region(String name, int size) { super(name); this.size = size; // just add center point for later use. addWayPoint(NavPoint.CENTER_POINT,50,50); } // navigation points public void addWayPoint(NavPoint p) { wayPoints.add(p); } public void addWayPoint(String name,int x, int y) { addWayPoint(new NavPoint(name,x,y)); }

// get scaled start point // output will be percentage from measured public Point getScaledPoint(int x,int y) { double totalSize = size * 2; int x2 = (int)(x/totalSize*100); int y2 = (int)(y/totalSize*100); return new Point(x2,y2); } // returns in actual inches public double getScaledMagnitude(double m) { double scale = size * 2 / 100.0; return m*scale; } //get points by name public NavPoint getPointByName(String name) { NavPoint pt = null; for (int x=0; x

}

Mientras se mueve dentro de las regiones no es ningn problema, ya que hemos estado

haciendo eso, est navegando de regin a regin dentro de la misma habitacin que no hemos

hecho. Para ayudarme con eso, Voy a crear un nuevo objeto llamado, simplemente lo suficiente,

Sala.

Una habitacin se conectar y orientar todas las regiones en que respecto a la otra. Voy a hacer

esto conectndolos con bordes. Esto es conveniente puesto que ya tenemos bordes como

DistanceVectors. Siempre voy a construir los vectores de distancia por lo que se mide desde el

centro de cada regin punto. Para ilustrar la creacin de la regin de mapa en un entorno real

(mi stano), ver Figura 7-12.

El siguiente paso es crear una estructura de datos que se puede representar esto, llama de

habitaciones. la Sala clase, como la clase regin, tiene regiones como vrtices y aristas, que sern

DistanceVectors. He aadido una sala de la muestra en el mtodo esttico llamado getBasement

(), que define el habitacin en la figura anterior. Vase el ejemplo 7-17.

Ejemplo 7-17. Room.java

package com.scottpreston.javarobot.chapter7; import java.util.ArrayList; public class Room extends Vertex { private ArrayList regions = new ArrayList(); private ArrayList edges = new ArrayList(); public Room(String name) { super(name); } public void addRegion(Region r) { regions.add(r); }

public void addEdge(Region r1, Region r2, DistanceVector vect) { vect.v1 = r1; vect.v2 = r2; edges.add(vect); }

public static Room getBasement() { // 1st create regions Region a = new Region("home",36); a.setCharacteristic(new int[]{0,0,1,1}); // add specific location of the trash can Region b = new Region("trash",36); b.setCharacteristic(new int[]{1,0,0,1}); b.addWayPoint("can",80,20); Region c = new Region("desk",24); c.setCharacteristic(new int[]{1,1,0,0}); Region d = new Region("exit",24); d.setCharacteristic(new int[]{0,1,0,1}); Region e = new Region("treadmill",48); c.setCharacteristic(new int[]{0,1,1,0}); Region f = new Region("fridge",36); c.setCharacteristic(new int[]{1,0,0,0}); Region g = new Region("sofa",24); c.setCharacteristic(new int[]{0,0,0,1});

// create room by linking regions Room basement = new Room("shop"); basement.addEdge(a,b,new DistanceVector(190,260)); basement.addEdge(b,d,new DistanceVector(290,288)); basement.addEdge(b,c,new DistanceVector(260,216)); basement.addEdge(c,d,new DistanceVector(315,60)); basement.addEdge(d,e,new DistanceVector(280,72)); basement.addEdge(e,f,new DistanceVector(345,260)); basement.addEdge(e,g,new DistanceVector(325,200)); basement.addEdge(g,f,new DistanceVector(210,72)); return basement; } public ArrayList getRegions() { return regions; } public ArrayList getEdges() { return edges; } public void setEdges(ArrayList edges) { this.edges = edges; } }

El grfico que necesito para navegar esta casi completa. Lo nico que falta es un algoritmo que

le dice al robot el camino ms corto para tomar desde un vrtice a otro. el algoritmo Voy a

utilizar para que se llama Algoritmo de Dijkstra (Edsger Dijkstra nombrado despus), que

determina el camino ms corto para un grafo ponderado dirigido. Para ilustrar este ejemplo, voy

a tomar el siguiente grfico de la parte derecha del stano. Pero dar los pesos a la grfica que

corresponde a la distancia entre ellos.

AB = 260 BD = 288 BC = 216 CD = 60

A continuacin, en lugar de imaginar un robot se mueve entre estos puntos, digamos que

estamos utilizando tuberas y el agua en su lugar. El agua sera correr en una lnea, con una

velocidad constante. Ahora, vamos a poner vlvulas especiales en cada uno de los vrtices B, C,

y D-tal que si entra agua hay primero de cualquier tubera de entrada, se cierra la vlvula para

todos los otros tubos que entra en el la vlvula. La vlvula se pone una bandera diciendo que

este vrtice es el camino ms corto. Si usted puede imaginar el flujo de agua, se puede encontrar

la distancia ms corta, en este caso los vrtices ser de A a B a C a D como la suma de BD = 288,

que es mayor que BC + CD = 276. Ver Figura 7-13.

Si usted quiere entender el funcionamiento interno del algoritmo ms profundamente que la

analoga de plomera, recoger el libro Una Disciplina de programacin por el propio Dijkstra o

cheque mi pgina de referencia en l a www.scottsbots.com/definitiveguide. Vase el ejemplo

7-18.

Ejemplo 7-18. Dijkstra.java

package com.scottpreston.javarobot.chapter7; import java.util.ArrayList; import java.util.Collections; import java.util.HashMap; import java.util.HashSet; import java.util.Iterator; public class Dijkstra { private ArrayList vertices = new ArrayList(); private ArrayList edges = new ArrayList(); private HashMap oldVertex = new HashMap(); private HashMap distances = new HashMap(); private HashSet unsettled = new HashSet(); private HashSet settled = new HashSet(); public void addEdge(Edge e) { edges.add(e); } public void addAllEdges(ArrayList e) { edges = e; }

public void addVertex(Vertex v) { vertices.add(v); } public void addAllVertices(ArrayList v) { vertices = v; } public int getDist(Vertex start, Vertex end) { int[][] adj = getAdj();

int size = vertices.size(); int w = 0; for (int i = 0; i < size; i++) { Vertex vi = (Vertex) vertices.get(i); for (int j = 0; j < size; j++) { Vertex vj = (Vertex) vertices.get(j); if (vi.equals(start) && vj.equals(end)) { w = adj[i][j]; } } } return w; } public void setShortDistance(Vertex v, int dist) { unsettled.remove(v); distances.put(v, new Integer(dist)); unsettled.add(v); }

public void setPred(Vertex a, Vertex b ){ oldVertex.put(a,b); } public Vertex getPred(Vertex a) { return (Vertex)oldVertex.get(a); } public int getShortDistance(Vertex v) { Integer d = (Integer) distances.get(v); if (d == null) { return Integer.MAX_VALUE; } else { return d.intValue(); } }

public Vertex extractMinimum() { Iterator i = unsettled.iterator(); int min = Integer.MAX_VALUE; Vertex minV = null; while (i.hasNext()) { Vertex tmp = (Vertex) i.next(); if (getShortDistance(tmp) < min) { min = getShortDistance(tmp); minV = tmp; } } unsettled.remove(minV); return minV; } public void relaxNeighbors(Vertex u) { int[][] adj = getAdj(); int size = vertices.size(); for (int i = 0; i < size; i++) { Vertex vi = (Vertex) vertices.get(i); if (vi.equals(u)) { // only check this i'th column for (int j = 0; j < size; j++) { Vertex v = (Vertex) vertices.get(j); int w2 = adj[i][j]; // should give all adjacent vertices not settled if (w2 > 0 && w2 < Integer.MAX_VALUE && (settled.contains(v) == false)) { // does a shorter distance exist? if (getShortDistance(v) > getShortDistance(u) + getDist(u, v)) { int d = getShortDistance(u) + getDist(u, v); setShortDistance(v, d); setPred(v,u); }

} } } } } public ArrayList getShortestPath( Vertex start, Vertex end) { unsettled.add(start); setShortDistance(start,0); while (unsettled.size() > 0) {

Vertex u = extractMinimum(); // gets shortest Vertext settled.add(u); relaxNeighbors(u); } ArrayList l = new ArrayList(); for (Vertex v = end; v != null; v = getPred(v)) { l.add(v); } Collections.reverse(l); System.out.println("--- PRINT ORDER ---"); for (int d=0;d < l.size();d++) { Vertex v = (Vertex) l.get(d); System.out.println(v.name); } return l; } public Vertex getVertexByName(String n) { int size = vertices.size(); for (int i = 0; i < size; i++) { Vertex vi = (Vertex) vertices.get(i); if (vi.name.equals(n)) { return vi; } } return null; }

private int[][] getAdj() { int[][] adjMatrix = new int[vertices.size()][vertices.size()]; // init all large for (int i = 0; i < vertices.size(); i++) { for (int j = 0; j < vertices.size(); j++) { adjMatrix[i][j] = Integer.MAX_VALUE; } } // set to actual values to zero for (int i = 0; i < vertices.size(); i++) { Vertex vi = (Vertex) vertices.get(i); for (int j = 0; j < vertices.size(); j++) { Vertex vj = (Vertex) vertices.get(j); if (i == j) { adjMatrix[i][j] = 0;

} else { for (int k = 0; k < edges.size(); k++) { Edge e = (Edge) edges.get(k); if (e.v1.equals(vi) && e.v2.equals(vj)) adjMatrix[i][j] = e.weight; if (e.v2.equals(vi) && e.v1.equals(vj)) adjMatrix[i][j] = e.weight; } } } } return adjMatrix; } public static void main(String[] args) { Dijkstra dijkstra = new Dijkstra();

Vertex a = new Vertex("a"); dijkstra.addVertex(a); Vertex b = new Vertex("b"); dijkstra.addVertex(b); Vertex c = new Vertex("c"); dijkstra.addVertex(c); Vertex d = new Vertex("d"); dijkstra.addVertex(d); dijkstra.addEdge(new Edge(a, d, 2)); dijkstra.addEdge(new Edge(a, b, 2)); dijkstra.addEdge(new Edge(a, c, 4)); dijkstra.addEdge(new Edge(b, c, 1)); dijkstra.getShortestPath(d,c); //System.out.println(d.adjToString(d.getAdj())); }

/** * @return Returns the vertices. */ public ArrayList getVertices() { return vertices; } /** * @param vertices The vertices to set. */ public void setVertices(ArrayList vertices) { this.vertices = vertices; }

/** * @return Returns the edges. */ public ArrayList getEdges() { return edges; } /** * @param edges The edges to set. */ public void setEdges(ArrayList edges) { this.edges = edges; }

}Objetivo del codigo

El objetivo cdigo aqu es darle al robot la capacidad de navegar en el interior, y lo ms importante a la

nevera.

Discusin del codigo

La clase IndoorNavigation extiende ObstacleNavigation porque yo no quiero que mi robot de bateo nada en

su camino hacia el refrigerador. Cuenta con dos campos de tipo de habitaciones y regin donde el regin

ser la regin actual y la sala ser la sala pasado por el constructor. El mtodo move () en este algoritmo

toma un solo extremo parmetro String, que ser el nombre de la final NavPoint. El mtodo en s consta de

tres partes: en primer lugar, para obtener la corriente regin y el punto a travs de la localizacin de

comenzar; segundo, para pasar de su regin actual a su fin regin a travs de la ruta ms corta determinada

por el algoritmo de Dijkstra; tercero, para pasar a la deseada punto final en la regin de extremo.

Obtencin de la posicin de inicio se realiza mediante el mtodo getBestRegion (). Este mtodo obtiene

cuatro ejes de coordenadas de los getFourCoordinates () mtodo de localizacin. Estas cuatro coordenadas

se miden contra los tamaos y caractersticas de todas las regiones para producir un voto. la regin con el

mayor nmero de votos ser entonces la mejor regin, y de ah la posicin de inicio ser obtenidas de las

mejores lecturas de los cuatro ejes: N, E, S y W. Ver Ejemplo 7-19.

Ejemplo 7-19. IndoorNavigation.java

package com.scottpreston.javarobot.chapter7; import java.util.ArrayList; import com.scottpreston.javarobot.chapter2.JSerialPort;

import com.scottpreston.javarobot.chapter2.WebSerialClient; public class IndoorNavigation extends ObstacleNavigation { private Room currentRoom; private Region currentRegion; public IndoorNavigation(JSerialPort serialPort, Room room) throws Exception { super(serialPort); currentRoom = room; } public void move(String end) throws Exception{ ArrayList path = new ArrayList(); getBestRegion(); NavPoint start = currentRegion.getPointByName(NavPoint.START_POINT); NavPoint startCenter = currentRegion.getPointByName(NavPoint.CENTER_POINT); // start vector will be in virtual points 100x100 DistanceVector startVector = getDistanceVector(start,startCenter); // convert from 100x100 to scaled version

startVector.magnitude = currentRegion.getScaledMagnitude (startVector.magnitude); path.add(startVector); // middle vectors ArrayList regions = currentRoom.getRegions(); Region endRegion = null; NavPoint endPoint = null; for (int r=0;r

} if (vote > maxVote) { bestRegion = tmpRegion; } } int [] bestChar = bestRegion.getCharacteristic(); int x=0,y=0; if (bestChar[2] == 1) { y=nesw[2]; }

} if (bestChar[0] == 1 && bestChar[2] == 0) { y= 100 - nesw[0]; } if (bestChar[3] == 1) { y=nesw[3]; } if (bestChar[1] == 1 && bestChar[3] == 0) { y= 100 - nesw[1]; }

bestRegion.addWayPoint(new NavPoint(NavPoint.START_POINT, bestRegion.getScaledPoint(x,y))); currentRegion = bestRegion; }

public static void main(String[] args) { Room basement = Room.getBasement(); try { WebSerialClient sPort = new WebSerialClient("10.10.10.99", "8080", "1"); IndoorNavigation nav = new IndoorNavigation(sPort,basement); nav.move("fridge"); } catch (Exception e) { e.printStackTrace(); System.exit(1); } } }

Resumen de la Seccion

En esta seccin, he mostrado que mediante la adicin de un poco de estructura a la que desea que su robot

los viajes, el robot puede desplazarse a cualquier lugar que lo necesite para el interior. Las clases creadas

en este seccin fueron

Regin: sistema de una red de 100 100 La coordenada idealizada

Sala: Una sala que consta de muchas regiones conectadas por DistanceVectors

Dijkstra: Un algoritmo para encontrar el camino ms corto a travs de un grafo ponderado entre vrtices

IndoorNavigation: Un programa de navegacin utilizando las regiones y habitaciones para trasladarse a

cualquier punto de llamada.

Desde mi robot puede ahora moverse en el interior bastante bien, pens que el tiempo lo enseo a mover

al aire libre. Aunque no he perfeccionado cortar el csped, recoger el correo, o pasear al perro, He sido

capaz de conseguir que el robot para mover cerca del buzn a travs de GPS. Y eso es lo que soy vamos

a hablar a continuacin.

7.5 Navegacion al aire libre

Qu es el GPS? Sistema de Posicionamiento Global (GPS) es un sistema de navegacin por satlite

formado por 24 satlites que viajan en rbitas muy precisas. Mientras GPS fue pensado originalmente para

su uso por los militares, se abri para uso civil en la dcada de 1980.

GPS triangula las seales de estos satlites mediante la medicin de la diferencia de tiempo entre cuando

se transmiten las seales y cuando se recibieron. Es la diferencia que permite un dispositivo GPS para fijar

una posicin en alguna coordenada en la tierra. Un receptor GPS debe bloquear a al menos tres satlites

para obtener una posicin de 2-D de longitud (N / S) y latitud (E / W), y s