www.pudn.com > SHIPCONTROL.rar > bridgemotionshipalertpoint.m, change:2014-12-14,size:6556b


function bridgemotionshipalertpoint 
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=270; 
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=4.1; 
hR=7.2; 
epxlong=0.92; 
hs=6.9; 
delta=pi/6; 
t0=0; 
tf=6; 
LACD=0; 
 
n=800; 
YY=zeros(n,7); 
R=300; 
 
 vv=10; 
 
 
% x=zeros(1,n); 
% y=zeros(1,n); 
Dis=zeros(1,n); 
vx2=vv*cos(rand*pi/2); 
vy2=vv*sin(rand*pi/2); 
x0=0-vx2; 
y0=200-vy2; 
tds=600; 
 
 
y1=200+tds*vy2; 
x1=0+tds*vx2; 
%sqrt(R^2-(y1-yR0)^2)+xR0; 
 
 
xm=(x0+x1)/2; 
ym=(y0+y1)/2; 
yR0=9*y1/10;  %-300; 
de=0.001; 
buchang=1; 
ind=2; 
Dis(ind-1)=2; 
Dis(ind)=3; 
j=0; 
 
bs=0.9; 
vx=0; %bs*vv*cos(-170); 
vy=0; %bs*vv*sin(-170); 
%Y0=[xm ym 0 0 vv 0 0 0]; 
 
DCPA=rand*R 
 
                
%R+5*1860; 
vx1=vx 
 
vy1=vy 
vx2=vx2 
vy2=vy2 
xR0=-(DCPA*sqrt(vx2^2+vy2^2)-vx2*(yR0-y0)-vy2*x0)/vy2+x0   %-(DCPA*sqrt((-vx-vv)^2+vy^2)+(-vx-vv)*(yR0-ym))/vy+xm; 
 
 
x11=xR0 
y11=yR0 
x22=xm 
y22=ym 
 
 
 
 
 
alpha=0:pi/20:2*pi; 
x=xR0+R*cos(alpha); 
y=yR0+R*sin(alpha); 
 
bcb=plot([xR0-vx xR0-1000*vx],[yR0-vy yR0-1000*vy],'g-.'); 
hold on 
quiver(xR0-1000*vx,yR0-1000*vy,-999*vx,-999*vy,0.1,'g-','MaxHeadSize',3,'AutoScale','off') 
hold on 
mbb=plot([0-vx2 0+tds*vx2],[200-vy2 200+tds*vy2],'r--'); 
hold on 
quiver(0+tds*vx2,200+tds*vy2,0+tds*vx2,200+tds*vy2,'r--','MaxHeadSize',3,'AutoScale','off') %,'AutoScale','on' 
hold on 
  G=plot(x, y, 'r:');  
% plot(x0,y0,x1,y1,'r--') 
 %xm=157.6; 
 %ym=200; 
 
while sqrt((Dis(ind-1)-R)^2+(Dis(ind)-R)^2)>=(R/100)&&j<8 
      %sqrt((Dis(ind-1)-R)^2+(Dis(ind)-R)^2)>=(R/100) 
    Y0=[0 vv 0 0 xm ym 10*pi/180]; 
  j=j+1 
for i=2:n 
    YY(1,:)=Y0; YY(2,:)=Y0; 
ui(i)=20*pi/180; 
gt=mariner(YY(i,:),ui(i)); 
   for jv=1:7 
       if i>1 
    YY(i+1,jv)=YY(i,jv)+gt(jv); 
       end 
   end 
 
 
x01=YY(i,5); 
y01=YY(i,6); 
 
xR=xR0-vx*i*buchang; 
yR=yR0-vy*i*buchang; 
alpha=0:pi/20:2*pi; 
x=xR+R*cos(alpha); 
y=yR+R*sin(alpha); 
set(G,'xdata',x,'ydata',y); 
% rem(i,n/10); 
% if rem(i,n/10)==0 
%      
% % plot(x,y),drawnow; %pause(0.00000001); 
% end 
axis equal 
% drawnow 
 hold on 
 
gjb=plot(x01,y01,'r'); pause(0.00000001);   % drawnow,'linewidth',10 
 hold on 
 
Dis(i-1)=sqrt((YY((i-1),5)-xR)^2+(YY((i-1),6)-yR)^2); 
Dis(i)=sqrt((YY((i),5)-xR)^2+(YY((i),6)-yR)^2); 
 
if i>3 
    we=(Dis(i)-Dis(i-1)); we2=(Dis(i-2)-Dis(i-1)); 
end 
     
if i>3&&we>0 %&&we2>0    %||Dis(i)<1.02*R 
    ind=i; 
    YY(i,:); 
    break 
%     sm=sm+1;     
end 
 
% if sm>1 
%     
%      
% end 
  
 
end 
   %(YY(ind-1,1)-xR)*(YY(ind,2)-yR)-(YY(ind-1,2)-yR)*(YY(ind,1)-xR) 
    
% plot(YY(ind-1,1),YY(ind-1,2),6000,(YY(ind,2)-YY(ind-1,2))/(YY(ind,1)-YY(ind-1,1))*(6000-YY(ind-1,1))+YY(ind,2),'b'),pause(0.00000001);   % drawnow 
%  hold on 
 
if (YY(ind-1,5)-xR)*(YY(ind,6)-yR)-(YY(ind-1,6)-yR)*(YY(ind,5)-xR)>0&&Dis(ind)>R&&Dis(ind-1)>R 
    x0=xm; 
    y0=ym; 
     
% else 
%   x1=xm; 
%   y1=ym;   
end 
 
if    ((YY(ind-1,5)-xR)*(YY(ind,6)-yR)-(YY(ind-1,6)-yR)*(YY(ind,5)-xR))<0 ||Dis(ind)<=R||Dis(ind-1)<=R 
    x1=xm; 
    y1=ym; 
end 
 
%if Dis(ind)<=R||Dis(ind-1)<=R||(yR<=(YY(ind,6)/YY(ind,5)*(xR-YY(ind,1))+YY(ind,2))) %&& 
    %x1=xm; 
    %y1=ym; 
%end 
 
sqrt((Dis(ind-1)-R)^2+(Dis(ind)-R)^2); 
hold on 
qswz=plot(xm,ym,'r*'); pause(0.00000001);  % 
hold on 
%   plot([x0 x1],[y0 y1],'r--') 
%   hold on 
   xm=(x0+x1)/2; 
   ym=(y0+y1)/2; 
  LACD=sqrt((xm-xR)^2+(ym-yR)^2) 
end 
 
legend([mbb,gjb,qswz], '本船计划航向', '本船轨迹','开始转舵位置'); 
 
function xdot=mariner(x,ui) 
%M.S.Chislett and J, Stroem-Tejsen(1965) 
 
if ~(length(x)==7),error('x-vector must have dimension 7 !'); end 
if ~(length(ui)==1),error('u-vector must have dimension 1!'); end 
 
 
%Normalization variables 
U0=7.72; 
U=sqrt((U0+x(1)^2)+x(2)^2); 
L=160.93; 
%cruise speed UO =- 7.72m/s 15 knots 
%total speed Uin m/s 
%length of Ship in ID 
 
%Non-dimensional states and inputs 
delta_c=ui(1); 
u=x(1)/U; v=x(2)/U; delta=x(7); 
r=x(3)*L/U; psi=x(4); 
 
  
  
 
%Parameters,hydrodynamic derivatives and maindimensions 
 
delta_max=10; 
Ddelta_max=5; 
 
m=798e-5; Iz=39.2e-5; xG=-0.023; 
 
 
Xudot= -840e-5; Yvdot= -1546e-5; Nvdot=23e-5; 
Xu= -184e-5; Yrdot=9e-5; Nrdot=-83e-5; 
Xuu= -110e-5; Yv= -1160e-5; Nv= -264e-5; 
Xuuu= -215e-5; Yr= -499e-5; Nr=-166e-5; 
Xvv=-899e-5; Yvvv=-8078e-5;Nvvv=1636e-5; 
Xrr=18e-5; Yvvr=15356e-5; Nvvr=-5483e-5; 
Xdd=-95e-5; Yvu =-1160e-5; Nvu=-264e-5; 
Xudd=-190e-5; Yru=-499e-5; Nru=-166e-5; 
Xrv=798e-5; Yd =278e-5; Nd=-139e-5; 
Xvd=93e-5; Yddd = -90e-5; Nddd = 45e-5; 
Xuvd=93e-5; Yud=556e-5; Nud=-278e-5; 
Yuud=278e-5; Nuud=-139e-5; 
Yvdd=-4e-5;  Nvdd=13e-5; 
Yvvd=1190e-5; Nvvd=-489e-5; 
Y0=-4e-5; N0=3e-5; 
Y0u=-8e-5; N0u=6e-5; 
Y0uu=-4e-5; N0uu=3e-5; 
 
 
 
%Masses and moments of inertia 
m11=m-Xudot; m23= m*xG-Yrdot; m33=Iz-Nrdot; 
m22=m-Yvdot; m32=m*xG-Nvdot; 
 
% Rudder saturation and dynamics 
 
% delta_c=(delta_max*pi/180)*tanh(dj/(delta_max*pi/180)); 
 
 
 
 
if abs(delta_c) >= delta_max*pi/180 
delta_c=sign(delta_c)*delta_max*pi/180;  
end 
 delta_dot=(delta_c- delta); 
% delta=delta_c-(delta_c-delta)*exp(-1/4); 
if abs(delta_dot)>= Ddelta_max*pi/180 
delta_dot=sign(delta_dot)*Ddelta_max*pi/180;  
end 
 
%Forces and moments 
X=Xu*u+Xuu*u^2 + Xuuu*u^3+ Xvv*v^2+ Xrr*r^2+ Xrv*r*v+ Xdd*delta^2+... 
  Xudd*u*delta^2+ Xvd*v*delta+ Xuvd*u*v*delta; 
 
Y=Yv*v+ Yr*r+Yvvv*v^3+ Yvvr*v^2*r+ Yvu*v*u+ Yru*r*u+ Yd*delta+... 
  Yddd*delta^3+Yud*u*delta+ Yuud*u^2*delta+Yvdd*v*delta^2+... 
  Yvvd*v^2*delta+ (Y0+Y0u*u+Y0uu*u^2); 
N=Nv*v+ Nr*r+Nvvv*v^3+ Nvvr*v^2*r+ Nvu*v*u+ Nru*r*u+ Nd*delta+... 
Nddd*delta^3+ Nud*u*delta+ Nuud*u^2*delta+ Nvdd*v*delta^2+ ... 
Nvvd*v^2*delta+ (N0 +N0u*u+N0uu*u^2); 
%Dimensional state derivative 
xdot= [ X*(U^2/L)/m11 
-(-m33*Y+m23*N)/(m22*m33-m23*m32)*U^2/L 
 (-m32*Y+m22*N)/(m22*m33-m23*m32)*U^2/L^2 
  r*U/L 
(cos(psi)*(U0/U+u)-sin(psi)*v)*U 
(sin(psi)*(U0/U+u)+cos(psi)*v)*U 
     delta_dot     ]; 
 
 
 
% bosai0=YY(:,3); 
% FAI0=YY(:,4); 
% u=YY(:,5); 
% v=YY(:,6); 
% r=YY(:,7); 
% p=YY(:,8); 
 
 
% [hang,lie]=size(x); 
% x0=zeros(1,hang); 
% y0=zeros(1,hang); 
% bosai0=zeros(1,hang); 
% FAI0=zeros(1,hang); 
% for i=2:hang-1 
%     x0(i+1)=(u(i)*cos(bosai(i))-v(i)*cos(FAI(i))*sin(bosai(i)))*(time(i)-time(i-1))+x0(i); 
%     y0(i+1)=(u(i)*sin(bosai(i))+v(i)*cos(FAI(i))*cos(bosai(i)))*(time(i)-time(i-1))+y0(i); 
%     bosai0(i+1)=(r(i)*cos(FAI(i)))*(time(i)-time(i-1))+bosai0(i); 
%     FAI0(i+1)=p(i)*(time(i)-time(i-1))+FAI0(i); 
% end 
 
 
 
% figure 
% plot(bosai0); 
% figure 
% plot(FAI0);