Planning-Apollo速度决策规划

Apollo采用路径规划和速度规划解耦的方式，先进行路径规划，在得到的规划路径上再进行速度规划。利用ST Graph，将障碍物、限速等投影在ST图上，利用全局搜索方法DP算法得到决策，在利用最优化方法进行速度规划。

1. 算法原理

百度已经将算法原理发表在论文《Optimal Trajectory Generation for Autonomous Vehicles Under Centripetal Acceleration Constraints for In-lane Driving Scenarios》中。

1.1 优化模型离散方式

在处理最优化问题时，一般会转化成离散形式，将轨迹$s(t)$按照某参数离散，并计算离散点处的约束和$cost$。不同的离散方式会构建不同的优化模型。对于速度规划问题，一般可以按照等间距离散(Spatial Parameter Discretization)和等时间离散(Temporal Parameter Discretization)。

1.1.1 Spatial Parameter Discretization

使用纵向距离$s$作为离散采样参数，假设第一个采样点$s=0$，连续采样离散点之间等间隔$\Delta s$离散，那么优化问题的决策变量为每个离散点的$\dot{s},\ddot{s}$和相邻采样点之间的时间间隔$\Delta t$。

优点：

• 参考线或者规划路径的曲率$\kappa$是纵向距离$s$的函数，因此向心加速度约束是容易处理的；
• 地图限速等速度约束是和纵向距离$s$相关的，那么优化问题的$\dot{s}$约束容易计算；

缺点：

• 优化问题的目标函数难以建模。Apollo规划算法用来优化纵向冲击度最小，冲击度使用相邻两个离散点的差分计算，如果采用$\Delta s$离散会使决策变量之间有除法运算，形成非凸优化问题；
• 难以施加安全约束。障碍物一般投影在ST Graph中，会得到每个时刻下的安全行驶范围$(s_{min},s_{max})$，即可行驶区域$s_{free}(t)$是时间$t$的函数。

1.1.2 Temporal Parameter Discretization

使用时间$t$作为离散采样参数，假设第一个采样点$t=0$，连续采样离散点之间等间隔$\Delta t$离散，那么优化问题的决策变量为每个离散点的$s,\dot{s},\ddot{s}$。

使用等时间间隔离散的方式，和使用等距离离散方式的优缺点恰好相反。

优点：

• 优化模型可以保持凸包性质，冲击度的计算仍然是线性形式；
• 障碍物的安全约束容易施加在优化模型中；

缺点：

• 由于曲率是$s$的函数，因此向心加速度约束难以处理；
• 速度约束难以处理；
• 最小任务完成时间的优化目标难以处理；

处理方法：

• 使用规划路径的最大曲率计算向心加速度限制；
• 将离散点处的位置$s$与DP得到纵向位置$s$的差作为目标函数的一部分，使用DP结果在采样时刻t处的s处对应的速度限值；
• 将离散采样点处的速度$\dot{s}$与目标速度(reference speed)的差作为目标函数的一部分。以保证完成任务花费的时间最少；

此外，Apollo还设计了非线性速度规划来处理以上问题。

1.2 优化问题建模

1.2.1 cost function

$$J=w_{\ddot{s}}\ast \sum _{i=0}^{n-1}{\ddot{s}}_i^2+w_{s^{(3)}}\ast \sum _{i=0}^{n-2}(s^{(3)}{)}_{i\to i+1}^2+w_{a_c}\ast \sum _{i=0}^{n-1}{\dot{s}}_i^2\ast \kappa (s_i)$$