forked from MAIResearchGroup/Paraglider
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Model.m
107 lines (86 loc) · 4.04 KB
/
Model.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
function dy = Model(t, x)
%% Íà÷àëüíûå ïàðàìåòðû
% r = x(1:3); % ïîëîæåíèå
v = x(4:6); % ñêîðîñòü
omega = x(7:9); % óãëîâûå ñêîðîñòè
ean = x(10:12); % Óãëû Ýéëåðà
%TET = x(13);
%% Ïàðàìåòðû âíåøíåé ñðåäû (íåèçìåíÿåìûå)
ro = 1.225; % Ïëîòíîñòü âîçäóõà
g = 9.81; % Óñêîðåíèå ñâîáîäíîãî ïàäåíèÿ
Vw = 5.0; % Ñêîðîñòü íàáåãàþùåãî ïîòîêà
%% Ãàáàðèòû ãðóçà (êóá)
width = 0.1; % Øèðèíà, ì
hight = 0.1; % Âûñîòà, ì
depth = 0.1; % Äëèíà, ì
mass1 = 2.0; % ìàññà ãðóçà, êã
%% Äâèãàòåëü àïïàðàòà
vP = [1; 0; 0]; % âåêòîð íàïðàâëåíèÿ òÿãè
P = 4.45; % òÿãà äâèãàòåëÿ
%% Ãàáàðèòû êðûëà
mass2 = 0.2; % ìàññà êðûëà, êã
lenght = 1.20; % äëèíà ïðîôèëÿ, ì
wide = 0.38; % øèðèíà ïðîôèëÿ, ì
c = wide; % ïîä øèðèíîé áóäåì ïîíèìàòü è ñðåäíþþ õîðäó
% êðûëà
S = lenght*wide; % ïëîùàäü êðûëà, ì^2
angle = deg2rad(2); % Óãîë àòàêè, ðàä
%% Ìàòðèöû ïåðåõîäîâ
% R^u_b
Rub = [ cos(ean(2))*cos(ean(3)), sin(ean(1))*sin(ean(2))*cos(ean(3)) - cos(ean(1))*sin(ean(3)), cos(ean(1))*sin(ean(2))*cos(ean(3)) + sin(ean(1))*sin(ean(3));...
cos(ean(2))*sin(ean(3)), sin(ean(1))*sin(ean(2))*sin(ean(3)) + cos(ean(1))*cos(ean(3)), cos(ean(1))*sin(ean(2))*sin(ean(3)) - sin(ean(1))*cos(ean(3));...
-sin(ean(2)), sin(ean(1))*cos(ean(2)), cos(ean(1))*cos(ean(2))];
% R_ea
Rea = [1, sin(ean(1))*tan(ean(2)), cos(ean(1))*tan(ean(2));...
0, cos(ean(1)), -sin(ean(1));...
0, sin(ean(1))/cos(ean(2)), cos(ean(1))/cos(ean(2))];
%% Ñèëà òÿãè è ìîìåíò
FPPoint = [-0.05; 0; 0]; % ïîëîæåíèå òî÷êè ïðèëîæåíèÿ ñèëû òÿãè
FP = P*vP; % Ñèëà òÿãè
MP = cross(FPPoint, FP); % Ìîìåíò òÿãè
%% Ãðàâèòàöèîííàÿ ñèëà è ìîìåíò
MGPoint = [0; 0; 0]; % Öåíòð ìàññ
TM = mass1 + mass2; % Îáùàÿ ìàññà
FG = Rub\[0; 0; 1]*g*(TM); % Ñèëà òÿæåñòè
MG = cross(MGPoint, FG); % Ìîìåíò ñèëû òÿæåñòè
%% Êîýôèöèåíò âðàùåíèÿ äëÿ êóáà
Jxx = (1/12)*mass1*(width^2 + depth^2);
Jyy = (1/12)*mass1*(hight^2 + depth^2);
Jzz = (1/12)*mass1*(hight^2 + width^2);
J = [Jxx 0 0;
0 Jyy 0;
0 0 Jzz];
%% Àýðîäèíàìèêà
% ×òåíèå äàííûõ ïðîôèëÿ è ïîëó÷åíèå ÀÄ êîýôôèöèåíòîâ
profile = ClarkYH(angle);
Cy = profile(1); % Êîýô. ïîäúåìíîé ñèëû
Cx = profile(2); % Êîýô. òîðìîçÿùåé ñèëû
% Cm = profile(3); % Êîýô. ìîìåíòà ïåðåäíåé êðîìêè, ïîêà íå
% % ÿñíî çà÷åì îí íóæåí
Cd = profile(4); % Êîýô. Öåíòðà äàâëåíèÿ
% MAPoint = [0.1; 2; 0]; % Òî÷êà ïðèëîæåíèÿ ÀÄÑ
% X = [-1; 0; 0] * Cx * ro * (Vw^2 / 2) *S; % Òîðìîçÿùàÿ ñèëà
% Y = [0; 1; 0] * Cy * ro * (Vw^2 / 2) *S; % Ïîäúåìíàÿ ñèëà
% Îáùàÿ ÀÄ ñèëà
FA = ro*S*(Vw^2/2)*[-Cx*cos(angle)+Cy*sin(angle);...
0;...
-Cx*sin(angle)-Cy*cos(angle)];
% MX = cross(MAPoint, X);
% MY = cross(MAPoint, Y);
% Ìîìåíò ÀÄ ñèë
MA = ro*S*(Vw^2/2)*[ 0;...
c*(Cd);...
0];
%% Ñóììà ñèë è ìîìåíòîâ
F = FG + FP + FA;
M = MG + MP + MA;
% V = ((v(1)^2+v(2)^2)^2+v(3)^2);
%% Èíòåãðèðîâàíèå
dr_dt = Rub*v;
dv_dt = cross(omega, v) + (F/TM);
dean_dt = Rea*ean;
domega_dt = J\M - J\cross(omega, J*omega);
% dTET_dt = (P*(sin(angle))+ Y(2) -FG(2)*cos(TET))/((mass1+mass2)*V);
% dH_dt = dv_dt;
dy = [dr_dt; dv_dt; domega_dt; dean_dt];
end