提示：文章写完后，目录可以自动生成，如何生成可参考右边的帮助文档
文章目录
前言
本文采用基于优化的横向规划方法和基于动态规划的纵向规划方法
横向优化控制详见第一篇文章 https://blog.csdn.net/AAAndrea/article/details/139444710
纵向速度规划
本文采用基于动态规划（DP）的速度规划方法。
首先需要了解下动态规划的原理：
在应用动态规划处理特定的优化问题时，可以将整个问题解决过程依据特定顺序（例如时间、空间或其他逻辑顺序）划分成多个相互依赖的「阶段」。接着，依次对每个阶段进行「决策」，这些决策不仅决定了当前阶段的效益，也影响了下一阶段的起始条件。完成所有阶段的决策后，便形成了针对整个问题的决策序列。
1. 对路程和时间进行采样
首先对路程和时间进行采样。
按照Apollo的做法是对比较近的距离以较小间隔采样；对比较远的距离以较大间隔采样。
但是这个时候会出现
如果采样点过多，之后计算的cost就越多，耗时就越长。所以在选择采样点的时候就需要删选。
maxsA = prev_maxs + prev_maxv * self.delta_t + 0.5 * self.a_max * self.delta_t ** 2
minsA = prev_mins + prev_minv * self.delta_t + 0.5 * self.a_min * self.delta_t ** 2
maxs = min(maxs, maxsA) # maxs为最大前探距离
mins = max(mins, minsA) # mins为自车起始S0
如果循环点的s不满足 mins <= s <= maxs,后续不会进行cost 计算，直接给cost = 10000（极大值）。
2. 计算代价函数
代价函数主要分为三个部分:
① 障碍物代价
② 加速度和速度代价
③ 损失代价
1.障碍物代价
障碍物处理的部分比较复杂，所以放在下一章。
本章直接使用处理完的障碍物列表。
障碍物代价函数的计算公式为: （同Apollo）
将障碍物绘制在S-T图上可以看到，绿色点构成的是障碍物的超车安全距离；蓝色点构成的是障碍物的跟车安全距离。
2.加速度和速度代价
计算速度的时候需要运用到"父节点"，因此在这一部分至少需要两个循环。
一个对当前节点cur_node的循环，一个是对当前节点的父节点prev_node进行循环。
在计算加速度的时候，则需要父节点的父节点，可以用父节点的’parent’属性找到pprev_node。
v = (cur_node['s'] - prev_node['s']) / (cur_node['t'] - prev_node['t'])
cost = (v - cv) ** 2 / 100
# 如果前一个节点有父节点
if prev_node['parent'] is not None:
# 获取前一个节点所在层的上一层的所有节点
pprev_nodes = st_node_tab[lvlnum - 1]
# 根据前一个节点的父节点索引获取前一个节点
pprev_node = pprev_nodes[prev_node['parent']]
# 计算前一个节点与前前一个节点之间的速度
prev_v = (prev_node['s'] - pprev_node['s']) / self.delta_t
a = (v - prev_v) / (cur_node['t'] - prev_node['t'])
cost += math.fabs(a) ** 2 / 100
3.损失代价
损失代价的设置目的是为了自车能够更快地达到最大前探距离（目的地），避免过于保守的驾驶行为。
3.选出最小代价的点进行回溯
如果只是将每一层的代价最小的点记录，连接起来的话，有可能出现以下情况:
最小点出现的位置连线会穿过障碍物，也就是在实际情况中已经发生碰撞了。
因此本文选用从最后一层开始向前回溯，以避免这种情况的发生。
大致逻辑是:
- 在最后一层中找到cost最小的点A
- 遍历倒数第二层，找出所有’parent’属性是A的点，找到其中cost最小的点B
- 遍历倒数第三层，找到所有’parent’属性是B的点，找到其中cost最小的点C
- 以此类推，找出所有delta_t间隔上的s
- 根据s和t，计算出v和a
4. 实验结果
第一张图中的障碍物一直是持续的，可以想象为位于自车正前方的车辆，自车处于跟车状态。
第二张图中的障碍物不是一直持续的，可以想象为若自车发生了变道，那自车原来正前方的车辆在自车有一定偏移量之后就不会在影响自车，在S-T中便也不会画出来了。
具体的处理方式详见
[轨迹规划实操] 横向优化算法+纵向DP算法的python复现（3）
