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

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

1、 建模

程序如下：

% 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

2、 轨迹仿真

轨迹要求：动平台中心画一个圆轨迹

3、 运动空间分析

根据连杆的活动范围，得到动平台得活动范围，如下图：

下载咨询链接

word+全仿真代码下载见收费内容