Commit 74908ce8 by Markus Koschi

### update to Release 2018a

parent fc628bd0
 function acceleration = accelerationConstraints(velocity,acceleration,p) % accelerationConstraints - adjusts the acceleration based on acceleration % constraints % % Syntax: % acceleration = accelerationConstraints(velocity,acceleration,p) % % Inputs: % acceleration - acceleration in driving direction % velocity - velocity in driving direction % p - longitudinal parameter structure % % Outputs: % acceleration - acceleration in driving direction % % Example: % % Other m-files required: none % Subfunctions: none % MAT-files required: none % % See also: --- % Author: Matthias Althoff % Written: 15-December-2017 % Last update: --- % Last revision:--- %------------- BEGIN CODE -------------- %positive acceleration limit if velocity>p.v_switch posLimit = p.a_max*p.v_switch/velocity; else posLimit = p.a_max; end %acceleration limit reached? if (velocity<=p.v_min && acceleration<=0) || (velocity>=p.v_max && acceleration>=0) acceleration = 0; elseif acceleration<=-p.a_max acceleration = -p.a_max; elseif acceleration>=posLimit acceleration = posLimit; end %------------- END OF CODE --------------
 function x0 = init_KS(initState, p) function x0 = init_KS(initState) % init_KS - generates the initial state vector for the kinematic % init_KS - generates the initial state vector for the kinematic % single-track model % single-track model % % ... @@ -7,7 +7,6 @@ function x0 = init_KS(initState, p) ... @@ -7,7 +7,6 @@ function x0 = init_KS(initState, p) % % % Inputs: % Inputs: % initState - core initial states % initState - core initial states % p - parameter vector % % % Outputs: % Outputs: % x0 - initial state vector % x0 - initial state vector ... @@ -22,7 +21,7 @@ function x0 = init_KS(initState, p) ... @@ -22,7 +21,7 @@ function x0 = init_KS(initState, p) % Author: Matthias Althoff % Author: Matthias Althoff % Written: 11-January-2017 % Written: 11-January-2017 % Last update: --- % Last update: 15-December-2017 % Last revision:--- % Last revision:--- %------------- BEGIN CODE -------------- %------------- BEGIN CODE -------------- ... @@ -41,23 +40,9 @@ function x0 = init_KS(initState, p) ... @@ -41,23 +40,9 @@ function x0 = init_KS(initState, p) %obtain initial states from vector %obtain initial states from vector sx0 = initState(1); sx0 = initState(1); sy0 = initState(2); sy0 = initState(2); vel0 = initState(3); delta0 = initState(3); Psi0 = initState(4); vel0 = initState(4); dotPsi0 = initState(5); Psi0 = initState(5); beta0 = initState(6); %create equivalent bicycle parameters g = 9.81; %[m/s^2] mu = p.tire.p_dy1; C_Sf = -p.tire.p_ky1/p.tire.p_dy1; C_Sr = -p.tire.p_ky1/p.tire.p_dy1; lf = p.a; lr = p.b; %initial steering angle from steady state of slip angle delta0 = vel0*(lf + lr)/(C_Sf*g*lr*mu)*dotPsi0 ... + 1/(C_Sf*lr)*((C_Sr*lf + C_Sf*lr)*beta0 - (C_Sr - C_Sf)*lr*lf*dotPsi0/vel0); %sprung mass states %sprung mass states x0(1) = sx0; % s_x x-position in a global coordinate system x0(1) = sx0; % s_x x-position in a global coordinate system ... @@ -66,4 +51,4 @@ x0(3) = delta0; % steering angle of front wheels ... @@ -66,4 +51,4 @@ x0(3) = delta0; % steering angle of front wheels x0(4) = vel0; % velocity x0(4) = vel0; % velocity x0(5) = Psi0; % Ψ yaw angle x0(5) = Psi0; % Ψ yaw angle %------------- END OF CODE -------------- %------------- END OF CODE -------------- \ No newline at end of file
 ... @@ -21,7 +21,7 @@ function x0 = init_MB(initState, p) ... @@ -21,7 +21,7 @@ function x0 = init_MB(initState, p) % Author: Matthias Althoff % Author: Matthias Althoff % Written: 11-January-2017 % Written: 11-January-2017 % Last update: --- % Last update: 15-December-2017 % Last revision:--- % Last revision:--- %------------- BEGIN CODE -------------- %------------- BEGIN CODE -------------- ... @@ -69,23 +69,15 @@ function x0 = init_MB(initState, p) ... @@ -69,23 +69,15 @@ function x0 = init_MB(initState, p) %obtain initial states from vector %obtain initial states from vector sx0 = initState(1); sx0 = initState(1); sy0 = initState(2); sy0 = initState(2); vel0 = initState(3); delta0 = initState(3); Psi0 = initState(4); vel0 = initState(4); dotPsi0 = initState(5); Psi0 = initState(5); beta0 = initState(6); dotPsi0 = initState(6); beta0 = initState(7); %create equivalent bicycle parameters %create equivalent bicycle parameters g = 9.81; %[m/s^2] g = 9.81; %[m/s^2] mu = p.tire.p_dy1; C_Sf = -p.tire.p_ky1/p.tire.p_dy1; C_Sr = -p.tire.p_ky1/p.tire.p_dy1; lf = p.a; lr = p.b; %initial steering angle from steady state of slip angle delta0 = vel0*(lf + lr)/(C_Sf*g*lr*mu)*dotPsi0 ... + 1/(C_Sf*lr)*((C_Sr*lf + C_Sf*lr)*beta0 - (C_Sr - C_Sf)*lr*lf*dotPsi0/vel0); %auxiliary initial states %auxiliary initial states F0_z_f = p.m_s*g*p.b/((p.a + p.b)) + p.m_uf*g; F0_z_f = p.m_s*g*p.b/((p.a + p.b)) + p.m_uf*g; ... ...
 function x0 = init_ST(initState, p) function x0 = init_ST(initState) % init_MB - generates the initial state vector for the single-track model % init_MB - generates the initial state vector for the single-track model % % % Syntax: % Syntax: ... @@ -6,7 +6,6 @@ function x0 = init_ST(initState, p) ... @@ -6,7 +6,6 @@ function x0 = init_ST(initState, p) % % % Inputs: % Inputs: % initState - core initial states % initState - core initial states % p - parameter vector % % % Outputs: % Outputs: % x0 - initial state vector % x0 - initial state vector ... @@ -21,52 +20,11 @@ function x0 = init_ST(initState, p) ... @@ -21,52 +20,11 @@ function x0 = init_ST(initState, p) % Author: Matthias Althoff % Author: Matthias Althoff % Written: 11-January-2017 % Written: 11-January-2017 % Last update: --- % Last update: 15-December-2017 % Last revision:--- % Last revision:--- %------------- BEGIN CODE -------------- %------------- BEGIN CODE -------------- %states x0 = initState; %x1 = s_x x-position in a global coordinate system %x2 = s_y y-position in a global coordinate system %x3 = δ steering angle of front wheels %x4 = u velocity in x-direction %x5 = Ψ yaw angle %x6 = Ψ yaw rate %x7 = β slip angle at vehicle center %u1 = v_delta steering angle velocity of front wheels %u2 = ax longitudinal acceleration %obtain initial states from vector sx0 = initState(1); sy0 = initState(2); vel0 = initState(3); Psi0 = initState(4); dotPsi0 = initState(5); beta0 = initState(6); %create equivalent bicycle parameters g = 9.81; %[m/s^2] mu = p.tire.p_dy1; C_Sf = -p.tire.p_ky1/p.tire.p_dy1; C_Sr = -p.tire.p_ky1/p.tire.p_dy1; lf = p.a; lr = p.b; %initial steering angle from steady state of slip angle delta0 = vel0*(lf + lr)/(C_Sf*g*lr*mu)*dotPsi0 ... + 1/(C_Sf*lr)*((C_Sr*lf + C_Sf*lr)*beta0 - (C_Sr - C_Sf)*lr*lf*dotPsi0/vel0); %sprung mass states x0(1) = sx0; % s_x x-position in a global coordinate system x0(2) = sy0; % s_y y-position in a global coordinate system x0(3) = delta0; % steering angle of front wheels x0(4) = vel0; % velocity x0(5) = Psi0; % Ψ yaw angle x0(6) = dotPsi0; % Ψ yaw rate x0(7) = beta0; % β slip angle at vehicle center %------------- END OF CODE -------------- %------------- END OF CODE -------------- \ No newline at end of file
 ... @@ -10,7 +10,7 @@ function p = parameters_vehicle1() ... @@ -10,7 +10,7 @@ function p = parameters_vehicle1() % --- % --- % % % Outputs: % Outputs: % p - parameter vector % p - parameter structure % % % Example: % Example: % % ... @@ -23,6 +23,7 @@ function p = parameters_vehicle1() ... @@ -23,6 +23,7 @@ function p = parameters_vehicle1() % Author: Matthias Althoff % Author: Matthias Althoff % Written: 15-January-2017 % Written: 15-January-2017 % Last update: 05-July-2017 % Last update: 05-July-2017 % 15-December-2017 % Last revision:--- % Last revision:--- %------------- BEGIN CODE -------------- %------------- BEGIN CODE -------------- ... @@ -31,6 +32,18 @@ function p = parameters_vehicle1() ... @@ -31,6 +32,18 @@ function p = parameters_vehicle1() p.l = 4.298; %vehicle length [m] p.l = 4.298; %vehicle length [m] p.w = 1.674; %vehicle width [m] p.w = 1.674; %vehicle width [m] %steering constraints p.steering.min = -0.910; %minimum steering angle [rad] p.steering.max = 0.910; %maximum steering angle [rad] p.steering.v_min = -0.4; %minimum steering velocity [rad/s] p.steering.v_max = 0.4; %maximum steering velocity [rad/s] %longitudinal constraints p.longitudinal.v_min = -13.9; %minimum velocity [m/s] p.longitudinal.v_max = 45.8; %minimum velocity [m/s] p.longitudinal.v_switch = 4.755; %switching velocity [m/s] p.longitudinal.a_max = 11.5; %maximum absolute acceleration [m/s^2] %masses %masses p.m = lb_sec2_ft_IN_kg(84); %vehicle mass [kg]; MASS p.m = lb_sec2_ft_IN_kg(84); %vehicle mass [kg]; MASS p.m_s = lb_sec2_ft_IN_kg(75); %sprung mass [kg]; SMASS p.m_s = lb_sec2_ft_IN_kg(75); %sprung mass [kg]; SMASS ... ...
 ... @@ -10,7 +10,7 @@ function p = parameters_vehicle2() ... @@ -10,7 +10,7 @@ function p = parameters_vehicle2() % --- % --- % % % Outputs: % Outputs: % p - parameter vector % p - parameter structure % % % Example: % Example: % % ... @@ -23,6 +23,7 @@ function p = parameters_vehicle2() ... @@ -23,6 +23,7 @@ function p = parameters_vehicle2() % Author: Matthias Althoff % Author: Matthias Althoff % Written: 15-January-2017 % Written: 15-January-2017 % Last update: 05-July-2017 % Last update: 05-July-2017 % 15-December-2017 % Last revision:--- % Last revision:--- %------------- BEGIN CODE -------------- %------------- BEGIN CODE -------------- ... @@ -31,6 +32,18 @@ function p = parameters_vehicle2() ... @@ -31,6 +32,18 @@ function p = parameters_vehicle2() p.l = 4.508; %vehicle length [m] (with US bumpers) p.l = 4.508; %vehicle length [m] (with US bumpers) p.w = 1.610; %vehicle width [m] p.w = 1.610; %vehicle width [m] %steering constraints p.steering.min = -1.066; %minimum steering angle [rad] p.steering.max = 1.066; %maximum steering angle [rad] p.steering.v_min = -0.4; %minimum steering velocity [rad/s] p.steering.v_max = 0.4; %maximum steering velocity [rad/s] %longitudinal constraints p.longitudinal.v_min = -13.6; %minimum velocity [m/s] p.longitudinal.v_max = 50.8; %minimum velocity [m/s] p.longitudinal.v_switch = 7.319; %switching velocity [m/s] p.longitudinal.a_max = 11.5; %maximum absolute acceleration [m/s^2] %masses %masses p.m = lb_sec2_ft_IN_kg(74.91452); %vehicle mass [kg]; MASS p.m = lb_sec2_ft_IN_kg(74.91452); %vehicle mass [kg]; MASS p.m_s = lb_sec2_ft_IN_kg(66.17221); %sprung mass [kg]; SMASS p.m_s = lb_sec2_ft_IN_kg(66.17221); %sprung mass [kg]; SMASS ... ...
 ... @@ -10,7 +10,7 @@ function p = parameters_vehicle3() ... @@ -10,7 +10,7 @@ function p = parameters_vehicle3() % --- % --- % % % Outputs: % Outputs: % p - parameter vector % p - parameter structure % % % Example: % Example: % % ... @@ -23,6 +23,7 @@ function p = parameters_vehicle3() ... @@ -23,6 +23,7 @@ function p = parameters_vehicle3() % Author: Matthias Althoff % Author: Matthias Althoff % Written: 15-January-2017 % Written: 15-January-2017 % Last update: 05-July-2017 % Last update: 05-July-2017 % 15-December-2017 % Last revision:--- % Last revision:--- %------------- BEGIN CODE -------------- %------------- BEGIN CODE -------------- ... @@ -31,6 +32,18 @@ function p = parameters_vehicle3() ... @@ -31,6 +32,18 @@ function p = parameters_vehicle3() p.l = 4.569; %vehicle length [m] p.l = 4.569; %vehicle length [m] p.w = 1.844; %vehicle width [m] p.w = 1.844; %vehicle width [m] %steering constraints p.steering.min = -1.023; %minimum steering angle [rad] p.steering.max = 1.023; %maximum steering angle [rad] p.steering.v_min = -0.4; %minimum steering velocity [rad/s] p.steering.v_max = 0.4; %maximum steering velocity [rad/s] %longitudinal constraints p.longitudinal.v_min = -11.2; %minimum velocity [m/s] p.longitudinal.v_max = 41.7; %minimum velocity [m/s] p.longitudinal.v_switch = 7.824; %switching velocity [m/s] p.longitudinal.a_max = 11.5; %maximum absolute acceleration [m/s^2] %masses %masses p.m = lb_sec2_ft_IN_kg(101.3367); %vehicle mass [kg]; MASS p.m = lb_sec2_ft_IN_kg(101.3367); %vehicle mass [kg]; MASS p.m_s = lb_sec2_ft_IN_kg(90.21635); %sprung mass [kg]; SMASS p.m_s = lb_sec2_ft_IN_kg(90.21635); %sprung mass [kg]; SMASS ... ...
 function steeringVelocity = steeringConstraints(steeringAngle,steeringVelocity,p) % steeringConstraints - adjusts the steering velocity based on steering % constraints % % Syntax: % steeringVelocity = steeringConstraints(steeringAngle,steeringVelocity,p) % % Inputs: % steeringAngle - steering angle % steeringVelocity - steering velocity % p - steering parameter structure % % Outputs: % steeringVelocity - steering velocity % % Example: % % Other m-files required: none % Subfunctions: none % MAT-files required: none % % See also: --- % Author: Matthias Althoff % Written: 15-December-2017 % Last update: --- % Last revision:--- %------------- BEGIN CODE -------------- %steering limit reached? if (steeringAngle<=p.min && steeringVelocity<=0) || (steeringAngle>=p.max && steeringVelocity>=0) steeringVelocity = 0; elseif steeringVelocity<=p.v_min steeringVelocity = p.v_min; elseif steeringVelocity>=p.v_max steeringVelocity = p.v_max; end %------------- END OF CODE --------------
 ... @@ -20,7 +20,7 @@ function testVehicle() ... @@ -20,7 +20,7 @@ function testVehicle() % Author: Matthias Althoff % Author: Matthias Althoff % Written: 11-January-2017 % Written: 11-January-2017 % Last update: --- % Last update: 16-December-2017 % Last revision:--- % Last revision:--- %------------- BEGIN CODE -------------- %------------- BEGIN CODE -------------- ... @@ -33,27 +33,18 @@ g = 9.81; %[m/s^2] ... @@ -33,27 +33,18 @@ g = 9.81; %[m/s^2] tStart = 0; %start time tStart = 0; %start time tFinal = 1; %start time tFinal = 1; %start time % vel0 = 15; delta0 = 0; % Psi0 = 0.1; % dotPsi0 = -0.1; % beta0 = 0.05; % sy0 = 1; vel0 = 15; vel0 = 15; Psi0 = 0; Psi0 = 0; dotPsi0 = 0; dotPsi0 = 0; beta0 = 0; beta0 = 0; sy0 = 0; sy0 = 0; initialState = [0,sy0,vel0,Psi0,dotPsi0,beta0]; %initial state for simulation initialState = [0,sy0,delta0,vel0,Psi0,dotPsi0,beta0]; %initial state for simulation x0_KS = init_KS(initialState, p); %initial state for kinematic single-track model x0_KS = init_KS(initialState); %initial state for kinematic single-track model x0_ST = init_ST(initialState, p); %initial state for single-track model x0_ST = init_ST(initialState); %initial state for single-track model x0_MB = init_MB(initialState, p); %initial state for multi-body model x0_MB = init_MB(initialState, p); %initial state for multi-body model %-------------------------------------------------------------------------- %-------------------------------------------------------------------------- % %specify continuous dynamics----------------------------------------------- % carDyn = vehicleSys('vehicleDynamics',28,2,@vehicleDynamics_MB,[]); %initialize car % carDyn_bicycle = vehicleSys('vehicleDynamics_bicycle',6,2,@DOTBicycleDynamics,[]); %initialize car % %-------------------------------------------------------------------------- % %generate ode options % %generate ode options % stepsizeOptions = odeset('MaxStep',options.tStart-options.tFinal); % stepsizeOptions = odeset('MaxStep',options.tStart-options.tFinal); % opt = odeset(stepsizeOptions); % opt = odeset(stepsizeOptions); ... @@ -90,11 +81,21 @@ plot(t_brake,x_brake(:,9)); ... @@ -90,11 +81,21 @@ plot(t_brake,x_brake(:,9)); v_delta = 0.15; v_delta = 0.15; acc = 0.63*g; acc = 0.63*g; u = [v_delta acc]; u = [v_delta acc]; %simulate car %simulate full car [t_acc,x_acc] = ode45(getfcn(@vehicleDynamics_MB,u,p),[tStart, tFinal],x0_MB); [t_acc,x_acc] = ode45(getfcn(@vehicleDynamics_MB,u,p),[tStart, tFinal],x0_MB); %simulate single-track model [~,x_acc_st] = ode45(getfcn(@vehicleDynamics_ST,u,p),[tStart, tFinal],x0_ST); %simulate kinematic single-track model [~,x_acc_ks] = ode45(getfcn(@vehicleDynamics_KS,u,p),[tStart, tFinal],x0_KS); figure %position figure %position hold on hold on plot(x_acc(:,1),x_acc(:,2)) plot(x_acc(:,1),x_acc(:,2)) plot(x_acc_st(:,1),x_acc_st(:,2)) plot(x_acc_ks(:,1),x_acc_ks(:,2)) figure % velocity figure % velocity plot(t_acc,x_acc(:,4)); plot(t_acc,x_acc(:,4)); figure % wheel spin figure % wheel spin ... ...
 function res = test_derivatives() % test_derivatives - unit_test_function for checking whether the derivative % is computed correctly % % Syntax: % res = test_derivatives() % % Inputs: % --- % % Outputs: % res - boolean result (0/empty: test not passed, 1: test passed) % % Example: % % Other m-files required: none % Subfunctions: none % MAT-files required: none % % See also: --- % Author: Matthias Althoff % Written: 17-December-2017 % Last update: --- % Last revision:--- %------------- BEGIN CODE -------------- % initialize result res = []; % load parameters p = parameters_vehicle2; g = 9.81; %[m/s^2] % set state values x_ks = [ ... 3.9579422297936526, 0.0391650102771405, 0.0378491427211811, 16.3546957860883566, 0.0294717351052816]; x_st = [ ... 2.0233348142065677, 0.0041907137716636, 0.0197545248559617, 15.7216236334290116, 0.0025857914776859, 0.0529001056654038, 0.0033012170610298]; x_mb = [ ... 10.8808433066274794, 0.5371850187869442, 0.0980442671005920, 18.0711398687457745, 0.1649995631003776, 0.6158755000936103, -0.1198403612262477, -0.2762672756169581, 0.0131909920269115, -0.0718483683742141, -0.3428324595054725, 0.0103233373083297, -0.0399590564140291, -0.0246468320579360, -0.0551575051990853, 0.5798277643297529, 0.0172059354801703, 0.0067890113155477, -0.0184269459410162, -0.0408207136116175, -1.0064484829203018, 0.0166808347900582, -0.0049188492004049, 53.6551710641082451, 50.7045242506744316, 56.7917911784219598, 200.7079633169796296, -0.0939969123691911, -0.0881514621614376]; % set input: (accelerating and steering) v_delta = 0.15; acc = 0.63*g; u = [v_delta acc]; % insert into vehicle dynamics to obtain derivatives f_ks = vehicleDynamics_KS(x_ks,u,p); f_st = vehicleDynamics_ST(x_st,u,p); f_mb = vehicleDynamics_MB(x_mb,u,p); % ground truth f_ks_gt = [16.3475935934250209, 0.4819314886013121, 0.1500000000000000, 5.1464424102339752, 0.2401426578627629]; f_st_gt = [15.7213512030862397, 0.0925527979719355, 0.1500000000000000, 5.3536773276413925, 0.0529001056654038, 0.6435589397748606, 0.0313297971641291]; f_mb_gt = [17.8820162482414098, 2.6300428035858809, 0.1500000000000000, 2.7009396644636450, 0.6158755000936103, 1.3132879472301846, -0.2762672756169581, -0.1360581472638375, -0.0718483683742141, 0.4909514227532223, -2.6454134031927374, -0.0399590564140291, 0.0486778649724968, -0.0551575051990853, 0.0354501802049087, -1.1130397141873534, 0.0067890113155477, -0.0886810139593130, -0.0408207136116175, 0.0427680029698811, -4.3436374104751501, -0.0049188492004049, 0.1142109377736169, 10.0527321757776047, 0.0436512393438736, 14.3044528684659404, 590.5602192640882322, -0.2105876149500405, -0.2126005780977984]; % comparison res(end+1) = all(abs(f_ks' - f_ks_gt) < 1e-14); res(end+1) = all(abs(f_st' - f_st_gt) < 1e-14); res(end+1) = all(abs(f_mb' - f_mb_gt) < 1e-14); %-------------------------------------------------------------------------- % obtain final result res = all(res); %------------- END OF CODE --------------
 function res = test_zeroInitialVelocity() % test_zeroInitialVelocity - unit_test_function for starting with zero % initial velocity % % Some vehicle models have a singularity when the vehicle is not moving. % This test checks whether the vehicles can handle this situation % % Syntax: % res = test_zeroInitialVelocity() % % Inputs: % --- % % Outputs: % res - boolean result (0/empty: test not passed, 1: test passed) % % Example: % % Other m-files required: none % Subfunctions: none % MAT-files required: none % % See also: --- % Author: Matthias Althoff % Written: 15-December-2017 % Last update: --- % Last revision:--- %------------- BEGIN CODE -------------- % initialize result res = []; % load parameters p = parameters_vehicle2; g = 9.81; %[m/s^2] % set options -------------------------------------------------------------- tStart = 0; %start time