Commit 33ed3597 authored by MatthiasAlthoff's avatar MatthiasAlthoff

updated to version 2019a

parent b7933eb3
function f = vehicleDynamics_ST(x,u,p)
% vehicleDynamics_ST - single-track vehicle dynamics
%
% Syntax:
% f = vehicleDynamics_ST(x,u,p)
%
% Inputs:
% x - vehicle state vector
% u - vehicle input vector
% p - vehicle parameter structure
%
% Outputs:
% f - right-hand side of differential equations
%
% Example:
%
% Other m-files required: none
% Subfunctions: none
% MAT-files required: none
%
% See also: ---
% Author: Matthias Althoff
% Written: 12-January-2017
% 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
%consider steering constraints
u(1) = steeringConstraints(x(3),u(1),p.steering);
%consider acceleration constraints
u(2) = accelerationConstraints(x(4),u(2),p.longitudinal);
% switch to kinematic model for small velocities
if abs(x(4)) < 0.1
%wheelbase
lwb = p.a + p.b;
%system dynamics
f(1:5,1) = vehicleDynamics_KS(x(1:5),u,p);
f(6,1) = u(2)*lwb*tan(x(3)) + x(4)/(lwb*cos(x(3))^2)*u(1);
f(7,1) = 0;
else
% set gravity constant
g = 9.81; %[m/s^2]
%create equivalent bicycle parameters
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;
h = p.h_s;
m = p.m;
I = p.I_z;
%system dynamics
f(1,1) = x(4)*cos(x(7) + x(5));
f(2,1) = x(4)*sin(x(7) + x(5));
f(3,1) = u(1);
f(4,1) = u(2);
f(5,1) = x(6);
f(6,1) = -mu*m/(x(4)*I*(lr+lf))*(lf^2*C_Sf*(g*lr-u(2)*h) + lr^2*C_Sr*(g*lf + u(2)*h))*x(6) ...
+mu*m/(I*(lr+lf))*(lr*C_Sr*(g*lf + u(2)*h) - lf*C_Sf*(g*lr - u(2)*h))*x(7) ...
+mu*m/(I*(lr+lf))*lf*C_Sf*(g*lr - u(2)*h)*x(3);
f(7,1) = (mu/(x(4)^2*(lr+lf))*(C_Sr*(g*lf + u(2)*h)*lr - C_Sf*(g*lr - u(2)*h)*lf)-1)*x(6) ...
-mu/(x(4)*(lr+lf))*(C_Sr*(g*lf + u(2)*h) + C_Sf*(g*lr-u(2)*h))*x(7) ...
+mu/(x(4)*(lr+lf))*(C_Sf*(g*lr-u(2)*h))*x(3);
end
%------------- END OF CODE --------------
\ No newline at end of file
function f = vehicleDynamics_ST(x,u,p)
% vehicleDynamics_ST - single-track vehicle dynamics
%
% Syntax:
% f = vehicleDynamics_ST(x,u,p)
%
% Inputs:
% x - vehicle state vector
% u - vehicle input vector
% p - vehicle parameter structure
%
% Outputs:
% f - right-hand side of differential equations
%
% Example:
%
% Other m-files required: none
% Subfunctions: none
% MAT-files required: none
%
% See also: ---
% Author: Matthias Althoff
% Written: 12-January-2017
% Last update: 15-December-2017
% 03-September-2019
% 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
%consider steering constraints
u(1) = steeringConstraints(x(3),u(1),p.steering);
%consider acceleration constraints
u(2) = accelerationConstraints(x(4),u(2),p.longitudinal);
% switch to kinematic model for small velocities
if abs(x(4)) < 0.1
%wheelbase
lwb = p.a + p.b;
%system dynamics
f(1:5,1) = vehicleDynamics_KS(x(1:5),u,p);
f(6,1) = u(2)/lwb*tan(x(3)) + x(4)/(lwb*cos(x(3))^2)*u(1);
f(7,1) = 0;
else
% set gravity constant
g = 9.81; %[m/s^2]
%create equivalent bicycle parameters
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;
h = p.h_s;
m = p.m;
I = p.I_z;
%system dynamics
f(1,1) = x(4)*cos(x(7) + x(5));
f(2,1) = x(4)*sin(x(7) + x(5));
f(3,1) = u(1);
f(4,1) = u(2);
f(5,1) = x(6);
f(6,1) = -mu*m/(x(4)*I*(lr+lf))*(lf^2*C_Sf*(g*lr-u(2)*h) + lr^2*C_Sr*(g*lf + u(2)*h))*x(6) ...
+mu*m/(I*(lr+lf))*(lr*C_Sr*(g*lf + u(2)*h) - lf*C_Sf*(g*lr - u(2)*h))*x(7) ...
+mu*m/(I*(lr+lf))*lf*C_Sf*(g*lr - u(2)*h)*x(3);
f(7,1) = (mu/(x(4)^2*(lr+lf))*(C_Sr*(g*lf + u(2)*h)*lr - C_Sf*(g*lr - u(2)*h)*lf)-1)*x(6) ...
-mu/(x(4)*(lr+lf))*(C_Sr*(g*lf + u(2)*h) + C_Sf*(g*lr-u(2)*h))*x(7) ...
+mu/(x(4)*(lr+lf))*(C_Sf*(g*lr-u(2)*h))*x(3);
end
%------------- END OF CODE --------------
......@@ -28,7 +28,8 @@ def vehicleDynamics_ST(x,uInit,p):
# Author: Matthias Althoff
# Written: 12-January-2017
# Last update:16-December-2017
# Last update: 16-December-2017
# 03-September-2019
# Last revision:---
#------------- BEGIN CODE --------------
......@@ -73,7 +74,7 @@ def vehicleDynamics_ST(x,uInit,p):
x_ks = [x[0], x[1], x[2], x[3], x[4]]
f_ks = vehicleDynamics_KS(x_ks,u,p)
f = [f_ks[0], f_ks[1], f_ks[2], f_ks[3], f_ks[4],
u[1]*lwb*math.tan(x[2]) + x[3]/(lwb*math.cos(x[2])**2)*u[0],
u[1]/lwb*math.tan(x[2]) + x[3]/(lwb*math.cos(x[2])**2)*u[0],
0]
else:
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment