解析论文：A real_time motion planner with trajectory optimization for autonomous vehicles
论文的核心结构是这样的：
1，规划上是时空分离的，横向规划是建立曲率kappa关于位置s的函数，纵向上建立速度v关于位置s的函数。
2，函数建立后使用lattice planner 的方式进行撒点，生成对应的cost function,选择cost 最低的曲线作为最优曲线。
3，完成曲线选取之后还多做一步优化，在时间和空间上进行滚动优化，进一步完善输出结果。
总体的结构如图：
接下来我们分析每一个模块。
空间上的曲线生成：
关于空间上的曲线生成，我们有一些数学推导，我写在了手稿中：
可以看出，按照lattice planner的方法，我们希望写出一个曲率kappa关于位置s的多项式，至于多项式应该是几阶的，取决于我们定多少约束。文章中列举了我上图中写的五个约束：起点处的曲率，终点处的曲率，终点处的x位置，终点处的y位置，终点处的heading 对于起点处heading的变换量。五个约束意味着多项式应该是4阶的，所以作者使用了四阶多项式进行空间上的曲线生成。
作者是这么写的，到底是会议论文，质量就是不行。说的是quartic四阶多项式，写的却是三阶，还乱扯说s也是个位置参量？我反正是佛了。再次强调，这里是四阶多项式。
时间上的曲线生成
作者强调，时间上的曲线生成也应该是关于位置s的多项式，而不是关于时间t的多项式，因为我们刚才在空间上做的曲线生成已经选择了 函数是关于位置S的函数，因此为了保证一致性，这里也是关于s的函数。作者表示这里选择一个三阶函数就差不多了。
三阶多项式的参量有四个，我们定义四个约束即可：
作者选择了起始点和终点的速度和加速度，其中起点的速度和加速度是从车上直接获得的，终点的速度我们自己采样，终点的加速度我们希望是0，这样直接可以获得这几个参量的close form的表达：
cost function:
分成了静态cost 和动态 cost, 论文里面大家用的都差不多，没什么好解释的：
最后的cost function 就是所有项的加权累计：
进一步速度和轨迹优化，
所谓的进一步优化就是花里胡哨，做法就是稍微放松对终点的约束。对于轨迹优化来说就是我们虽然选择了某条最优曲线，这个规划终点的状态都已定好，现在我们放松一点这个约束，终点可以在这个点的附近稍微动一动，做到微调。速度优化也是一样，终点的采样速度也可以在小范围内微调。这种优化没有意义，因为你不一定保证微调后的曲线能够通过碰撞检测，所以你额外还要做一次碰撞检测，另外微调的目的是让曲线更合理，而对曲线合理性的评估已经通过cost function 给出来了，这里就没有必要多此一举了。
