Baidu Apollo代码解析之EM Planner中的DP Speed Optimizer

大家好，我已经把CSDN上的博客迁移到了知乎上，欢迎大家在知乎关注我的专栏慢慢悠悠小马车（https://zhuanlan.zhihu.com/duangduangduang）。希望大家可以多多交流，互相学习。

---

SpeedOptimizer类继承自Task类，实现了继承来的Execute()函数，又在Execute()中调用了其纯虚成员函数Process()。而DpStSpeedOptimizer类又继承了SpeedOptimizer类，并实现了Process()函数，在Process()中调用了成员函数SearchStGraph()，到这里，才算进入了使用动态规划算法进行速度规划的正题。总之，Apollo的这个层次设计是真的优美！以上，可见apollo\modules\planning\tasks\task.h 及 apollo\modules\planning\tasks\optimizers\speed_optimizer.h 及 apollo\modules\planning\tasks\optimizers\dp_st_speed\dp_st_speed_optimizer.h 文件。

bool DpStSpeedOptimizer::SearchStGraph(SpeedData* speed_data) const {
  DpStGraph st_graph(reference_line_info_->st_graph_data(), dp_st_speed_config_,
                     reference_line_info_->path_decision()->obstacles().Items(),
                     init_point_, adc_sl_boundary_);

  if (!st_graph.Search(speed_data).ok()) {
    AERROR << "failed to search graph with dynamic programming.";
    return false;
  }
  return true;
}

在DpStSpeedOptimizer::SearchStGraph()中，首先构造了DpStGraph类实例，该类包含了所有可能的(s,t)节点的代价表，应用动态规划算法查找出代价最小的路径，然后计算速度，就得到了速度规划的结果。

Status DpStGraph::Search(SpeedData* const speed_data) {
  constexpr double kBounadryEpsilon = 1e-2;
  for (const auto& boundary : st_graph_data_.st_boundaries()) {
    //KEEP_CLEAR的范围不影响速度规划
    if (boundary->boundary_type() == STBoundary::BoundaryType::KEEP_CLEAR) {
      continue;
    }
    //若boundary包含原点或非常接近原点（即自车现在的位置），自车应该stop，不再前进
    //故s,v,a都是0，即随着t推移，自车不动
    if (boundary->IsPointInBoundary({0.0, 0.0}) ||
        (std::fabs(boundary->min_t()) < kBounadryEpsilon &&
         std::fabs(boundary->min_s()) < kBounadryEpsilon)) {
      std::vector<SpeedPoint> speed_profile;
      double t = 0.0;
      for (int i = 0; i <= dp_st_speed_config_.matrix_dimension_t(); ++i, t += unit_t_) {
        SpeedPoint speed_point;
        speed_point.set_s(0.0);
        speed_point.set_t(t);
        speed_point.set_v(0.0);
        speed_point.set_a(0.0);
        speed_profile.emplace_back(speed_point);
      }
      *speed_data = SpeedData(speed_profile);
      return Status::OK();
    }
  }

  if (!InitCostTable().ok()) { ... }

  if (!CalculateTotalCost().ok()) { ... }

  if (!RetrieveSpeedProfile(speed_data).ok()) { ... }
  return Status::OK();
}

在对自车当前位置（原点）判断后，速度规划主要分为3步：构建代价表InitCostTable()，更新代价表中每个节点的代价CalculateTotalCost()，查找代价最小的规划结束点并逆向构建路径、求取速度RetrieveSpeedProfile()。

// cost_table_[t][s]
// row: s, col: t
//StGraphPoint的total_cost_被初始化为+inf
std::vector<std::vector<StGraphPoint>> cost_table_;

DpStGraph::InitCostTable()没什么好说的，主要小心cost_table_外层是t，内层是s，行数是s，列数是t。

DpStGraph::CalculateTotalCost()的目的是对cost_table_中的每个节点更新其cost，在此，我们先说如何更新单个节点的cost，请看DpStGraph::CalculateCostAt()函数，动态规划的核心都在这个函数里。

void DpStGraph::CalculateCostAt(const std::shared_ptr<StGraphMessage>& msg) {
  //动态规划
  ...
  const auto& cost_init = cost_table_[0][0];
  if (c == 0) {
    //设置起始点的cost为0，为什么不再刚进入函数的时候检查？
    DCHECK_EQ(r, 0) << "Incorrect. Row should be 0 with col = 0. row: " << r;
    cost_cr.SetTotalCost(0.0);
    return;
  }
  ...
  if (c == 1) {
    //c=1需要单独处理，因为c=0的1列只有1个节点，即初始节点
    const double acc = (r * unit_s_ / unit_t_ - init_point_.v()) / unit_t_;
    //如果加速度超出范围，不计算cost
    if (acc < dp_st_speed_config_.max_deceleration() ||
        acc > dp_st_speed_config_.max_acceleration()) {
      return;
    }
    //如果从起点到当前点有障碍物，不计算cost
    if (CheckOverlapOnDpStGraph(st_graph_data_.st_boundaries(), cost_cr,
                                cost_init)) {
      return;
    }
    cost_cr.SetTotalCost(
        //点内cost
        cost_cr.obstacle_cost() + cost_init.total_cost() +
        //点间cost，从起点到[c=1,r]的各项cost，即[t0,s0]->[t1,sn]的cost
        CalculateEdgeCostForSecondCol(r, speed_limit, soft_speed_limit));
    cost_cr.SetPrePoint(cost_init);
    return;
  }

  constexpr double kSpeedRangeBuffer = 0.20;
  //单时间步长最大可能前进的s
  const uint32_t max_s_diff =
      static_cast<uint32_t>(FLAGS_planning_upper_speed_limit *
                            (1 + kSpeedRangeBuffer) * unit_t_ / unit_s_);
  //缩小行范围到[r_low,r]，因为要更新[c,r]的cost，就要考察
  //[c前一列即前一时刻,r前几行]分别到[c,r]的代价，取最小值，松弛操作，动态规划
  const uint32_t r_low = (max_s_diff < r ? r - max_s_diff : 0);

  const auto& pre_col = cost_table_[c - 1];

  if (c == 2) {
    for (uint32_t r_pre = r_low; r_pre <= r; ++r_pre) {
      const double acc =
          (r * unit_s_ - 2 * r_pre * unit_s_) / (unit_t_ * unit_t_);
      if (acc < dp_st_speed_config_.max_deceleration() ||
          acc > dp_st_speed_config_.max_acceleration()) {
        continue;
      }

      if (CheckOverlapOnDpStGraph(st_graph_data_.st_boundaries(), cost_cr,
                                  pre_col[r_pre])) {
        continue;
      }

      const double cost =
          //第二项是考察的s方向前面某点的cost
          cost_cr.obstacle_cost() + pre_col[r_pre].total_cost() +
          CalculateEdgeCostForThirdCol(r, r_pre, speed_limit, soft_speed_limit);
      //在未赋有效cost值之前，cost_cr的total_cost是+inf
      if (cost < cost_cr.total_cost()) {
        cost_cr.SetTotalCost(cost);
        cost_cr.SetPrePoint(pre_col[r_pre]);
      }
    }
    return;
  }
  //固定col即t，考察row即s
  for (uint32_t r_pre = r_low; r_pre <= r; ++r_pre) {
    //[pre_col,r_pre]不可达，自然不必考虑从该点到[c,r]点
    if (std::isinf(pre_col[r_pre].total_cost()) ||
        pre_col[r_pre].pre_point() == nullptr) {
      continue;
    }

    const double curr_a = (cost_cr.index_s() * unit_s_ +
                           pre_col[r_pre].pre_point()->index_s() * unit_s_ -
                           2 * pre_col[r_pre].index_s() * unit_s_) /
                          (unit_t_ * unit_t_);
    if (curr_a > vehicle_param_.max_acceleration() ||
        curr_a < vehicle_param_.max_deceleration()) {
      continue;
    }
    if (CheckOverlapOnDpStGraph(st_graph_data_.st_boundaries(), cost_cr,
                                pre_col[r_pre])) {
      continue;
    }

    //接下来的2个if判断是为了确认triple_pre_point有效，以计算jerk cost
    //因为jerk cost的计算方式不同，对col分3类讨论（思路相同），对应DpStCost类
    //中3种计算jerk cost的函数。jerk cost是速度规划有效性的保障，若此处不计算，
    //在后续的轨迹validaty check中排除无效轨迹也行，这种预先限制的方法更好
    //accel的cost计算和范围检查也是同理，此处没有对jerk范围进行检查

    uint32_t r_prepre = pre_col[r_pre].pre_point()->index_s();
    const StGraphPoint& prepre_graph_point = cost_table_[c - 2][r_prepre];
    if (std::isinf(prepre_graph_point.total_cost())) {
      continue;
    }

    if (!prepre_graph_point.pre_point()) {
      continue;
    }
    const STPoint& triple_pre_point = prepre_graph_point.pre_point()->point();
    const STPoint& prepre_point = prepre_graph_point.point();
    const STPoint& pre_point = pre_col[r_pre].point();
    const STPoint& curr_point = cost_cr.point();
    //triple_pre_point和prepre_point只是计算jerk cost需要
    double cost = cost_cr.obstacle_cost() + pre_col[r_pre].total_cost() +
                  CalculateEdgeCost(triple_pre_point, prepre_point, pre_point,
                                    curr_point, speed_limit, soft_speed_limit);

    if (cost < cost_cr.total_cost()) {
      cost_cr.SetTotalCost(cost);
      cost_cr.SetPrePoint(pre_col[r_pre]);
    }
  }
}

 DpStGraph::CalculateTotalCost()中是对每一行、每一列的遍历，巧的地方在于使用了GetRowRange()来计算由当前节点可到达的s（行）范围。

Status DpStGraph::CalculateTotalCost() {
  // col and row are for STGraph
  // t corresponding to col
  // s corresponding to row
  size_t next_highest_row = 0;
  size_t next_lowest_row = 0;

  for (size_t c = 0; c < cost_table_.size(); ++c) {
    size_t highest_row = 0;
    auto lowest_row = cost_table_.back().size() - 1;  //行数-1

    int count = static_cast<int>(next_highest_row) -
                static_cast<int>(next_lowest_row) + 1;
    //count始终>0
    if (count > 0) {
      std::vector<std::future<void>> results;
      for (size_t r = next_lowest_row; r <= next_highest_row; ++r) {
        auto msg = std::make_shared<StGraphMessage>(c, r);
        ...
        CalculateCostAt(msg);        
      }
      ...
    }

    for (size_t r = next_lowest_row; r <= next_highest_row; ++r) {
      const auto& cost_cr = cost_table_[c][r];
      if (cost_cr.total_cost() < std::numeric_limits<double>::infinity()) {
        size_t h_r = 0;
        size_t l_r = 0;
        GetRowRange(cost_cr, &h_r, &l_r);
        //由第c列、第 next_lowest_row~next_highest_row 行的节点可到达的最远的s
        highest_row = std::max(highest_row, h_r);
        //可到达的最近的s
        lowest_row = std::min(lowest_row, static_cast<size_t>(l_r));
      }
    }
    next_highest_row = highest_row;
    next_lowest_row = lowest_row;
  }

  return Status::OK();
}

DpStGraph::GetRowRange()中我有个疑惑的地方，就是对s的计算为什么不采用这个公式，而是使用了？

不过，考虑到 max_acceleration>0，max_deceleration<0，从计算结果来说，前者计算的 [lower_bound, upper_bound] 是后者计算结果的子集，倒是不影响程序正常逻辑。

//计算从point出发可能到达的s范围
void DpStGraph::GetRowRange(const StGraphPoint& point, size_t* next_highest_row,
                            size_t* next_lowest_row) {
  ...
  const auto max_s_size = cost_table_.back().size() - 1;  //行数-1

  const double speed_coeff = unit_t_ * unit_t_;
  //为什么不是v0*t+0.5am*t^2?
  const double delta_s_upper_bound =
      v0 * unit_t_ + vehicle_param_.max_acceleration() * speed_coeff;
  *next_highest_row =
      point.index_s() + static_cast<int>(delta_s_upper_bound / unit_s_);
  if (*next_highest_row >= max_s_size) {
    *next_highest_row = max_s_size;
  }

  const double delta_s_lower_bound = std::fmax(
      0.0, v0 * unit_t_ + vehicle_param_.max_deceleration() * speed_coeff);
  *next_lowest_row =
      point.index_s() + static_cast<int>(delta_s_lower_bound / unit_s_);
  if (*next_lowest_row > max_s_size) {
    *next_lowest_row = max_s_size;
  } else if (*next_lowest_row < 0) {
    *next_lowest_row = 0;
  }
}

CalculateCostAt()和GetRowRange()中都有基于当前点，判断可到达s（行）范围的操作。不同的是，GetRowRange()中是向s增大的方向查找，因为要逐行的扩展节点、更新其cost。而CalculateCostAt()中是向s减小的方向查找，即查找哪些行可以到达该点，因为该函数用来计算某节点的cost，就要基于之前经过的节点的cost。

DpStGraph::RetrieveSpeedProfile()以及过程中的其他类与函数，代码都很简洁清晰，就不在此赘述了。只以DpStCost::GetAccelCost()函数为例，提一下动态规划中常用的计算结果缓存技巧。具体请参考apollo\modules\planning\tasks\optimizers\dp_st_speed\dp_st_cost.h

//boundary_cost_也使用了缓存机制
std::vector<std::vector<std::pair<double, double>>> boundary_cost_;

//2个array用来缓存数据，避免多次重复计算
std::array<double, 200> accel_cost_;
std::array<double, 400> jerk_cost_;

double DpStCost::GetAccelCost(const double accel) {
  double cost = 0.0;
  constexpr double kEpsilon = 0.1;
  constexpr size_t kShift = 100;
  //将accel的数值和在accel_cost_中的index联系起来
  //根据0<=10*accel+0.5+100<=200，得出程序默认accel正常在[-10,10]范围内，符合实际
  const size_t accel_key = static_cast<size_t>(accel / kEpsilon + 0.5 + kShift);
  DCHECK_LT(accel_key, accel_cost_.size());
  //过大的加速度
  if (accel_key >= accel_cost_.size()) {
    return kInf;
  }

  //accel_cost_初始化为-1，<0即从没有计算过
  if (accel_cost_.at(accel_key) < 0.0) {
    const double accel_sq = accel * accel;
    ...
    if (accel > 0.0) {
      cost = accel_penalty * accel_sq;
    } else {
      cost = decel_penalty * accel_sq;
    }
    cost += accel_sq * decel_penalty * decel_penalty /
                (1 + std::exp(1.0 * (accel - max_dec))) +
            accel_sq * accel_penalty * accel_penalty /
                (1 + std::exp(-1.0 * (accel - max_acc)));
    accel_cost_.at(accel_key) = cost;
  } else {
    //针对这个accel的cost早就被计算过，并保存在accel_cost_内
    cost = accel_cost_.at(accel_key);
  }
  return cost * unit_t_;
}

DpStCost::JerkCost()的流程同理。这里提一个有趣的点。

double DpStCost::JerkCost(const double jerk) {
  double cost = 0.0;
  constexpr double kEpsilon = 0.1;
  constexpr size_t kShift = 200;
  //同上面accel cost的处理，认为jerk正常情况下在[-20,20]范围内
  const size_t jerk_key = static_cast<size_t>(jerk / kEpsilon + 0.5 + kShift);
  if (jerk_key >= jerk_cost_.size()) {
    return kInf;
  }
  ...
}