大家好，我已經把CSDN上的部落格遷移到了知乎上，歡迎大家在知乎關注我的專欄慢慢悠悠小馬車（https://zhuanlan.zhihu.com/duangduangduang）。希望大家可以多多交流，互相學習。
目錄
1. 處理流程
2. fallback場景
3. pull over場景
4. lane change 場景
5. regular場景（lane borrow）
6. 結尾
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。
![rFpro推出自動駕駛模拟平台，能以每秒120幀的速度運作高清品質圖形[圖]](data:image/gif;base64,R0lGODlhAQABAIAAAP///wAAACwAAAAAAQABAAACAkQBADs=)