Apollo Planning——TASK之PathBoundsDecider_apollo getneighborpassages-CSDN博客

本文链接：https://blog.csdn.net/weixin_42905141/article/details/135093180

在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的第四个TASK——PATH_BOUNDS_DECIDER

PATH_BOUNDS_DECIDER功能简介

在这里插入图片描述
利用前几个决策器，根据相应条件，产生相应的SL边界。这里说明以下Nudge障碍物的概念——主车旁边的障碍物未完全阻挡主车，主车可以通过绕行避过障碍物（注意图中的边界）。

PATH_BOUNDS_DECIDER相关配置

在modules/planning/conf/planning_config.pb.txt中有相关配置。

default_task_config: {
  task_type: PATH_BOUNDS_DECIDER
  path_bounds_decider_config {
    is_lane_borrowing: false
    is_pull_over: false
    is_extend_lane_bounds_to_include_adc: false
    pull_over_destination_to_adc_buffer: 25.0
    pull_over_destination_to_pathend_buffer: 4.0
    pull_over_road_edge_buffer: 0.15
    pull_over_approach_lon_distance_adjust_factor: 1.5
  }
}

modules/planning/proto/task_config.proto中也有相关参数

// PathBoundsDeciderConfig

message PathBoundsDeciderConfig {
  optional bool is_lane_borrowing = 1;
  optional bool is_pull_over = 2;
  // not search pull-over position if the destination is within this distance
  // from ADC
  optional double pull_over_destination_to_adc_buffer = 3 [default = 25.0];
  // not search pull-over position if the destination is within this distance to
  // path-end
  optional double pull_over_destination_to_pathend_buffer = 4 [default = 10.0];
  // disquality a pull-over position if the available path boundary's edge is
  // not within this distance from the road edge
  optional double pull_over_road_edge_buffer = 5 [default = 0.15];
  optional double pull_over_approach_lon_distance_adjust_factor = 6
      [default = 1.5];
  optional double adc_buffer_coeff = 7 [default = 1.0];
  optional bool is_extend_lane_bounds_to_include_adc = 8 [default = true];
}

数据结构

// modules/planning/tasks/deciders/path_bounds_decider/path_bounds_decider.cc
namespace {
// PathBoundPoint contains: (s, l_min, l_max).路径边界点
using PathBoundPoint = std::tuple<double, double, double>;
// PathBound contains a vector of PathBoundPoints.路径边界
using PathBound = std::vector<PathBoundPoint>;
// ObstacleEdge contains: (is_start_s, s, l_min, l_max, obstacle_id).障碍物的边
using ObstacleEdge = std::tuple<int, double, double, double, std::string>;
}  // namespace

参数设置

// modules/planning/tasks/deciders/path_bounds_decider/path_bounds_decider.h
// s方向的距离
constexpr double kPathBoundsDeciderHorizon = 100.0;
// s方向的间隔
constexpr double kPathBoundsDeciderResolution = 0.5;
// Lane宽度
constexpr double kDefaultLaneWidth = 5.0;
// Road的道路
constexpr double kDefaultRoadWidth = 20.0;
// TODO(all): Update extra tail point base on vehicle speed.
constexpr int kNumExtraTailBoundPoint = 20;
constexpr double kPulloverLonSearchCoeff = 1.5;
constexpr double kPulloverLatSearchCoeff = 1.25;

PATH_BOUNDS_DECIDER总体流程

总体流程依然先看Process部分
在这里插入图片描述
在Process方法中，分四种场景对路径边界进行计算，按照处理的顺序分别是：fallback，pull-over，lane-change，regular。其中regular场景根据是否借道又分为LEFT_BORROW, NO_BORROW, RIGHT_BORROW。

fallback场景的path bounds一定会生成，另外三种看情况，都是需要if判断。

InitPathBoundsDecider

// InitPathBoundsDecider初始化planning_start_point、获取ADC S/L坐标信息、获取ADC当前所在的车道线宽度
void PathBoundsDecider::InitPathBoundsDecider(
    const Frame& frame, const ReferenceLineInfo& reference_line_info) {
  const ReferenceLine& reference_line = reference_line_info.reference_line();
  common::TrajectoryPoint planning_start_point = frame.PlanningStartPoint();
  // 如果在路径规划中使用前轴中心，路径可以更加灵活。
  if (FLAGS_use_front_axe_center_in_path_planning) { // false
    planning_start_point = InferFrontAxeCenterFromRearAxeCenter(planning_start_point);
  }
  ADEBUG << "Plan at the starting point: x = "
         << planning_start_point.path_point().x()
         << ", y = " << planning_start_point.path_point().y()
         << ", and angle = " << planning_start_point.path_point().theta();

  // Initialize some private variables.
  // ADC s/l info.
  auto adc_sl_info = reference_line.ToFrenetFrame(planning_start_point);
  adc_frenet_s_ = adc_sl_info.first[0];  // frenet纵向位移
  adc_frenet_l_ = adc_sl_info.second[0]; // frenet横向位移
  adc_frenet_sd_ = adc_sl_info.first[1]; // frenet纵向速度
  // 计算横向速度：dl/ds * ds /dt = dl / dt
  adc_frenet_ld_ = adc_sl_info.second[1] * adc_frenet_sd_; //  Frenet横向位移对弧长的一阶导数 * frenet纵向速度
  double offset_to_map = 0.0;
  // 计算到参考线中心的距离，并将结果存储在offset_to_map中。
  reference_line.GetOffsetToMap(adc_frenet_s_, &offset_to_map);
  // 得到车辆当前位置到参考线中心的距离
  adc_l_to_lane_center_ = adc_frenet_l_ + offset_to_map;

  // ADC's lane width.
  double lane_left_width = 0.0;
  double lane_right_width = 0.0;
  // 用于获取参考线上指定位置的左右车道宽度
  if (!reference_line.GetLaneWidth(adc_frenet_s_, &lane_left_width, &lane_right_width)) {
    AWARN << "Failed to get lane width at planning start point.";
    adc_lane_width_ = kDefaultLaneWidth;
  } else {
    adc_lane_width_ = lane_left_width + lane_right_width;
  }
}

modules/planning/reference_line/reference_line.cc

std::pair<std::array<double, 3>, std::array<double, 3>>
ReferenceLine::ToFrenetFrame(const common::TrajectoryPoint& traj_point) const {
  ACHECK(!reference_points_.empty());

  common::SLPoint sl_point;
  // XY坐标转换为SL坐标，并将结果存储在sl_point中
  XYToSL(traj_point.path_point(), &sl_point);

  std::array<double, 3> s_condition; // Frenet纵向位移,Frenet纵向速度,Frenet纵向加速度
  std::array<double, 3> l_condition; // Frenet横向位移,Frenet横向位移对弧长的一阶导数,Frenet横向位移对弧长的二阶导数
  // 获取参考线上距离指定位置最近的参考点
  ReferencePoint ref_point = GetReferencePoint(sl_point.s());
  // SL坐标转换为Frenet坐标,并将结果存储在s_condition和l_condition中。
  CartesianFrenetConverter::cartesian_to_frenet(
      sl_point.s(), ref_point.x(), ref_point.y(), ref_point.heading(),
      ref_point.kappa(), ref_point.dkappa(), traj_point.path_point().x(),
      traj_point.path_point().y(), traj_point.v(), traj_point.a(),
      traj_point.path_point().theta(), traj_point.path_point().kappa(),
      &s_condition, &l_condition);

  return std::make_pair(s_condition, l_condition);
}

modules/common/math/cartesian_frenet_conversion.cc

/**
 * 笛卡尔坐标系向Frenet坐标系转换
 * 参考链接:https://news.sohu.com/a/662247461_560178
 * **/
void CartesianFrenetConverter::cartesian_to_frenet(
    const double rs, const double rx, const double ry, const double rtheta,
    const double rkappa, const double rdkappa, const double x, const double y,
    const double v, const double a, const double theta, const double kappa,
    std::array<double, 3>* const ptr_s_condition,
    std::array<double, 3>* const ptr_d_condition) {
  const double dx = x - rx;
  const double dy = y - ry;

  const double cos_theta_r = std::cos(rtheta);
  const double sin_theta_r = std::sin(rtheta);

  const double cross_rd_nd = cos_theta_r * dy - sin_theta_r * dx;
  // 横向位移
  ptr_d_condition->at(0) = std::copysign(std::sqrt(dx * dx + dy * dy), cross_rd_nd);

  const double delta_theta = theta - rtheta;
  const double tan_delta_theta = std::tan(delta_theta);
  const double cos_delta_theta = std::cos(delta_theta);

  const double one_minus_kappa_r_d = 1 - rkappa * ptr_d_condition->at(0);
  // 横向位移对弧长的一阶导数
  ptr_d_condition->at(1) = one_minus_kappa_r_d * tan_delta_theta;

  const double kappa_r_d_prime = rdkappa * ptr_d_condition->at(0) + rkappa * ptr_d_condition->at(1);
  // 横向位移对弧长的二阶导数
  ptr_d_condition->at(2) = -kappa_r_d_prime * tan_delta_theta +
      one_minus_kappa_r_d / cos_delta_theta / cos_delta_theta * (kappa * one_minus_kappa_r_d / cos_delta_theta - rkappa);
  // 纵向位移
  ptr_s_condition->at(0) = rs;
  // 纵向速度
  ptr_s_condition->at(1) = v * cos_delta_theta / one_minus_kappa_r_d;
  const double delta_theta_prime = one_minus_kappa_r_d / cos_delta_theta * kappa - rkappa;
  // 纵向加速度
  ptr_s_condition->at(2) = (a * cos_delta_theta - ptr_s_condition->at(1) * ptr_s_condition->at(1) *
           (ptr_d_condition->at(1) * delta_theta_prime - kappa_r_d_prime)) / one_minus_kappa_r_d;
}

fallback

无论当前处于何种场景，都会调用GenerateFallbackPathBound() 来生成备用的path bound，在计算FallbackPathBound时不考虑障碍物边界，仅使用道路边界，并考虑借道信息来进行计算。
在这里插入图片描述
fallback部分主要包含两个函数，InitPathBoundary 和GetBoundaryFromLanesAndADC。fallback是备选的方案，只考虑adc信息和静态道路信息。

GenerateFallbackPathBound

// fallback倒车场景
Status PathBoundsDecider::GenerateFallbackPathBound(
    const ReferenceLineInfo& reference_line_info, PathBound* const path_bound) {
  // 1.将路径边界初始化为无限大的区域。
  // InitPathBoundary(reference_line_info, path_bound))这个函数较好理解，就不单独拿出来分析
  // 就是先将InitPathBoundary()在合理的s范围内，以kPathBoundsDeciderResolution为采样间隔对s采样，
  // 将每个采样点处的横向边界（右边界，左边界）设定为最小值和最大值
  if (!InitPathBoundary(reference_line_info, path_bound)) {
    const std::string msg = "Failed to initialize fallback path boundaries.";
    AERROR << msg;
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }

  // 2.根据车道信息和ADC位置确定粗略边界
  // GetBoundaryFromLanesAndADC（...）这个函数在后文单独拿出来说一下
  std::string dummy_borrow_lane_type;
  if (!GetBoundaryFromLanesAndADC(reference_line_info,
                                  LaneBorrowInfo::NO_BORROW, 0.5, path_bound,
                                  &dummy_borrow_lane_type, true)) {
    const std::string msg =
        "Failed to decide a rough fallback boundary based on "
        "road information.";
    AERROR << msg;
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }

  ADEBUG << "Completed generating fallback path boundaries.";
  return Status::OK();
}

InitPathBoundary

通过函数InitPathBoundary（）沿着参考线初始化一个横向无穷大的区域，该区域的纵向距离取决于车速和参考线的长度，fallback boundary的初始值就是基于这个区域，纵向间隔为0.5米生成的一系列边界点

bool PathBoundsDecider::InitPathBoundary(
    const ReferenceLineInfo& reference_line_info, PathBound* const path_bound) {
  // Sanity checks.
  CHECK_NOTNULL(path_bound);
  path_bound->clear();
  const auto& reference_line = reference_line_info.reference_line();

  // 从 ADC 的当前位置开始，递增直到地平线，并将每个点的横向边界设置为无限
  // kPathBoundsDeciderHorizon = 100.0;
  // trajectory_time_length：8.0      kPathBoundsDeciderResolution:0.5 
  for (double curr_s = adc_frenet_s_; curr_s < std::fmin(adc_frenet_s_ + std::fmax(kPathBoundsDeciderHorizon,
    reference_line_info.GetCruiseSpeed() * FLAGS_trajectory_time_length),reference_line.Length());curr_s += kPathBoundsDeciderResolution) {
    path_bound->emplace_back(curr_s, std::numeric_limits<double>::lowest(), std::numeric_limits<double>::max());
  }

  // Return.
  if (path_bound->empty()) {
    ADEBUG << "Empty path boundary in InitPathBoundary";
    return false;
  }
  return true;
}

GetBoundaryFromLanesAndADC

调用函数GetBoundaryFromLanesAndADC（）遍历（1）中生成的边界点根据自车状态及道路信息生成boundary，其中入参里的是否借道设置为不借道，ADC_buffer固定设置为0.5米，根据中心线各个S上的宽度生成。

// TODO(jiacheng): this function is to be retired soon.
bool PathBoundsDecider::GetBoundaryFromLanesAndADC(
    const ReferenceLineInfo& reference_line_info,
    const LaneBorrowInfo& lane_borrow_info, double ADC_buffer,
    PathBound* const path_bound, std::string* const borrow_lane_type,
    bool is_fallback_lanechange) {
  // Sanity checks.
  CHECK_NOTNULL(path_bound);
  ACHECK(!path_bound->empty());
  const ReferenceLine& reference_line = reference_line_info.reference_line();
  bool is_left_lane_boundary = true;
  bool is_right_lane_boundary = true;
  const double boundary_buffer = 0.05;  // meter

  // // 通过每个点，根据车道信息和ADC的位置更新边界
  double past_lane_left_width = adc_lane_width_ / 2.0;
  double past_lane_right_width = adc_lane_width_ / 2.0;
  int path_blocked_idx = -1;
  bool borrowing_reverse_lane = false;
  for (size_t i = 0; i < path_bound->size(); ++i) {
    double curr_s = std::get<0>((*path_bound)[i]);
    // 1. Get the current lane width at current point.
    double curr_lane_left_width = 0.0;
    double curr_lane_right_width = 0.0;
    double offset_to_lane_center = 0.0;
    // 据相对地图的偏移量计算当前边界点所对应的车道宽度
    if (!reference_line.GetLaneWidth(curr_s, &curr_lane_left_width,&curr_lane_right_width)) {
      AWARN << "Failed to get lane width at s = " << curr_s;
      curr_lane_left_width = past_lane_left_width;
      curr_lane_right_width = past_lane_right_width;
    } else {
      // 检查车道边界是否也是道路边界
      double curr_road_left_width = 0.0;
      double curr_road_right_width = 0.0;
      // 获取每一点boundary处对应的车道宽度及道路宽度，然后比较左右车道及道路宽度的值，
      // 判断车道边界是否就是道路边界，
      if (reference_line.GetRoadWidth(curr_s, &curr_road_left_width,&curr_road_right_width)) {
        is_left_lane_boundary = (std::abs(curr_road_left_width - curr_lane_left_width) > boundary_buffer);
        is_right_lane_boundary = (std::abs(curr_road_right_width - curr_lane_right_width) > boundary_buffer);
      }
      /*
      最后根据相对地图的偏移量计算当前边界点所对应的车道宽度；
      计算结果为 车道的左右宽度curr_lane_left_width， curr_lane_right_width
      */
     /*
     GetLaneWidth获取的地图中设定的道路的左右侧宽度，
     GetOffsetToMap获取参考线相对于车道中心线的偏移量，
     根据两个函数得到的结果计算得到的是相对于参考线的宽度
     */
      reference_line.GetOffsetToMap(curr_s, &offset_to_lane_center);
      curr_lane_left_width += offset_to_lane_center;
      curr_lane_right_width -= offset_to_lane_center;
      past_lane_left_width = curr_lane_left_width;
      past_lane_right_width = curr_lane_right_width;
    }

    // 2. Get the neighbor lane widths at the current point.
    double curr_neighbor_lane_width = 0.0;
    //CheckLaneBoundaryType()在向左或向右借道时，返回true
    if (CheckLaneBoundaryType(reference_line_info, curr_s, lane_borrow_info)) {
      hdmap::Id neighbor_lane_id;
      if (lane_borrow_info == LaneBorrowInfo::LEFT_BORROW) {
        // Borrowing left neighbor lane.  向左侧同向顺行车道借道
        if (reference_line_info.GetNeighborLaneInfo(
                ReferenceLineInfo::LaneType::LeftForward, curr_s,
                &neighbor_lane_id, &curr_neighbor_lane_width)) {
          ADEBUG << "Borrow left forward neighbor lane.";
        } else if (reference_line_info.GetNeighborLaneInfo(
                       ReferenceLineInfo::LaneType::LeftReverse, curr_s,
                       &neighbor_lane_id, &curr_neighbor_lane_width)) {
          borrowing_reverse_lane = true; // 向左侧反向逆行车道借道
          ADEBUG << "Borrow left reverse neighbor lane.";
        } else {
          ADEBUG << "There is no left neighbor lane.";
        }
      } else if (lane_borrow_info == LaneBorrowInfo::RIGHT_BORROW) {
        // Borrowing right neighbor lane.
        if (reference_line_info.GetNeighborLaneInfo(
                ReferenceLineInfo::LaneType::RightForward, curr_s,
                &neighbor_lane_id, &curr_neighbor_lane_width)) {
          ADEBUG << "Borrow right forward neighbor lane.";
        } else if (reference_line_info.GetNeighborLaneInfo(
                       ReferenceLineInfo::LaneType::RightReverse, curr_s,
                       &neighbor_lane_id, &curr_neighbor_lane_width)) {
          borrowing_reverse_lane = true;
          ADEBUG << "Borrow right reverse neighbor lane.";
        } else {
          ADEBUG << "There is no right neighbor lane.";
        }
      }
    }

    // 3.根据车道宽度、ADC位置和ADC速度计算适当的边界。
    static constexpr double kMaxLateralAccelerations = 1.5;
    double offset_to_map = 0.0;
    reference_line.GetOffsetToMap(curr_s, &offset_to_map);
    // 给定初始速度和加速度，刹停行驶的距离（横向）
    // 对横向来说，就是横向速度变为0的过程行驶的横向位移
    // ADC_speed_buffer是按照最大横向加速度车辆横向速度降为0所需要的横向距离
    double ADC_speed_buffer = (adc_frenet_ld_ > 0 ? 1.0 : -1.0) *
                              adc_frenet_ld_ * adc_frenet_ld_ /
                              kMaxLateralAccelerations / 2.0;
    // curr_left_bound_lane、curr_right_bound_lane基于车道边界的左右边界，如果需要借道，则为当前车道宽度加上相邻车道宽度；
    // 如果要向左借道，可行驶道路左边界会变为左侧道路左边界
    double curr_left_bound_lane = curr_lane_left_width + (lane_borrow_info == LaneBorrowInfo::LEFT_BORROW
                                    ? curr_neighbor_lane_width: 0.0);

    double curr_right_bound_lane = -curr_lane_right_width -
        (lane_borrow_info == LaneBorrowInfo::RIGHT_BORROW? curr_neighbor_lane_width: 0.0);

    double curr_left_bound = 0.0;
    double curr_right_bound = 0.0;

// 根据配置文件中的is_extend_lane_bounds_to_include_adc（计算的边界值是否需要考虑自车状态）以及上述三组变量计算左右边界值
/*
如果需要考虑自车状态，则在计算时边界值为同时考虑基于自车信息的左右边界和基于车道边界的左右边界，取两者的边界较大值；若不需要考虑自车状态，
则只需以基于车道边界的左右边界作为边界值。
另外需要注意的是，得到的边界值curr_left_bound、 curr_right_bound是基于参考线的，因此需要考虑参考线相对map的偏移量offset_to_map。
*/
    if (config_.path_bounds_decider_config().is_extend_lane_bounds_to_include_adc() ||
        is_fallback_lanechange) {
      //扩展路径边界以将ADC包括在回退或更改通道路径边界中。
      /*
      curr_left_bound_adc、curr_right_bound_adc，基于自车信息的左右边界，
      根据ADC_speed_buffer，ADC_buffer（0.5，可以认为是横向安全区间）,
      半车宽，adc_l_to_lane_center_（自车相对于车道中心的偏移量）计算
      */
      double curr_left_bound_adc = std::fmax(adc_l_to_lane_center_,adc_l_to_lane_center_ + ADC_speed_buffer) +
          // 获取ADC中心和边缘之间的缓冲区
          GetBufferBetweenADCCenterAndEdge() + ADC_buffer;
      curr_left_bound = std::fmax(curr_left_bound_lane, curr_left_bound_adc) - offset_to_map;

      double curr_right_bound_adc = std::fmin(adc_l_to_lane_center_,adc_l_to_lane_center_ + ADC_speed_buffer) -
          GetBufferBetweenADCCenterAndEdge() - ADC_buffer;
      curr_right_bound = std::fmin(curr_right_bound_lane, curr_right_bound_adc) - offset_to_map;
    } else {
      curr_left_bound = curr_left_bound_lane - offset_to_map;
      curr_right_bound = curr_right_bound_lane - offset_to_map;
    }

    ADEBUG << "At s = " << curr_s
           << ", left_lane_bound = " << curr_lane_left_width
           << ", right_lane_bound = " << curr_lane_right_width
           << ", offset = " << offset_to_map;

    // 4. Update the boundary.
    /*
    调用函数UpdatePathBoundaryWithBuffer（）根据上述得到的边界值curr_left_bound、 curr_right_bound，
    以及车道边界是否是道路边界的判定结果is_left_lane_boundary，is_right_lane_boundary更新boundary
    */
    if (!UpdatePathBoundaryWithBuffer(i, curr_left_bound, curr_right_bound,
                                      path_bound, is_left_lane_boundary,
                                      is_right_lane_boundary)) {
      path_blocked_idx = static_cast<int>(i);
    }
    if (path_blocked_idx != -1) {
      break;
    }
  }
  // 从boundary中删除被截断之后的边界点
  TrimPathBounds(path_blocked_idx, path_bound);

  if (lane_borrow_info == LaneBorrowInfo::NO_BORROW) {
    *borrow_lane_type = "";
  } else {
    *borrow_lane_type = borrowing_reverse_lane ? "reverse" : "forward";
  }

  return true;
}

PS:
涉及到的几个参数：

ADC_speed_buffer，我的理解是车辆以最大横向加速度将横向速度降为0所经过的距离。

    double ADC_speed_buffer = (adc_frenet_ld_ > 0 ? 1.0 : -1.0) *
                              adc_frenet_ld_ * adc_frenet_ld_ /
                              kMaxLateralAccelerations / 2.0;

curr_left_bound_lane、curr_right_bound_lane基于车道线的左右边界，如果产生变道，则加上相邻车道的宽度。

    double curr_left_bound_lane = curr_lane_left_width + (lane_borrow_info == LaneBorrowInfo::LEFT_BORROW
                                    ? curr_neighbor_lane_width: 0.0);

    double curr_right_bound_lane = -curr_lane_right_width -
        (lane_borrow_info == LaneBorrowInfo::RIGHT_BORROW? curr_neighbor_lane_width: 0.0);

curr_left_bound_adc基于自车产生的边界。根据ADC_speed_buffer，ADC_buffer（0.5，可以认为是横向安全区间）,半车宽，adc_l_to_lane_center_（自车相对于车道中心的偏移量）计算。

      double curr_left_bound_adc = std::fmax(adc_l_to_lane_center_,adc_l_to_lane_center_ + ADC_speed_buffer) +
          // 获取ADC中心和边缘之间的缓冲区
          GetBufferBetweenADCCenterAndEdge() + ADC_buffer;

计算示意图大致如下：
在这里插入图片描述
（4）offset_to_map：相对地图的偏移量。

UpdatePathBoundaryWithBuffer

调用函数UpdatePathBoundaryWithBuffer（）根据上述得到的边界值curr_left_bound、 curr_right_bound，以及车道边界是否是道路边界的判定结果is_left_lane_boundary，is_right_lane_boundary更新boundary

bool PathBoundsDecider::UpdatePathBoundaryWithBuffer(
    size_t idx, double left_bound, double right_bound,
    PathBound* const path_boundaries, bool is_left_lane_bound,
    bool is_right_lane_bound) {
  // substract vehicle width when bound does not come from the lane boundary
  const double default_adc_buffer_coeff = 1.0;
  // 计算缓冲边界，默认是1.0
  double left_adc_buffer_coeff =
      (is_left_lane_bound
           ? config_.path_bounds_decider_config().adc_buffer_coeff()
           : default_adc_buffer_coeff);
  double right_adc_buffer_coeff =
      (is_right_lane_bound
           ? config_.path_bounds_decider_config().adc_buffer_coeff()
           : default_adc_buffer_coeff);

  // Update the right bound (l_min):
  double new_l_min =
      std::fmax(std::get<1>((*path_boundaries)[idx]),
                right_bound + right_adc_buffer_coeff *
                                  GetBufferBetweenADCCenterAndEdge());
  // Update the left bound (l_max):
  double new_l_max = std::fmin(
      std::get<2>((*path_boundaries)[idx]),
      left_bound - left_adc_buffer_coeff * GetBufferBetweenADCCenterAndEdge());

  // Check if ADC is blocked.
  // If blocked, don't update anything, return false.
  if (new_l_min > new_l_max) {
    ADEBUG << "Path is blocked at idx = " << idx;
    return false;
  }
  // Otherwise, update path_boundaries and center_line; then return true.
  std::get<1>((*path_boundaries)[idx]) = new_l_min;
  std::get<2>((*path_boundaries)[idx]) = new_l_max;
  return true;
}

TrimPathBounds

从boundary中删除被截断之后的边界点

void PathBoundsDecider::TrimPathBounds(const int path_blocked_idx,
                                       PathBound* const path_boundaries) {
  if (path_blocked_idx != -1) {
    if (path_blocked_idx == 0) {
      ADEBUG << "Completely blocked. Cannot move at all.";
    }
    int range = static_cast<int>(path_boundaries->size()) - path_blocked_idx;
    for (int i = 0; i < range; ++i) {
      path_boundaries->pop_back();
    }
  }
}

pull over

在这里插入图片描述

GeneratePullOverPathBound

核心逻辑在GeneratePullOverPathBound中。
InitPathBoundary在fallback部分已经讨论过了，接着来看GetBoundaryFromRoads。

GetBoundaryFromRoads

GetBoundaryFromRoads主要完成：获取道路边界；更新道路边界；裁剪block之后的边界点。

GetBoundaryFromRoads根据道路信息确定一个大致的边界。返回的边界是相对于车道中心（而不是reference_line）的，尽管在大多数情况下reference_line与车道中心的偏差可以忽略不计。

UpdatePullOverBoundaryByLaneBoundary

之后调用UpdatePullOverBoundaryByLaneBoundary更新pull over的边界。pull over有两种状态：PULL_OVER和EMERGENCY_PULL_OVER。
对于PULL_OVER，选取车道左边界作为边界。对于EMERGENCY_PULL_OVER，选取左/右车道边界作为边界，因为可能会在对向车道停车。

GetBoundaryFromStaticObstacles

根据静态障碍物对边界进行调整。它将确保边界不包含任何静态障碍，这样之后优化时生成的路径就不会与任何静态障碍碰撞。

在这里插入图片描述

SearchPullOverPosition

它的目的是搜索停车点，为了能够容纳车的尺寸，因为要先搜索可以停车的区域，然后在该区域内取一点作为停车点。搜索区域时，要先确定一个端点，然后向前或向后考察另一个端点，以及考察两端点之间的区域是否符合要求。

搜索pull over位置的过程：

根据pull_over_status.pull_over_type()判断是前向搜索（pull over开头第一个点），还是后向搜索（pull over末尾后一个点）
两层循环，外层控制搜索的索引idx，内层控制进一步的索引（前向idx+1，后向idx-1）。
根据内外两层循环的索引，判断搜索到的空间是否满足宽度和长度要求，判断是否可以pull over

bool PathBoundsDecider::SearchPullOverPosition(
    const Frame& frame, const ReferenceLineInfo& reference_line_info,
    const std::vector<std::tuple<double, double, double>>& path_bound,
    std::tuple<double, double, double, int>* const pull_over_configuration) {
  const auto& pull_over_status =
      injector_->planning_context()->planning_status().pull_over();

  // 搜索方向，默认前向搜索
  bool search_backward = false;  // search FORWARD by default

  double pull_over_s = 0.0;
  if (pull_over_status.pull_over_type() ==
      PullOverStatus::EMERGENCY_PULL_OVER) {...}

  int idx = 0;
  if (search_backward) {
    // 后向搜索，定位pull over末尾的一个点.
    idx = static_cast<int>(path_bound.size()) - 1;
    while (idx >= 0 && std::get<0>(path_bound[idx]) > pull_over_s) {
      --idx;
    }
  } else {
    // 前向搜索，定位emergency pull over开头后的第一个点.
    while (idx < static_cast<int>(path_bound.size()) &&
           std::get<0>(path_bound[idx]) < pull_over_s) {
      ++idx;
    }
  }
  
  // 为pull over搜索到一个可行的位置，主要是确定该区域的宽度和长度
  const double pull_over_space_length =
      kPulloverLonSearchCoeff *
          VehicleConfigHelper::GetConfig().vehicle_param().length() -
      FLAGS_obstacle_lon_start_buffer - FLAGS_obstacle_lon_end_buffer;
  const double pull_over_space_width =
      (kPulloverLatSearchCoeff - 1.0) *
      VehicleConfigHelper::GetConfig().vehicle_param().width();
  const double adc_half_width =
      VehicleConfigHelper::GetConfig().vehicle_param().width() / 2.0;

  // 2. Find a window that is close to road-edge.
  /*
    这里用了内外两层循环进行搜索，外层循环控制搜索的开始的端点idx。
    内层控制另一个端点。根据找到的两个端点，判断区域是否可以pull over
  */
  bool has_a_feasible_window = false;
  while ((search_backward && idx >= 0 &&
          std::get<0>(path_bound[idx]) - std::get<0>(path_bound.front()) >
              pull_over_space_length) ||
         (!search_backward && idx < static_cast<int>(path_bound.size()) &&
          std::get<0>(path_bound.back()) - std::get<0>(path_bound[idx]) >
              pull_over_space_length)) {

    while ((search_backward && j >= 0 &&
            std::get<0>(path_bound[idx]) - std::get<0>(path_bound[j]) <
                pull_over_space_length) ||
           (!search_backward && j < static_cast<int>(path_bound.size()) &&
            std::get<0>(path_bound[j]) - std::get<0>(path_bound[idx]) <
                pull_over_space_length)) {...}
    
    // 找到可行区域，获取停车区域的位置和姿态
    if (is_feasible_window) {
    ...
    break;}
    ...}  // 外层while
...
}

lane change

在这里插入图片描述
核心逻辑在GenerateLaneChangePathBound函数中，与前面的场景计算流程大同小异。主要核心部分在GetBoundaryFromLaneChangeForbiddenZone部分。

GenerateLaneChangePathBound

Status PathBoundsDecider::GenerateLaneChangePathBound(
    const ReferenceLineInfo& reference_line_info,
    std::vector<std::tuple<double, double, double>>* const path_bound) {
  // 1.初始化，和前面的步骤类似
  if (!InitPathBoundary(reference_line_info, path_bound)) {...}


  // 2. 根据道路和adc的信息获取一个大致的路径边界
  std::string dummy_borrow_lane_type;
  if (!GetBoundaryFromLanesAndADC(reference_line_info,
                                  LaneBorrowInfo::NO_BORROW, 0.1, path_bound,
                                  &dummy_borrow_lane_type, true)) {...}
 

  // 3. Remove the S-length of target lane out of the path-bound.
  GetBoundaryFromLaneChangeForbiddenZone(reference_line_info, path_bound);


  // 根据静态障碍物调整边界.
  if (!GetBoundaryFromStaticObstacles(reference_line_info.path_decision(),
                                      path_bound, &blocking_obstacle_id)) {...}
  ...
}

GetBoundaryFromLaneChangeForbiddenZone

GetBoundaryFromLaneChangeForbiddenZone函数运行流程如下：

如果当前位置可以变道，则直接变道
如果有一个lane-change的起点，则直接使用它
逐个检查变道前的点的边界，改变边界的值（如果已经过了变道点，则返回）

void PathBoundsDecider::GetBoundaryFromLaneChangeForbiddenZone(
    const ReferenceLineInfo& reference_line_info, PathBound* const path_bound) {
  // Sanity checks.
  CHECK_NOTNULL(path_bound);
  const ReferenceLine& reference_line = reference_line_info.reference_line();

  // If there is a pre-determined lane-change starting position, then use it;
  // otherwise, decide one.
  auto* lane_change_status = injector_->planning_context()
                                 ->mutable_planning_status()
                                 ->mutable_change_lane();
  // 1.当前位置直接变道。
  if (lane_change_status->is_clear_to_change_lane()) {
    ADEBUG << "Current position is clear to change lane. No need prep s.";
    lane_change_status->set_exist_lane_change_start_position(false);
    return;
  }
  double lane_change_start_s = 0.0;
  // 2.如果已经有一个lane-change的起点，就直接使用它，否则再找一个
  if (lane_change_status->exist_lane_change_start_position()) {
    common::SLPoint point_sl;
    reference_line.XYToSL(lane_change_status->lane_change_start_position(),
                          &point_sl);
    lane_change_start_s = point_sl.s();
  } else {
    // TODO(jiacheng): train ML model to learn this.
    // 设置为adc前方一段距离为变道起始点
    lane_change_start_s = FLAGS_lane_change_prepare_length + adc_frenet_s_;
    
    // Update the decided lane_change_start_s into planning-context.
    // 更新变道起始点的信息
    common::SLPoint lane_change_start_sl;
    lane_change_start_sl.set_s(lane_change_start_s);
    lane_change_start_sl.set_l(0.0);
    common::math::Vec2d lane_change_start_xy;
    reference_line.SLToXY(lane_change_start_sl, &lane_change_start_xy);
    lane_change_status->set_exist_lane_change_start_position(true);
    lane_change_status->mutable_lane_change_start_position()->set_x(
        lane_change_start_xy.x());
    lane_change_status->mutable_lane_change_start_position()->set_y(
        lane_change_start_xy.y());
  }

  // Remove the target lane out of the path-boundary, up to the decided S.
  if (lane_change_start_s < adc_frenet_s_) {
    // If already passed the decided S, then return.
    // lane_change_status->set_exist_lane_change_start_position(false);
    return;
  }
  // 逐个检查变道前的点的边界，改变边界的值
  for (size_t i = 0; i < path_bound->size(); ++i) {
    double curr_s = std::get<0>((*path_bound)[i]);
    if (curr_s > lane_change_start_s) {
      break;
    }
    double curr_lane_left_width = 0.0;
    double curr_lane_right_width = 0.0;
    double offset_to_map = 0.0;
    reference_line.GetOffsetToMap(curr_s, &offset_to_map);
    if (reference_line.GetLaneWidth(curr_s, &curr_lane_left_width,
                                    &curr_lane_right_width)) {
      double offset_to_lane_center = 0.0;
      reference_line.GetOffsetToMap(curr_s, &offset_to_lane_center);
      curr_lane_left_width += offset_to_lane_center;
      curr_lane_right_width -= offset_to_lane_center;
    }
    curr_lane_left_width -= offset_to_map;
    curr_lane_right_width += offset_to_map;

    std::get<1>((*path_bound)[i]) =
        adc_frenet_l_ > curr_lane_left_width   //右变道已跨过车道线，设定右界限
            ? curr_lane_left_width + GetBufferBetweenADCCenterAndEdge()
            : std::get<1>((*path_bound)[i]);
    std::get<1>((*path_bound)[i]) =
        std::fmin(std::get<1>((*path_bound)[i]), adc_frenet_l_ - 0.1);
    std::get<2>((*path_bound)[i]) =
        adc_frenet_l_ < -curr_lane_right_width
            ? -curr_lane_right_width - GetBufferBetweenADCCenterAndEdge()
            : std::get<2>((*path_bound)[i]);  //左变道已跨过车道线，设定左界限
    std::get<2>((*path_bound)[i]) =
        std::fmax(std::get<2>((*path_bound)[i]), adc_frenet_l_ + 0.1);
  }
}

PS：变道时是以相邻车道的参考线作为参考线。以向右变道为例，变道前（指跨过车道线线前adc_frenet_l_ > curr_lane_left_width），使用原先的path bound，不给右侧留空间；变道之后，再进行更新。

regular（lane borrow）

在这里插入图片描述

处理步骤非常相似，不同之处在于调用GetBoundaryFromLanesAndADC()时，会将3种LaneBorrowInfo传入，即不借道、向左借道、向右借道。而之前的场景在调用该函数时，均设定不借道。

Status PathBoundsDecider::GenerateRegularPathBound(
    const ReferenceLineInfo& reference_line_info,
    const LaneBorrowInfo& lane_borrow_info, PathBound* const path_bound,
    std::string* const blocking_obstacle_id,
    std::string* const borrow_lane_type) {
  // 1. Initialize the path boundaries to be an indefinitely large area.
  if (!InitPathBoundary(reference_line_info, path_bound)) {
    const std::string msg = "Failed to initialize path boundaries.";
    AERROR << msg;
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }
  // PathBoundsDebugString(*path_bound);

  // 2. Decide a rough boundary based on lane info and ADC's position
  if (!GetBoundaryFromLanesAndADC(reference_line_info, lane_borrow_info, 0.1,
                                  path_bound, borrow_lane_type)) {
    const std::string msg =
        "Failed to decide a rough boundary based on "
        "road information.";
    AERROR << msg;
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }
  // PathBoundsDebugString(*path_bound);

  // TODO(jiacheng): once ready, limit the path boundary based on the
  //                 actual road boundary to avoid getting off-road.

  // 3. Fine-tune the boundary based on static obstacles
  PathBound temp_path_bound = *path_bound;
  if (!GetBoundaryFromStaticObstacles(reference_line_info.path_decision(),
                                      path_bound, blocking_obstacle_id)) {
    const std::string msg =
        "Failed to decide fine tune the boundaries after "
        "taking into consideration all static obstacles.";
    AERROR << msg;
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }
  // Append some extra path bound points to avoid zero-length path data.
  int counter = 0;
  while (!blocking_obstacle_id->empty() &&
         path_bound->size() < temp_path_bound.size() &&
         counter < kNumExtraTailBoundPoint) {
    path_bound->push_back(temp_path_bound[path_bound->size()]);
    counter++;
  }
  // PathBoundsDebugString(*path_bound);

  // 4. Adjust the boundary considering dynamic obstacles
  // TODO(all): may need to implement this in the future.

  ADEBUG << "Completed generating path boundaries.";
  return Status::OK();
}

流程和上面的几个基本类似，借道有三种类型

  enum class LaneBorrowInfo {
    LEFT_BORROW,
    NO_BORROW,
    RIGHT_BORROW,
  };