rnncontr.m

神经网络在线训练和控制仿真程序

angl1k1=0.5;
angl2k1=0.5;
velo1k1=0;
velo2k1=0;
acce1k1=0;
acce2k1=0;
dt=0.0005;
t=0;
m1=4;
m2=2;
l1=1;
l2=0.5;
g=9.8;

%%%%%%%%%%%initial paramentres%%%%%%%%%
alpha=50;
ks1=0.1;
ks2=0.1;
gama=0.05;
lv=0.1;

%%%%%%%%%%%%%%%initial Net weights%%%%%%%%%
W1=random('unif',0,1,4,4);
W2k1=random('unif',0,1,2,4);  %W2(k+1)

%%%%%%%%%%%%%%%%%%
ANG1=[];
ANG2=[];
DEANG1=[];
DEANG2=[];
E1=[];
E2=[];
TIME=[];
TOR1=[];
TOR2=[];

%%%%%%%%%%%%%%%%%%%%%main%%%%%%%%%%%
for iternum=1:8000
    %%sampling
    angl1k=angl1k1;
    angl2k=angl2k1;
    velo1k=velo1k1;
    velo2k=velo2k1;
    acce1k=acce1k1;
    acce2k=acce2k1;
    W2k=W2k1;
    
    %%%%%%%%%%%%computing the desired trajectory
    t=t+dt;
    de_angl1k=sin(2*pi*t);
    de_angl2k=cos(2*pi*t);
    de_velo1k=2*pi*cos(2*pi*t);
    de_velo2k=-2*pi*sin(2*pi*t);
    de_acce1k=-4*pi*pi*sin(2*pi*t);
    de_acce2k=-4*pi*pi*cos(2*pi*t);
    QD=[de_angl1k de_angl2k]';
    QDCHG=[de_velo1k de_velo2k]';
    QDACC=[de_acce1k de_acce2k]';
    
    %%%%%%%%%%%%%%%%%%compute the error
    erro1k=angl1k-de_angl1k;
    erro2k=angl2k-de_angl2k;
    errchg1k=velo1k-de_velo1k;
    errchg2k=velo2k-de_velo2k;
    ERRO=[erro1k,erro2k]';
    ERRCHG=[errchg1k,errchg2k]';
    
    %%%%%%%%%%%%%%%computing the standard model of robot
    M=[m1*l1*l1+m2*(l1*l1+l2*l2+2*l1*l2*cos(angl2k)) m2*l2*l2+m2*l1*l2*cos(angl2k) 
        m2*l2*l2+m2*l1*l2*cos(angl2k) m2*l2*l2];
    V=[-2*m2*l1*l2*sin(angl2k)*velo2k -m2*l1*l2*sin(angl2k)*velo2k 
        m2*l1*l2*sin(angl2k)*velo1k 0];
    V1=[-m2*l1*l2*sin(angl2k)*velo2k*velo2k-2*m2*l1*l2*sin(angl2k)*velo1k*velo2k 
        m2*l1*l2*sin(angl2k)*velo1k*velo1k];
    G=[m2*l2*g*cos(angl1k+angl2k)+(m1+m2)*l1*g*cos(angl1k) 
        m2*l2*g*cos(angl1k+angl2k)];
    
    %%%%%%%%%%Distrubance and model error
    %d1k=-5*cos(5*t);
    %d2k=-10*sin(10*t);
    %D=[d1k,d2k]';
    %F=0.5*cos(velo1k velo2k]');
    d1k=0;%random('unif',-10,10,1,1);
    d2k=0;%random('unif',-10,10,1,1);
    D=[d1k,d2k]';
    f1=0;%0.5*sign(velo1k)*(0.1+exp(-abs(velo1k)));
    f2=0;%1*sign(velo2k)*(0.2+exp(-abs(velo2k)));
    F=[f1,f2]';
    
    %%computing the network output
    Netin(1,1)=angl1k;
    Netin(2,1)=angl2k;
    Netin(3,1)=velo1k;
    Netin(4,1)=velo2k;
    %Layer 1
    OUT1=Netin;
    %Layer 2
    IN2=W1*OUT1;
    for j=1:4
        OUT2(j,1)=1/(1+exp(-IN2(j)));
    end
    %Layer 3
    Netout=W2k*OUT2;
    
    %%%Learning Algorithm%%%%%%%%%
    W2k1=W2k-lv*W2k*dt;
    
    %%%computing auxilary state
    X1=ERRO;
    X2=ERRCHG+alpha*ERRO;
    OMG=M*alpha*ERRCHG+V*alpha*ERRO;
    
    %computing the control signal U
    U=-X1-OMG+Netout-(ks2+0.5/gama^2)*X2;
    
    %%computing T
    T=U+Netout+M*QDACC+V*QDCHG+G;
    
    %%%%%Apply toques on the robot;
    ACCE=inv(M)*(T+D-F-V1-G);
    
    %%%%%%%%%%
    acce1k1=ACCE(1);
    acce2k1=ACCE(2);
    velo1k1=velo1k+acce1k*dt;
    velo2k1=velo2k+acce2k*dt;
    angl1k1=angl1k+velo1k*dt;
    angl2k1=angl2k+velo2k*dt;
    ANG1=[ANG1 angl1k];
    ANG2=[ANG2 angl2k];
    DEANG1=[DEANG1 de_angl1k];
    DEANG2=[DEANG2 de_angl2k];
    E1=[E1 erro1k];
    E2=[E2 erro2k];
    TOR1=[TOR1 T(1)];
    TOR2=[TOR2 T(2)];
    TIME=[TIME t];
    %%%%%
end

figure
plot(TIME,E1,'k-',TIME,E2,'r-');grid
figure
plot(TIME,ANG1,'k-',TIME,DEANG1,'r-');grid
figure
plot(TIME,ANG2,'k-',TIME,DEANG2,'r-');grid
figure
plot(TIME,E1,'k-');grid
figure
plot(TIME,E2,'k-');grid
figure
plot(TIME,TOR1,'k-');grid
figure
plot(TIME,TOR2,'k-');grid