利用fsolve函数求解Stewart平台运动学正解

function  STEWART_fsolve()
format long
x=fsolve(@myfun,[0;0;20;0;0;0]) %调用函数
end

function f=myfun(x)
syms x1 x2 x3 a b g
X=[x1;x2;x3];
RX=[1 0 0;0 cos(a) -sin(a);0 sin(a) cos(a)];
RY=[cos(b) 0 sin(b);0 1 0;-sin(b) 0 cos(b)];
RZ=[cos(g) -sin(g) 0;sin(g) cos(g) 0; 0 0 1];
R=RZ*RY*RX;
p1=[12;-2;0];
p2=[12;2;0];
p3=[-4.26794919;11.39230485;0];
p4=[-7.73205081;9.39230485;0];
p5=[-7.73205081;-9.39230485;0];
p6=[-4.26794919;-11.39230485;0];
r1=[12;-14;5];
r2=[12;14;5];
r3=[6.12435565;17.39230485;5];
r4=[-18.12435565;3.39230485;5];
r5=[-18.12435565;-3.39230485;5];
r6=[6.12435565;-17.39230485;5];
l1=20;
l2=20;
l3=25;
l4=25;
l5=25;
l6=30;
f1=(norm(X+R*p1-r1))^2-l1^2;
f2=(norm(X+R*p2-r2))^2-l2^2;
f3=(norm(X+R*p3-r3))^2-l3^2;
f4=(norm(X+R*p4-r4))^2-l4^2;
f5=(norm(X+R*p5-r5))^2-l5^2;
f6=(norm(X+R*p6-r6))^2-l6^2;
f=[f1;f2;f3;f4;f5;f6];
x1=x(1);
x2=x(2);
x3=x(3);
a=x(4);
b=x(5);
g=x(6);
f=eval(f);
end