MATLAB程序：已知三个位置设计平面四杆机构求解程序(位移矩阵法)

%MATLAB程序：已知三个位置设计平面四杆机构求解程序（位移矩阵法）

clear;clc;

%凡是变量名前带v的为数值变量，不带的是符号变量

vxp1=0; vyp1=0; vsita1=0*pi/180;

vxp2=-2; vyp2=6; vsita2=40*pi/180;

vxp3=-10; vyp3=8; vsita3=90*pi/180; %精确位置P1，P2，P3及各角度

vsita12=vsita2-vsita1;

vsita13=vsita3-vsita1;

vxa=-10; vya=-2;

vxd=-5; vyd=-2; %选定A,D点

%所有数值均在此确定，更改此处即可解出不同数值的四杆机构位移矩阵方程

syms xp1 yp1 xp2 yp2 xp3 yp3 sita12 sita13;

syms xa ya xb1 yb1 xb2 yb2 xb3 yb3;

f1='(xb2-xa)^2+(yb2-ya)^2=(xb1-xa)^2+(yb1-ya)^2';

f2='(xb3-xa)^2+(yb3-ya)^2=(xb1-xa)^2+(yb1-ya)^2'; %前两个机构方程

f3='xb2=cos(sita12)*xb1-sin(sita12)*yb1+xp2-xp1*cos(sita12)+yp1*sin(sita12)';

f4='yb2=sin(sita12)*xb1+cos(sita12)*yb1+yp2-xp1*sin(sita12)-yp1*cos(sita12)'; %由第一个位移矩阵方程得出

f5='xb3=cos(sita13)*xb1-sin(sita13)*yb1+xp3-xp1*cos(sita13)+yp1*sin(sita13)';

f6='yb3=sin(sita13)*xb1+cos(sita13)*yb1+yp3-xp1*sin(sita13)-yp1*cos(sita13)'; %由第二个位移矩阵方程得出

f1=subs(f1,{xa,ya},{vxa,vya});

f2=subs(f2,{xa,ya},{vxa,vya});

f3=subs(f3,{xp1,xp2,yp1,sita12},{vxp1,vxp2,vyp1,vsita12});

f4=subs(f4,{xp1,yp1,yp2,sita12},{vxp1,vyp1,vyp2,vsita12});

f5=subs(f5,{xp1,xp3,yp1,sita13},{vxp1,vxp3,vyp1,vsita13});

f6=subs(f6,{xp1,yp1,yp3,sita13},{vxp1,vyp1,vyp3,vsita13}); %代入具体数值

[xb1,xb2,xb3,yb1,yb2,yb3]=solve(f1,f2,f3,f4,f5,f6); %解方程

vxb1=vpa(xb1);

vyb1=vpa(yb1);

vxb2=vpa(xb2);

vyb2=vpa(yb2);

vxb3=vpa(xb3);

vyb3=vpa(yb3);

(vxb1-vxa)^2+(vyb1-vya)^2;

(vxb2-vxa)^2+(vyb2-vya)^2;

(vxb3-vxa)^2+(vyb3-vya)^2; %去掉这三行分号可验证B点三个位置是否距离A点相等

syms xd yd xc1 yc1 xc2 yc2 xc3 yc3;

f7='(xc2-xd)^2+(yc2-yd)^2=(xc1-xd)^2+(yc1-yd)^2';

f8='(xc3-xd)^2+(yc3-yd)^2=(xc1-xd)^2+(yc1-yd)^2'; %前两个机构方程

f9='xc2=cos(sita12)*xc1-sin(sita12)*yc1+xp2-xp1*cos(sita12)+yp1*sin(sita12)';

f10='yc2=sin(sita12)*xc1+cos(sita12)*yc1+yp2-xp1*sin(sita12)-yp1*cos(sita12)'; %由第一个位移矩阵方程得出

f11='xc3=cos(sita13)*xc1-sin(sita13)*yc1+xp3-xp1*cos(sita13)+yp1*sin(sita13)';

f12='yc3=sin(sita13)*xc1+cos(sita13)*yc1+yp3-xp1*sin(sita13)-yp1*cos(sita13)'; %由第二个位移矩阵方程得出

f7=subs(f7,{xd,yd},{vxd,vyd});

f8=subs(f8,{xd,yd},{vxd,vyd});

f9=subs(f9,{xp1,xp2,yp1,sita12},{vxp1,vxp2,vyp1,vsita12});

f10=subs(f10,{xp1,yp1,yp2,sita12},{vxp1,vyp1,vyp2,vsita12});

f11=subs(f11,{xp1,xp3,yp1,sita13},{vxp1,vxp3,vyp1,vsita13});

f12=subs(f12,{xp1,yp1,yp3,sita13},{vxp1,vyp1,vyp3,vsita13}); %代入具体数值

[xc1,xc2,xc3,yc1,yc2,yc3]=solve(f7,f8,f9,f10,f11,f12); %解方程

vxc1=vpa(xc1);

vyc1=vpa(yc1);

vxc2=vpa(xc2);

vyc2=vpa(yc2);

vxc3=vpa(xc3);

vyc3=vpa(yc3);

(vxc1-vxd)^2+(vyc1-vyd)^2;

(vxc2-vxd)^2+(vyc2-vyd)^2;

(vxc3-vxd)^2+(vyc3-vyd)^2; %去掉这三行分号可验证C点三个位置是否距离D点相等

%最终答案

xb1,yb1,xc1,yc1

Lab=sqrt((vxb1-vxa)^2+(vyb1-vya)^2)

Lbc=sqrt((vxb1-vxc1)^2+(vyb1-vyc1)^2)

Lcd=sqrt((vxc1-vxd)^2+(vyc1-vyd)^2)

Lad=sqrt((vxa-vxd)^2+(vya-vyd)^2) %得到四杆长

'曲柄存在条件：'

%得出四杆长后计算得到

'可靠到位条件：'

[vxc1-vxb1,vyc1-vyb1]*[vxc1-vxd,vyc1-vyd]'

[vxc2-vxb2,vyc2-vyb2]*[vxc2-vxd,vyc2-vyd]'

[vxc3-vxb3,vyc3-vyb3]*[vxc3-vxd,vyc3-vyd]'

'顺序到位条件：'

%未完成