Matlab:sps并联机构空间求解源码

admin

[mw_shl_code=applescript,true]clear;clc;
lb=[-300,200,200,0,0,0];
ub=[0,300,300,700,700,700];
rng default
x0=100*randn(6,1);

MIN_leglength=300;%支链最短值
MAX_leglength=600;%支链最长值
interval=20;%数据细分度1.5
MAX_angle=40;%球面副最大倾角
currentnum=0;%当前循环个数，用于百分比表示计算进度
osnumber=1;%动平面中心的坐标指针
erropost=0;%不符合机构转角限制的位姿个数

L_1=500;%固定平台边长
L_2=500;%动平台边长
Rx=[-L_1/2;L_1/2;0];
Ry=[-sqrt(3).*L_1/6;-sqrt(3).*L_1/6;sqrt(3).*L_1/3];
Rz=[0;0;0];
R1_steady=[Rx(1,1);Ry(1,1);Rz(1,1)];
R2_steady=[Rx(2,1);Ry(2,1);Rz(2,1)];
R3_steady=[Rx(3,1);Ry(3,1);Rz(3,1)];
G1min=MIN_leglength;
G2min=MIN_leglength;
G3min=MIN_leglength;
leenum=(MAX_leglength-MIN_leglength)/interval+1;%单支链长数据个数
totalprocess=leenum+leenum*(leenum-1)+nchoosek(leenum,3);
%totalprocess=leenum^3;

Rz120_matrix=RZ(2*pi/3);
Rz240_matrix=RZ(-2*pi/3);

for G1=G1min:interval:MAX_leglength
     for G2=G2min:interval:MAX_leglength           
            for G3=G3min:interval:MAX_leglength

p=setcoeffirent(L_1,L_2,G1,G2,G3);
[x,res]=lsqnonlin(p,x0,lb,ub);
for i=1:1:3   
    Z(i)=x(i+3);   
end
X(1)=x(1);
X(2)=x(2);
X(3)=0;
Y(1)=sqrt(3)*X(1)/3;
Y(2)=-sqrt(3)*X(2)/3;
Y(3)=x(3);

OX=(X(1)+X(2)+X(3))/3;
OY=(Y(1)+Y(2)+Y(3))/3;
OZ=(Z(1)+Z(2)+Z(3))/3;

vectorOS_S1=[X(1)-OX;Y(1)-OY;Z(1)-OZ]/sqrt((X(1)-OX)^2+(Y(1)-OY)^2+(Z(1)-OZ)^2);
vectorOS_S2=[X(2)-OX;Y(2)-OY;Z(2)-OZ]/sqrt((X(2)-OX)^2+(Y(2)-OY)^2+(Z(2)-OZ)^2);
vectorOS_S3=[X(3)-OX;Y(3)-OY;Z(3)-OZ]/sqrt((X(3)-OX)^2+(Y(3)-OY)^2+(Z(3)-OZ)^2);
vectorOS_S=cross(vectorOS_S1,vectorOS_S2);%动平台法向量

vectorR1_S1=[X(1)-Rx(1,1);Y(1)-Ry(1,1);Z(1)-Rz(1,1)]/sqrt((X(1)-Rx(1,1))^2+(Y(1)-Ry(1,1))^2+(Z(1)-Rz(1,1))^2);
vectorR2_S2=[X(2)-Rx(2,1);Y(2)-Ry(2,1);Z(2)-Rz(2,1)]/sqrt((X(2)-Rx(2,1))^2+(Y(2)-Ry(2,1))^2+(Z(2)-Rz(2,1))^2);
vectorR3_S3=[X(3)-Rx(3,1);Y(3)-Ry(3,1);Z(3)-Rz(3,1)]/sqrt((X(3)-Rx(3,1))^2+(Y(3)-Ry(3,1))^2+(Z(3)-Rz(3,1))^2);

currentnum=currentnum+1;
disp(['当前计算完成    ',num2str(currentnum*100/totalprocess),'%'])
xgroup=[X(1);X(2);X(3)];
ygroup=[Y(1);Y(2);Y(3)];
zgroup=[Z(1);Z(2);Z(3)];
theta1=asin(Z(1)/G1)*180/pi;%旋转副夹角
theta2=asin(Z(2)/G2)*180/pi;
theta3=asin(Z(3)/G3)*180/pi;
phy1=subspace(vectorOS_S,vectorR1_S1)*180/pi;%球副夹角
phy2=subspace(vectorOS_S,vectorR2_S2)*180/pi;
phy3=subspace(vectorOS_S,vectorR3_S3)*180/pi;
alphaOS_OR=subspace(vectorOS_S,[0;0;1])*180/pi;%动平台相对于定平台的偏角
alphaOS3_OR3=subspace([vectorOS_S(1,1);vectorOS_S(2,1)],[1;0])*180/pi;%动平台相对于定平台转角
%alphaOS3_OR3=(asin(vectorOS_S(1,1)/sqrt(vectorOS_S(1,1)^2+vectorOS_S(2,1)^2)))*180/pi;
%subspace([vectorOS_S(1,1);vectorOS_S(2,1)],[0;1])*180/pi;

if phy1>MAX_angle
    erropost=erropost+1;
    EROG(erropost,1)=G1;
    EROG(erropost,2)=G2;
    EROG(erropost,3)=G3;
    EROG(erropost,4)=phy1;
        
    continue;
elseif phy2>MAX_angle
    erropost=erropost+1;  
    EROG(erropost,1)=G1;
    EROG(erropost,2)=G2;
    EROG(erropost,3)=G3;
    EROG(erropost,5)=phy2;
   
    continue;
elseif phy3>MAX_angle
    erropost=erropost+1;  
    EROG(erropost,1)=G1;
    EROG(erropost,2)=G2;
    EROG(erropost,3)=G3;
    EROG(erropost,6)=phy3;
   
    continue;
end

OS_0(osnumber,1)=OX;
OS_0(osnumber,2)=OY;
OS_0(osnumber,3)=OZ;

S1_0(osnumber,1)=X(1);
S1_0(osnumber,2)=Y(1);
S1_0(osnumber,3)=Z(1);

S2_0(osnumber,1)=X(2);
S2_0(osnumber,2)=Y(2);
S2_0(osnumber,3)=Z(2);

S3_0(osnumber,1)=X(3);
S3_0(osnumber,2)=Y(3);
S3_0(osnumber,3)=Z(3);


Eular_Cos=vectorOS_S(1,1)/sqrt(vectorOS_S(1,1)^2+vectorOS_S(2,1)^2);
Eular_Sin=vectorOS_S(2,1)/sqrt(vectorOS_S(1,1)^2+vectorOS_S(2,1)^2);
if sqrt(vectorOS_S(1,1)^2+vectorOS_S(2,1)^2)==0
    Eular_Cos=0;
    Eular_Sin=0;
end
EULAR(osnumber,1)=alphaOS_OR*Eular_Cos;
EULAR(osnumber,2)=alphaOS_OR*Eular_Sin;
EULAR(osnumber,3)=OZ;



osnumber=osnumber+1;

            end
         G3min=G3min+interval;
    end
     G2min=G2min+interval;
     G3min=G2min;
end
disp('正在进行空间对称操作');
S1_120=S1_0*Rz120_matrix';
S2_120=S2_0*Rz120_matrix';
S3_120=S3_0*Rz120_matrix';
OS_120=OS_0*Rz120_matrix';
EULAR_120=EULAR*Rz120_matrix';


S1_240=S1_0*Rz240_matrix';
S2_240=S2_0*Rz240_matrix';
S3_240=S3_0*Rz240_matrix';
OS_240=OS_0*Rz240_matrix';
EULAR_240=EULAR*Rz240_matrix';


OS=[OS_0;OS_120;OS_240];
S1=[S1_0;S3_120;S2_240];
S2=[S2_0;S1_120;S3_240];
S3=[S3_0;S2_120;S1_240];
EULAR=[EULAR;EULAR_120;EULAR_240];

% OS=[OS_120;OS_240];
% S1=[S3_120;S2_240];
% S2=[S1_120;S3_240];
% S3=[S2_120;S1_240];



EULAR_YMIRRO=EULAR;
EULAR_YMIRRO(:,1)=-EULAR_YMIRRO(:,1);
EULAR=[EULAR;EULAR_YMIRRO];


OS_YMIRRO=OS;
OS_YMIRRO(:,1)=-OS_YMIRRO(:,1);
OS=[OS;OS_YMIRRO];
S1_YMIRRO=S1;
S1_YMIRRO(:,1)=-S1_YMIRRO(:,1);
S2_YMIRRO=S2;
S2_YMIRRO(:,1)=-S2_YMIRRO(:,1);
S3_YMIRRO=S3;
S3_YMIRRO(:,1)=-S3_YMIRRO(:,1);
S1=[S1;S2_YMIRRO];
S2=[S2;S1_YMIRRO];
S3=[S3;S3_YMIRRO];



[column,row]=size(S3);

disp('正在绘制图像');

% figure(1236)
% fill3(Rx,Ry,Rz,'d');hold on
% plot([Rx(1,1),0],[Ry(1,1),0]);hold on
% plot([Rx(2,1),0],[Ry(2,1),0]);hold on
% plot([Rx(3,1),0],[Ry(3,1),0]);hold on
% scatter3(0,0,0);
%
% for i=1:1:column
%     %fill3([S1(i,1),S2(i,1),S3(i,1)],[S1(i,2),S2(i,2),S3(i,2)],[S1(i,3),S2(i,3),S3(i,3)],rand(1,3));hold on
% plot3([S1(i,1),S2(i,1),S3(i,1),S1(i,1)],...
%       [S1(i,2),S2(i,2),S3(i,2),S1(i,2)],...
%       [S1(i,3),S2(i,3),S3(i,3),S1(i,3)])
%   grid on
% hold on
% end
%
% for i=1:1:column
% plot3([S1(i,1),OS(i,1)],...
%       [S1(i,2),OS(i,2)],...
%       [S1(i,3),OS(i,3)])
%   grid on
%  hold on
% end

% figure(4)
% fill3(Rx,Ry,Rz,'d');hold on
% plot([Rx(1,1),0],[Ry(1,1),0]);hold on
% plot([Rx(2,1),0],[Ry(2,1),0]);hold on
% plot([Rx(3,1),0],[Ry(3,1),0]);hold on
% % S1point=alphaShape(S1(:,1),S1(:,2),S1(:,3),1);
% % S2point=alphaShape(S2(:,1),S2(:,2),S2(:,3),1);
% % S3point=alphaShape(S3(:,1),S3(:,2),S3(:,3),1);
% % [triS1, xyzS1] = boundaryFacets(S1point);
% % trisurf(triS1,xyzS1(:,1),xyzS1(:,2),xyzS1(:,3),...
% %     'FaceColor','cyan','FaceAlpha',0.5)
% % axis equal
% % [triS2, xyzS2] = boundaryFacets(S2point);
% % trisurf(triS2,xyzS2(:,1),xyzS2(:,2),xyzS2(:,3),...
% %     'FaceColor','red','FaceAlpha',0.5)
% % axis equal
% % [triS3, xyzS3] = boundaryFacets(S3point);
% % trisurf(triS3,xyzS3(:,1),xyzS3(:,2),xyzS3(:,3),...
% %     'FaceColor','green','FaceAlpha',0.5)
% % axis equal
% scatter3(S1(:,1),S1(:,2),S1(:,3),'f');hold on
% scatter3(S2(:,1),S2(:,2),S2(:,3),'g');hold on
% scatter3(S3(:,1),S3(:,2),S3(:,3),'s');hold on
%
% hold on

OSpicturestep1=unique(OS,'rows');
OSpicture=sortrows(OSpicturestep1,3);
OSpoint=alphaShape(OSpicture(:,1),OSpicture(:,2),OSpicture(:,3));
EULARpicturestep1=unique(EULAR,'rows');
EULARpicture=sortrows(EULARpicturestep1,3);
EULARpoint=alphaShape(EULARpicture(:,1),EULARpicture(:,2),EULARpicture(:,3));
figure(111)
[triOS, xyzOS] = boundaryFacets(OSpoint);
%plot(alphaShape(xyzOS(:,1),xyzOS(:,2),xyzOS(:,3),'HoleThreshold',1))
trisurf(triOS,xyzOS(:,1),xyzOS(:,2),xyzOS(:,3),'FaceColor','magenta','FaceAlpha',1)
title('中心点工作空间');
xlabel('X(mm)');
ylabel('Y(mm)');
zlabel('Z(mm)');
%light('position',[0.2 0.3 1]);
axis equal

% figure(112)
% num=boundary(EULAR(:,3),EULAR(:,2));
% plot(EULAR(num,3),EULAR(num,2))
% num1=boundary(EULAR(:,3),EULAR(:,1));
% plot(EULAR(num1,3),EULAR(num1,1))

figure(12313)
[triEULAR, xyzEULAR] = boundaryFacets(EULARpoint);

trisurf(triEULAR,xyzEULAR(:,1),xyzEULAR(:,2),xyzEULAR(:,3),'FaceColor','magenta','FaceAlpha',1)
title('姿态偏转工作空间');
xlabel('X轴角度分量（°）');
ylabel('Y轴角度分量（°）');
zlabel('Z(mm)');
axis equal







%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function p=setcoeffirent(L_1,L_2,G1,G2,G3)%正运动学约束方程组
p=@norforwardcarculate;

function F=norforwardcarculate(X)

F(1)= (X(1)+L_1/2)^2+3*(X(1)/3+L_1/6)^2+X(4)^2-G1^2;
F(2)= (X(2)-L_1/2)^2+3*(-X(2)/3+L_1/6)^2+X(5)^2-G2^2;
F(3)= (X(3)-sqrt(3)*L_1/3)^2+X(6)^2-G3^2;
F(4)= (X(1)-X(2))^2+((X(1)+X(2))^2)/3+(X(4)-X(5))^2-L_2^2;
F(5)= X(1)^2+(sqrt(3)*X(1)/3-X(3))^2+(X(4)-X(6))^2-L_2^2;
F(6)= X(2)^2+(sqrt(3)*X(2)/3+X(3))^2+(X(5)-X(6))^2-L_2^2;
end
end

function Rz_matrix=RZ(Theta)

Rz_matrix=[cos(Theta),-sin(Theta),0;sin(Theta),cos(Theta),0;0,0,1];
%绕z轴旋转z角

end

[/mw_shl_code]