The name of the initial branch for new projects is now "main" instead of "master". Existing projects remain unchanged. More information: https://doku.lrz.de/display/PUBLIC/GitLab

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 % single-track model % ... ... @@ -7,7 +7,6 @@ function x0 = init_KS(initState, p) % % Inputs: % initState - core initial states % p - parameter vector % % Outputs: % x0 - initial state vector ... ... @@ -22,7 +21,7 @@ function x0 = init_KS(initState, p) % Author: Matthias Althoff % Written: 11-January-2017 % Last update: --- % Last update: 15-December-2017 % Last revision:--- %------------- BEGIN CODE -------------- ... ... @@ -41,23 +40,9 @@ function x0 = init_KS(initState, p) %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); delta0 = initState(3); vel0 = initState(4); Psi0 = initState(5); %sprung mass states x0(1) = sx0; % s_x x-position in a global coordinate system ... ... @@ -66,4 +51,4 @@ x0(3) = delta0; % steering angle of front wheels x0(4) = vel0; % velocity x0(5) = Psi0; % Ψ yaw angle %------------- END OF CODE -------------- \ No newline at end of file %------------- END OF CODE --------------
 ... ... @@ -21,7 +21,7 @@ function x0 = init_MB(initState, p) % Author: Matthias Althoff % Written: 11-January-2017 % Last update: --- % Last update: 15-December-2017 % Last revision:--- %------------- BEGIN CODE -------------- ... ... @@ -69,23 +69,15 @@ function x0 = init_MB(initState, p) %obtain initial states from vector sx0 = initState(1); sy0 = initState(2); vel0 = initState(3); Psi0 = initState(4); dotPsi0 = initState(5); beta0 = initState(6); delta0 = initState(3); vel0 = initState(4); Psi0 = initState(5); dotPsi0 = initState(6); beta0 = initState(7); %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); %auxiliary initial states 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 % % Syntax: ... ... @@ -6,7 +6,6 @@ function x0 = init_ST(initState, p) % % Inputs: % initState - core initial states % p - parameter vector % % Outputs: % x0 - initial state vector ... ... @@ -21,52 +20,11 @@ function x0 = init_ST(initState, p) % Author: Matthias Althoff % Written: 11-January-2017 % Last update: --- % Last update: 15-December-2017 % Last revision:--- %------------- BEGIN CODE -------------- %states %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 x0 = initState; %------------- END OF CODE -------------- \ No newline at end of file
 ... ... @@ -10,7 +10,7 @@ function p = parameters_vehicle1() % --- % % Outputs: % p - parameter vector % p - parameter structure % % Example: % ... ... @@ -23,6 +23,7 @@ function p = parameters_vehicle1() % Author: Matthias Althoff % Written: 15-January-2017 % Last update: 05-July-2017 % 15-December-2017 % Last revision:--- %------------- BEGIN CODE -------------- ... ... @@ -31,6 +32,18 @@ function p = parameters_vehicle1() p.l = 4.298; %vehicle length [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 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 ... ...
 ... ... @@ -10,7 +10,7 @@ function p = parameters_vehicle2() % --- % % Outputs: % p - parameter vector % p - parameter structure % % Example: % ... ... @@ -23,6 +23,7 @@ function p = parameters_vehicle2() % Author: Matthias Althoff % Written: 15-January-2017 % Last update: 05-July-2017 % 15-December-2017 % Last revision:--- %------------- BEGIN CODE -------------- ... ... @@ -31,6 +32,18 @@ function p = parameters_vehicle2() p.l = 4.508; %vehicle length [m] (with US bumpers) 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 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 ... ...
 ... ... @@ -10,7 +10,7 @@ function p = parameters_vehicle3() % --- % % Outputs: % p - parameter vector % p - parameter structure % % Example: % ... ... @@ -23,6 +23,7 @@ function p = parameters_vehicle3() % Author: Matthias Althoff % Written: 15-January-2017 % Last update: 05-July-2017 % 15-December-2017 % Last revision:--- %------------- BEGIN CODE -------------- ... ... @@ -31,6 +32,18 @@ function p = parameters_vehicle3() p.l = 4.569; %vehicle length [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 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 ... ...
 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() % Author: Matthias Althoff % Written: 11-January-2017 % Last update: --- % Last update: 16-December-2017 % Last revision:--- %------------- BEGIN CODE -------------- ... ... @@ -33,27 +33,18 @@ g = 9.81; %[m/s^2] tStart = 0; %start time tFinal = 1; %start time % vel0 = 15; % Psi0 = 0.1; % dotPsi0 = -0.1; % beta0 = 0.05; % sy0 = 1; delta0 = 0; vel0 = 15; Psi0 = 0; dotPsi0 = 0; beta0 = 0; sy0 = 0; initialState = [0,sy0,vel0,Psi0,dotPsi0,beta0]; %initial state for simulation x0_KS = init_KS(initialState, p); %initial state for kinematic single-track model x0_ST = init_ST(initialState, p); %initial state for single-track model initialState = [0,sy0,delta0,vel0,Psi0,dotPsi0,beta0]; %initial state for simulation x0_KS = init_KS(initialState); %initial state for kinematic 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 %-------------------------------------------------------------------------- % %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 % stepsizeOptions = odeset('MaxStep',options.tStart-options.tFinal); % opt = odeset(stepsizeOptions); ... ... @@ -90,11 +81,21 @@ plot(t_brake,x_brake(:,9)); v_delta = 0.15; acc = 0.63*g; u = [v_delta acc]; %simulate car %simulate full car [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 hold on 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 plot(t_acc,x_acc(:,4)); 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 tFinal = 1; %start time delta0 = 0; vel0 = 0; Psi0 = 0; dotPsi0 = 0; beta0 = 0; sy0 = 0; initialState = [0,sy0,delta0,vel0,Psi0,dotPsi0,beta0]; %initial state for simulation x0_KS = init_KS(initialState); %initial state for kinematic 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 %-------------------------------------------------------------------------- % set input: rolling car (velocity should stay constant) u = [0 0]; % simulate multi-body model [~,x_roll] = ode45(getfcn(@vehicleDynamics_MB,u,p),[tStart, tFinal],x0_MB); % simulate single-track model [~,x_roll_st] = ode45(getfcn(@vehicleDynamics_ST,u,p),[tStart, tFinal],x0_ST); % simulate kinematic single-track model [~,x_roll_ks] = ode45(getfcn(@vehicleDynamics_KS,u,p),[tStart, tFinal],x0_KS); % check correctness % ground truth x_roll_gt = [ ... 0.0000000000000000, 0.0000000000000000, 0.0000000000000000, 0.0000000000000000, 0.0000000000000000, 0.0000000000000000, -0.0000000003174207, 0.0000000848065981, -0.0013834133396573, -0.0020336367252011, -0.0000000247286655, 0.0176248518072475, 0.0071655470428753, 0.0000000006677358, -0.0000001709775865, 0.0000001839820148, 0.0186763737562366, 0.0003752526345970, 0.0000000006728055, -0.0000001734436431, 0.0000001850020879, 0.0154621865353889, 0.0000251622262094, -0.0000174466440656, -0.0000174466440656, -0.0000014178345014, -0.0000014178345014, 0.0000000008088692, 0.0000000008250785]; % comparison res(end+1) = all(abs(x_roll(end,:) - x_roll_gt) < 1e-14); res(end+1) = all(x_roll_st(end,:) == x0_ST); res(end+1) = all(x_roll_ks(end,:) == x0_KS); %-------------------------------------------------------------------------- % set input: decelerating car --------------------------------------------- v_delta = 0; acc = -0.7*g; u = [v_delta acc]; % simulate multi-body model [~,x_dec] = ode45(getfcn(@vehicleDynamics_MB,u,p),[tStart, tFinal],x0_MB); % simulate single-track model [~,x_dec_st] = ode45(getfcn(@vehicleDynamics_ST,u,p),[tStart, tFinal],x0_ST); % simulate kinematic single-track model [~,x_dec_ks] = ode45(getfcn(@vehicleDynamics_KS,u,p),[tStart, tFinal],x0_KS); % check correctness % ground truth x_dec_gt = [ ... 3.9830932439714242, -0.0601543816394752, 0.0000000000000000, -8.0013986587693893, -0.0026467910011601, -0.0053025639381128, -0.0019453336082831, -0.0002270008481489, -0.0431740570135472, -0.0305313864800172, 0.1053033709671266, 0.0185102262795369, 0.0137681838589760, -0.0003400843778018, -0.0000161129034342, 0.0994502177784091, 0.0256268504637763, 0.0034700280714177, -0.0002562443897593, -0.0000034699487919, 0.1128675292571417, 0.0086968977905411, -0.0020987862166353, -0.0000183158385631, -0.0000183158385631, -0.0000095073736467, -0.0000095073736467, -0.0016872664171374, -0.0012652511246015]; x_dec_st_gt = [ ... -3.4335000000000013, 0.0000000000000000, 0.0000000000000000, -6.8670000000000018, 0.0000000000000000, 0.0000000000000000, 0.0000000000000000]; x_dec_ks_gt = [ ... -3.4335000000000013, 0.0000000000000000, 0.0000000000000000, -6.8670000000000018, 0.0000000000000000]; % comparison res(end+1) = all(abs(x_dec(end,:) - x_dec_gt) < 1e-14); res(end+1) = all(abs(x_dec_st(end,:) - x_dec_st_gt) < 1e-14); res(end+1) = all(abs(x_dec_ks(end,:) - x_dec_ks_gt) < 1e-14); %-------------------------------------------------------------------------- % set input: accelerating car (wheel spin and velocity should increase; more wheel spin at rear) v_delta = 0.15; acc = 0.63*g; u = [v_delta acc]; % simulate multi-body model