开发者说丨动态规划及其在Apollo项目Planning模块的应用

动态规划的英文为:Dynamic Programming,这里的“Programming”并非指编写程序代码,而是指一种表格计算法(A tabular method),即基于表格查询的方法计算得到最优结果,因此中文将其翻译成“动态规划”不甚严谨。关于动态规划算法的原理,MIT出版的专著:“Introduction to Algorithms Third Edition (Thomas H. Cormen, Charles E. leiserson, Ronald L. Rivest, Clifford Stein)”(中文版《算法导论》)讲解得不错,本文的算法原理及示例均摘自该书。

 

本文由社区荣誉布道师——贺志国撰写,对动态规划及其在Apollo项目Planning模块的应用进行了详细讲解,希望这篇文章能给感兴趣的开发者带来更多帮助。

 

  以下,ENJOY  

 

一、动态规划算法原理

 

动态规划与分治(The Divide-and-Conquer Method)有些类似,也是将问题分解为多个子问题,并且基于子问题的结果获得最终解。二者的区别是,分治法将初始问题划分为多个不关联(Disjoint)的子问题(Subproblem)(即子问题相互之间互不依赖),递归地解决子问题,然后将子问题的解合并得到初始问题的解。与之相反,动态规划法分解得到的子问题是相互重叠的(Overlap),即子问题依赖于子子问题(Subsubproblem),子子问题又进一步依赖于下一级的子子问题,这样不断循环直至抵达不需分解的初始条件。在求解过程中,为了避免重复计算子子问题从而提高算法效率,需要将一系列子子问题的解保存到一张表中(table,C++编程一般使用std::array、std::vector或std::list实现),这也就是动态规划又被称为查表计算法的原因。

 

动态规划一般应用于最优化问题(Optimization Problems)。这类问题一般存在多个解,每个解都具有一个度量值,我们期望得到具有度量值最优(即取最大或最小值)的解。该最优解一般称为最优化问题的一个解。注意,这个解并非唯一,因为最优化问题可能存在多个最优解。

 

构建一个动态规划算法的一般步骤如下:

 

1、刻画出一个最优解的结构特征(即使用数学表达式来表述一个最优解);

 

2、迭代定义一个最优解的度量值;

 

3、计算每个最优解的度量值,通常采用自下而上的方式;

 

4、根据计算得到的信息构建出原问题的一个最优解。步骤1-3是使用动态规划求解问题的基础形式。如果我们只需获得最优解的度量值而非最优解本身,则可以忽略步骤4。

 

二、动态规划在Apollo项目Planning模块的应用

 

Apollo项目Planning模块的EMPlanner中使用动态规划生成代价(Cost)最小的多项式路径(DP路径,见Apollo项目中的DPRoadGraph类)和速度(DP速度,见Apollo项目中的DpStGraph类),DP路径算法和DP速度算法的示意性描述如下图所示(来自百度Apollo项目公开课PPT):

 

▲图1. DP路径算法

 

▲图2. DP速度算法

 

DP路径算法的基本思路是,基于给定的一条中心路径(称为参考线,ReferenceLine)和车道边界条件,每隔一定间隔的纵向距离(称为不同级(Level)上的s值)对道路截面进行均匀采样(与中心点的横向偏移值称为为l值),这样会得到图中黑点所示的采样点(这些采样点称为航点,Waypoint)数组。基于一定的规则,可以给出各航点迁移的代价值(Cost)。航点迁移不一定需要逐级进行,可以从第一级跳到第三级甚至终点,只要迁移代价值最小化即可,这显然满足动态规划的求解思路。

 

DP速度算法的基本思路是,在DP路径算法生成一条可行驶的路径后,从起点开始,考虑避开路径中的所有障碍物,并且让加减速最为平顺,以最优的速度曲线(即t-s平面中的绿色曲线)安全抵达终点,这也可以使用动态规划的思路求解。

 

DP路径算法的代码注释

 

航点数组生成

 

 1// 采样获得路径航点
  2bool DPRoadGraph::SamplePathWaypoints(
  3    const common::TrajectoryPoint &init_point,
  4    std::vector<std::vector> *const points) {
  5  CHECK_NOTNULL(points);
  6
  7  // 最小采样距离
  8  const double kMinSampleDistance = 40.0;
  9  // 总长度 = min(初始点 + max(初始速度 × 8, 最小采样距离), 参考线长度)
 10  const double total_length = std::fmin(
 11      init_sl_point_.s() + std::fmax(init_point.v() * 8.0, kMinSampleDistance),
 12      reference_line_.Length());
 13
 14  // 采样前视时长
 15  constexpr double kSamplePointLookForwardTime = 4.0;We can just use `sl = common::util::MakeSLPoint(s, l);` . 
 16  // 采样步长 = 初始速度 × 采样前视时长,要求:
 17  // step_length_min(默认值:8) <= 采样步长 <= step_length_max(默认值:15)
 18  const double level_distance =
 19      common::math::Clamp(init_point.v() * kSamplePointLookForwardTime,
 20                          config_.step_length_min(), config_.step_length_max());
 21  // 累计轨迹弧长
 22  double accumulated_s = init_sl_point_.s();
 23  // 上次轨迹弧长
 24  double prev_s = accumulated_s;
 25  // 累计轨迹弧长小于总长度时,将累计轨迹弧长每次加上采样步长,进行循环采样
 26  for (std::size_t i = 0; accumulated_s < total_length; ++i) {
 27    accumulated_s += level_distance;
 28    if (accumulated_s + level_distance / 2.0 > total_length) {
 29      accumulated_s = total_length;
 30    }
 31    // 本次轨迹弧长:取累计轨迹弧长与总长度之间的最小值
 32    const double s = std::fmin(accumulated_s, total_length);
 33    // 最小允许采样步长
 34    constexpr double kMinAllowedSampleStep = 1.0;
 35    // 若本次轨迹弧长与上次轨迹弧长间的差值小于最小允许采样步长,跳过本次采样
 36    if (std::fabs(s - prev_s) < kMinAllowedSampleStep) {
 37      continue;
 38    }
 39    prev_s = s;
 40
 41    // 左车道宽
 42    double left_width = 0.0;
 43    // 右车道宽
 44    double right_width = 0.0;
 45    reference_line_.GetLaneWidth(s, &left_width, &right_width);
 46
 47    // 边界缓冲
 48    constexpr double kBoundaryBuff = 0.20;
 49    const auto &vehicle_config =
 50        common::VehicleConfigHelper::instance()->GetConfig();
 51    const double half_adc_width = vehicle_config.vehicle_param().width() / 2.0;
 52    // 右侧允许宽度 = 右车道宽 - 半车宽 - 边界缓冲
 53    const double eff_right_width = right_width - half_adc_width - kBoundaryBuff;
 54    // 左侧允许宽度 = 左车道宽 - 半车宽 - 边界缓冲
 55    const double eff_left_width = left_width - half_adc_width - kBoundaryBuff;
 56    // 每步采样点数
 57    const size_t num_sample_per_level =
 58        FLAGS_use_navigation_mode ? config_.navigator_sample_num_each_level()
 59                                  : config_.sample_points_num_each_level();
 60    // 默认横向采样间隔
 61    double kDefaultUnitL = 1.2 / (num_sample_per_level - 1);
 62    if (reference_line_info_.IsChangeLanePath() && !IsSafeForLaneChange()) {
 63      kDefaultUnitL = 1.0;
 64    }
 65    // 横向采样距离
 66    const double sample_l_range = kDefaultUnitL * (num_sample_per_level - 1);
 67    // 右采样边界(车辆右侧为负值)
 68    double sample_right_boundary = -eff_right_width;
 69    // 左采样边界(车辆左侧为正值)
 70    double sample_left_boundary = eff_left_width;
 71    // 参考线正在改变车道时
 72    if (reference_line_info_.IsChangeLanePath()) {
 73      // 右采样边界取右采样边界与初始点横向偏移之间的最小值
 74      sample_right_boundary = std::fmin(-eff_right_width, init_sl_point_.l());
 75      // 左采样边界取左采样边界与初始点横向偏移之间的最大值
 76      sample_left_boundary = std::fmax(eff_left_width, init_sl_point_.l());
 77
 78      // 若初始点横向偏移 > 左侧允许宽度,则将右侧采样边界设置为右侧采样边界与(初始点横向偏移
 79      // - 横向采样距离)之间的最大值
 80      if (init_sl_point_.l() > eff_left_width) {
 81        sample_right_boundary = std::fmax(sample_right_boundary,
 82                                          init_sl_point_.l() - sample_l_range);
 83      }
 84      // 若初始点横向偏移 < 右侧允许宽度,则将左侧采样边界设置为左侧采样边界与(初始点横向偏移
 85      // + 横向采样距离)之间的最小值
 86      if (init_sl_point_.l() < eff_right_width) {
 87        sample_left_boundary = std::fmin(sample_left_boundary,
 88                                         init_sl_point_.l() + sample_l_range);
 89      }
 90    }
 91
 92    // 横向采样距离数组
 93    std::vector<double> sample_l;
 94    // 参考线正在改变车道且改变车道不安全时,将当前参考线到其他参考线的偏移值存储到横向采样距离数组
 95    if (reference_line_info_.IsChangeLanePath() && !IsSafeForLaneChange()) {
 96      sample_l.push_back(reference_line_info_.OffsetToOtherReferenceLine());
 97    } else {
 98      // 其他情形,从右采样边界到左采样边界,按照每步采样点数进行均匀采样,并将结果存储到横向采样距离数组
 99      common::util::uniform_slice(sample_right_boundary, sample_left_boundary,
100                                  num_sample_per_level - 1, &sample_l);
101      if (HasSidepass()) {
102        // currently only left nudge is supported. Need road hard boundary for
103        // both sides
104        sample_l.clear();
105        switch (sidepass_.type()) {
106          case ObjectSidePass::LEFT: {
107            // 左侧绕行:将(左侧允许宽度 + 左侧绕行距离)存储到横向采样距离数组
108            sample_l.push_back(eff_left_width + config_.sidepass_distance());
109            break;
110          }
111          case ObjectSidePass::RIGHT: {
112            // 右侧绕行:将-(右侧允许宽度 + 右侧绕行距离)存储到横向采样距离数组
113            sample_l.push_back(-eff_right_width - config_.sidepass_distance());
114            break;
115          }
116          default:
117            break;
118        }
119      }
120    }
121    // 本次采样点数组
122    std::vector level_points;
123    planning_internal::SampleLayerDebug sample_layer_debug;
124    for (size_t j = 0; j < sample_l.size(); ++j) {
125      // 横向偏移值
126      const double l = sample_l[j];
127      constexpr double kResonateDistance = 1e-3;
128      common::SLPoint sl;
129      // 若为奇数采样点或者(总长度 - 累计轨迹弧长)几乎为0即已抵达采样终点,
130      // 则直接将当前采样点坐标设置为(s, l)
131      if (j % 2 == 0 ||
132          total_length - accumulated_s < 2.0 * kResonateDistance) {
133        sl = common::util::MakeSLPoint(s, l);
134      } else {
135        // 其他情形,将当前采样点坐标设置为(min(总长度,s+误差),l)
136        sl = common::util::MakeSLPoint(
137            std::fmin(total_length, s + kResonateDistance), l);
138      }
139      sample_layer_debug.add_sl_point()->CopyFrom(sl);
140      // 将当前采样点坐标存储到本次采样点数组
141      level_points.push_back(std::move(sl));
142    }
143    // 若参考线未改变车道且不绕行,则将横向偏移值为0的采样点(即沿参考线方向的采样点)也加入本次采样点数组
144    if (!reference_line_info_.IsChangeLanePath() && !HasSidepass()) {
145      auto sl_zero = common::util::MakeSLPoint(s, 0.0);
146      sample_layer_debug.add_sl_point()->CopyFrom(sl_zero);
147      level_points.push_back(sl_zero);
148    }
149
150    if (!level_points.empty()) {
151      planning_debug_->mutable_planning_data()
152          ->mutable_dp_poly_graph()
153          ->add_sample_layer()
154          ->CopyFrom(sample_layer_debug);
155      // 将本次的所有采样点存储到总采样点数组
156      points->emplace_back(level_points);
157    }
158  }
159  return true;
160}
 基于航点数组,使用动态规划算法求解代价值最小的路径。
  1    bool DPRoadGraph::GenerateMinCostPath(
  2    const std::vector<const PathObstacle *> &obstacles,
  3    std::vector *min_cost_path) {
  4  CHECK(min_cost_path != nullptr);
  5
  6  // 基于当前参考线及初始点,生成候选路径采样点数组
  7  // 路径航点(path_waypoints)里面的每个vecotr存储相同s值(轨迹累计弧长)下的多个采样点
  8  std::vector<std::vector> path_waypoints;
  9  if (!SamplePathWaypoints(init_point_, &path_waypoints) ||
 10      path_waypoints.size() < 1) {
 11    AERROR << "Fail to sample path waypoints! reference_line_length = "
 12           << reference_line_.Length();
 13    return false;
 14  }
 15  // 将初始点加入到路径航点数组的最前面
 16  path_waypoints.insert(path_waypoints.begin(),
 17                        std::vector{init_sl_point_});
 18  if (path_waypoints.size() < 2) {
 19    AERROR << "Too few path_waypoints.";
 20    return false;
 21  }
 22
 23  // 输出路径航点调试信息
 24  for (uint32_t i = 0; i < path_waypoints.size(); ++i) {
 25    const auto &level_waypoints = path_waypoints.at(i);
 26    for (uint32_t j = 0; j < level_waypoints.size(); ++j) {
 27      ADEBUG << "level[" << i << "], "
 28             << level_waypoints.at(j).ShortDebugString();
 29    }
 30  }
 31
 32  // 读取车辆配置信息
 33  const auto &vehicle_config =
 34      common::VehicleConfigHelper::instance()->GetConfig();
 35
 36  // 轨迹代价
 37  TrajectoryCost trajectory_cost(
 38      config_, reference_line_, reference_line_info_.IsChangeLanePath(),
 39      obstacles, vehicle_config.vehicle_param(), speed_data_, init_sl_point_);
 40
 41  // 最小代价值路图节表点链表
 42  std::list<std::list</std::list</std::vector> graph_nodes;
 43  graph_nodes.emplace_back();
 44  graph_nodes.back().emplace_back(init_sl_point_, nullptr, ComparableCost());
 45  auto &front = graph_nodes.front().front();
 46  size_t total_level = path_waypoints.size();
 47  // 采用自下而上的动态规划算法,迭代更新最小代价值
 48  // graph_nodes存储的就是各级(level)路径航点(path_waypoints)所包含的最小代价航点
 49  // graph_nodes.back()(即最后一条航点链表)就是我们所需的最小代价航点链表
 50  for (std::size_t level = 1; level < path_waypoints.size(); ++level) {
 51    const auto &prev_dp_nodes = graph_nodes.back();
 52    const auto &level_points = path_waypoints[level];
 53
 54    graph_nodes.emplace_back();
 55
 56    for (size_t i = 0; i < level_points.size(); ++i) {
 57      const auto &cur_point = level_points[i];
 58
 59      graph_nodes.back().emplace_back(cur_point, nullptr);
 60      auto &cur_node = graph_nodes.back().back();
 61      // 采用多线程并行计算最小代价值航点
 62      if (FLAGS_enable_multi_thread_in_dp_poly_path) {
 63        PlanningThreadPool::instance()->Push(std::bind(
 64            &DPRoadGraph::UpdateNode, this, std::ref(prev_dp_nodes), level,
 65            total_level, &trajectory_cost, &(front), &(cur_node)));
 66
 67      } else {
 68        // 采用单线程计算最小代价值航点
 69        UpdateNode(prev_dp_nodes, level, total_level, &trajectory_cost, &front,
 70                   &cur_node);
 71      }
 72    }
 73    // 多线程模式下的同步
 74    if (FLAGS_enable_multi_thread_in_dp_poly_path) {
 75      PlanningThreadPool::instance()->Synchronize();
 76    }
 77  }
 78
 79  // graph_nodes.back()(即最后一条航点链表)就是我们所需的最小代价航点链表
 80  // find best path
 81  DPRoadGraphNode fake_head;
 82  for (const auto &cur_dp_node : graph_nodes.back()) {
 83    fake_head.UpdateCost(&cur_dp_node, cur_dp_node.min_cost_curve,
 84                         cur_dp_node.min_cost);
 85  }
 86
 87  // 从终点顺藤摸瓜向起点逐个找出最小代价值航点,并将其加入min_cost_path
 88  const auto *min_cost_node = &fake_head;
 89  while (min_cost_node->min_cost_prev_node) {
 90    min_cost_node = min_cost_node->min_cost_prev_node;
 91    min_cost_path->push_back(*min_cost_node);
 92  }
 93  if (min_cost_node != &graph_nodes.front().front()) {
 94    return false;
 95  }
 96
 97  // 将航点顺序调整为起点到终点
 98  std::reverse(min_cost_path->begin(), min_cost_path->end());
 99
100  for (const auto &node : *min_cost_path) {
101    ADEBUG << "min_cost_path: " << node.sl_point.ShortDebugString();
102    planning_debug_->mutable_planning_data()
103        ->mutable_dp_poly_graph()
104        ->add_min_cost_point()
105        ->CopyFrom(node.sl_point);
106  }
107  return true;
108}
109
110// 在当前level下,获得一条代价值最小的航点链表
111void DPRoadGraph::UpdateNode(const std::list &prev_nodes,
112                             const uint32_t level, const uint32_t total_level,
113                             TrajectoryCost *trajectory_cost,
114                             DPRoadGraphNode *front,
115                             DPRoadGraphNode *cur_node) {
116  DCHECK_NOTNULL(trajectory_cost);
117  DCHECK_NOTNULL(front);
118  DCHECK_NOTNULL(cur_node);
119  for (const auto &prev_dp_node : prev_nodes) {
120    const auto &prev_sl_point = prev_dp_node.sl_point;
121    const auto &cur_point = cur_node->sl_point;
122    double init_dl = 0.0;
123    double init_ddl = 0.0;
124    if (level == 1) {
125      init_dl = init_frenet_frame_point_.dl();
126      init_ddl = init_frenet_frame_point_.ddl();
127    }
128    // 生成当前点到前一level所有航点的的曲线
129    QuinticPolynomialCurve1d curve(prev_sl_point.l(), init_dl, init_ddl,
130                                   cur_point.l(), 0.0, 0.0,
131                                   cur_point.s() - prev_sl_point.s());
132
133    if (!IsValidCurve(curve)) {
134      continue;
135    }
136    const auto cost =
137        trajectory_cost->Calculate(curve, prev_sl_point.s(), cur_point.s(),
138                                   level, total_level) +
139        prev_dp_node.min_cost;
140    // 根据代价最小的原则,在前一level所有航点中找到与当前点连接代价最小的航点,
141    // 将结果存储于prev_dp_node中
142    cur_node->UpdateCost(&prev_dp_node, curve, cost);
143
144    // 尝试将当前点直接连接到初始点,看其代价是否比当前点到前一level航点的最小代价还小,
145    // 若小于则将最小代价航点更新。这种情况一般只会存在于改变车道的情形。
146    // try to connect the current point with the first point directly
147    // only do this at lane change
148    if (level >= 2) {
149      init_dl = init_frenet_frame_point_.dl();
150      init_ddl = init_frenet_frame_point_.ddl();
151      QuinticPolynomialCurve1d curve(init_sl_point_.l(), init_dl, init_ddl,
152                                     cur_point.l(), 0.0, 0.0,
153                                     cur_point.s() - init_sl_point_.s());
154      if (!IsValidCurve(curve)) {
155        continue;
156      }
157      const auto cost = trajectory_cost->Calculate(
158          curve, init_sl_point_.s(), cur_point.s(), level, total_level);
159      cur_node->UpdateCost(front, curve, cost);
160    }
161  }
162}


DP速度算法的代码注释

1// 从st图中寻找代价值最小的速度曲线
  2// s:行驶距离,纵坐标;
  3// t:行驶时间,横坐标
  4// SpeedData* const speed_data表示speed_data本身(即指针自身)不能被修改,
  5// 但speed_data指向的内容可被修改,该函数就是通过它返回最优速度数据的。
  6Status DpStGraph::Search(SpeedData* const speed_data) {
  7  constexpr double kBounadryEpsilon = 1e-2;
  8  // 对边界条件进行初步筛选
  9  for (const auto& boundary : st_graph_data_.st_boundaries()) {
 10    // 若边界类型为禁停区,直接跳过
 11    if (boundary->boundary_type() == StBoundary::BoundaryType::KEEP_CLEAR) {
 12      continue;
 13    }
 14    // 若边界位于(0.0, 0.0)(即起始位置)或者边界的min_t和min_s比边界最小分辨率
 15    // kBounadryEpsilon还小,则将速度点的s值设为0.0,t值均匀采样递增。
 16    if (boundary->IsPointInBoundary({0.0, 0.0}) ||
 17        (std::fabs(boundary->min_t()) < kBounadryEpsilon &&
 18         std::fabs(boundary->min_s()) < kBounadryEpsilon)) {
 19      std::vector speed_profile;
 20      double t = 0.0;
 21      for (int i = 0; i < dp_st_speed_config_.matrix_dimension_t();
 22           ++i, t += unit_t_) {
 23        SpeedPoint speed_point;
 24        speed_point.set_s(0.0);
 25        speed_point.set_t(t);
 26        speed_profile.emplace_back(speed_point);
 27      }
 28      speed_data->set_speed_vector(speed_profile);
 29      return Status::OK();
 30    }
 31  }
 32
 33  // 若st图数据的边界条件为空,意味着前方没有障碍物,匀速前进即可。
 34  // speed_profile[i] = (0.0 + i * unit_s_, 0.0 + i * unit_t_);
 35  if (st_graph_data_.st_boundaries().empty()) {
 36    ADEBUG << "No path obstacles, dp_st_graph output default speed profile.";
 37    std::vector speed_profile;
 38    double s = 0.0;
 39    double t = 0.0;
 40    for (int i = 0; i < dp_st_speed_config_.matrix_dimension_t() &&
 41                    i < dp_st_speed_config_.matrix_dimension_s();
 42         ++i, t += unit_t_, s += unit_s_) {
 43      SpeedPoint speed_point;
 44      speed_point.set_s(s);
 45      speed_point.set_t(t);
 46      speed_profile.emplace_back(speed_point);
 47    }
 48    speed_data->set_speed_vector(speed_profile);
 49    return Status::OK();
 50  }
 51
 52  // 初始化代价表cost_table_
 53  if (!InitCostTable().ok()) {
 54    const std::string msg = "Initialize cost table failed.";
 55    AERROR << msg;
 56    return Status(ErrorCode::PLANNING_ERROR, msg);
 57  }
 58  // 计算代价表cost_table_中所有节点的总代价
 59  if (!CalculateTotalCost().ok()) {
 60    const std::string msg = "Calculate total cost failed.";
 61    AERROR << msg;
 62    return Status(ErrorCode::PLANNING_ERROR, msg);
 63  }
 64  // 通过计算得到的代价表cost_table_中所有节点的总代价,获取速度数据
 65  if (!RetrieveSpeedProfile(speed_data).ok()) {
 66    const std::string msg = "Retrieve best speed profile failed.";
 67    AERROR << msg;
 68    return Status(ErrorCode::PLANNING_ERROR, msg);
 69  }
 70  return Status::OK();
 71}
 72
 73// 初始化代价表
 74Status DpStGraph::InitCostTable() {
 75  // 从配置文件读取s和t的维数
 76  uint32_t dim_s = dp_st_speed_config_.matrix_dimension_s();
 77  uint32_t dim_t = dp_st_speed_config_.matrix_dimension_t();
 78  // 维数检查,要求大于2
 79  DCHECK_GT(dim_s, 2);
 80  DCHECK_GT(dim_t, 2);
 81  // 生成代价表cost_table_,列为dim_t,行为dim_s,所有节点均初始化为:StGraphPoint()
 82  cost_table_ = std::vector<std::vector>(
 83      dim_t, std::vector(dim_s, StGraphPoint()));
 84
 85  // cost_table_[i][j] = STPoint(0.0 + j * unit_s_, 0.0 + i * unit_t_);
 86  double curr_t = 0.0;
 87  for (uint32_t i = 0; i < cost_table_.size(); ++i, curr_t += unit_t_) {
 88    auto& cost_table_i = cost_table_[i];
 89    double curr_s = 0.0;
 90    for (uint32_t j = 0; j < cost_table_i.size(); ++j, curr_s += unit_s_) {
 91      cost_table_i[j].Init(i, j, STPoint(curr_s, curr_t));
 92    }
 93  }
 94  return Status::OK();
 95}
 96
 97// 计算总代价
 98Status DpStGraph::CalculateTotalCost() {
 99  // col and row are for STGraph
100  // t corresponding to col,横坐标
101  // s corresponding to row,纵坐标
102  uint32_t next_highest_row = 0;
103  uint32_t next_lowest_row = 0;
104
105  // 对于第一、二,直至最后一个时间采样值,循环计算不同距离采样值上的代价
106  // 注意:每个时间采样值上的距离采样值数目是不相同的。例如:
107  // 第一个时间采样值为起点,在该点上只能有一个距离采样值:0,否则
108  // 代价表cost_table_就不正确。正常情况下,第二个时间采样值上的距离采样值数目
109  // 会大于1,不然就是匀速前进,玩不下去了^_^
110  for (size_t c = 0; c < cost_table_.size(); ++c) {
111    // 最高行,即最大加速度情形下所允许的最大距离采样值
112    uint32_t highest_row = 0;
113    // 最低行,即最大减速度情形下所允许的最小距离采样值
114    uint32_t lowest_row = cost_table_.back().size() - 1;
115
116    // 对于时间采样值c上的不同距离采样值r: next_lowest_row<=4<=next_highest_row
117    // 计算抵达节点(c, r)的最小总代价
118    for (uint32_t r = next_lowest_row; r <= next_highest_row; ++r) {
119      if (FLAGS_enable_multi_thread_in_dp_st_graph) {
120        // 采用线程池方式计算(c, r)的最小总代价
121        PlanningThreadPool::instance()->Push(
122            std::bind(&DpStGraph::CalculateCostAt, this, c, r));
123      } else {
124        // 采用单线程方式计算(c, r)的最小总代价
125        CalculateCostAt(c, r);
126      }
127    }
128    // 线程池方式间的同步
129    if (FLAGS_enable_multi_thread_in_dp_st_graph) {
130      PlanningThreadPool::instance()->Synchronize();
131    }
132
133    // 给定时间采样值c的情形下,
134    // 更新最高行(即最大采样距离值)和最低行(即最小采样距离值)
135    for (uint32_t r = next_lowest_row; r <= next_highest_row; ++r) {
136      const auto& cost_cr = cost_table_[c][r];
137      if (cost_cr.total_cost() < std::numeric_limits<double>::infinity()) {
138        uint32_t h_r = 0;
139        uint32_t l_r = 0;
140        // 获取当前节点的最高行和最低行
141        GetRowRange(cost_cr, &h_r, &l_r);
142        highest_row = std::max(highest_row, h_r);
143        lowest_row = std::min(lowest_row, l_r);
144      }
145    }
146    // 更新下一次循环的最高行(即最大采样距离)和
147    // 最低行(即最小采样距离)
148    next_highest_row = highest_row;
149    next_lowest_row = std::max(next_lowest_row, lowest_row);
150  }
151
152  return Status::OK();
153}
154
155// 基于当前ST图上的点point,获取下一个允许的
156// 最高行(即最大采样距离)和最低行(即最小采样距离)
157void DpStGraph::GetRowRange(const StGraphPoint& point,
158                            uint32_t* next_highest_row,
159                            uint32_t* next_lowest_row) {
160  double v0 = 0.0;
161  if (!point.pre_point()) {
162    // 起始点速度
163    v0 = init_point_.v();
164  } else {
165    // 其他点速度
166    v0 = (point.index_s() - point.pre_point()->index_s()) * unit_s_ / unit_t_;
167  }
168
169  // cost_table_中最后一个时间采样值所包含的距离采样值数目
170  const size_t max_s_size = cost_table_.back().size() - 1;
171
172  const double speed_coeff = unit_t_ * unit_t_;
173  // 最大加速度情形下所允许的最大距离
174  const double delta_s_upper_bound =
175      v0 * unit_t_ + vehicle_param_.max_acceleration() * speed_coeff;
176  *next_highest_row =
177      point.index_s() + static_cast<uint32_t>(delta_s_upper_bound / unit_s_);
178  if (*next_highest_row >= max_s_size) {
179    *next_highest_row = max_s_size;
180  }
181  // 最大减速度情形下所允许的最小距离
182  const double delta_s_lower_bound = std::fmax(
183      0.0, v0 * unit_t_ + vehicle_param_.max_deceleration() * speed_coeff);
184  *next_lowest_row += static_cast<int32_t>(delta_s_lower_bound / unit_s_);
185  if (*next_lowest_row > max_s_size) {
186    *next_lowest_row = max_s_size;
187  }
188}
189
190// 使用动态规划算法计算(c, r)点的总代价
191// c: 时间坐标,即横坐标
192// r: 距离坐标,即纵坐标
193void DpStGraph::CalculateCostAt(const uint32_t c, const uint32_t r) {
194  auto& cost_cr = cost_table_[c][r];
195  // 设置当前点的障碍物代价
196  cost_cr.SetObstacleCost(dp_st_cost_.GetObstacleCost(cost_cr));
197  // 当前点的障碍物代价无穷大,直接返回
198  if (cost_cr.obstacle_cost() > std::numeric_limits<double>::max()) {
199    return;
200  }
201  // 初始代价
202  const auto& cost_init = cost_table_[0][0];
203  // 第一个时间采样值为c(即时间t)== 0,因此r(即距离)必须为0,表示是起点,代价值自然设置为0.0。
204  if (c == 0) {
205    DCHECK_EQ(r, 0) << "Incorrect. Row should be 0 with col = 0. row: " << r;
206    cost_cr.SetTotalCost(0.0);
207    return;
208  }
209
210  // 获取速度限制条件
211  double speed_limit =
212      st_graph_data_.speed_limit().GetSpeedLimitByS(unit_s_ * r);
213  // 第二个时间采样值
214  if (c == 1) {
215    // 加速度或减速度超出范围,返回
216    const double acc = (r * unit_s_ / unit_t_ - init_point_.v()) / unit_t_;
217    if (acc < dp_st_speed_config_.max_deceleration() ||
218        acc > dp_st_speed_config_.max_acceleration()) {
219      return;
220    }
221
222    // 当前点与初始点有重叠,返回
223    if (CheckOverlapOnDpStGraph(st_graph_data_.st_boundaries(), cost_cr,
224                                cost_init)) {
225      return;
226    }
227    // 设置当前点的代价值
228    cost_cr.SetTotalCost(cost_cr.obstacle_cost() + cost_init.total_cost() +
229                         CalculateEdgeCostForSecondCol(r, speed_limit));
230    // 设置其前序节点为起点
231    cost_cr.SetPrePoint(cost_init);
232    return;
233  }
234
235  constexpr double kSpeedRangeBuffer = 0.20;
236  // 允许的最大距离采样差值
237  const uint32_t max_s_diff =
238      static_cast<uint32_t>(FLAGS_planning_upper_speed_limit *
239                            (1 + kSpeedRangeBuffer) * unit_t_ / unit_s_);
240  // 最小距离采样值
241  const uint32_t r_low = (max_s_diff < r ? r - max_s_diff : 0);
242  // 上一个时间采样值下不同采样距离的代价数组
243  const auto& pre_col = cost_table_[c - 1];
244
245  // 第三个时间采样值
246  if (c == 2) {
247    for (uint32_t r_pre = r_low; r_pre <= r; ++r_pre) {
248      // 从距离采样值r_pre到r所需的加速度
249      const double acc =
250          (r * unit_s_ - 2 * r_pre * unit_s_) / (unit_t_ * unit_t_);
251      // 若加速度越界,则忽略该距离采样值
252      if (acc < dp_st_speed_config_.max_deceleration() ||
253          acc > dp_st_speed_config_.max_acceleration()) {
254        continue;
255      }
256      // 与前一时间采样值上的节点有重合,忽略该距离采样值
257      if (CheckOverlapOnDpStGraph(st_graph_data_.st_boundaries(), cost_cr,
258                                  pre_col[r_pre])) {
259        continue;
260      }
261      // 计算当前节点(c, r)的代价值
262      const double cost = cost_cr.obstacle_cost() +
263                          pre_col[r_pre].total_cost() +
264                          CalculateEdgeCostForThirdCol(r, r_pre, speed_limit);
265      // 若新代价值比节点(c, r)的原有代价值更小,则更新当前节点(c, r)的总代价值
266      if (cost < cost_cr.total_cost()) {
267        cost_cr.SetTotalCost(cost);
268        cost_cr.SetPrePoint(pre_col[r_pre]);
269      }
270    }
271    return;
272  }
273
274  // 其他时间采样值
275  for (uint32_t r_pre = r_low; r_pre <= r; ++r_pre) {
276
277    // 若节点(c - 1, r_pre)上的总代价无穷大或前一次时间采样c - 1上没有前序节点,忽略该节点    
278    if (std::isinf(pre_col[r_pre].total_cost()) ||
279        pre_col[r_pre].pre_point() == nullptr) {
280      continue;
281    }
282    // 从r_pre抵达r所需的加速度
283    const double curr_a = (cost_cr.index_s() * unit_s_ +
284                           pre_col[r_pre].pre_point()->index_s() * unit_s_ -
285                           2 * pre_col[r_pre].index_s() * unit_s_) /
286                          (unit_t_ * unit_t_);
287    // 若加速度越界,忽略该距离采样值
288    if (curr_a > vehicle_param_.max_acceleration() ||
289        curr_a < vehicle_param_.max_deceleration()) {
290      continue;
291    }
292    // 与前一时间采样值上的节点有重合,忽略该距离采样值
293    if (CheckOverlapOnDpStGraph(st_graph_data_.st_boundaries(), cost_cr,
294                                pre_col[r_pre])) {
295      continue;
296    }
297
298    // 上上次距离采样值
299    uint32_t r_prepre = pre_col[r_pre].pre_point()->index_s();
300    // ST图上的上上个点
301    const StGraphPoint& prepre_graph_point = cost_table_[c - 2][r_prepre];
302    // 若上上个节点总代价无穷大,忽略之。
303    if (std::isinf(prepre_graph_point.total_cost())) {
304      continue;
305    }
306    // 若上上个节点没有前序节点,忽略之。
307    if (!prepre_graph_point.pre_point()) {
308      continue;
309    }
310    // 上上上个节点
311    const STPoint& triple_pre_point = prepre_graph_point.pre_point()->point();
312    // 上上个节点
313    const STPoint& prepre_point = prepre_graph_point.point();
314    // 上个节点
315    const STPoint& pre_point = pre_col[r_pre].point();
316    // 当前节点
317    const STPoint& curr_point = cost_cr.point();
318    // 计算从上上上个节点、上上个节点、上个节点与当前节点之间的最小连接代价
319    double cost = cost_cr.obstacle_cost() + pre_col[r_pre].total_cost() +
320                  CalculateEdgeCost(triple_pre_point, prepre_point, pre_point,
321                                    curr_point, speed_limit);
322
323    // 若新代价值比节点(c, r)的原有代价值更小,则更新当前节点(c, r)的总代价值
324    if (cost < cost_cr.total_cost()) {
325      cost_cr.SetTotalCost(cost);
326      cost_cr.SetPrePoint(pre_col[r_pre]);
327    }
328  }
329}
330
331// 获取代价值最小的速度数据
332Status DpStGraph::RetrieveSpeedProfile(SpeedData* const speed_data) {
333  // 最小代价值
334  double min_cost = std::numeric_limits<double>::infinity();
335  // 最优终点(即包含最小代价值的节点)
336  const StGraphPoint* best_end_point = nullptr;
337  //  cost_table_.back()存储的是最后一个时间采样值上不同距离的代价值
338  for (const StGraphPoint& cur_point : cost_table_.back()) {
339    if (!std::isinf(cur_point.total_cost()) &&
340        cur_point.total_cost() < min_cost) {
341      best_end_point = &cur_point;
342      min_cost = cur_point.total_cost();
343    }
344  }
345  // 对于cost_table_中的每一行,即第一个、第二个、...、最后一个时间采样值上的
346  // 代价值数组,其最后一个元素存储的是本级时间采样值上的最小代价节点。
347  // 将这些节点与现有最优终点best_end_point比较,
348  // 不断更新最小代价值min_cost和最优终点best_end_point,
349  // 直至找到全局最优终点
350  for (const auto& row : cost_table_) {
351    const StGraphPoint& cur_point = row.back();
352    if (!std::isinf(cur_point.total_cost()) &&
353        cur_point.total_cost() < min_cost) {
354      best_end_point = &cur_point;
355      min_cost = cur_point.total_cost();
356    }
357  }
358
359  // 寻找最优终点失败
360  if (best_end_point == nullptr) {
361    const std::string msg = "Fail to find the best feasible trajectory.";
362    AERROR << msg;
363    return Status(ErrorCode::PLANNING_ERROR, msg);
364  }
365
366  // 设置最优终点的速度数据,并顺藤摸瓜找出其连接的倒数第二个、倒数第三个直到第一个时间节点
367  // 分别设置这些时间节点的速度数据
368  std::vector speed_profile;
369  const StGraphPoint* cur_point = best_end_point;
370  while (cur_point != nullptr) {
371    SpeedPoint speed_point;
372    speed_point.set_s(cur_point->point().s());
373    speed_point.set_t(cur_point->point().t());
374    speed_profile.emplace_back(speed_point);
375    cur_point = cur_point->pre_point();
376  }
377  // 将速度数据按起点到终点的顺序重新排列
378  std::reverse(speed_profile.begin(), speed_profile.end());
379
380  // 若速度数据中起始点的时间(t)或距离(s)大于编译器定义的最小双精度浮点数(即起点的时间t
381  // 或距离s不为0),则视结果为错误输出。
382  constexpr double kEpsilon = std::numeric_limits<double>::epsilon();
383  if (speed_profile.front().t() > kEpsilon ||
384      speed_profile.front().s() > kEpsilon) {
385    const std::string msg = "Fail to retrieve speed profile.";
386    AERROR << msg;
387    return Status(ErrorCode::PLANNING_ERROR, msg);
388  }
389
390  // 设置最终的速度数据
391  speed_data->set_speed_vector(speed_profile);
392  return Status::OK();
393}
394
395// 计算第一、二、三、四个节点之间的连接代价
396double DpStGraph::CalculateEdgeCost(const STPoint& first, const STPoint& second,
397                                    const STPoint& third, const STPoint& forth,
398                                    const double speed_limit) {
399  return dp_st_cost_.GetSpeedCost(third, forth, speed_limit) +
400         dp_st_cost_.GetAccelCostByThreePoints(second, third, forth) +
401         dp_st_cost_.GetJerkCostByFourPoints(first, second, third, forth);
402}
403
404// 为第二列(即第三个时间采样值上的代价数组)计算连接代价
405double DpStGraph::CalculateEdgeCostForSecondCol(const uint32_t row,
406                                                const double speed_limit) {
407  double init_speed = init_point_.v();
408  double init_acc = init_point_.a();
409  const STPoint& pre_point = cost_table_[0][0].point();
410  const STPoint& curr_point = cost_table_[1][row].point();
411  return dp_st_cost_.GetSpeedCost(pre_point, curr_point, speed_limit) +
412         dp_st_cost_.GetAccelCostByTwoPoints(init_speed, pre_point,
413                                             curr_point) +
414         dp_st_cost_.GetJerkCostByTwoPoints(init_speed, init_acc, pre_point,
415                                            curr_point);
416}
417
418// 为第三列(即第四个时间采样值上的代价数组)计算连接代价
419double DpStGraph::CalculateEdgeCostForThirdCol(const uint32_t curr_row,
420                                               const uint32_t pre_row,
421                                               const double speed_limit) {
422  double init_speed = init_point_.v();
423  const STPoint& first = cost_table_[0][0].point();
424  const STPoint& second = cost_table_[1][pre_row].point();
425  const STPoint& third = cost_table_[2][curr_row].point();
426  return dp_st_cost_.GetSpeedCost(second, third, speed_limit) +
427         dp_st_cost_.GetAccelCostByThreePoints(first, second, third) +
428         dp_st_cost_.GetJerkCostByThreePoints(init_speed, first, second, third);
429}
收藏 评论(0)
分享到: