nnc1.m

固高公司的三自由度直升飞机的神经网络控制的自校正控制方法2

%Single Link Helicopter Control
clear all;
close all;
global A B C D

%Single Link Inverted Pendulum Parameters
g = 9.8;  %重力加速度 单位：m/s2
pi = 3.14; %圆周率

mh = 1.8; %直升机本体质量 单位：kg
l1 = 0.88; %直升机本体到旋转轴的距离 单位：m
mw = 3.433; %平衡块的质量 单位：kg
l2 = 0.35; %平衡快到支点的距离 单位：m

mf = 0.552; %直升机单个推进器的质量 单位：kg
mb = 0.552; %直升机单个推进器的质量 单位：kg
lp = 0.17; %直升机推进器到翻转轴的距离 单位：m

Jp = mf*lp^2 + mb*lp^2; %直升机系统翻转轴的转动惯量 单位：m*kg2

Tg = mh*g*l1 - mw*g*l2; %系统有效的重力矩 单位：N*m
Je = mh*l1^2 + mw*l2^2; %直升机系统旋转轴的转动惯量 单位：m*kg2

G = Tg/l1; %直升机的等效重力 单位：N
Jt = mh*l1^2 + mw*l2^2; %直升机系统旋转轴的转动惯量 单位：m*kg2

%KC = 1.0; %直升机电机的力常数
Kc = 12; %直升机电机的力常数


A=[0,1,0,0;
   0,0,0,0;
   G*l1/Jt,0,0,0;
   0,0,1,0];
B=[0;Kc*lp/Jp;0;0];
C=[1,0,0,0;
   0,0,1,0];
D=[0;0];

Q=[100,0,0,0;   %100,10,1,1 express importance of theta,dtheta,x,dx
   0,10,0,0;
   0,0,1,0;
   0,0,0,1];
R=[0.1];
K=LQR(A,B,Q,R); %LQR Gain    

e1_1=0;e2_1=0;e3_1=0;e4_1=0;
u_1=0;
xk=[-10/57.3,0,0.20,0];   %Initial state

ts=0.02;
%初始化权值变量;
w1=rand(10,4);b1=rand(10,1);
w2=rand(1,10);b2=rand(1,10);
% w1_range=-0.03:0.03;b1_range=-0.03:0.03;
% w2_range=-0.03:0.03;b2_range=-0.03:0.03;
dw1=zeros(size(w1));db1=zeros(size(b1));
dw2=zeros(size(w2));db2=zeros(size(b2));
disp_fqre=10;max_epoch=1000;err_goal=0.001;lr=0.5;mc=0.4;
% tp=[50;500;0.001;0.01;1.05;0.7;0.9;1.04];
tp=[50;50;0.001;0.001;0.001;10;0.1;1e10];
for k=1:1:1000
time(k)=k*ts;
Tspan=[0 ts];

para=u_1;
[t,x]=ode45('helicopterstate',Tspan,xk,[],para);

xk=x(length(x),:);

r1(k)=0.0;    %Side Angle
r2(k)=0.0;    %Side Angle Rate
r3(k)=0.0;    %Travel Rate
r4(k)=0.0;    %Travel Angle
x1(k)=xk(1);
x2(k)=xk(2);
x3(k)=xk(3);
x4(k)=xk(4);

e1(k)=r1(k)-x1(k);
e2(k)=r2(k)-x2(k);
e3(k)=r3(k)-x3(k);
e4(k)=r4(k)-x4(k);

S=2;
if S==1      %LQR
	u(k)=K(1)*e1(k)+K(2)*e2(k)+K(3)*e3(k)+K(4)*e4(k);
elseif S==2  %PD
   de1(k)=e1(k)-e1_1;
   u1(k)=-50*e1(k)-10*de1(k);
   de2(k)=e2(k)-e2_1;
   u2(k)=-10*e2(k)-10*de2(k);
   de3(k)=e3(k)-e3_1;
   u3(k)=-10*e3(k)-10*de3(k);
   de4(k)=e4(k)-e4_1;
   u4(k)=-10*e4(k)-10*de4(k);
   u(k)=u1(k)+u2(k)+u3(k)+u4(k);
end

if u(k)>=5
   u(k)=5;
elseif u(k)<=-5
  	u(k)=-5;
end

	e1_1=e1(k);
	e2_1=e2(k);
	e3_1=e3(k);
	e4_1=e4(k);
	
% 神经网络训练
e=[e1(k);e2(k);e3(k);e4(k)];
% x=[x1(k);x2(k);x3(k);x4(k)];
t=u(k);A2(k)=purelin(w2*tansig(w1*e,b1),b2);
        en(k)=u(k)-A2(k); 
  nntwarn off;
%   while abs(en(k))>0.001
% [w1,b1,w2,b2,te,tr]=trainbpx(w1,b1,'tansig',w2,b2,'purelin',e,t,tp);
[w1,b1,w2,b2,te,tr]=trainlm(w1,b1,'tansig',w2,b2,'purelin',e,t,tp);

%     while abs(en)>0.001                   %误差超出范围则神经网络辨识器做如下调节
%          A1=tansig(w1*e,b1);        %计算隐含层神经元的输出矩阵
%          A2(k)=purelin(w2*A1,b2);      %计算网络输出值
% %          t(1,k)=0.8*sin(y(1,k))+y(1,k)/7;%计算要求网络输出的目标值
%               en=u(k)-A2(k);              %网络输出误差
%   %计算权值变化(修正值及其新的权值)

%     while abs(en)>0.001 
%      D2=deltalin(A2,en);
%      D1=deltatan(A1,D2,w2);
%      [dw1,db1]=learnbpm(e,D1,lr,mc,dw1,db1);
%      [dw2,db2]=learnbpm(A1,D2,lr,mc,dw2,db2);
%      w1=w1+dw1;b1=b1+db1;
%      w2=w2+dw2;b2=b2+db2;
  %权值修正后的误差
%      A1=tansig(w1*e,b1);
     A2(k)=purelin(w2*tansig(w1*e,b1),b2);
        en(k)=u(k)-A2(k);  
   %判断误差平方和 
%      sse=sumsqr(en);
%      if abs(sse)<0.001
%           break;
%      end
%  end
 %辨识器满足要求,退出算法后,系统输出及误差计算;
%   u_1=A2(k);
   u_1=A2(k);%+v(1,k);
end

figure(1);
subplot(411);
plot(time,r1,'k',time,x1,'k'); grid;     %Side Angle
xlabel('time(s)');ylabel('Side Angle');
subplot(412);
plot(time,r2,'k',time,x2,'k'); grid;      %Side Angle Rate
xlabel('time(s)');ylabel('Side Angle Rate');
subplot(413);
plot(time,r3,'k',time,x3,'k'); grid;      %Travel Rate
xlabel('time(s)');ylabel('Travel Rate');
subplot(414);
plot(time,r4,'k',time,x4,'k'); grid;      %Travel Angle
xlabel('time(s)');ylabel('Travel Angle');
figure(5);
plot(time,A2,'k');        grid;            %Force F change
xlabel('time(s)');ylabel('Force');