【Apollo学习笔记】——规划模块TASK之SPEED_DECIDER

TASK系列解析文章

1.【Apollo学习笔记】——规划模块TASK之LANE_CHANGE_DECIDER
2.【Apollo学习笔记】——规划模块TASK之PATH_REUSE_DECIDER
3.【Apollo学习笔记】——规划模块TASK之PATH_BORROW_DECIDER
4.【Apollo学习笔记】——规划模块TASK之PATH_BOUNDS_DECIDER
5.【Apollo学习笔记】——规划模块TASK之PIECEWISE_JERK_PATH_OPTIMIZER
6.【Apollo学习笔记】——规划模块TASK之PATH_ASSESSMENT_DECIDER
7.【Apollo学习笔记】——规划模块TASK之PATH_DECIDER
8.【Apollo学习笔记】——规划模块TASK之RULE_BASED_STOP_DECIDER
9.【Apollo学习笔记】——规划模块TASK之SPEED_BOUNDS_PRIORI_DECIDER&&SPEED_BOUNDS_FINAL_DECIDER
10.【Apollo学习笔记】——规划模块TASK之SPEED_HEURISTIC_OPTIMIZER
11.【Apollo学习笔记】——规划模块TASK之SPEED_DECIDER
12.【Apollo学习笔记】——规划模块TASK之PIECEWISE_JERK_SPEED_OPTIMIZER
13.【Apollo学习笔记】——规划模块TASK之PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER(一)
14.【Apollo学习笔记】——规划模块TASK之PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER(二)

前言

在Apollo星火计划学习笔记——Apollo路径规划算法原理与实践与【Apollo学习笔记】——Planning模块讲到……Stage::Process的PlanOnReferenceLine函数会依次调用task_list中的TASK，本文将会继续以LaneFollow为例依次介绍其中的TASK部分究竟做了哪些工作。由于个人能力所限，文章可能有纰漏的地方，还请批评斧正。

在modules/planning/conf/scenario/lane_follow_config.pb.txt配置文件中，我们可以看到LaneFollow所需要执行的所有task。

stage_config: {
    
    
  stage_type: LANE_FOLLOW_DEFAULT_STAGE
  enabled: true
  task_type: LANE_CHANGE_DECIDER
  task_type: PATH_REUSE_DECIDER
  task_type: PATH_LANE_BORROW_DECIDER
  task_type: PATH_BOUNDS_DECIDER
  task_type: PIECEWISE_JERK_PATH_OPTIMIZER
  task_type: PATH_ASSESSMENT_DECIDER
  task_type: PATH_DECIDER
  task_type: RULE_BASED_STOP_DECIDER
  task_type: SPEED_BOUNDS_PRIORI_DECIDER
  task_type: SPEED_HEURISTIC_OPTIMIZER
  task_type: SPEED_DECIDER
  task_type: SPEED_BOUNDS_FINAL_DECIDER
  task_type: PIECEWISE_JERK_SPEED_OPTIMIZER
  # task_type: PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER
  task_type: RSS_DECIDER

本文将继续介绍LaneFollow的第11个TASK——SPEED_DECIDER

SPEED_DECIDER功能简介

产生速度决策
    根据粗规划出的速度曲线，依据曲线在障碍物的上方还是下方，采取不同的决策。

与路径决策器思路一致，当路径规划器完成路径规划以后，可以得到一条无人车的最优行驶路线，路径决策器就需要对静态障碍物做标签的更新，尤其是那些不在特殊路况下的障碍物，由于缺乏时间信息，路径决策器PathDecider无法对动态障碍物进行标签更新。

而在速度规划器完成未来时间8s或者未来前向距离150m的规划以后，已经得到了一条未来每个时刻无人车出现在参考线上的位置(累积距离s)，再配合事先已经规划好的路径，就可以完成对动态障碍物的标签更新，这里仅仅对最近的st障碍物边界框做标签更新，没有对所有的预测时间进行更新。

SPEED_DECIDER相关配置

SPEED_DECIDER流程

SpeedDecider直接继承自Task类，首先构造函数加载相关配置，接着Execute为函数入口，进行具体执行。Execute调用了函数MakeObjectDecision，而整个SpeedDecider的主要逻辑也正在MakeObjectDecision之中。

• 输入：common::Status SpeedDecider::Execute(Frame* frame, ReferenceLineInfo* reference_line_info) { frame和reference_line_info

因此接下来我们直接来看MakeObjectDecision函数。
主要流程如下图所示：

扫描二维码关注公众号，回复： 16861144 查看本文章

MakeObjectDecision

SpeedDecider根据路径决策、DP算法生成的速度曲线和障碍物的STBoundary的位置关系生成Ignore、Stop、Follow、Yield、Overtake的纵向决策。

Status SpeedDecider::MakeObjectDecision(
    const SpeedData& speed_profile, PathDecision* const path_decision) const {
    
    
  // 检查动态规划的结果
  if (speed_profile.size() < 2) {
    
    
    const std::string msg = "dp_st_graph failed to get speed profile.";
    AERROR << msg;
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }
  // 遍历障碍物
  for (const auto* obstacle : path_decision->obstacles().Items()) {
    
    
    auto* mutable_obstacle = path_decision->Find(obstacle->Id());
    const auto& boundary = mutable_obstacle->path_st_boundary();
    // 若障碍物的STBoundary在ST Graph的范围内，忽略
    if (boundary.IsEmpty() || boundary.max_s() < 0.0 ||
        boundary.max_t() < 0.0 ||
        boundary.min_t() >= speed_profile.back().t()) {
    
    
      AppendIgnoreDecision(mutable_obstacle);
      continue;
    }
    // 若障碍物已经有纵向决策的障碍物，忽略
    if (obstacle->HasLongitudinalDecision()) {
    
    
      AppendIgnoreDecision(mutable_obstacle);
      continue;
    }

    // for Virtual obstacle, skip if center point NOT "on lane"
    // 对于虚拟障碍物，判断其中心是否在车道上，不在则跳过
    if (obstacle->IsVirtual()) {
    
    
      const auto& obstacle_box = obstacle->PerceptionBoundingBox();
      if (!reference_line_->IsOnLane(obstacle_box.center())) {
    
    
        continue;
      }
    }

    // always STOP for pedestrian
    // 遇到行人障碍物，添加stop决策
    if (CheckStopForPedestrian(*mutable_obstacle)) {
    
    
      ObjectDecisionType stop_decision;
      if (CreateStopDecision(*mutable_obstacle, &stop_decision,
                             -FLAGS_min_stop_distance_obstacle)) {
    
    
        mutable_obstacle->AddLongitudinalDecision("dp_st_graph/pedestrian",
                                                  stop_decision);
      }
      continue;
    }
    // 根据路径决策、DP得到的速度曲线、障碍物边界获取STlocation
    auto location = GetSTLocation(path_decision, speed_profile, boundary);
    // 检查KEEP_CLEAR区域是否阻塞
    if (!FLAGS_use_st_drivable_boundary) {
    
    
      if (boundary.boundary_type() == STBoundary::BoundaryType::KEEP_CLEAR) {
    
    
        if (CheckKeepClearBlocked(path_decision, *obstacle)) {
    
    
          location = BELOW;
        }
      }
    }

    switch (location) {
    
    
      case BELOW:
        // 若障碍物类型为KEEP_CLEAR，创建停止决策
        if (boundary.boundary_type() == STBoundary::BoundaryType::KEEP_CLEAR) {
    
    
          ObjectDecisionType stop_decision;
          if (CreateStopDecision(*mutable_obstacle, &stop_decision, 0.0)) {
    
    
            mutable_obstacle->AddLongitudinalDecision("dp_st_graph/keep_clear",
                                                      stop_decision);
          }
        // 检查ADC是否处于跟车的状态
        } else if (CheckIsFollow(*obstacle, boundary)) {
    
    
          // stop for low_speed decelerating
          // 是否跟车距离太近，
          if (IsFollowTooClose(*mutable_obstacle)) {
    
    
            ObjectDecisionType stop_decision;
            // 创建停止决策
            if (CreateStopDecision(*mutable_obstacle, &stop_decision,
                                   -FLAGS_min_stop_distance_obstacle)) {
    
    
              mutable_obstacle->AddLongitudinalDecision("dp_st_graph/too_close",
                                                        stop_decision);
            }
          } else {
    
      // high speed or low speed accelerating
            // FOLLOW decision
            ObjectDecisionType follow_decision;
            if (CreateFollowDecision(*mutable_obstacle, &follow_decision)) {
    
    
              mutable_obstacle->AddLongitudinalDecision("dp_st_graph",
                                                        follow_decision);
            }
          }
        } else {
    
    
          // YIELD decision
          ObjectDecisionType yield_decision;
          if (CreateYieldDecision(*mutable_obstacle, &yield_decision)) {
    
    
            mutable_obstacle->AddLongitudinalDecision("dp_st_graph",
                                                      yield_decision);
          }
        }
        break;
      case ABOVE:
        if (boundary.boundary_type() == STBoundary::BoundaryType::KEEP_CLEAR) {
    
    
          ObjectDecisionType ignore;
          ignore.mutable_ignore();
          mutable_obstacle->AddLongitudinalDecision("dp_st_graph", ignore);
        } else {
    
    
          // OVERTAKE decision
          ObjectDecisionType overtake_decision;
          if (CreateOvertakeDecision(*mutable_obstacle, &overtake_decision)) {
    
    
            mutable_obstacle->AddLongitudinalDecision("dp_st_graph/overtake",
                                                      overtake_decision);
          }
        }
        break;
      case CROSS:
        // 障碍物是否堵塞
        if (mutable_obstacle->IsBlockingObstacle()) {
    
    
          ObjectDecisionType stop_decision;
          // 建立停止决策
          if (CreateStopDecision(*mutable_obstacle, &stop_decision,
                                 -FLAGS_min_stop_distance_obstacle)) {
    
    
            mutable_obstacle->AddLongitudinalDecision("dp_st_graph/cross",
                                                      stop_decision);
          }
          const std::string msg = absl::StrCat(
              "Failed to find a solution for crossing obstacle: ",
              mutable_obstacle->Id());
          AERROR << msg;
          return Status(ErrorCode::PLANNING_ERROR, msg);
        }
        break;
      default:
        AERROR << "Unknown position:" << location;
    }
    AppendIgnoreDecision(mutable_obstacle);
  }

  return Status::OK();
}

其中的核心部分就是这一句代码：根据路径决策、DP得到的速度曲线、障碍物边界获取STlocation。

    // 根据路径决策、DP得到的速度曲线、障碍物边界获取STlocation
    auto location = GetSTLocation(path_decision, speed_profile, boundary);

GetSTLocation

接着来看GetSTLocation函数的具体实现：
GetSTLocation依据在ST图中障碍物与速度曲线上的各点之间的位置关系来确定决策。

SpeedDecider::STLocation SpeedDecider::GetSTLocation(
    const PathDecision* const path_decision, const SpeedData& speed_profile,
    const STBoundary& st_boundary) const {
    
    
  // 若boundary为空，设置为BELOW；一般情况下这个障碍物直接被跳过，不考虑
  if (st_boundary.IsEmpty()) {
    
    
    return BELOW;
  }

  STLocation st_location = BELOW;
  bool st_position_set = false;
  const double start_t = st_boundary.min_t();
  const double end_t = st_boundary.max_t();
  // 遍历速度曲线中的每一个点
  for (size_t i = 0; i + 1 < speed_profile.size(); ++i) {
    
    
    const STPoint curr_st(speed_profile[i].s(), speed_profile[i].t());
    const STPoint next_st(speed_profile[i + 1].s(), speed_profile[i + 1].t());
    // 跳过和障碍物不在一个ST范围内的
    if (curr_st.t() < start_t && next_st.t() < start_t) {
    
    
      continue;
    }
    if (curr_st.t() > end_t) {
    
    
      break;
    }

    if (!FLAGS_use_st_drivable_boundary) {
    
    
      common::math::LineSegment2d speed_line(curr_st, next_st);
      // 检查是否碰撞
      if (st_boundary.HasOverlap(speed_line)) {
    
    
        ADEBUG << "speed profile cross st_boundaries.";
        st_location = CROSS;

        if (!FLAGS_use_st_drivable_boundary) {
    
    
          // 检查类型是否是KEEP_CLEAR
          if (st_boundary.boundary_type() ==
              STBoundary::BoundaryType::KEEP_CLEAR) {
    
    
            // 若CheckKeepClearCrossable为false，则设置为BELOW
            if (!CheckKeepClearCrossable(path_decision, speed_profile,
                                         st_boundary)) {
    
    
              st_location = BELOW;
            }
          }
        }
        break;
      }
    }

    // note: st_position can be calculated by checking two st points once
    //       but we need iterate all st points to make sure there is no CROSS
    if (!st_position_set) {
    
    
      if (start_t < next_st.t() && curr_st.t() < end_t) {
    
    
        STPoint bd_point_front = st_boundary.upper_points().front();
        double side = common::math::CrossProd(bd_point_front, curr_st, next_st);
        st_location = side < 0.0 ? ABOVE : BELOW;
        st_position_set = true;
      }
    }
  }
  return st_location;
}

Check类函数

Check类函数，这一类函数主要用于对不同障碍物进行判断。

CheckKeepClearCrossable

bool SpeedDecider::CheckKeepClearCrossable(
    const PathDecision* const path_decision, const SpeedData& speed_profile,
    const STBoundary& keep_clear_st_boundary) const {
    
    
  bool keep_clear_crossable = true;
  // 计算最后一点的速度
  const auto& last_speed_point = speed_profile.back();
  double last_speed_point_v = 0.0;
  if (last_speed_point.has_v()) {
    
    
    last_speed_point_v = last_speed_point.v();
  } else {
    
    
    const size_t len = speed_profile.size();
    if (len > 1) {
    
    
      const auto& last_2nd_speed_point = speed_profile[len - 2];
      last_speed_point_v = (last_speed_point.s() - last_2nd_speed_point.s()) /
                           (last_speed_point.t() - last_2nd_speed_point.t());
    }
  }
  static constexpr double kKeepClearSlowSpeed = 2.5;  // m/s
  ADEBUG << "last_speed_point_s[" << last_speed_point.s()
         << "] st_boundary.max_s[" << keep_clear_st_boundary.max_s()
         << "] last_speed_point_v[" << last_speed_point_v << "]";
  // 若最后一点的s小于KeepClear区域的最大值，且最后一点的速度小于阈值速度，
  // 则keep_clear_crossable = false
  if (last_speed_point.s() <= keep_clear_st_boundary.max_s() &&
      last_speed_point_v < kKeepClearSlowSpeed) {
    
    
    keep_clear_crossable = false;
  }
  return keep_clear_crossable;
}

CheckStopForPedestrian

bool SpeedDecider::CheckStopForPedestrian(const Obstacle& obstacle) const {
    
    
  const auto& perception_obstacle = obstacle.Perception();
  // 检查类型是否是行人
  if (perception_obstacle.type() != PerceptionObstacle::PEDESTRIAN) {
    
    
    return false;
  }

  const auto& obstacle_sl_boundary = obstacle.PerceptionSLBoundary();
  // 不考虑在车之后的行人
  if (obstacle_sl_boundary.end_s() < adc_sl_boundary_.start_s()) {
    
    
    return false;
  }

  // read pedestrian stop time from PlanningContext
  auto* mutable_speed_decider_status = injector_->planning_context()
                                           ->mutable_planning_status()
                                           ->mutable_speed_decider();
   // 用hash表存储停止时间                                        
  std::unordered_map<std::string, double> stop_time_map;
  for (const auto& pedestrian_stop_time :
       mutable_speed_decider_status->pedestrian_stop_time()) {
    
    
    stop_time_map[pedestrian_stop_time.obstacle_id()] =
        pedestrian_stop_time.stop_timestamp_sec();
  }

  const std::string& obstacle_id = obstacle.Id();

  // update stop timestamp on static pedestrian for watch timer
  // check on stop timer for static pedestrians
  static constexpr double kSDistanceStartTimer = 10.0;
  static constexpr double kMaxStopSpeed = 0.3;
  static constexpr double kPedestrianStopTimeout = 4.0;

  bool result = true;
  if (obstacle.path_st_boundary().min_s() < kSDistanceStartTimer) {
    
    
    const auto obstacle_speed = std::hypot(perception_obstacle.velocity().x(),
                                           perception_obstacle.velocity().y());
    // 如果行人速度超过最大停车速度，则删除行人
    if (obstacle_speed > kMaxStopSpeed) {
    
    
      stop_time_map.erase(obstacle_id);
    } else {
    
    
      if (stop_time_map.count(obstacle_id) == 0) {
    
    
        // add timestamp
        stop_time_map[obstacle_id] = Clock::NowInSeconds();
        ADEBUG << "add timestamp: obstacle_id[" << obstacle_id << "] timestamp["
               << Clock::NowInSeconds() << "]";
      } else {
    
    
        // check timeout
        double stop_timer = Clock::NowInSeconds() - stop_time_map[obstacle_id];
        ADEBUG << "stop_timer: obstacle_id[" << obstacle_id << "] stop_timer["
               << stop_timer << "]";
        // 检查其是否已经等待了足够长的时间
        if (stop_timer >= kPedestrianStopTimeout) {
    
    
          result = false;
        }
      }
    }
  }

  // write pedestrian stop time to PlanningContext
  mutable_speed_decider_status->mutable_pedestrian_stop_time()->Clear();
  for (const auto& stop_time : stop_time_map) {
    
    
    auto pedestrian_stop_time =
        mutable_speed_decider_status->add_pedestrian_stop_time();
    pedestrian_stop_time->set_obstacle_id(stop_time.first);
    pedestrian_stop_time->set_stop_timestamp_sec(stop_time.second);
  }
  return result;
}

CheckIsFollow

bool SpeedDecider::CheckIsFollow(const Obstacle& obstacle,
                                 const STBoundary& boundary) const {
    
    
  const double obstacle_l_distance =
      std::min(std::fabs(obstacle.PerceptionSLBoundary().start_l()),
               std::fabs(obstacle.PerceptionSLBoundary().end_l()));
  // FLAGS_follow_min_obs_lateral_distance: obstacle min lateral distance to follow; 2.5
  if (obstacle_l_distance > FLAGS_follow_min_obs_lateral_distance) {
    
    
    return false;
  }

  // move towards adc
  if (boundary.bottom_left_point().s() > boundary.bottom_right_point().s()) {
    
    
    return false;
  }

  static constexpr double kFollowTimeEpsilon = 1e-3;
  static constexpr double kFollowCutOffTime = 0.5;
  if (boundary.min_t() > kFollowCutOffTime ||
      boundary.max_t() < kFollowTimeEpsilon) {
    
    
    return false;
  }

  // cross lane but be moving to different direction
  // FLAGS_follow_min_time_sec: 
  // " min follow time in st region before considering a valid follow,"
  // " this is to differentiate a moving obstacle cross adc's"
  // " current lane and move to a different direction"
  // 2.0
  if (boundary.max_t() - boundary.min_t() < FLAGS_follow_min_time_sec) {
    
    
    return false;
  }

  return true;
}

CheckKeepClearBlocked

bool SpeedDecider::CheckKeepClearBlocked(
    const PathDecision* const path_decision,
    const Obstacle& keep_clear_obstacle) const {
    
    
  bool keep_clear_blocked = false;

  // check if overlap with other stop wall
  for (const auto* obstacle : path_decision->obstacles().Items()) {
    
    
    if (obstacle->Id() == keep_clear_obstacle.Id()) {
    
    
      continue;
    }
    const double obstacle_start_s = obstacle->PerceptionSLBoundary().start_s();
    const double adc_length =
        VehicleConfigHelper::GetConfig().vehicle_param().length();
    const double distance =
        obstacle_start_s - keep_clear_obstacle.PerceptionSLBoundary().end_s();

    if (obstacle->IsBlockingObstacle() && distance > 0 &&
        distance < (adc_length / 2)) {
    
    
      keep_clear_blocked = true;
      break;
    }
  }
  return keep_clear_blocked;
}

Create类函数

建立各类决策。