機器人工作空間收到機械臂連桿長度的影響,而工作空間的大小與機械臂的操控靈活性又非正相關。基於優化理論,考慮不同杆長因素對工作空間和操控靈活性之間的關係。爲構型的設計提供一定參考依據。文章初步實現這一思想,後續進行繼續改進優化。
matlab代碼:
clc;clear all;
d4min=0.3; d4max=1.5; d4Num=10;
d5min=0.3; d5max=1.5; d5Num=10;
a2min=0.3; a2max=1.5; a2Num=10;
deltad4=(d4max-d4min)/d4Num;
deltad5=(d5max-d5min)/d5Num;
deltaa2=(a2max-a2min)/a2Num;
[d4]=d4min:deltad4:d4max;
[d5]=d5min:deltad5:d5max;
[a2]=a2min:deltaa2:a2max;
for i=1:1:d4Num
for j=1:1:d5Num
for k=1:1:a2Num
[v,m]=myVolum(d4(i),d5(j),a2(k));
if v==0||m==0
break ;
end
Resu(i*j*k,:)=[v,m,d4(i),d5(j),a2(k)];
end
end
end
Resu(all(Resu==0,2),:)=[]; %去除全爲零的行
fprintf('輸出計算結果:\r\n')
fprintf('------------------------------------\r\n Volum m d4 d5 a2')
Resu
%%
function [v,m]=myVolum(d4,d5,a2)
d1=0;d2=0;d3=0;d6=0;
% d4=0.5;d5=1;
% a2=.5;
a3=0;a4=0;
d=[d1,d2,d3,d4,d5,d6];
a=[0,0,a2,a3,a4,0];
alp=[0,pi/2,0,pi/2,pi/2,-pi/2];
q=[0,0,0,pi/2,0,0];
% theta d a alpha offset
L1=Link([0 d(1) a(1) alp(1) 0 ],'modified');
L2=Link([0 d(2) a(2) alp(2) 0 ],'modified');
L3=Link([0 d(3) a(3) alp(3) 0 ],'modified');
L4=Link([0 d(4) a(4) alp(4) 0 ],'modified');
L5=Link([0 d(5) a(5) alp(5) 0 ],'modified');
L6=Link([0 d(6) a(6) alp(6) 0 ],'modified');
myrobot=SerialLink([L1 L2 L3 L4 L5 L6],'name','modified-6R');
% figure(1)
% myrobot.plot(q);
% myrobot.teach();hold on;
m=myrobot.maniplty(q,'yoshikawa');
% J=myrobot.jacob0(q);J=J(1:3,:); plot_ellipse(J*J'); %operateablity ellipse
if m<0.5 v=0; %對於操作度小於0.5的直接捨去工作空間的計算
return ;
end
la=[-pi/2,pi/2];
lb=[0, pi];
lc=[0, pi];
ld=[0, pi];
le=[-pi, pi];
lf=[-pi, pi];
N=30000;
A=unifrnd(la(1),la(2),[1,N]);
B=unifrnd(lb(1),lb(2),[1,N]);
C=unifrnd(lc(1),lc(2),[1,N]);
D=unifrnd(ld(1),ld(2),[1,N]);
E=unifrnd(le(1),le(2),[1,N]);
F=unifrnd(lf(1),lf(2),[1,N]);
G= cell(N, 6);
for n = 1:N
G{n} =[A(n) B(n) C(n) D(n) E(n) F(n)];
end
H1=cell2mat(G);
T=double(myrobot.fkine(H1));
% figure(2)
% subplot(1,2,1)
% scatter3(squeeze(T(1,4,:)),squeeze(T(2,4,:)),squeeze(T(3,4,:))); %random points
% myrobot.plot(q)
% hold on
xx(:,1)=T(1,4,:);
yy(:,1)=T(2,4,:);
zz(:,1)=T(3,4,:);
% subplot(1,2,2)
alp = 0.2; region = 0.75; %hole = 1; region = 0.75;
shp = alphaShape(xx,yy,zz,alp); %generate point cloud envelope datas
% plot(shp); %show
v= volume(shp);
% title(['v= ',num2str(v) ,'v']) %compute the envelope volume
end
%%
%繪圖顯示4維曲面
datas=Resu;
x=datas(:,3);y=datas(:,4);z=datas(:,5);v=datas(:,1);
[xi,yi]=meshgrid(x,y);
[xi,yi,zi]=griddata(x,y,z,xi,yi,'v4');
[xi,yi,vi]=griddata(x,y,v,xi,yi,'v4');
figure
surf(xi,yi,zi,vi);
shading interp;
hold on
scatter(x,y,z,'ro');
plot3(x,y,z,'ro')
grid on;
% shading flat
% shading faceted
colorbar
axis tight
輸出計算結果:
------------------------------------
Volum m d4 d5 a2
Resu =
8.5901 0.5036 1.2600 0.7800 0.3000
8.8428 0.5130 1.1400 0.9000 0.3000
9.1855 0.5105 1.0200 1.0200 0.3000
9.4455 0.5515 1.3800 0.7800 0.3000
9.6539 0.5670 1.2600 0.9000 0.3000
9.9568 0.5705 1.1400 1.0200 0.3000
10.3966 0.6210 1.3800 0.9000 0.3000
10.8041 0.6306 1.2600 1.0200 0.3000
11.2243 0.6281 1.1400 1.1400 0.3000
11.5987 0.6906 1.3800 1.0200 0.3000
11.9490 0.6942 1.2600 1.1400 0.3000
12.8679 0.7604 1.3800 1.1400 0.3000
13.3560 0.7580 1.2600 1.2600 0.3000
14.2303 0.8302 1.3800 1.2600 0.3000
12.7404 0.7649 1.1400 0.9000 0.4200
13.2435 0.7560 1.0200 1.0200 0.4200
15.5346 0.9001 1.3800 1.3800 0.3000
13.9165 0.8455 1.2600 0.9000 0.4200
14.4656 0.8449 1.1400 1.0200 0.4200
15.1265 0.9260 1.3800 0.9000 0.4200
15.4837 0.9339 1.2600 1.0200 0.4200
16.2439 0.9251 1.1400 1.1400 0.4200
16.4666 1.0432 1.2600 0.7800 0.5400
16.7363 1.0228 1.3800 1.0200 0.4200
17.2362 1.0225 1.2600 1.1400 0.4200
17.5748 1.0266 1.0200 1.0200 0.5400
17.9761 1.1426 1.3800 0.7800 0.5400
18.4775 1.1199 1.3800 1.1400 0.4200
19.4510 1.1113 1.2600 1.2600 0.4200
18.9135 1.1474 1.1400 1.0200 0.5400
20.3798 1.2172 1.3800 1.2600 0.4200
20.4980 1.2682 1.2600 1.0200 0.5400
21.1719 1.2498 1.1400 1.1400 0.5400
21.9624 1.3231 1.0200 1.0200 0.6600
22.5688 1.3146 1.3800 1.3800 0.4200
21.9614 1.3890 1.3800 1.0200 0.5400
22.8850 1.3814 1.2600 1.1400 0.5400
23.5176 1.4788 1.1400 1.0200 0.6600
24.6821 1.7141 1.2600 0.7800 0.7800
24.2349 1.5130 1.3800 1.1400 0.5400
25.2861 1.4950 1.2600 1.2600 0.5400
26.1057 1.6460 1.0200 1.0200 0.7800
26.9864 1.8774 1.3800 0.7800 0.7800
25.1694 1.6344 1.2600 1.0200 0.6600
26.2841 1.6031 1.1400 1.1400 0.6600
26.6723 1.6373 1.3800 1.2600 0.5400
27.4375 1.7901 1.3800 1.0200 0.6600
28.2179 1.7718 1.2600 1.1400 0.6600
30.4491 1.9958 1.0200 1.0200 0.9000
29.6138 1.7620 1.3800 1.3800 0.5400
30.3641 2.0333 1.2600 1.0200 0.7800
29.9648 1.9406 1.3800 1.1400 0.6600
31.4899 1.9098 1.2600 1.2600 0.6600
32.7612 2.2306 1.1400 1.0200 0.9000
34.9279 2.3729 1.0200 1.0200 1.0200
32.6540 2.2270 1.3800 1.0200 0.7800
32.9205 2.0917 1.3800 1.2600 0.6600
35.4862 2.4655 1.2600 1.0200 0.9000
36.5399 2.3977 1.1400 1.1400 0.9000
37.3039 2.6521 1.1400 1.0200 1.0200
36.2533 2.2432 1.3800 1.3800 0.6600
36.9761 2.3565 1.2600 1.2600 0.7800
37.8554 2.7003 1.3800 1.0200 0.9000
38.8441 2.6501 1.2600 1.1400 0.9000
40.2153 2.9313 1.2600 1.0200 1.0200
41.5861 2.8400 1.1400 1.1400 1.0200
38.9453 2.5809 1.3800 1.2600 0.7800
41.4322 2.9025 1.3800 1.1400 0.9000
42.3889 2.8357 1.2600 1.2600 0.9000
42.4813 3.2104 1.3800 1.0200 1.0200
42.5680 2.7590 1.3800 1.3800 0.7800
43.8616 3.1389 1.2600 1.1400 1.0200
46.4366 3.3128 1.1400 1.1400 1.1400
45.0935 3.1058 1.3800 1.2600 0.9000
46.8139 3.4379 1.3800 1.1400 1.0200
48.0944 3.3479 1.2600 1.2600 1.0200
49.1066 3.6616 1.2600 1.1400 1.1400
48.9555 3.3101 1.3800 1.3800 0.9000
49.9600 3.6668 1.3800 1.2600 1.0200
51.5439 4.0103 1.3800 1.1400 1.1400
53.3726 3.8936 1.2600 1.2600 1.1400
54.6575 3.8969 1.3800 1.3800 1.0200
55.4739 4.2644 1.3800 1.2600 1.1400
58.0997 4.4730 1.2600 1.2600 1.2600
60.1194 4.5200 1.3800 1.3800 1.1400
59.9018 4.8990 1.3800 1.2600 1.2600
65.1913 5.1798 1.3800 1.3800 1.2600
69.3312 5.8765 1.3800 1.3800 1.3800