前置学习内容:【自动驾驶】【零基础】基础自动驾驶控制算法笔记
注意:最好学习前置控制算法,因为决策规划仿真中需要用到
决策规划算法第一章链接
决策规划仿真平台搭建
软件安装
老王的github主页:VincentWong3
如果github下载慢,可以用下面链接下载:
仿真需要以下软件:注意版本也要对应
仿真采用PreScan+Matlab+CarSim的联合仿真
PreScan提供环境搭建,传感器模型等等。CarSim提供车辆动力学模型。Matlab主要是写算法和跑仿真的工具。此外需要C++的编译器,使用VS201
注意:CarSim这里要有Matlab2020a,然后选择它
前置准备工作
打开prescan,新建一个实验,名为testplanner
选择Actor,将AudiA8拖出来
接下来吧location改成,不用管
接下来启动CarSim选择右下角的这个选项
选择文件为Prescan里面的cpar文件,里面包含了一些底盘、转向,轮胎等等数据
接下来选择吧cpar解压放在那里,在PreScan的实验文件夹中新建一个DynamicModel文件夹确定
接下来CarSim会导入数据,
回到PreScan,对车右键,选择第一个
选择Dynamics,然后Userspcified
选择这个文件
接下来吧CarSim的模型仿真频率改成1000Hz,PreScan的不用改,20就行
点击build按钮,会把prescan的模型生成simulink模型
build完打开matlab,不能直接打开,需要在桌面右下角打开
第一次打开时间比较长
打开后将matlab工作路径改成刚才新建的testplanner
testplanner_cs.slx为prescanbuild出来的模型,打开这个模型
左上角Regenerate作用:在Prescan中改动后需要build然后点一下Regenerate
点击车,进入里面去,
然后点击车辆动力学模型左下角箭头
如果没有的话,这么解决
这里的simfile.sim我们要手动绑定一下
接下来去CarSim设置输入输出
前三个是方向盘转角,油门和制动压力。从第四个开始是坡度,从第16-23个是路面附着系数。
打开prescan自带的模型
打开之后进入Audi这个,然后打开动力学模型,
把输入输出复制进去
输入为11112也就是23个输入,对应CarSim的输入,前三个是方向盘转角,油门和制动压力,做一个接口。
这里用in1和ground和constant模块,附着系数注意为向量
原本的CarSim输出有35个,没有ax,ay,这里添加进去
然后点击sendtosimulink
进入模型,给输出增加一个28的demux
最后两个接口接出来,输出出去,然后加上8的增益
回到外面,设置四个输出,vx,vy,ax,ay,然后连上去
模型搭建完毕,接下来适配算法
打开github的文件,
将这三个文件复制到prescan的testplanner里面
回到prescan里面regene一下
打开emplanner_init.m运行一下,获得控制需要的参数,纵向pid横向lqr和油门刹车标定表都获得了
打开github下载的EMPlanner_cs.slx,把这部分复制到模型内
再把输入输出接口粘贴过来
这样就算搭建完毕了
定位模块:把prescan自身传感器的数据输出出去,这里不研究定位算法
决策规划模块:主要完成这一部分
下来是控制模块,输入是定位和规划,
转向不足的积分补偿
前轮转角转化为方向盘转角
转角关系在这里
横向LQR模块
预测模块以及离线LQR的查表
纵向双pid模块
这里标定表给好了
控制算法运行需要提前运行EMplanner那个文件让变量放到工作区内
规划时间是30S,
仿真时间也30S
这个时候报错:
解决办法
可以尝试在Export:Outputs右下角Optionalequations添加DEFINE_OUTPUTmod1_L1=0;-;mod1_L1DEFINE_OUTPUTmod2_L1=0;-;mod2_L1DEFINE_OUTPUTmod3_L1=0;-;mod3_L1DEFINE_OUTPUTmod4_L1=0;-;mod4_L1DEFINE_OUTPUTmod1_R1=0;-;mod1_R1DEFINE_OUTPUTmod2_R1=0;-;mod2_R1DEFINE_OUTPUTmod3_R1=0;-;mod3_R1DEFINE_OUTPUTmod4_R1=0;-;mod4_R1DEFINE_OUTPUTmod1_L2=0;-;mod1_L2DEFINE_OUTPUTmod2_L2=0;-;mod2_L2DEFINE_OUTPUTmod3_L2=0;-;mod3_L2DEFINE_OUTPUTmod4_L2=0;-;mod4_L2DEFINE_OUTPUTmod1_R2=0;-;mod1_R2DEFINE_OUTPUTmod2_R2=0;-;mod2_R2DEFINE_OUTPUTmod3_R2=0;-;mod3_R2DEFINE_OUTPUTmod4_R2=0;-;mod4_R2EQ_OUTMOD1_L1=/FZ_REF)*MUY_L1;EQ_OUTMOD2_L1=SIN,15/DR))*ABS;EQ_OUTMOD3_L1=/FZ_REF)*MUX_L1;EQ_OUTMOD4_L1=MIN,*ABS;EQ_OUTMOD1_R1=/FZ_REF)*MUY_R1;EQ_OUTMOD2_R1=SIN,15/DR))*ABS;EQ_OUTMOD3_R1=/FZ_REF)*MUX_R1;EQ_OUTMOD4_R1=MIN,*ABS;EQ_OUTMOD1_L2=/FZ_REF)*MUY_L2;EQ_OUTMOD1_L2=SIN,15/DR))*ABS;EQ_OUTMOD3_L2=/FZ_REF)*MUX_L2;EQ_OUTMOD4_L2=MIN,*ABS;EQ_OUTMOD1_R2=/FZ_REF)*MUY_R2;EQ_OUTMOD2_R2=SIN,15/DR))*ABS;EQ_OUTMOD3_R2=/FZ_REF)*MUX_R2;EQ_OUTMOD4_R2=MIN,*ABS;
下面去prescan设置一个跟随车的视角,选择custom
点右上角,然后改变XYZ的位置
接下来build然后regenerate再run,在visviewer里面humanview
可以正常跑起来,接下来运行testctrl.m
蓝色为规划轨迹,黄色为跟踪的轨迹,仿真平台搭建成功
打开EMPlanner_cs.slx,注意不能直接复制粘贴全部
更改如下:
首先是控制模块的双pid改为离散pid
其次是加入调度系统用来调控控制信号和规划信号的更新频率
决策规划总体概览
已有导航路径,决策规划流程
决策算法对障碍物做决策开辟最优的凸空间,然后搜索轨迹交给控制执行
如果往左绕,就在紫色空间中搜索。往右就蓝色搜索。这里有个评价标准
④规划算法在凸空间中搜索出最优路径
决策选择了蓝色空间,规划再搜索出一个路径
⑤后处理,在规划轨迹中选一个点,坐标转换成Cartisian,输出给控制去跟踪
这个流程比较粗略,暂时没考虑动态障碍物
过长的路径不利于坐标转换
过长的路径,障碍物投影可能不唯
不平滑。中蓝色点导数不连续
以这三个点做向量
是衡量平滑与不平滑的标准,越小,越平滑
如紫色的线,虽然平滑了,但是几何形状差距太大。
那么如何衡量几何形状呢?
如果这三个长度加起来越小,说明越接近原路径几何
紧凑性的衡量标准如下:
综上,平滑算法的数学表达如下
问题:已知,未知,求
Costfunction定义如下:
目的是选择合适的让代价函数越小
这是一个典型的二次规划问题
将平滑代价函数写成二次规划形式:
如果是n个点的形式
2n行,2n-4列的矩阵记为A_{1}^{T}
平滑代价可以写成
其中:
紧凑代价
直接写n个点
那么有紧凑代价:
几何相似代价
几何相似代价:
有总的Costfunction
与二次规划标准式对比有
下面是约束
这里的buff给的是0.1
此外还有曲率约束,一般是不需要的,曲率约束是非线性约束,处理起来比较麻烦
曲率约束一般与车的最大侧向加速度有关,可以放到后面考虑
下面看下曲率怎么算
这里就可以看到l是非线性的,取模这一步得开根号
算法优化
上面讲的平滑算法在Apollo中叫做Femsmoother
优点:优化变量较少
缺点:无法保证曲率是连续的,添加曲率约束求解较麻烦
此外还有优化一阶和二阶导数的方法,这种方法则优化变量更多,二次规划规模比较大。
方法流程
找匹配点
采样
Femsmooth平滑
做出来不难,做好很难。在工程上很难实现。
期望:快,节省计算量
原因:决策规划流程
怎样才能快呢?答:方法不唯一
优化思路:
减少规划频率,规划算法每100ms执行一次,控制算法每10ms执行一次
充分利用上一个规划周期的结果
如何快速找到匹配点?
普通做法:每个规划周期都做遍历
这种做法太慢,导航路径很可能很长,遍历很费时
以上个周期的matchpoint为起点做遍历,一旦有l_{i+1}>l_{i},立刻退出遍历li对应的点作为本周期的匹配点
tips,判断遍历的方向
本质有点像梯度下降
此方法可以大大加速找匹配点算法
问题:只适用于在上个匹配点结果附近只有一个极小值点
落入下面的绿点,实际匹配点是在上面那个绿点
这种情况下,一般不会出现这种情况,规划周期是100ms,即使以50m/s运动,车也只是走了5m,5m内道路不太可能出现这么扭曲的几何,在上个规划周期的匹配点上一般只有一个极小值
万一出现这种情况,使用双保险:increasecount变量记录l增加的次数
那么如果检查到了另外的极小值怎么办呢
在Github算法里
第一次运行时,遍历
不是第一次运行的话,把上次的匹配点当成起始点,往后找3个点如果没发现另外极小值就可以
点不够的情况
要保持总长度不变,即二次规划的矩阵规模不变,
轨迹拼接问题
具体做法如下:
计算这个周期需要拼接多少点,把上个周期的两个点去掉,这个周期新加的两个点加进去。前面的点上个周期平滑过了,已经有结果了,所以这个周期只需要平滑新的点就可以了
只需要优化新加进来的点即可,前面不需要动的两个点视为等式约束,后面为不等式约束,
这样优化二次规划计算量就会少很多
此外还有个问题,如何快速的找到车在全局路径下的投影,如果找不到投影没法往前150m往后30m这个操作。
代码实践部分
2022年9月27日
请等待后续更新
文章为作者独立观点,不代表 股票程序化软件自动交易接口观点