-
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