写在前面：
? 欢迎光临 清流君 的博客小天地，这里是我分享技术与心得的温馨角落。?
个人主页：清流君_CSDN博客，期待与您一同探索 移动机器人 领域的无限可能。
? 本文系 清流君 原创之作，荣幸在CSDN首发?
若您觉得内容有价值，还请评论告知一声，以便更多人受益。
转载请注明出处，尊重原创，从我做起。
? 点赞、评论、收藏，三连走一波，让我们一起养成好习惯?
在这里，您将收获的不只是技术干货，还有思维的火花！
? 系列专栏：【决策规划】系列，带您深入浅出，领略自动驾驶决策规划的魅力。?
愿我的分享能为您带来启迪，如有不足，敬请指正，让我们共同学习，交流进步！
? 人生如戏，我们并非能选择舞台和剧本，但我们可以选择如何演绎 ?
感谢您的支持与关注，让我们一起在知识的海洋中砥砺前行~~~
文章目录
引言一、仿真平台搭建1.1 软件安装与配置1.2 Prescan 与 Carsim 联合仿真设置(1) 找到联合仿真文件(2) 设置车辆位置(3) 设置仿真频率(4) 构建模型 1.3 Matlab 与 Prescan 联合仿真操作(1) 打开 Matlab(2) 配置接口(3) 编译 C++ 程序(4) 调整工作路径 1.4 适配算法与仿真平台搭建(1) 加载数据(2) 参数设置(3) 复制粘贴算法(4) 匹配输入输出接口 二、各模块算法概述2.1 定位模块2.2 决策规划模块2.3 控制模块 三、仿真结果与分析3.1 模型算法运行与调试3.2 摄像头视角设置与可视化3.3 轨迹跟踪效果分析3.4 速度与加速度跟踪 四、总结参考资料
引言
本篇博客是自动驾驶决策规划算法第二章的第一节，是正式第二章讲决策规划算法的开篇。内容整理自 B站知名up主 忠厚老实的老王 的视频，作为博主的学习笔记，分享给大家共同学习。
本系列将理论与代码实践结合，即边讲理论边写代码，就像搭积木一样，慢慢地把规划算法拼出来，这样对各位来说是更容易的学习方式。所以在 Githab 上传了一份代码，各位可以把它下载下来，作为整个规划系列的基础，可以把这份代码理解为地基，以后就拿这份代码作为基础不断地迭代。
每讲到新的知识，就写成代码，拼到基础的代码上去。随着学习的内容越来越丰富，代码也会随之变得越来越多、越来越丰富，直到最终完成决策规划的整个代码。
一、仿真平台搭建
本篇博客讲解如何使用这份代码，下载链接如下：
自动驾驶决策规划代码与模型Github地址
1.1 软件安装与配置
首先要下载这四个软件，缺一不可，而且版本也有严格要求。仿真平台是以 PreScan 为基础，即 PreScan +MATLAB + Carsim 三个软件联合仿真。
MATLAB R2020a
主要写算法和跑仿真的工具
Carsim 2019.1
提供车辆动力学习模型
PreScan 8.5
提供传感器感知环境信息，搭建道路环境
MinGW-64
PreScan 和 Matlab 联合仿真需要用的 C++ 编译器
下面讲解这三个软件的操作。
1.2 Prescan 与 Carsim 联合仿真设置
(1) 找到联合仿真文件
首先找到 PreScan 自带的和 Carsim 联合仿真的文件，如果装了 PreScan ，一定能找到，要用倒数第四个 Demo_prescanCarsim3D 文件：
点进去，就会找到 Demo_PreScanCarSim3D.cpar 文件：
找到文件后，打开 PreScan ，关于 PreScan 的具体操作就不细讲了， PreScan 的操作其实很简单，而且 B 站上也有相关教程，可以适当学一学。
下面直接讲怎么做。首先打开PreScan，先新建实验，起个名字：
首先点 Actors，把 Audi A8 Sedan 车拖出来。
可以看到 Actors 里有很多车，但只能拖奥迪 A8 出来，不能拖别的车，必须是奥迪A8 。
(2) 设置车辆位置
拖出来之后在右边 Location 这把位置改一下，把车辆起点的 x , y x,y x,y 都改成 0 , 0 0,0 0,0。 z z z 不用改：
改完后按回车，车辆就移动到 ( 0 , 0 ) (0,0) (0,0) 位置上。
Prescan 先放一边，打开 Carsim ，选右下角最后的选项：
选择刚才所说的 Demo_PreScanCarSim3D.cpar 文件。
Demo_PreScanCarSim3D.cpar文件其实就是 Prescan 自己做的和 Carsim 联仿的类似于接口的东西。 c part 它他打包了 Carsim 相关的数据，比如底盘数据、转向数据、轮胎数据，是类似于压缩包一样的东西，所以在这里就是选择案例， CarSim 软件要把压缩包里的东西提取出来，但压缩包只能用 CarSim 自带的软件提取。
选择文件后，提示要把压缩包里的动力学数据提取到哪个文件夹下面。理论上可以放在任何地方，但推荐最好放在 Prescan 的 Experiments 下面，比较方便，因为所有 Prescan 的工程新建文件都会放在 Experiments，这样比较好找。
给文件夹起名字，最好不要用中文名，可能会遇到各种各样的 BUG，比如就起动力学模型的英文名 DynamicModel。
点确定后， Carsim 就会把 .cpar 文件解压缩到 DynamicModel 文件夹里。
看一下 Carsim 的输入和输出，输入有很多个，输出也有很多个。
这样， Carsim 的动力学模型就弄好了。现在要用 Prescan 和 Carsim 一起仿真，所以在 Prescan 中也要做相应的设置。
回到 Prescan ，首先右键小车，在 Object configuration 里选 Dynamics，勾选用户自定义，DynamicModel 里有 Extensions，找到 Simulink，选择CarSim_VehicleDynamics.mdl 文件。
(3) 设置仿真频率
再点最上面像时钟一样的按钮，设置仿真频率（或者叫迭代步长）。
对于 Carsim 的动力学模型来说要 1000 H z 1000Hz 1000Hz 才行，不然会不收敛。上面是 Prescan 自带的仿真频率，因为只提供环境传感器的信息，只要 20 H z 20Hz 20Hz 就可以，但动力学模型必须要以较快的频率运行，否则不会收敛。所以下面要改成 1000 H z 1000Hz 1000Hz。
频率过高的话计算量会变大，所以电脑可能计算不了，如果不能是 1000 H z 1000Hz 1000Hz 的话，至少也要是 200 H z 200Hz 200Hz 以上，最低也要 200 H z 200Hz 200Hz。同时 Carsim 的步长也得改成 200 H z 200Hz 200Hz。在本篇博客使用的是 1000 H z 1000Hz 1000Hz ，Carsim 也是 1000 H z 1000Hz 1000Hz，总之同步即可。
(4) 构建模型
最后要点屏幕上面的 Build 按钮，如果不点，所有工作都白做。
Build 的作用是什么呢？
Build 是把 Prescan 相关数据自动生成为 Simlink 模型。所以在 Prescan 里做过任何相关的改动，比如加了什么车，或者把车移到什么位置，哪怕就是把车移一下位置，做完后都得 Build 一下，因为会根据新的位置重新生成 Simlink 模型。
1.3 Matlab 与 Prescan 联合仿真操作
(1) 打开 Matlab
Build 完后就可以打开 Matlab 了。但如果想要 Prescan 和 Matlab 联合仿真的话， Matlab 不能直接双击打开，要按照以下操作去做。
(2) 配置接口
首先右键屏幕右下角 PreScan 的小图标，选择 open，在这里有 Matlab ，点 Start
(3) 编译 C++ 程序
要在这里打开 Matlab，会配置跟 Prescan 相关的接口，不能直接在桌面上双击。
如果是第一次用 Prescan 和 Matlab 联仿，会调用 MinGW-64 编译器做一系列 C++ 相关程序的编译，过程会比较长，大概 5 − 10 5-10 5−10 分钟，此过程只做一次即可，第二次打开就很快，大概几秒钟就弄好了。
(4) 调整工作路径
当一切准备完毕后，Matlab 的工作路径会自动跳转到 Prescan Experience 路径下。
把工作路径改成 Prescan 刚建的工程 TestEMPlanner文件夹下。
TestEMPlanner文件夹里多了很多东西，首先有 TestEMPlanner_cs.slx 文件，这就是 Prescan Build 出来的 Simulink 模型。
1.4 适配算法与仿真平台搭建
打开模型后会先读取 TestEMPlanner.pb 文件，生成相关配置，模型如下图所示。
(1) 加载数据
模型左上角是 Prescan 图标，右上方的按钮叫 Regenerate，当在 Prescan 里 Build 后，在 Simulink 里还要再点 Regenerate 重新生成一下，改动才能在 Simulink 里生效。
也就是说，无论在 Prescan 里做了什么改动，加了什么道路，加了什么障碍物之类的，首先在 Prescan 里要 Build，在 Simulink 里要 Regenerate 。而且每次刚打开时，会自动 Regenerate 一下。
(2) 参数设置
双击到车的模块里看一下，里面有车辆动力学模型：
点向下的箭头就进去了
里面有 Casim 的 S S S 函数 CarSim S-Function2。
若提示找不到模块，尝试把 S-Function2 中的 2 2 2 去掉。
点开 S S S 函数，需要给出 simfile 文件的具体路径。
simfile 文件目前还没有，所以要先生成 simfile，打开 Carsim ，点击Send to Simulink，Carsim就会自动调用 Matlab，在 DynamicModel 文件夹里就生成了 simfile 文件。按住 Shift 加右键，复制文件地址，粘到上面去就可以了。
注意：需要删除粘贴出来的引号。
最后再配置一下 Carsim 的输入和输出。
设置输入可以参考模板，因为 Prescan 有自带的 Prescan 和 Carsim 联仿的模型，参考模型的设置来设置自己的模型。在 .cpar 文件所在的位置，里面有 Simulink 模型，打开模型看一下。
打开车辆模型，里面有设置好的输入、输出，粘过来即可。
如果 Carsim S-Function 是错误的状态，可能是名字不对，尝试把 Carsim S-Function 改成 Carsim S-Function2。
如果没问题，就不要动它，把输入和输出粘到自己的模型上去。
看一下输入。
是 [ 1 1 1 12 8 ] [1\ 1\ 1\ 12\ 8] [1 1 1 12 8]，共有 23 23 23 个输入，正好对应 Carsim 输入也是 23 23 23 个。
前三个是方向盘油门、刹车。作为接口开放出来。
v x v_x vx 和 v y v_y vy 是现成的，都输出来，但 a x a_x ax 和 a y a_y ay 没有，所以要在 Carsim 中添加两个输出量接口 a x , a y a_x,a_y ax,ay。
点击Send to Simulink，在 Simulink 中点击 CarSim2PreScanSTATE 模块，查看输出是 [ 333 − 1 ] [3 3 3 -1] [333−1] 也就是前面有 9 9 9 个了，一共是 37 37 37 个，所以后面应该还有 28 28 28 个，使用 Debux 模块设置 28 28 28 个输出，把最后两个接口引出来，就是 a x a_x ax 和 a y a_y ay。
注意单位换算，因为单位都是 g g g，所以要乘 9.8 9.8 9.8 变成 m / s 2 m/s^2 m/s2。
这样整个模型基本的 Carsim 和 Prescan 之间的相关接口就已经全部完成了。
(3) 复制粘贴算法
接下来开始适配算法，打开 GitHub 上下载的文件。
把这三个文件复制粘贴到 Prescan 的 TestEMPlanner工程根根目录下。先在 Prescan 里 Regenerate 刷新一下。
看一下这三个文件到底是什么。
emplanner_init.m初始化文件，加载数据。
emplanner_init.m
%%%%%EM PLANNER初始化与配置文件，主要是加载全局路径，加载油门刹车标定表，设置规划和控制参数等等%%%%加载全局路径和油门刹车标定表%加载油门刹车标定表，标定表是以前已经做好的，不用标，直接用就可以。load('table_calibration.mat')%%%%%前轮转角与方向盘转角的映射关系right_wheel_ground=[-70 ,-67.2 , -64.4 , -61.6 , -58.8 , -56 , -53.2 , -50.4 , -47.6 , -44.8 , -42 , ... -39.2 , -36.4 , -33.6 , -30.8 , -28 , -25.2 , -22.4 , -19.6 , -16.8 , -14 , -11.2 , -8.4 , -5.6 , ... -2.8 , 0 , 2.8 , 5.6 , 8.4 , 11.2 , 14 , 16.8 , 19.6 , 22.4 , 25.2 , 28 , 30.8 , 33.6 , 36.4 , ... 39.2 , 42 , 44.8 , 47.6 , 50.4 , 53.2 , 56 , 58.8 , 61.6 , 64.4 , 67.2 , 70 ];rack_displacement=[-39.14 , -37.2 , -35.29 , -33.43 , -31.6 , -29.81 , -28.06 , -26.34 , -24.66 , ... -23.01 , -21.38 , -19.79 , -18.23 , -16.69 , -15.18 , -13.7 , -12.23 , -10.8 , -9.38 , -7.98 ,... -6.61 , -5.25 , -3.91 , -2.59 , -1.29 , 0 , 1.27 , 2.54 , 3.78 , 5.02 , 6.24 , 7.46 , 8.66 , ... 9.86 , 11.05 , 12.24 , 13.41 , 14.59 , 15.76 , 16.92 , 18.09 , 19.25 , 20.42 , 21.59 , 22.76 , ... 23.93 , 25.11 , 26.3 , 27.5 , 28.71 , 29.94];%转向系统特性c_factor=43.75;%%单位: mm/rev%%%%%%%%参数设置%%%%%%DEG2RAD=pi/180;RAD2DEG=180/pi;%%%%整车参数%%%%%cf=-175016;cr=-130634;m=2020;Iz=4095.0;la=1.265;lb=2.947-1.265;%%%%%%%横向LQR参数LQR_Q1=25;LQR_Q2=3;LQR_Q3=10;LQR_Q4=4;LQR_R=15;%%%%纵向双PID参数KP_PID_distance=0.5;KI_PID_distance=0.0;KD_PID_distance=0.0;KP_PID_speed=1.8;KI_PID_speed=0;KD_PID_speed=0;%%%%%LQR_OFFLINEk=zeros(5000,4);vx_break_point=zeros(1,5000);for i=1:5000 vx_break_point(i)=0.01*i; A=[0,1,0,0; 0,(cf+cr)/(m*vx_break_point(i)),-(cf+cr)/m,(la*cf-lb*cr)/(m*vx_break_point(i)); 0,0,0,1; 0,(la*cf-lb*cr)/(Iz*vx_break_point(i)),-(la*cf-lb*cr)/Iz,(la*la*cf+lb*lb*cr)/(Iz*vx_break_point(i))]; B=[0; -cf/m; 0; -la*cf/Iz];LQR_Q=1*[LQR_Q1,0,0,0; 0,LQR_Q2,0,0; 0,0,LQR_Q3,0; 0,0,0,LQR_Q4]; k(i,:)=lqr(A,B,LQR_Q,LQR_R);endLQR_K1=k(:,1)';LQR_K2=k(:,2)';LQR_K3=k(:,3)';LQR_K4=k(:,4)';%%%%车辆初始位置host_x_init=0; host_y_init=0;这些其实都是自动驾驶的控制算法所需要的参数，运行一下脚本。运行完毕后，工作区就有这些变量了，无论是油门、刹车和标定表，还是纵向的 PID 参数，以及横向的 LQR 参数，都已经标好了，直接用就可以，当然如果觉得标定不满意，也可自己标，就是用脚本修改相关参数，运行一遍即可。
(4) 匹配输入输出接口
脚本运行完后，把 GitHub 上的 Simulink 模型打开，把算法整个算法粘贴过来，把输入输出接口匹配上
这样整个仿真平台搭建完毕。
二、各模块算法概述
下面简要介绍一下算法。
2.1 定位模块
首先是定位模块。
定位模块也非常简单，就是直接把 Prescan 自身传感器的数据输出来。因为不做定位算法，所以定位算法直接用 Prescan 自带的定位传感器即可。但因为本篇博客做的是通用仿真平台，所以如果各位对定位算法有兴趣的话，可以直接在定位模块里写相关算法，在这里就不写定位模块了，因为本系列讲的是决策规划，不讲定位。
2.2 决策规划模块
再看决策规划模块。
决策规划模块是本系列的重点，但现在这里还没东西，目前规划算法是在自动驾驶控制算法 12 节里所写的规划。此处写的规划其实没什么用，大家也不用看规划，纯粹是为了验证控制算法，测试轨迹跟踪性能。以后此模块就是工作重点，会在模块里写相关算法，把 EM Planner 算法完成。
所以规划的 MATLAB Function 在测试完后把删掉即可。
2.3 控制模块
最后来看控制模块已经，已经写完了，输入是定位规划以及预瞄输出，就是方向、前轮转角、油门、刹车以及相关的 debug 调试信息。
首先是主函数 control main
主函数里分横向控制 LQR 以及纵向控制双 PID，还有因为转向不足所加入的积分补偿部分。
补偿没有开，目前是关着的。
以及从前端转角转化为方向盘转角相关的部分。
标定表为右前轮转向系统与齿条位移之间的映射关系，在 emplanner_init.m 文件中定义。齿条位移又和 c_factor 作用变成方向盘转角的映射关系。但这里 LQR_final_angle，是 LQR 自己算出来的 angle，是左前轮和右前轮合并出来的自行车模型的 angle。但汽车毕竟不是自行车，所以自行车模型的 angle 要转化成左右前轮转角，所以自行车模型的转角并不等于右前轮的转角。这里还要使用阿克曼几何转化成右前轮转角。
刹车标定表已经弄好了，横向控制是离线 LQR 查表法。
离线 LQR 的标定表、油门刹车标定表，以及相关的 PID 的控制参数都在 emplanner_init.m 脚本文件里。所以运行模型前，要先运行 emplanner_init.m ，把这些参数都写到工作区里，这样 Simulink 就能读这些数据。
三、仿真结果与分析
3.1 模型算法运行与调试
下面运行模型算法，规划时间为 30 30 30 秒，所以仿真也设置为 30 30 30 秒。运行一下看看。
报错信息如下：
Invalid setting in
‘testemplanner_cs/Audi_A8_Sedan_1/Dynamics_UserSpecified/CarSim
S-Function2’ for parameter ‘vs_state’. Caused by: Error evaluating
parameter ‘vs_state’ in
‘testemplanner_cs/Audi_A8_Sedan_1/Dynamics_UserSpecified/CarSim
S-Function2’ 函数或变量 ‘vs_state’ 无法识别。 Variable ‘vs_state’ has been
deleted from base workspace.
在命令行中加上 vs_state =-1 和 StopMode=-1
可看到车辆运行。
3.2 摄像头视角设置与可视化
在 Prescan 里面设置跟随车的摄像头，点击倒数第二个Visu Aids 添加，选择 Custom，添加视角，设置摄像机的视角位置。
先 Build 再 Regenerate 运行，在弹出的 VisServer 中将视角切换为HumanView_1 。
3.3 轨迹跟踪效果分析
怎么判断控制跟踪轨迹的好坏呢？
写个脚本 testctrl.m
plot(control_debug.LQR_debug.host_x.data,control_debug.LQR_debug.host_y.data,control_debug.xr_desire.data,control_debug.yr_desire.data); 首先运行一下模型， Simulink 模型运行完后有 Control debug 变量，testctrl.m 就会读取 Control debug 相关信息，把跟踪轨迹和车辆实际的轨迹画出来。
蓝色线是规划轨迹，黄色线是跟踪轨迹。如果最终 testctrl.m 运行的结果和上图差不多，就证明已经成功了，控制可以完美跟踪规划轨迹，这样仿真平台就已经搭建成功了。
如果没有跑出来这样的效果，相差比较大，那就要检查检查了，检查一下车辆初始位置是不是在 ( 0 , 0 ) (0,0) (0,0) 点，因为规划比较傻，只能在 ( 0 , 0 ) (0,0) (0,0) 点上规划才有用。车辆如果放在其他位置，规划就跟踪不了了。因为这只是测试，实际上的规划肯定不会像现在这样，如果车辆在 ( 0 , 0 ) (0,0) (0,0) 点位置上，跟踪的还是不理想，那就可能就是软件的版本问题。
或者再从头看一遍本篇博客，保证做的步骤和的本篇博客步骤一模一样，再看一看是不是漏了什么步骤。
如果是软件版本问题，换成推荐的软件版本，因为软件版本的问题没法解决。
最后看一下testctrl.m 脚本写了什么，其实就是画了两个图，host_x 和 host_y 是定位给出的车辆实际轨迹，xr_desire， yr_desire 是规划轨迹。其实就画了两个图，如果这两条线贴得比较近，就像像刚才图一样，那就证明这可以完美的跟踪，就认为控制算法没问题。
3.4 速度与加速度跟踪
当然除了位置跟踪，还有速度跟踪和加速度跟踪，也可以把速度曲线、加速度速曲线都画出来，看一下贴的近不近。如果觉得自己不满意的话，也可以调控制参数，调成满意的结果。
四、总结
希望大家都要自己亲手做一遍，把仿真环境搭好，以后所有的规划算法都会在此模型上去运行。
没有仿真平台的话，后续的内容就没法学了。所以一定要亲手把仿真环境弄好，把参数调好，调成两条曲线贴近的样子才可以。
本篇博客到此结束，欢迎关注后续内容。
参考资料
自动驾驶决策规划算法第二章第一节 决策规划仿真平台搭建
后记：
? 感谢您耐心阅读这篇关于 决策规划仿真平台搭建 | Matlab + Prescan + Carsim 联合仿真基本操作 的技术博客。 ?
? 如果您觉得这篇博客对您有所帮助，请不要吝啬您的点赞和评论 ?
?您的支持是我继续创作的动力。同时，别忘了收藏本篇博客，以便日后随时查阅。?
? 让我们一起期待更多的技术分享，共同探索移动机器人的无限可能！?
?感谢您的支持与关注，让我们一起在知识的海洋中砥砺前行 ?