Linea camara

De Proyecto Butiá
Saltar a: navegación, buscar

Introducción

Consiga: Se desea implementar un seguidor de línea que perciba el entorno con una cámara. La ventaja de su uso, es el poder percibir no solo el entorno local sino también el camino por venir para tomar la mejor decisión a futuro.
Tutor: Federico Andrade
Grupo:

  1. Josefina Fasoli
  2. Gonzalo Herrera
  3. Camila Serena

Motivación

Tomamos ejemplo de los seguidores de línea con sensores de grises y vimos que el hecho de que censan su entorno en un área pequeña no les permite realizar consideraciones respecto a la trayectoria óptima a recorrer. Solo cuentan con los datos actuales. En cambio, si utilizamos un seguidor dotado de una cámara podemos hacer consideraciones con respecto a la pista, no solo visualizando lo que necesita recorrerse en un determinado instante sino también de otros elementos de la pista que vendrán como ser que tan cerrada es una curva o si dos pendientes se anulan entre sí.

Algoritmo

A partir de la imagen recabada por la cámara, se toman puntos del centro de la linea a seguir. Con los puntos identificados, se calculan las respectivas pendientes generadas, para luego promediarlas y transformar este nuevo valor en un factor f utilizado para calcular la velocidad de cada rueda, que permite que haga el movimiento correspondiente.

Código

import pygame
import pygame.camera
import pygame.surfarray as surfarray
import sys 
sys.path.insert(0, '/usr/share/sugar/activities/TurtleBots.activity/plugins/butia')
from pygame.locals import *
from pybot import usb4butia
import time

pygame.init()
pygame.camera.init()
robot = usb4butia.USB4Butia()
robot.refresh()

def capturarImg(camara):
	camara.start()
	im = camara.get_image()
	camara.stop()
	return im

def inclinaciones(pixeles):
	i = 0
	l = largoLista(pixeles)
	l = l - 1
	incl = []
	while i < l:
		incl.append(inclinacion(pixeles[i],pixeles[i + 1]))
		i = i + 1
	return incl

def largoLista(lista):
	i = 0
	for a in lista:
		i = i + 1
	return i

def pendientesAfactores(pendientes):
	i = 0
	l = largoLista(pendientes)
	while i < l:
		if pendientes[i] > 30 or pendientes[i] == 0:
			pendientes[i] = 1
		elif pendientes[i] > 10:
			pendientes[i] = 1.1
		elif pendientes[i] > 5:
			pendientes[i] = 1.3
		elif pendientes[i] > 2:
			pendientes[i] = 1.5
		elif pendientes[i] > 0:
			pendientes[i] = 1.7
		elif pendientes[i] < -30:
			pendientes[i] = 1
		elif pendientes[i] < -10:
			pendientes[i] = 0.9
		elif pendientes[i] < -5:
			pendientes[i] = 0.7
		elif pendientes[i] < -2:
			pendientes[i] = 0.5
		else:
			pendientes[i] = 0.3
		i += 1
	return pendientes

def calculoFactorVelocidad(pendientes):
	suma = float(0)
	largo = largoLista(pendientes)
	for i in range(0, largo -1):	
		suma += pendientes[i]
	suma = suma / largo
	return suma

def inclinacion((x1,y1),(x2,y2)):
	if x2 == x1:
		m = 0
	else:	
		m = (y2 - y1) / (x2 - x1)
	return m

def movimientoRobot(robot, f):
	if f > 1:
		if f == 2:
			f = 0
		else:
			f = f - 1
		f = 1 - f
		velocidadDerecha = 200 * f
		velocidadIzquierda = 200
	else:
		velocidadDerecha = 200
		velocidadIzquierda = 200 * f	

	robot.set2MotorSpeed(1,int(velocidadDerecha),1,int(velocidadIzquierda))
	return 0 

def hallarPixelesIma(im, cant):
	ima = surfarray.array3d(im)
	pixeles = []
	w = im.get_width()
	h = im.get_height()
	x = h - 1
	while x >= 0:
		i = 0
		while i < w and (int(ima[i][x][0]) + int(ima[i][x][1]) + int(ima[i][x][2])) / 3 > 120:
			i = i + 1
		if i < w:
			j = w - 1
			while j > 0 and (int(ima[j][x][0]) + int(ima[j][x][1]) + int(ima[j][x][2])) / 3 > 120:
				j = j - 1
			y = (i + j) / 2
			pixeles.append((y,x))
			x = x - 30
		else:
			x = x - 1
	return pixeles

def obtenerX((x,y)):
	return x

def centradoFactor(k,p):
	xs = 0
	for pix in p:
		xs += obtenerX(pix)
	xs = xs / largoLista(p)
	if xs < 25:
		 k = 1.9
	elif xs < 55:
		if k >= 1 and k < 1.6:
			k = k + 0.4
		elif k < 1:
			k = 1.5
		elif k < 1.7:
			k = k + 0.3
		elif k < 1.8:
			k = k + 0.2
	elif xs < 67:
		if k >= 1 and k < 1.7:
			k = k + 0.3
		elif k < 1:
			k = 1.3
	elif xs > 130:
		k = 0.1
	elif xs > 105:
		if k <= 1 and k > 0.4:
			k = k - 0.4
		elif k > 1:
			k = 0.5
		elif k > 0.3:
			k = k - 0.3
		elif k > 0.2:
			k= k - 0.2
	elif xs > 93:
		if k <= 1 and k > 0.3:
			k = k - 0.3
		elif k > 1:
			k  = 0.7
	return k
				

camlist = pygame.camera.list_cameras()
if camlist:
	while cantVeces < 500:
		s = capturarImg(pygame.camera.Camera(camlist[0],(160,120)))
		p = hallarPixelesIma(s,cantVeces)
		pendientes = inclinaciones(p)
		factores = pendientesAfactores(pendientes)
		k = 1
		if largoLista(p) == 0:
			robot.set2MotorSpeed(0,0,0,0)
		else:
			if largoLista(p) > 1:			
				k = calculoFactorVelocidad(factores)
			k = centradoFactor(k,p)
			movimientoRobot(robot, k)

Nota:

Multimedia

Link a video de youtube: [1]

Trabajo a futuro

  • Seguir mejorando la eficiencia y/o trabajar sobre computadoras con mejor capacidad de procesamiento.
  • Parametrizar el factor de inclinación en base a la velocidad.
  • Reconocimiento del camino correcto frente a múltiples posibilidades.
  • Reconocimiento de obstáculos.