【carsim+simulink 联合仿真——车辆轨迹MPC跟踪】

学习北理工的无人驾驶车辆模型预测控制第2版第四章，使用的仿真软件为Carsim8和MatlabR2019a联合仿真，使用MPC控制思想对车辆进行轨迹跟踪控制，并给出仿真结果。

mpc控制器函数：s-function

function [sys,x0,str,ts] = MY_MPCController3(t,x,u,flag)
%   该函数是写的第3个S函数控制器(MATLAB版本：R2011a)
%   限定于车辆运动学模型，控制量为速度和前轮偏角，使用的QP为新版本的QP解法
%   [sys,x0,str,ts] = MY_MPCController3(t,x,u,flag)
%
% is an S-function implementing the MPC controller intended for use
% with Simulink. The argument md, which is the only user supplied
% argument, contains the data structures needed by the controller. The
% input to the S-function block is a vector signal consisting of the
% measured outputs and the reference values for the controlled
% outputs. The output of the S-function block is a vector signal
% consisting of the control variables and the estimated state vector,
% potentially including estimated disturbance states.

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 dsfunc.

%==============================================================
% 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; % this parameter doesn't matter
sizes.NumOutputs     = 2; %[speed, steering]
sizes.NumInputs      = 5; %[x,y,yaw,vx,steer_sw]
sizes.DirFeedthrough = 1; % Matrix D is non-empty.
sizes.NumSampleTimes = 1;
sys = simsizes(sizes);
x0 =[0;0;0];
global U; % store current ctrl vector:[vel_m, delta_m]
U=[0;0];
% Initialize the discrete states.
str = [];             % Set str to an empty matrix.
ts  = [0.05 0];       % sample time: [period, offset]
%End of mdlInitializeSizes

%==============================================================
% Update the discrete states
%==============================================================
function sys = mdlUpdates(t,x,u)

sys = x;
%End of mdlUpdate.

%==============================================================
% Calculate outputs
%==============================================================

function sys = mdlOutputs(t,x,u)
global a b u_piao;
%u_piao矩阵，用于存储每一个仿真时刻，车辆的实际控制量（实际运动状态）与目标控制量（运动状态）之间的偏差

global U; %store chi_tilde=[vel-vel_ref; delta - delta_ref]
global kesi;

tic
Nx=3;%状态量的个数
Nu =2;%控制量的个数
Np =60;%预测步长
Nc=30;%控制步长
Row=10;%松弛因子
fprintf('Update start, t=%6.3f\n',t)
t_d =u(3)*3.1415926/180;%CarSim输出的Yaw angle为角度，角度转换为弧度

%    %直线路径
%     r(1)=5*t; %ref_x-axis
%     r(2)=5;%ref_y-axis
%     r(3)=0;%ref_heading_angle
%     vd1=5;% ref_velocity
%     vd2=0;% ref_steering


% %     半径为25m的圆形轨迹, 圆心为(0, 35), 速度为5m/s
    r(1)=25*sin(0.2*t);
    r(2)=35-25*cos(0.2*t);
    r(3)=0.2*t;
    vd1=5;
    vd2=0.104;

%     %半径为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/s
%      r(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), 速度为4m/s
% r(1)=25*sin(0.16*t);
% r(2)=25+10-25*cos(0.16*t);
% r(3)=0.16*t;
% vd1=4;
% vd2=0.104;


%     t_d =  r(3);
kesi=zeros(Nx+Nu,1);
kesi(1) = u(1)-r(1);%u(1)==X(1),x_offset
kesi(2) = u(2)-r(2);%u(2)==X(2),y_offset
heading_offset = t_d - r(3); %u(3)==X(3),heading_angle_offset

if (heading_offset < -pi)
    heading_offset = heading_offset + 2*pi;
end
if (heading_offset > pi)
    heading_offset = heading_offset - 2*pi;
end
kesi(3)=heading_offset;

%      U(1) = u(4)/3.6 - vd1; % vel, km/h-->m/s
%      steer_SW = u(5)*pi/180;
%      steering_angle = steer_SW/18.0;
%      U(2) = steering_angle - vd2;

kesi(4)=U(1); % vel-vel_ref
kesi(5)=U(2); % steer_angle - steering_ref
fprintf('vel-offset=%4.2f, steering-offset, U(2)=%4.2f\n',U(1), U(2))

T=0.05;
T_all=40;%临时设定，总的仿真时间，主要功能是防止计算期望轨迹越界
% Mobile Robot Parameters
L = 2.6; % wheelbase of carsim vehicle
% Mobile Robot variable


%矩阵初始化
u_piao=zeros(Nx,Nu);
Q=eye(Nx*Np,Nx*Np);
R=5*eye(Nu*Nc);
a=[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/(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:Np
    PHI_cell{j,1}=C*A^j;
    for k=1:1:Nc
        if k<=j
            THETA_cell{j,k}=C*A^(j-k)*B;
        else
            THETA_cell{j,k}=zeros(Nx,Nu);
        end
    end
end
PHI=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}=zero