基于MFAC无模型自适应控制的无人艇航向控制
作者：互联网
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）。
本文控制方法使用紧格式动态线性化的无模型自适应控制。
参考文献：《无人艇重定义无模型自适应艏向控制方法与试验》
控制律：
控制原理：
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：
PID参数手动调优的效果比MFAC的最优效果要好。
当加上干扰后：
PID：
MFAC：
加上干扰后，PID在稳定后依然有误差，且无法真正稳定，MFAC则不需要调整参数也能保证优秀的控制效果。
但是MFAC在调整时间上始终比PID控制慢，需要进一步研究解决。
标签：MFAC,alfa,GR,适应控制,0.1,航向,fai,Y0 来源： https://blog.csdn.net/angusfy/article/details/119906959