The goal of this project was to develop an algorithm that allows a self-driving car within a simulator to drive safely on a highway. The setting includes (sometimes very irrational) traffic and the car had to be able to perform safe lane changes. Additional constraints are things like speed limit, maximum acceleration and jerk. The main tasks were the implementation of a behavior planner (when to do what) and based on the chosen behavior, trajectory generation (how to do it). The simulator provides the data from the sensor fusion and state information. The implementation of a controller was not necessary, since the car follows exactly the calculated trajectory.

## Implementation

The process starts with localization based on the sensor data the simulator provides. The additional sensor fusion data contains information like position and velocity of nearby vehicles. This information is used to determine the possible speed in the current lane. The decision process works like a finite state machine whose base state is `stay in lane`. If the current lane is blocked and therefore prevents the car from reaching the speed limit the algorithm changes to the `Consider lane change` state. This happens during the calculation of the desired speed. The agent will always try to choose the fastest possible lane.

``````double ref_vel = Mph2Mps(_car_speed);
if (not _lane_conflict || _actual_distance > _safe_distance) {
ref_vel += ref_vel + _max_acc > _max_speed ? _max_speed - ref_vel : _max_acc;
} else if (_lane_conflict && _actual_distance <= _safe_distance) {
if (ref_vel > CalculateObstacleSpeed(_sensor_fusion[conflict_idx])) {
ref_vel -= _max_acc;
}
ConsiderLaneChange();
} else if (_lane_conflict && _actual_distance <= _critical_distance) {
ref_vel -= _max_acc;
ConsiderLaneChange();
}
return ref_vel;
``````

How the agent adapts its velocity and invokes the lane change evaluation

While in `Consider lane change` state the algorithm evaluates its options (stay in lane, change lane, which lane). This evaluation is done with cost functions. The cost function considers the possible speed in that lane, the distance to the car in front and lane changes get an additional penalty to prevent unnecessary lane changes. However, if a change to the specific lane is considered unsafe or no speed gain can be achieved, the evaluation of that lane is terminated.

``````// check if car in that lane is in front of us and slower
if (obstacle_lane == lane && obstacle_vel < Mph2Mps(_car_speed) &&
(dist > _lane_change_dist_back && dist < 50)) {
lane_good = false;
break;
// check if car is in that lane and faster but with not enough space to pull out
} else if (obstacle_lane == lane && obstacle_vel > Mph2Mps(_car_speed) &&
(dist > - (obstacle_vel * 0.5) && dist < _lane_change_dist_front)) {
lane_good = false;
break;
// if no knockout criteria is met, identify nearest car for following cost calc
} else if (obstacle_lane == lane && dist > 0 && dist < nearest_dist) {
nearest_dist = dist;
if (dist < 60) {
lane_speed = obstacle_vel;
}

``````

Definitions of knockout criteria for unsafe conditions for a lane change

If no such case was detected, the cost for that lane is calculated. I defined the cost function as follows:

This defines the cost for choosing the i-th lane, where the maximum possible speed is `v_lane` and `c_i` is a constant penalty for performing lane changes, which is 0 for the current lane. `d` is the distance to the nearest car in front of the agent. The `alpha`’s are just some parameters to indicate the possibility for further parameterization. Finally the lane with the lowest cost will be chosen.

The trajectory will then be calculated based on this decision. To find a decent trajectory which satisfies the constraints a spline interpolation is performed. This process requires several coordinate transforms between Frenet coordinates, the global map coordinates and the local car reference frame (both Cartesian).

Coordinate transformation with rotation and translation from local (car) reference frame to global (map) coordinates. `x_loc_ref` is the x coordinate in the global reference frame of the local frame’s origin. `x_loc_obs` is the observations x coordinate expressed in the local frame.

The transformation from observations in the map reference frame to the car’s reference frame is done via:

The final trajectory is obtained by picking points from the interpolated spline. Since the simulator expects the map coordinates for each trajectory and updates the car’s position every 20ms to the next trajectory point, the reference velocity has to be converted to distance increments. However, the actual distance is the arc length of the spline, which is not trivial to calculate. Instead I use a linear approximation for the distance travelled, which works quite well since the track isn’t too winding.

I’ve refactored the starter code in such a way that I was able to write a PathPlanner class. This way it was possible to parameterize many of the functions in the class header file and keep the `main.cpp` short.

## Results

With the last parameters I’ve tested, the car was able to drive several times at least 20 miles without any incident. A simple lane change to overtake a slower vehicle is shown below:

In some rare cases the car will still not be able to cope with every (crazy) behavior of the other cars and violate some constraints or even collide. This could be further improved by tuning the parameters or improving the decision logic. However, since the simulator is random and doesn’t provide the opportunity to test corner cases, this process is extremely time consuming. The better the car drives, the rarer are the events to study. The gif below shows a more complex lane change maneuver in a dense traffic situation.

Tags:

Categories: