Baidu Apollo代码解析之path_bounds_decider
大家好，我已经把CSDN上的博客迁移到了知乎上，欢迎大家在知乎关注我的专栏慢慢悠悠小马车（https://zhuanlan.zhihu.com/c_1132958996826546176）。希望大家可以多多交流，互相学习。...
·
大家好，我已经把CSDN上的博客迁移到了知乎上，欢迎大家在知乎关注我的专栏慢慢悠悠小马车（https://zhuanlan.zhihu.com/duangduangduang）。希望大家可以多多交流，互相学习。
目录
PathBoundsDecider是负责计算道路上可行区域边界的类，其产生的结果如下面代码所示，是对纵向s等间隔采样、横向d对应的左右边界。这样，即限定了s的范围，又限定了d的范围。（注：代码中大多用小写L代表横向，我为了容易辨别，使用d）PathBoundsDecider类继承自Decider类，而Decider类继承自Task类，所以PathBoundsDecider也是Apollo中众多任务的其中之一。文件路径是 apollo\modules\planning\tasks\deciders\path_bounds_decider\path_bounds_decider.cc，基于apollo 5.5.0版本。
// 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>;1. 处理流程
PathBoundsDecider类的主要工作在Process()中完成，在该函数中，分4种场景对PathBound进行计算，按处理的顺序分别是fallback、pull over、lane change、regular，regular场景根据是否lane borrow又分为no borrow、left borrow、right borrow，这3种子场景是在一个函数内处理的。之所以要不同场景对应不同的处理方式，我认为是在不同的场景下，自车会有不同的决策和行为，因此要考察的纵向和横向的范围就不一样，在计算时也要考虑不同的环境模型上下文。要注意的是，fallback对应的PathBound一定会生成，其余3个场景只有1个被激活，成功生成PathBound后退出函数。
Status PathBoundsDecider::Process(Frame* const frame,
ReferenceLineInfo* const reference_line_info) {
...
// Initialization.
InitPathBoundsDecider(*frame, *reference_line_info);
// Generate the fallback path boundary.
PathBound fallback_path_bound;
Status ret = GenerateFallbackPathBound(*reference_line_info, &fallback_path_bound);
...
// Update the fallback path boundary into the reference_line_info.
std::vector<std::pair<double, double>> fallback_path_bound_pair;
for (size_t i = 0; i < fallback_path_bound.size(); ++i) {
fallback_path_bound_pair.emplace_back(std::get<1>(fallback_path_bound[i]),
std::get<2>(fallback_path_bound[i]));
}
candidate_path_boundaries.emplace_back(std::get<0>(fallback_path_bound[0]),
kPathBoundsDeciderResolution,
fallback_path_bound_pair);
candidate_path_boundaries.back().set_label("fallback");
//1.先生成fallback界限，存入candidate_path_boundaries，并标记为“fallback”
// If pull-over is requested, generate pull-over path boundary.
...
if (plan_pull_over_path) {
PathBound pull_over_path_bound;
Status ret = GeneratePullOverPathBound(*frame, *reference_line_info,
&pull_over_path_bound);
if (!ret.ok()) {
AWARN << "Cannot generate a pullover path bound, do regular planning.";
} else {
// Update the fallback path boundary into the reference_line_info.
std::vector<std::pair<double, double>> pull_over_path_bound_pair;
for (size_t i = 0; i < pull_over_path_bound.size(); ++i) {
pull_over_path_bound_pair.emplace_back(std::get<1>(pull_over_path_bound[i]),
std::get<2>(pull_over_path_bound[i]));
}
candidate_path_boundaries.emplace_back(std::get<0>(pull_over_path_bound[0]),
kPathBoundsDeciderResolution, pull_over_path_bound_pair);
candidate_path_boundaries.back().set_label("regular/pullover");
//2.生成pullover界限，存入candidate_path_boundaries，并标记为“regular/pullover”
reference_line_info->SetCandidatePathBoundaries(
std::move(candidate_path_boundaries));
return Status::OK();
}
}
// If it's a lane-change reference-line, generate lane-change path boundary.
if (FLAGS_enable_smarter_lane_change && reference_line_info->IsChangeLanePath()) {
PathBound lanechange_path_bound;
Status ret = GenerateLaneChangePathBound(*reference_line_info,
&lanechange_path_bound);
...
// Update the fallback path boundary into the reference_line_info.
std::vector<std::pair<double, double>> lanechange_path_bound_pair;
for (size_t i = 0; i < lanechange_path_bound.size(); ++i) {
lanechange_path_bound_pair.emplace_back(std::get<1>(lanechange_path_bound[i]),
std::get<2>(lanechange_path_bound[i]));
}
candidate_path_boundaries.emplace_back(std::get<0>(lanechange_path_bound[0]),
kPathBoundsDeciderResolution, lanechange_path_bound_pair);
candidate_path_boundaries.back().set_label("regular/lanechange");
//3.生成lanechange界限，存入candidate_path_boundaries，并标记为“regular/lanechange”
reference_line_info->SetCandidatePathBoundaries(
std::move(candidate_path_boundaries));
return Status::OK();
}
// Generate regular path boundaries.
std::vector<LaneBorrowInfo> lane_borrow_info_list;
lane_borrow_info_list.push_back(LaneBorrowInfo::NO_BORROW);
if (reference_line_info->is_path_lane_borrow()) {
const auto& path_decider_status =
PlanningContext::Instance()->planning_status().path_decider();
for (const auto& lane_borrow_direction :
path_decider_status.decided_side_pass_direction()) {
if (lane_borrow_direction == PathDeciderStatus::LEFT_BORROW) {
lane_borrow_info_list.push_back(LaneBorrowInfo::LEFT_BORROW);
} else if (lane_borrow_direction == PathDeciderStatus::RIGHT_BORROW) {
lane_borrow_info_list.push_back(LaneBorrowInfo::RIGHT_BORROW);
}
}
}
// Try every possible lane-borrow option
for (const auto& lane_borrow_info : lane_borrow_info_list) {
PathBound regular_path_bound;
std::string blocking_obstacle_id = "";
std::string borrow_lane_type = "";
Status ret = GenerateRegularPathBound(
*reference_line_info, lane_borrow_info, ®ular_path_bound,
&blocking_obstacle_id, &borrow_lane_type);
...
// Update the path boundary into the reference_line_info.
std::vector<std::pair<double, double>> regular_path_bound_pair;
for (size_t i = 0; i < regular_path_bound.size(); ++i) {
regular_path_bound_pair.emplace_back(std::get<1>(regular_path_bound[i]),
std::get<2>(regular_path_bound[i]));
}
candidate_path_boundaries.emplace_back(std::get<0>(regular_path_bound[0]),
kPathBoundsDeciderResolution,
regular_path_bound_pair);
std::string path_label = "";
switch (lane_borrow_info) {
case LaneBorrowInfo::LEFT_BORROW:
path_label = "left";
break;
case LaneBorrowInfo::RIGHT_BORROW:
path_label = "right";
break;
default:
path_label = "self";
break;
}
//4.生成laneborrow界限，存入candidate_path_boundaries，并标记为“regular/...”
candidate_path_boundaries.back().set_label(
absl::StrCat("regular/", path_label, "/", borrow_lane_type));
candidate_path_boundaries.back().set_blocking_obstacle_id(
blocking_obstacle_id);
}
reference_line_info->SetCandidatePathBoundaries(std::move(candidate_path_boundaries));
return Status::OK();
}2. fallback场景
接下来，我们分析每种场景下的处理细节。fallback是其他3种场景计算PathBound失败时的备选，只考虑自车信息和静态道路信息，不考虑静态障碍物。因此，speed decider负责让自车在障碍物前停车。
Status PathBoundsDecider::GenerateFallbackPathBound(
const ReferenceLineInfo& reference_line_info, PathBound* const path_bound) {
// 1. Initialize the path boundaries to be an indefinitely large area.
if (!InitPathBoundary(reference_line_info, path_bound)) { ... }
// 2. Decide a rough boundary based on lane info and ADC's position
std::string dummy_borrow_lane_type;
if (!GetBoundaryFromLanesAndADC(reference_line_info,
LaneBorrowInfo::NO_BORROW, 0.5, path_bound,
&dummy_borrow_lane_type)) { ... }
return Status::OK();
}InitPathBoundary()在合理的s范围内，以kPathBoundsDeciderResolution为采样间隔对s采样，将每个采样点处的横向边界【右边界，左边界】设定为 【最小值，最大值】。
bool PathBoundsDecider::InitPathBoundary(
const ReferenceLineInfo& reference_line_info, PathBound* const path_bound) {
...
// Starting from ADC's current position, increment until the horizon, and
// set lateral bounds to be infinite at every spot.
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());
}
...
}GetBoundaryFromLanesAndADC()则包含了根据自车信息、车道信息计算PathBound的具体细节。首先根据当前位置获取当前车道的左右宽度，然后根据左右借道获取相邻车道的宽度（当然，fallback设定不借道），最后综合各因素，更新PathBound。
bool PathBoundsDecider::GetBoundaryFromLanesAndADC( ... ) {
// Go through every point, update the boundary based on lane info and ADC's position.
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.
reference_line.GetLaneWidth(curr_s, &curr_lane_left_width, &curr_lane_right_width));
// 2. Get the neighbor lane widths at the current point.
//CheckLaneBoundaryType()在向左或向右借道时，返回true
if (CheckLaneBoundaryType(reference_line_info, curr_s, lane_borrow_info)) {
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. 与向左借道类似
}
}
// 3.Calculate the proper boundary based on lane-width, ADC's position, and ADC's velocity.
static constexpr double kMaxLateralAccelerations = 1.5;
//给定初始速度和加速度，刹停行驶的距离（横向）
//对横向来说，就是横向速度变为0的过程行驶的横向位移
double ADC_speed_buffer = (adc_frenet_ld_ > 0 ? 1.0 : -1.0) *
adc_frenet_ld_ * adc_frenet_ld_ / kMaxLateralAccelerations / 2.0;
//如果要向左借道，可行驶道路左边界会变为左侧道路左边界
double curr_left_bound_lane = curr_lane_left_width +
(lane_borrow_info == LaneBorrowInfo::LEFT_BORROW ? curr_neighbor_lane_width : 0.0);
... //向右借道同上。更新curr_left_bound和curr_right_bound
// 4. Update the boundary.
if (!UpdatePathBoundaryAndCenterLine(i, curr_left_bound, curr_right_bound,
path_bound, &dummy)) { ... }
}
}3. pull over场景
pull over场景下，要根据停车点的选择，限定PathBound的横向和纵向范围，在该范围内遍历搜索。
Status PathBoundsDecider::GeneratePullOverPathBound( ... ) {
// 1. Initialize the path boundaries to be an indefinitely large area.
if (!InitPathBoundary(reference_line_info, path_bound)) { ... }
// 2. Decide a rough boundary based on road boundary
if (!GetBoundaryFromRoads(reference_line_info, path_bound)) { ... }
// 3. Fine-tune the boundary based on static obstacles
std::string blocking_obstacle_id;
if (!GetBoundaryFromStaticObstacles(reference_line_info.path_decision(),
path_bound, &blocking_obstacle_id)) { ... }
...
// If already found a pull-over position, simply check if it's valid.
int curr_idx = -1;
if (pull_over_status->has_position()) {
curr_idx = IsPointWithinPathBound(
reference_line_info, pull_over_status->position().x(),
pull_over_status->position().y(), *path_bound);
}
// If haven't found a pull-over position, search for one.
//如果点不在bound限制的可行驶区域内，搜索可以停车的地点
if (curr_idx < 0) {
//<pull_over_x, pull_over_y, pull_over_theta, index_in_bounds>
std::tuple<double, double, double, int> pull_over_configuration;
if (!SearchPullOverPosition(frame, reference_line_info, *path_bound,
&pull_over_configuration)) { ... }
curr_idx = std::get<3>(pull_over_configuration);
// If have found a pull-over position, update planning-context
... //设定停车地点的相关信息
}
// Trim path-bound properly
//停车后面的bounds就没用了，删除一部分，修改一部分
while (static_cast<int>(path_bound->size()) - 1 > curr_idx + kNumExtraTailBoundPoint) {
path_bound->pop_back();
}
//停车后面的bounds全部修改为停车点处的bound
for (size_t idx = curr_idx + 1; idx < path_bound->size(); ++idx) {
std::get<1>((*path_bound)[idx]) = std::get<1>((*path_bound)[curr_idx]);
std::get<2>((*path_bound)[idx]) = std::get<2>((*path_bound)[curr_idx]);
}
return Status::OK();
}SearchPullOverPosition()应该是这个文件中最绕的函数了。它的目的是搜索停车点，为了能够容纳车的尺寸，因为要先搜索可以停车的区域，然后在该区域内取一点作为停车点。搜索区域时，要先确定一个端点，然后向前或向后考察另一个端点，以及考察两端点之间的区域是否符合要求。
bool PathBoundsDecider::SearchPullOverPosition( ... ) {
// search direction
bool search_backward = false; // search FORWARD by default
//01.先根据不同场景搜索一个考察可停车区域的大致端点，然后从该端点出发确定可停车区域
double pull_over_s = 0.0;
if (pull_over_status.pull_over_type() == PullOverStatus::EMERGENCY_PULL_OVER) {
//紧急情况，前方停车
if (!FindEmergencyPullOverS(reference_line_info, &pull_over_s)) { ... }
search_backward = false; // search FORWARD from target position
} else if (pull_over_status.pull_over_type() == PullOverStatus::PULL_OVER) {
if (!FindDestinationPullOverS(frame, reference_line_info, path_bound,
&pull_over_s)) { ... }
//理想的停车点是route的终点，因此要反向搜索，以找到匹配的bounds
search_backward = true; // search BACKWARD from target position
} else {
return false;
}
int idx = 0;
if (search_backward) {
// 1. Locate the first point before destination.
idx = static_cast<int>(path_bound.size()) - 1;
while (idx >= 0 && std::get<0>(path_bound[idx]) > pull_over_s) {
--idx;
}
//反向搜索时，idx表示停车区域的末端
} else {
// 1. Locate the first point after emergency_pull_over s.
while (idx < static_cast<int>(path_bound.size()) &&
std::get<0>(path_bound[idx]) < pull_over_s) {
++idx;
}
//前向搜索时，idx表示停车区域的开端
}
if (idx < 0 || idx >= static_cast<int>(path_bound.size())) { ... }
// Search for a feasible location for pull-over
... //根据一些条件计算pull_over_space_length 和 pull_over_space_width
// 2. Find a window that is close to road-edge.(not in any intersection)
//02.搜索可停车的区域始末。内外2层while循环，外循环控制一个开始搜索的端点idx，因为当
//考察的区域不符合安全性和尺寸条件时，idx也要变化。内循环控制另一个端点j。
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)) {
int j = idx;
bool is_feasible_window = true;
... // Check if the point of idx is within intersection.
//如果遇到路口，调整开始搜索的端点idx，重新搜索
if (!junctions.empty()) {
AWARN << "Point is in PNC-junction.";
idx = search_backward ? idx - 1 : idx + 1;
continue;
}
//搜索一段宽度达标、长度达标的可停车区域。即，搜索合适的端点j
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)) {
double curr_s = std::get<0>(path_bound[j]);
double curr_right_bound = std::fabs(std::get<1>(path_bound[j]));
reference_line_info.reference_line().GetRoadWidth(
curr_s, &curr_road_left_width, &curr_road_right_width);
if (curr_road_right_width - (curr_right_bound + adc_half_width) >
config_.path_bounds_decider_config().pull_over_road_edge_buffer()) {
AERROR << "Not close enough to road-edge. Not feasible for pull-over.";
is_feasible_window = false;
break;
}
if(std::get<2>(path_bound[j])-std::get<1>(path_bound[j]) < pull_over_space_width) {
AERROR << "Not wide enough to fit ADC. Not feasible for pull-over.";
is_feasible_window = false;
break;
}
j = search_backward ? j - 1 : j + 1;
}
if (j < 0) {
return false;
}
//03.找到可停车区域后，获取停车目标点的位姿
if (is_feasible_window) {
// estimate pull over point to have the vehicle keep same safety distance
// to front and back
...
int start_idx = j;
int end_idx = idx;
if (!search_backward) {
start_idx = idx;
end_idx = j;
}
//根据start_idx和end_idx计算pull_over_idx。注意index是相对于bounds的
...
const auto& pull_over_point = path_bound[pull_over_idx];
//根据找到的停车点，设定相关信息，并根据参考线计算停车点的朝向角
...
*pull_over_configuration = std::make_tuple(pull_over_x, pull_over_y,
pull_over_theta, static_cast<int>(pull_over_idx));
break; //一旦找到可停车区域，退出最外层while循环，返回结果
}
idx = search_backward ? idx - 1 : idx + 1;
}
}4. lane change 场景
Status PathBoundsDecider::GenerateLaneChangePathBound( ... ) {
// 1. Initialize the path boundaries to be an indefinitely large area.
if (!InitPathBoundary(reference_line_info, path_bound)) { ... }
// 2. Decide a rough boundary based on lane info and ADC's position
if (!GetBoundaryFromLanesAndADC(reference_line_info,
LaneBorrowInfo::NO_BORROW, 0.1, path_bound,
&dummy_borrow_lane_type)) { ... }
// 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)) { ... }
...
}计算PathBound的步骤大体类似，不同之处在于GetBoundaryFromLaneChangeForbiddenZone()。我认为该函数是检查【当前位置，变道起始点】区间内的PathBound的，却不明白还没有变道，为何要加入隔壁车道的宽度？
void PathBoundsDecider::GetBoundaryFromLaneChangeForbiddenZone( ... ) {
// If there is a pre-determined lane-change starting position, then use it;
// otherwise, decide one.
//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.利用已存在的变道起始点
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 {
//3.无变道起始点，就设置为自车前方一段距离的某点，lane_change_start_s是在当前车道上的距离
lane_change_start_s = FLAGS_lane_change_prepare_length + adc_frenet_s_;
// Update the decided lane_change_start_s into planning-context.
... //设定变道起始点的相关信息
}
if (lane_change_start_s < adc_frenet_s_) {
// If already passed the decided S, then return.
return;
}
//逐个考察lane_change_start_s之前的bound
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;
}
if (reference_line.GetLaneWidth(curr_s, &curr_lane_left_width,
&curr_lane_right_width)) { ... }
//有一点不明白：for循环是在lane_change_start_s范围内执行的，既然还没有到变道起始点，
//怎么会偏移出本车道呢？可是若没有下面这段对bounds的修改，本函数就没有意义了
//path_bound中的每个元素是tuple(s, l_min, l_max)
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);
}
}5. regular场景（lane borrow）
Status PathBoundsDecider::GenerateRegularPathBound( ... ) {
// 1. Initialize the path boundaries to be an indefinitely large area.
if (!InitPathBoundary(reference_line_info, 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)) { ... }
// 3. Fine-tune the boundary based on static obstacles
if (!GetBoundaryFromStaticObstacles(reference_line_info.path_decision(),
path_bound, blocking_obstacle_id)) { ... }
...
}处理步骤非常相似，不同之处在于调用GetBoundaryFromLanesAndADC()时，会将3种LaneBorrowInfo传入，即不借道、向左借道、向右借道。而之前的场景在调用该函数时，均设定不借道。
enum class LaneBorrowInfo {
LEFT_BORROW,
NO_BORROW,
RIGHT_BORROW,
};6. 结尾
不同场景产生的PathBound都会存入ReferenceLineInfo 类的 candidate_path_boundaries_ 成员变量中，并标记了不同的标签。其他模块通过在candidate_path_boundaries_ 中查找，以取用合适的对应的PathBound。
腾讯云面向开发者汇聚海量精品云计算使用和开发经验，营造开放的云计算技术生态圈。
更多推荐
6
0- 0
已为社区贡献1条内容
所有评论(0)