通达信行情接口c++源码-自动驾驶决策规划算法第二章Apollo EM Planner理论篇

模型搭建完毕，接下来适配算法

控制算法运行需要提前运行EMplanner那个文件让变量放到工作区内

问题：已知，未知，求

横向LQR模块

打开之后进入Audi这个，然后打开动力学模型，

以上个周期的matchpoint为起点做遍历，一旦有l_{i+1}>l_{i}，立刻退出遍历li对应的点作为本周期的匹配点

进入模型，给输出增加一个28的demux

最后两个接口接出来，输出出去，然后加上8的增益

PreScan提供环境搭建，传感器模型等等。CarSim提供车辆动力学模型。Matlab主要是写算法和跑仿真的工具。此外需要C++的编译器，使用VS201

打开后将matlab工作路径改成刚才新建的testplanner

点击车，进入里面去，

定位模块：把prescan自身传感器的数据输出出去，这里不研究定位算法

前三个是方向盘转角，油门和制动压力。从第四个开始是坡度，从第16-23个是路面附着系数。

过长的路径，障碍物投影可能不唯

接下来build然后regenerate再run，在visviewer里面humanview

第一次运行时，遍历

充分利用上一个规划周期的结果

打开EMPlanner_cs.slx，注意不能直接复制粘贴全部

可以正常跑起来，接下来运行testctrl.m

build完打开matlab，不能直接打开，需要在桌面右下角打开

接下来选择吧cpar解压放在那里，在PreScan的实验文件夹中新建一个DynamicModel文件夹确定

这种情况下，一般不会出现这种情况，规划周期是100ms，即使以50m/s运动，车也只是走了5m，5m内道路不太可能出现这么扭曲的几何，在上个规划周期的匹配点上一般只有一个极小值

打开prescan，新建一个实验，名为testplanner

老王的github主页：VincentWong3

接下来CarSim会导入数据，

仿真需要以下软件：注意版本也要对应

采样

再把输入输出接口粘贴过来

接下来去CarSim设置输入输出

减少规划频率，规划算法每100ms执行一次，控制算法每10ms执行一次

蓝色为规划轨迹，黄色为跟踪的轨迹，仿真平台搭建成功

期望：快，节省计算量

点不够的情况

这种做法太慢，导航路径很可能很长，遍历很费时

上面讲的平滑算法在Apollo中叫做Femsmoother

这样优化二次规划计算量就会少很多

这里的buff给的是0.1

普通做法：每个规划周期都做遍历

输入为11112也就是23个输入，对应CarSim的输入，前三个是方向盘转角，油门和制动压力，做一个接口。

更改如下：

缺点：无法保证曲率是连续的，添加曲率约束求解较麻烦

选择这个文件

原因：决策规划流程

转角关系在这里

综上，平滑算法的数学表达如下

把输入输出复制进去

前置准备工作

回到外面，设置四个输出，vx，vy，ax，ay，然后连上去

2n行，2n-4列的矩阵记为A_{1}^{T}

决策算法对障碍物做决策开辟最优的凸空间，然后搜索轨迹交给控制执行

回到PreScan，对车右键，选择第一个

下面看下曲率怎么算

转向不足的积分补偿

仿真采用PreScan+Matlab+CarSim的联合仿真

落入下面的绿点，实际匹配点是在上面那个绿点

如果这三个长度加起来越小，说明越接近原路径几何

这个流程比较粗略，暂时没考虑动态障碍物

仿真时间也30S

然后点击车辆动力学模型左下角箭头

决策规划仿真平台搭建

这里的simfile.sim我们要手动绑定一下

④规划算法在凸空间中搜索出最优路径

打开prescan自带的模型

⑤后处理，在规划轨迹中选一个点，坐标转换成Cartisian，输出给控制去跟踪

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

不平滑。中蓝色点导数不连续

此方法可以大大加速找匹配点算法

此外还有优化一阶和二阶导数的方法，这种方法则优化变量更多，二次规划规模比较大。

怎样才能快呢？答：方法不唯一

万一出现这种情况，使用双保险：increasecount变量记录l增加的次数

可以尝试在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;