Project: Path Planning

View on Github

This project is part of Udacity’s Self-Driving-Car Nanodegree. The project resources and build instructions can be found here, the required simulator here.

Driving with minimum-jerk trajectories

Implementation of a path planning algorithm for the navigation of an autonomous vehicle on a highway, satisfying safety, feasibility and comfort boundary conditions. Amongst others these are:

obeying and tracking the speed limit of 50 mph
driving comfortable and safe maneuvers respecting jerk and acceleration thresholds (10 m/s³, 10 m/s²)
efficient maneuver selection, such as to change lanes in order to pass a slower moving car
avoid collisions under all circumstances

In this approach to the project we will implement the method presented in Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet Frame.

Overview

The overall planning problem is a complex task, and the ability of planning in real time is crucial. Therefore, it will be beneficial to subdivide the complete planning problem into a hierarchy of specific subtasks. By constraining the amount of information and ressources provided to each subtask, an efficient computation can be achieved. Each level in the following hierarchy takes care of a certain aspect of the planning process. Higher levels being of higher abstraction.

The topmost level handles the mission planning whose main focus is the navigation of the ego vehicle from its current location to a given goal destination on the map. Lower level details like lane changes are still neglected at this stage. The output of the mission planner is a trajectory that may be interpreted as the main route to be followed by the autonomous vehicle. We will subsequently refer to it as the reference line, or the reference trajectory. In unstructered environments the reference trajectory could be generated by using the output of a path planning algorithm like A*. However, in our case the highway track is structured, and we will simply use the road center as the reference trajectory to follow.

The behaviour planner on the next stage focuses on handling the current driving scenario. It proposes viable maneuvers based on the present environment conditions, e.g., other traffic participants, rules of the road, etc. The process of this decision-making is an area of active research and there is no specific best method to achieve this. Available architectures could be finite state machines, rule based systems or methods based on reinforcement learning. In our implementation we will use a finite state machine which consists of states and transitions in between them. The states represent some chosen ego vehicle maneuvers that are required to fullfill the highway driving task. The transitions dictate how these states should evolve depending on the current environement conditions. For example: If there are no leading vehicles in the current lane (transition condition), track the speed limit (state).

On the next level, the local planner is responsible for the implementation details of the maneuvers proposed by the behaviour planner. For each of these maneuvers, the local planner will generate a set of kinematically feasible and comfortable low level trajectory canditates. All collision-free candidates are compared to each other, whereby a cost functional quantifies the effectiveness and comfortness of each trajectory. The determined minimum cost trajectory is passed to the vehicle controller and will be followed up to the next replanning cycle. In this stage we will implement the method proposed in [1] which uses combinations of longitudinal and lateral jerk-minimized polynomials to define the trajectory candidates.

The vehicle controller on the last stage is responsible for the execution of the chosen trajectory. This stage needs no explicit own design, since the controller is integrated in the simulator. It requires as input a vector of discretized waypoints describing the chosen trajectory.

Let’s have a closer look into each of these stages.

Stage 1: The Mission Planner

The main task of the mission planner is to provide a reference trajectory on the highway, around which the more detailed local planning will subsequently occur. The planning will mainly take place in the Frenet frame, which allows to represent a position on the road in a more intuitive way than the traditional Cartesian frame with its x and y coordinates.

Planning in the Frenet frame

A position in the Frenet frame is expressed by its s and d coordinates, whereby s represents the longitudinal propagation along the reference trajectory and d the lateral distance from it. In the convention used in this implementation d is negative to the left and positive to the right in the direction of the reference line.

By carrying out the planning in the Frenet frame it is possible to separate the planning into a lateral and a longitudinal component. These components can subsequently be combined to global composite trajectories that accomplish different tasks such as “Change to left lane (lateral component) while accelerating (longitudinal component)” or “Keep the lane (lateral) while following the leading vehicle (longitudinal)”. This, in someway, mimics human-driving behaviour.

The reference trajectory

The highway map data is provided in the file data/highway_map.csv. The highway has 6 lanes in total, each being 4 meters wide, and 3 lanes heading in each direction. The highway track is described by a list of 181 waypoints, which define a loop of approximately 6946 meters length. The waypoints represent the center line of the road which separates the lanes into the two driving directions. Each waypoint provides information about its global x, y location, its Frenet s coordinate and the dx, dy components of the Frenet unit normal vector n_r. The latter is perpendicular to the center line and points to the right-hand side of the road (see figure below). This differs from the convention used in [1], where the unit normal vector points to the left-hand side.

Waypoints

The raw waypoints are approximately 30 meters apart from each other. This is too coarse to be used directly for the several subtasks of the planning problem. We will therefore augment the provided data by using splines to fill the gaps.

The highway center line is used as reference trajectory (red) for the subsequent generation of the trajectory candidates by the local planner (green). Thereby each point x(s, d) of the trajectory candidates is expressed as a time dependent function of the covered arc length s(t) at the center line, and the point’s perpendicular offset d(t) from the center line.

The augmentation of the map data is accomplished by the Map class defined in src/map.cpp. This class also provides methods for the conversions between Frenet and global coordinates and for the computation of the center line’s curvature at a specific location. The latter is required to determine the global velocity of the ego vehicle. More details on these transformations can be found in [1].

/** Highway map class. 
 * 
 * The highway is represented by its center line and is defined by the 
 * waypoints in the map file which hold 
 * 
 *    - the global x and y coordinates 
 *    - the Frenet s coordinate 
 *    - and the components of the Frenet unit normal vector n_r = (dx, dy) 
 * 
 * However, the raw waypoints are approximatly 30 meters apart from each other 
 * which is too coarse to be used directly for the several sub tasks of the 
 * planning problem. We will therefore use splines to fill the gaps. This will 
 * be beneficial for the generation of jerk-minimal polynomial trajectories of 
 * higher precision. 
 * 
 * @param map_file: the file containing the map data
*/
Map::Map(std::string map_file)

Stage 2: The Behaviour Planner

The main objective of the behaviour planner is to suggest legal and efficient driving maneuvers, allowing the ego vehicle to safely navigate at the current driving scenario. These maneuvers will be provided to the local planner on the next stage, whose job is to generate sets of trajectories for each of them.

For the determination of the maneuvers the behaviour planner will consider

the rules of the road and
static or dynamic objects around the ego vehicle.

However, it is not responsible for

the choice of the best of the proposed maneuvers,
the maneuver’s execution details or
the collision avoidance.

These tasks will be handled by the local planner.

We will implement the concept of finite state machines (FSM) as behaviour planner. A finite state machine is composed of states and transitions between them. The states represent the driving maneuvers, and the transitions define the conditions required to move from one state to another. We will define two types of finite state machines, a lateral and a longitudinal one.

The lateral FSM will propose maneuvers for the movement perpendicular to the road, that is

keeping the lane,
single lane changes to the left or right,
double lane changes to the left or right.

Typically, there are several viable maneuvers for a given driving scenario. The FSM will propose all of them.

Keeping the lane is always a valid option. It can be reached from every other state without a transition condition. However, all other states, namely the lane changes, are dependent on the current driving scenario. For instance, a double lane change to the left is only possible if the ego vehicle is currently in the right lane and both the center and left lanes are free. Free in terms of the surrounding vehicles leaving sufficient time and space for the ego vehicle to safely complete the lane change.

Each of the proposed lateral maneuvers has a specific target lane. E.g., if the ego vehicle is currently in the right lane and examines a lane change to the left, the center lane is the target lane. For each of the proposed target lanes, the longitudinal FSM will suggest a suitable maneuver for the movement in road direction. Available longitudinal maneuvers are to

keep the velocity if the target lane is free,
follow the leading vehicle in the target lane if one is present,
fallback if the ego vehicle is trapped between adjacent vehicles.

Each of these states can be reached from every other one. The only requirement is the fulfillment of the corresponding transition condition. This allows the consideration of evolving driving scenarios. For instance, if another vehicle suddenly merges into the ego vehicle’s front in the current lane, the ego vehicle will begin to follow it, keeping a safety distance.

The presented finite state machines are implemented in the method EgoVehicle::get_successor_states. This method returns a vector of Successor state objects, of which each defines a viable composite maneuver. A composite maneuver is the combination of a lateral and a longitudinal maneuver. For example, Successor(LAT:KEEP, LON:FOLLOW) defines a successor state to keep the lane and follow the leading vehicle.

/** Determine the possible successor states. 
 * 
 * The successor states are determined based on the ego vehicle's current lane 
 * and the nearby and/or leading vehicles. Combinations of the following 
 * lateral and longitudinal successor states are possible: 
 * 
 * Lateral successor states: 
 *  a) keeping the lane 
 *  b) single lane changes to the left or right 
 *  c) double lane changes to the left or right 
 * 
 * Each lateral successor state has a specific target lane for which the 
 * corresponding longitudinal successor state will be determined.
 * 
 * Longitudinal successor states: 
 *  a) keep velocity if no leading vehicle is in the target lane 
 *  a) follow a leading vehicle in the target lane
 *  c) fallback if deadlocked and the opposite lane is free 
 * 
 * Returns a vector of possible successor state objects. Each successor state 
 * object is defined by a combination of lateral and longitudinal successor 
 * states, e.g. Successor(LAT::KEEP, LON::FOLLOW) defining a follow while 
 * keeping lane successor state.
*/
vector<Successor> EgoVehicle::get_successor_states()

Stage 3: The Local Planner

The local planner is the workhorse of the planning process. Its main objectives are to

create sets of trajectory candidates for each of the maneuvers proposed by the behaviour planner,
check them for collisions with other obstacles and for their physical feasibility,
rate the comfort- and effectiveness of each trajectory, and to
choose the optimal successor trajectory of all viable ones.

The ego vehicle will follow the chosen optimal trajectory up to the next replanning cycle.

Jerk-minimal polynomials

Optimality could be defined to be the best compromise between ease and comfort perceived from within the car, and the time it takes to execute the desired maneuver. From a mathematical viewpoint, ease and comfort may be best described by the jerk: the change of acceleration over time. However, the jerk is not only important for reasons of comfort. Jerky maneuvers are difficult do predict for other traffic participants. They should be avoided in favor of smooth movements, which adapt much better to the traffic flow. Due to these reasons, it is convenient to describe the trajectories as a jerk-minimal connection from a start state to a goal state within a desired time interval T. This can be achieved by minimzing a specific cost functional: the time integral of squared jerk.

As a necessary condition for the minimization of this functional, the Euler-Lagrange equations need to be satisfied [3].

Since the functional is only dependent on the jerk, the terms of lower order partial derivatives vanish. This leads to the ordinary sixth-order differential equation

which has the solution

This is a quintic polynomial with six unknown coefficients. It describes the vehicle’s position in time. The first and second derivatives are the vehicle’s velocity and acceleration.

The six unknown coefficients can be determined by solving a system of linear equations as shown here. Six boundary conditions need to be defined. These boundary conditions describe the vehicle’s state (its position, velocity and acceleration) at the start t=0 and the end t=T of the trajectory. The trajectory duration T is also an adjustable parameter.

Having determined the coefficients, it is possible to formulate the ego vehicle’s jerk-minimal motion in time between the starting state X₀, the end state X₁, and within the time interval T in a closed form.

In many situations, such as driving with no vehicles directly ahead, the ego vehicle does not necessarily need to be at a certain end location, but only needs to adapt to a desired end velocity. The final position of the vehicle is not required in the end state. With one degree of freedom less, a quartic polynomial is sufficient to describe the jerk-minimal vehicle dynamics.

To solve for the five unknown coefficients only five boundary conditions are required.

Motion planning in the Frenet frame

The local planner will generate a set of trajectory candidates for each of the maneuvers proposed by the behavioural level. Thereby, in a first step, the motion planning will be performed separately for the lateral and longitudinal movement, both described by polynomials defined in the Frenet space. In a subsequent step the lateral and longitudinal polynomials will be combined to composite trajectories, describing the global movement of the ego vehicle. For the definition of the required quintics and quartics, a Polynomial class is implemented in src/polynomials.cpp.

/** Create a jerk minimal trajectory by solving a quintic or quartic polynomial. 
 * 
 * The determined trajectory describes the movement of the ego vehicle in one 
 * of the Frenet s- or d- dimensions from an initial state to a final state 
 * during the time T. The start and end states define the boundary conditions 
 * at the beginning and the end of the trajectory. Two kinds of polynomials are 
 * considered in this implementation: quintics and quartics. 
 * 
 * Quintics: 
 * Quintics are 5th degree polynomials with six unknown coefficients describing 
 * the ego vehicle's jerk minimized motion in time. 
 * 
 *    x = a0 + a1 * t + a2 * t^2 + a3 * t^3 + a4 * t^4 + a5 * t^5
 * 
 * In order to be able to solve for the six unknown coefficients six variables 
 * need to be defined as boundary conditions: the ego vehicle's position, 
 * velocity and acceleration at the trajectory's start and end.
 * 
 *    start_state = {start_pos, start_vel, start_acc}
 *    final_state = {final_pos, final_vel, final_acc}
 * 
 * Quartics: 
 * In many situations, such as driving with no vehicles directly ahead, the 
 * ego vehicle does not necessarily have to be at a certain position but needs 
 * to adapt to a desired velocity given by the behavioral level. That's why the 
 * final location of the car is not required in the boundary condition at the 
 * end of trajectory. With one degree of freedom less a quartic polynomial is 
 * sufficient to describe the jerk minimized motion in time. 
 * 
 *    x = a0 + a1 * t + a2 * t^2 + a3 * t^3 + a4 * t^4
 * 
 * with boundary states
 * 
 *    start_state = {start_pos, start_vel, start_acc}
 *    final_state = {final_vel, final_acc}
 * 
 * @param start: inital state vector of the vehicle, e.g. {s_pos, s_vel, s_acc}
 * @param end: final state vector of the vehicle, e.g. {s_vel, s_acc}
 * @param T: duration in seconds required to travers the trajectory
 * @param degree: polynomial degree, DEG::QUINTIC (5) or DEG::QUARTIC (4)
 * @param dof: degree of freedom of trajectory, FRE::S (0) or FRE::D (1)
 */
Polynomial::Polynomial(const std::vector<double> start, 
                       const std::vector<double> end, 
                       double T, int degree, int dof)

This class comes with several methods, which amongst others are:

Polynomial::solve_quintic / Polynomial::solve_quartic to solve the linear system of equations and determine the polynomial’s coefficients. These methods are called automatically at the Polynomial’s initialization.
Polynomial::evaluate to compute the discrete trajectory waypoints in the Frenet frame and perform first feasibility checks
Polynomial::compute_cost to determine the polynomial’s cost

Lateral movement

The lateral maneuvers proposed by the behaviour planner involve lane keeping and lane changing. For each of these maneuvers a set of lateral polynomial candidates will be generated. To avoid discontinuities in the transition from the previous trajectory into the updated candidates, the start state D₀ of the candidates will be shifted 0.5 seconds into the future. The ego vehicle will follow the previous trajectory up to this point. Since the simulator expects a waypoint every Δt=20ms, the transition time interval corresponds to NUM_TRANSITION_WPTS = 25 waypoints. Each polynomial candidate will have the same start state

The start state will be combined with different end states D₁ and trajectory durations T. At the end of each maneuver, the ego vehicle shall be aligned with the road, thus parallel to the center line. We therefore let the end velocity and acceleration be zero. The parameters to vary are the end positions d_i, and the trajectory durations T_j

The end positions variants d_i are determined by adding different perpendicular offsets Δd_i to the target lane’s center d_target

When keeping the lane, the target lane center equals the current lane center. By setting the target lane center to an adjacent lane, lane changes can be achieved [2].

A cost functional C_d is defined, to measure each trajectory’s performance in terms of comfort and efficiency. It penalizes solutions that are jerky, of slow convergence and off from the desired target location. The location term is additionally increased by a lane cost C_lane which disvafours crowded target lanes. Each cost term gets its own weight factor.

The following table summarizes the context.

The generation of the lateral trajectory sets is performed by the method EgoVehicle::get_d_polys

/** Create a set of lateral polynomials. 
 * 
 * All polynomial candidates in the set will have the same starting state which 
 * is NUM_TRANSITION_WPTS in the future beginning to count at the waypoint 
 * the replanning began. 
 *
 *        d_start = {d_pos, d_vel, d_acc}. 
 * 
 * The set of polynomials is created for all durations T_j in T_set and with 
 * varying end locations d_i perpendicular to the target lane's center. The 
 * lateral end velocities and accelerations are defined to be zero: 
 * 
 *        d_end = {d_i, 0, 0}. 
 * 
 * For instance, if the target lane is the left lane (d = 2) the varying end 
 * locations are d_i = {1.5, 2., 2.5}. The target lane is determined from the 
 * lateral successor state. 
 * 
 * Each polynomial will be evaluated for all waypoints to be replanned. In 
 * case of recovery and emergency scenarios the feasibility checks are eased.
 * 
 * Args:
 *    d_start: starting state for all polynomials in the set
 *    T_set: set of durations to create the polynomials for 
 *    successor: successor state object with longitudinal/lateral targets
 *    scenario: driving scenario case (standard, recovery or emergency)
 */
vector<Polynomial> EgoVehicle::get_d_polys(
  vector<double> d_start, 
  vector<double> T_set, 
  Successor successor,
  int scenario)

The computation of the lane cost term C_lane is perfomed by the method EgoVehicle::get_lane_cost

/** Compute a lane cost depending on the closeness of the surrounding vehicles. 
 * 
 * The following rules apply: 
 *  a) Lanes without leading vehicle's have zero cost 
 *  b) The cost increases linearly with the leading vehicle's closeness 
 *  c) The ego vehicle's current lane is favored and gets a lower cost 
 *  d) If the ego vehicle is deadlocked its current lane gets an increased cost 
 */
vector<double> EgoVehicle::get_lane_cost()

Longitudinal movement

The behaviour planner provides longitudinal maneuvers to keep the velocity, follow the leading vehicle or fallback, if the ego vehicle is trapped between adjacent vehicles. For each of these maneuvers we need to determine the boundary conditions required to generate the polynomial candidates. As with the lateral movement, the start state S₀ will be defined NUM_TRANSITION_WPTS = 25 in the future, in order to achieve a smooth transition from the previous trajectory into the new candidates, .

The start state is the same for all maneuvers.

Keeping velocity

If the ego vehicle’s target lane is free, there is no specific end location to track, and the end position will be left as degree of freedom. Quartic polynomials are sufficient to describe the longitudinal movement. Since no vehicles are directly ahead, the ego vehicle will adapt to a desired target velocity, usually the speed limit. At the end of the trajectory the acceleration shall be zero. The parameters to vary are the end velocity and the trajectory duration.

Usually, the target velocity is simply the speed limit. However, if the ego vehicle is moving with a low speed, the target velocity will be the velocity achievable, when speeding up with the maximum feasible acceleration.

The end velocity variants will have the speed limit as a maximum, since the ego vehicle shall adapt to, but not exceed it.

The cost functional penalizes solutions that are jerky, of slow convergence, and velocities differing from the speed limit.

Following and Fallback

If the target lane is not free, we require trajectories, that describe the transfer from the start state to the target position at a safety distance behind the leading vehicle. Thus, the movement of the leading vehicle needs to be predicted. Since the simulator’s sensor fusion only provides information about the other vehicles’ current velocity, it is simplifying assumed, that they will keep moving without any acceleration. This corresponds to the simplest model based prediction model: the constant velocity prediction.

The leading vehicle’s end location can be predicted by computing the distance travelled with its current velocity in the time T_j, starting from its position at the beginning of the trajectory

The ego vehicle’s target location is at a safety distance behind the leading vehicle.

Fallback maneuvers try to free the ego vehicle if it is trapped between adjacent vehicles. This is done by increasing the distance to the leading vehicle (and the adjacent vehicle) in order to eventually offer a possibility to change the lane. Thus, the target location for fallback maneuvers is farther behind the leading vehicle, than for follow maneuvers.

The safety distance is the way required to come to a complete halt, if the leading vehicle does so too. It is defined by the constant time gap law with the constants σ₀ and τ. As the prediction model assumes the leading vehicle’s velocity to be constant, the safety distance is constant too.

Since a desired target location exists at the end of the trajectory, we require quintic polynomials for the generation of the longitudinal trajectory sets. The target velocity at the end of the trajectory is dependent on the current maneuver. The parameters to vary are the end positions s_i and the trajectory durations T_j.

The end position variants s_i are determined by subtracting different longitudinal offsets Δs_i from the target position s_targ, which we derived above.

The target velocity for following maneuvers is simply the leading vehicle’s velocity.

The target velocity for fallback maneuvers is the leading vehicle’s velocity reduced by some amount.

The cost functional penalizes solutions that are jerky, of slow convergence, do not track the speed limit, and are far off the desired target location.

The following table summarizes the context.

The generation of the longitudinal trajectory sets is performed by the method EgoVehicle::get_s_polys

/** Create a set of longitudinal polynomials. 
 * 
 * All polynomial candidates in the set will have the same starting state which 
 * is NUM_TRANSITION_WPTS in the future beginning to count at the waypoint 
 * the replanning began. 
 *
 *        s_start = {s_pos, s_vel, s_acc}. 
 * 
 * The polynomials will be created for all durations T_j in T_set and with 
 * varying end states depending on the longitudinal successor state. 
 * 
 * LON::KEEP: 
 * If the target lane is free, a set of quartic polynomials will be created,
 * in order to keep the velocity. The end states will 
 *
 *        a) leave the location as a degree of freedom, 
 *        b) contain variations of velocities (s_vel_i) varying up to the 
 *           velocity limit and
 *        c) have zero acceleration (s_acc = 0). 
 *
 *        >>> s_end = {s_vel_i, 0}
 * 
 * LON::FOLLOW: 
 * If there is a leading vehicle in the target lane and it is driving slower 
 * than the maximum allowed velocity, a set of quintic following polynomials 
 * will be created. The end states will have  
 *
 *        a) varying end locations (s_pos_i) of which the closest one is at 
 *           a safety distance behind the leading vehicle, 
 *        b) a speed equal to the leading vehicle's one (s_vel_lead) and 
 *        c) zero acceleration (s_acc = 0). 
 *
 *        >>> s_end = {s_pos_i, s_vel_lead, 0} 
 * 
 * LON::FALLBACK: 
 * If the ego vehicle is deadlocked in its current lane and its opposite lane 
 * is free, a set of quintic following polynomials will be created. The end 
 * states will have  
 *
 *        a) varying end locations (s_pos_i) keeping larger distances to the  
 *           leading vehicle in the current lane, than in the follow maneuver 
 *        b) a speed equal to the deadlock causing vehicle in the adjacent 
 *           lane (s_vel_adjacent) 
 *        c) zero acceleration (s_acc = 0). 
 *
 *        >>> s_end = {s_pos_i, s_vel_adjacent, 0} 
 *    
 * 
 * Each polynomial will be evaluated for all waypoints to be replanned. In 
 * case of recovery and emergency scenarios the feasibility checks are eased.
 * 
 * @param s_start: starting state for all polynomials in the set
 * @param T_set: set of durations to create the polynomials for
 * @param successor: successor state object with longitudinal/lateral targets 
 * @param scenario: driving scenario case (standard, recovery or emergency)
 */
vector<Polynomial> EgoVehicle::get_s_polys(
  vector<double> s_start, 
  vector<double> T_set, 
  Successor successor, 
  int scenario)

Global movement

Having determined the lateral and longitudinal trajectory sets, they need to be combined to composite trajectories. These are transformed back from the Frenet into the global system, where they are checked for their physical feasibility and for collisions. In order to reduce the computational effort, the polynomials with excessively high jerks or accelerations relative to the road were already sorted out in the generation step.

As for the collision determination, a simple circle-to-circle collision check is performed. Three circles define each vehicle’s contour. The simple constant velocity prediction model shows its issues here. Since it only considers the movement of the other vehicles along the road, their lane changes can not be predicted by it. This is a hazard to the ego vehicle. We will therefore consider possible lane changes in a different manner: If the vehicle is not near the center of its current lane, it could be about to change the lane. Conservatively, it will be treated as like being in both its current and the nearest adjacent lane. Of course, this is also a very basic approach. There are many other prediction models out there, e.g., data driven approaches or multi-modal estimators. However, the integration of an advanced prediction model is beyond the scope of this project.

There may be cases in which no feasible or collision free trajectory could be determined. Then, the ego vehicle switches from the SCN::STANDARD into the SCN::RECOVERY mode. The same process is repeated with eased feasibility constraints: safety before comfort. If still no viable trajectory could be found, the SCN::EMERGENCY mode gets active. The feasibility constraints are further eased, and the hierarchical zero/one collision check is replaced by a cost based approach. It may be better to have a trajectory with a controlled collision, than no trajectory at all. To assure, that the potential collision has as low an impact as possible, the trajectory cost increases with each collision event, taking into account the degree of overlap, and the velocity difference of the involved vehicles.

The cost of each remaining feasible and collision-free composite trajectory is calculated as the weighted sum of its lateral and longitudinal cost components. Finally, the trajectory with the minimum cost is passed to the controller on the next stage.

A Trajectory class is implemented in src/vehicles.cpp for the definition of the composite trajectories. The class provides the methods Trajectory::update for the waypoint computation and feasibility checks and Trajectory::check_collision for the collision checks. The ego vehicle attribute EgoVehicle::trajectory is always the most current composite trajectory, and it is updated at each replanning cycle.

The first composite trajectory is initialized by EgoVehicle::init_trajectory right after connecting to the simulator.

/** Initialize the ego vehicle's trajectory at the beginning of the simulation. 
 * 
 * The starting state is the initial ego vehicle's position without movement: 
 * 
 *    s_start = {s_init, 0, 0} 
 *    d_start = {d_init, 0, 0} 
 * 
 * The end state is the longitudinal velocity limit while keeping the starting lane: 
 * 
 *    s_end = {s_vel_limit, 0} 
 *    d_end = {d_init, 0, 0} 
 * 
 * The initial maneveur has a duration T of 4 sec to achieve a smooth start: 
 * 
 *    T = 4
 * 
 * However, the initialized trajectory will be updated in the first replaning 
 * cycle anyway. 
 */
void EgoVehicle::init_trajectory() {

  // Definition of the boundary states at start and end
  vector<double> s_start{this->s_pos, 0, 0};
  vector<double> d_start{this->d_pos, 0, 0};
  vector<double> s_end{this->s_vel_limit, 0};
  vector<double> d_end{this->d_pos, 0, 0};
  double T = 4;

  // Initialize the longitudinal (quartic) and lateral (quintic) trajectories 
  Polynomial s_quartic, d_quintic;
  s_quartic = Polynomial(s_start, s_end, T, DEG::QUARTIC, FRE::S);
  d_quintic = Polynomial(d_start, d_end, T, DEG::QUINTIC, FRE::D);

  // Compute the polynomial costs
  s_quartic.compute_cost();
  d_quintic.compute_cost(this->d_pos, this->lane_cost);

  // Compute discrete waypoints for the polynomials in Frenet space
  // The initial polynomials are evaluated for the complete planning horizon
  s_quartic.evaluate(NUM_TRAJECTORY_WPTS);
  d_quintic.evaluate(NUM_TRAJECTORY_WPTS);
  
  // Initialize the vehicles's composite trajectory and its waypoints
  this->trajectory = Trajectory(s_quartic, d_quintic, this->map);
}

At each replanning cycle the optimal trajectory is updated by EgoVehicle::update_trajectory

/** Update the trajectory based on the current simulator data. 
 * 
 * The replaning will take place in the moment the simulator has consumed  
 * num_consumed_wpts > REPLAN_THRESHOLD waypoints. Since the computation of 
 * the updated trajectory will take some time in which the vehicle will keep 
 * moving and consuming waypoints, the starting state of the updated trajectory 
 * will be defined a few timesteps in the future. This will allow a smooth 
 * transition from the current to the updated trajectory. 
 * 
 * @param num_consumed_wpts: number of consumed waypoints up to now  
 */
void EgoVehicle::update_trajectory(int num_consumed_wpts) {
  
  // Define polynomial starting states
  // The update of the trajectory polynomials will begin NUM_TRANSITION_WPTS 
  // in the future, starting to count from the first waypoint not jet consumed 
  // when the replaning began. 
  int starting_wpt = num_consumed_wpts + NUM_TRANSITION_WPTS;
  vector<double> s_start{
    this->trajectory.s_pos_wpts[starting_wpt],
    this->trajectory.s_vel_wpts[starting_wpt],
    this->trajectory.s_acc_wpts[starting_wpt]};
  vector<double> d_start{
    this->trajectory.d_pos_wpts[starting_wpt],
    this->trajectory.d_vel_wpts[starting_wpt],
    this->trajectory.d_acc_wpts[starting_wpt]};

  // Initialize the optimal trajectory as empty trajectory
  Trajectory optimal_trajectory;

  // Determine the possible successors states
  vector<Successor> successors = get_successor_states();

  // Standard driving scenario: 
  // Determine the optimal trajectory out of all successor state trajectories
  optimal_trajectory = get_optimal_trajectory(
    s_start, d_start, successors, num_consumed_wpts, SCN::STANDARD);

  // Recovery driving scenario:
  // If for some reason (collisions/feasibility) no optimal trajectory could be 
  // generated, try to recover the state with eased feasibility constraints
  if (optimal_trajectory.is_empty) {
    optimal_trajectory = get_optimal_trajectory(
      s_start, d_start, successors, num_consumed_wpts, SCN::RECOVERY);
  }

  // Emergency driving scenario:
  // If the recovery was not successful, start an emergency maneuver. The 
  // feasibility constraints are eased and the collision check is cost based 
  // instead of a hierarchical zero/one decision.
  if (optimal_trajectory.is_empty) {
    optimal_trajectory = get_optimal_trajectory(
      s_start, d_start, successors, num_consumed_wpts, SCN::EMERGENCY);
  }

  if (!optimal_trajectory.is_empty) {
    // Replace the current trajectory by the new optimal one
    this->trajectory = optimal_trajectory;
  } else {
    printf("No optimal trajectory available. \n");
  }
}

The method makes use of EgoVehicle::get_optimal_trajectory to determine the optimal trajectory of all the possible successor states and EgoVehicle::get_best_candidate to determine the best trajectory candidate of each successor state.

Stage 4: The Motion Control

On the last stage, the vehicle controller is responsible for the execution of the optimal trajectory delivered by the local planner. The controller is already implemented in the simulator. It requires as input a vector of discretized waypoints describing the optimal trajectory in the global system. It is kind of an ideal controller, since it will perfectly follow each provided waypoint every 20ms. Since the trajectory is based on the time dependent lateral and longitudinal polynomials, of which we know the closed form, we simply need to evaluate the jerk-minimal trajectories in time steps of Δt=20ms.

The evaluation and feasibility checks in the Frenet system are performed by the method Polynomial::evaluate, and the composition and transformation into the global system by Trajectory::update.

Results

The following snippets give an insight on how the algorithm works in the simulator.

Snippets

Collision Avoidance	Maneuver Selection

Full Lap

References

[1] Moritz Werling, Julius Ziegler, Sören Kammel, and Sebastian Thrun. Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet Frame. IEEE International Conference on Robotics and Automation, Anchorage, 2010

[2] Moritz Werling. Ein neues Konzept für die Trajektoriengenerierung und -stabilisierung in zeitkritischen Verkehrsszenarien. Schriftenreihe des Instituts für Angewandte Informatik - Automatisierungstechnik, Karlsruher Institut für Technologie, 2011

[3] A. Takahashi, T. Hongo, Y. Ninomiya, and G. Sugimoto. Local path planning and motion control for AGV in positioning. IEEE/RSJ International Workshop on Intelligent Robots and Systems, Tsukuba, Japan, 1989

PREVIOUSPID Controller

NEXTKidnapped Vehicle