自动驾驶决策规划算法第二章Apollo EM Planner理论篇

前置学习内容：【自动驾驶】【零基础】基础自动驾驶控制算法笔记

注意：最好学习前置控制算法，因为决策规划仿真中需要用到

决策规划算法第一章链接

决策规划仿真平台搭建

软件安装

老王的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;