基于simulink的MPC车辆行驶控制器仿真【包含程序操作录像,代码中文注释】

%%将图像添加到路径 addpath(genpath('Images')); %%定义参考点 r = 75;%圆的半径 theta = 0:0.01:2*pi; x1 = -r*cos(theta); %我们想创建一个滑雪板轨道，因此我们使用了圆方程。 %或者，您也可以使用图八方程来生成参考点。 y1 = r*sin(theta); x2 = -2*r+r*cos(theta); y2 = r*sin(theta); xEight = [x1 x1 x2 x2]; yEight = [y1 y1 y2 y2]; %绘图（xEight，yEight） xRef = yEight; yRef = xEight; %%定义模型中使用的车辆参数 L = 3; %自行车长度 ld = 4; %前瞻距离 X_o = xRef(1); %车辆初始位置 Y_o = yRef(1); %车辆初始位置 psi_o = 0; %初始化偏航角是一个重要步骤 %%八字形航道参考姿态矢量的计算 %根据车辆行驶的距离，使用1-D生成姿势 %计算距离矢量 rr = [yEight;xEight;xEight*0]'; distancematrix = squareform(pdist(rr)); distancesteps = zeros(length(rr)-1,1); for i = 2:length(rr) distancesteps(i-1,1) = distancematrix(i,i-1); end totalDistance = sum(distancesteps); %总行驶距离 distbp = cumsum([0; distancesteps]); %每个航路点的距离 gradbp = linspace(0,totalDistance,2500); %线性化距离 %基于距离线性化X和Y矢量 xRef2 = interp1(distbp,xRef,gradbp,'pchip'); yRef2 = interp1(distbp,yRef,gradbp,'pchip'); yRef2s = smooth(gradbp,yRef2); xRef2s = smooth(gradbp,xRef2); %计算曲率矢量 curvature = getCurvature(xRef2,yRef2); %%计算MPC模型中使用的A、B、C矩阵 tau = 0.5; Vx = 10; m = 2000; Iz = 4000; Lf = 1.4; Lr = 1.6; Cf = 12e3; Cr = 11e3; %纵向模型 A1 = [-1/tau 0; 1 0]; B1 = [1/tau; 0]; C1 = [ 0 1]; D1 = 0; %横向模型 A2 = [-2*(Cf+Cr)/m/Vx -Vx-2*(Cf*Lf-Cr*Lr)/m/Vx;... -2*(Cf*Lf-Cr*Lr)/Iz/Vx -2*(Cf*Lf^2+Cr*Lr^2)/Iz/Vx]; B2 = 2*Cf*[1/m; Lf/Iz]; C2 = [1 0; 0 1]; D2 = [0;0]; %组合模型 A = [A1 zeros(2,2); zeros(2,2) A2]; B = [B1 zeros(2,1); zeros(2,1) B2]; C = [C1 zeros(1,2); zeros(2,2) C2]; D = [D1 zeros(1,1); zeros(2,1) D2]; %%MPC踏板地图 %附加车辆参数 rho = 1.21; Cd = 0.3; Af = 2; tire_r = 0.309; %二维查找表的边界 accel_vec = (-4:0.5:4)'; % 加速度在-4和4m/s^2之间 vel_vec = 0:2:20; % 车辆速度在0到20 m/s之间 torque_map = zeros(length(accel_vec),length(vel_vec)); %计算所需扭矩 for i = 1:length(accel_vec) for j = 1:length(vel_vec) torque_map(i,j) = tire_r*((m*accel_vec(i))+(0.5*rho*Cd*Af*vel_vec(j)^2)+160); end end %基于动力系统参数将扭矩转换为踏板 pedal_map = torque_map; %正扭矩是根据动力系统的最大车轮扭矩进行缩放的 max_prop_torque = 425*9.5; pedal_map(pedal_map>0) = pedal_map(pedal_map>0)/max_prop_torque; %计算从扭矩到最大压力的转换 pressure_conv = (0.2*7.5e6*pi*0.05*0.05*.177*2/4)*4*1.01; pedal_map(pedal_map<0) = pedal_map(pedal_map<0)/pressure_conv; %曲率函数 function curvature = getCurvature(xRef,yRef) %通过X和Y矢量的梯度计算梯度 DX = gradient(xRef); D2X = gradient(DX); DY = gradient(yRef); D2Y = gradient(DY); curvature = (DX.*D2Y - DY.*D2X) ./(DX.^2+DY.^2).^(3/2); end