本文作者：Apollo开发者社区
动态规划的英文为：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速度算法的代码注释
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}原文链接地址：https://developer.baidu.com/topic/show/290438
来源：oschina
链接：https://my.oschina.net/u/4299156/blog/3218133