基于 Carsim 2016 和 Simulink的无人车运动控制联合仿真（四）
简介
本篇主要说明Simulink模型的搭建、MPC控制器的S-function编写
1、Carsim设置
新建一个空白的Simulink模型，或复制Carsim自带的Simulink模型，存放在如下路径C:\Users\Public\Documents\CarSim2016.1_Data\Extensions\Simulink）
单击下图中的红圈按钮
如下图所示，将新建好的Simulink模型添加到进来
单击Send to Simulink按钮，如下图红圈所示，该操作将打开MATLAB及Simulink
2、Simulink模型搭建
步骤1的操作将自动打开MATLAB的Simulink（需等待几十秒，MATLAB启动较慢），并且Simulink Library Browser中将出现Carsim S-Function模块，之前在Carsim里搭建的汽车模型被封装成S-Function用以被调用。
搭建如下的Simulink模型
经测算，所选车型的方向盘转角约为车前轮转角的17.86倍
Simulink模型的下载链接
https://github.com/xiaoyuyang0901/Simulation-platform/tree/master/MPC-Simulink-Carsim
3、S-Function的编写
新建MPC_Costfunction.m 、MPC_Nonlcon.m 、MPC_S_Function.m 三个.m文件，存放在如下路径C:\Users\Public\Documents\CarSim2016.1_Data\Extensions\Simulink）
双击Simulink模型中的MPC_S_Function中模块，出现如下的窗口，点击Edit按钮
单击Browse按钮，添加已经建好的MPC_S_Function.m 文件
4、源码
a. 参考轨迹有两个可供选择
y = 0.5 * sin( 0.1 * pi * x )
y = 1
b. 目标函数中的各个惩罚项权重需要进行调参，过多的惩罚项将大大降低实时性
c. 增加预测的周期周期数可以降低调参的压力，但是会牺牲实时性
MPC_S_Function.m 文件的代码
%===============================================================
% MPC模块的S-Function
%===============================================================
function [sys,x0,str,ts] = MPC_S_Function(t,x,u,flag)
switch flag
case 0
[sys,x0,str,ts] = mdlInitializeSizes; % Initialization
case 2
sys = mdlUpdates(t,x,u); % Update discrete states
case 3
sys = mdlOutputs(t,x,u); % Calculate outputs
case {1,4,9} % Unused flags
sys = [];
otherwise
error(['unhandled flag = ',num2str(flag)]); % Error handling
end
% End of S Function
%==============================================================
% Initialization
%==============================================================
function [sys,x0,str,ts] = mdlInitializeSizes
% Call simsizes for a sizes structure, fill it in, and convert it
% to a sizes array.
sizes = simsizes;
sizes.NumContStates = 0;
sizes.NumDiscStates = 3;
sizes.NumOutputs = 2;
sizes.NumInputs = 5;
sizes.DirFeedthrough = 1; % Matrix D is non-empty.
sizes.NumSampleTimes = 1;
sys = simsizes(sizes);
x0 =[0;0;0];
% Initialize the discrete states.
str = []; % Set str to an empty matrix.
ts = [0.1 0]; % sample time: [period, offset]
%==============================================================
% Update the discrete states
%==============================================================
function sys = mdlUpdates(t,x,u)
sys = x;
%==============================================================
% Calculate outputs
%==============================================================
function sys = mdlOutputs(t,x,u)
tic
fprintf('Update start, t=%6.3f\n',t)
NP = 7;
NC = 2;
dt = 0.1;
v_ref = 10;
x_start = 0; %横向位置
y_start = x_start + NP; %纵向位置
phi_start = y_start + NP; %yaw的角度，与X轴正向的夹角
cte_start = phi_start + NP; %位置误差
ephi_start = cte_start + NP; %姿态误差
delta_start = ephi_start + NP; %前轮转角
m = 6 * NP ; %状态变量和控制变量的总个数
state_0 = u;
nlcon = @(var)MPC_Nonlcon( var,state_0,NP,dt,v_ref ); %非线性约束
cl = zeros( 6*NP+1,1 ); %约束的下界
cu = zeros( 6*NP+1,1 ); %约束上界
for i = (5*NP+1):6*NP
cl(i) = -0.45; %车前轮转角限制25°
cu(i) = 0.45;
end
state_intial = zeros(m,1); %初始化
func = @(var)MPC_Costfunction( var,NP,dt ); %损失函数
% opts = optiset('solver','IPOPT','display','iter','maxiter',50,'maxtime',0.5);
opts = optiset('solver','ipopt','maxiter',60,'maxtime',0.5);
Opt = opti('fun',func,'nl',nlcon,cl,cu,'x0',state_intial,'options',opts);
[A,feval,exitflag,info] = solve(Opt,state_intial);
A
info
result = zeros(2,1);
result(1) = v_ref; %输出车速 5m/s
% result(2) = A(delta_start + NC); %输出优化器求解的方向盘转角
result(2) = 0;
else
result(2) = A(delta_start + NC); %输出优化器求解的方向盘转角
end
sys = result;
%每一步的可视化，但会增加运算时长
% plot( A(1:NP),A(NP+1:2*NP),'-.p' );
% axis ([0 100 -30 30]);
% axis equal;
toc
% End of mdlOutputs.下载链接
https://github.com/xiaoyuyang0901/Simulation-platform/blob/master/MPC-Simulink-Carsim/MPC_S_Function.m
MPC_Costfunction.m文件的代码
%===============================================================
% MPC模块的目标函数
%===============================================================
function cost = MPC_Costfunction(var,NP,dt)
x_start = 0;
y_start = x_start + NP;
phi_start = y_start + NP;
cte_start = phi_start + NP;
ephi_start = cte_start + NP;
delta_start = ephi_start + NP;
cte = 0;
ephi = 0;
delta = 0;
delta_change = 0;
for i = 1:1:NP
cte = cte + var( cte_start+i )^2;
ephi = ephi + var( ephi_start+i)^2;
delta = delta + var( delta_start+i )^2;
end
for i = 1:1:NP-1
delta_change = delta_change + ( var( delta_start+i+1 ) - var( delta_start+i ) )^2;
end
% 各个惩罚项的权重需要精心的调参，惩罚项过多会大大降低实时性
cost = 8*cte + 0 * ephi + 0*delta + 60*delta_change;
end下载链接
https://github.com/xiaoyuyang0901/Simulation-platform/blob/master/MPC-Simulink-Carsim/MPC_Costfunction.m
MPC_Nonlcon.m文件的代码
%===============================================================
% MPC模块的非线性约束方程
%===============================================================
function c = MPC_Nonlcon( var,state_0,NP,dt,v_ref )
x_start = 0;
y_start = x_start + NP;
phi_start = y_start + NP;
cte_start = phi_start + NP;
ephi_start = cte_start + NP;
delta_start = ephi_start + NP;
x_0 = state_0(1);
y_0 = state_0(2);
phi_0 = state_0(3) / 180 * pi;
v = v_ref;
% delta_0 = state_0(5);
delta_0 = 0;
lf = 3;
c = zeros( 6*NP+1,1 );
for i = 1:1:NP
if i == 1
c( x_start + i ) = var(x_start + i ) - x_0 - v * cos( phi_0 ) * dt;
c( y_start + i ) = var(y_start + i ) - y_0 - v * sin( phi_0 ) * dt;
c( phi_start + i ) = var(phi_start + i ) - phi_0 - v /lf * tan( delta_0 ) * dt;
% 参考轨迹为 y = 0.5 * sin( 0.1 * pi * x )
% c( cte_start + i ) = var(cte_start + i ) - ( 0.5*sin(0.1*pi*x_0) - var(y_start + i ) );
% c( ephi_start + i ) = var(ephi_start + i) - ( var(phi_start + i ) - 0.05*pi*cos(0.1*pi* x_0) );
% 参考轨迹为 y = 1
c( cte_start + i ) = var(cte_start + i ) - ( 1 - var(y_start + i ) );
c( ephi_start + i ) = var(ephi_start + i) - ( var(phi_start + i ) - 0 );
c( delta_start + i )= var(delta_start + i);
c( delta_start + i + NP)= var(delta_start + i) - delta_0 ;
else
c( x_start + i ) = var(x_start + i ) - var(x_start + i-1 ) - v * cos( var(phi_start + i-1) ) * dt;
c( y_start + i ) = var(y_start + i ) - var(y_start + i-1) - v * sin( var(phi_start + i-1) ) * dt;
c( phi_start + i ) = var(phi_start + i ) - var(phi_start + i-1) - v/lf * tan( var(delta_start + i-1) ) * dt;
% 参考轨迹为 y = 0.5 * sin( 0.1 * pi * x )
% c( cte_start + i ) = var(cte_start + i ) - ( 0.5*sin(0.1*pi*var(x_start + i)) - var(y_start + i ) + v * sin( var(ephi_start+i-1) ) * dt );
% c( ephi_start + i ) = var(ephi_start + i) - ( var(phi_start + i ) - 0.05*pi*cos(0.1*pi*var(x_start + i)) );
% 参考轨迹为 y = 1
c( cte_start + i ) = var(cte_start + i ) - ( 1 - var(y_start + i ) + v * sin( var(ephi_start+i-1) ) * dt );
c( ephi_start + i ) = var(ephi_start + i) - ( var(phi_start + i ) - 0 );
c( delta_start + i )= var(delta_start + i);
end
end
end下载链接
https://github.com/xiaoyuyang0901/Simulation-platform/blob/master/MPC-Simulink-Carsim/MPC_Nonlcon.m
References
1. 龚建伟, 姜岩, 徐威. 无人驾驶车辆模型预测控制[M]. 北京理工大学出版社, 2014.
2. Matlab中S-函数的编写https://blog.csdn.net/acelit/article/details/59082349
欢迎来到泡泡论坛，这里有大牛为你解答关于SLAM的任何疑惑。
有想问的问题，或者想刷帖回答问题，泡泡论坛欢迎你！
泡泡网站：www.paopaorobot.org
泡泡论坛：http://paopaorobot.org/bbs/
泡泡机器人SLAM的原创内容均由泡泡机器人的成员花费大量心血制作而成，希望大家珍惜我们的劳动成果，转载请务必注明出自【泡泡机器人SLAM】微信公众号，否则侵权必究！同时，我们也欢迎各位转载到自己的朋友圈，让更多的人能进入到SLAM这个领域中，让我们共同为推进中国的SLAM事业而努力！
商业合作及转载请联系liufuqiang_robot@hotmail.com
相关内容
微信扫码咨询专知VIP会员