[Apollo Study Notes]——SPEED_HEURISTIC_OPTIMIZER of Planning Module TASK

Article directory

TASK series analysis articles
Preface
SPEED_BOUNDS_PRIORI_DECIDER function introduction
SPEED_BOUNDS_PRIORI_DECIDER related configuration
SPEED_BOUNDS_PRIORI_DECIDER process
reference

TASK series analysis articles

1. [Apollo study notes] - LANE_CHANGE_DECIDER of planning module TASK
2. [Apollo study notes] - PATH_REUSE_DECIDER of planning module TASK
3. [Apollo study notes] - PATH_BORROW_DECIDER of planning module TASK
4. [Apollo study notes] - — PATH_BOUNDS_DECIDER of planning module TASK
5. [Apollo study notes] — PIECEWISE_JERK_PATH_OPTIMIZER of planning module TASK
6. [Apollo study notes] — PATH_ASSESSMENT_DECIDER of planning module TASK
7. [Apollo study notes] — PATH_DECIDER of planning module TASK
8. [Apollo study notes]——RULE_BASED_STOP_DECIDER of planning module TASK
9. [Apollo study notes]——SPEED_BOUNDS_PRIORI_DECIDER&&SPEED_BOUNDS_FINAL_DECIDER of planning module TASK
10. [Apollo study notes]——SPEED_HEURISTIC_OPTIMIZER of planning module
TASK 11. [Apollo study notes]——Planning Module TASK SPEED_DECIDER
12.[Apollo study notes]——PIECEWISE_JERK_SPEED_OPTIMIZER of the planning module TASK
13. [Apollo study notes]——PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER of the planning module TASK (1)
14. [Apollo study notes]——PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER of the planning module TASK (2)

Preface

In the Apollo Spark Project Study Notes - Principles and Practices of Apollo Path Planning Algorithms and [ Apollo Study Notes] - Planning module talks about... The function of Stage::Process PlanOnReferenceLinewill call the TASK in the task_list in turn. This article will continue to use LaneFollow as the The examples introduce in turn what exactly the TASK part does. Due to the limitation of personal ability, the article may have some flaws, please criticize and correct me.

In modules/planning/conf/scenario/lane_follow_config.pb.txtthe configuration file, we can see all the tasks that LaneFollow needs to execute.

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

This article will continue to introduce LaneFollow’s 10th TASK——SPEED_BOUNDS_PRIORI_DECIDER

SPEED_BOUNDS_PRIORI_DECIDER function introduction

Dynamic programming planning goals

Acceleration as small as possible
Stay as far away from obstacles as possible
Meet vehicle acceleration and deceleration requirements
Meet speed limits

Generate rough velocity planning curve

Insert image description here

SPEED_BOUNDS_PRIORI_DECIDER related configuration

modules/planning/conf/planning_config.pb.txt

default_task_config: {
    
    
  task_type: SPEED_HEURISTIC_OPTIMIZER
  speed_heuristic_optimizer_config {
    
    
    default_speed_config {
    
    
      unit_t: 1.0
      dense_dimension_s: 101
      dense_unit_s: 0.1
      sparse_unit_s: 1.0
      speed_weight: 0.0
      accel_weight: 10.0
      jerk_weight: 10.0
      obstacle_weight: 1.0
      reference_weight: 0.0
      go_down_buffer: 5.0
      go_up_buffer: 5.0

      default_obstacle_cost: 1e4

      default_speed_cost: 1.0e3
      exceed_speed_penalty: 1.0e3
      low_speed_penalty: 10.0
      reference_speed_penalty: 10.0
      keep_clear_low_speed_penalty: 10.0
      accel_penalty: 1.0
      decel_penalty: 1.0

      positive_jerk_coeff: 1.0
      negative_jerk_coeff: 1.0

      max_acceleration: 2.0
      max_deceleration: -4.0
      spatial_potential_penalty: 1.0e2
    }
    lane_change_speed_config {
    
    
      unit_t: 1.0
      dense_dimension_s: 21
      dense_unit_s: 0.25
      sparse_unit_s: 1.0
      speed_weight: 0.0
      accel_weight: 10.0
      jerk_weight: 10.0
      obstacle_weight: 1.0
      reference_weight: 0.0
      go_down_buffer: 5.0
      go_up_buffer: 5.0

      default_obstacle_cost: 1e4

      default_speed_cost: 1.0e3
      exceed_speed_penalty: 1.0e3
      low_speed_penalty: 10.0
      reference_speed_penalty: 10.0
      keep_clear_low_speed_penalty: 10.0
      accel_penalty: 1.0
      decel_penalty: 1.0

      positive_jerk_coeff: 1.0
      negative_jerk_coeff: 1.0

      max_acceleration: 2.0
      max_deceleration: -2.5
      spatial_potential_penalty: 1.0e5
      is_lane_changing: true
    }
  }
}

modules/planning/proto/task_config.proto

// SpeedHeuristicOptimizerConfig

message SpeedHeuristicOptimizerConfig {
    
    
  optional DpStSpeedOptimizerConfig default_speed_config = 1;
  optional DpStSpeedOptimizerConfig lane_change_speed_config = 2;
}

message DpStSpeedOptimizerConfig {
    
    
  optional double unit_t = 1 [default = 1.0];
  optional int32 dense_dimension_s = 2 [default = 41];
  optional double dense_unit_s = 3 [default = 0.5];
  optional double sparse_unit_s = 4 [default = 1.0];

  optional double speed_weight = 10 [default = 0.0];
  optional double accel_weight = 11 [default = 10.0];
  optional double jerk_weight = 12 [default = 10.0];
  optional double obstacle_weight = 13 [default = 1.0];
  optional double reference_weight = 14 [default = 0.0];
  optional double go_down_buffer = 15 [default = 5.0];
  optional double go_up_buffer = 16 [default = 5.0];

  // obstacle cost config
  optional double default_obstacle_cost = 20 [default = 1e10];

  // speed cost config
  optional double default_speed_cost = 31 [default = 1.0];
  optional double exceed_speed_penalty = 32 [default = 10.0];
  optional double low_speed_penalty = 33 [default = 2.5];
  optional double reference_speed_penalty = 34 [default = 1.0];
  optional double keep_clear_low_speed_penalty = 35 [default = 10.0];

  // accel cost config
  optional double accel_penalty = 40 [default = 2.0];
  optional double decel_penalty = 41 [default = 2.0];

  // jerk cost config
  optional double positive_jerk_coeff = 50 [default = 1.0];
  optional double negative_jerk_coeff = 51 [default = 300.0];

  // other constraint
  optional double max_acceleration = 60 [default = 4.5];
  optional double max_deceleration = 61 [default = -4.5];

  // buffer
  optional double safe_time_buffer = 70 [default = 3.0];
  optional double safe_distance = 71 [default = 20.0];

  // spatial potential cost config for minimal time traversal
  optional double spatial_potential_penalty = 80 [default = 1.0];

  optional bool is_lane_changing = 81 [default = false];
}

SPEED_BOUNDS_PRIORI_DECIDER process

In apollo Planning, quadratic programming (PIECEWISE_JERK_SPEED_OPTIMIZER) or nonlinear programming (PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER) is used to optimize the final st curve. However, this optimization method is difficult to find the optimal solution when facing non-convex problems, so it needs to be used before the final planning. A coarse velocity program transforms a non-convex problem into a convex one. Dynamic programming algorithm is used in apollo as the transformation of non-convex to convex problems in speed planning.

Dynamic programming - a method of decomposing the original problem into relatively simple sub-problems, and then solving the original problem based on the solutions to the sub-problems. State transition equation
Insert image description here
f
$\min \{ f(R) + {w_{R \to P}}\}$
Dynamic programming algorithm has two core concepts:state and state transition equation. The state represents a situation of the problem or the solution of a sub-problem, and the state transition equation describes the transition process from one state to another, where each state is only related to the previous states. Through the state transition equation, we can obtain all possible states and thus obtain the optimal solution to the problem.

The process of speed planning based on dynamic programming is as follows:

Sample distance and time
Design state transition equation (cost calculation)
Backtrack to find the optimal ST curve

The program entry of SPEED_HEURISTIC_OPTIMIZER is in modules/planning/tasks/optimizers/path_time_heuristic/path_time_heuristic_optimizer.cc. Let’s first look at the Process part:

Insert image description here

Status PathTimeHeuristicOptimizer::Process(
    const PathData& path_data, const common::TrajectoryPoint& init_point,
    SpeedData* const speed_data) {
    
    
  init_point_ = init_point;
  // 检查
  if (path_data.discretized_path().empty()) {
    
    
    const std::string msg = "Empty path data";
    AERROR << msg;
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }
  // 搜索ST图
  if (!SearchPathTimeGraph(speed_data)) {
    
    
    const std::string msg = absl::StrCat(
        Name(), ": Failed to search graph with dynamic programming.");
    AERROR << msg;
    RecordDebugInfo(*speed_data, reference_line_info_->mutable_st_graph_data()
                                     ->mutable_st_graph_debug());
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }
  RecordDebugInfo(
      *speed_data,
      reference_line_info_->mutable_st_graph_data()->mutable_st_graph_debug());
  return Status::OK();
}

It can be seen that the input of Process is const PathData& path_data, const common::TrajectoryPoint& init_point, SpeedData* const speed_data.

At the same time, Process mainly calls SearchPathTimeGraphthe function, and then let’s look at the implementation of the function:

First, select the configuration file according to whether to change lanes, then build st_graph, the type is GriddedPathTimeGraph, and finally call Search()to search for a rough feasible route.

bool PathTimeHeuristicOptimizer::SearchPathTimeGraph(
    SpeedData* speed_data) const {
    
    
  // 根据是否换道选择配置文件
  const auto& dp_st_speed_optimizer_config =
      reference_line_info_->IsChangeLanePath()
          ? speed_heuristic_optimizer_config_.lane_change_speed_config()
          : speed_heuristic_optimizer_config_.default_speed_config();
  // 初始化GriddedPathTimeGraph类对象 st_graph
  GriddedPathTimeGraph st_graph(
      reference_line_info_->st_graph_data(), dp_st_speed_optimizer_config,
      reference_line_info_->path_decision()->obstacles().Items(), init_point_);
  // 搜索ST图
  if (!st_graph.Search(speed_data).ok()) {
    
    
    AERROR << "failed to search graph with dynamic programming.";
    return false;
  }
  return true;
}

GriddedPathTimeGraphThe class contains the following data structures:

st_graph_data_
gridded_path_time_graph_config_
obstacles_
init_point_
dp_st_cost_ // 动态规划Cost
total_length_t_
unit_t_ //采样时间
total_length_s_  
dense_unit_s_	//采样密集区域的间隔
sparse_unit_s_ //采样稀疏区域的间隔
dense_dimension_s_ //采样密集区域的点数
max_acceleration_
max_deceleration_

Next, let’s look at the implementation of the Search() part:
Insert image description here
Search first filters the boundary conditions, and then goes through four steps: initializing CostTable , initializing the speed limit query table , calculating all costs , updating CostTable , and backtracking to obtain SpeedProfile .

Status GriddedPathTimeGraph::Search(SpeedData* const speed_data) {
    
    
  static constexpr double kBounadryEpsilon = 1e-2;
  // 遍历boundary,对边界条件进行初步筛选
  for (const auto& boundary : st_graph_data_.st_boundaries()) {
    
    
    // KeepClear obstacles not considered in Dp St decision
    // 若边界类型为KEEP_CLEAR禁停区，直接跳过
    if (boundary->boundary_type() == STBoundary::BoundaryType::KEEP_CLEAR) {
    
    
      continue;
    }
    // If init point in collision with obstacle, return speed fallback
    // 若边界位于(0.0, 0.0)（即起始位置）或者边界的min_t和min_s比边界最小分辨率
    // kBounadryEpsilon还小，则车辆在[0,t]范围内s,v,a,da都为0，速度均匀递增
    if (boundary->IsPointInBoundary({
    
    0.0, 0.0}) ||
        (std::fabs(boundary->min_t()) < kBounadryEpsilon &&
         std::fabs(boundary->min_s()) < kBounadryEpsilon)) {
    
    
      // t维度上的点数
      dimension_t_ = static_cast<uint32_t>(std::ceil(
                         total_length_t_ / static_cast<double>(unit_t_))) +
                     1;
      std::vector<SpeedPoint> speed_profile;
      double t = 0.0;
      for (uint32_t i = 0; i < dimension_t_; ++i, t += unit_t_) {
    
    
        speed_profile.push_back(PointFactory::ToSpeedPoint(0, t));
      }
      *speed_data = SpeedData(speed_profile);
      return Status::OK();
    }
  }
  // 1 初始化CostTable
  if (!InitCostTable().ok()) {
    
    
    const std::string msg = "Initialize cost table failed.";
    AERROR << msg;
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }
  // 2 初始化限速查询表
  if (!InitSpeedLimitLookUp().ok()) {
    
    
    const std::string msg = "Initialize speed limit lookup table failed.";
    AERROR << msg;
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }
  // 3 计算所有的cost，并更新CostTable
  if (!CalculateTotalCost().ok()) {
    
    
    const std::string msg = "Calculate total cost failed.";
    AERROR << msg;
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }
  // 4 回溯得到SpeedProfile
  if (!RetrieveSpeedProfile(speed_data).ok()) {
    
    
    const std::string msg = "Retrieve best speed profile failed.";
    AERROR << msg;
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }
  return Status::OK();
}

1. Sampling distance and time and speed limit

The function of sampling distance and time is InitCostTableimplemented in .
Insert image description here
The speed planning is sampled in the ST map, and in Sampling is performed at fixed intervals in the direction of $t$ $Sampling is carried out in a dense and then sparse manner in the s$ direction (the closer to the main vehicle, the higher the accuracy of planning required; the further away from the main vehicle, the sampling accuracy is sacrificed and the sampling efficiency is improved)

// 时间采样的一般参数设置
unit_t: 1.0  //采样时间
dense_dimension_s: 101  // 采样密集区域的点数
dense_unit_s: 0.1   //采样密集区域的间隔
sparse_unit_s: 1.0  //采样稀疏区域的间隔

After the sampling is completed, a CostTable is formed with dimension_t_ as the column and dimension_s_ as the row. All nodes are initialized to: StGraphPoint(). Obviously StGraphPoint is the defined state variable. Let’s take a look at the data structure of StGraphPoint:

class StGraphPoint {
    
    
  STPoint point_;
  const StGraphPoint* pre_point_ = nullptr;
  std::uint32_t index_s_ = 0;
  std::uint32_t index_t_ = 0;

  double optimal_speed_ = 0.0;
  double reference_cost_ = 0.0;
  double obstacle_cost_ = 0.0;
  double spatial_potential_cost_ = 0.0;
  double total_cost_ = std::numeric_limits<double>::infinity();
};

From the definition of the code, we can see that in addition to saving the position information of the current grid point_, each status point also saves the optimized speed (optimal_speed_), reference speed cost (reference_cost_), obstacle cost (obstacle_cost_), and spatial distance. cost (spatial_potential_cost_), and the total cost of the current point (total_cost_). The design of this part of cost will be further introduced in the subsequent state transition equation.

It is worth noting that the total cost is initially set to an infinite value, which means that the position is not yet reachable at the beginning. When the point is iterated to and assigned a true value, the point becomes reachable. Therefore, all points are unreachable after cost_table is initialized.

Status GriddedPathTimeGraph::InitCostTable() {
    
    
  // Time dimension is homogeneous while Spatial dimension has two resolutions,
  // dense and sparse with dense resolution coming first in the spatial horizon

  // Sanity check for numerical stability
  if (unit_t_ < kDoubleEpsilon) {
    
    
    const std::string msg = "unit_t is smaller than the kDoubleEpsilon.";
    AERROR << msg;
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }

  // Sanity check on s dimension setting
  if (dense_dimension_s_ < 1) {
    
    
    const std::string msg = "dense_dimension_s is at least 1.";
    AERROR << msg;
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }
  // t维度上的离散点数
  dimension_t_ = static_cast<uint32_t>(std::ceil(
                     total_length_t_ / static_cast<double>(unit_t_))) +
                 1;
  // 稀疏部分s的长度(default:total_length_s_ - (101 -1) * 0.1 )
  double sparse_length_s =
      total_length_s_ -
      static_cast<double>(dense_dimension_s_ - 1) * dense_unit_s_;
  // 稀疏部分的离散点数
  sparse_dimension_s_ =
      sparse_length_s > std::numeric_limits<double>::epsilon()
          ? static_cast<uint32_t>(std::ceil(sparse_length_s / sparse_unit_s_))
          : 0;
  // 密集部分的离散点数
  dense_dimension_s_ =
      sparse_length_s > std::numeric_limits<double>::epsilon()
          ? dense_dimension_s_
          : static_cast<uint32_t>(std::ceil(total_length_s_ / dense_unit_s_)) +
                1;
  // s维度上的离散点数
  dimension_s_ = dense_dimension_s_ + sparse_dimension_s_;

  // Sanity Check
  if (dimension_t_ < 1 || dimension_s_ < 1) {
    
    
    const std::string msg = "Dp st cost table size incorrect.";
    AERROR << msg;
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }
  // 生成代价表cost_table_，列为dimension_t_，行为dimension_s_，所有节点均初始化为：StGraphPoint()
  cost_table_ = std::vector<std::vector<StGraphPoint>>(
      dimension_t_, std::vector<StGraphPoint>(dimension_s_, StGraphPoint()));

  double curr_t = 0.0;
  for (uint32_t i = 0; i < cost_table_.size(); ++i, curr_t += unit_t_) {
    
    
    auto& cost_table_i = cost_table_[i];
    double curr_s = 0.0;
    //先对dense_s 初始化
    for (uint32_t j = 0; j < dense_dimension_s_; ++j, curr_s += dense_unit_s_) {
    
    
      cost_table_i[j].Init(i, j, STPoint(curr_s, curr_t));
    }
    curr_s = static_cast<double>(dense_dimension_s_ - 1) * dense_unit_s_ +
             sparse_unit_s_;
    //再对sparse_s 初始化
    for (uint32_t j = dense_dimension_s_; j < cost_table_i.size();
         ++j, curr_s += sparse_unit_s_) {
    
    
      cost_table_i[j].Init(i, j, STPoint(curr_s, curr_t));
    }
  }
  // spatial_distance_by_index_记录每一个点的s
  const auto& cost_table_0 = cost_table_[0];
  spatial_distance_by_index_ = std::vector<double>(cost_table_0.size(), 0.0);
  for (uint32_t i = 0; i < cost_table_0.size(); ++i) {
    
    
    spatial_distance_by_index_[i] = cost_table_0[i].point().s();
  }
  return Status::OK();
}

InitSpeedLimitLookUpRegarding s, get the speed limit. Speed limits have been introduced in Speed Bounds Decider .

Status GriddedPathTimeGraph::InitSpeedLimitLookUp() {
    
    
  speed_limit_by_index_.clear();

  speed_limit_by_index_.resize(dimension_s_);
  const auto& speed_limit = st_graph_data_.speed_limit();

  for (uint32_t i = 0; i < dimension_s_; ++i) {
    
    
    speed_limit_by_index_[i] =
        speed_limit.GetSpeedLimitByS(cost_table_[0][i].point().s());
  }
  return Status::OK();
}

2. Design state transition equation (cost calculation)

Continuing to look at the code in the Search() part, you can see that the main function of CalculateTotalCost is to calculate the total cost. There are also two functions, CalculateCostAt and GetRowRange. The functions of the two will be introduced later.

The CalculateTotalCost flow chart is as follows:

Insert image description here

Status GriddedPathTimeGraph::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;


  // 注意：每一列t上所对应的s节点数目一般是不一致的。
  // 外循环，每一列的index，即每一个t
  for (size_t c = 0; c < cost_table_.size(); ++c) {
    
    
    size_t highest_row = 0;
    size_t lowest_row = cost_table_.back().size() - 1;
    // count为每一列的节点数
    int count = static_cast<int>(next_highest_row) -
                static_cast<int>(next_lowest_row) + 1;
    if (count > 0) {
    
    
      std::vector<std::future<void>> results;
      // 内循环，每行的index，即每一个s
      for (size_t r = next_lowest_row; r <= next_highest_row; ++r) {
    
    
        auto msg = std::make_shared<StGraphMessage>(c, r);
        // FLAGS_enable_multi_thread_in_dp_st_graph: Enable multiple thread to calculation curve cost in dp_st_graph
        // 是否使用多线程去计算(c, r)的最小总代价
        if (FLAGS_enable_multi_thread_in_dp_st_graph) {
    
    
          results.push_back(
              cyber::Async(&GriddedPathTimeGraph::CalculateCostAt, this, msg));
        } else {
    
    
          // 采用单线程方式计算(c, r)的最小总代价
          CalculateCostAt(msg);
        }
      }
      // 线程池方式间的同步
      if (FLAGS_enable_multi_thread_in_dp_st_graph) {
    
    
        for (auto& result : results) {
    
    
          result.get();
        }
      }
    }
    // 给定时间采样值c的情形下，
    // 更新最高行（即最大节点行数）和最低行（即最小节点行数）
    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);
        // 获取当前节点的最高行和最低行
        highest_row = std::max(highest_row, h_r);
        lowest_row = std::min(lowest_row, l_r);
      }
    }
    // 更新下一次循环的最高行（即最大节点行数）和
    // 最低行（即最小节点行数）
    next_highest_row = highest_row;
    next_lowest_row = lowest_row;
  }

  return Status::OK();
}

2.0 CalculateCostAt cost calculation

The CalculateCostAt flow chart is as follows:

Insert image description here
CalculateCostAt mainly calculates the cost of the node. It performs special calculations on the first, second and third columns. After calculation, the cost value is updated.

Cost mainly consists of four parts:

cost of obstacles
distance cost
The cost of the previous node/starting point
State transfer cost

The specific cost calculation will be introduced later.

void GriddedPathTimeGraph::CalculateCostAt(
    const std::shared_ptr<StGraphMessage>& msg) {
    
    

  const uint32_t c = msg->c;
  const uint32_t r = msg->r;
  auto& cost_cr = cost_table_[c][r];
  // 获取并设置当前obstacle_cost
  cost_cr.SetObstacleCost(dp_st_cost_.GetObstacleCost(cost_cr));
  // 当前点的障碍物代价无穷大，直接返回
  if (cost_cr.obstacle_cost() > std::numeric_limits<double>::max()) {
    
    
    return;
  }
  // 获取并设置当前SpatialPotentialCost(距离cost)
  cost_cr.SetSpatialPotentialCost(dp_st_cost_.GetSpatialPotentialCost(cost_cr));
  // 起始点cost为0，速度为起始速度
  const auto& cost_init = cost_table_[0][0];
  if (c == 0) {
    
    
    DCHECK_EQ(r, 0U) << "Incorrect. Row should be 0 with col = 0. row: " << r;
    cost_cr.SetTotalCost(0.0);
    cost_cr.SetOptimalSpeed(init_point_.v());
    return;
  }

  const double speed_limit = speed_limit_by_index_[r];
  const double cruise_speed = st_graph_data_.cruise_speed();
  // The mininal s to model as constant acceleration formula
  // default: 0.25 * 7 = 1.75 m
  const double min_s_consider_speed = dense_unit_s_ * dimension_t_;
  //第2列的特殊处理
  if (c == 1) {
    
    
    // v1 = v0 + a * t, v1^2 - v0^2 = 2 * a * s => a = 2 * (s/t - v)/t
    const double acc =
        2 * (cost_cr.point().s() / unit_t_ - init_point_.v()) / unit_t_;
    // 加速度或减速度超出范围，返回
    if (acc < max_deceleration_ || acc > max_acceleration_) {
    
    
      return;
    }
    // 若v1小于0，但s却大于min_s_consider_speed，倒车，返回
    if (init_point_.v() + acc * unit_t_ < -kDoubleEpsilon &&
        cost_cr.point().s() > min_s_consider_speed) {
    
    
      return;
    }
    // 若与起始点的连线与障碍物发生重合,返回。
    if (CheckOverlapOnDpStGraph(st_graph_data_.st_boundaries(), cost_cr,
                                cost_init)) {
    
    
      return;
    }
    // 计算当前点的total_cost
    cost_cr.SetTotalCost(
        cost_cr.obstacle_cost() + cost_cr.spatial_potential_cost() +
        cost_init.total_cost() +
        CalculateEdgeCostForSecondCol(r, speed_limit, cruise_speed));
    cost_cr.SetPrePoint(cost_init); //前一个点为初始点
    cost_cr.SetOptimalSpeed(init_point_.v() + acc * unit_t_);
    return;
  }

  static constexpr double kSpeedRangeBuffer = 0.20;
  // 估算前一点最小的s
  const double pre_lowest_s =
      cost_cr.point().s() -
      FLAGS_planning_upper_speed_limit * (1 + kSpeedRangeBuffer) * unit_t_;
  // 找到第一个不小于pre_lowest_s的元素
  const auto pre_lowest_itr =
      std::lower_bound(spatial_distance_by_index_.begin(),
                       spatial_distance_by_index_.end(), pre_lowest_s);
  uint32_t r_low = 0;
  // 找到r_low
  if (pre_lowest_itr == spatial_distance_by_index_.end()) {
    
    
    r_low = dimension_s_ - 1;
  } else {
    
    
    r_low = static_cast<uint32_t>(
        std::distance(spatial_distance_by_index_.begin(), pre_lowest_itr));
  }
  // 由当前点推出能到达该点的前一列最小的s
  // 将当前点的pre_col缩小至 [r_low, r]
  const uint32_t r_pre_size = r - r_low + 1;
  const auto& pre_col = cost_table_[c - 1];
  double curr_speed_limit = speed_limit;
  // 第3列的特殊处理
  if (c == 2) {
    
    
    // 对于前一列，遍历从r->r_low的点， 
    // 依据重新算得的cost，当前点的pre_point，也就是DP过程的状态转移方程
    for (uint32_t i = 0; i < r_pre_size; ++i) {
    
    
      uint32_t r_pre = r - i;
      // 跳过无穷大和为空的点
      if (std::isinf(pre_col[r_pre].total_cost()) ||
          pre_col[r_pre].pre_point() == nullptr) {
    
    
        continue;
      }
      // TODO(Jiaxuan): Calculate accurate acceleration by recording speed
      // data in ST point.
      // Use curr_v = (point.s - pre_point.s) / unit_t as current v
      // Use pre_v = (pre_point.s - prepre_point.s) / unit_t as previous v
      // Current acc estimate: curr_a = (curr_v - pre_v) / unit_t
      // = (point.s + prepre_point.s - 2 * pre_point.s) / (unit_t * unit_t)
      // 计算加速度
      const double curr_a =
          2 *
          ((cost_cr.point().s() - pre_col[r_pre].point().s()) / unit_t_ -
           pre_col[r_pre].GetOptimalSpeed()) /
          unit_t_;
      // 加速度超出范围，跳过
      if (curr_a < max_deceleration_ || curr_a > max_acceleration_) {
    
    
        continue;
      }
      // 不考虑倒车，跳过
      if (pre_col[r_pre].GetOptimalSpeed() + curr_a * unit_t_ <
              -kDoubleEpsilon &&
          cost_cr.point().s() > min_s_consider_speed) {
    
    
        continue;
      }

      // Filter out continuous-time node connection which is in collision with
      // obstacle
      // 检查连线是否会发生碰撞
      if (CheckOverlapOnDpStGraph(st_graph_data_.st_boundaries(), cost_cr,
                                  pre_col[r_pre])) {
    
    
        continue;
      }
      curr_speed_limit =
          std::fmin(curr_speed_limit, speed_limit_by_index_[r_pre]);
      const double cost = cost_cr.obstacle_cost() +
                          cost_cr.spatial_potential_cost() +
                          pre_col[r_pre].total_cost() +
                          CalculateEdgeCostForThirdCol(
                              r, r_pre, curr_speed_limit, cruise_speed);
      // 若新代价值比节点（c, r）的原有代价值更小，则更新当前节点（c, r)的总代价值
      if (cost < cost_cr.total_cost()) {
    
    
        cost_cr.SetTotalCost(cost);
        cost_cr.SetPrePoint(pre_col[r_pre]);
        cost_cr.SetOptimalSpeed(pre_col[r_pre].GetOptimalSpeed() +
                                curr_a * unit_t_);
      }
    }
    return;
  }
  // 其他列的处理
  // 依据重新算得的cost，当前点的pre_point，也就是DP过程的状态转移方程
  for (uint32_t i = 0; i < r_pre_size; ++i) {
    
    
    uint32_t r_pre = r - i;
    // 对于前一列，遍历从r->r_low的点， 
    if (std::isinf(pre_col[r_pre].total_cost()) ||
        pre_col[r_pre].pre_point() == nullptr) {
    
    
      continue;
    }
    // Use curr_v = (point.s - pre_point.s) / unit_t as current v
    // Use pre_v = (pre_point.s - prepre_point.s) / unit_t as previous v
    // Current acc estimate: curr_a = (curr_v - pre_v) / unit_t
    // = (point.s + prepre_point.s - 2 * pre_point.s) / (unit_t * unit_t)
    const double curr_a =
        2 *
        ((cost_cr.point().s() - pre_col[r_pre].point().s()) / unit_t_ -
         pre_col[r_pre].GetOptimalSpeed()) /
        unit_t_;
    if (curr_a > max_acceleration_ || curr_a < max_deceleration_) {
    
    
      continue;
    }

    if (pre_col[r_pre].GetOptimalSpeed() + curr_a * unit_t_ < -kDoubleEpsilon &&
        cost_cr.point().s() > min_s_consider_speed) {
    
    
      continue;
    }

    if (CheckOverlapOnDpStGraph(st_graph_data_.st_boundaries(), cost_cr,
                                pre_col[r_pre])) {
    
    
      continue;
    }
    // 前前一点的row
    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();
    curr_speed_limit =
        std::fmin(curr_speed_limit, speed_limit_by_index_[r_pre]);
    double cost = cost_cr.obstacle_cost() + cost_cr.spatial_potential_cost() +
                  pre_col[r_pre].total_cost() +
                  CalculateEdgeCost(triple_pre_point, prepre_point, pre_point,
                                    curr_point, curr_speed_limit, cruise_speed);

    if (cost < cost_cr.total_cost()) {
    
    
      cost_cr.SetTotalCost(cost);
      cost_cr.SetPrePoint(pre_col[r_pre]);
      cost_cr.SetOptimalSpeed(pre_col[r_pre].GetOptimalSpeed() +
                              curr_a * unit_t_);
    }
  }
}

2.1 GetObstacleCost obstacle cost calculation

Insert image description here
S_safe_overtakeThe safe distance for overtaking
S_safe_followand the safe distance for following

Traverse the ST boundary of each obstacle. When point j happens to be within the obstacle boundary at time i, it means that it will collide with the obstacle, and the cost is infinite.
When s at j position is above the upper boundary of the obstacle and there is a safe overtaking distance s_safe_overtake, the cost is 0
When j is below the lower boundary of the obstacle and there is sufficient safety distance s_safe_follow, the cost is 0
In other cases, it is the square of the difference from the safety distance multiplied by the obstacle weight.

So we can get:
$C_{obs}(i,j)=\left\{ {\begin{array}{ccccccccccccccc} { inf， }&s_{obs \cdot lower}(i)\leq s(j)\leq s_{obs \cdot upper}(i)\\0，&s(j)\geq s_{obs \cdot upper}(i)+s_{safe.overtake}\\0，&s(j)\leq s_{obs.lower}(i)-s_{safe.follow}\\w_{obs}[s(j)-(s_{obs.lower}(i)-s_{safe.follow})]^2&s_{obs.lower}(i)-s_{safe.follow}\leq s(j)\leq s_{obs.lower}(i)\\w_{obs}[(s_{obs.upper}(i)+s_{safe.upper})-s(j)]^2&s_{obs.upper}(i)\leq s(j)\leq s_{obs.upper}(i)+s_{safe.overtake} \end{array}}\right\}$

i is the index in the t direction, j is the index in the s direction

double DpStCost::GetObstacleCost(const StGraphPoint& st_graph_point) {
    
    
  const double s = st_graph_point.point().s();
  const double t = st_graph_point.point().t();

  double cost = 0.0;

  if (FLAGS_use_st_drivable_boundary) {
    
    
    // TODO(Jiancheng): move to configs
    static constexpr double boundary_resolution = 0.1;
    int index = static_cast<int>(t / boundary_resolution);
    const double lower_bound =
        st_drivable_boundary_.st_boundary(index).s_lower();
    const double upper_bound =
        st_drivable_boundary_.st_boundary(index).s_upper();
    // 在障碍物之内，无穷大
    if (s > upper_bound || s < lower_bound) {
    
    
      return kInf;
    }
  }
  // 遍历障碍物
  for (const auto* obstacle : obstacles_) {
    
    
    // Not applying obstacle approaching cost to virtual obstacle like created
    // stop fences
    // 跳过虚拟障碍物
    if (obstacle->IsVirtual()) {
    
    
      continue;
    }

    // Stop obstacles are assumed to have a safety margin when mapping them out,
    // so repelling force in dp st is not needed as it is designed to have adc
    // stop right at the stop distance we design in prior mapping process
    // 拥有STOP决策的障碍物已经留有安全空间（Speedboundsdecider），跳过
    if (obstacle->LongitudinalDecision().has_stop()) {
    
    
      continue;
    }

    auto boundary = obstacle->path_st_boundary();
    // FLAGS_speed_lon_decision_horizon: Longitudinal horizon for speed decision making (meter)  200.0
    // 跳过距离太远的障碍物
    if (boundary.min_s() > FLAGS_speed_lon_decision_horizon) {
    
    
      continue;
    }
    // 跳过不在时间范围内的障碍物
    if (t < boundary.min_t() || t > boundary.max_t()) {
    
    
      continue;
    }
    // 若与障碍物发生碰撞，cost为无穷大
    if (boundary.IsPointInBoundary(st_graph_point.point())) {
    
    
      return kInf;
    }
    double s_upper = 0.0;
    double s_lower = 0.0;
    // 获取障碍物的s_upper和s_lower
    int boundary_index = boundary_map_[boundary.id()];
    // 障碍物逆向行驶?
    if (boundary_cost_[boundary_index][st_graph_point.index_t()].first < 0.0) {
    
    
      boundary.GetBoundarySRange(t, &s_upper, &s_lower);
      boundary_cost_[boundary_index][st_graph_point.index_t()] =
          std::make_pair(s_upper, s_lower);
    } else {
    
    
      s_upper = boundary_cost_[boundary_index][st_graph_point.index_t()].first;
      s_lower = boundary_cost_[boundary_index][st_graph_point.index_t()].second;
    }
    if (s < s_lower) {
    
    
      const double follow_distance_s = config_.safe_distance(); // safe_distance: 20.0
      // 在障碍物下边界以下，且保留有足够跟车安全距离follow_distance_s，cost为0
      if (s + follow_distance_s < s_lower) {
    
    
        continue;
      } else {
    
    
        auto s_diff = follow_distance_s - s_lower + s;
        // obstacle_weight: 1.0, default_obstacle_cost: 1e4
        // 用距离差值的平方进行计算
        cost += config_.obstacle_weight() * config_.default_obstacle_cost() *
                s_diff * s_diff;
      }
    } else if (s > s_upper) {
    
    
      const double overtake_distance_s =
          StGapEstimator::EstimateSafeOvertakingGap();
      // 在障碍物上边界以上，且保留有安全超车的距离overtake_distance_s，cost为0
      if (s > s_upper + overtake_distance_s) {
    
      // or calculated from velocity
        continue;
      } else {
    
    
        auto s_diff = overtake_distance_s + s_upper - s;
        cost += config_.obstacle_weight() * config_.default_obstacle_cost() *
                s_diff * s_diff;
      }
    }
  }
  // 意味着直接continue的情况就是cost = 0
  return cost * unit_t_;
}

2.2 GetSpatialPotentialCost distance cost calculation

The purpose is to reach the destination faster.
Insert image description here
The distance cost is calculated as follows

${C_{spatial}} = {w_{spatial}}({s_{total}} - s(j))$ ${w_{spatial}}$ is the loss weight
${s_{total}} - s(j))$ The difference between the current point and the target point.

double DpStCost::GetSpatialPotentialCost(const StGraphPoint& point) {
    
    
  return (total_s_ - point.point().s()) * config_.spatial_potential_penalty(); // spatial_potential_penalty: 1.0e2
}

2.3 State transfer cost

This part mainly involves three functions: CalculateEdgeCost, , CalculateEdgeCostForSecondColand CalculateEdgeCostForThirdCol. The latter two are special cases, so look at CalculateEdgeCost first.

2.3.1 CalculateEdgeCost

double GriddedPathTimeGraph::CalculateEdgeCost(
    const STPoint& first, const STPoint& second, const STPoint& third,
    const STPoint& forth, const double speed_limit, const double cruise_speed) {
    
    
  return dp_st_cost_.GetSpeedCost(third, forth, speed_limit, cruise_speed) +
         dp_st_cost_.GetAccelCostByThreePoints(second, third, forth) +
         dp_st_cost_.GetJerkCostByFourPoints(first, second, third, forth);
}

The state transfer cost calculation is divided into three parts:
${C_{edge}} = {C_{speed}} + {C_{acc}} + {C_{jerk}}$ ${C_{speed}}$ ——Speed cost
${C_{acc}}$ ——Acceleration cost
${C_{jerk}}$ ——Jerk acceleration cost

2.3.1.1 ${C_{speed}}$ ——Speed cost

The speed between nodes is: $\frac{ {s(j ) - s(j-1)}}{ {\Delta t}}$
限速比率： ${v_{det }} = \frac{ {v - {v_{limit}}}}{ { {v_{limit}}}}$
Cruise speed difference: $v_{diff} = v-v_{cruise}$

${C_{speed}}$ 速度代价的计算如下：
$\left.C_{speed}\right.=\left\{\begin{array}{cc}inf&v<0\\w_{KeepClear}w_{v-{default}}\Delta t& v<v_{max.adc.stop},InKeepClearRange(s(j))\\w_{v-exceed}w_{v-{default}}({v_{det }})^2\Delta t&v>0,v_{det}>0\\-w_{v-low}w_{v-{default}}v_{det}\Delta t&v>0,v_{det}<0\\w_{v-{ref}}w_{v-{default}}|v_{diff} |\Delta t&\text{enable dp reference speed}\end{array}\right.$
If the speed is <0, it is a reversing situation, the trajectory is not feasible, and the cost value is set to infinity; if the speed is low and the current position is in a no-stop zone, it needs to pass quickly; if the speed is >0 and it is higher than the speed limit, then There will be a penalty for overspeeding; if the speed is <0 and lower than the speed limit, there will be a penalty for low speed; if enabled FLAGS_enable_dp_reference_speed, the speed will be close to the cruising speed. In Apollo, the penalty value for speeding (1000) is much greater than the penalty value for slow speed (10).

double DpStCost::GetSpeedCost(const STPoint& first, const STPoint& second,
                              const double speed_limit,
                              const double cruise_speed) const {
    
    
  double cost = 0.0;
  const double speed = (second.s() - first.s()) / unit_t_;
  // 倒车的状况，轨迹不可行，代价值设为无穷大
  if (speed < 0) {
    
    
    return kInf;
  }
  // max_abs_speed_when_stopped: 0.2
  const double max_adc_stop_speed = common::VehicleConfigHelper::Instance()
                                        ->GetConfig()
                                        .vehicle_param()
                                        .max_abs_speed_when_stopped();
  // 禁停区快速驶过
  if (speed < max_adc_stop_speed && InKeepClearRange(second.s())) {
    
    
    // first.s in range
    // keep_clear_low_speed_penalty: 10.0 ; default_speed_cost: 1.0e3
    cost += config_.keep_clear_low_speed_penalty() * unit_t_ *
            config_.default_speed_cost();
  }

  double det_speed = (speed - speed_limit) / speed_limit;
  if (det_speed > 0) {
    
    
    // exceed_speed_penalty: 1.0e3
    cost += config_.exceed_speed_penalty() * config_.default_speed_cost() *
            (det_speed * det_speed) * unit_t_;
  } else if (det_speed < 0) {
    
    
    // low_speed_penalty: 10.0
    cost += config_.low_speed_penalty() * config_.default_speed_cost() *
            -det_speed * unit_t_;
  }
  // True to penalize dp result towards default cruise speed
  // reference_speed_penalty: 10.0
  if (FLAGS_enable_dp_reference_speed) {
    
    
    double diff_speed = speed - cruise_speed;
    cost += config_.reference_speed_penalty() * config_.default_speed_cost() *
            fabs(diff_speed) * unit_t_;
  }

  return cost;
}

2.3.1.2 ${C_{acc}}$ ——Acceleration cost

The acceleration is calculated as follows:
$=\frac {s_3+s_1-2s_2} { {\Delta t}^2}$
${C_{acc}}$ The acceleration cost is calculated as follows:

$cost_a=\left\{\begin{array}{}inf&a>a_{max}|a<a_{demax}\\w_{acc}a^2++\frac{w_{deacc}^2a^2}{1+e^{a-a_{demax}}}+\frac{w_{deacc}^2a^2}{1+e^{-a+a_{max}}}&0<a<a_{max}\\w_{deacc}a^2+\frac{w_{deacc}^2a^2}{1+e^{a-a_{demax}}}+\frac{w_{deacc}^2a^2}{1+e^{-a+a_{max}}}&a_{min}<a<0\end{array}\right.$

If it exceeds the maximum acceleration or is less than the minimum acceleration, the cost value is set to infinity. If it is between, Apollo designed such a cost function for calculation: ${x^2} + \frac{ { {x^2}}}{ {1 + {e^{x + a}}}} + \frac{ { {x^2}}}{ {1 + {e^{x + b}}}}$
The function image is as follows: Insert image description here
the closer to 0, the smaller the cost value; the closer to the target value, the larger the cost value, which satisfies comfort and smoothness.

2.3.1.3 ${C_{jerk}}$ ——Jerk acceleration cost

Jerk is calculated as follows: $\frac{ { { s_4} - 3{s_3} + 3{s_2} - {s_1}}} { {\Delta {t^3}}}$

$cost_{jerk}=\left\{\begin{array}{}inf&jerk>jerk_{max },jerk<jerk_{min}\\w_{positive,jerk}jerk^2\Delta jerk<jerk_{max}\\w_{negative,jerk}jerk^2\Delta jerk_{min}<jerk<0 \ end { array } \ right$

If the jerk exceeds the set boundary, it is set to infinity; if it is between, it is calculated in a quadratic manner. The smaller the jerk, the better.

Next are the calculations for two special cases:

2.3.2 CalculateEdgeCostForSecondCol

2.3.2.1 ${C_{speed}}$ ——Speed cost

This part is consistent.

2.3.2.2 ${C_{acc}}$ ——Acceleration cost

Use the speed of the starting point $v_{pre}$ , current point and previous point (starting point) for calculation.
The speed of the current point $v_{cur}=\frac {s_{cur}-s_{pre}} {\Delta t}$
加速度 $a=\frac {v_{cur}-v_{pre}} {\Delta t}$
The rest is consistent.

2.3.2.3 ${C_{jerk}}$ ——Jerk acceleration cost

Use the speed of the starting point $v_{pre}$ , the acceleration of the starting point $a_{pre}$ , current point and previous point (starting point) for calculation.
The speed of the current point $v_{cur}=\frac {s_{cur}-s_{pre}} {\Delta t}$
Acceleration of the current point $a_{cur}=\frac {v_{cur}-v_{pre}} {\Delta t}$
加加速度 $jerk=\frac {a_{cur}-a_{pre}} {\Delta t}$
The rest is consistent.

2.3.3 CalculateEdgeCostForThirdCol

2.3.3.1 ${C_{speed}}$ ——Speed cost

This part is consistent.

2.3.3.2 ${C_{acc}}$ ——Acceleration cost

This part is consistent.

2.3.3.3 ${C_{jerk}}$ ——Jerk acceleration cost

Using the speed $v_1 of the starting point$ , current point 3, previous point 2, and previous two points (starting point) 1 for calculation.
The speed of the previous point $v_2=\frac {s_2-s_1} {\Delta t}$
Acceleration of the previous point $a_2=\frac {v_2-v_1} {\Delta t}$
Current point speed $v_3=\frac {s_3-s_2} {\Delta t}$
Current point acceleration $a_3=\frac {v_3-v_2} {\Delta t}$
Jerk $jerk=\frac {a_{3}-a_{2}} {\Delta t}$
The rest is consistent.

2.4 GetRowRange

GetRowRange uses the maximum acceleration/deceleration to calculate the range of the next s (number of rows)

void GriddedPathTimeGraph::GetRowRange(const StGraphPoint& point,
                                       size_t* next_highest_row,
                                       size_t* next_lowest_row) {
    
    
  double v0 = 0.0;
  // TODO(all): Record speed information in StGraphPoint and deprecate this.
  // A scaling parameter for DP range search due to the lack of accurate
  // information of the current velocity (set to 1 by default since we use
  // past 1 second's average v as approximation)
  double acc_coeff = 0.5;
  if (!point.pre_point()) {
    
    
    // 没有前继节点，用起始点速度代替
    v0 = init_point_.v();
  } else {
    
    
    // 获取当前点的速度
    v0 = point.GetOptimalSpeed();
  }

  const auto max_s_size = dimension_s_ - 1;
  const double t_squared = unit_t_ * unit_t_;
  // 用最大匀加速推算s的上限
  const double s_upper_bound = v0 * unit_t_ +
                               acc_coeff * max_acceleration_ * t_squared +
                               point.point().s();
  const auto next_highest_itr =
      std::lower_bound(spatial_distance_by_index_.begin(),
                       spatial_distance_by_index_.end(), s_upper_bound);
  if (next_highest_itr == spatial_distance_by_index_.end()) {
    
    
    *next_highest_row = max_s_size;
  } else {
    
    
    *next_highest_row =
        std::distance(spatial_distance_by_index_.begin(), next_highest_itr);
  }
  // 用最大匀减速推算s的下限
  const double s_lower_bound =
      std::fmax(0.0, v0 * unit_t_ + acc_coeff * max_deceleration_ * t_squared) +
      point.point().s();
  const auto next_lowest_itr =
      std::lower_bound(spatial_distance_by_index_.begin(),
                       spatial_distance_by_index_.end(), s_lower_bound);
  if (next_lowest_itr == spatial_distance_by_index_.end()) {
    
    
    *next_lowest_row = max_s_size;
  } else {
    
    
    *next_lowest_row =
        std::distance(spatial_distance_by_index_.begin(), next_lowest_itr);
  }
}

3. Backtrack to find the optimal ST curve

Need to pay attention to the selection of the starting point for backtracking

Traverse the last point of each column, find the current best_end_point, and update min_cost
The min_cost point of the last column is not directly used as the end point here.

Because the sampling time is an estimated time window, the end point may be reached before this time.
Insert image description here

Status GriddedPathTimeGraph::RetrieveSpeedProfile(SpeedData* const speed_data) {
    
    
  // 初始化最小代价值min_cost
  double min_cost = std::numeric_limits<double>::infinity();
  // 初始化最优终点(即包含最小代价值的节点)
  const StGraphPoint* best_end_point = nullptr;
  // 从最后一列找到min_cost
  for (const StGraphPoint& cur_point : cost_table_.back()) {
    
    
    if (!std::isinf(cur_point.total_cost()) &&
        cur_point.total_cost() < min_cost) {
    
    
      best_end_point = &cur_point;
      min_cost = cur_point.total_cost();
    }
  }
  // 遍历每一列的最后一个点，找正在的best_end_point，并更新min_cost
  // 这里不直接使用最后一列的min_cost点作为终点
  // 因为采样时间是一个预估时间窗口，在这之前可能就到达终点了
  for (const auto& row : cost_table_) {
    
    
    const StGraphPoint& cur_point = row.back();
    if (!std::isinf(cur_point.total_cost()) &&
        cur_point.total_cost() < min_cost) {
    
    
      best_end_point = &cur_point;
      min_cost = cur_point.total_cost();
    }
  }
  // 寻找最优失败
  if (best_end_point == nullptr) {
    
    
    const std::string msg = "Fail to find the best feasible trajectory.";
    AERROR << msg;
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }
  // 回溯，得到最优的 speed_profile
  std::vector<SpeedPoint> speed_profile;
  const StGraphPoint* cur_point = best_end_point;
  while (cur_point != nullptr) {
    
    
    ADEBUG << "Time: " << cur_point->point().t();
    ADEBUG << "S: " << cur_point->point().s();
    ADEBUG << "V: " << cur_point->GetOptimalSpeed();
    SpeedPoint speed_point;
    speed_point.set_s(cur_point->point().s());
    speed_point.set_t(cur_point->point().t());
    speed_profile.push_back(speed_point);
    cur_point = cur_point->pre_point();
  }
  // 倒序
  std::reverse(speed_profile.begin(), speed_profile.end());

  static constexpr double kEpsilon = std::numeric_limits<double>::epsilon();
  if (speed_profile.front().t() > kEpsilon ||
      speed_profile.front().s() > kEpsilon) {
    
    
    const std::string msg = "Fail to retrieve speed profile.";
    AERROR << msg;
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }
  // 计算速度
  for (size_t i = 0; i + 1 < speed_profile.size(); ++i) {
    
    
    const double v = (speed_profile[i + 1].s() - speed_profile[i].s()) /
                     (speed_profile[i + 1].t() - speed_profile[i].t() + 1e-3);
    speed_profile[i].set_v(v);
  }

  *speed_data = SpeedData(speed_profile);
  return Status::OK();
}

The final output is speed_data.

reference

[1] Planning Speed planning based on dynamic programming
[2] Apollo 6.0's EM Planner (3): Dynamic planning DP process of speed planning
[3] Apollo Spark Plan study notes - Apollo speed planning algorithm principle and practice
[4] Dynamic Planning and its application in the Apollo project Planning module
[5] Apollo planning control study notes