linbppid1.m

bp神经网络pid控制小车倒立摆的摆角和小车的位移

%BP based PID Control
clear all;
close all;

xite=0.20;
alfa=0.05;

S=1;%signal type
IN=4;H=5;Out=3;%NN Structure
if S==1 %Step Signal
wi =[-0.6394 -0.2696 -0.3756 -0.7023;
    -0.8603 -0.2013 -0.5024 -0.2596;
    -1.0749 0.5543 -1.6820 -0.5437;
    -0.3625 -0.0724 -0.6463 -0.2859;
    0.1425 0.0279 -0.5406 -0.7660];
     %wi=0.50*rands(H,IN);
   
wi_1=wi;wi_2=wi;wi_3=wi;  
wo =[0.7576 0.2616 0.5820 -0.1416 -0.1325;
   -0.1146 0.2949 0.8352 0.2205 0.4508;
    0.7201 0.4566 0.7672 0.4962 0.3632];
%wo=0.50*rands(Out,H);
%wo =[-0.3066   -0.1216    0.3216   -0.1588   -0.1296;
 %   0.1822    0.3600    0.1449    0.0341    0.2027;
  % -0.1972    0.3537    0.3180    0.2271    0.0466;
   % 0.0417    0.0936    0.1602   -0.1907   -0.0551;
   %-0.3491   -0.0034   -0.1580    0.3385    0.1946;
    %0.1979    0.3998   -0.2103    0.0681    0.1213];


wo_1=wo;wo_2=wo;wo_3=wo;
end

if S==2 %Sine Signal
  wi =[-0.2846 0.2193 -0.5097 -1.0668;
      -0.7484 -0.1210 -0.4708 0.0988;
      -0.7176 0.8297 -1.6000 0.2049;
      -0.0858 0.1925 -0.6346 0.0347;
      0.4358 0.2369 -0.4565 -0.1324];
%wi=0.50%rands(H,IN);
wi_1=wi;wi_2=wi;wi_3=wi;  
wo=[1.0438 0.5478 0.8682 0.1446 0.1537;
    0.1716 0.5811 1.1214 0.5067 0.7370;
    1.0063 0.7428 1.0534 0.7824 0.6494];
% wo=0.50*rands(Out,H);
wo_1=wo;wo_2=wo;wo_3=wo;
end

x=[0,0,0];
du_1=0;
u_1=0;u_2=0;u_3=0;u_4=0;
y_1=0;y_2=0;y_3=0;y_4=0;

Oh=zeros(H,1) %Output from NN middle layer
I=Oh;         %Input to  NN middle layer
error_2=0;
error_1=0;

ts=0.001;
for k=1:1:6000
time(k)=k*ts;

if S==1
    rin(k)=1.0;
elseif S==2
    rin(k)=sin(1*2*pi*k*ts);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Unlinear model  %%%%%%%%%%%%%%%%%%被控对象近似数学模型
%yout(k)=263*y_1-489.4*y_2+228.2*y_3-0.8338*y_4+2.228*u_1-179.3*u_2-170*u_3+2.095*u_4%小车位置控制
yout(k)=262*y_1-227.4*y_2+0.8338*y_3+18.49*u_1-1.763*u_2-16.73*u_3                 %摆角控制
error(k)=rin(k)-yout(k);
xi=[rin(k),yout(k),error(k),0];
x(1)=error(k)-error_1
x(2)=error(k)
x(3)=error(k)-2*error_1+error_2

epid=[x(1);x(2);x(3)]
I=xi*wi'
for j=1:1:H;
    Oh(j)=(exp(I(j))-exp(-I(j)))/(exp(I(j))+exp(-I(j)))%Middle Layer
end 
K=wo*Oh; %Output layer
for l=1:1:Out
    K(l)=exp(K(l))/(exp(K(l))+exp(-K(l)));
end
kp(k)=K(1);ki(k)=K(2);kd(k)=K(3);
Kpid=[kp(k),ki(k),kd(k)];
du(k)=Kpid*epid;
u(k)=u_1+du(k);

dyu(k)=sign((yout(k)-y_1)/(du(k)-du_1+0.0001));

%Output layer
for j=1:1:Out
    dK(j)=2/(exp(K(j))+exp(-K(j)))^2;
end
for l=1:1:Out
    delta3(l)=error(k)*dyu(k)*epid(l)*dK(l);
end

for l=1:1:Out
    for i=1:1:H;
        d_wo=xite*delta3(l)*Oh(i)+alfa*(wo_1-wo_2);
    end
end
wo=wo_1+d_wo+alfa*(wo_1-wo_2);
%Hidden layer
for i=1:1:H;
    dO(i)=4/(exp(I(i))+exp(-I(i)))^2;
end
segma=delta3*wo;
%for i=1;1:H;
    delta2(i)=dO(i).*segma(i);
    %end
%for i=1:1:H;
   % for j=1:1:IN
      d_wi=xite*delta2'*xi%+alfa*(wi_1 - wi_2)
      %end
      %end
wi=wi_1+d_wi+alfa*(wi_1-wi_2)

%Parameters Update
du_1=du(k);
u_4=u_3;u_3=u_2;u_2=u_1;u_1=u(k);
y_4=y_3;y_3=y_2;y_2=y_1;y_1=yout(k);

wo_3=wo_2;
wo_2=wo_1;
wo_1=wo;
wi_3=wi_2;
wi_2=wi_1;
wi_1=wi;

error_2=error_1;
error_1=error(k);
end
figure(1);
plot(time,rin,'r',time,yout,'y');
xlabel('time(s)');ylabel('rin,yout');
figure(2);
plot(time,error,'r');
xlabel('time(s)');ylabel('error');
figure(3);
plot(time,u,'r');
xlabel('time(s)');ylabel('u');
figure(4);
subplot(311);
plot(time,kp,'r');
xlabel('time(s)');ylabel('kp');
subplot(312);
plot(time,ki,'g');
xlabel('time(s)');ylabel('ki');
subplot(313);
plot(time,kd,'b');
xlabel('time(s)');ylabel('kd');