1.主程序:
function [position,velocity,acceleration] = calc ulat(L1,L2,Lbe,theta1,omega1)
% 计算theta2,theta5,s1,s2
theta2=acos(-L1*cos(theta1)/L2);
if theta2<pi
theta2=2*pi-theta2;
end
s1=-L1*sin(theta1)-L2*sin(theta2);
sth1=L1*cos(theta1)+Lbe*cos(theta2-112*pi/180)+24;
sth2=L1*sin(theta1)+Lbe*sin(theta2-112*pi/180)-64;
s2=sqrt(sth1*sth1+sth2*sth2);
theta5=acos(sth1/s2);
if theta5<pi
theta5=2*pi-theta5;
end
position=[theta2;theta5;s1;s2];
%计算omega2.omega5,speed1,speed2
addpath('E:\Program Files')
A=[ L2*sin(theta2),0,0,0;
-L2*cos(theta2),0,-1,0;
Lbe*sin(theta2-112*pi/180),-s2*sin(theta5),0,cos(theta5);
-Lbe*cos(theta2-112*pi/180),s2*cos(theta5),0,sin(theta5);
];
B=[-L1*sin(theta1);L1*cos(theta1);-L1*sin(theta1);L1*cos(theta1);];
velocity =A\(omega1*B);
omega2=velocity(1);
omega5=velocity(2);
speed1=velocity(3);
speed2=velocity(4);
%计算alpha2,alpha5,accele1,accele2
F=[ L2*sin(theta2),0,0,0;
-L2*cos(theta2),-1,0,0;
-Lbe*sin(theta2-112*pi/180),0,s2*sin(theta5),-cos(theta5);
Lbe*cos(theta2-112*pi/180),0,-s2*cos(theta5),-sin(theta5);
];
C=[
-L2*omega2*cos(theta2),0,0,0;
-L2*omega2*sin(theta2),0,0,0;
Lbe*omega2*cos(theta2-112*pi/180),0,-2*speed2*sin(theta5)+s2*omega5*cos(theta5),0;
Lbe*omega2*sin(theta2-112*pi/180),0,2*speed2*cos(theta5)-s2*omega5*sin(theta5), 0;
];
D=[omega2;speed1;omega5;speed2;];
E=[-L1*cos(theta1);-L1*sin(theta1);-L1*cos(theta1);L1*sin(theta1);];
acceleration=F\(C*D+omega1*omega1*E);
%acceleration=A\(-C*velocity+omega1*D);
alpha2=acceleration(1);
accele1=acceleration(2);
alpha5=acceleration(3);
accele2=acceleration(4);
End
2.功能函数:
function example3_6()
close all
clc
%(1) 已知条件赋值
L1=32;
L2=100;
Lbe=28;
omega1=5;
Lfg=90;
ja_to_hu=pi/180;%角度转弧度
%(2) 计算位置,(角)速度,(角)加速度
for n1=1:1:361
theta1=(n1-1)*ja_to_hu;
[position,velocity,acceleration] = calculat(L1,L2,Lbe,theta1,omega1);
%%位置
theta2(n1)=position(1);%*hu_to_ja
theta5(n1)=position(2);%*hu_to_ja
s1(n1)=position(3);
s2(n1)=position(4);
Ex(n1)=L1*cos(n1*pi/180)+Lbe*cos(theta2(n1)-112*pi/180);
Ey(n1)=L1*sin(n1*pi/180)+Lbe*sin(theta2(n1)-112*pi/180);
%%速度
omega2(n1)=velocity(1);
omega5(n1)=velocity(2);
speed1(n1)=velocity(3);
speed2(n1)=velocity(4);
%%加速度
alpha2(n1)=acceleration(1);
accele1(n1)=acceleration(2);
alpha5(n1)=acceleration(3);
accele2(n1)=acceleration(4);
end
%(3) 画位置线图
figure(1);
n2=1:360;
subplot(2,1,1);
plot(n2,theta2(n2)*180/pi,n2,theta5(n2)*180/pi,'k');
title('角度线图');
xlabel('主动杆角位移 /\circ');
ylabel('从动杆角位移/\circ');
grid on;hold on;
text(170,277,'\theta_2')
text(180,230,'\theta_5')
subplot(2,1,2);
plot(n2,s1(n2),n2,s2(n2),'k');
% title('杆长度线图');
ylabel('杆长度 /\circ');
xlabel('主动杆角位移/\circ');
grid on;hold on;
text(170,90,'S1')
text(190,70,'S2')
figure(2);
subplot(2,1,1);
plot(n2,omega2(n2)*180/pi,n2,omega5(n2)*180/pi,'k');%rad/s
title('角速度线图');
xlabel('主动杆角位移 /\circ');
ylabel('从动杆角速度/\circ');
grid on;hold on;
text(70,60,'\omega_2')
text(80,-260,'\omega_5')
subplot(2,1,2);
plot(n2,speed1(n2),n2,speed2(n2),'k');%
title('速度线图');
xlabel('主动杆角位移 /\circ');
ylabel('从动杆速度/\circ');
grid on;hold on;
text(55,117,'speed2')
text(105,20,'speed1')
figure(3);
subplot(2,1,1);
plot(n2,alpha2(n2)*180/pi,n2,alpha5(n2)*180/pi,'k');
title('角加度线图');
xlabel('主动杆角位移 /\circ');
ylabel('角加速度/\circ');
text(100,-600,'\alpha_2')
text(120,1300,'\alpha_5')
grid on;hold on;
subplot(2,1,2);
plot(n2,accele1(n2),n2,accele2(n2),'k');
title('加度线图');
xlabel('主动杆角位移 /\circ');
ylabel('加速度/\circ');
text(100,1800,'accele2')
text(70,230,'accele1')
grid on;hold on;
% % %
% % %(6) 画机构运动简图
figure(4)
m=moviein(1);
j=0;
for n1 = 0:10:360
j=j+1;
clf;
x(1)=0;
y(1)=0;
x(2)=L1*cos(n1*pi/180);
y(2)=L1*sin(n1*pi/180);
x(3)=0;
y(3)=-s1(n1+1);
x(4)=0;
y(4)=-s1(n1+1)-80;
x(5)=0;
y(5)=-s1(n1+1);
x(6)=L1*cos(n1*pi/180);
y(6)=L1*sin(n1*pi/180);
x(7)=L1*cos(n1*pi/180)+Lbe*cos(theta2(n1+1)-112*pi/180);
y(7)=L1*sin(n1*pi/180)+Lbe*sin(theta2(n1+1)-112*pi/180);
x(8)=-24;
y(8)=64;
x(9)=Lfg*cos(theta5(n1+1)+118*pi/180)-24;
y(9)=Lfg*sin(theta5(n1+1)+118*pi/180)+64;
x(10)=-24;
y(10)=64;
x(11)=110*cos(theta5(n1+1))-24;
y(11)=110*sin(theta5(n1+1))+64;
plot(x,y);
grid on;hold on;
plot(x(1),y(1),'ro');
plot(x(2),y(2),'o');
plot(x(3),y(3),'o');
plot(x(6),y(6),'o');
plot(x(7),y(7),'r*');
plot(x(8),y(8),'ro');
axis([-220 200 -270 200]);
m(j)=getframe;
end
movie(m,10);
end
function [position,velocity,acceleration] = calc ulat(L1,L2,Lbe,theta1,omega1)
% 计算theta2,theta5,s1,s2
theta2=acos(-L1*cos(theta1)/L2);
if theta2<pi
theta2=2*pi-theta2;
end
s1=-L1*sin(theta1)-L2*sin(theta2);
sth1=L1*cos(theta1)+Lbe*cos(theta2-112*pi/180)+24;
sth2=L1*sin(theta1)+Lbe*sin(theta2-112*pi/180)-64;
s2=sqrt(sth1*sth1+sth2*sth2);
theta5=acos(sth1/s2);
if theta5<pi
theta5=2*pi-theta5;
end
position=[theta2;theta5;s1;s2];
%计算omega2.omega5,speed1,speed2
addpath('E:\Program Files')
A=[ L2*sin(theta2),0,0,0;
-L2*cos(theta2),0,-1,0;
Lbe*sin(theta2-112*pi/180),-s2*sin(theta5),0,cos(theta5);
-Lbe*cos(theta2-112*pi/180),s2*cos(theta5),0,sin(theta5);
];
B=[-L1*sin(theta1);L1*cos(theta1);-L1*sin(theta1);L1*cos(theta1);];
velocity =A\(omega1*B);
omega2=velocity(1);
omega5=velocity(2);
speed1=velocity(3);
speed2=velocity(4);
%计算alpha2,alpha5,accele1,accele2
F=[ L2*sin(theta2),0,0,0;
-L2*cos(theta2),-1,0,0;
-Lbe*sin(theta2-112*pi/180),0,s2*sin(theta5),-cos(theta5);
Lbe*cos(theta2-112*pi/180),0,-s2*cos(theta5),-sin(theta5);
];
C=[
-L2*omega2*cos(theta2),0,0,0;
-L2*omega2*sin(theta2),0,0,0;
Lbe*omega2*cos(theta2-112*pi/180),0,-2*speed2*sin(theta5)+s2*omega5*cos(theta5),0;
Lbe*omega2*sin(theta2-112*pi/180),0,2*speed2*cos(theta5)-s2*omega5*sin(theta5), 0;
];
D=[omega2;speed1;omega5;speed2;];
E=[-L1*cos(theta1);-L1*sin(theta1);-L1*cos(theta1);L1*sin(theta1);];
acceleration=F\(C*D+omega1*omega1*E);
%acceleration=A\(-C*velocity+omega1*D);
alpha2=acceleration(1);
accele1=acceleration(2);
alpha5=acceleration(3);
accele2=acceleration(4);
End
2.功能函数:
function example3_6()
close all
clc
%(1) 已知条件赋值
L1=32;
L2=100;
Lbe=28;
omega1=5;
Lfg=90;
ja_to_hu=pi/180;%角度转弧度
%(2) 计算位置,(角)速度,(角)加速度
for n1=1:1:361
theta1=(n1-1)*ja_to_hu;
[position,velocity,acceleration] = calculat(L1,L2,Lbe,theta1,omega1);
%%位置
theta2(n1)=position(1);%*hu_to_ja
theta5(n1)=position(2);%*hu_to_ja
s1(n1)=position(3);
s2(n1)=position(4);
Ex(n1)=L1*cos(n1*pi/180)+Lbe*cos(theta2(n1)-112*pi/180);
Ey(n1)=L1*sin(n1*pi/180)+Lbe*sin(theta2(n1)-112*pi/180);
%%速度
omega2(n1)=velocity(1);
omega5(n1)=velocity(2);
speed1(n1)=velocity(3);
speed2(n1)=velocity(4);
%%加速度
alpha2(n1)=acceleration(1);
accele1(n1)=acceleration(2);
alpha5(n1)=acceleration(3);
accele2(n1)=acceleration(4);
end
%(3) 画位置线图
figure(1);
n2=1:360;
subplot(2,1,1);
plot(n2,theta2(n2)*180/pi,n2,theta5(n2)*180/pi,'k');
title('角度线图');
xlabel('主动杆角位移 /\circ');
ylabel('从动杆角位移/\circ');
grid on;hold on;
text(170,277,'\theta_2')
text(180,230,'\theta_5')
subplot(2,1,2);
plot(n2,s1(n2),n2,s2(n2),'k');
% title('杆长度线图');
ylabel('杆长度 /\circ');
xlabel('主动杆角位移/\circ');
grid on;hold on;
text(170,90,'S1')
text(190,70,'S2')
figure(2);
subplot(2,1,1);
plot(n2,omega2(n2)*180/pi,n2,omega5(n2)*180/pi,'k');%rad/s
title('角速度线图');
xlabel('主动杆角位移 /\circ');
ylabel('从动杆角速度/\circ');
grid on;hold on;
text(70,60,'\omega_2')
text(80,-260,'\omega_5')
subplot(2,1,2);
plot(n2,speed1(n2),n2,speed2(n2),'k');%
title('速度线图');
xlabel('主动杆角位移 /\circ');
ylabel('从动杆速度/\circ');
grid on;hold on;
text(55,117,'speed2')
text(105,20,'speed1')
figure(3);
subplot(2,1,1);
plot(n2,alpha2(n2)*180/pi,n2,alpha5(n2)*180/pi,'k');
title('角加度线图');
xlabel('主动杆角位移 /\circ');
ylabel('角加速度/\circ');
text(100,-600,'\alpha_2')
text(120,1300,'\alpha_5')
grid on;hold on;
subplot(2,1,2);
plot(n2,accele1(n2),n2,accele2(n2),'k');
title('加度线图');
xlabel('主动杆角位移 /\circ');
ylabel('加速度/\circ');
text(100,1800,'accele2')
text(70,230,'accele1')
grid on;hold on;
% % %
% % %(6) 画机构运动简图
figure(4)
m=moviein(1);
j=0;
for n1 = 0:10:360
j=j+1;
clf;
x(1)=0;
y(1)=0;
x(2)=L1*cos(n1*pi/180);
y(2)=L1*sin(n1*pi/180);
x(3)=0;
y(3)=-s1(n1+1);
x(4)=0;
y(4)=-s1(n1+1)-80;
x(5)=0;
y(5)=-s1(n1+1);
x(6)=L1*cos(n1*pi/180);
y(6)=L1*sin(n1*pi/180);
x(7)=L1*cos(n1*pi/180)+Lbe*cos(theta2(n1+1)-112*pi/180);
y(7)=L1*sin(n1*pi/180)+Lbe*sin(theta2(n1+1)-112*pi/180);
x(8)=-24;
y(8)=64;
x(9)=Lfg*cos(theta5(n1+1)+118*pi/180)-24;
y(9)=Lfg*sin(theta5(n1+1)+118*pi/180)+64;
x(10)=-24;
y(10)=64;
x(11)=110*cos(theta5(n1+1))-24;
y(11)=110*sin(theta5(n1+1))+64;
plot(x,y);
grid on;hold on;
plot(x(1),y(1),'ro');
plot(x(2),y(2),'o');
plot(x(3),y(3),'o');
plot(x(6),y(6),'o');
plot(x(7),y(7),'r*');
plot(x(8),y(8),'ro');
axis([-220 200 -270 200]);
m(j)=getframe;
end
movie(m,10);
end


