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

写在前面:

🌟 欢迎光临 清流君 的博客小天地,这里是我分享技术与心得的温馨角落 。📝
个人主页清流君_CSDN博客,期待与您一同探索 移动机器人 领域的无限可能。

🔍 本文系 清流君 原创之作,荣幸在CSDN首发🐒
若您觉得内容有价值,还请评论告知一声,以便更多人受益。
转载请注明出处,尊重原创,从我做起。

👍 点赞、评论、收藏 ,三连走一波,让我们一起养成好习惯😜
在这里,您将收获的不只是技术干货 ,还有思维的火花

📚 系列专栏:【决策规划】系列,带您深入浅出,领略自动驾驶决策规划的魅力。🖊
愿我的分享能为您带来启迪,如有不足,敬请指正,让我们共同学习,交流进步!

🎭 人生如戏,我们并非能选择舞台和剧本,但我们可以选择如何演绎 🌟
感谢您的支持与关注,让我们一起在知识的海洋中砥砺前行~~~


文章目录

  • 引言
  • 一、仿真平台搭建
    • [1.1 软件安装与配置](#1.1 软件安装与配置)
    • [1.2 Prescan 与 Carsim 联合仿真设置](#1.2 Prescan 与 Carsim 联合仿真设置)
      • [(1) 找到联合仿真文件](#(1) 找到联合仿真文件)
      • [(2) 设置车辆位置](#(2) 设置车辆位置)
      • [(3) 设置仿真频率](#(3) 设置仿真频率)
      • [(4) 构建模型](#(4) 构建模型)
    • [1.3 Matlab 与 Prescan 联合仿真操作](#1.3 Matlab 与 Prescan 联合仿真操作)
      • [(1) 打开 Matlab](#(1) 打开 Matlab)
      • [(2) 配置接口](#(2) 配置接口)
      • [(3) 编译 C++ 程序](#(3) 编译 C++ 程序)
      • [(4) 调整工作路径](#(4) 调整工作路径)
    • [1.4 适配算法与仿真平台搭建](#1.4 适配算法与仿真平台搭建)
      • [(1) 加载数据](#(1) 加载数据)
      • [(2) 参数设置](#(2) 参数设置)
      • [(3) 复制粘贴算法](#(3) 复制粘贴算法)
      • [(4) 匹配输入输出接口](#(4) 匹配输入输出接口)
  • 二、各模块算法概述
    • [2.1 定位模块](#2.1 定位模块)
    • [2.2 决策规划模块](#2.2 决策规划模块)
    • [2.3 控制模块](#2.3 控制模块)
  • 三、仿真结果与分析
    • [3.1 模型算法运行与调试](#3.1 模型算法运行与调试)
    • [3.2 摄像头视角设置与可视化](#3.2 摄像头视角设置与可视化)
    • [3.3 轨迹跟踪效果分析](#3.3 轨迹跟踪效果分析)
    • [3.4 速度与加速度跟踪](#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

matlab 复制代码
%%%%%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 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 里面设置跟随车的摄像头,点击倒数第二个Visu Aids 添加,选择 Custom,添加视角,设置摄像机的视角位置。

BuildRegenerate 运行,在弹出的 VisServer 中将视角切换为HumanView_1

3.3 轨迹跟踪效果分析

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

写个脚本 testctrl.m

matlab 复制代码
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_xhost_y 是定位给出的车辆实际轨迹,xr_desireyr_desire 是规划轨迹。其实就画了两个图,如果这两条线贴得比较近,就像像刚才图一样,那就证明这可以完美的跟踪,就认为控制算法没问题。

3.4 速度与加速度跟踪

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


四、总结

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

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

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


参考资料

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


后记:

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

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

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

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

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

相关推荐
曹勖之9 天前
simuilink和ROS2数据联通,Run后一直卡在Initializting
windows·matlab·simulink·ros2
懒惰的bit9 天前
STM32F103C8T6 学习笔记摘要(四)
笔记·stm32·学习
zkyqss9 天前
OVS Faucet练习(下)
linux·笔记·openstack
浦东新村轱天乐10 天前
【麻省理工】《how to speaking》笔记
笔记
奔跑的蜗牛AZ10 天前
TiDB 字符串行转列与 JSON 数据查询优化知识笔记
笔记·json·tidb
cwtlw10 天前
Excel学习03
笔记·学习·其他·excel
杭州杭州杭州10 天前
计算机网络笔记
笔记·计算机网络
cyborg10 天前
终于再也不用在notion中写公式了
笔记
Zevalin爱灰灰10 天前
MATLAB GUI界面设计 第三章——仪器组件
开发语言·ui·matlab
循环过三天10 天前
1.2、CAN总线帧格式
笔记·stm32·单片机·嵌入式硬件·学习