Mejoras del controlador del vehículo e implementación de PID
Semana 12
Esta semana el objetivo ha sido el de crear:
- Una variante mejorada de nuestro controlador del vehículo con una variable intermedia que haga que tanto la aceleración como la deceleración sean más graduales.
- Un controlador PID para mejorar el control acelerador (throttle).
Todas estas modificaciones o mejoras se han realizado sobre el script vehicle.py de nuestro BRAIN, creado en la semana 4.
Control con Variable Intermedia para Throttle
Con el objetivo de poder tener más muestras con valores intermedios de throttle entre 0 y 1, y para que la aceleración de nuestro vehículo sea más gradual, hemos metido la variable del tiempo. Es decir, vamos a dejar de incrementar o decrementar el valor del acelerador en cada frame y lo haremos en función del tiempo con tal de no discretizar (a 0 o 1) el valor de throttle. El incremento o decremento será de +- 0.25 cada 0.5 segundos. A continuación se muestra el nuevo código:
import carla
import pygame
import time
throttle = 0.0
steer = 0.0
last_step_time = time.time()
STEP_INTERVAL = 0.5 # tiempo entre incrementos en segundos
THROTTLE_STEP = 0.25 # tamaño del incremento
def control(vehicle):
global throttle, steer, last_step_time
control = carla.VehicleControl()
keys = pygame.key.get_pressed()
now = time.time()
# W acelerar // Va en función del tiempo, para hacer más gradual la subida y caída del acelerador
if keys[pygame.K_w] and (now - last_step_time) >= STEP_INTERVAL:
throttle = min(throttle + THROTTLE_STEP, 1.0)
last_step_time = now
elif not keys[pygame.K_w] and (now - last_step_time) >= STEP_INTERVAL:
throttle = max(throttle - THROTTLE_STEP, 0.0)
last_step_time = now
control.throttle = throttle
# S stop
if keys[pygame.K_s]:
control.brake = 1.0
# A girar a la izquierda
if keys[pygame.K_a]:
control.steer = -0.5
# D : girar a la derecha
if keys[pygame.K_d]:
control.steer = 0.5
vehicle.apply_control(control)
print(f"STEER={control.steer:.2f} | THROTTLE={control.throttle:.2f} | BRAKE={control.brake:.2f}")
return control.throttle, control.steer, control.brake
A modo de prueba, para observar variedad de muestras de valores en los actuadores que podemos obtener durante la conducción, se ha capturado un pequeño conjunto de datos (6200 muestras aprox.). Como se puede observar en el siguiente histograma, los valores intermedios entre 0.0 y 1.0 tienen una cantidad de datos similar. En cambio, en los datasets creados en las semanas anteriores, se podía ver un conjunto de datos muy descompensados en el que destacaban los valores para throttle de 0 y 1.
Control PID de Throttle
Para tratar de mejorar el control del vehículo simulado en Carla, hemos implementado un controlador PID para el acelerador, con tal de que se vaya ajustando de forma más estable hacia una velocidad objetivo.
El controlador va a comparar continuamente un valor deseado (setpoint) con un valor medido (measurement) y generar una corrección compuesta por tres efectos: una acción proporcional que responde al error actual (P), una acción integral que acumula errores pasados para eliminar sesgos persistentes (I), y una acción derivativa que anticipa tendencias futuras para evitar oscilaciones (D).
class PID:
def __init__(self, Kp, Ki, Kd):
self.Kp = Kp
self.Ki = Ki
self.Kd = Kd
self.setpoint = 0.0
self.integral = 0.0
self.prev_error = 0.0
self.last_time = time.time()
def update(self, measurement):
now = time.time()
dt = now - self.last_time
self.last_time = now
error = self.setpoint - measurement
P = self.Kp * error
self.integral += error * dt
I = self.Ki * self.integral
D = self.Kd * ((error - self.prev_error) / dt) if dt > 0 else 0
self.prev_error = error
return P + I + D
Establecemos un objetivo de velocidad máximo y los incrementos de velocidad por cada vez que se pulsa W. Con get_speed() obtenemos la velocidad de nuestro vehículo. Con speed_pid actualizamos el valor de la velocidad y calculamos la corrección que hay que aplicar al acelerador. Este cálculo es la diferenia entre la velocidad actual con la deseada en función del tiempo.
A continuación, se muestra la implementación del PID en el controlador del vehículo:
speed_pid = PID(Kp=0.05, Ki=0.01, Kd=0.02)
MAX_SETPOINT = 50.0 # km/h
SETPOINT_INCREMENT = 1.0 # cómo sube al mantener W
SETPOINT_DECREMENT = 1.0 # cómo baja al soltar W
def get_speed(vehicle):
v = vehicle.get_velocity()
return 3.6 * math.sqrt(v.x**2 + v.y**2 + v.z**2)
def control(vehicle):
control = carla.VehicleControl()
keys = pygame.key.get_pressed()
# Setpoint W
if keys[pygame.K_w]:
speed_pid.setpoint += SETPOINT_INCREMENT
else:
speed_pid.setpoint -= SETPOINT_DECREMENT
speed_pid.setpoint = max(0.0, min(MAX_SETPOINT, speed_pid.setpoint))
# Velocidad medida
current_speed = get_speed(vehicle)
# Throttle
throttle_cmd = speed_pid.update(current_speed)
throttle_cmd = max(0.0, min(1.0, throttle_cmd))
control.throttle = throttle_cmd
# S stop
if keys[pygame.K_s]:
control.brake = 1.0
else:
control.brake = 0.0
# A: girar a la izquierda
if keys[pygame.K_a]:
control.steer = -0.5
# D : girar a la derecha
if keys[pygame.K_d]:
control.steer = 0.5
vehicle.apply_control(control)
print(f"Speed={current_speed:.1f} km/h | Throttle={control.throttle:.2f} | Setpoint={speed_pid.setpoint:.1f}")
return control.throttle, control.steer, control.brake
A modo de prueba, para observar variedad de muestras de valores en los actuadores que podemos obtener durante la conducción, se ha capturado un pequeño conjunto de datos (4500 muestras aprox.). Se puede observar que hay muchos valores distintos (entre 0 y 1) para el throttle. Ya no son los mismos siempre (0.25, 0.5, 0.75). Por lo que hay mucha variedad y, por tanto, poca cantidad para cada tipo de muestra.
Otros controladores
Además de los anteriores, se ha estado probando la implementación de un PID para el volante (steering) y mejoras en el PID para el acelerador. Todavía se están probando dichas mejoras, pues no se han podido obtener buenos resultados en la teleoperación del vehículo.