planning模块(16) task SPEED_DECIDER

按照task的执行顺序，SPEED_HEURISTIC_OPTIMIZER task执行完会接着执行SPEED_DECIDER task

common::Status SpeedDecider::Execute(Frame* frame,
                                     ReferenceLineInfo* reference_line_info) {

MakeObjectDecision函数

if (!MakeObjectDecision(reference_line_info->speed_data(),
                        reference_line_info->path_decision())
         .ok()) {
  const std::string msg = "Get object decision by speed profile failed.";
  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();

  if (boundary.IsEmpty() || boundary.max_s() < 0.0 ||
      boundary.max_t() < 0.0 ||
      boundary.min_t() >= speed_profile.back().t()) {
    AppendIgnoreDecision(mutable_obstacle);
    continue;
  }

检查障碍物的时空边界，如果满足以下任一条件就忽略：

边界为空（无时空交集）
在车辆后方（s < 0）
出现在过去（t < 0）
在规划时间范围外出现

void SpeedDecider::AppendIgnoreDecision(Obstacle* obstacle) const {
  ObjectDecisionType ignore_decision;
  ignore_decision.mutable_ignore();
  if (!obstacle->HasLongitudinalDecision()) {
    obstacle->AddLongitudinalDecision("dp_st_graph", ignore_decision);
  }
  if (!obstacle->HasLateralDecision()) {
    obstacle->AddLateralDecision("dp_st_graph", ignore_decision);
  }
}

给障碍物添加"忽略"决策

创建忽略决策对象
如果没有纵向决策，添加纵向忽略
如果没有横向决策，添加横向忽略
标签为 "dp_st_graph"

void Obstacle::AddLongitudinalDecision(const std::string& decider_tag,
                                       const ObjectDecisionType& decision) {

添加纵向决策

void Obstacle::AddLateralDecision(const std::string& decider_tag,
                                  const ObjectDecisionType& decision) {

添加横向决策

ObjectDecisionType Obstacle::MergeLongitudinalDecision(
    const ObjectDecisionType& lhs, const ObjectDecisionType& rhs) {

合并纵向决策

ObjectDecisionType Obstacle::MergeLateralDecision(
    const ObjectDecisionType& lhs, const ObjectDecisionType& rhs)

合并横向决策

auto location = GetSTLocation(path_decision, speed_profile, boundary);

GetSTLocation函数

用于判断速度曲线(Speed Profile)与障碍物ST边界的相对位置关系

ABOVE:速度曲线在 ST 边界上方(安全，不会碰撞）
BELOW:速度曲线在 ST 边界下方(危险，可能碰撞）
CROSS:速度曲线与 ST 边界交叉(危险，会碰撞）

if (st_boundary.IsEmpty()) {
return BELOW;
}
如果ST边界为空(障碍物不在路径上),直接返回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();

st_location :存储最终的位置关系，初始为 BELOW
st_position_set:标记是否已经确定了位置关系
start_t / end_t:ST边界的时间范围

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());

遍历速度曲线的每一段线段

curr_st:当前时间点的位置
next_st:下一个时间点的位置
两点构成一条速度曲线线段

if (curr_st.t() < start_t && next_st.t() < start_t) {
  continue;  // 速度曲线段在边界开始之前，跳过
}
if (curr_st.t() > end_t) {
  break;  // 速度曲线段在边界结束之后，退出循环
}

只检查与ST边界时间范围重叠的速度曲线段

如果线段完全在边界之前 → 跳过
如果线段在边界之后 → 退出（后续都不用检查）

if (!FLAGS_use_st_drivable_boundary) {
  // 第106行：创建速度曲线线段
  common::math::LineSegment2d speed_line(curr_st, next_st);
  // 第107行：检查是否与 ST 边界交叉
  if (st_boundary.HasOverlap(speed_line)) {
    ADEBUG << "speed profile cross st_boundaries.";
    // 第109行：标记为交叉
    st_location = CROSS;

线段与ST边界是否交叉的判断方法在planning模块(15)-速度动态规划优化算法(path time heuristic optimizer) 介绍过

CheckKeepClearCrossable函数

bool SpeedDecider::CheckKeepClearCrossable(
    const PathDecision* const path_decision, 
    const SpeedData& speed_profile,
    const STBoundary& keep_clear_st_boundary) const

判断车辆是否可以穿越KEEP_CLEAR区域

const auto& last_speed_point = speed_profile.back();
double last_speed_point_v = 0.0;

last_speed_point:速度曲线的最后一个点（规划终点）
last_speed_point_v:最后一个点的速度，初始化为0.0

if (last_speed_point.has_v()) {
  last_speed_point_v = last_speed_point.v();
}

直接使用已有的速度值v()

else {
  const size_t len = speed_profile.size();
  if (len > 1) {
    const auto& last_2nd_speed_point = speed_profile[len - 2];  // 倒数第二个点
    // 用差分计算速度：v = Δs / Δt
    last_speed_point_v = (last_speed_point.s() - last_2nd_speed_point.s()) /
                         (last_speed_point.t() - last_2nd_speed_point.t());
  }
}

使用倒数第二个点和最后一个点计算平均速度

if (last_speed_point.s() <= keep_clear_st_boundary.max_s() &&
    last_speed_point_v < config_.keep_clear_last_point_speed()) {
  keep_clear_crossable = false;
}

车辆规划的终点还在 KEEP_CLEAR 区域内或之前
车辆在规划终点几乎静止
同时，满足这两个条件时，表示禁停区不可以穿越

if (st_boundary.boundary_type() ==
   STBoundary::BoundaryType::KEEP_CLEAR) {
 if (!CheckKeepClearCrossable(path_decision, speed_profile,
                              st_boundary)) {
   st_location = BELOW;
 }
}

如果发现交叉，但区域是KEEP_CLEAR且不可穿越
将 st_location从CROSS改回BELOW
表示车辆应该在边界下方(即停车等待，不穿越）

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;
  }
}

判断速度曲线在ST边界的上方还是下方

叉积side < 0,说明曲线段在障碍物ST边界上方，否则说明曲线段在障碍物ST边界下方

bool SpeedDecider::CheckKeepClearBlocked(
    const PathDecision* const path_decision,
    const Obstacle& keep_clear_obstacle) const

判断 KEEP_CLEAR 区域(如路口)前方是否有其他障碍物阻挡

返回值：

true ：KEEP_CLEAR区域被阻挡(不能进入）
false ：KEEP_CLEAR区域畅通(可以进入）

for (const auto* obstacle : path_decision->obstacles().Items()) {

if (obstacle->Id() == keep_clear_obstacle.Id()) {
  continue;  // 跳过 KEEP_CLEAR 障碍物自身
}

遍历路径决策中的所有障碍物，跳过自身，看是否有其他障碍物阻挡

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();

如果distance > 0说明障碍物在KEEP_CLESR前方

if (obstacle->IsBlockingObstacle() && distance > 0 &&
        distance < (adc_length / 2)) {
      keep_clear_blocked = true;
      break;
    }

障碍物是阻挡型的 && 障碍物在路口前方 && 距离很近(小于半个车长）
说明路口被阻挡了

if (!FLAGS_use_st_drivable_boundary) {
  if (boundary.boundary_type() == STBoundary::BoundaryType::KEEP_CLEAR) {
    if (CheckKeepClearBlocked(path_decision, *obstacle)) {
      location = BELOW;
    }
  }
}

在KEEP_CLESR区域如果被阻挡，强制设为BELOW

CreateStopDecision函数

创建停车决策点

bool SpeedDecider::CreateStopDecision(const Obstacle& obstacle,
                                      ObjectDecisionType* const stop_decision,
                                      double stop_distance) const {
  const auto& boundary = obstacle.path_st_boundary();

  // TODO(all): this is a bug! Cannot mix reference s and path s!
  // Replace boundary.min_s() with computed reference line s
  // fence is set according to reference line s.
  double fence_s = adc_sl_boundary_.end_s() + boundary.min_s() + stop_distance;

这块是有问题的，停车围栏位置应该是障碍物在参考线上的位置-stop_distance
但现在
adc_sl_boundary_.end_s():是基于参考线计算出来的
boundary.min_s():是基于路径规划出来的路径计算出来的
基准不同不能相加

const double main_stop_s =
      reference_line_info_->path_decision()->stop_reference_line_s();
  if (main_stop_s < fence_s) {
    ADEBUG << "Stop fence is further away, ignore.";
    return false;
  }

main_stop_s:

Apollo可能有多个停车决策来源
需要一个统一的、最优先的停车位置
这个位置是所有停车决策中的最小值(最保守的）
如果已存在的停车位置更早(s 值更小）
说明车辆需要在更近的位置停下
当前障碍物产生的停车决策是多余的
忽略当前障碍物的停车决策

const auto fence_point = reference_line_->GetReferencePoint(fence_s);

// set STOP decision
auto* stop = stop_decision->mutable_stop();
stop->set_distance_s(stop_distance);
auto* stop_point = stop->mutable_stop_point();
stop_point->set_x(fence_point.x());
stop_point->set_y(fence_point.y());
stop_point->set_z(0.0);
stop->set_stop_heading(fence_point.heading());

坐标转换
将参考线 s 坐标转换为世界坐标
获取停车点的完整信息(位置 + 航向）
创建 STOP 决策
初始化停车决策对象
设置停车参数
distance_s:安全距离
stop_point:停车位置 (x, y, z)
stop_heading:停车航向角

CreateFollowDecision函数

创建跟车决策点

EstimateProperFollowGap函数

根据车速动态计算跟车距离
跟车距离 = 基础距离 + 速度 × 增长率

速度越快，跟车距离越大
低速时距离增长快，高速时增长慢

double follow_distance = config_.stop_follow_distance();

从配置中获取基础停车距离
默认值:2.0 米
这是速度为0时的最小跟车距离

  for (const auto& follow_function :
       config_.follow_distance_scheduler().follow_distance()) {
    follow_distance_function_.emplace_back(
        std::make_pair(follow_function.speed(), follow_function.slope()));
  }

follow_distance_function_是从配置modules/planning/tasks/speed_decider/conf/default_conf.pb.txt
获取到的速度和斜率

for (i = 1 to 3) {
  if (当前速度 <= 第 i 个速度点) {
    // 计算当前区间的增量
    follow_distance += 前一个斜率 × (当前速度 - 前一个速度点)
    break
  } else {
    // 累加这个区间的完整增量
    follow_distance += 前一个斜率 × (第 i 个速度点 - 前一个速度点)
  }
}

if (当前速度 > 最大速度点) {
  follow_distance += 最后一个斜率 × (当前速度 - 最大速度点)
}

const auto& boundary = obstacle.path_st_boundary();
  const double reference_s =
      adc_sl_boundary_.end_s() + boundary.min_s() + follow_distance_s;

跟车位置计算也是错误的，同样应该会障碍物在参考线上的位置-follow_distance_s
后续设置跟车决策点信息和设置停车决策点信息相似不再介绍

CreateYieldDecision函数

创建让行决策

const auto& obstacle_boundary = obstacle.path_st_boundary();
  const double yield_distance_s =
      std::max(-obstacle_boundary.min_s(), -yield_distance);

  const double reference_line_fence_s =
      adc_sl_boundary_.end_s() + obstacle_boundary.min_s() + yield_distance_s;

计算的让行位置也是错误的，同样是障碍物在参考线上的位置 + yield_distance_s，这里yield_distance_s是负数
后续设置让行决策点信息和设置停车决策点信息相似不再介绍

CreateOvertakeDecision函数

创建超车决策

const auto& velocity = obstacle.Perception().velocity();

获取障碍物速度

common::math::Vec2d::CreateUnitVec2d(init_point_.path_point().theta())

创建自车前进方向的单位向量

Vec2d(velocity.x(), velocity.y())

创建障碍物速度向量

.InnerProd(Vec2d(velocity.x(), velocity.y()))

障碍物速度在自车前进方向上的投影分量

障碍物速度向量 v = (vx, vy)
自车方向单位向量 u = (ux, uy)

点积结果 = |v| × |u| × cosθ
        = |v| × cosθ  （因为 |u| = 1）

点积的几何意义

double overtake_distance_s =
      EstimateProperOvertakingGap(obstacle_speed, init_point_.v());

double SpeedDecider::EstimateProperOvertakingGap(const double target_obs_speed,
                                                 const double adc_speed) const {
  const double overtake_distance_s = std::fmax(
      std::fmax(adc_speed, target_obs_speed) * config_.overtake_time_buffer(),
      config_.overtake_min_distance());
  return overtake_distance_s;
}

计算超车时需要保持的安全距离
超车距离 = max(最大速度 × 时间缓冲区, 最小距离)
后续设置超车决策点信息和设置停车决策点信息相似不再介绍

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);
  }
}

当规划的速度曲线位于 KEEP_CLEAR 区域下方时，生成停车决策，要求自车在 KEEP_CLEAR 区域边界前停车

else if (obstacle->IsStatic()) {
  ObjectDecisionType stop_decision;
  if (CreateStopDecision(*mutable_obstacle, &stop_decision,
                         -FLAGS_min_stop_distance_obstacle)) {
    mutable_obstacle->AddLongitudinalDecision("dp_st_graph",
                                              stop_decision);
  }
}

当速度曲线位于静态障碍物下方时，生成停车决策，要求在障碍物前方 6 米(FLAGS_min_stop_distance_obstacle默认为6)处停车，确保安全距离

CheckIsFollow函数

const double obstacle_l_distance =
    std::min(std::fabs(obstacle.PerceptionSLBoundary().start_l()),
             std::fabs(obstacle.PerceptionSLBoundary().end_l()));

障碍物到参考线的最小横向距离

if (obstacle_l_distance > config_.follow_min_obs_lateral_distance()) {
  return false;
}

如果障碍物的最小横向距离 > 2.5 米 → 障碍物不在自车车道内 → 不跟车

if (boundary.bottom_left_point().s() > boundary.bottom_right_point().s()) {
    return false;
  }

检测障碍物是否朝向自车运动(s 坐标随时间减小).如果是，说明是对向来车或倒车，不能作为跟随目标，直接返回false,不跟车

static constexpr double kFollowTimeEpsilon = 1e-3;
  static constexpr double kFollowCutOffTime = 0.5;
  if (boundary.min_t() > kFollowCutOffTime ||
      boundary.max_t() < kFollowTimeEpsilon) {
    return false;
  }

检查障碍物是否在合理的时间范围内(min_t ≤ 0.5s 且 max_t ≥ 0.001s).如果障碍物太晚出现或太早消失，说明不在当前规划范围内，不跟车

if (boundary.max_t() - boundary.min_t() < config_.follow_min_time_sec()) {
    return false;
  }

检查障碍物在车道内持续的时间是否至少 2 秒。如果时间太短，说明障碍物可能是横穿车道或转弯离开，不能作为跟随目标

IsFollowTooClose函数

检查是否跟车太近

 if (!obstacle.IsBlockingObstacle()) {
    return false;
  }

如果当前车道没有阻塞障碍物，返回false

if (obstacle.path_st_boundary().min_t() > 0.0) {
  return false;
}

说明当前时刻，自车前方没有障碍物出现，>0说明在未来时刻有障碍物

const double obs_speed = obstacle.speed();
 const double ego_speed = init_point_.v();
 if (obs_speed > ego_speed) {
   return false;
 }

比较前车和自车的速度。如果前车速度大于自车速度，说明前车在远离，距离会越来越大，不存在追尾风险，无需检查跟车距离

const double distance =
      obstacle.path_st_boundary().min_s() - FLAGS_min_stop_distance_obstacle;

FLAGS_min_stop_distance_obstacle:停车缓冲距离，默认是6m
distance:最小制动位置

v^{2} = v_{0}^{2}+2as
s=\frac{v^{2}-v_{0}^{2}}{2\left |a \right | }

double distance_numerator = std::pow((ego_speed - obs_speed), 2) * 0.5;

s=\frac{v^{2}-v_{0}^{2}}{2}

distance_numerator / distance_denominator

s=\frac{v^{2}-v_{0}^{2}}{2\left |a \right | }

distance < distance_numerator / distance_denominator;

实际距离 < 所需距离 → 太近

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);
  }
}

当障碍物不满足跟随条件，但仍影响自车行驶时，生成让行决策，要求自车减速让障碍物先通过

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);
    }
  }

当速度曲线从障碍物上方通过时，如果是KEEP_CLEAR 区域则生成IGNORE决策，否则生成超车决策

 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);
    }

检测到速度曲线会撞上障碍物时，立即生成停车决策