产品
clc clear all %close all
tic
%-----------------------初始位置的位恣------------------------------------- XP = 0; % 动平台 相对 静平台 的初始位置坐标 YP = 0; ZP = 627.472;
%-------------------------动平台的位恣------------------------------------- X = 0; % 相对初始位置运动的坐标 Y = 0; Z = 200;
ROLL = 60; % 相对静平台的恣态 PITCH = 0; YAW = 0;
P = [ X+XP; Y+YP; Z+ZP ]; % 动平台圆心点 相对 静平台 的坐标
%--------------------------平台的基本尺寸---------------------------------- R = 124; % 动平台铰点的外接圆半径 r = 500; % 静平台铰点的外接圆半径
up_angle0 = -133.5; % 动平台铰点的安装角度 up_angle1 = -46.5; up_angle2 = -13.5; up_angle3 = 73.5; up_angle4 = 106.5; up_angle5 = 193.5;
down_angle0 = -110.0; % 静平台铰点的安装角度 down_angle1 = -70.0; down_angle2 = 10.0; down_angle3 = 50.0; down_angle4 = 130.0; down_angle5 = 170.0;
%----------动平台的6个铰点，在动平台坐标系中的位置矢量--------------------- bR1 = [ R*cosd( up_angle0 ); R*sind( up_angle0 ); 0 ]; bR2 = [ R*cosd( up_angle1 ); R*sind( up_angle1 ); 0 ]; bR3 = [ R*cosd( up_angle2 ); R*sind( up_angle2 ); 0 ]; bR4 = [ R*cosd( up_angle3 ); R*sind( up_angle3 ); 0 ]; bR5 = [ R*cosd( up_angle4 ); R*sind( up_angle4 ); 0 ]; bR6 = [ R*cosd( up_angle5 ); R*sind( up_angle5 ); 0 ];
%----------静平台的6个铰点，在静平台坐标系中的位置矢量--------------------- Br1 = [ r*cosd( down_angle0 ) ; r*sind( down_angle0 ); 0 ]; Br2 = [ r*cosd( down_angle1 ) ; r*sind( down_angle1 ); 0 ]; Br3 = [ r*cosd( down_angle2 ) ; r*sind( down_angle2 ); 0 ]; Br4 = [ r*cosd( down_angle3 ) ; r*sind( down_angle3 ); 0 ]; Br5 = [ r*cosd( down_angle4 ) ; r*sind( down_angle4 ); 0 ]; Br6 = [ r*cosd( down_angle5 ) ; r*sind( down_angle5 ); 0 ];
TransM = rotz(YAW) * roty(PITCH) * rotx(ROLL) % XYZ旋转矩阵
%TransM = rotx(ROLL) * roty(PITCH) * rotz(YAW) % ZYX旋转矩阵
% TransM =... % [ cosd(YAW), -sind(YAW), 0; ... % sind(YAW), cosd(YAW), 0; ... % 0, 0, 1 ] * ... % [ cosd(PITCH), 0, sind(PITCH);... % 0, 1, 0; ... % -sind(PITCH), 0, cosd(PITCH) ] * ... % [ 1, 0, 0; ... % 0, cosd(ROLL), -sind(ROLL);... % 0, sind(ROLL), cosd(ROLL)];
%----------动平台的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;
%--动平台的6个铰点位置矢量，减去，静平台的6个铰点位置矢量，得到每个杆长矢量 L1 = br1 - Br1; L2 = br2 - Br2; L3 = br3 - Br3; L4 = br4 - Br4; L5 = br5 - Br5; L6 = br6 - Br6;
%-----------求模，得到每个杆的杆长----------------------------------------- LenL1 = norm(L1); LenL2 = norm(L2); LenL3 = norm(L3); LenL4 = norm(L4); LenL5 = norm(L5); LenL6 = norm(L6);
toc % 显示计算时间
%-------------------------画出3D图----------------------------------------- h=gca; % 把原来的图删掉，免得新的图和旧的图相互重叠 delete(h);
figure(1); % 把 hold on 开启 hold on;
view([1,1,1]); % 设定3D图的观察视角 axis equal; % XYZ轴显示比例相等
plot3( [0,300], [0,0], [0,0], '-b', 'LineWidth', 1 );% 在图中画出X轴，方便观察 plot3( 300, 0, 0, '>b' ); plot3( [0,0], [0,300], [0,0], '-r', 'LineWidth', 1 );% 在图中画出Y轴，方便观察 plot3( 0, 300, 0, '>r' ); plot3( [0,0], [0,0], [0,300], '-k', 'LineWidth', 1 );% 在图中画出Z轴，方便观察 plot3( 0, 0, 300, '>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 );% 在图中画出X轴，方便观察 plot3( [PBy0(1),PBy1(1)], [PBy0(2),PBy1(2)], [PBy0(3),PBy1(3)], '-r', 'LineWidth', 1 );% 在图中画出Y轴，方便观察 plot3( [PBz0(1),PBz1(1)], [PBz0(2),PBz1(2)], [PBz0(3),PBz1(3)], '-k', 'LineWidth', 1 );% 在图中画出Z轴，方便观察
% 动平台用红色的图块，静平台用蓝色的图块 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)], 'y' );
plot3( br1(1),br1(2),br1(3), '.m', 'MarkerSize', 30); % 画出动平台上的6个铰点 plot3( br2(1),br2(2),br2(3), '.m', 'MarkerSize', 30); plot3( br3(1),br3(2),br3(3), '.m', 'MarkerSize', 30); plot3( br4(1),br4(2),br4(3), '.m', 'MarkerSize', 30); plot3( br5(1),br5(2),br5(3), '.m', 'MarkerSize', 30); plot3( br6(1),br6(2),br6(3), '.m', 'MarkerSize', 30);
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),Br1(3)], '-k', 'LineWidth', 2 ); % 动、静平台的铰点连接，也就是连杆 plot3( [br2(1),Br2(1)], [br2(2),Br2(2)], [br2(3),Br2(3)], '-k', 'LineWidth', 2 ); plot3( [br3(1),Br3(1)], [br3(2),Br3(2)], [br3(3),Br3(3)], '-k', 'LineWidth', 2 ); plot3( [br4(1),Br4(1)], [br4(2),Br4(2)], [br4(3),Br4(3)], '-k', 'LineWidth', 2 ); plot3( [br5(1),Br5(1)], [br5(2),Br5(2)], [br5(3),Br5(3)], '-k', 'LineWidth', 2 ); plot3( [br6(1),Br6(1)], [br6(2),Br6(2)], [br6(3),Br6(3)], '-k', 'LineWidth', 2 );
%-------------------------设置3D图的一些格式，方便观察--------------------- xlim([ -500,600 ]); % X轴固定显示范围 ylim([ -500,500 ]); % Y轴固定显示范围 zlim([ 0,900 ]); % Z轴固定显示范围
xlabel('X-axis / mm'); % X轴标识 ylabel('Y-axis / mm'); % Y轴标识 zlabel('Z-axis / mm'); % Z轴标识
str = ['LenL1 = ',num2str(LenL1)]; % 在图上显示每条连杆的长度，mm为单位 text(-300,200,900,[str,' mm']); str = ['LenL2 = ',num2str(LenL2)]; text(-300,200,800,[str,' mm']); str = ['LenL3 = ',num2str(LenL3)]; text(-300,200,700,[str,' mm']); str = ['LenL4 = ',num2str(LenL4)]; text(-300,200,600,[str,' mm']); str = ['LenL5 = ',num2str(LenL5)]; text(-300,200,500,[str,' mm']); str = ['LenL6 = ',num2str(LenL6)]; text(-300,200,400,[str,' 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' );
155-2731-8020
