robot.asv

人工神经网络中cmac网络应用于机器人手臂控制的算法研究

function robot()
% neural networks housework
%use cmac simulink 2D robotic operation
% CMAC operates as a form of lookup table. Note, however,
% that it generalises, i.e. is capable of producing outputs
% in response to inputs not previously experienced.
%5 dim input 3 dim output
help robot;
clf reset;
quantisation=200;
c=40;
memsize=3080;
seta=0.1;
l=[0.5 0.5 0.5];
ipdim=5;
disp(sprintf('quantisation is; %d',quantisation));
disp(sprintf('acceptive field is: %d',c));
disp(sprintf('memsize is : %d',memsize));
iprange=360;
width=iprange/200;

% data initial    


xd=zeros(200,2);
disp('any key to continue');

pause
z = menu('select target function', ...
  'Circle', ...
 'Line');
disp('')

if z == 1
    for t=1:200
        xd(t,1)=0.5-0.25*cos(0.5*pi*0.02*(t-1));
        xd(t,2)=0.25+0.25*sin(0.5*pi*0.02*(t-1));
    end;
else
    for t=1:200
        %xd(t,1)=0.5-0.25*cos(0.5*pi*0.02*(t-1));
        %xd(t,2)=0.25+0.25*sin(0.5*pi*0.02*(t-1));
        xd(t,1)=0.5-0.0025*t;
        xd(t,2)=0.0025*t;
    end;
end;
k=1:200;
hold off;
plot(xd(k,1),xd(k,2));
hold on;
deta=[-9.14 69.87 131.58];
x=[l(1)*cos(deta(1)*2*pi/360)+l(2)*cos((deta(1)+deta(2))*2*pi/360)+l(3)*cos((deta(1)+deta(2)+deta(3))*2*pi/360) ...
        l(1)*sin(deta(1)*2*pi/360)+l(2)*sin((deta(1)+deta(2))*2*pi/360)+l(3)*sin((deta(1)+deta(2)+deta(3))*2*pi/360)];
jmat=[-l(1)*sin(deta(1)*2*pi/360)-l(2)*sin((deta(1)+deta(2))*2*pi/360)-l(3)*sin((deta(1)+deta(2)+deta(3))*2*pi/360) ...
        -l(2)*sin((deta(1)+deta(2))*2*pi/360)-l(3)*sin((deta(1)+deta(2)+deta(3))*2*pi/360) ...
       -l(3)*sin((deta(1)+deta(2)+deta(3))*2*pi/360); ...
        l(1)*cos(deta(1)*2*pi/360)+l(2)*cos((deta(1)+deta(2))*2*pi/360)+l(3)*cos((deta(1)+deta(2)+deta(3))*2*pi/360) ...
        l(2)*cos((deta(1)+deta(2))*2*pi/360)+l(3)*cos((deta(1)+deta(2)+deta(3))*2*pi/360) ...
       l(3)*cos((deta(1)+deta(2)+deta(3))*2*pi/360) ];

wts=zeros(memsize,3);
% weight stack
sse=1;
%sse=||Et|| 
addrs=zeros(c,3);
epcoh=4;
for p=1:epcoh
    disp('any key to continue');
    pause
    k=1:200;
    hold off;
    plot(xd(k,1),xd(k,2));
    hold on;
    for t=1:200
        delta_x=xd(t)-x;
        ip=[deta 360*delta_x]+[90 90 90 0 0];
        HASH=123456;
        offset=width/c;
        ofs=0;
        seta_delta=[0 0 0];
        for i=1:c
            address=0;
            shift=1;
            for j=1:ipdim
                address=address+(rem((ceil((ip(j)+ofs)/width)),quantisation))*shift;
              %address=address+(ceil((ip(j)/width)))*shift;
                shift=shift*quantisation;
            end;
            address=address+shift*i;
            for j=1:3
                addrs(i,j)=ceil(rem((log(address+1)*HASH),memsize));
              %addrs(i,j)=mod(address,memsize);
            end;
           ofs=ofs+offset;
            for j=1:3
                seta_delta(j)=seta_delta(j)+wts(addrs(i,j),j);
            end;
        end;
        deta=deta+seta_delta;
       x=[l(1)*cos(deta(1)*2*pi/360)+l(2)*cos((deta(1)+deta(2))*2*pi/360)+l(3)*cos((deta(1)+deta(2)+deta(3))*2*pi/360) ...
        l(1)*sin(deta(1)*2*pi/360)+l(2)*sin((deta(1)+deta(2))*2*pi/360)+l(3)*sin((deta(1)+deta(2)+deta(3))*2*pi/360)];
       jmat=[-l(1)*sin(deta(1)*2*pi/360)-l(2)*sin((deta(1)+deta(2))*2*pi/360)-l(3)*sin((deta(1)+deta(2)+deta(3))*2*pi/360) ...
            -l(2)*sin((deta(1)+deta(2))*2*pi/360)-l(3)*sin((deta(1)+deta(2)+deta(3))*2*pi/360) ...
             -l(3)*sin((deta(1)+deta(2)+deta(3))*2*pi/360); ...
            l(1)*cos(deta(1)*2*pi/360)+l(2)*cos((deta(1)+deta(2))*2*pi/360)+l(3)*cos((deta(1)+deta(2)+deta(3))*2*pi/360) ...
            l(2)*cos((deta(1)+deta(2))*2*pi/360)+l(3)*cos((deta(1)+deta(2)+deta(3))*2*pi/360) ...
            l(3)*cos((deta(1)+deta(2)+deta(3))*2*pi/360) ];

       Et=xd(t)-x;
       seta_delta=seta_delta-0.1*(-300*Et*jmat+seta_delta);
       deta=deta+seta_delta;
       x=[l(1)*cos(deta(1)*2*pi/360)+l(2)*cos((deta(1)+deta(2))*2*pi/360)+l(3)*cos((deta(1)+deta(2)+deta(3))*2*pi/360) ...
       l(1)*sin(deta(1)*2*pi/360)+l(2)*sin((deta(1)+deta(2))*2*pi/360)+l(3)*sin((deta(1)+deta(2)+deta(3))*2*pi/360)];
       jmat=[-l(1)*sin(deta(1)*2*pi/360)-l(2)*sin((deta(1)+deta(2))*2*pi/360)-l(3)*sin((deta(1)+deta(2)+deta(3))*2*pi/360) ...
             -l(2)*sin((deta(1)+deta(2))*2*pi/360)-l(3)*sin((deta(1)+deta(2)+deta(3))*2*pi/360) ...
             -l(3)*sin((deta(1)+deta(2)+deta(3))*2*pi/360); ...
             l(1)*cos(deta(1)*2*pi/360)+l(2)*cos((deta(1)+deta(2))*2*pi/360)+l(3)*cos((deta(1)+deta(2)+deta(3))*2*pi/360) ...
             l(2)*cos((deta(1)+deta(2))*2*pi/360)+l(3)*cos((deta(1)+deta(2)+deta(3))*2*pi/360) ...
             l(3)*cos((deta(1)+deta(2)+deta(3))*2*pi/360) ];
        Et=xd(t)-x;
        %if((t==40)&(epcoh==4))
        %       x(1)
        %       x(2)
        %       xd(t,1)
        %       xd(t,2)
        %       Et
        %       end;
        temp=Et*jmat;
        for i=1:c
            for j=1:3
                wts(addrs(i,j),j)=wts(addrs(i,j),j)+seta*temp(j);
                
            end;
        end;
        if(rem(t,30)==0)
            temp1=[l(1)*cos(deta(1)*2*pi/360),l(1)*sin(deta(1)*2*pi/360)];
            temp2=[l(1)*cos(deta(1)*2*pi/360)+l(2)*cos((deta(1)+deta(2))*2*pi/360),l(1)*sin(deta(1)*2*pi/360)+l(2)*sin((deta(1)+deta(2))*2*pi/360)];
            plot([0 temp1(1)],[0 temp1(2)]
            plot([0 xd(t,1)+Et(1)],[0 xd(t,2)+Et(1)],'-r');
            Et
           
        end;

    
           % if (((rem(t,200))==0)&(epcoh==4))
           
             
           %plot([0 x(1)],[0 x(2)]);
               % disp(sprintf('t is:%d',t));
                %disp(sprintf('x is:%f',x(1)));
                %disp(sprintf('xd is:%f',xd(t,1)));
               %Et
              % x(1)
               %x(2)
           %    jmat
             %  temp
             %  for i=1:c
           % for j=1:3
              %  wts(addrs(i,j),j)
                
              %  end;
              %   end; 
                
              %end;
          %  if t==50
                
           %     for i=1:c
            %        disp(sprintf('addrs is :%d',addrs(i,1)));
            %   end;
            %end;
                        
            %        end;
   end;
        disp(sprintf('epcoh: %f:',p));
end;
        disp('any key to continue');

        pause

        hold off
        bar(wts,'r')
        title('Network Weights');
        xlabel('Weight Number');
        ylabel('Weight Value');

       set(gca,'xlim',[0 memsize]);