Commit 74908ce8 authored by Markus Koschi's avatar 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
......
...@@ -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
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