基于模型预测控制（MPC）无人驾驶汽车轨迹跟踪控制算法，基于MATLAB/simulink与carsim联合仿真

function [sys,x0,str,ts] = MPC_4WS(t,x,u,flag) % Q_40*40,R_5*5,a_6*6,b_6*1,d_k 6*1,A_7*7,B_7*1,C_2*7 % Nx=6;%状态量的个数；Nu=1;%控制量的个数；Ny=2;%输出量的个数；Np =20;%预测时域；Nc=5;%控制时域 % 状态量=[phi,y_dot,Y,phi_dot]，控制量为前轮偏角[delta_f,delta_r] 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 4 % sys = mdlGetTimeOfNextVarHit(t,x,u); % Get next sample time 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 = 4; sizes.NumOutputs = 2; sizes.NumInputs = 8; sizes.DirFeedthrough = 1; % Matrix D is non-empty. sizes.NumSampleTimes = 1; sys = simsizes(sizes); x0 =[0.001;0.0001;0.0001;0.00001]; global U;%U为我们的控制量 U=[0, 0];% % Initialize the discrete states. str = []; % Set str to an empty matrix. ts = [0.05 0]; % sample time: [period, offset]，采样时间影响最大。 global VehiclePara; VehiclePara.m = 1723; %m为车辆质量,Kg; Sprung mass = 1370 VehiclePara.g = 9.8; VehiclePara.Lf = 1.232; % 1.05 VehiclePara.Lr = 1.468; % 1.55 VehiclePara.L = 2.6; %VehiclePara.Lf + VehiclePara.Lr; VehiclePara.Iz = 4175; %I为车辆绕Z轴的转动惯量，车辆固有参数 VehiclePara.Ccf = 66900; VehiclePara.Ccr = 62700; VehiclePara.Clf = 66900; VehiclePara.Clr = 62700; global MPCParameters; MPCParameters.Ts = 0.05; % the sample time of near term % Update the discrete states function sys = mdlUpdates(t,x,u) sys = x; %End of mdlUpdate. %============================================================== % Calculate outputs %============================================================== function sys = mdlOutputs(t,x,u) global VehiclePara; global MPCParameters; global U; %设置U为全局量 tic Nx=4;%状态量的个数 Nu=2;%控制量的个数 Ny=2;%输出量的个数 Np=20;%预测时域 Nc=5;%控制时域 Row=1000;%松弛因子权重 fprintf('Update start, t=%6.3f\n',t) % 反馈状态量 y_dot=u(1)/3.6; %横向速度化为m/s x_dot=u(2)/3.6+0.0001;%CarSim输出的是km/h，转换为m/s phi=u(3)*pi/180; %CarSim输出的为角度，角度转换为弧度，使用弧度 phi_dot=u(4)*pi/180;%角速度 Y=u(5);%单位为m，横向位置 X=u(6);%单位为米，纵向位置 % %% 车辆参数输入 %syms sf为前轮滑移率，sr为后轮滑移率 Sf= u(7); Sr= u(8); %syms lf%前轮距离车辆质心的距离，lr为后轮距离车辆质心的距离 Lf=1.232;Lr=1.468; %syms C_cf前轮线性横向侧偏刚度； C_cr后轮线性横向侧偏刚度 ；C_lf 前轮纵向侧偏刚度； C_lr 后轮纵向侧偏刚度 Ccf=66900;Ccr=62700;Clf=66900;Clr=62700; %syms m g I;%m为车辆质量，g为重力加速度，I为车辆绕Z轴的转动惯量，车辆固有参数 m=1723;g=9.8;Iz=4175; % %% 参考轨迹生成 X_predict=zeros(Np,1);%用于保存预测时域内的纵向位置信息，这是计算期望轨迹的基础 phi_ref=zeros(Np,1);%用于保存预测时域内的参考横摆角信息 Y_ref=zeros(Np,1);%用于保存预测时域内的参考横向位置信息 load path5_mpc4ws % 以下计算状态量，即状态量与控制量合在一起 kesi=zeros(Nx+Nu,1);%状态量4个，控制量2个 kesi(1)=phi; kesi(2)=y_dot; kesi(3)=Y; kesi(4)=phi_dot; kesi(5)=U(1); kesi(6)=U(2); %这个是控制量前轮偏角，即在轨迹跟踪过程中通过对前轮偏角进行控制，纵向速度保持不变 delta_f=U(1); %前轮转角 delta_r=U(2); %前轮转角 % fprintf('Update start, u(1)=%4.2f\n',U(1)) vx = 10; T=0.05;%仿真步长 T_all=20;%总的仿真时间，主要功能是防止计算期望轨迹越界 %权重矩阵设置 Q_cell=cell(Np,Np);%总的元胞Q为20行20列的，40*40 for i=1:1:Np for j=1:1:Np if i==j Q_cell{i,j}=[3000 0;0 5000;]; %作用是将方阵对角线的地方设置为该矩阵，其余为止全部为0 else Q_cell{i,j}=zeros(Ny,Ny); %20行20列的矩阵中每一个小的位置上都是2*2的小矩阵 end end end R=50000*eye(Nu*Nc); %采用动力学模型，该矩阵与车辆参数密切相关，通过对动力学方程求解雅克比矩阵得到，a为6*6，b为6*1 [a, b] = func_Model_linearization_Jacobian1(kesi,Sf,Sr,MPCParameters,VehiclePara); d_k=zeros(Nx,1);%计算偏差,4*1 state_k1=zeros(Nx,1);%预测下一时刻状态量，用于计算偏差，4*1 %以下即为根据离散非线性模型预测下一时刻状态量 %注意，为避免前后轴距的表达式（a,b）与控制器的a,b矩阵冲突，将前后轴距的表达式改为lf和lr，下面这些公式见（P35） %自主车辆线性时变模型预测路径跟踪控制 state_k1(1,1)=phi+T*phi_dot; state_k1(2,1)=y_dot+T*(-(Ccf+Ccr)*y_dot/(m*vx) - (Lf*Ccf-Lr*Ccr)*phi_dot/(m*vx) + vx*phi_dot + Ccf*delta_f/m + Ccr*delta_r/m); state_k1(3,1)=Y+T*(vx * phi + y_dot); state_k1(4,1)=phi_dot+T*(-(Lf*Ccf-Lr*Ccr)*y_dot/(Iz*vx) - (Lf^2*Ccf+Lr^2*Ccr)*phi_dot/(Iz*vx) + Lf*Ccf*delta_f/Iz - Lr*Ccr*delta_r/Iz); %到此为止，下一时刻的状态量已经推导完毕 d_k=state_k1-a*kesi(1:4,1)-b*kesi(5:6,1); d_piao_k=zeros(Nx+Nu,1);%偏差矩阵d_k的增广形式，又加了控制量的那2行，维度6*1 d_piao_k(1:4,1)=d_k; %给d_k的增广形式进行赋值，前6行为我们的偏差 d_piao_k(5:6,1)=[0;0]; A_cell=cell(2,2); B_cell=cell(2,1); A_cell{1,1}=a;%第一行第一列4*4 A_cell{1,2}=b;%第一行第二列为4*2 A_cell{2,1}=zeros(Nu,Nx);%第二行第一列为1*6 A_cell{2,2}=eye(Nu);%最二行第二列为1*1 B_cell{1,1}=b;%B矩阵第一行第一列为b,4*2 B_cell{2,1}=eye(Nu);%第二行第一列为2*2单位阵 A=cell2mat(A_cell);%A矩阵维度7*7 B=cell2mat(B_cell);%B矩阵维度7*1 C=[1 0 0 0 0 0;0 0 1 0 0 0;];%我们只输出状态看空间的第1个量（横摆角）和第3个量（横向位置Y） PSI_cell=cell(Np,1);%输出方程的第一个系数矩阵，维度 20*1 THETA_cell=cell(Np,Nc);%输出方程的第二个系数矩阵，维度 20*5 GAMMA_cell=cell(Np,Np);%维度 20*20 PHI_cell=cell(Np,1);%维度 20*1 for p=1:1:Np PHI_cell{p,1}=d_piao_k;% 6行1列的偏差，理论上来说，这个是要实时更新的，但是为了简便，这里又一次近似 for q=1:1:Np if q<=p %下三角矩阵 GAMMA_cell{p,q}=C*A^(p-q); %该矩阵由C和A构造而成 else GAMMA_cell{p,q}=zeros(Ny,Nx+Nu); %每一个元胞的大小为2（输出两个数）*7（状态空间量的个数） end end end %最终该矩阵为40*140 for j=1:1:Np %j从1到20 PSI_cell{j,1}=C*A^j; %构造输出方程式（21）中第一项的系数矩阵，2*7*7*7=2*7 for k=1:1:Nc %k从1到5，构造输出方程式（21）中第二项的系数矩阵