Apollo Planning——PathLaneBorrowDecider

引用

组件数据缓存 DependencyInjector

• DependencyInjector：依赖注入器，这是一个过于专业的名词，来自软件设计模式的依赖倒置原则的一种具体实现方式，起到模块解耦作用。
• “依赖倒置原则(Dependence Inversion Principle)是程序要依赖于抽象接口，不要依赖于具体实现。简单的说就是要求对抽象进行编程，不要对实现进行编程，这样就降低了客户与实现模块间的耦合。”
• DependencyInjector本质上是一个数据缓存中心，叫做DataCacheCenter更为贴切，可以叫做数据缓存器
• DependencyInjector对象内部管理了planning模块工程过程中的实时数据和几乎全部历史数据，以便于规划任务的前后帧之间的承接，以及异常处理的回溯。
• DependencyInjector是以空对象的形式引入到planning组件中，进而引入到planning模块中用来承载中间数据。

DependencyInjector类结构如下：

依赖注入结构中主要有6类成员变量，分别如下：

• PlanningContext planning_context_：负责planning上下文的缓存，比如是否触发重新路由的ReroutingStatus信息

• History history_： 负责障碍物状态的缓存，包括运动状态，决策结果。该数据与routing结果绑定，routing变更后会清理掉历史数据

• FrameHistory 　frame_history_：是一个可索引队列，负责planning的输入，输出等主要信息的缓存，以Frame类进行组织，内部包含LocalView结构体（负责输入数据的融合管理）。与上述的History是不同的是，该缓数据自模块启动后就开始缓存所有的Frame对象，不受routing变动的影响。

• EgoInfo ego_info_：提供车辆动、静信息，即车辆运动状态参数（轨迹、速度、加速度等）和车辆结构参数（长宽高等）

• apollo::common::VehicleStateProvider vehicle_state_：车辆状态提供器，用于获取车辆实时信息

• LearningBasedData learning_based_data_：基于学习的数据，用于学习建模等

modules/planning/common/dependency_injector.h

class DependencyInjector {
 public:
  DependencyInjector() = default;
  ~DependencyInjector() = default;

  PlanningContext* planning_context() {
    return &planning_context_;
  }
  FrameHistory* frame_history() {
    return &frame_history_;
  }
  History* history() {
    return &history_;
  }
  EgoInfo* ego_info() {
    return &ego_info_;
  }
  apollo::common::VehicleStateProvider* vehicle_state() {
    return &vehicle_state_;
  }
  LearningBasedData* learning_based_data() {
    return &learning_based_data_;
  }

 private:
  PlanningContext planning_context_;
  FrameHistory frame_history_;
  History history_;
  EgoInfo ego_info_;
  apollo::common::VehicleStateProvider vehicle_state_;
  LearningBasedData learning_based_data_;
};

modules/planning/common/planning_context.h

class PlanningContext {
 public:
  PlanningContext() = default;

  void Clear();
  void Init();

  /*
   * please put all status info inside PlanningStatus for easy maintenance.
   * do NOT create new struct at this level.
   * */

  const PlanningStatus& planning_status() const { return planning_status_; }
  PlanningStatus* mutable_planning_status() { return &planning_status_; }

 private:

  /**
   * modules/planning/proto/planning_status.proto
   * 
   * message PlanningStatus {
   * optional BareIntersectionStatus bare_intersection = 1;
   * optional ChangeLaneStatus change_lane = 2;
   * optional CreepDeciderStatus creep_decider = 3;
   * optional CrosswalkStatus crosswalk = 4;
   * optional DestinationStatus destination = 5;
   * optional EmergencyStopStatus emergency_stop = 6;
   * optional OpenSpaceStatus open_space = 7;
   * optional ParkAndGoStatus park_and_go = 8;
   * optional PathDeciderStatus path_decider = 9;
   * optional PullOverStatus pull_over = 10;
   * optional ReroutingStatus rerouting = 11;
   * optional ScenarioStatus scenario = 12;
   * optional SpeedDeciderStatus speed_decider = 13;
   * optional StopSignStatus stop_sign = 14;
   * optional TrafficLightStatus traffic_light = 15;
   * optional YieldSignStatus yield_sign = 16;
   * }
   * **/
  PlanningStatus planning_status_;
};

PlanningStatus

message PlanningStatus {
  optional BareIntersectionStatus bare_intersection = 1;
  optional ChangeLaneStatus change_lane = 2;
  optional CreepDeciderStatus creep_decider = 3;
  optional CrosswalkStatus crosswalk = 4;
  optional DestinationStatus destination = 5;
  optional EmergencyStopStatus emergency_stop = 6;
  optional OpenSpaceStatus open_space = 7;
  optional ParkAndGoStatus park_and_go = 8;
  optional PathDeciderStatus path_decider = 9;
  optional PullOverStatus pull_over = 10;
  optional ReroutingStatus rerouting = 11;
  optional ScenarioStatus scenario = 12;
  optional SpeedDeciderStatus speed_decider = 13;
  optional StopSignStatus stop_sign = 14;
  optional TrafficLightStatus traffic_light = 15;
  optional YieldSignStatus yield_sign = 16;
}

PathDeciderStatus

message PathDeciderStatus {
  enum LaneBorrowDirection {
    LEFT_BORROW = 1;   // borrow left neighbor lane
    RIGHT_BORROW = 2;  // borrow right neighbor lane
  }
  optional int32 front_static_obstacle_cycle_counter = 1 [default = 0];
  optional int32 able_to_use_self_lane_counter = 2 [default = 0];
  optional bool is_in_path_lane_borrow_scenario = 3 [default = false];
  optional string front_static_obstacle_id = 4 [default = ""];
  repeated LaneBorrowDirection decided_side_pass_direction = 5;
}

它包含以下字段：

• LaneBorrowDirection：枚举类型，表示车道借用的方向，包括LEFT_BORROW和RIGHT_BORROW两种。

• front_static_obstacle_cycle_counter：可选的int32类型字段，表示前方静态障碍物的循环计数器，默认值为0。

• able_to_use_self_lane_counter：可选的int32类型字段，表示能够使用自身车道的计数器，默认值为0。

• is_in_path_lane_borrow_scenario：可选的bool类型字段，表示是否处于车道借用场景中，默认值为false。

• front_static_obstacle_id：可选的string类型字段，表示前方静态障碍物的ID，默认值为空字符串。

• decided_side_pass_direction：重复的LaneBorrowDirection类型字段，表示已经决定的侧向通行方向。

PathDeciderStatus消息类型用于在路径决策过程中存储状态信息，例如前方障碍物的信息、车道借用的方向等

概述

借道决策器的主要功能为判断当前车辆是否具备借道能力，其实现在类PathLaneBorrowDecider的成员函数process()中。process()函数的功能一共分为三部分：检查输入、如果路径复用则跳过借道决策。

路径：modules/planning/tasks/deciders/path_lane_borrow_decider/path_lane_borrow_decider.cc

Status PathLaneBorrowDecider::Process(
    Frame* const frame, ReferenceLineInfo* const reference_line_info) {
  // Sanity checks.
  CHECK_NOTNULL(frame);
  CHECK_NOTNULL(reference_line_info);

  // 如果路径复用则跳过借道决策
  if (FLAGS_enable_skip_path_tasks && reference_line_info->path_reusable()) {
    // for debug
    AINFO << "skip due to reusing path";
    return Status::OK();
  }

  // By default, don't borrow any lane.
  reference_line_info->set_is_path_lane_borrow(false);
  /*
    task_config: {
    task_type: PATH_LANE_BORROW_DECIDER
    path_lane_borrow_decider_config {
      allow_lane_borrowing: true
    }
  }
  */
  if (Decider::config_.path_lane_borrow_decider_config().allow_lane_borrowing() &&
      IsNecessaryToBorrowLane(*frame, *reference_line_info)) {
    reference_line_info->set_is_path_lane_borrow(true);
  }
  return Status::OK();
}

输入、输出

输出信息

借道判断IsNecessaryToBorrowLane（）

借道判断主要通过核心函数IsNecessaryToBorrowLane()判断是否借道，主要涉及一些rules，包括距离信号交叉口的距离，与静态障碍物的距离，是否是单行道，是否所在车道左右车道线是虚线等规则。主要有两个功能：

• 已处于借道场景下判断是否退出避让；

• 未处于借道场景下判断是否具备借道能力。

bool PathLaneBorrowDecider::IsNecessaryToBorrowLane(
    const Frame& frame, const ReferenceLineInfo& reference_line_info) {
  auto* mutable_path_decider_status = injector_->planning_context()->mutable_planning_status()->mutable_path_decider();
  // 如果当前处于借道场景中                                        
  if (mutable_path_decider_status->is_in_path_lane_borrow_scenario()) {
    // 根据数值优化求解轨迹后的信息计算是否退出借道场景（如：避让输出轨迹无解时退出借道），
    // 滤波周期为6 able_to_use_self_lane_counter：能够使用自身车道的计数器
    if (mutable_path_decider_status->able_to_use_self_lane_counter() >= 6) {
      // 如果已经能够使用自车道一段时间，那么就切换到非借道。
      mutable_path_decider_status->set_is_in_path_lane_borrow_scenario(false);
      mutable_path_decider_status->clear_decided_side_pass_direction();
      AINFO << "Switch from LANE-BORROW path to SELF-LANE path.";
    }
  } else { // 如果当前不处于借道场景中
    // If originally not borrowing neighbor lane:
    ADEBUG << "Blocking obstacle ID[" << mutable_path_decider_status->front_static_obstacle_id() << "]";
    // ADC requirements check for lane-borrowing:

    // 当下面这些条件必须全部满足，才能借道:
    // 只有一条参考线，才能借道
    if (!HasSingleReferenceLine(frame)) {
      return false;
    }
    // 起点速度小于最大借道允许速度
    if (!IsWithinSidePassingSpeedADC(frame)) {
      return false;
    }

    // 阻塞障碍物必须远离路口
    if (!IsBlockingObstacleFarFromIntersection(reference_line_info)) {
      return false;
    }
    // 阻塞障碍物会一直存在
    if (!IsLongTermBlockingObstacle()) {
      return false;
    }
    // 阻塞障碍物与终点位置满足要求
    if (!IsBlockingObstacleWithinDestination(reference_line_info)) {
      return false;
    }
    // 为可侧面通过的障碍物
    if (!IsSidePassableObstacle(reference_line_info)) {
      return false;
    }

    // 在无避让方向时重新计算避让方向，若左、右借道空间均不满足则不借道，is_in_path_lane_borrow_scenario_标志为false；
    // 左借道条件满足左借道、右借道条件满足右借道，is_in_path_lane_borrow_scenario_为true。
    const auto& path_decider_status = injector_->planning_context()->planning_status().path_decider();
    if (path_decider_status.decided_side_pass_direction().empty()) {
      // first time init decided_side_pass_direction
      bool left_borrowable;
      bool right_borrowable;
      // 2m间隔一个点遍历车前100m参考线或全部参考线，如果车道线类型为黄实线、白实线则不借道。
      CheckLaneBorrow(reference_line_info, &left_borrowable, &right_borrowable);
      if (!left_borrowable && !right_borrowable) {
        mutable_path_decider_status->set_is_in_path_lane_borrow_scenario(false);
        return false;
      } else {
        mutable_path_decider_status->set_is_in_path_lane_borrow_scenario(true);
        if (left_borrowable) {
          mutable_path_decider_status->add_decided_side_pass_direction(PathDeciderStatus::LEFT_BORROW);
        }
        if (right_borrowable) {
          mutable_path_decider_status->add_decided_side_pass_direction(PathDeciderStatus::RIGHT_BORROW);
        }
      }
    }

    AINFO << "Switch from SELF-LANE path to LANE-BORROW path.";
  }
  return mutable_path_decider_status->is_in_path_lane_borrow_scenario();
}

已处于借道场景下

已处于避让场景下，根据数值优化求解轨迹后的信息计算是否退出借道场景（如：避让输出轨迹无解时退出借道），滤波周期为6，条件满足后is_path_lane_borrow_标志、is_in_path_lane_borrow_scenario_标志均为false。

  auto* mutable_path_decider_status = injector_->planning_context()->mutable_planning_status()->mutable_path_decider();
  // 如果当前处于借道场景中                                        
  if (mutable_path_decider_status->is_in_path_lane_borrow_scenario()) {
    // 根据数值优化求解轨迹后的信息计算是否退出借道场景（如：避让输出轨迹无解时退出借道），
    // 滤波周期为6 able_to_use_self_lane_counter：能够使用自身车道的计数器
    if (mutable_path_decider_status->able_to_use_self_lane_counter() >= 6) {
      // 如果已经能够使用自车道一段时间，那么就切换到非借道。
      mutable_path_decider_status->set_is_in_path_lane_borrow_scenario(false);
      mutable_path_decider_status->clear_decided_side_pass_direction();
      AINFO << "Switch from LANE-BORROW path to SELF-LANE path.";
    }
  } else { // 如果当前不处于借道场景中

未处于借道场景下

在车辆未借道时输出车辆当前的借道状态:左借道、右借道 、不借道。

1）避让约束条件如下:

    // 当下面这些条件必须全部满足，才能借道:
    // 只有一条参考线，才能借道
    if (!HasSingleReferenceLine(frame)) {
      return false;
    }
    // 起点速度小于最大借道允许速度
    if (!IsWithinSidePassingSpeedADC(frame)) {
      return false;
    }
    // 阻塞障碍物必须远离路口
    if (!IsBlockingObstacleFarFromIntersection(reference_line_info)) {
      return false;
    }
    // 阻塞障碍物会一直存在
    if (!IsLongTermBlockingObstacle()) {
      return false;
    }
    // 阻塞障碍物与终点位置满足要求
    if (!IsBlockingObstacleWithinDestination(reference_line_info)) {
      return false;
    }
    // 为可侧面通过的障碍物
    if (!IsSidePassableObstacle(reference_line_info)) {
      return false;
    }

2.避让方向
在无避让方向时重新计算避让方向，若左、右借道空间均不满足则不借道，is_in_path_lane_borrow_scenario_标志为false；左借道条件满足左借道、右借道条件满足右借道，is_in_path_lane_borrow_scenario_为true。

    // 在无避让方向时重新计算避让方向，若左、右借道空间均不满足则不借道，is_in_path_lane_borrow_scenario_标志为false；
    // 左借道条件满足左借道、右借道条件满足右借道，is_in_path_lane_borrow_scenario_为true。
    const auto& path_decider_status = injector_->planning_context()->planning_status().path_decider();
    if (path_decider_status.decided_side_pass_direction().empty()) {
      // first time init decided_side_pass_direction
      bool left_borrowable;
      bool right_borrowable;
      // 2m间隔一个点遍历车前100m参考线或全部参考线，如果车道线类型为黄实线、白实线则不借道。
      CheckLaneBorrow(reference_line_info, &left_borrowable, &right_borrowable);
      if (!left_borrowable && !right_borrowable) {
        mutable_path_decider_status->set_is_in_path_lane_borrow_scenario(false);
        return false;
      } else {
        mutable_path_decider_status->set_is_in_path_lane_borrow_scenario(true);
        if (left_borrowable) {
          mutable_path_decider_status->add_decided_side_pass_direction(PathDeciderStatus::LEFT_BORROW);
        }
        if (right_borrowable) {
          mutable_path_decider_status->add_decided_side_pass_direction(PathDeciderStatus::RIGHT_BORROW);
        }
      }
    }

重要子函数

CheckLaneBorrow（）

在此函数中2m间隔一个点遍历车前100m参考线或全部参考线，如果车道线类型为黄实线、白实线则不借道。

void PathLaneBorrowDecider::CheckLaneBorrow(
    const ReferenceLineInfo& reference_line_info,
    bool* left_neighbor_lane_borrowable, bool* right_neighbor_lane_borrowable) {
  // 获取参考线信息中的参考线
  const ReferenceLine& reference_line = reference_line_info.reference_line();

  *left_neighbor_lane_borrowable = true;
  *right_neighbor_lane_borrowable = true;

  static constexpr double kLookforwardDistance = 100.0;
  double check_s = reference_line_info.AdcSlBoundary().end_s();
  const double lookforward_distance = std::min(check_s + kLookforwardDistance, reference_line.Length());
  // 2m间隔一个点遍历车前100m参考线或全部参考线，如果车道线类型为黄实线、白实线则不借道。
  while (check_s < lookforward_distance) {
    auto ref_point = reference_line.GetNearestReferencePoint(check_s);
    if (ref_point.lane_waypoints().empty()) {
      *left_neighbor_lane_borrowable = false;
      *right_neighbor_lane_borrowable = false;
      return;
    }

    const auto waypoint = ref_point.lane_waypoints().front();
    hdmap::LaneBoundaryType::Type lane_boundary_type = hdmap::LaneBoundaryType::UNKNOWN;

    if (*left_neighbor_lane_borrowable) {
      lane_boundary_type = hdmap::LeftBoundaryType(waypoint);
      if (lane_boundary_type == hdmap::LaneBoundaryType::SOLID_YELLOW || lane_boundary_type == hdmap::LaneBoundaryType::SOLID_WHITE) {
        *left_neighbor_lane_borrowable = false;
      }
      ADEBUG << "s[" << check_s << "] left_lane_boundary_type[" << LaneBoundaryType_Type_Name(lane_boundary_type) << "]";
    }
    if (*right_neighbor_lane_borrowable) {
      lane_boundary_type = hdmap::RightBoundaryType(waypoint);
      if (lane_boundary_type == hdmap::LaneBoundaryType::SOLID_YELLOW || lane_boundary_type == hdmap::LaneBoundaryType::SOLID_WHITE) {
        *right_neighbor_lane_borrowable = false;
      }
      ADEBUG << "s[" << check_s << "] right_neighbor_lane_borrowable[" << LaneBoundaryType_Type_Name(lane_boundary_type) << "]";
    }
    check_s += 2.0;
  }
}

IsSidePassableObstacle（）

IsSidePassableObstacle（）的实现主要在IsNonmovableObstacle（）函数中实现。

IsNonmovableObstacle（）函数中障碍物距离车辆很远不借道、前方障碍物停车借道、障碍物是被其他障碍物阻塞不借道

总结

经过借道决策模块计算输出的借道能力仅表示为当前车辆有借道需求且满足借道条件，但是否真的借道仍需要下游的路径生成、路径选优决定。