Кинематические уравнения поступательного движения

vg={vgx[t], vgy[t], vgz[t]}

r={l[t], h[t], z[t]}

kur1=TableForm[D[r[[1]],t]==vg[[1]]]

kur2=TableForm[D[r[[2]],t]==vg[[2]]]

kur3=TableForm[D[r[[3]],t]==vg[[3]]]

pr={kur1, kur2, kur3}

TableForm[m1={{Cos[θ[t]],-Sin[θ[t]],0},{Sin[θ[t]],Cos[θ[t]],0},{0,0,1}}]

TableForm[m2={{Cos[ψ[t]], 0, Sin[ψ[t]]}, {0, 1, 0}, {-Sin[ψ[t]], 0, Cos[ψ[t]]}}]

TableForm[m3=m2.m1]

TableForm[ReplaceAll[m3, {θ[t]->0, ψ[t]->0}]]

rg=m3.vg

q1=D[r[[1]], t]==rg[[1]]

q2=D[r[[2]], t]==rg[[2]]

q3=D[r[[3]], t]==rg[[3]]

TableForm[{q1, q2, q3}]

Динамические уравнения поступательного движения

v={vx[t], vy[t], vz[t]}

w={wx[t], wy[t], wz[t]}

(*Матрица перехода от связанной системы координат к нормальной системе координат*)

TableForm[mm1={{Cos[psi[t]], 0, Sin[psi[t]]}, {0, 1, 0}, {-Sin[psi[t]], 0, Cos[psi[t]]}}]

TableForm[mm2={{Cos[teta[t]], -Sin[teta[t]], 0}, {Sin[teta[t]], Cos[teta[t]], 0}, {0, 0, 1}}]

TableForm[mm3={{1,0,0},{0,Cos[gam[t]],-Sin[gam[t]]},{0,Sin[gam[t]],Cos[gam[t]]}}]

TableForm[mm4=Transpose[mm3.mm2.mm1]]

(*Матрица перехода от скоростной системы координат к связанной системе координат*)

TableForm[sm1={{Cos[alfa[t]],-Sin[alfa[t]],0},{Sin[alfa[t]],Cos[alfa[t]],0},{0,0,1}}]

TableForm[sm2={{Cos[beta[t]],0,Sin[beta[t]]},{0,1,0},{-Sin[beta[t]],0,Cos[beta[t]]}}]

TableForm[sm3=Transpose[sm2.sm1]]

Cy[alfa[t],mah[t],ugstrel,dzak[t],dh[t]]=D[Cy[alfa[t],mah[t],ugstrel,dzak[t],dv[t]],alfa[t]]*(alfa[t]-alfa0) (*Коэффициент подъёмной силы*)

lyam=(l^2)/S (*Относительное удлинение крыла*)

Cx=Cx0+(Cy[alfa[t],mah[t],ugstrel,dzak[t],dv[t]]^2)/(lyam*Pi) (*Коэффициент силы лобового сопротивления*)

q=(ro[h[t]]*vk^2)/2 (*Скоростной напор*)

faer={-Cx*q*s,Cy[alfa[t],mah[t],ugstrel,dzak[t],dv[t]]*q*s,Cz[beta[t],dh[t]]*q*s} (*Аэродинамическая сила в скоростной системе координат*)

TableForm[faersv=sm3.faer](*Аэродинамическая сила в связанной системе координат*)

TableForm[gs=Transpose[mm4].{0,-g,0}] (*Сила тяжести в связанной системе координат*)

fx=faersv[[1]]+gs[[1]]+p[dsg[t],v,h[t]]*Cos[phi]

fy=faersv[[2]]+gs[[2]]+p[dsg[t],vk[t],h[t]]*Sin[phi]

fz=faersv[[3]]+gs[[3]]

f={fx,fy,fz}

f1={fx1[dv[t],dh[t],de[t],dsg[t]], fy1[dv[t],dh[t],de[t],dsg[t]], fz1[dv[t],dh[t],de[t],dsg[t]]}

d=m*(D[v, t]+Cross[w, v])

q4=d[[1]]==f[[1]]

q5=d[[2]]==f[[2]]

q6=d[[3]]==f[[3]]

TableForm[{q4, q5, q6}]

Кинематические уравнения вращательного движения

ug={psi[t], teta[t], gam[t]}

w={wx[t], wy[t], wz[t]}

(*TableForm[mm4=mm3.mm2.mm1]*)

Спроецируем угловые скорости рысканья, тангажа и крена на оси связанной с.к.

prpsi=Transpose[mm2.mm3].{0,D[psi[t],t],0}

prteta=Transpose[mm3].{0,0,D[teta[t],t]}

prgam={D[gam[t],t],0,0}

pr=prgam+prpsi+prteta

q7=w[[1]]==pr[[1]]

q8=w[[2]]==pr[[2]]

q9=w[[3]]==pr[[3]]

TableForm[{q7, q8, q9}]

Динамические уравнения вращательного движения

TableForm[TI={{Ixx,-Ixy,-Ixz},{-Iyx,Iyy,-Iyz},{-Izx,-Izy,Izz}}] (*Тензор инерции*)

Т.к.  OZ перпендикулярна плоскости симметрии XOY, то

Ixz=Izx=Iyz=Izy=0

Iyx=Ixy

TableForm[TI]

Главный момент количеств движения в связанной системе координат:

TableForm[K=TI.w]

M={Mx[alfa[t],mah[t], dv[t],dh[t],de[t],dsg[t]],My[alfa[t],mah[t], dv[t],dh[t],de[t],dsg[t]],Mz[alfa[t], mah[t], dv[t],dh[t],de[t],dsg[t]]}

Полная производная по времени главного вектора момента количеств движения

TableForm[pdtgv=D[K,t]+Cross[w,K]]

q10=pdtgv[[1]]==M[[1]]

q11=pdtgv[[2]]==M[[2]]

q12=pdtgv[[3]]==M[[3]]

TableForm[{q10, q11, q12}]

MatrixForm[urav={q1, q2, q3, q4, q5, q6, q7, q8, q9, q10, q11, q12}]

vxq=Flatten[{D[r,t], D[v,t], D[w,t], D[ug,t]}]

vq=Flatten[{r,v, w, ug}]

MatrixForm[fk=Flatten[Solve[urav,vxq, Sort->False]]]

TableForm[uravn=Table[fk[[i,1]]==fk[[i,2]], {i,1,12}]]

MatrixForm[matrA=Table[D[uravn[[i,2]],vq[[j]]], {i,1,12}, {j,1,12}]]

upr={dv[t],dh[t], de[t], dsg[t]}

MatrixForm[matrB=Table[D[uravn[[i,2]], upr[[j]]], {i, 1, 12}, {j, 1, 4}]]

MatrixForm[matper={{1,0,0,0,0,0,0,0,0,0,0,0},{0,1,0,0,0,0,0,0,0,0,0,0},{0,0,0,1,0,0,0,0,0,0,0,0},{0,0,0,0,1,0,0,0,0,0,0,0},{0,0,0,0,0,0,0,0,1,0,0,0},{0,0,0,0,0,0,0,0,0,0,1,0},{0,0,1,0,0,0,0,0,0,0,0,0},{0,0,0,0,0,1,0,0,0,0,0,0},{0,0,0,0,0,0,1,0,0,0,0,0},{0,0,0,0,0,0,0,0,1,0,0,0},{0,0,0,0,0,0,0,0,0,1,0,0},{0,0,0,0,0,0,0,0,0,0,0,1}}]

MatrixForm[prodbok=matper.fk]

MatrixForm[mrazdA=matper.matrA]

MatrixForm[mrazdB=matper.matrB]

For[i=1,i<7,For[j=7, j<13,mrazdA[[i,j]]=0;j++];i++]

For[i=7,i<13,For[j=1, j<7,mrazdA[[i,j]]=0;j++];i++]

Матрица А для продольного движения:

MatrixForm[mApr=Table[mrazdA[[i,j]], {i, 1, 6}, {j, 1, 6}]]

Матрица А для бокового движения:

MatrixForm[mAb=Table[mrazdA[[i,j]], {i, 7, 12}, {j, 7, 12}]]

For[i=1,i<7,For[j=3, j<5,mrazdB[[i,j]]=0;j++];i++]

For[i=7,i<13,For[j=1, j<3,mrazdB[[i,j]]=0;j++];i++]

Матрица B для продольного движения:

MatrixForm[mBpr=Table[mrazdB[[i,j]], {i, 1, 6}, {j, 1, 2}]]

Матрица B для бокового движения:

MatrixForm[mBb=Table[mrazdB[[i,j]], {i, 6, 12}, {j, 3, 4}]]

MatrixForm[Xb={wx[t],wy[t],beta[t],gam[t],psi[t]}] (*Вектор состояния бокового движения*)

MatrixForm[Ub={{dh[t]},{de[t]},{0},{0},{0}}]

a1=0.635;a2=5.47;a3=2.72;a4=0.269;a5=3.26;a6=0.709;a7=0.043;

b1=3.1;b2=20.2;b3=17.6;b4=0.072;b5=-0.518;b6=0.057;b7=0.065;

c1=0.82;c2=11.0;c3=8.8;c4=1.2;c5=0.3;c6=7.3;

MatrixForm[Ab={{-b1,-a6,-b2,0,0},{-b6,-a1,-a2,0,0},{b7,a8,-a4,b4,0},{1,-a9,0,b8,0},{0,a10,0,-b9,0}}](*Матрица А для бокового движения*)

MatrixForm[Bb={{-a5,-a3,-a7,0,0},{-b3,-b5,0,0,0},{0,0,0,0,0},{0,0,0,0,0},{0,0,0,0,0}}](*Матрица В для бокового движени*)

dxb=D[Xb,t]

lxb=LaplaceTransform[dxb,t,s]

lxb=Xb.DiagonalMatrix[{s,s,s,s,s}]

urB=MatrixForm[lxb-Ab.Xb]

MatrixForm[murB=Table[D[urB[[1,i]],Xb[[j]]],{i,1,5},{j,1,5}]] (*матрица коэфф-в системы лин. ур-й в опер-й форме бокового движения*)