Self-Driving Car Motion Planning

A comprehensive motion planning system for autonomous vehicles implemented in Python, featuring behavioral planning, local path planning, velocity profiling, and a 2D controller for trajectory tracking in the CARLA simulator.

Table of Contents

• Overview
• Features
• System Architecture
• Installation
• Usage
• Project Structure
• Algorithm Details
• Results

Overview

This project implements a complete motion planning pipeline for autonomous vehicles, enabling a self-driving car to navigate through complex urban environments while avoiding obstacles, following traffic rules, and maintaining safe distances from other vehicles. The system integrates behavioral decision-making, local trajectory optimization, velocity profiling, and real-time control.

Features

• Behavioral Planning: State machine implementation for high-level decision making (lane following, stopping at stop signs, maintaining stopped state)
• Local Path Planning: Polynomial spiral optimization for generating smooth, feasible trajectories
• Collision Avoidance: Circle-based collision checking with multiple offset points along the vehicle body
• Velocity Profiling: Adaptive speed planning for different scenarios:
  • Nominal lane maintenance
  • Deceleration to stop lines
  • Lead vehicle following with time gap maintenance
• 2D Controller: Combined longitudinal (PID) and lateral (pure pursuit with heading error) control
• Real-time Visualization: Live plotting of trajectories, velocities, and control outputs
• CARLA Integration: Full integration with CARLA simulator for realistic testing

System Architecture

┌─────────────────────────────────────────────────────────────┐
│                        MAIN LOOP                            │
│                       (main.py)                             │
└─────────────────┬───────────────────────────────────────────┘
                  │
        ┌─────────┴─────────┐
        │                   │
        ▼                   ▼
┌───────────────┐   ┌──────────────────┐
│  Behavioral   │   │  Local Planner   │
│   Planner     │──▶│  - Path Gen      │
│               │   │  - Collision     │
└───────────────┘   │  - Velocity      │
                    └──────┬───────────┘
                           │
                           ▼
                    ┌──────────────┐
                    │  Controller  │
                    │    (2D)      │
                    └──────┬───────┘
                           │
                           ▼
                    ┌──────────────┐
                    │    CARLA     │
                    │  Simulator   │
                    └──────────────┘

Key Components

1. Behavioral Planner (behavioral_planner.py)

   • Manages state transitions between different driving behaviors
   • Handles stop sign detection and compliance
   • Manages lead vehicle tracking
   • Computes goal states based on waypoints and lookahead distance

2. Local Planner (local_planner.py)

   • Generates multiple candidate paths using spiral optimization
   • Performs collision checking against static obstacles
   • Selects optimal path based on goal proximity and collision avoidance

3. Velocity Profiler (velocity_profiler.py)

   • Creates velocity profiles for different scenarios
   • Implements smooth acceleration/deceleration profiles
   • Handles lead vehicle following with configurable time gaps

4. Path Optimizer (path_optimizer.py)

   • Uses polynomial spirals for smooth path generation
   • Optimizes paths to minimize curvature and deviation
   • Ensures kinematic feasibility

5. Collision Checker (collision_checker.py)

   • Circle-based collision detection
   • Evaluates path safety against obstacles
   • Ranks collision-free paths for selection

6. 2D Controller (controller2d.py)

   • PID controller for longitudinal speed control
   • Stanley controller variant for lateral control
   • Combines heading error and cross-track error

Installation

Prerequisites

• Python 3.6+
• CARLA Simulator 0.9.x
• Ubuntu 16.04+ or Windows 10

Dependencies

pip install numpy scipy matplotlib configparser

CARLA Setup

1. Download CARLA simulator from CARLA Releases
2. Extract and run the CARLA server:

   ./CarlaUE4.sh

Clone Repository

git clone https://github.com/yourusername/self-driving-car-motion-planning.git
cd self-driving-car-motion-planning

Usage

Basic Run

python main.py

With Options

python main.py --host localhost --port 2000 --quality-level Low

Command Line Arguments

• --host: IP address of CARLA server (default: localhost)
• --port: TCP port (default: 2000)
• --quality-level: Graphics quality [Low, Epic] (default: Low)
• -v, --verbose: Enable debug output

Configuration

Edit options.cfg to modify:

• Live plotting settings
• Visualization update frequency

Project Structure

.
├── main.py                          # Main simulation loop
├── options.cfg                      # Configuration file
├── src/
│   ├── behavioral_planner.py       # Behavioral state machine
│   ├── local_planner.py            # Local path planning
│   ├── controller2d.py             # Vehicle controller
│   └── local_planner_utils/
│       ├── path_optimizer.py       # Spiral path optimization
│       ├── collision_checker.py    # Collision detection
│       └── velocity_profiler.py    # Velocity profile generation
├── data/
│   ├── waypoints.txt               # Global waypoints
│   ├── stop_sign_params.txt        # Stop sign locations
│   └── parked_vehicle_params.txt   # Parked vehicle obstacles
├── controller_output/              # Simulation results
│   ├── trajectory.png
│   ├── forward_speed.png
│   └── ...
└── README.md

Algorithm Details

Behavioral Planning States

FOLLOW_LANE         # Normal lane following
DECELERATE_TO_STOP  # Approaching stop sign
STAY_STOPPED        # Waiting at stop sign

Path Planning

The system generates 7 candidate paths with lateral offsets from the goal state:

• Center path aligned with goal heading
• 3 paths offset to the left
• 3 paths offset to the right

Each path is optimized using cubic spiral interpolation to ensure:

• Smooth curvature
• Kinematic feasibility
• Minimal deviation from goal

Collision Checking

Uses 3 circular approximations along the vehicle body:

• Offsets: [-1.0m, 1.0m, 3.0m]
• Radii: [1.5m, 1.5m, 1.5m]

Velocity Profiling

Three modes:

1. Nominal: Accelerate/decelerate to desired speed
2. Stop Sign: Trapezoidal profile (decelerate → coast → brake)
3. Lead Vehicle: Match lead vehicle speed with time gap buffer

Controller

Longitudinal (PID):

throttle = Kp*e_v + Ki*∫e_v + Kd*de_v/dt

Lateral (Stanley):

δ = θ_e + arctan(k_cte * e_cte / (v + k_soft))

Where:

• e_v: velocity error
• θ_e: heading error
• e_cte: cross-track error

Results

Simulation Output

The system generates:

• Trajectory Plot: Vehicle path vs waypoints
• Speed Profile: Actual vs reference speeds
• Control Outputs: Throttle, brake, steering over time
• Collision Count: Safety metrics

Performance Metrics

• Waypoint Tracking: Sub-meter accuracy
• Speed Tracking: ±0.5 m/s error
• Collision Avoidance: 100% success rate with static obstacles
• Stop Sign Compliance: Full stops within buffer zone

Demo

Vehicle successfully navigating through waypoints while avoiding obstacles

Full Video Demo

Configuration

Planning Parameters

NUM_PATHS = 7                    # Number of candidate paths
BP_LOOKAHEAD_BASE = 8.0          # Base lookahead distance (m)
BP_LOOKAHEAD_TIME = 2.0          # Time-based lookahead (s)
PATH_OFFSET = 1.5                # Lateral path spacing (m)
TIME_GAP = 1.0                   # Lead vehicle time gap (s)
A_MAX = 1.5                      # Max acceleration (m/s²)
SLOW_SPEED = 2.0                 # Coasting speed (m/s)

Controller Gains

Kp = 0.50    # Proportional gain
Ki = 0.30    # Integral gain
Kd = 0.13    # Derivative gain
Kp_heading = 8.00    # Heading control gain