EM planner学习

城市道路中，规划模块通常需要提供一个至少八秒或者二百米的轨迹，为了能留出更多的安全驾驶的空间。
运动规划算法的执行时间非常重要，在紧急情况下系统反应时间需要达到100ms以内，人类的反应时间大约是300ms。
EMplanner是一个基于EM类型迭代算法，迭代地优化路径和速度。
在车道级别的运动规划中，最优性和时间消耗都很重要。因此，很多自动驾驶规划算法在参考线的帮助下，在frenet坐标系和时间三个轴下开发算法来减小规划的维数。在frenet坐标系找到一个最优轨迹本质上是一个三维的带约束的最优控制问题。
有两种典型的求解最优轨迹的方法，第一种是直接法，另一种是路径速度解耦的方法。
直接法是直接找到时空最优的轨迹，通过lattice搜索，随着时间空间搜索精度的增加，搜索的复杂度也会增加，如果减小搜索精度，那么生成的轨迹就是次优的。
路径速度解耦的方法分开优化路径和速度，路径优化只考虑静态障碍物，然后在生成的路径上进行速度规划。路径速度解耦的方法在有动态障碍物的环境下就可能不是最优的了。

EMplanner的多车道策略

下图是EMplanner的整体框架，

数据中心收集和同步所有信息，然后会根据交通规则和障碍物在不同车道生成一些参考线。然后对于每一条车道，首先根据本车道的车道线构建一个frenet坐标系，然后计算车和周围环境的关系，最后将这些信息发送给车道优化器。
车道级别优化器分成路径优化和速度优化，在路径优化中，首先将周围环境映射到frenet坐标系（E-step），然后基于映射的信息生成一条光滑的path（M-step），在速度优化中，障碍物被投影到st图上（E-step），然后速度优化器可以优化出一条平滑的速度profiles（M-step），结合路径和速度profiles，最后就得到了一条单车道的光滑的轨迹。
最后，将多个车道生成的轨迹发送到轨迹决策器，决策器根据车辆当前状态，交规和轨迹cost决定一条最好的轨迹。

EMplanner单车道

车道级别的规划一次规划周期包括两次E-step和两次M-step，如下图所示，

在第一次E-step中，静态和动态障碍物会被映射在frenet坐标系中，静态障碍物直接根据笛卡尔和frenet坐标系的关系转换，动态障碍物通过预测得到障碍物的轨迹，然后加上本车上一次规划的轨迹可以计算出两车即将发生碰撞的位置和时间，两车碰撞的重叠区域也将会映射到frenet坐标系。因为路径优化会产生nudge，所以为了安全起见SL映射只考虑低速和迎面驶来的障碍物。高速驾驶中，emplanner 采用换道策略。
在第二次E-step中，基于生成的path，所有的障碍物都会被映射到ST图上，如果障碍物轨迹和path有重合区域就会被映射到ST图。
在两次M-steps中，路径和速度profiles都是通过动态规划和二次规划结合生成的。尽管把障碍物投影到了SL和ST图上，路径和速度优化的解还是在非凸空间，因此使用动态规划获得一个粗略的解，这个解还可以提供一些决策，类似nudge，超车和跟车等。还可以使用这个粗略的解为二次规划生成一个凸包，这样优化就可以在凸空间求解了。

SL和ST映射（E-step）

不重复写了。

M-step DP path

路径优化器希望在非凸(在障碍物左边或右边nudge是两个局部最优解)的SL空间找到相对于s的最优的l。所以可以把路径优化分成两步：基于动态规划的决策和基于样条的路径规划。
动态规划这步提供了一个粗略的解，一个可行的通道（凸）和从哪边nudge障碍物的决策，这一步包括了lattice采样，cost function 和动态规划搜索，如下图所示，

下图是在frenet坐标系下的lattice采样，

首先在车前采样很多行点，不同行的点之间使用五次多项式连接。不同行之间点的距离取决于车速，道路结构，是否换道等，如换道可能需要更长的采样间隔，然后出于安全考虑这个littice的长度至少能行驶八秒或200米。
当littice图构建完成之后，使用投影到SL系的信息，交通规则和车辆动力学来构建cost，每条边的costfunction是平滑性，碰撞项和车道cost的线性组合，如下所示，

平滑项的cost function如下，

f一阶导和heading相关，二阶导和曲率相关，三阶导和曲率导数相关，文中说因为是多项式，所以这项cost可以解析求解。
碰撞项cost function如下，

这个costfunction基于车和障碍物轮廓之间的距离d，小于dc就用Ccollision，这是一个很大的值，在dc和dn之间用一个递减函数，离dc越远，cNudge越小，如果大于dn，那这项就不起作用了。
lane costfunction包括guidance linecost和on-code cost，guidance linecost如下，

这项主要是惩罚离道路中心线的距离。onrodecost取决于路径边界，超出边界的点会有一个很高的惩罚。
然后这些边的cost会被动态规划搜索用来搜索出一条cost最低的路径。这条路径也做出了从障碍物哪边nudge的决策。

M-step 样条QP path

因为前端dp生成的路径不是最优的，所以后端要用QP对dp的粗略路径进行优化，优化空间也是根据dp路径生成的，如下图所示，

样条QP路径通过求解一个带有线性约束的QP问题得到，下图是QP的框架，

QP的cost function是平滑项和guidance line的线性组合，此时的guidance line是DP生成的path，整体cost function如下，

这个目标函数是平滑性和nudging障碍物的权衡。
QP的约束是边界约束和动力学可行性，采样s得到n个点，每个s对应的l应该在最小l和最大l之间，但是只考虑点还不行，还需要考虑车的朝向，如图所示，

为了让边界约束是严格凸的而且是线性的，在车的前轴中心和后轴中心添加了两个半圆。
求出左前角的l公式如下，

因为yaw通常很小，所以可以线性化如下，
，
其他几个角也可以得到。
l的二阶导和三阶导可以用于动力学可行项。
因为所有约束都是线性的，所以这个问题可以被QP求解器很快求解出来。

M-step DP speed

路径优化器利用st图生成一个速度profile，表示为每一时刻的s。
和路径优化类似，速度优化的解空间也是非凸的，所以也是结合DP和QP生成一个速度profile。
DP速度优化框架如下图所示，

DP速度这一步设计costfunction， 栅格化ST图和动态规划搜索三步，最后得到一个分段线性的速度profile，一个可行的通道和对障碍物的决策。
ST图中的障碍物信息首先被离散化成栅格，按时间轴把时间离散成n份，分段线性速度profile用s0到sn表示，之后速度加速度加加速度都可以通过差分法得到，如下所示，

DP速度优化器的cost function如下，

第一项是保持参考速度的项，Vref取决于道路速度限制，曲率和其他交通规则，g函数对小于参考速度和大于参考速度进行惩罚。第二项和第三项分别是加速度和jerk的积分，共同描述速度profile的平滑性，最后一项是障碍物项，通过车到所有障碍物的距离决定。

M-step速度优化

因为DP生成的分段线性速度profile不能满足动力学需要，加速度不连续了，所以用QP来继续优化，框架如下图所示，

这一步包括设计cost function，线性化约束和QP求解。
cost function描述成下面形式，

第一项惩罚优化的速度profile和DP生成的速度profile的距离，加速度和jerk的积分项描述速度profile的平滑性，因此，这个目标函数是DPguidance line和平滑性的权衡。
边界约束如下：

第一个约束是保证单调性，因为不需要倒车，后几项约束要从交通规则和车辆动力学约束获得。

求解二次规划问题的注意事项

为了安全考虑，路径和速度优化大约选择了100个位置和时间点，约束个数超过六百个。不管是路径优化还是速度优化，分段五次多项式已经足够好了。样条通常包括三到五个多项式大约30个参数，因此，二次规划问题目标函数很小但是约束个数很多，因此，active setQP求解器很适合求解这类问题，使用上一个周期的结果做暖启动，qp问题求解时间平均3ms，满足时间消耗需求。

用DP和QP求解非凸优化问题的注意事项

DP和QP求解非凸问题都有自己的局限性，所以将它俩结合可以互相取长补短得到一个理想的解。

1. DP：DP求解依赖于采样生成的一堆解，由于执行时间的限制，采样的解个数被限制了，这样只能产生一个粗略的DP解，不会产生最优解。例如DP能选择从哪边nudge但是不能用最适合的距离nudge。
2. 没有DP的帮助，QP可能会求解失败，QP需要一个决策，一个随机的或者基于规则的决策可能会使QP求解失败或者陷入局部极小值。
3. DP+QP：EMplanner首先使用DP在栅格中搜索出一个粗糙的解，用这个解生成凸可行域并指导QP，QP在凸可行域内更可能得到全局最优解。

案例研究

大多数杰出的规划算法都基于重决策，但是EMplanner是基于轻决策的规划器。重决策或者严重基于规则的算法更容易理解和解释，但是缺点也很明显，可能会陷入极端情况，比如决策频率和规则的数量相关，不总是最优的。下面将会用一些例子来说明轻决策算法的好处。下图展示了EMplanner是怎么在规划周期之内和规划周期之间迭代并找到最优轨迹的。这个例子是主车以十米每秒速度行驶，有一个动态障碍物也以十米每秒和主车相反的速度驶向主车，EMplanner会迭代地生成路径和速度。

图a是主车在没有遇到障碍车之前用历史的轨迹以十米每秒的速度朝前行驶。
图b开始第一次路径迭代，用本车历史轨迹和障碍车的预测轨迹推算出在s等于 40的时候会发生碰撞，因此最好的办法是在s等于40时候从障碍车右边nudge过去。
图c开始第一次速度迭代，此时基于第一次路径优化得到的从右边nudge的路径，生成一个速度profile，乘客肯定希望在障碍物旁边通过时速度放慢，所以主车通过障碍车时速度降为5m/s。
图d开始第二次路径迭代，基于第一次速度迭代生成的新的速度profile，然后结合障碍车预测轨迹计算出主车将会在s等于30时从障碍车右侧通过，所以nudge障碍物的路径应该更新为在s等于30的时候取得最大的nudge距离。
图e开始第二次速度迭代，因为在s等于30时nudge，所以在s等于40减速不再必要了，所以生成新的速度profile会在s等于30缓慢通过，之后就可以加速了。
经过以上四步迭代得到了最终的在s等于30时nudge的轨迹。
需要注意不一定每次都需要迭代四步，迭代次数取决于场景的复杂度，越复杂的场景需要越多步迭代。
因为三维slt的优化问题被分成了sl和st两个二维问题，EMplanner的计算复杂度大大降低，因此这个规划器非常有效。每次规划平均小于100ms。