b站老王 自动驾驶决策规划学习记录（五）

自动驾驶之参考线模块

上一讲：b站老王 自动驾驶决策规划学习记录（四）
接着上一讲学习记录b站老王对自动驾驶规划系列的讲解

参考视频：

1. 自动驾驶决策规划算法第二章第二节(上) 参考线模块
2. 自动驾驶决策规划算法第二章第二节(中) 参考线模块

1 决策规划总体概览

已有导航路径，决策规划的流程

1. 定位+导航：生成参考线
2. 静态障碍物投影到以参考线为坐标轴的$frenet$坐标系上
3. 决策算法对障碍物做决策（往左绕，往右绕，忽略）开辟最优凸空间
4. 规划算法在凸空间中搜索出最优的路径
5. 后处理，在规划轨迹中选一个点，坐标转成Cartesian，输出给控制去跟踪

2 参考线reference line

参考线是解决导航路径过长，不平滑问题的方案。
如下图所示导航路径

（1）过长的路径不利于坐标转换(找匹配点，计算s)
（2）过长的路径，障碍物投影可能不唯一
（3）不平滑

解决方案：参考线

每个规划周期内，找到车在导航路径上的投影点，以投影点为坐标原点，往后取30m，往前取150m范围内的点，做平滑，平滑后的点的集合称为参考线。

2.1 参考线平滑算法

$|\overrightarrow{P_1{P}_3}|$衡量平滑与不平滑的标准。$|\overrightarrow{P_1{P}_3}|$越小，越平滑。

如下图，虽然平滑了，但是几何形状差距太大。

$|\overrightarrow{P_1{P}_{1r}}|+|\overrightarrow{P_2{P}_{2r}}|+|\overrightarrow{P_3{P}_{3r}}|$越小，越接近原路径几何。

长度尽可能均匀、紧凑

总结：

$x_1,x_2,x_3$为优化后的路径点。
希望cost function越小越好

本质是二次规划问题

2.1.1 平滑代价

n个点：


平滑代价：$w_{smoot{h}_{}cost}.x^T{A}_1^T{A}_{1}x$
$A_1^T$ size:(2n,2n-4)
$A_1$ size:(2n-4,2n)

2.1.2 紧凑代价

2.1.3 几何相似代价

2.2 二次规划

约束：
$x=(x_1,y_1,...,x_n,y_n)$
$x_{ref}=(x_{1r},y_{1r},...,x_{nr},y_{nr})$
$x$与$x_{ref}$不要太远

曲率约束：非线性约束
曲率约束一般与车的最大侧向加速度有关

2.3 Fem smooth平滑

平滑算法，在Apollo中叫$Fe{m}_{}smoother$
优点：优化变量少
缺点：无法保证曲率是连续的，添加曲率约束求解较麻烦

做出来不难，做好很难
期望：快，节省计算量
原因：决策规划流程

参考线是决策规划的基础和前提，必须要快
快的方案：
（1）减少规划频率，规划算法会100ms执行一次，控制算法每10ms执行一次。
（2）充分利用上一个规划周期的结果

如何快速找到匹配点？
普通做法：每个规划周期做遍历

太慢，导航路径很可能很长，遍历很费时。
改进：

以上个周期的match point为起点做遍历，一旦有$l_{i+1}>l_i$，立即退出遍历。$l_i$对应的点作为本周期的匹配点。
Tips:判断遍历的方向


此方法可以大大加速找匹配点算法
问题：只适用于上个匹配点结果附近只有一个极小值点

一般情况下不会，规划周期是100ms，即使以50m/s运动，车也只是走了五米。五米内的道路不太可能出现这么扭曲的几何，在上个规划周期的匹配点上一般只有一个极小值。
解决：
increase_count 变量记录l连续增加的次数

在Github算法里
（1）遍历
第一次运行遍历

不是第一次运行

（2）点不够的情况

（3）参考线拼接

具体做法：

比较

Fem至少需要3个点。