【自动驾驶】决策规划算法（一）决策规划仿真平台搭建 | Matlab + Prescan + Carsim 联合仿真基本操作

写在前面：
🌟 欢迎光临 清流君 的博客小天地，这里是我分享技术与心得的温馨角落。📝
个人主页：清流君_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$ 都改成 $0, 0$ 。 $z$ 不用改：

在这里插入图片描述

改完后按回车，车辆就移动到 $(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 Hz$ 才行，不然会不收敛。上面是 Prescan 自带的仿真频率，因为只提供环境传感器的信息，只要 $20 Hz$ 就可以，但动力学模型必须要以较快的频率运行，否则不会收敛。所以下面要改成 $1000 Hz$ 。

频率过高的话计算量会变大，所以电脑可能计算不了，如果不能是 $1000 Hz$ 的话，至少也要是 $200 Hz$ 以上，最低也要 $200 Hz$ 。同时 Carsim 的步长也得改成 $200 Hz$ 。在本篇博客使用的是 $1000 Hz$ ，Carsim 也是 $1000 Hz$ ，总之同步即可。

(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$ 分钟，此过程只做一次即可，第二次打开就很快，大概几秒钟就弄好了。

(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$ 函数 CarSim S-Function2。

若提示找不到模块，尝试把 S-Function2 中的 $2$ 去掉。

点开 $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]$ ，共有 $23$ 个输入，正好对应 Carsim 输入也是 $23$ 个。

前三个是方向盘油门、刹车。作为接口开放出来。

$v_x$ 和 $v_y$ 是现成的，都输出来，但 $a_x$ 和 $a_y$ 没有，所以要在 Carsim 中添加两个输出量接口 $a_x,a_y$ 。

在这里插入图片描述

点击Send to Simulink，在 Simulink 中点击 CarSim2PreScanSTATE 模块，查看输出是 $[333 - 1]$ 也就是前面有 $9$ 个了，一共是 $37$ 个，所以后面应该还有 $28$ 个，使用 Debux 模块设置 $28$ 个输出，把最后两个接口引出来，就是 $a_x$ 和 $a_y$ 。

在这里插入图片描述

注意单位换算，因为单位都是 $g$ ，所以要乘 $9.8$ 变成 $m/s^2$ 。

这样整个模型基本的 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_OFFLINE
k=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);
end
LQR_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$ 秒。运行一下看看。

报错信息如下：

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 。

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)$ 点位置上，跟踪的还是不理想，那就可能就是软件的版本问题。

或者再从头看一遍本篇博客，保证做的步骤和的本篇博客步骤一模一样，再看一看是不是漏了什么步骤。

如果是软件版本问题，换成推荐的软件版本，因为软件版本的问题没法解决。

最后看一下testctrl.m 脚本写了什么，其实就是画了两个图，host_x 和 host_y 是定位给出的车辆实际轨迹，xr_desire， yr_desire 是规划轨迹。其实就画了两个图，如果这两条线贴得比较近，就像像刚才图一样，那就证明这可以完美的跟踪，就认为控制算法没问题。

3.4 速度与加速度跟踪

当然除了位置跟踪，还有速度跟踪和加速度跟踪，也可以把速度曲线、加速度速曲线都画出来，看一下贴的近不近。如果觉得自己不满意的话，也可以调控制参数，调成满意的结果。

四、总结

希望大家都要自己亲手做一遍，把仿真环境搭好，以后所有的规划算法都会在此模型上去运行。

没有仿真平台的话，后续的内容就没法学了。所以一定要亲手把仿真环境弄好，把参数调好，调成两条曲线贴近的样子才可以。

本篇博客到此结束，欢迎关注后续内容。

参考资料

自动驾驶决策规划算法第二章第一节决策规划仿真平台搭建

后记：

🌟 感谢您耐心阅读这篇关于 决策规划仿真平台搭建 | Matlab + Prescan + Carsim 联合仿真基本操作 的技术博客。 📚

🎯 如果您觉得这篇博客对您有所帮助，请不要吝啬您的点赞和评论 📢

🌟您的支持是我继续创作的动力。同时，别忘了收藏本篇博客，以便日后随时查阅。🚀

🚗 让我们一起期待更多的技术分享，共同探索移动机器人的无限可能！💡

🎭感谢您的支持与关注，让我们一起在知识的海洋中砥砺前行 🚀