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

Práctica 1: follow line

En esta práctica emplearemos el simulador Gazebo, la plataforma Jderobot (nos basamos en el código de https://github.com/JdeRobot/TeachingRobotics/tree/master/src/follow_line) y la librería OpenCV para procesamiento de imagen. Para poder utilizar la librería OpenCV tendremos que importarla con import cv2.

La práctica consiste en realizar un control reactivo PID y completar una vuelta del circuito Fórmula 1. Para ello nuestro fórmula 1 constará de dos sensores, que son dos cámaras, una izquierda y una derecha; además tendremos dos actuadores: la velocidad de tracción y la velocidad de rotación. Las cámaras nos proporcionarán información de los píxeles como una matriz de números, que nos servirá para guiar a nuestro fórmula1. El circuito lo podemos ver en la siguiente imagen:

Los pasos que debemos seguir son los siguientes:

1. Conseguir las imágenes de las dos cámaras (estas imágenes estarán en el espacio de color RGB):

        imageLeft = self.sensor.getImageLeft()
        imageRight = self.sensor.getImageRight()

 2. Para poder filtrar la imagen y quedarnos únicamente con la carretera, tendremos que transformar primero las imágenes al espacio de color HSV:

        imageRight_HSV = cv2.cvtColor(imageRight, cv2.COLOR_RGB2HSV)
        imageLeft_HSV = cv2.cvtColor(imageLeft, cv2.COLOR_RGB2HSV)

3.  Después tendremos que elegir un valor mínimo y uno máximo para poder filtrar el resto de la imagen y quedarnos solamente con la carretera:

        value_min_HSV = np.array([0, 235, 60])
        value_max_HSV = np.array([180, 255, 255])

4. Filtramos las imagenes obteniendo unas imágenes binarias:

        imageRight_HSV_filtered = cv2.inRange(imageRight_HSV, value_min_HSV, value_max_HSV)
        imageLeft_HSV_filtered = cv2.inRange(imageLeft_HSV, value_min_HSV, value_max_HSV)

5.  Las imágenes que obtenemos con la función anterior, únicamente tienen un canal, por lo que para mostrar las imágenes tendremos que emplear la siguiente función de tres canales, para la cual empleamos tres veces la misma imagen:

        imageRight_HSV_filtered_Mask = np.dstack((imageRight_HSV_filtered, imageRight_HSV_filtered, imageRight_HSV_filtered))
        imageLeft_HSV_filtered_Mask = np.dstack((imageLeft_HSV_filtered, imageLeft_HSV_filtered, imageLeft_HSV_filtered))

Mostramos las imágenes:

        #SHOW THE FILTERED IMAGE ON THE GUI
        self.setRightImageFiltered(imageRight_HSV_filtered_Mask)
        self.setLeftImageFiltered(imageLeft_HSV_filtered_Mask)

A continuación vemos las imágenes de la cámara derecha y de la izquierda tanto en color como en binario:

6. Con la imagen binaria, que tiene en blanco la carretera y en negro el resto, ya podemos analizar los puntos de interés de la imagen. Primero debemos encontrar los dos puntos extremos de la línea roja:

        position_pixel_left = []
        position_pixel_right  = []

        for i in range(0, columns-1):
            value = imageLeft_HSV_filtered[365, i] - imageLeft_HSV_filtered[365, i-1]
            if(value != 0):
                if (value == 255):
                    position_pixel_left.append(i)
                else:

                    position_pixel_right.append(i-1)

En el código anterior vemos que miramos la fila 365 y tendremos que añadir al array de la izquierda el número de la columna donde value (resta del píxel actual y el píxel anterior) es 255, y al array derecho le añadiremos el número de la columna donde value es -255.

7. Tendremos que comprobar que se rellenan los dos arrays para que no haya un problema de ejecución. De estar algún array vacío lo rellenamos y además calcularemos la posición media de la carretera:

        if ((len(position_pixel_left) != 0) and (len(position_pixel_right) != 0)):
            position_middle = (position_pixel_left[0] + position_pixel_right[0]) / 2
        elif ((len(position_pixel_left) != 0) and (len(position_pixel_right) == 0)):
            position_middle = (position_pixel_left[0] + columns) / 2
        elif ((len(position_pixel_left) == 0) and (len(position_pixel_right) != 0)):
            position_middle = (0 + position_pixel_right[0]) / 2
        else:
            position_pixel_right.append(1000)
            position_pixel_left.append(1000)
            position_middle = (position_pixel_left[0] + position_pixel_right[0])/ 2

8. Calcularemos la desviación de nuestro coche:

        desviation = position_middle - (columns/2)

9. Hacemos un control P, para ello empleamos diferentes velocidades de tracción y de rotación:

        if (desviation == 0):
             self.sensor.setV(10)
        elif (position_pixel_right[0] == 1000):
             self.sensor.setW(-0.0000035)
        elif ((abs(desviation)) < 85):
             if ((abs(desviation)) < 15):
                 self.sensor.setV(6)
             else:
                 self.sensor.setV(3.5)
             self.sensor.setW(-0.000045 * desviation)
        elif ((abs(desviation)) < 150):
             if ((abs(desviation)) < 120):
                 self.sensor.setV(1.8)
             else:
                 self.sensor.setV(1.5)
             self.sensor.setW(-0.00045 * desviation)
        else:
             self.sensor.setV(1.5)
             self.sensor.setW(-0.005 * desviation)

El vídeo de la práctica es el siguiente:

El código está en el siguiente enlace:

https://github.com/RoboticsURJC-students/2016-tfg-vanessa-fernandez/tree/master/Practices%20of%20robotics/follow_line