写在前面:毕业后就转行了,没做完笔记,反正也没人看
目录
参考以下博主的文章、视频(主要是基于老王的视频):
忠厚老实的老王的个人空间_哔哩哔哩_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' 的泰勒展开式联立化简为矩阵
将所有的点放入一个矩阵
将大矩阵简记,此为等式约束
不等式约束
要考虑汽车的体积
近似处理:
为了保守起见可以设立上下界。
不等式约束最后化简为
总不等式约束为:
二次规划通过约束跟代价函数求得最优的,最后对应到相应的
转化为笛卡尔坐标系