本文经自动驾驶之心公众号授权转载，转载请联系出处。
Apollo速度规划分为：DP（动态规划）+ QP（轨迹优化）两部分。
DP给出一个粗解，为了开辟一个凸空间，然后再由osqp/ipopt来做优化平滑。
1. ST坐标系中的S：
- 路径规划中的SL坐标系：S是参考线上的s；
- 速度规划中的ST坐标系：S是路径规划输出的路径上的s；
2. 基于动态规划的速度规划
task：SPEED_HEURISTIC_OPTIMIZER
Apollo自动驾驶planning纵向速度规划之DP详解 - 知乎 (zhihu.com)
Planning 基于动态规划的速度规划 - 知乎 (zhihu.com)
基于处理好的ST图，通过采样构建一个二维的cost_table，并对每一格计算cost值，从而通过动态规划算法，得到一条粗的最优ST轨迹。
一般动态规划问题包括以下几个步骤：
- 确定状态变量
- 设计状态转移方程
- 初始化边界
- 返回结果
- 状态变量
状态变量的定义就是StGraphPoint, 从代码的定义我们可以看到，每个状态点保存了：
- 当前栅格的位置信息point_，
- 优化后的速度（optimal_speed_），
- 参考速度cost （reference_cost_），
- 障碍物cost(obstacle_cost_)，
- 空间距离cost （spatial_potential_cost_），
- 当前点总的cost（total_cost_）
- 状态转移方程
总体状态转移方程如下：
其中k表示了在最大加减速度约束下，从i时间j位置向i+1时间转移可达范围内的所有点
遍历从（0,0）点开始，向（1，k）逐步遍历。对于遍历点C（i+1，j+k）会比较其本身cost和从C（i,j）转移到C(i+1，j+k)的cost中最小cost作为其新的cost。
3. 基于osqp的二次速度规划
task：PIECEWISE_JERK_SPEED_OPTIMIZER
apollo PiecewiseJerkSpeedProblem 速度优化 QP 数学推导 - 知乎 (zhihu.com)
Planning 基于二次规划的速度规划 - 知乎 (zhihu.com)
- 优化变量
- 目标函数
第4项的 ：根据决策制定的速度分配确定的 i 时刻的曲率确定的权重；是曲率关于时间t的函数
但是这里如果曲线优化后，在对应时间的s就不是参考位置的s了，曲率的惩罚也不是优化后对应位置的曲率，所以apollo还提供了一直非线性规划方法。
4. 基于ipopt的非线性速度规划
task：PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER
osqp二次规划存在的问题
二次规划的曲率规划的惩罚pi是曲率关于时间t的函数，但实际上路径的曲率是和s相关的。
二次规划的速度规划通过对动态规划粗糙的st曲线，将关于s的区域惩罚转化为关于t的区域的惩罚，此时如果平滑后的曲线和原来动态规划的st曲线相差不大的时候，其惩罚位置是正确的，如果平滑后的曲线和原始的动态规划的st曲线相差较大，实际的惩罚区间就和设定的不一样。
比如上图中应该惩罚的区间为橙色区间，而二次规划算法实际对于曲率惩罚的区域是在绿色的区域。还有比如地图的限速约束也是和s相关的，所以这样的问题就在于，限速或者曲率的函数是关于s的函数，而s又是待求的量(优化量)，就无法对目标函数施加速度约束或者曲率的约束，只能通过动态规划的st曲线进行转化，转化成t的函数再施加约束，但这样就会导致二次规划算法的st曲线的速度约束不精确的问题。
ipopt非线性规划求解
f(x)为代价函数，g(x)为限制条件，f(x)和g(x)都可以为非线性或者是非凸的函数，但必须为二次连续可微（及二阶导数是连续的）；
非线性优化器求解主要是考虑路径曲率对速度规划的影响，因为曲率约束是一个非线性的函数。
Planning 基于非线性规划的速度规划 - 知乎 (zhihu.com)
- 速度点集的osqp优化平滑
- 道路曲率的osqp优化平滑
- 道路限速的osqp优化平滑
等间隔的时间采样，s，s一阶导数，s二阶导数，以及s的两组松弛变量，用于避免求解失败
额外加入了横向加速度和松弛变量的优化。
横向加速度和曲率有关，由于曲率是关于s的关系式，这里还要对该曲率进行进一步的平滑 ，因为对于非线性规划求解器，无论是目标函数还是约束都是要求函数能够二阶可导：
曲率之前是怎么来的呢？
上节课讲到了，规划路径是通过分段多项式的二次规划算法求得的，每一段的s和l都是保持的三阶导为定值的一个多项式的曲线关系，它可以保证sl是二阶可导，但是求得s的曲率实际上并不是二阶可导的，所以这里需要对曲率曲线重新做一个拟合，生成s关于曲率二阶可导的一个关系式。这里apollo同样采用分段多项式的方法获得s关于曲率kappa的关系式。首先对s进行采样，然后求解s关于曲率的分段多项式的关系式。
限速函数既不连续也不可导，所以要对限速曲线进行平滑；
对于速度规划问题，我们如何计算一个初始解：
分段多项式二次求解作为非线性规的初值
原文链接：https://mp.weixin.qq.com/s/GJO8_DwyJHI2rQ0j05dGYw
