Apollo planning论文解析(1)

本文解析Apollo发表于ITSC2019的一篇规划方面的论文：Optimal Trajectory Generation for Autonomous Vehicles Under Centripetal Acceleration Constraints for In-lane Driving Scenarios
文章简述了规划模块的in lane driving 的纵向规划的最新方案之一，更确切的说是不带变道的部分。文章分为两个主要部分，第一部分是参考线生成，第二部分是如何生成一条path-time 轨迹。本文解决的问题不考虑变道，默认车辆能够很好的沿着参考线轨迹运动，横向偏差可以快速通过控制进行纠正。可以看出，文章的方法没有使用DP环节，而是直接使用了LCQP（linear constraint quadratic programming) 在实践过程中，我也发现DP给整个规划带来的提速并不多，此外DP的主要作用是解决QP问题要去解一个非凸问题，（QP有最优解的充分必要条件是：目标函数，可行解集，约束均为凸的）比如向左右两侧变道具有对称性，约束就有可能非凸的（注意：QP问题的目标函数永远是至少半正定的，所以一定是凸的或者拟凸的）。对于本文所论述的问题，我们发现：第一，目标函数一定是凸的，可行解集是正的全空间，也是凸的，约束是框约束，凸函数或者是仿射集，也是凸的。那么QP就一定有最优解，所以可以直接跳过DP了。

第一部分：参考线生成

参考线的输入是来自地图的离散点，通过参考线生成算法，将离散点连接成一段段光滑曲线。并且参考线使用了theta(s)的形式来表达光滑曲线。生成轨迹的目标是让参考线尽量贴合原来的离散点。轨迹生成的具体方法并没有详细叙述，但是大概也是个QP问题，带上一些约束，并不复杂。最后，轨迹可以写成piece-wise quintic polynomial的形式：

很容易通过求导数就可以得到kappa关于s的表达式：

第二部分：st轨迹的生成

文章通过使用st图的信息来规划纵向轨迹。比如说如下图的场景，参考线已经获得后，通过预测模块就可以知道障碍物关于这条参考线在st上的关系：

在加上车辆动力学约束，车子的加速度是有上下限的，以此选出一块s_free,就是绿色部分围成的区域，那么车辆就要在避撞得绿色区域中规划出合理的轨迹。那么怎么评判轨迹呢：有下面几个考虑点：
纵向加速度，纵向jerk，离心加速度，时间，具体的描述如下：


对应的约束就可以被写为：

接下来，文章还分析了一下求解QP离散化的方法。很直观可想到两个离散化的方法，要么可以在s上面离散化，要么可以在t上面离散化，通过分析发现离散t更好，具体可以看文章，这里不赘述，接下来我们看通过t离散化前和后的问题形式。
离散化前目标函数写成了：

目标函数里面惩罚了这么几项：
纵向加速度，jerk, 离心加速度，与参考速度的偏差（所谓的参考速度就是没有障碍物下的定速巡航速度），与终点期望状态的位置，速度，加速度偏差。

离散化后：

其中：

由于是离散点，当然还有两点之间的连续性条件：

要保证任意两点之间的三阶连续。