【Apollo学习笔记】——规划模块TASK之PATH_BORROW_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_STAGEenabled: truetask_type: LANE_CHANGE_DECIDERtask_type: PATH_REUSE_DECIDERtask_type: PATH_LANE_BORROW_DECIDERtask_type: PATH_BOUNDS_DECIDERtask_type: PIECEWISE_JERK_PATH_OPTIMIZERtask_type: PATH_ASSESSMENT_DECIDERtask_type: PATH_DECIDERtask_type: RULE_BASED_STOP_DECIDERtask_type: SPEED_BOUNDS_PRIORI_DECIDERtask_type: SPEED_HEURISTIC_OPTIMIZERtask_type: SPEED_DECIDERtask_type: SPEED_BOUNDS_FINAL_DECIDERtask_type: PIECEWISE_JERK_SPEED_OPTIMIZER# task_type: PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZERtask_type: RSS_DECIDER

本文将继续介绍第三个TASK——PATH_BORROW_DECIDER

PATH_BORROW_DECIDER功能简介

主要功能：产生是否借道的决策
判断条件：
• 是否只有一条车道
• 是否存在阻塞道路的障碍物
• 阻塞障碍物是否远离路口
• 阻塞障碍物长期存在
• 旁边车道是实线还是虚线
当所有判断条件都满足时，会产生借道决策。

PATH_BORROW_DECIDER相关配置

PATH_BORROW_DECIDER的相关配置集中在以下两个文件：modules/planning/conf/planning_config.pb.txt和modules/planning/conf/scenario/lane_follow_config.pb.txt

// modules/planning/conf/scenario/lane_follow_config.pb.txttask_config: {task_type: PATH_LANE_BORROW_DECIDERpath_lane_borrow_decider_config {allow_lane_borrowing: true}}
// modules/planning/conf/planning_config.pb.txt
default_task_config: {task_type: PATH_LANE_BORROW_DECIDERpath_lane_borrow_decider_config {allow_lane_borrowing: true}
}

PATH_BORROW_DECIDER总体流程

流程依旧看Process：

主要完成数据检查；判断是否启用reuse path，若有则跳过剩余步骤；判断是否有必要进行借道操作等步骤。

Status PathLaneBorrowDecider::Process(Frame* const frame, ReferenceLineInfo* const reference_line_info) {// Sanity checks.CHECK_NOTNULL(frame);CHECK_NOTNULL(reference_line_info);// skip path_lane_borrow_decider if reused pathif (FLAGS_enable_skip_path_tasks && reference_line_info->path_reusable()) {// for debugAINFO << "skip due to reusing path";return Status::OK();}// By default, don't borrow any lane.reference_line_info->set_is_path_lane_borrow(false);// Check if lane-borrowing is needed, if so, borrow lane.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();
}

同时还可以注意一下相关结构体定义：
modules/planning/proto/planning_status.proto

message PathDeciderStatus {enum LaneBorrowDirection {LEFT_BORROW = 1;   // borrow left neighbor laneRIGHT_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;
}

PATH_BORROW_DECIDER相关子函数

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()) {// If originally borrowing neighbor lane:// 如果当前已经在借道状态了，并且自车车道可用的counter >=6，取消lane_borrow scenario 状态if (mutable_path_decider_status->able_to_use_self_lane_counter() >= 6) {// If have been able to use self-lane for some time, then switch to// non-lane-borrowing.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;}// Obstacle condition check for lane-borrowing:// 阻塞障碍物是否远离路口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;}// switch to lane-borrowing// set side-pass directionconst 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_directionbool left_borrowable;bool right_borrowable;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();
}

IsBlockingObstacleFarFromIntersection

IsBlockingObstacleFarFromIntersection流程部分在总体流程图中已经展示。

bool PathLaneBorrowDecider::IsBlockingObstacleFarFromIntersection(const ReferenceLineInfo& reference_line_info) {const auto& path_decider_status =injector_->planning_context()->planning_status().path_decider();const std::string blocking_obstacle_id =path_decider_status.front_static_obstacle_id();if (blocking_obstacle_id.empty()) {ADEBUG << "There is no blocking obstacle.";return true;}const Obstacle* blocking_obstacle =reference_line_info.path_decision().obstacles().Find(blocking_obstacle_id);if (blocking_obstacle == nullptr) {ADEBUG << "Blocking obstacle is no longer there.";return true;}// Get blocking obstacle's s.double blocking_obstacle_s =blocking_obstacle->PerceptionSLBoundary().end_s();ADEBUG << "Blocking obstacle is at s = " << blocking_obstacle_s;// Get intersection's s and compare with threshold.const auto& first_encountered_overlaps =reference_line_info.FirstEncounteredOverlaps();for (const auto& overlap : first_encountered_overlaps) {ADEBUG << overlap.first << ", " << overlap.second.DebugString();// if (// overlap.first != ReferenceLineInfo::CLEAR_AREA &&// overlap.first != ReferenceLineInfo::CROSSWALK &&// overlap.first != ReferenceLineInfo::PNC_JUNCTION &&if (overlap.first != ReferenceLineInfo::SIGNAL &&overlap.first != ReferenceLineInfo::STOP_SIGN) {continue;}auto distance = overlap.second.start_s - blocking_obstacle_s;if (overlap.first == ReferenceLineInfo::SIGNAL ||overlap.first == ReferenceLineInfo::STOP_SIGN) {if (distance < kIntersectionClearanceDist) {ADEBUG << "Too close to signal intersection (" << distance<< "m); don't SIDE_PASS.";return false;}} else {if (distance < kJunctionClearanceDist) {ADEBUG << "Too close to overlap_type[" << overlap.first << "] ("<< distance << "m); don't SIDE_PASS";return false;}}}return true;
}

IsNonmovableObstacle

在IsNonmovableObstacle() 中主要对前方障碍物进行判断，利用预测以及参考线的信息来进行判断。代码位置在modules/planning/common/obstacle_blocking_analyzer.cc。大致流程在总体流程图中已经展示。

bool IsNonmovableObstacle(const ReferenceLineInfo& reference_line_info,const Obstacle& obstacle) {// Obstacle is far away.const SLBoundary& adc_sl_boundary = reference_line_info.AdcSlBoundary();if (obstacle.PerceptionSLBoundary().start_s() >adc_sl_boundary.end_s() + kAdcDistanceThreshold) {ADEBUG << " - It is too far ahead and we are not so sure of its status.";return false;}// Obstacle is parked obstacle.if (IsParkedVehicle(reference_line_info.reference_line(), &obstacle)) {ADEBUG << "It is Parked and NON-MOVABLE.";return true;}// Obstacle is blocked by others too.for (const auto* other_obstacle :reference_line_info.path_decision().obstacles().Items()) {if (other_obstacle->Id() == obstacle.Id()) {continue;}if (other_obstacle->IsVirtual()) {continue;}if (other_obstacle->PerceptionSLBoundary().start_l() >obstacle.PerceptionSLBoundary().end_l() ||other_obstacle->PerceptionSLBoundary().end_l() <obstacle.PerceptionSLBoundary().start_l()) {// not blocking the backside vehiclecontinue;}double delta_s = other_obstacle->PerceptionSLBoundary().start_s() -obstacle.PerceptionSLBoundary().end_s();if (delta_s < 0.0 || delta_s > kObstaclesDistanceThreshold) {continue;}// TODO(All): Fix the segmentation bug for large vehicles, otherwise// the follow line will be problematic.ADEBUG << " - It is blocked by others, and will move later.";return false;}ADEBUG << "IT IS NON-MOVABLE!";return true;
}

CheckLaneBorrow

主要根据前方道路的线型判断是否可以借道；在此函数中2m间隔一个点遍历前视距离。

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();// 定义左、右车道是否可借用的标志位，并初始化为true*left_neighbor_lane_borrowable = true;*right_neighbor_lane_borrowable = true;static constexpr double kLookforwardDistance = 100.0;// 获取自车的S坐标double check_s = reference_line_info.AdcSlBoundary().end_s();// 前视距离const double lookforward_distance =std::min(check_s + kLookforwardDistance, reference_line.Length());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::DOUBLE_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;}
}

参考

[1] Apollo Planning决策规划代码详细解析 (8): PathLaneBorrowDecider
[2] Apollo规划模块详解（六）：算法实现-path lane borrow decider
[3] https://www.cnblogs.com/icathianrain/p/14407619.html