The goals of this project are the following:

• Pilot the ego vehicle along the simulated highway for at least one round (4.32 miles) without incidents.

The detailed definition of incidents are as below:

• Obey Speed Limit
  • The car doesn't drive faster than the speed limit (50 MPH).
  • Also the car isn't driving much slower than speed limit unless obstructed by traffic.
• Not Exceeded Max Acceleration & Jerk
  • The car does not exceed a total acceleration of 10 m/s^2 and a jerk of 10 m/s^3.
• No Collision
  • The car must not come into contact with any of the other cars on the road.
• Keep Lane
  • The car doesn't spend more than a 3 second length out side the lanes during lane changing.
  • Every other time the car stays inside one of the 3 lanes on the right hand side of the road.
• Change Lane
  • The car is able to smoothly change lanes when it makes sense to do so, such as:
    • Behind a slower moving car
    • An adjacent lane is clear of other traffic.

• cmake >= 3.5
  • All OSes: click here for installation instructions
• make >= 4.1
  • 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
  • Linux: gcc / g++ is installed by default on most Linux distros
  • Mac: same deal as make - [install Xcode command line tools]((https://developer.apple.com/xcode/features/)
  • Windows: recommend using MinGW
• uWebSockets
  • Run either install-mac.sh or install-ubuntu.sh.
  • If you install from source, checkout to commit e94b6e1, i.e.

    git clone https://github.com/uWebSockets/uWebSockets 
    cd uWebSockets
    git checkout e94b6e1
    

You can download the simulator for testing highway scenario from [releases tab (https://github.com/udacity/self-driving-car-sim/releases/tag/T3_v1.2).

1. Clone this repo.
2. Make a build directory: mkdir build && cd build
3. Compile: cmake .. && make
4. Run it: ./path_planning.

1. Location: data/udacity-map-original.csv
2. Schema
   • s the distance along the center line to get to that waypoint in meters
   • x & y the waypoint's map coordinate position
   • dx & dy the unit normal vector pointing outward of the highway loop.

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

1. ["x"] The car's x position in map coordinates
2. ["y"] The car's y position in map coordinates
3. ["s"] The car's s position in frenet coordinates
4. ["d"] The car's d position in frenet coordinates
5. ["yaw"] The car's yaw angle in the map
6. ["speed"] The car's speed in MPH

1. ["end_path_s"] The previous list's last point's frenet s value
2. ["end_path_d"] The previous list's last point's frenet d value

1. ["previous_path_x"] The previous list of x points previously given to the simulator
2. ["previous_path_y"] The previous list of y points previously given to the simulator

Note Return the previous list but with processed points removed, can be a nice tool to show how far along the path has processed since last time.

1. ["sensor_fusion"]
   • A 2d vector of cars and then that car's:
     • car's unique ID
     • car's x position in map coordinates
     • car's y position in map coordinates
     • car's x velocity in m/s
     • car's y velocity in m/s
     • car's s position in frenet coordinates
     • car's d position in frenet coordinates.

1. The car uses a perfect controller and will visit every (x,y) point it recieves in the list every .02 seconds. The units for the (x,y) points are in meters and 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. The (x,y) point paths that the planner recieves should not have a total acceleration that goes over 10 m/s^2, also the jerk should not go over 50 m/s^3. (NOTE: As this is BETA, these requirements might change. Also currently jerk is over a .02 second interval, it would probably be better to average total acceleration over 1 second and measure jerk from that.

2. There will be some latency between the simulator running and the path planner returning a path, with optimized code usually its not very long maybe just 1-3 time steps. During this delay the simulator will continue using points that it was last given, because of this its a good idea to store the last points you have used so you can have a smooth transition. previous_path_x, and previous_path_y can be helpful for this transition since they show the last points given to the simulator controller with the processed points already removed. You would either return a path that extends this previous path or make sure to create a new path that has a smooth transition with this last path.

Current solution architecture is organized as follows:

* High-Definition Map: Provide map info of driving scenario
* Localization: Provide ego vehicle's localization and odometry
* Prediction: Predict surrouding objects' short-term motion
* Path Planner: Generate feasible & optimal trajectory for ego vehicle's next step motion
* Driving Strategy: Evaluate next step motion's feasibility and optimality
* Trajectory Generator: Generate trajectory for next step motion
* Actuator: Interface to simulator

High-Definition Map is a class as both map reader, writer and center line smoother.

• Map Reader & Writer

  • Use CSV library for convenient map loading.
  • Save the processed map(e.g, the one attained through resolution improvement) as CSV.

• Center Line Smoother

  • Use spline library to generate smooth center line between original map waypoints

Localization is a convenient wrapper for ego vehicle's state and odometry.

• Ego Vehicle State

  • Convenient access to ego vehicle's world frame & frenet frame position, heading and speed

• Odometry

  • Access to previous planned trajectory.
  • Estimate next actuation time for next step motion planning.

• Start Configuration Estimation

  • Estimate start configuration for next step motion planning.

Prediction predicts surrounding objects's short-term motion.

• Maneuvers Implemented
  • Follow Lane: Surrounding objects would follow current lane at current speed for the whole horizon.
  • Drive Freespace: Surrounding objects would keep current speed and heading and drive as if in freespace, ignore lane totally.

Path Planner generates both feasible and optimal trajectory for ego vehicle's next step motion. It follows the methodology proposed in this paper Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet Frame

• Feasible Zone Generator

  • Generate feasible zone, end configuration space in adjacent alnes, using ego vehicle's current state and surrouding object prediction.

• Trajectory Generation and Evaluation

  • Given feasible zone, propose end configurations.
  • Solve jerk minimized trajectory using selected start & end configuration.
  • Evaluate the feasibility and optimality of proposed trajectory.

Driving Strategy evaluates the feasibility and optimality of proposed trajectory

• Feasibility--Satisfy Max Speed, Acceleration and Jerk

  • Check for Parametric Trajectory
    • Given the parametric form of proposed trajectory(coefficients of the quintic polynomial), evaluate its 1st, 2nd and 3rd order derivatives at each future timestamp at the interval of 0.02 seconds.
  • Check for Numerical Trajectory
    • Given the proposed trajectory in C++ vector format, evaluate its 1st, 2nd and 3rd order numerical derivatives using second order forward difference formula at each evaluatable timestamp.

• Optimality--Evaluate from Efficiency, Flexibility and Safety

  • Efficiency
    • Feasible distance covered
    • Feasible speed reached
    • Lane change cost
  • Flexibity
    • Is leading or following vehicle presents
    • Available are in lane
    • Possbile future maneuvers of selected lane
  • Safety
    • Safety distance needed for emergency case handling.

Trajectory Generator propose trajectory for next step motion and optimize final trajectory through spline smoothing of previous and newly generated trajectory.

• Trajectory Proposal

  • Solve the coefficients of the quintic polynomial for both S and D trajectory using the method in courseware

• Trajectory Optimization

  • Smooth the final trajectory using previously executed waypoints and newly generated waypoints through spline smoothing

Current cost function only has a very high probability of completing the round trip with no incidents. It has a high probability to cause incidents in dense traffic situations due to frequent failure of trajectory planning. Fine tuning is still needed to guarantee its performance.

Current cost function is based on human heuristics and manual fine tuning of hyper-parameters. This is an extremely painful job. Next step plan would be bring ideas from Robotics Software Engineer Term 2 such as deep reinforcement learning based approaches to better solve this planning problem.

1. What is the Difference between Path Planning and Motion Planning

2.a Introduction to Path Planning

2.b Path Planning and Collision Avoidance

3.a Robot Motion Planning