article-6-pss 并联六自由度机构轨迹规划

最新推荐文章于 2023-10-08 15:35:18 发布

CAE工作者

最新推荐文章于 2023-10-08 15:35:18 发布

阅读量561

点赞数

分类专栏：机器人学习文章标签：机器人并联机器人轨迹规划

本文链接：https://blog.csdn.net/yjw0911/article/details/131271388

版权

机器人学习专栏收录该内容

60 篇文章 35 订阅

订阅专栏

建模

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-djltxj90-1687070335773)(data:image/svg+xml;utf8, )]

程序如下：
% clc
% clear all
%
function []=PSS1(XP,YP,ZP)
tic
%%%%%鍒濆浣嶇疆鐨勪綅濮?%%%%%
% XP = 0; %鍔ㄥ钩鍙扮浉瀵归潤骞冲彴鐨勫垵濮嬩綅缃潗鏍?
% YP = 0;
% ZP = 900;%
% 鍔ㄥ钩鍙扮殑浣嶅Э
X = 0; %鐩稿鍒濆浣嶇疆杩愬姩鐨勫潗鏍?
Y = 0;
Z = 0;
%鏉嗛暱/mm
L = 750;
roll = 0; %鐩稿闈欏钩鍙扮殑濮挎??
pitch = 0;
yaw = 0;
P = [ X+XP; Y+YP; Z+ZP ]; %鍔ㄥ钩鍙板渾蹇冪偣鐩稿 闈欏钩鍙? 鐨勫潗鏍?
% 骞冲彴鐨勫熀鏈昂瀵?
R = 200; %鍔ㄥ钩鍙伴摪鐐圭殑澶栨帴鍦嗗崐寰?
r = 500; %闈欏钩鍙伴摪鐐圭殑澶栨帴鍦嗗崐寰?
d0 = -133.5; %鍔ㄥ钩鍙伴摪鐐圭殑瀹夎瑙掑害
d1 = -46.5;
d2 = -13.5;
d3 = 73.5;
d4 = 106.5;
d5 = 193.5;
j0 = -110.0; %闈欏钩鍙伴摪鐐圭殑瀹夎瑙掑害
j1 = -70.0;
j2 = 10.0;
j3 = 50.0;
j4 = 130.0;
j5 = 170.0;
% 鍔ㄥ钩鍙扮殑6涓摪鐐癸紝鍦ㄥ姩骞冲彴鍧愭爣绯讳腑鐨勪綅缃煝閲?
bR1 = [ R*cosd( d0 ); R*sind( d0 ); 0 ];
bR2 = [ R*cosd( d1 ); R*sind( d1 ); 0 ];
bR3 = [ R*cosd( d2 ); R*sind( d2 ); 0 ];
bR4 = [ R*cosd( d3 ); R*sind( d3 ); 0 ];
bR5 = [ R*cosd( d4 ); R*sind( d4 ); 0 ];
bR6 = [ R*cosd( d5 ); R*sind( d5 ); 0 ];
% 闈欏钩鍙扮殑6涓摪鐐癸紝鍦ㄩ潤骞冲彴鍧愭爣绯讳腑鐨勪綅缃煝閲?
Br1 = [ r*cosd( j0 ) ; r*sind( j0 ); 0];
Br2 = [ r*cosd( j1 ) ; r*sind( j1 ); 0];
Br3 = [ r*cosd( j2 ) ; r*sind( j2 ); 0];
Br4 = [ r*cosd( j3 ) ; r*sind( j3 ); 0];
Br5 = [ r*cosd( j4 ) ; r*sind( j4 ); 0];
Br6 = [ r*cosd( j5 ) ; r*sind( j5 ); 0];
TransM = rotz(yaw) * roty(pitch) * rotx(roll); %XYZ鏃嬭浆鐭╅樀
% 鍔ㄥ钩鍙扮殑6涓摪鐐癸紝鍦ㄩ潤骞冲彴鍧愭爣绯讳腑鐨勪綅缃煝閲?
br1 = TransM * bR1 + P;
br2 = TransM * bR2 + P;
br3 = TransM * bR3 + P;
br4 = TransM * bR4 + P;
br5 = TransM * bR5 + P;
br6 = TransM * bR6 + P;
%婊戝潡Z鏂瑰悜鍧愭爣
h1 = -sqrt((L^{2-(Br1(1)-br1(1))}2-(Br1(2)-br1(2))^2))+br1(3);
h2 = -sqrt((L^{2-(Br2(1)-br2(1))}2-(Br2(2)-br2(2))^2))+br2(3);
h3 = -sqrt((L^{2-(Br3(1)-br3(1))}2-(Br3(2)-br3(2))^2))+br3(3);
h4 = -sqrt((L^{2-(Br4(1)-br4(1))}2-(Br4(2)-br4(2))^2))+br4(3);
h5 = -sqrt((L^{2-(Br5(1)-br5(1))}2-(Br5(2)-br5(2))^2))+br5(3);
h6 = -sqrt((L^{2-(Br6(1)-br6(1))}2-(Br6(2)-br6(2))^2))+br6(3);
% 寤虹珛婊戝潡绉诲姩閾扮偣
ar1 = [ r*cosd( j0 ) ; r*sind( j0 ); h1];
ar2 = [ r*cosd( j1 ) ; r*sind( j1 ); h2];
ar3 = [ r*cosd( j2 ) ; r*sind( j2 ); h3];
ar4 = [ r*cosd( j3 ) ; r*sind( j3 ); h4];
ar5 = [ r*cosd( j4 ) ; r*sind( j4 ); h5];
ar6 = [ r*cosd( j5 ) ; r*sind( j5 ); h6];
%鏉嗛暱鐭㈤噺
L1 = br1 - ar1;
L2 = br2 - ar2;
L3 = br3 - ar3;
L4 = br4 - ar4;
L5 = br5 - ar5;
L6 = br6 - ar6;
%姹傚嚭鏉嗛暱
%G1 = norm(L1);
%G2 = norm(L2);
%G3 = norm(L3);
%G4 = norm(L4);
%G5 = norm(L5);
%G6 = norm(L6);
toc
% 鐢?3D鍥?
h=gca;
delete(h);
% 鎶? hold on 寮?鍚?
figure(1);
hold on;
grid on
title(‘6-PSS’);
view([2,2,2]); % 璁惧畾3D鍥剧殑瑙傚療瑙嗚
axis equal; % XYZ杞存樉绀烘瘮渚嬬浉绛?
plot3( [0,100], [0,0], [0,0], ‘-b’,‘LineWidth’, 1.5 );% 鐢诲嚭X杞?
plot3( 100, 0, 0, ‘>b’ );
plot3( [0,0], [0,100], [0,0], ‘-r’,‘LineWidth’, 1.5 );% 鐢诲嚭Y杞?
plot3( 0, 100, 0, ‘>r’ );
plot3( [0,0], [0,0], [0,100], ‘-k’,‘LineWidth’, 1.5 );% 鐢诲嚭Z杞?
plot3( 0, 0, 100, ‘>k’ );
PBx0 = TransM * [ 0; 0; 0 ] + P;
PBx1 = TransM * [ 0+100; 0; 0 ] + P;
PBy0 = TransM * [ 0; 0; 0 ] + P;
PBy1 = TransM * [ 0; 0+100;0 ] + P;
PBz0 = TransM * [ 0; 0; 0 ] + P;
PBz1 = TransM * [ 0; 0; 0+100] + P;
plot3( [PBx0(1),PBx1(1)], [PBx0(2),PBx1(2)], [PBx0(3),PBx1(3)], ‘-g’, ‘LineWidth’, 1);
plot3( [PBy0(1),PBy1(1)], [PBy0(2),PBy1(2)], [PBy0(3),PBy1(3)], ‘-r’, ‘LineWidth’, 1);
plot3( [PBz0(1),PBz1(1)], [PBz0(2),PBz1(2)], [PBz0(3),PBz1(3)], ‘-k’, ‘LineWidth’, 1);
patch( [Br1(1),Br2(1),Br3(1),Br4(1),Br5(1),Br6(1)], [Br1(2),Br2(2),Br3(2),Br4(2),Br5(2),Br6(2)],[Br1(3),Br2(3),Br3(3),Br4(3),Br5(3),Br6(3)], ‘c’ );
patch( [br1(1),br2(1),br3(1),br4(1),br5(1),br6(1)], [br1(2),br2(2),br3(2),br4(2),br5(2),br6(2)], [br1(3),br2(3),br3(3),br4(3),br5(3),br6(3)], ‘c’ );
plot3( ar1(1),ar1(2),ar1(3), ‘.m’, ‘MarkerSize’, 25); % 鐢诲嚭绉诲姩閾扮偣
plot3( ar2(1),ar2(2),ar2(3), ‘.m’, ‘MarkerSize’, 25);
plot3( ar3(1),ar3(2),ar3(3), ‘.m’, ‘MarkerSize’, 25);
plot3( ar4(1),ar4(2),ar4(3), ‘.m’, ‘MarkerSize’, 25);
plot3( ar5(1),ar5(2),ar5(3), ‘.m’, ‘MarkerSize’, 25);
plot3( ar6(1),ar6(2),ar6(3), ‘.m’, ‘MarkerSize’, 25);
plot3( br1(1),br1(2),br1(3), ‘.m’, ‘MarkerSize’, 25); % 鐢诲嚭鍔ㄥ钩鍙颁笂鐨?6涓摪鐐?
plot3( br2(1),br2(2),br2(3), ‘.m’, ‘MarkerSize’, 25);
plot3( br3(1),br3(2),br3(3), ‘.m’, ‘MarkerSize’, 25);
plot3( br4(1),br4(2),br4(3), ‘.m’, ‘MarkerSize’, 25);
plot3( br5(1),br5(2),br5(3), ‘.m’, ‘MarkerSize’, 25);
plot3( br6(1),br6(2),br6(3), ‘.m’, ‘MarkerSize’, 25);
plot3( P(1),P(2),P(3), ‘.k’, ‘MarkerSize’, 10); % 鐢诲嚭鍔ㄩ潤骞冲彴鐨勫渾蹇?
plot3( 0, 0, 0, ‘.k’, ‘MarkerSize’, 10);
% 鐢诲嚭瀵艰建
plot3( [Br1(1),Br1(1)], [Br1(2),Br1(2)], [Br1(3)+300,Br1(3)], ‘-b’, ‘LineWidth’, 2.5);
plot3( [Br2(1),Br2(1)], [Br2(2),Br2(2)], [Br2(3)+300,Br2(3)], ‘-b’, ‘LineWidth’, 2.5);
plot3( [Br3(1),Br3(1)], [Br3(2),Br3(2)], [Br3(3)+300,Br3(3)], ‘-b’, ‘LineWidth’, 2.5);
plot3( [Br4(1),Br4(1)], [Br4(2),Br4(2)], [Br4(3)+300,Br4(3)], ‘-b’, ‘LineWidth’, 2.5);
plot3( [Br5(1),Br5(1)], [Br5(2),Br5(2)], [Br5(3)+300,Br5(3)], ‘-b’, ‘LineWidth’, 2.5);
plot3( [Br6(1),Br6(1)], [Br6(2),Br6(2)], [Br6(3)+300,Br6(3)], ‘-b’, ‘LineWidth’, 2.5);
% 鍔ㄥ钩鍙颁笌婊戝姩閾扮偣閾炬帴
plot3( [br1(1),ar1(1)], [br1(2),ar1(2)], [br1(3),ar1(3)], ‘-k’, ‘LineWidth’, 2);
plot3( [br2(1),ar2(1)], [br2(2),ar2(2)], [br2(3),ar2(3)], ‘-k’, ‘LineWidth’, 2);
plot3( [br3(1),ar3(1)], [br3(2),ar3(2)], [br3(3),ar3(3)], ‘-k’, ‘LineWidth’, 2);
plot3( [br4(1),ar4(1)], [br4(2),ar4(2)], [br4(3),ar4(3)], ‘-k’, ‘LineWidth’, 2);
plot3( [br5(1),ar5(1)], [br5(2),ar5(2)], [br5(3),ar5(3)], ‘-k’, ‘LineWidth’, 2);
plot3( [br6(1),ar6(1)], [br6(2),ar6(2)], [br6(3),ar6(3)], ‘-k’, ‘LineWidth’, 2);
% 璁剧疆3D鍥剧殑鏍煎紡
% xlim([ -500,600 ]); %鍥哄畾鏄剧ず鑼冨洿
% ylim([ -500,500 ]);
% zlim([ 0,900 ]);
xlabel(‘X-axis / mm’); % XYZ杞存爣璇?
ylabel(‘Y-axis / mm’);
zlabel(‘Z-axis / mm’);
text( Br1(1), Br1(2)-50, ‘1’ ); %鍦ㄩ潤骞冲彴鐨勯摪鐐规梺锛屾樉绀鸿繛鏉嗗簭鍙?
text( Br2(1)+50, Br2(2)-50, ‘2’ );
text( Br3(1)+50, Br3(2)-50, ‘3’ );
text( Br4(1)+50, Br4(2)+50, ‘4’ );
text( Br5(1)+50, Br5(2)+50, ‘5’ );
text( Br6(1)+50, Br6(2)-50, ‘6’ );
end