【小项目】四连杆机构的Python运动学求解和MATLAB图形仿真

任务

Python运动学解算

python">from math import cos, sin, pi, sqrt, atan2  
from matplotlib import pyplot as plt  
import matplotlib  
import pandas as pd  """
变量说明：
位置s
速度v
加速度a
角度phi
角速度omega
角加速度alpha
杆长l
其中位置、速度、加速度均为双元素列表，索引0为x方向，索引1为y方向
"""class RR:  """RR杆组参数：A点位置、速度、加速度杆组绕A点旋转初始角度、角速度、角加速度、已转过角度杆件长度属性：B点位置、速度、加速度"""def __init__(self, sA, vA, aA, phi_i, omega_i, alpha_i, l_i, delta_i):  self.sB = [0, 0]  self.vB = [0, 0]  self.aB = [0, 0]  theta_i = phi_i + delta_i  self.sB[0] = sA[0] + l_i * cos(theta_i)  self.sB[1] = sA[1] + l_i * sin(theta_i)  self.vB[0] = vA[0] - l_i * sin(theta_i) * omega_i  self.vB[1] = vA[1] + l_i * cos(theta_i) * omega_i  self.aB[0] = aA[0] - l_i * cos(theta_i) * omega_i ** 2 - l_i * sin(theta_i) * alpha_i  self.aB[1] = aA[1] - l_i * sin(theta_i) * omega_i ** 2 + l_i * cos(theta_i) * alpha_i  def __str__(self):  return (f'xB={self.sB[0]:.2f}\n'  f'yB={self.sB[1]:.2f}\n'  f'vxB={self.vB[0]:.2f}\n'  f'vyB={self.vB[1]:.2f}\n'  f'axB={self.aB[0]:.2f}\n'  f'ayB={self.aB[1]:.2f}\n')  class RRR:  """RRR杆组参数：外副B、D两点的位置、速度、加速度BC、CD杆长l_i、l_j属性：内副C点位置、速度、加速度BC、CD杆件与x轴夹角、角速度、角加速度"""def __init__(self, sB, vB, aB, sD, vD, aD, l_i, l_j, DIR_flag):  self.sC = [0, 0]  self.vC = [0, 0]  self.aC = [0, 0]  self.phi_i = 0  self.omega_i = 0  self.alpha_i = 0  #location  l_BD = sqrt((sD[0] - sB[0]) ** 2 + (sD[1] - sB[1]) ** 2)  A0 = 2 * l_i * (sD[0] - sB[0])  B0 = 2 * l_i * (sD[1] - sB[1])  C0 = l_i ** 2 + l_BD ** 2 - l_j ** 2  self.phi_i = 2 * atan2(B0 + DIR_flag * sqrt(A0 ** 2 + B0 ** 2 - C0 ** 2), A0 + C0)  self.sC[0] = sB[0] + l_i * cos(self.phi_i)  self.sC[1] = sB[1] + l_i * sin(self.phi_i)  phi_j = atan2(self.sC[1] - sD[1], self.sC[0] - sD[0])  #speed  C_i = l_i * cos(self.phi_i)  C_j = l_j * cos(phi_j)  S_i = l_i * sin(self.phi_i)  S_j = l_j * sin(phi_j)  G1 = C_i * S_j - C_j * S_i  self.omega_i = (C_j * (vD[0] - vB[0]) + S_j * (vD[1] - vB[1])) / G1  omega_j = (C_i * (vD[0] - vB[0]) + S_i * (vD[1] - vB[1])) / G1  self.vC[0] = vB[0] - self.omega_i * l_i * sin(self.phi_i)  self.vC[1] = vB[1] + self.omega_i * l_i * cos(self.phi_i)  #acceleration  G2 = aD[0] - aB[0] + self.omega_i ** 2 * C_i - omega_j ** 2 * C_j  G3 = aD[1] - aB[1] + self.omega_i ** 2 * S_i - omega_j ** 2 * S_j  self.alpha_i = (G2 * C_j + G3 * S_j) / G1  alpha_j = (G2 * C_i + G3 * S_i) / G1  self.aC[0] = aB[0] - self.alpha_i * l_i * sin(self.phi_i) - self.omega_i ** 2 * l_i * cos(self.phi_i)  self.aC[1] = aB[1] + self.alpha_i * l_i * cos(self.phi_i) - self.omega_i ** 2 * l_i * sin(self.phi_i)  def __str__(self):  return (f'xC={self.sC[0]:.2f}\n'  f'yC={self.sC[1]:.2f}\n'  f'vxC={self.vC[0]:.2f}\n'  f'vyC={self.vC[1]:.2f}\n'  f'axC={self.aC[0]:.2f}\n'  f'ayC={self.aC[1]:.2f}\n')  #固定点A、D状态
sA = [0, 0]  
vA = [0, 0]  
aA = [0, 0]  
sD = [170, 0]  
vD = [0, 0]  
aD = [0, 0]  
#存储旋转过程中F点状态的列表
xF = []  
yF = []  
vxF = []  
vyF = []  
axF = []  
ayF = []  
vF = []  
aF = []  
#用于图表绘制的角度坐标
phi = [item for item in range(0, 360)]  
#∠BEF
theta_EBF = atan2(45, 60)  
#BF杆长
l_BF = sqrt(45 ** 2 + 60 ** 2)  #以1为角度步长旋转
for i in range(360):  #转换为弧度制delta_i = pi / 180 * i  #解算B点状态AB = RR(sA, vA, aA, 0, 10, 0, 75, delta_i)  #解算C点状态和BC杆旋转状态BCD = RRR(AB.sB, AB.vB, AB.aB, sD, vD, aD, 120, 135, 1)  #解算F点状态BF = RR(AB.sB, AB.vB, AB.aB, BCD.phi_i, BCD.omega_i, BCD.alpha_i, l_BF, theta_EBF)  #将F点状态存储进列表并将单位由毫米转换为米xF.append(BF.sB[0] / 100)  yF.append(BF.sB[1] / 100)  vxF.append(BF.vB[0] / 100)  vyF.append(BF.vB[1] / 100)  axF.append(BF.aB[0] / 100)  ayF.append(BF.aB[1] / 100)  vF.append(sqrt(vxF[i] ** 2 + vyF[i] ** 2))  aF.append(sqrt(axF[i] ** 2 + ayF[i] ** 2))  #绘制图表
font = {'family': 'MicroSoft YaHei',  'weight': 'bold',  'size': 10}  
matplotlib.rc('font', **font)  
fig = plt.figure(figsize=(20, 20), dpi=300)  
plt.grid(alpha=1, linestyle="--")  plt.xlabel('x')  
plt.ylabel('y')  
plt.title("F点轨迹曲线")  
plt.scatter(xF, yF)  
plt.show()  plt.scatter(phi, vxF)  
plt.xlabel('φ')  
plt.ylabel('v_Fx')  
plt.title("F点水平速度-角度曲线")  
plt.show()  plt.scatter(phi, vyF)  
plt.xlabel('φ')  
plt.ylabel('v_Fy')  
plt.title("F点竖直速度-角度曲线")  
plt.show()  plt.scatter(phi, axF)  
plt.xlabel('φ')  
plt.ylabel('a_Fx')  
plt.title("F点水平加速度-角度曲线")  
plt.show()  plt.scatter(phi, ayF)  
plt.xlabel('φ')  
plt.ylabel('a_Fy')  
plt.title("F点竖直加速度-角度曲线")  
plt.show()  plt.xlabel('x')  
plt.ylabel('y')  
plt.title("F点轨迹-速度曲线")  
plt.scatter(xF, yF, c=vF, cmap='coolwarm')  
plt.colorbar()  
plt.show()  plt.xlabel('x')  
plt.ylabel('y')  
plt.title("F点轨迹-加速度曲线")  
plt.scatter(xF, yF, c=aF, cmap='coolwarm')  
plt.colorbar()  
plt.show()  #导出表格
data = {  'degree': range(0, 360),  'sFx': xF,  'sFy': yF,  'vFx': vxF,  'vFy': vyF,  'axF': axF,  'ayF': ayF,  
}  df = pd.DataFrame(data)  
df.to_excel('output.xlsx', index=False)

运行结果：

MATLAB图形仿真

RR.m（RR杆组位置解算函数）

function [sBx, sBy] = RR(sAx, sAy, phi_i, l_i, delta_i)theta_i = phi_i + delta_i;sBx = sAx + l_i * cos(theta_i);sBy = sAy + l_i * sin(theta_i);

RRR.m（RRR杆组位置解算函数）

function [sCx, sCy, phi_i] = RRR(sBx, sBy, sDx, sDy, l_i, l_j)BD = sqrt((sDx - sBx) ^ 2 + (sDy - sBy) ^ 2);A0 = 2 * l_i * (sDx - sBx);B0 = 2 * l_i * (sDy - sBy);C0 = l_i ^ 2 + BD ^ 2 - l_j ^ 2;phi_i = 2 * atan2(B0 + sqrt(A0 ^ 2 + B0 ^ 2 - C0 ^ 2), A0 + C0);sCx = sBx + l_i * cos(phi_i);sCy = sBy + l_i * sin(phi_i);

main.m

clear;AB = 75;BC = 120;CD = 135;AD = 170;BE = 60;EF = 45;BF = sqrt(BE ^ 2 + EF ^ 2);EBF = atan2(45, 60);Bx = [];Cx = [];Dx = [];Ex = [];Fx = [];By = [];Cy = [];Dy = [];Ey = [];Fy = [];figure;grid on;filename = 'animation.gif';% 位置解算for i = 1: 1: 361delta_i = i * (pi / 180);[sBx, sBy] = RR(0, 0, 0, AB, delta_i);[sCx, sCy, phi_i] = RRR(sBx, sBy, AD, 0, BC, CD);[sFx, sFy] = RR(sBx, sBy, phi_i, BF, EBF);sEx = (sBx + sCx) / 2;sEy = (sBy + sCy) / 2;Bx(end + 1) = sBx;Cx(end + 1) = sCx;Ex(end + 1) = sEx;Fx(end + 1) = sFx;By(end + 1) = sBy;Cy(end + 1) = sCy;Ey(end + 1) = sEy;Fy(end + 1) = sFy;endfor i = 1: 1: 360clf;hold on;% 绘制杆件plot([0, Bx(i)], [0, By(i)], 'b');plot([Bx(i), Cx(i)], [By(i), Cy(i)], 'b');plot([Cx(i), AD], [Cy(i), 0], 'b');plot([Ex(i), Fx(i)], [Ey(i), Fy(i)], 'b');% 绘制运动副plot(0, 0, 'ro');plot(Bx(i), By(i), 'ro');plot(Cx(i), Cy(i), 'ro');plot(AD, 0, 'ro');plot(Ex(i), Ey(i), 'ro');plot(Fx(i), Fy(i), 'ro');plot(Fx, Fy)hold off;axis([-100 200 -100 200]);% 生成gifframe = getframe(gcf);im = frame2im(frame);[imind, cm] = rgb2ind(im, 256)if i == 1imwrite(imind, cm, filename, 'gif', 'LoopCount', inf, 'DelayTime', 0.01);elseimwrite(imind, cm, filename, 'gif', 'WriteMode', 'append', 'DelayTime', 0.01);endpause(0.01);end

运行结果：