3 动态规划 | 自动驾驶小白说

3.1 概述

动态规划是一种通过把原问题分解为相对简单的子问题的方式求解复杂问题的方法。在自动驾驶决策中，动态规划能够处理包含多个阶段和状态转移的优化问题，通过定义状态、状态转移方程和目标函数，逐步求解子问题，最终得到全局最优解。

其适用场景如下：

最优子结构性质。如果问题的最优解所包含的子问题的解也是最优的，我们就称该问题具有最优子结构性质（即满足最优化原理）。最优子结构性质为动态规划算法解决问题提供了重要线索。

无后效性。即子问题的解一旦确定，就不再改变，不受在这之后、包含它的更大的问题的求解决策影响。

子问题重叠性质。子问题重叠性质是指在用递归算法自顶向下对问题进行求解时，每次产生的子问题并不总是新问题，有些子问题会被重复计算多次。动态规划算法正是利用了这种子问题的重叠性质，对每一个子问题只计算一次，然后将其计算结果保存在一个表格中，当再次需要计算已经计算过的子问题时，只是在表格中简单地查看一下结果，从而获得较高的效率，降低了时间复杂度。

动态规划的核心步骤‌主要包括定义状态、确定状态转移方程、初始化、递推计算以及求解目标。这些步骤共同构成了动态规划算法设计的基础，确保了算法的正确性和效率。

‌定义状态‌：首先需要明确问题的状态，并用状态变量表示。这一步骤是动态规划的核心，因为状态的定义直接影响到后续步骤的实现和算法的效率。

‌确定状态转移方程‌：根据问题的最优子结构，确定状态之间的转移关系。这一步骤建立了问题各个阶段之间的联系，使得问题可以通过逐步求解子问题的最优解来得到原问题的最优解。

‌初始化‌：设置初始状态的值。这是问题的起点，通常是无法通过状态转移方程计算出来的，因此需要明确地给出。

‌递推计算‌：根据状态转移方程，从初始状态逐步计算到目标状态。这一步骤通过逐步计算，根据状态转移方程，从初始状态逐步推导出目标状态的最优解。

‌求解目标‌：根据最终状态的值，得到问题的解。这是动态规划算法的最终目标，通过前面的步骤，可以得到问题的最优解。

3.2 动态规划在自动驾驶中的应用

在Apollo的EM算法中，动态规划主要用来生成path decision和speed decision，以此来构造凸空间，供下游qp来优化出最优path和speed。

3.2.1 SL投影（E-step）

SL投影基于G2（连续曲率导数）平滑参考线。在笛卡尔空间中，障碍物和自车状态通过位置和方向以及自车的曲率和曲率导数来描述。然后，这些信息被映射到Frenet框架坐标上，这些坐标分别代表纵向位置、横向位置、横向位置一阶导、横向位置二阶导和横向位置三阶导。处理静态障碍物和动态障碍物的方式有所不同：

静态障碍物的位置不随时间变化，因此映射过程相对直接，直接投影即可

动态障碍物，我们借助自车上一周期的轨迹来进行映射。将上一周期的移动轨迹投影到Frenet坐标系下，以提取出纵向的速度分布图，这将根据特定时间给出自车纵向坐标的估计。估计的自车纵向坐标有助于评估与动态障碍物的交互。一旦自车的纵向坐标与障碍物轨迹上同一时间点的点发生交互，就会在SL图上标记一个阴影区域，作为与动态障碍物交互的估计。这里的交互定义为自车与障碍物边界框的重叠。

如下图所示，一个迎面而来的动态障碍物及其由预测模块估计的轨迹以红色标记，自车以蓝色标记。首先，将迎面而来的动态障碍物的轨迹按时间离散化为多个轨迹点，然后将这些点投影到Frenet坐标系下。一旦发现自车的纵向坐标与投影后的障碍物点发生交互，重叠区域（图中以紫色显示）将在Frenet框架上被标记出来。

3.2.2 DP path decision（M-step）

M-step在求解Frenet坐标系下的最优路径规划时，实际上是在一个非凸空间（从左和从右避让是两个局部最优场景）求最优的方程：。但对于二次优化而言，其是求解凸空间下的最优问题，因此我们需要利用动态规划给障碍物做决策，从而构造凸空间。

在Frenet坐标系下，我们在自车前方的不同纵向位置和不同横向位置采样点，其实纵向和横向的采样分辨率取决于车辆速度和道路结构等元素。

不同纵向位置处的点之间的连接：通过五次多项式边缘进行平滑连接，以确保路径的连续性和平滑性

动态规划寻找最优路径：在构建好的格点图（lattice graph）中，可以利用动态规划（DP）算法来找到从起点到终点的最优路径。动态规划通过考虑所有可能的路径并评估它们的成本（如时间、距离、安全性等），来选择出最佳的行驶路径。

粗糙的路径、障碍物决策和凸空间：经过动态规划得到粗糙轨迹后，我们就可以跟据粗糙轨迹和障碍物之间的相对位置关系，做出横向决策：left nudge、right nudge或者no nudge，并在决策的基础上得到凸空间供下游qp optimizer进行优化。

在此动态规划问题中，我们需要计算每条边的cost，其是由smooth cost，obstacle cost和guidance cost组成。

具体如下：

smooth cost：一阶表示自车和车道lane之间的航向偏差，二阶与路径的曲率相关，三阶与路径曲率的导数相关。

obstacle cost：由以下方程给出，其中表示自车bounding box到障碍物bounding box的距离，为单调递减函数，为了安全考虑，留有一定的buffer，是一个很大的值，表示有碰撞。

guidance cost：考虑是否在道路上行驶和与参考线之间的差异。包括两部分：引导线代价和车道代价。引导线定义为周围没有障碍物时的理想路径，通常为道路中心线。车道代价通常被道路边界决定，道路外的路径点将受到很高代价的惩罚。

3.2.3 ST投影（E-step）

ST投影有助于帮助评估自车的速度曲线。在生成一条光滑路径之后，如果存在交互，则静态障碍物和动态障碍物都被投影到路径上。

🌟Note: 这里的交互是指自车和障碍物的bounding box是否有overlap，这可以通过SAT或GJK的方式得到。

如下图所示：一个障碍物在2秒后、距离自动驾驶车辆40米的前方切入其行驶路径，以红色标记；另一个障碍物位于自动驾驶车辆后方，以绿色标记。其余区域即为速度轮廓的可行区域。速度优化的M步骤将尝试在此区域内找到一个可行的平滑解。

3.2.4 DP speed decision（M-step）

同path generator相同，在整个st graph中生成速度曲线，也是一个非凸问题，因此我们仍然需要借助DP来生成凸空间。DP速度规划步骤包含代价函数、ST图和动态规划搜索。输出结果为分段速度曲线、可行的凸空间和障碍物的速度决策（follow，yield，stop和overtake等），如下图所示：

障碍物信息首先在st graph上离散成网格。将表示为时间轴上具有间隔的等距评估点。分段速度曲线函数在网络上表示为，此外利用有限差分法对导数进行了逼近。

目标是在st graph中优化一个带有约束的代价函数。其代价如下：

其中：第一项是速度保持成本。表示当没有障碍物或红绿灯限制时，车辆应遵循指定速度。描述了由道路速度限制、曲率和其他交通法规确定的参考速度。函数设计为对小于或大于的值具有不同的惩罚。加速度和加加速度平方的积分描述了速度曲线的平滑度。最后一项描述了总障碍成本。评估ego汽车到所有障碍物的距离，以确定总障碍物成本。

3.3 参考

https://arxiv.org/pdf/1807.08048