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];