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]'
'顺序到位条件：'
%未完成
输出结果：
xb1 =
(-7-4*sin(2/9*pi)+4*cos(2/9*pi))/(4*cos(2/9*pi)+4*sin(2/9*pi)-5)
yb1 =
-1
xc1 =
-6*(27+24*sin(2/9*pi)-64*cos(2/9*pi))/(-31*cos(2/9*pi)-5+39*sin(2/9*pi))
yc1 =
-2*(72*cos(2/9*pi)-175+192*sin(2/9*pi))/(-31*cos(2/9*pi)-5+39*sin(2/9*pi))
Lab =
1.0288436025165976748172169832223
Lbc =
2.9872531417317691216303250912289
Lcd =
6.9831476545729886023199865357226
Lad =
5
ans =
曲柄存在条件：
ans =
可靠到位条件：
ans =
14.605219997928496422368168445525
ans =
19.799913716084881287517588922012
ans =
20.814756669957613005391246805307
ans =
顺序到位条件：。