Path Planning Project

Overview

This project implements path planning algorithm for Udacity self-driving car simulator.

Goals

In this project the goal is to safely navigate around a virtual highway with other traffic. Using the car's localization and sensor fusion data. There is also a sparse map list of waypoints around the highway.

Acceptance criteria:

The car should try to go as close as possible to the 50 MPH speed limit, which means passing slower traffic when possible.
The car should avoid hitting other cars at all cost.
The car should avoid driving inside of the marked road lanes at all times, unless going from one lane to another.
The car should be able to make one complete loop around the 6946m highway.
The car should not experience total acceleration over 10 m/s^2
the car should not experience jerk that is greater than 10 m/s^3

Other vehicles on the highway:

Are driving ±10 MPH of the 50 MPH speed limit.
Might try to change lanes too.

Demo

./t3p1

Video

Usage

Usage:
  t3p1 [options]
Available options:
  -w, --waypoints  File with route waypoints (defaults: data/highway_map.csv)
  -p, --port       Port to use (default: 4567)
  -h, --help       print this help screen

Dependencies

Runtime

Term 3 Simulator

Tools

cmake >= 3.5
- All OSes: click here for installation instructions
make >= 4.1 (Linux, Mac), 3.81 (Windows)
- Linux: make is installed by default on most Linux distros
- Mac: install Xcode command line tools to get make
- Windows: Click here for installation instructions
gcc/g++ >= 5.4, clang
- Linux: gcc/g++ is installed by default on most Linux distros
- Mac: same deal as make - install Xcode command line tools
- Windows: recommend using MinGW

Libraries not included into the project

uWebSocketIO == v0.13.0
- Ubuntu/Debian: the repository includes install-ubuntu.sh that can be used to set up and install uWebSocketIO
- Mac: the repository includes install-mac.sh that can be used to set up and install uWebSocketIO
- Windows: use either Docker, VMware, or even Windows 10 Bash on Ubuntu

Libraries included into the project

Eigen - C++ template library for linear algebra: matrices, vectors, numerical solvers, and related algorithms
JSON for Modern C++ - JSON parser
Catch2 - Unit-testing framework
ProgramOptions.hxx - Single-header program options parsing library for C++11
nanoflann - a C++11 header-only library for Nearest Neighbor (NN) search wih KD-trees
spline.h - Cubic Spline interpolation in C++

Build

Local manual build

Clone this repo.
mkdir build
cd build
cmake .. -DCMAKE_BUILD_TYPE=Release -DLOCAL_BUILD=ON -DDOCKER_BUILD=OFF
make
make test

Pre-built Docker container

docker pull sgalkin/carnd-t3p1

Manual build using containerized development environment

Clone this repo.
mkdir build
cd build
cmake .. -DCMAKE_BUILD_TYPE=Release -DLOCAL_BUILD=OFF -DDOCKER_BUILD=ON
make docker-build
make docker-test
make docker-run or make docker-shell; ./t3p1

Map

Each waypoint in the list contains [x,y,s,dx,dy] values.

x and y are the waypoint's map coordinate position
s value is the distance along the road to get to that waypoint in meters
dx and dy values define the unit normal vector pointing outward of the highway loop

The highway's waypoints loop around so the frenet s value, distance along the road, goes from 0 to 6945.554.

Protocol

The project uses uWebSocketIO request-response protocol in communicating with the simulator.

INPUT: values provided by the simulator to the c++ program

{
  "x": "(float) - The car's x position in map coordinates",
  "y": "(float) - The car's y position in map coordinates",
  "yaw": "(float) - The car's speed in MPH",
  "speed": "(float) - The velocity of the vehicle (magnitude)",
  "s": "(float) - The car's s position in frenet coordinates",
  "d": "(float) - The car's d position in frenet coordinates",
  "previous_path_x": "(Array<float>) - The global x positions of the previous path, processed points removed",
  "previous_path_y": "(Array<float>) - The global y positions of the previous path, processed points removed",
  "end_path_s": "The previous list's last point's frenet s value",
  "end_path_d": "The previous list's last point's frenet d value",
   "sensor_fusion": [ "2D vector of cars and then that car's",
    [
      "<id> - car's unique ID",
      "<x> - car's x position in map coordinates",
      "<y> - car's y position in map coordinates",
      "<vx> - car's x velocity in m/s",
      "<vy> - car's y velocity in m/s",
      "<s> - car's s position in frenet coordinates",
      "<d> - car's d position in frenet coordinates",
    ]
  ]
}

OUTPUT: values provided by the c++ program to the simulator

{
  "next_x": "(Array<float>) - The global x positions of the trajectory",
  "next_y": "(Array<float>) - The global y positions of the trajectory",
}

Algorithm

Constants

Path planning horizon - 1 second
Points in path - 50 (horizon / simulator tick)
Hard speed limit - 50 MPH
Recommended speed - 99% of hard speed limit
Comfort forward gap (distance to the obstacle in front) - 30 m
Comfort backward gap (distance to the obstacle behind) - 10 m
Minimal gap (safety limit) - 15 m

Fusion

Position of each vehicle predicted for the end of current path (up to 1 second)
Constant velocity model is used for prediction
Closest car (if any) and its velocity associated to each lane

Reference lane

Cost function used in order to choose lane for the next iteration
Safety check applied for each lane - no obstacles closer than minimal gap
- the only possible action - keep the lane
Main terms of the cost function
- penalty for changing lanes (try to stay on lane)
- penalty for using leftmost lane
- forward gap size
- velocity of the car in front (if any)
Implicit FSM used in the project in order to protect the car during lane change

Reference velocity

Slow down if obstacle too close (closer than minimal gap)
Follow the car in front with its velocity
Speed up to the recommended velocity

Trajectory generation

At each step the application extends previous path with new points, this guaranties smoothness of the path
Cubic spline used in the project as a reference trajectory
Velocity is constant for each step

Control

The car uses a perfect controller and will visit every (x, y) point it receives in the list every 0.02 seconds
The units for the (x, y) points are in meters
The spacing of the points determines the speed of the car
The vector going from a point to the next point in the list dictates the angle of the car.
Acceleration both in the tangential and normal directions is measured along with the jerk (the rate of change of total acceleration)

TODO

Try to use more advanced cost function.
Increase test coverage.
Tune safety parameters.
Try to change not only to neighboring lane

sgalkin / CarND-T3P1