基于MFAC无模型自适应控制的无人艇航向控制

2023-12-12 23:01:57

        MFAC 算法基本原理是在每个工作点处，建立非线性系统等价的动态线性数据模型，利用受控系统的I/O数据在线估计系统的伪偏导数，然后设计加权一步向前的控制器，进而实现非线性系统数据驱动的无模型自适应控制。MFAC 有三种不同动态线性化方法的算法设计，即基于紧格式动态线性化的无模型自适应控制（Compact Form Dynamic Linearization based MFAC，CFDL-MFAC），基于偏格式动态线性化的无模型自适应控制（Partial  Form  Dynamic  Linearization  based MFAC，PFDL-MFAC）以及基于全格式动态线性化的无模型自适应控制（Full  Form Dynamic Linearization based MFAC，FFDL-MFAC）。

       本文控制方法使用紧格式动态线性化的无模型自适应控制。

参考文献：《无人艇重定义无模型自适应艏向控制方法与试验》

控制律：

基于MFAC无模型自适应控制的无人艇航向控制

基于MFAC无模型自适应控制的无人艇航向控制

控制原理：

基于MFAC无模型自适应控制的无人艇航向控制

 MATLAB 实现：

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 使用CFDL-MFAC(紧格式动态线性化方法)进行无人艇航向控制
% 
% 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
close all;
clear;
%% 算法参数设定
%仿真时间步长
ts=0.5;
%非线性KT方程参数k,t,α
Kit=0.701;
Tit=0.332;
afa=0.001;
% Kit=0.18;
% Tit=4.65;
% afa=1.07;
% Kit=2.36;
% Tit=5.489;
% afa=0.000094;

%风浪流干扰参数
Wn=0.958;
yip=0.9;

%风流浪干扰
%浪干扰
langrr=wgn(1,800,(0.1/0.1),'linear');

%4级海况
w0=0.808;                               %波浪频率
tw=6;                               %波浪周期
kw=0.255;                               %增益常数
citam=0.527;                            %波浪强度系数
yipp=0.3;                              %阻尼系数

%MFAC算法参数设置
nata=0.5;                %λ
miu=1;                   %μ
yita=0.1;                %η
ru=1;                    %ρ
Kz=8;                    %K1
yipsl=0.0000001;         %ε

PI=3.1415926;
%% 参数初始化
num=[kw 0];
den=[1 (2*yipp*w0) (w0*w0)];
hs=tf(num,den);

tt=0.1:0.1:80;


[langr,xr]=lsim(hs,langrr,tt);

Y0 = [0;0];%初值
t = 0.1:ts:80;
t0=0;
Y = cell(1,length(t));
z = zeros(2,length(t));

Y0_m=[0;0];   %参考模型
Y_m = cell(1,length(t));
z_m = zeros(2,length(t));
kp=1;
kd=1;
% kp=0.98;
% kd=0.95;
ki=0.0;
error_1=0;error_2=0;
u_1=0;u_2=0;
y_1=0;y_2=0;
r_1=0;r_2=0;
sf_1=0.1;

%% 循环
for k=1:200
%     d=(langr(k,1)+0.5)*PI/180;
    d=0;
    time(k)=k*ts;
     M=0;
     if M==1
     rin(k)=30;
     fai_r=30;
     if time(k)>=20&&time(k)<40
         rin(k)=0;
         fai_r=0;
     else
         if time(k)>=40&&time(k)<60
             rin(k)=30;
             fai_r=30;
         else
            if time(k)>=60&&time(k)<80
                rin(k)=0;
                fai_r=0;
            end
         end
     end
     else
         rin(k)=PI/3;
         fai_r=PI/3;
     end
     
   Y_m{k}=rkfa_m( ts,t0,Y0_m,Wn,yip,fai_r);                                    %龙格库塔法解航向一阶非线性方程
   z_m(1,k) = Y_m{k}(1);                                                     %参考fai
   z_m(2,k) = Y_m{k}(2);
   yrout(k)=z_m(1,k);
%    yr(1,k)=z_m(2,k);                                                       %参考r
%    yr(2,k)=(yr(1,k)-dyr_1)/ts;                                             %参考r'
%    u_m(k)=(Tit*yr(2,k)+yr(1,k)+afa*yr(1,k)*yr(1,k)*yr(1,k))/Kit;   
     
     
   Y{k}=rkfa_GR( ts,t0,Y0,Kit,Tit,u_1,afa,d);                                    %龙格库塔法解航向一阶非线性方程
   z(1,k) = Y{k}(1);                                                        %fai
   z(2,k) = Y{k}(2);                                                        %r
   yout(k)=z(1,k);
   r(k)=z(2,k);
   error(k)=rin(k)-z(1,k);
   
   M2=1;                   %0:PD控制 1：MFAC控制
   if M2==0
   %PD控制部分
   z_error(k)=yrout(k)-z(1,k);
   m=1;
   if m==1
   %位置式pid
   u(k)=kp*error(k)+kd*(error(k)-error_1)/ts;
   else
   %增量式pid
   xc(1)=error(k)-error_1;             %Calculating P
   xc(2)=error(k)-2*error_1+error_2;   %Calculating D
   xc(3)=error(k);                     %Calculating I
   du(k)=kp*xc(1)+kd*xc(2)/ts+ki*xc(3)*ts;
   if du(k)>PI/6
   du(k)=PI/6;
    end   
    if du(k)<-PI/6
       du(k)=-PI/6;
    end  
   u(k)=u_1+du(k);
   end

   else
%MFAC方法

  sf(k)=sf_1+((yita*(u_1-u_2))*((yout(k)-y_1+(Kz*r(k)-(Kz*r_1)))-(sf_1*(u_1-u_2))))/(miu+(u_1-u_2)*(u_1-u_2));
  if ((abs(sf(k))<yipsl)||(abs(u_1-u_2)<yipsl))
      sf(k)=sf_1;
  end
  u(k)=u_1+((ru*sf(k))*(rin(k)-(yout(k)+Kz*r(k))))/(nata+(sf(k)*sf(k)));
  sf_2=sf_1;sf_1=sf(k);
   end  
  
   if u(k)>PI/6
   u(k)=PI/6;
    end   
    if u(k)<-PI/6
       u(k)=-PI/6;
    end
    

     %舵速控制
%     if (((u(k)-u_1)/ts)>10)
%         u(k)=10*ts+u_1;
%     else
%         if(((u(k)-u_1)/ts)<-10)
%             u(k)=-10*ts+u_1;
%         end
%     end
error_2=error_1;
error_1=error(k);
u_2=u_1;u_1=u(k);
y_2=y_1;y_1=yout(k);
r_2=r_1;r_1=r(k);

Y0=Y{k};
z_1=z(2,k);
   Y0_m=Y_m{k};
end

%% 结果可视化
kk=1:199;
figure;   
plot(kk*ts,yout(1:199)*180/PI,'r-',kk*ts,rin(1:199)*180/PI,'--');
title('船舶实际航向与设定航向');
xlabel('时间 \fontname{Times New Roman}t/s');
legend('实际航向','设定航向') 
ylabel('角度 \fontname{Times New Roman}/° ');

figure;   
plot(kk*ts,u(1:199)*180/PI,'r-');
title('舵角');
xlabel('时间 \fontname{Times New Roman}t/s');
ylabel('角度 \fontname{Times New Roman}/° ');


figure
plot(kk*ts,error(1:199)*180/PI,'r');
title('航向误差');
xlabel('时间 \fontname{Times New Roman}t/s');
ylabel('角度 \fontname{Times New Roman}/° ');

% figure
% plot(kk*ts,rin(1:199),'--',kk*ts,yrout(1:199),'b',kk*ts,yout(1:199),'r-',kk*ts,u(1:199),'g');
% title('船舶航向角与舵角');
% axis([0 80 -20 40])
% xlabel('时间 \fontname{Times New Roman}t/s');
% legend('设定航向角','参考航向角','实际航向角','实际舵角')
% ylabel('角度 \fontname{Times New Roman}/° ');

 

function Y = rkfa_GR( h,t0,Y0,K,T,u,alfa,d )
k1 = fai_GR(t0,Y0,K,T,u,alfa,d);
k2 = fai_GR(t0+h/2,Y0+h/2*k1,K,T,u,alfa,d);
k3 = fai_GR(t0+h/2,Y0+h/2*k2,K,T,u,alfa,d);
k4 = fai_GR(t0+h,Y0+h*k3,K,T,u,alfa,d);
Y = Y0+h*(k1+2*k2+2*k3+k4)/6;
end

 

function Y = fai_GR( ~,Y0,K,T,u,alfa,d )
% 此处Y0为一个列向量，因为时间t未显含在一阶方程组中
% 所以ode函数的第一个参数为空，要根据具体情况而定。
Y = [Y0(2);
    (K*(u+d)-Y0(2)-alfa*(Y0(2))^3)/T;];
end

 仿真结果分析：

PID：

基于MFAC无模型自适应控制的无人艇航向控制

 MFAC：

基于MFAC无模型自适应控制的无人艇航向控制

 PID参数手动调优的效果比MFAC的最优效果要好。

当加上干扰后：

PID：

基于MFAC无模型自适应控制的无人艇航向控制

 MFAC：

基于MFAC无模型自适应控制的无人艇航向控制

 加上干扰后，PID在稳定后依然有误差，且无法真正稳定，MFAC则不需要调整参数也能保证优秀的控制效果。

但是MFAC在调整时间上始终比PID控制慢，需要进一步研究解决。

 

 

 

 

上一篇：MySQL InnoDB的磁盘文件及落盘机制


下一篇：Oracle系统结构概述二(实例结构)