Udacity 4.Behavior_Planning 2

udacity4_Behavior_Planning 2

创建合理的cost函数很重要

为车辆速度设计合适的损失函数

速度快，但是又不能超速

也不能完全停止

完成一个车道保持/变道之间的 损失函数

$\operatorname{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.

损失函数产生的值在0-1范围内

.h函数

#ifndef COST_H
#define COST_H
//goal_lane：目标道路
//intended_lane：预期车道 有助于计算检查向左、右变道是否可行
//final_lane：the lane of the vehicle at the end of the trajectory
// distance_to_goal：举例目标的举例
double goal_distance_cost(int goal_lane, int intended_lane, int final_lane, 
                          double distance_to_goal);

#endif  // COST_H

cost 函数

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;
}