Práctica - follow_line - 1 (Fin)

He englobado toda la práctica en una publicación ya que la práctica se ha realizado en pocos días y los primeros avances no eran significativos.

Primero se ha detectado la línea roja del centro de la carretera utilizando un filtro de color,  y para ello se  ha utilizado la herramienta ColorTuner de JdeRobot para calcular los valores HSV óptimos.

HSV mín = [0, 200, 45]

HSV máx = [10, 255, 255]

Después era interesante saber cuando empezaba la línea roja central de la carretera, y para ello, utilizando el filtro de color anterior y recorriendo todas las filas de la imagen desde el 60% de la imagen, se vio cuando empezaba la línea. Se comprobó por el centro de la imagen ya que a los laterales están las vallas que son del mismo color, o similar, a la línea.

self.row_with_line = 249

Una vez se consiguieron los datos anteriores ya se podía empezar a trabajar con la función execute() de MyAlgorithm.py. Como tan solo se utiliza la imagen de la cámara izquierda, se ha optado por mostrar en la imagen de la izquierda el filtrado que se ha realizado de la línea(rojo) y una cruz (verde) mostrando la fila que se escogió y la columna ideal de la línea en el eje horizontal, que es en el centro de la anchura de la imagen. Por otra parte, en la imagen de la derecha se han mostrado los datos relevantes:

• Velocidad (V)
• Velocidad angular (W)
• Error, con respecto a la posición ideal de la línea en el eje horizontal (E)
• Estado actual (S)
• Tiempo, en segundos, de la vuelta (T)
• Número de vueltas dado (LAPS)

Antes de explicar el funcionamiento del algoritmo es interesante nombrar los estados posibles así como su significado:

• STRAIGHT, significa que el coche se encuentra en el centro de la línea con un error:

-20 < Error(e) < 20

• +/- CURVE, son dos estados diferentes y el positivo significa que el error es superior a 20 y el negativo inferior a -20.

• NOLINE, significa que en la fila 249 no se ha encontrado línea y se inicia el contador para pasar al siguiente estado.

• SEARCH, este estado aparece cuando pasan más de 10 repeticiones seguidas sin encontrar la línea, entonces se establece una velocidad y una velocidad angular (si la anterior es negativa pasa a positiva y a la inversa) por defecto con tal de que el coche de vueltas poco a poco hasta encontrar de nuevo la línea, aunque esto no asegura que el coche siga la carretera hacia el sentido adecuado.

Para explicar el cambio de estados se ha creado un modelo de estados para que sea mucho más visual que redactado:

Para calcular el error se ha creado una función straight() que utiliza la posición ideal de la línea (mitad de imagen(eje x), self.initial_position_value en el código) y la posición actual de la línea(eje x) para calcular la norma lineal entre estas dos, con la función lineal.norm() de la biblioteca numpy. Para determinar si este error es negativo o positivo se utiliza también la posición ideal y la actual, ya que si la actual es menor que la ideal esta se pasa a negativo y en caso contrario sigue siendo positivo. En caso del estado NOLINE se ha optado por aumentar el error anterior en un 20% para el error actual.

Finalmente, utilizando este error y el error anterior (self.last_error en el código) se han creado los controladores Proporcional y Derivativo,  tanto para la velocidad como para la velocidad angular, siendo estas las fórmulas:

w_1, w_2 = - self.Kp_w * self.error, - self.Kd_w * (self.error - self.last_error)

self.w = w_1 + w_2

v_1, v_2 = - self.Kp_v * self.error, - self.Kd_v * (self.error - self.last_error)

        self.v = 13 - abs(v_1 + v_2) + self.time_v * 0.3

Siendo self.last_error  el error anterior, self.time_v es el número de iteraciones en el estado STRAIGHT, self.error el error actual y 13 una constante para la velocidad, ya que en caso estar en los estados +/-CURVE la velocidad sería menor a 13 y en caso del estado STRAIGHT la velocidad iría aumentando con las iteraciones. Para las constantes self.Kp_w/v y  self.Kd_w/v se ha ido adaptando el valor para obtener el mejor tiempo por vuelta posible, quedando finalmente en los valores:

self.Kp_w = 0.02
self.Kd_w = 0.015
self.Kp_v = 0.010
self.Kd_v = 0.015

Obteniendo así el siguiente resultado:

Dos vueltas en 1:40 minutos

Como añadido para saber exactamente cuanto tarda en cada vuelta, se ha realizado un filtrado de las líneas de inicio de carrera (blancas) cuando el self.time_straight(== self.time_v) sea mayor que 30 ya que es la recta más larga de la carrera y se ha guardado una imagen de la posición y el tiempo actual, como también se ha reiniciado el tiempo de inicio (self.start_time en el código):

Lap 1  Tiempo = 50.25s

Lap 2 Tiempo = 50.24s

Como se puede ver es un poco antes del letrero de START pero más o menos se puede estimar que como máximo tardaría 51 segundos cada vuelta.

Otra funcionalidad añadida es el poder pausar el tiempo cuando se para el algoritmo, para seguir con el mismo tiempo que se ha parado, aunque éste tiene un retraso de ~5 mili-segundos.