3-RPS并联平台可达工作空间MATLAB求解

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

MIN_leglength=300;%支链最短值
MAX_leglength=600;%支链最长值
interval=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（11）;Ry（11）;Rz（11）];
R2_steady=[Rx（21）;Ry（21）;Rz（21）];
R3_steady=[Rx（31）;Ry（31）;Rz（31）];
G1min=MIN_leglength;
G2min=MIN_leglength;
G3min=MIN_leglength;
leenum=（MAX_leglength-MIN_leglength）/interval+1;%单支链长数据个数
totalprocess=leenum+leenum*（leenum-1）+nchoosek（leenum3）;
%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_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;%动平台相对于定平台的偏角
alphaOS3_OR3=subspace（[vectorOS_S（11）;vectorOS_S（21）][1;0]）*180/pi;%动平台相对于定平台转角
%alphaOS3_OR3=（asin（vectorOS_S（11）/sqrt（vectorOS_S（11）^2+vectorOS_S（21）^2）））*180/pi;
%subspace（[vectorOS_S（11）;vectorOS_S（21）][0;1]）*180/pi;

if phy1>MAX_angle
    erropost=erropost+1; 
    EROG（erropost1）=G1;
    EROG（erropost2）=G2;
    EROG（erropost3）=G3;
    EROG（erropost4）=phy1;
        
    continue;
elseif phy2>MAX_angle
    erropost=erropost+1;  
    EROG（erropost1）=G1;
    EROG（erropost2）=G2;
    EROG（erropost3）=G3;
    EROG（erropost5）=phy2;
    
    continue;
elseif phy3>MAX_angle
    erropost=erropost+1;  
    EROG（erropost1）=G1;
    EROG（erropost