0、摘要
提出了一种基于轨迹优化的高效实时自动驾驶运动规划方法。将轨迹空间离散化->利用cost functions 来找到最佳轨迹。
引[CSDN1]： 规划器首先把规划空间离散化 ，并根据一组代价函数搜索最优轨迹，然后对轨迹的path和speed进行迭代优化。文中提出了一种“post-optimization”的方法，可以弥补离散化难以求出最优解的问题（离散越密，越接近最优解），并可以减少52%的规划时间。
1、介绍
A. 背景
自动驾驶复杂的情况与搜索方法的局限性，启发式搜索难以全部满足。时间维度、运动学约束、Jerk并没有好的方法同时处理。
B. 相关工作
论文1-12
C. 贡献
实现了一种平滑、连续、可行的运动规划器。通过高效的路线和速度离散化。通过后后处理补偿离散的问题。
2、算法框架
使用[3]曲率多项式来找到路径。使用[13]的方法实现一个随即方向的单一优化算法。
A、轨迹生成
A-1 路径生成
使用与[5]类似的方法，但是使用的是四次的曲率多项式->曲率变化率的连续性。
a) 端点采样
路中央的轨迹：横纵坐标，当前方向和曲率。
使用类似Frenet的方式去定义一个空间的点：
相当于将该处中线切圆的半径扩大了些。
b) 路径模型
将成对的采样点于采样端点以及当前车辆姿态连接起来。
三次拟合： 
。其中有五个约束项对应x，y的移动，起点到终点的方向，起点的曲率和终点的曲率。梯度下降算法可以解决这个问题（衍生于[3]）。
。其中有五个约束项对应x，y的移动，起点到终点的方向，起点的曲率和终点的曲率。梯度下降算法可以解决这个问题（衍生于[3]）。
从[5]中，三次曲率多项式适用于处理通常的曲率变化连续的场景。但是在双循环轨迹生成中，往往会出现轨迹的曲率并不是一直连续的[问题1]。加入了四次曲率多项式。仅用于从当前车辆位置启示的那一段路。
c) 速度生成
将速度空间离散化，设置一个三次多项式将他们连接起来。
速度多项式定义为： 
，使用了s-弧长来定义。四个未知变量（假设弧长是已知的），分别对应四个约束：初速度，初加速度，终速度，终加速度。其中，初始速度和初始加速度由实际传感器测得。定义了最大最小速度以及离散速度个数N_speed，将速度离散为： 
.
，使用了s-弧长来定义。四个未知变量（假设弧长是已知的），分别对应四个约束：初速度，初加速度，终速度，终加速度。其中，初始速度和初始加速度由实际传感器测得。定义了最大最小速度以及离散速度个数N_speed，将速度离散为： .
d) Cost 函数
包含的变量：静态变量和动态变量
静态希望更短、曲率变化更小、更靠近车道中心且远离障碍物
动态上期望更快、耗能更小、加速度更小、加加速度更小、向心加速度更小并且距离障碍物更远。
距离使用有关距离的计算：
其中，lambda 是带宽 g是距离
C、最优轨迹生成
将以上代价全部加起来，得到的代价函数。将路径和速度分开优化。应该是先选路程，再选速度。然后进入优化模块。
1) 路径优化
取消采样点的中朝向、横向偏移、以及曲率要求的约束[问题2]，增加了起始位置朝向偏移以及起始位置的曲率偏移两个变量。重新构建轨迹并且使用Simplex优化。（单纯形算法）
还剩沿道路的的横向距离...
2) 速度优化
这个过程中会改变到达任一节点的动态障碍物的位置，使得梯度算法难以使用。所以使用非衍生的 Simplx算法[问题3]。
D、性能评估
初始轨迹的以及速度的选取占用的时间最长，迭代几次其实消耗的时间并没有那么多。
3、系统实现
找到车辆最近的一个路点，确定一个算法结束的时候最近一个车辆肯定达不到的地方，从那儿开始进行规划。
针对于人工驾驶，总是以当前位置作为初始点开始。
995、阅读总结
该算法先通过离散的点选取了一条参考轨迹，然后使用单纯形方法进行迭代优化。从而在保证了轨迹质量的同时，降低了运算时间。
这个优化过程中似乎没有用到距离障碍物的距离，使得优化过程的安全性降低。
亮点：合理的分配轨迹预生成以及轨迹后优化。
缺点：优化时的单纯形算法对碰撞的影响。
997、阅读时存在的问题
1、但是在双循环轨迹生成中，往往会出现轨迹的曲率并不是一直连续的。
相邻周期的path计算时经过了一次速度优化，丢失了继承性？
[CSDN1] : 可以理解为相邻周期计算出的path没有继承性，转向会有突变。为了解决这个问题可以再引入一个新的约束：即当前车辆姿态点处曲率的一阶导数。那么此时就应当是4次多项式了。
998、 Linked Papers
A. Related Works
[1][2] 一系列测试平台的搭建
[3][4] 曲率多项式来确保速度和加速度->确保曲率变化得当
[5][6] 在[3][4]的基础上实现了将轨迹与道路形状贴合
[7] 时间五次多项式
[8] A Star ARA Star [9] RPT [10] D Star Lite
[11] 使用共轭梯度方法，速度不是最优的
[12] 梯度最优方法，很好的避开了静态障碍物，但是却没有联系时间维度
B、算法框架
[13] 一种随机方向的单一优化方法生成路径和速度
[5] [14] 一种前向的速度生成算法。
[7] [15] 一种反向的速度生成-使用了基于时间的多项式
[16] 一种车辆所在区域的表示方法
[17] Simplex 算法
[18] PMP-partial montion planning
999、词语积累
iterative v. 迭代的，重复的，反复的
resultant adj. 由此引起的
computational complexity n. 计算复杂度
commuter n. 通勤者
commercial adj. 商业的
cruise n. 巡航
constraint n.约束
convature n.曲率
comform v.符合
inverse adj. 相反的
profile n. 轮廓、总则
coincide with v. 吻合
derivative n. 派生物
feasible adj. 可行的
oriented adj. 以...为方向的
simplex adj.单一的
perpendicular adj. 正交的
vertex n. 顶点
discretized adj. 离散的
respectively adv. 分别地
lattice n. 晶格
granulatity n. 间隔尺寸、粒度
scheme n. 计划
参考文章:
CSDN 1： 自动驾驶之轨迹规划7—A Real-Time Motion Planner with Trajectory Optimization for Autonomous Vehicles文献阅读_IHTY_NUI的博客
