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

清流君 2024-10-18 15:35:02 阅读 68

写在前面:

🌟 欢迎光临 清流君 的博客小天地,这里是我分享技术与心得的温馨角落。📝

个人主页:清流君_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 为基础,即 <code>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 文件:

在这里插入图片描述

点进去,就会找到 <code>Demo_PreScanCarSim3D.cpar 文件:

在这里插入图片描述

找到文件后,打开 PreScan ,关于 PreScan 的具体操作就不细讲了, PreScan 的操作其实很简单,而且 B 站上也有相关教程,可以适当学一学。

下面直接讲怎么做。首先打开PreScan,先新建实验,起个名字:

在这里插入图片描述

首先点 <code>Actors,把 Audi A8 Sedan 车拖出来。

在这里插入图片描述

可以看到 <code>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 ,选右下角最后的选项:

在这里插入图片描述

选择刚才所说的 <code>Demo_PreScanCarSim3D.cpar 文件。

Demo_PreScanCarSim3D.cpar文件其实就是 Prescan 自己做的和 Carsim 联仿的类似于接口的东西。 c part 它他打包了 Carsim 相关的数据,比如底盘数据、转向数据、轮胎数据,是类似于压缩包一样的东西,所以在这里就是选择案例, CarSim 软件要把压缩包里的东西提取出来,但压缩包只能用 CarSim 自带的软件提取。

选择文件后,提示要把压缩包里的动力学数据提取到哪个文件夹下面。理论上可以放在任何地方,但推荐最好放在 Prescan 的 Experiments 下面,比较方便,因为所有 Prescan 的工程新建文件都会放在 Experiments,这样比较好找。

给文件夹起名字,最好不要用中文名,可能会遇到各种各样的 BUG,比如就起动力学模型的英文名 DynamicModel

在这里插入图片描述

点确定后, Carsim 就会把 <code>.cpar 文件解压缩到 DynamicModel 文件夹里。

看一下 Carsim 的输入和输出,输入有很多个,输出也有很多个。

在这里插入图片描述

在这里插入图片描述

这样, Carsim 的动力学模型就弄好了。现在要用 Prescan 和 Carsim 一起仿真,所以在 Prescan 中也要做相应的设置。

回到 Prescan ,首先右键小车,在 <code>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) 构建模型

最后要点屏幕上面的 <code>Build 按钮,如果不点,所有工作都白做。

在这里插入图片描述

<code>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 联仿,会调用 <code>MinGW-64 编译器做一系列 C++ 相关程序的编译,过程会比较长,大概

5

10

5-10

5−10 分钟,此过程只做一次即可,第二次打开就很快,大概几秒钟就弄好了。

(4) 调整工作路径

当一切准备完毕后,Matlab 的工作路径会自动跳转到 Prescan Experience 路径下。

在这里插入图片描述

把工作路径改成 Prescan 刚建的工程 <code>TestEMPlanner文件夹下。

在这里插入图片描述

<code>TestEMPlanner文件夹里多了很多东西,首先有 TestEMPlanner_cs.slx 文件,这就是 Prescan Build 出来的 Simulink 模型。

1.4 适配算法与仿真平台搭建

打开模型后会先读取 TestEMPlanner.pb 文件,生成相关配置,模型如下图所示。

在这里插入图片描述

(1) 加载数据

模型左上角是 Prescan 图标,右上方的按钮叫 <code>Regenerate,当在 Prescan 里 Build 后,在 Simulink 里还要再点 Regenerate 重新生成一下,改动才能在 Simulink 里生效。

也就是说,无论在 Prescan 里做了什么改动,加了什么道路,加了什么障碍物之类的,首先在 Prescan 里要 Build,在 Simulink 里要 Regenerate 。而且每次刚打开时,会自动 Regenerate 一下。

(2) 参数设置

双击到车的模块里看一下,里面有车辆动力学模型:

在这里插入图片描述

点向下的箭头就进去了

在这里插入图片描述

里面有 Casim 的

S

S

S 函数 <code>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 联仿的模型,参考模型的设置来设置自己的模型。在 <code>.cpar 文件所在的位置,里面有 Simulink 模型,打开模型看一下。

打开车辆模型,里面有设置好的输入、输出,粘过来即可。

在这里插入图片描述

如果 <code>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​。

在这里插入图片描述

点击<code>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 的 <code>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 节里所写的规划。此处写的规划其实没什么用,大家也不用看规划,纯粹是为了验证控制算法,测试轨迹跟踪性能。以后此模块就是工作重点,会在模块里写相关算法,把 <code>EM Planner 算法完成。

所以规划的 MATLAB Function 在测试完后把删掉即可。

2.3 控制模块

最后来看控制模块已经,已经写完了,输入是定位规划以及预瞄输出,就是方向、前轮转角、油门、刹车以及相关的 debug 调试信息。

在这里插入图片描述

首先是主函数 <code>control main

在这里插入图片描述

主函数里分横向控制 LQR 以及纵向控制双 PID,还有因为转向不足所加入的积分补偿部分。

在这里插入图片描述

补偿没有开,目前是关着的。

以及从前端转角转化为方向盘转角相关的部分。

在这里插入图片描述

标定表为右前轮转向系统与齿条位移之间的映射关系,在 <code>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 =-1StopMode=-1

可看到车辆运行。

在这里插入图片描述

3.2 摄像头视角设置与可视化

在 Prescan 里面设置跟随车的摄像头,点击倒数第二个<code>Visu Aids 添加,选择 Custom,添加视角,设置摄像机的视角位置。

在这里插入图片描述

先 <code>Build 再 Regenerate 运行,在弹出的 VisServer 中将视角切换为HumanView_1

HumanView_1

3.3 轨迹跟踪效果分析

怎么判断控制跟踪轨迹的好坏呢?

写个脚本 <code>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 相关信息,把跟踪轨迹和车辆实际的轨迹画出来。

在这里插入图片描述

蓝色线是规划轨迹,黄色线是跟踪轨迹。如果最终 <code>testctrl.m 运行的结果和上图差不多,就证明已经成功了,控制可以完美跟踪规划轨迹,这样仿真平台就已经搭建成功了。

如果没有跑出来这样的效果,相差比较大,那就要检查检查了,检查一下车辆初始位置是不是在

(

0

,

0

)

(0,0)

(0,0) 点,因为规划比较傻,只能在

(

0

,

0

)

(0,0)

(0,0) 点上规划才有用。车辆如果放在其他位置,规划就跟踪不了了。因为这只是测试,实际上的规划肯定不会像现在这样,如果车辆在

(

0

,

0

)

(0,0)

(0,0) 点位置上,跟踪的还是不理想,那就可能就是软件的版本问题。

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

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

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

3.4 速度与加速度跟踪

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


四、总结

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

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

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


参考资料

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


后记:

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

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

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

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

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



声明

本文内容仅代表作者观点,或转载于其他网站,本站不以此文作为商业用途
如有涉及侵权,请联系本站进行删除
转载本站原创文章,请注明来源及作者。