【Carsim Simulink自动驾驶仿真】基于MPC的轨迹跟踪控制
如果对Carsim的基础使用还不了解，可以参考：【Carsim Simulink自动驾驶仿真】基于MPC的速度控制
如果对MPC算法原理不清楚，可以参考：如何理解MPC模型预测控制理论
项目介绍：
教程为北理工的无人驾驶车辆模型预测控制第2版。所用的仿真软件为Carsim2020.0和MatlabR2021a。使用MPC控制思想对车辆进行轨迹跟踪控制，并给出仿真结果。
整整弄了两天，踩了无数的坑，所以篇幅比较大，如果还有什么其他问题，欢迎一起讨论。
从网上下载下来的代码运行不了，所以本文的代码是经过修改和调试的。
基于MPC的轨迹跟踪控制
- 效果图
- 直线跟踪
- 3m/s
- 5m/s
- 10m/s
- 圆跟踪
- 3m/s
- 5m/s
- 10m/s
- MATLAB框架搭建
- 代码的数学遗漏问题与修改
- 运动学模型少L
- 改写之后的目标函数线性项少系数
- 代码中的其他问题及讨论
- 输入维度不匹配的情况
- 索引超出数组范围
- 1.变量的范围
- 2.换一个求解器
- 3.初始值全部设为0
- 4.对于kesi(4)和kesi(5)的赋值 （这个是我的问题）
- 一些理解
- 参数的选取
- 速度问题
- 附录：全代码(修改过后的)
效果图
因为参数经过了调整，所以效果上和书上不一样。
直线跟踪
图像为：路径，速度，误差和前轮偏角
前轮偏角的局部放大图为：
3m/s
5m/s
10m/s
速度的具体图像为：
圆跟踪
图像为：路径，速度和前轮偏角(误差不知道该在Carsim中怎么画，知道的大佬请私信，呜呜呜)：
轨迹放大之后：
前轮偏角为：
前轮偏角放大之后：
3m/s
速度为：
5m/s
速度为：
10m/s
速度为：
MATLAB框架搭建
书中给出的MATLAB框架如下：
因为对于这种方法输出需要再另外处理。所以我这里直接将速度乘3.6，角度也在程序中直接处理了。对应的框架图为：
代码的数学遗漏问题与修改
书中的代码和下载下来的代码不一样，所以还是检查一下这两个地方，
运动学模型少L
在书中，推导运动学模型的时候，最后一个数字分母是有一个l的：
但是在代码中，这个地方少了，需要加上。
改写之后的目标函数线性项少系数
这里的2在书中没有少，但是下载下来的代码中没有这个系数。如果没有的可以加一下。
代码中的其他问题及讨论
输入维度不匹配的情况
看一下S-Function的输入是3个还是5个，输出是2个还是直接给出5个，对应的代码中也应该和其一样。具体在S-Function中是初始化的第一个函数中的：
sizes.NumOutputs =5;%[speed,steering]sizes.NumInputs =3;
索引超出数组范围
这个是我的主要问题，之所以报错，是因为求解器求解不出来优化问题，方法的话我找了一些，然后自己也试了一些：
1.变量的范围
这里有两种情况：
- 第一种情况是检查变量范围
delta_umin=[-0.05;-0.0082;];如果当中的-0.05是0.05，将其加一个负号。 - 第二种情况其实扩大变量的范围可以求解，但是这里不建议，如果你在扩大范围之后可以求解，那么建议还是将其变回原来的数，更改其他值。
2.换一个求解器
使用interior-point-convex，而不是active-set
%options = optimset('Algorithm','active-set');
options = optimset('Algorithm','interior-point-convex');
3.初始值全部设为0
检查初始化函数mdlInitializeSizes中的x0和U是否都为零。注意，这里的U表示的就是控制量与参考控制的差值。
function [sys,x0,str,ts] = mdlInitializeSizes
%找到以下两个值
x0 = [0;0;0];
U=[0;0];
4.对于kesi(4)和kesi(5)的赋值 （这个是我的问题）
这个我认为争议比较大，但是改了之后确实没有报错了。
先说一下，按照理论：
kesi(4)和kesi(5)就是这里的控制变量u~\tilde{u}u~。而这个u~\tilde{u}u~根据定义是：
是当前速度与参考速度的插值，所以这里我们应该将Carsim中的5个变量全部输入到S-Funciton中，然后进行如下操作：
U(1) = u(4)/3.6 - vd1;steer_SW = u(5)*pi/180;steering_angle = steer_SW/18;U(2) = steering_angle - vd2;kesi(4)=U(1); % vel-vel_refkesi(5)=U(2); % steer_angle - steering_ref
之前我从来没有怀疑过这里，直到我看到书中的代码和网上下载的不一样的时候，我就尝试了一下书上的代码，即将前四行全部注释掉：
% U(1) = u(4)/3.6 - vd1;
% steer_SW = u(5)*pi/180;
% steering_angle = steer_SW/18;
% U(2) = steering_angle - vd2;kesi(4)=U(1); % vel-vel_refkesi(5)=U(2); % steer_angle - steering_ref
这样改之后我的代码就不会出现问题了。
一些理解
首先我们要清楚，kesi(4,5)是控制减去参考控制，求解器解出来的X是前后两次的控制差值。
我们先看能跑对的程序，大致过程可以简化成设置值，求解，给出输出三步：
U = [0,0]
for 循环：
#第一步：设置kesikesi(4)=U(1); % vel-vel_refkesi(5)=U(2); % steer_angle - steering_ref
#第二步：求解[X,~,~]=quadprog(H,f,A_cons,b_cons,[],[],lb,ub,[],options);
#第三步：给出输出u_dot(1)=X(1);u_dot(2)=X(2);U(1)=kesi(4)+u_dot(1);%用于存储上一个时刻的控制量U(2)=kesi(5)+u_dot(2);u_real(1) = U(1) + vd1;u_real(2) = U(2) + vd2;
这里列一个表格：
| 时间 | 求解之前的U，也就是上一步的U | kesi | u_dot | 求解之后的U | 输出 |
|---|---|---|---|---|---|
| k-1 | uk−1−uk−1refu_{k-1}-u_{k-1}^{ref}uk−1−uk−1ref | uk−1−uk−1refu_{k-1}-u_{k-1}^{ref}uk−1−uk−1ref | uk−ukref−uk−1+uk−1refu_k-u_{k}^{ref}-u_{k-1}+u_{k-1}^{ref}uk−ukref−uk−1+uk−1ref | uk−1−uk−1ref+uk−ukref−uk−1+uk−1ref=uk−ukrefu_{k-1}-u_{k-1}^{ref}+u_k-u_{k}^{ref}-u_{k-1}+u_{k-1}^{ref}=u_k-u_{k}^{ref}uk−1−uk−1ref+uk−ukref−uk−1+uk−1ref=uk−ukref | u_k |
| 之后就可以征程循环。注意这里的初始值设置为[0,0]是合理的。 |
我出错的程序：
U = [0,0]
for 循环：
#第一步：设置kesiU(1) = u(4)/3.6 - vd1;steer_SW = u(5)*pi/180;steering_angle = steer_SW/18;U(2) = steering_angle - vd2;kesi(4)=U(1); % vel-vel_refkesi(5)=U(2); % steer_angle - steering_ref
#第二步：求解[X,~,~]=quadprog(H,f,A_cons,b_cons,[],[],lb,ub,[],options);
#第三步：给出输出u_dot(1)=X(1);u_dot(2)=X(2);U(1)=kesi(4)+u_dot(1);%用于存储上一个时刻的控制量U(2)=kesi(5)+u_dot(2);u_real(1) = U(1) + vd1;u_real(2) = U(2) + vd2;
建立同样的表格，这里唯一不一样的是，之前的输入其实是记忆到了程序里面，这里的输入变成了从程序中获取：
| 时间 | 求解之前的U，也就是上一步的U | kesi | u_dot | 求解之后的U | 输出 |
|---|---|---|---|---|---|
| k-1 | uk−1input−uk−1refu_{k-1}^{input}-u_{k-1}^{ref}uk−1input−uk−1ref | uk−1input−uk−1refu_{k-1}^{input}-u_{k-1}^{ref}uk−1input−uk−1ref | uk−ukref−uk−1input+uk−1refu_k-u_{k}^{ref}-u_{k-1}^{input}+u_{k-1}^{ref}uk−ukref−uk−1input+uk−1ref | uk−ukrefu_k-u_{k}^{ref}uk−ukref | u_k |
| k | ukinput−ukrefu_{k}^{input}-u_{k}^{ref}ukinput−ukref | ||||
| 但是按照道理来说，这个和上一个表格是一样的，这里不一样的是速度输入之后需要进行变换，输出的时候也要进行变换，这变换之后会存在的误差比较大。还有一种可能，这里其实输入进来的是速度和方向盘转角，这个方向盘转角和轮胎转角之间有一个系数，这个系数这里设置的18，但是具体设置多少，也没有具体的深究。 | |||||
| 总之我个人感觉可能是上一时刻的输出uku_kuk和这一时刻的输入ukinputu_k^{input}ukinput可能不一样。希望各位大佬也给出一些解释。 |
参数的选取
参数这里有4个，预测步长，控制步长，Q和R。这里Q和R都是对角阵，表格中的数值是对角阵中的数，而不是Q和R的实际值。
这里我的参数为：
| 预测步长(NpN_pNp) | 控制步长(N_c) | Q | R |
|---|---|---|---|
| 100 | 30 | 1 | 100 |
我先调整的Q和R，当轨迹不贴合的时候，调大Q,当速度收敛太慢，震荡太久的时候调整R。根据我的经验Q取1就好，之后就根据速度图像的震荡和你的评判标准进行选择就好。
对于预测步长和控制步长，预测的越久，说明这个智能体越有远见，超调就会相对小一点，个人感觉控制步长太多不是什么好的事情，因为本身希望汽车进行相对较少的控制。
速度问题
这里有一个小问题，书中的速度在初始时刻是没有减速的：
而在我做的过程中，速度在一开始都有减少：
这里我找到了问题，是两个前轮的速度在一开始不一样，但是按照道理，输入都是一样的，出现这样的差异不知道怎么解决。
附录：全代码(修改过后的)
function [sys,x0,str,ts] = MPCtracking(t,x,u,flag)%控制量为速度和前轮偏角，模型是用运动学模型switch flagcase 0[sys,x0,str,ts]=mdlInitializeSizes;case 2sys = mdlUpdates(t,x,u);case 3sys = mdlOutputs(t,x,u);case {1,4,9}sys = [];otherwiseerror(['unhandled flag =', num2str(flag)])end
%End of MPCtracking%==============================================================
% 初始化
%==============================================================
function [sys,x0,str,ts] = mdlInitializeSizessizes = simsizes;sizes.NumContStates =0;sizes.NumDiscStates =3;sizes.NumOutputs =5;%[speed,steering]sizes.NumInputs =3;sizes.DirFeedthrough =1;sizes.NumSampleTimes =1;sys = simsizes(sizes);x0 = [0;0;0];global U;%存储目前的控制变量[vel,delta]U=[0;0];str = [];ts = [0.05,0]; %采样时间：[period,offset]
%End of mdlInitializeSizes%==============================================================
% 更新离散状态
%==============================================================
function sys = mdlUpdates(t,x,u)sys = x;
%End of mdlUpdate.%==============================================================
% 计算输出，核心输出
%==============================================================
function sys = mdlOutputs(t,x,u)global a b u_dot;global U;global kesi;ticNx = 3;%状态量的个数Nu = 2;%控制量的个数Np = 100;%预测步长Nc = 30;%控制步长Row = 10; %松弛因子yaw_angle =u(3)*pi/180;%CarSim输出的Yaw angle为角度，角度转换为弧度%==================================================
%最后调参
%==================================================u_bound1 = 0.2;u_bound2 = 0.436;delta1 = 0.05;delta2 = 0.0082;Q_a = 1;R_a = 100;
%==================================================
%构建路径
%==================================================
% %直线路径
% vd1 = 10;%ref_velocity
% vd2 = 0;%ref_steering
% r(1)=vd1*t; %ref_x
% r(2)=5;%ref_y
% r(3)=0;%ref_heading_angle% %半径为35m的圆形轨迹, 圆心为(0, 35), 速度为3m/s
% r(1)=25*sin(0.12*t);
% r(2)=25+10-25*cos(0.12*t);
% r(3)=0.12*t;
% vd1=3;
% vd2=0.104;%半径为25m的圆形轨迹, 圆心为(0, 35), 速度为10m/sr(1)=25*sin(0.4*t);r(2)=25+10-25*cos(0.4*t);r(3)=0.4*t;vd1=10;vd2=0.104;
% %半径为25m的圆形轨迹, 圆心为(0, 35), 速度为5m/s
% r(1)=25*sin(0.2*t);
% r(2)=25+10-25*cos(0.2*t);
% r(3)=0.2*t;
% vd1=5;
% vd2=0.104;
%==================================================
%构建结束
%==================================================%添加转化后的自变量kesikesi=zeros(Nx+Nu,1);kesi(1) = u(1)-r(1);%u(1)==X(1),x_offsetkesi(2) = u(2)-r(2);%u(2)==X(2),y_offsetheading_offset = yaw_angle - r(3); %u(3)==X(3),heading_angle_offsetif (heading_offset < -pi)heading_offset = heading_offset + 2*pi;endif (heading_offset > pi)heading_offset = heading_offset - 2*pi;endkesi(3)=heading_offset;% U(1) = u(4)/3.6 - vd1;
% steer_SW = u(5)*pi/180;
% steering_angle = steer_SW/18;
% U(2) = steering_angle - vd2;kesi(4)=U(1); % vel-vel_refkesi(5)=U(2); % steer_angle - steering_refT = 0.05;L = 2.6;%轮间距%矩阵初始化t_d = r(3);%系数Q = Q_a*eye(Nx*Np,Nx*Np);
% Q(Nx*Np,Nx*Np) = Q_a*0.01;R = R_a*eye(Nu*Nc);%运动学方程u_dot = zeros(Nx,1);%%%%这里应该没有xa = [1 0 -vd1*sin(t_d)*T;0 1 vd1*cos(t_d)*T;0 0 1;];b = [cos(t_d)*T 0;sin(t_d)*T 0;tan(vd2)*T/L vd1*T/(L*(cos(vd2))^2);];%构建新的状态空间A_cell=cell(2,2);B_cell=cell(2,1);A_cell{1,1}=a;A_cell{1,2}=b;A_cell{2,1}=zeros(Nu,Nx);A_cell{2,2}=eye(Nu);B_cell{1,1}=b;B_cell{2,1}=eye(Nu);A=cell2mat(A_cell);B=cell2mat(B_cell);C=[ 1 0 0 0 0;0 1 0 0 0;0 0 1 0 0];PHI_cell=cell(Np,1);THETA_cell=cell(Np,Nc);for j=1:1:NpPHI_cell{j,1}=C*A^j;for k=1:1:Ncif k<=jTHETA_cell{j,k}=C*A^(j-k)*B;else THETA_cell{j,k}=zeros(Nx,Nu);endendendPHI=cell2mat(PHI_cell);%size(PHI)=[Nx*Np Nx+Nu]THETA=cell2mat(THETA_cell);%size(THETA)=[Nx*Np Nu*(Nc+1)]H_cell=cell(2,2);H_cell{1,1}=THETA'*Q*THETA+R;H_cell{1,2}=zeros(Nu*Nc,1);H_cell{2,1}=zeros(1,Nu*Nc);H_cell{2,2}=Row;H=cell2mat(H_cell);
% H=(H+H')/2;error=PHI*kesi;f_cell=cell(1,2);f_cell{1,1} = 2*(error'*Q*THETA);f_cell{1,2} = 0;f=cell2mat(f_cell);%% 以下为约束生成区域%不等式约束A_t=zeros(Nc,Nc);%见falcone论文 P181for p=1:1:Ncfor q=1:1:Ncif q<=p A_t(p,q)=1;else A_t(p,q)=0;endend end A_I=kron(A_t,eye(Nu));%对应于falcone论文约束处理的矩阵A,求克罗内克积Ut=kron(ones(Nc,1), U);%umin=[-u_bound1; -u_bound2];%[min_vel, min_steer]维数与控制变量的个数相同umax=[u_bound1; u_bound2]; %[max_vel, max_steer],%0.436rad = 25degdelta_umin = [-delta1; -delta2]; % 0.0082rad = 0.47degdelta_umax = [delta1; delta2];Umin=kron(ones(Nc,1),umin);Umax=kron(ones(Nc,1),umax);A_cons_cell={A_I zeros(Nu*Nc, 1); -A_I zeros(Nu*Nc, 1)};b_cons_cell={Umax-Ut;-Umin+Ut};A_cons=cell2mat(A_cons_cell);%（求解方程）状态量不等式约束增益矩阵，转换为绝对值的取值范围b_cons=cell2mat(b_cons_cell);%（求解方程）状态量不等式约束的取值% 状态量约束delta_Umin = kron(ones(Nc,1),delta_umin);delta_Umax = kron(ones(Nc,1),delta_umax);lb = [delta_Umin; 0];%（求解方程）状态量下界ub = [delta_Umax; 10];%（求解方程）状态量上界%尝试初始值%x0 = zeros(Nc*Nu+1,1);%% 开始求解过程%options = optimset('Algorithm','active-set');options = optimset('Algorithm','interior-point-convex'); warning off all % close the warnings during computation [X,~,~]=quadprog(H,f,A_cons,b_cons,[],[],lb,ub,[],options);%% 计算输出 u_dot(1)=X(1);u_dot(2)=X(2);U(1)=kesi(4)+u_dot(1);%用于存储上一个时刻的控制量U(2)=kesi(5)+u_dot(2);u_real(1) = U(1) + vd1;u_real(2) = U(2) + vd2;sys= [u_real(1)*3.6; u_real(2)*180/pi; u_real(2)*180/pi;0;0]; % vel, steering, x, ytoc% End of mdlOutputs.【Carsim Simulink自动驾驶仿真】基于MPC的轨迹跟踪控制相关推荐
- 【Carsim Simulink自动驾驶仿真】基于MPC的动力学控制
如果对Carsim的基础使用还不了解,可以参考:[Carsim Simulink自动驾驶仿真]基于MPC的速度控制 如果对MPC算法原理不清楚,可以参考:如何理解MPC模型预测控制理论 项目介绍: 教 ...
- 【Carsim Simulink自动驾驶仿真】基于MPC的速度控制
本人也是刚开始探索,大家一起讨论一起进步! 项目介绍: 教程为北理工的无人驾驶车辆模型预测控制第2版,代码为开源代码.所用的仿真软件为Carsim2020.0和MatlabR2021a.使用MPC控制 ...
- MPC算法轨迹跟踪控制资源
点击文末卡片,加入会员全年无限制学习后台(MPC各矩阵的底层逻辑.MPC纵向控制.模型验证.MPC自适应巡航控制.非线性系统如何线性化及MPC动力学跟踪任何轨迹.约束添加及新求解器的求解.轨迹规划.纵 ...
- 自动驾驶仿真(五)—— 基于Carsim、Prescan、Simulink的联合仿真
自动驾驶仿真五--基于Carsim.Prescan.Simulink的联合仿真 1. 联合仿真流程 2. CarSim联合仿真配置要点 3. Prescan联合仿真配置要点 4. Carsim.Pre ...
- 自动驾驶仿真 (三)—— 基于PreScan与Simulink的AEB系统仿真
自动驾驶仿真三-- 基于PreScan与Simulink的AEB系统仿真 1. AEB自动紧急制动系统 1. 1 TTC碰撞时间模型 1. 2 C-NCAP法规部分术语与定义 1. 3 主动安全ADA ...
- 对自动驾驶仿真软件研发方向的看法
快毕业了,计划入职的公司的产品经理,让我写一下关于使用Carla的使用心得,所以就简单得写了一下,我个人对自动驾驶仿真软件的看法. 对自动驾驶软件的个人看法 在介绍Carla的基本功能前,我想先说一下 ...
- 自动驾驶仿真(六)—— SIL软件在环仿真测试
自动驾驶仿真六-- SIL软件在环仿真测试 1. 在环仿真测试 2. SIL软件在环仿真测试 2.1 模型配置参数 2.2 S function生成实现 2.3 结果对比 3. 参考学习的书目教材 博 ...
- 自动驾驶仿真类初创企业信息梳理
引言 对于高阶自动驾驶来说,虚拟仿真测试是研发及测试验证过程中不可缺少的一个关键环节.并且虚拟仿真测试所占的比重也越来越大,虚拟仿真测试技术的发展进步,将直接影响到高阶自动驾驶技术商业化落地的时间. ...
- python仿真智能驾驶_自动驾驶仿真工程师
禁止私自转载,转载请联系作者. 想要做一个自动驾驶仿真工程师,我们要学的还远远不够. 对自动驾驶仿真工程师这个岗位,相关介绍还是少了些.有些公司是直接把它纳入到基础架构组里,有些是单独招聘这个岗位,还 ...
最新文章
- GTD （Getting Things Done）时间管理 提高效率 简介
- Asp.net读取AD域信息的方法（一）
- 如何把握创业时机：当前的痛点也许是巨大的风险
- js发送get、post请求的方法简介
- realtek网卡mac硬改工具_七彩虹联合Realtek发布粉色固态硬盘 首发评测
- iOS 14 大改还有神秘硬件登场，苹果 WWDC20 今夜线上发布
- 结构体03：结构体指针
- mfc之CPtrArray数组
- 解决navicat在未联网的情况下访问不了MySQL数据库的现象
- 633. 平方数之和
- 基于R语言的贝叶斯网络模型的实践技术
- Java利用MessageDigest获取字符串或文件MD5详解
- pdftomusic pro（音乐谱曲软件） v1.0.4
- 计算机控制系统信号的采样,计算机控制系统-信号采样与分析演示.ppt
- 专业抠图软件Super PhotoCut Pro for Mac
- css椭圆轨迹运动动画
- Jetson Nano设置开机启动程序
- viper4android fx原理,ViPer4android. FX顶级音效！
- Python递归小案例，斐波那契，阶乘等小案例
- Dialogue Summarization with Supporting Utterance Flow Modeling and Fact Regularization阅读笔记