clear all;clc;

%初始条件

theta1=linspace(-18,342,100);%单位度

theta1=theta1*pi/180;%转换为弧度制

W1=80*pi/30;%角速度单位rad/s

H=0.5;%行程单位m

L1=0.1329;%O2A的长度单位m

L3=0.8091;%O3B的长度单位m

L4=0.2589;%BF的长度单位m

L6=0.430;%O2O3的长度单位m

L6u=0.7893;%O3D的长度单位m

Z=pi/180;%角度与弧度之间的转换

dT=(theta1(3)-theta1(2))/W1;%时间间隔

for j=1:100

t(j)=dT*(j-1);%时间因素

end

%求解S3、Theta3、Theta4和SE四个变量

theta3(i)=atan((L6+L1*sin(theta1(i)))/L1/cos(theta(i))); for i=1:100

S3=L1*cos(theta1(i))/cos(theta3(i));

theta4(i)=asin((L6u-L3*sin(theta3(i)))/L4);

SE(i)=L3*cos(theta3(i))+L4*cos(theta4(i));

end

%求解完成

%求解完成

%求解VS3、W3、W4和VE四个变量

for i=1:100

J= inv([cos(theta3(i)),-S3(i)*sin(theta3(i)),0,0;

sin(theta3(i)),S3(i)*cos(theta3(i)),0,0;

0,-L3*sin(theta3(i)),-L4*sin(theta4(i)),-1; 0,L3*cos(theta3(i)),L4*cos(theta4(i)),0]);

K=J*W1*[-L1*sin(theta1(i));L1*cos(theta1(i));0;0]; VS3(i)=K(1);

W3(i)=K(2);

W4(i)=K(3);

VE(i)=K(4);

end%求解完成

%求解aS3、a3、a4、aE四个变量

for i=1:100

J= inv([cos(theta3(i)),-S3(i)*sin(theta3(i)),0,0;

sin(theta3(i)),S3(i)*cos(theta3(i)),0,0;

0,-L3*sin(theta3(i)),-L4*sin(theta4(i)),-1; 0,L3*cos(theta3(i)),L4*cos(theta4(i)),0]);

P=W1*[-L1*cos(theta1(i));L1*sin(theta1(i));0;0];