Práctica 2: Obstacle avoidance

En esta práctica el objetivo es implementar la lógica del algoritmo de navegación VFF (Virtual Force  Field). El principio de esta técnica es que cada objeto del entorno del robot genera una fuerza repulsora hacia el robot, y el destino genera una fuerza atractora en el Fórmula1. El fórmula1 se moverá en la dirección de la suma de las dos fuerzas, de esta forma se podrán evitar obstáculos, además de llegar al destino. Para poder llegar a un destino remoto, utilizaremos  la navegación local, para la cual tendremos subojetivos a los que llegar.

Para poder implementar esta práctica disponemos del simulador Gazebo y la plataforma JdeRobot. Además el Fórmula1 tendrá un sensor láser (que nos permite medir distancias en un plano, proporcionándonos un array de 180 números), además de las cámaras derecha e izquierda; y como actuadores tendremos los motores, para los cuales tendremos en cuenta la velocidad de tracción y la velocidad de rotación.

En la siguiente imagen podemos ver un manto azul que son las lecturas del láser, además tenemos un vector verde, que es el vector atractor; un vector rojo, que es el vector repulsor; y por último un vector negro, que es la combinación de los dos anteriores.

 

En la práctica tenemos los siguientes pasos:

1.  Obtenemos la posición del subobjetivo al que deseamos llegar:

        self.currentTarget=self.getNextTarget()
        self.targetx = self.currentTarget.getPose().x
        self.targety = self.currentTarget.getPose().y

 2. Obtenemos la posición del robot y la orientación del robot respecto al mapa:

        rx = self.sensor.getRobotX()
        ry = self.sensor.getRobotY()

        rt = self.sensor.getRobotTheta()

3. Marcamos los subobjetivos por los que ya hemos pasado:

        if(abs(ry)<(abs(self.targety)+1) and abs(ry)>(abs(self.targety)-1)):
            self.currentTarget.setReached(True)

4. Obtenemos los datos del sensor láser, que consiste en 180 pares de valores y obtenemos los datos del láser de una forma más clara:

        laser_data = self.sensor.getLaserData()
        laser = parse_laser_data(laser_data)

5. Convertimos las coordenadas absolutas del subobjetivo en coordenadas relativas al robot:

        self.carx,self.cary=absolutas2relativas(self.targetx,self.targety,rx,ry,rt)

 6. Con los datos del láser, obtenemos el vector repulsor:

        dist_threshold = 6
        vff_repulsor_list = []
        for d,a in laser:
            # (4.2.1) laser into GUI reference system
            if(d < dist_threshold):
                x = (d - dist_threshold) * math.cos(a) * -1
                y = (d - dist_threshold) * math.sin(a) * -1
                v = (x,y)
                vff_repulsor_list += [v]

        vff_repulsor = np.mean(vff_repulsor_list, axis=0)

        self.obsx,self.obsy = vff_repulsor

7. Calculamos el módulo del vector repulsor y en función de su valor se queda como está o  aumentamos su peso:

        mod_repulsor = pow(pow(self.obsx,2) + pow(self.obsy,2),0.5)
        if (mod_repulsor > 1.55):
            self.obsx,self.obsy = vff_repulsor * 4.5

8. Hacemos la suma del vector atractor y del vector repulsor para obtener el vector resultante:

        self.avgx = self.carx + self.obsx
        self.avgy = self.cary + self.obsy
 

9. Cálculo del módulo de la velocidad:

         speed = pow(pow(self.avgx,2) + pow(self.avgy,2),0.5)

10. Cálculo de la corrección:

        if (abs(self.obsx) > 2):
            if (abs(self.obsx) < abs(self.carx)):
                if (self.obsx >= 0):
                    self.avgx = abs(self.avgx)
                else:
                    self.avgx = -abs(self.avgx)

        if ((self.obsx == (-self.carx)) and (self.obsy == (-self.cary))):
            self.avgx = self.obsx
            self.avgy = self.cary

11. Calculamos el ángulo:

        if (speed < 1):
            # Use the tangent to avoid indeterminacy
            angle = math.atan(abs(self.avgx/self.avgy))
        else:
            angle = math.asin(abs(self.avgx/speed))
        if(self.avgy > 0):
            angle = math.pi - angle 

12. Asignamos las velocidades de tracción y de rotación:

# Linear speed
        if ((speed < 1) or (speed > 3)):
            self.sensor.setV(3)
        else:
            self.sensor.setV(speed)

        # Angular speed
        if(self.avgx < 0):
            self.sensor.setW(angle * 0.75)
        else:
            self.sensor.setW(-angle * 0.75)

 A continuación podemos ver un vídeo de la práctica:

 

El enlace con el código de la práctica es el siguiente:  https://github.com/RoboticsURJC-students/2016-tfg-vanessa-fernandez/tree/master/Practices%20of%20robotics/obstacle_avoidance