Este documento describe un algoritmo para la planificación de trayectorias de un robot llamado Bug 2. El algoritmo genera una línea recta entre los puntos de inicio y objetivo, y el robot la seguirá salvo que encuentre un obstáculo, en cuyo caso lo rodeará hasta volver a la línea. Se proporcionan detalles sobre la implementación en Player/Stage, incluyendo los archivos de configuración y mundo necesarios. El código calcula la pendiente y ángulo de la línea recta, y usa sensores sonar y bucles para moverse, rodear obst
2. Bug 2
Método Start – Goal
Desconocemos los obstáculos
existentes en el entorno
Basado en 2 comportamientos
fundamentales:
Ir al Goal
Rodear obstáculo
3. Algoritmo
Generar una línea entre los
puntos Start y Goal
El robot seguirá esa
trayectoria salvo que
encuentre un obstáculo
En este caso rodeará el
obstáculo hasta que
encuentre un punto
perteneciente a la línea
En ese momento dejará de
rodear el obstáculo y
continuará hacia el Goal
4. Player / Stage
Los ficheros necesarios para la
implementación y prueba de este algoritmo
son:
Simple.world Definición del mundo por
donde nos vamos a mover (obstáculos, punto
de salida, tamaños etc..)
bug2.cfg Configuración donde se define el
fichero de world y los drivers disponibles en los
robots
Pioneer.inc Definición de los componentes
del robot.
5. bug2.cfg
Configuración:
# Plugin de simulacion para Stage
driver
(
name "stage"
provides ["simulation:0" ]
plugin "libstageplugin"
# Cargamos el mundo
worldfile "simple.world"
)
#definimos driver de sonar
driver
(
name "stage"
provides ["position2d:0" "sonar:0" ]
model "robot1"
)
6. simple.world
Se definen variables del mapa como tamaño, resolución, refresco o tamaño
de la ventana GUI
También se describen en este fichero el mapa a utilizar y los robots que
intervienen.
# defines Pioneer-like robots
include "pioneer.inc"
# defines 'map' object used for floorplans
include "map.inc"
# load an environment bitmap
map
(
bitmap "bitmaps/cave.png"
size [16 16]
name "cave"
)
# create a robot
pioneer2dx
(
name "robot1"
color "blue"
pose [-5 -5 0]
)
7. Pioneer.inc
Configuración de un robot Pioneer con
sonar.
Definición de parámetros propios del robot
como tamaño del robot, peso, forma, tipo
de movimiento
8. Ejecución
Para iniciar la
ejecución:
robot-player bug2.cfg
Como veíamos en el
fichero de configuración
del mundo se indica la
posición de salida del
robot (al crear el robot):
# create a robot
pioneer2dx
(
name "robot1"
color "blue"
pose [-5 -5 0]
)
9. Ejecución
Para comenzar con el
algoritmo basta con
ejecutar la clase java que
implementa el Bug 2.
Java –jar Bug2.jar
Cuando ejecutamos el
código vemos que los
sonar se activan.
En nuestro caso, la
posición Goal se
encuentra en la
coordenada 7,1
10. Ejecución
El robot debería
seguir la línea roja
hasta encontrarse
con un obstáculo.
Cuando lo encuentre
deberá rodearlo
hasta que se vuelva
a encontrar en la
línea al Goal.
En esta practica, se
ha definido que los
obstáculos se rodeen
por la derecha.
12. Resultado
El resultado de
la ejecución
completa es la
mostrada en la
imagen.
13.
14. Código fuente del algoritmo
//Calculamos la pendiente entre el punto de origen definido en el plano y el punto final
//(y-y0)/(x-xo) = m
pendiente = (goalY-startY)/ (goalX-startX);
// Para el calculo del angulo usamos trigonometria (arco-tangente)
//angulo = (float) Math.atan(pendiente); Inicializaciones
angulo = (float)Math.toDegrees(Math.atan(Math.abs(pendiente)));
//Enfocamos al robot hacia el objetivo
de variables y
apuntar(posi);
//Cuando estamos enfocados iniciamos el proceso
calculo de la
boolean rodear = false;
boolean fin = false;
state = 0;
recta, Angulo y
while (!fin) {
// obtenemos los datos de los sonar
pendiente.
getSonars (soni);
// salvo algo lo impida nos moveremos de frente
xSpeed = DEF_X_SPEED;
yawSpeed = 0;
15. Código fuente del algoritmo
while (!posi.isDataReady ());
float posRealx = posi.getX()+startX;
while (!posi.isDataReady ());
float posRealy = posi.getY()+startY;
// si detecto un obstaculo -> rodear igual a true
if (frontSide < MAX_WALL_THRESHOLD) {
rodear = true;
}
boolean enLinea = enLaLinea(posRealx,posRealy);
Core del
if (enLinea && !rodear){
posi.setSpeed(xSpeed,yawSpeed);
if ( posRealx> (goalX - 0.5) && posRealx < (goalX + 0.5)
algoritmo, el
&& posRealy > (goalY -0.5) && posRealy < (goalY + 0.5)) {
posi.setSpeed(0,0);
fin = true;
bucle se repite
}
}else{
if (rodear){
hasta que se
rodear(soni, posi);
}
if (state==5){
llegue al Goal.
posi.setSpeed(0,0);
apuntar(posi);
rodear = false;
state=0;
posChoqueX = posRealx;
posChoqueY = posRealy;
}
}
try { Thread.sleep (100); } catch (Exception e) { }
}
System.out.println("============== FIN =====================");
return;
}
16. Código fuente del algoritmo
static boolean enLaLinea(float x, float y){
float deno1 = 0;
float deno2 = 0;
float coeficiente = 0;
if (goalX-startX !=0){
deno1 = goalX-startX;
}
Método de soporte if (goalY-startY !=0){
que devuelve si un deno2 = goalY-startY;
}
punto esta en la if (deno1 == 0){
coeficiente =0 - (y-startY)/deno2;
línea o no }else{
if (deno2 ==0){
coeficiente =(x-startX)/deno1 - 0;
}else{
coeficiente =(x-startX)/deno1 - (y-startY)/deno2;
Tiene en cuenta un }
posible margen de }
error if (-0.03f<coeficiente && coeficiente<0.03f){
if (Math.abs(posChoqueX-x) < 0.5 && Math.abs(posChoqueY-y)
<0.5){
posChoqueX = x;
posChoqueY = y;
}else{
state = 5;
}
return true;
}
return false;
}
17. Código fuente del algoritmo
static void apuntar(Position2DInterface posi) {
while (!posi.isDataReady ()); Método que produce
boolean salir =false;
el giro del robot
hasta que mire en la
while (!salir) {
// giramos dirección del punto
posi.setSpeed(0, 0.15f); Goal.
try { Thread.sleep (10); } catch (Exception e) { }
float min = (float)(angulo - 1.5f);
float max = (float)(angulo + 1.5f); También tiene en
if (posi.getYaw() > min && posi.getYaw() < max) {
cuenta un rango en
salir = true; el ángulo.
}
}
posi.setSpeed(0, 0);
}
18. Arturo San Feliciano Martín
Master en Sistemas Inteligentes – Universidad de Salamanca