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

1. Planificación de trayectorias

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.

11. Pasos del Algoritmo Bug 2

12. Resultado
 El resultado de
la ejecución
completa es la
mostrada en la
imagen.

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