SC42125模型预测控制matlab代码.zip

function [y_final, yN] = ref_gen(x, Ts, I, N, maxVel) scenario = 0; nInputs = length(I.inputs); Veh_width = 1.75; %[m] Width of the vehicle Adj = 10; %[m] Adjustment on when the manoeuvre starts x_vel = maxVel; X_p = x(I.xPos-nInputs); Y_p = x(I.yPos-nInputs); yaw = x(I.heading-nInputs); y_vel = x(I.yVel-nInputs); r = x(I.yawRate-nInputs); samp_t = Ts; my_offset = 0; N_len = N; %Manoeuvre parameter selection based on man_select port. If 1, the ISO %3888-1 moose test will be done. If 2, the ISO 3888-2 Double Lane Change %will be done. If neither 1 nor 2 is selected, the program will default to %the Double Lane Change switch scenario case 2 %ISO 3888-1 Moose test manoeuvre %required parameters to define manoeuvre L_crvo = 13.5; %[m] length in x direction of curve one L_mid = 11; %[m] length in x direction of middle part L_crvt = 12.5; %[m] length in x direction of curve two a1 = 0.5; %slope of curve 1 a2 = 0.5; %slope of curve 2 %calculated parameters B1 = 1 + 1.05*Veh_width + 0.625; %[m] total lateral displacement curve 1 B2 = B1; %[m] total lateral displacement curve 2 case 1 %ISO 3888-2 Double Lane Change manoeuvre L_crvo = 30; %[m] length in x direction of curve one L_mid = 25; %[m] length in x direction of middle part L_crvt = 25; %[m] length in x direction of curve two a1 = 0.3221395; %slope of curve 1 a2 = 0.3221395; %slope of curve 2 B1 = 3.5 + 0.05*Veh_width; %[m] total lateral displacement curve 1 B2 = 3.5 + 0.05*Veh_width; %[m] total lateral displacement curve 2 otherwise L_crvo = 30; %[m] length in x direction of curve one L_mid = 25; %[m] length in x direction of middle part L_crvt = 25; %[m] length in x direction of curve two a1 = 0.25; %slope of curve 1 a2 = 0.25; %slope of curve 2 B1 = 3.5 + 0.05*Veh_width; %[m] total lateral displacement curve 1 B2 = 3.5 - 0.05*Veh_width; %[m] total lateral displacement curve 2 end %calculating parameters from switch case values c1 = L_crvo/2; %[m] central point of manoeuvre curve 1; c2 = L_crvt/2; %[m] central point of manoeuvre curve 2 s2 = L_crvo + L_mid; %[m] starting point of second curve (0 = Adj) %other initialisation parameters y = zeros(length([I.inputs, I.states]),N_len); X_vel = x_vel*cos(yaw) - y_vel*sin(yaw); % global longitudinal velocity Y_vel = x_vel*sin(yaw) + y_vel*cos(yaw); % global lateral velocity X_1 = X_p - Adj; %To normalise manoeuvre to start turning at point Adj %% Create reference for visualisation (1 sample per iteration instead of 30 in the next step) if X_p <= Adj + L_crvo + 0.5*L_mid %For DLC 42.5m after initial steering is 12.5m before start of second turn, hence halfway trough the intermediate straight yaw_ref1 = atan((a1*B1*exp(-a1*(X_1 - c1)))/(1+exp(-a1*(X_1 - c1))).^2); % reference yaw angle curvature_ref1 = (B1*a1.^2*exp(a1*(c1 - (X_1 + (samp_t*x_vel))))*(exp(a1*(c1 - (X_1 + (samp_t*x_vel)))) - 1))/((exp(a1*(c1 - (X_1 + (samp_t*x_vel)))) + 1).^3*((B1.^2*a1.^2*exp(2*a1*(c1 - (X_1 + (samp_t*x_vel)))))/(exp(a1*(c1 - (X_1 + (samp_t*x_vel)))) + 1).^4 + 1).^(3/2)); yawrate_ref1 = curvature_ref1 * x_vel; % reference yaw rate y_ref1 = B1/(1+exp(-a1*(X_1 - c1))) + my_offset; % reference lateral position %vx_ref1 = X_vel*cos(yaw_ref1) + Y_vel*sin(yaw_ref1); % reference longitudinal velocity vx_ref1 = maxVel; % reference longitudinal velocity else %For second part, create a mirrored manoeuvre, hence some values with %negative sign. Also adjust X_1 with s2 such that the turn starts at %the correct position yaw_ref1 = -atan((a2*B2*exp(-a2*(X_1 - s2 - c2)))/(1+exp(-a2*(X_1 - s2 - c2))).^2); curvature_ref1 = -(B2*a2.^2*exp(a2*(c2 - X_1 + s2))*(exp(a2*(c2 - X_1 + s2 )) - 1))/((exp(a2*(c2 - X_1 + s2)) + 1).^3*((B2.^2*a2.^2*exp(2*a2*(c2 - X_1 + s2 )))/(exp(a2*(c2 - X_1 + s2)) + 1).^4 + 1).^(3/2)); yawrate_ref1 = curvature_ref1 * x_vel; y_ref1 = -B2/(1+exp(-a2*(X_1 - s2 - c2))) + my_offset + B1; %vx_ref1 = X_vel*cos(yaw_ref1) + Y_vel*sin(yaw_ref1); vx_ref1 = maxVel; % reference longitudinal velocity end %% Create reference points for the next N steps (to be used in MPC) for i = 1:1:N_len if X_p < Adj-200 X_i = -200; else X_i = X_p - Adj + (i*samp_t*x_vel); %increment X_i position end X_r = X_p +(i*samp_t*X_vel); %assign reference X position if scenario == 3 if X_r <= Adj % X position is before the manoeuvre yaw_ref = atan((B1*a1*exp(a1*(c1 - X_i)))/(exp(a1*(c1 - X_i)) + 1).^2); % reference yaw angle % X_vel_ref_local = X_vel*cos(yaw_ref) - Y_vel*sin(yaw_ref); X_vel_ref_local = maxVel; Y_vel_ref_local = -X_vel*sin(yaw_ref) + Y_vel*cos(yaw_ref); curvature_ref = (B1*a1.^2*exp(a1*(c1 - X_i))*(exp(a1*(c1 - X_i)) - 1))/((exp(a1*(c1 - X_i)) + 1).^3*((B1.^2*a1.^2*exp(2*a1*(c1 - X_i)))/(exp(a1*(c1 - X_i)) + 1).^4 + 1).^(3/2)); %yaw_rate = curvature_ref * x_vel; yaw_rate = curvature_ref1 * x_vel; Y_ref = B1/(1+exp(-a1*(X_i - c1))) + my_offset; % reference lateral position flag_ss = 1; elseif X_r > Adj && X_r <= Adj + L_crvo + 0.5*L_mid %X position is between the start of manoeuvre and middle of the intermediate straight flag_ss = 2; yaw_ref = atan((B1*a1*exp(a1*(c1 - X_i)))/(exp(a1*(c1 - X_i)) + 1).^2); % reference yaw angle % X_vel_ref_local = X_vel*cos(yaw_ref) - Y_vel*sin(yaw_ref); X_vel_ref_local = maxVel; Y_vel_ref_local = -X_vel*sin(yaw_ref) + Y_vel*cos(yaw_ref); curvature_ref = (B1*a1.^2*exp(a1*(c1 - X_i))*(exp(a1*(c1 - X_i)) - 1))/((exp(a1*(c1 - X_i)) + 1).^3*((B1.^2*a1.^2*exp(2*a1*(c1 - X_i)))/(exp(a1*(c1 - X_i)) + 1).^4 + 1).^(3/2)); yaw_rate = curvature_ref1 * x_vel; Y_ref = B1/(1+exp(-a1*(X_i - c1))) + my_offset; % reference lateral position else %X position is past halfway trough the intermediate straight flag_ss = 2; yaw_ref = atan((B1*a1*exp(a1*(c1 - X_i)))/(exp(a1*(c1 - X_i)) + 1).^2); % reference yaw angle % X_vel_ref_local = X_vel*cos(yaw_ref) - Y_vel*sin(yaw_ref); X_vel_ref_local = maxVel; Y_vel_ref_local = -X_vel*sin(yaw_ref) + Y_vel*cos(yaw_ref); curvature_ref = (B1*a1.^2*exp(a1*(c1 - X_i))*(exp(a1*(c1 - X_i)) - 1))/((exp(a1*(c1 - X_i)) + 1).^3*((B1.^2*a1.^2*exp(2*a1*(c1 - X_i)))/(exp(a1*(c1 - X_i)) + 1).^4 + 1).^(3/2)); yaw_rate = 0; Y_ref = B1/(1+exp(-a1*(X_i - c1))) + my_offset; % reference lateral position end else if X_r <= Adj % X position is before the manoeuvre yaw_ref = atan((B1*a1*exp(a1*(c1 - X_i)))/(exp(a1*(c1 - X_i)) + 1).^2); % reference yaw angle % X_vel_ref_local = X_vel*cos(yaw_ref) - Y_vel*sin(yaw_ref); X_vel_ref_local = maxVel; Y_vel_ref_local = -X_vel*sin(yaw_ref) + Y_vel*cos(yaw_ref); curvature_ref = (B1*a1.^2*exp(a1*(c1 - X_i))*(exp(a1*(c1 - X_i)) - 1))/((exp(a1*(c1 - X_i)) + 1).^3*((B1.^2*a1.^2*exp(2*a1*(c1 - X_i)))/(exp(a1*(c1 - X_i)) + 1).^4 + 1).^(3/2)); %yaw_rate = curvature_ref * x_vel; yaw_rate = curvature_ref1 * x_vel; Y_ref = B1/(1+exp(-a1*(X_i - c1))) + my_offset; % reference lateral position flag_ss = 1; elseif X_r > Adj && X_r <= Adj + L_crvo + 0.5*L_mid %X position is between the start of manoeuvre and middle of the intermediate straight flag_ss = 2; yaw_ref = atan((B1*a1*exp(a