diff --git a/MATLAB/vehicleDynamics_ST.m b/MATLAB/vehicleDynamics_ST.m index b139c98e346ac0b1d958c77e9056b13c027055c1..b79f3ea60271dde3716de7b95f35c46d1000e862 100644 --- a/MATLAB/vehicleDynamics_ST.m +++ b/MATLAB/vehicleDynamics_ST.m @@ -1,86 +1,87 @@ -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 -------------- diff --git a/Python/vehicleDynamics_ST.py b/Python/vehicleDynamics_ST.py index 1c2a3de06578d2f3a5c00d759c7f839515a7ac2c..bbcd9a056d7ab034c592ef4b438de826c233c13b 100644 --- a/Python/vehicleDynamics_ST.py +++ b/Python/vehicleDynamics_ST.py @@ -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: diff --git a/vehicleModels_commonRoad.pdf b/vehicleModels_commonRoad.pdf index 9cf06bfc95daf2ceb707b471e4231f4c3e03e99f..d43317c518429fa545a690193fbe25a1d6556cf9 100644 Binary files a/vehicleModels_commonRoad.pdf and b/vehicleModels_commonRoad.pdf differ