写在前面：毕业后就转行了，没做完笔记，反正也没人看
目录
参考以下博主的文章、视频（主要是基于老王的视频）：
忠厚老实的老王的个人空间_哔哩哔哩_bilibilihttps://space.bilibili.com/287989852
Apollo轨迹规划技术分享 - 程十三 - 博客园 (cnblogs.com)https://www.cnblogs.com/liuzubing/p/11051390.html
导论
自动驾驶级别分类
区分准则：功能（L0-L2）、责任（L3-L5）
决策规划是L2到L2+的重要模块，进阶的关键不在于算法而在于责任定位，不过越往上，决策部分就越重要。
决策规划分类
决策规划分为：路径规划->行为决策（难点）->轨迹规划
路径规划：位形 轨迹规划：运动状态
利用cost函数量化评估标准
采用一阶导数极值求cost函数最小值的难点：
极值点很多,难从快速计算
约束复杂,是许多不连续小区间并集，处理较繁琐
因此求解复杂约束下的最小值问题采用迭代法求解
迭代法有如下几种：1.牛顿法 2.梯度下降法 3.高斯牛顿法
梯度下降法：利用一阶导数。导数为负，正向移动，导数为正，反向移动；导数大移动大，导数小移动小
缺点：对初值敏感，可能会收敛到局部最优解
凸优化问题
对初值不敏感的优化问题称为：凸优化
凸优化两性质：
- cost函数仅有一个极值点且为极小值。
- 约束空间完整不破碎。
凸优化问题可以简单描述为：一个问题如果有两个可行解，即x与y可行，则要满足这两个可行解的线性组合（x+y）/2也是可行的，且不比这两个可行解都差。
横向规划中这条红线的变道轨迹明显是不能用的——它变道太慢，骑线行驶距离过长。所以，根据数学定义，横向规划是非凸优化问题。
纵向规划中当一个行人横穿马路时，我们可以用白色矩形在t - s图上表示此过程。面对这样场景，纵向规划将有两种选择，一种是车要让人，对应图中白色矩形下方的绿色路线；一种是车辆加速超过，即上方的绿色路线。但它的线性组合可能是图中的红线，而这条路线中行人与车已经撞上，显然是不可行的路线（解）。所以纵向规划也不是非凸优化问题。
解决非凸问题
这种启发式算法本质上是把约束空间离散并找出其中的最优解（粗解）
非凸空间->离散->粗解->迭代->最终解
采样少容易局部最优解，采样多容易发生维度灾难（一维100个，二维，三维
）
五次多项式
Jerk为位移的三阶导数，代表加速度变化快慢，也代表乘客舒适程度 ，轨迹规划就是在各种约束的情况下尽量使乘客舒适，即Jerk最小。
使总Jerk 最小的两个离散点之间的位移函数
可以借助五次多项式求出。
结论：
途中f三阶导即为Jerk，平方是为了避免绝对值所带来的烦恼。
直角坐标与自然坐标转换
并非拟合的多项式次数越高拟合精度越好，尽可能采用分段低次坐标系。
结论
路径决策
什么是自动驾驶的决策模块呢？从数学上来讲，决策就是为了限定非凸问题（轨迹规划）的解空间，将问题转化为凸的。
比如在躲避碰撞时，根据决策选择左绕右绕可以让非凸优化问题转变成凸优化问题
参考线模块
参考线
是指当导航路线过长、不平滑时，用于避免以下问题
- 坐标转换不顺利
- 障碍物投影
- 不平滑，乘坐不舒适
而拟合的用于投影建议坐标轴的跟原路线相近的线。
每个规划周期内，找到车在导航上的投影点，以投影为坐标原点，往后取30米往前取150米范围内的点做平滑，平滑后的点的集合称为参考线。
一种平滑算法
约束1:判断是否平滑
约束2：判断是否与原路径点相似
约束3：判断是否紧凑
综上：
利用二次规划得出最适合的
寻找匹配点
第一次寻找：遍历，寻找匹配点，找到后按一定长度向前检查避免局部最小值
非第一次寻找：与上一次匹配点连线，矢量判断遍历方向，少量检查
检查：
当选中的点往继续遍历一定点后仍为最小值则认定为最小值
参考线拼接：现已不再用
动态规划
动态规划过程是：每次决策依赖于当前状态，又随即引起状态的转移。一个决策序列就是在变化的状态中产生出来的，所以，这种多阶段最优化决策解决问题的过程就称为动态规划(DP)。
规控周期不同步
直接使用原代码中的控制接口会导致两个两个进程不同步。如规划100s一次，控制10s一次，则控制就会循环10此规划数据，造成控制的结果不好，所以将规划中增加时间戳，发送一整条带的轨迹给控制可以解决这个问题
确定规划起点
错误做法：因为控制不完美，每次以直接按定位投影作为起点会导致规划线割裂,control参考状态突变。
正确做法:规划起点应考虑规划算法运行时间，采用轨迹拼接的方法使控制连续平滑。
即选取绝对时间中的（3）
1.做拼接、控制，绝对时间更直观方便。
2.为了保证每时刻都依照规划而控制必须提前规划，所以选择规划延后一段时间的内容，规划时按照上个周期已经规划好的轨迹运行。
避障
本质上是在使cost函数（包括平滑、与障碍物距离、与参考线距离)最小的情况下选择最佳路径。
避障空间为非凸空间，为了解决这个问题，我们可以选择一个方向前进，如左拐，这样就形成了凸空间，而选择转向是决策的工作，所以决策本质上是转化非凸问题。
那么如何选择这个方向？（如何决策？）
答：通过离散化的方法，启发式搜索出一个粗解，以粗解为基础优化出最终解。
如何在离散空间上找到粗解？动态规划。（善于决策，开辟凸空间）
利用得出的粗解得到开辟的凸空间，再在此基础上做二次规划，得到最终解。
如何凸空间优化出最终解？二次规划。（二次规划善于在连续空间——凸空间中找最优解）
搜索路径过程
已知离散点状态（中间点一阶二阶导均设为0，因为是粗解所以不影响结果）根据五次多项式求得路径→根据cost函数（平滑代价、障碍物距离代价、参考线距离代价）求出个路径代价→利用算法求出最优解。
此处用到的算法：
层层递接，最终在最后一层里选择cost最小的。
轻决策与重决策
轻决策：基于代价函数。
优点：
- 无人为规则，可以处理复杂场景。
- 有粗解，通过设计代价函数可以使粗解“满足”硬约束（碰撞、最小曲率），这样使二次规划求解成功的几率大大增加，也避免形成的凸空间过于扭曲。（至少比放宽约束条件的粗解要舒缓）
缺点：
- 复杂场景计算量大。
- 依赖预测。
- 对感知要求高，要求对环境全知。
- 代价函数设计/最优解未必符合人的驾驶习惯。
重决策：基于人给定的规则。
缺点：
- 场景太多，人难免疏漏。
- 人给出的决策所开辟的凸空间未必满足约束。
优点：
- 计算量小。
- 对感知要求不高。
目前：L2+高速L3 => 重决策 L4=> 轻决策 博弈问题：轻重决策结合
二次规划
lmin 到 lmax构成了凸空间，二次规划的解空间在凸空间中搜索。
等式约束：分段加加速度优化法
假设连接两个轨迹点间三阶导数为常数
通过泰勒展开，将每个点 l 与 l' 的泰勒展开式联立化简为矩阵
将所有的点放入一个矩阵
将大矩阵简记，此为等式约束 
不等式约束
要考虑汽车的体积
近似处理：
为了保守起见可以设立上下界。
不等式约束最后化简为
总不等式约束为：
二次规划通过约束跟代价函数求得最优的
，最后对应到相应的转化为笛卡尔坐标系
2116
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
