Apollo6.0 StBoundsDecider流程与代码解析

Apollo6.0 StBoundsDecider流程与代码解析

前言

对于纵向决策与规划而言，需要依据障碍物的预测轨迹、类型等，构建其st_boundary映射至planned_path上，并对其进行决策打标签(yield, overtake…);轨迹规划模块再依据决策信息，添加对应求解目标、约束条件，求解处"最优"轨迹。

本文主要对StBoundsDecider进行解析，StBoundsDecider模块内容较多，之前花了不少时间解析。

文中如有错误之处，恳请各位大佬指正！。

一、流程与代码解析

STBoundsDecider 主要是对动态以及最近的一个静态且阻塞当前引导路径的障碍物进行st图构建,对不影响纵向规划的障碍物设置IGNORE属性,并按照设定轨迹给出每一个障碍物boundary的最优决策(overtake/yield),最终决策出最优的Drivable_st_boundary;
算法主流程见STBoundsDecider::Process()
主要包含内容可见下图：

1. InitSTBoundsDecider()函数

• 初始化st_obstacles_process_;
• 基于优化求解后的横向路径path_data,构建障　碍物映射至该路径上的st图,见STObstaclesProcessor::MapObstaclesToSTBoundary函数:
  • 记录低路权的路段(OUT_ON_FORWARD_LANE/OUT_ON_REVERSE_LANE)
  • 遍历障碍物：
    • ComputeObstacleSTBoundary(),计算障碍物的st_boundary;对于静态障碍物，标记为is_caution_obstacle;动态障碍物，则与低路权路权进行位置匹配,确定其obs_caution_end_t时间;

Status STObstaclesProcessor::MapObstaclesToSTBoundaries(
    PathDecision* const path_decision) {
  // Sanity checks.
  ...
  ...

  // 更新低路权区域
  bool is_adc_low_road_right_beginning = true;
  for (const auto& path_pt_info : path_data_.path_point_decision_guide()) {
    double path_pt_s = 0.0;
    PathData::PathPointType path_pt_type;
    std::tie(path_pt_s, path_pt_type, std::ignore) = path_pt_info;
    if (path_pt_type == PathData::PathPointType::OUT_ON_FORWARD_LANE ||
        path_pt_type == PathData::PathPointType::OUT_ON_REVERSE_LANE) {
      if (is_adc_low_road_right_beginning) {
        adc_low_road_right_segments_.emplace_back(path_pt_s, path_pt_s);
        is_adc_low_road_right_beginning = false;
      } else {
        adc_low_road_right_segments_.back().second = path_pt_s;
      }
    } else if (path_pt_type == PathData::PathPointType::IN_LANE) {
      if (!is_adc_low_road_right_beginning) {
        is_adc_low_road_right_beginning = true;
      }
    }
  }

  std::unordered_set<std::string> non_ignore_obstacles;
  std::tuple<std::string, STBoundary, Obstacle*> closest_stop_obstacle;
  std::get<0>(closest_stop_obstacle) = "NULL";
  //遍历每一个障碍物，计算其st_boundary；注意，该模块计算出的lower_points和upper_points分辨率较粗，碰撞校验不精确
  for (const auto* obs_item_ptr : path_decision->obstacles().Items()) {
    // Sanity checks.
    Obstacle* obs_ptr = path_decision->Find(obs_item_ptr->Id());
    if (obs_ptr == nullptr) {
      const std::string msg = "Null obstacle pointer.";
      AERROR << msg;
      return Status(ErrorCode::PLANNING_ERROR, msg);
    }

    // Draw the obstacle's st-boundary.
    std::vector<STPoint> lower_points;
    std::vector<STPoint> upper_points;
    bool is_caution_obstacle = false;
    double obs_caution_end_t = 0.0;
    //依据优化后的路径计算障碍物的上下界，计算出的边界粒度较粗。
    if (!ComputeObstacleSTBoundary(*obs_ptr, &lower_points, &upper_points,
                                   &is_caution_obstacle, &obs_caution_end_t)) {
      continue;
    }
  　//对上下边界点不做删减，保留原始数值
    auto boundary =
        STBoundary::CreateInstanceAccurate(lower_points, upper_points);
    boundary.set_id(obs_ptr->Id());
    if (is_caution_obstacle) {
      boundary.set_obstacle_road_right_ending_t(obs_caution_end_t);
    }
    // 将处于非低路权区域的上下边界删掉
    while (lower_points.size() > 2 &&
           lower_points.back().t() > obs_caution_end_t) {
      lower_points.pop_back();
    }
    while (upper_points.size() > 2 &&
           upper_points.back().t() > obs_caution_end_t) {
      upper_points.pop_back();
    }
    auto alternative_boundary =
        STBoundary::CreateInstanceAccurate(lower_points, upper_points);
    alternative_boundary.set_id(obs_ptr->Id());
    obs_id_to_alternative_st_boundary_[obs_ptr->Id()] = alternative_boundary;

    // Store all Keep-Clear zone together.
    if (obs_item_ptr->Id().find("KC") != std::string::npos) {
      candidate_clear_zones_.push_back(
          make_tuple(obs_ptr->Id(), boundary, obs_ptr));
      continue;
    }

    //找到距离最近的一个static障碍物，将其存至closest_stop_obstacle;
    //忽略自车身后的动态障碍物
    if (obs_ptr->Trajectory().trajectory_point().empty()) {
      // Obstacle is static.
      if (std::get<0>(closest_stop_obstacle) == "NULL" ||
          std::get<1>(closest_stop_obstacle).bottom_left_point().s() >
              boundary.bottom_left_point().s()) {
        // If this static obstacle is closer for ADC to stop, record it.
        closest_stop_obstacle =
            std::make_tuple(obs_ptr->Id(), boundary, obs_ptr);
      }
    } else {
      // Obstacle is dynamic.
      if (boundary.bottom_left_point().s() - adc_path_init_s_ <
              kSIgnoreThreshold &&
          boundary.bottom_left_point().t() > kTIgnoreThreshold) {
        continue;
      }
      obs_id_to_st_boundary_[obs_ptr->Id()] = boundary;
      obs_ptr->set_path_st_boundary(boundary);
      non_ignore_obstacles.insert(obs_ptr->Id());
      ADEBUG << "Adding " << obs_ptr->Id() << " into the ST-graph.";
    }
  }

  //存储最近的static障碍物boundary到obs_id_boundary map容器中
  //如果static障碍物边界与keepclear区域发生重叠，则更新对应的id/boundary/obs_ptr
  if (std::get<0>(closest_stop_obstacle) != "NULL") {
    std::string closest_stop_obs_id;
    STBoundary closest_stop_obs_boundary;
    Obstacle* closest_stop_obs_ptr;
    std::tie(closest_stop_obs_id, closest_stop_obs_boundary,
             closest_stop_obs_ptr) = closest_stop_obstacle;

    // Go through all Keep-Clear zones, and see if there is an even closer
    // stop fence due to them.
    if (!closest_stop_obs_ptr->IsVirtual()) {
      for (const auto& clear_zone : candidate_clear_zones_) {
        const auto& clear_zone_boundary = std::get<1>(clear_zone);
        if (closest_stop_obs_boundary.min_s() >= clear_zone_boundary.min_s() &&
            closest_stop_obs_boundary.min_s() <= clear_zone_boundary.max_s()) {
          std::tie(closest_stop_obs_id, closest_stop_obs_boundary,
                   closest_stop_obs_ptr) = clear_zone;
          ADEBUG << "Clear zone " << closest_stop_obs_id << " is closer.";
          break;
        }
      }
    }
    obs_id_to_st_boundary_[closest_stop_obs_id] = closest_stop_obs_boundary;
    closest_stop_obs_ptr->set_path_st_boundary(closest_stop_obs_boundary);
    non_ignore_obstacles.insert(closest_stop_obs_id);
    ADEBUG << "Adding " << closest_stop_obs_ptr->Id() << " into the ST-graph.";
    ADEBUG << "min_s = " << closest_stop_obs_boundary.min_s();
  }

  // 将未添加进non_ignore_obstacles容器中的障碍物横纵向Decision为IGNORE
  for (const auto* obs_item_ptr : path_decision->obstacles().Items()) {
    Obstacle* obs_ptr = path_decision->Find(obs_item_ptr->Id());
    if (non_ignore_obstacles.count(obs_ptr->Id()) == 0) {
      ObjectDecisionType ignore_decision;
      ignore_decision.mutable_ignore();
      if (!obs_ptr->HasLongitudinalDecision()) {
        obs_ptr->AddLongitudinalDecision("st_obstacle_processor",
                                         ignore_decision);
      }
      if (!obs_ptr->HasLateralDecision()) {
        obs_ptr->AddLateralDecision("st_obstacle_processor", ignore_decision);
      }
    }
  }

  // Preprocess the obstacles for sweep-line algorithms.
  // Fetch every obstacle's beginning end ending t-edges only.
  //每个障碍物只需要存储其首尾t/上下界s/id信息
  for (const auto& it : obs_id_to_st_boundary_) {
    obs_t_edges_.emplace_back(true, it.second.min_t(),
                              it.second.bottom_left_point().s(),
                              it.second.upper_left_point().s(), it.first);
    obs_t_edges_.emplace_back(false, it.second.max_t(),
                              it.second.bottom_right_point().s(),
                              it.second.upper_right_point().s(), it.first);
  }
  // Sort the edges.
  //先按起始start_s位置升序排序，若相同，则按是否为start降序排序
  std::sort(obs_t_edges_.begin(), obs_t_edges_.end(),
            [](const ObsTEdge& lhs, const ObsTEdge& rhs) {
              if (std::get<1>(lhs) != std::get<1>(rhs)) {
                return std::get<1>(lhs) < std::get<1>(rhs);
              } else {
                return std::get<0>(lhs) > std::get<0>(rhs);
              }
            });

  return Status::OK();
}

利用向量点乘、叉乘计算障碍物与规划路径上自车碰撞时刻的最近位置
bool STObstaclesProcessor::IsPathPointAwayFromObstacle(
    const PathPoint& path_point, const PathPoint& direction_point,
    const Box2d& obs_box, const double s_thresh, const bool is_before) {
  Vec2d path_pt(path_point.x(), path_point.y());
  Vec2d dir_pt(direction_point.x(), direction_point.y());
  LineSegment2d path_dir_lineseg(path_pt, dir_pt);
  LineSegment2d normal_line_seg(path_pt, path_dir_lineseg.rotate(M_PI_2));

  auto corner_points = obs_box.GetAllCorners();
  for (const auto& corner_pt : corner_points) {
    Vec2d normal_line_ft_pt;
    normal_line_seg.GetPerpendicularFoot(corner_pt, &normal_line_ft_pt);
    Vec2d path_dir_unit_vec = path_dir_lineseg.unit_direction();
    Vec2d perpendicular_vec = corner_pt - normal_line_ft_pt;
    double corner_pt_s_dist = path_dir_unit_vec.InnerProd(perpendicular_vec);
    if (is_before && corner_pt_s_dist < s_thresh) {
      return false;
    }
    if (!is_before && corner_pt_s_dist > -s_thresh) {
      return false;
    }
  }
  return true;
}

示意图如下:

• 初始化启发式st曲线st_guide_line和自车运动学约束边界st_driving_limits_;

  //此处desire_speed应该结合自车set_speed以及tfl speed_limit约束
  static constexpr double desired_speed = 15.0;
  // If the path_data optimization is guided from a reference path of a
  // reference trajectory, use its reference speed profile to select the st
  // bounds in LaneFollow Hybrid Mode
  if (path_data.is_optimized_towards_trajectory_reference()) {
    st_guide_line_.Init(desired_speed,
                        injector_->learning_based_data()
                            ->learning_data_adc_future_trajectory_points());
  } else {
    st_guide_line_.Init(desired_speed);
  }
  static constexpr double max_acc = 2.5;
  static constexpr double max_dec = 5.0;
  static constexpr double max_v = desired_speed * 1.5;
  st_driving_limits_.Init(max_acc, max_dec, max_v,
                          frame.PlanningStartPoint().v());

1. GenerateRegularSTBound:生成常规可通行st_bound和vt_bound边界，遍历st_bound:

• 依据运动学driving_limits更新s_lower, s_upper;
• 依据障碍物st_boundary约束，更新可通行s_bounds以及对应的决策(GetBoundsFromDecisions),对于同一障碍物的st_boundary可能存在多个决策，比如overtake或yield,见GetSBoundsFromDecisions函数;

bool STObstaclesProcessor::GetSBoundsFromDecisions(
    double t, std::vector<std::pair<double, double>>* const available_s_bounds,
    std::vector<std::vector<std::pair<std::string, ObjectDecisionType>>>* const
        available_obs_decisions) {
    ...
  while (obs_t_edges_idx_ < static_cast<int>(obs_t_edges_.size()) &&
         std::get<1>(obs_t_edges_[obs_t_edges_idx_]) <= t) {
    if (std::get<0>(obs_t_edges_[obs_t_edges_idx_]) == 0 &&
        std::get<1>(obs_t_edges_[obs_t_edges_idx_]) == t) {
      break;
    }
    ...
    //只有当障碍物刚进入，或者当前t大于障碍物的end_t，才会push进来
    new_t_edges.push_back(obs_t_edges_[obs_t_edges_idx_]);
    ++obs_t_edges_idx_;
  }
  //如果障碍物is_starting 标志为false,则需要将其删除
  for (const auto& obs_t_edge : new_t_edges) {
    if (std::get<0>(obs_t_edge) == 0) {
      ADEBUG << "Obstacle id: " << std::get<4>(obs_t_edge)
             << " is leaving st-graph.";
      if (obs_id_to_decision_.count(std::get<4>(obs_t_edge)) != 0) {
        obs_id_to_decision_.erase(std::get<4>(obs_t_edge));
      }
    }
  }
  //对于已经超过的障碍物,需要在超过其end_t(加阈值时间)后将其从存储容器中删掉
  std::vector<std::string> obs_id_to_remove;
  for (const auto& obs_id_to_decision_pair : obs_id_to_decision_) {...}
  for (const auto& obs_id : obs_id_to_remove) {
    obs_id_to_decision_.erase(obs_id);
    ...
  }

  //依据障碍物obs_id_decision信息，从其st_boundary计算处s_min/s_max边界，比如这个障碍物obs_decision == overtake,则s_min >= obs_s_max(t时刻障碍物boundary上边界)
  double s_min = 0.0;
  double s_max = planning_distance_;
  for (auto it : obs_id_to_decision_) {
  ...
  }

//对新进入的障碍物信息，进行位置上判断，做出明确或者模糊的决策
  std::vector<ObsTEdge> ambiguous_t_edges;
  for (auto obs_t_edge : new_t_edges) {
    if (std::get<0>(obs_t_edge) == 1) {
      //障碍物的下边界>= s_max，则判断yield　action 
      if (std::get<2>(obs_t_edge) >= s_max) {
        ADEBUG << "  Apparently, it should be yielded.";
        obs_id_to_decision_[std::get<4>(obs_t_edge)] =
            DetermineObstacleDecision(std::get<2>(obs_t_edge),
                                      std::get<3>(obs_t_edge), s_max);
        obs_id_to_st_boundary_[std::get<4>(obs_t_edge)].SetBoundaryType(
            STBoundary::BoundaryType::YIELD);
      } else if (std::get<3>(obs_t_edge) <= s_min) {
        ADEBUG << "  Apparently, it should be overtaken.";
        obs_id_to_decision_[std::get<4>(obs_t_edge)] =
            DetermineObstacleDecision(std::get<2>(obs_t_edge),
                                      std::get<3>(obs_t_edge), s_min);
        obs_id_to_st_boundary_[std::get<4>(obs_t_edge)].SetBoundaryType(
            STBoundary::BoundaryType::OVERTAKE);
      } else {
        ADEBUG << "  It should be further analyzed.";
        ambiguous_t_edges.push_back(obs_t_edge);
      }
    }
  }
//对于决策模糊的情况，先去寻找对应的可通行上下边界
//FindSGaps内部排序方法：先按s大小排序；再者按是否为staring_flag来排序
  auto s_gaps = FindSGaps(ambiguous_t_edges, s_min, s_max);

//依据可通行空间sgap,遍历，并映射至对应的障碍物上，对每个具有st map属性的障碍物进行打标签。
  for (auto s_gap : s_gaps) {
    available_s_bounds->push_back(s_gap);
    std::vector<std::pair<std::string, ObjectDecisionType>> obs_decisions;
    for (auto obs_t_edge : ambiguous_t_edges) {
      std::string obs_id = std::get<4>(obs_t_edge);
      double obs_s_min = std::get<2>(obs_t_edge);
      double obs_s_max = std::get<3>(obs_t_edge);
      obs_decisions.emplace_back(
          obs_id,
          DetermineObstacleDecision(obs_s_min, obs_s_max,
                                    (s_gap.first + s_gap.second) / 2.0));//决策对该障碍物是overtake还是yield
    }
    available_obs_decisions->push_back(obs_decisions);
  }
}

• 依据自车运动学约束，剔除不可达的障碍物decision,见RemoveInvalidDecisions函数;

void STBoundsDecider::RemoveInvalidDecisions(
    std::pair<double, double> driving_limit,
    std::vector<std::pair<STBoundPoint, ObsDecSet>>* available_choices) {
  // Remove those choices that don't even fall within driving-limits.
  size_t i = 0;
  while (i < available_choices->size()) {
    double s_lower = 0.0;
    double s_upper = 0.0;
    std::tie(std::ignore, s_lower, s_upper) = available_choices->at(i).first;
    if (s_lower > driving_limit.second || s_upper < driving_limit.first) {
      //超出运动学约束的decision choice,将其移至末尾pop掉
      // Invalid bound, should be removed.
      if (i != available_choices->size() - 1) {
        swap(available_choices->at(i),
             available_choices->at(available_choices->size() - 1));
      }
      available_choices->pop_back();
    } else {
      // Valid bound, proceed to the next one.
      ++i;
    }
  }
}

• 接下来是对avaliable_choices进行按规则迭代排序，直至容器内元素变得有序退出。比如对于obs2，有着overtake 和yield两种决策。按照可通行区间大小优先，其次可通行空间包含guide_line优先。

void STBoundsDecider::RankDecisions(
    double s_guide_line, std::pair<double, double> driving_limit,
    std::vector<std::pair<STBoundPoint, ObsDecSet>>* available_choices) {
  // Perform sorting of the existing decisions.
  bool has_swaps = true;
  while (has_swaps) {
    has_swaps = false;
    for (int i = 0; i < static_cast<int>(available_choices->size()) - 1; ++i) {
      ...

      // 只要两个可通行空间中有一个通行区间小于阈值，选其中较大空间的通行区域
      double A_room = std::fmin(driving_limit.second, A_s_upper) -
                      std::fmax(driving_limit.first, A_s_lower);
      double B_room = std::fmin(driving_limit.second, B_s_upper) -
                      std::fmax(driving_limit.first, B_s_lower);
      if (A_room < kSTPassableThreshold || B_room < kSTPassableThreshold) {
        if (A_room < B_room) {
          swap(available_choices->at(i + 1), available_choices->at(i));
          has_swaps = true;
          ADEBUG << "Swapping to favor larger room.";
        }
        continue;
      }

      // 优先选择与s_guide_line有重叠的通行区域
      bool A_contains_guideline =
          A_s_upper >= s_guide_line && A_s_lower <= s_guide_line;
      bool B_contains_guideline =
          B_s_upper >= s_guide_line && B_s_lower <= s_guide_line;
      if (A_contains_guideline != B_contains_guideline) {
        if (!A_contains_guideline) {
          swap(available_choices->at(i + 1), available_choices->at(i));
          has_swaps = true;
        }
        continue;
      }
    }
  }
}

下图，B_room与s_guide_line有重叠，优先选择B_room;

• 找到优先级最高的可通行空间并赋值给obstacleDecision()

auto top_choice_decision = available_choices.front().second;
st_obstacles_processor_.SetObstacleDecision(top_choice_decision);

• 更新s_guide_line/st_driving_limits/v_limts

// Update st-guide-line, st-driving-limit info, and v-limits,主要是st/sv上下边界.
std::pair<double, double> limiting_speed_info;
if (st_obstacles_processor_.GetLimitingSpeedInfo(t, &limiting_speed_info)) {
  st_driving_limits_.UpdateBlockingInfo(t, s_lower, limiting_speed_info.first, s_upper,
          limiting_speed_info.second);
  st_guide_line_.UpdateBlockingInfo(t, s_lower, true);
  st_guide_line_.UpdateBlockingInfo(t, s_upper, false);
  if (is_limited_by_lower_obs) {
    lower_obs_v = limiting_speed_info.first;
  }
  if (is_limited_by_upper_obs) {
    upper_obs_v = limiting_speed_info.second;
  }
}

st_bound->at(i) = std::make_tuple(t, s_lower, s_upper);
vt_bound->at(i) = std::make_tuple(t, lower_obs_v, upper_obs_v);

3. 将可通行区域st_bound,vt_bound赋值给reference_line_info中的STDrivableBoundary()
   st_graph_data->SetSTDrivableBoundary(regular_st_bound, regular_vt_bound);

二、总结

StBoundsDecider模块内容很多，个人觉得该模块主要的工作就是：

• 生成DrivableSTBoundary;
• 对不影响纵向规划的障碍物设置IGNORE属性;

本模块障碍物的ST图计算耗时也较高，但st_boundary精度较低;同理最终生成的st_drivable_boundary_边界精度也较低。如果在下游gridded_path_time_graph模块中FLAGS_use_st_drivable_boundary不置位时,则无需使用该模块输出的st_drivable_boundary；因此可结合实际项目需求，来精简该模块的冗余计算。

三、参考

• apollo

在此，感谢本文中索引的博客大佬们将知识share，感谢Baidu apollo开源算法!。