话不多说了先上图
【↓】没有用carsim，调用了simulink自带的三自由度自行车模型
【↓】车速5mps
【↓】车速10mps
【↓】车速15mps
话不多说，贴上代码【基本是网上以及参考书上借鉴的，互相借鉴学习哈】
function [sys,x0,str,ts] = MPC_Controller_d(t,x,u,flag)
%SFUNTMPL General MATLAB S-Function Template
% With MATLAB S-functions, you can define you own ordinary differential
% equations (ODEs), discrete system equations, and/or just about
% any type of algorithm to be used within a Simulink block diagram.
%
% The general form of an MATLAB S-function syntax is:
% [SYS,X0,STR,TS,SIMSTATECOMPLIANCE] = SFUNC(T,X,U,FLAG,P1,...,Pn)
%
% What is returned by SFUNC at a given point in time, T, depends on the
% value of the FLAG, the current state vector, X, and the current
% input vector, U.
%
% FLAG RESULT DESCRIPTION
% ----- ------ --------------------------------------------
% 0 [SIZES,X0,STR,TS] Initialization, return system sizes in SYS,
% initial state in X0, state ordering strings
% in STR, and sample times in TS.
% 1 DX Return continuous state derivatives in SYS.
% 2 DS Update discrete states SYS = X(n+1)
% 3 Y Return outputs in SYS.
% 4 TNEXT Return next time hit for variable step sample
% time in SYS.
% 5 Reserved for future (root finding).
% 9 [] Termination, perform any cleanup SYS=[].
%
%
% The state vectors, X and X0 consists of continuous states followed
% by discrete states.
%
% Optional parameters, P1,...,Pn can be provided to the S-function and
% used during any FLAG operation.
%
% When SFUNC is called with FLAG = 0, the following information
% should be returned:
%
% SYS(1) = Number of continuous states.
% SYS(2) = Number of discrete states.
% SYS(3) = Number of outputs.
% SYS(4) = Number of inputs.
% Any of the first four elements in SYS can be specified
% as -1 indicating that they are dynamically sized. The
% actual length for all other flags will be equal to the
% length of the input, U.
% SYS(5) = Reserved for root finding. Must be zero.
% SYS(6) = Direct feedthrough flag (1=yes, 0=no). The s-function
% has direct feedthrough if U is used during the FLAG=3
% call. Setting this to 0 is akin to making a promise that
% U will not be used during FLAG=3. If you break the promise
% then unpredictable results will occur.
% SYS(7) = Number of sample times. This is the number of rows in TS.
%
%
% X0 = Initial state conditions or [] if no states.
%
% STR = State ordering strings which is generally specified as [].
%
% TS = An m-by-2 matrix containing the sample time
% (period, offset) information. Where m = number of sample
% times. The ordering of the sample times must be:
%
% TS = [0 0, : Continuous sample time.
% 0 1, : Continuous, but fixed in minor step
% sample time.
% PERIOD OFFSET, : Discrete sample time where
% PERIOD > 0 & OFFSET < PERIOD.
% -2 0]; : Variable step discrete sample time
% where FLAG=4 is used to get time of
% next hit.
%
% There can be more than one sample time providing
% they are ordered such that they are monotonically
% increasing. Only the needed sample times should be
% specified in TS. When specifying more than one
% sample time, you must check for sample hits explicitly by
% seeing if
% abs(round((T-OFFSET)/PERIOD) - (T-OFFSET)/PERIOD)
% is within a specified tolerance, generally 1e-8. This
% tolerance is dependent upon your model's sampling times
% and simulation time.
%
% You can also specify that the sample time of the S-function
% is inherited from the driving block. For functions which
% change during minor steps, this is done by
% specifying SYS(7) = 1 and TS = [-1 0]. For functions which
% are held during minor steps, this is done by specifying
% SYS(7) = 1 and TS = [-1 1].
%
% SIMSTATECOMPLIANCE = Specifices how to handle this block when saving and
% restoring the complete simulation state of the
% model. The allowed values are: 'DefaultSimState',
% 'HasNoSimState' or 'DisallowSimState'. If this value
% is not speficified, then the block's compliance with
% simState feature is set to 'UknownSimState'.
% Copyright 1990-2010 The MathWorks, Inc.
%
% The following outlines the general structure of an S-function.
%
switch flag,
%%%%%%%%%%%%%%%%%%
% Initialization %
%%%%%%%%%%%%%%%%%%
case 0,
[sys,x0,str,ts]=mdlInitializeSizes;
%%%%%%%%%%%%%%%
% Derivatives %
%%%%%%%%%%%%%%%
case 1,
sys=[];
%%%%%%%%%%
% Update %
%%%%%%%%%%
case 2,
sys=mdlUpdate(t,x,u);
%%%%%%%%%%%
% Outputs %
%%%%%%%%%%%
case 3,
sys=mdlOutputs(t,x,u);
%%%%%%%%%%%%%%%%%%%%%%%
% GetTimeOfNextVarHit %
%%%%%%%%%%%%%%%%%%%%%%%
case 4,
sys=[];
%%%%%%%%%%%%%
% Terminate %
%%%%%%%%%%%%%
case 9,
sys=[];
%%%%%%%%%%%%%%%%%%%%
% Unexpected flags %
%%%%%%%%%%%%%%%%%%%%
otherwise
DAStudio.error('Simulink:blocks:unhandledFlag', num2str(flag));
end
% end sfuntmpl
%
%=============================================================================
% mdlInitializeSizes
% Return the sizes, initial conditions, and sample times for the S-function.
%=============================================================================
%
function [sys,x0,str,ts]=mdlInitializeSizes
%
% call simsizes for a sizes structure, fill it in and convert it to a
% sizes array.
%
% Note that in this example, the values are hard coded. This is not a
% recommended practice as the characteristics of the block are typically
% defined by the S-function parameters.
%
sizes = simsizes;
sizes.NumContStates = 0;%连续状态的个数
sizes.NumDiscStates = 6;%离散状态的个数
sizes.NumOutputs = 3;%输出量的个数
sizes.NumInputs = 6;%输入量的个数:状态反馈
sizes.DirFeedthrough = 1;%Matrix D is non-empty
sizes.NumSampleTimes = 1; % at least one sample time is needed
sys = simsizes(sizes);
%
% initialize the initial conditions
%
x0 = [0.0001;0.0001;0.0001;0.0001;0.0001;0.0001];
global U;
U = [0];
global DELTA_T;
DELTA_T = 0.01;
%初始化阶段完成预测模型的线性化计算,不太行得通
%
% str is always an empty matrix
%
str = [];
%
% initialize the array of sample times
%
ts = [DELTA_T 0];
% Specify the block simStateCompliance. The allowed values are:
% 'UnknownSimState', < The default setting; warn and assume DefaultSimState
% 'DefaultSimState', < Same sim state as a built-in block
% 'HasNoSimState', < No sim state
% 'DisallowSimState' < Error out when saving or restoring the model sim state
% end mdlInitializeSizes
%
%=============================================================================
% mdlDerivatives
% Return the derivatives for the continuous states.
%=============================================================================
%
% end mdlDerivatives
%
%=============================================================================
% mdlUpdate
% Handle discrete state updates, sample time hits, and major time step
% requirements.
%=============================================================================
%
function sys=mdlUpdate(t,x,u)
sys = x;
% end mdlUpdate
%
%=============================================================================
% mdlOutputs
% Return the block outputs.
%=============================================================================
%
function sys=mdlOutputs(t,x,u)
global a b;
global U;
global DELTA_T;
global kesi;
tic
Nx = 6;%状态量的个数
Nu = 1;%控制量的个数
Ny = 2;%输出量的个数：横摆角+横向位置
Np = 10;%预测步长
Nc = 9;%控制步长
%%%%%%单位转换%%%%%%%
pi = 3.1415926;
%u为模块的输入，即系统的状态反馈
y_dot = u(1);%mps
x_dot = u(2)+0.0001;%mps
phi = u(3);%弧度
phi_dot = u(4);%弧度
Y = u(5);%m
X = u(6);%m
%%%%%%车辆参数%%%%%%%
%前、后轮滑移率
sf=0.2;sr=0.2;
%前后轮距离质心距离
lf=1.232;lr=1.468;
%前后车轮的横、纵向侧偏刚度
Ccf=66900;Ccr=62700;Clf=66900;Clr=62700;
%整车质量、重力加速度、转动惯量
m=1723;g=9.8;I=4175;
%%%%%%参数设计%%%%%%%
%u是函数输入，为车辆状态反馈
kesi = zeros(Nx+Nu,1);
kesi(1) = y_dot;
kesi(2) = x_dot;
kesi(3) = phi;
kesi(4) = phi_dot;
kesi(5) = Y;
kesi(6) = X;
kesi(7) = U(1);
delta_f = U(1);
fprintf('Update start, u(1)=%4.2f\n',U(1))
T = DELTA_T;
%%%%%%矩阵初始化%%%%%%
%权重矩阵设置
Q_ii = [100 0;
0 10];%size(Q_ii)=[Ny Ny]
%权重矩阵组合
Q_cell = cell(Np,Np);
for i=1:1:Np
for j=1:1:Np
if i==j
Q_cell{i,j} = Q_ii;
else
Q_cell{i,j} = zeros(Ny,Ny);
end
end
end
Q = cell2mat(Q_cell);%size(Q_cell) = [Ny*Np Ny*Np]
R = 100*eye(Nu*Nc);
Row = 100;%松弛因子
%系统状态转移矩阵
a = [ 1.0 - (150.44*T)/x_dot, -1.0*T*(phi_dot - (77.655*(1.232*phi_dot + y_dot))/x_dot^2 + (72.78*(1.468*phi_dot - 1.0*y_dot))/x_dot^2), 0, -1.0*T*(x_dot - 11.17/x_dot), 0, 0;
T*(phi_dot - (77.655*delta_f)/x_dot), (77.655*T*delta_f*(1.232*phi_dot + y_dot))/x_dot^2 + 1.0, 0, T*(y_dot - (95.671*delta_f)/x_dot), 0, 0;
0, 0, 1.0, T, 0, 0;
(4.6097*T)/x_dot, T*((39.483*(1.232*phi_dot + y_dot))/x_dot^2 + (0.00047904*(135120.0*phi_dot - 92044.0*y_dot))/x_dot^2), 0, 1.0 - (113.37*T)/x_dot, 0, 0;
T*cos(phi), T*sin(phi), T*(x_dot*cos(phi) - 1.0*y_dot*sin(phi)), 0, 1.0, 0;
-1.0*T*sin(phi), T*cos(phi), -1.0*T*(y_dot*cos(phi) + x_dot*sin(phi)), 0, 0, 1.0];
b = [ 77.655*T;
T*(155.31*delta_f - (77.655*(1.232*phi_dot + y_dot))/x_dot);
0;
39.483*T;
0;
0];
%以上为离散后的线性误差模型
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 = [0 0 1 0 0 0 0;
0 0 0 0 1 0 0];
%以上为离散的线性误差预测模型的参数
%%%%%%计算偏差%%%%%%%%%%
state_k1 = zeros(Nx,1);%预测下一时刻的状态，用于计算偏差
%根据离散的非线性模型预测下一时刻的状态量
dy_dot = -x_dot*phi_dot+2*(Ccf*(delta_f-((y_dot+lf*phi_dot)/x_dot))+Ccr*((lr*phi_dot-y_dot)/x_dot))/m;
dx_dot = y_dot*phi_dot+2*(Clf*sf+Ccf*(delta_f-(y_dot+lf*phi_dot)/x_dot)*delta_f+Clr*sr)/m;
dphi = phi_dot;
dphi_dot = 2/I*(lf*Ccf*(delta_f-(y_dot+lf*phi_dot)/x_dot)-lr*Ccr*(lr*phi_dot-y_dot)/x_dot);
dY = x_dot*sin(phi)+y_dot*cos(phi);
dX = x_dot*cos(phi)-y_dot*sin(phi);
state_k1(1,1) = y_dot+T*dy_dot;
state_k1(2,1) = x_dot+T*dx_dot;
state_k1(3,1) = phi+T*dphi;
state_k1(4,1) = phi_dot+T*dphi_dot;
state_k1(5,1) = Y+T*dY;
state_k1(6,1) = X+T*dX;
d_k = state_k1-a*kesi(1:6,1)-b*kesi(7,1);
d_piao_k = zeros(Nx+Nu,1);%d_k的增广形式
d_piao_k(1:6,1) = d_k;
d_piao_k(7,1) = 0;
%计算预测域和控制域内的预测模型系统参数
PSI_cell = cell(Np,1);
THETA_cell = cell(Np,Nc);
GAMMA_cell = cell(Np,Np);
PHI_cell = cell(Np,1);
for j=1:1:Np
PSI_cell{j,1} = C*A^j;%元胞数组中的元素赋值用大括号
PHI_cell{j,1} = d_piao_k;%理论上来说，这个是要实时更新的，但是为了简便，这里又一次近似
for k=1:1:Nc
if k<=j
THETA_cell{j,k} = C*A^(j-k)*B;
else
THETA_cell{j,k} = zeros(Ny,Nu);
end
end
for i=1:1:Np
if i<=j
GAMMA_cell{j,i} = C*A^(j-i);
else
GAMMA_cell{j,i} = zeros(Ny,Nx+Nu);
end
end
end
PSI = cell2mat(PSI_cell);%size(PSI) = [Ny*Np Nx+Nu]
THETA = cell2mat(THETA_cell);%size(THETA) = [Ny*Np Nu*Nc]
GAMMA = cell2mat(GAMMA_cell);%size(GAMMA) = [Ny*Np (Nx+Nu)*Np]
PHI = cell2mat(PHI_cell);%size(PHI) = [(Nx+Nu)*Np 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;
%%%%%%%%%获得局部参考轨迹%%%%%%%%%%%%%%%%%%%%%
shape=2.4;%参数名称
dx1=25;dx2=21.95;%没有任何实际意义，只是参数名称
dy1=4.05;dy2=5.7;%没有任何实际意义，只是参数名称
Xs1=27.19;Xs2=56.46;%参数名称，以上参数用来是生成双移线
X_predict=zeros(Np,1);%保存预测时域内的纵向位置信息，计算期望轨迹的基础
phi_ref=zeros(Np,1);
Y_ref=zeros(Np,1);
Yita_ref_cell = cell(Np,1);
%防止越界
T_all = 40;%总的仿真时间，主要功能是防止计算期望轨迹越界
X_DOT = dX;
for p=1:1:Np
X_predict(p,1) = X+X_DOT*p*T;%当t+pT大于局部轨迹边界时？
z1(p,1) = shape/dx1*(X_predict(p,1)-Xs1)-shape/2;
z2(p,1) = shape/dx2*(X_predict(p,1)-Xs2)-shape/2;
Y_ref(p,1) = dy1/2*(1+tanh(z1(p,1)))-dy2/2*(1+tanh(z2(p,1)));
phi_ref(p,1) = atan(dy1*(1/cosh(z1(p,1)))^2*(1.2/dx1)-dy2*(1/cosh(z2(p,1)))^2*(1.2/dx2));
Yita_ref_cell{p,1} = [phi_ref(p,1);Y_ref(p,1)];
end
Yita_ref = cell2mat(Yita_ref_cell);
error_1 = PSI*kesi+GAMMA*PHI-Yita_ref;
f_cell = cell(1,2);
f_cell{1,1} = 2*error_1'*Q*THETA;
f_cell{1,2} = 0;
f = cell2mat(f_cell);
%以上为转化为标准二次型的目标函数的参数
%以下生成约束
%%%%%%%不等式约束%%%%%%%%
%控制量约束
A_t = tril(ones(Nc,Nc),0);%构造下三角矩阵
A_I = kron(A_t,eye(Nu));
Ut = kron(ones(Nc,1),U);
umin = -0.2;%前轮转角控制量
umax = 0.2;
Umin = kron(ones(Nc,1),umin);
Umax = kron(ones(Nc,1),umax);
%输出量约束
ycmax = [0.4;5];
ycmin = [-0.4;-5];
Ycmax = kron(ones(Np,1),ycmax);
Ycmin = kron(ones(Np,1),ycmin);
%结合控制量约束和输出量约束
A_cons_cell = { A_I zeros(Nu*Nc,1);
-A_I zeros(Nu*Nc,1);
THETA zeros(Ny*Np,1);
-THETA zeros(Ny*Np,1)};
b_cons_cell = { Umax-Ut;
-Umin+Ut;
Ycmax-PSI*kesi-GAMMA*PHI;
-Ycmin+PSI*kesi+GAMMA*PHI};
A_cons = cell2mat(A_cons_cell);
b_cons = cell2mat(b_cons_cell);
%以上为状态量不等式约束的取值
M = 10;%松弛因子
delta_umin = -0.0148;%前轮转角控制增量
delta_umax = 0.0148;
delta_Umin = kron(ones(Nc,1),delta_umin);
delta_Umax = kron(ones(Nc,1),delta_umax);
lb = [delta_Umin;0];
ub = [delta_Umax;M];
%以上为状态量上下界约束，包括控制时域内的控制增量和松弛因子
%%%%%%%求解%%%%%%%%%
x_start = zeros(Nc+1,1);
options = optimset('algorithm','interior-point-convex');
[X_result,~,~] = quadprog(H,f,A_cons,b_cons,[],[],lb,ub,x_start,options);
%%%%%%%计算输出%%%%%%%%%%
u_piao = X_result(1);
U = kesi(7,1)+u_piao;
sys = [U;Yita_ref(1);Yita_ref(2)];
toc
%
%=============================================================================
% mdlGetTimeOfNextVarHit
% Return the time of the next hit for this block. Note that the result is
% absolute time. Note that this function is only used when you specify a
% variable discrete-time sample time [-2 0] in the sample time array in
% mdlInitializeSizes.
%=============================================================================
%
% end mdlGetTimeOfNextVarHit
%
%=============================================================================
% mdlTerminate
% Perform any end of simulation tasks.
%=============================================================================
%
% end mdlTerminate
说一点调参经验：我的仿真步长是0.01；示例是0.05，仿真步长变小了以后Q,R等矩阵的值应当相应缩小，否则会出现超调，导致求解器发散。
1903
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
