About This Project

This was Project 1 for ENPM667 - Control of Robotic Systems at University of Maryland. I worked on this with my teammate Pranav Koli. The goal was to implement and compare three different lateral controllers for autonomous vehicle path following.

We used the CARLA simulator to test everything, which made it really easy to visualize how each controller performed on both straight roads and sharp turns.

GitHub: Compare-PP-LQR-MPC

Quick Demo

The Three Controllers

1. Pure Pursuit (PP)

This is the simplest of the three. The idea is to pick a “lookahead point” on the path ahead of the car, and steer towards it. The steering angle is calculated using some basic geometry.

# The steering angle depends on the lookahead distance and curvature
steering_angle = atan2(2 * L * sin(alpha), lookahead_distance)

It works surprisingly well for most cases, and it’s super fast to compute. The main tuning parameter is the lookahead distance - too short and the car oscillates, too long and it cuts corners.

2. Linear Quadratic Regulator (LQR)

LQR is an optimal control technique. Instead of just pointing at a target, it considers the current errors (how far off the path and how much the heading is wrong) and finds the “best” steering input.

We modeled the car as a simple bicycle model with two states:

  • Lateral error (how far off the path)
  • Heading error (how much the yaw differs from path direction)

The controller solves the Discrete Algebraic Riccati Equation (DARE) to find optimal gains:

# Solve DARE to get optimal gain K
P = scipy.linalg.solve_discrete_are(A, B, Q, R)
K = np.linalg.inv(R + B.T @ P @ B) @ (B.T @ P @ A)

# Control law
steer = -K @ [lateral_error, heading_error]

The Q and R matrices let you tune how aggressive the controller is. Higher Q means “fix errors faster”, higher R means “don’t steer too hard”.

3. Model Predictive Controller (MPC)

MPC is the most sophisticated one. Instead of just reacting to current errors, it predicts the future trajectory over a horizon (like 10-20 timesteps) and optimizes the entire sequence of steering commands.

We used CVXPY to set up the optimization problem:

# State: [lateral_error, heading_error]
# Control: steering angle

for k in range(N):
    # Cost: minimize errors and steering effort
    cost += cp.quad_form(x[:, k], Q) + cp.sum_squares(u[:, k]) * R
    
    # Dynamics constraint
    constraints.append(x[:, k+1] == A @ x[:, k] + B @ u[:, k])
    
    # Steering limits
    constraints.append(cp.abs(u[:, k]) <= max_steer)

prob = cp.Problem(cp.Minimize(cost), constraints)
prob.solve(solver=cp.OSQP)

The cool thing about MPC is that it can handle constraints naturally - like steering rate limits so the car doesn’t jerk around.

The Implementation

CARLA Setup

After setting up the Carla simulator, we connected to CARLA through the Python scripts and spawned a Lincoln MKZ car. The car has sensors but for this project we mainly used the ground truth pose from the simulator.

self.client = carla.Client(host, port)
self.world = self.client.get_world()
self.vehicle = self.setup_vehicle(config)

Path Planning

We used CARLA’s built-in GlobalRoutePlanner to generate a path between spawn points:

grp = GlobalRoutePlanner(self.world.get_map(), sampling_resolution=2.0)
route = grp.trace_route(start_location, end_location)

Error Computation

Both LQR and MPC need the lateral and heading errors. We compute these by finding the closest waypoint and projecting:

def get_error_states(self, target_path):
    # Heading error
    heading_error = normalize_angle(vehicle_yaw - path_yaw)
    
    # Lateral error (cross-track error)
    dx = vehicle_x - path_x
    dy = vehicle_y - path_y
    lat_error = -dx * sin(path_yaw) + dy * cos(path_yaw)
    
    return lat_error, heading_error

Longitudinal Control

For speed control, we use a simple PID controller. It computes the error between target speed and current speed, then outputs throttle or brake commands.

def longitudinal_control(self, target_speed):
    current_speed = sqrt(vel.x**2 + vel.y**2 + vel.z**2)
    error = target_speed - current_speed
    
    # PID output
    pid_out = Kp * error + Ki * integral + Kd * derivative
    
    if pid_out >= 0:
        throttle = min(1.0, pid_out)
        brake = 0.0
    else:
        throttle = 0.0
        brake = min(1.0, -pid_out)
    
    return throttle, brake

The gains (Kp, Ki, Kd) are configurable in the YAML file. We kept this simple since the focus was on lateral control.

Main Control Loop

The main loop runs at a fixed timestep. Each iteration:

  1. Find closest waypoint
  2. Compute errors
  3. Run the selected controller
  4. Apply throttle (simple PID for speed) and steering
  5. Log the data
while True:
    self.compute_closest_path_index(target_path)
    
    if controller == "PP":
        steer = self.pure_pursuit_controller(target_path)
    elif controller == "LQR":
        steer = self.LQR_controller(target_path)
    elif controller == "MPC":
        steer = self.MPC_controller(target_path)
    
    throttle, brake = self.longitudinal_control(target_speed)
    self.vehicle.apply_control(carla.VehicleControl(
        throttle=throttle, steer=steer, brake=brake))

Additional Features

The codebase includes several helpful features for experimentation:

Spawn Point Visualization: At startup, all available spawn points in the map are visualized with their indices. This makes it easy to pick routes for testing.

def visualize_all_spawn_points(self):
    for idx in range(len(spawn_points)):
        self.world.debug.draw_string(loc, str(idx), color=carla.Color(0, 0, 255))

Path Visualization: The generated path is drawn in the simulator with red arrows showing direction, making it easy to verify the route.

Random Route Generation: Routes can be generated randomly using a seed for reproducibility. Set randomly_select_route_points_seed > 0 in the config, and the same seed will generate the same route every time.

# config.yaml
route_points: [129, 28, 33, 40, 132, 146]  # Manual route
randomly_select_route_points_seed: 7        # Or use random with seed

YAML Configuration: All parameters (vehicle type, controller tuning, sensor setup, simulation settings) are in a single YAML config file. No need to modify code to change experiments.

Data Logging: Every timestep is logged to CSV with timestamp, controller type, speed, lateral error, heading error, and execution time for easy analysis.

Sensor Suite: The code supports attaching multiple sensors (RGB cameras, LiDAR, GNSS, IMU, Radar) with configurable attributes, though for this project we mainly used ground truth pose.

Results

We logged lateral error, heading error, and execution time for each controller. In general:

  • Pure Pursuit: Fastest computation (~0.1ms), but larger errors on tight curves
  • LQR: Good balance (~0.5ms), smooth tracking
  • MPC: Good accuracy but slowest (~5-10ms depending on horizon)

For a car going 10-15 m/s on typical roads, all three worked fine. MPC really shines when you have constraints to satisfy or need tighter tracking.

What We Learned

  1. Start simple - Pure Pursuit is a great baseline. Don’t jump to MPC unless you need it.
  2. Tuning matters a lot - We spent more time tuning parameters than expected.
  3. CARLA is great for testing - Being able to visualize everything made debugging so much easier.
  4. MPC is expensive - On an embedded system, you’d need to be careful about the horizon length.

Running the Code

# Start CARLA
./CarlaUE4.sh

# Run the simulation
python lateral_control_suite.py --config config/config.yaml --controller MPC

Check out the GitHub repo for the full code and config files!

References

This project is an implementation based on the following paper:

Zhang, W.; Wang, J.; Pang, T. Research on the Performance of Vehicle Lateral Control Algorithm Based on Vehicle Speed Variation. World Electric Vehicle Journal 2025, 16, 259. https://doi.org/10.3390/wevj16050259