Data-EnablEd Predictive Leading Cruise Control (DeeP-LCC)¶
Introduction¶
One of the biggest obstacles to the control of connected and autonomous vehicles (CAVs) is the lack of a trivial and accurate way to find car-following dynamics of human-driven vehicles. DeeP-LCC (Data-EnablEd Predictive Leading Cruise Control) introduced in the paper proposed a way to control CAVs in mixed traffic relying on data-driven non-parametric strategies. We used Willems' fundamental lemma in the paper to obtain a data-centric representation of mixed traffic behavior. Then we used a receding horizon strategy to solve the optimization problem at each time step. We further did numeric experiments to validate the performance of DeeP-LCC, which shows good improvements in the traffic and reveals excellent potential for further research. In particular, with a CAV penetration rate of 25%, DeeP-LCC help reduce up to 24.69% fuel consumption with safety guarantees in the emergency braking scenario. The following content explains how Matlab and Python implementations work for the numeric simulation, and includes a brief discussion of the results.
Getting Started¶
Graph of the dependency of all Matlab files:
Matlab Implementation¶
Main File¶
main_brake_simulation.m¶
This main function is used for safety-critical simulation in the scenario that the head vehicle takes a sudden brake. More detailed can be refered to Section V of the paper "Data-Driven Predicted Control for Connected and Autonomous Vehicles in Mixed Traffic".
Add path and initialization.
Parameter setup. This section includes Scenario Setup and HDV setup.
- Scenario Setup
Click here to view code
% whether traffic flow is mixed
mix = 1; % 0. all HDVs; 1. there exist CAVs
ID = [0,0,1,0,0,1,0,0]; % ID of vehicle types
% 1: CAV 0: HDV
pos_cav = find(ID==1); % position of CAVs
n_vehicle = length(ID); % number of vehicles
n_cav = length(pos_cav); % number of CAVs
n_hdv = n_vehicle-n_cav; % number of HDVs
% perturbation on the head vehicle
per_type = 2; % 1. sinuoid perturbation 2. brake perturbation
sine_amp = 5; % amplitidue of sinuoid perturbation
brake_amp = 10; % brake amplitude of brake perturbation
% time
total_time = 40; % Total Simulation Time
Tstep = 0.05; % Time Step
total_time_step = total_time/Tstep;
- HDV setup
Click here to view code
% Type for HDV car-following model
hdv_type = 1; % 1. OVM 2. IDM
% Parameter setup for HDV
data_str = '2'; % 1. random ovm 2. manual heterogeneous ovm 3. homogeneous ovm
switch hdv_type
case 1
load(['_data/hdv_ovm_',data_str,'.mat']);
case 2
load('_data/hdv_idm.mat');
end
% Uncertainty for HDV acceleration
acel_noise = 0.1; % A white noise signal on HDV's acceleration
Formulation for DeeP-LCC
- Parameter setup
Click here to view code
% Type of the controller
controller_type = 2; % 1. DeeP-LCC 2. MPC
% Initialize Equilibrium Setup (they might be updated in the control process)
v_star = 15; % Equilibrium velocity
s_star = 20; % Equilibrium spacing for CAV
% Horizon setup
Tini = 20; % length of past data in control process
N = 50; % length of future data in control process
% Performance cost
weight_v = 1; % weight coefficient for velocity error
weight_s = 0.5; % weight coefficient for spacing error
weight_u = 0.1; % weight coefficient for control input
% Setup in DeeP-LCC
T = 2000; % length of data samples
lambda_g = 100; % penalty on ||g||_2^2 in objective
lambda_y = 1e4; % penalty on ||sigma_y||_2^2 in objective
% Constraints
constraint_bool = 1; % whether there exist constraints
acel_max = 2; % maximum acceleration
dcel_max = -5; % minimum acceleration (maximum deceleration)
spacing_max = 40; % maximum spacing
spacing_min = 5; % minimum spacing
u_limit = [dcel_max,acel_max];
s_limit = [spacing_min,spacing_max]-s_star;
% what signals are measurable (for output definition)
measure_type = 3; % 1. Only the velocity errors of all the vehicles are measurable;
% 2. All the states, including velocity error and spacing error are measurable;
% 3. Velocity error and spacing error of the CAVs are measurable,
% and the velocity error of the HDVs are measurable.
% equilibrium setup
fixed_spacing_bool = 0; % wheter fix equilibrium spacing
% 0. the equilibrium spacing will be updated via an OVM-type spacing policy
% 1. fix the equilibrium spacing
- Process parameters
Click here to view code
n_ctr = 2*n_vehicle; % number of state variables
m_ctr = n_cav; % number of input variables
switch measure_type % number of output variables
case 1
p_ctr = n_vehicle;
case 2
p_ctr = 2*n_vehicle;
case 3
p_ctr = n_vehicle + n_cav;
end
Q_v = weight_v*eye(n_vehicle); % penalty for velocity error
Q_s = weight_s*eye(p_ctr-n_vehicle); % penalty for spacing error
Q = blkdiag(Q_v,Q_s); % penalty for trajectory error
R = weight_u*eye(m_ctr); % penalty for control input
u = zeros(m_ctr,total_time_step); % control input
x = zeros(n_ctr,total_time_step); % state variables
y = zeros(p_ctr,total_time_step); % output variables
pr_status = zeros(total_time_step,1); % problem status
e = zeros(1,total_time_step); % external input
- Pre-collected data
Click here to view code
Simulation
Click here to view code
% Mixed traffic states
% S(time,vehicle id,state variable), in state variable: 1. position; 2. velocity; 3. acceleration
S = zeros(total_time_step,n_vehicle+1,3);
S(1,1,1) = 0;
for i = 2 : n_vehicle+1
S(1,i,1) = S(1,i-1,1) - hdv_parameter.s_star(i-1);
end
S(1,:,2) = v_star * ones(n_vehicle+1,1);
% reference trajectory is all zeros: stabilize the system to equilibrium
r = zeros(p_ctr,total_time_step+N);
Experiment starts here
- Initialization: the CAVs and the head vehicle have zero control input
Click here to view code
% initial past data in control process
uini = zeros(m_ctr,Tini);
eini = zeros(1,Tini);
yini = zeros(p_ctr,Tini);
for k = 1:Tini-1
% calculate acceleration for the HDVs
acel = HDV_dynamics(S(k,:,:),hdv_parameter) ...
-acel_noise + 2*acel_noise*rand(n_vehicle,1);
S(k,1,3) = 0; % the head vehicle
S(k,2:end,3) = acel; % all the vehicles use HDV model
S(k,pos_cav+1,3) = uini(:,k); % the CAV
% update traffic states
S(k+1,:,2) = S(k,:,2) + Tstep*S(k,:,3);
S(k+1,1,2) = eini(k) + v_star; % the velocity of the head vehicle
S(k+1,:,1) = S(k,:,1) + Tstep*S(k,:,2);
% update past output data
yini(:,k) = measure_mixed_traffic(S(k,2:end,2),S(k,:,1),ID,v_star,s_star,measure_type);
end
k_end = k+1;
yini(:,k_end) = measure_mixed_traffic(S(k_end,2:end,2),S(k_end,:,1),ID,v_star,s_star,measure_type);
% update data in u,e,y
u(:,1:Tini) = uini;
e(:,1:Tini) = eini;
y(:,1:Tini) = yini;
% For MPC, which might have infeasible cases
previous_u_opt = 0;
- Continue the simulation
Click here to view code
for k = Tini:total_time_step-1
% calculate acceleration for the HDVs
acel = HDV_dynamics(S(k,:,:),hdv_parameter) ...
-acel_noise + 2*acel_noise*rand(n_vehicle,1);
S(k,2:end,3) = acel; % all the vehicles use HDV model
if mix
switch controller_type
case 1 % calculate control input via DeeP-LCC
if constraint_bool
[u_opt,y_opt,pr] = qp_DeeP_LCC(Up,Yp,Uf,Yf,Ep,Ef,uini,yini,eini,Q,R,r(:,k:k+N-1),...
lambda_g,lambda_y,u_limit,s_limit);
else
[u_opt,y_opt,pr] = qp_DeeP_LCC(Up,Yp,Uf,Yf,Ep,Ef,uini,yini,eini,Q,R,r(:,k:k+N-1),...
lambda_g,lambda_y);
end
case 2 % calculate control input via MPC
if constraint_bool
[u_opt,y_opt,pr] = qp_MPC(ID,Tstep,hdv_type,measure_type,v_star,uini,yini,N,Q,R,r(:,k:k+N-1),u_limit,s_limit,previous_u_opt);
previous_u_opt = u_opt;
else
[u_opt,y_opt,pr] = qp_MPC(ID,Tstep,hdv_type,measure_type,v_star,uini,yini,N,Q,R,r(:,k:k+N-1));
end
end
% one-step implementation in receding horizon manner
u(:,k) = u_opt(1:m_ctr,1);
% update accleration for the CAV
S(k,pos_cav+1,3) = u(:,k);
% judge whether AEB (automatic emergency braking, which is implemented in the function of 'HDV_dynamics') commands to brake
brake_vehicle_ID = find(acel==dcel_max); % the vehicles that need to brake
brake_cav_ID = intersect(brake_vehicle_ID,pos_cav); % the CAVs that need to brake
if ~isempty(brake_cav_ID)
S(k,brake_cav_ID+1,3) = dcel_max;
end
% record problem status
pr_status(k) = pr;
end
% update traffic states
S(k+1,:,2) = S(k,:,2) + Tstep*S(k,:,3);
% perturbation for the head vehicle
switch per_type
case 1
S(k+1,1,2) = v_star + sine_amp*sin(2*pi/(10/Tstep)*(k-Tini));
S(k+1,:,1) = S(k,:,1) + Tstep*S(k,:,2);
case 2
if (k-Tini)*Tstep < brake_amp/5
S(k+1,1,3) = -5;
elseif (k-Tini)*Tstep < brake_amp/5+5
S(k+1,1,3) = 0;
elseif (k-Tini)*Tstep < brake_amp/5+5+5
S(k+1,1,3) = 2;
else
S(k+1,1,3) = 0;
end
S(k+1,:,2) = S(k,:,2) + Tstep*S(k,:,3);
S(k+1,:,1) = S(k,:,1) + Tstep*S(k,:,2);
end
% update equilibrium setup for the CAVs
v_star = mean(S(k-Tini+1:k,1,2)); % update v_star
if ~fixed_spacing_bool
s_star = acos(1-v_star/30*2)/pi*(35-5) + 5; % update s_star
end
% update past data in control process
uini = u(:,k-Tini+1:k);
% the output needs to be re-calculated since the equilibrium might have been updated
for k_past = k-Tini+1:k
% Record output
y(:,k_past) = measure_mixed_traffic(S(k_past,2:end,2),S(k_past,:,1),ID,v_star,s_star,measure_type);
e(k_past) = S(k_past,1,2) - v_star;
end
yini = y(:,k-Tini+1:k);
eini = S(k-Tini+1:k,1,2) - v_star;
fprintf('Simulation number: %d | process... %2.2f%% \n',i_data,(k-Tini)/total_time_step*100);
%fprintf('Fixed Spacing: %d',fixed_spacing_bool);
%fprintf('Current spacing of the first CAV: %4.2f \n',S(k,3,1)-S(k,4,1));
end
k_end = k+1;
y(:,k_end) = measure_mixed_traffic(S(k_end,2:end,2),S(k_end,:,1),ID,v_star,s_star,measure_type);
tsim = toc;
fprintf('Simulation ends at %6.4f seconds \n', tsim);
- Results output
Click here to view code
if mix
switch controller_type
case 1
save(['..\_data\simulation_data\DeeP_LCC\constrained_simulation\simulation_data',data_str,'_',num2str(i_data),'_perType_',num2str(per_type),'_noiseLevel_',num2str(acel_noise),...
'_fixSpacing_',num2str(fixed_spacing_bool),...
'_hdvType_',num2str(hdv_type),'_lambdaG_',num2str(lambda_g),'_lambdaY_',num2str(lambda_y),'.mat'],...
'hdv_type','acel_noise','S','T','Tini','N','ID','Tstep','v_star','pr_status');
case 2
save(['..\_data\simulation_data\MPC\constrained_simulation\simulation_data',data_str,'_',num2str(i_data),'_perType_',num2str(per_type),'_noiseLevel_',num2str(acel_noise),...
'_fixSpacing_',num2str(fixed_spacing_bool),...
'_hdvType_',num2str(hdv_type),'.mat'],...
'hdv_type','acel_noise','S','T','Tini','N','ID','Tstep','v_star','pr_status');
end
else
save(['..\_data\simulation_data\HDV\constrained_simulation\simulation_data',data_str,'_',num2str(i_data),'_perType_',num2str(per_type),'_noiseLevel_',num2str(acel_noise),...
'_hdvType_',num2str(hdv_type),'.mat'],...
'hdv_type','acel_noise','S','T','Tini','N','ID','Tstep','v_star');
end
main_nedc_simulation.m¶
This main function is used for NEDC simulation in the scenario that the head vehicle follows a trajectory modified from the Extra-Urban Driving Cycle (EUDC) in New European Driving Circle (NEDC). More detailed can be refered to Section V of the paper "Data-Driven Predicted Control for Connected and Autonomous Vehicles in Mixed Traffic".
Add path and initialization.
Parameter setup. This section includes Scenario Setup and HDV setup.
- Scenario Setup
Click here to view code
% whether traffic flow is mixed
mix = 1; % 0. all HDVs; 1. there exist CAVs
ID = [0,0,1,0,0,1,0,0]; % ID of vehicle types
% 1: CAV 0: HDV
pos_cav = find(ID==1); % position of CAVs
n_vehicle = length(ID); % number of vehicles
n_cav = length(pos_cav); % number of CAVs
n_hdv = n_vehicle-n_cav; % number of HDVs
% Definition for Head vehicle trajectory
head_vehicle_trajectory = load('./_data/nedc_modified_v1.mat');
end_time = head_vehicle_trajectory.time(end); % end time for the head vehicle trajectory
head_vehicle_trajectory.vel = head_vehicle_trajectory.vel/3.6;
% Initialization Time
initialization_time = 30; % Time for the original HDV-all system to stabilize
adaption_time = 20; % Time for the CAVs to adjust to their desired state
% Total Simulation Time
total_time = initialization_time + adaption_time + end_time;
Tstep = 0.05; % Time Step
total_time_step = round(total_time/Tstep);
- HDV setup
Click here to view code
% Type for HDV car-following model
hdv_type = 1; % 1. OVM 2. IDM
% Parameter setup for HDV
data_str = '2'; % 1. random ovm 2. manual heterogeneous ovm 3. homogeneous ovm
switch hdv_type
case 1
load(['_data/hdv_ovm_',data_str,'.mat']);
case 2
load('_data/hdv_idm.mat');
end
% Uncertainty for HDV acceleration
acel_noise = 0.1; % A white noise signal on HDV's acceleration
Formulation for DeeP-LCC
- Parameter setup
Click here to view code
% Type of the controller
controller_type = 1; % 1. DeeP-LCC 2. MPC
% Initialize Equilibrium Setup (they might be updated in the control process)
v_star = 15; % Equilibrium velocity
s_star = 20; % Equilibrium spacing for CAV
% Horizon setup
Tini = 20; % length of past data in control process
N = 50; % length of future data in control process
% Performance cost
weight_v = 1; % weight coefficient for velocity error
weight_s = 0.5; % weight coefficient for spacing error
weight_u = 0.1; % weight coefficient for control input
% Setup in DeeP-LCC
T = 2000; % length of data samples
lambda_g = 100; % penalty on ||g||_2^2 in objective
lambda_y = 1e4; % penalty on ||sigma_y||_2^2 in objective
% Constraints
constraint_bool = 1; % whether there exist constraints
acel_max = 2; % maximum acceleration
dcel_max = -5; % minimum acceleration (maximum deceleration)
spacing_max = 40; % maximum spacing
spacing_min = 5; % minimum spacing
u_limit = [dcel_max,acel_max];
s_limit = [spacing_min,spacing_max]-s_star;
% what signals are measurable (for output definition)
measure_type = 3; % 1. Only the velocity errors of all the vehicles are measurable;
% 2. All the states, including velocity error and spacing error are measurable;
% 3. Velocity error and spacing error of the CAVs are measurable,
% and the velocity error of the HDVs are measurable.
- Process parameters
Click here to view code
n_ctr = 2*n_vehicle; % number of state variables
m_ctr = n_cav; % number of input variables
switch measure_type
case 1
p_ctr = n_vehicle; % number of output variables
case 2
p_ctr = 2*n_vehicle;
case 3
p_ctr = n_vehicle + n_cav;
end
Q_v = weight_v*eye(n_vehicle); % penalty for velocity error
Q_s = weight_s*eye(p_ctr-n_vehicle); % penalty for spacing error
Q = blkdiag(Q_v,Q_s); % penalty for trajectory error
R = weight_u*eye(m_ctr); % penalty for control input
u = zeros(m_ctr,total_time_step); % control input
x = zeros(n_ctr,total_time_step); % state variables
y = zeros(p_ctr,total_time_step); % output variables
pr_status = zeros(total_time_step,1); % problem status
e = zeros(1,total_time_step); % external input
- Pre-collected data
Click here to view code
Simulation
Click here to view code
% Mixed traffic states
% S(time,vehicle id,state variable), in state variable: 1. position; 2. velocity; 3. acceleration
S = zeros(total_time_step,n_vehicle+1,3);
S(1,1,1) = 0;
for i = 2 : n_vehicle+1
S(1,i,1) = S(1,i-1,1) - hdv_parameter.s_star(i-1);
end
S(1,:,2) = v_star * ones(n_vehicle+1,1);
% reference trajectory is all zeros: stabilize the system to equilibrium
r = zeros(p_ctr,total_time_step+N);
Experiment starts here - Initialization: all the vehicles use the HDV model
Click here to view code
for k = 1:initialization_time/Tstep-1
% calculate acceleration
acel = HDV_dynamics(S(k,:,:),hdv_parameter) ...
-acel_noise + 2*acel_noise*rand(n_vehicle,1);
S(k,1,3) = 0; % the head vehicle
S(k,2:end,3) = acel; % all the vehicles use HDV model
% update traffic states
S(k+1,:,2) = S(k,:,2) + Tstep*S(k,:,3);
S(k+1,1,2) = head_vehicle_trajectory.vel(1); % the velocity of the head vehicle
S(k+1,:,1) = S(k,:,1) + Tstep*S(k,:,2);
% update equilibrium velocity
v_star = head_vehicle_trajectory.vel(1);
% update states in DeeP-LCC
y(:,k) = measure_mixed_traffic(S(k,2:end,2),S(k,:,1),ID,v_star,s_star,measure_type);
e(k) = S(k,1,2) - v_star;
u(:,k) = S(k,pos_cav+1,3);
end
% update past data in control process
uini = u(:,k-Tini+1:k);
yini = y(:,k-Tini+1:k);
eini = S(k-Tini+1:k,1,2) - v_star;
- The CAVs start to use DeeP-LCC
Click here to view code
for k = initialization_time/Tstep:total_time_step-1
% calculate acceleration for the HDVs
acel = HDV_dynamics(S(k,:,:),hdv_parameter) ...
-acel_noise + 2*acel_noise*rand(n_vehicle,1);
S(k,2:end,3) = acel;
if mix
switch controller_type
case 1 % calculate control input via DeeP-LCC
if constraint_bool
[u_opt,y_opt,pr] = qp_DeeP_LCC(Up,Yp,Uf,Yf,Ep,Ef,uini,yini,eini,Q,R,r(:,k:k+N-1),...
lambda_g,lambda_y,u_limit,s_limit);
else
[u_opt,y_opt,pr] = qp_DeeP_LCC(Up,Yp,Uf,Yf,Ep,Ef,uini,yini,eini,Q,R,r(:,k:k+N-1),...
lambda_g,lambda_y);
end
case 2 % calculate control input via MPC
if constraint_bool
[u_opt,y_opt,pr] = qp_MPC(ID,Tstep,hdv_type,measure_type,v_star,uini,yini,N,Q,R,r(:,k:k+N-1),u_limit,s_limit);
else
[u_opt,y_opt,pr] = qp_MPC(ID,Tstep,hdv_type,measure_type,v_star,uini,yini,N,Q,R,r(:,k:k+N-1));
end
end
% one-step implementation in receding horizon manner
u(:,k) = u_opt(1:m_ctr,1);
% update accleration for the CAV
S(k,pos_cav+1,3) = u(:,k);
% judge whether AEB (automatic emergency braking, which is implemented in the function of 'HDV_dynamics') commands to brake
brake_vehicle_ID = find(acel==dcel_max); % the vehicles that need to brake
brake_cav_ID = intersect(brake_vehicle_ID,pos_cav); % the CAVs that need to brake
if ~isempty(brake_cav_ID)
S(k,brake_cav_ID+1,3) = dcel_max;
end
% record problem status
pr_status(k) = pr;
end
% update traffic states
S(k+1,:,2) = S(k,:,2) + Tstep*S(k,:,3);
% trajectory for the head vehicle
% before adaption_time, the head vehicle maintains its velocity and the CAVs first stabilize the traffic system
if k*Tstep < initialization_time + adaption_time
S(k+1,1,2) = head_vehicle_trajectory.vel(1);
else
S(k+1,1,2) = head_vehicle_trajectory.vel(k-(initialization_time+adaption_time)/Tstep+1);
end
S(k+1,:,1) = S(k,:,1) + Tstep*S(k,:,2);
% update equilibrium setup for the CAVs
v_star = mean(S(k-Tini+1:k,1,2)); % average velocity of the head vehicle among the past Tini time
s_star = acos(1-v_star/30*2)/pi*(35-5) + 5; % use the OVM-type spacing policy to calculate the equilibrium spacing of the CAVs
% update past data in control process
uini = u(:,k-Tini+1:k);
% the output needs to be re-calculated since the equilibrium has been updated
for k_past = k-Tini+1:k
y(:,k_past) = measure_mixed_traffic(S(k_past,2:end,2),S(k_past,:,1),ID,v_star,s_star,measure_type);
e(k_past) = S(k_past,1,2) - v_star;
end
yini = y(:,k-Tini+1:k);
eini = S(k-Tini+1:k,1,2) - v_star;
fprintf('Current simulation time: %.2f seconds (%.2f%%) \n',k*Tstep,(k*Tstep-initialization_time)/(total_time-initialization_time)*100);
end
k_end = k+1;
y(:,k_end) = measure_mixed_traffic(S(k_end,2:end,2),S(k_end,:,1),ID,v_star,s_star,measure_type);
tsim = toc;
fprintf('Simulation ends at %6.4f seconds \n', tsim);
Results output
Click here to view code
if mix
switch controller_type
case 1
save(['..\_data\simulation_data\DeeP_LCC\nedc_simulation\simulation_data',data_str,'_',num2str(i_data),'_modified_v',num2str(trajectory_id),'_noiseLevel_',num2str(acel_noise),...
'_hdvType_',num2str(hdv_type),'_lambdaG_',num2str(lambda_g),'_lambdaY_',num2str(lambda_y),'.mat'],...
'hdv_type','acel_noise','S','T','Tini','N','ID','Tstep','v_star');
case 2
save(['..\_data\simulation_data\MPC\nedc_simulation\simulation_data',data_str,'_',num2str(i_data),'_modified_v',num2str(trajectory_id),'_noiseLevel_',num2str(acel_noise),...
'_hdvType_',num2str(hdv_type),'.mat'],...
'hdv_type','acel_noise','S','T','Tini','N','ID','Tstep','v_star');
end
else
save(['..\_data\simulation_data\HDV\nedc_simulation\simulation_data',data_str,'_',num2str(i_data),'_modified_v',num2str(trajectory_id),'_noiseLevel_',num2str(acel_noise),...
'_hdvType_',num2str(hdv_type),'.mat'],...
'hdv_type','acel_noise','S','T','Tini','N','ID','Tstep','v_star');
end
Functions¶
HDV_dynamics.m¶
This function takes S and parameter as input to calculate the acceleration of HDVs.
- S: state of all the vehicles
- type: type of the HDV car-following model
- parameter: Parameter value in the car-following model
- acel: acceleration of HDVs
Click here to view code
function [acel] = HDV_dynamics(S,parameter)
num_vehicle = size(S,2)-1;
acel_max = 2;
dcel_max = -5;
switch parameter.type
case 1
V_diff = S(1,1:(end-1),2) - S(1,2:end,2);
D_diff = S(1,1:(end-1),1) - S(1,2:end,1);
cal_D = D_diff'; % For the boundary of Optimal Veloicity Calculation
for i = 1:num_vehicle
if cal_D(i) > parameter.s_go(i)
cal_D(i) = parameter.s_go(i);
elseif cal_D(i) < parameter.s_st
cal_D(i) = parameter.s_st;
end
end
% nonlinear OVM Model
% V_d = v_max/2*(1-cos(pi*(s-s_st)/(s_go-s_st)));
% a = alpha*(V_d-v2)+beta*(v1-v2);
acel = parameter.alpha.*(parameter.v_max/2.*(1-cos(pi*(cal_D-parameter.s_st)./(parameter.s_go-parameter.s_st))) ...
- S(1,2:end,2)') + parameter.beta.*V_diff';
% % acceleration saturation
acel(acel>acel_max) = acel_max;
acel(acel<dcel_max) = dcel_max;
%
% SD as ADAS to prevent crash
acel_sd = (S(1,2:end,2).^2-S(1,1:(end-1),2).^2)./2./D_diff;
acel(acel_sd>abs(dcel_max)) = dcel_max;
case 2
% Driver Model: IDM
v_max = 30;
T_gap = 1;
a = 1;
b = 1.5;
delta = 4;
s_st = 5;
V_diff = S(1,1:(end-1),2) - S(1,2:end,2);
D_diff = S(1,1:(end-1),1) - S(1,2:end,1);
acel = a.*(1- (S(1,2:end,2)/v_max).^delta -...
((s_st+T_gap.*S(1,2:end,2)-V_diff.*S(1,2:end,2)/2./sqrt(a)./sqrt(b))./D_diff).^2);
acel = acel';
% % acceleration saturation
% acel(acel>acel_max) = acel_max;
% acel(acel<dcel_max) = dcel_max;
%
% SD as ADAS to prevent crash
acel_sd = (S(1,2:end,2).^2-S(1,1:(end-1),2).^2)./2./D_diff;
acel(acel_sd>abs(dcel_max)) = dcel_max;
end
end
hankel_matrix.m¶
This function is used for generating a Hankel matrix of order L.
Click here to view code
measure_mixed_traffic.m¶
This function take the a series of parameters setting as input to measure the output in mixed traffic flow.
- vel: velocity of each vehicle
- pos: position of each vehicle
- ID: ID of vehicle types (1 for CAV, 0 for HDV)
- v_star: equilibrium velocity
- s_star: equilibrium spacing
- type:
(1) Only the velocity errors of all the vehicles are measurable;
(2) All the states, including velocity error and spacing error are measurable;
(3) Velocity error and spacing error of the CAVs are measurable,and the velocity error of the HDVs are measurable.
Click here to view code
function [y] = measure_mixed_traffic(vel,pos,ID,v_star,s_star,type)
pos_cav = find(ID==1); % position of CAVs
switch type
case 1
y = (vel - v_star)';
case 2
spacing = pos(1:end-1) - pos(2:end);
y = [(vel - v_star)';
(spacing - s_star)'];
case 3
spacing = pos(1:end-1) - pos(2:end);
y = [(vel - v_star)';
(spacing(pos_cav) - s_star)'];
end
end
qp_DeeP_LCC.m¶
Input:
- Up & Uf: Hankel matrix of pre-collected input data
- Yp & Yf: Hankel matrix of pre-collected output data
- Ep & Ef: Hankel matrix of pre-collected external input data
- uini & yini & eini: past data of length Tini in control process
- Q & R: weight coefficient matrix in performance cost
- r: reference trajectory
- lambda_g & lambda_y: coefficient in regulation for nonlinearty and uncertainty
- u_limit & s_limit: bound on control input & spacing
Output:
- u_opt: designed optimal future control input
- y_opt: predicted output in the optimal future control input
- problem_status: problem status in optimization calculation
Optimization formulation:
minimize
subject to: where e = 0, u in u_limit, [0 I_m]y in s_limit
We transform the problem into standard quadratic programming for calculation.
Click here to view code
function [u_opt,y_opt,problem_status] = qp_DeeP_LCC(Up,Yp,Uf,Yf,Ep,Ef,...
uini,yini,eini,Q,R,r,lambda_g,lambda_y,u_limit,s_limit)
% whether there exists input/output constraints
if nargin < 15
constraint_bool = 0;
else
constraint_bool = 1;
end
% ------------
% parameters
% ------------
m = size(uini,1); % dimension of control input
p = size(yini,1); % dimension of output
Tini = size(Up,1)/m; % horizon of past data
N = size(Uf,1)/m; % horizon of future data
T = size(Up,2) + Tini + N - 1; % time length of pre-collected data
% reshape past data into one single trajectory
% uini = col(u(-Tini),u(-Tini+1),...,u(-1)) (similarly for yini and eini)
uini_col = reshape(uini,[m*Tini,1]);
yini_col = reshape(yini,[p*Tini,1]);
eini_col = reshape(eini,[Tini,1]);
r_col = reshape(r,[p*N,1]);
Q_blk = zeros(p*N);
R_blk = zeros(m*N);
for i = 1:N
Q_blk((i-1)*p+1:i*p,(i-1)*p+1:i*p) = Q;
R_blk((i-1)*m+1:i*m,(i-1)*m+1:i*m) = R;
end
% ---------------------
% Standard QP in MATLAB
% [x,fval,exitflag,output,lambda]=quadprog(H,f,A,b,B,c,l,u,x0,options)
% minimize 0.5*x'*H*x+f'*x
% subject to A*x <= b
% B*x = c
% l <= x <= u
% ---------------------
% Coefficient
H = Yf'*Q_blk*Yf + Uf'*R_blk*Uf + lambda_g*eye(T-Tini-N+1) + lambda_y*Yp'*Yp;
f = -lambda_y*Yp'*yini_col;
B = [Up;Ep;Ef];
c = [uini_col;eini_col;zeros(N,1)];
if constraint_bool % there exists input/output constraints
Sf = [zeros(m,p-m),eye(m)];
Sf_blk = Sf;
for i = 2:N
Sf_blk = blkdiag(Sf_blk,Sf);
end
A = [Uf;-Uf;Sf_blk*Yf;-Sf_blk*Yf];
b = [max(u_limit)*ones(m*N,1);-min(u_limit)*ones(m*N,1);...
max(s_limit)*ones(m*N,1);-min(s_limit)*ones(m*N,1)];
else
A = [];
b = [];
end
options = optimoptions('quadprog');
options = optimoptions(options,'MaxIterations',1e4);
% options = optimoptions('quadprog','MaxIterations',1e4);
% Optimization
[g_opt,fval,exitflag,output,lambda] = quadprog(H,f,A,b,B,c,[],[],[],options);
% Solution
u_opt = Uf*g_opt;
y_opt = Yf*g_opt;
problem_status = exitflag;
% % For infeasible cases
% if exitflag ~= 1
% u_opt = previous_u_opt;
% end
end
qp_MPC.m¶
Input:
- ID: vehicle ID
- Ts: sampling time
- hdv_type: type of HDV car-following model
- measure_type: measure type for output definition
- v_star: equilibrium velocity
- uini & yini: past data of length Tini in control process
- N: future time length (predicted horizon)
- Q & R: weight coefficient matrix in performance cost
- r: reference trajectory
- u_limit & s_limit: bound on control input & spacing
- previous_u_opt: control input in previous time step
Output:
- u_opt: designed optimal future control input
- y_opt: predicted output in the optimal future control input
- problem_status: problem status in optimization calculation
Optimization Formulation:
mininize
subject to:
- xini is estimated from past data uini,yini
- x = Ax + Bu
- y = Cx
We transform the problem into standard quadratic programming for calculation
Click here to view code
function [u_opt,y_opt,problem_status] = qp_MPC(ID,Ts,hdv_type,measure_type,...
v_star,uini,yini,N,Q,R,r,u_limit,s_limit,previous_u_opt)
% -------------------------------------------------------------------------
% Calculate via Linear model
% -------------------------------------------------------------------------
if nargin < 12 % whether there exists input/output constraints
constraint_bool = 0;
else
constraint_bool = 1;
end
% linear model
[A,B,C] = traffic_linear_model(ID,Ts,hdv_type,measure_type,v_star);
% dimension
m = size(uini,1); % dimension of control input
p = size(yini,1); % dimension of output
n = size(A,1); % dimension of state
Tini = size(uini,2); % horizon of past data
% reshape past data into one single trajectory
% uini = col(u(-Tini),u(-Tini+1),...,u(-1)) (similarly for yini and eini)
uini_col = reshape(uini,[m*Tini,1]);
yini_col = reshape(yini,[p*Tini,1]);
Obsv_Tini = zeros(p*Tini,n);
for i = 1:Tini
Obsv_Tini((i-1)*p+1:i*p,:) = C*A^(i-1);
end
Toep_Tini = zeros(p*Tini,m*Tini);
Toep_Tini(1*p+1:2*p,0*m+1:1*m) = C*B;
for i = 3:Tini
for j = 1:i-1
Toep_Tini((i-1)*p+1:i*p,(j-1)*m+1:j*m) = C*A^(i-1-j)*B;
end
end
Obsv_N = zeros(p*N,n);
for i = 1:N
Obsv_N((i-1)*p+1:i*p,:) = C*A^(i-1);
end
Toep_N = zeros(p*N,m*N);
Toep_N(1*p+1:2*p,0*m+1:1*m) = C*B;
for i = 3:N
for j = 1:i-1
Toep_N((i-1)*p+1:i*p,(j-1)*m+1:j*m) = C*A^(i-1-j)*B;
end
end
Ctrb_Tini = zeros(n,m*Tini);
for i = 1:Tini
Ctrb_Tini(:,(i-1)*m+1:i*m) = A^(Tini-i)*B;
end
x_1 = pinv(Obsv_Tini)*(yini_col-Toep_Tini*uini_col);
x_Tini_plus1 = A^Tini*x_1 + Ctrb_Tini*uini_col;
r_col = reshape(r,[p*N,1]);
Q_blk = zeros(p*N);
R_blk = zeros(m*N);
for i = 1:N
Q_blk((i-1)*p+1:i*p,(i-1)*p+1:i*p) = Q;
R_blk((i-1)*m+1:i*m,(i-1)*m+1:i*m) = R;
end
Cu = Obsv_N*(Ctrb_Tini-A^Tini*pinv(Obsv_Tini)*Toep_Tini)*uini_col + Obsv_N*A^Tini*pinv(Obsv_Tini)*yini_col;
% ---------------------
% Standard QP in MATLAB
% [x,fval,exitflag,output,lambda]=quadprog(H,f,A,b,B,c,l,u,x0,options)
% minimize 0.5*x'*H*x+f'*x
% subject to A*x <= b
% B*x = c
% l <= x <= u
% ---------------------
% Coefficient
H = Toep_N'*Q_blk*Toep_N+R_blk;
f = Toep_N'*Q_blk*Obsv_N*x_Tini_plus1;
if constraint_bool % there exists input/output constraints
Sf = [zeros(m,p-m),eye(m)];
Sf_blk = Sf;
for i = 2:N
Sf_blk = blkdiag(Sf_blk,Sf);
end
A = [eye(m*N);-eye(m*N);...
Sf_blk*Toep_N;-Sf_blk*Toep_N];
b = [max(u_limit)*ones(m*N,1);-min(u_limit)*ones(m*N,1);...
max(s_limit)*ones(m*N,1)-Sf_blk*Cu;-min(s_limit)*ones(m*N,1)+Sf_blk*Cu];
else
A = [];
b = [];
end
% Optimization
[u_opt,fval,exitflag,output,lambda] = quadprog(H,f,A,b,[],[]);
% For infeasible cases
if exitflag ~= 1
u_opt = previous_u_opt;
end
% Solution
y_opt = Obsv_N*x_Tini_plus1+Toep_N*u_opt;
problem_status = exitflag;
end
traffic_linear_model.m¶
This function is used for CLCC model.
- ID: vehicle ID
- Ts: sampling time
- hdv_type: type of HDV car-following model
- measure_type: measure type
Click here to view code
function [pos_cav, n_vehicle, n_cav, A, Ad,Bd,Cd] = traffic_linear_model(ID,Ts,hdv_type,measure_type,v_star)
switch hdv_type
case 1
% Driver Model: OVM
alpha = 0.6;
beta = 0.9;
s_st = 5;
s_go = 35;
v_max = 30;
% Equilibrium spacing
s_star = acos(1-v_star/v_max*2)/pi*(s_go-s_st)+s_st;
% Linear coefficients
alpha1 = alpha*v_max/2*pi/(s_go-s_st)*sin(pi*(s_star-s_st)/(s_go-s_st));
alpha2 = alpha+beta;
alpha3 = beta;
case 2
% Driver Model: IDM
v_max = 30;
T_gap = 1;
a = 1;
b = 1.5;
delta = 4;
s_st = 5;
% Equilibrium spacing
s_star = (s_st+T_gap*v_star)/sqrt(1-(v_star/v_max)^delta);
% Linear coefficients
alpha1 = 2*a*(s_st+T_gap*v_star)^2/s_star^3;
alpha2 = sqrt(a/b)*v_star*(s_st+T_gap*v_star)/s_star^2+2*a*(2*v_star^3/v_max^4+T_gap*(s_st+T_gap*v_star)/s_star^2);
alpha3 = sqrt(a/b)*v_star*(s_st+T_gap*v_star)/s_star^2;
end
pos_cav = find(ID==1); % position of CAVs
n_vehicle = length(ID); % number of vehicles
n_cav = length(pos_cav); % number of CAVs
A = zeros(n_vehicle*2);
P1 = [0,-1;alpha1,-alpha2];
P2 = [0,1;0,alpha3];
S1 = [0,-1;0,0];
S2 = [0,1;0,0];
A(1:2,1:2) = P1;
for i = 2:n_vehicle
if ID(i) == 0
A(2*i-1:2*i,2*i-1:2*i) = P1;
A(2*i-1:2*i,2*i-3:2*i-2) = P2;
else
A(2*i-1:2*i,2*i-1:2*i) = S1;
A(2*i-1:2*i,2*i-3:2*i-2) = S2;
end
end
B = zeros(2*n_vehicle,n_cav);
for i = 1:n_cav
B(pos_cav(i)*2,i) = 1;
end
switch measure_type
case 1
C = zeros(n_vehicle,2*n_vehicle);
for i = 1:n_vehicle
C(i,2*i) = 1;
end
case 2
C = zeros(2*n_vehicle);
for i = 1:n_vehicle
C(i,2*i) = 1;
C(n_vehicle+i,2*i-1) = 1;
end
case 3
C = zeros(n_vehicle+n_cav,2*n_vehicle);
for i = 1:n_vehicle
C(i,2*i) = 1;
end
for i = 1:n_cav
C(n_vehicle+i,2*pos_cav(i)-1) = 1;
end
end
Ad = Ts*A + eye(2*n_vehicle);
Bd = B*Ts;
Cd = C;
end
Data Generation¶
data_hdvParameter.m¶
This function is used for generating heterogeneous HDV paramters.
Click here to view code
hdv_type = 1;
v_star = 15;
% -------------------
% ID = [0,0,1,0,0,1,0,0]; % ID of vehicle types
% 1: CAV 0: HDV
% -------------------
% Homegeneous setup for OVM
alpha = 0.6*ones(8,1);
beta = 0.9*ones(8,1);
s_go = 25*ones(8,1);
s_st = 5;
v_max = 30;
% Equilibrium spacing
s_star = acos(1-v_star/v_max*2)./pi*(s_go-s_st) + s_st;
hdv_parameter = struct('type',hdv_type,...
'alpha',alpha,'beta',beta,'s_st',s_st,'s_go',s_go,'v_max',v_max,'s_star',s_star);
save('_data/hdv_ovm_homogeneous.mat','hdv_parameter');
switch hdv_type
case 1
% Driver Model: OVM
alpha = 0.4 + 0.4*rand(8,1);
beta = 0.7 + 0.4*rand(8,1);
s_go = 30 + 10*rand(8,1);
s_st = 5;
v_max = 30;
% Manual set for parameters
alpha = [0.45;0.75;0.60;0.70;0.50;0.60;0.40;0.80];
beta = [0.60;0.95;0.90;0.95;0.75;0.90;0.80;1.00];
s_go = [ 38 ; 31 ; 35 ; 33 ; 37 ; 35 ; 39 ; 34 ];
% Consider nominal parameter for the CAV position, which only works
% in comparison for all the vehicles are HDVs
alpha(3) = 0.6;
alpha(6) = 0.6;
beta(3) = 0.9;
beta(6) = 0.9;
s_go(3) = 35;
s_go(6) = 35;
% Equilibrium spacing
s_star = acos(1-v_star/v_max*2)./pi*(s_go-s_st) + s_st;
case 2
% Driver Model: IDM
v_max = 30;
T_gap = ones(8,1);
a = 1;
b = 1.5;
delta = 4;
s_st = 5;
% Equilibrium spacing
s_star = (s_st+T_gap.*v_star)./sqrt(1-(v_star/v_max)^delta);
end
switch hdv_type
case 1
hdv_parameter = struct('type',hdv_type,...
'alpha',alpha,'beta',beta,'s_st',s_st,'s_go',s_go,'v_max',v_max,'s_star',s_star);
save('_data/hdv_ovm.mat','hdv_parameter');
case 2
hdv_parameter = struct('type',hdv_type,...
'v_max',v_max,'T_gap',T_gap,'a',a,'b',b,'delta',delta,'s_st',s_st,'s_star',s_star);
save('_data/hdv_idm.mat','hdv_parameter');
end
data_trajectoryDataCollection.m¶
This function is used for data collection for random times.
Click here to view code
data_total_number = 100;
h_wait = waitbar(0,'please wait');
for i_data = 1:data_total_number
% -------------------------------------------------------------------------
% Parameter setup
% -------------------------------------------------------------------------
% Type for HDV car-following model
hdv_type = 1; % 1. OVM 2. IDM
% Uncertainty for HDV behavior
acel_noise = 0.1; % A white noise signal on HDV's original acceleration
% Data set
data_str = '3'; % 1. random ovm 2. manual ovm 3. homogeneous ovm
% Parameters in Simulation
total_time = 40; % Total Simulation Time
Tstep = 0.05; % Time Step
total_time_step = total_time/Tstep;
% DeePC Formulation
T = 2000; % length of data samples
Tini = 20; % length of past data
N = 50; % length of predicted horizon
weight_v = 1; % weight coefficient for velocity error
weight_s = 0.5; % weight coefficient for spacing error
weight_u = 0.1; % weight coefficient for control input
lambda_g = 1; % penalty on ||g||_2^2 in objective
lambda_y = 1e3; % penalty on ||sigma_y||_2^2 in objective
% System Dynamics
% vel_noise = 0.1; % noise signal in velocity signal
% ------------------------------------------
% Parameters in Mixed Traffic
% ------------------------------------------
ID = [0,0,1,0,0,1,0,0]; % ID of vehicle types
% 1: CAV 0: HDV
pos_cav = find(ID==1); % position of CAVs
n_vehicle = length(ID); % number of vehicles
n_cav = length(pos_cav); % number of CAVs
n_hdv = n_vehicle-n_cav; % number of HDVs
mix = 1; % whether mixed traffic flow
v_star = 15; % Equilibrium velocity
s_star = 20; % Equilibrium spacing for CAV
switch hdv_type
case 1
% Driver Model: OVM
load(['_data/hdv_ovm_',num2str(data_str),'.mat']);
case 2
% Driver Model: IDM
load('_data/hdv_idm.mat');
% v_max = 30;
% T_gap = 1;
% a = 1;
% b = 1.5;
% delta = 4;
% s_st = 5;
% % Equilibrium spacing
% s_star = (s_st+T_gap*v_star)/sqrt(1-(v_star/v_max)^delta);
end
acel_max = 2;
dcel_max = -5;
% What is measurable
% for measure_type = 2:3
measure_type = 3;
% 1. Only the velocity errors of all the vehicles are measurable;
% 2. All the states, including velocity error and spacing error are measurable;
% 3. Velocity error and spacing error of the CAVs are measurable,
% and the velocity error of the HDVs are measurable.
% ------------------
% size in DeePC
% ------------------
n_ctr = 2*n_vehicle; % number of state variables
m_ctr = n_cav; % number of input variables
switch measure_type % number of output variables
case 1
p_ctr = n_vehicle;
case 2
p_ctr = 2*n_vehicle;
case 3
p_ctr = n_vehicle + n_cav;
end
% -------------------------------------------------------------------------
% Scenario initialization
%--------------------------------------------------------------------------
% There is one head vehicle at the very beginning
S = zeros(total_time_step,n_vehicle+1,3);
S(1,1,1) = 0;
for i = 2 : n_vehicle+1
S(1,i,1) = S(1,i-1,1) - hdv_parameter.s_star(i-1);
end
S(1,:,2) = v_star * ones(n_vehicle+1,1);
% -------------------------------------------------------------------------
% Data collection
%--------------------------------------------------------------------------
% ------------------
% persistently exciting input data
% ------------------
ud = -1+2*rand(m_ctr,T);
ed = -1+2*rand(1,T);
yd = zeros(p_ctr,T);
% ------------------
% generate output data
% ------------------
for k = 1:T-1
% Update acceleration
acel = HDV_dynamics(S(k,:,:),hdv_parameter) ...
-acel_noise + 2*acel_noise*rand(n_vehicle,1);
S(k,1,3) = 0; % the head vehicle
S(k,2:end,3) = acel; % all the vehicles using HDV model
S(k,pos_cav+1,3) = ud(:,k); % the CAVs
S(k+1,:,2) = S(k,:,2) + Tstep*S(k,:,3);
S(k+1,1,2) = ed(k) + v_star; % the velocity of the head vehicle
S(k+1,:,1) = S(k,:,1) + Tstep*S(k,:,2);
yd(:,k) = measure_mixed_traffic(S(k,2:end,2),S(k,:,1),ID,v_star,s_star,measure_type);
end
k = k+1;
yd(:,k) = measure_mixed_traffic(S(k,2:end,2),S(k,:,1),ID,v_star,s_star,measure_type);
% ------------------
% organize past data and future data
% ------------------
U = hankel_matrix(ud,Tini+N);
Up = U(1:Tini*m_ctr,:);
Uf = U((Tini*m_ctr+1):end,:);
E = hankel_matrix(ed,Tini+N);
Ep = E(1:Tini,:);
Ef = E((Tini+1):end,:);
Y = hankel_matrix(yd,Tini+N);
Yp = Y(1:Tini*p_ctr,:);
Yf = Y((Tini*p_ctr+1):end,:);
str=['Processing...',num2str(i_data/data_total_number*100),'%'];
waitbar(i_data/data_total_number,h_wait,str);
save(['_data\trajectory_data_collection\data',num2str(data_str),'_',num2str(i_data),'_noiseLevel_',num2str(acel_noise),'.mat'],...
'hdv_type','acel_noise','Up','Yp','Uf','Yf','Ep','Ef','T','Tini','N','ID','Tstep','v_star');
end
close(h_wait);
data_trajectoryDataCollection_HDVTrajectoryProcess.m¶
This function is used for data collection for random times. It serves the same purpose as data_trajectoryDataCollection.m. It was created for testing purpose. It has no impact if not used at all.
Click here to view code
% Data set
data_str = '2'; % 1. random ovm 2. manual ovm 3. homogeneous ovm
% Mix or not
mix = 1; % 0. all HDVs; 1. mix
% Type of the controller
controller_type = 2; % 1. DeePC 2. MPC 3.SPC 4. SPC without Regulation
% Type for HDV car-following model
hdv_type = 1; % 1. OVM 2. IDM
% Uncertainty for HDV behavior
acel_noise = 0.1; % A white noise signal on HDV's original acceleration
% Perturbation amplitude
per_type = 2; % 1. sinuoid perturbation 2. brake perturbation 3. ngsim simulation
per_amp = 5;
% Whether there exists constraints
constraint_bool = 1;
i_data = 1; % Data set number
if per_type ~= 3
if constraint_bool
load(['_data\simulation_data\HDV\constrained_simulation\simulation_data',data_str,'_',num2str(i_data),'_perType_',num2str(per_type),'_noiseLevel_',num2str(acel_noise),...
'_hdvType_',num2str(hdv_type),'.mat']);
controller_str = 'DeePC';
else
load(['_data\simulation_data\HDV\simulation_data',data_str,'_',num2str(i_data),'_perType_',num2str(per_type),'_noiseLevel_',num2str(acel_noise),...
'_hdvType_',num2str(hdv_type),'_lambdaG_',num2str(lambda_g),'_lambdaY_',num2str(lambda_y),'.mat']);
controller_str = 'DeePC';
end
else % ngsim simulation
load(['_data\simulation_data\HDV\simulation_data',data_str,'_',num2str(i_data),'_perType_',num2str(per_type),'_noiseLevel_',num2str(acel_noise),...
'_hdvType_',num2str(hdv_type),'.mat']);
controller_str = 'HDV';
end
% -------------------------------------------------------------------------
% Parameter setup
% -------------------------------------------------------------------------
% Parameters in Simulation
total_time = 40; % Total Simulation Time
Tstep = 0.05; % Time Step
total_time_step = total_time/Tstep;
% DeePC Formulation
T = 2000; % length of data samples
Tini = 20; % length of past data
N = 50; % length of predicted horizon
weight_v = 1; % weight coefficient for velocity error
weight_s = 0.5; % weight coefficient for spacing error
weight_u = 0.1; % weight coefficient for control input
lambda_g = 1; % penalty on ||g||_2^2 in objective
lambda_y = 1e3; % penalty on ||sigma_y||_2^2 in objective
% System Dynamics
% vel_noise = 0.1; % noise signal in velocity signal
% ------------------------------------------
% Parameters in Mixed Traffic
% ------------------------------------------
ID = [0,0,1,0,0,1,0,0]; % ID of vehicle types
% 1: CAV 0: HDV
pos_cav = find(ID==1); % position of CAVs
n_vehicle = length(ID); % number of vehicles
n_cav = length(pos_cav); % number of CAVs
n_hdv = n_vehicle-n_cav; % number of HDVs
mix = 1; % whether mixed traffic flow
v_star = 15; % Equilibrium velocity
s_star = 20; % Equilibrium spacing for CAV
acel_max = 2;
dcel_max = -5;
% What is measurable
% for measure_type = 2:3
measure_type = 3;
% 1. Only the velocity errors of all the vehicles are measurable;
% 2. All the states, including velocity error and spacing error are measurable;
% 3. Velocity error and spacing error of the CAVs are measurable,
% and the velocity error of the HDVs are measurable.
% ------------------
% size in DeePC
% ------------------
n_ctr = 2*n_vehicle; % number of state variables
m_ctr = n_cav; % number of input variables
switch measure_type % number of output variables
case 1
p_ctr = n_vehicle;
case 2
p_ctr = 2*n_vehicle;
case 3
p_ctr = n_vehicle + n_cav;
end
% -------------------------------------------------------------------------
% Scenario initialization
%--------------------------------------------------------------------------
% There is one head vehicle at the very beginning
S = zeros(total_time_step,n_vehicle+1,3);
S(1,1,1) = 0;
for i = 2 : n_vehicle+1
% S(1,i,1) = S(1,i-1,1) - hdv_parameter.s_star(i-1);
% S(1,i,1) = S(1,i-1,1) - s_star(i-1);
S(1,i,1) = S(1,i-1,1) - s_star;
end
S(1,:,2) = v_star * ones(n_vehicle+1,1);
% -------------------------------------------------------------------------
% Data collection
%--------------------------------------------------------------------------
% ------------------
% persistently exciting input data
% ------------------
ud = -0.1+0.1*rand(m_ctr,T);
ed = -1+2*rand(1,T);
yd = zeros(p_ctr,T);
% ------------------
% generate output data
% ------------------
for k = 1:T-1
% Update acceleration
acel = HDV_dynamics(S(k,:,:),hdv_parameter) ...
-acel_noise + 2*acel_noise*rand(n_vehicle,1);
S(k,1,3) = 0; % the head vehicle
S(k,2:end,3) = acel; % all the vehicles using HDV model
S(k,pos_cav+1,3) = ud(:,k); % the CAVs
S(k+1,:,2) = S(k,:,2) + Tstep*S(k,:,3);
S(k+1,1,2) = ed(k) + v_star; % the velocity of the head vehicle
S(k+1,:,1) = S(k,:,1) + Tstep*S(k,:,2);
yd(:,k) = measure_mixed_traffic(S(k,2:end,2),S(k,:,1),ID,v_star,s_star,measure_type);
end
k = k+1;
yd(:,k) = measure_mixed_traffic(S(k,2:end,2),S(k,:,1),ID,v_star,s_star,measure_type);
% ------------------
% organize past data and future data
% ------------------
U = hankel_matrix(ud,Tini+N);
Up = U(1:Tini*m_ctr,:);
Uf = U((Tini*m_ctr+1):end,:);
E = hankel_matrix(ed,Tini+N);
Ep = E(1:Tini,:);
Ef = E((Tini+1):end,:);
Y = hankel_matrix(yd,Tini+N);
Yp = Y(1:Tini*p_ctr,:);
Yf = Y((Tini*p_ctr+1):end,:);
str=['Processing...',num2str(i_data/data_total_number*100),'%'];
waitbar(i_data/data_total_number,h_wait,str);
save(['_data\trajectory_data_collection\data',num2str(data_str),'_',num2str(i_data),'_noiseLevel_',num2str(acel_noise),'.mat'],...
'hdv_type','acel_noise','Up','Yp','Uf','Yf','Ep','Ef','T','Tini','N','ID','Tstep','v_star');
%end
close(h_wait);
nedc_trajectory.m¶
Defining velocity of the head vehicle based on nedc.
Click here to view code
Tstep = 0.05;
total_time = 50+8+69+13+50+35+30+20+10+14+20;
time = 1:Tstep:total_time;
vel = zeros(total_time/Tstep,1);
cruise_time_i = 1;
change_time_i = 1;
for i = 1:total_time/Tstep
if i*Tstep <= 50
vel(i) = 70;
elseif i*Tstep <= 50+8
vel(i) = 70 - 20/8*(i*Tstep-50);
elseif i*Tstep <= 50+8+69
vel(i) = 50;
elseif i*Tstep <= 50+8+69+13
vel(i) = 50 + 20/13*(i*Tstep-(50+8+69));
elseif i*Tstep <= 50+8+69+13+50
vel(i) = 70;
elseif i*Tstep <= 50+8+69+13+50+35
vel(i) = 70 + 30/35*(i*Tstep-(50+8+69+13+50));
elseif i*Tstep <= 50+8+69+13+50+35+30
vel(i) = 100;
elseif i*Tstep <= 50+8+69+13+50+35+30+20
vel(i) = 100 + 20/20*(i*Tstep-(50+8+69+13+50+35+30));
elseif i*Tstep <= 50+8+69+13+50+35+30+20+10
vel(i) = 120;
elseif i*Tstep <= 50+8+69+13+50+35+30+20+10+14
vel(i) = 120 - 50/14*(i*Tstep-(50+8+69+13+50+35+30+20+10));
elseif i*Tstep <= 50+8+69+13+50+35+30+20+10+14+20
vel(i) = 70;
end
end
save('nedc.mat','time','vel');
nedc_modified_trajectory.m¶
It serves the same purpose as nedc_trajectory.m. It was created for testing purpose and has no impact if not used at all.
Click here to view code
Tstep = 0.05;
total_time = 10+8+20+13+20+25+20+10+20;
time = 1:Tstep:total_time;
vel = zeros(total_time/Tstep,1);
cruise_time_i = 1;
change_time_i = 1;
for i = 1:total_time/Tstep
if i*Tstep <= 10
vel(i) = 70;
elseif i*Tstep <= 10+8
vel(i) = 70 - 20/8*(i*Tstep-10);
elseif i*Tstep <= 10+8+20
vel(i) = 50;
elseif i*Tstep <= 10+8+20+13
vel(i) = 50 + 20/13*(i*Tstep-(10+8+20));
elseif i*Tstep <= 10+8+20+13+20
vel(i) = 70;
elseif i*Tstep <= 10+8+20+13+20+25
vel(i) = 70 + 30/25*(i*Tstep-(10+8+20+13+20));
elseif i*Tstep <= 10+8+20+13+20+25+20
vel(i) = 100;
elseif i*Tstep <= 10+8+20+13+20+25+20+10
vel(i) = 100 - 30/10*(i*Tstep-(10+8+20+13+20+25+20));
elseif i*Tstep <= 10+8+20+13+20+25+20+10+20
vel(i) = 70;
end
end
plot(vel);
% save('.\_data\nedc_modified_v2.mat','time','vel');
nedc_trajectory_modified.m¶
It serves the same purpose as nedc_trajectory.m. It was created for testing purpose and has no impact if not used at all.
Click here to view code
Tstep = 0.05;
total_time = 50+8+69+13+50+35+30+20+10+14+20;
time = 1:Tstep:total_time;
vel = zeros(total_time/Tstep,1);
cruise_time_i = 1;
change_time_i = 1;
for i = 1:total_time/Tstep
if i*Tstep <= 50
vel(i) = 70;
elseif i*Tstep <= 50+8
vel(i) = 70 - 20/8*(i*Tstep-50);
elseif i*Tstep <= 50+8+69
vel(i) = 50;
elseif i*Tstep <= 50+8+69+13
vel(i) = 50 + 20/13*(i*Tstep-(50+8+69));
elseif i*Tstep <= 50+8+69+13+50
vel(i) = 70;
elseif i*Tstep <= 50+8+69+13+50+35
vel(i) = 70 + 30/35*(i*Tstep-(50+8+69+13+50));
elseif i*Tstep <= 50+8+69+13+50+35+30
vel(i) = 100;
elseif i*Tstep <= 50+8+69+13+50+35+30+20
vel(i) = 100 + 20/20*(i*Tstep-(50+8+69+13+50+35+30));
elseif i*Tstep <= 50+8+69+13+50+35+30+20+10
vel(i) = 120;
elseif i*Tstep <= 50+8+69+13+50+35+30+20+10+14
vel(i) = 120 - 50/14*(i*Tstep-(50+8+69+13+50+35+30+20+10));
elseif i*Tstep <= 50+8+69+13+50+35+30+20+10+14+20
vel(i) = 70;
end
end
plot(vel);
save('nedc.mat','time','vel');
Plot Figure¶
Figure_BrakePerturbation_Trajectory.m¶
This function is used for analysis for simulation results under same data sample.
Click here to view code
% Data set
data_str = '2'; % 1. random ovm 2. manual ovm 3. homogeneous ovm
% Mix or not
mix = 0; % 0. all HDVs; 1. mix
% Type of the controller
controller_type = 1; % 1. DeeP-LCC 2. MPC
% Type for HDV car-following model
hdv_type = 1; % 1. OVM 2. IDM
% Uncertainty for HDV behavior
acel_noise = 0.1; % A white noise signal on HDV's original acceleration
% Perturbation amplitude
per_type = 2; % 1. sinuoid perturbation 2. brake perturbation 3. ngsim simulation
per_amp = 5;
% whether there exists constraints
constraint_bool = 1;
% wheter fix equilibrium spacing
fixed_spacing_bool = 0;
% Simulation Time
begin_time = 0.05;
end_time = 40;
i_data = 1; % Data set number
weight_v = 1; % weight coefficient for velocity error
weight_s = 0.5; % weight coefficient for spacing error
weight_u = 0.1; % weight coefficient for control input
lambda_g = 100; % penalty on ||g||_2^2 in objective
lambda_y = 1e4; % penalty on ||sigma_y||_2^2 in objective
if mix
switch controller_type
case 1
load(['..\_data\simulation_data\DeeP_LCC\constrained_simulation\simulation_data',data_str,'_',num2str(i_data),'_perType_',num2str(per_type),'_noiseLevel_',num2str(acel_noise),...
'_fixSpacing_',num2str(fixed_spacing_bool),...
'_hdvType_',num2str(hdv_type),'_lambdaG_',num2str(lambda_g),'_lambdaY_',num2str(lambda_y),'.mat']);
controller_str = 'DeeP-LCC';
case 2
load(['..\_data\simulation_data\MPC\constrained_simulation\simulation_data',data_str,'_',num2str(i_data),'_perType_',num2str(per_type),'_noiseLevel_',num2str(acel_noise),...
'_fixSpacing_',num2str(fixed_spacing_bool),...
'_hdvType_',num2str(hdv_type),'.mat']);
controller_str = 'MPC';
end
else
if constraint_bool
load(['..\_data\simulation_data\HDV\constrained_simulation\simulation_data',data_str,'_',num2str(i_data),'_perType_',num2str(per_type),'_noiseLevel_',num2str(acel_noise),...
'_hdvType_',num2str(hdv_type),'.mat']);
controller_str = 'DeeP-LCC';
else
load(['..\_data\simulation_data\HDV\simulation_data',data_str,'_',num2str(i_data),'_perType_',num2str(per_type),'_noiseLevel_',num2str(acel_noise),...
'_hdvType_',num2str(hdv_type),'_lambdaG_',num2str(lambda_g),'_lambdaY_',num2str(lambda_y),'.mat']);
controller_str = 'DeeP-LCC';
end
end
n_vehicle = length(ID); % number of vehicles
% -------------------------------------------------------------------------
% Plot Results
%--------------------------------------------------------------------------
color_gray = [190 190 190]/255;
color_red = [244, 53, 124]/255;
color_blue = [67, 121, 227]/255;
color_black = [0 0 0];
color_orange = [255,132,31]/255;
label_size = 18;
total_size = 14;
line_width = 2;
% Velocity
figure;
id_cav = 1;
plot(begin_time:Tstep:end_time,S(begin_time/Tstep:end_time/Tstep,1,2),'Color',color_black,'linewidth',line_width-0.5); hold on;
for i = 1:n_vehicle
if ID(i) == 0
plot(begin_time:Tstep:end_time,S(begin_time/Tstep:end_time/Tstep,i+1,2),'Color',color_gray,'linewidth',line_width-0.5); hold on; % line for velocity of HDVs
end
end
for i = 1:n_vehicle
if ID(i) == 1
if id_cav == 1
plot(begin_time:Tstep:end_time,S(begin_time/Tstep:end_time/Tstep,i+1,2),'Color',color_red,'linewidth',line_width); hold on; % line for velocity of CAVs
id_cav = id_cav+1;
elseif id_cav == 2
plot(begin_time:Tstep:end_time,S(begin_time/Tstep:end_time/Tstep,i+1,2),'Color',color_blue,'linewidth',line_width); hold on; % line for velocity of CAVs
end
end
end
grid on;
set(gca,'TickLabelInterpreter','latex','fontsize',total_size);
set(gca,'YLim',[0 20]);
set(gca,'XLim',[0 30]);
xl = xlabel('$t$ [$\mathrm{s}$]','fontsize',label_size,'Interpreter','latex','Color','k');
yl = ylabel('Velocity [$\mathrm{m/s}$]','fontsize',label_size,'Interpreter','latex','Color','k');
set(gcf,'Position',[250 150 400 300]);
fig = gcf;
fig.PaperPositionMode = 'auto';
% if mix
% print(gcf,['.\figs\BrakePerturbation_VelocityProfile_Controller_',num2str(controller_type)],'-painters','-depsc2','-r300');
% else
% print(gcf,'.\figs\BrakePerturbation_VelocityProfile_AllHDVs','-painters','-depsc2','-r300');
% end
% Spacing
figure;
id_cav = 1;
for i = 1:n_vehicle
if ID(i) == 1
if id_cav ==1
plot(begin_time:Tstep:end_time,S(begin_time/Tstep:end_time/Tstep,i,1)-S(begin_time/Tstep:end_time/Tstep,i+1,1),'Color',color_red,'linewidth',line_width); hold on; % line for velocity of CAVs
id_cav = id_cav + 1;
elseif id_cav == 2
plot(begin_time:Tstep:end_time,S(begin_time/Tstep:end_time/Tstep,i,1)-S(begin_time/Tstep:end_time/Tstep,i+1,1),'Color',color_blue,'linewidth',line_width); hold on; % line for velocity of CAVs
end
end
end
% if mix
% plot(begin_time:Tstep:end_time,5*ones(round((end_time-begin_time)/Tstep)+1),'--k','linewidth',line_width);
% text(11,6.2,'$s_\mathrm{min}=5\,\mathrm{m}$','Interpreter','latex','FontSize',label_size);
% end
grid on;
set(gca,'TickLabelInterpreter','latex','fontsize',total_size);
set(gca,'YLim',[5 35]);
set(gca,'XLim',[0 30]);
set(gca,'YTick',5:10:35);
xl = xlabel('$t$ [$\mathrm{s}$]','fontsize',label_size,'Interpreter','latex','Color','k');
yl = ylabel('Spacing [$\mathrm{m}$]','fontsize',label_size,'Interpreter','latex','Color','k');
set(gcf,'Position',[550 150 400 300]);
fig = gcf;
fig.PaperPositionMode = 'auto';
% if mix
% print(gcf,['.\figs\BrakePerturbation_SpacingProfile_Controller_',num2str(controller_type)],'-painters','-depsc2','-r300');
% else
% print(gcf,'.\figs\BrakePerturbation_SpacingProfile_AllHDVs','-painters','-depsc2','-r300');
% end
% Problem status
if mix
figure;
plot(begin_time:Tstep:end_time,pr_status);
set(gcf,'Position',[850 150 400 300]);
fig = gcf;
fig.PaperPositionMode = 'auto';
end
% Acceleration
figure;
id_cav = 1;
plot(begin_time:Tstep:end_time,S(begin_time/Tstep:end_time/Tstep,1,3),'Color',color_black,'linewidth',line_width-0.5); hold on;
for i = 1:n_vehicle
if ID(i) == 1
if id_cav == 1
plot(begin_time:Tstep:end_time,S(begin_time/Tstep:end_time/Tstep,i+1,3),'Color',color_red,'linewidth',line_width); hold on; % line for velocity of CAVs
id_cav = id_cav+1;
elseif id_cav == 2
plot(begin_time:Tstep:end_time,S(begin_time/Tstep:end_time/Tstep,i+1,3),'Color',color_blue,'linewidth',line_width); hold on; % line for velocity of CAVs
end
end
end
grid on;
set(gca,'TickLabelInterpreter','latex','fontsize',total_size);
set(gca,'YLim',[-6 4]);
set(gca,'XLim',[0 30]);
xl = xlabel('$t$ [$\mathrm{s}$]','fontsize',label_size,'Interpreter','latex','Color','k');
yl = ylabel('Acceleration [$\mathrm{m/s^2}$]','fontsize',label_size,'Interpreter','latex','Color','k');
set(gcf,'Position',[250 450 400 300]);
fig = gcf;
fig.PaperPositionMode = 'auto';
% if mix
% print(gcf,['.\figs\BrakePerturbation_AccelerationProfile_Controller_',num2str(controller_type)],'-painters','-depsc2','-r300');
% else
% print(gcf,'.\figs\BrakePerturbation_AccelerationProfile_AllHDVs','-painters','-depsc2','-r300');
% end
% -------------------------------------------------------------------------
% Calculate Performance Indexes
%--------------------------------------------------------------------------
smooth_window = 10;
for i = 2:n_vehicle+1
S(:,i,3) = smooth(S(:,i,3),smooth_window);
end
FuelConsumption = 0;
VelocityError = 0;
for i=begin_time/Tstep:end_time/Tstep
R = 0.333 + 0.00108*S(i,4:end,2).^2 + 1.2*S(i,4:end,3);
Fuel = 0.444 + 0.09*R.*S(i,4:end,2) + 0.054 * max(0,S(i,4:end,3)).^2.*S(i,4:end,2);
Fuel(R <= 0) = 0.444;
FuelConsumption = FuelConsumption + sum(Fuel)*Tstep;
VelocityError = VelocityError + sum(abs(S(i,4:end,2)-S(i,1,2))/S(i,1,2));
end
VelocityError = VelocityError/n_vehicle/((end_time-begin_time)/Tstep);
fprintf('Fuel comsumption: %4.2f \n',FuelConsumption);
fprintf('Velocity error: %4.2f \n',VelocityError);
Figure_NEDC_Statistics.m¶
This function is used for analysis for simulation results under same data sample.
Click here to view code
% Data set
data_str = '2'; % 1. random ovm 2. manual ovm 3. homogeneous ovm
% Type for HDV car-following model
hdv_type = 1; % 1. OVM 2. IDM
% Uncertainty for HDV behavior
acel_noise = 0.1; % A white noise signal on HDV's original acceleration
% Head vehicle trajectory
trajectory_id = '1';
head_vehicle_trajectory = load(['./_data/nedc_modified_v',num2str(trajectory_id),'.mat']);
end_time = head_vehicle_trajectory.time(end);
initialization_time = 30; % Time for the original HDV-all system to stabilize
adaption_time = 20; % Time for the CAVs to adjust to their desired state
% Total simulation results
% total_time = initialization_time + adaption_time + end_time; % Total Simulation Time
% begin_time = initialization_time + adaption_time;
% Seperate different phases in the simulation
% time_point = [60,88,121,176,206]; % For Trajectory V1
time_point = [60,88,121,166,196]; % For Trajectory V2
time_i = 4;
begin_time = time_point(time_i);
total_time = time_point(time_i+1);
weight_v = 1; % weight coefficient for velocity error
weight_s = 0.5; % weight coefficient for spacing error
weight_u = 0.1; % weight coefficient for control input
lambda_g = 100; % penalty on ||g||_2^2 in objective
lambda_y = 1e4; % penalty on ||sigma_y||_2^2 in objective
i_data = 2;
load(['_data\simulation_data\MPC\nedc_simulation\simulation_data',data_str,'_modified_v',num2str(trajectory_id),'_noiseLevel_',num2str(acel_noise),...
'_hdvType_',num2str(hdv_type),'.mat']);
S_MPC = S;
load(['_data\simulation_data\DeePC\nedc_simulation\simulation_data',data_str,'_',num2str(i_data),'_modified_v',num2str(trajectory_id),'_noiseLevel_',num2str(acel_noise),...
'_hdvType_',num2str(hdv_type),'_lambdaG_',num2str(lambda_g),'_lambdaY_',num2str(lambda_y),'.mat']);
S_DeePC = S;
load(['_data\simulation_data\HDV\nedc_simulation\simulation_data',data_str,'_modified_v',num2str(trajectory_id),'_noiseLevel_',num2str(acel_noise),...
'_hdvType_',num2str(hdv_type),'.mat']);
S_HDV = S;
n_vehicle = length(ID); % number of vehicles
% Smooth acceleration signal
smooth_window = 10;
for i = 2:n_vehicle+1
S_DeePC(:,i,3) = smooth(S_DeePC(:,i,3),smooth_window);
S_MPC(:,i,3) = smooth(S_MPC(:,i,3),smooth_window);
S_HDV(:,i,3) = smooth(S_HDV(:,i,3),smooth_window);
end
FuelConsumption = zeros(1,3);
VelocityError = zeros(1,3);
for i=begin_time/Tstep:total_time/Tstep
R_DeePC = 0.333 + 0.00108*S_DeePC(i,4:end,2).^2 + 1.2*S_DeePC(i,4:end,3);
F_DeePC = 0.444 + 0.09*R_DeePC.*S_DeePC(i,4:end,2) + 0.054 * max(0,S_DeePC(i,4:end,3)).^2.*S_DeePC(i,4:end,2);
F_DeePC(R_DeePC <= 0) = 0.444;
FuelConsumption(1) = FuelConsumption(1) + sum(F_DeePC)*Tstep;
VelocityError(1) = VelocityError(1) + sum(abs(S_DeePC(i,4:end,2)-S_DeePC(i,1,2))/S_DeePC(i,1,2));
R_MPC = 0.333 + 0.00108*S_MPC(i,4:end,2).^2 + 1.2*S_MPC(i,4:end,3);
F_MPC = 0.444 + 0.09*R_MPC.*S_MPC(i,4:end,2) + 0.054 * max(0,S_MPC(i,4:end,3)).^2.*S_MPC(i,4:end,2);
F_MPC(R_MPC <= 0) = 0.444;
FuelConsumption(2) = FuelConsumption(2) + sum(F_MPC)*Tstep;
VelocityError(2) = VelocityError(2) + sum(abs(S_MPC(i,4:end,2)-S_MPC(i,1,2))/S_MPC(i,1,2));
R_HDV = 0.333 + 0.00108*S_HDV(i,4:end,2).^2 + 1.2*S_HDV(i,4:end,3);
F_HDV = 0.444 + 0.09*R_HDV.*S_HDV(i,4:end,2) + 0.054 * max(0,S_HDV(i,4:end,3)).^2.*S_HDV(i,4:end,2);
F_HDV(R_HDV <= 0) = 0.444;
FuelConsumption(3) = FuelConsumption(3) + sum(F_HDV)*Tstep;
VelocityError(3) = VelocityError(3) + sum(abs(S_HDV(i,4:end,2)-S_HDV(i,1,2))/S_HDV(i,1,2));
end
VelocityError = VelocityError/n_vehicle/((total_time-begin_time)/Tstep);
fprintf('Fuel Consumption: DeePC | MPC | HDVs \n');
fprintf(' %4.2f | %4.2f | %4.2f \n',FuelConsumption);
fprintf(' %4.2f%% | %4.2f%% | %4.2f%% \n',(FuelConsumption(3)-FuelConsumption)/FuelConsumption(3)*100);
fprintf('Velocity Error: DeePC | MPC | HDVs \n');
fprintf(' %4.4f | %4.4f | %4.4f \n',VelocityError);
fprintf(' %4.2f%% | %4.2f%% | %4.2f%% \n',(VelocityError(3)-VelocityError)/VelocityError(3)*100);
Figure_NEDC_Trajectory.m¶
This function is used for analysis for simulation results under same data sample.
Click here to view code
% Data set
data_str = '2'; % 1. random ovm 2. manual ovm 3. homogeneous ovm
% Mix or not
mix = 1; % 0. all HDVs; 1. mix
% Type of the controller
controller_type = 2; % 1. DeeP-LCC 2. MPC
% Type for HDV car-following model
hdv_type = 1; % 1. OVM 2. IDM
% Uncertainty for HDV behavior
acel_noise = 0.1; % A white noise signal on HDV's original acceleration
% Head vehicle trajectory
trajectory_id = '1';
head_vehicle_trajectory = load(['../_data/nedc_modified_v',num2str(trajectory_id),'.mat']);
end_time = head_vehicle_trajectory.time(end);
initialization_time = 30; % Time for the original HDV-all system to stabilize
adaption_time = 20; % Time for the CAVs to adjust to their desired state
total_time = initialization_time + adaption_time + end_time; % Total Simulation Time
begin_time = initialization_time + adaption_time;
weight_v = 1; % weight coefficient for velocity error
weight_s = 0.5; % weight coefficient for spacing error
weight_u = 0.1; % weight coefficient for control input
lambda_g = 100; % penalty on ||g||_2^2 in objective
lambda_y = 1e4; % penalty on ||sigma_y||_2^2 in objective
i_data = 1;
if mix
switch controller_type
case 1
load(['..\_data\simulation_data\DeeP_LCC\nedc_simulation\simulation_data',data_str,'_',num2str(i_data),'_modified_v',num2str(trajectory_id),'_noiseLevel_',num2str(acel_noise),...
'_hdvType_',num2str(hdv_type),'_lambdaG_',num2str(lambda_g),'_lambdaY_',num2str(lambda_y),'.mat']);
controller_str = 'DeePC';
case 2
load(['..\_data\simulation_data\MPC\nedc_simulation\simulation_data',data_str,'_modified_v',num2str(trajectory_id),'_noiseLevel_',num2str(acel_noise),...
'_hdvType_',num2str(hdv_type),'.mat']);
controller_str = 'MPC';
end
else % ngsim simulation
load(['..\_data\simulation_data\HDV\nedc_simulation\simulation_data',data_str,'_modified_v',num2str(trajectory_id),'_noiseLevel_',num2str(acel_noise),...
'_hdvType_',num2str(hdv_type),'.mat']);
controller_str = 'HDV';
end
n_vehicle = length(ID); % number of vehicles
% -------------------------------------------------------------------------
% Plot Results
%--------------------------------------------------------------------------
color_gray = [170 170 170]/255;
color_red = [244, 53, 124]/255;
color_blue = [67, 121, 227]/255;
color_black = [0 0 0];
color_orange = [255,132,31]/255;
color_blue_2 = [61, 90, 128]/255;
color_red_2 = [238, 108, 77]/255;
label_size = 18;
total_size = 14;
line_width = 1.5;
% Head vehicle trajectory
time_point = [60,88,121,176,206]; % For Trajectory V1
% time_point = [60,88,121,166,196]; % For Trajectory V2
time_scale = (begin_time:Tstep:total_time)-(initialization_time + adaption_time);
figure;
plot(time_scale,S(begin_time/Tstep:round(total_time/Tstep),1,2),'Color',color_black,'linewidth',line_width); hold on;
grid on;
set(gca,'TickLabelInterpreter','latex','fontsize',total_size);
set(gca,'XLim',[begin_time total_time]-(initialization_time + adaption_time));
set(gca,'YLim',[10,30]);
for time_i = 1:4
plot(time_point(time_i)*ones(20,1)-(initialization_time + adaption_time),linspace(10,S(time_point(time_i)/Tstep,1,2),20),'--','Color',color_blue,'linewidth',line_width); hold on;
text_phase = text((time_point(time_i)+time_point(time_i+1))/2-(initialization_time + adaption_time),11.5,['phase ',num2str(time_i)],...
'Interpreter','latex','FontSize',label_size,'HorizontalAlignment','center','Color',color_red);
end
xl = xlabel('$t$ [$\mathrm{s}$]','fontsize',label_size,'Interpreter','latex','Color','k');
yl = ylabel('Velocity [$\mathrm{m/s}$]','fontsize',label_size,'Interpreter','latex','Color','k');
set(gcf,'Position',[250 550 850 300]);
fig = gcf;
fig.PaperPositionMode = 'auto';
% print(gcf,'.\figs\NEDC_HeadVehicleTrajectory','-painters','-depsc2','-r300');
% Velocity
% for time_i = 1:4
% begin_time = time_point(time_i);
% total_time = time_point(time_i+1);
% figure;
% plot(begin_time:Tstep:total_time,S(begin_time/Tstep:round(total_time/Tstep),1,2),'Color',color_gray,'linewidth',line_width); hold on;
% for i = 1:n_vehicle
% if ID(i) == 1
% plot(begin_time:Tstep:total_time,S(begin_time/Tstep:round(total_time/Tstep),i+1,2),'Color',color_red,'linewidth',line_width); hold on; % line for velocity of CAVs
% else
% plot(begin_time:Tstep:total_time,S(begin_time/Tstep:round(total_time/Tstep),i+1,2),'Color',color_blue,'linewidth',line_width); hold on; % line for velocity of HDVs
% end
% end
% grid on;
% set(gca,'TickLabelInterpreter','latex','fontsize',total_size);
% set(gca,'XLim',[begin_time total_time]);
%
% xl = xlabel('$t$ [$\mathrm{s}$]','fontsize',label_size,'Interpreter','latex','Color','k');
% yl = ylabel('Velocity [$\mathrm{m/s}$]','fontsize',label_size,'Interpreter','latex','Color','k');
%
% set(gcf,'Position',[250 150 400 300]);
% fig = gcf;
% fig.PaperPositionMode = 'auto';
%
% end
total_time = initialization_time + adaption_time + end_time; % Total Simulation Time
begin_time = initialization_time + adaption_time;
figure;
% h = axes('position',[0 0 1 1]);
% axis(h);
plot(time_scale,S(begin_time/Tstep:round(total_time/Tstep),1,2),'Color',color_black,'linewidth',line_width); hold on;
for i = 1:n_vehicle
if ID(i) == 0
plot(time_scale,S(begin_time/Tstep:round(total_time/Tstep),i+1,2),'Color',color_gray,'linewidth',line_width/2); hold on; % line for velocity of HDVs
end
end
id_cav = 1;
for i = 1:n_vehicle
if ID(i) == 1
if id_cav == 1
plot(time_scale,S(begin_time/Tstep:round(total_time/Tstep),i+1,2),'Color',color_red,'linewidth',line_width); hold on; % line for velocity of CAVs
id_cav = id_cav+1;
elseif id_cav == 2
plot(time_scale,S(begin_time/Tstep:round(total_time/Tstep),i+1,2),'Color',color_blue,'linewidth',line_width); hold on; % line for velocity of CAVs
end
end
end
for time_i = 1:4
plot(time_point(time_i)*ones(20,1)-(initialization_time + adaption_time),linspace(10,S(time_point(time_i)/Tstep,1,2),20),'--','Color',color_blue_2,'linewidth',line_width/2); hold on;
text_phase = text((time_point(time_i)+time_point(time_i+1))/2-(initialization_time + adaption_time),12.5,['Phase ',num2str(time_i)],...
'Interpreter','latex','FontSize',label_size,'HorizontalAlignment','center','Color',color_red_2);
end
grid on;
set(gca,'TickLabelInterpreter','latex','fontsize',total_size);
set(gca,'XLim',[time_scale(1) time_scale(end)]);
xl = xlabel('$t$ [$\mathrm{s}$]','fontsize',label_size,'Interpreter','latex','Color','k');
yl = ylabel('Velocity [$\mathrm{m/s}$]','fontsize',label_size,'Interpreter','latex','Color','k');
set(gca,'YLim',[11.5,28.5]);
set(gcf,'Position',[250 150 850 300]);
fig = gcf;
fig.PaperPositionMode = 'auto';
% h1 = axes;
% axis(h1);
% begin_time_1 = 65;
% total_time_1 = 75;
%
% plot(begin_time_1:Tstep:total_time_1,S(begin_time_1/Tstep:round(total_time_1/Tstep),1,2),'Color',color_black,'linewidth',line_width); hold on; % line for velocity of HDVs
% for i = 1:n_vehicle
% if ID(i) == 0
% plot(begin_time_1:Tstep:total_time_1,S(begin_time_1/Tstep:round(total_time_1/Tstep),i+1,2),'Color',color_gray,'linewidth',line_width/2); hold on; % line for velocity of HDVs
% end
% end
% id_cav = 1;
% for i = 1:n_vehicle
% if ID(i) == 1
% if id_cav == 1
% plot(begin_time_1:Tstep:total_time_1,S(begin_time_1/Tstep:round(total_time_1/Tstep),i+1,2),'Color',color_red,'linewidth',line_width); hold on; % line for velocity of CAVs
% id_cav = id_cav+1;
% elseif id_cav == 2
% plot(begin_time_1:Tstep:total_time_1,S(begin_time_1/Tstep:round(total_time_1/Tstep),i+1,2),'Color',color_blue,'linewidth',line_width); hold on; % line for velocity of CAVs
% end
% end
% end
%
% set(gca,'xticklabel',[])
% set(gca,'yticklabel',[])
% set(gca,'YLim',[13 18]);
% h1.Position = [0.15 0.6 0.15 0.25];
% if mix
% print(gcf,['.\figs\NEDC_VelocityProfile_Controller_',num2str(controller_type)],'-painters','-depsc2','-r300');
% else
% print(gcf,'.\figs\NEDC_VelocityProfile_AllHDVs','-painters','-depsc2','-r300');
% end
% Spacing
figure;
for i = 1:n_vehicle
if ID(i) == 0
plot(time_scale,S(begin_time/Tstep:round(total_time/Tstep),i,1)-S(begin_time/Tstep:round(total_time/Tstep),i+1,1),'Color',color_gray,'linewidth',line_width/2); hold on; % line for velocity of HDVs
end
end
id_cav = 1;
for i = 1:n_vehicle
if ID(i) == 1
if id_cav == 1
plot(time_scale,S(begin_time/Tstep:round(total_time/Tstep),i,1)-S(begin_time/Tstep:round(total_time/Tstep),i+1,1),'Color',color_red,'linewidth',line_width); hold on; % line for velocity of CAVs
id_cav = id_cav+1;
elseif id_cav == 2
plot(time_scale,S(begin_time/Tstep:round(total_time/Tstep),i,1)-S(begin_time/Tstep:round(total_time/Tstep),i+1,1),'Color',color_blue,'linewidth',line_width); hold on; % line for velocity of CAVs
end
end
end
grid on;
set(gca,'TickLabelInterpreter','latex','fontsize',total_size);
set(gca,'XLim',[time_scale(1) time_scale(end)]);
xl = xlabel('$t$ [$\mathrm{s}$]','fontsize',label_size,'Interpreter','latex','Color','k');
yl = ylabel('Spacing [$\mathrm{m}$]','fontsize',label_size,'Interpreter','latex','Color','k');
set(gcf,'Position',[1050 150 750 300]);
fig = gcf;
fig.PaperPositionMode = 'auto';
% if mix
% print(gcf,['.\figs\NGSIM_SpacingProfile_Controller_',num2str(controller_type)],'-painters','-depsc2','-r300');
% else
% print(gcf,'.\figs\NGSIM_SpacingProfile_AllHDVs','-painters','-depsc2','-r300');
% end
% acceleration
figure;
for i = 1:n_vehicle
% smooth acceleration signal
S(:,i+1,3) = smooth(S(:,i+1,3),10);
end
for i = 1:n_vehicle
if ID(i) == 0
plot(time_scale,S(begin_time/Tstep:round(total_time/Tstep),i+1,3),'Color',color_gray,'linewidth',line_width/2); hold on; % line for velocity of HDVs
end
end
id_cav = 1;
for i = 1:n_vehicle
if ID(i) == 1
if id_cav == 1
plot(time_scale,S(begin_time/Tstep:round(total_time/Tstep),i+1,3),'Color',color_red,'linewidth',line_width); hold on; % line for velocity of CAVs
id_cav = id_cav+1;
elseif id_cav == 2
plot(time_scale,S(begin_time/Tstep:round(total_time/Tstep),i+1,3),'Color',color_blue,'linewidth',line_width); hold on; % line for velocity of CAVs
end
end
end
grid on;
set(gca,'TickLabelInterpreter','latex','fontsize',total_size);
set(gca,'XLim',[time_scale(1) time_scale(end)]);
xl = xlabel('$t$ [$\mathrm{s}$]','fontsize',label_size,'Interpreter','latex','Color','k');
yl = ylabel('Acceleration [$\mathrm{m/s^2}$]','fontsize',label_size,'Interpreter','latex','Color','k');
set(gcf,'Position',[1050 550 700 300]);
fig = gcf;
fig.PaperPositionMode = 'auto';
% -------------------------------------------------------------------------
% Calculate Performance Indexes
%--------------------------------------------------------------------------
FuelConsumption = 0;
VelocityError = 0;
for i=begin_time/Tstep:total_time/Tstep
R = 0.333 + 0.00108*S(i,4:end,2).^2 + 1.2*S(i,4:end,3);
Fuel = 0.444 + 0.09*R.*S(i,4:end,2) + 0.054 * max(0,S(i,4:end,3)).^2.*S(i,4:end,2);
Fuel(R <= 0) = 0.444;
FuelConsumption = FuelConsumption + sum(Fuel)*Tstep;
VelocityError = VelocityError + sum(abs(S(i,4:end,2)-S(i,1,2))/S(i,1,2));
end
VelocityError = VelocityError/n_vehicle/((total_time-begin_time)/Tstep);
fprintf('Fuel comsumption: %4.2f \n',FuelConsumption);
fprintf('Velocity error: %4.4f \n',VelocityError);
Figure_OVMSpacingPolicy.m¶
Click here to view code
alpha = 0.6;
beta = 0.9;
s_st = 5;
s_go = 35;
v_max = 30;
v = 0:0.05:v_max;
s = acos(1-v/30*2)/pi*(35-5) + 5;
f1 = figure(1);
plot(v,s,'linewidth',2);
axis([0 35 0 40]);
hold on;
plot(s,v_max*ones(size(s)),'--k','linewidth',1);
Wsize = 14; % word size
set(gca,'TickLabelInterpreter','latex','fontsize',11);
grid on;
xlabel('Spacing','fontsize',Wsize,'Interpreter','latex','Color','k');
ylabel('Desired velocity','fontsize',Wsize,'Interpreter','latex','Color','k');
set(gca,'ytick',[])
set(gca,'xtick',[])
text(1,32,'$v_{\mathrm{max}}$','fontsize',Wsize,'Interpreter','latex','Color','k');
text(30,3,'$s_{\mathrm{go}}$','fontsize',Wsize,'Interpreter','latex','Color','k');
text(4.5,3,'$s_{\mathrm{st}}$','fontsize',Wsize,'Interpreter','latex','Color','k');
plot([5,5],[-1,1],'-k','linewidth',1);
plot([35,35],[0,30],'--k','linewidth',1);
set(gcf,'Position',[250 150 250 200]);
fig = gcf;
fig.PaperPositionMode = 'auto';
print(gcf,'./Figures_Fig2_OVMSpacingPolicy','-painters','-depsc2','-r300');
Python Implementation¶
Python implementation follows closely with Matlab implementation. It simulates two scenarios:
- Scenario 1: Safety test with head vehicle takeing a sudden brake
- Scenario 2: New European Driving Cyccle (NEDC) simulation with head vehicle following a trajectory modified from Extra-Urban Driving Cycle (EUDC) in NEDC
Packages¶
The python implementation requires user to have the following packages installed:
- numpy
- cvxopt
- scipy.io
- ttictoc
- math
- matplotlib.pyplot
- sys
- dill
Function¶
Two main functions are:
-
main_brake_simulation.py This simulates scenario 1.
-
main_nedc_simulation.py This simulates scenario 2.
Both functions allow user to switch between MPC and DeePC method by changing variable values.
The optimization problem is solved in the following functions:- qp_DeeP_LCC.py
- qp_MPC.py
Both functions utilize cvxopt solver to solve the optimization problem.
Experiment Results¶
Experiment Setup¶
We consider eight vehicles with two CAVs, i.e., in the above figure.
In DeeP-LCC, we use the following parameters.
- In offline data collection: the length for the pre-collected data is $T = 2000$ with $\Delta t = 0.05s$
- In online predictive control: the time horizons for the future and past trajectories are set to $N = 50$. $T_{ini} = 20$. For constraints, we have .
Numerical Results¶
Experiment A: We first design a comprehensive simulation scenario to validate the capability of DeeP-LCC in improving traffic performance. Specifically, motivated by EUDC driving cycle, we design a velocity trajectory for the head vehicle. For performance evaluation, we calculate the fuel consumption using the numerical model in [2] for the vehivles indexed from 3 to 8, given that the first two HDVs would not be influnced.
The simulation results are as below:
This figure shows how velocity profiles in Experiment A. The black profile represents the head vehicle, and the gray profile represents the HDVs. The red profile and the blue profile represent the first and the second CAV, respectively. (a) All the vehicles are HDVs. (b) The CAVs utilize DeeP-LCC.
The results of the fuel consumption is presented in the below table, with the whole simulation separated into four phases:
Experiment B: To further validate the safety performance of DeeP-LCC, we design a particular braking scenario motivated by EUDC, where the head vehicle takes a sudden emergency brake with maximum deceleration capability, maintains the low velocity for a while, and finally accelerates to the original normal velocity. This is a typical emergency case in real traffic flow, which requires the CAVs’ control to have strict safety guarantees from rear-end collision.
The results are shown below:
Simulation results in Experiment B. $(a)(c)(e)$ show the velocity, spacing, and acceleration profiles, respectively when all the vehicles are HDVs, while (b)(d)(f) show the corresponding profiles where the two CAVs utilize DeeP-LCC. In $(c)-(f)$, the profiles of the other HDVs are hidden.
Runtime¶
Here we present the runtime of both simulations at the time of creation of this webpage.
Optimization for main_brake_simulation
Matlab: 13.5 min
Python: 14 min
Optimization for main_nedc_simulation
Matlab: 56 min
Python: 39.6 min
Reference¶
[1] Wang, J., Zheng, Y., Li, K.,& Xu, Q.,(2023). DeeP-LCC: Data-EnablEd Predictive Leading Cruise Control in Mixed Traffic Flow. 2203-10639. [pdf]
[2] Wang, J., Zheng, Y., Li, K.,& Xu, Q.,(2022). Data-Driven Predictive Control for Connected and Autonomous Vehicles in Mixed Traffic. 2110-100. [pdf]
[3] D. P. Bowyer, R. Akccelik, and D. Biggs, Guide to fuel consumption analysis for urban traffic management. Vermont South, Australia: ARRB Transport Research Ltd, 1985, no. 32.