Self Driving Car Engineer Nanodegree Program -- Path Planning 3.4 Bahavior Planning

3.4 Bahavior Planning

3.4.1 Lesson Outline

  The behavior planning team is responsible for providing guidance to trajectory planner about what sorts of maneuvers they should plan trajectories for.

3.4.3 Uderstanding Output

行为决策层的输出结果的格式定义：

3.4.4 Behavior Planning Overview

  To summarize, the bahavior planner is currently a black box which takes as input a map of the world, a route to the destinations, and predictions about what other static and dynamic obstacles are likely to do. and it produces as output a adjutsted maneuver for the vehicle which the trajectory planner is responsible for reaching collision-free, smooth and safe.

  So the responsibilities of the behavior module are to suggest maneuvers whick are feasible, as safe as possible, legal and efficient. What the behavior planner is not responsible are execution details and collision avoidance.

3.4.5 Finite State Machines

  A finite state machine makes decisions based on a finite set of discrete states. When initialized, a Finite State Machine begins in some start state. Any pair of states within the finite state machine can be connected by one or more transitions. And sometimes there is a transition back to the same state. This is called a self transition. Not all transitions are necessarily possible. In the language of finite state machines this would be called an Accepting State.
  A Finite State Machine needs to handle some sort of input and then use a state transition function to decide what state to go next.

  Finite State Machines have their strengths and weaknesses.

• Strengths:
  • Very easy to reason about. They are basically self-documenting because they map the logical state of a system directly to the physical state.
  • Maintainable for small state space.
• Weakness:
  • Easily abused
  • Not maintainable for large state space.

3.4.8 States for Self Driving Cars

  On one hand we want to keep our state spaces as small as possible for maintainability reasons, but on the other hand we want to make sure we have enough logical states to acutally represent all the physical states that we care about.

Vehicle Behavior on a highway.

• Accelerate

• Stop

• Keep target speed

• Slow down

• Keep lane

• Prepare lane change left

• Prepare lane change right

• Change lane right

• Change lane left

• Follow vehicle
  

3.4.10 Inputs to Transition Functions

Deciding how those states transition and what inputs the transition functions use is crucial to the actual implementation of a finite state machine.

Data needed to pass to our transition functions as input:

• Predictions
• Map
• Speed limit
• Localization Data
• Current state

3.4.11 Behavior Planning Pseudocode

 One way to implement a transition function is by generating rough trajectories for each accessible “next state” and then finding the best.
 To “find the best” we generally use cost functions. We can then figure out how costly each rough trajectory is and then select the state with the lowest cost trajectory.

Pseudocode for how a transition function might work

def transition_function(predictions, current_fsm_state, current_pose, cost_functions, weights):
    # only consider states which can be reached from current FSM state.
    possible_successor_states = successor_states(current_fsm_state)

    # keep track of the total cost of each state.
    costs = []
    for state in possible_successor_states:
        # generate a rough idea of what trajectory we would
        # follow IF we chose this state.
        trajectory_for_state = generate_trajectory(state, current_pose, predictions)

        # calculate the "cost" associated with that trajectory.
        cost_for_state = 0
        for i in range(len(cost_functions)) :
            # apply each cost function to the generated trajectory
            cost_function = cost_functions[i]
            cost_for_cost_function = cost_function(trajectory_for_state, predictions)

            # multiply the cost by the associated weight
            weight = weights[i]
            cost_for_state += weight * cost_for_cost_function
         costs.append({'state' : state, 'cost' : cost_for_state})

    # Find the minimum cost state.
    best_next_state = None
    min_cost = 9999999
    for i in range(len(possible_successor_states)):
        state = possible_successor_states[i]
        cost  = costs[i]
        if cost < min_cost:
            min_cost = cost
            best_next_state = state 

    return best_next_state

3.4.12 Create a Cost Function - Speed Penalty

  A key part of getting transitions to happen when we want them to is design of reasonable cost functions. We want to penalize and reward the right things.
  On one hand, we want to get to our destination quickly, but on the other hand, we don’t want to break the law. An essential quantity we have to control is the desired velocity of the car.
  For the sake of simplicity, let’s assume that all of the cost functions will have an output between zero and one. We will adjust the importance of each cost function later by adjusting the weights.
  If we are going well above the speed limit that should be maximum cost. We set an ideal zero cost speed that’s slightly below the speed limit so that we have some buffer. And then we can think about how much we want to penalize not moving at all. Obviously, not moving is bad, but maybe not as bad as breaking the speed limit.

3.4.13 Example Cost Function - Lane Change Penalty

  In the image above, the blue self driving car (bottom left) is trying to get to the goal (gold star). It’s currently in the correct lane but the green car is going very slowly, so it considers whether it should perform a lane change (LC) or just keep lane (KL). These options are shown as lighter blue vehicles with a dashed outline.

  If we want to design a cost function that deals with lane choice, it will be helpful to establish what the relevant variables are. In this case, we can define:

• $\Delta s=s_G-s$ how much distance the vehicle will have before it has to get into the goal lane.
• $\Delta d=d_G-d_{LC/KL}$ the lateral distance between the goal lane and the options being considered. In this case $\Delta d_{KL}=d_G-d_{KL}$ would be zero and $\Delta d_{LC}=d_G-d_{LC}$ would not.

So we want a cost function that penalizes large |\Delta d|∣Δd∣ and we want that penalty to be bigger when \Delta sΔs is small. Furthermore, we want to make sure that the maximum cost of this cost function never exceeds one and that the minimum never goes below zero.
惩罚变换车道的目标函数项
$$\text{cost}=1-e^{-\frac{|\Delta d|}{\Delta s}}$$

3.4.14&15 Implement a Cost Function in C++ and solution

  A cost function to choose a lane when trying to reach a goal in highway driving:

$$\text{cost}=1-e^{-\frac{|\Delta d|}{\delta s}}$$

 Here, $\Delta d$ was the lateral distance between the goal lane and the final chosen lane, and $\Delta s$ was the longitudinal distance from the vehicle to the goal.

  Here is one possible solution for the previous quiz:

double goal_distance_cost(int goal_lane, int intended_lane, int final_lane, double distance_to_goal) 
{
  // The cost increases with both the distance of intended lane from the goal
  //   and the distance of the final lane from the goal. The cost of being out 
  //   of the goal lane also becomes larger as the vehicle approaches the goal.
  int delta_d = 2.0 * goal_lane - intended_lane - final_lane;
  double cost = 1 - exp(-(std::abs(delta_d) / distance_to_goal));

  return cost;
}

3.4.16 Implement a Second Cost Function in C++

  In most situations, a single cost function will not be sufficient to produce complex vehicle behavior. In this quiz, we’d like you to implement one more cost function in C++. We will use these two C++ cost functions later in the lesson. The goal with this quiz is to create a cost function that would make the vehicle drive in the fastest possible lane, given several behavior options. We will provide the following four inputs to the function:

• Target speed: Currently set as 10 (unitless), the speed at which you would like the vehicle to travel.
• Intended lane: the intended lane for the given behavior. For PLCR, PLCL, LCR, and LCL, this would be the one lane over from the current lane.
• Final lane: the immediate resulting lane of the given behavior. **For LCR and LCL, this would be one lane over.**对于变道行为，期望车道和最终车道是一条
  A vector of lane speeds, based on traffic in that lane: {6, 7, 8, 9}.
  Your task in the implementation will be to create a cost function that satisifes:

The cost decreases as both intended lane and final lane are higher speed lanes.
The cost function provides different costs for each possible behavior: KL, PLCR/PLCL, LCR/LCL.
The values produced by the cost function are in the range 0 to 1.

  Here is one possible solution for the previous quiz:

double inefficiency_cost(int target_speed, int intended_lane, int final_lane, 
                         const std::vector<int> &lane_speeds) {
  // Cost becomes higher for trajectories with intended lane and final lane 
  //   that have traffic slower than target_speed.
  double speed_intended = lane_speeds[intended_lane];
  double speed_final = lane_speeds[final_lane];
  double cost = (2.0*target_speed - speed_intended - speed_final)/target_speed;

  return cost;
}

3.4.18 Cost Fucntion Design and Weight Tweaking

Difficulities

• Solving new problems without “unsolving” old ones.
  • By regression testing! (Wherever we make a change, we simulate the vehicle in all of out test cases and make sure that it still behaves as expected.
• Balancing costs of drastically different magnitudes.
  • Feasibility >> Safety >> Legality >> Comfort >> Efficiency:
    One way to do that is to have weights which reflect the type of problem the cost function addresses. So we want to most heavily penalize any behavior which simply isn’t possible due to physics, then we want to think about safety, legality, comfort. And only once those are satisfied do we want to think about efficiency.
  • Weights can change depending on situatiuon. If a light turns red, for example, legality becomes a much more relevant concern than we engage in normal highway driving.
• Resonsing about individual cost functions.(推导出每一个成本函数）
  • Specificity of cost function responsibility: Idealy, each cost function will serve a very specific responsibility,
  • Binary vs Discrete vc Continuous cost functions: In which case we might have a binary cost function which just checks to see if we are breaking the speed limit and the continuous cost function which pulls us towards our target speed.
  • All CFs output between -1 and 1: By assigning each cost function to a very specific role, like safety versus legality versus efficiency, we can then standardize the output of all cost functions to be between -1 and 1.
  • Parametrizaition (when possible): This allows us to use some parameter optimization technique like gradient descent along with our set of regression tests to programmatically tweak our cost functions.
  • Thinking in terms of vehicle state (position, velocity, acceleration)

It’s pretty hard to avoid this exploding complexity when using finite state machines.

3.4.19 Cost Function Matching

 

COST FUNCTION VERBAL DESCRIPTION	COST FUNCTION NUMBER
Penalizes trajectories that do not stay near the center of the lane.	$$\frac{1}{1+e^{-{\left(d-d\_\text{lane center}\right)}^2}}$$
Penalizes trajectories that attempt to accelerate at a rate which is not possible for the vehicle.	$$\{\begin{array}{ll}1,& \ddot{s}>a\_\text{max};\\ 0,& \ddot{s}<a\_\text{max}.\end{array}$$
Rewards trajectories that stay near the target lane.	$${\left(\text{lane number}-\text{target lane number}\right)}^2$$
Penalizes trajectories that drive off the road.	$$\{\begin{array}{ll}1,& d\ge d\text{max}\\ 1,& d\le d\_\text{min}\\ 0,& d\_\text{min}<d<d\_max\end{array}$$
Penalizes trajectories that exceed the speed limit.	$$\{\begin{array}{ll}1,& \dot{s}\ge v\_\text{speed limit}\\ 0,& \dot{s}<v\_\text{speed limit}\end{array}$$

3.4.20 Scheduling Compute Time

  The high level decisions made in the behavior module spend a longer time horizon and just don’t change as frequently.
​