【Apollo学习笔记】——规划模块TASK之SPEED_BOUNDS_PRIORI_DECIDER

前言

在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

本文将继续介绍LaneFollow的第9个TASK——SPEED_BOUNDS_PRIORI_DECIDER

SPEED_BOUNDS_PRIORI_DECIDER功能简介

产生速度可行驶边界

所形成的区域是非凸的，不能用之前凸优化的方法去做，需要用动态规划的方法去做。

SPEED_BOUNDS_PRIORI_DECIDER相关配置

modules/planning/proto/task_config.proto

// SpeedBoundsDeciderConfigmessage SpeedBoundsDeciderConfig {optional double total_time = 1 [default = 7.0];optional double boundary_buffer = 2 [default = 0.1];optional double max_centric_acceleration_limit = 3 [default = 2.0];optional double minimal_kappa = 4 [default = 0.00001];optional double point_extension = 5 [default = 1.0];optional double lowest_speed = 6 [default = 2.5];optional double collision_safety_range = 7 [default = 1.0];optional double static_obs_nudge_speed_ratio = 8;optional double dynamic_obs_nudge_speed_ratio = 9;
}

modules/planning/conf/planning_config.pb.txt

default_task_config: {task_type: ST_BOUNDS_DECIDERst_bounds_decider_config {total_time: 7.0}
}

SPEED_BOUNDS_PRIORI_DECIDER流程

通过modules/planning/tasks/task_factory.cc，我们可以看到SPEED_BOUNDS_PRIORI_DECIDER按以下方式进行注册：

  task_factory_.Register(TaskConfig::SPEED_BOUNDS_PRIORI_DECIDER,[](const TaskConfig& config,const std::shared_ptr<DependencyInjector>& injector) -> Task* {return new SpeedBoundsDecider(config, injector);});

也据此可知，SPEED_BOUNDS_PRIORI_DECIDER代码实现的部分在modules/planning/tasks/deciders/speed_bounds_decider/speed_bounds_decider.cc中。

Speed bounds decider 主要完成以下任务：

1. 将障碍物映射到ST图中
2. 创建速度限制
3. 获取路径长度以及时间长度作为ST边界

SpeedBoundsDecider是一个继承自Decider的派生类。当task_list中运行task::Execute()时，SpeedBoundsDecider中的Process部分开始运行。

• 输入：frame 和reference_line_info。通过计算PathData/ReferenceLine/PathDecision/PlanningStartPoint等等信息，来得到ST_Graph。

• Process：

  • 将障碍物映射到ST图中。(boundary_mapper.ComputeSTBoundary(path_decision).code() == ErrorCode::PLANNING_ERROR) {}此处将会遍历所有的障碍物去生成ST graph。当有纵向决策产生时，会对边界进行细微调整。再此之后，所有的障碍物的st_boundary会送入一个boundaries vector之中进行保存。
  • 创建速度限制。if (!speed_limit_decider.GetSpeedLimits(path_decision->obstacles(), &speed_limit).ok())此处会遍历每一个离散的路径点并且找到其速度限制。在每一个循环中，基本速度会取决于map/path_curvature/nudge obstacles等因素。对于nudge obstacles，需要找到最近的障碍物。
  • 获取路径长度以及时间长度作为搜索边界。时间长度来自于配置文件total_time: 7.0（配置部分已有介绍）

• 输出：boundaries/speed_limit 会存储在reference_line_info的st_graph_data中。

Status SpeedBoundsDecider::Process(Frame *const frame, ReferenceLineInfo *const reference_line_info) {// retrieve data from frame and reference_line_infoconst PathData &path_data = reference_line_info->path_data();const TrajectoryPoint &init_point = frame->PlanningStartPoint();const ReferenceLine &reference_line = reference_line_info->reference_line();PathDecision *const path_decision = reference_line_info->path_decision();// 1. Map obstacles into st graphauto time1 = std::chrono::system_clock::now();// 构造一个STBoundary映射对象STBoundaryMapper boundary_mapper(speed_bounds_config_, reference_line, path_data,path_data.discretized_path().Length(), speed_bounds_config_.total_time(),injector_);// FLAGS_use_st_drivable_boundary: True to use st_drivable boundary in speed planning// default: false// 清除STBoundaryif (!FLAGS_use_st_drivable_boundary) {path_decision->EraseStBoundaries();}// 将障碍物投影到ST Gragh上if (boundary_mapper.ComputeSTBoundary(path_decision).code() ==ErrorCode::PLANNING_ERROR) {const std::string msg = "Mapping obstacle failed.";AERROR << msg;return Status(ErrorCode::PLANNING_ERROR, msg);}auto time2 = std::chrono::system_clock::now();std::chrono::duration<double> diff = time2 - time1;ADEBUG << "Time for ST Boundary Mapping = " << diff.count() * 1000<< " msec.";// 所有的障碍物的st_boundary送入到一个boundaries vector之中进行保存std::vector<const STBoundary *> boundaries;for (auto *obstacle : path_decision->obstacles().Items()) {const auto &id = obstacle->Id();const auto &st_boundary = obstacle->path_st_boundary();if (!st_boundary.IsEmpty()) {if (st_boundary.boundary_type() == STBoundary::BoundaryType::KEEP_CLEAR) {path_decision->Find(id)->SetBlockingObstacle(false);} else {path_decision->Find(id)->SetBlockingObstacle(true);}boundaries.push_back(&st_boundary);}}const double min_s_on_st_boundaries = SetSpeedFallbackDistance(path_decision);// 2. Create speed limit along pathSpeedLimitDecider speed_limit_decider(speed_bounds_config_, reference_line,path_data);SpeedLimit speed_limit;if (!speed_limit_decider.GetSpeedLimits(path_decision->obstacles(), &speed_limit).ok()) {const std::string msg = "Getting speed limits failed!";AERROR << msg;return Status(ErrorCode::PLANNING_ERROR, msg);}// 3. Get path_length as s axis search bound in st graphconst double path_data_length = path_data.discretized_path().Length();// 4. Get time duration as t axis search bound in st graphconst double total_time_by_conf = speed_bounds_config_.total_time();// Load generated st graph data back to frameStGraphData *st_graph_data = reference_line_info_->mutable_st_graph_data();// Add a st_graph debug info and save the pointer to st_graph_data for// optimizer loggingauto *debug = reference_line_info_->mutable_debug();STGraphDebug *st_graph_debug = debug->mutable_planning_data()->add_st_graph();st_graph_data->LoadData(boundaries, min_s_on_st_boundaries, init_point,speed_limit, reference_line_info->GetCruiseSpeed(),path_data_length, total_time_by_conf, st_graph_debug);// Create and record st_graph debug infoRecordSTGraphDebug(*st_graph_data, st_graph_debug);return Status::OK();
}

将障碍物映射到ST图中

由Process部分代码可知，(boundary_mapper.ComputeSTBoundary(path_decision).code() == ErrorCode::PLANNING_ERROR) {}此处是函数的入口。

该部分的流程示意图如下图所示：

ComputeSTBoundary(PathDecision* path_decision)

ComputeSTBoundary将会遍历所有的障碍物去生成ST graph。当有纵向决策产生时，会对边界进行细微调整。再此之后，所有的障碍物的st_boundary会送入一个boundaries vector之中进行保存。

Status STBoundaryMapper::ComputeSTBoundary(PathDecision* path_decision) const {// Sanity checks.CHECK_GT(planning_max_time_, 0.0);if (path_data_.discretized_path().size() < 2) {AERROR << "Fail to get params because of too few path points. path points ""size: "<< path_data_.discretized_path().size() << ".";return Status(ErrorCode::PLANNING_ERROR,"Fail to get params because of too few path points");}// Go through every obstacle.Obstacle* stop_obstacle = nullptr;ObjectDecisionType stop_decision;double min_stop_s = std::numeric_limits<double>::max();for (const auto* ptr_obstacle_item : path_decision->obstacles().Items()) {Obstacle* ptr_obstacle = path_decision->Find(ptr_obstacle_item->Id());ACHECK(ptr_obstacle != nullptr);// If no longitudinal decision has been made, then plot it onto ST-graph.if (!ptr_obstacle->HasLongitudinalDecision()) {ComputeSTBoundary(ptr_obstacle);continue;}// If there is a longitudinal decision, then fine-tune boundary.const auto& decision = ptr_obstacle->LongitudinalDecision();if (decision.has_stop()) {// 1. Store the closest stop fence info.// TODO(all): store ref. s value in stop decision; refine the code then.common::SLPoint stop_sl_point;reference_line_.XYToSL(decision.stop().stop_point(), &stop_sl_point);const double stop_s = stop_sl_point.s();if (stop_s < min_stop_s) {stop_obstacle = ptr_obstacle;min_stop_s = stop_s;stop_decision = decision;}} else if (decision.has_follow() || decision.has_overtake() ||decision.has_yield()) {// 2. Depending on the longitudinal overtake/yield decision,//    fine-tune the upper/lower st-boundary of related obstacles.ComputeSTBoundaryWithDecision(ptr_obstacle, decision);} else if (!decision.has_ignore()) {// 3. Ignore those unrelated obstacles.AWARN << "No mapping for decision: " << decision.DebugString();}}if (stop_obstacle) {bool success = MapStopDecision(stop_obstacle, stop_decision);if (!success) {const std::string msg = "Fail to MapStopDecision.";AERROR << msg;return Status(ErrorCode::PLANNING_ERROR, msg);}}return Status::OK();
}

ComputeSTBoundary(Obstacle* obstacle)

调用GetOverlapBoundaryPoints来获取给定障碍物的上下点，然后在此基础上制定STBoundary。它还根据以前记录的决策标记边界类型。

void STBoundaryMapper::ComputeSTBoundary(Obstacle* obstacle) const {if (FLAGS_use_st_drivable_boundary) {return;}std::vector<STPoint> lower_points;std::vector<STPoint> upper_points;// Map the given obstacle onto the ST-Graph.if (!GetOverlapBoundaryPoints(path_data_.discretized_path(), *obstacle,&upper_points, &lower_points)) {return;}// 构建障碍物的boundaryauto boundary = STBoundary::CreateInstance(lower_points, upper_points);boundary.set_id(obstacle->Id());// TODO(all): potential bug here.const auto& prev_st_boundary = obstacle->path_st_boundary();const auto& ref_line_st_boundary = obstacle->reference_line_st_boundary();if (!prev_st_boundary.IsEmpty()) {boundary.SetBoundaryType(prev_st_boundary.boundary_type());} else if (!ref_line_st_boundary.IsEmpty()) {boundary.SetBoundaryType(ref_line_st_boundary.boundary_type());}obstacle->set_path_st_boundary(boundary);
}

GetOverlapBoundaryPoints

将给定的障碍物映射到ST图中。

// Map the given obstacle onto the ST-Graph. 
// The boundary is represented as upper and lower points for every s of interests.
// Note that upper_points.size() = lower_points.size()
bool STBoundaryMapper::GetOverlapBoundaryPoints(const std::vector<PathPoint>& path_points, const Obstacle& obstacle,std::vector<STPoint>* upper_points,std::vector<STPoint>* lower_points) const {// Sanity checks.DCHECK(upper_points->empty());DCHECK(lower_points->empty());if (path_points.empty()) {AERROR << "No points in path_data_.discretized_path().";return false;}const auto* planning_status = injector_->planning_context()->mutable_planning_status()->mutable_change_lane();// lane_change_obstacle_nudge_l_buffer: minimum l-distance to nudge when changing lane (meters);0.3// nonstatic_obstacle_nudge_l_buffer: minimum l-distance to nudge a non-static obstacle (meters);0.4double l_buffer =planning_status->status() == ChangeLaneStatus::IN_CHANGE_LANE? FLAGS_lane_change_obstacle_nudge_l_buffer: FLAGS_nonstatic_obstacle_nudge_l_buffer;// Draw the given obstacle on the ST-graph.const auto& trajectory = obstacle.Trajectory();if (trajectory.trajectory_point().empty()) {// For those with no predicted trajectories, just map the obstacle's// current position to ST-graph and always assume it's static.if (!obstacle.IsStatic()) {AWARN << "Non-static obstacle[" << obstacle.Id()<< "] has NO prediction trajectory."<< obstacle.Perception().ShortDebugString();}// 遍历离散路径点for (const auto& curr_point_on_path : path_points) {// planning_max_distance_ = path_data.discretized_path().Length()// 若当前点超过了规划最大距离，退出if (curr_point_on_path.s() > planning_max_distance_) {break;}// 获取障碍物的boundingboxconst Box2d& obs_box = obstacle.PerceptionBoundingBox();if (CheckOverlap(curr_point_on_path, obs_box, l_buffer)) {// If there is overlapping, then plot it on ST-graph.const double backward_distance = -vehicle_param_.front_edge_to_center();const double forward_distance = obs_box.length();double low_s =std::fmax(0.0, curr_point_on_path.s() + backward_distance);double high_s = std::fmin(planning_max_distance_,curr_point_on_path.s() + forward_distance);// It is an unrotated rectangle appearing on the ST-graph.// 静止的障碍物在ST图中就是一个矩形// TODO(jiacheng): reconsider the backward_distance, it might be// unnecessary, but forward_distance is indeed meaningful though.lower_points->emplace_back(low_s, 0.0);lower_points->emplace_back(low_s, planning_max_time_);upper_points->emplace_back(high_s, 0.0);upper_points->emplace_back(high_s, planning_max_time_);break;}}} else {// For those with predicted trajectories (moving obstacles):// 1. Subsample to reduce computation time.const int default_num_point = 50;DiscretizedPath discretized_path;if (path_points.size() > 2 * default_num_point) {const auto ratio = path_points.size() / default_num_point;std::vector<PathPoint> sampled_path_points;for (size_t i = 0; i < path_points.size(); ++i) {if (i % ratio == 0) {sampled_path_points.push_back(path_points[i]);}}discretized_path = DiscretizedPath(std::move(sampled_path_points));} else {discretized_path = DiscretizedPath(path_points);}// 2. Go through every point of the predicted obstacle trajectory.for (int i = 0; i < trajectory.trajectory_point_size(); ++i) {const auto& trajectory_point = trajectory.trajectory_point(i);// 得到障碍物在轨迹点处的boundingboxconst Box2d obs_box = obstacle.GetBoundingBox(trajectory_point);// 得到障碍物在轨迹点处的相对时间double trajectory_point_time = trajectory_point.relative_time();static constexpr double kNegtiveTimeThreshold = -1.0;// 跳过小于阈值时间的轨迹点if (trajectory_point_time < kNegtiveTimeThreshold) {continue;}// 步长const double step_length = vehicle_param_.front_edge_to_center();// FLAGS_max_trajectory_len: (unit: meter) max possible trajectory length. 1000.0auto path_len =std::min(FLAGS_max_trajectory_len, discretized_path.Length());// Go through every point of the ADC's path.for (double path_s = 0.0; path_s < path_len; path_s += step_length) {// 估计当前车辆的位置const auto curr_adc_path_point =discretized_path.Evaluate(path_s + discretized_path.front().s());if (CheckOverlap(curr_adc_path_point, obs_box, l_buffer)) {// Found overlap, start searching with higher resolutionconst double backward_distance = -step_length;const double forward_distance = vehicle_param_.length() +vehicle_param_.width() +obs_box.length() + obs_box.width();const double default_min_step = 0.1;  // in metersconst double fine_tuning_step_length = std::fmin(default_min_step, discretized_path.Length() / default_num_point);bool find_low = false;bool find_high = false;double low_s = std::fmax(0.0, path_s + backward_distance);double high_s =std::fmin(discretized_path.Length(), path_s + forward_distance);// Keep shrinking by the resolution bidirectionally until finally// locating the tight upper and lower bounds.while (low_s < high_s) {if (find_low && find_high) {break;}if (!find_low) {const auto& point_low = discretized_path.Evaluate(low_s + discretized_path.front().s());if (!CheckOverlap(point_low, obs_box, l_buffer)) {low_s += fine_tuning_step_length;} else {find_low = true;}}if (!find_high) {const auto& point_high = discretized_path.Evaluate(high_s + discretized_path.front().s());if (!CheckOverlap(point_high, obs_box, l_buffer)) {high_s -= fine_tuning_step_length;} else {find_high = true;}}}if (find_high && find_low) {lower_points->emplace_back(low_s - speed_bounds_config_.point_extension(),trajectory_point_time);upper_points->emplace_back(high_s + speed_bounds_config_.point_extension(),trajectory_point_time);}break;}}}}// Sanity checks and return.DCHECK_EQ(lower_points->size(), upper_points->size());return (lower_points->size() > 1 && upper_points->size() > 1);
}

ComputeSTBoundaryWithDecision

对于产生纵向决策的障碍物产生的ST boundary进行调整。

// Fine-tune the boundary for yielding or overtaking obstacles. 
// Increase boundary on the s-dimension or set the boundary type, etc., when necessary.
void STBoundaryMapper::ComputeSTBoundaryWithDecision(Obstacle* obstacle, const ObjectDecisionType& decision) const {DCHECK(decision.has_follow() || decision.has_yield() ||decision.has_overtake())<< "decision is " << decision.DebugString()<< ", but it must be follow or yield or overtake.";std::vector<STPoint> lower_points;std::vector<STPoint> upper_points;if (FLAGS_use_st_drivable_boundary &&obstacle->is_path_st_boundary_initialized()) {const auto& path_st_boundary = obstacle->path_st_boundary();lower_points = path_st_boundary.lower_points();upper_points = path_st_boundary.upper_points();} else {if (!GetOverlapBoundaryPoints(path_data_.discretized_path(), *obstacle,&upper_points, &lower_points)) {return;}}auto boundary = STBoundary::CreateInstance(lower_points, upper_points);// get characteristic_length and boundary_type.STBoundary::BoundaryType b_type = STBoundary::BoundaryType::UNKNOWN;double characteristic_length = 0.0;if (decision.has_follow()) {characteristic_length = std::fabs(decision.follow().distance_s());b_type = STBoundary::BoundaryType::FOLLOW;} else if (decision.has_yield()) {characteristic_length = std::fabs(decision.yield().distance_s());boundary = STBoundary::CreateInstance(lower_points, upper_points).ExpandByS(characteristic_length);b_type = STBoundary::BoundaryType::YIELD;} else if (decision.has_overtake()) {characteristic_length = std::fabs(decision.overtake().distance_s());b_type = STBoundary::BoundaryType::OVERTAKE;} else {DCHECK(false) << "Obj decision should be either yield or overtake: "<< decision.DebugString();}boundary.SetBoundaryType(b_type);boundary.set_id(obstacle->Id());boundary.SetCharacteristicLength(characteristic_length);obstacle->set_path_st_boundary(boundary);
}

SetSpeedFallbackDistance

找到障碍物路径上最低的 s 值，该 s 值将用作速度回退的距离。

double SpeedBoundsDecider::SetSpeedFallbackDistance(PathDecision *const path_decision) {// Set min_s_on_st_boundaries to guide speed fallback.static constexpr double kEpsilon = 1.0e-6;double min_s_non_reverse = std::numeric_limits<double>::infinity();double min_s_reverse = std::numeric_limits<double>::infinity();// 遍历障碍物for (auto *obstacle : path_decision->obstacles().Items()) {const auto &st_boundary = obstacle->path_st_boundary();// 障碍物ST边界为空，跳过if (st_boundary.IsEmpty()) {continue;}// 获取st边界底部左侧点和右侧点的s值，并选择较小的值作为最低的s值const auto left_bottom_point_s = st_boundary.bottom_left_point().s();const auto right_bottom_point_s = st_boundary.bottom_right_point().s();const auto lowest_s = std::min(left_bottom_point_s, right_bottom_point_s);// 如果左侧点的 s 值减去右侧点的 s 值大于 kEpsilon（即左侧点在右侧点之后），则说明这是一个反向行驶的边界if (left_bottom_point_s - right_bottom_point_s > kEpsilon) {// 更新 min_s_reverse，将其设置为最低的 s 值if (min_s_reverse > lowest_s) {min_s_reverse = lowest_s;}} else if (min_s_non_reverse > lowest_s) {// 更新 min_s_non_reverse，将其设置为最低的 s 值。min_s_non_reverse = lowest_s;}}min_s_reverse = std::max(min_s_reverse, 0.0);min_s_non_reverse = std::max(min_s_non_reverse, 0.0);return min_s_non_reverse > min_s_reverse ? 0.0 : min_s_non_reverse;
}

创建速度限制

SpeedLimits来源于三个部分：

1. 来自于地图的速度限制
2. 来自于道路曲率的速度限制
3. 来自于Nudge障碍物的速度限制

该部分流程图如下所示：

GetSpeedLimits

Status SpeedLimitDecider::GetSpeedLimits(const IndexedList<std::string, Obstacle>& obstacles,SpeedLimit* const speed_limit_data) const {CHECK_NOTNULL(speed_limit_data);const auto& discretized_path = path_data_.discretized_path();const auto& frenet_path = path_data_.frenet_frame_path();// 遍历离散路径点for (uint32_t i = 0; i < discretized_path.size(); ++i) {const double path_s = discretized_path.at(i).s();const double reference_line_s = frenet_path.at(i).s();if (reference_line_s > reference_line_.Length()) {AWARN << "path w.r.t. reference line at [" << reference_line_s<< "] is LARGER than reference_line_ length ["<< reference_line_.Length() << "]. Please debug before proceeding.";break;}// (1) speed limit from mapdouble speed_limit_from_reference_line =reference_line_.GetSpeedLimitFromS(reference_line_s);// (2) speed limit from path curvature//  -- 2.1: limit by centripetal force (acceleration)const double speed_limit_from_centripetal_acc =// max_centric_acceleration_limit default = 2.0std::sqrt(speed_bounds_config_.max_centric_acceleration_limit() /std::fmax(std::fabs(discretized_path.at(i).kappa()),speed_bounds_config_.minimal_kappa()));// (3) speed limit from nudge obstacles// TODO(all): in future, expand the speed limit not only to obstacles with// nudge decisions.double speed_limit_from_nearby_obstacles =std::numeric_limits<double>::max();const double collision_safety_range =speed_bounds_config_.collision_safety_range();// default = 1.0// 遍历障碍物for (const auto* ptr_obstacle : obstacles.Items()) {// 跳过虚拟的障碍物if (ptr_obstacle->IsVirtual()) {continue;}// 障碍物没有横向Nudge决策，跳过if (!ptr_obstacle->LateralDecision().has_nudge()) {continue;}/* ref line:* -------------------------------*    start_s   end_s* ------|  adc   |---------------* ------------|  obstacle |------*/// TODO(all): potential problem here;// frenet and cartesian coordinates are mixed.const double vehicle_front_s =reference_line_s + vehicle_param_.front_edge_to_center();const double vehicle_back_s =reference_line_s - vehicle_param_.back_edge_to_center();const double obstacle_front_s =ptr_obstacle->PerceptionSLBoundary().end_s();const double obstacle_back_s =ptr_obstacle->PerceptionSLBoundary().start_s();// 若车辆与障碍物在s方向上没有发生重合,跳过if (vehicle_front_s < obstacle_back_s ||vehicle_back_s > obstacle_front_s) {continue;}const auto& nudge_decision = ptr_obstacle->LateralDecision().nudge();// Please notice the differences between adc_l and frenet_point_lconst double frenet_point_l = frenet_path.at(i).l();// obstacle is on the right of ego vehicle (at path point i)bool is_close_on_left =(nudge_decision.type() == ObjectNudge::LEFT_NUDGE) &&(frenet_point_l - vehicle_param_.right_edge_to_center() -collision_safety_range <ptr_obstacle->PerceptionSLBoundary().end_l());// obstacle is on the left of ego vehicle (at path point i)bool is_close_on_right =(nudge_decision.type() == ObjectNudge::RIGHT_NUDGE) &&(ptr_obstacle->PerceptionSLBoundary().start_l() -collision_safety_range <frenet_point_l + vehicle_param_.left_edge_to_center());// TODO(all): dynamic obstacles do not have nudge decisionif (is_close_on_left || is_close_on_right) {double nudge_speed_ratio = 1.0;// 静态障碍物 x 0.6if (ptr_obstacle->IsStatic()) {nudge_speed_ratio =speed_bounds_config_.static_obs_nudge_speed_ratio(); // static_obs_nudge_speed_ratio: 0.6} else {// 动态障碍物 x 0.8nudge_speed_ratio =speed_bounds_config_.dynamic_obs_nudge_speed_ratio(); // dynamic_obs_nudge_speed_ratio: 0.8}speed_limit_from_nearby_obstacles =nudge_speed_ratio * speed_limit_from_reference_line;break;}}double curr_speed_limit = 0.0;// FLAGS_enable_nudge_slowdown: True to slow down when nudge obstaclesif (FLAGS_enable_nudge_slowdown) {curr_speed_limit =std::fmax(speed_bounds_config_.lowest_speed(),   // lowest_speed:2.5std::min({speed_limit_from_reference_line,speed_limit_from_centripetal_acc,speed_limit_from_nearby_obstacles}));} else {curr_speed_limit =std::fmax(speed_bounds_config_.lowest_speed(),std::min({speed_limit_from_reference_line,speed_limit_from_centripetal_acc}));}speed_limit_data->AppendSpeedLimit(path_s, curr_speed_limit);}return Status::OK();
}

设置标志位（is_close_on_left、is_close_on_right）部分的示意图如下：

GetSpeedLimitFromS

double ReferenceLine::GetSpeedLimitFromS(const double s) const {// 对于速度限制列表speed_limit_中已经有的部分，直接返回相应的值。for (const auto& speed_limit : speed_limit_) {if (s >= speed_limit.start_s && s <= speed_limit.end_s) {return speed_limit.speed_limit;}}const auto& map_path_point = GetReferencePoint(s);// FLAGS_planning_upper_speed_limit: Maximum speed (m/s) in planning;31.3double speed_limit = FLAGS_planning_upper_speed_limit;bool speed_limit_found = false;// 根据lane_waypoint道路的情况进行速度限制for (const auto& lane_waypoint : map_path_point.lane_waypoints()) {if (lane_waypoint.lane == nullptr) {AWARN << "lane_waypoint.lane is nullptr.";continue;}speed_limit_found = true;speed_limit =std::fmin(lane_waypoint.lane->lane().speed_limit(), speed_limit);}// 若未找到lane_waypoint.lane，根据道路类型进行限制if (!speed_limit_found) {// use default speed limit based on road_type// FLAGS_default_city_road_speed_limit: default speed limit (m/s) for city road. 35 mphspeed_limit = FLAGS_default_city_road_speed_limit;hdmap::Road::Type road_type = GetRoadType(s);if (road_type == hdmap::Road::HIGHWAY) {// FLAGS_default_highway_speed_limit: default speed limit (m/s) for highway. 65 mphspeed_limit = FLAGS_default_highway_speed_limit;}}return speed_limit;
}

参考

[1] Planning Piecewise Jerk Nonlinear Speed Optimizer Introduction
[2] Apollo规划控制学习笔记
[3] 1.10 Speed Bounds Prior Decider