Commit 33ed3597 authored by MatthiasAlthoff's avatar MatthiasAlthoff

updated to version 2019a

parent b7933eb3
function f = vehicleDynamics_ST(x,u,p) function f = vehicleDynamics_ST(x,u,p)
% vehicleDynamics_ST - single-track vehicle dynamics % vehicleDynamics_ST - single-track vehicle dynamics
% %
% Syntax: % Syntax:
% f = vehicleDynamics_ST(x,u,p) % f = vehicleDynamics_ST(x,u,p)
% %
% Inputs: % Inputs:
% x - vehicle state vector % x - vehicle state vector
% u - vehicle input vector % u - vehicle input vector
% p - vehicle parameter structure % p - vehicle parameter structure
% %
% Outputs: % Outputs:
% f - right-hand side of differential equations % f - right-hand side of differential equations
% %
% Example: % Example:
% %
% Other m-files required: none % Other m-files required: none
% Subfunctions: none % Subfunctions: none
% MAT-files required: none % MAT-files required: none
% %
% See also: --- % See also: ---
% Author: Matthias Althoff % Author: Matthias Althoff
% Written: 12-January-2017 % Written: 12-January-2017
% Last update: 15-December-2017 % Last update: 15-December-2017
% Last revision:--- % 03-September-2019
% Last revision:---
%------------- BEGIN CODE --------------
%------------- BEGIN CODE --------------
%states
%x1 = s_x x-position in a global coordinate system %states
%x2 = s_y y-position in a global coordinate system %x1 = s_x x-position in a global coordinate system
%x3 = δ steering angle of front wheels %x2 = s_y y-position in a global coordinate system
%x4 = u velocity in x-direction %x3 = δ steering angle of front wheels
%x5 = Ψ yaw angle %x4 = u velocity in x-direction
%x6 = Ψ yaw rate %x5 = Ψ yaw angle
%x7 = β slip angle at vehicle center %x6 = Ψ yaw rate
%x7 = β slip angle at vehicle center
%u1 = v_delta steering angle velocity of front wheels
%u2 = ax longitudinal acceleration %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 steering constraints
u(1) = steeringConstraints(x(3),u(1),p.steering);
%consider acceleration constraints
u(2) = accelerationConstraints(x(4),u(2),p.longitudinal); %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 % switch to kinematic model for small velocities
%wheelbase if abs(x(4)) < 0.1
lwb = p.a + p.b; %wheelbase
lwb = p.a + p.b;
%system dynamics
f(1:5,1) = vehicleDynamics_KS(x(1:5),u,p); %system dynamics
f(6,1) = u(2)*lwb*tan(x(3)) + x(4)/(lwb*cos(x(3))^2)*u(1); f(1:5,1) = vehicleDynamics_KS(x(1:5),u,p);
f(7,1) = 0; f(6,1) = u(2)/lwb*tan(x(3)) + x(4)/(lwb*cos(x(3))^2)*u(1);
else f(7,1) = 0;
else
% set gravity constant
g = 9.81; %[m/s^2] % set gravity constant
g = 9.81; %[m/s^2]
%create equivalent bicycle parameters
mu = p.tire.p_dy1; %create equivalent bicycle parameters
C_Sf = -p.tire.p_ky1/p.tire.p_dy1; mu = p.tire.p_dy1;
C_Sr = -p.tire.p_ky1/p.tire.p_dy1; C_Sf = -p.tire.p_ky1/p.tire.p_dy1;
lf = p.a; C_Sr = -p.tire.p_ky1/p.tire.p_dy1;
lr = p.b; lf = p.a;
h = p.h_s; lr = p.b;
m = p.m; h = p.h_s;
I = p.I_z; m = p.m;
I = p.I_z;
%system dynamics
f(1,1) = x(4)*cos(x(7) + x(5)); %system dynamics
f(2,1) = x(4)*sin(x(7) + x(5)); f(1,1) = x(4)*cos(x(7) + x(5));
f(3,1) = u(1); f(2,1) = x(4)*sin(x(7) + x(5));
f(4,1) = u(2); f(3,1) = u(1);
f(5,1) = x(6); f(4,1) = u(2);
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) ... f(5,1) = 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) ... 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))*lf*C_Sf*(g*lr - u(2)*h)*x(3); +mu*m/(I*(lr+lf))*(lr*C_Sr*(g*lf + u(2)*h) - lf*C_Sf*(g*lr - u(2)*h))*x(7) ...
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*m/(I*(lr+lf))*lf*C_Sf*(g*lr - u(2)*h)*x(3);
-mu/(x(4)*(lr+lf))*(C_Sr*(g*lf + u(2)*h) + C_Sf*(g*lr-u(2)*h))*x(7) ... 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_Sf*(g*lr-u(2)*h))*x(3); -mu/(x(4)*(lr+lf))*(C_Sr*(g*lf + u(2)*h) + C_Sf*(g*lr-u(2)*h))*x(7) ...
end +mu/(x(4)*(lr+lf))*(C_Sf*(g*lr-u(2)*h))*x(3);
end
%------------- END OF CODE --------------
\ No newline at end of file %------------- END OF CODE --------------
...@@ -28,7 +28,8 @@ def vehicleDynamics_ST(x,uInit,p): ...@@ -28,7 +28,8 @@ def vehicleDynamics_ST(x,uInit,p):
# Author: Matthias Althoff # Author: Matthias Althoff
# Written: 12-January-2017 # Written: 12-January-2017
# Last update:16-December-2017 # Last update: 16-December-2017
# 03-September-2019
# Last revision:--- # Last revision:---
#------------- BEGIN CODE -------------- #------------- BEGIN CODE --------------
...@@ -73,7 +74,7 @@ def vehicleDynamics_ST(x,uInit,p): ...@@ -73,7 +74,7 @@ def vehicleDynamics_ST(x,uInit,p):
x_ks = [x[0], x[1], x[2], x[3], x[4]] x_ks = [x[0], x[1], x[2], x[3], x[4]]
f_ks = vehicleDynamics_KS(x_ks,u,p) f_ks = vehicleDynamics_KS(x_ks,u,p)
f = [f_ks[0], f_ks[1], f_ks[2], f_ks[3], f_ks[4], 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] 0]
else: 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