%This function shows the simulation result of robot manipulator control
%bassed Fuzzy Bspline Net
global angl1k;%angle at time K;
global angl1k1;%angle at time K+1;
global angl2k;%angle at time K;
global angl2k1;%angle at time K+1;
global velo1k;
global velo1k1;
global velo2k;
global velo2k1;
global acce1k;
global acce1k1;
global acce2k;
global acce2k1;
global t;%sampling period
global x;
angl1k1=0;
angl2k1=0;
velo1k1=0;
velo2k1=0;
acce1k1=0;
acce2k1=0;
erro1k=0;
erro2k=0;
erro1k_1=0;
erro2k_1=0;
x=1;
t1k=0;%torque apply on link1 at time K;
t2k=0;%torque apply on link2 at time K;
rule=[-5 -5 -5 -5 -5 -5 -5 -4 -3 -2 0 0 0;
-5 -5 -5 -5 -5 -5 -5 -4 -3 -2 0 0 0;
-5 -5 -5 -5 -5 -5 -5 -3 -3 -2 0 0 0;
-4 -4 -4 -4 -4 -4 -4 -3 -2 -1 1 1 1;
-4 -4 -4 -4 -4 -4 -4 -2 -1 0 2 2 2;
-4 -4 -4 -3 -3 -3 -3 -1 2 2 3 3 3
-4 -4 -4 -3 -3 -1 0 1 3 3 4 4 4
-3 -3 -3 -2 -2 1 3 3 3 3 4 4 4
-2 -2 0 0 1 2 4 4 4 4 4 4 4
-1 -1 0 1 2 3 4 4 4 4 4 4 4
0 0 1 2 3 4 5 5 5 5 5 5 5
0 0 1 2 3 4 5 5 5 5 5 5 5
0 0 1 2 3 4 5 5 5 5 5 5 5];
ke1=200;
kec1=1;
kcw1=600;
ke2=200;
kec2=1;
kcw2=100;
A1=[];%angle1 matrix
A2=[];%angle2 matrix;
DA1=[];
DA2=[];
ERR1=[];
ERR2=[];
X=[];%x axis matrix
% % % % % % % % % % % % % % % % % % % % % % % %
ovectornum=[1 2 3
4 5 6];%????
tvectornum=[7 8 9;10 11 12];
load 'checkpoints.mat';
xpotvector=[-2.25 -2 -1.75 -1.5 -1.25 -1 -0.75 -0.5 -0.25 0 0.25
-1.25 -1 -0.75 -0.5 -0.25 0 0.25 0.5 0.75 1 1.25
0.25 0 0.25 0.5 0.75 1 1.25 1.5 1.75 2 2.25
-2.25 -2 -1.75 -1.5 -1.25 -1 -0.75 -0.5 -0.25 0 0.25
-1.25 -1 -0.75 -0.5 -0.25 0 0.25 0.5 0.75 1 1.25
-0.25 0 0.25 0.5 0.75 1 1.25 1.5 1.75 2 2.25
-2.25 -2 -1.75 -1.5 -1.25 -1 -0.75 -0.5 -0.25 0 0.25
-1.25 -1 -0.75 -0.5 -0.25 0 0.25 0.5 0.75 1 1.25
-0.25 0 0.25 0.5 0.75 1 1.25 1.5 1.75 2 2.25
-2.25 -2 -1.75 -1.5 -1.25 -1 -0.75 -0.5 -0.25 0 0.25
-1.25 -1 -0.75 -0.5 -0.25 0 0.25 0.5 0.75 1 1.25
-0.25 0 0.25 0.5 0.75 1 1.25 1.5 1.75 2 2.25];
cnum=9;
%main
for i=0:1999
erro1k_1=erro1k;
erro2k_1=erro2k;
angl1k=angl1k1;
angl2k=angl2k1;
velo1k=velo1k1;
velo2k=velo2k1;
acce1k=acce1k1;
acce2k=acce2k1;
x=x+t;
de_angl1k=sin(2*pi*x);
de_angl2k=sin(2*pi*x);
%de_angl1k=1;
%de_angl2k=1;
erro1k=de_angl1k-angl1k;
derro1k=(erro1k-erro1k_1)/t; %error1 change volicity
erro2k=de_angl2k-angl2k;
derro2k=(erro2k-erro2k_1)/t;
% % % % % % % % % % % % % % % % % % % % % % %
E1=round(erro1k*ke1);
if E1>6
E1=6;
elseif E1
E1=-6;
end
EC1=round(derro1k*kec1);
if EC1>6
EC1=6;
elseif EC1
EC1=-6;
end
% % % % % % % % % % % % % % % % % % % % % % %
E2=round(erro2k*ke2);
if E2>6
E2=6;
elseif E2
E2=-6;
end
EC2=round(derro2k*kec2);
if EC2>6
EC2=6;
elseif EC2
EC2=-6;
end
% % % % % % % % % % % % % % % % % % % % % % %
%Fuzzy Bspline Net
% % % % % % % % % % % % % % % % % % % % % % %
out1_one(1)=E1/6;
out1_one(2)=EC1/6;
out1_two(1)=E2/6;
out1_two(2)=EC2/6;
%network1
%second layer,getting B spline membership
for i=1:2
for j=1:3
m=ovectornum(i,j);
if out1_one(i)>=xpotvector(m,1)&out1_one(i)
out2_one(i,j)=cpvector(m,1)*(out1_one(i)-xpotvector(m,1))/(xpotvector(m,2)-xpotvector(m,1));
end
if out1_one(i)>=xpotvector(m,cnum+1)&out1_one(i)
out2_one(i,j)=cpvector(m,cnum)*(xpotvector(m,cnum+2)-out1_one(i))/(xpotvector(m,cnum+2)-xpotvector(m,cnum+1));
end
for p=1:cnum-1
if out1_one(i)>=xpotvector(m,p+1)&out1_one(i)
out2_one(i,j)=cpvector(m,p)*(xpotvector(m,p+2)-out1_one(i))/(xpotvector(m,p+2)-xpotvector(m,p+1))+cpvector(m,p+1)*(out1_one(i)-xpotvector(m,p+1))/(xpotvector(m,p+2)-xpotvector(m,p+1));
end
end
end
end
%third layer,'and operation'
for i=1:3
for j=1:3
out3_one(i,j)=out2_one(1,i)*out2_one(2,j);
end
end
%forth layer defuzzization
sumone=0;
wholesumone=0;
for i=1:3
for j=1:3
sumone=sumone+out3_one(i,j);
wholesumone=wholesumone+out3_one(i,j)*weight3_one(i,j);
end
end
netout(1)=wholesumone/sumone;
%network2
%first layer
for i=1:2
for j=1:3
m=tvectornum(i,j);
if out1_two(i)>=xpotvector(m,1)&out1_one(i)
out2_two(i,j)=cpvector(m,1)*(out1_two(i)-xpotvector(m,1))/(xpotvector(m,2)-xpotvector(m,1));
end
if out1_two(i)>=xpotvector(m,cnum+1)&out1_two(i)
out2_two(i,j)=cpvector(m,cnum)*(xpotvector(m,cnum+2)-out1_two(i))/(xpotvector(m,cnum+2)-xpotvector(m,cnum+1));
end
for p=1:cnum-1
if out1_two(i)>=xpotvector(m,p+1)&out1_two(i)
out2_one(i,j)=cpvector(m,p)*(xpotvector(m,p+2)-out1_two(i))/(xpotvector(m,p+2)-xpotvector(m,p+1))+cpvector(m,p+1)*(out1_two(i)-xpotvector(m,p+1))/(xpotvector(m,p+2)-xpotvector(m,p+1));
end
end
end
end
%third layer,'and operation'
for i=1:3
for j=1:3
out3_two(i,j)=out2_two(1,i)*out2_two(2,j);
end
end
%forth layer defuzzization
sumtwo=0;
wholesumtwo=0;
for i=1:3
for j=1:3
sumtwo=sumtwo+out3_two(i,j);
wholesumtwo=wholesumtwo+out3_two(i,j)*weight3_two(i,j);%加权值进行模糊化
end
end
netout(2)=wholesumtwo/sumtwo;
%fifth layer
wn(1)=weight4(1,1)*netout(1)+weight4(1,2)*netout(2);
wn(2)=weight4(2,1)*netout(1)+weight4(2,2)*netout(2);
t1k=wn(1)*kcw1*6;
t2k=wn(2)*kcw2*6;
% % % % % % % % % % % % % % % % % % % % % % %
[angl1k1,angl2k1,velo1k1,velo2k1,acce1k1,acce2k1]=robot(t1k,t2k,angl1k,angl2k,velo1k,velo2k,acce1k,acce2k,t);
% % % % % % % % % % % % % % % % % % % % % % %
A1=[A1,angl1k];
A2=[A2,angl2k];
DA1=[DA1,de_angl1k];
DA1=[DA1,de_angl2k];
ERR1=[ERR1,erro1k];
ERR2=[ERR2,erro2k];
X=[X,x];
end
figure(1)
plot(X,A1,'-',X,DA1,'-');
figure(2)
plot(X,A21,'-',X,DA2,'-');
figure(3)
plot(x,ERR1,'*')
axis([0 1 -0.15 0.15]);
figure(4)
plot(X,ERR2,'*');
axis([0 1 -0.15 0.15]);
savefile='checkpoints.mat';
weight3_one=[-1.3528 -1.2754 0.2837
-1.0793 -0.3656 1.2762
-0.2773 0.5975 1.3375];
weight3_two=[-0.9358 -1.2305 -0.5353
-1.0460 0.2630 0.4732
-0.1908 1.7938 1.3357];
cpvector=[ 0 0.1429 0.2857 0.4286 0.5714 0.7143 0.8571 1.0000 0.8571 0.7143 0.4287 0.4287 0.4405 0.4651 0.2872
0 0.1429 0.2857 0.5714 0.5714 0.5677 0.5733 0.8563 0.5007 0.5045 0.5014 0.4995 0.4966 0.4909 0.0547
0.9142 0.5397 0.5541 0.5650 0.5714 0.5816 0.6007 0.9472 0.8571 0.7143 0.5714 0.4286 0.2857 0.1429 0
0 0.1429 0.2857 0.4286 0.5714 0.7143 0.8571 1.0000 0.8571 0.7143 0.4114 0.4084 0.4092 0.4103 0.9379
0 0.1429 0.2857 0.5821 0.5837 0.5832 0.5837 0.9509 0.4260 0.4263 0.4293 0.4286 0.4163 0.2826 0.0007
0.5040 0.4353 0.4405 0.4286 0.4317 0.5877 0.7098 0.9985 0.8571 0.7143 0.5714 0.4286 0.3857 0.1429 0
0 0.1429 0.2857 0.4286 0.5714 0.7143 0.8571 0.8748 0.7385 0.5818 0.5881 0.5710 0.5742 0.5490 0.6883
0.1713 0.2644 0.4946 0.5736 0.5757 0.5752 0.5664 0.4579 0.5226 0.5388 0.5371 0.5328 0.5291 0.5316 0.0064
0.6819 0.6454 0.6416 0.6500 0.5840 0.5936 0.5988 0.9801 0.8571 0.7143 0.5714 0.4286 0.2857 0.1426 0
0 0.1429 0.2857 0.4286 0.5714 0.7143 0.8571 0.9939 0.4472 0.4271 0.4362 0.4464 0.4298 0.4548 0.9375
0.2548 0.5588 0.5772 0.5718 0.5757 0.5822 0.5735 0.7332 0.5045 0.4912 0.4959 0.4924 0.4945 0.4781 0.3072
0.4993 0.4268 0.4120 0.4170 0.4129 0.4244 0.4815 0.8993 0.8571 0.7143 0.5714 0.4286 0.2857 0.1429 0];
weight4=[0.3167 1.1974;1.6862 -0.8926];
save(savefile, 'weight3_one', 'weight3_two','cpvector','weight4')
运行时x变量为空,希望帮助看看。
这个Matlab程序示例展示了机器人操纵器控制的模糊Bspline网络模拟过程。全局变量初始化,包括角速度、角度等。通过一系列计算和模糊逻辑处理,计算并应用关节扭矩。在模拟过程中,对关节角度、角速度等进行记录和绘制图表。当遇到x变量为空的情况,程序可能无法正常运行,需要检查和修正。
5201

被折叠的 条评论
为什么被折叠?



