t3UPU_并联机构_t3UPU_三自由度并联机构性能分析_移动型3-UPU_运动/力传递_

%3-UPU，三个平移自由度，动平台半径r，定平台半径R clc;clearvars;close all; format long syms x y z r R; R=300;r=200; A1=R*[0;1;0]; A2=R*[-sqrt(3)/2;-1/2;0]; A3=R*[sqrt(3)/2;-1/2;0]; B1=r*[0;1;0]; B2=r*[-sqrt(3)/2;-1/2;0]; B3=r*[sqrt(3)/2;-1/2;0]; P=[x;y;z]; rl1=B1+P-A1; rl2=B2+P-A2; rl3=B3+P-A3; %位置反解 l1=sqrt(x^2+(y-110)^2+z^2); l2=sqrt((50*sqrt(3)+x)^2+(y+50)^2+z^2); l3=sqrt((-50*sqrt(3)+x)^2+(y+50)^2+z^2); % %工作空间 % i=1; % W=zeros(100000,3); % for x=-100:2:100 % for y=-180:2:200 % for z=0:2:180 % l1=(x^2+(36.6025+y)^2+z^2)^(1/2); % l2=((x-86.6025)^2+(y-50)^2+z^2)^(1/2); % l3=((x+86.6025)^2+(y-50)^2+z^2)^(1/2); % if l1<=200 && l2<=200 && l3<=200 % W(i,1)=x; % W(i,2)=y; % W(i,3)=z; % i=i+1; % end % end % end % end % xp=W(:,1); % yp=W(:,2); % zp=W(:,3); % plot3(xp,yp,zp); % xlabel('x'); % ylabel('y'); % zlabel('z'); % grid on; %LTI i=1;%z=400; X=zeros(40000,3); %用于计算虎克铰第二旋转轴的投影矩阵 Aro1=[0 1 0;0 0 1]'; Aro2=[0.5*3^0.5 0.5 0;0 0 1]'; Aro3=[-0.5*3^0.5 0.5 0;0 0 1]'; Pro1=Aro1/(Aro1'*Aro1)*Aro1'; Pro2=Aro2/(Aro2'*Aro2)*Aro2'; Pro3=Aro3/(Aro3'*Aro3)*Aro3'; %循环计算LTI z=800; for x=-600:5:600 for y=-600:5:600 %上虎克铰中心 ppp=[x;y;z]; b1=r*[0;1;0]+ppp; b2=r*[-sqrt(3)/2;-1/2;0]+ppp; b3=r*[sqrt(3)/2;-1/2;0]+ppp; %移动副和动平台虎克铰方向 p1=[x,y-100,z]; p1=p1/norm(p1); p2=[x + 50*3^(1/2),y+50,z]; p2=p2/norm(p2); p3=[x - 50*3^(1/2),y+50,z]; p3=p3/norm(p3); u11=[1,0,0]; u21=[-0.5,0.5*3^(1/2),0]; u31=[0.5,0.5*3^0.5,0]; p1ro=Pro1*p1'; p2ro=Pro2*p2'; p3ro=Pro3*p3'; u12=cross(u11,p1ro'); u12=u12/norm(u12); u22=cross(u21,p2ro'); u22=u22/norm(u22); u32=cross(u31,p3ro'); u32=u32/norm(u32); %传递力旋量 ST1=[p1,cross(b1',p1)]; ST2=[p2,cross(b2',p2)]; ST3=[p3,cross(b3',p3)]; %约束力旋量 SC1=[0 0 0 cross(u11,u12)]; SC2=[0 0 0 cross(u21,u22)]; SC3=[0 0 0 cross(u31,u32)]; %单位输入运动旋量 SI1=[0 0 0 p1]; SI2=[0 0 0 p2]; SI3=[0 0 0 p3]; %单位输出运动旋量 ST=[ST1;ST2;ST3;SC1;SC2;SC3]; if rank(ST)<6 break; end ST11=[ST2;ST3;SC1;SC2;SC3]; ST22=[ST1;ST3;SC1;SC2;SC3]; ST33=[ST1;ST2;SC1;SC2;SC3]; SO1=null(ST11)'; SO2=null(ST22)'; SO3=null(ST33)'; SO1=[SO1(4:6) SO1(1:3)]; SO1=SO1/norm(SO1); SO2=[SO2(4:6) SO2(1:3)]; SO2=SO2/norm(SO2); SO3=[SO3(4:6) SO3(1:3)]; SO3=SO3/norm(SO3); dmax1=1; P1=abs(hyj(ST1,SO1))/dmax1; dmax2=1; P2=abs(hyj(ST2,SO2))/dmax2; dmax3=1; P3=abs(hyj(ST3,SO3))/dmax3; P=min([P1,P2,P3]); X(i,1)=x;X(i,2)=y;X(i,3)=P; i=i+1; end end [F,G] = meshgrid(-600:5:600, -600:5:600); Z = griddata(X(1:i-1,1), X(1:i-1,2), X(1:i-1,3), F ,G); contourf(F ,G, Z); colormap(jet); colorbar