【Apollo学习笔记】——规划模块TASK之SPEED_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的第11个TASK——SPEED_DECIDER

SPEED_DECIDER功能简介

产生速度决策
    根据粗规划出的速度曲线，依据曲线在障碍物的上方还是下方，采取不同的决策。

与路径决策器思路一致，当路径规划器完成路径规划以后，可以得到一条无人车的最优行驶路线，路径决策器就需要对静态障碍物做标签的更新，尤其是那些不在特殊路况下的障碍物，由于缺乏时间信息，路径决策器PathDecider无法对动态障碍物进行标签更新。

而在速度规划器完成未来时间8s或者未来前向距离150m的规划以后，已经得到了一条未来每个时刻无人车出现在参考线上的位置(累积距离s)，再配合事先已经规划好的路径，就可以完成对动态障碍物的标签更新，这里仅仅对最近的st障碍物边界框做标签更新，没有对所有的预测时间进行更新。

SPEED_DECIDER相关配置

SPEED_DECIDER流程

SpeedDecider直接继承自Task类，首先构造函数加载相关配置，接着Execute为函数入口，进行具体执行。Execute调用了函数MakeObjectDecision，而整个SpeedDecider的主要逻辑也正在MakeObjectDecision之中。

• 输入：common::Status SpeedDecider::Execute(Frame* frame, ReferenceLineInfo* reference_line_info) {frame和reference_line_info

因此接下来我们直接来看MakeObjectDecision函数。
主要流程如下图所示：

MakeObjectDecision

SpeedDecider根据路径决策、DP算法生成的速度曲线和障碍物的STBoundary的位置关系生成Ignore、Stop、Follow、Yield、Overtake的纵向决策。

Status SpeedDecider::MakeObjectDecision(const SpeedData& speed_profile, PathDecision* const path_decision) const {// 检查动态规划的结果if (speed_profile.size() < 2) {const std::string msg = "dp_st_graph failed to get speed profile.";AERROR << msg;return Status(ErrorCode::PLANNING_ERROR, msg);}// 遍历障碍物for (const auto* obstacle : path_decision->obstacles().Items()) {auto* mutable_obstacle = path_decision->Find(obstacle->Id());const auto& boundary = mutable_obstacle->path_st_boundary();// 若障碍物的STBoundary在ST Graph的范围内，忽略if (boundary.IsEmpty() || boundary.max_s() < 0.0 ||boundary.max_t() < 0.0 ||boundary.min_t() >= speed_profile.back().t()) {AppendIgnoreDecision(mutable_obstacle);continue;}// 若障碍物已经有纵向决策的障碍物，忽略if (obstacle->HasLongitudinalDecision()) {AppendIgnoreDecision(mutable_obstacle);continue;}// for Virtual obstacle, skip if center point NOT "on lane"// 对于虚拟障碍物，判断其中心是否在车道上，不在则跳过if (obstacle->IsVirtual()) {const auto& obstacle_box = obstacle->PerceptionBoundingBox();if (!reference_line_->IsOnLane(obstacle_box.center())) {continue;}}// always STOP for pedestrian// 遇到行人障碍物，添加stop决策if (CheckStopForPedestrian(*mutable_obstacle)) {ObjectDecisionType stop_decision;if (CreateStopDecision(*mutable_obstacle, &stop_decision,-FLAGS_min_stop_distance_obstacle)) {mutable_obstacle->AddLongitudinalDecision("dp_st_graph/pedestrian",stop_decision);}continue;}// 根据路径决策、DP得到的速度曲线、障碍物边界获取STlocationauto location = GetSTLocation(path_decision, speed_profile, boundary);// 检查KEEP_CLEAR区域是否阻塞if (!FLAGS_use_st_drivable_boundary) {if (boundary.boundary_type() == STBoundary::BoundaryType::KEEP_CLEAR) {if (CheckKeepClearBlocked(path_decision, *obstacle)) {location = BELOW;}}}switch (location) {case BELOW:// 若障碍物类型为KEEP_CLEAR，创建停止决策if (boundary.boundary_type() == STBoundary::BoundaryType::KEEP_CLEAR) {ObjectDecisionType stop_decision;if (CreateStopDecision(*mutable_obstacle, &stop_decision, 0.0)) {mutable_obstacle->AddLongitudinalDecision("dp_st_graph/keep_clear",stop_decision);}// 检查ADC是否处于跟车的状态} else if (CheckIsFollow(*obstacle, boundary)) {// stop for low_speed decelerating// 是否跟车距离太近，if (IsFollowTooClose(*mutable_obstacle)) {ObjectDecisionType stop_decision;// 创建停止决策if (CreateStopDecision(*mutable_obstacle, &stop_decision,-FLAGS_min_stop_distance_obstacle)) {mutable_obstacle->AddLongitudinalDecision("dp_st_graph/too_close",stop_decision);}} else {  // high speed or low speed accelerating// FOLLOW decisionObjectDecisionType follow_decision;if (CreateFollowDecision(*mutable_obstacle, &follow_decision)) {mutable_obstacle->AddLongitudinalDecision("dp_st_graph",follow_decision);}}} else {// YIELD decisionObjectDecisionType yield_decision;if (CreateYieldDecision(*mutable_obstacle, &yield_decision)) {mutable_obstacle->AddLongitudinalDecision("dp_st_graph",yield_decision);}}break;case ABOVE:if (boundary.boundary_type() == STBoundary::BoundaryType::KEEP_CLEAR) {ObjectDecisionType ignore;ignore.mutable_ignore();mutable_obstacle->AddLongitudinalDecision("dp_st_graph", ignore);} else {// OVERTAKE decisionObjectDecisionType overtake_decision;if (CreateOvertakeDecision(*mutable_obstacle, &overtake_decision)) {mutable_obstacle->AddLongitudinalDecision("dp_st_graph/overtake",overtake_decision);}}break;case CROSS:// 障碍物是否堵塞if (mutable_obstacle->IsBlockingObstacle()) {ObjectDecisionType stop_decision;// 建立停止决策if (CreateStopDecision(*mutable_obstacle, &stop_decision,-FLAGS_min_stop_distance_obstacle)) {mutable_obstacle->AddLongitudinalDecision("dp_st_graph/cross",stop_decision);}const std::string msg = absl::StrCat("Failed to find a solution for crossing obstacle: ",mutable_obstacle->Id());AERROR << msg;return Status(ErrorCode::PLANNING_ERROR, msg);}break;default:AERROR << "Unknown position:" << location;}AppendIgnoreDecision(mutable_obstacle);}return Status::OK();
}

其中的核心部分就是这一句代码：根据路径决策、DP得到的速度曲线、障碍物边界获取STlocation。

    // 根据路径决策、DP得到的速度曲线、障碍物边界获取STlocationauto location = GetSTLocation(path_decision, speed_profile, boundary);

GetSTLocation

接着来看GetSTLocation函数的具体实现：
GetSTLocation依据在ST图中障碍物与速度曲线上的各点之间的位置关系来确定决策。

SpeedDecider::STLocation SpeedDecider::GetSTLocation(const PathDecision* const path_decision, const SpeedData& speed_profile,const STBoundary& st_boundary) const {// 若boundary为空，设置为BELOW；一般情况下这个障碍物直接被跳过，不考虑if (st_boundary.IsEmpty()) {return BELOW;}STLocation st_location = BELOW;bool st_position_set = false;const double start_t = st_boundary.min_t();const double end_t = st_boundary.max_t();// 遍历速度曲线中的每一个点for (size_t i = 0; i + 1 < speed_profile.size(); ++i) {const STPoint curr_st(speed_profile[i].s(), speed_profile[i].t());const STPoint next_st(speed_profile[i + 1].s(), speed_profile[i + 1].t());// 跳过和障碍物不在一个ST范围内的if (curr_st.t() < start_t && next_st.t() < start_t) {continue;}if (curr_st.t() > end_t) {break;}if (!FLAGS_use_st_drivable_boundary) {common::math::LineSegment2d speed_line(curr_st, next_st);// 检查是否碰撞if (st_boundary.HasOverlap(speed_line)) {ADEBUG << "speed profile cross st_boundaries.";st_location = CROSS;if (!FLAGS_use_st_drivable_boundary) {// 检查类型是否是KEEP_CLEARif (st_boundary.boundary_type() ==STBoundary::BoundaryType::KEEP_CLEAR) {// 若CheckKeepClearCrossable为false，则设置为BELOWif (!CheckKeepClearCrossable(path_decision, speed_profile,st_boundary)) {st_location = BELOW;}}}break;}}// note: st_position can be calculated by checking two st points once//       but we need iterate all st points to make sure there is no CROSSif (!st_position_set) {if (start_t < next_st.t() && curr_st.t() < end_t) {STPoint bd_point_front = st_boundary.upper_points().front();double side = common::math::CrossProd(bd_point_front, curr_st, next_st);st_location = side < 0.0 ? ABOVE : BELOW;st_position_set = true;}}}return st_location;
}

Check类函数

Check类函数，这一类函数主要用于对不同障碍物进行判断。

CheckKeepClearCrossable

bool SpeedDecider::CheckKeepClearCrossable(const PathDecision* const path_decision, const SpeedData& speed_profile,const STBoundary& keep_clear_st_boundary) const {bool keep_clear_crossable = true;// 计算最后一点的速度const auto& last_speed_point = speed_profile.back();double last_speed_point_v = 0.0;if (last_speed_point.has_v()) {last_speed_point_v = last_speed_point.v();} else {const size_t len = speed_profile.size();if (len > 1) {const auto& last_2nd_speed_point = speed_profile[len - 2];last_speed_point_v = (last_speed_point.s() - last_2nd_speed_point.s()) /(last_speed_point.t() - last_2nd_speed_point.t());}}static constexpr double kKeepClearSlowSpeed = 2.5;  // m/sADEBUG << "last_speed_point_s[" << last_speed_point.s()<< "] st_boundary.max_s[" << keep_clear_st_boundary.max_s()<< "] last_speed_point_v[" << last_speed_point_v << "]";// 若最后一点的s小于KeepClear区域的最大值，且最后一点的速度小于阈值速度，// 则keep_clear_crossable = falseif (last_speed_point.s() <= keep_clear_st_boundary.max_s() &&last_speed_point_v < kKeepClearSlowSpeed) {keep_clear_crossable = false;}return keep_clear_crossable;
}

CheckStopForPedestrian

bool SpeedDecider::CheckStopForPedestrian(const Obstacle& obstacle) const {const auto& perception_obstacle = obstacle.Perception();// 检查类型是否是行人if (perception_obstacle.type() != PerceptionObstacle::PEDESTRIAN) {return false;}const auto& obstacle_sl_boundary = obstacle.PerceptionSLBoundary();// 不考虑在车之后的行人if (obstacle_sl_boundary.end_s() < adc_sl_boundary_.start_s()) {return false;}// read pedestrian stop time from PlanningContextauto* mutable_speed_decider_status = injector_->planning_context()->mutable_planning_status()->mutable_speed_decider();// 用hash表存储停止时间                                        std::unordered_map<std::string, double> stop_time_map;for (const auto& pedestrian_stop_time :mutable_speed_decider_status->pedestrian_stop_time()) {stop_time_map[pedestrian_stop_time.obstacle_id()] =pedestrian_stop_time.stop_timestamp_sec();}const std::string& obstacle_id = obstacle.Id();// update stop timestamp on static pedestrian for watch timer// check on stop timer for static pedestriansstatic constexpr double kSDistanceStartTimer = 10.0;static constexpr double kMaxStopSpeed = 0.3;static constexpr double kPedestrianStopTimeout = 4.0;bool result = true;if (obstacle.path_st_boundary().min_s() < kSDistanceStartTimer) {const auto obstacle_speed = std::hypot(perception_obstacle.velocity().x(),perception_obstacle.velocity().y());// 如果行人速度超过最大停车速度，则删除行人if (obstacle_speed > kMaxStopSpeed) {stop_time_map.erase(obstacle_id);} else {if (stop_time_map.count(obstacle_id) == 0) {// add timestampstop_time_map[obstacle_id] = Clock::NowInSeconds();ADEBUG << "add timestamp: obstacle_id[" << obstacle_id << "] timestamp["<< Clock::NowInSeconds() << "]";} else {// check timeoutdouble stop_timer = Clock::NowInSeconds() - stop_time_map[obstacle_id];ADEBUG << "stop_timer: obstacle_id[" << obstacle_id << "] stop_timer["<< stop_timer << "]";// 检查其是否已经等待了足够长的时间if (stop_timer >= kPedestrianStopTimeout) {result = false;}}}}// write pedestrian stop time to PlanningContextmutable_speed_decider_status->mutable_pedestrian_stop_time()->Clear();for (const auto& stop_time : stop_time_map) {auto pedestrian_stop_time =mutable_speed_decider_status->add_pedestrian_stop_time();pedestrian_stop_time->set_obstacle_id(stop_time.first);pedestrian_stop_time->set_stop_timestamp_sec(stop_time.second);}return result;
}

CheckIsFollow

bool SpeedDecider::CheckIsFollow(const Obstacle& obstacle,const STBoundary& boundary) const {const double obstacle_l_distance =std::min(std::fabs(obstacle.PerceptionSLBoundary().start_l()),std::fabs(obstacle.PerceptionSLBoundary().end_l()));// FLAGS_follow_min_obs_lateral_distance: obstacle min lateral distance to follow; 2.5if (obstacle_l_distance > FLAGS_follow_min_obs_lateral_distance) {return false;}// move towards adcif (boundary.bottom_left_point().s() > boundary.bottom_right_point().s()) {return false;}static constexpr double kFollowTimeEpsilon = 1e-3;static constexpr double kFollowCutOffTime = 0.5;if (boundary.min_t() > kFollowCutOffTime ||boundary.max_t() < kFollowTimeEpsilon) {return false;}// cross lane but be moving to different direction// FLAGS_follow_min_time_sec: // " min follow time in st region before considering a valid follow,"// " this is to differentiate a moving obstacle cross adc's"// " current lane and move to a different direction"// 2.0if (boundary.max_t() - boundary.min_t() < FLAGS_follow_min_time_sec) {return false;}return true;
}

CheckKeepClearBlocked

bool SpeedDecider::CheckKeepClearBlocked(const PathDecision* const path_decision,const Obstacle& keep_clear_obstacle) const {bool keep_clear_blocked = false;// check if overlap with other stop wallfor (const auto* obstacle : path_decision->obstacles().Items()) {if (obstacle->Id() == keep_clear_obstacle.Id()) {continue;}const double obstacle_start_s = obstacle->PerceptionSLBoundary().start_s();const double adc_length =VehicleConfigHelper::GetConfig().vehicle_param().length();const double distance =obstacle_start_s - keep_clear_obstacle.PerceptionSLBoundary().end_s();if (obstacle->IsBlockingObstacle() && distance > 0 &&distance < (adc_length / 2)) {keep_clear_blocked = true;break;}}return keep_clear_blocked;
}

Create类函数

建立各类决策。