Control
Control systems for the AV2.
Overview
The control system translates planned paths into actuator commands:
Pure Pursuit: Geometric path tracking for steering
Velocity Control: Speed and throttle management
Vehicle Actuator: Hardware interface
Pure Pursuit Controller
Pure pursuit is a geometric path tracking algorithm that computes steering angle to follow a path.
Algorithm
Find a lookahead point on the path ahead of the vehicle
Compute the curvature needed to reach that point
Convert curvature to steering angle
Lookahead Point
●
/|
/ |
/ |
/ | L_d (lookahead distance)
/ |
/ α |
●──────┘
Vehicle
Steering = atan2(2 × L × sin(α), L_d)
Where:
- L = wheel base
- α = angle to lookahead point
- L_d = lookahead distance
Implementation
class PurePursuitController:
def __init__(self, wheel_base=1.23):
self.wheel_base = wheel_base
self.K_dd = 0.5 # Lookahead gain
self.min_lookahead = 3.0
self.max_lookahead = 20.0
def compute_steering(self, waypoints, speed):
"""
Compute steering angle to follow waypoints.
Args:
waypoints: List of (x, y) in vehicle frame
speed: Current vehicle speed (m/s)
Returns:
steering: Steering angle in radians
"""
# Compute speed-adaptive lookahead
lookahead = self.K_dd * speed + self.min_lookahead
lookahead = np.clip(lookahead, self.min_lookahead, self.max_lookahead)
# Find lookahead point
target = self.find_lookahead_point(waypoints, lookahead)
if target is None:
return 0.0
# Compute steering angle
target_x, target_y = target
alpha = np.arctan2(target_y, target_x)
steering = np.arctan2(
2 * self.wheel_base * np.sin(alpha),
lookahead
)
return steering
def find_lookahead_point(self, waypoints, lookahead):
"""Find point on path at lookahead distance."""
for i, (x, y) in enumerate(waypoints):
dist = np.sqrt(x**2 + y**2)
if dist >= lookahead:
return (x, y)
# Return last waypoint if none at lookahead distance
return waypoints[-1] if waypoints else None
Parameters
Parameter |
Default |
Description |
|---|---|---|
wheel_base |
1.23 |
Vehicle wheel base (m) |
K_dd |
0.5 |
Lookahead distance gain |
min_lookahead |
3.0 |
Minimum lookahead distance (m) |
max_lookahead |
20.0 |
Maximum lookahead distance (m) |
Tuning Guide
Lookahead Distance:
Too short: Oscillation, overshooting turns
Too long: Cutting corners, slow response
K_dd (Speed Gain):
Higher: Smoother at high speeds
Lower: More responsive at low speeds
Velocity Control
Speed management for throttle and braking.
Simple Proportional Control
class VelocityController:
def __init__(self, Kp=0.5, max_throttle=0.5, max_brake=1.0):
self.Kp = Kp
self.max_throttle = max_throttle
self.max_brake = max_brake
def compute(self, target_speed, current_speed):
"""Compute throttle/brake command."""
error = target_speed - current_speed
if error > 0:
# Need to accelerate
throttle = min(self.Kp * error, self.max_throttle)
brake = 0.0
else:
# Need to decelerate
throttle = 0.0
brake = min(-self.Kp * error, self.max_brake)
return throttle, brake
PID Control
For smoother control:
class PIDVelocityController:
def __init__(self, Kp=0.5, Ki=0.1, Kd=0.05):
self.Kp = Kp
self.Ki = Ki
self.Kd = Kd
self.integral = 0.0
self.prev_error = 0.0
def compute(self, target_speed, current_speed, dt):
error = target_speed - current_speed
self.integral += error * dt
derivative = (error - self.prev_error) / dt
self.prev_error = error
output = self.Kp * error + self.Ki * self.integral + self.Kd * derivative
if output > 0:
return min(output, 1.0), 0.0 # throttle, brake
else:
return 0.0, min(-output, 1.0)
Vehicle Actuator
Interface to the vehicle’s actuators via Teensy CAN master.
Connection
class VehicleActuator:
def __init__(self, port='/dev/ttyACM0', baudrate=115200):
self.serial = serial.Serial(port, baudrate, timeout=0.1)
self.connected = True
def _send(self, command):
"""Send command to Teensy."""
self.serial.write(f"{command}\n".encode())
Commands
def set_throttle(self, value):
"""Set throttle (0.0 to 1.0)."""
value = np.clip(value, 0.0, 1.0)
self._send(f"T {value:.3f}")
def set_brake(self, value):
"""Set brake (0.0 to 1.0)."""
value = np.clip(value, 0.0, 1.0)
self._send(f"B {value:.3f}")
def set_steering(self, value):
"""Set steering (-1.0 to 1.0)."""
value = np.clip(value, -1.0, 1.0)
self._send(f"S {value:.3f}")
def set_steering_degrees(self, degrees):
"""Set steering in degrees (-28 to 28)."""
degrees = np.clip(degrees, -28, 28)
normalized = degrees / 28.0
self.set_steering(normalized)
def set_drive_mode(self, mode):
"""Set drive mode (N, D, S, R)."""
if mode in ['N', 'D', 'S', 'R']:
self._send(f"M {mode}")
def emergency_stop(self):
"""Activate emergency stop."""
self._send("E 1")
def reset_emergency_stop(self):
"""Reset emergency stop."""
self._send("E 0")
Safety Features
class SafeVehicleActuator(VehicleActuator):
def __init__(self, *args, **kwargs):
super().__init__(*args, **kwargs)
self.last_command_time = time.time()
self.watchdog_timeout = 0.2 # seconds
def check_watchdog(self):
"""Check if commands are being sent regularly."""
if time.time() - self.last_command_time > self.watchdog_timeout:
self.emergency_stop()
def set_throttle(self, value):
self.last_command_time = time.time()
super().set_throttle(value)
Control Loop Integration
Complete control loop:
class Controller:
def __init__(self):
self.path_tracker = PurePursuitController()
self.velocity_ctrl = VelocityController()
self.actuator = VehicleActuator()
def control_step(self, waypoints, target_speed, current_speed):
# Compute steering
steering = self.path_tracker.compute_steering(
waypoints, current_speed
)
# Compute throttle/brake
throttle, brake = self.velocity_ctrl.compute(
target_speed, current_speed
)
# Send commands
self.actuator.set_steering_degrees(np.degrees(steering))
if brake > 0:
self.actuator.set_throttle(0)
self.actuator.set_brake(brake)
else:
self.actuator.set_brake(0)
self.actuator.set_throttle(throttle)