Script_AP2_pricep

clc
clear

% Условия внешней среды
g = 9.8065; % ускорение свободного падения [м/с^2]
Po_w=1.25; % Плотность воздуха [кг/м^3]
% Характеристики тягача
m_1 = 8000; % масса тягача [кг]
L1 = 4.7; % база тягача [м]
B1 = 2.05; % колея тягача [м]
H=3.63; % высота тягача [м]
h_c1 = 1.15; % высота ЦМ тягача [м]
h_OSU = 1.25; % высота крюка [м]
J_z1 = m_1*(L1^2 + B1^2)/12; % момент инерции корпуса [кг*м^2]
n_k1 = 6; % число колес тягача
x_k1 = [1.56 1.56 -1.79 -1.79 -3.14 -3.14]; % координаты колес по оси X
y_k1 = [B1/2 -B1/2 B1/2 -B1/2 B1/2 -B1/2]; % координаты колес по оси Y
x_s1=-1.94; % расстояние от ЦМ тягача до шарнира ОСУ [м]
r_c1 = 0.51; %радиус колеса тягача [м]
J_k1 = 200; % момент инерции колеса тягача [кг*м^2]
F_l=0.9*B1*H; % лобовая площадь тягача [м^2]
C_x=1.1; % коэффициент лобового сопротивления
P_w0=0.5*C_x*Po_w*F_l; % сопротивление воздуха
% Параметры двигателя и трансмиссии
M_engine = [201 276 301 341 361 376 391 407 442 487;
282 534 754 840 980 1086 1186 1266 1306 1401;
361 824 1100 1240 1327 1381 1435 1477 1532 1657;
381 895 1300 1426 1547 1588 1628 1688 1783 1894;
348 859 1336 1525 1743 1849 1944 2025 2084 2210;
242 693 1236 1547 1778 1995 2155 2341 2431 2532;
132 497 1070 1477 1763 2010 2246 2452 2526 2600;
71 402 904 1351 1688 1959 2261 2467 2547 2600;
40 342 753 1211 1592 1904 2191 2407 2522 2587;
10 291 663 1125 1477 1854 2135 2366 2492 2567;
0 226 587 1055 1407 1794 2095 2331 2481 2562;
0 151 532 1000 1346 1733 2045 2291 2467 2552;
0 75 432 939 1246 1643 1994 2241 2446 2532;
0 35 301 844 1185 1547 1914 2185 2417 2507;
0 0 190 703 1090 1467 1829 2115 2385 2487;
0 0 115 613 1025 1402 1748 2020 2347 2462;
0 0 55 557 964 1321 1643 1949 2296 2417;
0 0 15 427 814 1196 1527 1828 2196 2356;
0 0 0 316 708 1080 1442 1718 2130 2281;
0 0 0 211 628 985 1382 1658 2065 2221;
0 0 0 145 503 894 1316 1567 1904 2030;
0 0 0 75 372 734 1176 1385 1547 1618;
0 0 0 0 267 519 845 985 1054 1120
0 0 0 0 0 0 0 0 0 0];
n_en = [550 600 650 700 800 900 1000 1100 1200 1250 1300 1350 1400 1450 1500 1550 1600 1700 1800 1900 2000 2100 2200 2300];
J_d=2.5; % момент инерции двигателя [кг*м^2]
k_cN = 0.93; % коэффициент снимаемой мощности
n_kp=9; % число передач в коробке передач
U_kp=[9.48 6.28 4.68 3.48 2.62 1.89 1.35 1 0.85]; % передаточные числа КП
U_gp=4.1; % передаточное число главной передачи
M_en_max = max(max(M_engine));
% Характеристики сцепления
f_cfr = 0.00033; % Коэф. силы кулоновского трения
k_b = 1.1; % Коэф. изменения силы страгивания
cw = 1.24; % Коэф. перевода
del_w = 0.5; % Пороговое значение разности угловых скоростей
mu = 0.4; % Коэф. трения покоя
D_nakl = 0.450;
d_nakl = 0.240;
S_nakl = 0.94*pi*(D_nakl^2 - d_nakl^2)/4;
r_ef = (D_nakl^3 - d_nakl^3)/(3*(D_nakl^2 - d_nakl^2));
N_sc = 1.1*M_en_max/(r_ef*mu);
% Параметры ОСУ
cc=50000000; %коэффициент жесткости шарнира сцепного устройства
bb=1000000; %коэффициент демпфирования в шарнире
% Характеристики полуприцепа
m_2 =37000; %масса прицепа [кг]
L2 = 13.25; % база полуприцепа [м]
B2 = 2.04; % колея полуприцепа [м]
h_c2 = 1.5; % высота ЦМ полуприцепа [м]
J_z2 = m_2 * (L2^2 + B2^2)/12; % момент инерции корпуса [кг*м^2]
x_k2 = [4.73 4.73 -1.84 -1.84 -3.19 -3.19 -4.54 -4.54]; % координаты колес и ОСУ по оси X [м]
l_k2 = [-1.84 -1.84 -3.19 -3.19 -4.54 -4.54];
y_k2 = [0 0 B2/2 -B2/2 B2/2 -B2/2 B2/2 -B2/2]; % координаты колес по оси Y [м]
y_k02 = [B2/2 -B2/2 B2/2 -B2/2 B2/2 -B2/2];
x_s2=4.73; %расстояние от ЦМ полуприцепа до ОСУ [м]
x_s3=-6.45; %расстояние от ЦМ полуприцепа до СУ [м]
r_c2 = 0.51; %радиус колеса полуприцепа [м]
J_k2 = 200; % момент инерции колеса полуприцепа [кг*м^2]
h_kr = 0.65; % Высота сцепного устройства [м]
% Характеристики прицепа
m_3=15000; % масса прицепа [кг]
L3 = 1.35; %база прицепа
B3 = 2.04; % колея прицепа
h_c3 = 1.5; % высота ЦМ прицепа [м]
J_z3 = m_3 * (L3^2 + B3^2)/12; % момент инерции корпуса [кг*м^2]
n_k3 = 4; % число колес прицепа
x_k3 = [0.675 0.675 -0.675 -0.675]; % координаты колес по оси X [м]
y_k3 = [B3/2 -B3/2 B3/2 -B3/2]; % координаты колес по оси Y [м]
x_s4 = 5.35; %расстояние от ЦМ прицепа до СУ [м]
r_c3 = 0.51; % статический радиус колеса прицепа [м]
J_k3 = 200; % момент инерции колеса прицепа [кг*м^2]

% Матрицы расчёта нормальных реакций на грунт
A_nr1 = zeros(n_k1+2, n_k1+2);
for i = 1:n_k1-1
A_nr1(i, i) = 1;
A_nr1(i, (i+1)) = -1;
A_nr1(i, n_k1+1) = x_k1(i)-x_k1(i+1);
A_nr1(i, n_k1+2) = y_k1(i)-y_k1(i+1);
end
A_nr1(n_k1, 1:n_k1) = 1;
A_nr1(n_k1+1, 1:n_k1) = x_k1;
A_nr1(n_k1+2, 1:n_k1) = y_k1;
A_nr2 = zeros(10, 10);
for i = 1:7
A_nr2(i, i) = 1;
A_nr2(i, (i+1)) = -1;
A_nr2(i, 9) = x_k2(i)-x_k2(i+1);
A_nr2(i, 10) = y_k2(i)-y_k2(i+1);
end
A_nr2(8, 1:8) = 1;
A_nr2(9, 1:8) = x_k2;
A_nr2(10, 1:8) = y_k2;
A_nr3 = zeros(n_k3+2, n_k3+2);
for i = 1:n_k3-1
A_nr3(i, i) = 1;
A_nr3(i, (i+1)) = -1;
A_nr3(i, n_k3+1) = x_k3(i)-x_k3(i+1);
A_nr3(i, n_k3+2) = y_k3(i)-y_k3(i+1);
end
A_nr3(n_k3, 1:n_k3) = 1;
A_nr3(n_k3+1, 1:n_k3) = x_k3;
A_nr3(n_k3+2, 1:n_k3) = y_k3;
% Параметры опорной поверхности
mu_x_max = 0.8; % коэффициент взаимодействия в продольном направлении при полном буксовании
mu_y_max = 0.6; % коэффициент взаимодействия в поперечном направлении при полном юзе
S_0 = 0.09; % 1-ый коэффицент, определяющий форму кривой fi(s)
S_1 = 0.3; % 2-ой коэффицент, определяющий форму кривой fi(s)
f = 0.015; % коэффициент сопротивления качению
alfa = 0*pi/180; % угол наклона опорной поверхности
% Начальные условия
x_0_1 = 0; % начальное положение ЦМ тягача по оси X
y_0_1 = 0; % начальное положение ЦМ тягача по оси Y
V_x_0_1 = 5; % начальная скорость ЦМ тягача по оси X
V_y_0_1 = 0; % начальная скорость ЦМ тягача по оси Y
teta_0 = 0; % начальный угол поворота тягача
omega_z_0_1 = 0; % начальная угловая скорость тягача
omega_k_0 = (V_x_0_1/r_c1); % начальная угловая скорость колес тягача
teta_k0 = [0 0*pi/180; 1.5 0*pi/180; 3.5 0*pi/180; 20 0*pi/180];
omega_d_0 = V_x_0_1*U_gp*U_kp(1)/r_c1;
x_0_2 = x_s1-x_s2; % начальное положение ЦМ полуприцепа по оси X
y_0_2 = 0; % начальное положение ЦМ полуприцепа по оси Y
teta_0_2 = 0; % начальный угол поворота полуприцепа
omega_z_0_2 = 0; % начальная угловая скорость полуприцепа
omega_k_0_2 = (V_x_0_1/r_c2); % начальная угловая скорость колес полуприцепа
teta_r2 = [0 0 0 0 0 0 0]; % углы поворота колес полуприцепа
x_0_3 = x_0_2+x_s3-x_s4; % начальное положение ЦМ прицепа по оси X
teta_r3 = [0 0 0 0 0]; % углы поворота колес прицепа
omega_k_0_3 = (V_x_0_1/r_c3); % начальная угловая скорость колес прицепа
KPD_tr = 0.9;