En esta práctica nos proporcionan un mundo en Gazebo en el que tenemos el cuadricóptero ArDrone y el robot Kobuki. El objetivo será que el drone localice al Kobuki y consiga seguirle. Para mover el Kobuki emplearemos el teleoperador, y de esta forma variaremos su posición y podremos ver como el drone le sigue. Para esta práctica empleamos la plataforma JdeRobot, el lenguaje de programación Python y la librería OpenCV para procesamiento de imagen.
Para la ejecución de la práctica tenemos que lanzar en un terminal el mundo en Gazebo:
gazebo ardrone-turtlebot.world
En otro terminal lanzamos el robot Kobuki:
kobukiViewer --Ice.Config=turtlebot.cfg
Por último, en otro terminal habrá que lanzar el componente follow_turtlebot:
./follow_turtlebot.py --Ice.Config=follow_turtlebot_conf.cfg
Para realizar la práctica he seguido los siguientes pasos:
1. El primer paso es crear una clase llamada pid, donde introduciré el código del controlador PID. Lo primero que hago es inicializar dicha clase y después me defino el método calculateU para poder calcular el controlador proporcional, el controlador derivativo y el controlador integral, y después sumarlos, proporcionando la corrección que habrá que aplicar al dar la velocidad al drone. Para la realización de dicho método debemos conocer como se calcula cada controlador. El controlador proporcional sigue la siguiente fórmula: u = -kp * error. El controlador derivativo se consigue siguiendo la fórmula: u = -kd * (error actual - error anterior). El controlador integral lo obtenemos empleando la siguiente fórmula: u = -ki * (error actual + todos los errores anteriores). La clase es la siguiente:
1. El primer paso es crear una clase llamada pid, donde introduciré el código del controlador PID. Lo primero que hago es inicializar dicha clase y después me defino el método calculateU para poder calcular el controlador proporcional, el controlador derivativo y el controlador integral, y después sumarlos, proporcionando la corrección que habrá que aplicar al dar la velocidad al drone. Para la realización de dicho método debemos conocer como se calcula cada controlador. El controlador proporcional sigue la siguiente fórmula: u = -kp * error. El controlador derivativo se consigue siguiendo la fórmula: u = -kd * (error actual - error anterior). El controlador integral lo obtenemos empleando la siguiente fórmula: u = -ki * (error actual + todos los errores anteriores). La clase es la siguiente:
class pid(object):
| def __init__(self, kp, kd, ki): |
| # Constant of PID control |
| self.kp = kp |
| self.kd = kd |
| self.ki = ki |
| self.error = 0 |
| self.acumulate_error = 0 |
| def calculateU(self, e): |
| proportional = self.kp * e |
| derivate = self.kd * (e - self.error) |
| self.acumulate_error = self.acumulate_error + e |
| integral = self.ki*(self.acumulate_error) |
| u = -(proportional) -(derivate) -(integral) |
| self.error = e |
2. Lo siguiente es definir el valor del error mínimo e instanciar dos objetos (uno por el eje y otro por el eje y) en el constructor de la clase MyAlgorithm:
self.minError = 0.013
self.pidX = self.pid(2.655,0.000112,0.00029)
self.pidY = self.pid(2.655,0.000112,0.00029)
3. En el método execute tendremos que obtener la imagen de la cámara (la cual vamos a segmentar) mediante input_image = self.camera.getImage(). Si conseguimos la imagen entonces podemos mostrarla en la imagen que nos proporciona la imagen color filter mediante la siguiente sentencia:
self.camera.setColorImage(input_image)
4. El siguiente paso es emplear un filtro de suavizado puesto que la imagen
tiene mucho ruido e imperfecciones. Lo recomendable es emplear un filtro
gaussiano:
gaussian_image = cv2.GaussianBlur(input_image, (5,5), 0.2)
5. Convertimos nuestra imagen que esta en el espacio de color RGB al espacio de color HSV:
hsv_image = cv2.cvtColor(gaussian_image, cv2.COLOR_RGB2HSV)
6. Aplicamos un filtro de color con unos valores mínimo y máximo para el tono, la saturación y la intensidad. Con dicha función obtendremos una imagen donde el objeto deseado (parte verde del turtlebot) aparecerá en blanco y el resto en negro.
image_HSV_filtered = cv2.inRange(hsv_image, value_min_HSV, value_max_HSV)
hsv_image = cv2.cvtColor(gaussian_image, cv2.COLOR_RGB2HSV)
6. Aplicamos un filtro de color con unos valores mínimo y máximo para el tono, la saturación y la intensidad. Con dicha función obtendremos una imagen donde el objeto deseado (parte verde del turtlebot) aparecerá en blanco y el resto en negro.
image_HSV_filtered = cv2.inRange(hsv_image, value_min_HSV, value_max_HSV)
7. Realizamos la operación mofológica cierre para que se rellenen los huecos que quedan en la pelota:
kernel = np.ones((19,19), np.uint8)
image_HSV_filt_close = cv2.morphologyEx(image_HSV_filtered, cv2.MORPH_CLOSE, kernel)
self.camera.setThresoldImage(image_HSV_filt_close)
8. Copiamos la imagen original para no modificarla y la imagen filtrada:
image_HSV_filtered_Copy = np.copy(image_HSV_filt_close)
input_image_Copy = np.copy(input_image)
9. Detectamos el contorno del objeto:
_, contours, hierarchy = cv2.findContours(image_HSV_filtered_Copy, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
10. Elegimos el color rojo para realizar el bounding box de la pelota. Para ello he creado una función llamada bounding. Primero en la función realizamos una aproximación a rectángulos, mediante la cual se obtienen varios rectángulos, por lo que habrá que filtrarlos para quedarse con el cuadrado de mayor tamaño.También al final la función nos devolverá el centro del rectángulo detectado Dicha función es la siguiente:
def bounding(self, contours, colors, input_image_Copy):
area = 0
if (len(contours) != 0):
cnt = cv2.approxPolyDP(contours[0], 3, True)
rectX, rectY, rectW, rectH = cv2.boundingRect(cnt)
for cnt in contours:
cnt = cv2.approxPolyDP(cnt, 3, True);
x, y, w, h= cv2.boundingRect(cnt)
if((x - w) * (y -h) > area):
rectX = x
rectY = y
rectH = h
rectW = w
if (len(contours) != 0):
cv2.rectangle(input_image_Copy,(rectX,rectY), (rectX+rectW,rectY+rectH), (colors[0],colors[1], colors[2]), 2)
center = [rectX+(rectW/2), rectY+(rectH/2)]
return center
Llamamos a la función bounding y mostramos en colorfilter el resultado de todo el proceso llevado a cabo. Por lo que veremos como encima de la parte verde del turtlebot se superpone un rectángulo encima como resultado de la detección de dicho objeto.
color = [255, 0, 0]
center = self.bounding(contours, color, input_image_Copy)
self.camera.setColorImage(input_image_Copy)
11. Calculamos el centro de la imagen que detecta el drone, es decir, la imagen de la cámara del drone:
size = input_image.shape
height = size[0]
width = size[1]
img_center = [width/2, height/2]
12. Si en el campo visual se detecta el rectángulo verde del turtlebot entonces tendremos que ejecutar un algoritmo para corregir la posición del drone y acercarnos al turtlebot. Si no se detecta es porque no está en el campo visual del drone y entonces hay que subir el drone hasta que consiga ver el turtlebot.
Si el no se detecta el turtlebot haremos:
self.cmdvel.sendCMDVel(0,0,0.2,0,0,0)
Si se detectan los contornos del turtlebot (if self.turtlebot_visual_field(contours)) entonces seguiremos los siguientes pasos:
12.1. Calcular el error de la posición del drone. Para ello nuestro error será el centro del rectángulo detectado por el turtlebot en cada eje menos el centro de la imagen de la cámara del drone en cada eje. Esto hará que el turtlebot cuando corrijamos la posición del drone quede centrado en la imagen que nos da la cámara del drone. Además este error lo dividimos por 320 para normalizarlo entre 0 y 1
errorx = (center[0] - img_center[0])/320
errory = (center[1] - img_center[1])/320
12.2. Calculo la corrección de posición que habría que aplicar en cada eje:
controladorX = self.pidX.calculateU(errorx)
controladorY = self.pidY.calculateU(errory)
12.3. Ahora tenemos que dar las ordenes de velocidad a los motores del drone. Si el error es menor que el error mínimo en cualquiera de los ejes, en dicho eje la velocidad a aplicar será cero. Mientras que si el error es mayor al error mínimo en un eje, en ese eje la velocidad a aplicar será 1.5*cotrolador calculado. El 1.5 es para que vaya a mayor velocidad y se corrija antes la posición. Con un valor muy alto de velocidad oscilaría mucho, por lo que por eso he elegido este valor de 1.5. Vemos esta parte de código:
if (abs(errorx) <= self.minError) and (abs(errory) >= self.minError):
self.cmdvel.sendCMDVel(1.5*controladorY,0,0,0,0,0)
if (abs(errorx) >= self.minError) and (abs(errory) <= self.minError):
self.cmdvel.sendCMDVel(0,1.5*controladorX,0,0,0,0)
if (abs(errorx) >= self.minError) and (abs(errory) >= self.minError):
self.cmdvel.sendCMDVel(1.5*controladorY,1.5*controladorX,0,0,0,0)
elif (abs(errorx) <= self.minError) and (abs(errory) <= self.minError):
self.pidX.acumulate_error = 0
self.pidY.acumulate_error = 0
self.cmdvel.sendCMDVel(0,0,0,0,0,0)
El vídeo de la práctica es el siguiente:
El código de la práctica se encuentra en el siguiente enlace:
