www.pudn.com > SHIPCONTROL.rar > fourdegreesfreedomshipmotion20120929foralex.m, change:2015-12-02,size:6516b


%write by xiong yong,wuhan university of technology,20120929 
%bear_brave@163.com 
function testfourdegreesfreedomshipmotion 
% clear all;clc; 
% global m L D B midu CB ZG H T  Dp np  tp b1 b2  P hR  epxlong hs delta; 
% m=55000*10^3; 
% L=280; 
% D=9.45; 
% B=35.5; 
% midu=1000; 
% CB=0.597; 
% ZG=14; 
% H=2.9; 
% T=0.058*D; 
% Dp=4.3; 
% np=3; 
% tp=0.08; 
% b1=6.5; 
% b2=4.6; 
% P=3.7; 
% hR=7.2; 
% epxlong=0.92; 
% hs=6.9; 
% delta=pi/9; 
 
close all; 
clear all; 
clc; 
global m L D B midu CB ZG H T  Dp np  tp b1 b2  P hR  epxlong hs delta nw  deltaomiga h13  Zb boxiangjiao; 
m=55000*10^3; 
L=280; 
D=9.45; 
B=35.5; 
midu=1000; 
CB=0.597; 
ZG=14; 
H=2.9; 
T=0.058*D; 
Dp=4.3; 
np=3; 
tp=0.08; 
b1=6.5; 
b2=4.6; 
P=3.7; 
hR=7.2; 
epxlong=0.92; 
hs=6.9; 
delta=pi/6; 
nw=12; 
deltaomiga=0.1; 
h13=1; 
Zb=9; 
boxiangjiao=pi/20; 
 
 
t0=0; 
tf=10; 
 
tspan=[t0,tf]; 
Y0=[0 0 0 0 9 0 0 0]; 
[time,YY]=ode45(@DYDT,tspan,Y0); 
x1=YY(:,1);  
y1=YY(:,2); 
bosai=YY(:,3); 
FAI=YY(:,4); 
u=YY(:,5); 
v=YY(:,6); 
r=YY(:,7); 
p=YY(:,8); 
 
 
[hang,lie]=size(x1) 
 
 
plot(x1,y1);  
 
figure 
plot(bosai); 
figure 
plot(FAI); 
figure 
plot(v/u) 
 
function  dy=DYDT(t,y) 
% 船型参数 螺旋桨参数  舵参数 
% global m L D B midu CB ZG H T  Dp np  tp b1 b2  P hR  epxlong hs  delta; 
global m L D B midu CB ZG H T  Dp np  tp b1 b2  P hR  epxlong hs  delta   nw  deltaomiga h13  Zb boxiangjiao; 
 
dy=zeros(8,1); 
x0=y(1); 
y0=y(2); 
bosai=y(3); 
FAI=y(4); 
u=y(5); 
v=y(6); 
r=y(7); 
p=y(8); 
V=sqrt(u^2+v^2); 
RP=r*L/V; 
Beita=atan(-v/u); 
 
% 各纬度惯性矩及附加质量的计算 
IZ=m*(1+CB^4.5)+(L^2+B^2.4)/24; 
m11=m*(1/100)*(0.398+11.98*CB*(1+3.73*(D/B))-(2.89*CB*(L/B))*(1+1.13*(D/B))+0.175*CB*(L/B)^2*(1+0.541*(D/B))-1.107*(L/B)*(D/B)); 
m22=m*(0.882-0.54*CB*(1-1.6*(D/B))-0.156*(1-0.673*CB)*(L/B)+0.826*(D/B)*(L/B)*(1-0.678*(D/B))-0.638*(L/B)*(D/B)*(1-0.669*(D/B))); 
m66=m*(L*0.01*(33-76.85*CB*(1-0.784*CB)+3.43*(L/B)*(1-0.63*CB)))^2; 
%m26=0.5*pi*MIDU*(L*D)^2*(0.67*(B/L)-0.0033*(B/D)^2); 
 
 
%纵向水动力的计算 
Xu=59.12*u^4-462.8*u^3+8775*u^2+28940*u+67640; 
XVR=(1.6757*CB-0.5054-1)*m22; 
XH=XVR*v*r-Xu; 
 
 
%横向水动力的计算 
K=2*D/L;TP=T/D; 
Yv=-0.5*midu*L*D*V*(pi*K/2+1.4*CB*(B/L))*(1+0.67*TP); 
Yr=0.5*midu*L^2*D*V*(pi*K/4)*(1+0.8*TP); 
Yvv=0.5*midu*L*D*(0.048265-6.293*(1-CB)*(D/B)); 
Yvr=0.5*midu*L^2*D*(-0.3791+1.28*(1-CB)*(D/B)); 
Yrr=0.5*midu*L^3*D*(0.0045-0.445*(1-CB)*(D/B)); 
YH=Yv*v+Yr*r+Yvv*v*abs(v)+Yvr*r*abs(v)+Yrr*r*abs(r); 
 
 
 
%首摇力矩的计算 
Lv=K/(0.5*pi*K+1.4*CB*(B/L)); 
Nv=-0.5*midu*L^2*D*V*K*(1-0.27*TP/Lv); 
Nr=-0.5*midu*L^3*D*V*(0.54*K-K^2)*(1+0.3*TP); 
Nvvr=0.5*midu*L^3*D*(-6.0856+137.4735*CB*(B/L)-1029.514*(CB*(B/L))^2+2480.6082*(CB*(B/L))^3); 
Nvrr=0.5*midu*L^4*D*(-0.0635+0.044145*CB*(D/B)); 
Nrr=0.5*midu*L^4*D*(-0.0805+8.6092*(CB*(B/L))^2-36.9816*(CB*(B/L))^3); 
Nvfai=0.5*midu*L^2*D*V*(-1.72*K); 
Nrfai=0.5*midu*L^3*D*V*(2.6*(0.54*K-K^2)); 
Nfai=0.5*midu*L^2*D*V^2*(-0.008); 
 
NH=Nv*v+Nr*r+Nvvr*v^2*r+Nrr*r*abs(r)+Nvrr*v*r^2+Nfai*FAI+Nvfai*v*abs(FAI)+Nrfai*r*abs(FAI); 
 
%横倾力矩的计算 
 
Np=0.12*sqrt((m*(B^2+4*ZG^2)/(12))*H*m*9.8); 
GZFAI=H*sin(FAI); 
ZH=ZG-(4-B/D+0.02*(B/D-5.35)^3)*D; 
 
KH=-Np*p-m*9.8*GZFAI-YH*ZH; 
 
 
%螺旋桨纵向推进力计算 
wp0=0.5*CB-0.05; 
 
Beitap=Beita-0.5*RP; 
wp=wp0*exp(-4*Beitap^2); 
Jp=u*(1-wp)/(Dp*np); 
Kt=0.25035*Jp^3-0.58638*Jp^2-0.067363*Jp+0.42379; 
 
XP=(1-tp)*midu*np^2*Dp^4*Kt; 
 
%舵力的计算  
S=1-u*(1-wp)/np*P; 
lamuda=2*hR/(b1+b2); 
fa=6.13*lamuda/(2.25+lamuda); 
sita=Dp/hR; 
gs=sita*(0.6/epxlong)*S*(2-(2-(0.6/epxlong))*S)/(1-S)^2; 
 
if delta>0 
    c1=1.065; 
else 
    c1=0.935; 
end 
 
uR=u*(1-wp)*sqrt(1+c1*gs); 
vR=(1.163314-1.982836*CB+1.390152*CB^2)*(v-L*r); 
UR=sqrt(uR^2+vR^2); 
S0=1-u*(1-wp0)/np*P; 
delta0=-(2*S0+0.6); 
alfaR=delta-delta0-atan(vR/uR); 
 
tR=1-(0.7382-0.0539*CB+0.1755*CB^2); 
aH=0.6784-1.3374*CB+1.8891*CB^2; 
xR=-0.5*L; 
xH=-(0.4+0.1*CB)*L; 
zR=ZG-D+hs; 
 
FN=0.5*midu*0.5*(b1+b2)*hR*fa*UR^2*sin(alfaR); 
 
 
XR=-(1-tR)*FN*sin(delta); 
YR=(1+aH)*FN*cos(delta); 
NR=(xR+aH*xH)*FN*cos(delta); 
KR=(1+aH)*zR*FN*cos(delta); 
 
 
%波浪主扰动力 
%deltaomiga=0.1; 
%nw=200; 
kafang=bosai-boxiangjiao; 
Ab=L*cos(kafang)/2*9.8; 
Bb=B*sin(kafang)/2*9.8; 
Cbb=4*midu*9.8^3/cos(kafang); 
Cbc=4*midu*9.8^3/sin(kafang); 
Db=V*cos(kafang)/9.8; 
omiga=zeros(1,nw); 
E=zeros(1,nw); 
zsx=zeros(1,nw); 
Skexi=zeros(1,nw); 
% bochangbi=zeros(1,nw); 
% Cxwd=zeros(1,nw); 
% Cywd=zeros(1,nw); 
% Cnwd=zeros(1,nw); 
Xw=0; 
Yw=0; 
Kw=0; 
Nw=0; 
% Xd=0; 
% Yd=0; 
% Nd=0; 
 
 
    xx=deltaomiga:deltaomiga:deltaomiga*nw; 
     
    omiga=xx; 
    Skexi=((8.1*10^(-3)*9.8^2)./(omiga.^5)).*exp(-3.11./(h13.*omiga.^4));     
    E=sqrt(2.*Skexi.*deltaomiga); 
    zsx=1-exp((-omiga.^2.*D)./9.8); 
    Xw1=-(Cbc./omiga.^4).*E.*zsx.*sin(omiga.^2.*Ab).*sin(omiga.^2.*Bb).*sin((omiga-omiga.^2.*Db).*t-2*pi*rand); 
    Yw1=(Cbb./omiga.^4).*E.*zsx.*sin(omiga.^2.*Ab).*sin(omiga.^2.*Bb).*sin((omiga-omiga.^2.*Db).*t-2*pi*rand); 
     
    Kw1=-(Cbb./omiga.^4).*E.*zsx.*sin(omiga.^2.*Ab).*sin(omiga.^2.*Bb).*sin((omiga-omiga.^2.*Db).*t-2*pi*rand).*Zb+... 
       (Cbb./2*9.8.*omiga.^2).*E.*zsx.*sin(omiga.^2.*Ab).*sin((omiga-omiga.^2.*Db).*t-2*pi*rand).*... 
       ((2*9.8^2.*sin(omiga.^2.*Bb))./omiga.^4.*sin(kafang)^2-(B*9.8.*cos(omiga.^2.*Bb))./omiga.^2.*sin(kafang)); 
    
    Nw1=-(sin(kafang)*Cbc./2*9.8.*omiga.^2).*E.*zsx.*sin(omiga.^2.*Bb).*cos((omiga-omiga.^2.*Db).*t-2*pi*rand).*... 
      ((2*9.8^2.*sin(omiga.^2.*Ab))./omiga.^4*cos(kafang)^2-(L*9.8.*cos(omiga.^2.*Bb))./omiga.^2*cos(kafang)); 
 
  Xw=sum(Xw1); 
  Yw=sum(Yw1); 
  Kw=sum(Kw1); 
  Nw=sum(Nw1); 
%   %波浪漂移力 
%    bochangbi(i)=(2*pi*9.8)/(omiga(i)^2*L); 
%    Cxwd(i)=0.05-0.2*bochangbi(i)+0.75*bochangbi(i)^2-0.51*bochangbi(i)^3; 
%    Cywd(i)=0.46+6.83*bochangbi(i)-15.65*bochangbi(i)^2+8.44*bochangbi(i)^3; 
%    Cnwd(i)=-0.11+0.68*bochangbi(i)-0.79*bochangbi(i)^2+0.21*bochangbi(i)^3; 
%    
%   Xd=Xd+midu*9.8*L*cos(kafang)*Cxwd(i)*Skexi(i)*deltaomiga; 
%   Yd=Yd+midu*9.8*L*sin(kafang)*Cywd(i)*Skexi(i)*deltaomiga; 
%   Nd=Nd+midu*9.8*L^2*sin(kafang)*Cnwd(i)*Skexi(i)*deltaomiga; 
%  
%    
   
 
  
 
 
 
 
 
 
 
 
 
 
 
dy(1)=y(5)*cos(y(3))-y(6)*cos(y(4))*sin(y(3)); 
dy(2)=y(5)*sin(y(3))-y(6)*cos(y(4))*cos(y(3)); 
dy(3)=y(7)*cos(y(4)); 
dy(4)=y(8); 
dy(5)=(XH+XP+XR+Xw+(m+m22)*y(6)*y(7))/(m+m11); 
dy(6)=(YH+YR+Yw-(m+m11)*y(5)*y(7))/(m+m22); 
dy(7)=(NH+NR+Nw)/(IZ+m66); 
dy(8)=(KH+KR+Kw)/(m*(B^2+4*ZG^2)/12); 
 
 
 
 
%yd=[u;v;r;p;(XH+XP+XR+(m+m22)*v*r)/(m+m11);(YH+YR-(m+m11)*u*r)/(m+m22);(NH+NR)/(IZ+m66);(KH+KR)/(m*(B^2+4*ZG^2)/(12))];