├── Collision_avoidance.slx ├── Double_lane_change_bezier_func_mcode_V2.m ├── Final_PPT.pptx ├── LateralControlTutorial.slx ├── Lowlevel_longitudinal_model.slx ├── Path_following_Model_w_dbl_lane_change.slx ├── Path_following_Model_w_dbl_lane_change.slx.r2019b ├── Path_following_Model_w_dbl_lane_change.slxc ├── Path_following_SS.m ├── Path_following_sim_file.m ├── Problem_Statement.pdf ├── README.md ├── bezier_curve.m ├── center_points_main.mat ├── helperLateralControlTutorialSetup.m ├── helperPlotRoadAndPath.m ├── longitudinal.m └── longitudinal_model.slx /Collision_avoidance.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Apoorva-Ramani/Autonomy_In_Vehicles/0c93109fe318f633fd48c084b51ebcd165b80d18/Collision_avoidance.slx -------------------------------------------------------------------------------- /Double_lane_change_bezier_func_mcode_V2.m: -------------------------------------------------------------------------------- 1 | %Path following 2 | 3 | %Initialize the variables 4 | Xp = []; 5 | Yp = []; 6 | 7 | load center_points_main.mat 8 | 9 | 10 | x=center_points(:,1); 11 | y=center_points(:,2); 12 | 13 | 14 | %DOUBLE LANE CHANGE MANOEUVER 15 | 16 | Veh_width = 1.57; 17 | L1 = Veh_width*1.1+0.15; 18 | L2 = Veh_width*1.2+0.15; 19 | L3 = Veh_width*1.3+0.15; 20 | L4 = Veh_width*1.4+0.15; 21 | L5 = Veh_width*1.5+0.15; 22 | L6 = Veh_width*1.6+0.15; 23 | L7 = Veh_width*1.7+0.15; 24 | L8 = Veh_width*1.8+0.15; 25 | L9 = Veh_width*1.9+0.15; 26 | L10 = Veh_width*2.0+0.15; 27 | L11 = Veh_width*2.1+0.15; 28 | 29 | p = [x,y]; 30 | 31 | %Change the points here to modify the shape of the curve. 32 | %If you are adding new points, make sure you modify the p & p_ub_pts array 33 | %points accordingly 34 | p_lb_pts = [x,y]; %Points on the curve considering 0,0 starting point and no offset L1,L2,L3 35 | p_ub_pts = p_lb_pts; 36 | p = p_lb_pts; 37 | 38 | %Beizer Curves passes through first and last points and approximates 39 | %through the middle points. This is the characteristic of the Beizer curve 40 | %splitting in 10 41 | p1 = p(1:38,:); 42 | p2 = p(39:93,:); 43 | p3 = p(94:143,:); 44 | p4= p(144:183,:); 45 | p5=p(184:204,:); 46 | p6=p(205:273,:); 47 | p7=p(274:296,:); 48 | p8=p(297:323,:); 49 | p9=p(324:349,:); 50 | p10=p(350:386,:); 51 | 52 | 53 | [Xp_1,Yp_1] = bezier_curve(p1); %For first curve 54 | [Xp_2,Yp_2] = bezier_curve(p2); %For second curve 55 | [Xp_3,Yp_3] = bezier_curve(p3); %For third curve 56 | [Xp_4,Yp_4] = bezier_curve(p4); %For third curve 57 | [Xp_5,Yp_5] = bezier_curve(p5); %For third curve 58 | [Xp_6,Yp_6] = bezier_curve(p6); %For third curve 59 | [Xp_7,Yp_7] = bezier_curve(p7); %For third curve 60 | [Xp_8,Yp_8] = bezier_curve(p8); %For third curve 61 | [Xp_9,Yp_9] = bezier_curve(p9); %For third curve 62 | [Xp_10,Yp_10] = bezier_curve(p10); %For third curve 63 | 64 | 65 | 66 | 67 | Xp = [Xp_1(1:end-1); Xp_2(1:end-1);Xp_3(1:end-1);Xp_4(1:end-1);Xp_5(1:end-1);Xp_6(1:end-1);Xp_7(1:end-1);Xp_8(1:end-1);Xp_9(1:end-1);Xp_10(1:end-1);]; 68 | Yp = [Yp_1(1:end-1);Yp_2(1:end-1);Yp_3(1:end-1);Yp_4(1:end-1);Yp_5(1:end-1);Yp_6(1:end-1);Yp_7(1:end-1);Yp_8(1:end-1);Yp_9(1:end-1);Yp_10(1:end-1);]; 69 | 70 | figure(1) 71 | plot(Xp_1(1:end-1),Yp_1(1:end-1),'Linewidth',2); hold on; 72 | plot(Xp_2(1:end-1),Yp_2(1:end-1),'Linewidth',2); hold on; 73 | plot(Xp_3(1:end-1),Yp_3(1:end-1),'Linewidth',2); hold on; 74 | plot(Xp_4(1:end-1),Yp_4(1:end-1),'Linewidth',2); hold on; 75 | plot(Xp_5(1:end-1),Yp_5(1:end-1),'Linewidth',2); hold on; 76 | plot(Xp_6(1:end-1),Yp_6(1:end-1),'Linewidth',2); hold on; 77 | plot(Xp_7(1:end-1),Yp_7(1:end-1),'Linewidth',2); hold on; 78 | plot(Xp_8(1:end-1),Yp_8(1:end-1),'Linewidth',2); hold on; 79 | plot(Xp_9(1:end-1),Yp_9(1:end-1),'Linewidth',2); hold on; 80 | plot(Xp_10(1:end-1),Yp_10(1:end-1),'Linewidth',2); hold on; 81 | 82 | % 83 | % plot(p(:,1),p(:,2),'k--','Linewidth',2); hold on; %Middle path 84 | % plot(p_ub_pts(:,1),p_ub_pts(:,2),'x'); hold on; %Upper points 85 | % plot(p_lb_pts(:,1),p_lb_pts(:,2),'o'); hold on; %Lower points 86 | 87 | xlabel('X Position [m]') 88 | ylabel('Y Position [m]') 89 | axis([-200 600 -500 500]) 90 | title('Double Lane Change Path') 91 | 92 | %Road Curvature Calculations 93 | 94 | no_of_pts = length(Xp); 95 | rho_ref = zeros(length(Xp)-1,1); %Create a Zero Vector for initialization 96 | 97 | %Compute the Differentiation terms 98 | del_1_Xp = diff(Xp); 99 | del_1_Yp = diff(Yp); 100 | del_2_Xp = diff(Xp,2); %Second order difference 101 | del_2_Yp = diff(Yp,2); %Second order difference 102 | del_pts = 1; %Difference in the index; 103 | 104 | %Curvature Formula given in Lecture 21, Slide 27 105 | rho_new = (((del_1_Xp(2:end)/del_pts).*(del_2_Yp/del_pts.^2))-((del_1_Yp(2:end)/del_pts).*((del_2_Xp/del_pts.^2))))./((del_1_Xp(2:end)./del_pts).^2+((del_1_Yp(2:end)./del_pts).^2)).^1.5; 106 | 107 | %Calculation of total distance travelled for the lookuo table - xaxis input 108 | Tot_d = 0; 109 | for j = 1:no_of_pts-1 110 | d(j) = sqrt((Xp(j+1)-Xp(j))^2+(Yp(j+1)-Yp(j))^2); 111 | Tot_d = Tot_d+d(j); 112 | Cum_dis(j) = Tot_d; %Cumulative distance 113 | end 114 | 115 | dis_vec = Cum_dis(1:end-1); %Distance vector for the lookup table in the model 116 | t_end = Tot_d/V; %for simulation time 117 | -------------------------------------------------------------------------------- /Final_PPT.pptx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Apoorva-Ramani/Autonomy_In_Vehicles/0c93109fe318f633fd48c084b51ebcd165b80d18/Final_PPT.pptx -------------------------------------------------------------------------------- /LateralControlTutorial.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Apoorva-Ramani/Autonomy_In_Vehicles/0c93109fe318f633fd48c084b51ebcd165b80d18/LateralControlTutorial.slx -------------------------------------------------------------------------------- /Lowlevel_longitudinal_model.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Apoorva-Ramani/Autonomy_In_Vehicles/0c93109fe318f633fd48c084b51ebcd165b80d18/Lowlevel_longitudinal_model.slx -------------------------------------------------------------------------------- /Path_following_Model_w_dbl_lane_change.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Apoorva-Ramani/Autonomy_In_Vehicles/0c93109fe318f633fd48c084b51ebcd165b80d18/Path_following_Model_w_dbl_lane_change.slx -------------------------------------------------------------------------------- /Path_following_Model_w_dbl_lane_change.slx.r2019b: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Apoorva-Ramani/Autonomy_In_Vehicles/0c93109fe318f633fd48c084b51ebcd165b80d18/Path_following_Model_w_dbl_lane_change.slx.r2019b -------------------------------------------------------------------------------- /Path_following_Model_w_dbl_lane_change.slxc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Apoorva-Ramani/Autonomy_In_Vehicles/0c93109fe318f633fd48c084b51ebcd165b80d18/Path_following_Model_w_dbl_lane_change.slxc -------------------------------------------------------------------------------- /Path_following_SS.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | %%ECE 5553 - Autonomy in Vehicles 3 | %%HW 4 - Path Following Linear Model 4 | %%Spring 2019 5 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 6 | 7 | %%Vehicle Parameters 8 | L=2.85; % Distance between the axles [m] 9 | g=9.81; % 10 | Lr = 1.513;% Distance from the center of gravity of the vehicle (CG) to the rear axle 11 | Lf = 1.195; % Distance from the center of gravity of the vehicle (CG) to the front axle 12 | Cf = 85275; % Cornering Stiffness of Front Tires 13 | Cr = 68922; % Cornering Stiffness of Rear Tires 14 | m = 1719; %Mass of the vehicle [kg] 15 | J = 3700; %Yaw moment of Inertia 16 | mu = 0.7; %Dry coefficient of Friction 17 | R = 0.3; 18 | Cs=77098; 19 | 20 | %%Operating condition 21 | V = 5; %Longitudinal Velocity [m/s] 22 | ls = 2; %Preview Distance [m] 23 | 24 | %Linear Parameters Calculation 25 | a11 = -(Cr+Cf)/(m*V); 26 | a12 = -1-((Cf*Lf-Cr*Lr)/(m*V^2)); 27 | a21 = (Lr*Cr-Lf*Cf)/J; 28 | a22 = -((Cf*Lf^2)+(Cr*Lr^2))/(V*J); 29 | b11 = Cf/(m*V); %Only Front wheel steering 30 | b12 = 0; %delta_r parameter 31 | b21 = Cf*Lf/J; %Only Front wheel steering 32 | b22 = 0; %delta_r parameter 33 | e2 = 1/J; % For yaw moment term - Not used in Path following model currently 34 | 35 | %%%%State Space Representation 36 | A_matrix = [a11 a12 0 0; a21 a22 0 0; 0 1 0 0; V ls V 0]; 37 | B_matrix = [b11 0; b21 0; 0 -V; 0 -ls*V]; 38 | C_matrix = [1 0 0 0; 0 1 0 0; 0 0 1 0; 0 0 0 1]; %Output is beta, r and ey 39 | D_matrix = [0 0; 0 0; 0 0; 0 0]; 40 | 41 | -------------------------------------------------------------------------------- /Path_following_sim_file.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | %%ECE 5553 - Autonomy in Vehicles 3 | %%HW 4 - Path Following Linear Model 4 | %%Spring 2019 5 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 6 | %%Simulation file 7 | clear all 8 | close all 9 | 10 | %Run the State space model m file 11 | Path_following_SS; 12 | 13 | %Run the Double lane change manoeuver 14 | Double_lane_change_bezier_func_mcode_V2 15 | 16 | sim('Path_following_Model_w_dbl_lane_change'); 17 | 18 | figure(1); 19 | plot(X_ref,Y_ref,'r--','Linewidth',2); hold on;grid on; 20 | plot(X_actual,Y_actual,'b','Linewidth',2); hold on;grid on; 21 | 22 | legend('Reference','Actual') 23 | xlabel('X Position [m]') 24 | ylabel('Y Position [m]') 25 | title('X-Y path') 26 | -------------------------------------------------------------------------------- /Problem_Statement.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Apoorva-Ramani/Autonomy_In_Vehicles/0c93109fe318f633fd48c084b51ebcd165b80d18/Problem_Statement.pdf -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Autonomy_In_Vehicles 2 | Utilized Simulink models in MATLAB, implementing the state-flow decision making, low level longitudinal and lateral control, path planning and following, collision avoidance, localization, as well as perception in autonomous vehicles for Easy Mile EZ10 (Vehicle capacity up to 15 passengers) 3 | -------------------------------------------------------------------------------- /bezier_curve.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | %%ECE 5553 - Autonomy in Vehicles 3 | %%HW 4 - Path Following Linear Model 4 | %%Spring 2019 5 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 6 | 7 | function [Xp,Yp] = bezier_curve(p1) 8 | %%Bezier curve for higher order polynomials% 9 | % https://ocw.mit.edu/courses/electrical-engineering-and-computer-science/6-837-computer-graphics-fall-2012/lecture-notes/MIT6_837F12_Lec01.pdf 10 | % Refer to Slide 62 11 | 12 | % Detailed explanation goes here 13 | p = p1; 14 | 15 | n = length(p); %number of points 16 | n1=n-1; 17 | 18 | for i=0:1:n1 19 | sigma(i+1)=factorial(n1)/(factorial(i)*factorial(n1-i)); % for calculating (x!/(y!(x-y)!)) values 20 | end 21 | l=[]; 22 | UB=[]; 23 | for u=0:0.002:1 24 | for d=1:n 25 | UB(d)=sigma(d)*((1-u)^(n-d))*(u^(d-1)); 26 | end 27 | l=cat(1,l,UB); 28 | end 29 | P=l*p; 30 | 31 | Xp = [P(:,1)]; %X_reference Points 32 | Yp = [P(:,2)]; %Y_reference Points 33 | end 34 | 35 | -------------------------------------------------------------------------------- /center_points_main.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Apoorva-Ramani/Autonomy_In_Vehicles/0c93109fe318f633fd48c084b51ebcd165b80d18/center_points_main.mat -------------------------------------------------------------------------------- /helperLateralControlTutorialSetup.m: -------------------------------------------------------------------------------- 1 | % Set up Script for the Lateral Control Tutorial example. 2 | % 3 | % This script initializes the Lateral Control Tutorial example model. 4 | % It loads necessary control constants and sets up the buses required for 5 | % the model. 6 | % 7 | % This is a helper script for example purposes and may be removed or 8 | % modified in the future. 9 | 10 | % Copyright 2018 The MathWorks, Inc. 11 | 12 | %% Vehicle Parameters 13 | wheelbase = 9.2; % Wheelbase of the vehicle (m) 14 | 15 | %% General Model Parameters 16 | Ts = 0.05; % Simulation sample time (s) 17 | 18 | %% Create scenario and road specifications 19 | [scenario,roadCenters,laneSpecification,speed] = helperCreateDrivingScenario; 20 | 21 | % You can use Driving Scenario Designer to explore the scenario 22 | % drivingScenarioDesigner('LateralControl') 23 | 24 | %% Generate data for Simulink simulation 25 | [refPoses,x0,y0,theta0,curvatures,cumLengths, simStopTime] = helperCreateReferencePath(scenario); 26 | 27 | directions = ones(size(refPoses, 1), 1); 28 | speedProfile = ones(size(refPoses, 1), 1)*speed; 29 | 30 | %% Bus Creation 31 | % Create the bus of actors from the scenario reader 32 | modelName = 'LateralControlTutorial'; 33 | wasModelLoaded = bdIsLoaded(modelName); 34 | if ~wasModelLoaded 35 | load_system(modelName) 36 | end 37 | 38 | % Create buses for lane sensor and lane sensor boundaries 39 | helperCreateLaneSensorBuses; 40 | 41 | 42 | 43 | %scenario1 = drivingScenario('SampleTime',0.5); 44 | %ego = actor(scenario,'Position',[0 0 0],'Yaw',0); 45 | 46 | 47 | %plot(scenario,'Waypoints','on') 48 | %title('Followed Path') 49 | %while advance(scenario1) 50 | % pause(0.1) 51 | % xlim([150 300]) 52 | % ylim([0 150]) 53 | %end 54 | 55 | function helperPlotRoadAndPath(scenario,refPoses) 56 | %helperPlotRoadAndPath Plot the road and the reference path 57 | h = figure('Color','white'); 58 | ax1 = axes(h, 'Box','on'); 59 | plot(scenario,'Parent',ax1) 60 | hold on 61 | plot(ax1,refPoses(:,1),refPoses(:,2),'b') 62 | xlim([150, 300]) 63 | ylim([0 150]) 64 | ax1.Title = text(0.5,0.5,'Road and Reference Path'); 65 | end 66 | 67 | 68 | -------------------------------------------------------------------------------- /helperPlotRoadAndPath.m: -------------------------------------------------------------------------------- 1 | function helperPlotRoadAndPath(scenario,refPoses) 2 | %helperPlotRoadAndPath Plot the road and the reference path 3 | h = figure('Color','white'); 4 | ax1 = axes(h, 'Box','on'); 5 | plot(scenario,'Parent',ax1) 6 | hold on 7 | plot(ax1,refPoses(:,1),refPoses(:,2),'b') 8 | xlim([150, 300]) 9 | ylim([0 150]) 10 | ax1.Title = text(0.5,0.5,'Road and Reference Path'); 11 | end 12 | 13 | -------------------------------------------------------------------------------- /longitudinal.m: -------------------------------------------------------------------------------- 1 | clear all 2 | clc 3 | close all 4 | 5 | %% Parameters 6 | M=2130; % Mass of the vehicle[Kg] 7 | eta_t=0.9; % Transmission Efficiency 8 | lambda_t=1.0; % Gear Ratio 9 | lambda_f=4.1; % Final Drive Ratio 10 | I_w=1; % Inertia of the Wheel[kgm2] 11 | R_w=0.3; % Wheel Radius[m] 12 | Meq=0.1*M; % Equivalent Mass Factor 13 | C_d=0.29; % Drag coefficient 14 | rho=1.225; % Air density[kg/m3] 15 | A_f=2.8;% Frontal area [m2] 16 | g=9.81;% [N/m2] 17 | C_rr=0.015;%Rolling resistance coefficient 18 | C_x=3e5; %Longitudinal Stiffness[N] 19 | C_alpha = 1.5e5; % Cornering Stiffness for 1 tire [N/rad] 20 | L=9.2;% Wheelbase[m] 21 | l_f=1.3;% Distance from the center of gravity of the vehicle (CG) to the front axle [m] 22 | l_r=1.55;% Distance from the center of gravity of the vehicle (CG) to the rear axle [m] 23 | C_s=1.5e5;% Cornering Stiffness of Front and Rear Tires [N/rad] 24 | I_z=3700;% Inertia moment around z axis J or Iz [kg/m2] 25 | mu=0.7; % Road friction coefficient 26 | %% 27 | -------------------------------------------------------------------------------- /longitudinal_model.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Apoorva-Ramani/Autonomy_In_Vehicles/0c93109fe318f633fd48c084b51ebcd165b80d18/longitudinal_model.slx --------------------------------------------------------------------------------