📄 dynamics.m
字号:
function s_l_d
b=0.005;
v=0;
air_rou=0.0889;
al=[pi/18 pi/9];
cza=[5.9 0;
6.3 0.62;
1.1 0.108;
5.9 0.14];
n=65;
l=0.55;
rou=0;
ixyz=0;
ar=l/b;
e=zeros(6,6);
e(1,1)=3.7e+6;
e(2,2)=2.6e+5;
e(3,3)=2.9e+5;
e(4,4)=0.183;
e(5,5)=0.707;
e(6,6)=276;
disp('sld start')
aero=[v^2*air_rou,ee,b,rootangle];
options=bvpset('RelTol',1e-5);
solinit = bvpinit(linspace(0,l,n),zeros(12,1));
sld_sol = bvp4c(@sld,@bcs,solinit,options,e,aero,cza,al,rou);
disp('sld end')
n1=10;
a10=rootangle+(deval(sld_sol,0.95*l,10))';
a10=a10/pi*180
ldn=l/n1;
dak=zeros(4*n1,4*n1);
dam=eye(4*n1,4*n1);
warning off MATLAB:deval:NonuniqueSolution
disp('dld start')
for i=1:n1
disp(i)
dam(4*(i-1)+1,4*(i-1)+1)=rou*l/n1;
dam(4*(i-1)+2,4*(i-1)+2)=rou*l/n1;
dam(4*(i-1)+3,4*(i-1)+3)=rou*l/n1;
dam(4*(i-1)+4,4*(i-1)+4)=ixyz*l/n1;
xl=(i-0.5)*l/n1;
slt = bvpinit([linspace(0,xl,min(round(xl+0.5)*8+1,33)),linspace(xl,l,min(round(l-xl+0.5)*8+1,33))],zeros(12,1));
for j=1:4
tic
dld_sol=bvp4c(@dld,@dld_bc,slt,options,sld_sol,e,xl,j,rou);
dsol=deval(dld_sol,[0.5:n1-0.5]*l/n1,[7:10]);
for k=1:n1
dak(4*(k-1)+1:4*(k-1)+4,4*(i-1)+j)=dsol(:,k);
end
end
end
%toc
disp('dld end')
dak=dak/100;
dak=(dak'+dak)/2;
[V,D] = eig(sqrt(dam)*dak*sqrt(dam));
function dydx=dld(x,y,k,sld_sol,e,xl,bll,rou)
yc= deval(sld_sol,x);
fc=yc(1:3);
mc=yc(4:6);
sc=yc(10:12);
rwc=e\yc(1:6);
wc=rwc(4:6);
wzct=[ 0 -wc(3) wc(2);
wc(3) 0 -wc(1);
-wc(2) wc(1) 0 ];
pgc=[0;0;-rou*9.8]; %% this is force to dynamics
mgc=[0;0;0];
f=y(1:3); %% this is strain to dynamics
m=y(4:6);
s=y(10:12);
rw=e\y(1:6); %% this is strain to dynamics
w=rw(4:6);
wzt=[ 0 -w(3) w(2);
w(3) 0 -w(1);
-w(2) w(1) 0 ];
t=zeros(3,3);
t(1,1)=-sin(sc(2))*cos(sc(3))*s(2)-cos(sc(2))*sin(sc(3))*s(3);
t(1,2)=-sin(sc(2))*sin(sc(3))*s(2)+cos(sc(2))*cos(sc(3))*s(3);
t(1,3)= cos(sc(2))*s(2);
t(2,1)=-cos(sc(3))*cos(sc(1))*s(3)+sin(sc(3))*sin(sc(1))*s(1)-cos(sc(1))*sin(sc(2))*cos(sc(3))*s(1)-sin(sc(1))*cos(sc(2))*cos(sc(3))*s(2)+sin(sc(1))*sin(sc(2))*sin(sc(3))*s(3);
t(2,2)=-sin(sc(1))*cos(sc(3))*s(1)-cos(sc(1))*sin(sc(3))*s(3)-cos(sc(1))*sin(sc(2))*sin(sc(3))*s(1)-sin(sc(1))*cos(sc(2))*sin(sc(3))*s(2)-sin(sc(1))*sin(sc(2))*cos(sc(3))*s(3);
t(2,3)= cos(sc(1))*cos(sc(2))*s(1)-sin(sc(1))*sin(sc(2))*s(2);
t(3,1)= cos(sc(1))*sin(sc(3))*s(1)+sin(sc(1))*cos(sc(3))*s(3)+sin(sc(1))*sin(sc(2))*cos(sc(3))*s(1)-cos(sc(1))*cos(sc(2))*cos(sc(3))*s(2)+cos(sc(1))*sin(sc(2))*sin(sc(3))*s(3);
t(3,2)=-cos(sc(1))*cos(sc(3))*s(1)+sin(sc(1))*sin(sc(3))*s(3)+sin(sc(1))*sin(sc(2))*sin(sc(3))*s(1)-cos(sc(1))*cos(sc(2))*sin(sc(3))*s(2)-cos(sc(1))*sin(sc(2))*cos(sc(3))*s(3);
t(3,3)=-sin(sc(1))*cos(sc(2))*s(1)-cos(sc(1))*sin(sc(2))*s(2);
df=-(wzct*f+wzt*fc+t*pgc);
dm=-(wzct*m+wzt*mc+t*mgc+[0;-f(3);f(2)]);
ds=[w(1)-cos(sc(1))*tan(sc(2))*wc(2)*s(1)-sin(sc(1))/cos(sc(2))/cos(sc(2))*wc(2)*s(2)-sin(sc(1))*tan(sc(2))*w(2)+sin(sc(1))*tan(sc(2))*wc(3)*s(1)-sin(sc(1))/cos(sc(2))/cos(sc(2))*wc(3)*s(2)-cos(sc(1))*tan(sc(2))*w(3);
sin(sc(1))*wc(2)*s(1)-cos(sc(1))*w(2)+cos(sc(1))*wc(3)*s(1)+sin(sc(1))*w(3);
(cos(sc(1))*wc(2)*s(1)+sin(sc(1))*tan(sc(2))*wc(2)*s(2)+sin(sc(1))*w(2)-sin(sc(1))*wc(3)*s(1)+cos(sc(1))*tan(sc(2))*wc(3)*s(2)+cos(sc(1))*w(3))/cos(sc(2)) ];
du= (1+rwc(1))*[t(1,1);t(1,2);t(1,3)]...
+rw(1)*[ cos(sc(2))*cos(sc(3));cos(sc(2))*sin(sc(3));sin(sc(2))];
dydx=[df;dm;du;ds];
function res=dld_bc(yl,yr,sld_sol,e,xl,j,rou)
sc=deval(sld_sol,xl,[10:12]);
switch j
case 1
fm=[cos(sc(2))*cos(sc(3)); -cos(sc(1))*sin(sc(3))-sin(sc(1))*sin(sc(2))*cos(sc(3)); sin(sc(1))*sin(sc(3))-cos(sc(1))*sin(sc(2))*cos(sc(3));0;0;0];
case 2
fm=[cos(sc(2))*sin(sc(3)); cos(sc(1))*cos(sc(3))-sin(sc(1))*sin(sc(2))*sin(sc(3)); -sin(sc(1))*cos(sc(3))-cos(sc(1))*sin(sc(2))*sin(sc(3));0;0;0];
case 3
fm=[ sin(sc(2)); sin(sc(1))*cos(sc(2)); cos(sc(1))*cos(sc(2));0;0;0];
case 4
fm=[0;0;0;1;0;0];
end
res=zeros(24,1);
res(1:6)=yl(7:12,1);
res(7:12)=yr(1:6,2);
res(13:18)=yr(7:12,1)-yl(7:12,2);
res(19:24)=yr(1:6,1)-yl(1:6,2)-fm*100;
function dfdy=dld_jac(x,y,k,sld_sol,e,xl,bll,rou)
dfdy=zeros(12);
c=inv(e);
mgc=[0;0;0];
pgc=[0;0;-rou*9.8];
yc= deval(sld_sol,x);
fc=yc(1:3);
mc=yc(4:6);
sc=yc(10:12);
rwc=e\yc(1:6);
df=-(wzct*f+wzt*fc+t*pgc);
df=-(wz'*f+pl+t*pg);
dfdy(1:6,1:6)=[fc([2,3])'*[c(6,:);-c(5,:)];
fc([1,3])'*[-c(6,:);c(4,:)];
fc([1,2])'*[c(5,:);-c(4,:)];
mc([2,3])'*[c(6,:);-c(5,:)];
mc([1,3])'*[-c(6,:);c(4,:)];
mc([1,2])'*[c(5,:);-c(4,:)]]...
+[[0 rwc(6) -rwc(5);-rwc(6) 0 rwc(4);rwc(5) -rwc(4) 0 ], zeros(3);
[0 0 0;0 0 1;0 -1 0],[0 rwc(6) -rwc(5);-rwc(6) 0 rwc(4);rwc(5) -rwc(4) 0 ]];
dfdy(7:9,1:6)=[cos(sc(2))*cos(sc(3));cos(sc(2))*sin(sc(3));sin(sc(2))]*c(1,:);
dfdy(10:12,1:6)=[1 -sin(sc(1))*tan(sc(2)) -cos(sc(1))*tan(sc(2));
0 -cos(sc(1)) sin(sc(1));
0 sin(sc(1))/cos(sc(2)) cos(sc(1))/cos(sc(2))]*c(4:6,:);
dfdy(1:3,10:11)=[ 0 -cos(sc(2));
-cos(sc(1))*cos(sc(2)) +sin(sc(1))*sin(sc(2));
+sin(sc(1))*cos(sc(2)) +cos(sc(1))*sin(sc(2))]*pgc(3);
dfdy(7:9,11:12)=[-sin(sc(2))*cos(sc(3)) -cos(sc(2))*sin(sc(3));
-sin(sc(2))*sin(sc(3)) cos(sc(2))*cos(sc(3));
cos(sc(2)) 0]*(1+rwc(1));
dfdy(10:12,10:11)=[[-cos(sc(1))*tan(sc(2)) +sin(sc(1))*tan(sc(2));
sin(sc(1)) +cos(sc(1));
cos(sc(1))/cos(sc(2)) -sin(sc(1))/cos(sc(2))]*rwc(5:6),...
[ -sin(sc(1))/cos(sc(2))^2 -sin(sc(1))/cos(sc(2))^2;
0 0;
sin(sc(1))*tan(sc(2))/cos(sc(2)) cos(sc(1))*tan(sc(2))/cos(sc(2))]*rwc(5:6)];
function dydx=sld(x,y,e,aero,cza,al,rou)
f=y(1:3);
m=y(4:6);
u=y(7:9);
s=y(10:12);
t=[ cos(s(2))*cos(s(3)) cos(s(2))*sin(s(3)) sin(s(2));
-cos(s(1))*sin(s(3))-sin(s(1))*sin(s(2))*cos(s(3)) cos(s(1))*cos(s(3))-sin(s(1))*sin(s(2))*sin(s(3)) sin(s(1))*cos(s(2));
sin(s(1))*sin(s(3))-cos(s(1))*sin(s(2))*cos(s(3)) -sin(s(1))*cos(s(3))-cos(s(1))*sin(s(2))*sin(s(3)) cos(s(1))*cos(s(2))];
fm=y(1:6);
rw=zeros(6,1);
rw=e\fm;
r=rw(1);
w=rw(4:6);
wz=[ 0 w(3) -w(2);
-w(3) 0 w(1);
w(2) -w(1) 0 ];
pl=[0;0;0];
pg=[0;0;0];
ml=[0;0;0];
mg=[0;0;0];
pg=[0;-rou*9.8*sin(aero(4));-rou*9.8*cos(aero(4))];
bs=aero(4)+s(1);
if bs<=al(1)
clm=cza(1,:)*bs;
elseif bs>al(1) & bs<=al(2)
clm=cza(1,:)*bs-cza(2,:)*(bs-al(1));
else
clm=cza(1,:)*bs-(cza(3,:)+cza(4,:)*(bs-al(2)));
end
yl=aero(1)*aero(3)*clm(1);
mz=aero(1)*aero(3)^2*clm(2);
pl(2)=yl*sin(bs);
pl(3)=yl*cos(bs);
ml(1)=2*pl(3)*aero(2)*aero(3)+mz;
df=-(wz'*f+pl+t*pg);
ft=[0;-f(3);f(2)];
dm=-(wz'*m+ft+ml+t*mg);
ds=[w(1)-sin(s(1))*tan(s(2))*w(2)-cos(s(1))*tan(s(2))*w(3);
-cos(s(1))*w(2)+sin(s(1))*w(3);
sin(s(1))/cos(s(2))*w(2)+cos(s(1))/cos(s(2))*w(3)];
du=[(1+r)*cos(s(2))*cos(s(3));
(1+r)*cos(s(2))*sin(s(3));
(1+r)*sin(s(2)) ];
dydx=[df;dm;du;ds];
function res=bcs(ya,yb,e,aero,cza,al,rou)
res(1:6,1)=ya(7:12);
res(7:12)=yb(1:6);
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -