記事ディレクトリ
TASKシリーズの分析記事
1. [Apollo 学習ノート]—— 計画モジュール TASK の LANE_CHANGE_DECIDER
2. [Apollo 学習ノート]—— 計画モジュール TASK の PATH_REUSE_DECIDER
3. [Apollo 学習ノート]—— 計画モジュール TASK の PATH_BORROW_DECIDER
4. [Apollo 学習ノート]— — 計画モジュール TASK
5. [Apollo の学習ノート] の PATH_BOUNDS_DECIDER — 計画モジュール TASK
6. [Apollo の学習ノート] の PATH_ASSESSMENT_DECIDER — 計画モジュール TASK
7. [Apollo の学習ノート] の PATH_DECIDER — 計画モジュール TASK
8. [Apollo の学習ノート] の PATH_DECIDERノート] - ルール_based_stop_decider計画モジュールモジュールタスク9. [Apollo Study Notes] ——— speed_bounds_priori_decider && speed_bounds_final_decider計画モジュールタスク10.
[ Apollo Study Notes]
。12.
[Apollo 学習ノート]——計画モジュール TASK の PIECEWISE_JERK_SPEED_OPTIMIZER
13. [Apollo 学習ノート]—計画モジュール TASK (1) の PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER
14. [Apollo 学習ノート]—計画モジュール TASK (2) の PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER
序文
Apollo Spark プロジェクトの学習ノート - Apollo パス計画アルゴリズムの原理と実践および [ Apollo Study Notes] - 計画モジュールでは、Stage::Process の関数がPlanOnReferenceLine
task_list 内の TASK を順番に呼び出します。例では、TASK 部分が正確に何を行うかを順番に紹介します。個人の能力の限界により、記事にはいくつかの不備があるかもしれません、批判と修正をお願いします。
構成ファイルではmodules/planning/conf/scenario/lane_follow_config.pb.txt
、LaneFollow が実行する必要があるすべてのタスクを確認できます。
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
本稿では引き続き、LaneFollowの8番目のTASKを紹介していきます——RULE_BASED_STOP_DECIDER
ルールベースの停止意思決定は計画モジュールのタスクであり、タスクの決定者カテゴリに属します。ルールベースの停止決定では、いくつかのルールに基づいて停止フラグが設定されます。
RULE_BASED_STOP_DECIDER 関連の構成
modules/planning/conf/planning_config.pb.txt
default_task_config: {
task_type: RULE_BASED_STOP_DECIDER
rule_based_stop_decider_config {
max_adc_stop_speed: 0.5
max_valid_stop_distance: 1.0
search_beam_length: 20.0
search_beam_radius_intensity: 0.08
search_range: 3.14
is_block_angle_threshold: 0.5
}
}
modules/planning/proto/task_config.proto
// RuleBasedStopDeciderConfig
message RuleBasedStopDeciderConfig {
optional double max_adc_stop_speed = 1 [default = 0.3];
optional double max_valid_stop_distance = 2 [default = 0.5];
optional double search_beam_length = 3 [default = 5.0];
optional double search_beam_radius_intensity = 4 [default = 0.08];
optional double search_range = 5 [default = 3.14];
optional double is_block_angle_threshold = 6 [default = 1.57];
optional double approach_distance_for_lane_change = 10 [default = 80.0];
optional double urgent_distance_for_lane_change = 11 [default = 50.0];
}
RULE_BASED_STOP_DECIDER 全体的なプロセス
-
入力
apollo::common::Status RuleBasedStopDecider::Process(Frame *const frame, ReferenceLineInfo *const reference_line_info)
入力はフレームとreference_line_infoです。 -
出力
出力はreference_line_infoに保存されます。
コード フローとフレームワーク
Process のコード フローを次の図に示します。
apollo::common::Status RuleBasedStopDecider::Process(
Frame *const frame, ReferenceLineInfo *const reference_line_info) {
// 1. Rule_based stop for side pass onto reverse lane
StopOnSidePass(frame, reference_line_info);
// 2. Rule_based stop for urgent lane change
if (FLAGS_enable_lane_change_urgency_checking) {
CheckLaneChangeUrgency(frame);
}
// 3. Rule_based stop at path end position
AddPathEndStop(frame, reference_line_info);
return Status::OK();
}
主なコア機能はStopOnSidePass
、CheckLaneChangeUrgency
、 でありAddPathEndStop
、これら 3 つを個別に分析します。
ストップオンサイドパス
void RuleBasedStopDecider::StopOnSidePass(
Frame *const frame, ReferenceLineInfo *const reference_line_info) {
static bool check_clear;// 默认false
static common::PathPoint change_lane_stop_path_point;
// 获取path data
const PathData &path_data = reference_line_info->path_data();
double stop_s_on_pathdata = 0.0;
// 找到"self"类型的路径,return
if (path_data.path_label().find("self") != std::string::npos) {
check_clear = false;
change_lane_stop_path_point.Clear();
return;
}
// CheckClearDone:Check if needed to check clear again for side pass
// 如果check_clear为true,且CheckClearDone成功。设置check_clear为false
if (check_clear &&
CheckClearDone(*reference_line_info, change_lane_stop_path_point)) {
check_clear = false;
}
// CheckSidePassStop:Check if necessary to set stop fence used for nonscenario side pass
// 如果check_clear为false,CheckSidePassStop为true
if (!check_clear &&
CheckSidePassStop(path_data, *reference_line_info, &stop_s_on_pathdata)) {
// 如果障碍物没有阻塞且可以换道,直接return
if (!LaneChangeDecider::IsPerceptionBlocked(
*reference_line_info,
rule_based_stop_decider_config_.search_beam_length(),
rule_based_stop_decider_config_.search_beam_radius_intensity(),
rule_based_stop_decider_config_.search_range(),
rule_based_stop_decider_config_.is_block_angle_threshold()) &&
LaneChangeDecider::IsClearToChangeLane(reference_line_info)) {
return;
}
// 检查adc是否停在了stop fence前,否返回true
if (!CheckADCStop(path_data, *reference_line_info, stop_s_on_pathdata)) {
// 设置stop fence,成功就执行 check_clear = true;
if (!BuildSidePassStopFence(path_data, stop_s_on_pathdata,
&change_lane_stop_path_point, frame,
reference_line_info)) {
AERROR << "Set side pass stop fail";
}
} else {
if (LaneChangeDecider::IsClearToChangeLane(reference_line_info)) {
check_clear = true;
}
}
}
}
チェッククリア完了
// Check if needed to check clear again for side pass
bool RuleBasedStopDecider::CheckClearDone(
const ReferenceLineInfo &reference_line_info,
const common::PathPoint &stop_point) {
// 获取ADC的SL坐标
const double adc_front_edge_s = reference_line_info.AdcSlBoundary().end_s();
const double adc_back_edge_s = reference_line_info.AdcSlBoundary().start_s();
const double adc_start_l = reference_line_info.AdcSlBoundary().start_l();
const double adc_end_l = reference_line_info.AdcSlBoundary().end_l();
double lane_left_width = 0.0;
double lane_right_width = 0.0;
reference_line_info.reference_line().GetLaneWidth(
(adc_front_edge_s + adc_back_edge_s) / 2.0, &lane_left_width,
&lane_right_width);
SLPoint stop_sl_point;
// 获取停止点的SL坐标
reference_line_info.reference_line().XYToSL(stop_point, &stop_sl_point);
// use distance to last stop point to determine if needed to check clear
// again
if (adc_back_edge_s > stop_sl_point.s()) {
if (adc_start_l > -lane_right_width || adc_end_l < lane_left_width) {
return true;
}
}
return false;
}
チェックサイドパスストップ
// @brief Check if necessary to set stop fence used for nonscenario side pass
bool RuleBasedStopDecider::CheckSidePassStop(
const PathData &path_data, const ReferenceLineInfo &reference_line_info,
double *stop_s_on_pathdata) {
const std::vector<std::tuple<double, PathData::PathPointType, double>>
&path_point_decision_guide = path_data.path_point_decision_guide();
// 初始化类型
PathData::PathPointType last_path_point_type =
PathData::PathPointType::UNKNOWN;
// 遍历 path_point_decision_guide
for (const auto &point_guide : path_point_decision_guide) {
// 若上一点在车道内,这一点在逆行车道上
if (last_path_point_type == PathData::PathPointType::IN_LANE &&
std::get<1>(point_guide) ==
PathData::PathPointType::OUT_ON_REVERSE_LANE) {
*stop_s_on_pathdata = std::get<0>(point_guide);
// Approximate the stop fence s based on the vehicle position
const auto &vehicle_config =
common::VehicleConfigHelper::Instance()->GetConfig();
const double ego_front_to_center =
vehicle_config.vehicle_param().front_edge_to_center();
common::PathPoint stop_pathpoint;
// 获取stop point
if (!path_data.GetPathPointWithRefS(*stop_s_on_pathdata,
&stop_pathpoint)) {
AERROR << "Can't get stop point on path data";
return false;
}
const double ego_theta = stop_pathpoint.theta();
Vec2d shift_vec{
ego_front_to_center * std::cos(ego_theta),
ego_front_to_center * std::sin(ego_theta)};
// stop_fence的位置
const Vec2d stop_fence_pose =
shift_vec + Vec2d(stop_pathpoint.x(), stop_pathpoint.y());
double stop_l_on_pathdata = 0.0;
const auto &nearby_path = reference_line_info.reference_line().map_path();
nearby_path.GetNearestPoint(stop_fence_pose, stop_s_on_pathdata,
&stop_l_on_pathdata);
return true;
}
last_path_point_type = std::get<1>(point_guide);
}
return false;
}
知覚がブロックされています
パラメータの説明:
search_beam_length
スキャン長
search_beam_radius_intensity
スキャン間隔
search_range
ADC 中心に基づくスキャン範囲
is_block_angle_threshold
障害物の角度サイズをフィルタリングするためのしきい値
bool LaneChangeDecider::IsPerceptionBlocked(
const ReferenceLineInfo& reference_line_info,
const double search_beam_length, const double search_beam_radius_intensity,
const double search_range, const double is_block_angle_threshold) {
// search_beam_length: 20.0 //is the length of scanning beam
// search_beam_radius_intensity: 0.08 //is the resolution of scanning
// search_range: 3.14 //is the scanning range centering at ADV heading
// is_block_angle_threshold: 0.5 //is the threshold to tell how big a block angle range is perception blocking
// 获取车辆状态、位置、航向角
const auto& vehicle_state = reference_line_info.vehicle_state();
const common::math::Vec2d adv_pos(vehicle_state.x(), vehicle_state.y());
const double adv_heading = vehicle_state.heading();
// 遍历障碍物
for (auto* obstacle :
reference_line_info.path_decision().obstacles().Items()) {
// NormalizeAngle将给定的角度值规范化到一个特定的范围内(-π到π之间)
double left_most_angle =
common::math::NormalizeAngle(adv_heading + 0.5 * search_range);
double right_most_angle =
common::math::NormalizeAngle(adv_heading - 0.5 * search_range);
bool right_most_found = false;
// 跳过虚拟障碍物
if (obstacle->IsVirtual()) {
ADEBUG << "skip one virtual obstacle";
continue;
}
// 获取障碍物多边形
const auto& obstacle_polygon = obstacle->PerceptionPolygon();
// 按角度进行搜索
for (double search_angle = 0.0; search_angle < search_range;
search_angle += search_beam_radius_intensity) {
common::math::Vec2d search_beam_end(search_beam_length, 0.0);
const double beam_heading = common::math::NormalizeAngle(
adv_heading - 0.5 * search_range + search_angle);
// search_beam_end绕adv_pos旋转beam_heading角度
search_beam_end.SelfRotate(beam_heading);
search_beam_end += adv_pos;
// 构造线段
common::math::LineSegment2d search_beam(adv_pos, search_beam_end);
// 判断最右边界是否找到,并更新右边界角度
if (!right_most_found && obstacle_polygon.HasOverlap(search_beam)) {
right_most_found = true;
right_most_angle = beam_heading;
}
// 如果最右边界已找到,且障碍物的感知多边形与搜索光束无重叠,则更新左边界角度并跳出循环。
if (right_most_found && !obstacle_polygon.HasOverlap(search_beam)) {
left_most_angle = beam_heading;
break;
}
}
// 如果最右边界未找到,则继续处理下一个障碍物。(说明该障碍物不在搜索范围内)
if (!right_most_found) {
// obstacle is not in search range
continue;
}
// 判断阈值,过滤掉小的障碍物
if (std::fabs(common::math::NormalizeAngle(
left_most_angle - right_most_angle)) > is_block_angle_threshold) {
return true;
}
}
return false;
}
レーンを変更するにはクリアです
これは、計画モジュール TASK の [Apollo Study Notes] - LANE_CHANGE_DECIDERで導入されています。
チェックサイドパスストップ
// @brief Check if necessary to set stop fence used for nonscenario side pass
bool RuleBasedStopDecider::CheckSidePassStop(
const PathData &path_data, const ReferenceLineInfo &reference_line_info,
double *stop_s_on_pathdata) {
const std::vector<std::tuple<double, PathData::PathPointType, double>>
&path_point_decision_guide = path_data.path_point_decision_guide();
// 初始化类型
PathData::PathPointType last_path_point_type =
PathData::PathPointType::UNKNOWN;
// 遍历 path_point_decision_guide
for (const auto &point_guide : path_point_decision_guide) {
// 若上一点在车道内,这一点在逆行车道上
if (last_path_point_type == PathData::PathPointType::IN_LANE &&
std::get<1>(point_guide) ==
PathData::PathPointType::OUT_ON_REVERSE_LANE) {
*stop_s_on_pathdata = std::get<0>(point_guide);
// Approximate the stop fence s based on the vehicle position
const auto &vehicle_config =
common::VehicleConfigHelper::Instance()->GetConfig();
const double ego_front_to_center =
vehicle_config.vehicle_param().front_edge_to_center();
common::PathPoint stop_pathpoint;
// 获取stop point
if (!path_data.GetPathPointWithRefS(*stop_s_on_pathdata,
&stop_pathpoint)) {
AERROR << "Can't get stop point on path data";
return false;
}
const double ego_theta = stop_pathpoint.theta();
Vec2d shift_vec{
ego_front_to_center * std::cos(ego_theta),
ego_front_to_center * std::sin(ego_theta)};
// stop_fence的位置
const Vec2d stop_fence_pose =
shift_vec + Vec2d(stop_pathpoint.x(), stop_pathpoint.y());
double stop_l_on_pathdata = 0.0;
const auto &nearby_path = reference_line_info.reference_line().map_path();
nearby_path.GetNearestPoint(stop_fence_pose, stop_s_on_pathdata,
&stop_l_on_pathdata);
return true;
}
last_path_point_type = std::get<1>(point_guide);
}
return false;
}
ビルド停止の決定
/*
* @brief: build virtual obstacle of stop wall, and add STOP decision
*/
int BuildStopDecision(const std::string& stop_wall_id, const double stop_line_s,
const double stop_distance,
const StopReasonCode& stop_reason_code,
const std::vector<std::string>& wait_for_obstacles,
const std::string& decision_tag, Frame* const frame,
ReferenceLineInfo* const reference_line_info) {
CHECK_NOTNULL(frame);
CHECK_NOTNULL(reference_line_info);
// 检查停止线是否在参考线上
const auto& reference_line = reference_line_info->reference_line();
if (!WithinBound(0.0, reference_line.Length(), stop_line_s)) {
AERROR << "stop_line_s[" << stop_line_s << "] is not on reference line";
return 0;
}
// create virtual stop wall
const auto* obstacle =
frame->CreateStopObstacle(reference_line_info, stop_wall_id, stop_line_s);
if (!obstacle) {
AERROR << "Failed to create obstacle [" << stop_wall_id << "]";
return -1;
}
const Obstacle* stop_wall = reference_line_info->AddObstacle(obstacle);
if (!stop_wall) {
AERROR << "Failed to add obstacle[" << stop_wall_id << "]";
return -1;
}
// build stop decision
const double stop_s = stop_line_s - stop_distance;
const auto& stop_point = reference_line.GetReferencePoint(stop_s);
const double stop_heading =
reference_line.GetReferencePoint(stop_s).heading();
ObjectDecisionType stop;
auto* stop_decision = stop.mutable_stop();
stop_decision->set_reason_code(stop_reason_code);
stop_decision->set_distance_s(-stop_distance);
stop_decision->set_stop_heading(stop_heading);
stop_decision->mutable_stop_point()->set_x(stop_point.x());
stop_decision->mutable_stop_point()->set_y(stop_point.y());
stop_decision->mutable_stop_point()->set_z(0.0);
for (size_t i = 0; i < wait_for_obstacles.size(); ++i) {
stop_decision->add_wait_for_obstacle(wait_for_obstacles[i]);
}
auto* path_decision = reference_line_info->path_decision();
path_decision->AddLongitudinalDecision(decision_tag, stop_wall->Id(), stop);
return 0;
}
ELSE: 他の機能が関係する
正規化角度
NormalizeAngle
指定された角度値を特定の範囲 (-π と π の間) に正規化します。
double NormalizeAngle(const double angle) {
double a = std::fmod(angle + M_PI, 2.0 * M_PI);
if (a < 0.0) {
a += (2.0 * M_PI);
}
return a - M_PI;
}
自己回転
ベクトルを原点を中心に角度角度だけ回転します角度。 _ _ _
void Vec2d::SelfRotate(const double angle) {
double tmp_x = x_;
x_ = x_ * cos(angle) - y_ * sin(angle);
y_ = tmp_x * sin(angle) + y_ * cos(angle);
}
CheckLaneChangeUrgency
緊急車線変更をチェックし、FLAGS_enable_lane_change_urgency_checking
true の場合、機能を有効にします。
void RuleBasedStopDecider::CheckLaneChangeUrgency(Frame *const frame) {
// 直接进入循环,检查每个reference_line_info
for (auto &reference_line_info : *frame->mutable_reference_line_info()) {
// Check if the target lane is blocked or not
// 1. 检查目标道路是否阻塞,如果在change lane path上,就跳过
if (reference_line_info.IsChangeLanePath()) {
is_clear_to_change_lane_ =
LaneChangeDecider::IsClearToChangeLane(&reference_line_info);
is_change_lane_planning_succeed_ =
reference_line_info.Cost() < kStraightForwardLineCost;
continue;
}
// If it's not in lane-change scenario || (target lane is not blocked &&
// change lane planning succeed), skip
// 2.如果不是换道的场景,或者(目标lane没有阻塞)并且换道规划成功,跳过
if (frame->reference_line_info().size() <= 1 ||
(is_clear_to_change_lane_ && is_change_lane_planning_succeed_)) {
continue;
}
// When the target lane is blocked in change-lane case, check the urgency
// Get the end point of current routing
const auto &route_end_waypoint =
reference_line_info.Lanes().RouteEndWaypoint();
// If can't get lane from the route's end waypoint, then skip
// 3.在route的末端无法获得lane,跳过
if (!route_end_waypoint.lane) {
continue;
}
auto point = route_end_waypoint.lane->GetSmoothPoint(route_end_waypoint.s);
auto *reference_line = reference_line_info.mutable_reference_line();
common::SLPoint sl_point;
// Project the end point to sl_point on current reference lane
// 将当前参考线的点映射到frenet坐标系下
if (reference_line->XYToSL(point, &sl_point) &&
reference_line->IsOnLane(sl_point)) {
// Check the distance from ADC to the end point of current routing
double distance_to_passage_end =
sl_point.s() - reference_line_info.AdcSlBoundary().end_s();
// If ADC is still far from the end of routing, no need to stop, skip
// 4. 如果adc距离routing终点较远,不需要停止,跳过
if (distance_to_passage_end >
rule_based_stop_decider_config_.approach_distance_for_lane_change()) {
continue;
}
// In urgent case, set a temporary stop fence and wait to change lane
// TODO(Jiaxuan Xu): replace the stop fence to more intelligent actions
// 5.如果遇到紧急情况,设置临时的stop fence,等待换道
const std::string stop_wall_id = "lane_change_stop";
std::vector<std::string> wait_for_obstacles;
util::BuildStopDecision(
stop_wall_id, sl_point.s(),
rule_based_stop_decider_config_.urgent_distance_for_lane_change(),
StopReasonCode::STOP_REASON_LANE_CHANGE_URGENCY, wait_for_obstacles,
"RuleBasedStopDecider", frame, &reference_line_info);
}
}
}
パスの追加終了終了
void RuleBasedStopDecider::AddPathEndStop(
Frame *const frame, ReferenceLineInfo *const reference_line_info) {
// 路径不为空且起点到终点的距离不小于20m
if (!reference_line_info->path_data().path_label().empty() &&
reference_line_info->path_data().frenet_frame_path().back().s() -
reference_line_info->path_data().frenet_frame_path().front().s() <
FLAGS_short_path_length_threshold) {
// FLAGS_short_path_length_threshold: Threshold for too short path length(20m)
// 创建虚拟墙的ID
const std::string stop_wall_id =
PATH_END_VO_ID_PREFIX + reference_line_info->path_data().path_label();
std::vector<std::string> wait_for_obstacles;
// 创建stop fence
util::BuildStopDecision(
stop_wall_id,
reference_line_info->path_data().frenet_frame_path().back().s() - 5.0,
0.0, StopReasonCode::STOP_REASON_REFERENCE_END, wait_for_obstacles,
"RuleBasedStopDecider", frame, reference_line_info);
}
}
参考
[1]ルールによる停止判定