现在分析单个车道的规划过程。
首先，迭代是发生在不同的cycle之间。计算新轨迹是基于上一个周期计算的轨迹结果来开始的。
SL投影：
障碍物投影到F坐标。
静态障碍物：
直接通过Cartesian-Frenet 转换（笛卡尔坐标到F坐标）。
动态障碍物：
在Apollo中通过障碍物移动轨迹描述，结合上一个cycle计算得到的轨迹，可以评估动态障碍物和ego car之间的在不同时间点的位置关系。其中两者重叠的部分会被标记在F坐标中。
（由于动态障碍物往往会导致闪避nudge，所以SL投影里只会考虑低速和靠近中的障碍物）
-
ST投影：
根据生成的path，将所有障碍物（包含高速的）投影到station-time frame，这个ST图是基于参考线做的，但无论是障碍物的轨迹，还是自车的轨迹，都是基于每条path来存储及对比是否重叠的以及何时重叠的。
如果障碍物轨迹与规划轨迹重叠，便标记出这个区域。
-
两个M-Steps：
通过动态规划获取一个rough solution（从此获取障碍物策略：闪躲、让道、超车等），然后基于此决定二次规划样点优化器的凸空间主体（a convex hull for the quadratic-programming-based spline optimizer）。
（虽然所有计算都基于凸空间——内部所有点的连接线都在空间内——但实际上最优轨迹在非凸空间里。）
-
以上模块再详细来分析一下：
A. SL and ST Mapping (E-step)
SL投影基于G2(continuous curvature derivative) 平滑参考线。
在笛卡尔坐标系，障碍物和自车状态通过定位和朝向(x,y,θ) ，车道曲度及曲度变化率(κ, dκ) 来描述。
在F坐标下，这些信息转换成(s, l, dl, ddl, dddl) ，即station、lateral及它的导数。
静态障碍物与时间无关，因此坐标转换能直接进行。
动态障碍物的坐标转换需要上一次计算出的自车轨迹。首先自车轨迹投射到F坐标，获取the station direction speed profile（提供自车在某个时间点的station坐标，从而评估与动态障碍物的相对关系）。自车station坐标与障碍物轨迹重叠部分就可以计算出来，如下图紫色部分：
ST投影能帮助评估自车的speed profile。
当path投影到F坐标后，若障碍物轨迹会跟自车产生交互，就会在图上显示。
上图中，一个障碍物在2秒后驶入自车道40m前；空白部分就是speed profile的可选空间。
B. M-Step DP Path
查找横向坐标l = f(s) 的最优解（在SL空间中基于station坐标计算），包含两步：
动态规划
spline-based 规划
（但实际上Apollo 6.0之中，path的DP过程被简化成获取粗略可选空间的path boundary，并不需要复杂的cost函数优化求解过程。）
包含cost计算、lattice采样、动态搜索。其中lattice采样需要在自车前获取数row的点，不同row的点平滑连接。row里撒点的间距由速度、道路结构、换道等决定，可以自定义。距离覆盖8秒或200米。
Lattice建立后，每个图边缘通过cost函数评估——基于SL投影、交规、车动态信息。总edge cost函数是“平滑性、避障、车道成本”的线性组合。
平滑cost函数中，f ′ (s)代表车道与自车的偏差角， f ′′ (s)是车道曲率。w则代表不同项的权重。
障碍物cost函数，与障碍物距离d大于设定的安全值dn时为0，小于碰撞距离dc则是无穷大。
参考线cost，代表与参考线的距离成本，其中g(s)是参考线函数。
C. M-Step Spline QP Path
QP的目标，是在DP产生的path可选空间中，通过QP spline solver对由线性化约束的目标函数进行最优化，在其内生成一条平滑的path。
QP的目标函数是平滑cost和guidance line cost的线性组合。后者用到的比较项是DP产生的可选空间里的大量path，用于评估障碍物的闪避距离。然后通过优化找最优解的方式，找出其中cost最低，也就是最优的path。
由于车辆的朝向会影响车辆边界与周围的位置关系，也就是需要确定车辆四个角与station点的关系，而不只是l=f(s)。
这样就需要知道车头朝向，其他三个角类似。
边界的cost函数都可以进行线性化处理，随后就可以用二次规划快速求解。
D. M-Step DP Speed Optimizer
DP在ST图里生成speed profile。包含cost函数，ST图栅格，动态搜索。
具体来说，首先障碍物信息分散到ST的栅格中，栅格根据间隔dt平均划分，速度的具体参数大概可以认为：
目标函数是令cost最低：
Vref 是参考速度，一般是车道的限速。第一项代表维持速度的cost，其后两项代表平滑性，最后是障碍物cost——自车与所有障碍物的距离。
E. M-Step QP Speed Optimizer
其中必须满足的边界约束：
平滑拟合的速度曲线：
F. Notes on Solving Quadratic Programming Problems
基于安全考虑，Apollo EM评估的点约100个，约束条件大于600个。
QP中计算结果包含3到5个多项式，约30个参数。
因此二次规划的目标函数较小而约束较多，求解较容易。同时为了加速计算，采用上周期的结果作为计算起点（hot start），平均3ms就能得到结果（看算力）。
G. Notes on Non-convex Optimization With DP and QP
DP和QP在非凸空间都有各自的不足。
DP：依赖于采样栅格，有限的栅格数量只能给出一个粗略的DP解。即很可能不是最优解。譬如DP能得出向左闪躲的路径，但闪躲的距离不一定是最好的。
QP：相反，QP解依赖于凸空间，因此不能脱离DP使用。譬如，没有DP的decision，QP无法决定该怎么走。
DP+QP：将各自的不足最小化。先找一个粗略的解，再在这个凸空间里找最优。
