博客转载自：https://blog.csdn.net/adamshan/article/details/80779615
动作规划动作在无人车规划模块的最底层，它负责根据当前配置和目标配置生成一序列的动作，我们前面讨论的三次样条插值实际上只是一个简单的路径，而非我们最终能够执行的轨迹，本文介绍一种基于Frenet坐标系的优化轨迹动作规划方法，该方法在高速情况下的高级车道保持和无人驾驶都具有很强的实用性，是目前普遍采用的一种动作规划算法。
基于Frenet坐标系的动作规划方法由于是由BMW的Moritz Werling提出的，为了简便，我们在后文中也会使用Werling方法简称。在讨论基于Frenet坐标系的动作规划方法之前，我们首先得定义什么是最优的动作序列：对于横向控制而言，假定由于车辆因为之前躲避障碍物或者变道或者其他制动原因而偏离了期望的车道线，那么此时最优的动作序列（或者说轨迹）是在车辆制动能力的限制下，相对最安全，舒适，简单和高效的轨迹。
同样的，纵向的最优轨迹也可·以这么定义：如果车辆此时过快，或者太接近前方车辆，那么就必须做减速，那么具体什么是“舒适而又简单的”减速呢？我们可以使用 Jerk 这个物理量来描述，Jerk即加速度的变化率，也即加加速度，通常来说，过高的加加速度会会引起乘坐者的不适，所以，从乘坐舒适性而言，应当优化Jerk这个量，同时，引入轨迹的制动周期 TT , 即一个制动的操作时间：
为什么使用Frenet坐标系
在Frenet坐标系中，我们使用道路的中心线作为参考线，使用参考线的切线向量 t 和法线向量 n 建立一个坐标系，如下图的右图所示，这个坐标系即为Frenet坐标系，它以车辆自身为原点，坐标轴相互垂直，分为 s 方向(即沿着参考线的方向，通常被称为纵向，Longitudinal)和 d 方向(即参考线当前的法向，被称为横向，Lateral)，相比于笛卡尔坐标系(下图的作图)，Frenet坐标系明显地简化了问题，因为在公路行驶中，我们总是能够简单的找到道路的参考线(即道路的中心线)，那么基于参考线的位置的表示就可以简单的使用纵向距离(即沿着道路方向的距离)和横向距离(即偏离参考线的距离)来描述，同样的，两个方向的速度（s′和 d′）的计算也相对简单。
那么现在我们的动作规划问题中的配置空间就一共有三个维度：(s,d,t) , t 是我们规划出来的每一个动作的时间点，轨迹和路径的本质区别就是轨迹考虑了时间这一维度。Werling的动作规划方法一个很关键的理念就是将动作规划这一高维度的优化问题分割成横向和纵向两个方向上的彼此独立的优化问题，具体来看下面的图：
假设我们的上层（行为规划层）要求当前车辆在 t8t8 越过虚线完成一次变道，即车辆在横向上需要完成一个 Δd以及纵向上完成一个 Δs的移动，则可以将 s和 d分别表示为关于 t的函数：s(t) 和 d(t) (上图右图)，那么 d,s 关于时间 t 的最优轨迹应该选择哪一条呢？通过这种转换原来的动作规划问题被分割成了两个独立的优化问题，对于横向和纵向的轨迹优化，我们选取损失函数 C ，将使得 C 最小的轨迹作为最终规划的动作序列。而Werling方法中损失函数的定义，则与我们前面提到的加加速度 Jerk 相关。
Jerk最小化和5次轨迹多项式求解
由于我们将轨迹优化问题分割成了 s和 d两个方向，所以Jerk最小化可以分别从横向和纵向进行，令 p为我们考量的配置（即 s或 d），加加速度Jt关于配置 p 在时间段 t0-t1内累计的Jerk的表达式为：
现在我们的任务是找出能够使得 Jt(p(t))最小的 p(t)，Takahashi的文章: Local path planning and motion control for AGV in positioning中已经证明，任何Jerk最优化问题中的解都可以使用一个5次多项式来表示：
