function t=liugan(th,th1)
s3=th(1);
Lab=0.032;
Lbc=0.084;
Lcd=0.045;
Lde=0.035;
L1=0.106;
L2=0.048;
t=[Lab*cos(th1)+Lbc*cos(th(2))-s3;
Lab*sin(th1)+Lbc*sin(th(2));
Lab*cos(th1)+Lbc*cos(th(2))+Lcd*cos(th(3))+Lde*cos(th(4))-L1;
Lcd*sin(th(3))+Lde*sin(th(4))-L2];
clear;
clc;
Lab=0.032;
Lbc=0.084;
Lcd=0.045;
Lde=0.035;
L1=0.106;
L2=0.048;
m2=3.0;
m3=50;
m4=4.5;
Js2=0.012;
Js4=0.025;
g=9.81;
Lef=0.026;
Lbs2=0.042;
Lcs4=0.030;
th1=0:pi/180:2*pi;
w1=12;
th2345=zeros(length(th1),4);
O=[];
P=[];
Q=[];
options=optimset('display','off');
for m=1:length(th1)
th2345(m,:)=fsolve('liugan',[0 0 0.995 2.793],options,th1(m));
end
th2345;
s3=th2345(:,1);
th2=th2345(:,2);
th4=th2345(:,3);
th5=th2345(:,4);
for m=1:length(th1);
A=[-1 -Lbc*sin(th2(m)) 0 0;
0 Lbc*cos(th2(m)) 0 0;
0 -Lbc*sin(th2(m)) -Lcd*sin(th4(m)) -Lde*sin(th5(m));
0 0 Lcd*cos(th4(m)) Lde*cos(th5(m))];
B=[Lab*sin(th1(m));
-Lab*cos(th1(m));
Lab*sin(th1(m));
0];
W=A\(w1*B);
O=[O;W'];
v3=W(1);
w2=W(2);
w4=W(3);
w5=W(4);
A=[-1 -Lbc*sin(th2(m)) 0 0;
0 Lbc*cos(th2(m)) 0 0;
0 -Lbc*sin(th2(m)) -Lcd*sin(th4(m)) -Lde*sin(th5(m));
0 0 Lcd*cos(th4(m)) Lde*cos(th5(m))];
C=[0 Lbc*w2*cos(th2(m)) 0 0;
0 Lbc*w2*sin(th2(m)) 0 0;
0 Lbc*w2*cos(th2(m)) Lcd*w4*cos(th4(m)) Lde*w5*cos(th5(m));
0 0 Lcd*w4*sin(th4(m)) Lde*w5*sin(th5(m))];
D=[Lab*cos(th1(m));Lab*sin(th1(m));Lab*cos(th1(m));0];
E=C*W+w1^2*D;
K=A\E;
P=[P;K'];
a3=K(1);
k2=K(2);
k4=K(3);
k5=K(4);
Xs2x=Lab*cos(th1)+Lbs2*cos(th2(m));
Ys2y=Lab*sin(th1)+Lbs2*sin(th2(m));
Xs4x=Lab*cos(th1)+Lbs2*cos(th2(m))+Lcs4*cos(th4(m));
Ys4y=Lcs4*sin(th4(m));
Vs2x=-Lab*w1*sin(th1)-Lbs2*w2*sin(th2(m));
Vs2y=Lab*w1*cos(th1)+Lbs2*w2*cos(th2(m));
Vs4x=-Lab*w1*sin(th1)-Lbc*w2*sin(th2(m))-Lcs4*w4*sin(th4(m));
Vs4y=Lcs4*cos(th4(m))*w4;
As2x=-Lab*w1^2*cos(th1)-0.5*Lbc*k2*sin(th2(m))-0.5*Lbc*w2^2*cos(th2(m));
As2y

这是啥

呜了哇啦

matlab？

不给