[Apollo Study Notes]——計画モジュール TASK の SPEED_HEURISTIC_OPTIMIZER

記事ディレクトリ

TASKシリーズの分析記事
序文
SPEED_BOUNDS_PRIORI_DECIDER 関数の概要
SPEED_BOUNDS_PRIORI_DECIDER 関連の構成
SPEED_BOUNDS_PRIORI_DECIDER プロセス
参考

TASKシリーズの分析記事

1. [Apollo 学習ノート]—— 計画モジュール TASK の LANE_CHANGE_DECIDER
2. [Apollo 学習ノート]—— 計画モジュール TASK の PATH_REUSE_DECIDER
3. [Apollo 学習ノート]—— 計画モジュール TASK の PATH_BORROW_DECIDER
4. [Apollo 学習ノート]— — 計画モジュール TASK
5. [Apollo の学習ノート] の PATH_BOUNDS_DECIDER — 計画モジュール TASK
6. [Apollo の学習ノート] の PATH_ASSESSMENT_DECIDER — 計画モジュール TASK
7. [Apollo の学習ノート] の PATH_DECIDER — 計画モジュール TASK
8. [Apollo の学習ノート] の PATH_DECIDERノート] - ルール_based_stop_decider計画モジュールモジュールタスク9. [Apollo Study Notes] ——— speed_bounds_priori_decider && speed_bounds_final_decider計画モジュールタスク10.
[ Apollo Study Notes]
。12.

[Apollo 学習ノート]——計画モジュール TASK の PIECEWISE_JERK_SPEED_OPTIMIZER
13. [Apollo 学習ノート]—計画モジュール TASK (1) の PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER
14. [Apollo 学習ノート]—計画モジュール TASK (2) の PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER

序文

Apollo Spark プロジェクトの学習ノート - Apollo パス計画アルゴリズムの原理と実践および [ Apollo Study Notes] - 計画モジュールでは、Stage::Process の関数がPlanOnReferenceLinetask_list 内の TASK を順番に呼び出します。例では、TASK 部分が正確に何を行うかを順番に紹介します。個人の能力の限界により、記事にはいくつかの不備があるかもしれません、批判と修正をお願いします。

構成ファイルではmodules/planning/conf/scenario/lane_follow_config.pb.txt、LaneFollow が実行する必要があるすべてのタスクを確認できます。

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の10番目のTASKを紹介していきます——SPEED_BOUNDS_PRIORI_DECIDER

SPEED_BOUNDS_PRIORI_DECIDER 関数の概要

動的プログラミング計画の目標

加速度はできるだけ小さくする
障害物からできるだけ離れてください
車両の加速および減速要件を満たします
制限速度を守る

大まかな速度計画曲線を生成する

ここに画像の説明を挿入します

SPEED_BOUNDS_PRIORI_DECIDER 関連の構成

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 プロセス

アポロ計画では、二次計画法 (PIECEWISE_JERK_SPEED_OPTIMIZER) または非線形計画法 (PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER) を使用して最終的な st 曲線を最適化しますが、この最適化手法は非凸問題に直面した場合に最適解を見つけるのが難しいため、事前に使用する必要があります。粗い速度プログラムは、非凸問題を凸問題に変換します。動的計画アルゴリズムは、速度計画における非凸問題から凸問題への変換として apollo で使用されます。

動的計画法- 元の問題を比較的単純な部分問題に分解し、その部分問題の解に基づいて元の問題を解く方法状態遷移方程式
ここに画像の説明を挿入します
f
$\min \{ f(R) + {w_{R \to P}}\}$
プログラミングアルゴリズムには、状態と状態遷移方程式という。状態は、問題の状況または部分問題の解決策を表し、状態遷移方程式は、ある状態から別の状態への遷移プロセスを記述します。各状態は前の状態とのみ関連します。状態遷移方程式を通じて、すべての可能な状態を取得し、問題の最適解を得ることができます。

動的計画法に基づく速度計画のプロセスは次のとおりです。

サンプルの距離と時間
設計状態遷移式（コスト計算）
バックトラックして最適な ST カーブを見つけます

SPEED_HEURISTIC_OPTIMIZER のプログラムエントリはにありますmodules/planning/tasks/optimizers/path_time_heuristic/path_time_heuristic_optimizer.cc。最初にプロセス部分を見てみましょう。

ここに画像の説明を挿入します

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

const PathData& path_dataProcess の入力が, const common::TrajectoryPoint& init_point,であることがわかりますSpeedData* const speed_data。

同時に、プロセスは主にSearchPathTimeGraph関数を呼び出します。次に、関数の実装を見てみましょう。

まず、車線変更するかどうかに応じて設定ファイルを選択し、次に st_graph をビルドします。タイプはですGriddedPathTimeGraph。最後に呼び出して、Search()大まかな実行可能なルートを検索します。

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

GriddedPathTimeGraphクラスには次のデータ構造が含まれています。

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_

次に、 Search() 部分の実装を見てみましょう。
ここに画像の説明を挿入します
Search は最初に境界条件をフィルタリングし、次に4 つのステップを実行します。CostTable の初期化、速度制限クエリテーブルの初期化、すべてのコストの計算、CostTable の更新、および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. サンプリング距離と時間と制限速度

サンプリング距離と時間の関数はInitCostTableに実装されています。
ここに画像の説明を挿入します
速度計画は ST マップと $のt$ 方向に一定の間隔で実行されます。 $サンプリングはs$ 方向に密に、次に疎に行われます（主車両に近づくほど、より高い計画精度が要求されます。主車両から遠ざかるほど、サンプリング精度が犠牲になり、サンプリング効率が向上します））

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

サンプリングが完了すると、列として次元_t_、行として次元_s_を使用して CostTable が形成され、すべてのノードが StGraphPoint() に初期化されます。明らかに、StGraphPoint は定義された状態変数です。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();
};

コードの定義から、現在のグリッドポイント_の位置情報に加えて、各ステータスポイントは最適化された速度 (optimal_speed_)、基準速度コスト (reference_cost_)、障害物コスト (obstacle_cost_) も保存することがわかります。空間距離、コスト (spatial_potential_cost_)、および現在の点の総コスト (total_cost_)。コストのこの部分の設計は、その後の状態遷移方程式でさらに導入されます。

総コストが最初に無限値に設定されていることに注意してください。これは、開始時点ではその位置にまだ到達できないことを意味しますが、ポイントが反復されて真の値が割り当てられると、そのポイントは到達可能になります。したがって、cost_table が初期化された後は、すべてのポイントに到達できなくなります。

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

InitSpeedLimitLookUpsについては、制限速度を取得します。速度制限は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. 設計状態遷移式（コスト計算）

引き続き Search() 部分のコードを見てみると、CalculateTotalCost の主な関数は総コストを計算することであることがわかります。他に、CalculateCostAt と GetRowRange という 2 つの関数もあります。この 2 つの関数については後ほど紹介します。

CalculateTotalCost のフローチャートは次のとおりです。

ここに画像の説明を挿入します

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 CalculateCost原価計算

CalculateCostAt のフローチャートは次のとおりです。

ここに画像の説明を挿入します
CalculateCostAt は主にノードのコストを計算します。1 列目、2 列目、3 列目に対して特別な計算を実行します。計算後、コスト値が更新されます。

コストは主に次の 4 つの部分で構成されます。

障害物のコスト
距離コスト
前のノード/開始点のコスト
州移転コスト

具体的なコスト計算については後ほど紹介します。

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 障害物コストの計算

ここに画像の説明を挿入します
S_safe_overtake追い越しの安全距離
S_safe_followと追従の安全距離

各障害物の ST 境界を通過し、時刻 i で点 j がたまたま障害物の境界内にある場合、障害物と衝突することを意味し、コストは無限大です。
j 位置の s が障害物の上部境界より上にあり、安全な追い越し距離 s_safe_overtake がある場合、コストは 0 です。
j が障害物の下限より下にあり、十分な安全距離 s_safe_follow がある場合、コストは 0 です。
他の場合は、安全距離と障害物の重量との差の 2 乗になります。

したがって、次のことが得られます。
$C_{obs}(i,j)=\left\{ {\begin{array}{cccccccccccccc} { 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}$

i は t 方向のインデックス、j は s 方向のインデックスです

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 距離コストの計算

目的は目的地に早く到着することです。
ここに画像の説明を挿入します
距離コストは次のように計算されます。

${C_{spatial}} = {w_{spatial}}({s_{total}} - s(j))$ ${w_{spatial}}$ は損失重量
${s_{total}} - s(j))$ 現在のポイントとターゲットポイントの差。

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

2.3 州移転コスト

この部分には主に、CalculateEdgeCost、、CalculateEdgeCostForSecondColの3 つの関数が含まれますCalculateEdgeCostForThirdCol。後の 2 つは特殊なケースなので、最初に CalculateEdgeCost を見てください。

2.3.1 エッジコストの計算

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

状態転送コストの計算は 3 つの部分に分かれています:
${C_{edge}} = {C_{speed}} + {C_{acc}} + {C_{jerk}}$ ${C_{速度}}$ ——速度コスト
${C_{acc}}$ ——加速コスト
${C_{ジャーク}}$ ——ジャーク加速コスト

2.3.1.1 ${C_{speed}}$ ——スピードコスト

ノード間の速度は次のとおりです: $\frac{ {s(j ) - s(j-1)}}{ {\Delta t}}$
限界速度比： ${v_{det }} = \frac{ {v - {v_{limit}}}}{ { {v_{limit}}}}$
巡航速度の差: $v_{diff} = v-v_{cruise}$

${C_{速度}}$ 速度代の計算は次のようになります。
$\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{dp 参照速度を有効にする}\end{array}\right。$
速度が 0 未満の場合は逆転状況であり、軌道は実行不可能であり、コスト値は無限大に設定されます。速度が低く、現在位置が停止禁止ゾーンにある場合は、素早く通過する必要があります。 ; 速度が >0 で速度制限より高い場合は、速度超過に対するペナルティが適用されます。速度が <0 で速度制限よりも低い場合は、低速に対するペナルティが適用されます。有効な場合はFLAGS_enable_dp_reference_speed、速度は巡航速度に近くなります。Apollo では、速度超過に対するペナルティ値 (1000) は、速度違反に対するペナルティ値 (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}}$ ——加速コスト

加速度は次のように計算されます。
$=\frac {s_3+s_1-2s_2} { {\Delta t}^2}$
${C_{acc}}$ 加速コストは次のように計算されます。

$0cost_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。$

最大加速度を超えるか最小加速度未満の場合、コスト値は無限大に設定され、その範囲内であれば、Apollo は次のようなコスト関数を計算用に設計しました: y = x 2 + x 2 1 + ex + a + ${x^2} + \frac{ { {x^2}}}{ {1 + {e^{x + a}}}} + \frac{ { { x^2}} }{ {1 + {e^{x + b}}}}$
機能イメージとしては、ここに画像の説明を挿入します
0に近いほどコスト値が小さくなり、目標値に近づくほどコスト値が大きくなり、快適性や滑らかさを満たします。

2.3.1.3 ${C_{ジャーク}}$ ——ジャーク加速コスト

ジャークは次のように計算されます。 $\frac{ { { s_4 } - 3{s_3} + 3{s_2} - {s_1}}} { {\デルタ {t^3}}}$

$0cost_{jerk}=\left\{\begin{array}{}inf&jerk>jerk_{max },ジャーク<ジャーク_{分}\\w_{正,ジャーク}ジャーク^2\デルタジャーク<ジャーク_{最大}\\w_{負,ジャーク}ジャーク^2\デルタジャーク_{分}<ジャーク<0 \ end { 配列 } \ 右$

ジャークが設定された境界を超える場合は無限大に設定され、その範囲内にある場合は二次関数で計算されます。ジャークは小さいほど良いです。

次に、2 つの特殊な場合の計算を示します。

2.3.2 SecondCol のエッジコストを計算する

2.3.2.1 ${C_{speed}}$ ——スピードコスト

この部分は一貫しています。

2.3.2.2 ${C_{acc}}$ ——加速コスト

$v_{pre}$ の速度を使用します。 $v_{事前}$ 、計算の現在の点と前の点 (開始点)。
現在点の速度 $v_{cur}=\frac {s_{cur}-s_{pre}} {\Delta t}$
加速度 $ta=\frac {v_{cur}-v_{pre}} {\Delta t}$
残りは一貫しています。

2.3.2.3 ${C_{ジャーク}}$ ——ジャーク加速コスト

$v_{pre}$ の速度を使用します。 $v_{事前}$ $a_{pre} 前の$ 開始点の加速度 $ある_{事前}$ 、計算の現在の点と前の点 (開始点)。
現在点の速度 $v_{cur}=\frac {s_{cur}-s_{pre}} {\Delta t}$
現在点の加速度 $a_{cur}=\frac {v_{cur}-v_{pre}} {\Delta t}$
加加速度 $jerk=\frac {a_{cur}-a_{pre}} {\Delta t}$
残りは一貫しています。

2.3.3 第 3 列のエッジコストの計算

2.3.3.1 ${C_{speed}}$ ——スピードコスト

この部分は一貫しています。

2.3.3.2 ${C_{acc}}$ ——加速コスト

この部分は一貫しています。

2.3.3.3 ${C_{ジャーク}}$ ——ジャーク加速コスト

開始点の速度 $v_1を使用する$ 、現在のポイント 3、前のポイント 2、および前の 2 つのポイント (開始ポイント) 1 を計算に使用します。
前の点の速度 $v_2=\frac {s_2-s_1} {\Delta t}$
前の点の加速度 $a_2=\frac {v_2-v_1} {\Delta t}$
現在点速度 $v_3=\frac {s_3-s_2} {\Delta t}$
現在点加速度 $a_3=\frac {v_3-v_2} {\Delta t}$
ジャーク $ジャーク=\frac {a_{3}-a_{2}} {\Delta t}$
残りは一貫しています。

2.4 GetRowRange

GetRowRange は、最大加速度/減速度を使用して次の s の範囲 (行数) を計算します。

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. バックトラックして最適な ST カーブを見つける

バックトラッキングの開始点の選択に注意する必要があります

各列の最後のポイントをたどり、現在の best_end_point を見つけて、min_cost を更新します。
最後の列の min_cost ポイントは、ここではエンドポイントとして直接使用されません。

サンプリング時間は推定時間枠であるため、この時間より前にエンドポイントに到達する可能性があります。
ここに画像の説明を挿入します

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

最終出力はspeed_dataです。

参考

[1]プランニング動的計画法に基づく速度計画
[2] Apollo 6.0 の EM Planner (3): 動的計画速度計画の DP プロセス
[3] Apollo Spark Plan 学習ノート - Apollo 速度計画アルゴリズムの原理と実践
[4] ]動的計画とアポロ計画におけるその応用計画モジュール
[5]アポロ計画制御研究ノート