【船舶控制】 PID控制算法船舶运动控制【含Matlab源码 3489期】.zip

clear all; close all; clc; warning off global T MM DD n Tu Tuu Bu MA MB nuship_d global T_b Vc btac Vw btaw b global QLQ RLQ gamma global r w_o w_c lambda lambdani K11 K22 K2 K3 K4 Kw e1 T=0.5;%采样周期,采样周期的取值影响特别大 N=1000; n=4;%推进器数量 %% 滤波初始噪声参数设定 surnoise=wgn(1,20000,0.1); swanoise=wgn(1,20000,0.1); yawnoise=wgn(1,20000,0.1)*pi/180; noise=[surnoise;swanoise;yawnoise]; meapos=[0;0;0];%船舶运动初始测量位置 %% %%%%%%%%%% y1=meapos;%滤波器初始高低频叠加值 船舶滤波初值 船舶运动初值 %% %%%%%%%%%%5 yh=[zeros(3,1);0*diag([0.001,0.001,0.0001])*ones(3,1)];%滤波器初始高频相对阻尼 yhm=[zeros(3,1);0*diag([0.001,0.001,0.0001])*ones(3,1)]; nufilter=[0;0;0];%滤波器初始低频速度 etafilter=y1;%滤波器初始低频位置 r=1; T_b=5*diag([100 100 100]);%时间偏差 w_o=0.8*diag([1,1,1]);%波谱的峰值频率 w_c=1.1*diag([1 1 1]);%滤波截断频率 lambda=0.3*diag([1,1,1]);%% 阻尼取得过大，滤波效果不好，阻尼取得过小，滤波会发散 lambdani=1*diag([1,1,1]); K11=-2*w_c.*(lambdani-0.3*diag([1,1,1]))/0.8; K22=2*0.8.*(lambdani-0.3*diag([1,1,1])); K2=w_c; K4=10*diag([0.5 1 1]); K3=5*diag([0.5 1 1]); Kw=2*diag([0.03;0.03;0.3]);%测量噪声的幅值 %% 环境力参数 b=0.0*[1;1;0];%固定环境力 Vc=0.00;%流速 btac=pi/3; Vw=0;%风速 btaw=-pi/2; bb=zeros(3,1);% 初始化 bfilter=zeros(3,1); %% 控制器的一些参数 QLQ=1*diag([1 1 1000 0.00001 0.00001 0.00001 0.01*[1 1 1 1]]); RLQ=0.1*diag([1 1 1 1]); gamma=0.1*diag([1,1,5]); e1=10000000; Juu(1)=0;%初始化 Juu1(1)=0;%初始化 %% 船舶部分 % Tu=1*diag(ones(1,n)); Tu=10*diag([3 3 3 3]); Tuu=1*diag([1 1 1 1]); Bu=[1,0,0,0;0,1,1,1;0,-1.25,1.3,1.5]; u=zeros(n,1); u_c=zeros(n,1); umax=10*[1,0.4,0.4,0.4]'; umin=-10*[1,0.4,0.4,0.4]'; %% 线性矩阵 MA=[49.67 0 0;0 490.07 128.12;0 128.12 155.36]; MB=[699 0 0; 0 699 -699*0.091404;0 -699*0.091404 505.0275]; MM=MA+MB; DD=1000*[0.014 0 0;0 0.102 0.084;0 0.084 0.095]; etaship=y1;%船舶运动初始位置 nuship=[0;0;0];%船舶运动初始速度 nushipp_dot=[0;0;0];% 初始化 nuship_d=[0;0;0];% 初始化 u_1=[0;0;0;0];% 初始化 %% 参考位置 etad=[5;5;5*pi/180]; setspeed=0.05; setpsi=1*pi/450; shutdown_area=0.5; shutdown_angle=5*pi/180; setvx=setspeed*etad(1)/sqrt((etad(1))^2+(etad(2))^2); setvy=setspeed*etad(2)/sqrt((etad(1))^2+(etad(2))^2); setspeed3=[0;0;0]; Ref=zeros(3,1); %%控制 load u_output_ad option=1; option1=3; option2=3; b_dot=zeros(3,1); b_est=zeros(3,1); tic for i=1:N switch(option1) case 1 b=0.1*[1;1;0]; etad=[0;0;5*pi/180];%设定的位置 case 2 etad=[0;0;0*pi/180];%设定的位置 if i<100 Vc=0.01;%流速 end if i>100 & i<550 Vc=0.06;%流速 end if i>550 & i<1000 Vc=0.01;%流速 end case 3 Tu=10*diag([3 3 3 3]); Tu=10*diag([4 4 4 4]); Bu=[1,0,0,0;0,1,1,1;0,-1.25,1.3,1.5]; Vc=0.1*sin(i*T*2*pi/1000);%流速 etad=[5;5;45*pi/180];%设定的位置 end % b=b-inv(2*T_b)*b;%缓变环境力 %参考位置的选取 distance=sqrt((etad(1))^2+(etad(2))^2)-shutdown_area; distance_angle=abs(etad(3))-shutdown_angle; %%求取周期划分 if distance<=0 disaccelerate=setspeed^2/2/(sqrt((etad(1))^2+(etad(2))^2)); NN_fanalxy=setspeed/disaccelerate/T; else disaccelerate=setspeed^2/(2*shutdown_area); NN_xy=distance/setspeed/T; NN_fanalxy=NN_xy+setspeed/abs(disaccelerate)/T; end %%%%%%%% if distance_angle<=0 disaccelerate_angle=setpsi^2/(2*abs(etad(3))); NN_fanalz=setpsi/disaccelerate_angle/T; else disaccelerate_angle=setpsi^2/(2*shutdown_angle); NN_z=distance_angle/setpsi/T; NN_fanalz=NN_z+setpsi/abs(disaccelerate_angle)/T; end %%速度设定 if distance<=0 & i<=NN_fanalxy dis_ax=disaccelerate*etad(1)/sqrt((etad(1))^2+(etad(2))^2); dis_ay=disaccelerate*etad(2)/sqrt((etad(1))^2+(etad(2))^2); setspeed3(1)=setvx+sign(-etad(1))*dis_ax*i*T; setspeed3(2)=setvy+sign(-etad(2))*dis_ay*i*T; end if distance<=0 & i>NN_fanalxy setspeed3(1)=0; setspeed3(2)=0; end if distance>0 & i>=NN_fanalxy setspeed3(1)=0; setspeed3(2)=0; end if distance>0 & i<=NN_xy setspeed3(1)=setvx; setspeed3(2)=setvy; end if distance>0 & i>NN_xy &i<NN_fanalxy dis_ax=disaccelerate*etad(1)/sqrt((etad(1))^2+(etad(2))^2); dis_ay=disaccelerate*etad(2)/sqrt((etad(1))^2+(etad(2))^2); setspeed3(1)=setvx+sign(-etad(1))*dis_ax*(i*T-NN_xy*T); setspeed3(2)=setvy+sign(-etad(2))*dis_ay*(i*T-NN_xy*T); end %%%%%%%%%%%% if distance_angle<=0 & i>NN_fanalz setspeed3(3)=0; end if distance_angle<=0 & i<=NN_fanalz setspeed3(3)=setpsi+sign(-etad(3))*disaccelerate_angle*i*T; end if distance_angle>0 & i>=NN_fanalz setspeed3(3)=0; end if distance_angle>0 & i<=NN_z setspeed3(3)=setpsi; end if distance_angle>0 & i>NN_z & i<NN_fanalz setspeed3(3)=setpsi+sign(-etad(3))*disaccelerate_angle*(i*T-NN_z*T); end Ref=Ref+setspeed3*T; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% [etaship,nuship,u]=mshipmotfunction(nuship,etaship,u,u_c,nushipp_dot); Rship=[cos(etaship(3)) -sin(etaship(3)) 0;sin(etaship(3)) cos(etaship(3)) 0;0 0 1]; Vcb=[Vc*cos(btac-etaship(3));Vc*sin(btac-etaship(3));0]; S=[0 -1 0;1 0 0;0 0 0]; vcb_dot=nuship(3)*S'*Vcb; e_rb(:,i)=Rship*(DD*Vcb+MA*vcb_dot); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % meapos=measure(etaship,yh,noise(:,i)); % meapos=etaship+[0.05;0.05;1].*noise(:,i); % meapos=etaship+1*[0.03;0.03;0.3].*noise(:,i)+1*[0.2;0.2;1.2/57.3]*sin(0.8*i*T);%+1.5*[0.2;0.2;1.2/57.3]*sin(1.0*i*T) [y,yhm]=measure(etaship,yhm,noise(:,i)); meapos=y; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% [lowestpos,speed,highestpos,bfilter,y1,yh,nufilter,etafilter]=Passivefilter(Bu*u,meapos,bfilter,y1,yh,nufilter,etafilter); bb=bfilter; %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% [u_c,bb]=MPLOB_controller(meapos,lowestpos,speed,u,Ref(1:2),Ref(3),bb); u_c=0.5*(umax+umin-abs(umax-u_c)+abs(u_c-umin)); tau=Bu*u_c; u_1=u_c; if i>=2 eta_d(:,i-1)=Ref(1:2); psid(:,i-1)=Ref(3); u_output(:,i-1)=u_c; e_b(:,i-1)=bb; u1(:,i-1)=u(1); u2(:,i-1)=u(2); u3(:,i-1)=u(3); u4(:,i-1)=u(4); Juu(i)=Juu(:,i-1)+u'*u; if option1==1 Juu1(i)=Juu1(:,i-1)+u_output_ad(:,i-1)'*u_output_ad(:,i-1); end %% 考虑测量的噪声 X_ref(:,i)=lowestpos(1); Y_ref(:,i)=lowestpos(2); Z_ref(:,i)=lowestpos(3)*180/pi; nuship_i(:,i)=speed; %% 存储测量值 %% 存储 tausurge(:,i)=tau(1); tausway(:,i)=tau(2); tauyaw(:,i)=tau(3); %测量位置存成数组 surmeapos(i)=meapos(1); swameapos(i)=meapos(2); yawmeapos(i)=meapos(3)*180/pi; %低频估算位置存成数组 surgeest(i-1)=lowestpos(1); swaest(i-1)=lowestpos(2); yawest(i-1)=lowestpos(3)*180/pi; %速度存储 nuship_i(:,i-1)=nuship; end end toc if option2==1 figure(1) plot(swameapos,surmeapos,'b') hold on plot(swaest,surgeest,'--r','LineWidth',1.5) legend('船舶测量北东位置','船舶低频北东位置',2) % title('Path following of the model ship') xlabel('Y 东向 (m)','FontSize',12); ylabel('X 北向 (m)','FontSize',12); h=gca; set(h,'FontSize',12); grid on figure(2) plot(0:0.5:length(surmeapos)*0.5-0.5,surmeapos,'-b') hold on plot(0:0.5:length(surgeest)*0.5-0.5,surgeest,'--r','LineWidth',1.5) legend('测量位置','低频位置','FontSize',12) % title('Path following of the model ship') xlabel('时间 (s)','FontSize',12);