3-SPR并联平台Matlab运动学正解及工作空间绘制

clear;clc;
lb=[-300200200000];
ub=[0300300700700700];
rng default
x0=100*randn（61）;

MIN_leglength=300;%支链最短值
MAX_leglength=600;%支链最长值
interval=10;%数据细分度.数据点间隔越小，图像越真实，但计算时间越长，，，可以改变
currentnum=0;%当前循环个数，用于百分比表示计算进度
osnumber=1;%动平面中心的坐标指针
erropost=0;%不符合机构转角限制的位姿个数

L_1=500;%上平台等边三角形边长，，可以改变
L_2=500;%下平台等边三角形边长，，可以改变
Rx=[-L_1/2;L_1/2;0];%旋转副平台旋转副X坐标，，可以改变
Ry=[-sqrt（3）.*L_1/6;-sqrt（3）.*L_1/6;sqrt（3）.*L_1/3];%旋转副平台旋转副Y坐标，，可以改变
Rz=[0;0;0];%旋转副平台旋转副Z坐标，，可以改变
R1_steady=[Rx（11）;Ry（11）;Rz（11）];%旋转副平台旋转副1坐标，，可以改变
R2_steady=[Rx（21）;Ry（21）;Rz（21）];%旋转副平台旋转副2坐标，，可以改变
R3_steady=[Rx（31）;Ry（31）;Rz（31）];%旋转副平台旋转副3坐标，，可以改变
OR_steady=[0;0;0];%旋转副平台中心点坐标
vectorOR_R1=[R1_steady（11）-OR_steady（11）;R1_steady（21）-OR_steady（21）;R1_steady（31）-OR_steady（31）]/（sqrt（3）.*L_1/3）;
vectorOR_R2=[R2_steady（11）-OR_steady（11）;R2_steady（21）-OR_steady（21）;R2_steady（31）-OR_steady（31）]/（sqrt（3）.*L_1/3）;
vectorOR_R3=[R3_steady（11）-OR_steady（11）;R3_steady（21）-OR_steady（21）;R3_steady（31）-OR_steady（31）]/（sqrt（3）.*L_1/3）;
vectorO_OR=[0;0;1];

G1min=MIN_leglength;
G2min=MIN_leglength;
G3min=MIN_leglength;
leenum=（MAX_leglength-MIN_leglength）/interval+1;%单支链长数据个数
totalprocess=leenum+leenum*（leenum-1）+nchoosek（leenum3）;

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_1L_2G1G2G3）;
[xres]=lsqnonlin（px0lbub）;
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_S1vectorOS_S2）;%动平台法向量

vectorR1_S1=[X（1）-Rx（11）;Y（1）-Ry（11）;Z（1）-Rz（11）]/sqrt（（X（1）-Rx（11））^2+（Y（1）-Ry（11））^2+（Z（1）-Rz（11））^2）;
vectorR2_S2=[X（2）-Rx（21）;Y（2）-Ry（21）;Z（2）-Rz（21）]/sqrt（（X（2）-Rx（21））^2+（Y（2）-Ry（21））^2+（Z（2）-Rz（21））^2）;
vectorR3_S3=[X（3）-Rx（31）;Y（3）-Ry（31）;Z（3）-Rz（31）]/sqrt（（X（3）-Rx（31））^2+（Y（3）-Ry（31））^2+（Z（3）-Rz（31））^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_SvectorR1_S1）*180/pi;%球副夹角
phy2=subspace（vectorOS_SvectorR2_S2）*180/pi;
phy3=subspace（vectorOS_SvectorR3_S3）*180/pi;
alphaOS_OR=subspace（vectorOS_S[0;0;1]）*180/pi;%动平台相对于定平台的偏角

if phy1>40
    erropost=erropost+1; 
    EROG（erropost1）=G1;
    EROG（erropost2）=G2;
    EROG（erropost3）=G3;
    EROG（erropost4）=phy1;