【Apollo学习笔记】——规划模块TASK之SPEED_DECIDER
文章目录
- 前言
- SPEED_DECIDER功能简介
- SPEED_DECIDER相关配置
- SPEED_DECIDER流程
-
- MakeObjectDecision
- GetSTLocation
- Check类函数
-
- CheckKeepClearCrossable
- CheckStopForPedestrian
- CheckIsFollow
- CheckKeepClearBlocked
- Create类函数
前言
在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_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的第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得到的速度曲线、障碍物边界获取STlocation
auto 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 decision
ObjectDecisionType follow_decision;
if (CreateFollowDecision(*mutable_obstacle, &follow_decision)) {
mutable_obstacle->AddLongitudinalDecision("dp_st_graph",
follow_decision);
}
}
} else {
// YIELD decision
ObjectDecisionType 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 decision
ObjectDecisionType 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得到的速度曲线、障碍物边界获取STlocation
auto 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_CLEAR
if (st_boundary.boundary_type() ==
STBoundary::BoundaryType::KEEP_CLEAR) {
// 若CheckKeepClearCrossable为false，则设置为BELOW
if (!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 CROSS
if (!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/s
ADEBUG << "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 = false
if (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 PlanningContext
auto* 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 pedestrians
static 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 timestamp
stop_time_map[obstacle_id] = Clock::NowInSeconds();
ADEBUG << "add timestamp: obstacle_id[" << obstacle_id << "] timestamp["
<< Clock::NowInSeconds() << "]";
} else {
// check timeout
double 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 PlanningContext
mutable_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.5
if (obstacle_l_distance > FLAGS_follow_min_obs_lateral_distance) {
return false;
}
// move towards adc
if (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.0
if (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 wall
for (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类函数
建立各类决策。