第5章.rar_主动转向_自动驾驶_车辆跟踪_车辆路径_转向

function [sys,x0,str,ts] = chapter5_2_2(t,x,u,flag) % 该程序功能：用LTV MPC 和车辆简化动力学模型（小角度假设）设计控制器，作为Simulink的控制器 % 程序版本 V1.0，MATLAB版本：R2011a,采用S函数的标准形式， % 程序编写日期 2013.12.11 % 最近一次改写 2013.12.16 % 状态量=[y_dot,x_dot,phi,phi_dot,Y,X]，控制量为前轮偏角delta_f 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 = 6; sizes.NumOutputs = 1; 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;0.00001;0.00001]; global U; U=[0];%控制量初始化,这里面加了一个期望轨迹的输出，如果去掉，U为一维的 % global x; % x = zeros(md.ne + md.pye + md.me + md.Hu*md.me,1); % Initialize the discrete states. str = []; % Set str to an empty matrix. ts = [0.02 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; %global u_piao; global U; %global kesi; tic Nx=6;%状态量的个数 Nu=1;%控制量的个数 Ny=2;%输出量的个数 Np =20;%预测步长 Nc=5;%控制步长 Row=1000;%松弛因子权重 fprintf('Update start, t=%6.3f\n',t) %输入接口转换,x_dot后面加一个非常小的数，是防止出现分母为零的情况 % y_dot=u(1)/3.6-0.000000001*0.4786;%CarSim输出的是km/h，转换为m/s y_dot=u(1)/3.6; x_dot=u(2)/3.6+0.0001;%CarSim输出的是km/h，转换为m/s phi=u(3)*3.141592654/180; %CarSim输出的为角度，角度转换为弧度 phi_dot=u(4)*3.141592654/180; Y=u(5);%单位为m X=u(6);%单位为米 Y_dot=u(7); X_dot=u(8); %% 车辆参数输入 %syms sf sr;%分别为前后车轮的滑移率,需要提供 Sf=0.2; Sr=0.2; %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;I=4175; %% 参考轨迹生成 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);%用于保存预测时域内的期望轨迹 % 以下计算kesi,即状态量与控制量合在一起 kesi=zeros(Nx+Nu,1); kesi(1)=y_dot;%u(1)==X(1) kesi(2)=x_dot;%u(2)==X(2) kesi(3)=phi; %u(3)==X(3) 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=0.02;%仿真步长 T_all=20;%总的仿真时间，主要功能是防止计算期望轨迹越界 %权重矩阵设置 Q_cell=cell(Np,Np); for i=1:1:Np; for j=1:1:Np; if i==j %Q_cell{i,j}=[200 0;0 100;]; Q_cell{i,j}=[2000 0;0 10000;]; else Q_cell{i,j}=zeros(Ny,Ny); end end end %R=5*10^4*eye(Nu*Nc); R=5*10^5*eye(Nu*Nc); %最基本也最重要的矩阵，是控制器的基础，采用动力学模型，该矩阵与车辆参数密切相关，通过对动力学方程求解雅克比矩阵得到 a=[ 1 - (259200*T)/(1723*x_dot), -T*(phi_dot + (2*((460218*phi_dot)/5 - 62700*y_dot))/(1723*x_dot^2) - (133800*((154*phi_dot)/125 + y_dot))/(1723*x_dot^2)), 0, -T*(x_dot - 96228/(8615*x_dot)), 0, 0 T*(phi_dot - (133800*delta_f)/(1723*x_dot)), (133800*T*delta_f*((154*phi_dot)/125 + y_dot))/(1723*x_dot^2) + 1, 0, T*(y_dot - (824208*delta_f)/(8615*x_dot)), 0, 0 0, 0, 1, T, 0, 0 (33063689036759*T)/(7172595384320*x_dot), T*(((2321344006605451863*phi_dot)/8589934592000 - (6325188028897689*y_dot)/34359738368)/(4175*x_dot^2) + (5663914248162509*((154*phi_dot)/125 + y_dot))/(143451907686400*x_dot^2)), 0, 1 - (813165919007900927*T)/(7172595384320000*x_dot), 0, 0 T*cos(phi), T*sin(phi), T*(x_dot*cos(phi) - y_dot*sin(phi)), 0, 1, 0 -T*sin(phi), T*cos(phi), -T*(y_dot*cos(phi) + x_dot*sin(phi)), 0, 0, 1]; b=[ 133800*T/1723 T*((267600*delta_f)/1723 - (133800*((154*phi_dot)/125 + y_dot))/(1723*x_dot)) 0 5663914248162509*T/143451907686400 0 0]; d_k=zeros(Nx,1);%计算偏差 state_k1=zeros(Nx,1);%预测的下一时刻状态量，用于计算偏差 %以下即为根据离散非线性模型预测下一时刻状态量 %注意，为避免前后轴距的表达式（a,b）与控制器的a,b矩阵冲突，将前后轴距的表达式改为lf和lr state_k1(1,1)=y_dot+T*(-x_dot*phi_dot+2*(Ccf*(delta_f-(y_dot+lf*phi_dot)/x_dot)+Ccr*(lr*phi_dot-y_dot)/x_dot)/m); state_k1(2,1)=x_dot+T*(y_dot*phi_dot+2*(Clf*Sf+Clr*Sr+Ccf*delta_f*(delta_f-(y_dot+phi_dot*lf)/x_dot))/m); state_k1(3,1)=phi+T*phi_dot; state_k1(4,1)=phi_dot+T*((2*lf*Ccf*(delta_f-(y_dot+lf*phi_dot)/x_dot)-2*lr*Ccr*(lr*phi_dot-y_dot)/x_dot)/I); state_k1(5,1)=Y+T*(x_dot*sin(phi)+y_dot*cos(phi)); state_k1(6,1)=X+T*(x_dot*cos(phi)-y_dot*sin(phi)); d_k=state_k1-a*kesi(1:6,1)-b*kesi(7,1);%根据falcone公式（2.11b）求得d(k,t) d_piao_k=zeros(Nx+Nu,1);%d_k的增广形式，参考falcone(B,4c) d_piao_k(1:6,1)=d_k; d_piao_k(7,1)=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{