www.pudn.com > SHIPCONTROL.rar > marineravoidcollisionnew.m, change:2014-02-24,size:22788b


function marineravoidcollisionnew 
clear all;clc; 
 
delta=0.1; 
iniangel=0; 
n=1500; 
R=0; 
indd=0; 
 meet=0; 
 xm=0; 
 ym=0; 
 laichuanshu=3; 
 buchang=1; 
 deltae=0; 
 hxu1=8;  
 xielv=0; 
 jieju=0; 
 adds=0.25; 
 xmm=0; 
 ymm=0; 
%  am=zeros(laichuanshu); 
%  bm=zeros(laichuanshu);   
 Dis=zeros(n,laichuanshu); 
 %Dis=zeros(laichuanshu); 
 sida1=zeros(1,laichuanshu); 
 sida2=zeros(1,laichuanshu); 
 sida3=zeros(1,laichuanshu); 
 rad=zeros(1,laichuanshu); 
 x=zeros(1,laichuanshu); 
 y=zeros(1,laichuanshu); 
 a4s=0; 
 a5s=zeros(1,laichuanshu); 
 rvx=zeros(1,laichuanshu); 
 rvy=zeros(1,laichuanshu); 
 Kp=4;    %4.1; 
 Td=0;     %1.5; 
 Ti=400; 
 ind=zeros(1,n); 
  
 ind2=zeros(1,n); 
 sdd=iniangel; 
 sdd1=0; 
 sdd2=0; 
  rad(1)=200+rand*2 
    rad(2)=200+rand*2 
    rad(3)=200+rand*2 
     % numbers of other ship  
% for i=1:laichuanshu  
x=input('please select ship meeting situation:1 or 2 or 3 or 4\n'); 
 switch (x)   
case 1 
     
     
    a11=rand*100+4000 
    a21=(a11+19*rad(1))*cos(asin(rad(1)/a11)*(0.3+rand*0.7)+asin(rad(2)/(a11+19*rad(1)))) 
    a31=a11+5000 
    b11=-rand*3+4 
    b21=-rand*3-4 
    b31=rand*3-4 
     
     
     
    a12=0   %-rand*100-1000; 
    a22=(a11+19*rad(1))*sin(asin(rad(1)/a11)*(0.3+rand*0.7)+asin(rad(2)/(a11+19*rad(1)))) 
    a32=a12-rad(1)*4 
     
     
    b12=0.1 %-rand*2-1 
    b22=-rand*0.2 
    b32=rand*1 
     
    case 2 
     
     
    a11=rand*100+4000 
    a21=(a11+15*rad(1))*cos(-asin(rad(1)/a11)*(0.3+rand*0.7)-asin(rad(2)/(a11+15*rad(1)))) 
    a31=a11+5000 
    b11=-rand*3-2 
    b21=-rand*3-2 
    b31=-rand*1-2 
     
     
     
    a12=0    %-rand*100-1000; 
    a22=(a11+15*rad(1))*sin(-asin(rad(1)/a11)*(0.3+rand*0.7)-asin(rad(2)/(a11+15*rad(1)))) 
    a32=a12+rad(1)*4 
     
     
    b12=0.1%-rand*2-1; 
    b22=rand*0.2+1 
    b32=-rand*0.1 -1  
         
         
         
         
    case 3 
    
     
    a11=rand*100+3500 
    a21=(a11+15*rad(1))*cos(asin(rad(1)/a11)*(1.1+rand*0.7)+asin(rad(2)/(a11+15*rad(1)))) 
    a31=(a11+22*rad(1))*cos(-asin(rad(1)/a11)*(2+rand*0.7)-asin(rad(2)/(a11+22*rad(1)))) 
    b11=-rand*3-2 
    b21=-rand*3-2 
    b31=-rand*3-2 
     
     
     
    a12=0    %-rand*100-1000; 
    a22=(a11+15*rad(1))*sin(asin(rad(1)/a11)*(1.1+rand*0.7)+asin(rad(2)/(a11+15*rad(1)))) 
    a32=(a11+22*rad(1))*sin(-asin(rad(1)/a11)*(2+rand*0.7)-asin(rad(2)/(a11+22*rad(1)))) 
     
     
    b12=0.01%-rand*2-1; 
    b22=-rand*0.1-1 
    b32=rand*0.2+0.2 
     
    case 4 
         
    a11=rand*100+3500 
    a21=(a11+15*rad(1))*cos(-asin(rad(1)/a11)*(0.3+rand*0.7)-asin(rad(2)/(a11+15*rad(1)))) 
    a31=(a11+20*rad(1))*cos(asin(rad(1)/a11)*(0.3+rand*0.7)+asin(rad(2)/(a11+20*rad(1)))) 
    b11=-rand*3-2 
    b21=-rand*3-2 
    b31=-rand*3-2 
     
     
     
    a12=0    %-rand*100-1000; 
    a22=(a11+15*rad(1))*sin(-asin(rad(1)/a11)*(0.3+rand*0.7)-asin(rad(2)/(a11+15*rad(1)))) 
    a32=(a11+20*rad(1))*sin(asin(rad(1)/a11)*(0.3+rand*0.7)+asin(rad(2)/(a11+20*rad(1)))) 
     
     
    b12=0.01  %-rand*2-1; 
    b22=rand*0.2+1 
    b32=-rand*0.2-1  
         
     
 end 
   
    alpha=0:pi/20:2*pi; 
     
    xer1=rad(1)*cos(alpha); 
    xer2=rad(2)*cos(alpha); 
    xer3=rad(3)*cos(alpha); 
     
    yer1=rad(1)*sin(alpha); 
    yer2=rad(2)*sin(alpha); 
    yer3=rad(3)*sin(alpha); 
%     xui=1:1:6000; 
%     xer4=xui; 
%     xer5=xui; 
%     yer4=xui; 
%     yer5=xui; 
%     xer6=xui; 
%     yer6=xui;     
    x02=xm; 
    y02=ym; 
%     x06=0; 
%     y06=0; 
    G=plot(x02, y02, 'r:');  
    hold on  
     Q1=plot(xer1, yer1,'-'); 
     %hold  on 
     Q2=plot(xer2, yer2,'-.'); 
     %hold  on 
     Q3=plot(xer3, yer3,'--'); 
     legend('own ship','target ship 1','target ship 2','target ship 3') 
%      Q4=plot(xer4,yer4); 
%      Q5=plot(xer5,yer5); 
     Q6=quiver(1,0,1,0,200,'LineWidth',1,'MaxHeadSize',3,'AutoScale','off'); 
     Q7=quiver(1,0,1,0,200,'LineWidth',1,'MaxHeadSize',3,'AutoScale','off'); 
     Q8=quiver(1,0,1,0,200,'LineWidth',1,'MaxHeadSize',3,'AutoScale','off'); 
     Q9=quiver(1,0,1,0,200,'LineWidth',1,'MaxHeadSize',3,'AutoScale','off'); 
  title('ship collision avoidance simulating')  
  xlabel('X/m') 
  ylabel('Y/m') 
 
 
ui=zeros(1,n); 
m7=7; 
YY=zeros(n+1,m7); 
YY01=zeros(1,n); 
YY(1,:)=[1 0 0 0 0 0 0]; 
for i=2:n 
     
   if i==50||i==300||i==350||i==390||i==500||i==410||i==550||i==260||i==600||i==700||i==750||i==800||i==850||i==900||i==950||i==1000||i==450||i==650 
       pause 
   end 
    
  sl=0;   
 gt=mariner(YY(i,:),ui(i)); 
   for jv=1:m7 
       if i>1 
    YY(i+1,jv)=YY(i,jv)+gt(jv); 
       end 
   end 
    
x01=YY(i,5); 
y01=YY(i,6); 
x02=[x02,x01]; 
y02=[y02,y01]; 
% delta=YY(i,7); 
x03=YY(i-1,5); 
y03=YY(i-1,6); 
x00=gt(5); 
 
y00=gt(6); 
 
lacd=zeros(1,laichuanshu)+5200; 
%YY01(i)=YY(i+1,4);   
YY01(i)=atan2(y00,x00);    
pause(0.001);  
 
    x(1)=a11+b11*i; 
    x(2)=a21+b21*i; 
    x(3)=a31+b31*i; 
     
    y(1)=a12+b12*i; 
    y(2)=a22+b22*i; 
    y(3)=a32+b32*i; 
     
     xer1=x(1)+rad(1)*cos(alpha); 
     xer2=x(2)+rad(2)*cos(alpha);    
     xer3=x(3)+rad(3)*cos(alpha); 
      
    yer1=y(1)+rad(1)*sin(alpha); 
    yer2=y(2)+rad(2)*sin(alpha);      
    yer3=y(3)+rad(3)*sin(alpha); 
     
     
       
     axis([0 8000+max(max(x02)) -300+min(min(y02)) 300+max(max(y02)) ]); 
     
     %set(gcf,'doublebuffer','on'); 
     hold on 
     %pause(0.1); 
    set(G,'xdata',x02,'ydata',y02); 
    hold on 
    axis equal 
    set(Q1,'xdata',xer1,'ydata',yer1); 
    axis equal 
    hold on 
    set(Q6,'xdata',x01,'ydata',y01,'Udata',x00,'Vdata',y00); 
    axis equal 
    hold on 
     
    set(Q7,'xdata',x(1),'ydata',y(1),'Udata',b11,'Vdata',b12); 
      axis equal 
    hold on 
     set(Q8,'xdata',x(2),'ydata',y(2),'Udata',b21,'Vdata',b22); 
      axis equal 
    hold on 
     set(Q9,'xdata',x(3),'ydata',y(3),'Udata',b31,'Vdata',b32); 
      axis equal 
    hold on 
     
     
    set(Q2,'xdata',xer2,'ydata',yer2); 
       axis equal 
     hold on 
    set(Q3,'xdata',xer3,'ydata',yer3); 
%     axis equal 
%      hold on 
%       set(Q4,'xdata',x06,'ydata',y06) 
%     axis equal 
   
for j1=1:laichuanshu 
 
    sida1(j1)= atan2(y(j1)-y01,x(j1)-x01);  %????????????????????? who  minus who?     
 
    Dis(i,j1)=sqrt((y(j1)-y01)^2+(x(j1)-x01)^2); 
    sida2(j1)=sida1(j1)-asin(rad(j1)/ Dis(i,j1));    %sin不能小于1,否则出错   
    sida3(j1)=sida1(j1)+asin(rad(j1)/ Dis(i,j1)); 
 
    rvx(j1)=eval(['-b',num2str(j1),'1+x00',';']);   %此处有问题,已修改 
    rvy(j1)=eval(['-b',num2str(j1),'2+y00',';']);   %此处有问题,已修改 
    a5s(j1)=atan2(rvy(j1),rvx(j1));                %此处有问题,已修改  
%     if a5s(j1)<0 
%         a5s(j1)=2*pi+a5s(j1); 
%     end 
      
end                                                   
%sida1 
 
 
 
[hxu2,hindex2]=sort(sida1); 
 
     
for j=1:laichuanshu 
    %j=hindex2(j); 
   a=rvy(j)/rvx(j);  
   b=y01-x01*a; 
   limjiao=deltajiaodian(a,b,x(j),y(j),rad(j)); 
   limqianhou=qianhou(x03,y03,x01,y01,x(j),y(j));  
   x101=x01; 
   y101=y01; 
    
if (i>2)&&(Dis(i,j)<5000)&&(limqianhou>0) 
     
if limjiao>0; 
    lacd(j)=Dis(i,j); 
        
     
    sl=sl+1; 
% x10=(a*(y(j)-b)+x(j))/(1+a^2); 
% y10=b+a*x10; 
% xmm=(x101+x10)/2; 
% ymm=(y101+y10)/2; 
%  inds=2; 
%  n0=10000; 
%  Diss=zeros(1,n0); 
%  Diss(inds-1)=2; 
%  Diss(inds)=3; 
%  j3=1; 
%  YYo=zeros(n0+1,7); 
%  while sqrt((Diss(inds-1)-R)^2+(Diss(inds)-R)^2)>=(R/100)&&j3<50 
%      j3=j3+1; 
%      YYo(1,:)=[1 0 0 0.5 0 0 YY(i+1,7)]; 
%      uio=0.6; 
%       for io=2:2:n0 
%           gt=mariner(YYo(io,:),uio); 
%    for jv=1:m7 
%        if io>1 
%     YYo(io+1,jv)=YYo(io,jv)+gt(jv); 
%        end 
%    end 
%            eval([ 'x',num2str(j),'10=x(',num2str(j),')+b',num2str(j),'1*',num2str(io),';']); 
%            eval([ 'y',num2str(j),'10=y(',num2str(j),')+b',num2str(j),'2*',num2str(io),';']); 
%            xRo=eval([ 'x',num2str(j),'10',';']); 
%            yRo=eval([ 'y',num2str(j),'10',';']); 
%            Diss(io)=sqrt((YYo(io,5)-xRo)^2+(YYo(io,6)-yRo)^2); 
%            we=(Diss(io)-Diss(io-1)); 
%            if we>0||Diss(io)<1.02*R 
%                inds=io;    
%                break; 
%            end 
%       end 
% if (Diss(inds)>R&&Diss(inds-1)>R&&((YYo(inds-1,5)-xRo)*(YYo(inds,6)-yRo)-(YYo(inds-1,6)-yRo)*(YYo(inds,5)-xRo))>0) 
%     
%     x101=xmm; 
%     y101=ymm; 
% else 
%    x10=xmm; 
%     y10=ymm;  
% end           
%   xmm=(x101+x10)/2; 
%   ymm=(y101+y10)/2; 
%   lacd(j)=sqrt((xmm-xRo)^2+(ymm-yRo)^2); 
%    
%     %size(lacd)  
%  end 
  
  
end 
end 
%lacd 
end 
% 
 
if sl~=0 
    [hxu1,hindex1]=min(lacd); 
    %size(hindex1) 
    for  j8=1:laichuanshu 
%         hindex1 
%         size(hindex1) 
      if  hindex1==hindex2(j8) 
    ind2(i)=hindex1; 
    ind(i)=j8; 
    indd=j8; 
      end 
    end 
end 
 
if indd~=0 
 if (Dis(i,indd)>1000&&Dis(i,indd)<1012) ||(Dis(i,indd)>450&&Dis(i,indd)<462)||(Dis(i,indd)>1.5*rad(hindex1)&&Dis(i,indd)<(1.5*rad(hindex1)+12))||(Dis(i,indd)>600&&Dis(i,indd)<612) 
        pause 
 end 
end 
 
 
 
 
   indd; 
       
   
     
    if ind(i)~=0  
    px2=YY(i,5)+rvx(hindex2(ind(i))); 
    py2=YY(i,6)+rvy(hindex2(ind(i))); 
    px3=x(hindex2(ind(i))); 
    py3=y(hindex2(ind(i))); 
    ab1=tan(sida3(hindex2(ind(i)))); 
    ab2=y01-x01*ab1; 
    end 
%     xer4=x01+xui; 
%     xer5=x01+xui; 
%     xer6=x01+xui; 
     
     
%    if ind(i)~=0  
%         ssxielv=tan(sida3(hindex2(ind(i)))); 
%         ssjieju=y01-x01*ssxielv; 
%         yer5=xer5*ssxielv+ssjieju; 
%         set(Q5,'xdata',xer5,'ydata',yer5) 
%          
%         sxielv=tan(sida2(hindex2(ind(i)))); 
%         sjieju=y01-x01*sxielv; 
%         yer4=xer4*sxielv+sjieju; 
%         set(Q4,'xdata',xer4,'ydata',yer4)  
%             
%        yer6=xer6*(rvy(hindex2(ind(i)))/rvx(hindex2(ind(i))))+y01-x01*(rvy(hindex2(ind(i)))/rvx(hindex2(ind(i)))); 
%         
%         
%        %yer6=xer6*y00/x00+y01-x01*y00/x00; 
%        %yer6=xer6*a+b; 
%        set(Q6,'xdata',xer6,'ydata',yer6) 
%         linshia=rvy(hindex2(ind(i)))/rvx(hindex2(ind(i)));  
%            linshib=y01-x01*linshia;    
%         
%        if deltajiaodian(linshia,linshib, x(hindex2(ind(i))),y(hindex2(ind(i))),rad(hindex2(ind(i))))<0 
%                     
%            linshijiao=deltajiaodian(linshia,linshib, x(hindex2(ind(i))),y(hindex2(ind(i))),rad(hindex2(ind(i)))) 
%            pause 
%        end 
%    end 
   
    
    
if ind(i)~=laichuanshu&&ind(i)~=0 
     
    x0z=x(hindex2(ind(i)+1)); 
    y0z=y(hindex2(ind(i)+1)); 
    Rz=rad(hindex2(ind(i)+1)); 
    panduan=(py3+(x00/y00)*px3-(y01+x01*(x00/y00)))*(y0z+(x00/y00)*x0z-(y01+x01*(x00/y00))); 
    bzjiaodian=deltajiaodian(ab1,ab2,x0z,y0z,Rz); 
end 
 
        
if ind(i)~=0 
    da=tan(sida2(hindex2(ind(i)))); 
   db=y01-x01*da; 
   
   u=eval(['b',num2str(hindex2(ind(i))),'2']); 
   v=eval(['b',num2str(hindex2(ind(i))),'1']); 
   V=sqrt(x00^2+y00^2); 
    
end 
 
if ind(i)~=1&&ind(i)~=0 
     
     x0y=x(hindex2(ind(i)-1)); 
     y0y=y(hindex2(ind(i)-1)); 
    panduan2=(py3+(x00/y00)*px3-(y01+x01*(x00/y00)))*(y0y+(x00/y00)*x0y-(y01+x01*(x00/y00))); 
     
    Ry=rad(hindex2(ind(i)-1)); 
    byjiaodian=deltajiaodian(da,db,x0y,y0y,Ry); 
 
end 
 
 
 
 
 
if ind(i)~=1&&ind(i)~=laichuanshu %&&Dis(i,1)<3000&&Dis(i,2)<3000&&Dis(i,3)<3000 
 
%case 1 
if (ind(i)~=0)&&bzjiaodian>0&&(panduan>0)&&byjiaodian<0&&(i>2); 
     
    meet=1; 
    s=tan(rem(sida2(hindex2(ind(i))),2*pi)-adds); 
%   sdd1=asin((v + s*(V^2*s^2 + V^2 - s^2*u^2 + 2*s*u*v - v^2)^(1/2) - s*u)/(V*(s^2 + 1))); 
%   sdd2=asin(-(s*(V^2*s^2 + V^2 - s^2*u^2 + 2*s*u*v - v^2)^(1/2) - v + s*u)/(V*(s^2 + 1))); 
     
  sdd1=asin((u + s*(V^2*s^2 + V^2 - s^2*v^2 + 2*s*u*v - u^2)^(1/2) - s*v)/(V*(s^2 + 1))); 
  sdd2=asin(-(s*(V^2*s^2 + V^2 - s^2*v^2 + 2*s*u*v - u^2)^(1/2) - u + s*v)/(V*(s^2 + 1))); 
  a4s=atan2(y00,x00); 
% if a4s<0 
%     a4s=2*pi+a4s; 
% end 
 
if (s*sdd1>0&&s*sdd2>0)||(s*sdd1<0&&s*sdd2<0) 
 
 
if abs(a4s-sdd1)<abs(a4s-sdd2) 
         sdd=sdd1; 
else 
    sdd=sdd2; 
end 
 
else 
     
    if s*sdd1>0 
        sdd=sdd1; 
    else 
        sdd=sdd2; 
    end 
 
 
 
end 
 
 
% if abs(linkxielv*laichuanxielv+1)<0.1;  %(ind(i)==ind(i-2))&&i>2&& 
%     sdd=iniangel; 
% end 
 
end 
 
%case 2 
if(ind(i)~=0)&&(i>2)&&bzjiaodian<0&&(panduan2>0)&&byjiaodian>0; 
  meet=2; 
    s=tan(rem(sida3(hindex2(ind(i))),2*pi)+adds); 
%   sdd1=asin((v + s*(V^2*s^2 + V^2 - s^2*u^2 + 2*s*u*v - v^2)^(1/2) - s*u)/(V*(s^2 + 1))); 
%   sdd2=asin(-(s*(V^2*s^2 + V^2 - s^2*u^2 + 2*s*u*v - v^2)^(1/2) - v + s*u)/(V*(s^2 + 1))); 
 
sdd1=asin((u + s*(V^2*s^2 + V^2 - s^2*v^2 + 2*s*u*v - u^2)^(1/2) - s*v)/(V*(s^2 + 1))); 
  sdd2=asin(-(s*(V^2*s^2 + V^2 - s^2*v^2 + 2*s*u*v - u^2)^(1/2) - u + s*v)/(V*(s^2 + 1))); 
 
  a4s=atan2(y00,x00); 
% if a4s<0 
%     a4s=2*pi+a4s; 
% end 
 
if (s*sdd1>0&&s*sdd2>0)||(s*sdd1<0&&s*sdd2<0) 
 
 
if abs(a4s-sdd1)<abs(a4s-sdd2) 
         sdd=sdd1; 
else 
    sdd=sdd2; 
end 
 
else 
     
    if s*sdd1>0 
        sdd=sdd1; 
    else 
        sdd=sdd2; 
    end 
 
 
 
end 
 
% if abs(linkxielv*laichuanxielv+1)<0.1;  %(ind(i)==ind(i-2))&&i>2&& 
%     sdd=iniangel; 
% end 
 
 
end 
 
 
%case 3 
 
if (ind(i)~=0)&&(i>2)&&bzjiaodian<0&&byjiaodian<0; 
      meet=3; 
   
   if (px1-px3)*(py2-py3)-(py1-py3)*(px2-px3)>0 
        s=tan(rem(sida2(hindex2(ind(i))),2*pi)-adds); 
   end 
    
   if (px1-px3)*(py2-py3)-(py1-py3)*(px2-px3)<0 
     s=tan(rem(sida3(hindex2(ind(i))),2*pi)+adds); 
   end 
    
%   sdd1=asin((v + s*(V^2*s^2 + V^2 - s^2*u^2 + 2*s*u*v - v^2)^(1/2) - s*u)/(V*(s^2 + 1))); 
%   sdd2=asin(-(s*(V^2*s^2 + V^2 - s^2*u^2 + 2*s*u*v - v^2)^(1/2) - v + s*u)/(V*(s^2 + 1))); 
    sdd1=asin((u + s*(V^2*s^2 + V^2 - s^2*v^2 + 2*s*u*v - u^2)^(1/2) - s*v)/(V*(s^2 + 1))); 
  sdd2=asin(-(s*(V^2*s^2 + V^2 - s^2*v^2 + 2*s*u*v - u^2)^(1/2) - u + s*v)/(V*(s^2 + 1))); 
 
 
  a4s=atan2(y00,x00); 
% if a4s<0 
%     a4s=2*pi+a4s; 
% end 
 
if (s*sdd1>0&&s*sdd2>0)||(s*sdd1<0&&s*sdd2<0) 
 
 
if abs(a4s-sdd1)<abs(a4s-sdd2) 
         sdd=sdd1; 
else 
    sdd=sdd2; 
end 
 
else 
     
    if s*sdd1>0 
        sdd=sdd1; 
    else 
        sdd=sdd2; 
    end 
 
 
 
end 
 
% if abs(linkxielv*laichuanxielv+1)<0.1;  %(ind(i)==ind(i-2))&&i>2&& 
%     sdd=iniangel; 
% end 
 
end 
 
%case 4 
 
if (ind(i)~=0)&&(i>2)&&(panduan2>0)&&(panduan>0)&&bzjiaodian>0&&byjiaodian>0; 
      meet=4; 
   
   if (px1-px3)*(py2-py3)-(py1-py3)*(px2-px3)>0 
        s=tan(rem(sida2(hindex2(ind(i)-1)),2*pi)-adds); 
   end 
    
   if (px1-px3)*(py2-py3)-(py1-py3)*(px2-px3)<0 
    s=tan(rem(sida3(hindex2(ind(i)+1)),2*pi)+adds); 
   end 
    
%   sdd1=asin((v + s*(V^2*s^2 + V^2 - s^2*u^2 + 2*s*u*v - v^2)^(1/2) - s*u)/(V*(s^2 + 1))); 
%   sdd2=asin(-(s*(V^2*s^2 + V^2 - s^2*u^2 + 2*s*u*v - v^2)^(1/2) - v + s*u)/(V*(s^2 + 1))); 
sdd1=asin((u + s*(V^2*s^2 + V^2 - s^2*v^2 + 2*s*u*v - u^2)^(1/2) - s*v)/(V*(s^2 + 1))); 
  sdd2=asin(-(s*(V^2*s^2 + V^2 - s^2*v^2 + 2*s*u*v - u^2)^(1/2) - u + s*v)/(V*(s^2 + 1))); 
 
  a4s=atan2(y00,x00); 
% if a4s<0 
%     a4s=2*pi+a4s; 
% end 
 
if (s*sdd1>0&&s*sdd2>0)||(s*sdd1<0&&s*sdd2<0) 
 
 
if abs(a4s-sdd1)<abs(a4s-sdd2) 
         sdd=sdd1; 
else 
    sdd=sdd2; 
end 
 
else 
     
    if s*sdd1>0 
        sdd=sdd1; 
    else 
        sdd=sdd2; 
    end 
 
 
 
end 
 
% if abs(linkxielv*laichuanxielv+1)<0.1;  %(ind(i)==ind(i-2))&&i>2&& 
%     sdd=iniangel; 
% end 
 
end 
 
 
end 
 
px1=YY(i,5); 
py1=YY(i,6); 
 
ind(i); 
if (ind(i)==1) %&&Dis(i,1)<3000&&Dis(i,2)<3000&&Dis(i,3)<3000 
    meet=5; 
    if ((px1-px3)*(py2-py3)-(py1-py3)*(px2-px3))<0&&bzjiaodian<0 
        
        s=tan(rem(sida3(hindex2(ind(i))),2*pi)+adds); 
    else 
        s=tan(rem(sida2(hindex2(ind(i))),2*pi)-adds); 
    end 
     
%     sdd1=asin((v + s*(V^2*s^2 + V^2 - s^2*u^2 + 2*s*u*v - v^2)^(1/2) - s*u)/(V*(s^2 + 1))); 
%   sdd2=asin(-(s*(V^2*s^2 + V^2 - s^2*u^2 + 2*s*u*v - v^2)^(1/2) - v + s*u)/(V*(s^2 + 1))); 
sdd1=asin((u + s*(V^2*s^2 + V^2 - s^2*v^2 + 2*s*u*v - u^2)^(1/2) - s*v)/(V*(s^2 + 1))); 
  sdd2=asin(-(s*(V^2*s^2 + V^2 - s^2*v^2 + 2*s*u*v - u^2)^(1/2) - u + s*v)/(V*(s^2 + 1))); 
  a4s=atan2(y00,x00); 
% if a4s<0 
%     a4s=2*pi+a4s; 
% end 
 
if (s*sdd1>0&&s*sdd2>0)||(s*sdd1<0&&s*sdd2<0) 
 
 
if abs(a4s-sdd1)<abs(a4s-sdd2) 
         sdd=sdd1; 
else 
    sdd=sdd2; 
end 
 
else 
     
    if s*sdd1>0 
        sdd=sdd1; 
    else 
        sdd=sdd2; 
    end 
 
 
 
end 
 
% if abs(linkxielv*laichuanxielv+1)<0.1;  %(ind(i)==ind(i-2))&&i>2&& 
%     sdd=iniangel; 
% end     
     
end 
 
 
 
if ind(i)==laichuanshu    %&&Dis(i,1)<3000&&Dis(i,2)<3000&&Dis(i,3)<3000 
    meet=6; 
     if ((px1-px3)*(py2-py3)-(py1-py3)*(px2-px3))>0&&byjiaodian<0 
        s=tan(rem(sida2(hindex2(ind(i))),2*pi)-adds); 
    else 
        s=tan(rem(sida3(hindex2(ind(i))),2*pi)+adds); 
    end 
     
%     sdd1=asin((v + s*(V^2*s^2 + V^2 - s^2*u^2 + 2*s*u*v - v^2)^(1/2) - s*u)/(V*(s^2 + 1))); 
%   sdd2=asin(-(s*(V^2*s^2 + V^2 - s^2*u^2 + 2*s*u*v - v^2)^(1/2) - v + s*u)/(V*(s^2 + 1))); 
sdd1=asin((u + s*(V^2*s^2 + V^2 - s^2*v^2 + 2*s*u*v - u^2)^(1/2) - s*v)/(V*(s^2 + 1))); 
  sdd2=asin(-(s*(V^2*s^2 + V^2 - s^2*v^2 + 2*s*u*v - u^2)^(1/2) - u + s*v)/(V*(s^2 + 1))); 
 
  a4s=atan2(y00,x00); 
% if a4s<0 
%     a4s=2*pi+a4s; 
% end 
 
if (s*sdd1>0&&s*sdd2>0)||(s*sdd1<0&&s*sdd2<0) 
 
 
if abs(a4s-sdd1)<abs(a4s-sdd2) 
         sdd=sdd1; 
else 
    sdd=sdd2; 
end 
 
else 
     
    if s*sdd1>0 
        sdd=sdd1; 
    else 
        sdd=sdd2; 
    end 
 
 
 
end 
% if abs(linkxielv*laichuanxielv+1)<0.1;  %(ind(i)==ind(i-2))&&i>2&& 
%     sdd=iniangel; 
% end        
     
     
end 
 
 
 
%    px1=YY(i,5); 
%   py1=YY(i,6); 
% if (ind(i)~=0)&&(i>2) 
%     if ((px1-px3)*(py2-py3)-(py1-py3)*(px2-px3))>0 
%      s=tan(rem(2*pi,sida2(hindex2(ind(i))))+adds); 
%     else 
%          s=tan(rem(2*pi,sida3(hindex2(ind(i))))+adds); 
%     end 
%      
%     sdd1=asin((v + s*(V^2*s^2 + V^2 - s^2*u^2 + 2*s*u*v - v^2)^(1/2) - s*u)/(V*(s^2 + 1))); 
%   sdd2=asin(-(s*(V^2*s^2 + V^2 - s^2*u^2 + 2*s*u*v - v^2)^(1/2) - v + s*u)/(V*(s^2 + 1))); 
%   a4s=atan2(y00,x00); 
% % if a4s<0 
% %     a4s=2*pi+a4s; 
% % end 
%  
% if abs(a4s-sdd1)<abs(a4s-sdd2) 
%          sdd=sdd1; 
% else 
%     sdd=sdd2; 
% end 
%  
% end    
     
     
     
     
     
     
     
 
 
               
 if (indd~=0)&&(i>25) 
       if (Dis(i,hindex2(indd))>4000)||qianhou(x03,y03,x01,y01,px3,py3)<0;%((Dis(i-20,hindex2(indd)   )>Dis(i-10,hindex2(indd)))&&(Dis(i,hindex2(indd))>Dis(i-10,hindex2(indd))))|| 
         sdd=iniangel; 
     end 
%         linkxielv=eval(['(YY(i,6)','-y',num2str(indd),')/(YY(i,5)','-x',num2str(indd),');']) 
%         laichuanxielv=v/u 
%               abs(linkxielv*laichuanxielv+1) 
%                
%    if abs(linkxielv*laichuanxielv+1)<0.1;  %(ind(i)==ind(i-2))&&i>2&& 
%     sdd=iniangel; 
%    end 
    
 end 
  
%  if meet~=0 
%      meet=meet 
%      %sida2=sida2(hindex2(ind(i))) 
%      sdd1=sdd1 
%      sdd2=sdd2 
%      a4s=a4s 
%      u=u 
%      v=v 
%      V=V 
%      s=s 
%      sdd=sdd 
%       
%      pause 
%  end   
    
  
  
   sdd ; 
  
%deltae=deltae+(Kp+Ti/2+Td)*(sdd-YY(i,3))+(Ti/2-Kp-2*Td)*(sdd-YY(i-1,3))+Td*(sdd-YY(i-2,3)); 
 
 
if i>2 
ui(i+1)=ui(i)+Kp*(1+buchang/Ti+Td/buchang)*(YY01(i)-sdd)-Kp*(1+2*Td/buchang)*(YY01(i-1)-sdd)+Kp*(Td/buchang)*(YY01(i-2)-sdd); 
end  
  
 
hold on 
end 
ind; 
ind2; 
% hindex2(ind) 
  figure  
plot(ui);   
title('own ship rudder angle vary curve')  
  xlabel('X/s') 
  ylabel('Y/rad') 
 
 
figure  
plot(YY01); 
title('own ship velocity direction vary curve')  
  xlabel('X/s') 
  ylabel('Y/rad') 
 
%hold on 
 
 
   
 
 
 
 
 
 
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     ]; 
 
    function  xiangjiaodian=deltajiaodian(a,b,c,d,r) %judge y=ax+b  and  (x-c)^2+(y-d)^2=r^2 whether have crossed 
        xiangjiaodian=(- a^2*c^2 + a^2*r^2 - 2*a*b*c + 2*a*c*d - b^2 + 2*b*d - d^2 + r^2); 
         
    function qianhoupanduan=qianhou(xq,yq,xp,yp,x0,y0)     %judge point (x0,y0) whether have been in front of moving point A(t)(xp,yp),and A(t-1)(xq,yq) 
         
        qianhoupanduan=(yp-xp*(-(xp-xq)/(yp-yq))-(yq+xq*(xp-xq)/(yp-yq)))*(y0-x0*(-(xp-xq)/(yp-yq))-(yq+xq*(xp-xq)/(yp-yq))); 
         
         
    function zuoyoupanduan=zuoyou(xa,ya,xb,yb,xc,yc)    %judge point C(xc,yc) whether have been at left of moving point A(t)(xb,yb),and A(t-1)(xa,ya) 
        zuoyoupanduan=(xa-xc)*(yb-yc)-(ya-yc)*(xb-xc);  %zuoyoupanduan>0 C at left of A(t-1)A(t),zuoyoupanduan<0  C at right of A(t-1)A(t)