├── README.md └── truck_platoon_cut_in.m /README.md: -------------------------------------------------------------------------------- 1 | [![View Truck platooning - Cut-in scenario on File Exchange](https://www.mathworks.com/matlabcentral/images/matlab-file-exchange.svg)](https://www.mathworks.com/matlabcentral/fileexchange/92165-truck-platooning-cut-in-scenario) 2 | # Truck platooning - Cut-in scenario 3 | Simulation and animation of a cut-in scenario between trucks one and two in a four-truck platoon with 1 second time gap. 4 | 5 | Watch the animation on YouTube: https://youtu.be/5ReMLXiI8yo 6 | 7 | ![Logo](https://www.mathworks.com/matlabcentral/mlc-downloads/downloads/5d76f6b8-ce1f-4735-9f0b-933bdee54eb2/7a8ffc2e-b556-4e63-a943-67d4367d0783/images/1621262511.png) 8 | 9 | ## Examples 10 | ### Truck platooning - Two cut-ins - 1 (Collision between car 2 and truck 3) 11 | Simulation and animation of two cut-ins to a four-truck platoon. The first cut-in occurs between trucks one and two and the second cut-in occurs between trucks three and four. 12 | 13 | Watch the animation on YouTube: https://youtu.be/d7El0c3c-3U 14 | 15 |   16 | 17 | ### Truck platooning - Two cut-ins - 2 (Collision between car 2 and truck 2) 18 | Simulation and animation of two cut-ins to a four-truck platoon. The first cut-in occurs between trucks one and two and the second cut-in occurs between trucks two and three. 19 | 20 | Watch the animation on YouTube: https://youtu.be/7QIwht2FWVM 21 | -------------------------------------------------------------------------------- /truck_platoon_cut_in.m: -------------------------------------------------------------------------------- 1 | %% Truck platooning - Cut-in scenario 2 | % Simulation and animation of a cut-in scenario between trucks one and two 3 | % in a four-truck platoon with 1 second time gap. 4 | % 5 | %% 6 | 7 | clear ; close all ; clc 8 | 9 | %% Scenario 10 | % https://www.ebanataw.com.br/trafegando/gabaritorodoviario.htm 11 | 12 | N_trucks = 4; % Number of trucks 13 | th = 1.0; % Time gap [s] 14 | 15 | % Parameters 16 | tf = 30; % Final time [s] 17 | fR = 30; % Frame rate [fps] 18 | dt = 1/fR; % Time resolution [s] 19 | time = linspace(0,tf,tf*fR); % Time [s] 20 | 21 | % Road 22 | distance_analysis = 160; % Distance of analysis [m] 23 | trackWidth = 20; % Track width [m] 24 | laneWidth = 4.3; % Lane width [m] 25 | laneMargin = 2; % Margin lane [m] 26 | 27 | % Truck 1 (Leading truck) 28 | truck_1_length = 18.6; % Length of the leading truck [m] 29 | truck_1_width = 2.6; % Width of the leading truck [m] 30 | truck_1_init_speed = 72/3.6; % Speed of the leading truck [m/s] 31 | % Initial position of the leading truck [m] 32 | truck_1_init_pos = distance_analysis; 33 | 34 | % Truck 2 (Second truck) 35 | truck_2_length = 18.6; % Length of the second truck [m] 36 | truck_2_width = 2.6; % Width of the second truck [m] 37 | truck_2_init_speed = 72/3.6; % Speed of the second truck [m/s] 38 | % Initial position of the second truck [m] 39 | truck_2_init_pos = truck_1_init_pos-truck_1_length-th*truck_2_init_speed; 40 | 41 | % Truck 3 (Third truck) 42 | truck_3_length = 18.6; % Length of the third truck [m] 43 | truck_3_width = 2.6; % Width of the third truck [m] 44 | truck_3_init_speed = 72/3.6; % Speed of the third truck [m/s] 45 | % Initial position of the third truck [m] 46 | truck_3_init_pos = truck_2_init_pos-truck_2_length-th*truck_3_init_speed; 47 | 48 | % Truck 4 (Fourth truck) 49 | truck_4_length = 18.6; % Length of the fourth truck [m] 50 | truck_4_width = 2.6; % Width of the fourth truck [m] 51 | truck_4_init_speed = 72/3.6; % Speed of the fourth truck [m/s] 52 | % Initial position of the fourth truck [m] 53 | truck_4_init_pos = truck_3_init_pos-truck_3_length-th*truck_4_init_speed; 54 | 55 | % Car (cut-in) 56 | car_length = 5; % Length of the car [m] 57 | car_width = 2.6; % Width of the car [m] 58 | car_init_speed = 72/3.6; % Speed of the car [m/s] 59 | % Initial position of the car [m] 60 | car_init_lon_pos = truck_1_init_pos-2*truck_1_length + 10; 61 | % Initial lateral position of the car [m] 62 | car_init_lat_pos = 4.3; 63 | 64 | % PARAMETERS 65 | % Struct to ode45 66 | % Trucks 67 | parameters.th = th; 68 | % Length 69 | parameters.truck_1_length = truck_1_length; 70 | parameters.truck_2_length = truck_2_length; 71 | parameters.truck_3_length = truck_3_length; 72 | parameters.truck_4_length = truck_4_length; 73 | % Width 74 | parameters.truck_2_width = truck_2_width; 75 | % Car 76 | parameters.car_length = car_length; 77 | parameters.car_width = car_width; 78 | 79 | %% Simulation 80 | 81 | % INITIAL CONDITIONS 82 | % Pair position and speed of first truck, pair position and speed of the 83 | % second truck and so on ... pair position and speed of the car 84 | states_initial_condition = [truck_1_init_pos 85 | truck_1_init_speed 86 | truck_2_init_pos 87 | truck_2_init_speed 88 | truck_3_init_pos 89 | truck_3_init_speed 90 | truck_4_init_pos 91 | truck_4_init_speed 92 | car_init_lon_pos 93 | car_init_speed 94 | car_init_lat_pos]; 95 | 96 | % SIMULATION 97 | options = odeset('RelTol',1e-9,'AbsTol',1e-9); 98 | [TOUT,YOUT] = ode45(@(t,z) simulation(t,z,parameters),time,states_initial_condition,options); 99 | 100 | % RETRIEVING STATES 101 | % Truck 1 102 | truck_1_position = YOUT(:,1); 103 | truck_1_speed = YOUT(:,2); 104 | % Truck 2 105 | truck_2_position = YOUT(:,3); 106 | truck_2_speed = YOUT(:,4); 107 | % Truck 3 108 | truck_3_position = YOUT(:,5); 109 | truck_3_speed = YOUT(:,6); 110 | % Truck 4 111 | truck_4_position = YOUT(:,7); 112 | truck_4_speed = YOUT(:,8); 113 | % Car 114 | car_pos_lon = YOUT(:,9); 115 | car_speed = YOUT(:,10); 116 | car_pos_lat = YOUT(:,11); 117 | 118 | % Acceleration 119 | % Preallocating 120 | truck_1_acc = zeros(1,length(TOUT)); 121 | truck_2_acc = zeros(1,length(TOUT)); 122 | truck_3_acc = zeros(1,length(TOUT)); 123 | truck_4_acc = zeros(1,length(TOUT)); 124 | for i=1:length(TOUT) 125 | [dz] = simulation(TOUT(i),YOUT(i,:),parameters); 126 | truck_1_acc(i) = dz(2); 127 | truck_2_acc(i) = dz(4); 128 | truck_3_acc(i) = dz(6); 129 | truck_4_acc(i) = dz(8); 130 | end 131 | 132 | % Distances 133 | dist_1_2 = truck_1_position - truck_2_position - truck_1_length; 134 | dist_2_3 = truck_2_position - truck_3_position - truck_2_length; 135 | dist_3_4 = truck_3_position - truck_4_position - truck_3_length; 136 | 137 | %% Results 138 | 139 | c = cool(N_trucks); % Colormap 140 | 141 | figure 142 | set(gcf,'Position',[50 50 1280 720]) % YouTube: 720p 143 | % set(gcf,'Position',[50 50 854 480]) % YouTube: 480p 144 | % set(gcf,'Position',[50 50 640 640]) % Social 145 | 146 | % Create and open video writer object 147 | v = VideoWriter('truck_platoon_cut_in.mp4','MPEG-4'); 148 | v.Quality = 100; 149 | open(v); 150 | 151 | for i=1:length(time) 152 | subplot(3,2,1) 153 | hold on ; grid on 154 | position_max = max(truck_1_position); 155 | set(gca,'xlim',[0 TOUT(end)],'ylim',[0 1.2*position_max]) 156 | cla 157 | plot(TOUT,truck_1_position,'color',c(1,:)) 158 | plot(TOUT,truck_2_position,'color',c(2,:)) 159 | plot(TOUT,truck_3_position,'color',c(3,:)) 160 | plot(TOUT,truck_4_position,'color',c(4,:)) 161 | plot([time(i) time(i)],[0 1.2*position_max],'k--') 162 | xlabel('Time [s]') 163 | ylabel('Position [m]') 164 | title('Position') 165 | legend('Truck 1','Truck 2','Truck 3','Truck 4','location','SouthEast') 166 | subplot(3,2,3) 167 | hold on ; grid on 168 | speed_max = max(max([truck_1_speed truck_2_speed truck_3_speed truck_4_speed])); 169 | set(gca,'xlim',[0 TOUT(end)],'ylim',[0 1.2*speed_max]) 170 | cla 171 | plot(TOUT,truck_1_speed,'color',c(1,:)) 172 | plot(TOUT,truck_2_speed,'color',c(2,:)) 173 | plot(TOUT,truck_3_speed,'color',c(3,:)) 174 | plot(TOUT,truck_4_speed,'color',c(4,:)) 175 | plot([time(i) time(i)],[0 1.2*speed_max],'k--') 176 | xlabel('Time [s]') 177 | ylabel('Speed [m/s]') 178 | title('Speed') 179 | legend('Truck 1','Truck 2','Truck 3','Truck 4','location','SouthEast') 180 | subplot(3,2,2) 181 | hold on ; grid on 182 | acc_min = min(min([truck_1_acc truck_2_acc truck_3_acc truck_4_acc])); 183 | acc_max = max(max([truck_1_acc truck_2_acc truck_3_acc truck_4_acc])); 184 | set(gca,'xlim',[0 TOUT(end)],'ylim',[1.2*acc_min 1.2*acc_max]) 185 | cla 186 | plot(TOUT,truck_1_acc,'color',c(1,:)) 187 | plot(TOUT,truck_2_acc,'color',c(2,:)) 188 | plot(TOUT,truck_3_acc,'color',c(3,:)) 189 | plot(TOUT,truck_4_acc,'color',c(4,:)) 190 | plot([time(i) time(i)],[1.2*acc_min 1.2*acc_max],'k--') 191 | xlabel('Time [s]') 192 | ylabel('Acceleration [m/s2]') 193 | title('Acceleration') 194 | legend('Truck 1','Truck 2','Truck 3','Truck 4','location','SouthEast') 195 | subplot(3,2,4) 196 | hold on ; grid on 197 | dist_max = max(max([dist_1_2 dist_2_3 dist_3_4])); 198 | set(gca,'xlim',[0 TOUT(end)],'ylim',[0 1.1*dist_max]) 199 | cla 200 | plot(TOUT,dist_1_2,'color',c(2,:)) 201 | plot(TOUT,dist_2_3,'color',c(3,:)) 202 | plot(TOUT,dist_3_4,'color',c(4,:)) 203 | plot([time(i) time(i)],[0 1.2*dist_max],'k--') 204 | xlabel('Time [s]') 205 | ylabel('Distance [m]') 206 | title('Separation Distance') 207 | legend('Trucks 1 & 2','Trucks 2 & 3','Trucks 3 & 4','location','SouthEast') 208 | subplot(3,2,5:6) 209 | hold on ; axis equal 210 | cla 211 | % POSITION AT INSTANT [m] 212 | % Trucks 213 | truck_1_position_inst = truck_1_position(i); 214 | truck_2_position_inst = truck_2_position(i); 215 | truck_3_position_inst = truck_3_position(i); 216 | truck_4_position_inst = truck_4_position(i); 217 | % Car 218 | car_pos_lon_inst = car_pos_lon(i); 219 | car_pos_lat_inst = car_pos_lat(i); 220 | 221 | % ROAD MARKINGS 222 | sideMarkingsX = [truck_1_position_inst-distance_analysis truck_1_position_inst]; 223 | set(gca,'xlim',[truck_1_position_inst-distance_analysis truck_1_position_inst],'ylim',[-trackWidth/2-laneMargin +trackWidth/2+laneMargin]) 224 | set(gca,'XTick',0:20:truck_1_position(end)) 225 | % Central lane left marking 226 | plot(sideMarkingsX,[+laneWidth/2 +laneWidth/2],'k--') 227 | % Central lane right marking 228 | plot(sideMarkingsX,[-laneWidth/2 -laneWidth/2],'k--') 229 | % left marking left lane 230 | plot(sideMarkingsX,[laneWidth+laneWidth/2 laneWidth+laneWidth/2],'k--') 231 | % right marking right lane 232 | plot(sideMarkingsX,[-laneWidth-laneWidth/2 -laneWidth-laneWidth/2],'k--') 233 | 234 | % DIMENSIONS 235 | % Truck 1 236 | truck_1_dimension_X = [truck_1_position_inst truck_1_position_inst truck_1_position_inst-truck_1_length truck_1_position_inst-truck_1_length]; 237 | truck_1_dimension_Y = [+truck_1_width/2 -truck_1_width/2 -truck_1_width/2 +truck_1_width/2]; 238 | % Truck 2 239 | truck_2_dimension_X = [truck_2_position_inst truck_2_position_inst truck_2_position_inst-truck_2_length truck_2_position_inst-truck_2_length]; 240 | truck_2_dimension_Y = [+truck_2_width/2 -truck_2_width/2 -truck_2_width/2 +truck_2_width/2]; 241 | % Truck 3 242 | truck_3_dimension_X = [truck_3_position_inst truck_3_position_inst truck_3_position_inst-truck_3_length truck_3_position_inst-truck_3_length]; 243 | truck_3_dimension_Y = [+truck_3_width/2 -truck_3_width/2 -truck_3_width/2 +truck_3_width/2]; 244 | % Truck 4 245 | truck_4_dimension_X = [truck_4_position_inst truck_4_position_inst truck_4_position_inst-truck_4_length truck_4_position_inst-truck_4_length]; 246 | truck_4_dimension_Y = [+truck_4_width/2 -truck_4_width/2 -truck_4_width/2 +truck_4_width/2]; 247 | % Car 248 | car_dimension_X = [car_pos_lon_inst car_pos_lon_inst car_pos_lon_inst-car_length car_pos_lon_inst-car_length]; 249 | car_dimension_Y = [car_pos_lat_inst+car_width/2 car_pos_lat_inst-car_width/2 car_pos_lat_inst-car_width/2 car_pos_lat_inst+car_width/2]; 250 | 251 | % Plotting trucks 252 | fill(truck_1_dimension_X,truck_1_dimension_Y,c(1,:)) 253 | fill(truck_2_dimension_X,truck_2_dimension_Y,c(2,:)) 254 | fill(truck_3_dimension_X,truck_3_dimension_Y,c(3,:)) 255 | fill(truck_4_dimension_X,truck_4_dimension_Y,c(4,:)) 256 | % Plotting car 257 | car_rear_right_corner_lateral_pos = car_pos_lat_inst - car_width/2; 258 | if car_rear_right_corner_lateral_pos < truck_2_width/2 259 | % Car detected distance sensor 260 | car_color = 'r'; 261 | else 262 | % Car not detected distance sensor 263 | car_color = 'g'; 264 | end 265 | fill(car_dimension_X,car_dimension_Y,car_color) 266 | 267 | xlabel('Lon. distance [m]') 268 | ylabel('Lat. distance [m]') 269 | 270 | frame = getframe(gcf); 271 | writeVideo(v,frame); 272 | 273 | end 274 | 275 | close(v); 276 | 277 | %% Auxiliary functions 278 | 279 | function dz = simulation(t,z,parameters) 280 | % PARAMETERS 281 | % Length 282 | truck_1_length = parameters.truck_1_length; 283 | truck_2_length = parameters.truck_2_length; 284 | truck_3_length = parameters.truck_3_length; 285 | car_length = parameters.car_length; 286 | % width 287 | truck_2_width = parameters.truck_2_width; 288 | 289 | % RETRIEVING STATES 290 | % Truck 1 291 | truck_1_position = z(1); 292 | truck_1_speed = z(2); 293 | truck_1_states = [truck_1_position truck_1_speed]; 294 | % Truck 2 295 | truck_2_position = z(3); 296 | truck_2_speed = z(4); 297 | truck_2_states = [truck_2_position truck_2_speed]; 298 | % Truck 3 299 | truck_3_position = z(5); 300 | truck_3_speed = z(6); 301 | truck_3_states = [truck_3_position truck_3_speed]; 302 | % Truck 4 303 | truck_4_position = z(7); 304 | truck_4_speed = z(8); 305 | truck_4_states = [truck_4_position truck_4_speed]; 306 | % Car 307 | car_pos_lon = z(9); 308 | car_speed = z(10); 309 | car_states = [car_pos_lon car_speed]; 310 | car_pos_lat = z(11); 311 | 312 | % SENSORS 313 | % Truck 2 314 | % Cut in detection between trucks one and two. 315 | car_width = parameters.car_width; 316 | car_rear_right_corner_lateral_pos = car_pos_lat - car_width/2; 317 | if car_rear_right_corner_lateral_pos < truck_2_width/2 318 | % If car in front 319 | Truck_2_sensors.distance_preceding = (car_pos_lon-car_length) - truck_2_position; 320 | Truck_2_sensors.speed_preceding = car_speed; 321 | else 322 | % If truck one in front 323 | Truck_2_sensors.distance_preceding = (truck_1_position-truck_1_length) - truck_2_position; 324 | Truck_2_sensors.speed_preceding = truck_1_speed; 325 | end 326 | % Truck 3 327 | Truck_3_sensors.distance_preceding = (truck_2_position-truck_2_length) - truck_3_position; 328 | Truck_3_sensors.speed_preceding = truck_2_speed; 329 | % Truck 4 330 | Truck_4_sensors.distance_preceding = (truck_3_position-truck_3_length) - truck_4_position; 331 | Truck_4_sensors.speed_preceding = truck_3_speed; 332 | 333 | % DYNAMIC MODELS 334 | % Truck 1 335 | truck_1_derivative_states = truck_model(t,truck_1_states,parameters,1,1); 336 | truck_2_derivative_states = truck_model(t,truck_2_states,parameters,2,Truck_2_sensors); 337 | truck_3_derivative_states = truck_model(t,truck_3_states,parameters,3,Truck_3_sensors); 338 | truck_4_derivative_states = truck_model(t,truck_4_states,parameters,4,Truck_4_sensors); 339 | car_derivative_states = car_model(t,car_states); 340 | 341 | % OUTPUT STATES 342 | % Truck 1 343 | dz(1,1) = truck_1_derivative_states(1,1); 344 | dz(2,1) = truck_1_derivative_states(2,1); 345 | % Truck 2 346 | dz(3,1) = truck_2_derivative_states(1,1); 347 | dz(4,1) = truck_2_derivative_states(2,1); 348 | % Truck 3 349 | dz(5,1) = truck_3_derivative_states(1,1); 350 | dz(6,1) = truck_3_derivative_states(2,1); 351 | % Truck 4 352 | dz(7,1) = truck_4_derivative_states(1,1); 353 | dz(8,1) = truck_4_derivative_states(2,1); 354 | % Car 355 | dz(9,1) = car_derivative_states(1,1); 356 | dz(10,1) = car_derivative_states(2,1); 357 | % Lateral speed car (kinematic model) 358 | if car_pos_lat <= 0 359 | dz(11,1) = 0; 360 | else 361 | dz(11,1) = -0.5; % lateral speed [m/s] 362 | end 363 | 364 | end 365 | 366 | function dstates = truck_model(~,states,parameters,truck_flag,truck_sensors) 367 | % truck_flag indicates the current truck 368 | 369 | % Parameters 370 | m = 40000; % Mass [kg] 371 | g = 9.81; % Gravity [m/s2] 372 | Cd = 0.78; % Drag coefficient [-] 373 | A = 10; % Frontal area [m2] 374 | rho = 1; % Air density [kg/m2] 375 | 376 | % States 377 | % X = states(1); 378 | V = states(2); 379 | 380 | % Drag resistance 381 | C = 0.5*rho*Cd*A; 382 | Dx = C*V^2; 383 | 384 | % Rolling resistance 385 | Rx=0; 386 | 387 | % Gravity force 388 | theta = 0; % Road slope [rad] 389 | Gx = m*g*sin(theta); % [N] 390 | 391 | if truck_flag == 1 392 | % CC 393 | V_r = 20; % Reference speed [m/s] 394 | Kp = 500; % Controller gain 395 | Ft = Kp*(V_r - V) + Dx; % Longitudinal force [N] 396 | else 397 | sensor_distance_preceding = truck_sensors.distance_preceding; 398 | sensor_speed_preceding = truck_sensors.speed_preceding; 399 | % ACC 400 | th = parameters.th; 401 | desired_distance = th*V + 0; 402 | Kp = 10000; 403 | Kd = 10000; 404 | Ft = Kp*(sensor_distance_preceding - desired_distance) + Kd*(sensor_speed_preceding - V) + Dx; 405 | end 406 | 407 | % Vehicle Dynamics 408 | dstates(1,1) = V; 409 | dstates(2,1) = (Ft - Dx - Rx - Gx)/m; 410 | 411 | end 412 | 413 | function dstates = car_model(~,states) 414 | 415 | % Parameters 416 | m = 1500; % Mass [kg] 417 | g = 9.81; % Gravity [m/s2] 418 | Cd = 0.4; % Drag coefficient [-] 419 | A = 2.5; % Frontal area [m2] 420 | rho = 1; % Air density [kg/m2] 421 | 422 | % States 423 | % X = states(1); 424 | V = states(2); 425 | 426 | % Drag resistance 427 | C = 0.5*rho*Cd*A; 428 | Dx = C*V^2; 429 | 430 | % Rolling resistance 431 | Rx=0; 432 | 433 | % Gravity force 434 | theta = 0; % Road slope [rad] 435 | Gx = m*g*sin(theta); % [N] 436 | 437 | % CC 438 | V_r = 20; % Reference speed [m/s] 439 | Kp = 500; % Controller gain 440 | Ft = Kp*(V_r - V) + Dx; % Longitudinal force [N] 441 | 442 | % Vehicle Dynamics 443 | dstates(1,1) = V; 444 | dstates(2,1) = (Ft - Dx - Rx - Gx)/m; 445 | 446 | end 447 | --------------------------------------------------------------------------------