planning模块(13) task PATH_DECIDER

modules/planning/scenarios/lane_follow/conf/pipeline.pb.txt 中的task会顺序执行,不过本专栏只介绍与路径规划和速度规划相关的task
本篇主要介绍task PATH_DECIDER

Status PathDecider::Process(const ReferenceLineInfo *reference_line_info,
                            const PathData &path_data,
                            PathDecision *const path_decision)

reference_line_info:当前参考线信息
path_data:路径规划出来的路径
path_decision:路径决策,在planning模块(12)-路径规划后构建速度规划所用的数据中通过AssessPath进行了初始化,并设置了默认值

const auto &vehicle_box =
        common::VehicleConfigHelper::Instance()->GetBoundingBox(path[0]);

获取以自车中心为中心的bounding box,构建过程可参考planning模块-静态障碍物碰撞检测

reference_line_info->GetBlockingObstacle()

是通过在planning模块(12)-路径规划后构建速度规划所用的数据 AssessPath函数

reference_line_info_->SetBlockingObstacle(
      curr_path_data.blocking_obstacle_id());

调用SetBlockingObstacle函数设置的

void ReferenceLineInfo::SetBlockingObstacle(
    const std::string& blocking_obstacle_id) {
  blocking_obstacle_ = path_decision_.Find(blocking_obstacle_id);
}

然后在SetBlockingObstacle函数中会通过obstacle_id在path_decision_中查找当前id的障碍物信息,path_decision_中的障碍物是通过ReferenceLineInfo::AddObstacle添加进来的,可以回看planning模块-路径边界障碍物回顾一下

if (reference_line_info->GetBlockingObstacle() != nullptr) {
    blocking_obstacle_id = reference_line_info->GetBlockingObstacle()->Id();
    int front_static_obstacle_cycle_counter =
        mutable_path_decider_status->front_static_obstacle_cycle_counter();
    mutable_path_decider_status->set_front_static_obstacle_cycle_counter(
        std::max(front_static_obstacle_cycle_counter, 0));
    mutable_path_decider_status->set_front_static_obstacle_cycle_counter(
        std::min(front_static_obstacle_cycle_counter + 1, 10));
    mutable_path_decider_status->set_front_static_obstacle_id(
        reference_line_info->GetBlockingObstacle()->Id());
  }

在planning模块(9)-路径规划(scenario/stage/task) 中介绍过injector_主要是一个数据缓存中心,管理planning过程中的实时数据和几乎所有的历史数据,用于规划任务的前后帧之间的承接,以及异常处理的回溯
上面逻辑主要从injector_获取到静态障碍物存在的帧数计数器,并更新主要是增加帧数计数,并设置障碍物ID

else {
    int front_static_obstacle_cycle_counter =
        mutable_path_decider_status->front_static_obstacle_cycle_counter();
    mutable_path_decider_status->set_front_static_obstacle_cycle_counter(
        std::min(front_static_obstacle_cycle_counter, 0));
    mutable_path_decider_status->set_front_static_obstacle_cycle_counter(
        std::max(front_static_obstacle_cycle_counter - 1, -10));
    if (mutable_path_decider_status->front_static_obstacle_cycle_counter() <
        -2) {
      std::string id = " ";
      mutable_path_decider_status->set_front_static_obstacle_id(id);
    }
  }

如果当前参考线不存在障碍物信息时,更新静态障碍物帧数计数器,此逻辑会减少计数,当计数小于-2时,认为当前障碍物不影响通行了,并将障碍物id设置为空

MakeObjectDecision函数

MakeStaticObstacleDecision函数

path_data:路径规划出来的路径
blocking_obstacle_id:当前参考线上，阻碍车辆行驶的静态障碍物id
path_decision:路径决策信息数据

const double half_width =
      common::VehicleConfigHelper::GetConfig().vehicle_param().width() / 2.0;
  const double lateral_radius = half_width + FLAGS_lateral_ignore_buffer;

lateral_radius:半车宽+横向buffer(默认3m)

for (const auto *obstacle : path_decision->obstacles().Items())

遍历当前帧的所有障碍物

为阻挡障碍物添加停止决策
GenerateObjectStopDecision函数
MinRadiusStopDistance函数
min_radius_stop_distance_默认是-1
MinSafeTurnRadius函数

double VehicleConfigHelper::MinSafeTurnRadius() {
  const auto &param = vehicle_config_.vehicle_param();
  double lat_edge_to_center =
      std::max(param.left_edge_to_center(), param.right_edge_to_center());
  double lon_edge_to_center =
      std::max(param.front_edge_to_center(), param.back_edge_to_center());
  return std::sqrt((lat_edge_to_center + param.min_turn_radius()) *
                       (lat_edge_to_center + param.min_turn_radius()) +
                   lon_edge_to_center * lon_edge_to_center);
}

lat_edge_to_center:获取到车辆左右边缘到后轴中心的最大值
lon_edge_to_center:获取到车辆前后边缘到后轴中心的最大值
lat_edge_to_center + param.min_turn_radius():车辆最外侧边缘到转弯时圆心的距离

std::sqrt((lat_edge_to_center + param.min_turn_radius()) *                (lat_edge_to_center + param.min_turn_radius()) + lon_edge_to_center * lon_edge_to_center);

MinSafeTurnRadius函数返回的是上图绿色线长度

double lateral_diff =
      vehicle_param.width() / 2.0 + std::max(std::fabs(sl_boundary_.start_l()),
                                             std::fabs(sl_boundary_.end_l()));

lateral_diff:为了保证车身不蹭到障碍物，自车"中轴线"至少需要横向挪开多远的距离
min_turn_radius - lateral_diff:可以理解为绕障碍物时的安全横向距离

double stop_distance =
      std::sqrt(std::fabs(min_turn_radius * min_turn_radius -
                          (min_turn_radius - lateral_diff) *
                              (min_turn_radius - lateral_diff))) +
      stop_distance_buffer;

以min_turn_radius为斜边,以min_turn_radius - lateral_diff为一条直角边，则上面的stop_distance表示的是安全停车距离，这个距离是以后轴中心为起点。

stop_distance -= vehicle_param.front_edge_to_center();

减去车头到后轴中心的距离，这个距离就是以车头为起点

object_stop.set_reason_code(StopReasonCode::STOP_REASON_OBSTACLE);

设置停车原因

object_stop.set_distance_s(-stop_distance);

相对于障碍物的相对距离，在障碍物后stop_distance的位置停车

const double stop_ref_s =
      obstacle.PerceptionSLBoundary().start_s() - stop_distance;

参考线上停车位置的纵向距离

const auto stop_ref_point =
      reference_line_info_->reference_line().GetReferencePoint(stop_ref_s);
  object_stop.mutable_stop_point()->set_x(stop_ref_point.x());
  object_stop.mutable_stop_point()->set_y(stop_ref_point.y());
  object_stop.set_stop_heading(stop_ref_point.heading());

根据这个位置的纵向距离，计算出基于参考线的(x,y)坐标及航向角
到这GenerateObjectStopDecision函数就介绍完了

path_decision->AddLongitudinalDecision("PathDecider/blocking_obstacle",
                                             obstacle->Id(), object_decision);

类型	含义	本质	决策类型
`ignore`	忽略	对规划无影响	横向决策
`stop`	停车	纵向强制约束	纵向决策
`follow`	跟随	纵向控制（ACC）	纵向决策
`yield`	让行	减速让路	纵向决策
`overtake`	超车	纵向 + 横向	纵向决策
`nudge`	轻微横向避让	横向	横向决策
`avoid`	避让	强避让（较少用）
`side_pass`	侧向绕行	复杂横向

纵向决策类型分为:stop, follow, yield,overtake
Obstacle::MergeLongitudinalDecision函数

    Obstacle::s_longitudinal_decision_safety_sorter_ = {
        {ObjectDecisionType::kIgnore, 0},
        {ObjectDecisionType::kOvertake, 100},
        {ObjectDecisionType::kFollow, 300},
        {ObjectDecisionType::kYield, 400},
        {ObjectDecisionType::kStop, 500}};

s_longitudinal_decision_safety_sorter_:是决策安全等级,数值越大决策越强 / 越保守 / 越安全
lhs:之前的决策
rhs:新的决策

const auto lhs_val =
      FindOrDie(s_longitudinal_decision_safety_sorter_, lhs.object_tag_case());
  const auto rhs_val =
      FindOrDie(s_longitudinal_decision_safety_sorter_, rhs.object_tag_case());

lhs_val:之前决策的安全等级
rhs_val:新的决策的安全等级

 else {
    if (lhs.has_ignore()) {
      return rhs;
    } else if (lhs.has_stop()) {
      return lhs.stop().distance_s() < rhs.stop().distance_s() ? lhs : rhs;
    } else if (lhs.has_yield()) {
      return lhs.yield().distance_s() < rhs.yield().distance_s() ? lhs : rhs;
    } else if (lhs.has_follow()) {
      return lhs.follow().distance_s() < rhs.follow().distance_s() ? lhs : rhs;
    } else if (lhs.has_overtake()) {
      return lhs.overtake().distance_s() > rhs.overtake().distance_s() ? lhs
                                                                       : rhs;
    } else {
      DCHECK(false) << "Unknown decision";
    }
  }

上面逻辑主要选择更安全保守的决策类型

  decisions_.push_back(decision);
  decider_tags_.push_back(decider_tag);

    if (sl_boundary.end_s() < frenet_path.front().s() ||
        sl_boundary.start_s() > frenet_path.back().s()) {
      path_decision->AddLongitudinalDecision("PathDecider/not-in-s",
                                             obstacle->Id(), object_decision);
      path_decision->AddLateralDecision("PathDecider/not-in-s", obstacle->Id(),
                                        object_decision);
      continue;
    }

障碍物不在路径规划计算出的路径范围内添加的路径决策，AddLateralDecision函数在planning模块-路径边界障碍物介绍过.

FrenetFramePoint FrenetFramePath::GetNearestPoint(const SLBoundary& sl) const {
  auto it_lower =
      std::lower_bound(begin(), end(), sl.start_s(), LowerBoundComparator);
  if (it_lower == end()) {
    return back();
  }
  auto it_upper =
      std::upper_bound(it_lower, end(), sl.end_s(), UpperBoundComparator);

lower_bound: 找到路径中第一个s≥sl.start_s的点
upper_bound: 找到路径中第一个s>sl.end_s的点
目的: 此时，我们只需要在 [it_lower, it_upper) 这个区间内寻找.这大大减少了计算量，因为我们只关心与障碍物纵向重叠的那部分路径.

for (auto it = it_lower; it != it_upper; ++it) {
    if (it->l() >= sl.start_l() && it->l() <= sl.end_l()) {
      return *it;
    }

在锁定的 s 范围内，程序开始检查每一个路径点的横向距离 l
如果路径点的 l 值恰好落在障碍物的横向范围 [start_l, end_l] 内，说明路径点已经在障碍物内部了
直接返回当前路径点

else if (it->l() > sl.end_l()) {
      double diff = it->l() - sl.end_l();
      if (diff < min_dist) {
        min_dist = diff;
        min_it = it;
      }
    } else {
      double diff = sl.start_l() - it->l();
      if (diff < min_dist) {
        min_dist = diff;
        min_it = it;
      }
    }

如果路径点不在障碍物内，则计算它到障碍物边界的最小横向距离
如果路径点在障碍物左侧（l>sl.end_l），距离 = l−sl.end_l.
如果路径点在障碍物右侧（l<sl.start_l），距离 = sl.start_l−l.
迭代: 循环会不断更新 min_dist，保存横向距离最近的迭代器 min_it(也就是离障碍物最近的路径点).

const auto frenet_point = frenet_path.GetNearestPoint(sl_boundary);
    const double curr_l = frenet_point.l();

首先，获取到路径上离障碍物最近的路径点，curr_l是最近路径点相对于参考线的横向距离

if (curr_l - lateral_radius > sl_boundary.end_l() ||
        curr_l + lateral_radius < sl_boundary.start_l()) {
      // 1. IGNORE if laterally too far away.
      path_decision->AddLateralDecision("PathDecider/not-in-l", obstacle->Id(),
                                        object_decision);
    }

curr_l - lateral_radius:自车的左边缘
curr_l + lateral_radius:自车的右边缘
如果自车的左边缘大于障碍物的最右侧边缘或者自车的右边缘小于障碍物的最左侧边缘，则添加一个横向 ignore 决策.

else if (sl_boundary.end_l() >= curr_l - min_nudge_l &&
               sl_boundary.start_l() <= curr_l + min_nudge_l) {
      if (config_.skip_overlap_stop_check()) {
        AINFO << "skip_overlap_stop_check";
      }

判断横向上是否进入了车辆路径的最小安全横向范围；如果进入了，而配置允许，则跳过“重叠必须 stop”这条强约束.
如果配置skip_overlap_stop_check为false
MergeWithMainStop函数

*object_decision.mutable_stop() = GenerateObjectStopDecision(*obstacle);

计算出停车点基于参考线的(x,y)坐标及航向角
MergeWithMainStop函数

common::PointENU stop_point = obj_stop.stop_point();
  common::SLPoint stop_line_sl;
  reference_line.XYToSL(stop_point, &stop_line_sl);

stop_line_sl:停车点基于参考线(s,l)坐标

stop_line_s = std::fmax(
      stop_line_s, adc_sl_boundary.end_s() -
                       vehicle_config.vehicle_param().front_edge_to_center());

保证停止点的纵向距离在自车后轴中心的纵向距离前面

  if (stop_line_s >= stop_reference_line_s_) {
    ADEBUG << "stop point is farther than current main stop point.";
    return false;
  }

如果当前决策周期在stop_line_s位置前面已经存在一个停止点，那么stop_line_s位置再当前周期就不需要再制作一个决策点了.

  main_stop_.Clear();
  main_stop_.set_reason_code(obj_stop.reason_code());
  main_stop_.set_reason("stop by " + obj_id);
  main_stop_.mutable_stop_point()->set_x(obj_stop.stop_point().x());
  main_stop_.mutable_stop_point()->set_y(obj_stop.stop_point().y());
  main_stop_.set_stop_heading(obj_stop.stop_heading());
  stop_reference_line_s_ = stop_line_s;

将最后的停止决策点，赋值给主决策变量main_stop_.

path_decision->AddLongitudinalDecision(
              "PathDecider/nearest-stop", obstacle->Id(), object_decision);

MergeWithMainStop返回true的时候，则添加这个停车点为最近停车决策点
如果返回false，则添加为非最近停车决策点

if (sl_boundary.end_l() < curr_l - min_nudge_l) {  // &&
        // sl_boundary.end_l() > curr_l - min_nudge_l - 0.3) {
        // LEFT_NUDGE
        ObjectNudge *object_nudge_ptr = object_decision.mutable_nudge();
        object_nudge_ptr->set_type(ObjectNudge::LEFT_NUDGE);
        object_nudge_ptr->set_distance_l(config_.static_obstacle_buffer());
        path_decision->AddLateralDecision("PathDecider/left-nudge",
                                          obstacle->Id(), object_decision);
      }

如果自车的左边缘大于障碍物的左边缘，则添加向左nudge的决策点

else if (sl_boundary.start_l() > curr_l + min_nudge_l) {  // &&
        // sl_boundary.start_l() < curr_l + min_nudge_l + 0.3) {
        // RIGHT_NUDGE
        ObjectNudge *object_nudge_ptr = object_decision.mutable_nudge();
        object_nudge_ptr->set_type(ObjectNudge::RIGHT_NUDGE);
        object_nudge_ptr->set_distance_l(-config_.static_obstacle_buffer());
        path_decision->AddLateralDecision("PathDecider/right-nudge",
                                          obstacle->Id(), object_decision);
      }

如果自车的右边缘小于障碍物的右边缘，则添加向右nudge的决策点

  if (config_.ignore_backward_obstacle()) {
    IgnoreBackwardObstacle(path_decision);
  }

如果ignore_backward_obstacle配置为true
IgnoreBackwardObstacle函数

bool PathDecider::IgnoreBackwardObstacle(PathDecision *const path_decision) {
  double adc_start_s = reference_line_info_->AdcSlBoundary().start_s();
  for (const auto *obstacle : path_decision->obstacles().Items()) {
    if (obstacle->IsStatic() || obstacle->IsVirtual()) {
      continue;
    }
    if (obstacle->Obstacle::PerceptionSLBoundary().end_s() < adc_start_s) {
      ObjectDecisionType object_decision;
      object_decision.mutable_ignore();
      path_decision->AddLongitudinalDecision(
          "PathDecider/ignore-backward-obstacle", obstacle->Id(),
          object_decision);
    }
  }
  return true;
}

对于自车后面的动态障碍物，添加为忽视后方障碍物决策
到这MakeObjectDecision 函数就介绍完了，PATH_DECIDER task也介绍完了.