文章目录
- 前言
- PATH_BORROW_DECIDER功能简介
- PATH_BORROW_DECIDER相关配置
- PATH_BORROW_DECIDER总体流程
- PATH_BORROW_DECIDER相关子函数
- IsNecessaryToBorrowLane
- IsBlockingObstacleFarFromIntersection
- IsNonmovableObstacle
- CheckLaneBorrow
- 参考
前言
在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_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
本文将继续介绍第三个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.txt
task_config: {
task_type: PATH_LANE_BORROW_DECIDER
path_lane_borrow_decider_config {
allow_lane_borrowing: true
}
}
// modules/planning/conf/planning_config.pb.txt
default_task_config: {
task_type: PATH_LANE_BORROW_DECIDER
path_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 path
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);
// 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 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;
}
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 direction
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;
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 vehicle
continue;
}
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