www.pudn.com > SHIPCONTROL.rar > mainopticontrol20121005foralex.m, change:2012-10-06,size:14458b


%增广状态罚函数法实现船舶避碰最短路径控制程序 
%2012.10.1 xiong yong 熊勇 qq:76289035 
%bear_brave@163.com 
function mainopticontrol20121005foralex 
clear all 
clc 
diedai=5; 
buchang=1; 
timechang=100; 
deltaini=zeros(1,timechang); 
chujiao=0.2; 
y0=[0; 0; chujiao; 0; 9.2; 0; 0; 0; 0]; 
vlami=5; 
xjiyi=zeros(timechang,9); 
xjiyi2=zeros(timechang,9); 
mp=zeros(6,1); 
mp(2)=5; 
mp(3)=1500; 
mp(4)=4; 
mp(5)=1500; 
mp(6)=600; 
af=7; 
mu=0.05; 
ddp=pi/6; 
mjo=zeros(1,diedai); 
kx=zeros(1,timechang); 
ky=zeros(1,timechang); 
deltaini=zeros(1,timechang); 
deltaini(1,:)=0.3; 
for i=1:diedai 
   jisuanzhibiao=i 
    for j=1:2:timechang       
  
    mp(1)=deltaini(j); 
     
    tspan=[(j-1)*buchang, j*buchang, (j+1)*buchang]; 
    [t,yy]=ode45(@xitongf,tspan,y0,[],mp); 
    y0=yy(end,:); 
    xjiyi(j:j+1,:)=yy(2:3,:) 
    end 
 
    y02=zeros(1,9); 
     y02(1)=(-1)*vlami*xjiyi(end,2)*xjiyi(end,1)^(-2) 
    y02(2)=vlami*xjiyi(end,1)^(-1)  
     
     
     for j2=timechang:-2:1     
         mp(1)=deltaini(j2);  
    tspan2=[(j2+1)*buchang,j2*buchang,(j2-1)*buchang]; 
    [t,yy]=ode45(@xietong,tspan2,y02,[],mp, xjiyi(j2,:)) 
    y02=yy(end,:); 
    xjiyi2(j2+1,:)=yy(1,:); 
    xjiyi2(j2,:)=yy(2,:); 
     end 
      
      
     for j3=1:timechang 
     mp(1)=deltaini(j3); 
       phamilton(j3*buchang,xjiyi(j3,:),xjiyi2(j3,:),mp) 
      deltaini(j3)=deltaini(j3)-af*phamilton(j3*buchang,xjiyi(j3,:),xjiyi2(j3,:),mp); 
     
     if deltaini(j3)<=-ddp  
         deltaini(j3)=-ddp; 
     elseif deltaini(j3)>=ddp  
         deltaini(j3)=ddp; 
     end         
          
     end 
      
     jo=0; 
     for j4=1:timechang 
          
     jo=jo+sqrt(xjiyi(j4,1)^2+xjiyi(j4,2).^2)*buchang; 
     end 
      
     mjo(i)=jo; 
      
     if (i-1)>0&abs(mjo(i-1)-mjo(i))/abs(mjo(i))<=mu; 
     break 
     end     
      
end 
 
 
 
for j4=1:timechang 
          
     kx(j4)=mp(2)*j4*buchang+mp(3); 
     ky(j4)=mp(4)*j4*buchang+mp(5);                                           
                                            
     end   
 
 
 
plot(xjiyi(:,1),xjiyi(:,2)) 
hold on 
plot(kx,ky) 
figure 
 
plot(mjo) 
 
figure 
 
plot(deltaini) 
 
function  dy=xitongf(t,y,mp) 
 
dy=zeros(9,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); 
 
delta=mp(1); 
a1=mp(2); 
a01=mp(3); 
b1=mp(4); 
b01=mp(5); 
r1=mp(6); 
 
 
 
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; 
 
 
 
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; 
 
 
    c1=1; 
 
 
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); 
 
if ((y(1)-a1*t-a01)^2+(y(2)-b1*t-b01)^2-r1^2)^2>=0 
h1=0; 
else 
h1=2000; 
end 
 
 
 
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+(m+m22)*y(6)*y(7))/(m+m11); 
dy(6)=(YH+YR-(m+m11)*y(5)*y(7))/(m+m22); 
dy(7)=(NH+NR)/(IZ+m66); 
dy(8)=(KH+KR)/(m*(B^2+4*ZG^2)/12); 
dy(9)=((y(1)-a1*t-a01)^2+(y(2)-b1*t-b01)^2-r1^2)^2*h1; 
 
function  dlami=xietong(t,y,mp,zhichuan) 
 
% dlami=zeros(9,1); 
x0=zhichuan(1); 
y0=zhichuan(2); 
bosai=zhichuan(3); 
FAI=zhichuan(4); 
u=zhichuan(5); 
v=zhichuan(6); 
r=zhichuan(7); 
p=zhichuan(8); 
x9=zhichuan(9); 
 
delta=mp(1); 
a1=mp(2); 
a01=mp(3); 
b1=mp(4); 
b01=mp(5); 
r1=mp(6); 
 
 
 
lami1=y(1); 
lami2=y(2); 
lami3=y(3); 
lami4=y(4); 
lami5=y(5); 
lami6=y(6); 
lami7=y(7); 
lami8=y(8); 
lami9=y(9) 
 
 
 
if ((x0-a1*t-a01)^2+(y0-b1*t-b01)^2-r1^2)^2>=0 
h1=0; 
else 
h1=2000; 
end 
syms H x0 y0 bosai FAI u v r p delta lami1 lami2 lami3 lami4 lami5 lami6 lami7 lami8 lami9 a1 a01 b1 b01 r1 t h1; 
H=(x0^2+y0^2)^(1/2)+lami1*(u*cos(bosai)-1.*v*cos(FAI)*sin(bosai))+lami2*(u*sin(bosai)-1.*v*cos(FAI)*cos(bosai))+lami3*r*cos(FAI)+lami4*p+lami5*(1.*v*r-.1e-5*u^4+.8e-5*u^3-.2e-3*u^2-.5e-3*u+.2e-1+.6e-5*u^3*(1.-.2*exp(-4.*(-1.*atan(v/u)-.1e3*r/(u^2+v^2)^(1/2))^2))^3-.2e-3*u^2*(1.-.2*exp(-4.*(-1.*atan(v/u)-.1e3*r/(u^2+v^2)^(1/2))^2))^2-.3e-3*u*(1.-.2*exp(-4.*(-1.*atan(v/u)-.1e3*r/(u^2+v^2)^(1/2))^2))+.1e-7*(.3e2*u^2*(1.-.2*exp(-4.*(-1.*atan(v/u)-.1e3*r/(u^2+v^2)^(1/2))^2))^2*(.1e4+.9e3*(.4-.5*u*(1.-.2*exp(-4.*(-1.*atan(v/u)-.1e3*r/(u^2+v^2)^(1/2))^2)))*(.7+2.*u*(1.-.2*exp(-4.*(-1.*atan(v/u)-.1e3*r/(u^2+v^2)^(1/2))^2)))/u^2/(1.-.2*exp(-4.*(-1.*atan(v/u)-.1e3*r/(u^2+v^2)^(1/2))^2))^2)+.4e5*(.5*v-.1e3*r)^2)*sin(-1.*delta-3.+2.*u+atan(.4e2*(.5*v-.1e3*r)/u/(1.-.2*exp(-4.*(-1.*atan(v/u)-.1e3*r/(u^2+v^2)^(1/2))^2))/(.1e4+.9e3*(.4-.5*u*(1.-.2*exp(-4.*(-1.*atan(v/u)-.1e3*r/(u^2+v^2)^(1/2))^2)))*(.7+2.*u*(1.-.2*exp(-4.*(-1.*atan(v/u)-.1e3*r/(u^2+v^2)^(1/2))^2)))/u^2/(1.-.2*exp(-4.*(-1.*atan(v/u)-.1e3*r/(u^2+v^2)^(1/2))^2))^2)^(1/2)))*sin(delta))+lami6*(-.4e-2*(u^2+v^2)^(1/2)*v+.3*(u^2+v^2)^(1/2)*r-.1e-1*v*abs(v)-1.*r*abs(v)-.6e2*r*abs(r)-.2e-7*(.3e2*u^2*(1.-.2*exp(-4.*(-1.*atan(v/u)-.1e3*r/(u^2+v^2)^(1/2))^2))^2*(.1e4+.9e3*(.4-.5*u*(1.-.2*exp(-4.*(-1.*atan(v/u)-.1e3*r/(u^2+v^2)^(1/2))^2)))*(.7+2.*u*(1.-.2*exp(-4.*(-1.*atan(v/u)-.1e3*r/(u^2+v^2)^(1/2))^2)))/u^2/(1.-.2*exp(-4.*(-1.*atan(v/u)-.1e3*r/(u^2+v^2)^(1/2))^2))^2)+.4e5*(.5*v-.1e3*r)^2)*sin(-1.*delta-3.+2.*u+atan(.4e2*(.5*v-.1e3*r)/u/(1.-.2*exp(-4.*(-1.*atan(v/u)-.1e3*r/(u^2+v^2)^(1/2))^2))/(.1e4+.9e3*(.4-.5*u*(1.-.2*exp(-4.*(-1.*atan(v/u)-.1e3*r/(u^2+v^2)^(1/2))^2)))*(.7+2.*u*(1.-.2*exp(-4.*(-1.*atan(v/u)-.1e3*r/(u^2+v^2)^(1/2))^2)))/u^2/(1.-.2*exp(-4.*(-1.*atan(v/u)-.1e3*r/(u^2+v^2)^(1/2))^2))^2)^(1/2)))*cos(delta)-.8*u*r)+lami7*(-.8e-4*(u^2+v^2)^(1/2)*v-.1e-1*(u^2+v^2)^(1/2)*r-.2*v^2*r-5.*r*abs(r)-6.*v*r^2+.4e-11*(-.3e7*u^2-.3e7*v^2)*FAI-.2e-3*(u^2+v^2)^(1/2)*v*abs(FAI)+.3e-1*(u^2+v^2)^(1/2)*r*abs(FAI)+.8e-9*(.3e2*u^2*(1.-.2*exp(-4.*(-1.*atan(v/u)-.1e3*r/(u^2+v^2)^(1/2))^2))^2*(.1e4+.9e3*(.4-.5*u*(1.-.2*exp(-4.*(-1.*atan(v/u)-.1e3*r/(u^2+v^2)^(1/2))^2)))*(.7+2.*u*(1.-.2*exp(-4.*(-1.*atan(v/u)-.1e3*r/(u^2+v^2)^(1/2))^2)))/u^2/(1.-.2*exp(-4.*(-1.*atan(v/u)-.1e3*r/(u^2+v^2)^(1/2))^2))^2)+.4e5*(.5*v-.1e3*r)^2)*sin(-1.*delta-3.+2.*u+atan(.4e2*(.5*v-.1e3*r)/u/(1.-.2*exp(-4.*(-1.*atan(v/u)-.1e3*r/(u^2+v^2)^(1/2))^2))/(.1e4+.9e3*(.4-.5*u*(1.-.2*exp(-4.*(-1.*atan(v/u)-.1e3*r/(u^2+v^2)^(1/2))^2)))*(.7+2.*u*(1.-.2*exp(-4.*(-1.*atan(v/u)-.1e3*r/(u^2+v^2)^(1/2))^2)))/u^2/(1.-.2*exp(-4.*(-1.*atan(v/u)-.1e3*r/(u^2+v^2)^(1/2))^2))^2)^(1/2)))*cos(delta))+lami8*(-.5e-1*p-.2*sin(FAI)+.4e-3*(u^2+v^2)^(1/2)*v-.3e-1*(u^2+v^2)^(1/2)*r+.1e-2*v*abs(v)+.1*r*abs(v)+6.*r*abs(r)-.2e-8*(.3e2*u^2*(1.-.2*exp(-4.*(-1.*atan(v/u)-.1e3*r/(u^2+v^2)^(1/2))^2))^2*(.1e4+.9e3*(.4-.5*u*(1.-.2*exp(-4.*(-1.*atan(v/u)-.1e3*r/(u^2+v^2)^(1/2))^2)))*(.7+2.*u*(1.-.2*exp(-4.*(-1.*atan(v/u)-.1e3*r/(u^2+v^2)^(1/2))^2)))/u^2/(1.-.2*exp(-4.*(-1.*atan(v/u)-.1e3*r/(u^2+v^2)^(1/2))^2))^2)+.4e5*(.5*v-.1e3*r)^2)*sin(-1.*delta-3.+2.*u+atan(.4e2*(.5*v-.1e3*r)/u/(1.-.2*exp(-4.*(-1.*atan(v/u)-.1e3*r/(u^2+v^2)^(1/2))^2))/(.1e4+.9e3*(.4-.5*u*(1.-.2*exp(-4.*(-1.*atan(v/u)-.1e3*r/(u^2+v^2)^(1/2))^2)))*(.7+2.*u*(1.-.2*exp(-4.*(-1.*atan(v/u)-.1e3*r/(u^2+v^2)^(1/2))^2)))/u^2/(1.-.2*exp(-4.*(-1.*atan(v/u)-.1e3*r/(u^2+v^2)^(1/2))^2))^2)^(1/2)))*cos(delta))+lami9*((x0-a1*t-a01)^2+(y0-b1*t-b01)^2-r1^2)^2*h1; 
% dlami=sym(dlami); 
eval(['dlami(1,1)=',char(-diff(H,x0)),';']) 
eval(['dlami(2,1)=',char(-diff(H,y0)),';']) 
eval(['dlami(3,1)=',char(-diff(H,bosai)),';']) 
eval(['dlami(4,1)=',char(-diff(H,FAI)),';']) 
eval(['dlami(5,1)=',char(-diff(H,u)),';']) 
eval(['dlami(6,1)=',char(-diff(H,v)),';']) 
eval(['dlami(7,1)=',char(-diff(H,r)),';']) 
eval(['dlami(8,1)=',char(-diff(H,p)),';']) 
% eval(['dlami(9)=','0;']) 
dlami(9,1)=0; 
 
% dlami(2)=double(-diff(H,y0)) 
% dlami(3)=double(-diff(H,bosai)); 
% dlami(4)=double(-diff(H,FAI)); 
% dlami(5)=double(-diff(H,u)); 
% dlami(6)=double(-diff(H,v)); 
% dlami(7)=double(-diff(H,r)); 
% dlami(8)=double(-diff(H,p)); 
% dlami(9)=0; 
 
% dlami(1)=double(dlami(1)); 
% dlami(2)=double(dlami(2)); 
% dlami(3)=double(dlami(3)); 
% dlami(4)=double(dlami(4)); 
% dlami(5)=double(dlami(5)); 
% dlami(6)=double(dlami(6)); 
% dlami(7)=double(dlami(7)); 
% dlami(8)=double(dlami(8)); 
% dlami(9)=double(dlami(9)); 
 
function y=phamilton(t,chuan1,chuan2,mp) 
x0=chuan1(1); 
y0=chuan1(2); 
bosai=chuan1(3); 
FAI=chuan1(4); 
u=chuan1(5); 
v=chuan1(6); 
r=chuan1(7); 
p=chuan1(8); 
delta=mp(1); 
a1=mp(2); 
a01=mp(3); 
b1=mp(4); 
b01=mp(5); 
r1=mp(6); 
lami1=chuan2(1); 
lami2=chuan2(2); 
lami3=chuan2(3); 
lami4=chuan2(4); 
lami5=chuan2(5); 
lami6=chuan2(6); 
lami7=chuan2(7); 
lami8=chuan2(8); 
lami9=chuan2(9); 
 
 
 
 
if ((x0-a1*t-a01)^2+(y0-b1*t-b01)^2-r1^2)^2>=0 
h1=0; 
else 
h1=2000; 
end 
H=(x0^2+y0^2)^(1/2)+lami1*(u*cos(bosai)-1.*v*cos(FAI)*sin(bosai))+lami2*(u*sin(bosai)-1.*v*cos(FAI)*cos(bosai))+lami3*r*cos(FAI)+lami4*p+lami5*(1.*v*r-.1e-5*u^4+.8e-5*u^3-.2e-3*u^2-.5e-3*u+.2e-1+.6e-5*u^3*(1.-.2*exp(-4.*(-1.*atan(v/u)-.1e3*r/(u^2+v^2)^(1/2))^2))^3-.2e-3*u^2*(1.-.2*exp(-4.*(-1.*atan(v/u)-.1e3*r/(u^2+v^2)^(1/2))^2))^2-.3e-3*u*(1.-.2*exp(-4.*(-1.*atan(v/u)-.1e3*r/(u^2+v^2)^(1/2))^2))+.1e-7*(.3e2*u^2*(1.-.2*exp(-4.*(-1.*atan(v/u)-.1e3*r/(u^2+v^2)^(1/2))^2))^2*(.1e4+.9e3*(.4-.5*u*(1.-.2*exp(-4.*(-1.*atan(v/u)-.1e3*r/(u^2+v^2)^(1/2))^2)))*(.7+2.*u*(1.-.2*exp(-4.*(-1.*atan(v/u)-.1e3*r/(u^2+v^2)^(1/2))^2)))/u^2/(1.-.2*exp(-4.*(-1.*atan(v/u)-.1e3*r/(u^2+v^2)^(1/2))^2))^2)+.4e5*(.5*v-.1e3*r)^2)*sin(-1.*delta-3.+2.*u+atan(.4e2*(.5*v-.1e3*r)/u/(1.-.2*exp(-4.*(-1.*atan(v/u)-.1e3*r/(u^2+v^2)^(1/2))^2))/(.1e4+.9e3*(.4-.5*u*(1.-.2*exp(-4.*(-1.*atan(v/u)-.1e3*r/(u^2+v^2)^(1/2))^2)))*(.7+2.*u*(1.-.2*exp(-4.*(-1.*atan(v/u)-.1e3*r/(u^2+v^2)^(1/2))^2)))/u^2/(1.-.2*exp(-4.*(-1.*atan(v/u)-.1e3*r/(u^2+v^2)^(1/2))^2))^2)^(1/2)))*sin(delta))+lami6*(-.4e-2*(u^2+v^2)^(1/2)*v+.3*(u^2+v^2)^(1/2)*r-.1e-1*v*abs(v)-1.*r*abs(v)-.6e2*r*abs(r)-.2e-7*(.3e2*u^2*(1.-.2*exp(-4.*(-1.*atan(v/u)-.1e3*r/(u^2+v^2)^(1/2))^2))^2*(.1e4+.9e3*(.4-.5*u*(1.-.2*exp(-4.*(-1.*atan(v/u)-.1e3*r/(u^2+v^2)^(1/2))^2)))*(.7+2.*u*(1.-.2*exp(-4.*(-1.*atan(v/u)-.1e3*r/(u^2+v^2)^(1/2))^2)))/u^2/(1.-.2*exp(-4.*(-1.*atan(v/u)-.1e3*r/(u^2+v^2)^(1/2))^2))^2)+.4e5*(.5*v-.1e3*r)^2)*sin(-1.*delta-3.+2.*u+atan(.4e2*(.5*v-.1e3*r)/u/(1.-.2*exp(-4.*(-1.*atan(v/u)-.1e3*r/(u^2+v^2)^(1/2))^2))/(.1e4+.9e3*(.4-.5*u*(1.-.2*exp(-4.*(-1.*atan(v/u)-.1e3*r/(u^2+v^2)^(1/2))^2)))*(.7+2.*u*(1.-.2*exp(-4.*(-1.*atan(v/u)-.1e3*r/(u^2+v^2)^(1/2))^2)))/u^2/(1.-.2*exp(-4.*(-1.*atan(v/u)-.1e3*r/(u^2+v^2)^(1/2))^2))^2)^(1/2)))*cos(delta)-.8*u*r)+lami7*(-.8e-4*(u^2+v^2)^(1/2)*v-.1e-1*(u^2+v^2)^(1/2)*r-.2*v^2*r-5.*r*abs(r)-6.*v*r^2+.4e-11*(-.3e7*u^2-.3e7*v^2)*FAI-.2e-3*(u^2+v^2)^(1/2)*v*abs(FAI)+.3e-1*(u^2+v^2)^(1/2)*r*abs(FAI)+.8e-9*(.3e2*u^2*(1.-.2*exp(-4.*(-1.*atan(v/u)-.1e3*r/(u^2+v^2)^(1/2))^2))^2*(.1e4+.9e3*(.4-.5*u*(1.-.2*exp(-4.*(-1.*atan(v/u)-.1e3*r/(u^2+v^2)^(1/2))^2)))*(.7+2.*u*(1.-.2*exp(-4.*(-1.*atan(v/u)-.1e3*r/(u^2+v^2)^(1/2))^2)))/u^2/(1.-.2*exp(-4.*(-1.*atan(v/u)-.1e3*r/(u^2+v^2)^(1/2))^2))^2)+.4e5*(.5*v-.1e3*r)^2)*sin(-1.*delta-3.+2.*u+atan(.4e2*(.5*v-.1e3*r)/u/(1.-.2*exp(-4.*(-1.*atan(v/u)-.1e3*r/(u^2+v^2)^(1/2))^2))/(.1e4+.9e3*(.4-.5*u*(1.-.2*exp(-4.*(-1.*atan(v/u)-.1e3*r/(u^2+v^2)^(1/2))^2)))*(.7+2.*u*(1.-.2*exp(-4.*(-1.*atan(v/u)-.1e3*r/(u^2+v^2)^(1/2))^2)))/u^2/(1.-.2*exp(-4.*(-1.*atan(v/u)-.1e3*r/(u^2+v^2)^(1/2))^2))^2)^(1/2)))*cos(delta))+lami8*(-.5e-1*p-.2*sin(FAI)+.4e-3*(u^2+v^2)^(1/2)*v-.3e-1*(u^2+v^2)^(1/2)*r+.1e-2*v*abs(v)+.1*r*abs(v)+6.*r*abs(r)-.2e-8*(.3e2*u^2*(1.-.2*exp(-4.*(-1.*atan(v/u)-.1e3*r/(u^2+v^2)^(1/2))^2))^2*(.1e4+.9e3*(.4-.5*u*(1.-.2*exp(-4.*(-1.*atan(v/u)-.1e3*r/(u^2+v^2)^(1/2))^2)))*(.7+2.*u*(1.-.2*exp(-4.*(-1.*atan(v/u)-.1e3*r/(u^2+v^2)^(1/2))^2)))/u^2/(1.-.2*exp(-4.*(-1.*atan(v/u)-.1e3*r/(u^2+v^2)^(1/2))^2))^2)+.4e5*(.5*v-.1e3*r)^2)*sin(-1.*delta-3.+2.*u+atan(.4e2*(.5*v-.1e3*r)/u/(1.-.2*exp(-4.*(-1.*atan(v/u)-.1e3*r/(u^2+v^2)^(1/2))^2))/(.1e4+.9e3*(.4-.5*u*(1.-.2*exp(-4.*(-1.*atan(v/u)-.1e3*r/(u^2+v^2)^(1/2))^2)))*(.7+2.*u*(1.-.2*exp(-4.*(-1.*atan(v/u)-.1e3*r/(u^2+v^2)^(1/2))^2)))/u^2/(1.-.2*exp(-4.*(-1.*atan(v/u)-.1e3*r/(u^2+v^2)^(1/2))^2))^2)^(1/2)))*cos(delta))+lami9*((x0-a1*t-a01)^2+(y0-b1*t-b01)^2-r1^2)^2*h1; 
H=sym(H); 
y=double(diff(H,delta));