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

MIN_leglength=300;%支鏈最短值
MAX_leglength=600;%支鏈最長(zhǎng)值
interval=10;%數(shù)據(jù)細(xì)分度.數(shù)據(jù)點(diǎn)間隔越小，圖像越真實(shí)，但計(jì)算時(shí)間越長(zhǎng)，，，可以改變
currentnum=0;%當(dāng)前循環(huán)個(gè)數(shù)，用于百分比表示計(jì)算進(jìn)度
osnumber=1;%動(dòng)平面中心的坐標(biāo)指針
erropost=0;%不符合機(jī)構(gòu)轉(zhuǎn)角限制的位姿個(gè)數(shù)

L_1=500;%上平臺(tái)等邊三角形邊長(zhǎng)，，可以改變
L_2=500;%下平臺(tái)等邊三角形邊長(zhǎng)，，可以改變
Rx=[-L_1/2;L_1/2;0];%旋轉(zhuǎn)副平臺(tái)旋轉(zhuǎn)副X坐標(biāo)，，可以改變
Ry=[-sqrt（3）.*L_1/6;-sqrt（3）.*L_1/6;sqrt（3）.*L_1/3];%旋轉(zhuǎn)副平臺(tái)旋轉(zhuǎn)副Y坐標(biāo)，，可以改變
Rz=[0;0;0];%旋轉(zhuǎn)副平臺(tái)旋轉(zhuǎn)副Z坐標(biāo)，，可以改變
R1_steady=[Rx（11）;Ry（11）;Rz（11）];%旋轉(zhuǎn)副平臺(tái)旋轉(zhuǎn)副1坐標(biāo)，，可以改變
R2_steady=[Rx（21）;Ry（21）;Rz（21）];%旋轉(zhuǎn)副平臺(tái)旋轉(zhuǎn)副2坐標(biāo)，，可以改變
R3_steady=[Rx（31）;Ry（31）;Rz（31）];%旋轉(zhuǎn)副平臺(tái)旋轉(zhuǎn)副3坐標(biāo)，，可以改變
OR_steady=[0;0;0];%旋轉(zhuǎn)副平臺(tái)中心點(diǎn)坐標(biāo)
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;%單支鏈長(zhǎng)數(shù)據(jù)個(gè)數(shù)
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）;%動(dòng)平臺(tái)法向量

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（[‘當(dāng)前計(jì)算完成????‘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;%旋轉(zhuǎn)副夾角
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;%動(dòng)平臺(tái)相對(duì)于定平臺(tái)的偏角

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