bp.m

使用matlab编写并联机器人控制的源程序

% 程序一
derd=pi/180;
pos_base=[];
body_pts=[];
pos_top=[];
xita_b=4*derd;
xita_t=54.66*derd;
%n=61.3*derd;

mheight=3.1561;    %中位高度
radius_b=3.6;
radius_t=1.4;
l=[];
n1=0;
n2=0;

for i=1:3,
    angle_m_b=(2*pi/3)*(i-1)-xita_b-pi/3;
    angle_p_b=(2*pi/3)*(i-1)+xita_b-pi/3;
    pos_base(2*i-1,:)=radius_b*[0,cos(angle_m_b),sin(angle_m_b)];
    pos_base(2*i,:)=radius_b*[0,cos(angle_p_b),sin(angle_p_b)];
    
    angle_m_t=(2*pi/3)*(i-1)-xita_t+pi/3;
    angle_p_t=(2*pi/3)*(i-1)+xita_t+pi/3;
    body_pts(2*i-1,:)=radius_t*[0,cos(angle_m_t),sin(angle_m_t)];
    body_pts(2*i,:)=radius_t*[0,cos(angle_p_t),sin(angle_p_t)];
end

pos_base
body_pts=[body_pts(5:6,:);body_pts(1:4,:)]

     plot(1.4*sin(0:0.01:2*pi),1.4*cos(0:0.01:2*pi))
     hold on;
     plot(3.6*sin(0:0.01:2*pi),3.6*cos(0:0.01:2*pi))
     hold on;
     axis square;   
     
     for i=1:6
        hold on;
         plot(body_pts(i,2),body_pts(i,3),'*');
         %line([0 body_pts(i,2)],[0 body_pts(i,3)])
         hold  on; 
        plot(pos_base(i,2),pos_base(i,3),'*');
        %line([0 pos_base(i,2)],[0 pos_base(i,3)])
        hold  on;
        grid on;
        line([pos_base(i,2) body_pts(i,2)],[pos_base(i,3) body_pts(i,3)])
        pause
    end
    
    pause
    
%for h=3.2:0.05:4.0
for h=3.55:0.05:3.75
    h
    n1
   
    for r=0:0.02:0.2    
         %load PC_1051 pc1051;
         %load LC_1051 lc1051;
         load PC_1052 pc1052;
         load LC_1052 lc1052;
        for xita=- 0.175:0.005: 0.175
            if xita<0  a=-1;
            else a=1;
            end
            for arfa=- 0.175:0.05:0.175                                     %z轴
                 r1=[cos(arfa),sin(arfa),0;
                     -sin(arfa),cos(arfa),0;
                     0,0,1];
        For  beta=-0.175:0.0.05:0.175                                  %y轴
                      r2=[cos(beta),0,-sin(beta);
                          0,1,0;
                          sin(beta),0,cos(beta)];
                      r4=r2*r1;
         for  gama=- 0.175:0.0.05:0.175                                 %x轴
                          r3=[1,0,0;
                              0,cos(gama),sin(gama);
                              0,-sin(gama),cos(gama)];
                          R=r3*r4;
                          Body_pts=R*(body_pts');
                          BODY_PTS=Body_pts';
                          pc=[h*ones(6,1),r*sin(xita)*ones(6,1),r*cos(xita)*ones(6,1)];   %上平台中心点位置坐标矩阵
                          pos_top=pc+BODY_PTS;                           %上平台经过旋转后的绝对坐标
                          l=pos_top-pos_base;
                          for i=1:6
                              L(i)=sqrt(l(i,1)^2+l(i,2)^2+l(i,3)^2);
                          end
                     
                           if min(L)>3.1&&max(L)<5.65
                              pa=[h;r;xita;arfa;beta;gama;a];
                            
                              %pc1051=[pc1051,pa];  
                              %lc1051=[lc1051,[L a]'];
                              
                               pa2=[h;r*sin(xita);r*cos(xita);arfa;beta;gama;a];
                              pc1052=[pc1052,pa2];  
                              lc1052=[lc1052,[L a]'];
                             
                              n1=n1+1;
                          end
                      n2=n2+1;
                    end
                end
            end
        end
%     save LC_1051 lc1051;
 %     save PC_1051 pc1051;
        save LC_1052 lc1052;
        save PC_1052 pc1052;
    end
    
end



% 程序二
load PC_105 pc105
load LC_105 lc105
p=lc105;
t=pc105;
sizeofp=size(p);
sizeoft=size(t);
[pn,meanp,stdp,tn,meant,stdt]=prestd(p,t);
pause
[R,Q]=size(pn)
pause
clc
iitst=2:4:Q;
iival=4:4:Q;
iitr=[1:4:Q 3:4:Q];
val.p=pn(:,iival)
val.t=tn(:,iival)
test.p=pn(:,iitst)
test.t=tn(:,iitst)
ptr=pn(:,iitr)
ttr=tn(:,iitr)
pause
clc
net=newff(minmax(ptr),[6,25,7],{'purelin','tansig','purelin'});
net.trainFcn='trainbr';
net.trainParam.epochs=1000;
net.trainParam.goal=0.001;
net=init(net);

pause
[net,tr]=train(net,ptr,ttr,[],[]);
plot(tr.epoch,tr.perf,tr.epoch,tr.vperf,':',tr.epoch,tr.tperf,'-')
legend('training','validation','test',-1);
ylabel('squared error');
xlabel('epoch');
pause
an=sim(net,pn);
a=poststd(an,meant,stdt);
figure(1)
[m(1),b(1),r(1)]=postreg(a(1,:),t(1,:));
pause
figure(2)
[m(2),b(2),r(2)]=postreg(a(2,:),t(2,:));
pause
figure(3)
[m(3),b(3),r(3)]=postreg(a(3,:),t(3,:));
pause
figure(4)
[m(4),b(4),r(4)]=postreg(a(4,:),t(4,:));
pause
figure(5)
[m(5),b(5),r(5)]=postreg(a(5,:),t(5,:));
pause
figure(6)
[m(6),b(6),r(6)]=postreg(a(6,:),t(6,:));
pause
echo off