Matlab：四杆机构函数

function [theta,omega,alpha]=crank_rocker(theta1,omega1,alpha1,l1,l2,l3,l4)
% 1. 计算从动件的角位移
L=sqrt(l4*l4+l1*l1-2*l1*l4*cos(theta1));
phi=asin((l1./L)*sin(theta1));
beta=acos((-l2*l2+l3*l3+L*L)/(2*l3*L));
if beta<0
    beta=beta+pi;
end
theta3=pi-phi-beta; % theta3 表示杆3转过角度
theta2=asin((l3*sin(theta3)-l1*sin(theta1))/l2);
% theta2 表示杆2转过角度
theta=[theta2;theta3] ;
% 2. 计算从动件的角速度
A=[-l2*sin(theta2), l3*sin(theta3); % 机构从动件的位置参数矩阵
    l2*cos(theta2), -l3*cos(theta3)];
B=[l1*sin(theta1); -l1*cos(theta1)]; % 机构原动件的位置参数列阵
omega=A\(omega1*B); % 机构从动件的速度列阵
omega2=omega(1);
omega3=omega(2);
% 3. 计算从动件的角加速度
A=[-l2*sin(theta2), l3*sin(theta3); l2*cos(theta2), -l3*cos(theta3)];
At=[-omega2*l2*cos(theta2), omega3*l3*cos(theta3); -omega2*l2*sin(theta2), omega3*l3*sin(theta3)];
B=[l1*sin(theta1); -l1*cos(theta1)];
% 机构原动件的位置参数列阵
Bt=[omega1*l1*cos(theta1); omega1*l1*sin(theta1)]; % Bt=dB/dt
alpha=A\(-At*omega+alpha1*B+omega1*Bt); % 机构从动件的加速度列阵

figure(6)
plot(0:0.001:(tend*2),data_axe,'r')
grid on
title('E点X方向加速度'); 
xlabel('时间(s)') 
ylabel('加速度(m/s^2)')

figure(7)
plot(0:0.001:(tend*2),data_aye,'r')
grid on
title('E点Y方向价速度'); 
xlabel('时间(s)') 
ylabel('加速度(m/s^2)')

plot(n1,theta2*du,n1,theta3*du,'k'); 
title('角位移线图'); 
xlabel('曲柄转角 \theta_1 / \circ');
ylabel('角位移 / \circ'); 
grid on; 
text(140,170,'\theta_3'); 
text(140,30,'\theta_2');

plot(n1,omega2,n1,omega3,'k');
title('角速度线图'); 
xlabel('曲柄转角 \theta_1 / \circ');
ylabel('角速度 / rad\cdots^{-1}');
grid on;
text(250,130,'\omega_2');
text(130,165,'\omega_3');

plot(n1,alpha2,n1,alpha3,'k');
title('角加速度线图');
xlabel('曲柄转角 \theta_1 / \circ');
ylabel('角加速度 / rad\cdots^{-2}');
grid on;
text(230,2e4,'\alpha_2');
text(30,7e4,'\alpha_3');

clear
clc

l1 = 5.9;
l2 = 18;
l3 = 18;
xa = 0;
ya = 0;
xd = 15.75;
yd = -14;
lad = sqrt((xa-xd)^2+(ya-yd)^2);
w1 = 1; %rad/s
a1 = 0; %rad/s^2

i = 0;
for theta1 = 0:0.05:2*pi
    i = i+1;
    xb = l1*cos(theta1);
    yb = l1*sin(theta1);
    lbd = sqrt((xb-xd)^2+(yb-yd)^2);
    theta_bdc = acos((lbd^2+l3^2-l2^2)/(2*lbd*l3));
    theta_bda = acos((lbd^2+lad^2-l1^2)/(2*lbd*lad));
    theta_adx = atan(-yd/xd);
    theta3 = pi-theta_bdc-theta_bda-theta_adx;
    if(theta1>pi-theta_adx && theta1<2*pi-theta_adx)
        theta3 = pi-theta_bdc+theta_bda-theta_adx;
    end
    theta2 = asin((yd+l3*sin(theta3)-l1*sin(theta1))/l2);
    xc = xd+l3*cos(theta3);
    yc = yd+l3*sin(theta3);
    A1 = [l2*sin(theta2), -l3*sin(theta3);
        -l2*cos(theta2), l3*cos(theta3)];
    B1 = [-w1*l1*sin(theta1); w1*l1*cos(theta1)];
    omega = A1\B1;
    w2 = omega(1);
    w3 = omega(2);
    A2 = [l2*sin(theta2), -l3*sin(theta3);
        -l2*cos(theta2), l3*cos(theta3)];
    B2 = [w2^2*l2*cos(theta2)-w3^2*l3*cos(theta3);
        w2^2*l2*sin(theta2)-w3^2*l3*sin(theta3)];
    C2 = [-a1*l1*sin(theta1)-w1^2*l1*cos(theta1);
        a1*l1*cos(theta1)-w1^2*l1*sin(theta1)];
    alpha = A2\(C2-B2);
    a2 = omega(1);
    a3 = omega(2);
    
    
    figure(1)
    clf
    plot(xa,ya,'mo',xb,yb,'mo',xc,yc,'mo',xd,yd,'mo')
    hold on 
    plot([xa,xb],[ya,yb],'r-')
    plot([xc,xb],[yc,yb],'b-')
    plot([xc,xd],[yc,yd],'c-')
    axis([-l2, 3*l2, -l2, 3*l2])
    xlabel('X(mm)')
    ylabel('Y(mm)')
end